diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 45a596bc63..d2800c3676 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -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)