From f35b92a4877836dcf1dda79c48de4fe782df3c9d Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 16 Jun 2025 11:00:04 +0200 Subject: [PATCH] rover: replace RX_MAX_THR_YAW_R with a correction factor RO_YAW_RATE_CORR --- .../init.d-posix/airframes/4009_gz_r1_rover | 2 +- .../airframes/4012_gz_rover_ackermann | 1 + .../airframes/4015_gz_r1_rover_mecanum | 2 +- .../airframes/50001_aion_robotics_r1_rover | 2 +- .../airframes/51001_axial_scx10_2_trail_honcho | 1 + src/lib/rover_control/RoverControl.cpp | 13 +++++++------ src/lib/rover_control/RoverControl.hpp | 6 ++++-- src/lib/rover_control/rovercontrol_params.c | 16 ++++++++++++++++ .../AckermannRateControl.cpp | 3 ++- .../AckermannRateControl.hpp | 3 ++- .../DifferentialRateControl.cpp | 7 ++++--- .../DifferentialRateControl.hpp | 5 +++-- src/modules/rover_differential/module.yaml | 17 ----------------- .../MecanumRateControl/MecanumRateControl.cpp | 10 +++++----- .../MecanumRateControl/MecanumRateControl.hpp | 5 +++-- src/modules/rover_mecanum/module.yaml | 17 ----------------- 16 files changed, 51 insertions(+), 59 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index 810b313b6b..647bba441d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -15,7 +15,6 @@ param set-default NAV_ACC_RAD 0.5 # Differential Parameters param set-default RD_WHEEL_TRACK 0.3 -param set-default RD_MAX_THR_YAW_R 1.5 param set-default RD_TRANS_DRV_TRN 0.349066 param set-default RD_TRANS_TRN_DRV 0.174533 @@ -31,6 +30,7 @@ param set-default RO_YAW_RATE_P 0.25 param set-default RO_YAW_RATE_LIM 180 param set-default RO_YAW_ACCEL_LIM 120 param set-default RO_YAW_DECEL_LIM 1000 +param set-default RO_YAW_RATE_CORR 1.43 # Rover Attitude Control Parameters param set-default RO_YAW_P 5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index aabeaa04fc..28025d79ac 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -30,6 +30,7 @@ param set-default RO_MAX_THR_SPEED 3.1 param set-default RO_YAW_RATE_I 0.1 param set-default RO_YAW_RATE_P 1 param set-default RO_YAW_RATE_LIM 180 +param set-default RO_YAW_RATE_CORR 1 # Rover Attitude Control Parameters param set-default RO_YAW_P 3 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum index 18802d76a5..5d96694e83 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum @@ -15,7 +15,6 @@ param set-default NAV_ACC_RAD 0.5 # Mecanum Parameters param set-default RM_WHEEL_TRACK 0.3 -param set-default RM_MAX_THR_YAW_R 1.2 # Rover Control Parameters param set-default RO_ACCEL_LIM 3 @@ -29,6 +28,7 @@ param set-default RO_YAW_RATE_P 0.1 param set-default RO_YAW_RATE_LIM 120 param set-default RO_YAW_ACCEL_LIM 240 param set-default RO_YAW_DECEL_LIM 1000 +param set-default RO_YAW_RATE_CORR 1.75 # Rover Attitude Control Parameters param set-default RO_YAW_P 5 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover index 2c2617df30..7b29f208aa 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover @@ -26,7 +26,6 @@ param set-default NAV_ACC_RAD 0.5 # Differential Parameters param set-default RD_WHEEL_TRACK 0.3 -param set-default RD_MAX_THR_YAW_R 0.7 param set-default RD_TRANS_DRV_TRN 0.785398 param set-default RD_TRANS_TRN_DRV 0.139626 @@ -42,6 +41,7 @@ param set-default RO_YAW_RATE_P 0.1 param set-default RO_YAW_RATE_LIM 250 param set-default RO_YAW_ACCEL_LIM 600 param set-default RO_YAW_DECEL_LIM 600 +param set-default RO_YAW_RATE_CORR 2.7 # Rover Attitude Control Parameters param set-default RO_YAW_P 2.5 diff --git a/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho b/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho index 2261d0fb8c..dd5835c8c5 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho +++ b/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho @@ -33,6 +33,7 @@ param set-default RO_MAX_THR_SPEED 2.8 param set-default RO_YAW_RATE_I 0.1 param set-default RO_YAW_RATE_P 0.1 param set-default RO_YAW_RATE_LIM 120 +param set-default RO_YAW_RATE_CORR 1 # Rover Attitude Control Parameters param set-default RO_YAW_P 2.5 diff --git a/src/lib/rover_control/RoverControl.cpp b/src/lib/rover_control/RoverControl.cpp index 05b93a1328..68d06c7d2b 100644 --- a/src/lib/rover_control/RoverControl.cpp +++ b/src/lib/rover_control/RoverControl.cpp @@ -161,7 +161,8 @@ float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, const } float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, const float yaw_rate_setpoint, - const float vehicle_yaw_rate, const float max_thr_yaw_r, const float max_yaw_accel, const float max_yaw_decel, + const float vehicle_yaw_rate, const float max_thr_speed, const float yaw_rate_corr, const float max_yaw_accel, + const float max_yaw_decel, const float wheel_track, const float dt) { // Apply acceleration and deceleration limit @@ -194,11 +195,11 @@ float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate // Transform yaw rate into speed difference float speed_diff_normalized{0.f}; - if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward - const float speed_diff = adjusted_yaw_rate_setpoint.getState() * wheel_track / - 2.f; - speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, - max_thr_yaw_r, -1.f, 1.f); + if (wheel_track > FLT_EPSILON && max_thr_speed > FLT_EPSILON) { // Feedforward + const float speed_diff = (adjusted_yaw_rate_setpoint.getState() * wheel_track / + 2.f) * yaw_rate_corr; + speed_diff_normalized = math::interpolate(speed_diff, -max_thr_speed, + max_thr_speed, -1.f, 1.f); } // Feedback control diff --git a/src/lib/rover_control/RoverControl.hpp b/src/lib/rover_control/RoverControl.hpp index 125a8a382a..97f38f21f4 100644 --- a/src/lib/rover_control/RoverControl.hpp +++ b/src/lib/rover_control/RoverControl.hpp @@ -100,7 +100,8 @@ float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, float * @param pid_yaw_rate Yaw rate PID (Updated by this function). * @param yaw_rate_setpoint Yaw rate setpoint [rad/s]. * @param vehicle_yaw_rate Measured vehicle yaw rate [rad/s]. - * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. + * @param max_thr_speed Speed at maximum throttle [m/s]. + * @param yaw_rate_corr Yaw rate correction factor to convert yaw rate to speed difference. * @param max_yaw_accel Maximum allowed yaw acceleration [rad/s^2]. * @param max_yaw_decel Maximum allowed yaw deceleration [rad/s^2]. * @param wheel_track Distance from the center of the right wheel to the center of the left wheel [m]. @@ -108,7 +109,8 @@ float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, float * @return Normalized speed difference setpoint [-1, 1]. */ float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, - float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, float max_yaw_decel, + float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_speed, float yaw_rate_corr, float max_yaw_accel, + float max_yaw_decel, float wheel_track, float dt); /** diff --git a/src/lib/rover_control/rovercontrol_params.c b/src/lib/rover_control/rovercontrol_params.c index 5cb7052e17..5a34b76433 100644 --- a/src/lib/rover_control/rovercontrol_params.c +++ b/src/lib/rover_control/rovercontrol_params.c @@ -131,6 +131,22 @@ PARAM_DEFINE_FLOAT(RO_YAW_ACCEL_LIM, -1.f); */ PARAM_DEFINE_FLOAT(RO_YAW_DECEL_LIM, -1.f); +/** + * Yaw rate correction factor + * + * Multiplicative correction factor for the feedforward mapping of the yaw rate controller. + * Increase this value (x > 1) if the measured yaw rate is lower than the setpoint, decrease (0 < x < 1) otherwise. + * Note: Tuning this is particularly useful for skid-steered rovers, or rovers with misaligned wheels/steering axes + * that cause a lot of friction when turning. + * + * @min 0.01 + * @max 10000 + * @increment 0.01 + * @decimal 2 + * @group Rover Rate Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_RATE_CORR, 1.f); + /** * Proportional gain for closed loop yaw controller * diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp index a0d0a9c6d0..60e123a140 100644 --- a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp @@ -86,7 +86,8 @@ void AckermannRateControl::updateRateControl() } // Feed forward - steering_setpoint = atanf(_adjusted_yaw_rate_setpoint.getState() * _param_ra_wheel_base.get() / _estimated_speed); + steering_setpoint = atanf(_adjusted_yaw_rate_setpoint.getState() * _param_ra_wheel_base.get() / _estimated_speed) * + _param_ro_yaw_rate_corr.get(); // Feedback (Only when driving forwards because backwards driving is NMP and can introduce instability) if (_estimated_speed > FLT_EPSILON) { diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp index d5014a4306..4a3ef1b2d6 100644 --- a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp @@ -123,6 +123,7 @@ private: (ParamFloat) _param_ro_yaw_rate_p, (ParamFloat) _param_ro_yaw_rate_i, (ParamFloat) _param_ro_yaw_accel_limit, - (ParamFloat) _param_ro_speed_th + (ParamFloat) _param_ro_speed_th, + (ParamFloat) _param_ro_yaw_rate_corr ) }; diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp index abdca32aa7..f70b258625 100644 --- a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp @@ -78,7 +78,8 @@ void DifferentialRateControl::updateRateControl() const float yaw_rate_setpoint = fabsf(_yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? _yaw_rate_setpoint : 0.f; const float speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, - yaw_rate_setpoint, _vehicle_yaw_rate, _param_rd_max_thr_yaw_r.get(), _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F, + yaw_rate_setpoint, _vehicle_yaw_rate, _param_ro_max_thr_speed.get(), _param_ro_yaw_rate_corr.get(), + _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F, _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rd_wheel_track.get(), dt); rover_steering_setpoint_s rover_steering_setpoint{}; rover_steering_setpoint.timestamp = _timestamp; @@ -103,12 +104,12 @@ bool DifferentialRateControl::runSanityChecks() { bool ret = true; - if ((_param_rd_wheel_track.get() < FLT_EPSILON || _param_rd_max_thr_yaw_r.get() < FLT_EPSILON) + if ((_param_rd_wheel_track.get() < FLT_EPSILON || _param_ro_max_thr_speed.get() < FLT_EPSILON) && _param_ro_yaw_rate_p.get() < FLT_EPSILON) { ret = false; events::send(events::ID("differential_rate_control_conf_invalid_rate_control"), events::Log::Error, "Invalid configuration for rate control: Neither feed forward nor feedback is setup", _param_rd_wheel_track.get(), - _param_rd_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get()); + _param_ro_max_thr_speed.get(), _param_ro_yaw_rate_p.get()); } return ret; diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp index a58908fc9e..83dc2f8dfa 100644 --- a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp @@ -107,11 +107,12 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_rd_wheel_track, - (ParamFloat) _param_rd_max_thr_yaw_r, (ParamFloat) _param_ro_yaw_rate_th, (ParamFloat) _param_ro_yaw_rate_p, (ParamFloat) _param_ro_yaw_rate_i, (ParamFloat) _param_ro_yaw_accel_limit, - (ParamFloat) _param_ro_yaw_decel_limit + (ParamFloat) _param_ro_yaw_decel_limit, + (ParamFloat) _param_ro_yaw_rate_corr, + (ParamFloat) _param_ro_max_thr_speed ) }; diff --git a/src/modules/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml index ef825f13e5..4a1132b857 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -15,23 +15,6 @@ parameters: decimal: 3 default: 0 - RD_MAX_THR_YAW_R: - description: - short: Yaw rate turning left/right wheels at max speed in opposite directions - long: | - This parameter is used to calculate the feedforward term of the closed loop yaw rate control. - The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate. - This desired speed difference is then linearly mapped to normalized motor commands. - A good starting point is half the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)). - Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0 - RD_TRANS_TRN_DRV: description: short: Yaw error threshhold to switch from spot turning to driving diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp index 8403498bab..0355eefb9b 100644 --- a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp @@ -78,7 +78,8 @@ void MecanumRateControl::updateRateControl() const float yaw_rate_setpoint = fabsf(_yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? _yaw_rate_setpoint : 0.f; const float speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, - yaw_rate_setpoint, _vehicle_yaw_rate, _param_rm_max_thr_yaw_r.get(), _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F, + yaw_rate_setpoint, _vehicle_yaw_rate, _param_ro_max_thr_speed.get(), _param_ro_yaw_rate_corr.get(), + _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F, _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rm_wheel_track.get(), dt); rover_steering_setpoint_s rover_steering_setpoint{}; rover_steering_setpoint.timestamp = _timestamp; @@ -110,13 +111,12 @@ bool MecanumRateControl::runSanityChecks() } - if ((_param_rm_wheel_track.get() < FLT_EPSILON || _param_rm_max_thr_yaw_r.get() < FLT_EPSILON) + if ((_param_rm_wheel_track.get() < FLT_EPSILON || _param_ro_max_thr_speed.get() < FLT_EPSILON) && _param_ro_yaw_rate_p.get() < FLT_EPSILON) { ret = false; events::send(events::ID("mecanum_rate_control_conf_invalid_rate_control"), events::Log::Error, - "Invalid configuration for rate control: Neither feed forward (RM_MAX_THR_YAW_R) nor feedback (RO_YAW_RATE_P) is setup", - _param_rm_wheel_track.get(), - _param_rm_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get()); + "Invalid configuration for rate control: Neither feed forward (RO_MAX_THR_SPEED) nor feedback (RO_YAW_RATE_P) is setup", + _param_rm_wheel_track.get(), _param_ro_max_thr_speed.get(), _param_ro_yaw_rate_p.get()); } return ret; diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp index ef93c90ad9..15a4a12b8c 100644 --- a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp @@ -107,12 +107,13 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_rm_wheel_track, - (ParamFloat) _param_rm_max_thr_yaw_r, + (ParamFloat) _param_ro_max_thr_speed, (ParamFloat) _param_ro_yaw_rate_limit, (ParamFloat) _param_ro_yaw_rate_th, (ParamFloat) _param_ro_yaw_rate_p, (ParamFloat) _param_ro_yaw_rate_i, (ParamFloat) _param_ro_yaw_accel_limit, - (ParamFloat) _param_ro_yaw_decel_limit + (ParamFloat) _param_ro_yaw_decel_limit, + (ParamFloat) _param_ro_yaw_rate_corr ) }; diff --git a/src/modules/rover_mecanum/module.yaml b/src/modules/rover_mecanum/module.yaml index 388c95edda..778f548ac2 100644 --- a/src/modules/rover_mecanum/module.yaml +++ b/src/modules/rover_mecanum/module.yaml @@ -15,23 +15,6 @@ parameters: decimal: 3 default: 0 - RM_MAX_THR_YAW_R: - description: - short: Yaw rate turning left/right wheels at max speed in opposite directions - long: | - This parameter is used to calculate the feedforward term of the closed loop yaw rate control. - The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate. - This desired speed difference is then linearly mapped to normalized motor commands. - A good starting point is half the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)). - Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0 - RM_COURSE_CTL_TH: description: short: Threshold to update course control in manual position mode