aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: use Location to change alt frames

Browse Source
This commit is contained in:
Peter Barker 2019-02-16 10:48:32 +11:00 committed by Andrew Tridgell
parent 97792d1774
commit 138d93702a
1 changed files with 9 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/GCS_MAVLink/GCS_Common.cpp
View File

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