From 8eb873a245a0699dac6eaeebadcfa7a7b9e16d78 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 31 Mar 2025 09:02:46 +0200 Subject: [PATCH] ackermann: streamline flow of information --- .../AckermannAttControl.cpp | 23 ++++++++++--------- .../AckermannAttControl.hpp | 4 ++-- .../AckermannRateControl.cpp | 4 ++-- .../AckermannRateControl.hpp | 4 ++-- 4 files changed, 18 insertions(+), 17 deletions(-) diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp index 1af95cbc1a..9c07a3ab3b 100644 --- a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp @@ -85,7 +85,7 @@ void AckermannAttControl::updateAttControl() } if (_vehicle_control_mode.flag_control_manual_enabled) { - generateAttitudeSetpoint(); + generateAttitudeAndThrottleSetpoint(); } generateRateSetpoint(); @@ -99,12 +99,12 @@ void AckermannAttControl::updateAttControl() rover_attitude_status_s rover_attitude_status; rover_attitude_status.timestamp = _timestamp; rover_attitude_status.measured_yaw = _vehicle_yaw; - rover_attitude_status.adjusted_yaw_setpoint = _adjusted_yaw_setpoint.getState(); + rover_attitude_status.adjusted_yaw_setpoint = matrix::wrap_pi(_adjusted_yaw_setpoint.getState()); _rover_attitude_status_pub.publish(rover_attitude_status); } -void AckermannAttControl::generateAttitudeSetpoint() +void AckermannAttControl::generateAttitudeAndThrottleSetpoint() { const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled; @@ -120,17 +120,18 @@ void AckermannAttControl::generateAttitudeSetpoint() rover_throttle_setpoint.throttle_body_y = 0.f; _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, - _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + const float yaw_delta = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(), + _max_yaw_rate / _param_ro_yaw_p.get()); - if (fabsf(yaw_rate_setpoint) > FLT_EPSILON + if (fabsf(yaw_delta) > FLT_EPSILON || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control _stab_yaw_ctl = false; - _adjusted_yaw_setpoint.setForcedValue(0.f); - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = _timestamp; - rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_estimated_speed_body_x) * yaw_rate_setpoint; - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + matrix::sign(manual_control_setpoint.throttle) * yaw_delta); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) if (!_stab_yaw_ctl) { diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp index 2acf8de524..93d17928be 100644 --- a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp @@ -81,9 +81,9 @@ protected: private: /** - * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode). + * @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint from manualControlSetpoint (Stab Mode). */ - void generateAttitudeSetpoint(); + void generateAttitudeAndThrottleSetpoint(); /** * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp index 3ec37eb844..f27f6127c2 100644 --- a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp @@ -82,7 +82,7 @@ void AckermannRateControl::updateRateControl() } if (_vehicle_control_mode.flag_control_manual_enabled) { - generateRateSetpoint(); + generateRateAndThrottleSetpoint(); } generateSteeringSetpoint(); @@ -102,7 +102,7 @@ void AckermannRateControl::updateRateControl() } -void AckermannRateControl::generateRateSetpoint() +void AckermannRateControl::generateRateAndThrottleSetpoint() { const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled; diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp index b7e194d70c..8f0891cb48 100644 --- a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp @@ -82,9 +82,9 @@ protected: private: /** - * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode). + * @brief Generate and publish roverRateSetpoint and roverThrottleSetpoint from manualControlSetpoint (Acro Mode). */ - void generateRateSetpoint(); + void generateRateAndThrottleSetpoint(); /** * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint.