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

AC_AttControl: Fix feedforward behavior

Browse Source
This commit is contained in:
lthall 2014-07-12 21:16:13 +09:30 committed by Randy Mackay
parent 581838b271
commit 2889f5abc4
1 changed files with 53 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -144,6 +144,9 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle
_rate_ef_desired.x = 0;
_rate_ef_desired.y = 0;
}
// constrain earth-frame angle targets
_angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max);
_angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max);
if (_accel_y_max > 0.0f) {
// set earth-frame feed forward rate for yaw
@ -156,26 +159,27 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error);
} else {
// set yaw feed forward to zero
_rate_ef_desired.z = 0;
_rate_ef_desired.z = yaw_rate_ef;
// calculate yaw target angle and angle error
update_ef_yaw_angle_and_error(yaw_rate_ef, angle_ef_error);
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error);
}
// constrain earth-frame angle targets
_angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max);
_angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max);
// convert earth-frame angle errors to body-frame angle errors
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
// convert earth-frame feed forward rates to body-frame feed forward rates
frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);
// convert body-frame angle errors to body-frame rate targets
update_rate_bf_targets();
// add body frame rate feed forward
if (_rate_bf_ff_enabled) {
// convert earth-frame feed forward rates to body-frame feed forward rates
frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);
_rate_bf_target += _rate_bf_desired;
} else {
Vector3f rate_ef_desired(0,0,_rate_ef_desired.z); // set roll and pitch feed forward to zero
// convert earth-frame feed forward rates to body-frame feed forward rates
frame_conversion_ef_to_bf(rate_ef_desired, _rate_bf_desired);
_rate_bf_target += _rate_bf_desired;
}
@ -192,10 +196,10 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw(float roll_angle_ef, fl
Vector3f angle_ef_error; // earth frame angle errors
// set earth-frame angle targets for roll and pitch and calculate angle error
_angle_ef_target.x = roll_angle_ef;
_angle_ef_target.x = constrain_float(roll_angle_ef, -_aparm.angle_max, _aparm.angle_max);
angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);
_angle_ef_target.y = pitch_angle_ef;
_angle_ef_target.y = constrain_float(pitch_angle_ef, -_aparm.angle_max, _aparm.angle_max);
angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);
if (_accel_y_max > 0.0f) {
@ -209,28 +213,23 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw(float roll_angle_ef, fl
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error);
} else {
// set yaw feed forward to zero
_rate_ef_desired.z = 0;
_rate_ef_desired.z = yaw_rate_ef;
// calculate yaw target angle and angle error
update_ef_yaw_angle_and_error(yaw_rate_ef, angle_ef_error);
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error);
}
// constrain earth-frame angle targets
_angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max);
_angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max);
// convert earth-frame angle errors to body-frame angle errors
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
// convert earth-frame feed forward rates to body-frame feed forward rates
frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);
// convert body-frame angle errors to body-frame rate targets
update_rate_bf_targets();
// add body frame rate feed forward
if (_rate_bf_ff_enabled) {
_rate_bf_target.z += _rate_bf_desired.z;
}
// set roll and pitch feed forward to zero
_rate_ef_desired.x = 0;
_rate_ef_desired.y = 0;
// convert earth-frame feed forward rates to body-frame feed forward rates
frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);
_rate_bf_target += _rate_bf_desired;
// body-frame to motor outputs should be called separately
}
@ -269,10 +268,13 @@ void AC_AttitudeControl::angle_ef_roll_pitch_yaw(float roll_angle_ef, float pitc
void AC_AttitudeControl::rate_ef_roll_pitch_yaw(float roll_rate_ef, float pitch_rate_ef, float yaw_rate_ef)
{
Vector3f angle_ef_error;
float rate_change_limit = _accel_rp_max * _dt;
float rate_change_limit, rate_change;
if (_accel_rp_max > 0.0f) {
rate_change_limit = _accel_rp_max * _dt;
// update feed forward roll rate after checking it is within acceleration limits
float rate_change = roll_rate_ef - _rate_ef_desired.x;
rate_change = roll_rate_ef - _rate_ef_desired.x;
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
_rate_ef_desired.x += rate_change;
@ -280,12 +282,21 @@ void AC_AttitudeControl::rate_ef_roll_pitch_yaw(float roll_rate_ef, float pitch_
rate_change = pitch_rate_ef - _rate_ef_desired.y;
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
_rate_ef_desired.y += rate_change;
} else {
_rate_ef_desired.x = roll_rate_ef;
_rate_ef_desired.y = pitch_rate_ef;
}
if (_accel_y_max > 0.0f) {
rate_change_limit = _accel_y_max * _dt;
// update feed forward yaw rate after checking it is within acceleration limits
rate_change_limit = _accel_y_max * _dt;
rate_change = yaw_rate_ef - _rate_ef_desired.z;
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
_rate_ef_desired.z += rate_change;
} else {
_rate_ef_desired.z = yaw_rate_ef;
}
// update earth frame angle targets and errors
update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error);
@ -299,16 +310,14 @@ void AC_AttitudeControl::rate_ef_roll_pitch_yaw(float roll_rate_ef, float pitch_
// convert earth-frame angle errors to body-frame angle errors
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
// convert earth-frame rates to body-frame rates
frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);
// convert body-frame angle errors to body-frame rate targets
update_rate_bf_targets();
// convert earth-frame rates to body-frame rates
frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);
// add body frame rate feed forward
if (_rate_bf_ff_enabled) {
_rate_bf_target += _rate_bf_desired;
}
// body-frame to motor outputs should be called separately
}
@ -382,15 +391,7 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_
}
// body-frame rate commands added
if (_rate_bf_ff_enabled) {
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;
}
}
_rate_bf_target += _rate_bf_desired;
}
//