Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttControl: check accel_rp_max instead of rate_bff_ff_enabled

Browse Source 
We use the accel_rp_max, accel_y_max to check whether to apply accel
limiting or not.  This is related to separate from the
body-frame-feed-forward.
This commit is contained in:
Randy Mackay 2014-06-06 19:40:38 +09:00
parent 91b1d20b70
commit b57c0dabf6
1 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -103,7 +103,7 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle
float rate_ef_desired;
float angle_to_target;
if (_rate_bf_ff_enabled) {
if (_accel_rp_max > 0.0f) {
// calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away
angle_to_target = roll_angle_ef - _angle_ef_target.x;
@ -145,7 +145,7 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle
_rate_ef_desired.y = 0;
}
if (_rate_bf_ff_enabled) {
if (_accel_y_max > 0.0f) {
// set earth-frame feed forward rate for yaw
rate_change_limit = _accel_y_max * _dt;
@ -221,7 +221,7 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw(float roll_angle_ef, fl
// add body frame rate feed forward
if (_rate_bf_ff_enabled) {
_rate_bf_target += _rate_bf_desired;
_rate_bf_target.z += _rate_bf_desired.z;
}
// body-frame to motor outputs should be called separately
@ -348,7 +348,7 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_
float rate_change, rate_change_limit;
// update the rate feed forward with angular acceleration limits
if (_rate_bf_ff_enabled) {
if (_accel_rp_max > 0.0f) {
rate_change_limit = _accel_rp_max * _dt;
rate_change = roll_rate_bf - _rate_bf_desired.x;
@ -363,7 +363,7 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_
_rate_bf_desired.y = pitch_rate_bf;
}
if (_rate_bf_ff_enabled) {
if (_accel_y_max > 0.0f) {
rate_change_limit = _accel_y_max * _dt;
rate_change = yaw_rate_bf - _rate_bf_desired.z;
@ -375,7 +375,13 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_
// body-frame rate commands added
if (_rate_bf_ff_enabled) {
_rate_bf_target += _rate_bf_desired;
if (_accel_rp_max > 0.0f) {
_rate_bf_target.x += _rate_bf_desired.x;
_rate_bf_target.y += _rate_bf_desired.y;
}
if (_accel_y_max > 0.0f) {
_rate_bf_target.z += _rate_bf_desired.z;
}
}
}