From 6723a0fc5533ef0564ffacc018be29e89fbc2174 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 3 Jun 2022 17:27:08 +0900
Subject: [PATCH] AP_Mount: handle gimbal_device_attitude_status

---
 libraries/AP_Mount/AP_Mount.cpp       | 13 +++++++++++++
 libraries/AP_Mount/AP_Mount.h         |  1 +
 libraries/AP_Mount/AP_Mount_Backend.h |  3 +++
 3 files changed, 17 insertions(+)

diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp
index 2d54be4716..2ab1264999 100644
--- a/libraries/AP_Mount/AP_Mount.cpp
+++ b/libraries/AP_Mount/AP_Mount.cpp
@@ -766,6 +766,9 @@ void AP_Mount::handle_message(mavlink_channel_t chan, const mavlink_message_t &m
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
         handle_global_position_int(msg);
         break;
+    case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS:
+        handle_gimbal_device_attitude_status(msg);
+        break;
     default:
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
         AP_HAL::panic("Unhandled mount case");
@@ -785,6 +788,16 @@ void AP_Mount::handle_param_value(const mavlink_message_t &msg)
 }
 
 
+// handle GIMBAL_DEVICE_ATTITUDE_STATUS message
+void AP_Mount::handle_gimbal_device_attitude_status(const mavlink_message_t &msg)
+{
+    for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != nullptr) {
+            _backends[instance]->handle_gimbal_device_attitude_status(msg);
+        }
+    }
+}
+
 // singleton instance
 AP_Mount *AP_Mount::_singleton;
 
diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h
index 47addfc144..bc2309c3cf 100644
--- a/libraries/AP_Mount/AP_Mount.h
+++ b/libraries/AP_Mount/AP_Mount.h
@@ -207,6 +207,7 @@ private:
     MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_long_t &packet);
     void handle_global_position_int(const mavlink_message_t &msg);
+    void handle_gimbal_device_attitude_status(const mavlink_message_t &msg);
 };
 
 namespace AP {
diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h
index a51500d605..b476fd2791 100644
--- a/libraries/AP_Mount/AP_Mount_Backend.h
+++ b/libraries/AP_Mount/AP_Mount_Backend.h
@@ -79,6 +79,9 @@ public:
     // handle a GLOBAL_POSITION_INT message
     bool handle_global_position_int(uint8_t msg_sysid, const mavlink_global_position_int_t &packet);
 
+    // handle GIMBAL_DEVICE_ATTITUDE_STATUS message
+    virtual void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) {}
+
 protected:
 
     // update_targets_from_rc - updates angle targets (i.e. _angle_ef_target_rad) using input from receiver