WIP

libraries/AP_Mount/AP_Mount.cpp

@@ -166,6 +166,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,156 @@
+#include "AP_Mount_CADDX.h"
+
+#if HAL_MOUNT_CADDX_ENABLED
+#include <AP_HAL/AP_HAL.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
+#include <GCS_MAVLink/include/mavlink/v2.0/checksum.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 = {
+        0xFA,
+        0x0E,
+        0x11,
+        0, // pitch
+        0, // roll
+        0, // yaw
+        0, // flags
+        0, // type
+        0, // crc
+    };
+
+    // exit immediately if not initialised
+    if (!_initialised) {
+        return;
+    }
+
+    if ((size_t)_uart->txspace() < sizeof(cmd_set_angles_data)) {
+        return;
+    }
+
+    // send CMD_SETANGLE (need to normalize here)
+    cmd_set_angles_data.pitch = degrees(angle_target_rad.pitch);
+    cmd_set_angles_data.roll = degrees(angle_target_rad.roll);
+    cmd_set_angles_data.yaw = degrees(angle_target_rad.get_bf_yaw());
+
+    uint8_t* buf = (uint8_t*)&cmd_set_angles_data;
+
+    cmd_set_angles_data.crc = crc_calculate(&buf[1], sizeof(cmd_set_angles_data)-3);
+
+    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,70 @@
+/*
+  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
+
+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 byte1;
+        uint8_t byte2;
+        uint8_t byte3;
+        float pitch;
+        float roll;
+        float yaw;
+        uint8_t flags;
+        uint8_t type;
+        uint16_t crc;
+    };
+
+
+    // 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