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是按照下面这样的形式来实现的：

从形式上我们也可以看出，这里的控制输入是 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