AP_Mount: add STorM32 mavlink v2 driver

libraries/AP_Mount/AP_Mount.cpp

@@ -15,6 +15,7 @@
 #include "AP_Mount_Scripting.h"
 #include "AP_Mount_Xacti.h"
 #include "AP_Mount_Viewpro.h"
+#include "AP_Mount_STorM32_MAVLink.h"
 #include <stdio.h>
 #include <AP_Math/location.h>
 #include <SRV_Channel/SRV_Channel.h>
@@ -149,6 +150,14 @@ void AP_Mount::init()
             _num_instances++;
             break;
 #endif // HAL_MOUNT_VIEWPRO_ENABLED
+
+#if HAL_MOUNT_STORM32_MAVLINK_V2_ENABLED
+        // check for STorM32_MAVLink mounts using MAVLink protocol
+        case Type::STorM32_MAVLink:
+            _backends[instance] = new AP_Mount_STorM32_MAVLink(*this, _params[instance], instance);
+            _num_instances++;
+            break;
+#endif // HAL_MOUNT_STORM32_MAVLINK_V2_ENABLED
         }
 
         // init new instance
@@ -329,13 +338,7 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com
 
     // check flags for change to RETRACT
     const uint32_t flags = packet.x;
-    if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
-        backend->set_mode(MAV_MOUNT_MODE_RETRACT);
-        return MAV_RESULT_ACCEPTED;
-    }
-    // check flags for change to NEUTRAL
-    if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
-        backend->set_mode(MAV_MOUNT_MODE_NEUTRAL);
+    if (!backend->handle_gimbal_manager_flags(flags)) {
         return MAV_RESULT_ACCEPTED;
     }
 
@@ -401,14 +404,7 @@ void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg)
 
     // check flags for change to RETRACT
     const uint32_t flags = packet.flags;
-    if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
-        backend->set_mode(MAV_MOUNT_MODE_RETRACT);
-        return;
-    }
-
-    // check flags for change to NEUTRAL
-    if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
-        backend->set_mode(MAV_MOUNT_MODE_NEUTRAL);
+    if (!backend->handle_gimbal_manager_flags(flags)) {
         return;
     }
 
@@ -467,13 +463,7 @@ void AP_Mount::handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg)
 
     // check flags for change to RETRACT
     uint32_t flags = (uint32_t)packet.flags;
-    if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
-        backend->set_mode(MAV_MOUNT_MODE_RETRACT);
-        return;
-    }
-    // check flags for change to NEUTRAL
-    if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
-        backend->set_mode(MAV_MOUNT_MODE_NEUTRAL);
+    if (!backend->handle_gimbal_manager_flags(flags)) {
         return;
     }
 
@@ -523,6 +513,15 @@ MAV_RESULT AP_Mount::handle_command(const mavlink_command_int_t &packet, const m
     }
 }
 
+uint8_t AP_Mount::get_gimbal_device_id(uint8_t instance) const
+{
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return 0;
+    }
+    return backend->get_gimbal_device_id();
+}
+
 /// Change the configuration of the mount
 void AP_Mount::handle_global_position_int(const mavlink_message_t &msg)
 {
@@ -895,6 +894,8 @@ void AP_Mount::handle_gimbal_report(mavlink_channel_t chan, const mavlink_messag
 
 void AP_Mount::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg)
 {
+    handle_message_extra(chan, msg);
+
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GIMBAL_REPORT:
         handle_gimbal_report(chan, msg);
@@ -1065,4 +1066,22 @@ AP_Mount *mount()
 
 };
 
+void AP_Mount::handle_message_extra(mavlink_channel_t chan, const mavlink_message_t &msg)
+{
+    for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != nullptr) {
+            _backends[instance]->handle_message_extra(msg);
+        }
+    }
+}
+
+void AP_Mount::send_banner()
+{
+    for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != nullptr) {
+            _backends[instance]->send_banner();
+        }
+    }
+}
+
 #endif /* HAL_MOUNT_ENABLED */

libraries/AP_Mount/AP_Mount.h

@@ -47,6 +47,7 @@ class AP_Mount_Siyi;
 class AP_Mount_Scripting;
 class AP_Mount_Xacti;
 class AP_Mount_Viewpro;
+class BP_Mount_STorM32_MAVLink;
 
 /*
   This is a workaround to allow the MAVLink backend access to the
@@ -67,6 +68,7 @@ class AP_Mount
     friend class AP_Mount_Scripting;
     friend class AP_Mount_Xacti;
     friend class AP_Mount_Viewpro;
+    friend class BP_Mount_STorM32_MAVLink;
 
 public:
     AP_Mount();
@@ -114,6 +116,9 @@ class AP_Mount
 #endif
 #if HAL_MOUNT_VIEWPRO_ENABLED
         Viewpro = 11,        /// Viewpro gimbal using a custom serial protocol
+#endif
+#if HAL_MOUNT_STORM32_MAVLINK_V2_ENABLED
+        STorM32_MAVLink = 12  /// STorM32 mount using MAVLink protocol v1 & v2
 #endif
     };
 
@@ -269,6 +274,15 @@ class AP_Mount
     // parameter var table
     static const struct AP_Param::GroupInfo        var_info[];
 
+    // this links into handle_message() to catch all messages
+    void handle_message_extra(mavlink_channel_t chan, const mavlink_message_t &msg);
+
+    // returns the gimbal device id for this instance, or 0 if the instance is not available
+    uint8_t get_gimbal_device_id(uint8_t instance) const;
+
+    // send banner
+    void send_banner();
+
 protected:
 
     static AP_Mount *_singleton;

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -180,7 +180,7 @@ void AP_Mount_Backend::send_gimbal_device_attitude_status(mavlink_channel_t chan
                                                    0,                                           // failure flags (not supported)
                                                    std::numeric_limits<double>::quiet_NaN(),    // delta_yaw (NaN for unknonw)
                                                    std::numeric_limits<double>::quiet_NaN(),    // delta_yaw_velocity (NaN for unknonw)
-                                                   _instance + 1);  // gimbal_device_id
+                                                   get_gimbal_device_id());  // gimbal_device_id
 }
 #endif
 
@@ -223,7 +223,7 @@ void AP_Mount_Backend::send_gimbal_manager_information(mavlink_channel_t chan)
     mavlink_msg_gimbal_manager_information_send(chan,
                                                 AP_HAL::millis(),                       // autopilot system time
                                                 get_gimbal_manager_capability_flags(),  // bitmap of gimbal manager capability flags
-                                                _instance + 1,                          // gimbal device id
+                                                get_gimbal_device_id(),                 // gimbal device id
                                                 radians(_params.roll_angle_min),        // roll_min in radians
                                                 radians(_params.roll_angle_max),        // roll_max in radians
                                                 radians(_params.pitch_angle_min),       // pitch_min in radians
@@ -232,19 +232,44 @@ void AP_Mount_Backend::send_gimbal_manager_information(mavlink_channel_t chan)
                                                 radians(_params.yaw_angle_max));        // yaw_max in radians
 }
 
-// send a GIMBAL_MANAGER_STATUS message to GCS
-void AP_Mount_Backend::send_gimbal_manager_status(mavlink_channel_t chan)
+uint8_t AP_Mount_Backend::get_gimbal_device_id() const
 {
-    uint32_t flags = GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK;
+    return _instance + 1;
+}
 
+// return gimbal manager flags used by GIMBAL_MANAGER_STATUS message
+uint32_t AP_Mount_Backend::get_gimbal_manager_flags() const
+{
+    uint32_t flags = GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK;
     if (_yaw_lock) {
         flags |= GIMBAL_MANAGER_FLAGS_YAW_LOCK;
     }
+    return flags;
+}
+
+// set gimbal manager flags, called from frontend's gimbal manager handlers
+bool AP_Mount_Backend::handle_gimbal_manager_flags(uint32_t flags)
+{
+    // check flags for change to RETRACT
+    if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
+        set_mode(MAV_MOUNT_MODE_RETRACT);
+        return false;
+    } else
+    // check flags for change to NEUTRAL
+    if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
+        set_mode(MAV_MOUNT_MODE_NEUTRAL);
+        return false;
+    }
+    return true;
+}
 
+// send a GIMBAL_MANAGER_STATUS message to GCS
+void AP_Mount_Backend::send_gimbal_manager_status(mavlink_channel_t chan)
+{
     mavlink_msg_gimbal_manager_status_send(chan,
                                            AP_HAL::millis(),    // autopilot system time
-                                           flags,               // bitmap of gimbal manager flags
-                                           _instance + 1,       // gimbal device id
+                                           get_gimbal_manager_flags(),  // bitmap of gimbal manager flags
+                                           get_gimbal_device_id(),      // gimbal device id
                                            mavlink_control_id.sysid,    // primary control system id
                                            mavlink_control_id.compid,   // primary control component id
                                            0,                           // secondary control system id

libraries/AP_Mount/AP_Mount_Backend.h

@@ -81,7 +81,7 @@ class AP_Mount_Backend
 
     // set yaw_lock.  If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North)
     // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle
-    void set_yaw_lock(bool yaw_lock) { _yaw_lock = yaw_lock; }
+    virtual void set_yaw_lock(bool yaw_lock) { _yaw_lock = yaw_lock; }
 
     // set angle target in degrees
     // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
@@ -117,13 +117,13 @@ class AP_Mount_Backend
 #endif
 
     // send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
-    void send_gimbal_device_attitude_status(mavlink_channel_t chan);
+    virtual void send_gimbal_device_attitude_status(mavlink_channel_t chan);
 
     // return gimbal capabilities sent to GCS in the GIMBAL_MANAGER_INFORMATION
     virtual uint32_t get_gimbal_manager_capability_flags() const;
 
     // send a GIMBAL_MANAGER_INFORMATION message to GCS
-    void send_gimbal_manager_information(mavlink_channel_t chan);
+    virtual void send_gimbal_manager_information(mavlink_channel_t chan);
 
     // send a GIMBAL_MANAGER_STATUS message to GCS
     void send_gimbal_manager_status(mavlink_channel_t chan);
@@ -196,6 +196,22 @@ class AP_Mount_Backend
     bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc);
 #endif
 
+    // handle msg - allows to process a msg from a gimbal
+    virtual void handle_message_extra(const mavlink_message_t &msg) {}
+
+    // send banner
+    virtual void send_banner() {}
+
+    // return gimbal device id used by GIMBAL_MANAGER_STATUS message, and other places
+    virtual uint8_t get_gimbal_device_id() const;
+
+    // return gimbal manager flags used by GIMBAL_MANAGER_STATUS message
+    virtual uint32_t get_gimbal_manager_flags() const;
+
+    // handle gimbal manager flags received from gimbal manager messages
+    // Return false to abort angle/rate processing.
+    virtual bool handle_gimbal_manager_flags(uint32_t flags);
+
     //
     // rangefinder
     //

libraries/AP_Mount/AP_Mount_Params.cpp

@@ -9,7 +9,7 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: Mount Type
     // @Description: Mount Type
-    // @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:Gremsy, 7:BrushlessPWM, 8:Siyi, 9:Scripting, 10:Xacti, 11:Viewpro
+    // @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:Gremsy, 7:BrushlessPWM, 8:Siyi, 9:Scripting, 10:Xacti, 11:Viewpro, 12:4:STorM32 MAVLink V2
     // @RebootRequired: True
     // @User: Standard
     AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Mount_Params, type, 0, AP_PARAM_FLAG_ENABLE),