aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: constraint SITL rover to 360deg/s yaw rate on ackerman

Browse Source 
This prevent the gsf to blow up due to the rover reaching 1042 deg/s yaw rate.
This commit is contained in:
Pierre Kancir 2023-03-07 00:12:40 +01:00 committed by Andrew Tridgell
parent 763702aa1a
commit f0777b5cb0
2 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/SITL/SIM_Rover.cpp
View File

@ -63,10 +63,10 @@ float SimRover::turn_circle(float steering) const
float SimRover::calc_yaw_rate(float steering, float speed)
{
if (skid_steering) {
return steering * skid_turn_rate;
return constrain_float(steering * skid_turn_rate, -MAX_YAW_RATE, MAX_YAW_RATE);
}
if (vectored_thrust) {
return steering * vectored_turn_rate_max;
return constrain_float(steering * vectored_turn_rate_max, -MAX_YAW_RATE, MAX_YAW_RATE);
}
if (fabsf(steering) < 1.0e-6 or fabsf(speed) < 1.0e-6) {
return 0;
@ -74,7 +74,7 @@ float SimRover::calc_yaw_rate(float steering, float speed)
float d = turn_circle(steering);
float c = M_PI * d;
float t = c / speed;
float rate = 360.0f / t;
float rate = constrain_float(360.0f / t, -MAX_YAW_RATE, MAX_YAW_RATE);
return rate;
}

1
libraries/SITL/SIM_Rover.h
View File

@ -38,6 +38,7 @@ public:
}
private:
static constexpr float MAX_YAW_RATE = 360.0; // MAX 360 deg/s yaw rate speed to not nuke the ekf gsf
float max_speed = 20.0f; // vehicle's maximum forward speed in m/s
float max_accel = 10.0f; // vehicle's maximum forward acceleration in m/s/s
float max_wheel_turn = 35.0f; // ackermann steering vehicle's maximum steering angle