From db07f8520dbb572045f17a0a2ae24971c9179a3a Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Thu, 10 Oct 2024 15:44:17 -0500 Subject: [PATCH] WIP --- libraries/AP_Mount/AP_Mount.cpp | 9 ++ libraries/AP_Mount/AP_Mount.h | 5 + libraries/AP_Mount/AP_Mount_CADDX.cpp | 156 +++++++++++++++++++++++++ libraries/AP_Mount/AP_Mount_CADDX.h | 70 +++++++++++ libraries/AP_Mount/AP_Mount_Params.cpp | 2 +- libraries/AP_Mount/AP_Mount_config.h | 4 + 6 files changed, 245 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 fe7f29f7eac3c1..26b83cedb3b40e 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/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 diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 54762e915307fc..1455e7f1ae39fb 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..a00a07be9bd22f --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_CADDX.cpp @@ -0,0 +1,156 @@ +#include "AP_Mount_CADDX.h" + +#if HAL_MOUNT_CADDX_ENABLED +#include +#include +#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 = { + 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 diff --git a/libraries/AP_Mount/AP_Mount_CADDX.h b/libraries/AP_Mount/AP_Mount_CADDX.h new file mode 100644 index 00000000000000..b51c1c35abca95 --- /dev/null +++ b/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 +#include +#include + +#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 diff --git a/libraries/AP_Mount/AP_Mount_Params.cpp b/libraries/AP_Mount/AP_Mount_Params.cpp index e2c727f4b41c4d..ee91a6fa94302e 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 c7b063b20e2ec8..6fca51a651557c 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