PX4源码分析3：attitude_estimator_q（转载）

原文链接：https://blog.csdn.net/zouxu634866/article/details/79806948

#include <drivers/drv_hrt.h>  //高精度的定时器。 在控制过程中多数环节都是使用经典的PID控制算法为
//了获取较为实时的响应最重要的时间变量，这就涉及如何获取高精度的时间问题。pixhawk中就有高精度的RTC（实时时钟），这个
//头文件就做了介绍

#include <lib/geo/geo.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>  /**滤波器**/
#include <mathlib/mathlib.h>
#include <px4_config.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <systemlib/err.h>         /**包含一些错误警告的功能**/
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>      /**性能评估**/
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>/**我们在这个程序中解算的姿态就要发布进这个**/
#include <uORB/topics/vehicle_global_position.h>   /**去看看有什么，后面讲位置估计时要用到**/

extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]);

using math::Vector;         /*****使用向量****/
using math::Matrix;         /*****使用矩阵****/
using math::Quaternion;     /*****使用四元数****/

class AttitudeEstimatorQ;

namespace attitude_estimator_q
{
AttitudeEstimatorQ *instance;
} // namespace attitude_estimator_q attitude_estimator_q

class AttitudeEstimatorQ
{
public:
    /**
     * Constructor
     */
    AttitudeEstimatorQ();

    /**
     * Destructor, also kills task.
     */
    ~AttitudeEstimatorQ();

    /**
     * Start task.
     *
     * @return      OK on success.
     */
    int  start();                           //创建一个新的任务 

    static void task_main_trampoline(int argc, char *argv[]);

    void        task_main();

private:
    const float _dt_min = 0.00001f;         /***dt是指更新数据的时间间隔，也就是离散pid公式中的T***/
    const float _dt_max = 0.02f;

    bool        _task_should_exit = false;  /**< if true, task should exit */
    int     _control_task = -1;             /**< task handle for task */

    int     _params_sub = -1;               /****与parameter_update有关****/
    int     _sensors_sub = -1;              /****这个变量是订阅传感器数据时用的句柄****/
    int     _global_pos_sub = -1;           /****这个变量是订阅地理位置数据时用的句柄****/
    int     _vision_sub = -1;               /****这个变量是视觉信息时用的句柄****/
    int     _mocap_sub = -1;                /****mocap=motion capture，动作捕捉****/

    orb_advert_t    _att_pub = nullptr;     /***nullptr是c++11标准引入的更严谨的空指针，这里的att_pub是用于发布姿态信息的handle***/

    struct {

        /**前面带w都是权重的意思**/
        param_t w_acc;              /**这里的acc为加速度计的权重，用于后面互补滤波**/
        param_t w_mag;              /**mag为磁力计**/
        param_t w_ext_hdg;
        param_t w_gyro_bias;        /***陀螺仪偏差***/
        param_t mag_decl;           /**磁方位角**/
        param_t mag_decl_auto;      /**利用gps自动获得磁方位角**/
        param_t acc_comp;           /**加速度的补偿**/
        param_t bias_max;          /**最大偏差**/
        param_t ext_hdg_mode;  /***外部的航向使用模式，=1表示来自于视觉，=2表示来自mocap***/
    } _params_handles{};        /**< handles for interesting parameters   这个结构体里面全是一些系统参数**/
                                /**在px4的程序中获取系统参数的方法是用param_get和param_find去获取，而不是通过uorb**/



     /**下面这些初始化的数据是用来存放从系统参数get过来的数据***/
    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;
    int32_t     _ext_hdg_mode = 0;

    Vector<3>   _gyro;     /**通过传感器获取的三轴的角速度**/
    Vector<3>    _accel;  /***通过加速度计获取的三轴的加速度***/
    Vector<3>   _mag;    /***通过磁力计获取的磁航向***/

    Vector<3>   _vision_hdg; /**通过视觉获取的handing**/
    Vector<3>   _mocap_hdg;  /**通过mocap获取的handing**/

    Quaternion  _q;
    Vector<3>   _rates;     /**与上面_gyro不同的是这个用于存放修正后的绕三轴角速度**/
    Vector<3>   _gyro_bias;

    Vector<3>   _vel_prev;  /**前一刻的速度**/
    hrt_abstime _vel_prev_t = 0;   /**uint64_t的typedef，abstime意思为绝对时间，这里为记录前一时刻速度时的绝对时间**/
                                   /**用于后面根据速度差除以时间差求加速度**/

    Vector<3>   _pos_acc;  //运动加速度，注意：这个加速度不包括垂直方向的

    bool        _inited = false; //初始化标识
    bool        _data_good = false;//数据可用
    bool        _ext_hdg_good = false;//外部航向可用

    void update_parameters(bool force);

    int update_subscriptions();  /**只有声明，无定义？？？字面的意思是更新订阅信息？？**/

    bool init();//初始化函数，初始化旋转矩阵和四元数

    bool update(float dt);//dt是更新周期，这个函数是整个程序的核心

    //Update magnetic declination (in rads) immediately changing yaw rotation
    //偏航角改变立刻更新磁方位角
    void update_mag_declination(float new_declination);
};


AttitudeEstimatorQ::AttitudeEstimatorQ()
{
    //先用find匹配系统参数，然后用get拷贝系统的参数，之所以这样不会直接影响到系统参数，但是又可以正常使用系统参数。

    _params_handles.w_acc       = param_find("ATT_W_ACC");
    _params_handles.w_mag       = param_find("ATT_W_MAG");
    _params_handles.w_ext_hdg   = param_find("ATT_W_EXT_HDG");
    _params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");
    _params_handles.mag_decl    = param_find("ATT_MAG_DECL");
    _params_handles.mag_decl_auto   = param_find("ATT_MAG_DECL_A");
    _params_handles.acc_comp    = param_find("ATT_ACC_COMP");
    _params_handles.bias_max    = param_find("ATT_BIAS_MAX");
    _params_handles.ext_hdg_mode    = param_find("ATT_EXT_HDG_M");

    _vel_prev.zero();/**清零**/
    _pos_acc.zero();

    _gyro.zero();
    _accel.zero();
    _mag.zero();

    _vision_hdg.zero();/**通过视觉得到的航向清零**/
    _mocap_hdg.zero();/**通过mocap得到的航向清零**/

    _q.zero();
    _rates.zero();
    _gyro_bias.zero();

    _vel_prev.zero();
    _pos_acc.zero();
}

/**
 * Destructor, also kills task.
 * 析构函数，也用于结束任务
 */
AttitudeEstimatorQ::~AttitudeEstimatorQ()
{
    if (_control_task != -1) {
        /* task wakes up every 100ms or so at the longest */
        _task_should_exit = true;

        /* wait for a second for the task to quit at our request */
        unsigned i = 0;

        do {
            /* wait 20ms */
            usleep(20000);

            /* if we have given up, kill it */
            if (++i > 50) {
                px4_task_delete(_control_task);
                break;
            }
        } while (_control_task != -1);
    }

    attitude_estimator_q::instance = nullptr;
}

int AttitudeEstimatorQ::start()
{
    ASSERT(_control_task == -1);    //AEESRT是assert函数的define了，作用是参数值为0时终止程序

    /* start the task */
    /*这里的px4_task_spawn_cmd函数的作用是创建一个新的任务，属于nuttx系统中的**/
    _control_task = px4_task_spawn_cmd("attitude_estimator_q",
                       SCHED_DEFAULT,
                       SCHED_PRIORITY_ESTIMATOR,
                       2000,
                       (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
                       nullptr);

    if (_control_task < 0)
    {
        warn("task start failed");
        return -errno;
    }

    return OK;
}

void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])  /**运行task_main**/
{
    attitude_estimator_q::instance->task_main();
}




void AttitudeEstimatorQ::task_main()
{

#ifdef __PX4_POSIX
    /**加速度计的表现性能。。第一个参数是counter的类型，第二个是counter的名字**/
    perf_counter_t _perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay"));
    /**mpu的表现性能**/
    perf_counter_t _perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay"));
    /**mag的表现性能**/
    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_supdbscribe(ORB_ID(att_pos_mocap));/**订阅mocap信息**/
    _params_sub = orb_subscribe(ORB_ID(parameter_update));/**订阅parameter信息，用于之后的parameter_update的copy**/
    _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));/**订阅位置信息**/


    /**上面是订阅，copy部分在下面这个函数中**/
    update_parameters(true);

    hrt_abstime last_time = 0;



    /************poll**************/
    /******nuttx任务使能********/
    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("POLL ERROR");
            continue;

        } else if (ret == 0) {
            // Poll timeout, do nothing
            PX4_WARN("POLL TIMEOUT");
            continue;
        }

        update_parameters(false); //498行

        // Update sensors
        sensor_combined_s sensors;   /**新建的sensor结构体用于装复制的传感器信息**/

        if (orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors) == PX4_OK) {
            // Feed validator with recent sensor data

            if (sensors.timestamp > 0) {
                _gyro(0) = sensors.gyro_rad[0];
                _gyro(1) = sensors.gyro_rad[1];
                _gyro(2) = sensors.gyro_rad[2];
            }

            if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
                _accel(0) = sensors.accelerometer_m_s2[0];
                _accel(1) = sensors.accelerometer_m_s2[1];
                _accel(2) = sensors.accelerometer_m_s2[2];

                if (_accel.length() < 0.01f) {
                    PX4_ERR("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_ERR("WARNING: degenerate mag!");
                    continue;
                }
            }

            _data_good = true;
        }

        // Update vision and motion capture heading
        // 通过uORB模型获取vision和mocap的数据（视觉和mocap）
        bool vision_updated = false;
        orb_check(_vision_sub, &vision_updated);

        if (vision_updated) {
            vehicle_attitude_s vision;  /**新建vision结构体**/

            if (orb_copy(ORB_ID(vehicle_vision_attitude), _vision_sub, &vision) == PX4_OK) {
                math::Quaternion q(vision.q);//将基于视觉获得的四元数赋给新建的math：：q

                math::Matrix<3, 3> Rvis = q.to_dcm();  /**基于视觉得到的转换矩阵**/
                math::Vector<3> v(1.0f, 0.0f, 0.4f);    /**从他给定的v可知这里的v是一个指北的向量**/
                                                       /**至于为什么初始给的是0.4,还有疑惑**/




                // Rvis is Rwr (robot respect to world) while v is respect to world.
                // Hence Rvis must be transposed having (Rwr)' * Vw  v是在地系下的，而dcm是b—>e,所以要转置
                // Rrw * Vw = vn. This way we have consistency
                _vision_hdg = Rvis.transposed() * v;/**转置后与v乘，就反映了视觉上的指北的向量**/

                // vision external heading usage (ATT_EXT_HDG_M 1)
                if (_ext_hdg_mode == 1) {
                    // Check for timeouts on data 检查数据超时
                    _ext_hdg_good = vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000);
                }
            }
        }


        /**基于mocap获取handing同理**/
        bool mocap_updated = false;
        orb_check(_mocap_sub, &mocap_updated);

        if (mocap_updated) {
            att_pos_mocap_s mocap;

            if (orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &mocap) == PX4_OK) {
                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;

                // Motion Capture external heading usage (ATT_EXT_HDG_M 2)
                if (_ext_hdg_mode == 2) {
                    // Check for timeouts on data
                    _ext_hdg_good = mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000);
                }
            }
        }


        //下面的是自动获取磁偏角
        bool gpos_updated = false;
        orb_check(_global_pos_sub, &gpos_updated);

        if (gpos_updated) {//如果位置已经更新 就取出位置数据
            vehicle_global_position_s gpos;

            if (orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &gpos) == PX4_OK)
            {
                //首先检测是否配置了自动磁方位角获取，即用下面的if
                if (_mag_decl_auto && gpos.eph < 20.0f && hrt_elapsed_time(&gpos.timestamp) < 1000000) {
                    /* set magnetic declination automatically */
                    //自动获取磁方位角，如果配置了则用当前的经纬度（longitude and latitude）通过get_mag_declination(_gpos.lat,_gpos.lon)函数获取当前位置的磁偏角
                    update_mag_declination(math::radians(get_mag_declination(gpos.lat, gpos.lon)));
                }

                 //获取机体的速度，通过速度计算机体的加速度 。
                //先判断位置信息是否有效
                if (_acc_comp && gpos.timestamp != 0 && hrt_absolute_time() < gpos.timestamp + 20000 && gpos.eph < 5.0f && _inited)
                {
                    /* position data is actual */
                    Vector<3> vel(gpos.vel_n, gpos.vel_e, gpos.vel_d); //北东地系

                    /* velocity updated */
                    if (_vel_prev_t != 0 && gpos.timestamp != _vel_prev_t) {
                        float vel_dt = (gpos.timestamp - _vel_prev_t) / 1e6f;  //us
                        /* calculate acceleration in body frame */
                        //计算e系下的运动加速度，这里的pos_acc很重要，要用于后面加速度计的校正
                        _pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
                    }

                    _vel_prev_t = gpos.timestamp;  //为迭代做准备
                    _vel_prev = vel;

                }
                else {/**否则位置信息过时，将加速度信息清零**/
                    /* position data is outdated, reset acceleration */
                    _pos_acc.zero();
                    _vel_prev.zero();
                    _vel_prev_t = 0;
                }
            }
        }

        /* time from previous iteration */
        hrt_abstime now = hrt_absolute_time();

        //下面的constrain函数用于限定作用，限定规则：return (val < min_val) ? min_val : ((val > max_val) ? max_val : val);
        const float dt = math::constrain((now  - last_time) / 1e6f, _dt_min, _dt_max);
        last_time = now;

        if (update(dt)) {
            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)},
                .delta_q_reset = {},
                .quat_reset_counter = 0,
            };

            /* the instance count is not used here */
            //最后发布给了vehicle_attitude,这也对应了最开始我们说的我们不能直接把陀螺仪的数据当成姿态信息，而应该
            //经过我们处理后才能使用。即vehicle_attitude的信息流为：订阅的是sensor combined，发布的是vehicle attitude
            int att_inst;
            orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
        }
    }

#ifdef __PX4_POSIX
    perf_end(_perf_accel);
    perf_end(_perf_mpu);
    perf_end(_perf_mag);
#endif

    orb_unsubscribe(_params_sub);
    orb_unsubscribe(_sensors_sub);
    orb_unsubscribe(_global_pos_sub);
    orb_unsubscribe(_vision_sub);
    orb_unsubscribe(_mocap_sub);
}

void AttitudeEstimatorQ::update_parameters(bool force)
{
    bool updated = force;

    if (!updated) //如果更新了
    {
        orb_check(_params_sub, &updated); /***检查一个主题在发布者最后更新后，有没parameter_update有人调用过orb_copy来接收如果主题在被公告前就有人订阅，那么这个API将返回“not-updated”直到主题被公告。***/
    }

    if (updated)  //如果没更新
    {
        parameter_update_s param_update;  /**建立建构体param_update，里面有更新的时间、是否更新、填充信息**/
        orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);/**根据头文件可知parameter_update是自带的主题*/
        /***疑问：上面的copy之前为什么没有订阅，是不是后面参数更新后还要发布出去？其实不是，订阅发生在前面的task_main函数中***/
        param_get(_params_handles.w_acc, &_w_accel);/**把获得的参数值赋值给后面那个参数，前面那些参数的值都在构造函数中定义了**/
        param_get(_params_handles.w_mag, &_w_mag);
        param_get(_params_handles.w_ext_hdg, &_w_ext_hdg);
        param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);

        float mag_decl_deg = 0.0f;
        param_get(_params_handles.mag_decl, &mag_decl_deg);
        update_mag_declination(math::radians(mag_decl_deg));/**radians函数为取弧度，外面的函数为消除偏差的积累***/

        int32_t mag_decl_auto_int;
        param_get(_params_handles.mag_decl_auto, &mag_decl_auto_int);
        _mag_decl_auto = (mag_decl_auto_int != 0);

        int32_t acc_comp_int;
        param_get(_params_handles.acc_comp, &acc_comp_int);
        _acc_comp = (acc_comp_int != 0);

        param_get(_params_handles.bias_max, &_bias_max);
        param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
    }
}



/***********init（）函数作用：由加速度计和磁力计初始化旋转矩阵和四元数**************/

bool AttitudeEstimatorQ::init()
{
    // Rotation matrix can be easily constructed from acceleration and mag field vectors
    // 'k' is Earth Z axis (Down) unit vector in body frame
    //，所以取反向
    Vector<3> k = -_accel;            //accel已经在task_main中订阅
                                      //k为加速度传感器测量到加速度方向向量，由于第一次测量数据时无人机一般为平稳状态无运动状态，
                                      //所以可以直接将测到的加速度视为重力加速度g，以此作为dcm旋转矩阵的第三行k

    k.normalize();                    //向量归一化，也就是向量除以他的长度



    // 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
    // i是体坐标系下指向正北的单位向量，与k正交
    Vector<3> i = (_mag - k * (_mag * k));//施密特正交化,强制使i与k垂直;因向量k已归一化，故分母等于1;
                                          //这里的_mag是指北的，所以下面求得的R是默认飞机机头也是指向北
    i.normalize();                    //向量归一化，也就是向量除以他的长度


    // 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
    //j是e坐标系下指向正东的单位向量在b系下的向量，与k、i正交
    Vector<3> j = k % i;

    // 填充旋转矩阵
    Matrix<3, 3> R;    //新建一个3*3的矩阵R

    R.set_row(0, i);   //set_row函数的第一个参数为第几行，将i向量填入矩阵第一行
    R.set_row(1, j);   //将j向量填入矩阵第二行
    R.set_row(2, k);   //将k向量填入矩阵第三行，注意：从这里可知该旋转矩阵为b系到n系

    // 将R矩阵转换为四元数
    _q.from_dcm(R);

    // Compensate for magnetic declination  补偿飞机的磁方位角，因为前面求得q是默认飞机指向北，但起飞时飞机不一定指向北
    Quaternion decl_rotation;
    decl_rotation.from_yaw(_mag_decl);/**将旋转矩阵仅仅通过yaw偏航的四元数表示**/
    _q = decl_rotation * _q;

    _q.normalize();  /**归一化**/  /**此时的q才是真正的初始的q**/

    if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&      /**判断是否发散**/
        PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
        _q.length() > 0.95f && _q.length() < 1.05f)
    {
        _inited = true;      /**初始化完成**/
    }
    else
    {
        _inited = false;     /**初始化失败**/
    }


    return _inited;
}



bool AttitudeEstimatorQ::update(float dt)
{
    if (!_inited)
    {

        if (!_data_good)   /**在前面task_main函数中如果传感器数据订阅正常，data_good就为true**/
        {
            return false;
        }

        return init();  /**没有初始化（前提：传感器数据订阅正常）就初始化**/
                        //并且注意：init在一次飞行trampoline中只执行一次，因为以后的四元数和转换矩阵由输出的角速度经过龙格库塔法求，而飞机在初始飞行时就需要通过init求转换矩阵
    }




    /**前面的init等都完成就继续往下执行**/
    Quaternion q_last = _q;  /**注意：此时四元数_q已经有内容了，此处的q_last的作用为给q弄一个备份，可从最后一个if看出**/

    // Angular rate of correction  修正角速率,下面的是重点
    Vector<3> corr;
    float spinRate = _gyro.length();  /**定义一个变量：旋转速率，length函数为平方和开方**/


    //首先判断是使用什么mode做修正的，比如vision、mocap、acc和mag，这里我们着重分析用acc和mag做修正，另外两个同理
    if (_ext_hdg_mode > 0 && _ext_hdg_good)
    {
        if (_ext_hdg_mode == 1)
        {
            // Vision heading correction  对基于视觉的航向模式进行修正
            // Project heading to global frame and extract XY component
            Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg);/**将b系转为e系**/
            float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
            // Project correction to body frame
            corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg;
        }

        if (_ext_hdg_mode == 2) {
            // Mocap heading correction
            // Project heading to global frame and extract XY component
            Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg);
            float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
            // Project correction to body frame
            corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg;
        }
    }

    if (_ext_hdg_mode == 0 || !_ext_hdg_good) {
        // Magnetometer correction
        // Project mag field vector to global frame and extract XY component
        Vector<3> mag_earth = _q.conjugate(_mag);    /**将b系转为e系**/

        //只考虑Vector<3> mag_earth中的前两维的数据mag_earth(1)和mag_earth(0)（即x、y，忽略z轴上的偏移），通
       //过arctan（mag_earth(1),mag_earth(0)）得到的角度和前面根据经纬度获取的磁方位角做差值得到误差角度mag_err
      //_wrap_pi函数是用于限定结果-pi到pi的函数，大于pi则与2pi做差取补，小于-pi则与2pi做和取补
      //注意1：这里在修正磁力计时用到的是有gps校准时的修正办法，即下面的减去自动获取的磁偏角。没有gps的校准方法见ppt
     //注意2：这里校正的方法是用磁力计计算的磁偏角与gps得出来的做差，然后转换到b系。
        float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);//与自动获取的磁方位角做差
        float gainMult = 1.0f;
        const float fifty_dps = 0.873f;

        if (spinRate > fifty_dps) {
            gainMult = math::min(spinRate / fifty_dps, 10.0f);
        }

        // Project magnetometer correction to body frame
        //下面*Vector<3>(0.0f, 0.0f, -mag_err))是因为raw本质上应该由磁力计产生的水平磁向量与gps产生的水平磁向量叉乘得到，而叉乘得到的正好向下
        corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag * gainMult;
    }

    _q.normalize();


    // Accelerometer correction
    // Project 'k' unit vector of earth frame to body frame
    // Vector<3> k = _q.conjuq.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
    // Optimized version with dropped zeros
    //下面的k是n系中重力的单位向量转换到b系中，即左乘旋转矩阵得到的
    Vector<3> k(
        2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
        2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
        (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
    );

    //总的受到合力的方向（_accel）减去机体加速度方向（_pos_acc）得到g的方向，即总加速度（加速度获取）减去机体运动加速度获取重力加速度
    //重力加速度的方向是导航坐标系下的z轴，加上运动加速度之后，总加速度的方向就不是与导航坐标系的天或地平行了，所以要消除这个误差，即“_accel-_pos_acc”
    //这里与k差乘得到的是与重力方向的夹角sinx，约等于x，即roll和pitch
    corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;

    // Gyro bias estimation
    if (spinRate < 0.175f) {
        _gyro_bias += corr * (_w_gyro_bias * dt);//对应积分控制

        //对_gyro_bias做约束处理。
        for (int i = 0; i < 3; i++) {
            _gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
        }

    }

    _rates = _gyro + _gyro_bias;//角速度 = 陀螺仪的测量值 + 误差校准

    // pi控制器的体现，对应比例部分
    corr += _rates;//corr为update函数中定义的变量，所以每次进入update函数中时会刷新corr变量的数据

    // Apply correction to state
    //最后就是使用修正的数据更新四元数，并把_rates和_gyro_bias置零便于下次调用时使用。
    _q += _q.derivative(corr) * dt;//用龙格库塔法更新四元数

    // Normalize quaternion
    _q.normalize();

    if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
          PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
        // 如果数据不合适就恢复备份的q，即q_last
        _q = q_last;
        _rates.zero();
        _gyro_bias.zero();
        return false;
    }

    return true;
}  //更新完四元数后，我们跳到update函数被调用的地方，即444行，发现后面将更新后的姿态信息发布出去了，到此整个过程结束


/**偏航角改变立刻更新磁方位角**/
void AttitudeEstimatorQ::update_mag_declination(float new_declination)
{
    // Apply initial declination or trivial rotations without changing estimation
    // 忽略微小旋转（比如机体的微小震动），继续沿用之前的磁方位角
    if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f)
    {
        _mag_decl = new_declination;
    }

    else  //转动较大时，修正姿态（q）和更新磁方位角
    {
        // Immediately rotate current estimation to avoid gyro bias growth
        Quaternion decl_rotation;
        decl_rotation.from_yaw(new_declination - _mag_decl);//偏航角生成的四元数，生成方法是令另外2个角度为0,使用欧拉角转四元数公式
        _q = decl_rotation * _q;//此处两个四元数相乘表示角度相加，因为在数学上q1*q2表示q1q2这个合成动作，所以此处修正了四元数
        _mag_decl = new_declination;//使用新的磁偏角
    }
}



//下面的总结起来：attitude_estimator_q { start }：实例化instance，运行instance->start()；

//attitude_estimator_q { stop }：delete instance，指针置空；

//attitude_estimator_q { status}：instance->print()，打印是否在running



int attitude_estimator_q_main(int argc, char *argv[])
{
    if (argc < 2)  /**命令行总的参数个数<2,由后面可知有三个参数**/
    {
        warnx("usage: attitude_estimator_q {start|stop|status}");
        return 1;
    }

    if (!strcmp(argv[1], "start"))  /**用户输入的第一个参数是否为start，是的话即继续执行**/
    {

        if (attitude_estimator_q::instance != nullptr) /**不为空即代表已生成了内容，所以已经启动**/
        {
            warnx("already running");
            return 1;
        }

      attitude_estimator_q::instance = new AttitudeEstimatorQ;/**instanc为空就申请空间，申请完成后不再为空指针**/

        if (attitude_estimator_q::instance == nullptr) /**再一次判断是否为空**/
        {
            warnx("alloc failed");    /**是的话即分配空间失败**/
            return 1;
        }

        if (OK != attitude_estimator_q::instance->start())  /**到这一步说明已经申请成功，start函数返回ok代表系统已经新建了一个进程任务，这里代表没有启动**/
        {
            delete attitude_estimator_q::instance;  /**没有启动就释放空间，重新变为空指针**/
            attitude_estimator_q::instance = nullptr;
            warnx("start failed");
            return 1;
        }

        return 0;
    }

    if (!strcmp(argv[1], "stop")) {
        if (attitude_estimator_q::instance == nullptr)
        {
            warnx("not running");
            return 1;
        }

        delete attitude_estimator_q::instance; /**能执行这一步说明instance不为空指针，任务已经执行，然后进行删除**/
        attitude_estimator_q::instance = nullptr; /**恢复原样**/
        return 0;
    }

    if (!strcmp(argv[1], "status"))
    {
        if (attitude_estimator_q::instance)   /**这里代码补全应为if (attitude_estimator_q::instance != nullptr)**/
        {
            warnx("running");
            return 0;
        }
        else
        {
            warnx("not running");
            return 1;
        }
    }

    warnx("unrecognized command"); /**出去start、stop、status外其他的为unrecognized command**/
    return 1;
}
————————————————
版权声明：本文为CSDN博主「这是小旭哦」的原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/zouxu634866/article/details/79806948