fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 23:11: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-18 08:35:48 +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/lib/rover_control/RoverControl.cpp
+7 -6
View File
@@ -161,7 +161,8 @@ float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, const
}
float rateControl(SlewRate<float> &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<float> &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<float>(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<float>(speed_diff, -max_thr_speed,
max_thr_speed, -1.f, 1.f);
}
// Feedback control