fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 09:00:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

rover: replace RX_MAX_THR_YAW_R with a correction factor RO_YAW_RATE_CORR

Browse Source
This commit is contained in:
chfriedrich98
2025-06-16 11:00:04 +02:00
committed by chfriedrich98
parent 3e8f054a1c
commit f35b92a487
16 changed files with 51 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp
+2 -1
View File
@@ -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) {
src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp
+2 -1
View File
@@ -123,6 +123,7 @@ private:
(ParamFloat<px4::params::RO_YAW_RATE_P>) _param_ro_yaw_rate_p,
(ParamFloat<px4::params::RO_YAW_RATE_I>) _param_ro_yaw_rate_i,
(ParamFloat<px4::params::RO_YAW_ACCEL_LIM>) _param_ro_yaw_accel_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::RO_YAW_RATE_CORR>) _param_ro_yaw_rate_corr
)
};
src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp
+4 -3
View File
@@ -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<float, float, float>(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;
src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp
+3 -2
View File
@@ -107,11 +107,12 @@ private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RD_WHEEL_TRACK>) _param_rd_wheel_track,
(ParamFloat<px4::params::RD_MAX_THR_YAW_R>) _param_rd_max_thr_yaw_r,
(ParamFloat<px4::params::RO_YAW_RATE_TH>) _param_ro_yaw_rate_th,
(ParamFloat<px4::params::RO_YAW_RATE_P>) _param_ro_yaw_rate_p,
(ParamFloat<px4::params::RO_YAW_RATE_I>) _param_ro_yaw_rate_i,
(ParamFloat<px4::params::RO_YAW_ACCEL_LIM>) _param_ro_yaw_accel_limit,
(ParamFloat<px4::params::RO_YAW_DECEL_LIM>) _param_ro_yaw_decel_limit
(ParamFloat<px4::params::RO_YAW_DECEL_LIM>) _param_ro_yaw_decel_limit,
(ParamFloat<px4::params::RO_YAW_RATE_CORR>) _param_ro_yaw_rate_corr,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
)
};
src/modules/rover_differential/module.yaml
-17
View File
@@ -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
src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp
+5 -5
View File
@@ -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<float, float, float>(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;
src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp
+3 -2
View File
@@ -107,12 +107,13 @@ private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RM_WHEEL_TRACK>) _param_rm_wheel_track,
(ParamFloat<px4::params::RM_MAX_THR_YAW_R>) _param_rm_max_thr_yaw_r,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_RATE_TH>) _param_ro_yaw_rate_th,
(ParamFloat<px4::params::RO_YAW_RATE_P>) _param_ro_yaw_rate_p,
(ParamFloat<px4::params::RO_YAW_RATE_I>) _param_ro_yaw_rate_i,
(ParamFloat<px4::params::RO_YAW_ACCEL_LIM>) _param_ro_yaw_accel_limit,
(ParamFloat<px4::params::RO_YAW_DECEL_LIM>) _param_ro_yaw_decel_limit
(ParamFloat<px4::params::RO_YAW_DECEL_LIM>) _param_ro_yaw_decel_limit,
(ParamFloat<px4::params::RO_YAW_RATE_CORR>) _param_ro_yaw_rate_corr
)
};
src/modules/rover_mecanum/module.yaml
-17
View File
@@ -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