LOFTER for ipad —— 让兴趣,更有趣

点击下载 关闭

LOFTER-网易轻博

四轴飞行器

929浏览    30参与
手工资源分享社区

无人机入门:构建和定制自己的四轴飞行器

想要制作可以飞行的东西吗?无人机怎么样?在本书中,您将了解无人机如何工作,如何解决无人机所呈现的一些工程(制作)挑战,以及如何构建自己的无人机 - 一种可以构建,定制和飞行的自主四轴飞行器。你的无人机将成为你在天空中的眼睛,到达人类永远无法到达的高度!


链接:https://pan.baidu.com/s/1YvdgZ6CHsNLAracCq1kieg 

提取码:a55z


想要制作可以飞行的东西吗?无人机怎么样?在本书中,您将了解无人机如何工作,如何解决无人机所呈现的一些工程(制作)挑战,以及如何构建自己的无人机 - 一种可以构建,定制和飞行的自主四轴飞行器。你的无人机将成为你在天空中的眼睛,到达人类永远无法到达的高度!



链接:https://pan.baidu.com/s/1YvdgZ6CHsNLAracCq1kieg 

提取码:a55z


小叶子

叶子日记2017年2月16日第170篇《四轴飞行器》

今天妈妈拿回个新玩具---四轴飞行器;

读日记的视频链接是;http://v.youku.com/v_show/id_XMjYyOTY3MzkwMA==.html


日记文本;



今天妈妈拿回个新玩具---四轴飞行器;

读日记的视频链接是;http://v.youku.com/v_show/id_XMjYyOTY3MzkwMA==.html

叶子日记2017年2月16日第170篇《四轴飞行器》
日记文本;

叶子日记2017年2月16日第170篇《四轴飞行器》

猫家猪头的…

MAVIC PRO.

by DJI.

个人心目中的2016年最佳消费型电子产品。

MAVIC PRO.

by DJI.

个人心目中的2016年最佳消费型电子产品。

蓝金

仔细看,还是可以看到这只飞行器的

仔细看,还是可以看到这只飞行器的

乐山

PX4 FMU [17] stabilize

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(bool ignore_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(bool ignore_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, bool apply_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 - 越长大越孤单  越长大越不安

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

乐山

PX4 IO [15] mixer

PX4 IO [15] mixer
                                                ...
PX4 IO [15] mixer
                                                                             -------- 转载请注明出处
                                                                             -------- 更多笔记请访问我的博客: merafour.blog.163.com 

                                                                             -------- 2015-1-5.冷月追风

                                                                             -------- email:merafour@163.com 



    看过了 IO的输入,我们现在来看看 IO中是怎么把信号输出的。
    这个我们当然得先知道在 fmu中是怎么往 IO发这些输出数据的。
    回过头去看第十一篇笔记,我们会看到 " AP_MotorsMatrix::output_test"函数的源码如下:

// output_test - spin a motor at the pwm value specified
//  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_MotorsMatrix::output_test(uint8_t motor_seq, int16_t pwm)
{
    // exit immediately if not armed
    if (!_flags.armed) {
        return;
    }

    // loop through all the possible orders spinning any motors that match that description
    for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motor_enabled[i] && _test_order[i] == motor_seq) {
            // turn on this motor
            hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), pwm);
        }
    }
}


也就是说通过 " hal.rcout->write"接口下发数据。" rcout"类型为 "PX4RCOutput"。从前面的分析中我们知道 "PX4RCOutput"其实是通过设备文件对 px4io进行调用,最终调用的是 "PX4IO::write"函数,源码如下:

ssize_t PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
/* Make it obvious that file * isn't used here */
{
    unsigned count = len / 2;

    if (count > _max_actuators)
        count = _max_actuators;

    if (count > 0) {

        perf_begin(_perf_write);
        int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
        perf_end(_perf_write);

        if (ret != OK)
            return ret;
    }

    return count * 2;
}


px4io在我们的 V2版本中通过串口跟 io板通信。所以两者通信的关键在于 " PX4IO_PAGE_DIRECT_PWM"这个宏,

radiolink@ubuntu:~/apm$ grep -nr PX4IO_PAGE_DIRECT_PWM ./PX4Firmware/src/
./PX4Firmware/src/modules/px4iofirmware/protocol.h:258:#define PX4IO_PAGE_DIRECT_PWM                    54              /**< 0..CONFIG_ACTUATOR_COUNT-1 */
./PX4Firmware/src/modules/px4iofirmware/registers.c:284:        case PX4IO_PAGE_DIRECT_PWM:
./PX4Firmware/src/modules/px4iofirmware/registers.c:883:        case PX4IO_PAGE_DIRECT_PWM:
./PX4Firmware/src/drivers/px4io/px4io.cpp:2366:                         ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
./PX4Firmware/src/drivers/px4io/px4io.cpp:2649:         int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
radiolink@ubuntu:~/apm$

因为我们现在关注的是 io固件,所以我们现在要找的源码在 "registers.c"源文件中。可能我们觉得这里值应该出现一个 "case"语句,但这里却出现了两个,为什么?如果你去看源码你会看到实际上有一个 "registers_set"和一个 "registers_get"函数。当然 get函数我们没有使用,就不去关心。下面我们就来看看 set函数。

int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
    switch (page) {
  /* ... */
        /* handle raw PWM input */
    case PX4IO_PAGE_DIRECT_PWM:
        /* copy channel data */
        while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
            /* XXX range-check value? */
            r_page_servos[offset] = *values;
            offset++;
            num_values--;
            values++;
        }
        system_state.fmu_data_received_time = hrt_absolute_time();
        r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM;
        break;

所以我们看到,在这里 set主要是数据拷贝动作。而我们现在要解决的是在把数据放到 "r_page_servos"数组中之后数据是怎样到大寄存器的。

radiolink@ubuntu:~/apm$ grep -nr r_page_servos ./PX4Firmware/src/modules/px4iofirmware/
./PX4Firmware/src/modules/px4iofirmware/px4io.h:76:extern uint16_t                      r_page_servos[];        /* PX4IO_PAGE_SERVOS */
./PX4Firmware/src/modules/px4iofirmware/mixer.cpp:217:                  r_page_servos[i] = r_page_servo_failsafe[i];
./PX4Firmware/src/modules/px4iofirmware/mixer.cpp:220:                  r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
./PX4Firmware/src/modules/px4iofirmware/mixer.cpp:237:          pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
./PX4Firmware/src/modules/px4iofirmware/mixer.cpp:240:                  r_page_servos[i] = 0;
./PX4Firmware/src/modules/px4iofirmware/mixer.cpp:276:                  up_pwm_servo_set(i, r_page_servos[i]);
./PX4Firmware/src/modules/px4iofirmware/registers.c:107:uint16_t                r_page_servos[PX4IO_SERVO_COUNT];
./PX4Firmware/src/modules/px4iofirmware/registers.c:221: * PAGE 104 uses r_page_servos.
./PX4Firmware/src/modules/px4iofirmware/registers.c:291:                                r_page_servos[offset] = *values;
./PX4Firmware/src/modules/px4iofirmware/registers.c:864:                SELECT_PAGE(r_page_servos);
./PX4Firmware/src/modules/px4iofirmware/registers.c:884:                SELECT_PAGE(r_page_servos);
radiolink@ubuntu:~/apm$

去阅读源码我们就会发现, mixer.cpp中的结果均来自同一个函数: mixer_tick。该函数同样是在 Px4io.c中由 user_start调用。代码如下:

int user_start(int argc, char *argv[])
{
    /* ... */
    for (;;) {
        /* track the rate at which the loop is running */
        perf_count(loop_perf);
        /* kick the mixer */
        perf_begin(mixer_perf);
        mixer_tick();
        perf_end(mixer_perf);
        /* kick the control inputs */
        perf_begin(controls_perf);
        controls_tick();
        perf_end(controls_perf);

这样我们就只需要关心 mixer_tick函数即可。而该函数从头到尾超过 150行代码,我们在看源码的时候要将它拆成几段。

void mixer_tick(void)
{
    /* check that we are receiving fresh data from the FMU */
    if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
        /* too long without FMU input, time to go to failsafe */
        if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
            isr_debug(1, "AP RX timeout");
        }
        r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
        r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
    } else {
        r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
    }
#define FMU_INPUT_DROP_LIMIT_US        200000

这段代码并不难理解,是用来检测超时的。超时时间为 200ms。可能我们都会奇怪, user_start中的主循环执行一次会消耗 200ms吗?关于这点,我只能说目前我还没有去研究它是怎么控制循环的,暂时不予讨论。

    /* default to failsafe mixing */
    source = MIX_FAILSAFE;
    /*
     * Decide which set of controls we're using.
     */
    /* do not mix if RAW_PWM mode is on and FMU is good */
    if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
            (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
        /* don't actually mix anything - we already have raw PWM values */
        source = MIX_NONE;
    } else {
        if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
             (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
             (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
            /* mix from FMU controls */
            source = MIX_FMU;
        }
        if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
             (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
             (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
             !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
             !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
             /* if allowed, mix from RC inputs directly */
            source = MIX_OVERRIDE;
        } else     if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
             (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
             (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
             !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
             (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
            /* if allowed, mix from RC inputs directly up to available rc channels */
            source = MIX_OVERRIDE_FMU_OK;
        }
    }
    /*
     * Set failsafe status flag depending on mixing source
     */
    if (source == MIX_FAILSAFE) {
        r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
    } else {
        r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
    }

r_status_flags就是前面我们在 set函数中设置的标志,如果set函数被正常调用,那么到这里 source的值为 MIX_NONE。关于失控保护,正常情况下是可以忽略的。

    /*
     * Decide whether the servos should be armed right now.
     *
     * We must be armed, and we must have a PWM source; either raw from
     * FMU or from the mixer.
     *
     * XXX correct behaviour for failsafe may require an additional case
     * here.
     */
    should_arm = (
        /* IO initialised without error */   (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
        /* and IO is armed */           && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
        /* and FMU is armed */           && (
                                ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
        /* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
        /* or direct PWM is set */               || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
        /* or failsafe was set manually */     || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
                             )
    );
    should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
                        && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
                        && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);

这段代码注释说的比较清楚,是进行解锁检查的。说到解锁,我们都还没看 fmu是怎么解锁的,待会还真得看看。飞控通常都是要解锁之后才能飞的,这是出于安全考虑。我想 fmu解锁了应该也会通过串口发送一个信息给 io并最终调用 set函。

    /*
     * Run the mixers.
     */
    if (source == MIX_FAILSAFE) {
        /* copy failsafe values to the servo outputs */
        for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
            r_page_servos[i] = r_page_servo_failsafe[i];
            /* safe actuators for FMU feedback */
            r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
        }
    } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
        float    outputs[PX4IO_SERVO_COUNT];
        unsigned mixed;
        /* mix */
        /* poor mans mutex */
        in_mixer = true;
        mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
        in_mixer = false;
        pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
        for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
            r_page_servos[i] = 0;
        for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
            r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
        }
    }

从前面的代码中我们知道 source的值为 MIX_NONE,所以这一段不会执行。这段代码只是给 r_page_servos一个特定的值,前面是失控保护,而后面应该是关闭电机。其实如果 fmu发过来的数据是 1ms的脉宽,电机也是关闭的。

    /* set arming */
    bool needs_to_arm = (should_arm || should_always_enable_pwm);
    /* check any conditions that prevent arming */
    if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
        needs_to_arm = false;
    }
    if (!should_arm && !should_always_enable_pwm) {
        needs_to_arm = false;
    }
    if (needs_to_arm && !mixer_servos_armed) {
        /* need to arm, but not armed */
        up_pwm_servo_arm(true);
        mixer_servos_armed = true;
        r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
        isr_debug(5, "> PWM enabled");
    } else if (!needs_to_arm && mixer_servos_armed) {
        /* armed but need to disarm */
        up_pwm_servo_arm(false);
        mixer_servos_armed = false;
        r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
        isr_debug(5, "> PWM disabled");
    }
    if (mixer_servos_armed && should_arm) {
        /* update the servo outputs. */
        for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
            up_pwm_servo_set(i, r_page_servos[i]);
    } else if (mixer_servos_armed && should_always_enable_pwm) {
        /* set the disarmed servo outputs. */
        for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
            up_pwm_servo_set(i, r_page_servo_disarmed[i]);
    }
int up_pwm_servo_set(unsigned channel, servo_position_t value)
{
    if (channel >= PWM_SERVO_MAX_CHANNELS)
        return -1;

    unsigned timer = pwm_channels[channel].timer_index;

    /* test timer for validity */
    if ((pwm_timers[timer].base == 0) ||
        (pwm_channels[channel].gpio == 0))
        return -1;

    /* configure the channel */
    if (value > 0)
        value--;

    switch (pwm_channels[channel].timer_channel) {
    case 1:
        rCCR1(timer) = value;
        break;

    case 2:
        rCCR2(timer) = value;
        break;

    case 3:
        rCCR3(timer) = value;
        break;

    case 4:
        rCCR4(timer) = value;
        break;

    default:
        return -1;
    }

    return 0;
}

所以最终是通过 up_pwm_servo_set函数最终将 PWM信号输出的。其中 r_page_servos这组数据是 fmu发送过来的,而 r_page_servo_disarmed这组数据是前面 pwm_limit_calc函数计算得到的。

    而其中具体每一个参数是做什么的,这个就要搞懂了流程以后详细去分析了。

乐山

PX4 IO [14] serial

PX4 IO [14] serial
                                               ...
PX4 IO [14] serial
                                                                             -------- 转载请注明出处
                                                                             -------- 更多笔记请访问我的博客: merafour.blog.163.com 

                                                                             -------- 2014-12-31.冷月追风

                                                                             -------- email:merafour@163.com 


1.hrt_ppm_decode

    我们已经不止一次接触到 fmu跟 IO通讯,之前我们看到的只是 fmu部分源码,那么 IO中又是怎么处理的呢?下面我们就来瞧瞧。
    IO固件的 mk文件为: "./PX4Firmware/makefiles/config_px4io-v2_default.mk",其内容如下:

#
# Makefile for the px4iov2_default configuration
#
# Board support modules
#
MODULES         += drivers/stm32
MODULES         += drivers/boards/px4io-v2
MODULES         += modules/px4iofirmware

这是 IO固件所用到的一些文件,当然仅仅是 PX4Firmware目录下的。而现在我们要阅读的源码在 "PX4Firmware/src/modules/px4iofirmware/"目录,其内容如下:

radiolink@ubuntu:~/apm$ ls PX4Firmware/src/modules/px4iofirmware/
adc.c       dsm.c  mixer.cpp  protocol.h  px4io.h      safety.c  serial.c
controls.c  i2c.c  module.mk  px4io.c     registers.c  sbus.c
radiolink@ubuntu:~/apm$

因为前面我们知道 fmu跟 IO是通过串口进行通讯的,所以我们现在要阅读的源码主要在 serial.c中。
    前面我们看到,在 io_get_raw_rc_input函数中使用下面的代码来获取遥控器数据:

    if (channel_count > 9) {
        ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);

        if (ret != OK)
            return ret;
    }
int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
{
    /* range check the transfer */
    if (num_values > ((_max_transfer) / sizeof(*values))) {
        debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2);
        return -EINVAL;
    }

    int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values);

    if (ret != (int)num_values) {
        debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret);
        return -1;
    }

    return OK;
}

因此我有理由相信在 IO中也使用了宏 PX4IO_PAGE_RAW_RC_INPUT。所以:

radiolink@ubuntu:~/apm$ grep -nr PX4IO_PAGE_RAW_RC_INPUT ./PX4Firmware/src/modules/px4iofirmware/
./PX4Firmware/src/modules/px4iofirmware/px4io.h:77:extern uint16_t                      r_page_raw_rc_input[];  /* PX4IO_PAGE_RAW_RC_INPUT */
./PX4Firmware/src/modules/px4iofirmware/protocol.h:140:#define PX4IO_PAGE_RAW_RC_INPUT          4
./PX4Firmware/src/modules/px4iofirmware/registers.c:866:        case PX4IO_PAGE_RAW_RC_INPUT:
radiolink@ubuntu:~/apm$
int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
{
#define SELECT_PAGE(_page_name)                            \
    do {                                    \
        *values = &_page_name[0];                    \
        *num_values = sizeof(_page_name) / sizeof(_page_name[0]);    \
    } while(0)

    switch (page) {

    /* ... */
    /* status pages */
    case PX4IO_PAGE_CONFIG:
        SELECT_PAGE(r_page_config);
        break;
    case PX4IO_PAGE_ACTUATORS:
        SELECT_PAGE(r_page_actuators);
        break;
    case PX4IO_PAGE_SERVOS:
        SELECT_PAGE(r_page_servos);
        break;
    case PX4IO_PAGE_RAW_RC_INPUT:
        SELECT_PAGE(r_page_raw_rc_input);
        break;

在这段代码中我们并没有看到数据拷贝,所以数据拷贝应该是由调用 registers_get的函数来完成。

static void rx_handle_packet(void)
{
    /* check packet CRC */
    uint8_t crc = dma_packet.crc;
    dma_packet.crc = 0;
    if (crc != crc_packet(&dma_packet)) {
        perf_count(pc_crcerr);

        /* send a CRC error reply */
        dma_packet.count_code = PKT_CODE_CORRUPT;
        dma_packet.page = 0xff;
        dma_packet.offset = 0xff;

        return;
    }

    if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) {

        /* it's a blind write - pass it on */
        if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) {
            perf_count(pc_regerr);
            dma_packet.count_code = PKT_CODE_ERROR;
        } else {
            dma_packet.count_code = PKT_CODE_SUCCESS;
        }
        return;
    } 

    if (PKT_CODE(dma_packet) == PKT_CODE_READ) {

        /* it's a read - get register pointer for reply */
        unsigned count;
        uint16_t *registers;

        if (registers_get(dma_packet.page, dma_packet.offset, &registers, &count) < 0) {
            perf_count(pc_regerr);
            dma_packet.count_code = PKT_CODE_ERROR;
        } else {
            /* constrain reply to requested size */
            if (count > PKT_MAX_REGS)
                count = PKT_MAX_REGS;
            if (count > PKT_COUNT(dma_packet))
                count = PKT_COUNT(dma_packet);

            /* copy reply registers into DMA buffer */
            memcpy((void *)&dma_packet.regs[0], registers, count * 2);
            dma_packet.count_code = count | PKT_CODE_SUCCESS;
        }
        return;
    }

    /* send a bad-packet error reply */
    dma_packet.count_code = PKT_CODE_CORRUPT;
    dma_packet.page = 0xff;
    dma_packet.offset = 0xfe;
}
static

void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
{
    /* ... */
    rx_handle_packet();

rx_handle_packet函数中我们看到 IO提供了 registers_getregisters_set函数分别用来获取和设置 IO数据。而 rx_dma_callback函数的调用我们很容易想到是在串口收到数据的时候。这个我们回头再来折腾,现在我们要去看另外一个东西:r_page_raw_rc_input。在 registers_get函数中我们要读取的数据是来自 r_page_raw_rc_input,它的数据肯定也是有人放进去的,而不是凭空产生的。所以:

radiolink@ubuntu:~/apm$ grep -nr r_page_raw_rc_input ./PX4Firmware/src/modules/px4iofirmware/
./PX4Firmware/src/modules/px4iofirmware/controls.c:216: bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
./PX4Firmware/src/modules/px4iofirmware/controls.c:230: r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
./PX4Firmware/src/modules/px4iofirmware/px4io.h:77:extern uint16_t                      r_page_raw_rc_input[];  /* PX4IO_PAGE_RAW_RC_INPUT */
./PX4Firmware/src/modules/px4iofirmware/px4io.h:97:#define r_raw_rc_count               r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
./PX4Firmware/src/modules/px4iofirmware/px4io.h:98:#define r_raw_rc_values              (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
./PX4Firmware/src/modules/px4iofirmware/px4io.h:99:#define r_raw_rc_flags               r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
./PX4Firmware/src/modules/px4iofirmware/registers.c:114:uint16_t                r_page_raw_rc_input[] =
./PX4Firmware/src/modules/px4iofirmware/registers.c:867:                SELECT_PAGE(r_page_raw_rc_input);
radiolink@ubuntu:~/apm$

经分析, controls.c是用来获取遥控器数据的。其实,从其命名上也是相当明显的。

    /*
     * XXX each S.bus frame will cause a PPM decoder interrupt
     * storm (lots of edges).  It might be sensible to actually
     * disable the PPM decoder completely if we have S.bus signal.
     */
    perf_begin(c_gather_ppm);
    bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
    if (ppm_updated) {

        r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
        r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
        r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
    }
    perf_end(c_gather_ppm);

    /* limit number of channels to allowable data size */
    if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
        r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;

    /* store RSSI */
    r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
{
    bool result = false;

    /* avoid racing with PPM updates */
    irqstate_t state = irqsave();

    /*
     * If we have received a new PPM frame within the last 200ms, accept it
     * and then invalidate it.
     */
    if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) {

        /* PPM data exists, copy it */
        *num_values = ppm_decoded_channels;
        if (*num_values > PX4IO_RC_INPUT_CHANNELS)
            *num_values = PX4IO_RC_INPUT_CHANNELS;

        for (unsigned i = 0; i < *num_values; i++)
            values[i] = ppm_buffer[i];

        /* clear validity */
        ppm_last_valid_decode = 0;

        /* store PPM frame length */
        if (num_values)
            *frame_len = ppm_frame_length;

        /* good if we got any channels */
        result = (*num_values > 0);
    }

    irqrestore(state);

    return result;
}

从源码中我们看到,数组 r_page_raw_rc_input中所存放的并不仅仅是接收到的遥控器数据。遥控器数据仅仅是其中一部分而已。如果我们去看其定义将会更加清楚:

/**
 * PAGE 0
 *
 * Static configuration parameters.
 */
static const uint16_t    r_page_config[] = {
    [PX4IO_P_CONFIG_PROTOCOL_VERSION]    = PX4IO_PROTOCOL_VERSION,
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
    [PX4IO_P_CONFIG_HARDWARE_VERSION]    = 2,
#else
    [PX4IO_P_CONFIG_HARDWARE_VERSION]    = 1,
#endif
    [PX4IO_P_CONFIG_BOOTLOADER_VERSION]    = 3,    /* XXX hardcoded magic number */
    [PX4IO_P_CONFIG_MAX_TRANSFER]        = 64,    /* XXX hardcoded magic number */
    [PX4IO_P_CONFIG_CONTROL_COUNT]        = PX4IO_CONTROL_CHANNELS,
    [PX4IO_P_CONFIG_ACTUATOR_COUNT]        = PX4IO_SERVO_COUNT,
    [PX4IO_P_CONFIG_RC_INPUT_COUNT]        = PX4IO_RC_INPUT_CHANNELS,
    [PX4IO_P_CONFIG_ADC_INPUT_COUNT]    = PX4IO_ADC_CHANNEL_COUNT,
    [PX4IO_P_CONFIG_RELAY_COUNT]        = PX4IO_RELAY_CHANNELS,
};

当然我现在也不去研究这个数组中到底都放了些什么数据。我关心的是 ppm_buffer中的数据是怎么来的,直至数据的最源头。但是我们要知道 ppm_buffer跟 ppm_decoded_channels是分不开的。

radiolink@ubuntu:~/apm$ grep -nr ppm_buffer ./PX4Firmware/src/
./PX4Firmware/src/systemcmds/tests/test_hrt.c:90:extern uint16_t ppm_buffer[];
./PX4Firmware/src/systemcmds/tests/test_hrt.c:103:              printf("  %u\n", ppm_buffer[i]);
./PX4Firmware/src/modules/systemlib/ppm_decode.c:91:uint16_t    ppm_buffer[PPM_MAX_CHANNELS];
./PX4Firmware/src/modules/systemlib/ppm_decode.c:179:                                   ppm_buffer[i] = ppm_temp_buffer[i];
./PX4Firmware/src/modules/systemlib/ppm_decode.h:59:__EXPORT extern uint16_t    ppm_buffer[PPM_MAX_CHANNELS];   /**< decoded PPM channel values */
./PX4Firmware/src/modules/px4iofirmware/controls.c:472:                 values[i] = ppm_buffer[i];
./PX4Firmware/src/drivers/stm32/drv_hrt.c:352:__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
./PX4Firmware/src/drivers/stm32/drv_hrt.c:503:                                  ppm_buffer[i] = ppm_temp_buffer[i];
./PX4Firmware/src/drivers/px4fmu/fmu.cpp:722:                           rc_in.values[i] = ppm_buffer[i];
radiolink@ubuntu:~/apm$

在这里,我们可能会觉得是源文件 ppm_decode.c是我们要找的文件,但是:

void ppm_input_decode(bool reset, unsigned count)
{
    uint16_t width;
    uint16_t interval;
    unsigned i;

    /* if we missed an edge, we have to give up */
    if (reset)
        goto error;

    /* how long since the last edge? */
    width = count - ppm.last_edge;

    if (count < ppm.last_edge)
        width += ppm.count_max;    /* handle wrapped count */

    ppm.last_edge = count;

    if (width >= PPM_MIN_START) {
        if (ppm.next_channel != ppm_decoded_channels) {
            /* ... */
        } else {
            /* frame channel count matches expected, let's use it */
            if (ppm.next_channel > PPM_MIN_CHANNELS) {
                for (i = 0; i < ppm.next_channel; i++)
                    ppm_buffer[i] = ppm_temp_buffer[i];

                ppm_last_valid_decode = hrt_absolute_time();
            }
        }

        /* reset for the next frame */
        ppm.next_channel = 0;

        /* next edge is the reference for the first channel */
        ppm.phase = ARM;

        return;
    }
radiolink@ubuntu:~/apm$ grep -nr ppm_input_decode ./PX4Firmware/src/
./PX4Firmware/src/modules/systemlib/ppm_decode.c:123:ppm_input_decode(bool reset, unsigned count)
./PX4Firmware/src/modules/systemlib/ppm_decode.h:68: *                          ppm_input_decode, used to determine how to
./PX4Firmware/src/modules/systemlib/ppm_decode.h:84:__EXPORT void               ppm_input_decode(bool reset, unsigned count);
radiolink@ubuntu:~/apm$

所以函数 ppm_input_decode根本就没有被调用。而且我们前面的 mk文件中根本就没有添加 systemlib目录。那么我们要找的源码就只能在源文件 drv_hrt.c中了。

/**
 * Handle the PPM decoder state machine.
 */
static void hrt_ppm_decode(uint32_t status)
{
    uint16_t count = rCCR_PPM;
    uint16_t width;
    uint16_t interval;
    unsigned i;

    /* if we missed an edge, we have to give up */
    if (status & SR_OVF_PPM)
        goto error;

    /* how long since the last edge? - this handles counter wrapping implicitely. */
    width = count - ppm.last_edge;

    ppm_edge_history[ppm_edge_next++] = width;

    if (ppm_edge_next >= 32)
        ppm_edge_next = 0;

    /*
     * if this looks like a start pulse, then push the last set of values
     * and reset the state machine
     */
    if (width >= PPM_MIN_START) {

        /*
         * If the number of channels changes unexpectedly, we don't want
         * to just immediately jump on the new count as it may be a result
         * of noise or dropped edges.  Instead, take a few frames to settle.
         */
        if (ppm.next_channel != ppm_decoded_channels) {
            static unsigned new_channel_count;
            static unsigned new_channel_holdoff;

            if (new_channel_count != ppm.next_channel) {
                /* start the lock counter for the new channel count */
                new_channel_count = ppm.next_channel;
                new_channel_holdoff = PPM_CHANNEL_LOCK;

            } else if (new_channel_holdoff > 0) {
                /* this frame matched the last one, decrement the lock counter */
                new_channel_holdoff--;

            } else {
                /* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
                ppm_decoded_channels = new_channel_count;
                new_channel_count = 0;
            }

        } else {
            /* frame channel count matches expected, let's use it */
            if (ppm.next_channel > PPM_MIN_CHANNELS) {
                for (i = 0; i < ppm.next_channel; i++)
                    ppm_buffer[i] = ppm_temp_buffer[i];

                ppm_last_valid_decode = hrt_absolute_time();

            }
        }

        /* reset for the next frame */
        ppm.next_channel = 0;

        /* next edge is the reference for the first channel */
        ppm.phase = ARM;

        ppm.last_edge = count;
        return;
    }
/**
 * Handle the compare interupt by calling the callout dispatcher
 * and then re-scheduling the next deadline.
 */


static int
hrt_tim_isr( int irq, void *context)
{
    uint32_t status;

    /* grab the timer for latency tracking purposes */
    latency_actual = rCNT;

    /* copy interrupt status */
    status = rSR;

    /* ack the interrupts we just read */
    rSR = ~status;

# ifdef HRT_PPM_CHANNEL

    /* was this a PPM edge? */
     if (status & (SR_INT_PPM | SR_OVF_PPM)) {
        /* if required, flip edge sensitivity */
# ifdef PPM_EDGE_FLIP
        rCCER ^= CCER_PPM_FLIP;
# endif

         hrt_ppm_decode(status);
    }

# endif

这个时候我们已经跟踪到中断服务函数了,在往下那就是中断初始化了。从这里我们也看到,真正的 PPM数据最后是来自 ppm_temp_buffer数组,这是由 hrt_ppm_decode函数剩下的部分代码来完成的,负责 PPM解码工作。

    switch (ppm.phase) {
    case UNSYNCH:
        /* we are waiting for a start pulse - nothing useful to do here */
        break;

    case ARM:

        /* we expect a pulse giving us the first mark */
        if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
            goto error;        /* pulse was too short or too long */

        /* record the mark timing, expect an inactive edge */
        ppm.last_mark = ppm.last_edge;

        /* frame length is everything including the start gap */
        ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start);
        ppm.frame_start = ppm.last_edge;
        ppm.phase = ACTIVE;
        break;

    case INACTIVE:

        /* we expect a short pulse */
        if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
            goto error;        /* pulse was too short or too long */

        /* this edge is not interesting, but now we are ready for the next mark */
        ppm.phase = ACTIVE;
        break;

    case ACTIVE:
        /* determine the interval from the last mark */
        interval = count - ppm.last_mark;
        ppm.last_mark = count;

        ppm_pulse_history[ppm_pulse_next++] = interval;

        if (ppm_pulse_next >= 32)
            ppm_pulse_next = 0;

        /* if the mark-mark timing is out of bounds, abandon the frame */
        if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
            goto error;

        /* if we have room to store the value, do so */
        if (ppm.next_channel < PPM_MAX_CHANNELS)
            ppm_temp_buffer[ppm.next_channel++] = interval;

        ppm.phase = INACTIVE;
        break;

    }

    ppm.last_edge = count;
    return;

    /* the state machine is corrupted; reset it */

error:
    /* we don't like the state of the decoder, reset it and try again */
    ppm.phase = UNSYNCH;
    ppm_decoded_channels = 0;

}

实际上就是对脉宽进行测量。

    关于脉宽测量,我们目前还没有必要去研究它,所以就不深入其细节了。

2.user_start

    现在,我们就来看看 ppm_input函数是如何被调用的。

void
controls_tick() {

    uint16_t rssi = 0;

#ifdef ADC_RSSI
    if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
        unsigned counts = adc_measure(ADC_RSSI);
        if (counts != 0xffff) {
            /* use 1:1 scaling on 3.3V ADC input */
            unsigned mV = counts * 3300 / 4096;

            /* scale to 0..253 */
            rssi = mV / 13;
        }
    }
#endif

    perf_begin(c_gather_dsm);
    uint16_t temp_count = r_raw_rc_count;
    bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
    if (dsm_updated) {
        r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
        r_raw_rc_count = temp_count & 0x7fff;
        if (temp_count & 0x8000)
            r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
        else
            r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;

        r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
        r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);

    }
    perf_end(c_gather_dsm);

    perf_begin(c_gather_sbus);

    bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);

    bool sbus_failsafe, sbus_frame_drop;
    bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);

    if (sbus_updated) {
        r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;

        rssi = 255;

        if (sbus_frame_drop) {
            r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
            rssi = 100;
        } else {
            r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
        }

        if (sbus_failsafe) {
            r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
            rssi = 0;
        } else {
            r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
        }

    }

    perf_end(c_gather_sbus);

    /*
     * XXX each S.bus frame will cause a PPM decoder interrupt
     * storm (lots of edges).  It might be sensible to actually
     * disable the PPM decoder completely if we have S.bus signal.
     */
    perf_begin(c_gather_ppm);
    bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
    if (ppm_updated) {

        r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
        r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
        r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
    }
    perf_end(c_gather_ppm);

    /* limit number of channels to allowable data size */
    if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
        r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;

    /* store RSSI */
    r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;

我以为 PX4仅仅支持 ppmsbus,但从这里我们知道,其实 PX4还支持另外一种 dsm输入。当然,采用那种输入方式并不是我们现在所关心的。我们现在关系的是谁调用了 controls_tick函数。

radiolink@ubuntu:~/apm$ grep -nr controls_tick ./PX4Firmware/src/
./PX4Firmware/src/modules/px4iofirmware/controls.c:145:controls_tick() {
./PX4Firmware/src/modules/px4iofirmware/px4io.h:217:extern void controls_tick(void);
./PX4Firmware/src/modules/px4iofirmware/px4io.c:354:            controls_tick();
radiolink@ubuntu:~/apm$
int user_start(int argc, char *argv[])
{
    /* ... */
    
    for (;;) {

        /* track the rate at which the loop is running */
        perf_count(loop_perf);

        /* kick the mixer */
        perf_begin(mixer_perf);
        mixer_tick();
        perf_end(mixer_perf);

        /* kick the control inputs */
        perf_begin(controls_perf);
        controls_tick();

user_start函数是 IO的 init进程,这个前面我们已经分析过了。所以,最后是由 IO的主循环对 controls_tick函数进行调用。

    以上就是 IO中遥控器的输入。说完了输入,我们还得说说输出。这个我们稍后再来看。


乐山

PX4 FMU [12] RC_Channel

PX4 FMU [12] RC_Channel
                                              ...
PX4 FMU [12] RC_Channel
                                                                             -------- 转载请注明出处
                                                                             -------- 更多笔记请访问我的博客: merafour.blog.163.com 

                                                                             -------- 2014-12-25.冷月追风

                                                                             -------- email:merafour@163.com 


1. setup 
 
    在前一篇笔记中我们把 RC_Channel一笔带过了。但其实如果你到 " ardupilot/libraries/RC_Channel/"目录下去看你会看到:

radiolink@ubuntu:~/apm$ ls ardupilot/libraries/RC_Channel/
examples  RC_Channel_aux.cpp  RC_Channel_aux.h  RC_Channel.cpp  RC_Channel.h
radiolink@ubuntu:~/apm$


所以这里也有一个示例教你怎么使用 RC_Channel。完整路径为: " ardupilot/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde"。

#define CH_1 0
#define CH_2 1
#define CH_3 2
#define CH_4 3
#define CH_5 4
#define CH_6 5
#define CH_7 6
#define CH_8 7
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;

RC_Channel rc_1(CH_1);
RC_Channel rc_2(CH_2);
RC_Channel rc_3(CH_3);
RC_Channel rc_4(CH_4);
RC_Channel rc_5(CH_5);
RC_Channel rc_6(CH_6);
RC_Channel rc_7(CH_7);
RC_Channel rc_8(CH_8);
RC_Channel *rc = &rc_1;


实际上 RC_Channel很容易让人误以为它是遥控器的输入,但从前一篇笔记中我们看到其实不是。这一段就是定义了八个 RC_Channel变量,也就是可以处理八个通道数据。

void setup()
{
    hal.console->println("ArduPilot RC Channel test");
    hal.scheduler->delay(500);

    setup_radio();

    print_radio_values();

    // set type of output, symmetrical angles or a number range;
    // XXX BROKEN
    rc_1.set_angle(4500);
    rc_1.set_default_dead_zone(80);
    rc_1.set_type(RC_CHANNEL_TYPE_ANGLE_RAW);

    // XXX BROKEN
    rc_2.set_angle(4500);
    rc_2.set_default_dead_zone(80);
    rc_2.set_type(RC_CHANNEL_TYPE_ANGLE_RAW);

    rc_3.set_range(0,1000);
    rc_3.set_default_dead_zone(20);

    rc_4.set_angle(6000);
    rc_4.set_default_dead_zone(500);
    rc_4.set_type(RC_CHANNEL_TYPE_ANGLE_RAW);

    rc_5.set_range(0,1000);
    rc_6.set_range(200,800);

    rc_7.set_range(0,1000);

    rc_8.set_range(0,1000);

    for (int i = 0; i < 30; i++) {
        read_radio();
    }
    rc_1.trim();
    rc_2.trim();
    rc_4.trim();
}


set_range函数跟 set_range函数前面我们已经接触过了,用来设置范围的,就不重复去看了。而 set_default_dead_zone函数根据经验是用来设置死区的。要证明这点其实也简单,我们只需跟踪代码即可。

void RC_Channel::set_default_dead_zone(int16_t dzone)
{
    if (!_dead_zone.load()) {
        _dead_zone.set(abs(dzone));
    }
}
radiolink@ubuntu:~/apm$ grep -nr _dead_zone ./ardupilot/libraries/RC_Channel/
./ardupilot/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde:71:    rc_1.set_default_dead_zone(80);
./ardupilot/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde:75:    rc_2.set_default_dead_zone(80);
./ardupilot/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde:79:    rc_3.set_default_dead_zone(20);
./ardupilot/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde:82:    rc_4.set_default_dead_zone(500);
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:73:    // Note: index 4 was used by the previous _dead_zone value. We
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:84:    AP_GROUPINFO("DZ",   5, RC_Channel, _dead_zone, 0),
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:115:RC_Channel::set_default_dead_zone(int16_t dzone)
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:117:    if (!_dead_zone.load()) {
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:118:        _dead_zone.set(abs(dzone));
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:241:        int16_t radio_trim_low  = radio_min + _dead_zone;
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:258:    _dead_zone.load();
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:268:    _dead_zone.save();
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:315:    return pwm_to_angle_dz(_dead_zone);
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:358:    return pwm_to_range_dz(_dead_zone);
./ardupilot/libraries/RC_Channel/RC_Channel.h:59:    void        set_default_dead_zone(int16_t dzone);
./ardupilot/libraries/RC_Channel/RC_Channel.h:143:    AP_Int16        _dead_zone;
radiolink@ubuntu:~/apm$


稍加分析我们就能够发现我们要去查看的其实也就是那两个 return语句。

/*
  return an "angle in centidegrees" (normally -4500 to 4500) from
  the current radio_in value
 */
int16_t RC_Channel::pwm_to_angle()
{
    return pwm_to_angle_dz(_dead_zone);
}
/*
  convert a pulse width modulation value to a value in the configured
  range
 */


int16_t RC_Channel::pwm_to_range()
{
     return pwm_to_range_dz(_dead_zone);
}
那么我们就列举其中一个函数来看看具体是如何使用的:

/*
  return an "angle in centidegrees" (normally -4500 to 4500) from
  the current radio_in value using the specified dead_zone
 */
int16_t RC_Channel::pwm_to_angle_dz(uint16_t dead_zone)
{
    int16_t radio_trim_high = radio_trim + dead_zone;
    int16_t radio_trim_low  = radio_trim - dead_zone;

    // prevent div by 0
    if ((radio_trim_low - radio_min) == 0 || (radio_max - radio_trim_high) == 0)
        return 0;

    if(radio_in > radio_trim_high) {
        return _reverse * ((long)_high * (long)(radio_in - radio_trim_high)) / (long)(radio_max  - radio_trim_high);
    }else if(radio_in < radio_trim_low) {
        return _reverse * ((long)_high * (long)(radio_in - radio_trim_low)) / (long)(radio_trim_low - radio_min);
    }else
        return 0;
}


稍加分析我们就会知道这是量程映射,而且其中有那么一段映射完后值始终为 0,这一段便是死区。那么最后还剩下三个函数: setup_radio、print_radio_values和 read_radio。

void print_radio_values()
{
    for (int i=0; i<8; i++) {
         hal.console->printf("CH%u: %u|%u\n",
              (unsigned)i+1, 
              (unsigned)rc[i].radio_min, 
              (unsigned)rc[i].radio_max); 
    }
}


你会发现这里打印的一个数据格式跟我们校准成功时候的信息是一致的,如:

PX4 FMU [12] RC_Channel - Lonelys - 越长大越孤单  越长大越不安

 

这是我们校准成功时的一个截图,那么我就有理由认为 hal.console就是跟上位机通信的那个串口。下面我们来看另外一个函数:

void setup_radio(void)
{
    hal.console->println("\n\nRadio Setup:");
    uint8_t i;
    
    for(i = 0; i < 100;i++){
        hal.scheduler->delay(20);
        read_radio();
    }
        
    rc_1.radio_min = rc_1.radio_in;
    rc_2.radio_min = rc_2.radio_in;
    rc_3.radio_min = rc_3.radio_in;
    rc_4.radio_min = rc_4.radio_in;
    rc_5.radio_min = rc_5.radio_in;
    rc_6.radio_min = rc_6.radio_in;
    rc_7.radio_min = rc_7.radio_in;
    rc_8.radio_min = rc_8.radio_in;

    rc_1.radio_max = rc_1.radio_in;
    rc_2.radio_max = rc_2.radio_in;
    rc_3.radio_max = rc_3.radio_in;
    rc_4.radio_max = rc_4.radio_in;
    rc_5.radio_max = rc_5.radio_in;
    rc_6.radio_max = rc_6.radio_in;
    rc_7.radio_max = rc_7.radio_in;
    rc_8.radio_max = rc_8.radio_in;

    rc_1.radio_trim = rc_1.radio_in;
    rc_2.radio_trim = rc_2.radio_in;
    rc_4.radio_trim = rc_4.radio_in;
    // 3 is not trimed
    rc_5.radio_trim = 1500;
    rc_6.radio_trim = 1500;
    rc_7.radio_trim = 1500;
    rc_8.radio_trim = 1500;
            
    hal.console->println("\nMove all controls to each extreme. Hit Enter to save:");
    while(1){
        
        hal.scheduler->delay(20);
        // Filters radio input - adjust filters in the radio.pde file
        // ----------------------------------------------------------
        read_radio();

        rc_1.update_min_max();
        rc_2.update_min_max();
        rc_3.update_min_max();
        rc_4.update_min_max();
        rc_5.update_min_max();
        rc_6.update_min_max();
        rc_7.update_min_max();
        rc_8.update_min_max();
        
        if(hal.console->available() > 0) {
            hal.console->println("Radio calibrated, Showing control values:");
            break;
        }
    }
    return;
}


这个函数虽然不短,但它其实就只是获取最大值和最小值。说白了这就是用来校准遥控器的。而 read_radio函数就是用来读取遥控器数据。但我们知道,连接遥控器的是 io,所以 read_radio肯定是通过跟 io通信的方式获取数据的。但这里有个地方让我觉得很奇怪,为什么 3通道没有 " radio_trim"?结合 pwm_to_angle_dz函数我觉得比较合理的解释是 " radio_trim"是通道的中点,而三通是油门通道,不需要对中点进行处理,故而不需要中点。

void read_radio()
{
    rc_1.set_pwm(hal.rcin->read(CH_1));
    rc_2.set_pwm(hal.rcin->read(CH_2));
    rc_3.set_pwm(hal.rcin->read(CH_3));
    rc_4.set_pwm(hal.rcin->read(CH_4));
    rc_5.set_pwm(hal.rcin->read(CH_5));
    rc_6.set_pwm(hal.rcin->read(CH_6));
    rc_7.set_pwm(hal.rcin->read(CH_7));
    rc_8.set_pwm(hal.rcin->read(CH_8));
}


前面我们在 AP_Motors_test函数中看到给 RC_Channel传值是通过 servo_out进行的,但这里我们看到了另外一种方式,即 set_pwm函数。

// read input from APM_RC - create a control_in value
void RC_Channel::set_pwm(int16_t pwm)
{
    radio_in = pwm;

    if (_type == RC_CHANNEL_TYPE_RANGE) {
        control_in = pwm_to_range();
    } else {
        //RC_CHANNEL_TYPE_ANGLE, RC_CHANNEL_TYPE_ANGLE_RAW
        control_in = pwm_to_angle();
    }
}


这里调用的 pwm_to_range跟 pwm_to_angle函数其实就是我们前面看到的,在这里其实就是处理死区跟映射。如果是三通最后是调用 pwm_to_range_dz函数来完成的。

/*
  convert a pulse width modulation value to a value in the configured
  range, using the specified deadzone
 */
int16_t RC_Channel::pwm_to_range_dz(uint16_t dead_zone)
{
    int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get());

    if (_reverse == -1) {
        r_in = radio_max.get() - (r_in - radio_min.get());
    }

    int16_t radio_trim_low  = radio_min + dead_zone;

    if (r_in > radio_trim_low)
        return (_low + ((long)(_high - _low) * (long)(r_in - radio_trim_low)) / (long)(radio_max - radio_trim_low));
    else if (dead_zone > 0)
        return 0;
    else
        return _low;
}


经过对比我们看到该函数只是把数据映射到 [_high - _low], 但并没有使用中点数据。那么我们很容易想到 "hal.rcin->read"函数就是用来读遥控器数据的了。那么这个我们还得从 hal说起。

2. hal 

    如果我们回到 HAL_PX4的构造函数:

HAL_PX4::HAL_PX4() :
    AP_HAL::HAL(
        &uartADriver,  /* uartA */
        &uartBDriver,  /* uartB */
        &uartCDriver,  /* uartC */
        &uartDDriver,  /* uartD */
        &uartEDriver,  /* uartE */
        &i2cDriver, /* i2c */
        &spiDeviceManager, /* spi */
        &analogIn, /* analogin */
        &storageDriver, /* storage */
        &uartADriver, /* console */
        &gpioDriver, /* gpio */
        &rcinDriver,  /* rcinput */
        &rcoutDriver, /* rcoutput */
        &schedulerInstance, /* scheduler */
        &utilInstance) /* util */
{}
static PX4RCInput rcinDriver;


我们看到其中有一个 "rcinput",它其实就是 "hal.rcin"。类定义为:

#include <pthread.h>

class PX4::PX4RCInput : public AP_HAL::RCInput {
public:
    void init(void* machtnichts);
    bool new_input();
    uint8_t num_channels();
    uint16_t read(uint8_t ch);
    uint8_t read(uint16_t* periods, uint8_t len);

    bool set_overrides(int16_t *overrides, uint8_t len);
    bool set_override(uint8_t channel, int16_t override);
    void clear_overrides();

    void _timer_tick(void);

private:
    /* override state */
    uint16_t _override[RC_INPUT_MAX_CHANNELS];
    struct rc_input_values _rcin;
    int _rc_sub;
    uint64_t _last_read;
    bool _override_valid;
    perf_counter_t _perf_rcin;
    pthread_mutex_t rcin_mutex;
};


我们看到它其实有两个 read函数。我们也需要注意这里这个头文件: " pthread.h",区区一个头文件为什么值得特别注意呢?因为该头文件表明这个类会去创建任务。其中 _timer_tick函数应该就是这个任务需要执行的函数。而由于该函数不是私有的,所以它可能是由外部进行调用的。
    那么下面我们就不妨去看看这两个 read函数。

uint16_t PX4RCInput::read(uint8_t ch) 
{
    if (ch >= RC_INPUT_MAX_CHANNELS) {
        return 0;
    }
        pthread_mutex_lock(&rcin_mutex);
    _last_read = _rcin.timestamp_last_signal;
    _override_valid = false;
    if (_override[ch]) {
            uint16_t v = _override[ch];
            pthread_mutex_unlock(&rcin_mutex);
            return v;
    }
    if (ch >= _rcin.channel_count) {
            pthread_mutex_unlock(&rcin_mutex);
            return 0;
    }
    uint16_t v = _rcin.values[ch];
        pthread_mutex_unlock(&rcin_mutex);
        return v;
}

uint8_t PX4RCInput::read(uint16_t* periods, uint8_t len) 
{
    if (len > RC_INPUT_MAX_CHANNELS) {
        len = RC_INPUT_MAX_CHANNELS;
    }
    for (uint8_t i = 0; i < len; i++){
        periods[i] = read(i);
    }
    return len;
}


最后数据来自 _override跟 _rcin.values,那这两个家伙又是什么呢?我们一个个来跟踪。

bool PX4RCInput::set_overrides(int16_t *overrides, uint8_t len) 
{
    bool res = false;
    for (uint8_t i = 0; i < len; i++) {
        res |= set_override(i, overrides[i]);
    }
    return res;
}
bool

PX4RCInput::set_override(uint8_t channel, int16_t override) {
    if (override < 0) {
        return false; /* -1: no change. */
    }
    if (channel >= RC_INPUT_MAX_CHANNELS) {
        return false;
    }
    _override[channel] = override;
    if (override != 0) {
        _override_valid = true;
        return true;
    }
    return false;
}
radiolink@ubuntu:~/apm$ grep -nr set_overrides ./ |grep rcin
./ardupilot/ArduCopter/GCS_Mavlink.pde:1046:        hal.rcin->set_overrides(v, 8);
radiolink@ubuntu:~/apm$
所以这个就是校准的时候获取的数据。由于我们是从 io获取数据,所以我们的数据来自 _rcin.values。它是在 _timer_tick函数中把数据放进去的:

void PX4RCInput::_timer_tick(void)
{
    perf_begin(_perf_rcin);
    bool rc_updated = false;
    if (orb_check(_rc_sub, &rc_updated) == 0 && rc_updated) {
            pthread_mutex_lock(&rcin_mutex);
            orb_copy(ORB_ID(input_rc), _rc_sub, &_rcin);
            pthread_mutex_unlock(&rcin_mutex);
    }
        // note, we rely on the vehicle code checking new_input() 
        // and a timeout for the last valid input to handle failsafe
    perf_end(_perf_rcin);
}


折腾了半天又是 ORB。但 ORB究竟是什么我却始终未能参透。

int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
{
    int ret;

    ret = read(handle, buffer, meta->o_size);

    if (ret < 0)
        return ERROR;

    if (ret != (int)meta->o_size) {
        errno = EIO;
        return ERROR;
    }

    return OK;
}


orb_copy函数使用 read接口操作 _rc_sub来看 _rc_sub应该是一个文件描述符。

void PX4RCInput::init(void* unused)
{
    _perf_rcin = perf_alloc(PC_ELAPSED, "APM_rcin");
    _rc_sub = orb_subscribe(ORB_ID(input_rc));
    if (_rc_sub == -1) {
        hal.scheduler->panic("Unable to subscribe to input_rc");        
    }
    clear_overrides();
        pthread_mutex_init(&rcin_mutex, NULL);
}
int

orb_subscribe(const struct orb_metadata *meta)
{
    return node_open(PUBSUB, meta, nullptr, false);
}
/**
 * Common implementation for orb_advertise and orb_subscribe.
 *
 * Handles creation of the object and the initial publication for
 * advertisers.
 */
int node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser)
{
    char path[orb_maxpath];
    int fd, ret;

    /*
     * If meta is null, the object was not defined, i.e. it is not
     * known to the system.  We can't advertise/subscribe such a thing.
     */
    if (nullptr == meta) {
        errno = ENOENT;
        return ERROR;
    }

    /*
     * Advertiser must publish an initial value.
     */
    if (advertiser && (data == nullptr)) {
        errno = EINVAL;
        return ERROR;
    }

    /*
     * Generate the path to the node and try to open it.
     */
    ret = node_mkpath(path, f, meta);

    if (ret != OK) {
        errno = -ret;
        return ERROR;
    }

    /* open the path as either the advertiser or the subscriber */
    fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);

    /* we may need to advertise the node... */
    if (fd < 0) {

        /* try to create the node */
        ret = node_advertise(meta);

        /* on success, try the open again */
        if (ret == OK)
            fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
    }

    if (fd < 0) {
        errno = EIO;
        return ERROR;
    }

    /* everything has been OK, we can return the handle now */
    return fd;
}
代码跟踪到这里我们发现它直接就去 open了一个文件(应该是设备文件),但总不可能凭空产生一个文件吧,而且就算是凭空产生了一个文件,那 read操作也不可能成功啊。而且这里面为什么要两次 open操作? node_advertise函数有到底有什么密码呢?根据注释 " try to create the node"我觉得 node_advertise函数应该会去创建一个 node,但 node指的是设备文件吗?

/**
 * Advertise a node; don't consider it an error if the node has
 * already been advertised.
 *
 * @todo verify that the existing node is the same as the one
 *       we tried to advertise.
 */
int node_advertise(const struct orb_metadata *meta)
{
    int fd = -1;
    int ret = ERROR;

    /* open the control device */
    fd = open(TOPIC_MASTER_DEVICE_PATH, 0);

    if (fd < 0)
        goto out;

    /* advertise the object */
    ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)meta);

    /* it's OK if it already exists */
    if ((OK != ret) && (EEXIST == errno))
        ret = OK;

out:

    if (fd >= 0)
        close(fd);

    return ret;
}


真是一波未平一波又起啊,旧的文件都没搞明白,新的文件又出现了。而且貌似这里也没有去创建设备文件啊。那难道是由 TOPIC_MASTER_DEVICE_PATH这个设备来完成的吗?也只有这样能够解释得通了。详细的我们就后面再来专门讨论了。
    那么以上就是读数据的大体过程了。

3. loop

    loop的代码不多,只有几行语句:

void loop()
{
    hal.scheduler->delay(20);
    debug_rcin();
    read_radio();
    print_pwm();
}


其中 read_radio函数我们前面已经看过了, debug_rcin函数跟 print_pwm函数现在来看就简单多了。瞧:

void debug_rcin() {
    uint16_t channels[8];
    hal.rcin->read(channels, 8);
    hal.console->printf_P(
        PSTR("rcin: %u %u %u %u %u %u %u %u\r\n"),
        channels[0],
        channels[1],
        channels[2],
        channels[3],
        channels[4],
        channels[5],
        channels[6],
        channels[7]);
}
void

print_pwm()
{
    for (int i=0; i<8; i++) {
        hal.console->printf("ch%u: %4d ", (unsigned)i+1, (int)rc[i].control_in);
    }
    hal.console->printf("\n");
}

两个函数都是在向上位机传递数据,区别在于 debug_rcin传的是原始数据, print_pwm传的是映射后的数据。
    关于这个测试程序就这命多了,下面我们还是赶紧来看看 ORB到底是怎么回事吧。

乐山

PX4 FMU [7] rgbled

PX4 FMU [7] rgbled
                                               ...
PX4 FMU [7] rgbled
                                                                             -------- 转载请注明出处
                                                                             -------- 更多笔记请访问我的博客: merafour.blog.163.com 

                                                                             -------- 2014-12-15.冷月追风

                                                                             -------- email:merafour@163.com 

1. start 

    我们在命令行启动 rgbled的命令如下:

if rgbled start
then
        set HAVE_RGBLED 1
        rgbled rgb 16 16 16
else
        set HAVE_RGBLED 0
fi


入口函数如下:

int rgbled_main(int argc, char *argv[])
{
    int i2cdevice = -1;
    int rgbledadr = ADDR; /* 7bit */

    int ch;

    /* jump over start/off/etc and look at options first */
    while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
        switch (ch) {
        case 'a':
            rgbledadr = strtol(optarg, NULL, 0);
            break;

        case 'b':
            i2cdevice = strtol(optarg, NULL, 0);
            break;

        default:
            rgbled_usage();
            exit(0);
        }
    }

        if (optind >= argc) {
            rgbled_usage();
            exit(1);
        }

    const char *verb = argv[optind];

    int fd;
    int ret;

    if (!strcmp(verb, "start")) {
        if (g_rgbled != nullptr)
            errx(1, "already started");

        if (i2cdevice == -1) {
            // try the external bus first
            i2cdevice = PX4_I2C_BUS_EXPANSION;
            g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr);

            if (g_rgbled != nullptr && OK != g_rgbled->init()) {
                delete g_rgbled;
                g_rgbled = nullptr;
            }

            if (g_rgbled == nullptr) {
                // fall back to default bus
                if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
                    errx(1, "init failed");
                }
                i2cdevice = PX4_I2C_BUS_LED;
            }
        }

        if (g_rgbled == nullptr) {
            g_rgbled = new RGBLED(i2cdevice, rgbledadr);

            if (g_rgbled == nullptr)
                errx(1, "new failed");

            if (OK != g_rgbled->init()) {
                delete g_rgbled;
                g_rgbled = nullptr;
                errx(1, "init failed");
            }
        }

        exit(0);
    }

    /* need the driver past this point */
    if (g_rgbled == nullptr) {
        warnx("not started");
        rgbled_usage();
        exit(1);
    }

    if (!strcmp(verb, "test")) {
        fd = open(RGBLED_DEVICE_PATH, 0);

        if (fd == -1) {
            errx(1, "Unable to open " RGBLED_DEVICE_PATH);
        }

        rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF},
            {500, 500, 500, 500, 1000, 0 }    // "0" indicates end of pattern
        };

        ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern);
        ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN);

        close(fd);
        exit(ret);
    }

    if (!strcmp(verb, "info")) {
        g_rgbled->info();
        exit(0);
    }

    if (!strcmp(verb, "off") || !strcmp(verb, "stop")) {
        fd = open(RGBLED_DEVICE_PATH, 0);

        if (fd == -1) {
            errx(1, "Unable to open " RGBLED_DEVICE_PATH);
        }

        ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
        close(fd);
        exit(ret);
    }

    if (!strcmp(verb, "stop")) {
        delete g_rgbled;
        g_rgbled = nullptr;
        exit(0);
    }

    if (!strcmp(verb, "rgb")) {
        if (argc < 5) {
            errx(1, "Usage: rgbled rgb <red> <green> <blue>");
        }

        fd = open(RGBLED_DEVICE_PATH, 0);

        if (fd == -1) {
            errx(1, "Unable to open " RGBLED_DEVICE_PATH);
        }

        rgbled_rgbset_t v;
        v.red   = strtol(argv[2], NULL, 0);
        v.green = strtol(argv[3], NULL, 0);
        v.blue  = strtol(argv[4], NULL, 0);
        ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
        ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
        close(fd);
        exit(ret);
    }

    rgbled_usage();
    exit(0);
}


我们看到,在 start中跟 MPU6050的不同在于这里没有单独做一个 start接口。这也说明 rgbled的 start过程要比 MPU6050要简单得多。主要是创建了 RGBLED对象,并调用其 init函数。然后是 rgb,rgb呢主要是获取我们命令中的 RGB值,因为我们知道这是用来控制板子上的那个 RGBLED的。发送数据使用的是 ioctl接口。当然,在 Linux设备驱动中通常是用 read接口来读数据,在 MPU6050中就是这样。

RGBLED::RGBLED(int bus, int rgbled) :
    I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000),
    _mode(RGBLED_MODE_OFF),
    _r(0),
    _g(0),
    _b(0),
    _brightness(1.0f),
    _running(false),
    _led_interval(0),
    _should_run(false),
    _counter(0)
{
    memset(&_work, 0, sizeof(_work));
    memset(&_pattern, 0, sizeof(_pattern));
}
I2C::I2C(const char *name,
     const char *devname,
     int bus,
     uint16_t address,
     uint32_t frequency,
     int irq) :
    // base class
    CDev(name, devname, irq),
    // public
    // protected
    _retries(0),
    // private
    _bus(bus),
    _address(address),
    _frequency(frequency),
    _dev(nullptr)
{
    // fill in _device_id fields for a I2C device
    _device_id.devid_s.bus_type = DeviceBusType_I2C;
    _device_id.devid_s.bus = bus;
    _device_id.devid_s.address = address;
    // devtype needs to be filled in by the driver
    _device_id.devid_s.devtype = 0;     
}


I2C跟 SPI一样都继承自 CDev,所以它们之间很多东西都是相通的。对 C++不太了解的可能会觉得在给 I2C传递参数的过程中有点莫名其妙,会觉得少了一个参数。其实你去看构造函数的定义你就会知道, irq有一个初值,即 0。当我们调用的时候不提供该参数的时候 irq即为 0。其它参数我们主要看两个: devname跟 address。前者是设备文件名,后者是设备地址。其值为:

/* more devices will be 1, 2, etc */
#define RGBLED_DEVICE_PATH "/dev/rgbled0"
#define ADDR   PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */
#define PX4_I2C_OBDEV_LED 0x55


如果你有兴趣你可以去查看 PX4所使用的驱动芯片的 I2C地址。我没去研究硬件,就不深究了。
    其实构造函数中还有一个参数:bus。在这里, bus应该也指的是总线。那么它是指什么总线呢?
    我们会在 init函数中看到 bus被使用了:

int I2C::init()
{
    int ret = OK;

    /* attach to the i2c bus */
    _dev = up_i2cinitialize(_bus);

    if (_dev == nullptr) {
        debug("failed to init I2C");
        ret = -ENOENT;
        goto out;
    }

    // call the probe function to check whether the device is present
    ret = probe();

    if (ret != OK) {
        debug("probe failed");
        goto out;
    }

    // do base class init, which will create device node, etc
    ret = CDev::init();

    if (ret != OK) {
        debug("cdev init failed");
        goto out;
    }

    // tell the world where we are
    log("on I2C bus %d at 0x%02x", _bus, _address);

out:
    return ret;
}


在 SPI中我们也遇到个 up函数,返回的也是 dev,后面就是通过这个 dev跟真正的硬件打交道。

bitcraze@bitcraze-vm:~/apm$ grep -nr up_i2cinitialize ./PX4NuttX/nuttx/arch/arm/src/stm32
./PX4NuttX/nuttx/arch/arm/src/stm32/stm32_i2c.c:1888: * Name: up_i2cinitialize
./PX4NuttX/nuttx/arch/arm/src/stm32/stm32_i2c.c:1895:FAR struct i2c_dev_s *up_i2cinitialize(int port)
bitcraze@bitcraze-vm:~/apm$
FAR struct i2c_dev_s *up_i2cinitialize(int port)
{
    struct stm32_i2c_priv_s * priv = NULL;  /* private data of device with multiple instances */
    struct stm32_i2c_inst_s * inst = NULL;  /* device, single instance */
    int irqs;
    
#if STM32_PCLK1_FREQUENCY < 4000000
#   warning STM32_I2C_INIT: Peripheral clock must be at least 4 MHz to support 400 kHz operation.
#endif
    
#if STM32_PCLK1_FREQUENCY < 2000000
#   warning STM32_I2C_INIT: Peripheral clock must be at least 2 MHz to support 100 kHz operation.
    return NULL;
#endif
    
    /* Get I2C private structure */
    
    switch (port)
    {
#ifdef CONFIG_STM32_I2C1
        case 1:
            priv = (struct stm32_i2c_priv_s *)&stm32_i2c1_priv;
            break;
#endif
#ifdef CONFIG_STM32_I2C2
        case 2:
            priv = (struct stm32_i2c_priv_s *)&stm32_i2c2_priv;
            break;
#endif
#ifdef CONFIG_STM32_I2C3
        case 3:
            priv = (struct stm32_i2c_priv_s *)&stm32_i2c3_priv;
            break;
#endif
        default:
            return NULL;
    }
    
    /* Allocate instance */
    
    if (!(inst = kmalloc( sizeof(struct stm32_i2c_inst_s))))
    {
        return NULL;
    }
    
    /* Initialize instance */
    
    inst->ops       = &stm32_i2c_ops;
    inst->priv      = priv;
    inst->frequency = 100000;
    inst->address   = 0;
    inst->flags     = 0;
    
    /* Init private data for the first time, increment refs count,
     * power-up hardware and configure GPIOs.
     */
    
    irqs = irqsave();
    
    if ((volatile int)priv->refs++ == 0)
    {
        stm32_i2c_sem_init( (struct i2c_dev_s *)inst );
        stm32_i2c_init( priv );
    }
    
    irqrestore(irqs);
    return (struct i2c_dev_s *)inst;
}


跟 SPI是类似的。从代码来看,bus指的应该是所外部设备所挂载的 I2C控制器,因为 STM32存在多个 I2C控制器,而上层获取操作接口都是通过同一个函数,这样就需要区分不同的硬件接口,这就是 bus的作用。在 SPI中其实也一样。其实我们也很容易想到 priv不是用来操作硬件的, ops才是。只要稍微看下相关源码就清楚了。

    下面我们来看看 init。

int RGBLED::init()
{
    int ret;
    ret = I2C::init();

    if (ret != OK) {
        return ret;
    }

    /* switch off LED on start */
    send_led_enable(false);
    send_led_rgb();

    return OK;
}


通常来讲 init都是用来初始化的。这里其实也就是初始化。先是初始化 I2C,然后就把 LED给关了。在 I2C初始化过程中我们前面看到,它调用了一个 probe函数。在 Linux中写 platform驱动的时候我们经常碰到这个接口,在设备跟驱动匹配成功时会调用这个驱动,用来做一些初始化工作。在这里其实都差不多。

int RGBLED::probe()
{
    int ret;
    bool on, powersave;
    uint8_t r, g, b;

    /**
       this may look strange, but is needed. There is a serial
       EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to
       a bunch of I2C addresses, including the 0x55 used by this
       LED device. So we need to do enough operations to be sure
       we are talking to the right device. These 3 operations seem
       to be enough, as the 3rd one consistently fails if no
       RGBLED is on the bus.
     */
    if ((ret=get(on, powersave, r, g, b)) != OK ||
        (ret=send_led_enable(false) != OK) ||
        (ret=send_led_enable(false) != OK)) {
        return ret;
    }

    return ret;
}


单从代码上看,这里有点让人费解。而从注释来看是为了确保我们操作对的设备。可能要详细理解这段代码还需要专门花点时间,毕竟我们现在只是从软件方面解读 PX4。

2. 控制LED

    test其实已经提供了控制 LED的示例,即通过 ioctl进行控制。那么在应用层又是如何控制 LED的呢?

bitcraze@bitcraze-vm:~/apm$ grep -nr RGBLED_DEVICE_PATH ./ardupilot/
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:38:    _rgbled_fd = open(RGBLED_DEVICE_PATH, 0);
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:40:        hal.console->printf("Unable to open " RGBLED_DEVICE_PATH);
bitcraze@bitcraze-vm:~/apm$
bool ToshibaLED_PX4::hw_init()
{
    // open the rgb led device
    _rgbled_fd = open(RGBLED_DEVICE_PATH, 0);
    if (_rgbled_fd == -1) {
        hal.console->printf("Unable to open " RGBLED_DEVICE_PATH);
        return false;
    }
    ioctl(_rgbled_fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
    last.zero();
    next.zero();
    hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&ToshibaLED_PX4::update_timer));
    return true;
}


这里打开了我们的 LED设备,而且通过 ioctl对设备进行了一次操作。那么可以想象,后面肯定都是通过 _rgbled_fd对设备进行操作的。

// set_rgb - set color as a combination of red, green and blue values
bool ToshibaLED_PX4::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{
    hal.scheduler->suspend_timer_procs();
    next[0] = red;
    next[1] = green;
    next[2] = blue;
    hal.scheduler->resume_timer_procs();
    return true;
}
void ToshibaLED_PX4::update_timer(void)
{
    if (last == next) {
        return;
    }
    rgbled_rgbset_t v;

    v.red   = next[0];
    v.green = next[1];
    v.blue  = next[2];

    ioctl(_rgbled_fd, RGBLED_SET_RGB, (unsigned long)&v);

    last = next;
}


真实情况也正是这样。但是这个时候如果继续跟踪代码我相信你会跟我一样抓狂。

radiolink@ubuntu:~/px4$ grep -nr ToshibaLED_PX4 ./ardupilot/
./ardupilot/libraries/AP_Notify/AP_Notify.h:25:#include <ToshibaLED_PX4.h>
./ardupilot/libraries/AP_Notify/AP_Notify.h:66:    ToshibaLED_PX4 toshibaled;
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.h:25:class ToshibaLED_PX4 : public ToshibaLED
./ardupilot/libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde:35:static ToshibaLED_PX4 toshiba_led;
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:22:#include "ToshibaLED_PX4.h"
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:35:bool ToshibaLED_PX4::hw_init()
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:46:    hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&ToshibaLED_PX4::update_timer));
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:51:bool ToshibaLED_PX4::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:61:void ToshibaLED_PX4::update_timer(void)
./ardupilot/module.mk:3:SRCS = 
radiolink@ubuntu:~/px4$

SRCS内容特别长,因为 SRCS记录了 ardupilot这个应用程序所用到的所有源文件。从这段匹配结果来看,我们很容易知道我们的 LED是由 "ToshibaLED_test.pde"进行控制的,但你可别忘了它的路径中可是有个 "examples"!你再取看它的源码:

void setup(void)
{
}
void loop(void)
{
}
AP_HAL_MAIN();

从这里看我们似乎已经找错方向了,却不知错误是从什么时候开始的。于是我开始怀疑 PX4运行的时候到底是不是通过 rgbled来进行控制的。于是我想了一招,执行 "rgbled stop"命令,结构灯真的就不亮了。然后我在执行 "rgbled rgb 16 16 16"命令,灯又开始闪烁了。那么也许 rgb就会是我们救命的稻草了。

3.rgb

    在 rgbled的入口函数中 rgb的内容如下:

    if (!strcmp(verb, "rgb")) {
        if (argc < 5) {
            errx(1, "Usage: rgbled rgb <red> <green> <blue>");
        }

        fd = open(RGBLED_DEVICE_PATH, 0);

        if (fd == -1) {
            errx(1, "Unable to open " RGBLED_DEVICE_PATH);
        }

        rgbled_rgbset_t v;
        v.red   = strtol(argv[2], NULL, 0);
        v.green = strtol(argv[3], NULL, 0);
        v.blue  = strtol(argv[4], NULL, 0);
        ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
        ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
        close(fd);
        exit(ret);
    }

貌似,好像这段代码除了调用了几个接口也没有什么特别之处,但根据前面的判断秘密肯定就藏在这里,除非说这一次我又判断失误了。既然已经肯定这里有秘密,用排除法 ioctl是最有嫌疑的。

int RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
{
    int ret = ENOTTY;

    switch (cmd) {
    case RGBLED_SET_RGB:
        /* set the specified color */
        _r = ((rgbled_rgbset_t *) arg)->red;
        _g = ((rgbled_rgbset_t *) arg)->green;
        _b = ((rgbled_rgbset_t *) arg)->blue;
        send_led_rgb();
        return OK;

    case RGBLED_SET_COLOR:
        /* set the specified color name */
        set_color((rgbled_color_t)arg);
        send_led_rgb();
        return OK;

    case RGBLED_SET_MODE:
        /* set the specified mode */
        set_mode((rgbled_mode_t)arg);
        return OK;

    case RGBLED_SET_PATTERN:
        /* set a special pattern */
        set_pattern((rgbled_pattern_t *)arg);
        return OK;

    default:
        /* see if the parent class can make any use of it */
        ret = CDev::ioctl(filp, cmd, arg);
        break;
    }

    return ret;
}

所以我们看到,这里又引出了另外两个函数: send_led_rgbset_mode。其源码如下:

int RGBLED::send_led_rgb()
{
    /* To scale from 0..255 -> 0..15 shift right by 4 bits */
    const uint8_t msg[6] = {
        SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4),
        SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4),
        SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4)
    };
    return transfer(msg, sizeof(msg), nullptr, 0);
}
void RGBLED::set_mode(rgbled_mode_t mode)
{
    if (mode != _mode) {
        _mode = mode;

        switch (mode) {
        default:
            warnx("mode unknown");
            break;
        }

        /* if it should run now, start the workq */
        if (_should_run && !_running) {
            _running = true;
            work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
        }

    }
}

mode部分源码我没有全部粘出来。
    可能你会觉得这里除了调用了几个函数似乎也并没有什么特别,但是请记住,这里调用了一个 work_queue函数,也就是说用到了工作队列,其定义如下:

int work_queue(int qid, FAR struct work_s *work, worker_t worker,
               FAR void *arg, uint32_t delay)

所以,在延时 delay之后会调用 RGBLED::led_trampoline这个函数。那么它又干了些啥事呢?

void RGBLED::led_trampoline(void *arg)
{
    RGBLED *rgbl = reinterpret_cast<RGBLED *>(arg);

    rgbl->led();
}

/**
 * Main loop function
 */
void RGBLED::led()
{
    if (!_should_run) {
        _running = false;
        return;
    }

    switch (_mode) {
    case RGBLED_MODE_BLINK_SLOW:
    case RGBLED_MODE_BLINK_NORMAL:
    case RGBLED_MODE_BLINK_FAST:
        if (_counter >= 2)
            _counter = 0;

        send_led_enable(_counter == 0);

        break;

    case RGBLED_MODE_BREATHE:

        if (_counter >= 62)
            _counter = 0;

        int n;

        if (_counter < 32) {
            n = _counter;

        } else {
            n = 62 - _counter;
        }

        _brightness = n * n / (31.0f * 31.0f);
        send_led_rgb();
        break;

    case RGBLED_MODE_PATTERN:

        /* don't run out of the pattern array and stop if the next frame is 0 */
        if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
            _counter = 0;

        set_color(_pattern.color[_counter]);
        send_led_rgb();
        _led_interval = _pattern.duration[_counter];
        break;

    default:
        break;
    }

    _counter++;

    /* re-queue ourselves to run again later */
    work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval);
}

由于 rgb中设置 _mode为 RGBLED_MODE_ON,所以必须有谁去修改它的值,否则 LED将一直处于亮的状态,而不是闪烁。
    当然,看到这里我们知道 LED如何能够保持闪烁,但是如何改变 LED状态,如颜色呢?从上面的代码看是跟 _mode这个变量有关,但继续跟踪代码你就会发现你只能通过 set_mode函数来设置 _mode,但是这个操作又只有通过 ioctl的 RGBLED_SET_MODE命令来调用。

radiolink@ubuntu:~/apm$ grep -nr RGBLED_SET_MODE ./
./PX4Firmware/src/modules/commander/commander_helper.cpp:272:           ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
./PX4Firmware/src/drivers/drv_rgbled.h:76:#define RGBLED_SET_MODE                       _RGBLEDIOC(6)
./PX4Firmware/src/drivers/drv_rgbled.h:112:/* enum passed to RGBLED_SET_MODE ioctl()*/
./PX4Firmware/src/drivers/drv_io_expander.h:66:/* enum passed to RGBLED_SET_MODE ioctl()*/
./PX4Firmware/src/drivers/rgbled/rgbled.cpp:234:        case RGBLED_SET_MODE:
./PX4Firmware/src/drivers/rgbled/rgbled.cpp:663:                ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN);
./PX4Firmware/src/drivers/rgbled/rgbled.cpp:681:                ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
./PX4Firmware/src/drivers/rgbled/rgbled.cpp:708:                ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:43:    ioctl(_rgbled_fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
radiolink@ubuntu:~/apm$

从匹配结果来判断这个操作是在 commander_helper.cpp中完成的。首先排除 rgbled本身, ToshibaLED_PX4.cpp前面我们分析的就是它,确定是死路一条,况且它设置的是固定的模式。于是剩下的就只有 commander_helper.cpp了,从匹配结果来看,它可以设置多种模式。
    但是问题也随之而来,当你继续跟踪代码的时候,你会最终找到一个函数:commander_main。从函数名我们很容易想到它对应了一个命令: commander。但是很遗憾,脚本中根本就没有该命令。而且,当你尝试进行代码匹配你会发现:

radiolink@ubuntu:~/apm$ grep -nr commander_main ./
./PX4Firmware/src/modules/commander/commander.cpp:200:extern "C" __EXPORT int commander_main(int argc, char *argv[]);
./PX4Firmware/src/modules/commander/commander.cpp:246:int commander_main(int argc, char *argv[])
radiolink@ubuntu:~/apm$

于是我也只能无语了。那么是否就到此为止了呢?事情还远远没有结束!
    既然这条路已经走不通了,那么只能回过头来再考虑 ToshibaLED_PX4.cpp了。那么我们前面到底忽略了什么呢?
    当我回过头去看前面的笔记时,我惊讶地发现:

radiolink@ubuntu:~/px4$ grep -nr ToshibaLED_PX4 ./ardupilot/
./ardupilot/libraries/AP_Notify/AP_Notify.h:25:#include <ToshibaLED_PX4.h>
./ardupilot/libraries/AP_Notify/AP_Notify.h:66:    ToshibaLED_PX4 toshibaled;
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.h:25:class ToshibaLED_PX4 : public ToshibaLED
./ardupilot/libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde:35:static ToshibaLED_PX4 toshiba_led;
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:22:#include "ToshibaLED_PX4.h"
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:35:bool ToshibaLED_PX4::hw_init()
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:46:    hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&ToshibaLED_PX4::update_timer));
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:51:bool ToshibaLED_PX4::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:61:void ToshibaLED_PX4::update_timer(void)
./ardupilot/module.mk:3:SRCS = 
radiolink@ubuntu:~/px4$

=前面我看到这段信息就很直接地以为是在 ToshibaLED_test.pde中对 LED进行控制,但是我们忽略了一个头文件: AP_Notify.h。因为我一直都是用 C的,所以很自然地以为这里只是普通的声明。所以我忘了一点:这是 PX4源码,用的是 C++!而事情也正是在这里峰回路转了。

class AP_Notify
{
public:
    /// notify_type - bitmask of notification types
    struct notify_type {
        uint16_t initialising       : 1;    // 1 if initialising and copter should not be moved
        uint16_t gps_status         : 3;    // 0 = no gps, 1 = no lock, 2 = 2d lock, 3 = 3d lock, 4 = dgps lock, 5 = rtk lock
        uint16_t gps_glitching      : 1;    // 1 if gps position is not good
        uint16_t armed              : 1;    // 0 = disarmed, 1 = armed
        uint16_t pre_arm_check      : 1;    // 0 = failing checks, 1 = passed
        uint16_t save_trim          : 1;    // 1 if gathering trim data
        uint16_t esc_calibration    : 1;    // 1 if calibrating escs
        uint16_t failsafe_radio     : 1;    // 1 if radio failsafe
        uint16_t failsafe_battery   : 1;    // 1 if battery failsafe
        uint16_t failsafe_gps       : 1;    // 1 if gps failsafe
        uint16_t arming_failed      : 1;    // 1 if copter failed to arm after user input
        uint16_t parachute_release  : 1;    // 1 if parachute is being released

        // additional flags
        uint16_t external_leds      : 1;    // 1 if external LEDs are enabled (normally only used for copter)
    };

    // the notify flags are static to allow direct class access
    // without declaring the object
    static struct notify_type flags;

    // initialisation
    void init(bool enable_external_leds);

    /// update - allow updates of leds that cannot be updated during a timed interrupt
    void update(void);

private:
    // individual drivers
    AP_BoardLED boardled;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
    ToshibaLED_PX4 toshibaled;
    ToneAlarm_PX4 tonealarm;
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2 
    ToshibaLED_I2C toshibaled;
    ExternalLED externalled;
    Buzzer buzzer;
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
    ToshibaLED_I2C toshibaled;
    ExternalLED externalled;
    Buzzer buzzer;
#else
    ToshibaLED_I2C toshibaled;
#endif
};

所以我们看到,这里对应的是一个类: AP_Notify。如果你去查 "notify"这个单词,你会看到它是 “通告,通知;公布”的意思。这样一来就说得通了。而这里的 flags请注意,它前面可有个 "static",在 C++中表示 flags属于整个类。所以我们很容易匹配到这样的一些结果:

./ardupilot/ArduCopter/motors.pde:40:                    AP_Notify::flags.arming_failed = true;
./ardupilot/ArduCopter/motors.pde:45:                AP_Notify::flags.arming_failed = true;
./ardupilot/ArduCopter/motors.pde:75:        AP_Notify::flags.arming_failed = false;
./ardupilot/ArduCopter/motors.pde:123:    AP_Notify::flags.armed = true;
./ardupilot/ArduCopter/motors.pde:151:            AP_Notify::flags.armed = false;

即直接通过类名称修改该变量的值。

radiolink@ubuntu:~/apm$ grep -nr AP_Notify ./ardupilot/
./ardupilot/ArduCopter/ArduCopter.pde:145:#include <AP_Notify.h>          // Notify library
./ardupilot/ArduCopter/ArduCopter.pde:203:// AP_Notify instance
./ardupilot/ArduCopter/ArduCopter.pde:204:static AP_Notify notify;
./ardupilot/ArduCopter/ArduCopter.pde:1207:        // run glitch protection and update AP_Notify if home has been initialised
./ardupilot/ArduCopter/ArduCopter.pde:1211:            if (AP_Notify::flags.gps_glitching != report_gps_glitch) {
./ardupilot/ArduCopter/ArduCopter.pde:1217:                AP_Notify::flags.gps_glitching = report_gps_glitch;
radiolink@ubuntu:~/apm$

那么 "ArduCopter.pde"这个源文件我们应该不陌生了,在 PX4中这是很重要的一个文件,因为如果是在 APM中我们认为我们的程序是从这里开始的。在 PX4中,如果抛开操作系统个人觉得我们也应该这样认为。那么分析到这里,我觉得这条路已经走通了,剩下的就是去看下 LED具体是如何控制的。这将顺着 notify继续讨论下去。

4. Notify

    前面我们看到在 AP_Notify中只有两个函数,我们现在就先来看下这两个函数都做了哪些事情。

// initialisation
void AP_Notify::init(bool enable_external_leds)
{
    AP_Notify::flags.external_leds = enable_external_leds;

    boardled.init();
    toshibaled.init();

#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
    tonealarm.init();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
    externalled.init();
    buzzer.init();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
    externalled.init();
    buzzer.init();
#endif
}

// main update function, called at 50Hz
void AP_Notify::update(void)
{
    boardled.update();
    toshibaled.update();

#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
    tonealarm.update();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
    externalled.update();
    buzzer.update();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
    externalled.update();
    buzzer.update();
#endif
}

其实就是调用别人的 init跟 update函数,而且注释还告诉了我们 update函数调用的频率为 50Hz。这里我们关系的只是 toshibaled,其它的我们现在就不去看了。其实你在这里把 toshibaled搞通了,其它的也一样理顺了。

radiolink@ubuntu:~/apm$ grep -nr notify.init ./ardupilot/
./ardupilot/APMrover2/APMrover2.pde:553:    notify.init(false);
./ardupilot/Build.ArduCopter/ArduCopter.cpp:15657:    notify.init(true);
./ardupilot/ArduPlane/ArduPlane.pde:840:    notify.init(false);
./ardupilot/AntennaTracker/AntennaTracker.pde:268:    notify.init(false);
./ardupilot/ArduCopter/system.pde:141:    notify.init(true);
radiolink@ubuntu:~/apm$
static void init_ardupilot()
{
    bool enable_external_leds = true;
    // init EPM cargo gripper
#if EPM_ENABLED == ENABLED
    epm.init();
    enable_external_leds = !epm.enabled();
#endif
    // initialise notify system
    // disable external leds if epm is enabled because of pin conflict on the APM
    notify.init(enable_external_leds);
}

前面我们分析 "ArduCopter"遇到过 "init_ardupilot"这个函数,当时我因为说不清楚这里的每个东西都是做什么用的,所以就略过了。而现在,我们终于又搞懂了一个。

radiolink@ubuntu:~/apm$ grep -nr notify.update ./ardupilot/
./ardupilot/APMrover2/system.pde:424:    notify.update();
./ardupilot/APMrover2/GCS_Mavlink.pde:1167:        notify.update();
./ardupilot/Build.ArduCopter/ArduCopter.cpp:3971:        notify.update();
./ardupilot/Build.ArduCopter/ArduCopter.cpp:13066:    notify.update();
./ardupilot/ArduPlane/system.pde:554:    notify.update();
./ardupilot/ArduPlane/GCS_Mavlink.pde:1605:        notify.update();
./ardupilot/AntennaTracker/system.pde:118:    notify.update();
./ardupilot/AntennaTracker/GCS_Mavlink.pde:772:        notify.update();
./ardupilot/ArduCopter/leds.pde:7:    notify.update();
./ardupilot/ArduCopter/GCS_Mavlink.pde:1632:        notify.update();
radiolink@ubuntu:~/apm$
// updates the status of notify
// should be called at 50hz
static void update_notify()
{
    notify.update();
}
static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
    { rc_loop,               4,     10 },
    { throttle_loop,         8,     45 },
    { update_GPS,            8,     90 },
    { update_notify,         8,     10 },

前面我们已经知道了 PX4控制速率是 400Hz,所以不必去看那个 100Hz的了。而 scheduler_tasks里面存放的是 ArduCopter中的任务,这是在线程内部实现的任务调度,在 APM中就已经存在,所以跟 Nuttx没关。
    那么我们现在要做的就是沿着 toshibaled.init()跟 toshibaled.update()这两个函数往下走。

5. toshibaled

    首先呢我们当然是应该看看 ToshibaLED_PX4的定义。

class ToshibaLED_PX4 : public ToshibaLED
{
public:
    bool hw_init(void);
    bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b);
private:
    int _rgbled_fd;
    void update_timer(void);
    
    VectorN<uint8_t,3> last, next;
};

从定义中我们看到, init跟 update并没有被重写,也就是继承了父类中的方法。那么:

void ToshibaLED::init()
{
    _healthy = hw_init();
}
// update - updates led according to timed_updated.  Should be called
// at 50Hz
void ToshibaLED::update()
{
    // return immediately if not enabled
    if (!_healthy) {
        return;
    }
    update_colours();
    set_rgb(_red_des, _green_des, _blue_des);
}

如果你手上的是最新版源码,你会看到一个新的类: "class ToshibaLED: public RGBLed"。多少个类都无所谓,都差不多。 hw_init这个函数我们前面也看了,就是做一些初始化工作,包括打开设备。

bool ToshibaLED_PX4::hw_init()
{
    // open the rgb led device
    _rgbled_fd = open(RGBLED_DEVICE_PATH, 0);
    if (_rgbled_fd == -1) {
        hal.console->printf("Unable to open " RGBLED_DEVICE_PATH);
        return false;
    }
    ioctl(_rgbled_fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
    last.zero();
    next.zero();
    hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&ToshibaLED_PX4::update_timer));
    return true;
}

其中稍微要注意的就是 hal这条语句。这里又注册了一个调度函数。因为这里是通过 hal注册的,所以我们知道这里又是由 ArduCopter提供任务调度。如果忽略了这一点你看 update又会觉得糊涂,因为你去看源码就会发现 set_rgb最终调用了 hw_set_rgb函数,但是你去看该函数:

// set_rgb - set color as a combination of red, green and blue values
bool ToshibaLED_PX4::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{
    hal.scheduler->suspend_timer_procs();
    next[0] = red;
    next[1] = green;
    next[2] = blue;
    hal.scheduler->resume_timer_procs();
    return true;
}

你会觉得它好像并没有实际操作硬件。但其实我们刚看到的只是更新了数据,真正把数据写入硬件的正是 ToshibaLED_PX4::update_timer这个函数的工作。具体颜色是如何更新的去看 update_colours这个函数最清楚。

void ToshibaLED_PX4::update_timer(void)
{
    if (last == next) {
        return;
    }
    rgbled_rgbset_t v;

    v.red   = next[0];
    v.green = next[1];
    v.blue  = next[2];

    ioctl(_rgbled_fd, RGBLED_SET_RGB, (unsigned long)&v);

    last = next;
}

知道这里我才知道前面我认为调用 ioctl应该使用 "RGBLED_SET_MODE"错得有多么离谱。但我们如果去看 rgbled的代码你会发现:

int
RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
{
    int ret = ENOTTY;

    switch (cmd) {
    case RGBLED_SET_RGB:
        /* set the specified color */
        _r = ((rgbled_rgbset_t *) arg)->red;
        _g = ((rgbled_rgbset_t *) arg)->green;
        _b = ((rgbled_rgbset_t *) arg)->blue;
        send_led_rgb();
        return OK;
/**
 * Send RGB PWM settings to LED driver according to current color and brightness
 */
int
RGBLED::send_led_rgb()
{
    /* To scale from 0..255 -> 0..15 shift right by 4 bits */
    const uint8_t msg[6] = {
        SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4),
        SUB_ADDR_PWM1

乐山

PX4 FMU [5] Loop

PX4 FMU [5] Loop...


PX4 FMU [5] Loop

                                                                             -------- 转载请注明出处
                                                                             -------- 更多笔记请访问我的博客:merafour.blog.163.com 

                                                                             -------- 2014-11-27.冷月追风

                                                                             -------- email:merafour@163.com 


1.loop 


    我们这里没有先去分析 setup,主要是初始化过程中很多东西说不清楚,既然说不清,那就不如不说。
    当然,我这里也并不是真的为了分析 loop而来,而是想知道它是怎么去读取 MPU6050这个传感器的,为我们后面分析 mpu6050做准备。
    那么,为什么是 MPU6050呢?因为在所有的这些传感器中 MPU6050相对来说是我比较熟悉的,之前在小四轴上用得也最多。人嘛不都是这样,做什么事都习惯从自己最熟悉的入手,要是实在没办法,那也只好硬着头皮上了。

void loop()
{
    // wait for an INS sample
    // 等待数据,AP_InertialSensor_PX4 ins
    if (!ins.wait_for_sample(1000)) {
        Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_MAIN_INS_DELAY);
        return;
    }
    uint32_t timer = micros();

    // check loop time
    // 计算最大的循环时间..
    perf_info_check_loop_time(timer - fast_loopTimer);

    // used by PI Loops
    G_Dt                    = (float)(timer - fast_loopTimer) / 1000000.f;
    fast_loopTimer          = timer; //记录当前循环时间...

    // for mainloop failure monitoring
    mainLoop_count++;

    // Execute the fast loop
    // ---------------------
    fast_loop();

    // tell the scheduler one tick has passed
    scheduler.tick();

    // run all the tasks that are due to run. Note that we only
    // have to call this once per loop, as the tasks are scheduled
    // in multiples of the main loop tick. So if they don't run on
    // the first call to the scheduler they won't run on a later
    // call until scheduler.tick() is called again
    uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros(); //任务周期..
    scheduler.run(time_available); //调度..
}


主循环中我个人是把它分为这么四部分的:等数据、计算周期、快速循环、任务调度。
    可能写过 Linux应用程序的人会觉得很奇怪,操作系统本身就有一个任务调度,为什么这里又有一个任务调度?通常在 Linux应用编程中除非我们基于某种目的主动放弃 CPU,否则是不需要调用调度相关的函数的。那么,这里干的是啥事呢?
    要回答这个问题就涉及到前面的快速循环。既然有快速循环,那么相对地就有一个慢速循环。而慢速循环就是通过这里的调度来实现的。也就是说这里的任务跟 Nuttx系统是没有关系的,它是运行在 ArduPilot之内。或者你这样想, arduino不是跑系统的,那么你又要实现一个多任务你应该怎么做呢?因为没有人为你提供任务调度,那么你也只好自己去实现。有一种相对廉价的方式就是轮询。而这里用的就是轮询。
    计算周期呢这个最简单,就是当前循环时间跟上一次循环时间做差,然后换算一些单位。
    那么最后还有一个等数据,那么等的是啥数据呢?

#if CONFIG_INS_TYPE == HAL_INS_MPU6000
AP_InertialSensor_MPU6000 ins;
#elif CONFIG_INS_TYPE == HAL_INS_PX4
AP_InertialSensor_PX4 ins; //这里被编译...
#elif CONFIG_INS_TYPE == HAL_INS_VRBRAIN
AP_InertialSensor_VRBRAIN ins;
#elif CONFIG_INS_TYPE == HAL_INS_HIL
AP_InertialSensor_HIL ins;
#elif CONFIG_INS_TYPE == HAL_INS_OILPAN
AP_InertialSensor_Oilpan ins( &apm1_adc );
#elif CONFIG_INS_TYPE == HAL_INS_FLYMAPLE
AP_InertialSensor_Flymaple ins;
#elif CONFIG_INS_TYPE == HAL_INS_L3G4200D
AP_InertialSensor_L3G4200D ins;
#elif CONFIG_INS_TYPE == HAL_INS_MPU9250
AP_InertialSensor_MPU9250 ins;
#else
  #error Unrecognised CONFIG_INS_TYPE setting.
#endif // CONFIG_INS_TYPE


这个时候我们已经不用测试都知道 ins实际上就是 "AP_InertialSensor_PX4"类型的。所以我们等数据的时候运行的是下面的代码:

bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
{
    if (_sample_available()) {
        return true;
    }
    uint32_t start = hal.scheduler->millis();
    while ((hal.scheduler->millis() - start) < timeout_ms) {
        uint64_t tnow = hrt_absolute_time();
        // we spin for the last timing_lag microseconds. Before that
        // we yield the CPU to allow IO to happen
        const uint16_t timing_lag = 400;
        if (_last_sample_timestamp + _sample_time_usec > tnow+timing_lag) {
            hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_time_usec - (tnow+timing_lag));
        }
        if (_sample_available()) {
            return true;
        }
    }
    return false;
}


然后我回过头去看它的形参:1000,妈呀,居然是整整 1s!那么它真的会傻傻地在哪里等 1s吗? 怎么可能!那早都炸机了。所以秘密就在 "_sample_available()"这个函数里边。

bool AP_InertialSensor_PX4::_sample_available(void)
{
    uint64_t tnow = hrt_absolute_time();
    while (tnow - _last_sample_timestamp > _sample_time_usec) {
        _have_sample_available = true;
        _last_sample_timestamp += _sample_time_usec;
    }
    return _have_sample_available;
}


所以说 "_sample_time_usec"才是我们真正的等待时间。那是多长时间呢?我们可以去看其初始化:

uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate ) 
{
    // assumes max 2 instances
    _accel_fd[0] = open(ACCEL_DEVICE_PATH, O_RDONLY);
    _accel_fd[1] = open(ACCEL_DEVICE_PATH "1", O_RDONLY);
    _accel_fd[2] = open(ACCEL_DEVICE_PATH "2", O_RDONLY);
    _gyro_fd[0] = open(GYRO_DEVICE_PATH, O_RDONLY);
    _gyro_fd[1] = open(GYRO_DEVICE_PATH "1", O_RDONLY);
    _gyro_fd[2] = open(GYRO_DEVICE_PATH "2", O_RDONLY);

    _num_accel_instances = 0;
    _num_gyro_instances = 0;
    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
        if (_accel_fd[i] >= 0) {
            _num_accel_instances = i+1;
        }
        if (_gyro_fd[i] >= 0) {
            _num_gyro_instances = i+1;
        }
    }    
    if (_num_accel_instances == 0) {
        hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH);
    }
    if (_num_gyro_instances == 0) {
        hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH);
    }

    switch (sample_rate) {
    case RATE_50HZ:
        _default_filter_hz = 15;
        _sample_time_usec = 20000;
        break;
    case RATE_100HZ:
        _default_filter_hz = 30;
        _sample_time_usec = 10000;
        break;
    case RATE_200HZ:
        _default_filter_hz = 30;
        _sample_time_usec = 5000;
        break;
    case RATE_400HZ:
    default:
        _default_filter_hz = 30;
        _sample_time_usec = 2500;
        break;
    }

    _set_filter_frequency(_mpu6000_filter);

#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
    return AP_PRODUCT_ID_PX4_V2;
#else
    return AP_PRODUCT_ID_PX4;
#endif
}


所以主要取决于我们选择多快的速率。但是在这里,我却以外地发现了另外两个东西: "_default_filter_hz"跟 "_accel_fd"。我们很容易就想到前者是滤波用的,而后者是用来操作加计的。那么现在的问题是,我们到底设置了一个多高的速率?
    在 AP_InertialSensor_PX4类的父类中有这样一个函数:

void AP_InertialSensor::init( Start_style style,
                         Sample_rate sample_rate)
{
    _product_id = _init_sensor(sample_rate);

    // check scaling
    for (uint8_t i=0; i<get_accel_count(); i++) {
        if (_accel_scale[i].get().is_zero()) {
            _accel_scale[i].set(Vector3f(1,1,1));
        }
    }

    if (WARM_START != style) {
        // do cold-start calibration for gyro only
        _init_gyro();
    }
}


不用我解释大家肯定都已经看出来这是一个初始化函数。有时候我不太喜欢 C++就是这个原因,父类子类,子类父类,真的可以把人都给整晕掉。那么这个初始化函数肯定也是有人去调用的。

void setup()  -->
static void init_ardupilot()  -->
static void startup_ground(bool force_gyro_cal)
//******************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//******************************************************************************
static void startup_ground(bool force_gyro_cal)
{
    gcs_send_text_P(SEVERITY_LOW,PSTR("GROUND START"));

    // initialise ahrs (may push imu calibration into the mpu6000 if using that device).
    ahrs.init();
    ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);

    // Warm up and read Gyro offsets
    // -----------------------------
    ins.init(force_gyro_cal?AP_InertialSensor::COLD_START:AP_InertialSensor::WARM_START,
             ins_sample_rate);
 #if CLI_ENABLED == ENABLED
    report_ins();
 #endif

    // setup fast AHRS gains to get right attitude
    ahrs.set_fast_gains(true);

    // set landed flag
    set_land_complete(true);
}


结合注释可以知道这里是在起飞之前用来校准的。 ins_sample_rate就是我们的速率,它又是多少呢?
////////////////////////////////////////////////////////////////////////////////
// the rate we run the main loop at
////////////////////////////////////////////////////////////////////////////////

#if MAIN_LOOP_RATE == 400
static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_400HZ;
#else
static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_100HZ;
#endif


实际上我们这里只有两个速率, 400Hz跟 100Hz。而这个宏是在配置文件: "ardupilot/ArduCopter/config.h"中定义的。

#if HAL_CPU_CLASS < HAL_CPU_CLASS_75 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 // low power CPUs (APM1, APM2 and SITL)
 # define MAIN_LOOP_RATE    100
 # define MAIN_LOOP_SECONDS 0.01
 # define MAIN_LOOP_MICROS  10000
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 // Linux boards
 # define MAIN_LOOP_RATE    200
 # define MAIN_LOOP_SECONDS 0.005
 # define MAIN_LOOP_MICROS  5000
#else
 // high power CPUs (Flymaple, PX4, Pixhawk, VRBrain)
 # define MAIN_LOOP_RATE    400
 # define MAIN_LOOP_SECONDS 0.0025
 # define MAIN_LOOP_MICROS  2500
#endif


我们是 PX4,所以是 400Hz。以前我一直以为 PX4的周期是 10ms,现在我想才知道其实是 2.5ms。别人都说 PX4的效果要比 APM好,速率都提高了4倍效果还不好那才真是见了鬼了。
    而从宏定义可以看出,这里我们所说的速率指的是循环速率,即 loop这的循环以 400Hz的速率在运行。

    然而,通过前面的分析我们已经清楚,获取 mpu6050的数据显然应该通过 ins。

2. ins


    那么什么是 "ins"?根据我的理解, "ins"是 "Inertial Sensor",也就是指惯性传感器。
    其实从前面的分析我们也看到, ins中只有加计跟陀螺。所以我认为 "ins"应该是这吗理解的。
    但是我们现在关心的是 mpu6050中的数据是怎么读的。前面我们说过, loop可以分为四个部分,而现在已经只剩下两个部分了,即快速循环跟调度。那么你觉得应该在哪里去读取 mpu6050的数据呢?自然是快速循环,我就不多做解释了。

// 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(); //PID
    
#if FRAME_CONFIG == HELI_FRAME
    update_heli_control_dynamics();
#endif //HELI_FRAME

    // write out the servo PWM values
    // ------------------------------
    set_servos_4(); //电机输出

    // Inertial Nav
    // --------------------
    read_inertia();

    // run the attitude controllers
    //更新控制模式,并计算本级控制输出下级控制输入...
    update_flight_mode();

    // optical flow
    // --------------------
#if OPTFLOW == ENABLED
    if(g.optflow_enabled) {
        update_optical_flow();
    }
#endif  // OPTFLOW == ENABLED

}


以前我一直以为 PX4的周期是 10ms就是因为这里注释写的 100Hz。现在看来这应该是保留的 APM的注释了。
    看这段代码,我们很容易误以为是在 "read_inertia"函数中读取 mpu6050,但实际情况却并不是这样,因为真正的读取是在 "read_AHRS"函数中完成的, "read_inertia"函数只是把数据拿过来用而已。

static void read_AHRS(void)
{
    // Perform IMU calculations and get attitude info
    //-----------------------------------------------
#if HIL_MODE != HIL_MODE_DISABLED
    // update hil before ahrs update
    gcs_check_input();
#endif

    ahrs.update();
}
// Inertial Navigation EKF
#if AP_AHRS_NAVEKF_AVAILABLE
AP_AHRS_NavEKF ahrs(ins, barometer, gps);
#else
AP_AHRS_DCM ahrs(ins, barometer, gps);
#endif


根据测试, AP_AHRS_NAVEKF_AVAILABLE是有定义的,也就是说我们有用卡尔曼滤波。而 DCM实际上是指方向余弦矩阵,也就是指用方向余弦矩阵来计算的。
    但是我们看它的初始化会发现它好像少了一个东西,对的,就是地磁!为什么这里没有地磁呢?我以前所小四轴 AHRS中没有地磁是因为板子上根本就没地磁,如果说 PX4不带地磁你信吗?反正我是不信 。

class AP_AHRS_NavEKF : public AP_AHRS_DCM
{
public:
    // Constructor
    AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) :
    AP_AHRS_DCM(ins, baro, gps),
        EKF(this, baro),
        ekf_started(false),
        startup_delay_ms(10000)
        {
        }


所以我们看到,他们之间实际上是继承的关系。而我们的 update函数:

void AP_AHRS_NavEKF::update(void)
{//AP_AHRS_NavEKF使用四元数方式进行更新...
    // 父类使用方向余弦矩阵方式更新...
    AP_AHRS_DCM::update();

    // keep DCM attitude available for get_secondary_attitude()
    // 向量初始化...
    _dcm_attitude(roll, pitch, yaw);
    // 卡尔曼滤波未开启...
    if (!ekf_started) {


所以我们还是使用了方向雨轩矩阵,至于用了多少,这里就不分析了,因为这不是我们今天的重点。

// run a full DCM update round
void
AP_AHRS_DCM::update(void)
{
    float delta_t;

    // tell the IMU to grab some data
    // ...
    _ins.update();

    // ask the IMU how much time this sensor reading represents
    delta_t = _ins.get_delta_time();


所以我们看到,最终是通过 update函数读取 mpu6050的数据的。所以这里我们需要记住 ins的类型是 AP_InertialSensor_PX4,然后找到我们对应的代码。

bool AP_InertialSensor_PX4::update(void
{
    if (!wait_for_sample(100)) {
        return false;
    }

    // get the latest sample from the sensor drivers
    _get_sample();


    for (uint8_t k=0; k<_num_accel_instances; k++) {
        _previous_accel[k] = _accel[k];
        _accel[k] = _accel_in[k];
        // 这里不是真实的旋转,只是方向处理...
        _accel[k].rotate(_board_orientation);
        _accel[k].x *= _accel_scale[k].get().x;
        _accel[k].y *= _accel_scale[k].get().y;
        _accel[k].z *= _accel_scale[k].get().z;
        _accel[k]   -= _accel_offset[k];
    }

    for (uint8_t k=0; k<_num_gyro_instances; k++) {
        _gyro[k] = _gyro_in[k];
        _gyro[k].rotate(_board_orientation);
        _gyro[k] -= _gyro_offset[k];
    }

    if (_last_filter_hz != _mpu6000_filter) {
        _set_filter_frequency(_mpu6000_filter);
        _last_filter_hz = _mpu6000_filter;
    }

    _have_sample_available = false;

    return true;
}


wait函数我们前面已经看过了,就是等待我们设计好的一个时间,其实程序运行到这里基本上都不需要继续等待,因为在 loop中已经进行了等待。
    而这里调用的另外一个函数 _get_sample就是我们这里真正用来读取数据的。在看它的源码之前呢我们不妨先看看数据读取上来之后这里都做了些什么。
    首先,我们看到 rotate很容易以为这里在对数据进行旋转,但实际上这里只是对数据的方向进行处理,因为传感器的方向实际可能跟我们板子的方向不一致,这样我们就需要对方向做一个处理,这个工作就是由 rotate方法来完成的。方向处理完了之后加计跟陀螺都有减去一个偏移量。这个如果你自己写过四轴代码你就会很清楚,就是传感器都会存在误差,当数据应该为 0的时候实际输出并不等于 0,这个值就是偏移。但我们还看到加计跟陀螺的处理又有不同,那么 _accel_scale又是什么?用过 PX4的都知道, PX4校准加计的时候是通过各个方向的翻转来进行校准的,这样就可以计算出单位重力在各方向上的输出,而 PX4认为加计的每一个方向存在差异,即单位重力输出不同,这样 PX4就计算了一个比例系数来保证单位重力个方向的输出一致。最后 _set_filter_frequency是用来设置传感器过滤的:

/*
  set the filter frequency
 */
void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz)
{
    if (filter_hz == 0) {
        filter_hz = _default_filter_hz;
    }
    for (uint8_t i=0; i<_num_gyro_instances; i++) {
        ioctl(_gyro_fd[i],  GYROIOCSLOWPASS,  filter_hz);
    }
    for (uint8_t i=0; i<_num_accel_instances; i++) {
        ioctl(_accel_fd[i], ACCELIOCSLOWPASS, filter_hz);
    }
}


可以看到这里是通过 ioctl接口进行设置的,具体的细节就需要去查看驱动了。当然根据我目前对 PX4的了解,这里的驱动实际上就是 mpu6000这个应用程序。这一点自然跟 Linux做法不一样。

void AP_InertialSensor_PX4::_get_sample(void)
{
    for (uint8_t i=0; i<_num_accel_instances; i++) {
        struct accel_report    accel_report;
        while (_accel_fd[i] != -1 && 
               ::read(_accel_fd[i], &accel_report, sizeof(accel_report)) == sizeof(accel_report) &&
               accel_report.timestamp != _last_accel_timestamp[i]) {        
            _accel_in[i] = Vector3f(accel_report.x, accel_report.y, accel_report.z);
            _last_accel_timestamp[i] = accel_report.timestamp;
        }
    }
    for (uint8_t i=0; i<_num_gyro_instances; i++) {
        struct gyro_report    gyro_report;
        while (_gyro_fd[i] != -1 && 
               ::read(_gyro_fd[i], &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) &&
               gyro_report.timestamp != _last_gyro_timestamp[i]) {        
            _gyro_in[i] = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
            _last_gyro_timestamp[i] = gyro_report.timestamp;
        }
    }
    _last_get_sample_timestamp = hrt_absolute_time();
}


这一段代码就是真正去读取我们 mpu6050数据的代码。它通过 Nuttx中的系统调用 read读取数据,然后构造向量。
    那么就下来我们就需要到驱动里边去看相应的操作了。


翻译文章

世界上最快的四轴飞行器?

自从神秘的法国爱好者四轴视频(座右铭:就象小偷一样迅速奔跑)在四月二十五日发布了这段视频之后,一百万用户进行了观看。

该机采用了一部纳米威尔的多旋翼飞行控制器,配备两部x2208发动机,依靠一块十八安的凯施静电放电涂布系统提供驱动,桨叶则是一百五十二X一百二十五毫米的中心支柱,电力来自一块四艾斯一千八百毫安时最大连续放电量四十安的电池。

资料来源:优图博


自从神秘的法国爱好者四轴视频(座右铭:就象小偷一样迅速奔跑)在四月二十五日发布了这段视频之后,一百万用户进行了观看。

该机采用了一部纳米威尔的多旋翼飞行控制器,配备两部x2208发动机,依靠一块十八安的凯施静电放电涂布系统提供驱动,桨叶则是一百五十二X一百二十五毫米的中心支柱,电力来自一块四艾斯一千八百毫安时最大连续放电量四十安的电池。

资料来源:优图博
乐山

MWC

官网:http://www.multiwii.com/
官网:http://www.multiwii.com/

Jackeriss

这TED上的四轴飞行器真是难以置信的强大!

这TED上的四轴飞行器真是难以置信的强大!

猫家猪头的…

体验DJI。

Phantom 2 Vision.
第一次近距离接触,必须要佩服DJI做出了最不国货的产品和设计,质感真心不错。
PS:同事今天第一次试飞,就出了事故,撞击后损失一块电池……杯具

体验DJI。

Phantom 2 Vision.
第一次近距离接触,必须要佩服DJI做出了最不国货的产品和设计,质感真心不错。
PS:同事今天第一次试飞,就出了事故,撞击后损失一块电池……杯具

猫家猪头的…

Rolling Spider。

Parrot Minidrones。

一大早逛Apple Store App的时候中了毒,起床之后京东下了单,没想到下午就送到了手上,这效率……相比较于Parrot当家的AR Drone来说,这货确实就是一玩具,滞空时间短,成像素质低,不过还是很好玩哈,特别是轮胎模式……先熟悉下四轴,为早日加入DJI大军做准备吧

Rolling Spider。

Parrot Minidrones。

一大早逛Apple Store App的时候中了毒,起床之后京东下了单,没想到下午就送到了手上,这效率……相比较于Parrot当家的AR Drone来说,这货确实就是一玩具,滞空时间短,成像素质低,不过还是很好玩哈,特别是轮胎模式……先熟悉下四轴,为早日加入DJI大军做准备吧

相机Beta

著名摄影器材电商Adorama发布自己的四轴飞行器 Aries Blackbird X10 。

著名摄影器材电商Adorama发布自己的四轴飞行器 Aries Blackbird X10 。这款飞行器内置相机,可以拍摄每秒30帧的1080p视频和1600万像素的静态照片,具备故障保护模式,如果超过控制范围,飞行器可以通过GPS引导自动返回。每次充电可以飞行25分钟,最大遥控范围500米。目前售价为799美元,约4900人民币。


著名摄影器材电商Adorama发布自己的四轴飞行器 Aries Blackbird X10 。这款飞行器内置相机,可以拍摄每秒30帧的1080p视频和1600万像素的静态照片,具备故障保护模式,如果超过控制范围,飞行器可以通过GPS引导自动返回。每次充电可以飞行25分钟,最大遥控范围500米。目前售价为799美元,约4900人民币。






乐山

PX4 FMU启动流程 2. 三、nsh_session

PX4 FMU启动流程 2. 三、nsh_session ...


PX4 FMU启动流程 2. 三、nsh_session 

                                                                             -------- 转载请注明出处

                                                                             -------- 2014-11-27.冷月追风

                                                                             -------- email:merafour@163.com 


    有了前面的铺垫,这时候我们看 “nsh_session”函数就简单很多了。

int nsh_session(FAR struct console_stdio_s *pstate)
{
  int ret;

  DEBUGASSERT(pstate);

  /* Present a greeting */

  fputs(g_nshgreeting, pstate->cn_outstream);
  fflush(pstate->cn_outstream);

  /* Execute the login script */

#ifdef CONFIG_NSH_ROMFSRC
  (void)nsh_loginscript(&pstate->cn_vtbl);
#endif

  /* Then enter the command line parsing loop */

  for (;;)
    {
      /* For the case of debugging the USB console... dump collected USB trace data */

#ifdef CONFIG_NSH_USBDEV_TRACE
      nsh_usbtrace();
#endif

      /* Display the prompt string */

      fputs(g_nshprompt, pstate->cn_outstream);
      fflush(pstate->cn_outstream);

      /* Get the next line of input. readline() returns EOF on end-of-file
       * or any read failure.
       */

      ret = readline(pstate->cn_line, CONFIG_NSH_LINELEN,
                     INSTREAM(pstate), OUTSTREAM(pstate));
      if (ret != EOF)
        {
          /* Parse process the command */

          (void)nsh_parse(&pstate->cn_vtbl, pstate->cn_line);
          fflush(pstate->cn_outstream);
        }

      /* Readline normally returns the number of characters read,
       * but will return EOF on end of file or if an error occurs.
       * EOF will cause the session to terminate.
       */

      else
        {
          fprintf(pstate->cn_outstream, g_fmtcmdfailed, "nsh_session", 
                  "readline", NSH_ERRNO_OF(-ret));
          return ret == 0 ? EXIT_SUCCESS : EXIT_FAILURE;
        }
    }

  /* We do not get here, but this is necessary to keep some compilers happy.
   * But others will complain that this code is not reachable.
   */

  return EXIT_SUCCESS;
}


跟 “nsh_script”函数还是挺相似的。不同在于这里的命令行不是来自脚本文件,而事实来自 outstream,是什么呢?我们可以在 “nsh_newconsole”函数中找到。其实就是与用户交互的命令行终端。我们用上位机连上飞控的时候可以在一个窗口中用命令与飞控交互,这部分工作就是由这里完成的。
    应该来说,从中断读数据是不会读到文件结束标志的,所以该循环不会跳出。
 

LOFTER

让兴趣,更有趣

简单随性的记录
丰富多彩的内容
让生活更加充实

下载移动端
关注最新消息