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读取数据，然后构造向量。
    那么就下来我们就需要到驱动里边去看相应的操作了。