From b0d1c2c162dde4229c12541f97da98495e53dc00 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sat, 19 Oct 2024 10:19:51 -0500 Subject: [PATCH] AP_Mount: add CADDX gimbal support Co-authored-by: Randy Mackay --- libraries/AP_Mount/AP_Mount.cpp | 10 ++ libraries/AP_Mount/AP_Mount.h | 5 + libraries/AP_Mount/AP_Mount_CADDX.cpp | 172 +++++++++++++++++++++++++ libraries/AP_Mount/AP_Mount_CADDX.h | 54 ++++++++ libraries/AP_Mount/AP_Mount_Params.cpp | 2 +- libraries/AP_Mount/AP_Mount_config.h | 4 + 6 files changed, 246 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_Mount/AP_Mount_CADDX.cpp create mode 100644 libraries/AP_Mount/AP_Mount_CADDX.h diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 0b64b1063bb0e2..a702c7db570a38 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/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 #include #include @@ -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 diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index d34f5816368938..0984d23b3d4d18 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/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 }; diff --git a/libraries/AP_Mount/AP_Mount_CADDX.cpp b/libraries/AP_Mount/AP_Mount_CADDX.cpp new file mode 100644 index 00000000000000..2dc9164072e3d9 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_CADDX.cpp @@ -0,0 +1,172 @@ +#include "AP_Mount_config.h" + +#if HAL_MOUNT_CADDX_ENABLED + +#include "AP_Mount_CADDX.h" +#include + +#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 0 +#define AXIS_MAX 4096 + +// 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 + 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) +{ + // gimbal does not provide attitude so simply return targets + att_quat.from_euler(mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.get_bf_yaw()); + 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 (_uart->txspace() < SET_ATTITUDE_BUF_SIZE) { + return; + } + + // calculate roll, pitch, yaw angles in range 0 to 2048 + const float scalar = AXIS_MAX / M_2PI; + uint16_t roll_target_cmd = constrain_uint16(wrap_2PI(angle_target_rad.roll) * scalar, AXIS_MIN, AXIS_MAX); + uint16_t pitch_target_cmd = constrain_uint16(wrap_2PI(angle_target_rad.pitch) * scalar, AXIS_MIN, AXIS_MAX); + uint16_t yaw_target_cmd = constrain_uint16(wrap_2PI(angle_target_rad.get_bf_yaw()) * scalar, AXIS_MIN, AXIS_MAX); + + // prepare packet to send to gimbal + uint8_t set_attitude_cmd_buf[SET_ATTITUDE_BUF_SIZE] {}; + + // first two bytes hold the header + set_attitude_cmd_buf[0] = SET_ATTITUDE_HEADER1; + set_attitude_cmd_buf[1] = SET_ATTITUDE_HEADER2; + + // byte 2's lower 3 bits are mode + // lower 5 bits are sensitivity but always left as zero + 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; + + // byte 3's lower 4 bits are reserved + // bottom 4 bits are roll's lower 4 bits + set_attitude_cmd_buf[3] = roll_target_cmd & 0x0F; + + // byte 4 is roll's upper 8 bits + set_attitude_cmd_buf[4] = (roll_target_cmd >> 4) & 0xFF; + + // byte 5 is pitch's lower 8 bits + set_attitude_cmd_buf[5] = pitch_target_cmd & 0xFF; + + // byte 6's lower 4 bits are pitch's upper 4 bits + // upper 4 bits are yaw's lower 4 bits + set_attitude_cmd_buf[6] = (pitch_target_cmd >> 8) & 0x0F; + set_attitude_cmd_buf[6] |= (yaw_target_cmd & 0x0F) << 4; + + // byte 7 is yaw's upper 8 bits + set_attitude_cmd_buf[7] = (yaw_target_cmd >> 4) & 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 diff --git a/libraries/AP_Mount/AP_Mount_CADDX.h b/libraries/AP_Mount/AP_Mount_CADDX.h new file mode 100644 index 00000000000000..4ad65f01c918da --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_CADDX.h @@ -0,0 +1,54 @@ +/* + CADDX mount using serial protocol backend class + */ +#pragma once + +#include "AP_Mount_Backend_Serial.h" + +#if HAL_MOUNT_CADDX_ENABLED + +#include +#include +#include + +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 _last_send_ms; // system time of last do_mount_control sent to gimbal +}; +#endif // HAL_MOUNT_CADDX_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Params.cpp b/libraries/AP_Mount/AP_Mount_Params.cpp index d077c603ec9767..30d14960a78fee 100644 --- a/libraries/AP_Mount/AP_Mount_Params.cpp +++ b/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), diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index c6e0791c2a6b03..a2a5db637bfab4 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/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