Add send_target_to_gimbal to AP_Mount_Backend

libraries/AP_Mount/AP_Mount_Alexmos.cpp

@@ -84,16 +84,7 @@ void AP_Mount_Alexmos::update()
     // update based on mount mode
     update_mnt_target();
 
-    // send target angles or rates depending on the target type
-    switch (mnt_target.target_type) {
-        case MountTargetType::RATE:
-            update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad);
-            FALLTHROUGH;
-        case MountTargetType::ANGLE:
-            // send latest angle targets to gimbal
-            control_axis(mnt_target.angle_rad);
-            break;
-    }
+    send_target_to_gimbal();
 }
 
 // has_pan_control - returns true if this mount can control its pan (required for multicopters)
@@ -155,7 +146,7 @@ void AP_Mount_Alexmos::get_boardinfo()
 /*
   control_axis : send new angle target to the gimbal at a fixed speed of 30 deg/s
 */
-void AP_Mount_Alexmos::control_axis(const MountTarget& angle_target_rad)
+void AP_Mount_Alexmos::send_target_angles(const MountAngleTarget& angle_target_rad)
 {
     alexmos_parameters outgoing_buffer;
     outgoing_buffer.angle_speed.mode = AP_MOUNT_ALEXMOS_MODE_ANGLE;

libraries/AP_Mount/AP_Mount_Alexmos.h

@@ -33,6 +33,11 @@ class AP_Mount_Alexmos : public AP_Mount_Backend
     // get attitude as a quaternion.  returns true on success
     bool get_attitude_quaternion(Quaternion& att_quat) override;
 
+    // servo only natively supports angles:
+    uint8_t natively_supported_mount_target_types() const override {
+        return NATIVE_ANGLES_ONLY;
+    };
+
 private:
 
     // get_angles -
@@ -45,7 +50,7 @@ class AP_Mount_Alexmos : public AP_Mount_Backend
     void get_boardinfo();
 
     // send new angles to the gimbal at a fixed speed of 30 deg/s
-    void control_axis(const MountTarget& angle_target_rad);
+    void send_target_angles(const MountAngleTarget& angle_rad) override;
 
     // read_params - read current profile profile_id and global parameters from the gimbal settings
     void read_params(uint8_t profile_id);

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -719,7 +719,7 @@ void AP_Mount_Backend::get_rc_input(float& roll_in, float& pitch_in, float& yaw_
 
 // get angle targets (in radians) to a Location
 // returns true on success, false on failure
-bool AP_Mount_Backend::get_angle_target_to_location(const Location &loc, MountTarget& angle_rad) const
+bool AP_Mount_Backend::get_angle_target_to_location(const Location &loc, MountAngleTarget& angle_rad) const
 {
     // exit immediately if vehicle's location is unavailable
     Location current_loc;
@@ -758,7 +758,7 @@ bool AP_Mount_Backend::get_angle_target_to_location(const Location &loc, MountTa
 
 // get angle targets (in radians) to ROI location
 // returns true on success, false on failure
-bool AP_Mount_Backend::get_angle_target_to_roi(MountTarget& angle_rad) const
+bool AP_Mount_Backend::get_angle_target_to_roi(MountAngleTarget& angle_rad) const
 {
     if (!_roi_target.initialised()) {
         return false;
@@ -767,7 +767,7 @@ bool AP_Mount_Backend::get_angle_target_to_roi(MountTarget& angle_rad) const
 }
 
 // return body-frame yaw angle from a mount target
-float AP_Mount_Backend::MountTarget::get_bf_yaw() const
+float AP_Mount_Backend::MountAngleTarget::get_bf_yaw() const
 {
     if (yaw_is_ef) {
         // convert to body-frame
@@ -779,7 +779,7 @@ float AP_Mount_Backend::MountTarget::get_bf_yaw() const
 }
 
 // return earth-frame yaw angle from a mount target
-float AP_Mount_Backend::MountTarget::get_ef_yaw() const
+float AP_Mount_Backend::MountAngleTarget::get_ef_yaw() const
 {
     if (yaw_is_ef) {
         // target is already earth-frame
@@ -791,7 +791,7 @@ float AP_Mount_Backend::MountTarget::get_ef_yaw() const
 }
 
 // sets roll, pitch, yaw and yaw_is_ef
-void AP_Mount_Backend::MountTarget::set(const Vector3f& rpy, bool yaw_is_ef_in)
+void AP_Mount_Backend::MountAngleTarget::set(const Vector3f& rpy, bool yaw_is_ef_in)
 {
     roll  = rpy.x;
     pitch = rpy.y;
@@ -802,7 +802,7 @@ void AP_Mount_Backend::MountTarget::set(const Vector3f& rpy, bool yaw_is_ef_in)
 // update angle targets using a given rate target
 // the resulting angle_rad yaw frame will match the rate_rad yaw frame
 // assumes a 50hz update rate
-void AP_Mount_Backend::update_angle_target_from_rate(const MountTarget& rate_rad, MountTarget& angle_rad) const
+void AP_Mount_Backend::update_angle_target_from_rate(const MountRateTarget& rate_rad, MountAngleTarget& angle_rad) const
 {
     // update roll and pitch angles and apply limits
     angle_rad.roll = constrain_float(angle_rad.roll + rate_rad.roll * AP_MOUNT_UPDATE_DT, radians(_params.roll_angle_min), radians(_params.roll_angle_max));
@@ -848,6 +848,10 @@ uint16_t AP_Mount_Backend::get_gimbal_device_flags() const
         case MountTargetType::ANGLE:
             yaw_lock_state = mnt_target.angle_rad.yaw_is_ef;
             break;
+        case MountTargetType::RETRACTED:
+        case MountTargetType::NEUTRAL:
+            yaw_lock_state = false;  // not locked onto the scenery
+            break;
         }
         break;
     case MAV_MOUNT_MODE_RC_TARGETING:
@@ -865,8 +869,8 @@ uint16_t AP_Mount_Backend::get_gimbal_device_flags() const
         break;
     }
 
-    const uint16_t flags = (get_mode() == MAV_MOUNT_MODE_RETRACT ? GIMBAL_DEVICE_FLAGS_RETRACT : 0) |
-                           (get_mode() == MAV_MOUNT_MODE_NEUTRAL ? GIMBAL_DEVICE_FLAGS_NEUTRAL : 0) |
+    const uint16_t flags = (mnt_target.target_type == MountTargetType::RETRACTED ? GIMBAL_DEVICE_FLAGS_RETRACT : 0) |
+                           (mnt_target.target_type == MountTargetType::NEUTRAL ? GIMBAL_DEVICE_FLAGS_NEUTRAL : 0) |
                            GIMBAL_DEVICE_FLAGS_ROLL_LOCK | // roll angle is always earth-frame
                            GIMBAL_DEVICE_FLAGS_PITCH_LOCK| // pitch angle is always earth-frame, yaw_angle is always body-frame
                            GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME | // yaw angle is always in vehicle-frame
@@ -876,7 +880,7 @@ uint16_t AP_Mount_Backend::get_gimbal_device_flags() const
 
 // get angle targets (in radians) to home location
 // returns true on success, false on failure
-bool AP_Mount_Backend::get_angle_target_to_home(MountTarget& angle_rad) const
+bool AP_Mount_Backend::get_angle_target_to_home(MountAngleTarget& angle_rad) const
 {
     // exit immediately if home is not set
     if (!AP::ahrs().home_is_set()) {
@@ -887,7 +891,7 @@ bool AP_Mount_Backend::get_angle_target_to_home(MountTarget& angle_rad) const
 
 // get angle targets (in radians) to a vehicle with sysid of  _target_sysid
 // returns true on success, false on failure
-bool AP_Mount_Backend::get_angle_target_to_sysid(MountTarget& angle_rad) const
+bool AP_Mount_Backend::get_angle_target_to_sysid(MountAngleTarget& angle_rad) const
 {
     // exit immediately if sysid is not set or no location available
     if (!_target_sysid_location.initialised()) {
@@ -913,13 +917,10 @@ void AP_Mount_Backend::update_mnt_target()
     mnt_target.fresh = false;
 
     switch (get_mode()) {
-    case MAV_MOUNT_MODE_RETRACT: {
+    case MAV_MOUNT_MODE_RETRACT:
         // move mount to a "retracted" position.  To-Do: remove support and replace with a relaxed mode?
-        const Vector3f &angle_bf_target = _params.retract_angles.get();
-        mnt_target.target_type = MountTargetType::ANGLE;
-        mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
+        mnt_target.target_type = MountTargetType::RETRACTED;
         return;
-    }
 
     case MAV_MOUNT_MODE_NEUTRAL: {
         // move mount to a neutral position, typically pointing forward
@@ -971,6 +972,65 @@ void AP_Mount_Backend::update_mnt_target()
     INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
 }
 
+void AP_Mount_Backend::send_target_to_gimbal()
+{
+    // the easy case, where the gimbal natively supports the MntTargetType:
+    if (natively_supports(mnt_target.target_type)) {
+        switch (mnt_target.target_type) {
+        case MountTargetType::ANGLE:
+            send_target_angles(mnt_target.angle_rad);
+            return;
+        case MountTargetType::RATE:
+            send_target_rates(mnt_target.rate_rads);
+            return;
+        case MountTargetType::RETRACTED:
+            send_target_retracted();
+            return;
+        case MountTargetType::NEUTRAL:
+            send_target_neutral();
+            return;
+        }
+        return;  // should not reach this as all cases return
+    }
+
+    // the more difficult case where we need to convert to something
+    // the gimbal understands:
+    switch (mnt_target.target_type) {
+    case MountTargetType::ANGLE:
+        // we don't know how to convert ANGLE to anything else.  Note
+        // that the Siyi backend *does* convert angles to rates, so we
+        // could potentially swipe code into here.
+        break;
+    case MountTargetType::RATE:
+        if (natively_supports(MountTargetType::ANGLE)) {
+            // we integrate the rates into the angle:
+            update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad);
+            send_target_angles(mnt_target.angle_rad);
+            return;
+        }
+        break;
+    case MountTargetType::RETRACTED: {
+        // just use the parameter values
+        // we update mnt_target for reporting purposes
+        const Vector3f &angle_bf_target = _params.retract_angles.get();
+        mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
+        send_target_angles(mnt_target.angle_rad);
+        break;
+    }
+    case MountTargetType::NEUTRAL: {
+        // just use the parameter values
+        // we update mnt_target for reporting purposes
+        const Vector3f &angle_bf_target = _params.neutral_angles.get();
+        mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
+        send_target_angles(mnt_target.angle_rad);
+        break;
+    }
+    }
+
+    send_warning_to_GCS("Failed to convert mount target to command gimbal");
+}
+
+
 // get target rate in deg/sec. returns true on success
 bool AP_Mount_Backend::get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame)
 {

libraries/AP_Mount/AP_Mount_Backend.h

@@ -228,12 +228,14 @@ class AP_Mount_Backend
 protected:
 
     enum class MountTargetType {
-        ANGLE,
-        RATE,
+        ANGLE     = 0,
+        RATE      = 1,
+        RETRACTED = 2,
+        NEUTRAL   = 3,
     };
 
     // class for a single angle or rate target
-    class MountTarget {
+    class MountAngleTarget {
     public:
         float roll;
         float pitch;
@@ -251,6 +253,45 @@ class AP_Mount_Backend
         // set roll, pitch, yaw and yaw_is_ef from Vector3f
         void set(const Vector3f& rpy, bool yaw_is_ef_in);
     };
+    class MountRateTarget {
+    public:
+        float roll;      // roll rate in radians/second
+        float pitch;     // roll rate in radians/second
+        float yaw;       // roll rate in radians/second
+        bool yaw_is_ef;  // if set then `yaw` is a rate in earth frame
+    };
+
+    // returns a bitmask of MountTargetTypes which this backend supports
+    // FIXME: make this pure-virtual
+    virtual uint8_t natively_supported_mount_target_types() const { return 0; };
+    // some static const masks to try to make the backends easier to read:
+    static constexpr uint8_t NATIVE_ANGLES_ONLY = (1U << uint8_t(MountTargetType::ANGLE));
+    static constexpr uint8_t NATIVE_RATES_ONLY = (1U << uint8_t(MountTargetType::RATE));
+    static constexpr uint8_t NATIVE_ANGLES_AND_RATES_ONLY = (
+        1U << uint8_t(MountTargetType::ANGLE) |
+        1U << uint8_t(MountTargetType::RATE)
+    );
+
+    // returns true if the backend natively supports type.  e.g. if
+    // "type" here is "MountTargetType::ANGLE" and the backend has
+    // indicated support of angles in the
+    // natively_supported_mount_target_types() bitmask then we can can
+    // send_angle_targets on the backend and have that work (meaning
+    // the backend has overrriden that method).
+    bool natively_supports(MountTargetType type) const {
+        uint8_t supported = natively_supported_mount_target_types();
+        return (supported & (1U<<(uint8_t(type)))) != 0;
+    }
+
+    // looks at what MountTargetTypes this mount supports, may convert
+    // to supported mount target type from unsupported
+    void send_target_to_gimbal();
+
+    // FIXME: make it an internal error for these to ever be called:
+    virtual void send_target_angles(const MountAngleTarget &angle_rad) { }
+    virtual void send_target_rates(const MountRateTarget &rate_rads) { }
+    virtual void send_target_retracted() { }
+    virtual void send_target_neutral() { }
 
     // options parameter bitmask handling
     enum class Options : uint8_t {
@@ -289,20 +330,20 @@ class AP_Mount_Backend
 
     // get angle targets (in radians) to ROI location
     // returns true on success, false on failure
-    bool get_angle_target_to_roi(MountTarget& angle_rad) const WARN_IF_UNUSED;
+    bool get_angle_target_to_roi(MountAngleTarget& angle_rad) const WARN_IF_UNUSED;
 
     // get angle targets (in radians) to home location
     // returns true on success, false on failure
-    bool get_angle_target_to_home(MountTarget& angle_rad) const WARN_IF_UNUSED;
+    bool get_angle_target_to_home(MountAngleTarget& angle_rad) const WARN_IF_UNUSED;
 
     // get angle targets (in radians) to a vehicle with sysid of _target_sysid
     // returns true on success, false on failure
-    bool get_angle_target_to_sysid(MountTarget& angle_rad) const WARN_IF_UNUSED;
+    bool get_angle_target_to_sysid(MountAngleTarget& angle_rad) const WARN_IF_UNUSED;
 
     // update angle targets using a given rate target
     // the resulting angle_rad yaw frame will match the rate_rad yaw frame
     // assumes a 50hz update rate
-    void update_angle_target_from_rate(const MountTarget& rate_rad, MountTarget& angle_rad) const;
+    void update_angle_target_from_rate(const MountRateTarget& rate_rad, MountAngleTarget& angle_rad) const;
 
     // helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
     uint16_t get_gimbal_device_flags() const;
@@ -319,8 +360,8 @@ class AP_Mount_Backend
     // structure for MAVLink Targeting angle and rate targets
     struct {
         MountTargetType target_type;// MAVLink targeting mode's current target type (e.g. angle or rate)
-        MountTarget angle_rad;      // angle target in radians
-        MountTarget rate_rads;      // rate target in rad/s
+        MountAngleTarget angle_rad; // angle target in radians
+        MountRateTarget rate_rads;  // rate target in rad/s
         uint32_t last_rate_request_ms;
 
         // 'fresh' indicates that the MountTarget data in this
@@ -341,7 +382,7 @@ class AP_Mount_Backend
 
     // get angle targets (in radians) to a Location
     // returns true on success, false on failure
-    bool get_angle_target_to_location(const Location &loc, MountTarget& angle_rad) const WARN_IF_UNUSED;
+    bool get_angle_target_to_location(const Location &loc, MountAngleTarget& angle_rad) const WARN_IF_UNUSED;
 
 #if AP_MOUNT_POI_TO_LATLONALT_ENABLED
     // calculate the Location that the gimbal is pointing at

libraries/AP_Mount/AP_Mount_CADDX.cpp

@@ -44,7 +44,7 @@ bool AP_Mount_CADDX::get_attitude_quaternion(Quaternion& att_quat)
 }
 
 // send_target_angles
-void AP_Mount_CADDX::send_target_angles(const MountTarget& angle_target_rad)
+void AP_Mount_CADDX::send_target_angles(const MountAngleTarget& angle_target_rad)
 {
     // exit immediately if not initialised
     if (!_initialised) {

libraries/AP_Mount/AP_Mount_CADDX.h

@@ -44,7 +44,7 @@ class AP_Mount_CADDX : public AP_Mount_Backend_Serial
     };
 
     // send_target_angles
-    void send_target_angles(const MountTarget& angle_target_rad);
+    void send_target_angles(const MountAngleTarget& angle_target_rad) override;
 
     // internal variables
     uint32_t _last_send_ms;     // system time of last do_mount_control sent to gimbal