diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 8bcf27275d..2f67b9a6f4 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -699,20 +699,11 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_do_reposition(const mavlink_com return MAV_RESULT_DENIED; } - Location request_location {}; - request_location.lat = packet.x; - request_location.lng = packet.y; - - if (fabsf(packet.z) > LOCATION_ALT_MAX_M) { + Location requested_location {}; + if (!location_from_command_t(packet, requested_location)) { return MAV_RESULT_DENIED; } - Location::AltFrame frame; - if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.frame, frame)) { - return MAV_RESULT_DENIED; // failed as the location is not valid - } - request_location.set_alt_cm((int32_t)(packet.z * 100.0f), frame); - if (!rover.control_mode->in_guided_mode()) { if (!rover.set_mode(Mode::Number::GUIDED, ModeReason::GCS_COMMAND)) { return MAV_RESULT_FAILED; @@ -726,7 +717,7 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_do_reposition(const mavlink_com } // set the destination - if (!rover.mode_guided.set_desired_location(request_location)) { + if (!rover.mode_guided.set_desired_location(requested_location)) { return MAV_RESULT_FAILED; }