diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index cb2159b62c..cff03b5004 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -284,6 +284,9 @@ protected: virtual bool in_hil_mode() const { return false; } + bool mavlink_coordinate_frame_to_location_alt_frame(uint8_t coordinate_frame, + Location::ALT_FRAME &frame); + // overridable method to check for packet acceptance. Allows for // enforcement of GCS sysid bool accept_packet(const mavlink_status_t &status, mavlink_message_t &msg); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 3fdb4c8a4f..e606cbe559 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4314,6 +4314,28 @@ void GCS::passthru_timer(void) } } +bool GCS_MAVLINK::mavlink_coordinate_frame_to_location_alt_frame(const uint8_t coordinate_frame, Location::ALT_FRAME &frame) +{ + switch (coordinate_frame) { + case MAV_FRAME_GLOBAL_RELATIVE_ALT: // solo shot manager incorrectly sends RELATIVE_ALT instead of RELATIVE_ALT_INT + case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + frame = Location::ALT_FRAME_ABOVE_HOME; + return true; + case MAV_FRAME_GLOBAL_TERRAIN_ALT: + case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: + frame = Location::ALT_FRAME_ABOVE_TERRAIN; + return true; + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_INT: + frame = Location::ALT_FRAME_ABSOLUTE; + return true; + default: +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + gcs().send_text(MAV_SEVERITY_INFO, "Unknown mavlink coordinate frame %u", coordinate_frame); +#endif + return false; + } +} GCS &gcs() {