AP_Mount: add CADDX gimbal

libraries/AP_Mount/AP_Mount.cpp

@@ -16,6 +16,7 @@
 #include "AP_Mount_Xacti.h"
 #include "AP_Mount_Viewpro.h"
 #include "AP_Mount_Topotek.h"
+#include "AP_Mount_CADDX.h"
 #include <stdio.h>
 #include <AP_Math/location.h>
 #include <SRV_Channel/SRV_Channel.h>
@@ -166,6 +167,15 @@ void AP_Mount::init()
             serial_instance++;
             break;
 #endif // HAL_MOUNT_TOPOTEK_ENABLED
+
+#if HAL_MOUNT_CADDX_ENABLED
+        // check for CADDX gimbal
+        case Type::CADDX:
+            _backends[instance] = NEW_NOTHROW AP_Mount_CADDX(*this, _params[instance], instance, serial_instance);
+            _num_instances++;
+            serial_instance++;
+            break;
+#endif // HAL_MOUNT_CADDX_ENABLED
         }
 
         // init new instance
@@ -280,6 +290,18 @@ void AP_Mount::set_yaw_lock(uint8_t instance, bool yaw_lock)
     backend->set_yaw_lock(yaw_lock);
 }
 
+// set mount operating mode
+void AP_Mount::set_mount_mode( uint8_t mount_mode)
+{
+    auto *backend = get_instance(_primary);
+    if (backend == nullptr) {
+        return;
+    }
+
+    // call backend's set_yaw_lock
+    backend->set_mount_mode(mount_mode);
+}
+
 // set angle target in degrees
 // roll and pitch are in earth-frame
 // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame

libraries/AP_Mount/AP_Mount.h

@@ -48,6 +48,7 @@ class AP_Mount_Scripting;
 class AP_Mount_Xacti;
 class AP_Mount_Viewpro;
 class AP_Mount_Topotek;
+class AP_Mount_CADDX;
 
 /*
   This is a workaround to allow the MAVLink backend access to the
@@ -69,6 +70,7 @@ class AP_Mount
     friend class AP_Mount_Xacti;
     friend class AP_Mount_Viewpro;
     friend class AP_Mount_Topotek;
+    friend class AP_Mount_CADDX;
 
 public:
     AP_Mount();
@@ -119,6 +121,9 @@ class AP_Mount
 #endif
 #if HAL_MOUNT_TOPOTEK_ENABLED
         Topotek = 12,        /// Topotek gimbal using a custom serial protocol
+#endif
+#if HAL_MOUNT_CADDX_ENABLED
+        CADDX = 13,        /// CADDX gimbal using a custom serial protocol
 #endif
     };
 
@@ -155,6 +160,9 @@ class AP_Mount
     //      this operation requires 60us on a Pixhawk/PX4
     void set_mode_to_default() { set_mode_to_default(_primary); }
     void set_mode_to_default(uint8_t instance);
+
+    //set operating mode, not to be confused with modes like retracted,usually axis locking modes
+    void set_mount_mode(uint8_t mount_lock_mode);   
 
     // set yaw_lock used in RC_TARGETING mode.  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

libraries/AP_Mount/AP_Mount_Backend.h

@@ -82,6 +82,10 @@ class AP_Mount_Backend
     // set yaw_lock used in RC_TARGETING mode.  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; }
+
+    //set operating mode, not to be confused with modes like retracted,usually axis locking modes
+    void set_mount_mode( uint8_t mount_lock_mode) { _mount_lock_mode = mount_lock_mode; } ;   
+    uint8_t _mount_lock_mode;
 
     // set angle target in degrees
     // roll and pitch are in earth-frame

libraries/AP_Mount/AP_Mount_CADDX.cpp

@@ -0,0 +1,176 @@
+#include "AP_Mount_CADDX.h"
+#include <GCS_MAVLink/GCS.h>
+#if HAL_MOUNT_CADDX_ENABLED
+#include <AP_HAL/AP_HAL.h>
+
+// update mount position - should be called periodically
+void AP_Mount_CADDX::update()
+{
+    // exit immediately if not initialised
+    if (!_initialised) {
+        return;
+    }
+
+     // change to RC_TARGETING mode if RC input has changed
+    set_rctargeting_on_rcinput_change();
+
+    // flag to trigger sending target angles to gimbal
+    bool resend_now = false;
+
+    // update based on mount mode
+    switch(get_mode()) {
+        // move mount to a "retracted" position.  To-Do: remove support and replace with a relaxed mode?
+        case MAV_MOUNT_MODE_RETRACT: {
+            const Vector3f &target = _params.retract_angles.get();
+            mnt_target.angle_rad.set(target*DEG_TO_RAD, false);
+            mnt_target.target_type = MountTargetType::ANGLE;
+            break;
+        }
+
+        // move mount to a neutral position, typically pointing forward
+        case MAV_MOUNT_MODE_NEUTRAL: {
+            const Vector3f &target = _params.neutral_angles.get();
+            mnt_target.angle_rad.set(target*DEG_TO_RAD, false);
+            mnt_target.target_type = MountTargetType::ANGLE;
+            break;
+        }
+
+        // point to the angles given by a mavlink message
+        case MAV_MOUNT_MODE_MAVLINK_TARGETING:
+            // mnt_target should have already been filled in by set_angle_target() or set_rate_target()
+            if (mnt_target.target_type == MountTargetType::RATE) {
+                update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad);
+            }
+            resend_now = true;
+            break;
+
+        // RC radio manual angle control, but with stabilization from the AHRS
+        case MAV_MOUNT_MODE_RC_TARGETING: {
+            // update targets using pilot's RC inputs
+            MountTarget rc_target;
+            get_rc_target(mnt_target.target_type, rc_target);
+            switch (mnt_target.target_type) {
+            case MountTargetType::ANGLE:
+                mnt_target.angle_rad = rc_target;
+                break;
+            case MountTargetType::RATE:
+                mnt_target.rate_rads = rc_target;
+                break;
+            }
+            resend_now = true;
+            break;
+        }
+
+        // point mount to a GPS point given by the mission planner
+        case MAV_MOUNT_MODE_GPS_POINT:
+            if (get_angle_target_to_roi(mnt_target.angle_rad)) {
+                mnt_target.target_type = MountTargetType::ANGLE;
+                resend_now = true;
+            }
+            break;
+
+        // point mount to Home location
+        case MAV_MOUNT_MODE_HOME_LOCATION:
+            if (get_angle_target_to_home(mnt_target.angle_rad)) {
+                mnt_target.target_type = MountTargetType::ANGLE;
+                resend_now = true;
+            }
+            break;
+
+        // point mount to another vehicle
+        case MAV_MOUNT_MODE_SYSID_TARGET:
+            if (get_angle_target_to_sysid(mnt_target.angle_rad)) {
+                mnt_target.target_type = MountTargetType::ANGLE;
+                resend_now = true;
+            }
+            break;
+
+        default:
+            // we do not know this mode so do nothing
+            break;
+    }
+
+    // resend target angles at least once per second
+    resend_now = resend_now || ((AP_HAL::millis() - _last_send) > AP_MOUNT_CADDX_RESEND_MS);
+
+        if (resend_now) {
+            send_target_angles(mnt_target.angle_rad);
+        }
+}
+
+// get attitude as a quaternion.  returns true on success
+bool AP_Mount_CADDX::get_attitude_quaternion(Quaternion& att_quat)
+{
+    att_quat.from_euler(radians(_current_angle.x * 0.01f), radians(_current_angle.y * 0.01f), radians(_current_angle.z * 0.01f));
+    return true;
+}
+
+
+// send_target_angles
+void AP_Mount_CADDX::send_target_angles(const MountTarget& angle_target_rad)
+{
+
+    static cmd_set_angles_struct cmd_set_angles_data = { //init some fields here. all others default to zero
+        .sync = {0xA5 , 0x5A},
+        .mode = GIMBAL_MODE_DEFAULT,
+        .sensitivity = 0,
+        .roll = 0,
+        .tilt = 0,
+        .pan = 0
+    };
+
+    // exit immediately if not initialised
+    if (!_initialised) {
+        return;
+    }
+
+    if ((size_t)_uart->txspace() < sizeof(cmd_set_angles_data)) {
+        return;
+    }
+
+    // compute angles
+    cmd_set_angles_data.tilt = degrees(angle_target_rad.pitch);
+    cmd_set_angles_data.roll = degrees(angle_target_rad.roll);
+    cmd_set_angles_data.pan = degrees(angle_target_rad.get_bf_yaw());
+
+    //normalize here
+    cmd_set_angles_data.tilt = int16_t(linear_interpolate(AXIS_MIN, AXIS_MAX, cmd_set_angles_data.tilt, _params.pitch_angle_min, _params.pitch_angle_max));
+    cmd_set_angles_data.roll = int16_t(linear_interpolate(AXIS_MIN, AXIS_MAX, cmd_set_angles_data.roll, _params.roll_angle_min, _params.roll_angle_max));
+    cmd_set_angles_data.pan = int16_t(linear_interpolate(AXIS_MIN, AXIS_MAX, cmd_set_angles_data.pan, _params.yaw_angle_min, _params.yaw_angle_max));
+
+    //todo:add mode here
+    switch (_mount_lock_mode) {
+    case 1:
+    cmd_set_angles_data.mode = GIMBAL_MODE_ROLL_LOCK;
+    break;
+    case 2:
+    cmd_set_angles_data.mode = GIMBAL_MODE_TILT_ROLL_LOCK;
+    break;
+    default:
+    cmd_set_angles_data.mode = GIMBAL_MODE_DEFAULT ;
+    break;
+    }
+
+    // CRC here
+    uint16_t crc16 = 0;
+    uint8_t *b = (uint8_t *)&cmd_set_angles_data;
+    for (uint8_t i = 0; i < sizeof(cmd_set_angles_data) - 2; i++) {
+        crc16 = crc_xmodem_update(crc16, *(b + i));
+    }
+    cmd_set_angles_data.crch = (crc16 >> 8) & 0xFF;
+    cmd_set_angles_data.crcl = crc16 & 0xFF;
+
+    //send to gimbal
+    uint8_t* buf = (uint8_t*)&cmd_set_angles_data;    
+    for (uint8_t i = 0;  i != sizeof(cmd_set_angles_data) ; i++) {
+        _uart->write(buf[i]);
+    }
+
+    // store time of send
+    _last_send = AP_HAL::millis();
+}
+
+
+
+
+#endif // HAL_MOUNT_CADDX_ENABLED

libraries/AP_Mount/AP_Mount_CADDX.h

@@ -0,0 +1,84 @@
+/*
+  CADDX mount using serial protocol backend class
+ */
+#pragma once
+
+#include "AP_Mount_Backend_Serial.h"
+
+#if HAL_MOUNT_CADDX_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_Common/AP_Common.h>
+
+#define AP_MOUNT_CADDX_RESEND_MS   1000    // resend angle targets to gimbal once per second
+
+#define AXIS_MAX 2047
+#define AXIS_MIN -2048
+
+typedef enum {
+    GIMBAL_MODE_FOLLOW = 0,
+    GIMBAL_MODE_TILT_LOCK = (1<<0),
+    GIMBAL_MODE_ROLL_LOCK = (1<<1),
+    GIMBAL_MODE_PAN_LOCK  = (1<<2),
+} gimbal_lock_mode;
+
+#define GIMBAL_MODE_DEFAULT             GIMBAL_MODE_FOLLOW
+#define GIMBAL_MODE_TILT_ROLL_LOCK      (GIMBAL_MODE_TILT_LOCK | GIMBAL_MODE_ROLL_LOCK)
+
+
+class AP_Mount_CADDX : public AP_Mount_Backend_Serial
+{
+
+public:
+    // Constructor
+    using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial;
+
+    // update mount position - should be called periodically
+    void update() override;
+
+    // has_pan_control - returns true if this mount can control its pan (required for multicopters)
+    bool has_pan_control() const override { return yaw_range_valid(); };
+
+    // has_roll_control - returns true if this mount can control its roll
+    bool has_roll_control() const override { return roll_range_valid(); };
+
+    // has_pitch_control - returns true if this mount can control its tilt
+    bool has_pitch_control() const override { return pitch_range_valid(); };
+
+
+protected:
+
+    // get attitude as a quaternion.  returns true on success
+    bool get_attitude_quaternion(Quaternion& att_quat) override;
+
+private:
+
+    // send_target_angles
+    void send_target_angles(const MountTarget& angle_target_rad);
+
+    //void add_next_reply(ReplyType reply_type);
+    //uint8_t get_reply_size(ReplyType reply_type);
+    //bool can_send(bool with_control);
+
+
+    struct PACKED cmd_set_angles_struct {
+        uint8_t  sync[2];       //data synchronization 0xA5, 0x5A
+	uint8_t  mode:3;        //Gimbal Mode [0~7] [Only 0 1 2 modes are supported for the time being]
+	int16_t  sensitivity:5; // Stabilization sensibility [-16~15]
+	uint8_t  reserved:4;    //hold on to one's reserve
+	int32_t  roll:12;       //Roll angle [-2048~2047] => [-180~180]
+	int32_t  tilt:12;       //Pich angle [-2048~2047] => [-180~180]
+	int32_t  pan:12;        //Yaw angle [-2048~2047] => [-180~180]
+	uint8_t  crch;          //Data validation H
+	uint8_t  crcl;          //Data validation L
+    };
+
+
+    // internal variables
+    uint32_t _last_send;            // system time of last do_mount_control sent to gimbal
+
+    // keep the last _current_angle values
+    Vector3l _current_angle;
+};
+#endif // HAL_MOUNT_CADDX_ENABLED

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, 12:Topotek
+    // @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:Topotek, 13:CADDX
     // @RebootRequired: True
     // @User: Standard
     AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Mount_Params, type, 0, AP_PARAM_FLAG_ENABLE),

libraries/AP_Mount/AP_Mount_config.h

@@ -27,6 +27,10 @@
 #define HAL_MOUNT_SERVO_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED
 #endif
 
+#ifndef HAL_MOUNT_CADDX_ENABLED
+#define HAL_MOUNT_CADDX_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024
+#endif
+
 #ifndef HAL_MOUNT_SIYI_ENABLED
 #define HAL_MOUNT_SIYI_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED
 #endif