diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index fe7f29f7eac3c..a1f93102d16d8 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/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 #include #include @@ -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 diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 54762e915307f..fe5efc153ef30 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 }; @@ -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 diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 16082d58f450b..20738de43f16d 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/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 diff --git a/libraries/AP_Mount/AP_Mount_CADDX.cpp b/libraries/AP_Mount/AP_Mount_CADDX.cpp new file mode 100644 index 0000000000000..533df3403f8f3 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_CADDX.cpp @@ -0,0 +1,176 @@ +#include "AP_Mount_CADDX.h" +#include +#if HAL_MOUNT_CADDX_ENABLED +#include + +// 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 diff --git a/libraries/AP_Mount/AP_Mount_CADDX.h b/libraries/AP_Mount/AP_Mount_CADDX.h new file mode 100644 index 0000000000000..95a3a44772f4f --- /dev/null +++ b/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 +#include +#include + +#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_YAW_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 diff --git a/libraries/AP_Mount/AP_Mount_Params.cpp b/libraries/AP_Mount/AP_Mount_Params.cpp index e2c727f4b41c4..ee91a6fa94302 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 c7b063b20e2ec..6fca51a651557 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/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