From 59926f99b952f72e292381bbbed908f557374593 Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Wed, 10 Feb 2016 16:26:43 -0800 Subject: [PATCH] Sub: Add solo shot message fixes --- ArduSub/GCS_Mavlink.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 94b74bfcd8..8545328f73 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -1173,7 +1173,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) roi_loc.lat = packet.x; roi_loc.lng = packet.y; roi_loc.alt = (int32_t)(packet.z * 100.0f); - copter.set_auto_yaw_roi(roi_loc); + sub.set_auto_yaw_roi(roi_loc); result = MAV_RESULT_ACCEPTED; break; } @@ -1724,11 +1724,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } // check for supported coordinate frames - if (packet.coordinate_frame != MAV_FRAME_GLOBAL_INT && - packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT && - packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) { - break; - } + if (packet.coordinate_frame != MAV_FRAME_GLOBAL_INT && + packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && // solo shot manager incorrectly sends RELATIVE_ALT instead of RELATIVE_ALT_INT + packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT && + packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) { + break; + } bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; @@ -1749,6 +1750,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) loc.lng = packet.lon_int; loc.alt = packet.alt*100; switch (packet.coordinate_frame) { + case MAV_FRAME_GLOBAL_RELATIVE_ALT: // solo shot manager incorrectly sends RELATIVE_ALT instead of RELATIVE_ALT_INT case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: loc.flags.relative_alt = true; loc.flags.terrain_alt = false;