Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: create and use location_from_command_t

Browse Source
This commit is contained in:
Peter Barker 2022-02-03 13:52:29 +11:00 committed by Peter Barker
parent dffec9325e
commit 10be59da17
1 changed files with 6 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduPlane/GCS_Mavlink.cpp
View File

@ -670,18 +670,10 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
return MAV_RESULT_DENIED; return MAV_RESULT_DENIED;
} }
Location requested_position {}; Location requested_position;
requested_position.lat = packet.x; if (!location_from_command_t(packet, requested_position)) {
requested_position.lng = packet.y;
if (fabsf(packet.z) > LOCATION_ALT_MAX_M) {
return MAV_RESULT_DENIED; 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
}
requested_position.set_alt_cm((int32_t)(packet.z * 100.0f), frame);
if (is_zero(packet.param4)) { if (is_zero(packet.param4)) {
requested_position.loiter_ccw = 0; requested_position.loiter_ccw = 0;
@ -997,10 +989,10 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
if (!is_zero(packet.param1)) { if (!is_zero(packet.param1)) {
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }
Location new_home_loc {}; Location new_home_loc;
new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f); if (!location_from_command_t(packet, MAV_FRAME_GLOBAL, new_home_loc)) {
new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f); return MAV_RESULT_DENIED;
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); }
if (!set_home(new_home_loc, true)) { if (!set_home(new_home_loc, true)) {
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }