From 2f0ecd8b6882124ca153b2ed10893aed386f8ad0 Mon Sep 17 00:00:00 2001 From: Andrii Fil Date: Sat, 5 Jul 2025 11:35:47 +0300 Subject: [PATCH] AntennaTracker: support attitude_target for guided, scan, auto modes --- AntennaTracker/GCS_MAVLink_Tracker.cpp | 97 ++++++++++++++++++++++++-- AntennaTracker/GCS_MAVLink_Tracker.h | 1 + AntennaTracker/mode.cpp | 8 ++- AntennaTracker/mode.h | 24 ++++++- 4 files changed, 121 insertions(+), 9 deletions(-) diff --git a/AntennaTracker/GCS_MAVLink_Tracker.cpp b/AntennaTracker/GCS_MAVLink_Tracker.cpp index 2d2a9ae9c71..bf123ea93fb 100644 --- a/AntennaTracker/GCS_MAVLink_Tracker.cpp +++ b/AntennaTracker/GCS_MAVLink_Tracker.cpp @@ -63,6 +63,87 @@ MAV_STATE GCS_MAVLINK_Tracker::vehicle_system_status() const return MAV_STATE_ACTIVE; } +void GCS_MAVLINK_Tracker::send_attitude_target() +{ + Quaternion quat; + bool use_yaw_rate = false; + float yaw_rate_rads = 0.0; + bool use_pitch_rate = false; + float pitch_rate_rads = 0.0; + float quat_out[4] {0.0, 0.0, 0.0, 0.0}; + uint16_t typemask = ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE | + ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE; + + if (tracker.mode->number() == Mode::Number::GUIDED) { + quat = tracker.mode_guided.get_attitude_target_quat(); + use_yaw_rate = tracker.mode_guided.get_attitude_target_use_yaw_rate(); + use_pitch_rate = tracker.mode_guided.get_attitude_target_use_pitch_rate(); + quat_out[0] = quat.q1; + quat_out[1] = quat.q2; + quat_out[2] = quat.q3; + quat_out[3] = quat.q4; + if (!use_yaw_rate) { + typemask |= ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE; + } else { + yaw_rate_rads = tracker.mode_guided.get_attitude_target_yaw_rate_rads(); + } + if (!use_pitch_rate) { + typemask |= ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE; + } else { + pitch_rate_rads = tracker.mode_guided.get_attitude_target_pitch_rate_rads(); + } + + } else if (tracker.mode->number() == Mode::Number::AUTO) { + float yaw = tracker.mode_auto.get_auto_target_yaw_deg(); + float pitch = tracker.mode_auto.get_auto_target_pitch_deg(); + quat.from_euler(0, radians(pitch), radians(yaw)); + quat_out[0] = quat.q1; + quat_out[1] = quat.q2; + quat_out[2] = quat.q3; + quat_out[3] = quat.q4; + + typemask |= + ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE | + ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE | + ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE | + ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE; + + } else if (tracker.mode->number() == Mode::Number::SCAN) { + struct Tracker::NavStatus &nav_status = tracker.nav_status; + Parameters &g = tracker.g; + + if (!nav_status.manual_control_yaw) { + yaw_rate_rads = radians(g.scan_speed_yaw); + } else { + typemask |= ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE; + } + + if (!nav_status.manual_control_pitch) { + pitch_rate_rads = radians(g.scan_speed_pitch); + } else { + typemask |= ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE; + } + + typemask |= ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE; + + } else if (tracker.mode->number() == Mode::Number::STOP) { + typemask |= ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE; + + } else { + return; + } + + mavlink_msg_attitude_target_send( + chan, + AP_HAL::millis(), // time since boot (ms) + typemask, // Bitmask that tells the system what control dimensions should be ignored by the vehicle + quat_out, // Attitude quaternion [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], unit-length + 0, // roll rate (rad/s) + pitch_rate_rads, // pitch rate (rad/s) + yaw_rate_rads, // yaw rate (rad/s) + 0); // Collective thrust +} + void GCS_MAVLINK_Tracker::send_nav_controller_output() const { float alt_diff = (tracker.g.alt_source == ALT_SOURCE_BARO) ? tracker.nav_status.alt_difference_baro : tracker.nav_status.alt_difference_gps; @@ -94,29 +175,33 @@ void GCS_MAVLINK_Tracker::handle_set_attitude_target(const mavlink_message_t &ms if (!is_zero(packet.body_roll_rate)) { return; } - if (!(packet.type_mask & (1<<0))) { + if (!(packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE)) { // not told to ignore body roll rate return; } - if (!(packet.type_mask & (1<<6))) { + if (!(packet.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE)) { // not told to ignore throttle return; } - if (packet.type_mask & (1<<7)) { + if (packet.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE) { // told to ignore attitude (we don't allow continuous motion yet) return; } - if ((packet.type_mask & (1<<3)) && (packet.type_mask&(1<<4))) { + if ((packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE) && + (packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE)) { // told to ignore both pitch and yaw rates - nothing to do?! return; } - const bool use_yaw_rate = !(packet.type_mask & (1<<2)); + const bool use_yaw_rate = !(packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE); + const bool use_pitch_rate = !(packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE); tracker.mode_guided.set_angle( Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), use_yaw_rate, - packet.body_yaw_rate); + packet.body_yaw_rate, + use_pitch_rate, + packet.body_pitch_rate); } /* diff --git a/AntennaTracker/GCS_MAVLink_Tracker.h b/AntennaTracker/GCS_MAVLink_Tracker.h index efcb5d2d109..e6a6c809efa 100644 --- a/AntennaTracker/GCS_MAVLink_Tracker.h +++ b/AntennaTracker/GCS_MAVLink_Tracker.h @@ -19,6 +19,7 @@ protected: return 0; // what if we have been picked up and carried somewhere? } + void send_attitude_target() override; void send_nav_controller_output() const override; void send_pid_tuning() override; diff --git a/AntennaTracker/mode.cpp b/AntennaTracker/mode.cpp index a79bf53481e..ff1f4626b19 100644 --- a/AntennaTracker/mode.cpp +++ b/AntennaTracker/mode.cpp @@ -8,8 +8,12 @@ void Mode::update_auto(void) Parameters &g = tracker.g; - float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100); // target yaw in centidegrees - float pitch = constrain_float(nav_status.pitch+g.pitch_trim, g.pitch_min, g.pitch_max) * 100; // target pitch in centidegrees + float yaw_deg = wrap_180(nav_status.bearing + g.yaw_trim); // target yaw in degrees + float pitch_deg = constrain_float(nav_status.pitch + g.pitch_trim, g.pitch_min, g.pitch_max); // target pitch in degrees + tracker.mode_auto.set_target(yaw_deg, pitch_deg); + + float yaw = yaw_deg * 100; // target yaw in centidegrees + float pitch = pitch_deg * 100; // target pitch in centidegrees bool direction_reversed = get_ef_yaw_direction(); diff --git a/AntennaTracker/mode.h b/AntennaTracker/mode.h index 0b0752cea65..15e43b963a7 100644 --- a/AntennaTracker/mode.h +++ b/AntennaTracker/mode.h @@ -46,6 +46,17 @@ public: const char* name() const override { return "Auto"; } bool requires_armed_servos() const override { return true; } void update() override; + + void set_target(float target_yaw_deg, float target_pitch_deg) { + _target_yaw_deg = target_yaw_deg; + _target_pitch_deg = target_pitch_deg; + } + float get_auto_target_yaw_deg() const { return _target_yaw_deg; } + float get_auto_target_pitch_deg() const { return _target_pitch_deg; } + +private: + float _target_yaw_deg; + float _target_pitch_deg; }; class ModeGuided : public Mode { @@ -55,16 +66,27 @@ public: bool requires_armed_servos() const override { return true; } void update() override; - void set_angle(const Quaternion &target_att, bool use_yaw_rate, float yaw_rate_rads) { + void set_angle(const Quaternion &target_att, + bool use_yaw_rate, float yaw_rate_rads, + bool use_pitch_rate, float pitch_rate_rads) { _target_att = target_att; _use_yaw_rate = use_yaw_rate; _yaw_rate_rads = yaw_rate_rads; + _use_pitch_rate = use_pitch_rate; + _pitch_rate_rads = pitch_rate_rads; } + Quaternion get_attitude_target_quat() { return _target_att; } + bool get_attitude_target_use_yaw_rate() const { return _use_yaw_rate; } + float get_attitude_target_yaw_rate_rads() const { return _yaw_rate_rads; } + bool get_attitude_target_use_pitch_rate() const { return _use_pitch_rate; } + float get_attitude_target_pitch_rate_rads() const { return _pitch_rate_rads; } private: Quaternion _target_att; bool _use_yaw_rate; float _yaw_rate_rads; + bool _use_pitch_rate; + float _pitch_rate_rads; }; class ModeInitialising : public Mode {