diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 92c863a286..26fc6ed74c 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -308,6 +308,7 @@ public: void send_gimbal_report() const; void send_home_position() const; void send_gps_global_origin() const; + virtual void send_attitude_target() {}; virtual void send_position_target_global_int() { }; virtual void send_position_target_local_ned() { }; void send_servo_output_raw(); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index fd691cc071..7a4bd62a3e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -886,6 +886,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_VIBRATION, MSG_VIBRATION}, { MAVLINK_MSG_ID_RPM, MSG_RPM}, { MAVLINK_MSG_ID_MISSION_ITEM_REACHED, MSG_MISSION_ITEM_REACHED}, + { MAVLINK_MSG_ID_ATTITUDE_TARGET, MSG_ATTITUDE_TARGET}, { MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, MSG_POSITION_TARGET_GLOBAL_INT}, { MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, MSG_POSITION_TARGET_LOCAL_NED}, { MAVLINK_MSG_ID_ADSB_VEHICLE, MSG_ADSB_VEHICLE}, @@ -5200,6 +5201,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) #endif break; + case MSG_ATTITUDE_TARGET: + CHECK_PAYLOAD_SIZE(ATTITUDE_TARGET); + send_attitude_target(); + break; + case MSG_POSITION_TARGET_GLOBAL_INT: CHECK_PAYLOAD_SIZE(POSITION_TARGET_GLOBAL_INT); send_position_target_global_int(); diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index a38c24b1ff..7e314fe69e 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -81,5 +81,6 @@ enum ap_message : uint8_t { MSG_AIS_VESSEL, MSG_MCU_STATUS, MSG_UAVIONIX_ADSB_OUT_STATUS, + MSG_ATTITUDE_TARGET, MSG_LAST // MSG_LAST must be the last entry in this enum };