From 402f3ec09c41c873d3887a412e383334fdfe463b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 4 Nov 2019 14:50:32 +1100 Subject: [PATCH] GCS_MAVLink: honour coordinate altitude frames for DO_SET_ROI Also use {} to construct the location. Note the assumption that the frame is relative-to-home when we receive the command via COMMAND (as opposed to COMMAND_LONG) Note that this is a behavioural change as --- libraries/GCS_MAVLink/GCS_Common.cpp | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 384af57bc1..bc45248c88 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3850,10 +3850,17 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const mavlink_command_int_t &p // x : lat // y : lon // z : alt - Location roi_loc; - roi_loc.lat = packet.x; - roi_loc.lng = packet.y; - roi_loc.alt = (int32_t)(packet.z * 100.0f); + Location::AltFrame frame; + if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.frame, frame)) { + // unknown coordinate frame + return MAV_RESULT_UNSUPPORTED; + } + const Location roi_loc { + packet.x, + packet.y, + (int32_t)(packet.z * 100.0f), + frame + }; return handle_command_do_set_roi(roi_loc); } @@ -3865,10 +3872,12 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const mavlink_command_long_t & // off support for MAV_CMD_DO_SET_ROI_LOCATION (which doesn't // support the extra fields). - Location roi_loc; - roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); - roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); - roi_loc.alt = (int32_t)(packet.param7 * 100.0f); + const Location roi_loc { + (int32_t)(packet.param5 * 1.0e7f), + (int32_t)(packet.param6 * 1.0e7f), + (int32_t)(packet.param7 * 100.0f), + Location::AltFrame::ABOVE_HOME + }; return handle_command_do_set_roi(roi_loc); }