Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: create and use location_from_command_t

This commit is contained in:
Peter Barker 2022-02-03 14:58:21 +11:00 committed by Peter Barker
parent 7fd3402e88
commit 8df3c5a346
1 changed files with 3 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
Rover/GCS_Mavlink.cpp
View File

@ -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;
}