AP_Mount: add CADDX gimbal support

Co-authored-by: Randy Mackay <[email protected]>

libraries/AP_Mount/AP_Mount.cpp

@@ -18,6 +18,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>
@@ -168,6 +169,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

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
     };
 

libraries/AP_Mount/AP_Mount_CADDX.cpp

@@ -0,0 +1,180 @@
+#include "AP_Mount_CADDX.h"
+#include <GCS_MAVLink/GCS.h>
+#if HAL_MOUNT_CADDX_ENABLED
+#include <AP_HAL/AP_HAL.h>
+
+#define AP_MOUNT_CADDX_RESEND_MS   1000    // resend angle targets to gimbal once per second
+#define SET_ATTITUDE_HEADER1 0xA5
+#define SET_ATTITUDE_HEADER2 0x5A
+#define SET_ATTITUDE_BUF_SIZE 10
+#define AXIS_MIN -2048
+#define AXIS_MAX 2047
+
+// update mount position - should be called periodically
+void AP_Mount_CADDX::update()
+{
+    // exit immediately if not initialised
+    if (!_initialised) {
+        return;
+    }
+
+    // read incoming packets
+    int16_t nbytes = MIN(_uart->available(), 1024U);
+    _bytes_read_tot += nbytes;
+    for (int16_t i = 0; i < nbytes; i++) {
+        uint8_t b;
+        if (!_uart->read(b)) {
+            break;
+        }
+    }
+
+    // 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
+            update_mnt_target_from_rc_target();
+            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_ms) > 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)
+{
+    // exit immediately if not initialised
+    if (!_initialised) {
+        return;
+    }
+
+    // ensure we have enough space to send the packet
+    if ((size_t)_uart->txspace() < SET_ATTITUDE_BUF_SIZE) {
+        return;
+    }
+
+    // calculate roll, pitch, yaw angles in range -2048 to 2047
+    int16_t roll_target_cmd = int16_t(linear_interpolate(AXIS_MIN, AXIS_MAX, degrees(angle_target_rad.roll), -180, 180));
+    int16_t pitch_target_cmd = int16_t(linear_interpolate(AXIS_MIN, AXIS_MAX, degrees(angle_target_rad.pitch), -180, 180));
+    int16_t yaw_target_cmd = int16_t(linear_interpolate(AXIS_MIN, AXIS_MAX, degrees(angle_target_rad.get_bf_yaw()), -180, 180));
+
+    // prepare packet to send to gimbal
+    uint8_t set_attitude_cmd_buf[SET_ATTITUDE_BUF_SIZE] {};
+    set_attitude_cmd_buf[0] = SET_ATTITUDE_HEADER1; // 0
+    set_attitude_cmd_buf[1] = SET_ATTITUDE_HEADER2; // 1
+
+    // mode is top 3 bites of byte 2
+    uint8_t mode = (uint8_t)LockMode::GIMBAL_MODE_TILT_LOCK | (uint8_t)LockMode::GIMBAL_MODE_ROLL_LOCK;
+    if (angle_target_rad.yaw_is_ef) {
+        mode |= (uint8_t)LockMode::GIMBAL_MODE_YAW_LOCK;
+    }
+    set_attitude_cmd_buf[2] = mode << 5;
+
+    // sensitivity is bottom 5 bits of byte 2
+    const uint8_t sensitivity = 7;
+    set_attitude_cmd_buf[2] |= 0x1F & sensitivity;
+
+    // byte 3's top 4 bits are reserved
+    // bottom 4 bits are top 4 bits of roll
+    set_attitude_cmd_buf[3] = (roll_target_cmd >> 8) & 0x0F;
+
+    // byte 4 is bottom 8 bits of roll
+    set_attitude_cmd_buf[4] = roll_target_cmd & 0xFF;
+
+    // byte 5 is top 8 bits of pitch
+    set_attitude_cmd_buf[5] = (pitch_target_cmd >> 4) & 0xFF;
+
+    // byte 6's top 4 bits are bottom 4 bits of pitch
+    // bottom 4 bits are top 4 bits of yaw
+    set_attitude_cmd_buf[6] = (pitch_target_cmd << 4) & 0xF0;
+    set_attitude_cmd_buf[6] |= (yaw_target_cmd >> 8) & 0x0F;
+
+    // byte 7 is bottom 8 bits of yaw
+    set_attitude_cmd_buf[7] = yaw_target_cmd & 0xFF;
+
+    // calculate CRC
+    const uint16_t crc16 = crc16_ccitt(set_attitude_cmd_buf, sizeof(set_attitude_cmd_buf) - 2, 0);
+    set_attitude_cmd_buf[8] = HIGHBYTE(crc16);
+    set_attitude_cmd_buf[9] = LOWBYTE(crc16);
+
+    // send packet to gimbal
+    _uart->write(set_attitude_cmd_buf, sizeof(set_attitude_cmd_buf));
+
+    // store time of send
+    _last_send_ms = AP_HAL::millis();
+}
+
+#endif // HAL_MOUNT_CADDX_ENABLED

libraries/AP_Mount/AP_Mount_CADDX.h

@@ -0,0 +1,56 @@
+/*
+  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>
+
+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:
+
+    // lock mode enum
+    typedef enum class LockMode {
+        GIMBAL_MODE_FOLLOW = 0,
+        GIMBAL_MODE_TILT_LOCK = (1<<0),
+        GIMBAL_MODE_ROLL_LOCK = (1<<1),
+        GIMBAL_MODE_YAW_LOCK  = (1<<2),
+    } _mount_lock_mode;
+
+    // send_target_angles
+    void send_target_angles(const MountTarget& angle_target_rad);
+
+    // internal variables
+    uint32_t _bytes_read_tot;   // total number of bytes received on serial port
+    uint32_t _last_send_ms;     // system time of last do_mount_control sent to gimbal
+    Vector3l _current_angle;    // keep the last _current_angle values
+};
+#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

@@ -29,6 +29,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