Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Plane Guided Alt Command: Use frame instead of bools for setting alt frame #28479

Merged
merged 1 commit into from
Nov 5, 2024
Merged

Plane Guided Alt Command: Use frame instead of bools for setting alt frame #28479

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
40 changes: 12 additions & 28 deletions ArduPlane/GCS_Mavlink.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
Loading