mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
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:
parent
3aa2f51465
commit
75af2d8cfb
@ -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_set_position_target_global_int_t pos_target;
|
||||||
mavlink_msg_set_position_target_global_int_decode(&msg, &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
|
// Unexpectedly, the mask is expecting "ones" for dimensions that should
|
||||||
// be IGNORNED rather than INCLUDED. See mavlink documentation of the
|
// be IGNORNED rather than INCLUDED. See mavlink documentation of the
|
||||||
// SET_POSITION_TARGET_GLOBAL_INT message, type_mask field.
|
// SET_POSITION_TARGET_GLOBAL_INT message, type_mask field.
|
||||||
const uint16_t alt_mask = 0b1111111111111011; // (z mask at bit 3)
|
const uint16_t alt_mask = 0b1111111111111011; // (z mask at bit 3)
|
||||||
|
|
||||||
bool msg_valid = true;
|
|
||||||
AP_Mission::Mission_Command cmd = {0};
|
AP_Mission::Mission_Command cmd = {0};
|
||||||
|
|
||||||
if (pos_target.type_mask & alt_mask)
|
if (pos_target.type_mask & alt_mask)
|
||||||
{
|
{
|
||||||
cmd.content.location.alt = pos_target.alt * 100;
|
const int32_t alt_cm = pos_target.alt * 100;
|
||||||
cmd.content.location.relative_alt = false;
|
cmd.content.location.set_alt_cm(alt_cm, frame);
|
||||||
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);
|
handle_change_alt_request(cmd);
|
||||||
}
|
}
|
||||||
} // end if alt_mask
|
|
||||||
}
|
}
|
||||||
|
|
||||||
MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlink_command_int_t &packet)
|
MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlink_command_int_t &packet)
|
||||||
|
Loading…
Reference in New Issue
Block a user