手控姿態增穩函數
Vector3f _angle_ef_target; // angle controller earth-frame targets
Vector3f _angle_bf_error; // angle controller body-frame error
Vector3f _rate_bf_target; // rate controller body-frame targets
Vector3f _rate_ef_desired; // earth-frame feed forward rates
Vector3f _rate_bf_desired; // body-frame feed forward rates
大概流程：
① RC信號進入
② RC信號匹配成角度（-4500~4500度）
③ RC角度生成目標滾轉角、目標俯仰角、目標航向速率、油門比例
④ RC目標值轉換成ef目標值
⑤ ef目標值變成ef速率目標值
⑥ 計算出ef_error
⑦ ef_error轉換成bf_error
⑧ 計算出bf_rate速率目標值
⑨ bf_rate目標值傳入速率控制函數即PID
自穩模式解讀之初始化解讀：
自穩初始化：
1）首先是判斷條件：解鎖&&着陸&&加鎖模式&&油門過高 如果為真就返false
2）初始化目標高度為0。
/// set_alt_target - set altitude target in cm above home
void set_alt_target(float alt_cm) { _pos_target.z = alt_cm; }
// stabilize_run - runs the main stabilize controller 運行主要增穩控制
// should be called at 100hz or more
static void stabilize_run()
{
float target_roll, target_pitch;
float target_yaw_rate;
int16_t pilot_throttle_scaled;
// if not armed or throttle at zero, set throttle to zero and exit immediately
//判斷是否解鎖或者油門是零 立即退出 重置積分I和油門置為零
if(!motors.armed() || g.rc_3.control_in <= 0) {
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
attitude_control.set_throttle_out(0, false);
return;
}
// apply SIMPLE mode transform to pilot inputs
//這是簡單飛行模式選擇
update_simple_mode();
// convert pilot input to lean angles RC輸入轉化成飛行器的傾斜角度
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
//這個函數主要是控制滾轉和俯仰之間的關系
//輸入g.rc_1.control_in，g.rc_2.control_in(-4500-4500)，輸出target_roll，target_pitch
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, target_roll, target_pitch);
// get pilot's desired yaw rate 獲得飛行器的期望航向速率
//返回值 return stick_angle * g.acro_yaw_p; 輸入值乘以系數P
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
// get pilot's desired throttle 獲得飛行器的期望油門
pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in);
// call attitude controller 姿態控制 地理坐標系
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// body-frame rate controller is run directly from 100hz loop 機體速率控制100HZ
// output pilot's throttle 油門輸出
attitude_control.set_throttle_out(pilot_throttle_scaled, true);
}
關鍵函數解讀
1、
// get_smoothing_gain - returns smoothing gain to be passed into attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth
//平滑增益 飛行器正對輸入的響應的滯帶還是很脆
// result is a number from 2 to 12 with 2 being very sluggish and 12 being very crisp
float get_smoothing_gain()
{
return (2.0f + (float)g.rc_feel_rp/10.0f);
}
2、增穩中關鍵函數
// 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);[B1]
// 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
//ef 目標速率轉換成bf目標速率
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
//ef 目標速率轉換成bf目標速率
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
}
ef轉換bf
// earth-frame <-> body-frame conversion functions
// frame_conversion_ef_to_bf - converts earth frame vector to body frame vector
void AC_AttitudeControl::frame_conversion_ef_to_bf(const Vector3f& ef_vector, Vector3f& bf_vector)
{
// convert earth frame rates to body frame rates
bf_vector.x = ef_vector.x - _ahrs.sin_pitch() * ef_vector.z;
bf_vector.y = _ahrs.cos_roll() * ef_vector.y + _ahrs.sin_roll() * _ahrs.cos_pitch() * ef_vector.z;
bf_vector.z = -_ahrs.sin_roll() * ef_vector.y + _ahrs.cos_pitch() * _ahrs.cos_roll() * ef_vector.z;
}
// sqrt_controller - response based on the sqrt of the error instead of the more common linear response
float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim)
{
if (second_ord_lim == 0.0f || p == 0.0f) {
return error*p;
}
float linear_dist = second_ord_lim/sq(p);
//當地理坐標系角度誤差大於線性列表
if (error > linear_dist) {
return safe_sqrt(2.0f*second_ord_lim*(error-(linear_dist/2.0f)));
} else if (error < -linear_dist) {
return -safe_sqrt(2.0f*second_ord_lim*(-error-(linear_dist/2.0f)));
} else {
return error*p;
}
}