APM关键姿态控制源码讲解
手控姿态增稳函数
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;
}
}
