aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttControl: zero _accel_xyz_max stops feed forward

Browse Source 
Also added place holder for turning off feed forward.
This commit is contained in:
lthall 2014-06-05 00:16:29 +09:30 committed by Randy Mackay
parent babe655b8f
commit c24d293e1b
1 changed files with 77 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -100,42 +100,58 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle
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_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);
// calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away if(_accel_rp_max > 0){
angle_to_target = roll_angle_ef - _angle_ef_target.x; // calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away
if (angle_to_target > linear_angle){ angle_to_target = roll_angle_ef - _angle_ef_target.x;
rate_ef_desired = safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f))); if (angle_to_target > linear_angle){
} else if (angle_to_target < -linear_angle){ rate_ef_desired = safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
rate_ef_desired = -safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f))); } else if (angle_to_target < -linear_angle){
rate_ef_desired = -safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else {
rate_ef_desired = smoothing_gain*angle_to_target;
}
_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
_angle_ef_target.x += _rate_ef_desired.x*_dt;
// calculate earth-frame feed forward pitch rate using linear response when close to the target, sqrt response when we're further away
angle_to_target = pitch_angle_ef - _angle_ef_target.y;
if (angle_to_target > linear_angle){
rate_ef_desired = safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else if (angle_to_target < -linear_angle){
rate_ef_desired = -safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else {
rate_ef_desired = smoothing_gain*angle_to_target;
}
_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
_angle_ef_target.y += _rate_ef_desired.y*_dt;
} else { } else {
rate_ef_desired = smoothing_gain*angle_to_target; // target roll and pitch to desired input roll and pitch
_angle_ef_target.x = roll_angle_ef;
_angle_ef_target.y = pitch_angle_ef;
// set roll and pitch feed forward to zero
_rate_ef_desired.x = 0;
_rate_ef_desired.y = 0;
} }
_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 if(_accel_y_max > 0){
_angle_ef_target.x += _rate_ef_desired.x*_dt; // set earth-frame feed forward rate for yaw
rate_change_limit = _accel_y_max * _dt;
// calculate earth-frame feed forward pitch rate using linear response when close to the target, sqrt response when we're further away float rate_change = yaw_rate_ef - _rate_ef_desired.z;
angle_to_target = pitch_angle_ef - _angle_ef_target.y; rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
if (angle_to_target > linear_angle){ _rate_ef_desired.z += rate_change;
rate_ef_desired = safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f))); // calculate yaw target angle and angle error
} else if (angle_to_target < -linear_angle){ update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error);
rate_ef_desired = -safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else { } else {
rate_ef_desired = smoothing_gain*angle_to_target; // set yaw feed forward to zero
_rate_ef_desired.z = 0;
// calculate yaw target angle and angle error
update_ef_yaw_angle_and_error(yaw_rate_ef, angle_ef_error);
} }
_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
_angle_ef_target.y += _rate_ef_desired.y*_dt;
// set earth-frame feed forward rate for yaw
rate_change_limit = _accel_y_max * _dt;
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
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error);
// convert earth-frame angle errors to body-frame angle errors // convert earth-frame angle errors to body-frame angle errors
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error); frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
@ -147,7 +163,9 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle
update_rate_bf_targets(); update_rate_bf_targets();
// add body frame rate feed forward // add body frame rate feed forward
// if(do feedforward){
_rate_bf_target += _rate_bf_desired; _rate_bf_target += _rate_bf_desired;
// }
// body-frame to motor outputs should be called separately // body-frame to motor outputs should be called separately
} }
@ -186,7 +204,9 @@ void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw(float roll_angle_ef, fl
update_rate_bf_targets(); update_rate_bf_targets();
// add body frame rate feed forward // add body frame rate feed forward
// if(do feedforward){
_rate_bf_target += _rate_bf_desired; _rate_bf_target += _rate_bf_desired;
// }
// body-frame to motor outputs should be called separately // body-frame to motor outputs should be called separately
} }
@ -258,7 +278,9 @@ void AC_AttitudeControl::rate_ef_roll_pitch_yaw(float roll_rate_ef, float pitch_
update_rate_bf_targets(); update_rate_bf_targets();
// add body frame rate feed forward // add body frame rate feed forward
// if(do feedforward){
_rate_bf_target += _rate_bf_desired; _rate_bf_target += _rate_bf_desired;
// }
// body-frame to motor outputs should be called separately // body-frame to motor outputs should be called separately
} }
@ -306,26 +328,36 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_
float rate_change, rate_change_limit; float rate_change, rate_change_limit;
// update the rate feed forward with angular acceleration limits // update the rate feed forward with angular acceleration limits
rate_change_limit = _accel_rp_max * _dt;
rate_change = roll_rate_bf - _rate_bf_desired.x; if (_accel_rp_max > 0) {
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); rate_change_limit = _accel_rp_max * _dt;
_rate_bf_desired.x += rate_change;
rate_change = pitch_rate_bf - _rate_bf_desired.y; rate_change = roll_rate_bf - _rate_bf_desired.x;
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
_rate_bf_desired.y += rate_change; _rate_bf_desired.x += rate_change;
rate_change_limit = _accel_y_max * _dt; rate_change = pitch_rate_bf - _rate_bf_desired.y;
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
_rate_bf_desired.y += rate_change;
} else {
_rate_bf_desired.x = roll_rate_bf;
_rate_bf_desired.y = pitch_rate_bf;
}
rate_change = yaw_rate_bf - _rate_bf_desired.z; if (_accel_y_max > 0) {
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); rate_change_limit = _accel_y_max * _dt;
_rate_bf_desired.z += rate_change;
rate_change = yaw_rate_bf - _rate_bf_desired.z;
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
_rate_bf_desired.z += rate_change;
} else {
_rate_bf_desired.z = yaw_rate_bf;
}
// body-frame rate commands added // body-frame rate commands added
// if(do feedforward){
_rate_bf_target += _rate_bf_desired; _rate_bf_target += _rate_bf_desired;
// }
// body-frame to motor outputs should be called separately
} }
// //