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

ArduPlane: create and use location_from_command_t

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;
}
Location requested_position {};
requested_position.lat = packet.x;
requested_position.lng = packet.y;
if (fabsf(packet.z) > LOCATION_ALT_MAX_M) {
Location requested_position;
if (!location_from_command_t(packet, requested_position)) {
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)) {
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)) {
return MAV_RESULT_FAILED;
}
Location new_home_loc {};
new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
Location new_home_loc;
if (!location_from_command_t(packet, MAV_FRAME_GLOBAL, new_home_loc)) {
return MAV_RESULT_DENIED;
}
if (!set_home(new_home_loc, true)) {
return MAV_RESULT_FAILED;
}