Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: cleanup angle_ef_roll_pitch_rate_ef_yaw_smooth

Browse Source
This commit is contained in:
Jonathan Challinger 2014-10-06 20:00:04 -07:00 committed by Randy Mackay
parent 779baa006d
commit 7224669399
1 changed files with 18 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -103,61 +103,59 @@ void AC_AttitudeControl::relax_bf_rate_controller()
void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef, float smoothing_gain) void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef, float smoothing_gain)
{ {
Vector3f angle_ef_error; // earth frame angle errors Vector3f angle_ef_error; // earth frame angle errors
float rate_change_limit;
// sanity check smoothing gain // sanity check smoothing gain
smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f); smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);
float linear_angle = _accel_rp_max/(smoothing_gain*smoothing_gain);
rate_change_limit = _accel_rp_max * _dt;
float rate_ef_desired;
float angle_to_target;
if (_accel_rp_max > 0.0f) { if (_accel_rp_max > 0.0f) {
float rate_ef_desired;
float rate_change_limit = _accel_rp_max * _dt;
// calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away // calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away
rate_ef_desired = sqrt_controller(roll_angle_ef-_angle_ef_target.x, smoothing_gain, _accel_rp_max); rate_ef_desired = sqrt_controller(roll_angle_ef-_angle_ef_target.x, smoothing_gain, _accel_rp_max);
_rate_ef_desired.x = constrain_float(rate_ef_desired, _rate_ef_desired.x-rate_change_limit, _rate_ef_desired.x+rate_change_limit); // apply acceleration limit to feed forward roll rate
_rate_ef_desired.x = constrain_float(rate_ef_desired, _rate_ef_desired.x-rate_change_limit, _rate_ef_desired.x+rate_change_limit);
// update earth-frame roll angle target using desired roll rate // update earth-frame roll angle target using desired roll rate
update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX); update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX);
// calculate earth-frame feed forward pitch rate using linear response when close to the target, sqrt response when we're further away // calculate earth-frame feed forward pitch rate using linear response when close to the target, sqrt response when we're further away
rate_ef_desired = sqrt_controller(pitch_angle_ef-_angle_ef_target.y, smoothing_gain, _accel_rp_max); rate_ef_desired = sqrt_controller(pitch_angle_ef-_angle_ef_target.y, smoothing_gain, _accel_rp_max);
_rate_ef_desired.y = constrain_float(rate_ef_desired, _rate_ef_desired.y-rate_change_limit, _rate_ef_desired.y+rate_change_limit); // apply acceleration limit to feed forward pitch rate
_rate_ef_desired.y = constrain_float(rate_ef_desired, _rate_ef_desired.y-rate_change_limit, _rate_ef_desired.y+rate_change_limit);
// update earth-frame pitch angle target using desired pitch rate // update earth-frame pitch angle target using desired pitch rate
update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX); update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX);
} else { } else {
// target roll and pitch to desired input roll and pitch // target roll and pitch to desired input roll and pitch
_angle_ef_target.x = roll_angle_ef; _angle_ef_target.x = roll_angle_ef;
angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor); 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 = pitch_angle_ef;
angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor); angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);
// set roll and pitch feed forward to zero // set roll and pitch feed forward to zero
_rate_ef_desired.x = 0; _rate_ef_desired.x = 0;
_rate_ef_desired.y = 0; _rate_ef_desired.y = 0;
} }
// constrain earth-frame angle targets // constrain earth-frame angle targets
_angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max); _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); _angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max);
if (_accel_y_max > 0.0f) { if (_accel_y_max > 0.0f) {
// set earth-frame feed forward rate for yaw // set earth-frame feed forward rate for yaw
rate_change_limit = _accel_y_max * _dt; float rate_change_limit = _accel_y_max * _dt;
// update yaw rate target with accele
_rate_ef_desired.z += constrain_float(yaw_rate_ef - _rate_ef_desired.z, -rate_change_limit, rate_change_limit);
float 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;
// calculate yaw target angle and angle error // calculate yaw target angle and angle error
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX);
} else { } else {
// set yaw feed forward to zero // set yaw feed forward to zero
_rate_ef_desired.z = yaw_rate_ef; _rate_ef_desired.z = yaw_rate_ef;
// calculate yaw target angle and angle error // calculate yaw target angle and angle error
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX);
} }