From 16c9c175ae31b800b713809589b73306ef1743c1 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Tue, 25 Mar 2025 16:29:21 +0100 Subject: [PATCH] differential: streamline flow of information --- .../DifferentialAttControl.cpp | 23 ++++++++++--------- .../DifferentialAttControl.hpp | 4 ++-- .../DifferentialRateControl.cpp | 4 ++-- .../DifferentialRateControl.hpp | 5 ++-- 4 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp index a36ed41ada..5bc9ab380a 100644 --- a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp @@ -78,7 +78,7 @@ void DifferentialAttControl::updateAttControl() if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { - generateAttitudeSetpoint(); + generateAttitudeAndThrottleSetpoint(); } generateRateSetpoint(); @@ -92,12 +92,12 @@ void DifferentialAttControl::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 DifferentialAttControl::generateAttitudeSetpoint() +void DifferentialAttControl::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; @@ -113,17 +113,18 @@ void DifferentialAttControl::generateAttitudeSetpoint() rover_throttle_setpoint.throttle_body_y = 0.f; _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - const 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 = yaw_rate_setpoint; - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + 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_differential/DifferentialAttControl/DifferentialAttControl.hpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp index e4198384c0..82087f8278 100644 --- a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp @@ -84,10 +84,10 @@ protected: private: /** - * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode) + * @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint from manualControlSetpoint (Stab Mode) * or trajectorySetpoint (Offboard attitude control). */ - void generateAttitudeSetpoint(); + void generateAttitudeAndThrottleSetpoint(); /** * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp index fe62328cd9..8b8e7005a1 100644 --- a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp @@ -75,7 +75,7 @@ void DifferentialRateControl::updateRateControl() if (_vehicle_control_mode.flag_control_rates_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { - generateRateSetpoint(); + generateRateAndThrottleSetpoint(); } generateSteeringSetpoint(); @@ -95,7 +95,7 @@ void DifferentialRateControl::updateRateControl() } -void DifferentialRateControl::generateRateSetpoint() +void DifferentialRateControl::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_differential/DifferentialRateControl/DifferentialRateControl.hpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp index 5b9bd6d632..da9b682f15 100644 --- a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp @@ -84,10 +84,9 @@ protected: private: /** - * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode) - * or trajectorySetpoint (Offboard rate control). + * @brief Generate and publish roverRateSetpoint and roverThrottleSetpoint from manualControlSetpoint (Acro Mode). */ - void generateRateSetpoint(); + void generateRateAndThrottleSetpoint(); /** * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint.