Commit c341da83 authored by evanblough's avatar evanblough

2019-2020 Mavlink adapter final version. Supports getparam, setparam, and getnodes

parent a51810f9
......@@ -6,172 +6,6 @@
#IncludeRegexTransform:
../../../../quad/src/commands/commands.h
stdio.h
-
stdlib.h
-
string.h
-
stdint.h
-
../../../../quad/src/computation_graph/computation_graph.h
stdio.h
-
math.h
-
../../../../quad/src/graph_blocks/graph_blocks.h
computation_graph.h
../../../../quad/src/graph_blocks/computation_graph.h
node_constant.h
../../../../quad/src/graph_blocks/node_constant.h
node_add.h
../../../../quad/src/graph_blocks/node_add.h
node_mult.h
../../../../quad/src/graph_blocks/node_mult.h
node_gain.h
../../../../quad/src/graph_blocks/node_gain.h
node_accumulator.h
../../../../quad/src/graph_blocks/node_accumulator.h
node_bounds.h
../../../../quad/src/graph_blocks/node_bounds.h
node_mixer.h
../../../../quad/src/graph_blocks/node_mixer.h
node_pid.h
../../../../quad/src/graph_blocks/node_pid.h
node_yaw_rot.h
../../../../quad/src/graph_blocks/node_yaw_rot.h
node_integrator.h
../../../../quad/src/graph_blocks/node_integrator.h
../../../../quad/src/graph_blocks/node_accumulator.h
computation_graph.h
../../../../quad/src/graph_blocks/computation_graph.h
graph_blocks.h
../../../../quad/src/graph_blocks/graph_blocks.h
../../../../quad/src/graph_blocks/node_add.h
computation_graph.h
../../../../quad/src/graph_blocks/computation_graph.h
graph_blocks.h
../../../../quad/src/graph_blocks/graph_blocks.h
../../../../quad/src/graph_blocks/node_bounds.h
computation_graph.h
../../../../quad/src/graph_blocks/computation_graph.h
graph_blocks.h
../../../../quad/src/graph_blocks/graph_blocks.h
../../../../quad/src/graph_blocks/node_constant.h
computation_graph.h
../../../../quad/src/graph_blocks/computation_graph.h
graph_blocks.h
../../../../quad/src/graph_blocks/graph_blocks.h
../../../../quad/src/graph_blocks/node_gain.h
computation_graph.h
../../../../quad/src/graph_blocks/computation_graph.h
graph_blocks.h
../../../../quad/src/graph_blocks/graph_blocks.h
../../../../quad/src/graph_blocks/node_integrator.h
computation_graph.h
../../../../quad/src/graph_blocks/computation_graph.h
graph_blocks.h
../../../../quad/src/graph_blocks/graph_blocks.h
../../../../quad/src/graph_blocks/node_mixer.h
computation_graph.h
../../../../quad/src/graph_blocks/computation_graph.h
graph_blocks.h
../../../../quad/src/graph_blocks/graph_blocks.h
../../../../quad/src/graph_blocks/node_mult.h
computation_graph.h
../../../../quad/src/graph_blocks/computation_graph.h
graph_blocks.h
../../../../quad/src/graph_blocks/graph_blocks.h
../../../../quad/src/graph_blocks/node_pid.h
computation_graph.h
../../../../quad/src/graph_blocks/computation_graph.h
graph_blocks.h
../../../../quad/src/graph_blocks/graph_blocks.h
../../../../quad/src/graph_blocks/node_yaw_rot.h
computation_graph.h
../../../../quad/src/graph_blocks/computation_graph.h
graph_blocks.h
../../../../quad/src/graph_blocks/graph_blocks.h
../../../../quad/src/quad_app/PID.h
type_def.h
../../../../quad/src/quad_app/type_def.h
../../../../quad/src/quad_app/biquad_filter.h
../../../../quad/src/quad_app/callbacks.h
commands.h
../../../../quad/src/quad_app/commands.h
../../../../quad/src/quad_app/communication.h
type_def.h
../../../../quad/src/quad_app/type_def.h
timer.h
../../../../quad/src/quad_app/timer.h
commands.h
../../../../quad/src/quad_app/commands.h
hw_iface.h
../../../../quad/src/quad_app/hw_iface.h
../../../../quad/src/quad_app/controllers.h
util.h
../../../../quad/src/quad_app/util.h
../../../../quad/src/quad_app/hw_iface.h
../../../../quad/src/quad_app/log_data.h
type_def.h
../../../../quad/src/quad_app/type_def.h
../../../../quad/src/quad_app/timer.h
log_data.h
../../../../quad/src/quad_app/log_data.h
../../../../quad/src/quad_app/type_def.h
stdint.h
-
commands.h
../../../../quad/src/quad_app/commands.h
computation_graph.h
../../../../quad/src/quad_app/computation_graph.h
hw_iface.h
../../../../quad/src/quad_app/hw_iface.h
biquad_filter.h
../../../../quad/src/quad_app/biquad_filter.h
../../../../quad/src/quad_app/util.h
type_def.h
../../../../quad/src/quad_app/type_def.h
stdlib.h
-
stdio.h
-
string.h
-
math.h
-
PID.h
../../../../quad/src/quad_app/PID.h
log_data.h
../../../../quad/src/quad_app/log_data.h
controllers.h
../../../../quad/src/quad_app/controllers.h
hw_iface.h
../../../../quad/src/quad_app/hw_iface.h
../c_library_v2/checksum.h
stdint.h
-
......@@ -1020,110 +854,34 @@ mavlink_types.h
mavlink_helpers.h
../c_library_v2/mavlink_helpers.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/adapterlib.h
packet.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/packet.h
commands.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/commands.h
stdint.h
-
adapterlib.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/adapterlib.h
commands.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/commands.h
communication.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/communication.h
packet.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/packet.h
type_def.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/type_def.h
callbacks.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/callbacks.h
stdio.h
-
stdlib.h
-
sys/socket.h
-
sys/un.h
-
unistd.h
-
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/mavlink_utils.c
mavlink_utils.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/mavlink_utils.h
stdio.h
-
errno.h
-
err.h
-
stdio.h
-
stdlib.h
-
unistd.h
-
signal.h
string.h
-
sys/socket.h
-
sys/un.h
-
netinet/in.h
-
arpa/inet.h
-
sys/select.h
-
pthread.h
-
assert.h
-
errno.h
-
string.h
-
sys/ioctl.h
sys/time.h
-
sys/types.h
-
netinet/tcp.h
-
fcntl.h
-
sys/stat.h
netinet/in.h
-
sys/time.h
unistd.h
-
stdlib.h
-
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/mavlink_callbacks.c
communication.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/communication.h
commands.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/commands.h
graph_blocks.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/graph_blocks.h
type_def.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/type_def.h
mavlink_callbacks.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/mavlink_callbacks.h
computation_graph.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/computation_graph.h
util.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/util.h
mavlink_utils.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/mavlink_utils.h
adapterlib.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/adapterlib.h
unistd.h
fcntl.h
-
time.h
-
common/mavlink.h
-
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/mavlink_callbacks.h
commands.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/commands.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/mavlink_utils.h
stdio.h
-
......@@ -1146,11 +904,3 @@ fcntl.h
time.h
-
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/packet.h
stdint.h
-
sys/types.h
-
commands.h
/home/rmasl/Desktop/MicroCART/groundStation/adapters/mavlink/commands.h
......@@ -233,6 +233,7 @@ int cb_getnodes(struct modular_structs *structs, struct metadata *meta, unsigned
//Fetch New nodes
request_params();
save_params();
display_params();
u8 resp_buf[4096];
const size_t id_len = 2;
size_t offset = 0;
......
......@@ -84,7 +84,6 @@ int request_mission() {
return 0;
}
int set_mission_item(int index, float value) {
int mission_item_index = index / 3;
int mission_item_orientation = index % 3;
......@@ -104,7 +103,6 @@ int set_mission_item(int index, float value) {
return 0;
}
int transfer_mission_items() {
//Init buffers and message holders
uint8_t buf[MP_BUFFER_LEN];
......@@ -220,7 +218,7 @@ int save_params() {
}
}
//On Timeout Re-request
if(timeout_end.tv_usec - timeout_start.tv_usec > MAVPROXY_TIMEOUT){
if (timeout_end.tv_usec - timeout_start.tv_usec > MAVPROXY_TIMEOUT) {
request_params();
}
}
......@@ -283,17 +281,18 @@ int set_node_value(short id, short subid, float value) {
switch (id) {
case AUTO_MAN_NODE:
// TODO make a set manual method
(!auto_mode) ? set_auto() : fprintf(stderr, "man not implemented\n");
if (!auto_mode) set_auto();
else fprintf(stderr, "man not implemented\n");
auto_mode = !auto_mode;
break;
case ARM_DISARM_NODE:
//TODO make arm/disarm
fprintf(stderr, "Arm not impelmented\n");
request_arm();
break;
case MISSION_ITEMS_NODES:
set_mission_item(subid, value);
break;
case TRANSMIT_MISSION_NODE:
request_mission();
transfer_mission_items();
break;
case MAVPARAM_START_NODE:
......@@ -313,7 +312,6 @@ int set_node_value(short id, short subid, float value) {
fprintf(stderr, "BAD NODE INDEX %d\n", id);
}
return -1;
}
}
......@@ -330,7 +328,7 @@ int set_mavparam(const char *name, float value, u_int8_t type) {
return -1;
}
gettimeofday(&timeout_start, NULL);
int i;
int i, j;
for (;;) {
gettimeofday(&timeout_end, NULL);
int len = read(mavlink_sock, recv_buf, MP_BUFFER_LEN);
......@@ -342,13 +340,19 @@ int set_mavparam(const char *name, float value, u_int8_t type) {
if (msg.msgid == MAVLINK_MSG_ID_PARAM_VALUE) {
mavlink_msg_param_value_decode(&msg, &param_val);
if (strncmp(name, param_val.param_id, strlen(name)) == 0) {
//Update Param value
for (j = 0; j < num_params; j++) {
if (strncmp(saved_params[i].param_id, name, strlen(name)) == 0) {
saved_params[i].param_value = param_val.param_value;
}
}
return 0;
}
}
}
}
//On Timeout Re-request
if(timeout_end.tv_usec - timeout_start.tv_usec > MAVPROXY_TIMEOUT){
if (timeout_end.tv_usec - timeout_start.tv_usec > MAVPROXY_TIMEOUT) {
mavlink_msg_param_set_pack(SYS_ID, COMP_ID, &msg, TARGET_ID, TARGET_COMP, name, value, type);
mavlink_msg_to_send_buffer(buf, &msg);
if (write(mavlink_sock, buf, MP_BUFFER_LEN) == -1) {
......@@ -360,7 +364,7 @@ int set_mavparam(const char *name, float value, u_int8_t type) {
}
}
int get_mavparam(const char * name, uint16_t index){
int get_mavparam(const char *name, uint16_t index) {
uint8_t buf[MP_BUFFER_LEN];
uint8_t recv_buf[MP_BUFFER_LEN];
mavlink_message_t msg;
......@@ -393,7 +397,7 @@ int get_mavparam(const char * name, uint16_t index){
}
}
//On Timeout Re-request
if(timeout_end.tv_usec - timeout_start.tv_usec > MAVPROXY_TIMEOUT){
if (timeout_end.tv_usec - timeout_start.tv_usec > MAVPROXY_TIMEOUT) {
mavlink_msg_param_request_read_pack(SYS_ID, COMP_ID, &msg, TARGET_ID, TARGET_COMP, name, index);
mavlink_msg_to_send_buffer(buf, &msg);
if (write(mavlink_sock, buf, MP_BUFFER_LEN) == -1) {
......@@ -403,5 +407,18 @@ int get_mavparam(const char * name, uint16_t index){
gettimeofday(&timeout_start, NULL);
}
}
}
int request_arm() {
uint8_t buf[MP_BUFFER_LEN];
mavlink_message_t msg;
//Yes because this is a reasonable ammount of parameters to pass to a function
mavlink_msg_command_long_pack(SYS_ID, COMP_ID, &msg, TARGET_ID, TARGET_COMP, MAV_CMD_COMPONENT_ARM_DISARM, 0, 1, 0,
0, 0, 0, 0, 0);
mavlink_msg_to_send_buffer(buf, &msg);
if (write(mavlink_sock, buf, MP_BUFFER_LEN) == -1) {
perror("failure to clear mission items\n");
return -1;
}
return 0;
}
\ No newline at end of file
......@@ -64,16 +64,18 @@ int connect_mavlink();
*/
int request_mission();
/**
*
* @param index
* @param value
* @brief Sets the mission item at index to the value given by the setparam command
* @param index the index of the mission item and lat/lon/alt. The index of the mission item is obtained by index / 3 and the
* lat/lon/alt or x/y/z is obtained from index % 3.
* Ex: Set the third waypoints y coordinate would be index = 7; 7 / 3 = 2; and 7 % 3 = 1
* @param value The value to be set units are depedent on the vehicle's frame of refrence
* @return
*/
int set_mission_item(int index, float value);
/**
* @brief This function is called after set_mission_items() and request_mission(). It begins to
* listen for mission_item_request message on comm0, and upon recieving that message it will transfer a mission item
* out of the mission_items[][] array.
* out of the mission_items[][] array. It will wait until the mission is acknowledged and retransmit if necessary.
* @return This function does not return until all mission items are transferred, there isn't a timeout implemented yet.
*/
int transfer_mission_items();
......@@ -92,27 +94,58 @@ int set_auto();
*/
int request_params();
/**
* @brief This function saves all the current mavlink parameters into a saved_params global variable.
* @return
* @brief This function saves all the current mavlink parameters into a saved_params global variable. It will poll for param_value messages and
* leave execuition after recieving all parameters. If not all parameters were recieved, then it will trigger retransmission after timeout
* @return -1 on fail and 0 otherwise
*/
int save_params();
/**
* @brief This function prints all the params stored in the saved_params variable to stdout. This can be used for debugging purposes.
*
*/
void display_params();
/**
* @brief
* @param id
* @param subid
* @return
* @brief This function gets the value of an individual node from the nodes obtained from the getnodes command. To see how
* the nodes are structured look at the mavlink adapter documentation in the git repo or look at the source code
* @param id The id of the node to be fetched
* @param subid the sub id of the node to be fetched. Note that this is only used for the mission items node
* @return returns -1 on fail or the value of the node otherwise. If you recieve a negative one it could be an actual value of the node, so
* be careful about handeling error from this.
*/
float get_node_value(short id, short subid);
/**
* @brief This function sets the value of an individual node from the nodes obtained from the getnodes MicroCART command. To see how the nodes are structured
* look at the mavlink adapter documentation in the git repo or look at the source.
* @param id The id of the node to be fetched
* @param subid The sub id of the node to be fetched. Only used by the Mission Item Reserved node
* @param value The value to set the node to.
* @return Returns 0 on success and -1 on failure
*/
int set_node_value(short id, short subid, float value);
/**
* @brief This function will set the value of a MAVParam on the autonomus vehicle. These MAVParams can control how the vehicle navigates. This function polls until it
* recieves acknowledgment from the vehicle.
* @param name The param_id char[16] of the parameter to be set. mavparam.param_id.
* @param value The new value of the parameter
* @param type the data type of the
* @return
*/
int set_mavparam(const char * name, float value, u_int8_t type);
/**
* @brief This function will get the value of a MAVParam on the autonomus vehicle. These MAVParams can control how the vehicle navigates. This function polls until it
* recieves acknowledgment from the vehicle.
* @param name the param_id[16] of the parameter to be sent
* @param index the index of the mavparam obtained from saved_params[i].param_index
* @return returns 0 on success and -1 on failure. It saves the value of the parameter into the saved_parameters struct after recieveing ACK
*/
int get_mavparam(const char * name, uint16_t index);
/**
* @brief Arms the vehicle
* @return 0 on success and -1 on failure
*/
int request_arm();
#endif //MAVLINK_MAVLINK_UTILS_H
#!/bin/bash
#This is a test script used to confirm getting and setting MAVParams work in the MAVLink Simulation Environement
#A MAVParam is just an on vehicle parameter than can be set to a certain value to affect navigation performence.
#Ex Paramter WP_RADIUS affects how close the vehicle need to get to the setpoint before it thinks it reached that point
#There are also P, I, and D gains for several control parameters
#The Process for setting up the testing environment is as follows
#(1) Start the mavlink adapter executable, it will listen for a client connection coming from the backend
#(2) Configure the trackables in config.c in the backend
#(3) Compile and launch the backend executable with VRPN Disabled
#(4) Install MAVProxy and Ardupilot on your system
#(5) SITL and MAVProxy using the python3 interperter on your system with the following args
#(6) python3 sim_vehicle.py -v APMrover2 --map --console --out=tcp:127.0.0.1:4250
#(7) Wait approxmately a minute for the vehicle simulator to setup
#(8) Open a terminal in the GCS directory with the getparam and setparam symbolic links
#(9) Run this script in that directory
#(10) Check output to see that previous value changed to 1.5
#(11) Make sure to change PARAM_VALUE if it is the same as what's currently on the vehicle
#(12) You can also confirm the value by going to the MAVProxy terminal and typing param show
PARAM_INDEX=62
PARAM_VALUE=1.5
START_INDEX=1000
./settrackable mavlink
echo "Setting Parameter Offset to 1000"
./setparam 4 0 1000
sleep 1
echo "Getting Value of WP_OVERSHOOT index at 62"
./getparam 62 0
sleep 1
echo "Setting Value of WP_OVERSHOOT to 1.5m index at 62"
./setparam 62 0 1.5
sleep 1
echo "Flushing Cached Value of WP_OVERSHOOT index at 62"
./getparam 62 0
sleep 10
echo "Recieving Actual Value of WP_OVERSHOOT index at 62"
./getparam 62 0
sleep 1
echo "Done"
#!/bin/bash
#This is a test script used to confirm the get_nodes command works in the MAVLink Simulation Environement
#A get_nodes is a MicroCART command like get or setparam
#Get_nodes doesn't work in the same way as it would for the quadcopter because the quadcopter controls and mavlink controls are severly different
#Get_nodes gets a sub window of parameters because the backend can only support packets with a max size of 4096 bytes
#See the docs on the git page for more elaboration on this
#The Process for setting up the testing environment is as follows
#(1) Start the mavlink adapter executable, it will listen for a client connection coming from the backend
#(2) Configure the trackables in config.c in the backend
#(3) Compile and launch the backend executable with VRPN Disabled
#(4) Install MAVProxy and Ardupilot on your system
#(5) SITL and MAVProxy using the python3 interperter on your system with the following args
#(6) python3 sim_vehicle.py -v APMrover2 --map --console --out=tcp:127.0.0.1:4250
#(7) Wait approxmately a minute for the vehicle simulator to setup
#(8) Open a terminal in the GCS directory with the getparam and setparam symbolic links
#(9) Run this script in that directory
#(10) Confirm it outputs nodes in the current subwindow
./settrackable mavlink
echo "Getting nodes"
./getnodes
sleep 1
echo "Done"
#!/bin/bash
#This is a test script used to confirm mission transfers work in the MAVLink Simulation Environement
#A mission is just a grouping of setpoints for the autonomus vehicle
#The Process for setting up the testing environment is as follows
#(1) Start the mavlink adapter executable, it will listen for a client connection coming from the backend
#(2) Configure the trackables in config.c in the backend
#(3) Compile and launch the backend executable with VRPN Disabled
#(4) Install MAVProxy and Ardupilot on your system
#(5) SITL and MAVProxy using the python3 interperter on your system with the following args
#(6) python3 sim_vehicle.py -v APMrover2 --map --console --out=tcp:127.0.0.1:4250
#(7) Wait approxmately a minute for the vehicle simulator to setup
#(8) Open a terminal in the GCS directory with the getparam and setparam symbolic links
#(9) Run this script in that directory
#(10) Observe the sim vehicle move on the map
#(11) Compare the location of the waypoints on the map to the values set in this script
./settrackable mavlink
sleep 5
echo "Sending Waypoints"
#Set Mission Item One X
./setparam 2 0 -35.362963
#Set Mission Item One Y
./setparam 2 1 149.165891
#Set Mission Item One Z
./setparam 2 2 -0.110000
#Set Mission Item Two X
./setparam 2 3 -35.362290
#Set Mission Item Two Y
./setparam 2 4 149.166207
#Set Mission Item Two Z
./setparam 2 5 100.000000
#Set Mission Item 3 X
./setparam 2 6 -35.361762
#Set Mission Item 3 Y
./setparam 2 7 149.165133
#Set Mission Item 3 Z
./setparam 2 8 100.000000
#Transmit Mission
echo "Transmitting Mission"
sleep 5
./setparam 3 0 0
#Set to Auto
echo "Setting Auto"
sleep 5
./setparam 0 0 0
#Engage Throttle
echo "Arming Throttle"
sleep 10
./setparam 1 0 0
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment