AP_Mount: handle gimbal_device_attitude_status

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;
 

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 {

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