mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
ArduPlane: create and use location_from_command_t
This commit is contained in:
parent
dffec9325e
commit
10be59da17
@ -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;
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user