// rate_ef_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw rate (all earth frame)
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, rate_change;
if (_accel_roll_max > 0.0f) {
rate_change_limit = _accel_roll_max * _dt;
// update feed forward roll rate after checking it is within acceleration limits
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;
} else {
_rate_ef_desired.x = roll_rate_ef;
}
if (_accel_pitch_max > 0.0f) {
rate_change_limit = _accel_pitch_max * _dt;
// update feed forward pitch rate after checking it is within acceleration limits
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.y = pitch_rate_ef;
}
if (_accel_yaw_max > 0.0f) {
rate_change_limit = _accel_yaw_max * _dt;
// update feed forward yaw rate after checking it is within acceleration limits
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, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX);
update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX);
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX);
// 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 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
_rate_bf_target += _rate_bf_desired;
// body-frame to motor outputs should be called separately
}
// rate_bf_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw rate (all body frame)
void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_rate_bf, float yaw_rate_bf)
{
Vector3f angle_ef_error;
float rate_change, rate_change_limit;
// update the rate feed forward with angular acceleration limits
if (_accel_roll_max > 0.0f) {
rate_change_limit = _accel_roll_max * _dt;
rate_change = roll_rate_bf - _rate_bf_desired.x;
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
_rate_bf_desired.x += rate_change;
} else {
_rate_bf_desired.x = roll_rate_bf;
}
// update the rate feed forward with angular acceleration limits
if (_accel_pitch_max > 0.0f) {
rate_change_limit = _accel_pitch_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.y = pitch_rate_bf;
}
if (_accel_yaw_max > 0.0f) {
rate_change_limit = _accel_yaw_max * _dt;
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;
}
// Update angle error
if (labs(_ahrs.pitch_sensor)<_acro_angle_switch) {
_acro_angle_switch = 6000;
// convert body-frame rates to earth-frame rates
frame_conversion_bf_to_ef(_rate_bf_desired, _rate_ef_desired);
// update earth frame angle targets and errors
update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX);
update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX);
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX);
// convert earth-frame angle errors to body-frame angle errors
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
} else {
_acro_angle_switch = 4500;
integrate_bf_rate_error_to_angle_errors();
if (frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error)) {
_angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor);
_angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor);
_angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor);
}
if (_angle_ef_target.y > 9000.0f) {
_angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
_angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.y);
_angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
}
if (_angle_ef_target.y < -9000.0f) {
_angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
_angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.y);
_angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
}
}
// convert body-frame angle errors to body-frame rate targets
update_rate_bf_targets();
// body-frame rate commands added
_rate_bf_target += _rate_bf_desired;
}
// angle_ef_roll_pitch_rate_ef_yaw_smooth - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) while smoothing the attitude based on the feel parameter
// smoothing_gain : a number from 1 to 50 with 1 being sluggish and 50 being very crisp
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)
{
float rate_ef_desired;
float rate_change_limit;
Vector3f angle_ef_error; // earth frame angle errors
// sanity check smoothing gain
smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);
// if accel limiting and feed forward enabled
if ((_accel_roll_max > 0.0f) && _rate_bf_ff_enabled) {
rate_change_limit = _accel_roll_max * _dt;
// 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_roll_max);
// 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_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX);
} else {
// target roll and pitch to desired input roll and pitch
_angle_ef_target.x = roll_angle_ef;
angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);
// set roll and pitch feed forward to zero
_rate_ef_desired.x = 0;
}
// constrain earth-frame angle targets
_angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max);
// if accel limiting and feed forward enabled
if ((_accel_pitch_max > 0.0f) && _rate_bf_ff_enabled) {
rate_change_limit = _accel_pitch_max * _dt;
// 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_pitch_max);
// 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_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX);
} else {
// target roll and pitch to desired input roll and pitch
_angle_ef_target.y = pitch_angle_ef;
angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);
// set roll and pitch feed forward to zero
_rate_ef_desired.y = 0;
}
// constrain earth-frame angle targets
_angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max);
if (_accel_yaw_max > 0.0f) {
// set earth-frame feed forward rate for yaw
rate_change_limit = _accel_yaw_max * _dt;
// update yaw rate target with acceleration limit
_rate_ef_desired.z += constrain_float(yaw_rate_ef - _rate_ef_desired.z, -rate_change_limit, rate_change_limit);
// 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);
} else {
// set yaw feed forward to zero
_rate_ef_desired.z = yaw_rate_ef;
// 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);
}
// convert earth-frame angle errors to body-frame angle errors
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
// 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 {
// convert earth-frame feed forward rates to body-frame feed forward rates
frame_conversion_ef_to_bf(Vector3f(0,0,_rate_ef_desired.z), _rate_bf_desired);
_rate_bf_target += _rate_bf_desired;
}
// body-frame to motor outputs should be called separately
}
// angle_ef_roll_pitch_rate_ef_yaw_smooth - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) while smoothing the attitude based on the feel parameter
// smoothing_gain : a number from 1 to 50 with 1 being sluggish and 50 being very crisp此函数通过目标角度计算出所需要的角速度，并更新了误差值
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)
{
float rate_ef_desired;//地面坐标系下的速度
float rate_change_limit;
Vector3f angle_ef_error; // earth frame angle errors地面坐标系下角度误差
// sanity check smoothing gain
smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);//把smoothing_gain约束在1到5之间，1的话反应迟钝，50反应迅速，反应慢的话在有风的情况下可能导致坠机
// if accel limiting and feed forward enabled
if ((_accel_roll_max > 0.0f) && _rate_bf_ff_enabled) {//如果roll角加速度(地面坐标系）最大值大于0且允许速度前馈？
rate_change_limit = _accel_roll_max * _dt;//_dt时间间隔，以秒为单位，此函数代表roll在单位时间内改变速率的最大值？即角加速度最大值？
// 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_roll_max);//target为实时目标点，不断更新，而遥控器的信号也是不断更新
// 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_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX);
} else {
// target roll and pitch to desired input roll and pitch
_angle_ef_target.x = roll_angle_ef;//目标角度为滚转角
angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);//角度误差为目标角度减去传感器的角度，限制在180度之内
// set roll and pitch feed forward to zero
_rate_ef_desired.x = 0;//所需要的滚转角速度设为0
}
// constrain earth-frame angle targets
_angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max);//约束目标角度
// if accel limiting and feed forward enabled
if ((_accel_pitch_max > 0.0f) && _rate_bf_ff_enabled) {
rate_change_limit = _accel_pitch_max * _dt;
// 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_pitch_max);
// 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_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX);
} else {
// target roll and pitch to desired input roll and pitch
_angle_ef_target.y = pitch_angle_ef;
angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);
// set roll and pitch feed forward to zero
_rate_ef_desired.y = 0;
}
// constrain earth-frame angle targets
_angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max);
if (_accel_yaw_max > 0.0f) {
// set earth-frame feed forward rate for yaw
rate_change_limit = _accel_yaw_max * _dt;
// update yaw rate target with acceleration limit
_rate_ef_desired.z += constrain_float(yaw_rate_ef - _rate_ef_desired.z, -rate_change_limit, rate_change_limit);
// 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);
} else {
// set yaw feed forward to zero
_rate_ef_desired.z = yaw_rate_ef;
// 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);
}
// convert earth-frame angle errors to body-frame angle errors
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
// 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 {
// convert earth-frame feed forward rates to body-frame feed forward rates
frame_conversion_ef_to_bf(Vector3f(0,0,_rate_ef_desired.z), _rate_bf_desired);
_rate_bf_target += _rate_bf_desired;
}
// body-frame to motor outputs should be called separately
}
