diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 29b74487a3..b10c4a0c4f 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -1,7 +1,5 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#include "Mavlink_compat.h" - // use this to prevent recursion during sensor init static bool in_mavlink_delay; @@ -476,7 +474,7 @@ static void NOINLINE send_gps_status(mavlink_channel_t chan) static void NOINLINE send_current_waypoint(mavlink_channel_t chan) { - mavlink_msg_waypoint_current_send( + mavlink_msg_mission_current_send( chan, g.command_index); } @@ -582,7 +580,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, break; case MSG_CURRENT_WAYPOINT: - CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); + CHECK_PAYLOAD_SIZE(MISSION_CURRENT); send_current_waypoint(chan); break; @@ -596,7 +594,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, break; case MSG_NEXT_WAYPOINT: - CHECK_PAYLOAD_SIZE(WAYPOINT_REQUEST); + CHECK_PAYLOAD_SIZE(MISSION_REQUEST); if (chan == MAVLINK_COMM_0) { gcs0.queued_waypoint_send(); } else if (gcs3.initialised) { @@ -1108,16 +1106,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { // decode - mavlink_waypoint_request_list_t packet; - mavlink_msg_waypoint_request_list_decode(msg, &packet); + mavlink_mission_request_list_t packet; + mavlink_msg_mission_request_list_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) break; // Start sending waypoints - mavlink_msg_waypoint_count_send( + mavlink_msg_mission_count_send( chan,msg->sysid, msg->compid, g.command_total + 1); // + home @@ -1132,15 +1130,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // XXX read a WP from EEPROM and send it to the GCS - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + case MAVLINK_MSG_ID_MISSION_REQUEST: { // Check if sending waypiont //if (!waypoint_sending) break; // 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request. DEW // decode - mavlink_waypoint_request_t packet; - mavlink_msg_waypoint_request_decode(msg, &packet); + mavlink_mission_request_t packet; + mavlink_msg_mission_request_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) break; @@ -1222,7 +1220,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - mavlink_msg_waypoint_send(chan,msg->sysid, + mavlink_msg_mission_item_send(chan,msg->sysid, msg->compid, packet.seq, frame, @@ -1243,11 +1241,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } - case MAVLINK_MSG_ID_WAYPOINT_ACK: + case MAVLINK_MSG_ID_MISSION_ACK: { // decode - mavlink_waypoint_ack_t packet; - mavlink_msg_waypoint_ack_decode(msg, &packet); + mavlink_mission_ack_t packet; + mavlink_msg_mission_ack_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; // turn off waypoint send @@ -1270,11 +1268,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { // decode - mavlink_waypoint_clear_all_t packet; - mavlink_msg_waypoint_clear_all_decode(msg, &packet); + mavlink_mission_clear_all_t packet; + mavlink_msg_mission_clear_all_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) break; // clear all commands @@ -1283,29 +1281,29 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // note that we don't send multiple acks, as otherwise a // GCS that is doing a clear followed by a set may see // the additional ACKs as ACKs of the set operations - mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); + mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); break; } - case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { // decode - mavlink_waypoint_set_current_t packet; - mavlink_msg_waypoint_set_current_decode(msg, &packet); + mavlink_mission_set_current_t packet; + mavlink_msg_mission_set_current_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; // set current command change_command(packet.seq); - mavlink_msg_waypoint_current_send(chan, g.command_index); + mavlink_msg_mission_current_send(chan, g.command_index); break; } - case MAVLINK_MSG_ID_WAYPOINT_COUNT: + case MAVLINK_MSG_ID_MISSION_COUNT: { // decode - mavlink_waypoint_count_t packet; - mavlink_msg_waypoint_count_decode(msg, &packet); + mavlink_mission_count_t packet; + mavlink_msg_mission_count_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; // start waypoint receiving @@ -1334,13 +1332,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #endif // XXX receive a WP from GCS and store in EEPROM - case MAVLINK_MSG_ID_WAYPOINT: + case MAVLINK_MSG_ID_MISSION_ITEM: { // decode - mavlink_waypoint_t packet; + mavlink_mission_item_t packet; uint8_t result = MAV_MISSION_ACCEPTED; - mavlink_msg_waypoint_decode(msg, &packet); + mavlink_msg_mission_item_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; // defaults @@ -1467,7 +1465,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) set_guided_WP(); // verify we recevied the command - mavlink_msg_waypoint_ack_send( + mavlink_msg_mission_ack_send( chan, msg->sysid, msg->compid, @@ -1494,7 +1492,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) waypoint_request_i++; if (waypoint_request_i > (uint16_t)g.command_total){ - mavlink_msg_waypoint_ack_send( + mavlink_msg_mission_ack_send( chan, msg->sysid, msg->compid, @@ -1510,7 +1508,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mission_failed: // we are rejecting the mission/waypoint - mavlink_msg_waypoint_ack_send( + mavlink_msg_mission_ack_send( chan, msg->sysid, msg->compid, @@ -1893,7 +1891,7 @@ GCS_MAVLINK::queued_waypoint_send() { if (waypoint_receiving && waypoint_request_i <= (unsigned)g.command_total) { - mavlink_msg_waypoint_request_send( + mavlink_msg_mission_request_send( chan, waypoint_dest_sysid, waypoint_dest_compid, diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index b422080519..d54121cbc0 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1,7 +1,5 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#include "Mavlink_compat.h" - // use this to prevent recursion during sensor init static bool in_mavlink_delay; @@ -450,7 +448,7 @@ static void NOINLINE send_gps_status(mavlink_channel_t chan) static void NOINLINE send_current_waypoint(mavlink_channel_t chan) { - mavlink_msg_waypoint_current_send( + mavlink_msg_mission_current_send( chan, (uint16_t)g.command_index); } @@ -552,7 +550,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, break; case MSG_CURRENT_WAYPOINT: - CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); + CHECK_PAYLOAD_SIZE(MISSION_CURRENT); send_current_waypoint(chan); break; @@ -566,7 +564,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, break; case MSG_NEXT_WAYPOINT: - CHECK_PAYLOAD_SIZE(WAYPOINT_REQUEST); + CHECK_PAYLOAD_SIZE(MISSION_REQUEST); if (chan == MAVLINK_COMM_0) { gcs0.queued_waypoint_send(); } else { @@ -1107,18 +1105,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } */ - case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: //43 + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: //43 { //send_text_P(SEVERITY_LOW,PSTR("waypoint request list")); // decode - mavlink_waypoint_request_list_t packet; - mavlink_msg_waypoint_request_list_decode(msg, &packet); + mavlink_mission_request_list_t packet; + mavlink_msg_mission_request_list_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) break; // Start sending waypoints - mavlink_msg_waypoint_count_send( + mavlink_msg_mission_count_send( chan,msg->sysid, msg->compid, g.command_total); // includes home @@ -1132,7 +1130,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } // XXX read a WP from EEPROM and send it to the GCS - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: // 40 + case MAVLINK_MSG_ID_MISSION_REQUEST: // 40 { //send_text_P(SEVERITY_LOW,PSTR("waypoint request")); @@ -1141,8 +1139,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request. DEW // decode - mavlink_waypoint_request_t packet; - mavlink_msg_waypoint_request_decode(msg, &packet); + mavlink_mission_request_t packet; + mavlink_msg_mission_request_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) break; @@ -1238,7 +1236,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - mavlink_msg_waypoint_send(chan,msg->sysid, + mavlink_msg_mission_item_send(chan,msg->sysid, msg->compid, packet.seq, frame, @@ -1258,13 +1256,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_WAYPOINT_ACK: //47 + case MAVLINK_MSG_ID_MISSION_ACK: //47 { //send_text_P(SEVERITY_LOW,PSTR("waypoint ack")); // decode - mavlink_waypoint_ack_t packet; - mavlink_msg_waypoint_ack_decode(msg, &packet); + mavlink_mission_ack_t packet; + mavlink_msg_mission_ack_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; // turn off waypoint send @@ -1289,13 +1287,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: // 45 + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: // 45 { //send_text_P(SEVERITY_LOW,PSTR("waypoint clear all")); // decode - mavlink_waypoint_clear_all_t packet; - mavlink_msg_waypoint_clear_all_decode(msg, &packet); + mavlink_mission_clear_all_t packet; + mavlink_msg_mission_clear_all_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) break; // clear all waypoints @@ -1304,34 +1302,34 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // send acknowledgement 3 times to makes sure it is received for (int i=0;i<3;i++) - mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, type); + mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, type); break; } - case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: // 41 + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: // 41 { //send_text_P(SEVERITY_LOW,PSTR("waypoint set current")); // decode - mavlink_waypoint_set_current_t packet; - mavlink_msg_waypoint_set_current_decode(msg, &packet); + mavlink_mission_set_current_t packet; + mavlink_msg_mission_set_current_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; // set current command change_command(packet.seq); - mavlink_msg_waypoint_current_send(chan, g.command_index); + mavlink_msg_mission_current_send(chan, g.command_index); break; } - case MAVLINK_MSG_ID_WAYPOINT_COUNT: // 44 + case MAVLINK_MSG_ID_MISSION_COUNT: // 44 { //send_text_P(SEVERITY_LOW,PSTR("waypoint count")); // decode - mavlink_waypoint_count_t packet; - mavlink_msg_waypoint_count_decode(msg, &packet); + mavlink_mission_count_t packet; + mavlink_msg_mission_count_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; // start waypoint receiving @@ -1360,11 +1358,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #endif // XXX receive a WP from GCS and store in EEPROM - case MAVLINK_MSG_ID_WAYPOINT: //39 + case MAVLINK_MSG_ID_MISSION_ITEM: //39 { // decode - mavlink_waypoint_t packet; - mavlink_msg_waypoint_decode(msg, &packet); + mavlink_mission_item_t packet; + mavlink_msg_mission_item_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; // defaults @@ -1484,7 +1482,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) set_next_WP(&guided_WP); // verify we recevied the command - mavlink_msg_waypoint_ack_send( + mavlink_msg_mission_ack_send( chan, msg->sysid, msg->compid, @@ -1512,7 +1510,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (waypoint_request_i == (uint16_t)g.command_total){ uint8_t type = 0; // ok (0), error(1) - mavlink_msg_waypoint_ack_send( + mavlink_msg_mission_ack_send( chan, msg->sysid, msg->compid, @@ -1910,7 +1908,7 @@ GCS_MAVLINK::queued_waypoint_send() { if (waypoint_receiving && waypoint_request_i < (unsigned)g.command_total) { - mavlink_msg_waypoint_request_send( + mavlink_msg_mission_request_send( chan, waypoint_dest_sysid, waypoint_dest_compid, diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index eb10a9d09e..8bea7bb330 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1,7 +1,5 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#include "Mavlink_compat.h" - // use this to prevent recursion during sensor init static bool in_mavlink_delay; @@ -446,7 +444,7 @@ static void NOINLINE send_gps_status(mavlink_channel_t chan) static void NOINLINE send_current_waypoint(mavlink_channel_t chan) { - mavlink_msg_waypoint_current_send( + mavlink_msg_mission_current_send( chan, g.command_index); } @@ -552,7 +550,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, break; case MSG_CURRENT_WAYPOINT: - CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); + CHECK_PAYLOAD_SIZE(MISSION_CURRENT); send_current_waypoint(chan); break; @@ -566,7 +564,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, break; case MSG_NEXT_WAYPOINT: - CHECK_PAYLOAD_SIZE(WAYPOINT_REQUEST); + CHECK_PAYLOAD_SIZE(MISSION_REQUEST); if (chan == MAVLINK_COMM_0) { gcs0.queued_waypoint_send(); } else if (gcs3.initialised) { @@ -1078,16 +1076,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { // decode - mavlink_waypoint_request_list_t packet; - mavlink_msg_waypoint_request_list_decode(msg, &packet); + mavlink_mission_request_list_t packet; + mavlink_msg_mission_request_list_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) break; // Start sending waypoints - mavlink_msg_waypoint_count_send( + mavlink_msg_mission_count_send( chan,msg->sysid, msg->compid, g.command_total + 1); // + home @@ -1102,15 +1100,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // XXX read a WP from EEPROM and send it to the GCS - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + case MAVLINK_MSG_ID_MISSION_REQUEST: { // Check if sending waypiont //if (!waypoint_sending) break; // 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request. DEW // decode - mavlink_waypoint_request_t packet; - mavlink_msg_waypoint_request_decode(msg, &packet); + mavlink_mission_request_t packet; + mavlink_msg_mission_request_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) break; @@ -1192,7 +1190,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - mavlink_msg_waypoint_send(chan,msg->sysid, + mavlink_msg_mission_item_send(chan,msg->sysid, msg->compid, packet.seq, frame, @@ -1213,11 +1211,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } - case MAVLINK_MSG_ID_WAYPOINT_ACK: + case MAVLINK_MSG_ID_MISSION_ACK: { // decode - mavlink_waypoint_ack_t packet; - mavlink_msg_waypoint_ack_decode(msg, &packet); + mavlink_mission_ack_t packet; + mavlink_msg_mission_ack_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; // turn off waypoint send @@ -1240,11 +1238,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { // decode - mavlink_waypoint_clear_all_t packet; - mavlink_msg_waypoint_clear_all_decode(msg, &packet); + mavlink_mission_clear_all_t packet; + mavlink_msg_mission_clear_all_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) break; // clear all commands @@ -1253,29 +1251,29 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // note that we don't send multiple acks, as otherwise a // GCS that is doing a clear followed by a set may see // the additional ACKs as ACKs of the set operations - mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); + mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); break; } - case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { // decode - mavlink_waypoint_set_current_t packet; - mavlink_msg_waypoint_set_current_decode(msg, &packet); + mavlink_mission_set_current_t packet; + mavlink_msg_mission_set_current_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; // set current command change_command(packet.seq); - mavlink_msg_waypoint_current_send(chan, g.command_index); + mavlink_msg_mission_current_send(chan, g.command_index); break; } - case MAVLINK_MSG_ID_WAYPOINT_COUNT: + case MAVLINK_MSG_ID_MISSION_COUNT: { // decode - mavlink_waypoint_count_t packet; - mavlink_msg_waypoint_count_decode(msg, &packet); + mavlink_mission_count_t packet; + mavlink_msg_mission_count_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; // start waypoint receiving @@ -1304,13 +1302,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #endif // XXX receive a WP from GCS and store in EEPROM - case MAVLINK_MSG_ID_WAYPOINT: + case MAVLINK_MSG_ID_MISSION_ITEM: { // decode - mavlink_waypoint_t packet; + mavlink_mission_item_t packet; uint8_t result = MAV_MISSION_ACCEPTED; - mavlink_msg_waypoint_decode(msg, &packet); + mavlink_msg_mission_item_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; // defaults @@ -1437,7 +1435,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) set_guided_WP(); // verify we recevied the command - mavlink_msg_waypoint_ack_send( + mavlink_msg_mission_ack_send( chan, msg->sysid, msg->compid, @@ -1464,7 +1462,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) waypoint_request_i++; if (waypoint_request_i > (uint16_t)g.command_total){ - mavlink_msg_waypoint_ack_send( + mavlink_msg_mission_ack_send( chan, msg->sysid, msg->compid, @@ -1480,7 +1478,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mission_failed: // we are rejecting the mission/waypoint - mavlink_msg_waypoint_ack_send( + mavlink_msg_mission_ack_send( chan, msg->sysid, msg->compid, @@ -1878,7 +1876,7 @@ GCS_MAVLINK::queued_waypoint_send() { if (waypoint_receiving && waypoint_request_i <= (unsigned)g.command_total) { - mavlink_msg_waypoint_request_send( + mavlink_msg_mission_request_send( chan, waypoint_dest_sysid, waypoint_dest_compid, diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 5c5e6a952a..60c3e50964 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -119,7 +119,7 @@ static void handle_process_do_command() // system to control the vehicle attitude and the attitude of various // devices such as cameras. // |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| - case MAV_CMD_DO_SET_ROI: + case MAV_CMD_NAV_ROI: #if 0 // send the command to the camera mount camera_mount.set_roi_cmd(&command_nav_queue); diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 441eb61187..75ba37bec8 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -191,7 +191,7 @@ static void init_ardupilot() //mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id - mavlink_system.type = MAV_FIXED_WING; + mavlink_system.type = MAV_TYPE_FIXED_WING; rc_override_active = APM_RC.setHIL(rc_override); // Set initial values for no override diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.cpp b/libraries/GCS_MAVLink/GCS_MAVLink.cpp index efbb462cf2..f906724f06 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.cpp +++ b/libraries/GCS_MAVLink/GCS_MAVLink.cpp @@ -29,3 +29,21 @@ uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) // If it is addressed to our system ID we assume it is for us return 0; // no error } + +// return a MAVLink variable type given a AP_Param type +uint8_t mav_var_type(enum ap_var_type t) +{ + if (t == AP_PARAM_INT8) { + return MAVLINK_TYPE_INT8_T; + } + if (t == AP_PARAM_INT16) { + return MAVLINK_TYPE_INT16_T; + } + if (t == AP_PARAM_INT32) { + return MAVLINK_TYPE_INT32_T; + } + // treat any others as float + return MAVLINK_TYPE_FLOAT; +} + + diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h index aac11bca42..6efa46f471 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.h +++ b/libraries/GCS_MAVLink/GCS_MAVLink.h @@ -115,4 +115,7 @@ static inline int comm_get_txspace(mavlink_channel_t chan) uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid); +// return a MAVLink variable type given a AP_Param type +uint8_t mav_var_type(enum ap_var_type t); + #endif // GCS_MAVLink_h diff --git a/libraries/GCS_MAVLink/Mavlink_compat.h b/libraries/GCS_MAVLink/Mavlink_compat.h deleted file mode 100644 index e827e2b106..0000000000 --- a/libraries/GCS_MAVLink/Mavlink_compat.h +++ /dev/null @@ -1,73 +0,0 @@ -/* - compatibility header during transition to MAVLink 1.0 - */ - -// in MAVLink 1.0 'waypoint' becomes 'mission'. We can remove these -// mappings once we are not trying to support both protocols - -#define MAVLINK_MSG_ID_WAYPOINT_CURRENT MAVLINK_MSG_ID_MISSION_CURRENT -#define MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN MAVLINK_MSG_ID_MISSION_CURRENT_LEN -#define mavlink_msg_waypoint_current_send mavlink_msg_mission_current_send -#define mavlink_msg_waypoint_current_decode mavlink_msg_mission_current_decode - -#define MAVLINK_MSG_ID_WAYPOINT_COUNT MAVLINK_MSG_ID_MISSION_COUNT -#define MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN MAVLINK_MSG_ID_MISSION_COUNT_LEN -#define mavlink_msg_waypoint_count_send mavlink_msg_mission_count_send -#define mavlink_msg_waypoint_count_decode mavlink_msg_mission_count_decode -#define mavlink_waypoint_count_t mavlink_mission_count_t - -#define MAVLINK_MSG_ID_WAYPOINT_REQUEST MAVLINK_MSG_ID_MISSION_REQUEST -#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN MAVLINK_MSG_ID_MISSION_REQUEST_LEN -#define mavlink_msg_waypoint_request_send mavlink_msg_mission_request_send -#define mavlink_msg_waypoint_request_decode mavlink_msg_mission_request_decode -#define mavlink_waypoint_request_t mavlink_mission_request_t - -#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST MAVLINK_MSG_ID_MISSION_REQUEST_LIST -#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN -#define mavlink_msg_waypoint_request_list_send mavlink_msg_mission_request_list_send -#define mavlink_msg_waypoint_request_list_decode mavlink_msg_mission_request_list_decode -#define mavlink_waypoint_request_list_t mavlink_mission_request_list_t - -#define MAVLINK_MSG_ID_WAYPOINT MAVLINK_MSG_ID_MISSION_ITEM -#define MAVLINK_MSG_ID_WAYPOINT_LEN MAVLINK_MSG_ID_MISSION_ITEM_LEN -#define mavlink_msg_waypoint_send mavlink_msg_mission_item_send -#define mavlink_msg_waypoint_decode mavlink_msg_mission_item_decode -#define mavlink_waypoint_t mavlink_mission_item_t - -#define MAVLINK_MSG_ID_WAYPOINT_ACK MAVLINK_MSG_ID_MISSION_ACK -#define MAVLINK_MSG_ID_WAYPOINT_ACK_LEN MAVLINK_MSG_ID_MISSION_ACK_LEN -#define mavlink_msg_waypoint_ack_send mavlink_msg_mission_ack_send -#define mavlink_msg_waypoint_ack_decode mavlink_msg_mission_ack_decode -#define mavlink_waypoint_ack_t mavlink_mission_ack_t - -#define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL MAVLINK_MSG_ID_MISSION_CLEAR_ALL -#define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN -#define mavlink_msg_waypoint_clear_all_send mavlink_msg_mission_clear_all_send -#define mavlink_msg_waypoint_clear_all_decode mavlink_msg_mission_clear_all_decode -#define mavlink_waypoint_clear_all_t mavlink_mission_clear_all_t - -#define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT MAVLINK_MSG_ID_MISSION_SET_CURRENT -#define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN -#define mavlink_msg_waypoint_set_current_send mavlink_msg_mission_set_current_send -#define mavlink_msg_waypoint_set_current_decode mavlink_msg_mission_set_current_decode -#define mavlink_waypoint_set_current_t mavlink_mission_set_current_t - -#define MAV_CMD_DO_SET_ROI MAV_CMD_NAV_ROI - -static uint8_t mav_var_type(enum ap_var_type t) -{ - if (t == AP_PARAM_INT8) { - return MAVLINK_TYPE_INT8_T; - } - if (t == AP_PARAM_INT16) { - return MAVLINK_TYPE_INT16_T; - } - if (t == AP_PARAM_INT32) { - return MAVLINK_TYPE_INT32_T; - } - // treat any others as float - return MAVLINK_TYPE_FLOAT; -} - -#define MAV_FIXED_WING MAV_TYPE_FIXED_WING -