Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-09 01:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_AttitudeControl: fix get_steering_out_rate use of max turn rate and acceleration

Browse Source
This commit is contained in:
Randy Mackay 2018-05-10 13:54:40 +09:00
parent ab42b76c04
commit bff86fb74a
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -244,7 +244,8 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_st
// rate limit desired turn rate
if (is_positive(_steer_rate_max)) {
_desired_turn_rate = constrain_float(_desired_turn_rate, -_steer_rate_max, _steer_rate_max);
const float steer_rate_max_rad = radians(_steer_rate_max);
_desired_turn_rate = constrain_float(_desired_turn_rate, -steer_rate_max_rad, steer_rate_max_rad);
}
float scaler = 1.0f;
@ -281,16 +282,16 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_st
if (reversed) {
yaw_rate_earth *= -1.0f;
}
const float rate_error = (desired_rate - yaw_rate_earth) * scaler;
const float rate_error = (_desired_turn_rate - yaw_rate_earth) * scaler;
// record desired rate for logging purposes only
_steer_rate_pid.set_desired_rate(desired_rate);
_steer_rate_pid.set_desired_rate(_desired_turn_rate);
// pass error to PID controller
_steer_rate_pid.set_input_filter_all(rate_error);
// get feed-forward
const float ff = _steer_rate_pid.get_ff(desired_rate * scaler);
const float ff = _steer_rate_pid.get_ff(_desired_turn_rate * scaler);
// get p
const float p = _steer_rate_pid.get_p();