Skip to content

Commit

Permalink
ArduPlane: use frame instead of bools for setting alt frame
Browse files Browse the repository at this point in the history
Signed-off-by: Ryan Friedman <[email protected]>
  • Loading branch information
Ryanf55 committed Oct 28, 2024
1 parent 92693e0 commit d4bbf2f
Showing 1 changed file with 4 additions and 6 deletions.
10 changes: 4 additions & 6 deletions ArduPlane/GCS_Mavlink.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1382,22 +1382,20 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess

if (pos_target.type_mask & alt_mask)
{
cmd.content.location.alt = pos_target.alt * 100;
cmd.content.location.relative_alt = false;
cmd.content.location.terrain_alt = false;
const int32_t alt_cm = pos_target.alt * 100;
switch (pos_target.coordinate_frame)
{
case MAV_FRAME_GLOBAL:
case MAV_FRAME_GLOBAL_INT:
cmd.content.location.set_alt_cm(alt_cm, Location::AltFrame::ABSOLUTE);
break; //default to MSL altitude
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
cmd.content.location.relative_alt = true;
cmd.content.location.set_alt_cm(alt_cm, Location::AltFrame::ABOVE_HOME);
break;
case MAV_FRAME_GLOBAL_TERRAIN_ALT:
case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
cmd.content.location.relative_alt = true;
cmd.content.location.terrain_alt = true;
cmd.content.location.set_alt_cm(alt_cm, Location::AltFrame::ABOVE_TERRAIN);
break;
default:
gcs().send_text(MAV_SEVERITY_WARNING, "Invalid coord frame in SET_POSTION_TARGET_GLOBAL_INT");
Expand Down

0 comments on commit d4bbf2f

Please sign in to comment.