diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 0b64b1063bb0e..a702c7db570a3 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 d34f581636893..0984d23b3d4d1 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 0000000000000..7ec80fe7cb6d9 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_CADDX.cpp @@ -0,0 +1,180 @@ +#include "AP_Mount_CADDX.h" +#include +#if HAL_MOUNT_CADDX_ENABLED +#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 -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 diff --git a/libraries/AP_Mount/AP_Mount_CADDX.h b/libraries/AP_Mount/AP_Mount_CADDX.h new file mode 100644 index 0000000000000..992a1b360216b --- /dev/null +++ b/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 +#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 _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 diff --git a/libraries/AP_Mount/AP_Mount_Params.cpp b/libraries/AP_Mount/AP_Mount_Params.cpp index d077c603ec976..30d14960a78fe 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 c6e0791c2a6b0..a2a5db637bfab 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