PX4 FMU [17] stabilize
PX4 FMU [17] stabilize
-------- 转载请注明出处
-------- 更多笔记请访问我的博客：merafour.blog.163.com
-------- 2015-3-2.冷月追风
-------- email：merafour@163.com
1.stabilize_init
到目前为止，在 APM官网上公布的飞行模式有十几种，" stabilize"也只不过是其中一种，即自稳，或者叫姿态。处于该飞行模式下， roll跟 pitch摇杆控制的是飞行器的倾斜角度，油门控制的是电机的平均转速，特点是摇杆回中则姿态回到水平。
在 "ardupilot/ArduCopter"目录下有不少以 "control_"开头的源文件，大部分都各自对应了一种飞行模式。
bitcraze@bitcraze-vm:~/apm$ ls ardupilot/ArduCopter/control_*
ardupilot/ArduCopter/control_acro.pde ardupilot/ArduCopter/control_flip.pde ardupilot/ArduCopter/control_poshold.pde
ardupilot/ArduCopter/control_althold.pde ardupilot/ArduCopter/control_guided.pde ardupilot/ArduCopter/control_rtl.pde
ardupilot/ArduCopter/control_auto.pde ardupilot/ArduCopter/control_land.pde ardupilot/ArduCopter/control_sport.pde
ardupilot/ArduCopter/control_autotune.pde ardupilot/ArduCopter/control_loiter.pde ardupilot/ArduCopter/control_stabilize.pde
ardupilot/ArduCopter/control_circle.pde ardupilot/ArduCopter/control_modes.pde
ardupilot/ArduCopter/control_drift.pde ardupilot/ArduCopter/control_ofloiter.pde
bitcraze@bitcraze-vm:~/apm$
所以" stabilize"模式的相关代码在 "ardupilot/ArduCopter/control_stabilize.pde"文件中。
在 "ardupilot/ArduCopter/control_stabilize.pde"源文件中其实就俩函数： stabilize_init跟 stabilize_run。从函数名就很容易看出来前者是用来初始化的，后者则是该飞行模式区别与其它飞行模式的主要代码。
// stabilize_init - initialise stabilize controller
static bool stabilize_init(bool ignore_checks)
{
// set target altitude to zero for reporting
// To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error?
pos_control.set_alt_target(0);
// stabilize should never be made to fail
return true;
}
初始化倒是挺简单的，但其实 "pos_control"并没有在 stabilize_run函数中使用，而这里显然是设置高度的初值的。既然都不使用，那么设置它干嘛？但是别忘了还有定高定点这样一些模式，这里是不使用，但是它们要用啊。你想想，当我以 " stabilize"模式起飞，在半空中直接切到定高模式，那么总不能以此时的高度作为初始高度，这显然不合适，而初始高度最合理的当然是起飞是的高度，所以最好的办法就是在这里设置初值。
下面一个问题是 stabilize_init函数是在哪里调用的？这个跟踪下代码就清楚了：
bitcraze@bitcraze-vm:~/apm$ grep -nr stabilize_init ardupilot/ArduCopter/
ardupilot/ArduCopter/flight_mode.pde:34: success = heli_stabilize_init(ignore_checks);
ardupilot/ArduCopter/flight_mode.pde:36: success = stabilize_init(ignore_checks);
ardupilot/ArduCopter/control_stabilize.pde:7:// stabilize_init - initialise stabilize controller
ardupilot/ArduCopter/control_stabilize.pde:8:static bool stabilize_init(boolignore_checks)
ardupilot/ArduCopter/heli_control_stabilize.pde:7:// stabilize_init - initialise stabilize controller
ardupilot/ArduCopter/heli_control_stabilize.pde:8:static bool heli_stabilize_init(boolignore_checks)
bitcraze@bitcraze-vm:~/apm$
static bool set_mode(uint8_t mode)
{
// boolean to record if flight mode could be set
bool success = false;
bool ignore_checks = !motors.armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
// return immediately if we are already in the desired mode
if (mode == control_mode) {
return true;
}
switch(mode) {
case STABILIZE:
#if FRAME_CONFIG == HELI_FRAME
success = heli_stabilize_init(ignore_checks);
#else
success = stabilize_init(ignore_checks);
#endif
break;
代码跟踪到这里直接匹配的话结果很多，工作量比较大，而且很多类都由这个方法。
此时又得回到 scheduler_tasks数组中了，其中第一个函数是：rc_loop，
static void rc_loop()
{
// Read radio and 3-position switch on radio
// -----------------------------------------
read_radio();
read_control_switch();
}
#define CONTROL_SWITCH_COUNTER 20 // 20 iterations at 100hz (i.e. 2/10th of a second) at a new switch position will cause flight mode change
static void read_control_switch()
{
static uint8_t switch_counter = 0;
uint8_t switchPosition = readSwitch();
// has switch moved?
// ignore flight mode changes if in failsafe
if (oldSwitchPosition != switchPosition && !failsafe.radio && failsafe.radio_counter == 0) {
switch_counter++;
if(switch_counter >= CONTROL_SWITCH_COUNTER) {
oldSwitchPosition = switchPosition;
switch_counter = 0;
// set flight mode and simple mode setting
if (set_mode(flight_modes[switchPosition])) {
if(g.ch7_option != AUX_SWITCH_SIMPLE_MODE && g.ch8_option != AUX_SWITCH_SIMPLE_MODE && g.ch7_option != AUX_SWITCH_SUPERSIMPLE_MODE && g.ch8_option != AUX_SWITCH_SUPERSIMPLE_MODE) {
// set Simple mode using stored paramters from Mission planner
// rather than by the control switch
if (BIT_IS_SET(g.super_simple, switchPosition)) {
set_simple_mode(2);
}else{
set_simple_mode(BIT_IS_SET(g.simple_modes, switchPosition));
}
}
}
}
}else{
// reset switch_counter if there's been no change
// we don't want 10 intermittant blips causing a flight mode change
switch_counter = 0;
}
}
可能你会觉得为什么是在这里？在别的地方其实可以找到想这样的调用："set_mode(STABILIZE)"，但那不是我们要找的代码，为什么？因为这里才是我们在上位机上设置的飞行模式。也许那部分代码也值得去看一看，但鉴于对 PX4源码还不是很熟悉，暂时就先放一放。
2.stabilize_run
那么现在我们就来看看 stabilize_run这个函数。
// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
static void stabilize_run()
{
int16_t 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
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
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
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
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
// output pilot's throttle
attitude_control.set_throttle_out(pilot_throttle_scaled, true);
}
这个函数代码倒不是很多，理解起来困难也不是太大。
当然，任何飞行模式都需要区分解锁跟加锁。需要说明的是 PX4采用的是 PID控制器级联的方式进行姿态控制的，即 PD+PID。而我们在这里看到的其实只是 PD控制器。
在没有解锁或者油门为 0的时候，做了三件事。关闭输出自然不必多说，设置机头也能够理解，关键是 "relax_bf_rate_controller"，这是个什么东西？
// relax_bf_rate_controller - ensure body-frame rate controller has zero errors to relax rate controller output
void AC_AttitudeControl::relax_bf_rate_controller()
{
// ensure zero error in body frame rate controllers
const Vector3f& gyro = _ins.get_gyro();
_rate_bf_target = gyro * AC_ATTITUDE_CONTROL_DEGX100;
}
从代码上看的话，其实就是关闭 PD输出，从而关闭 PID输出。
接下来的 update_simple_mode函数其实包含了简单模式跟超级简单模式。两者都是无头模式，更多的介绍见 APM官网介绍。
然后就是四个通道的映射。其实前面已经映射过一次了，为什么这里还要再映射一次？最直接的解释就是前面映射的并不是我们这里需要的。其实那一步映射每一种飞行模式都需要用到，那我想两次映射也许是 APM觉得比较好的一个做法了吧。
// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int16_t &roll_out, int16_t &pitch_out)
{
static float _scaler = 1.0;
static int16_t _angle_max = 0;
// range check the input
roll_in = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
pitch_in = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
// return filtered roll if no scaling required
if (aparm.angle_max == ROLL_PITCH_INPUT_MAX) {
roll_out = roll_in;
pitch_out = pitch_in;
return;
}
// check if angle_max has been updated and redo scaler
if (aparm.angle_max != _angle_max) {
_angle_max = aparm.angle_max;
_scaler = (float)aparm.angle_max/(float)ROLL_PITCH_INPUT_MAX;
}
// convert pilot input to lean angle
roll_out = (int16_t)((float)roll_in * _scaler);
pitch_out = (int16_t)((float)pitch_in * _scaler);
}
// get_pilot_desired_heading - transform pilot's yaw input into a desired heading
// returns desired angle in centi-degrees
// To-Do: return heading as a float?
static float get_pilot_desired_yaw_rate(int16_t stick_angle)
{
// convert pilot input to the desired yaw rate
return stick_angle * g.acro_yaw_p;
}
上面我们看到的是姿态跟偏航的映射。对于之态，如果行程不一样(默认是+/-45°)则计算一个系数进行修正。而对于偏航则通过一个系数进行调正。但是对于三通道油门就稍微复杂一点，原因是三通道进行了分段：
// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
// used only for manual throttle modes
// returns throttle output 0 to 1000
#define THROTTLE_IN_MIDDLE 500 // the throttle mid point
static int16_t get_pilot_desired_throttle(int16_t throttle_control)
{
int16_t throttle_out;
// exit immediately in the simple cases
if( throttle_control == 0 || g.throttle_mid == 500) {
return throttle_control;
}
// ensure reasonable throttle values
throttle_control = constrain_int16(throttle_control,0,1000);
g.throttle_mid = constrain_int16(g.throttle_mid,300,700);
// check throttle is above, below or in the deadband
if (throttle_control < THROTTLE_IN_MIDDLE) {
// below the deadband
throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(g.throttle_mid - g.throttle_min))/((float)(500-g.throttle_min));
}else if(throttle_control > THROTTLE_IN_MIDDLE) {
// above the deadband
throttle_out = g.throttle_mid + ((float)(throttle_control-500))*(float)(1000-g.throttle_mid)/500.0f;
}else{
// must be in the deadband
throttle_out = g.throttle_mid;
}
return throttle_out;
}
虽然进行了分段，但把每一段剖开其实还是等比例映射。最复杂的当然还是 angle_ef_roll_pitch_rate_ef_yaw_smooth，它是 PD控制器，自然不会简单。所以我们还是先从最简单的开始，先来看看 set_throttle_out函数：
// set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors
// provide 0 to cut motors
void AC_AttitudeControl::set_throttle_out(int16_t throttle_out, boolapply_angle_boost)
{
if (apply_angle_boost) {
_motors.set_throttle(get_angle_boost(throttle_out));
}else{
_motors.set_throttle(throttle_out);
// clear angle_boost for logging purposes
_angle_boost = 0;
}
// update compass with throttle value
// To-Do: find another method to grab the throttle out and feed to the compass. Could be done completely outside this class
//compass.set_throttle((float)g.rc_3.servo_out/1000.0f);
}
void set_throttle(int16_t throttle_in) { _rc_throttle.servo_out = throttle_in; }; // range 0 ~ 1000
其实我们看到，最终将油门数据写入电机是可以选择进行补偿的，补偿方式如下：
// get_angle_boost - returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1000
int16_t AC_AttitudeControl::get_angle_boost(int16_t throttle_pwm)
{
float temp = _ahrs.cos_pitch() * _ahrs.cos_roll();
int16_t throttle_out;
temp = constrain_float(temp, 0.5f, 1.0f);
// reduce throttle if we go inverted
temp = constrain_float(9000-max(labs(_ahrs.roll_sensor),labs(_ahrs.pitch_sensor)), 0, 3000) / (3000 * temp);
// apply scale and constrain throttle
// To-Do: move throttle_min and throttle_max into the AP_Vehicles class?
throttle_out = constrain_float((float)(throttle_pwm-_motors.throttle_min()) * temp + _motors.throttle_min(), _motors.throttle_min(), 1000);
// record angle boost for logging
_angle_boost = throttle_out - throttle_pwm;
return throttle_out;
}
根据注释，这段代码是使用 roll/pitch对油门进行补偿，那么具体又是如何补偿的呢？
throttle_out = constrain_float((float)(throttle_pwm-_motors.throttle_min()) * temp + _motors.throttle_min(), _motors.throttle_min(), 1000);
==>
throttle_out = (throttle_pwm-_motors.throttle_min()) * temp + _motors.throttle_min();
代码处理之后就好理解得多。其实就是把高于 throttle_min的部分通过 temp进行补偿，所以关键在于 temp的计算。那么这里为了便于理解，我们假定 pitch为 0，即 cos_pitch为 1，那么把代码处理一下：
float temp = _ahrs.cos_roll();
temp = constrain_float(9000-labs(_ahrs.roll_sensor), 0, 3000) / (3000 * temp);
//正常情况下 labs(_ahrs.roll_sensor)值在 4500以内，所以
float temp = _ahrs.cos_roll();
temp = 3000/ (3000 * temp);
==>
float temp = 1/_ahrs.cos_roll();
所以正常情况下，考虑两个方向的姿态，补偿系数就是： " temp = 1/(_ahrs.cos_pitch() * _ahrs.cos_roll());"。但是从数学上分析， temp是可以无穷大的，显然不能这么去补偿，所以这时候语句 "temp = constrain_float(temp, 0.5f, 1.0f);"就起作用了，它可以对结果进行约束，使得最大补偿为 1倍，即最大 2倍油门输出。当然，这里还有一个变量："_angle_boost"，它是用来记录 log的，这里我们可以不必关心。
那么，现在这里有个问题，就是什么情况下需要进行油门补偿？这个太简单了，匹配一下不就知道了嘛：
bitcraze@bitcraze-vm:~/apm$ grep -nr set_throttle_out ardupilot/ArduCopter/
ardupilot/ArduCopter/control_guided.pde:95: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/heli_control_acro.pde:46: attitude_control.set_throttle_out(g.rc_3.control_in, false);
ardupilot/ArduCopter/control_autotune.pde:218: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_autotune.pde:247: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_land.pde:59: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_land.pde:126: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_auto.pde:108: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_auto.pde:157: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_auto.pde:215: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_auto.pde:284: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_flip.pde:175: attitude_control.set_throttle_out(throttle_out, false);
ardupilot/ArduCopter/control_althold.pde:33: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_althold.pde:63: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_rtl.pde:132: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_rtl.pde:190: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_rtl.pde:261: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_rtl.pde:325: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_loiter.pde:40: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_loiter.pde:75: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_ofloiter.pde:41: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_ofloiter.pde:73: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_acro.pde:25: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_acro.pde:39: attitude_control.set_throttle_out(pilot_throttle_scaled, false);
ardupilot/ArduCopter/control_sport.pde:31: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_sport.pde:82: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_poshold.pde:162: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_poshold.pde:191: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_drift.pde:49: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_drift.pde:108: attitude_control.set_throttle_out(pilot_throttle_scaled + thr_assist, true);
ardupilot/ArduCopter/control_stabilize.pde:30: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/control_stabilize.pde:53: attitude_control.set_throttle_out(pilot_throttle_scaled, true);
ardupilot/ArduCopter/control_circle.pde:40: attitude_control.set_throttle_out(0, false);
ardupilot/ArduCopter/heli_control_stabilize.pde:57: attitude_control.set_throttle_out(pilot_throttle_scaled, false);
bitcraze@bitcraze-vm:~/apm$
根据匹配结果，只有 stabilize跟 drift两种飞行模式需要对油门进行补偿，为什么？ drift是漂移模式，在 APM网站上是这样解释的：
飘移模式能让用户就像飞行安装有“自动协调转弯”的飞机一样飞行多旋翼飞行器。
用户直接控制Yaw和Pitch，但是Roll是由自动驾驶仪控制的。 如果使用美国手的发射机，可以非常方便的用一个控制杆来控制的飞行器
其实 drift跟 stabilize是相似的，只不过 Roll变成了自动控制。那么这两种飞行模式跟其它的飞行模式之间有什么不同呢？其它一些飞行模式除 acro(特技)外都是带气压计定高的，既然有定高那么自然就不需要油门补偿了，因为油门补偿本身就是为了应对打舵时引起的高度降低。那么为什么特技模式不需要呢？这是因为特技模式本身的特点所决定的，在特技模式遥控器对飞行器进行直接的控制。
那么下面我们就来看看 angle_ef_roll_pitch_rate_ef_yaw_smooth具体是怎么回事。
3.angle_ef_roll_pitch_rate_ef_yaw_smooth
该函数是类 AC_AttitudeControl的方法，实现导航级的 PD控制，但是这里的 PD控制更我们之前用的 PD控制在实现形式上大有不同，传统 PID是按照下面这样的形式来实现的：
![PX4 FMU [17] stabilize - Lonelys - 越长大越孤单 越长大越不安](http://img2.ph.126.net/YYvkMwA4UH-qqZ85rDVskA==/6619291996166159195.bmp "PX4 FMU [17] stabilize - Lonelys - 越长大越孤单 越长大越不安")
从形式上我们也可以看出，这里的控制输入是 e，但 APM并没有这么设计，因为归根结底， PD控制器中， P跟 D它是一个微分的关系，换句话说只要符合 PD这样一个微分关系即可构成 PD控制器。具体我们用代码来进行分析。
不过，在调用该函数的时候，实参除了映射之后的摇杆数据还有一个数据是由函数 get_smoothing_gain获取的，
// 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);
}
从注释来看，这是用来获取平滑的增益的，具体这个增益是做什么用的，我们下面会看到。如果我们在上位机上把该参数给读出来，我们将会看到 g.rc_feel_rp值为 100，即该函数返回 12。
// methods to be called by upper controllers to request and implement a desired attitude
// 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(floatroll_angle_ef, float pitch_angle_ef, float yaw_rate_ef, float smoothing_gain)
{
Vector3f angle_ef_error; // earth frame angle errors
float rate_change_limit;
// sanity check smoothing gain
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) {
// calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away
angle_to_target = roll_angle_ef - _angle_ef_target.x;
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.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);
// 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
update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error);
} 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);
_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.x = 0;
_rate_ef_desired.y = 0;
}
if (_accel_y_max > 0.0f) {
// 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);
} else {
// 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);
}
// 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 earth-frame feed forward rates to body-frame feed forward rates
frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);
// 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) {
_rate_bf_target += _rate_bf_desired;
}
// body-frame to motor outputs should be called separately
}
我们现在看到的是该函数完整的代码。为了便于理解，我们需要对代码作一些处理，我们假设 _accel_y_max为 0，那么：
// methods to be called by upper controllers to request and implement a desired attitude
// 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(floatroll_angle_ef, float pitch_angle_ef, float yaw_rate_ef, float smoothing_gain)
{
Vector3f angle_ef_error; // earth frame angle errors
float rate_change_limit;
// sanity check smoothing gain
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) {
} 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);
_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.x = 0;
_rate_ef_desired.y = 0;
}
if (_accel_y_max > 0.0f) {
} else {
// 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);
}
// 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 earth-frame feed forward rates to body-frame feed forward rates
frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);
// 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) {
_rate_bf_target += _rate_bf_desired;
}
// body-frame to motor outputs should be called separately
}
// update_ef_yaw_angle_and_error - update _angle_ef_target.z using an earth frame yaw rate request
void AC_AttitudeControl::update_ef_yaw_angle_and_error(float yaw_rate_ef, Vector3f &angle_ef_error)
{
// calculate angle error with maximum of +- max angle overshoot
angle_ef_error.z = wrap_180_cd(_angle_ef_target.z - _ahrs.yaw_sensor);
angle_ef_error.z = constrain_float(angle_ef_error.z, -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX);
// update yaw angle target to be within max angle overshoot of our current heading
_angle_ef_target.z = angle_ef_error.z + _ahrs.yaw_sensor;
// increment the yaw angle target
_angle_ef_target.z += yaw_rate_ef * _dt;
_angle_ef_target.z = wrap_360_cd(_angle_ef_target.z);
}
// update_rate_bf_targets - converts body-frame angle error to body-frame rate targets for roll, pitch and yaw axis
// targets rates in centi-degrees taken from _angle_bf_error
// results in centi-degrees/sec put into _rate_bf_target
void AC_AttitudeControl::update_rate_bf_targets()
{
// stab roll calculation
_rate_bf_target.x = _p_angle_roll.kP() * _angle_bf_error.x;
// constrain roll rate request
if (_flags.limit_angle_to_rate_request) {
_rate_bf_target.x = constrain_float(_rate_bf_target.x,-_angle_rate_rp_max,_angle_rate_rp_max);
}
// stab pitch calculation
_rate_bf_target.y = _p_angle_pitch.kP() * _angle_bf_error.y;
// constrain pitch rate request
if (_flags.limit_angle_to_rate_request) {
_rate_bf_target.y = constrain_float(_rate_bf_target.y,-_angle_rate_rp_max,_angle_rate_rp_max);
}
// stab yaw calculation
_rate_bf_target.z = _p_angle_yaw.kP() * _angle_bf_error.z;
// constrain yaw rate request
if (_flags.limit_angle_to_rate_request) {
_rate_bf_target.z = constrain_float(_rate_bf_target.z,-_angle_rate_y_max,_angle_rate_y_max);
}
_rate_bf_target.x += -_angle_bf_error.y * _ins.get_gyro().z;
_rate_bf_target.y += _angle_bf_error.x * _ins.get_gyro().z;
}
把代码处理了一下就很清楚了，原来也只不过是计算角度差 angle_ef_error，然后通过 update_rate_bf_targets函数进行 P运算。函数 frame_conversion_ef_to_bf则是用来进行坐标系之间的转换，将参考坐标系中的误差转换到机体坐标系。但是前面我们不是是说了 PX4姿态级使用的是 PD控制的吗，为什么值看到了 P控制，D呢？别忘了，在函数的最后还有一条语句：" _rate_bf_target += _rate_bf_desired"，注释说这是前反馈，实际上就是 D控制，只不过现在我们所看到的是 0而已。
如果我们把参数通过上位机给读出来，我们将会看到这样两个参数：
ATC_ACCEL_RP_MAX,126000
ATC_ACCEL_Y_MAX,36000
也就是说实际情况是 _accel_rp_max跟 _accel_y_max是大于 0的值，执行的是 if段代码：
Vector3f angle_ef_error; // earth frame angle errors
float rate_change_limit;
// sanity check smoothing gain
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;
// calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away
angle_to_target = roll_angle_ef - _angle_ef_target.x;
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.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);
// 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
update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error);
// 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);
这段代码分析起来会比较绕。但说透了它其实也就是在 _angle_ef_target跟 _ahrs之间插了一个值，就是 _angle_ef_target。这样的好处是什么呢？好处在于，这样处理之后速率是像抛物线一样先增后减的，而不是一打舵猛地给速率控制器一个很高的速率，然后慢慢往 0靠拢。按我的说法就是 PX4在导航级做了一个平滑处理。想了解更多的细节我们还需要去看下 update_ef_roll_angle_and_error跟 update_ef_pitch_angle_and_error函数：
// update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate request
void AC_AttitudeControl::update_ef_roll_angle_and_error(float roll_rate_ef, Vector3f &angle_ef_error)
{
// calculate angle error with maximum of +- max angle overshoot
angle_ef_error.x = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor);
angle_ef_error.x = constrain_float(angle_ef_error.x, -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX);
// update roll angle target to be within max angle overshoot of our roll angle
_angle_ef_target.x = angle_ef_error.x + _ahrs.roll_sensor;
// increment the roll angle target
_angle_ef_target.x += roll_rate_ef * _dt;
_angle_ef_target.x = wrap_180_cd(_angle_ef_target.x);
}
// update_ef_pitch_angle_and_error - update _angle_ef_target.y using an earth frame pitch rate request
void AC_AttitudeControl::update_ef_pitch_angle_and_error(float pitch_rate_ef, Vector3f &angle_ef_error)
{
// calculate angle error with maximum of +- max angle overshoot
// To-Do: should we do something better as we cross 90 degrees?
angle_ef_error.y = wrap_180_cd(_angle_ef_target.y - _ahrs.pitch_sensor);
angle_ef_error.y = constrain_float(angle_ef_error.y, -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX);
// update pitch angle target to be within max angle overshoot of our pitch angle
_angle_ef_target.y = angle_ef_error.y + _ahrs.pitch_sensor;
// increment the pitch angle target
_angle_ef_target.y += pitch_rate_ef * _dt;
_angle_ef_target.y = wrap_180_cd(_angle_ef_target.y);
}
这样我们就清楚了，PX4是通过 smoothing_gain得到一个新的速率 rate_ef_desired，然后用这个新的速率去更新 _angle_ef_target，并使用 _angle_ef_target计算姿态误差参与控制。这个时候可能你会觉得 linear_angle是用来干嘛的？通过前面我们给出的数据，可以算出 linear_angle值为 875，PX4是精确到 0.01°，所以对应的是 8.75°。也就是说在 8.75°以内是一个线性区域，速率是线性变化的，超出这个角度就采用飞线性的方式计算 rate_ef_desired。具体是什么方式呢？我们可以根据我们的数据稍微整理下得到：
rate_ef_desired = safe_sqrt(2.0f*126000*(fabs(angle_to_target)-437.5));
==>
rate_ef_desired = safe_sqrt(252000)*safe_sqrt(fabs(angle_to_target)-437.5);
==>
rate_ef_desired = 501*safe_sqrt(fabs(angle_to_target)-437.5);
当然，如果能够写成数学表达式就更加清晰了。
而这个时候，由于 _rate_ef_desired不为 0，所以 _rate_bf_desired也不为 0。而且从计算过程来看，PX4是通过对 _rate_ef_desired积分计算姿态，所以姿态级使用的是 PD控制，只不过将 PD控制器中的主角稍稍换了一下。
那么下面我们将要看到的是姿态控制中相当重要的一环：速率控制器，即 PID控制器。
4.rate_controller_run
在源码 " ardupilot/ArduCopter/ArduCopter.pde"中有这样一段代码：
// Main loop - 100hz
static void fast_loop()
{
// IMU DCM Algorithm
// --------------------
read_AHRS();
// run low level rate controllers that only require IMU data
attitude_control.rate_controller_run();
我们没知道，fast_loop是快速循环，直接由 loop调用。而这里的 rate_controller_run便是我们的 PID控制器，即速率控制器。
// rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors
// should be called at 100hz or more
void AC_AttitudeControl::rate_controller_run()
{
// call rate controllers and send output to motors object
// To-Do: should the outputs from get_rate_roll, pitch, yaw be int16_t which is the input to the motors library?
// To-Do: skip this step if the throttle out is zero?
_motors.set_roll(rate_bf_to_motor_roll(_rate_bf_target.x));
_motors.set_pitch(rate_bf_to_motor_pitch(_rate_bf_target.y));
_motors.set_yaw(rate_bf_to_motor_yaw(_rate_bf_target.z));
}
_rate_bf_target就是前面导航级计算得到的速率，rate_bf_to_motor_用来把速率转换成输出，它用的就是我们传统的 PID形式。
// rate_bf_to_motor_roll - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second
float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_cds)
{
float p,i,d; // used to capture pid values for logging
float current_rate; // this iteration's rate
float rate_error; // simply target_rate - current_rate
// get current rate
// To-Do: make getting gyro rates more efficient?
current_rate = (_ins.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100);
// calculate error and call pid controller
rate_error = rate_target_cds - current_rate;
p = _pid_rate_roll.get_p(rate_error);
// get i term
i = _pid_rate_roll.get_integrator();
// update i term as long as we haven't breached the limits or the I term will certainly reduce
if (!_motors.limit.roll_pitch