Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 18:48:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_AttitudeControl: get_turn_rate_from_heading applies acceleration limit

Browse Source 
Limits rate output so that vehicle will not overshoot the target heading
This commit is contained in:
Randy Mackay 2022-02-08 19:24:03 +09:00
parent 95b8630a09
commit 619f4976a2
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -501,6 +501,12 @@ float AR_AttitudeControl::get_turn_rate_from_heading(float heading_rad, float ra
desired_rate = constrain_float(desired_rate, -rate_max_rads, rate_max_rads);
}
// if acceleration limit is provided, ensure rate can be slowed to zero in time to stop at heading_rad (i.e. avoid overshoot)
if (is_positive(_steer_accel_max)) {
const float steer_accel_rate_max_rads = safe_sqrt(2.0 * fabsf(yaw_error) * radians(_steer_accel_max));
desired_rate = constrain_float(desired_rate, -steer_accel_rate_max_rads, steer_accel_rate_max_rads);
}
return desired_rate;
}