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

ArduPlane: use frame instead of bools for setting alt frame 

* And switch to mavlink_coordinate_frame_to_location_alt_frame

Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
This commit is contained in:
Ryan Friedman 2024-10-28 10:51:16 +09:00 committed by Andrew Tridgell
parent 3aa2f51465
commit 75af2d8cfb
1 changed files with 12 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
ArduPlane/GCS_Mavlink.cpp
View File

@ -1372,43 +1372,27 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess
mavlink_set_position_target_global_int_t pos_target;
mavlink_msg_set_position_target_global_int_decode(&msg, &pos_target);
Location::AltFrame frame;
if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)pos_target.coordinate_frame, frame)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Invalid coord frame in SET_POSTION_TARGET_GLOBAL_INT");
// Even though other parts of the command may be valid, reject the whole thing.
return;
}
// Unexpectedly, the mask is expecting "ones" for dimensions that should
// be IGNORNED rather than INCLUDED. See mavlink documentation of the
// SET_POSITION_TARGET_GLOBAL_INT message, type_mask field.
const uint16_t alt_mask = 0b1111111111111011; // (z mask at bit 3)
bool msg_valid = true;
AP_Mission::Mission_Command cmd = {0};
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;
switch (pos_target.coordinate_frame)
{
case MAV_FRAME_GLOBAL:
case MAV_FRAME_GLOBAL_INT:
break; //default to MSL altitude
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
cmd.content.location.relative_alt = true;
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;
break;
default:
gcs().send_text(MAV_SEVERITY_WARNING, "Invalid coord frame in SET_POSTION_TARGET_GLOBAL_INT");
msg_valid = false;
break;
}
if (msg_valid) {
handle_change_alt_request(cmd);
}
} // end if alt_mask
const int32_t alt_cm = pos_target.alt * 100;
cmd.content.location.set_alt_cm(alt_cm, frame);
handle_change_alt_request(cmd);
}
}
MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlink_command_int_t &packet)