PX4代码学习系列博客(3)——px4固件目录结构和代码风格

写在前面

px4不是普通的单片机程序，其中没有main函数。它实际上是一个操作系统，上面运行着很多应用程序（类比windows），比如姿态解算，位置解算，姿态控制，位置控制等。每个应用之间通过uORB通信。所以要想读懂px4的程序，就不能按照单片机的思维，先找main函数，再看它调用了哪些函数，然后去别的文件看这些函数是怎么实现的。我的方法是：先明确每个目录下面有什么，然后有针对性地去看，比如要看旋翼的姿态控制，可以去目录Firmware\src\modules\mc_att_control下，要看NuttX的帮助文档，可以去目录Firmware\NuttX\nuttx\Documentation下，等等。

本文所用的固件是1.6.5。

目录结构：

px4的源码的根目录下内容如下：

由于我的代码用git和云端同步，用eclipse进行编译过，所以会比刚下载的固件多一些内容。

build打头的4个文件夹，是编译后的结果，刚下载的固件是没有的。其中Firmware\build_px4fmu-v2_default\src\firmware\nuttx\px4fmu-v2_default.px4是可以直接在地面站上给飞控下载的二进制文件。其他的编译选项目录结构和这个类似。

cmake文件夹下用到最多的是configs文件夹，都是.cmake文件，这些.cmake文件会调用其他文件夹下的.cmake文件，我们可以不关心其他文件夹。如果用pixhawk硬件，那么常用的是nuttx_px4fmu-v2_default.cmake。还有其他的一些配置，适用于不同的硬件和选择不同的模块（应用程序）进行编译。其中，fmu-v1和fmu-v2适用于Pixhawk，fmu-v3适用于Pixhawk 2，fmu-v4适用于Pixracer，stm32f4discovery用于stm32discovery开发板，rpi适用于树莓派。后缀_default代表默认，_lpe代码不用ekf2而用lpe进位置解算等等。

Documentation文件夹下是一些开发者文档，对理解代码结构和飞控的模式切换有帮助。
Images，integrationtests文件夹目前还不明确。

launch文件夹是仿真环境用到的文件，在gazebo中生成世界，配置ros节点等。比如gazebo_ardrone_empty_world_offboard_attitudedemo.launch这个文件，它需要Firmware\src\platforms\ros\nodes\demo_offboard_position_setpoints在ros中运行。这个demo的结果是gazebo中的飞机模型会上升的1m的高度然后悬停。我曾经在这个文件中改变代码实现在gazebo中飞特定轨迹。由于offboard模式比较危险，可以在这里进行算法的测试，稳定后再到实际飞行器上测试。

mavlink文件夹下是mavlink消息的定义，打包，发送，接收，解包的函数，全部由.h文件组成，相当于库函数。是飞行器与地面站，飞行器之间通信的协议，消息定义比较齐全，并且由于其开源特性可以自行扩展。

msg文件夹下是uORB通信用到的数据结构，它在编译的过程中会自行生成.h文件，比如position_setpoint.msg文件会在Firmware\build_px4fmu-v2_default\src\modules\uORB\topics下生成position_setpoint.h文件。

NuttX文件夹是NuttX系统需要的文件。它属于px4固件的子模块，在没有更新子模块之前，这个文件夹下面是没有任何文件的。

nuttx-打头的两个的文件夹，具体内容没有深究过，不过从字面意思上讲，一个是对于不同硬件版本的配置文件，一个是NuttX系统的补丁。

ROMFS文件夹下是启动过程中调用的文件，Firmware\ROMFS\px4fmu_common\ini.d\rcS是启动脚本，它在stm32底层初始化和NuttX系统初始化完成后执行。它根据参数设置的不同决定启用哪些模块（应用程序），如果是旋翼的话，它会调用Firmware\ROMFS\px4fmu_common\init.d\rc.mc_apps。

src文件夹是源代码，其中dirvers文件夹下是各种传感器的驱动文件，examples文件夹是一些测试用的模块（应用程序），include文件夹下是px比较基础的一些变量定义和文件包含，lib文件夹下是调用的库文件，包括向量，矩阵运算，GPS转XY距离坐标，滤波算法等，platforms文件夹下也是px4专属的一些变量和函数的定义，modules文件夹是学习px4飞控控制算法最关键的部分，因为它的下面是我本文一开始就说的在操作系统上运行的各种功能的应用程序，包括但不限于姿态解算，位置解算，姿态控制，位置控制。

代码阅读方法：

说到px4的代码，我们一般指的是src\modules文件夹下的代码。这个文件夹下的代码是分模块的，每个模块除功能外，代码的风格（函数和变量定义的格式，类的使用和封装）是一样的，所以只要看懂了一个的结构，其他的就很好理解了。我这里以最简单的模块attitude_estimator_q为例来说明。

打开Firmware\src\modules\attitude_estimator_q文件夹，下面有三个文件，其中CMakeLists.txt是编译时用到的文件，attitude_estimator_q_params.c是程序默认参数的设置，都不需要更改。我们只需要关注attitude_estimator_q_main.cpp。
下面这个文件是我把类的实现中除task_main之外的全部去掉，然后在类中定义变量和函数地方加注释形成的。

/****************************************************************************
 *
 *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

 /*
 * Modified from file attitude_estimator_q_main.cpp
 * 2017/08/19
 * author:qianrenzhan([email protected])
 * 为了展示px4代码结构，并不能通过编译
 */

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 

#include 
#include 
#include 
#include 


extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]);   
//这是main函数的申明，nuttx的main函数和普通单片机不一样，它的命名格式是“应用程序名称”+“_”+“main”。
//在NUTTX系统上注册的函数，在执行的时候回启动或者关闭这个模块（应用程序），还能返回模块的工作状态。


using math::Vector;
using math::Matrix;
using math::Quaternion;
//用到了math命名空间下的Vector，Matrix，Quaternion类，因为姿态估计涉及到向量，矩阵的运算，以及四元数。

class AttitudeEstimatorQ;
//这是类的前向申明，可以在类没有实现的时候定义类的指针对象。

namespace attitude_estimator_q
{
AttitudeEstimatorQ *instance;
}
//定义类的指针对象instance，它属于命名空间attitude_estimator_q，是全局变量。
//这里的命名空间很关键，因为别的模块也会定义这样的全局变量，但是不同模块可能不是一个人写的，
//变量有冲突的情况下用前缀attitude_estimator_q::就可以解决。


class AttitudeEstimatorQ
{
public:
    AttitudeEstimatorQ();
    //构造函数中得到该模块需要用到的参数的句柄，给变量赋初值。
    ~AttitudeEstimatorQ();

    int     start();                
    //函数功能：为attitude_estimator任务设置优先级，分配堆栈空间，指定服务函数。

    static void task_main_trampoline(int argc, char *argv[]);
    //attitude_estimator任务服务函数，它只调用task_main函数。

    void        task_main();
    //被attitude_estimator任务服务函数调用

private:
    static constexpr float _dt_max = 0.02;
    bool    _task_should_exit = false;      /**< if true, task should exit */
    int     _control_task = -1;             /**< task handle for task */

    //将要订阅的uORB消息
    int     _sensors_sub = -1;
    int     _params_sub = -1;
    int     _vision_sub = -1;
    int     _mocap_sub = -1;
    int     _airspeed_sub = -1;
    int     _global_pos_sub = -1;

    //将要发布的uORB消息
    orb_advert_t    _att_pub = nullptr;
    orb_advert_t    _ctrl_state_pub = nullptr;
    orb_advert_t    _est_state_pub = nullptr;

    struct {
        param_t w_acc;
        param_t w_mag;
        param_t w_ext_hdg;
        param_t w_gyro_bias;
        param_t mag_decl;
        param_t mag_decl_auto;
        param_t acc_comp;
        param_t bias_max;
        param_t ext_hdg_mode;
        param_t airspeed_mode;
    }       _params_handles;        /**< handles for interesting parameters */

    //可以配置的参数
    float       _w_accel = 0.0f;
    float       _w_mag = 0.0f;
    float       _w_ext_hdg = 0.0f;
    float       _w_gyro_bias = 0.0f;
    float       _mag_decl = 0.0f;
    bool        _mag_decl_auto = false;
    bool        _acc_comp = false;
    float       _bias_max = 0.0f;
    int     _ext_hdg_mode = 0;
    int     _airspeed_mode = 0;

    //从uORB获取到的传感器原始值
    Vector<3>   _gyro;
    Vector<3>   _accel;
    Vector<3>   _mag;
    vehicle_attitude_s _vision = {};
    Vector<3>   _vision_hdg;
    att_pos_mocap_s _mocap = {};
    Vector<3>   _mocap_hdg;
    airspeed_s _airspeed = {};

    //解算结果
    Quaternion  _q;
    Vector<3>   _rates;
    Vector<3>   _gyro_bias;
    vehicle_global_position_s _gpos = {};
    Vector<3>   _vel_prev;
    Vector<3>   _pos_acc;

    //低通滤波器
    math::LowPassFilter2p _lp_accel_x;
    math::LowPassFilter2p _lp_accel_y;
    math::LowPassFilter2p _lp_accel_z;
    math::LowPassFilter2p _lp_gyro_x;
    math::LowPassFilter2p _lp_gyro_y;
    math::LowPassFilter2p _lp_gyro_z;

    hrt_abstime _vel_prev_t = 0;

    bool        _inited = false;
    bool        _data_good = false;
    bool        _ext_hdg_good = false;

    orb_advert_t    _mavlink_log_pub = nullptr;

    void update_parameters(bool force);
    //是否进行参数更新

    bool init();
    //变量初始化

    bool update(float dt);
    //关键的解算算法在此处，它在task_main中的while循环中被调用

    // Update magnetic declination (in rads) immediately changing yaw rotation
    void update_mag_declination(float new_declination);
};

void AttitudeEstimatorQ::task_main()
{

#ifdef __PX4_POSIX
    perf_counter_t _perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay"));
    perf_counter_t _perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay"));
    perf_counter_t _perf_mag(perf_alloc_once(PC_ELAPSED, "sim_mag_delay"));
#endif

    //订阅需要的消息
    _sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
    _vision_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));
    _mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
    _airspeed_sub = orb_subscribe(ORB_ID(airspeed));
    _params_sub = orb_subscribe(ORB_ID(parameter_update));
    _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));

    //更新参数
    update_parameters(true);

    hrt_abstime last_time = 0;


    px4_pollfd_struct_t fds[1] = {};
    fds[0].fd = _sensors_sub;
    fds[0].events = POLLIN;

    while (!_task_should_exit) {
        int ret = px4_poll(fds, 1, 1000);
        if (ret < 0) {
            // Poll error, sleep and try again
            usleep(10000);
            PX4_WARN("Q POLL ERROR");
            continue;
            //等待有sensor_combined消息传来，否则继续等待。
        } else if (ret == 0) {
            // Poll timeout, do nothing
            PX4_WARN("Q POLL TIMEOUT");
            continue;
            //等待有sensor_combined消息传来，否则继续等待。
        }

        update_parameters(false);

        // Update sensors
        sensor_combined_s sensors;

        if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
            // 获取订阅到的主题

            if (sensors.timestamp > 0) {
                //原始数据经过低通滤波器
                _gyro(0) = _lp_gyro_x.apply(sensors.gyro_rad[0]);
                _gyro(1) = _lp_gyro_y.apply(sensors.gyro_rad[1]);
                _gyro(2) = _lp_gyro_z.apply(sensors.gyro_rad[2]);
            }
            if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
                //原始数据经过低通滤波器
                _accel(0) = _lp_accel_x.apply(sensors.accelerometer_m_s2[0]);
                _accel(1) = _lp_accel_y.apply(sensors.accelerometer_m_s2[1]);
                _accel(2) = _lp_accel_z.apply(sensors.accelerometer_m_s2[2]);
                if (_accel.length() < 0.01f) {
                    PX4_DEBUG("WARNING: degenerate accel!");
                    continue;
                }
            }
            if (sensors.magnetometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
                _mag(0) = sensors.magnetometer_ga[0];
                _mag(1) = sensors.magnetometer_ga[1];
                _mag(2) = sensors.magnetometer_ga[2];
                if (_mag.length() < 0.01f) {
                    PX4_DEBUG("WARNING: degenerate mag!");
                    continue;
                }
            }
            _data_good = true;
        }

        //更新视觉和动作捕捉的消息
        bool vision_updated = false;

        //检查是否有新一帧的消息被发布
        orb_check(_vision_sub, &vision_updated);

        bool mocap_updated = false;
        orb_check(_mocap_sub, &mocap_updated);

        if (vision_updated) {
            orb_copy(ORB_ID(vehicle_vision_attitude), _vision_sub, &_vision);
            math::Quaternion q(_vision.q);

            math::Matrix<3, 3> Rvis = q.to_dcm();
            math::Vector<3> v(1.0f, 0.0f, 0.4f);

            // Rvis is Rwr (robot respect to world) while v is respect to world.
            // Hence Rvis must be transposed having (Rwr)' * Vw
            // Rrw * Vw = vn. This way we have consistency
            _vision_hdg = Rvis.transposed() * v;
        }

        if (mocap_updated) {
            orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
            math::Quaternion q(_mocap.q);
            math::Matrix<3, 3> Rmoc = q.to_dcm();

            math::Vector<3> v(1.0f, 0.0f, 0.4f);

            // Rmoc is Rwr (robot respect to world) while v is respect to world.
            // Hence Rmoc must be transposed having (Rwr)' * Vw
            // Rrw * Vw = vn. This way we have consistency
            _mocap_hdg = Rmoc.transposed() * v;
        }

        //省略airspeed和gpos部分
        /*
        bool airspeed_updated = false;
        bool gpos_updated;
        */

        //计算时间间隔，hrt_absolute_time这个函数即使任务在执行过程中被中断打断，也能返回正确的时间。
        hrt_abstime now = hrt_absolute_time();
        float dt = (last_time > 0) ? ((now  - last_time) / 1000000.0f) : 0.00001f;
        last_time = now;

        if (dt > _dt_max) {
            dt = _dt_max;
        }

        if (!update(dt)) {
        //关键的解算算法在这个函数中
            continue;
        }

        //打包将要发布的消息vehicle_attitude
        {
            vehicle_attitude_s att = {
                .timestamp = sensors.timestamp,
                .rollspeed = _rates(0),
                .pitchspeed = _rates(1),
                .yawspeed = _rates(2),
                .q = {_q(0), _q(1), _q(2), _q(3)}
            };

            /* the instance count is not used here */
            int att_inst;
            orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
        }
        //打包将要发布的消息ctrl_state
        {
            //此处省略
        }
    }

    //任务结束后取消订阅这些消息
    orb_unsubscribe(_sensors_sub);
    orb_unsubscribe(_vision_sub);
    orb_unsubscribe(_mocap_sub);
    orb_unsubscribe(_airspeed_sub);
    orb_unsubscribe(_params_sub);
    orb_unsubscribe(_global_pos_sub);
}