Browse Source

GCS_MAVLink: use Location to change alt frames

mission-4.1.18
Peter Barker 6 years ago committed by Andrew Tridgell
parent
commit
138d93702a
  1. 27
      libraries/GCS_MAVLink/GCS_Common.cpp

27
libraries/GCS_MAVLink/GCS_Common.cpp

@ -3633,26 +3633,17 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_do_set_home(const mavlink_command_int
if (!is_zero(packet.param1)) { if (!is_zero(packet.param1)) {
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }
// check frame type is supported Location::ALT_FRAME frame;
if (packet.frame != MAV_FRAME_GLOBAL && if (!mavlink_coordinate_frame_to_location_alt_frame(packet.frame, frame)) {
packet.frame != MAV_FRAME_GLOBAL_INT && // unknown coordinate frame
packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
return MAV_RESULT_UNSUPPORTED; return MAV_RESULT_UNSUPPORTED;
} }
Location new_home_loc {}; const Location new_home_loc{
new_home_loc.lat = packet.x; packet.x,
new_home_loc.lng = packet.y; packet.y,
new_home_loc.alt = packet.z * 100; int32_t(packet.z * 100),
// handle relative altitude frame,
if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || };
packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
if (!AP::ahrs().home_is_set()) {
// cannot use relative altitude if home is not set
return MAV_RESULT_FAILED;
}
new_home_loc.alt += AP::ahrs().get_home().alt;
}
if (!set_home(new_home_loc, true)) { if (!set_home(new_home_loc, true)) {
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }

Loading…
Cancel
Save