Mahony 互补滤波

最新推荐文章于 2024-05-18 22:40:54 发布

qing101hua

最新推荐文章于 2024-05-18 22:40:54 发布

阅读量1.8w

点赞数 4

分类专栏： Mahony

Mahony 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

 版权声明：本文为博主原创文章，转载请注明，欢迎交流~
 Mahony互补滤波
 

目录(?)[+]

上接【互补滤波器】，继续学习互补滤波。。。。

参考：
Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs.
PX4/Pixhawk—uORB深入理解和应用

by luoshi006

如有错误或遗漏，还请留言指出。欢迎各位留言交流，或邮箱 luoshi006@gmail.com。

应用场景

本文中 mahony 的应用场景为多旋翼无人机的姿态估计。

这里写图片描述

陀螺仪、加速度计、MPU6050 详述，请参考：传送门

名词解释

陀螺仪

陀螺仪，测量角速度，具有高动态特性，它是一个间接测量角度的器件。它测量的是角度的导数，即角速度，要将角速度对时间积分才能得到角度。由于噪声等误差影响，在积分作用下不断积累，最终导致陀螺仪的低频干扰和漂移。

加速度计

输出当前加速度（包含重力加速度 g ）的方向【也叫重力感应器】，在悬停时，输出为 g 。由其测量原理导致的高频信号敏感，使得加速度计在振动环境中高频干扰较大。

磁力计

输出为当前机体与地磁场的夹角。测量原理与指南针相似。低频特性较好，易受周围磁场干扰。
磁力计的工作原理参考：WalkAnt的博客

坐标系

导航坐标系：在多旋翼中，又叫地球坐标系、地理坐标系。通常，采用北东地（NED）构成坐标系的 X,Y,Z 轴。

机体坐标系：固联在多旋翼飞行器上，即，坐标系原点定位于飞行器中心点（假设中心点与重心点重合）。

关于航空导航用到的坐标系，请参考 AIAA 系列丛书。在多旋翼中，因为只在低空飞行，且时间较短，只需要以上两个。

姿态表示

欧拉角：较直观，描述刚体在三维欧式空间中的姿态。此时，坐标系为机体坐标系，随着刚体的旋转而旋转。缺点：万向节锁。
详情参加：Wikipedia 传送门

四元数：由一组四维向量，表示刚体的三维旋转。适合用于计算机计算。
详情参加：Wikipedia 传送门

方向余弦矩阵DCM：用欧拉角余弦值或四元数，表示的坐标系的旋转。

mahony 滤波原理

互补滤波要求两个信号的干扰噪声处在不同的频率，通过设置两个滤波器的截止频率，确保融合后的信号能够覆盖需求频率。
在 IMU 的姿态估计中，互补滤波器对陀螺仪（低频噪声）使用高通滤波；对加速度/磁力计（高频噪声）使用低频滤波。
（此处尚未对传感器数据实测，噪声和有用频率未知。。。。待后期补足）

互补滤波中，两个滤波器的截止频率一致，此时就需要根据有用频率的值对其进行取舍。

机体水平时，加速度计无法测量绕 Z 轴的旋转量，即偏航角。磁力计也有同样问题，无法测得要磁轴的旋转量。故，需要加速度计和磁力计同时对陀螺仪进行校正。

mahony 滤波主要过程

q^˙ = 1 2 q^\otimes P (Ω ¯ ¯ ¯ + δ) - - - - - - - - (a)

δ = k P \cdot e + k I \cdot \int e - - - - - - - - (b)

e = v ¯ \times v^- - - - - - - - - - - - (c)

式中， q^ 表示系统姿态估计的四元数表示； δ 是经过 PI 调节器产生的新息； e 表示实测的惯性向量 v¯ 和预测的向量 v^ 之间的相对旋转（误差）。
P(⋅) —— pure quaternion operator（四元数实部为0），表示只有旋转。

PI 调节器中：[20160901更新]

参数 kp 用于控制加速度计和陀螺仪之间的交叉频率；
参数 kI 用于校正陀螺仪误差。

预备知识

主要是公式，不包含推导过程。。。。

欧拉角与机体角速度的关系：

Θ ˙ = W b ω

= ⎡ ⎣ ⎢ 100 t a n θ s i n ϕ c o s ϕ s i n ϕ / c o s θ t a n θ c o s ϕ - s i n ϕ c o s ϕ / c o s θ ⎤ ⎦ ⎥ b ω

旋转矩阵与机体角速度的关系：

R e b ˙ = R e b [b ω] \times

四元数与机体角速度的关系：

q ˙ b e (t) = 1 2 ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ 0 ω x ω y ω z - ω x 0 - ω z ω y - ω y ω z 0 - ω x - ω z - ω y ω x 0 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ q b e (t) - - - - - - - （ 1 ）

参考：北航全权老师课件第五章内容、惯性导航（秦永元）第九章内容。

预测

与卡尔曼滤波相似，mahony 滤波也分为预测-校正。
在预测环节，由三轴陀螺仪测得的角速度，通过式（1）计算出四元数姿态预测。 qbe 表示从地球坐标系到机体坐标系，或机体坐标系姿态在地球坐标系下的表示。

q b e (k) = q b e (k - 1) + q ˙ b e (k) Δ t

校正

在预测环节得到的四元数 qbe(k) ，通过加速度计和磁力计的值进行校正。该环节通常分为两部分：

通过加速度计得到 Δqaccˆ ，然后校正四元数中的横滚（roll）和俯仰（pitch）分量。
当磁力计可读时，通过 Δqmagˆ 校正四元数中的偏航（yaw）分量。

加速度计校正

加速度计信号首先经过低通滤波器（消除高频噪声）：

y (k) = R C T + R C y (k - 1) + T T + R C x (k)

然后，对得到的结果进行归一化（normalized）

Δ q a c c ˆ = Δ q a c c ¯ ¯ ¯ ¯ ¯ ¯ ¯ ¯ | | Δ q a c c ¯ ¯ ¯ ¯ ¯ ¯ ¯ ¯ | |

计算偏差：

e = Δ q a c c ˆ \times v

式中， v 表示重力向量在机体坐标系的向量。

⎡ ⎣ ⎢ v x v y v z ⎤ ⎦ ⎥ = C b n ⎡ ⎣ ⎢ 001 ⎤ ⎦ ⎥

= ⎡ ⎣ ⎢ 2 (q 1 q 3 - q 0 q 2) 2 (q 2 q 3 + q 0 q 1) q 20 - q 21 - q 22 + q 23 ⎤ ⎦ ⎥

此时，由 v 与加速度计向量垂直分量叉乘，得到误差值。【两个物理意义相同的向量，理论上叉乘为零】

磁力计校正

数据预处理与加速度计相同，先滤波，然后归一化，得到 Δqmagˆ 。

1. 无 GPS 校准时：

计算误差：

e = Δ q m a g ˆ \times w

式中， w 计算过程如下：

磁力计的输出（ m ）在机体坐标系下，将其转换到导航坐标系：

⎡ ⎣ ⎢ h x h y h z ⎤ ⎦ ⎥ = C n b ⎡ ⎣ ⎢ m x m y m z ⎤ ⎦ ⎥

导航坐标系的 x 轴与正北对齐，故，可以将磁力计在 xoy 平面的投影折算到 x 轴。

b x = h 2 x + h 2 y - - - - - - \sqrt

b z = h z

再次变换到机体坐标系：

⎡ ⎣ ⎢ w x w y w z ⎤ ⎦ ⎥ = C b n ⎡ ⎣ ⎢ b x 0 b z ⎤ ⎦ ⎥

2. 有 GPS 校准时：

在 px4 中，磁力计使用 GPS 信息 [0,0,mag] 进行校准，故，公式与加速度计相同。

⎡ ⎣ ⎢ w x w y w z ⎤ ⎦ ⎥ = C b n ⎡ ⎣ ⎢ 00 m a g ⎤ ⎦ ⎥

此时，由 w 与磁力计向量叉乘，得到误差值。【两个物理意义相同的向量，理论上叉乘为零】

更新四元数

由加速度计和磁力计校准得到的误差值：

e = e a c c + e m a g

由该误差值得到修正值：

δ = K P \cdot e + K I \cdot \int e d t

修正后的角速度值：

ω = ω g y r o + δ

根据一阶龙格库塔方法求解一阶微分方程：

q ˙ = f (q, ω)

q (t + T) = q (t) + T \cdot f (q, ω)

可以求出四元数微分方程的差分形式：

q 0 (t + T) = q 0 (t) + T 2 [- ω x q 1 (t) - ω y q 2 (t) - ω z q 3 (t)]

四元数规范化：

q = q 0 + q 1 i + q 2 j + q 3 k q 2 0 + q 2 1 + q 2 2 + q 2 3 - - - - - - - - - - - - - \sqrt

源码分析

该部分源码直接截取 px4 开源飞控源码（BSD证书）。
px4 为 pixhawk 飞控的固件代码，内部涉及很多滤波及导航的算法。有较大参考价值。

源码，参考日期：20160603；

https://github.com/hsteinhaus/PX4Firmware/blob/master/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

https://github.com/ArduPilot/PX4Firmware/blob/master/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

参数	默认值
ATT_W_ACC	0.2f	加速度计权重
ATT_W_MAG	0.1f	磁力计权重
ATT_W_EXT_HDG	0.1f	外部航向权重
ATT_W_GYRO_BIAS	0.1f	陀螺仪偏差权重
ATT_MAG_DECL	0.0f	磁偏角（°）
ATT_MAG_DECL_A	1	启用基于GPS的自动磁偏角校正
ATT_EXT_HDG_M	0	外部航向模式
ATT_ACC_COMP	1	启用基于GPS速度的加速度补偿
ATT_BIAS_MAX	0.05f	陀螺仪偏差上限
ATT_VIBE_THRESH	0.2f	振动水平报警阈值

主程序运行流程图：

函数功能简述

AttitudeEstimatorQ::AttitudeEstimatorQ();

构造函数，初始化参数;
AttitudeEstimatorQ::~AttitudeEstimatorQ();

析构函数，杀掉所有任务;
int AttitudeEstimatorQ::start();

启动【attitude_estimator_q】进程，主函数入口： task_main_trampoline
void AttitudeEstimatorQ::print();

打印姿态信息；
void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])

{

attitude_estimator_q::instance->task_main();

}
void AttitudeEstimatorQ::task_main()；

主任务进程;
void AttitudeEstimatorQ::update_parameters(bool force);

false: 查看参数是否更新；

true: 获取新参数, 并由磁偏角更新四元数；
bool AttitudeEstimatorQ::init();

由加速度计和磁力计初始化旋转矩阵，有GPS时，校正磁偏角。
bool AttitudeEstimatorQ::update(float dt);

调用init(); 互补滤波
void AttitudeEstimatorQ::update_mag_declination(float new_declination);

使用磁偏角更新四元数
int attitude_estimator_q_main(int argc, char *argv[]);

attitude_estimator_q { start }：实例化instance，运行instance->start()；

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

attitude_estimator_q { status}：instance->print()，打印姿态信息。

源码分析

此处源码逐行分析，可以使用 Ctrl+f 快速定位;
uORB 相关的数据结构，请参考 px4/Firmware/msg;

<code class="hljs ruby has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">/*代码前的注释段（<span class="hljs-constant" style="box-sizing: border-box;">L34</span>~<span class="hljs-constant" style="box-sizing: border-box;">L40</span>）
 * <span class="hljs-variable" style="color: rgb(102, 0, 102); box-sizing: border-box;">@file</span> attitude_estimator_q_main.cpp
 *
 * <span class="hljs-constant" style="box-sizing: border-box;">Attitude</span> estimator (quaternion based)
 *姿态估计（基于四元数）
 * <span class="hljs-variable" style="color: rgb(102, 0, 102); box-sizing: border-box;">@author</span> <span class="hljs-constant" style="box-sizing: border-box;">Anton</span> <span class="hljs-constant" style="box-sizing: border-box;">Babushkin</span> <anton.babushkin<span class="hljs-variable" style="color: rgb(102, 0, 102); box-sizing: border-box;">@me</span>.com>
 *<span class="hljs-regexp" style="color: rgb(0, 136, 0); box-sizing: border-box;">/</span></code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li></ul>

头文件

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">（l42~l76）
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <px4_config.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <px4_posix.h><span class="hljs-comment" style="box-sizing: border-box;">//add missing check;</span></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <unistd.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <stdlib.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <stdio.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <stdbool.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <poll.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <fcntl.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <float.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <errno.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <limits.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <math.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <uORB/uORB.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <uORB/topics/sensor_combined.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <uORB/topics/vehicle_attitude.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <uORB/topics/control_state.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <uORB/topics/vehicle_control_mode.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <uORB/topics/vehicle_global_position.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <uORB/topics/vision_position_estimate.h><span class="hljs-comment" style="box-sizing: border-box;">//视觉位置估计, 未找到文件【待查】;</span></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <uORB/topics/att_pos_mocap.h><span class="hljs-comment" style="box-sizing: border-box;">//mocap-->vicon;</span></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <uORB/topics/airspeed.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <uORB/topics/parameter_update.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <uORB/topics/estimator_status.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <drivers/drv_hrt.h></span>

<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <mathlib/mathlib.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <mathlib/math/filter/LowPassFilter2p.hpp></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <lib/geo/geo.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <lib/ecl/validation/data_validator_group.h></span>

<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <systemlib/systemlib.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <systemlib/param/param.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <systemlib/perf_counter.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <systemlib/err.h></span>
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#include <systemlib/mavlink_log.h></span></code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li></ul>

using @@@

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">（l78~l82）
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">extern</span> <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"C"</span> __EXPORT <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> attitude_estimator_q_main(<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> argc, <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">char</span> *argv[]);

<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">using</span> math::Vector;
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">using</span> math::Matrix;
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">using</span> math::Quaternion;</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li></ul>

此处，extern “C” 表示以 C 格式编译； __EXPORT 表示将函数名输出到链接器（Linker）； using 关键字表示引入名称到 using 说明出现的声明区域。。

__export
This keyword aids those programming Microsoft Windows. __export causes the function name to be exported to the linker.

namespace attitude_estimator_q

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">(l84~l89)
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">class</span> AttitudeEstimatorQ;

<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">namespace</span> attitude_estimator_q
{
AttitudeEstimatorQ *instance;
}<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//定义命名空间，通过命名空间调用instance;</span></code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li></ul>

类定义: class AttitudeEstimatorQ

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">      (l92~l210)
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">class</span> AttitudeEstimatorQ
{<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//类定义; </span>
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">public</span>:
    AttitudeEstimatorQ();
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//构造函数</span>
    ~AttitudeEstimatorQ();
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//析构函数</span>
     <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>        start();
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//开始任务，成功--返回OK；</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">static</span> <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">void</span> task_main_trampoline(<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> argc, <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">char</span> *argv[]);
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//跳转到 task_main() ，未使用传入参数；static函数只能被本文件中的函数调用；</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">void</span>        task_main();
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">void</span>        print();
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">private</span>:
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">static</span> <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">constexpr</span> <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span> _dt_max = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.02</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//最大时间间隔;</span>
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//constexpr(constant expression)常数表达式，c11新关键字；</span>
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//优化语法检查和编译速度；</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span>        _task_should_exit = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//如果为 true ，任务退出；</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>     _control_task = -<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//进程ID, 默认-1表示没有任务；</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>     _sensors_sub = -<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//sensor_combined subscribe(订阅);</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>     _params_sub = -<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//parameter_update subscribe;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>     _vision_sub = -<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//视觉位置估计;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>     _mocap_sub = -<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//vicon姿态位置估计;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>     _airspeed_sub = -<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//airspeed subscribe;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>     _global_pos_sub = -<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//vehicle_global_position subscribe;</span>
    orb_advert_t    _att_pub = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">nullptr</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//vehicle_attitude publish(发布);</span>
    orb_advert_t    _ctrl_state_pub = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">nullptr</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//发布控制状态主题control_state;</span>
    orb_advert_t    _est_state_pub = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">nullptr</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//estimator_status</span>

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">struct</span> {
        param_t w_acc;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//ATT_W_ACC</span>
        param_t w_mag;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//ATT_W_MAG</span>
        param_t w_ext_hdg;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//ATT_W_EXT_HDG 外部航向权重;</span>
        param_t w_gyro_bias;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//ATT_W_GYRO_BIAS</span>
        param_t mag_decl;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//ATT_MAG_DECL</span>
        param_t mag_decl_auto;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//ATT_MAG_DECL_A 磁偏角自动校正;</span>
        param_t acc_comp;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//ATT_ACC_COMP</span>
        param_t bias_max;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//ATT_BIAS_MAX 陀螺仪偏差上限;</span>
        param_t vibe_thresh;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//ATT_VIBE_THRESH 振动报警阈值;</span>
        param_t ext_hdg_mode;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//ATT_EXT_HDG_M 外部航向模式;</span>
    }       _params_handles;
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//有用参数的句柄；</span>

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span>       _w_accel = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0f</span>;
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//ATT_W_ACC >>> w_acc >>> _w_accel;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span>       _w_mag = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0f</span>;
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span>       _w_ext_hdg = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0f</span>;
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span>       _w_gyro_bias = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0f</span>;
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span>       _mag_decl = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0f</span>;
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span>        _mag_decl_auto = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span>        _acc_comp = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span>       _bias_max = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0f</span>;
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span>       _vibration_warning_threshold = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1.0f</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//振动警告阈值;</span>
    hrt_abstime _vibration_warning_timestamp = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>     _ext_hdg_mode = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;

    Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>>   _gyro;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//陀螺仪;</span>
    Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>>   _accel;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//加速度计;</span>
    Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>>   _mag;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//磁力计;</span>

    vision_position_estimate_s _vision = {};<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//buffer;</span>
    Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>>   _vision_hdg;

    att_pos_mocap_s _mocap = {};<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//buffer;</span>
    Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>>   _mocap_hdg;

    airspeed_s _airspeed = {};<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//buffer;</span>

    Quaternion  _q;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//四元数;</span>
    Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>>   _rates;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//姿态角速度;</span>
    Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>>   _gyro_bias;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//陀螺仪偏差;</span>

    vehicle_global_position_s _gpos = {};<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//buffer;</span>
    Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>>   _vel_prev;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//前一时刻的速度：</span>
    Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>>   _pos_acc;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//加速度（body frame??）</span>

    DataValidatorGroup _voter_gyro;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//数据验证，剔除异常值;</span>
    DataValidatorGroup _voter_accel;
    DataValidatorGroup _voter_mag;

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//姿态速度的二阶低通滤波器；</span>
    math::LowPassFilter2p _lp_roll_rate;
    math::LowPassFilter2p _lp_pitch_rate;
    math::LowPassFilter2p _lp_yaw_rate;

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//绝对时间(ms)</span>
    hrt_abstime _vel_prev_t = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//前一时刻计算速度时的绝对时间;</span>

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span>        _inited = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//初始化标识;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span>        _data_good = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//数据可用;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span>        _failsafe = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//故障保护;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span>        _vibration_warning = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//振动警告;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span>        _ext_hdg_good = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//外部航向可用;</span>

    orb_advert_t    _mavlink_log_pub = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">nullptr</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//mavlink log advert;</span>

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//performance measuring tools (counters)</span>
    perf_counter_t _update_perf;
    perf_counter_t _loop_perf;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//未看到使用。。。;</span>

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">void</span> update_parameters(<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span> force);<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//更新参数;</span>

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> update_subscriptions();<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//未使用【待查？？】</span>

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span> init();

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span> update(<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span> dt);

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// 偏航角旋转后，立即更新磁偏角；</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">void</span> update_mag_declination(<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span> new_declination);
};</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li><li style="box-sizing: border-box; padding: 0px 5px;">41</li><li style="box-sizing: border-box; padding: 0px 5px;">42</li><li style="box-sizing: border-box; padding: 0px 5px;">43</li><li style="box-sizing: border-box; padding: 0px 5px;">44</li><li style="box-sizing: border-box; padding: 0px 5px;">45</li><li style="box-sizing: border-box; padding: 0px 5px;">46</li><li style="box-sizing: border-box; padding: 0px 5px;">47</li><li style="box-sizing: border-box; padding: 0px 5px;">48</li><li style="box-sizing: border-box; padding: 0px 5px;">49</li><li style="box-sizing: border-box; padding: 0px 5px;">50</li><li style="box-sizing: border-box; padding: 0px 5px;">51</li><li style="box-sizing: border-box; padding: 0px 5px;">52</li><li style="box-sizing: border-box; padding: 0px 5px;">53</li><li style="box-sizing: border-box; padding: 0px 5px;">54</li><li style="box-sizing: border-box; padding: 0px 5px;">55</li><li style="box-sizing: border-box; padding: 0px 5px;">56</li><li style="box-sizing: border-box; padding: 0px 5px;">57</li><li style="box-sizing: border-box; padding: 0px 5px;">58</li><li style="box-sizing: border-box; padding: 0px 5px;">59</li><li style="box-sizing: border-box; padding: 0px 5px;">60</li><li style="box-sizing: border-box; padding: 0px 5px;">61</li><li style="box-sizing: border-box; padding: 0px 5px;">62</li><li style="box-sizing: border-box; padding: 0px 5px;">63</li><li style="box-sizing: border-box; padding: 0px 5px;">64</li><li style="box-sizing: border-box; padding: 0px 5px;">65</li><li style="box-sizing: border-box; padding: 0px 5px;">66</li><li style="box-sizing: border-box; padding: 0px 5px;">67</li><li style="box-sizing: border-box; padding: 0px 5px;">68</li><li style="box-sizing: border-box; padding: 0px 5px;">69</li><li style="box-sizing: border-box; padding: 0px 5px;">70</li><li style="box-sizing: border-box; padding: 0px 5px;">71</li><li style="box-sizing: border-box; padding: 0px 5px;">72</li><li style="box-sizing: border-box; padding: 0px 5px;">73</li><li style="box-sizing: border-box; padding: 0px 5px;">74</li><li style="box-sizing: border-box; padding: 0px 5px;">75</li><li style="box-sizing: border-box; padding: 0px 5px;">76</li><li style="box-sizing: border-box; padding: 0px 5px;">77</li><li style="box-sizing: border-box; padding: 0px 5px;">78</li><li style="box-sizing: border-box; padding: 0px 5px;">79</li><li style="box-sizing: border-box; padding: 0px 5px;">80</li><li style="box-sizing: border-box; padding: 0px 5px;">81</li><li style="box-sizing: border-box; padding: 0px 5px;">82</li><li style="box-sizing: border-box; padding: 0px 5px;">83</li><li style="box-sizing: border-box; padding: 0px 5px;">84</li><li style="box-sizing: border-box; padding: 0px 5px;">85</li><li style="box-sizing: border-box; padding: 0px 5px;">86</li><li style="box-sizing: border-box; padding: 0px 5px;">87</li><li style="box-sizing: border-box; padding: 0px 5px;">88</li><li style="box-sizing: border-box; padding: 0px 5px;">89</li><li style="box-sizing: border-box; padding: 0px 5px;">90</li><li style="box-sizing: border-box; padding: 0px 5px;">91</li><li style="box-sizing: border-box; padding: 0px 5px;">92</li><li style="box-sizing: border-box; padding: 0px 5px;">93</li><li style="box-sizing: border-box; padding: 0px 5px;">94</li><li style="box-sizing: border-box; padding: 0px 5px;">95</li><li style="box-sizing: border-box; padding: 0px 5px;">96</li><li style="box-sizing: border-box; padding: 0px 5px;">97</li><li style="box-sizing: border-box; padding: 0px 5px;">98</li><li style="box-sizing: border-box; padding: 0px 5px;">99</li><li style="box-sizing: border-box; padding: 0px 5px;">100</li><li style="box-sizing: border-box; padding: 0px 5px;">101</li><li style="box-sizing: border-box; padding: 0px 5px;">102</li><li style="box-sizing: border-box; padding: 0px 5px;">103</li><li style="box-sizing: border-box; padding: 0px 5px;">104</li><li style="box-sizing: border-box; padding: 0px 5px;">105</li><li style="box-sizing: border-box; padding: 0px 5px;">106</li><li style="box-sizing: border-box; padding: 0px 5px;">107</li><li style="box-sizing: border-box; padding: 0px 5px;">108</li><li style="box-sizing: border-box; padding: 0px 5px;">109</li><li style="box-sizing: border-box; padding: 0px 5px;">110</li><li style="box-sizing: border-box; padding: 0px 5px;">111</li><li style="box-sizing: border-box; padding: 0px 5px;">112</li><li style="box-sizing: border-box; padding: 0px 5px;">113</li><li style="box-sizing: border-box; padding: 0px 5px;">114</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li><li style="box-sizing: border-box; padding: 0px 5px;">41</li><li style="box-sizing: border-box; padding: 0px 5px;">42</li><li style="box-sizing: border-box; padding: 0px 5px;">43</li><li style="box-sizing: border-box; padding: 0px 5px;">44</li><li style="box-sizing: border-box; padding: 0px 5px;">45</li><li style="box-sizing: border-box; padding: 0px 5px;">46</li><li style="box-sizing: border-box; padding: 0px 5px;">47</li><li style="box-sizing: border-box; padding: 0px 5px;">48</li><li style="box-sizing: border-box; padding: 0px 5px;">49</li><li style="box-sizing: border-box; padding: 0px 5px;">50</li><li style="box-sizing: border-box; padding: 0px 5px;">51</li><li style="box-sizing: border-box; padding: 0px 5px;">52</li><li style="box-sizing: border-box; padding: 0px 5px;">53</li><li style="box-sizing: border-box; padding: 0px 5px;">54</li><li style="box-sizing: border-box; padding: 0px 5px;">55</li><li style="box-sizing: border-box; padding: 0px 5px;">56</li><li style="box-sizing: border-box; padding: 0px 5px;">57</li><li style="box-sizing: border-box; padding: 0px 5px;">58</li><li style="box-sizing: border-box; padding: 0px 5px;">59</li><li style="box-sizing: border-box; padding: 0px 5px;">60</li><li style="box-sizing: border-box; padding: 0px 5px;">61</li><li style="box-sizing: border-box; padding: 0px 5px;">62</li><li style="box-sizing: border-box; padding: 0px 5px;">63</li><li style="box-sizing: border-box; padding: 0px 5px;">64</li><li style="box-sizing: border-box; padding: 0px 5px;">65</li><li style="box-sizing: border-box; padding: 0px 5px;">66</li><li style="box-sizing: border-box; padding: 0px 5px;">67</li><li style="box-sizing: border-box; padding: 0px 5px;">68</li><li style="box-sizing: border-box; padding: 0px 5px;">69</li><li style="box-sizing: border-box; padding: 0px 5px;">70</li><li style="box-sizing: border-box; padding: 0px 5px;">71</li><li style="box-sizing: border-box; padding: 0px 5px;">72</li><li style="box-sizing: border-box; padding: 0px 5px;">73</li><li style="box-sizing: border-box; padding: 0px 5px;">74</li><li style="box-sizing: border-box; padding: 0px 5px;">75</li><li style="box-sizing: border-box; padding: 0px 5px;">76</li><li style="box-sizing: border-box; padding: 0px 5px;">77</li><li style="box-sizing: border-box; padding: 0px 5px;">78</li><li style="box-sizing: border-box; padding: 0px 5px;">79</li><li style="box-sizing: border-box; padding: 0px 5px;">80</li><li style="box-sizing: border-box; padding: 0px 5px;">81</li><li style="box-sizing: border-box; padding: 0px 5px;">82</li><li style="box-sizing: border-box; padding: 0px 5px;">83</li><li style="box-sizing: border-box; padding: 0px 5px;">84</li><li style="box-sizing: border-box; padding: 0px 5px;">85</li><li style="box-sizing: border-box; padding: 0px 5px;">86</li><li style="box-sizing: border-box; padding: 0px 5px;">87</li><li style="box-sizing: border-box; padding: 0px 5px;">88</li><li style="box-sizing: border-box; padding: 0px 5px;">89</li><li style="box-sizing: border-box; padding: 0px 5px;">90</li><li style="box-sizing: border-box; padding: 0px 5px;">91</li><li style="box-sizing: border-box; padding: 0px 5px;">92</li><li style="box-sizing: border-box; padding: 0px 5px;">93</li><li style="box-sizing: border-box; padding: 0px 5px;">94</li><li style="box-sizing: border-box; padding: 0px 5px;">95</li><li style="box-sizing: border-box; padding: 0px 5px;">96</li><li style="box-sizing: border-box; padding: 0px 5px;">97</li><li style="box-sizing: border-box; padding: 0px 5px;">98</li><li style="box-sizing: border-box; padding: 0px 5px;">99</li><li style="box-sizing: border-box; padding: 0px 5px;">100</li><li style="box-sizing: border-box; padding: 0px 5px;">101</li><li style="box-sizing: border-box; padding: 0px 5px;">102</li><li style="box-sizing: border-box; padding: 0px 5px;">103</li><li style="box-sizing: border-box; padding: 0px 5px;">104</li><li style="box-sizing: border-box; padding: 0px 5px;">105</li><li style="box-sizing: border-box; padding: 0px 5px;">106</li><li style="box-sizing: border-box; padding: 0px 5px;">107</li><li style="box-sizing: border-box; padding: 0px 5px;">108</li><li style="box-sizing: border-box; padding: 0px 5px;">109</li><li style="box-sizing: border-box; padding: 0px 5px;">110</li><li style="box-sizing: border-box; padding: 0px 5px;">111</li><li style="box-sizing: border-box; padding: 0px 5px;">112</li><li style="box-sizing: border-box; padding: 0px 5px;">113</li><li style="box-sizing: border-box; padding: 0px 5px;">114</li></ul>

构造函数

<code class="hljs cpp has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">     (l213~l235)
AttitudeEstimatorQ::AttitudeEstimatorQ() :
    _vel_prev(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>),
    _pos_acc(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>),
    _voter_gyro(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>),<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//数据成员3个;</span>
    _voter_accel(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>),
    _voter_mag(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>),
    _lp_roll_rate(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">250.0f</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">30.0f</span>),<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//低通滤波（采样频率,截止频率）;</span>
    _lp_pitch_rate(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">250.0f</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">30.0f</span>),
    _lp_yaw_rate(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">250.0f</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">20.0f</span>)
{
    _voter_mag.set_timeout(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">200000</span>);<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//磁力计超时;</span>

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//读取Attitude_estimator_q_params.c中的参数;</span>
    _params_handles.w_acc       = param_find(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"ATT_W_ACC"</span>);
    _params_handles.w_mag       = param_find(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"ATT_W_MAG"</span>);
    _params_handles.w_ext_hdg   = param_find(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"ATT_W_EXT_HDG"</span>);<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//外部航向权重;</span>
    _params_handles.w_gyro_bias = param_find(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"ATT_W_GYRO_BIAS"</span>);
    _params_handles.mag_decl    = param_find(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"ATT_MAG_DECL"</span>);
    _params_handles.mag_decl_auto   = param_find(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"ATT_MAG_DECL_A"</span>);<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//磁偏角自动校正;</span>
    _params_handles.acc_comp    = param_find(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"ATT_ACC_COMP"</span>);
    _params_handles.bias_max    = param_find(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"ATT_BIAS_MAX"</span>);<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//陀螺仪偏差上限;</span>
    _params_handles.vibe_thresh = param_find(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"ATT_VIBE_THRESH"</span>);<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//振动警告阈值;</span>
    _params_handles.ext_hdg_mode    = param_find(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"ATT_EXT_HDG_M"</span>);
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li></ul>

析构函数

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">l240~l262
AttitudeEstimatorQ::~AttitudeEstimatorQ()
{<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//杀掉所有任务;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_control_task != -<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>) {
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* task wakes up every 100ms or so at the longest */</span>
        _task_should_exit = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">true</span>;

        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* wait for a second for the task to quit at our request */</span>
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">unsigned</span> i = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">do</span> {
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* wait 20ms */</span>
            usleep(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">20000</span>);

            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* if we have given up, kill it */</span>
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (++i > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">50</span>) {
                px4_task_delete(_control_task);
                <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">break</span>;
            }
        } <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">while</span> (_control_task != -<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>);
    }

    attitude_estimator_q::instance = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">nullptr</span>;
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li></ul>

start();

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">l264~l282
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> AttitudeEstimatorQ::start()
{
    ASSERT(_control_task == -<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>);

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* start the task */</span>
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//启动任务，返回进程ID;</span>
    _control_task = px4_task_spawn_cmd(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"attitude_estimator_q"</span>,<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/*name*/</span>
                       SCHED_DEFAULT,<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/*任务调度程序*/</span>
                       SCHED_PRIORITY_MAX - <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">5</span>,<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/*优先级*/</span>
                       <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2500</span>,<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/*栈大小*/</span>
                       (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/*主函数入口*/</span>
                       <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">nullptr</span>);

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_control_task < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) {
        warn(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"task start failed"</span>);
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> -errno;
    }

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> OK;
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li></ul>

print();

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">l284~l292
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">void</span> AttitudeEstimatorQ::print()
{<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//打印当前姿态信息;</span>
    warnx(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"gyro status:"</span>);
    _voter_gyro.print();
    warnx(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"accel status:"</span>);
    _voter_accel.print();
    warnx(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"mag status:"</span>);
    _voter_mag.print();
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li></ul>

task_main_trampoline();

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">l294~l297
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">void</span> AttitudeEstimatorQ::task_main_trampoline(<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> argc, <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">char</span> *argv[])
{
    attitude_estimator_q::instance->task_main();
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li></ul>

task_main();

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">l299~l655
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">void</span> AttitudeEstimatorQ::task_main()
{

<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#ifdef __PX4_POSIX</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//记录事件执行所花费的时间，performance counters;</span>
    perf_counter_t _perf_accel(perf_alloc_once(PC_ELAPSED, <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"sim_accel_delay"</span>));
    perf_counter_t _perf_mpu(perf_alloc_once(PC_ELAPSED, <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"sim_mpu_delay"</span>));
    perf_counter_t _perf_mag(perf_alloc_once(PC_ELAPSED, <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"sim_mag_delay"</span>));
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#endif</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//从uORB订阅主题;</span>
    _sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//订阅传感器读数，包含数据参见：Firmware/msg/sensor_combined.msg</span>
    _vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//视觉;</span>
    _mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//vicon; </span>

    _airspeed_sub = orb_subscribe(ORB_ID(airspeed));<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//空速，见Firmware/msg/airspeed.msg;</span>

    _params_sub = orb_subscribe(ORB_ID(parameter_update));<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//bool saved;</span>
    _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//位置估计值(GPS);</span>

    update_parameters(<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">true</span>);<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//获取新参数;</span>

    hrt_abstime last_time = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;

    px4_pollfd_struct_t fds[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>] = {};
    fds[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>].fd = _sensors_sub;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//文件描述符;</span>
    fds[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>].events = POLLIN;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//读取事件标识;</span>
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//POLLIN: data other than high-priority data may be read without blocking;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">while</span> (!_task_should_exit) {
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> ret = px4_poll(fds, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1000</span>);
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//timeout = 1000; fds_size = 1; 详见Linux的poll函数;</span>
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//对字符设备读写;</span>

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (ret < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) {
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Poll error, sleep and try again</span>
            usleep(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">10000</span>);
            PX4_WARN(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"Q POLL ERROR"</span>);
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">continue</span>;

        } <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">else</span> <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (ret == <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) {
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Poll timeout, do nothing</span>
            PX4_WARN(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"Q POLL TIMEOUT"</span>);
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">continue</span>;
        }

        update_parameters(<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>);<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//检查orb是否更新;</span>

        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Update sensors</span>
        sensor_combined_s sensors;

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> best_gyro = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> best_accel = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> best_mag = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Feed validator with recent sensor data</span>
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//（put）将最近的传感器数据送入验证组（DataValidatorGroup）</span>

            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">for</span> (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">unsigned</span> i = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>; i < (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">sizeof</span>(sensors.gyro_timestamp) / <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">sizeof</span>(sensors.gyro_timestamp[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>])); i++) {
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//遍历每个陀螺仪数据;</span>

                <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* ignore empty fields */</span>
                <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//忽略空值;</span>
                <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (sensors.gyro_timestamp[i] > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) {

                    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span> gyro[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>];

                    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">for</span> (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">unsigned</span> j = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>; j < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>; j++) {
                        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (sensors.gyro_integral_dt[i] > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) {
                        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//delta time 大于零;</span>
                            gyro[j] = (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">double</span>)sensors.gyro_integral_rad[i * <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span> + j] / (sensors.gyro_integral_dt[i] / <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1e6</span>);
                            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//=角度/时间（1e6用于us->s转换）;</span>
                        } <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">else</span> {
                            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* fall back to angular rate */</span>
                            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//没有数据更新，回退;</span>
                            gyro[j] = sensors.gyro_rad_s[i * <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span> + j];
                        }
                    }

                    _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
                    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//最后一个参数gyro_priority[]用于支持传感器优先级;</span>
                }

                <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* ignore empty fields */</span>
                <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (sensors.accelerometer_timestamp[i] > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) {
                    _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>],
                             sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
                }
                <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//NED 坐标系下, 单位 m/s^2</span>

                <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* ignore empty fields */</span>
                <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (sensors.magnetometer_timestamp[i] > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) {
                    _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>],
                               sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
                }
                <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//NED 坐标系下, 单位 Gauss</span>
            }

            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Get best measurement values</span>
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//获取最佳测量值（DataValidatorGroup）</span>
            hrt_abstime curr_time = hrt_absolute_time();
            _gyro.<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">set</span>(_voter_gyro.get_best(curr_time, &best_gyro));
            _accel.<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">set</span>(_voter_accel.get_best(curr_time, &best_accel));
            _mag.<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">set</span>(_voter_mag.get_best(curr_time, &best_mag));

            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_accel.length() < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.01f</span>) {
                warnx(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"WARNING: degenerate accel!"</span>);
                <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">continue</span>;
            }
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//退化，即非满秩，此处为奇异值（0）;</span>

            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_mag.length() < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.01f</span>) {
                warnx(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"WARNING: degenerate mag!"</span>);
                <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">continue</span>;
            }<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//同上;</span>

            _data_good = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">true</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//数据可用;</span>

            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (!_failsafe) {
                uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;

<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#ifdef __PX4_POSIX</span>
                perf_end(_perf_accel);<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//执行事件结束，计算均值方差等;</span>
                perf_end(_perf_mpu);
                perf_end(_perf_mag);
<span class="hljs-preprocessor" style="color: rgb(68, 68, 68); box-sizing: border-box;">#endif</span>

                <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_voter_gyro.failover_count() > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) {
                <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//陀螺仪数据故障计数大于0, 记录错误日志;</span>
                    _failsafe = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">true</span>;
                    flags = _voter_gyro.failover_state();
                    mavlink_and_console_log_emergency(&_mavlink_log_pub, <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"Gyro #%i failure :%s%s%s%s%s!"</span>,
                                      _voter_gyro.failover_index(),
                                      ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" No data"</span> : <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">""</span>),
                                      ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" Stale data"</span> : <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">""</span>),
                                      ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" Data timeout"</span> : <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">""</span>),
                                      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" High error count"</span> : <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">""</span>),
                                      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" High error density"</span> : <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">""</span>));
                }

                <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_voter_accel.failover_count() > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) {
                <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//同上，故障日志;</span>
                    _failsafe = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">true</span>;
                    flags = _voter_accel.failover_state();
                    mavlink_and_console_log_emergency(&_mavlink_log_pub, <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"Accel #%i failure :%s%s%s%s%s!"</span>,
                                      _voter_accel.failover_index(),
                                      ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" No data"</span> : <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">""</span>),
                                      ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" Stale data"</span> : <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">""</span>),
                                      ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" Data timeout"</span> : <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">""</span>),
                                      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" High error count"</span> : <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">""</span>),
                                      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" High error density"</span> : <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">""</span>));
                }

                <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_voter_mag.failover_count() > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) {
                <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//同上，故障日志;</span>
                    _failsafe = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">true</span>;
                    flags = _voter_mag.failover_state();
                    mavlink_and_console_log_emergency(&_mavlink_log_pub, <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"Mag #%i failure :%s%s%s%s%s!"</span>,
                                      _voter_mag.failover_index(),
                                      ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" No data"</span> : <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">""</span>),
                                      ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" Stale data"</span> : <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">""</span>),
                                      ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" Data timeout"</span> : <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">""</span>),
                                      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" High error count"</span> : <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">""</span>),
                                      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" High error density"</span> : <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">""</span>));
                }

                <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_failsafe) {
                <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//故障安全机制;</span>
                    mavlink_and_console_log_emergency(&_mavlink_log_pub, <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"</span>);
                }
            }

            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//若启用振动报警，且振动级别超过设定阈值，触发报警; </span>
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//振动级别由数据的方均根（RMS）给出;</span>
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
                            _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
                            _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {

                <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_vibration_warning_timestamp == <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) {
                    _vibration_warning_timestamp = curr_time;

                } <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">else</span> <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (hrt_elapsed_time(&_vibration_warning_timestamp) > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">10000000</span>) {
                    _vibration_warning = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">true</span>;
                    mavlink_and_console_log_critical(&_mavlink_log_pub, <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"HIGH VIBRATION! g: %d a: %d m: %d"</span>,
                                     (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>)(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">100</span> * _voter_gyro.get_vibration_factor(curr_time)),
                                     (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>)(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">100</span> * _voter_accel.get_vibration_factor(curr_time)),
                                     (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>)(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">100</span> * _voter_mag.get_vibration_factor(curr_time)));
                }

            } <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">else</span> {
                _vibration_warning_timestamp = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;
            }
        }

        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Update vision and motion capture heading</span>
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//更新视觉和vicon航向</span>
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span> vision_updated = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;
        orb_check(_vision_sub, &vision_updated);

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span> mocap_updated = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;
        orb_check(_mocap_sub, &mocap_updated);

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (vision_updated) {
            orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//将订阅主题的内容复制到buffer(_vision)中;</span>
            math::Quaternion q(_vision.q);

            math::Matrix<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> Rvis = q.to_dcm();
            math::Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> v(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1.0f</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0f</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.4f</span>);
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//没看出 v 向量具体含义，疑似磁偏校正;</span>

            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Rvis is Rwr (robot respect to world) while v is respect to world.</span>
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Hence Rvis must be transposed having (Rwr)' * Vw</span>
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Rrw * Vw = vn. This way we have consistency</span>
            _vision_hdg = Rvis.transposed() * v;
        }
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//通过视觉得到的姿态估计q->Rvis，将v转换到机体坐标系;</span>

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (mocap_updated) {
            orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
            math::Quaternion q(_mocap.q);
            math::Matrix<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> Rmoc = q.to_dcm();

            math::Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> v(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1.0f</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0f</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.4f</span>);

            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Rmoc is Rwr (robot respect to world) while v is respect to world.</span>
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Hence Rmoc must be transposed having (Rwr)' * Vw</span>
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Rrw * Vw = vn. This way we have consistency</span>
            _mocap_hdg = Rmoc.transposed() * v;
        }

        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Update airspeed</span>
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span> airspeed_updated = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;
        orb_check(_airspeed_sub, &airspeed_updated);

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (airspeed_updated) {
            orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
        }

        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Check for timeouts on data</span>
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_ext_hdg_mode == <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>) {
            _ext_hdg_good = _vision.timestamp_boot > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span> && (hrt_elapsed_time(&_vision.timestamp_boot) < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">500000</span>);

        } <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">else</span> <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_ext_hdg_mode == <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>) {
            _ext_hdg_good = _mocap.timestamp_boot > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span> && (hrt_elapsed_time(&_mocap.timestamp_boot) < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">500000</span>);
        }

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span> gpos_updated;
        orb_check(_global_pos_sub, &gpos_updated);

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (gpos_updated) {
            orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);

            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_mag_decl_auto && _gpos.eph < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">20.0f</span> && hrt_elapsed_time(&_gpos.timestamp) < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1000000</span>) {
                <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* set magnetic declination automatically */</span>
                                update_mag_declination(math::radians(get_mag_declination(_gpos.lat, _gpos.lon)));
            }
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//启用磁偏自动校正，且水平偏差的标准差小于20，根据位置估计值(GPS)（vehicle_global_position）校正磁偏角;</span>
        }
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_acc_comp && _gpos.timestamp != <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span> && hrt_absolute_time() < _gpos.timestamp + <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">20000</span> && _gpos.eph < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">5.0f</span> && _inited) {
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* position data is actual */</span>
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//基于GPS的位置信息，微分得到加速度值;</span>
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (gpos_updated) {
                Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);

                <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* velocity updated */</span>
                <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_vel_prev_t != <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span> && _gpos.timestamp != _vel_prev_t) {
                    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span> vel_dt = (_gpos.timestamp - _vel_prev_t) / <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1000000.0f</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//时间间隔，单位（s）</span>
                    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* calculate acceleration in body frame */</span>
                    _pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
                }<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//由ned坐标系下的速度求出机体坐标系下的加速度;</span>

                _vel_prev_t = _gpos.timestamp;
                _vel_prev = vel;
            }

        } <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">else</span> {
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* position data is outdated, reset acceleration */</span>
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//位置信息已过时，重置;</span>
            _pos_acc.zero();
            _vel_prev.zero();
            _vel_prev_t = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;
        }

        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* time from previous iteration */</span>
        hrt_abstime now = hrt_absolute_time();
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span> dt = (last_time > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) ? ((now  - last_time) / <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1000000.0f</span>) : <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.00001f</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//用极小值0.00001表示零，预防除零错误;</span>
        last_time = now;

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (dt > _dt_max) {
            dt = _dt_max;
        }<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//时间间隔上限;</span>

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (!update(dt)) {
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">continue</span>;
        }<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//调用update(dt)，**互补滤波**，更新四元数;</span>
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//############若不熟悉update()，请转到函数查看;</span>

        Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> euler = _q.to_euler();

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">struct</span> vehicle_attitude_s att = {};
        att.timestamp = sensors.timestamp;

        att.roll = euler(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>);
        att.pitch = euler(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>);
        att.yaw = euler(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>);

        att.rollspeed = _rates(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>);
        att.pitchspeed = _rates(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>);
        att.yawspeed = _rates(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>);

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">for</span> (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> i = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>; i < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>; i++) {
            att.g_comp[i] = _accel(i) - _pos_acc(i);
        }<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//补偿重力向量;</span>

        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* copy offsets */</span>
        <span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">memcpy</span>(&att.rate_offsets, _gyro_bias.data, <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">sizeof</span>(att.rate_offsets));
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//memcpy(*dest,*src,size);</span>

        Matrix<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> R = _q.to_dcm();

        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* copy rotation matrix */</span>
        <span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">memcpy</span>(&att.R[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>], R.data, <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">sizeof</span>(att.R));
        att.R_valid = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">true</span>;
        <span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">memcpy</span>(&att.q[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>], _q.data, <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">sizeof</span>(att.q));
        att.q_valid = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">true</span>;
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//获取姿态振动, RMS;</span>
        att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time());
        att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time());
        att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time());

        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* the instance count is not used here */</span>
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> att_inst;
        orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//广播姿态信息;</span>

        {<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//使用当前姿态，更新control_state，并发布;</span>
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">struct</span> control_state_s ctrl_state = {};

            ctrl_state.timestamp = sensors.timestamp;

            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* attitude quaternions for control state */</span>
            ctrl_state.q[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>] = _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>);
            ctrl_state.q[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>] = _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>);
            ctrl_state.q[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>] = _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>);
            ctrl_state.q[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>] = _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>);

            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* attitude rates for control state */</span>
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//低通滤波，输入参数为当前值;</span>
            ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>));

            ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>));

            ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>));

            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* Airspeed - take airspeed measurement directly here as no wind is estimated */</span>
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1e6</span>
                && _airspeed.timestamp > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) {
                ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
                ctrl_state.airspeed_valid = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">true</span>;

            } <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">else</span> {
                ctrl_state.airspeed_valid = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;
            }

            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* the instance count is not used here */</span>
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> ctrl_inst;
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* publish to control state topic */</span>
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//发布控制状态主题,control_state.msg。</span>
            orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH);
        }

        {
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">struct</span> estimator_status_s est = {};

            est.timestamp = sensors.timestamp;
            est.vibe[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>] = _voter_accel.get_vibration_offset(est.timestamp, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>);
            est.vibe[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>] = _voter_accel.get_vibration_offset(est.timestamp, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>);
            est.vibe[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>] = _voter_accel.get_vibration_offset(est.timestamp, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>);

            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* the instance count is not used here */</span>
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> est_inst;
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">/* publish to control state topic */</span>
            orb_publish_auto(ORB_ID(estimator_status), &_est_state_pub, &est, &est_inst, ORB_PRIO_HIGH);
        }
    }
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li><li style="box-sizing: border-box; padding: 0px 5px;">41</li><li style="box-sizing: border-box; padding: 0px 5px;">42</li><li style="box-sizing: border-box; padding: 0px 5px;">43</li><li style="box-sizing: border-box; padding: 0px 5px;">44</li><li style="box-sizing: border-box; padding: 0px 5px;">45</li><li style="box-sizing: border-box; padding: 0px 5px;">46</li><li style="box-sizing: border-box; padding: 0px 5px;">47</li><li style="box-sizing: border-box; padding: 0px 5px;">48</li><li style="box-sizing: border-box; padding: 0px 5px;">49</li><li style="box-sizing: border-box; padding: 0px 5px;">50</li><li style="box-sizing: border-box; padding: 0px 5px;">51</li><li style="box-sizing: border-box; padding: 0px 5px;">52</li><li style="box-sizing: border-box; padding: 0px 5px;">53</li><li style="box-sizing: border-box; padding: 0px 5px;">54</li><li style="box-sizing: border-box; padding: 0px 5px;">55</li><li style="box-sizing: border-box; padding: 0px 5px;">56</li><li style="box-sizing: border-box; padding: 0px 5px;">57</li><li style="box-sizing: border-box; padding: 0px 5px;">58</li><li style="box-sizing: border-box; padding: 0px 5px;">59</li><li style="box-sizing: border-box; padding: 0px 5px;">60</li><li style="box-sizing: border-box; padding: 0px 5px;">61</li><li style="box-sizing: border-box; padding: 0px 5px;">62</li><li style="box-sizing: border-box; padding: 0px 5px;">63</li><li style="box-sizing: border-box; padding: 0px 5px;">64</li><li style="box-sizing: border-box; padding: 0px 5px;">65</li><li style="box-sizing: border-box; padding: 0px 5px;">66</li><li style="box-sizing: border-box; padding: 0px 5px;">67</li><li style="box-sizing: border-box; padding: 0px 5px;">68</li><li style="box-sizing: border-box; padding: 0px 5px;">69</li><li style="box-sizing: border-box; padding: 0px 5px;">70</li><li style="box-sizing: border-box; padding: 0px 5px;">71</li><li style="box-sizing: border-box; padding: 0px 5px;">72</li><li style="box-sizing: border-box; padding: 0px 5px;">73</li><li style="box-sizing: border-box; padding: 0px 5px;">74</li><li style="box-sizing: border-box; padding: 0px 5px;">75</li><li style="box-sizing: border-box; padding: 0px 5px;">76</li><li style="box-sizing: border-box; padding: 0px 5px;">77</li><li style="box-sizing: border-box; padding: 0px 5px;">78</li><li style="box-sizing: border-box; padding: 0px 5px;">79</li><li style="box-sizing: border-box; padding: 0px 5px;">80</li><li style="box-sizing: border-box; padding: 0px 5px;">81</li><li style="box-sizing: border-box; padding: 0px 5px;">82</li><li style="box-sizing: border-box; padding: 0px 5px;">83</li><li style="box-sizing: border-box; padding: 0px 5px;">84</li><li style="box-sizing: border-box; padding: 0px 5px;">85</li><li style="box-sizing: border-box; padding: 0px 5px;">86</li><li style="box-sizing: border-box; padding: 0px 5px;">87</li><li style="box-sizing: border-box; padding: 0px 5px;">88</li><li style="box-sizing: border-box; padding: 0px 5px;">89</li><li style="box-sizing: border-box; padding: 0px 5px;">90</li><li style="box-sizing: border-box; padding: 0px 5px;">91</li><li style="box-sizing: border-box; padding: 0px 5px;">92</li><li style="box-sizing: border-box; padding: 0px 5px;">93</li><li style="box-sizing: border-box; padding: 0px 5px;">94</li><li style="box-sizing: border-box; padding: 0px 5px;">95</li><li style="box-sizing: border-box; padding: 0px 5px;">96</li><li style="box-sizing: border-box; padding: 0px 5px;">97</li><li style="box-sizing: border-box; padding: 0px 5px;">98</li><li style="box-sizing: border-box; padding: 0px 5px;">99</li><li style="box-sizing: border-box; padding: 0px 5px;">100</li><li style="box-sizing: border-box; padding: 0px 5px;">101</li><li style="box-sizing: border-box; padding: 0px 5px;">102</li><li style="box-sizing: border-box; padding: 0px 5px;">103</li><li style="box-sizing: border-box; padding: 0px 5px;">104</li><li style="box-sizing: border-box; padding: 0px 5px;">105</li><li style="box-sizing: border-box; padding: 0px 5px;">106</li><li style="box-sizing: border-box; padding: 0px 5px;">107</li><li style="box-sizing: border-box; padding: 0px 5px;">108</li><li style="box-sizing: border-box; padding: 0px 5px;">109</li><li style="box-sizing: border-box; padding: 0px 5px;">110</li><li style="box-sizing: border-box; padding: 0px 5px;">111</li><li style="box-sizing: border-box; padding: 0px 5px;">112</li><li style="box-sizing: border-box; padding: 0px 5px;">113</li><li style="box-sizing: border-box; padding: 0px 5px;">114</li><li style="box-sizing: border-box; padding: 0px 5px;">115</li><li style="box-sizing: border-box; padding: 0px 5px;">116</li><li style="box-sizing: border-box; padding: 0px 5px;">117</li><li style="box-sizing: border-box; padding: 0px 5px;">118</li><li style="box-sizing: border-box; padding: 0px 5px;">119</li><li style="box-sizing: border-box; padding: 0px 5px;">120</li><li style="box-sizing: border-box; padding: 0px 5px;">121</li><li style="box-sizing: border-box; padding: 0px 5px;">122</li><li style="box-sizing: border-box; padding: 0px 5px;">123</li><li style="box-sizing: border-box; padding: 0px 5px;">124</li><li style="box-sizing: border-box; padding: 0px 5px;">125</li><li style="box-sizing: border-box; padding: 0px 5px;">126</li><li style="box-sizing: border-box; padding: 0px 5px;">127</li><li style="box-sizing: border-box; padding: 0px 5px;">128</li><li style="box-sizing: border-box; padding: 0px 5px;">129</li><li style="box-sizing: border-box; padding: 0px 5px;">130</li><li style="box-sizing: border-box; padding: 0px 5px;">131</li><li style="box-sizing: border-box; padding: 0px 5px;">132</li><li style="box-sizing: border-box; padding: 0px 5px;">133</li><li style="box-sizing: border-box; padding: 0px 5px;">134</li><li style="box-sizing: border-box; padding: 0px 5px;">135</li><li style="box-sizing: border-box; padding: 0px 5px;">136</li><li style="box-sizing: border-box; padding: 0px 5px;">137</li><li style="box-sizing: border-box; padding: 0px 5px;">138</li><li style="box-sizing: border-box; padding: 0px 5px;">139</li><li style="box-sizing: border-box; padding: 0px 5px;">140</li><li style="box-sizing: border-box; padding: 0px 5px;">141</li><li style="box-sizing: border-box; padding: 0px 5px;">142</li><li style="box-sizing: border-box; padding: 0px 5px;">143</li><li style="box-sizing: border-box; padding: 0px 5px;">144</li><li style="box-sizing: border-box; padding: 0px 5px;">145</li><li style="box-sizing: border-box; padding: 0px 5px;">146</li><li style="box-sizing: border-box; padding: 0px 5px;">147</li><li style="box-sizing: border-box; padding: 0px 5px;">148</li><li style="box-sizing: border-box; padding: 0px 5px;">149</li><li style="box-sizing: border-box; padding: 0px 5px;">150</li><li style="box-sizing: border-box; padding: 0px 5px;">151</li><li style="box-sizing: border-box; padding: 0px 5px;">152</li><li style="box-sizing: border-box; padding: 0px 5px;">153</li><li style="box-sizing: border-box; padding: 0px 5px;">154</li><li style="box-sizing: border-box; padding: 0px 5px;">155</li><li style="box-sizing: border-box; padding: 0px 5px;">156</li><li style="box-sizing: border-box; padding: 0px 5px;">157</li><li style="box-sizing: border-box; padding: 0px 5px;">158</li><li style="box-sizing: border-box; padding: 0px 5px;">159</li><li style="box-sizing: border-box; padding: 0px 5px;">160</li><li style="box-sizing: border-box; padding: 0px 5px;">161</li><li style="box-sizing: border-box; padding: 0px 5px;">162</li><li style="box-sizing: border-box; padding: 0px 5px;">163</li><li style="box-sizing: border-box; padding: 0px 5px;">164</li><li style="box-sizing: border-box; padding: 0px 5px;">165</li><li style="box-sizing: border-box; padding: 0px 5px;">166</li><li style="box-sizing: border-box; padding: 0px 5px;">167</li><li style="box-sizing: border-box; padding: 0px 5px;">168</li><li style="box-sizing: border-box; padding: 0px 5px;">169</li><li style="box-sizing: border-box; padding: 0px 5px;">170</li><li style="box-sizing: border-box; padding: 0px 5px;">171</li><li style="box-sizing: border-box; padding: 0px 5px;">172</li><li style="box-sizing: border-box; padding: 0px 5px;">173</li><li style="box-sizing: border-box; padding: 0px 5px;">174</li><li style="box-sizing: border-box; padding: 0px 5px;">175</li><li style="box-sizing: border-box; padding: 0px 5px;">176</li><li style="box-sizing: border-box; padding: 0px 5px;">177</li><li style="box-sizing: border-box; padding: 0px 5px;">178</li><li style="box-sizing: border-box; padding: 0px 5px;">179</li><li style="box-sizing: border-box; padding: 0px 5px;">180</li><li style="box-sizing: border-box; padding: 0px 5px;">181</li><li style="box-sizing: border-box; padding: 0px 5px;">182</li><li style="box-sizing: border-box; padding: 0px 5px;">183</li><li style="box-sizing: border-box; padding: 0px 5px;">184</li><li style="box-sizing: border-box; padding: 0px 5px;">185</li><li style="box-sizing: border-box; padding: 0px 5px;">186</li><li style="box-sizing: border-box; padding: 0px 5px;">187</li><li style="box-sizing: border-box; padding: 0px 5px;">188</li><li style="box-sizing: border-box; padding: 0px 5px;">189</li><li style="box-sizing: border-box; padding: 0px 5px;">190</li><li style="box-sizing: border-box; padding: 0px 5px;">191</li><li style="box-sizing: border-box; padding: 0px 5px;">192</li><li style="box-sizing: border-box; padding: 0px 5px;">193</li><li style="box-sizing: border-box; padding: 0px 5px;">194</li><li style="box-sizing: border-box; padding: 0px 5px;">195</li><li style="box-sizing: border-box; padding: 0px 5px;">196</li><li style="box-sizing: border-box; padding: 0px 5px;">197</li><li style="box-sizing: border-box; padding: 0px 5px;">198</li><li style="box-sizing: border-box; padding: 0px 5px;">199</li><li style="box-sizing: border-box; padding: 0px 5px;">200</li><li style="box-sizing: border-box; padding: 0px 5px;">201</li><li style="box-sizing: border-box; padding: 0px 5px;">202</li><li style="box-sizing: border-box; padding: 0px 5px;">203</li><li style="box-sizing: border-box; padding: 0px 5px;">204</li><li style="box-sizing: border-box; padding: 0px 5px;">205</li><li style="box-sizing: border-box; padding: 0px 5px;">206</li><li style="box-sizing: border-box; padding: 0px 5px;">207</li><li style="box-sizing: border-box; padding: 0px 5px;">208</li><li style="box-sizing: border-box; padding: 0px 5px;">209</li><li style="box-sizing: border-box; padding: 0px 5px;">210</li><li style="box-sizing: border-box; padding: 0px 5px;">211</li><li style="box-sizing: border-box; padding: 0px 5px;">212</li><li style="box-sizing: border-box; padding: 0px 5px;">213</li><li style="box-sizing: border-box; padding: 0px 5px;">214</li><li style="box-sizing: border-box; padding: 0px 5px;">215</li><li style="box-sizing: border-box; padding: 0px 5px;">216</li><li style="box-sizing: border-box; padding: 0px 5px;">217</li><li style="box-sizing: border-box; padding: 0px 5px;">218</li><li style="box-sizing: border-box; padding: 0px 5px;">219</li><li style="box-sizing: border-box; padding: 0px 5px;">220</li><li style="box-sizing: border-box; padding: 0px 5px;">221</li><li style="box-sizing: border-box; padding: 0px 5px;">222</li><li style="box-sizing: border-box; padding: 0px 5px;">223</li><li style="box-sizing: border-box; padding: 0px 5px;">224</li><li style="box-sizing: border-box; padding: 0px 5px;">225</li><li style="box-sizing: border-box; padding: 0px 5px;">226</li><li style="box-sizing: border-box; padding: 0px 5px;">227</li><li style="box-sizing: border-box; padding: 0px 5px;">228</li><li style="box-sizing: border-box; padding: 0px 5px;">229</li><li style="box-sizing: border-box; padding: 0px 5px;">230</li><li style="box-sizing: border-box; padding: 0px 5px;">231</li><li style="box-sizing: border-box; padding: 0px 5px;">232</li><li style="box-sizing: border-box; padding: 0px 5px;">233</li><li style="box-sizing: border-box; padding: 0px 5px;">234</li><li style="box-sizing: border-box; padding: 0px 5px;">235</li><li style="box-sizing: border-box; padding: 0px 5px;">236</li><li style="box-sizing: border-box; padding: 0px 5px;">237</li><li style="box-sizing: border-box; padding: 0px 5px;">238</li><li style="box-sizing: border-box; padding: 0px 5px;">239</li><li style="box-sizing: border-box; padding: 0px 5px;">240</li><li style="box-sizing: border-box; padding: 0px 5px;">241</li><li style="box-sizing: border-box; padding: 0px 5px;">242</li><li style="box-sizing: border-box; padding: 0px 5px;">243</li><li style="box-sizing: border-box; padding: 0px 5px;">244</li><li style="box-sizing: border-box; padding: 0px 5px;">245</li><li style="box-sizing: border-box; padding: 0px 5px;">246</li><li style="box-sizing: border-box; padding: 0px 5px;">247</li><li style="box-sizing: border-box; padding: 0px 5px;">248</li><li style="box-sizing: border-box; padding: 0px 5px;">249</li><li style="box-sizing: border-box; padding: 0px 5px;">250</li><li style="box-sizing: border-box; padding: 0px 5px;">251</li><li style="box-sizing: border-box; padding: 0px 5px;">252</li><li style="box-sizing: border-box; padding: 0px 5px;">253</li><li style="box-sizing: border-box; padding: 0px 5px;">254</li><li style="box-sizing: border-box; padding: 0px 5px;">255</li><li style="box-sizing: border-box; padding: 0px 5px;">256</li><li style="box-sizing: border-box; padding: 0px 5px;">257</li><li style="box-sizing: border-box; padding: 0px 5px;">258</li><li style="box-sizing: border-box; padding: 0px 5px;">259</li><li style="box-sizing: border-box; padding: 0px 5px;">260</li><li style="box-sizing: border-box; padding: 0px 5px;">261</li><li style="box-sizing: border-box; padding: 0px 5px;">262</li><li style="box-sizing: border-box; padding: 0px 5px;">263</li><li style="box-sizing: border-box; padding: 0px 5px;">264</li><li style="box-sizing: border-box; padding: 0px 5px;">265</li><li style="box-sizing: border-box; padding: 0px 5px;">266</li><li style="box-sizing: border-box; padding: 0px 5px;">267</li><li style="box-sizing: border-box; padding: 0px 5px;">268</li><li style="box-sizing: border-box; padding: 0px 5px;">269</li><li style="box-sizing: border-box; padding: 0px 5px;">270</li><li style="box-sizing: border-box; padding: 0px 5px;">271</li><li style="box-sizing: border-box; padding: 0px 5px;">272</li><li style="box-sizing: border-box; padding: 0px 5px;">273</li><li style="box-sizing: border-box; padding: 0px 5px;">274</li><li style="box-sizing: border-box; padding: 0px 5px;">275</li><li style="box-sizing: border-box; padding: 0px 5px;">276</li><li style="box-sizing: border-box; padding: 0px 5px;">277</li><li style="box-sizing: border-box; padding: 0px 5px;">278</li><li style="box-sizing: border-box; padding: 0px 5px;">279</li><li style="box-sizing: border-box; padding: 0px 5px;">280</li><li style="box-sizing: border-box; padding: 0px 5px;">281</li><li style="box-sizing: border-box; padding: 0px 5px;">282</li><li style="box-sizing: border-box; padding: 0px 5px;">283</li><li style="box-sizing: border-box; padding: 0px 5px;">284</li><li style="box-sizing: border-box; padding: 0px 5px;">285</li><li style="box-sizing: border-box; padding: 0px 5px;">286</li><li style="box-sizing: border-box; padding: 0px 5px;">287</li><li style="box-sizing: border-box; padding: 0px 5px;">288</li><li style="box-sizing: border-box; padding: 0px 5px;">289</li><li style="box-sizing: border-box; padding: 0px 5px;">290</li><li style="box-sizing: border-box; padding: 0px 5px;">291</li><li style="box-sizing: border-box; padding: 0px 5px;">292</li><li style="box-sizing: border-box; padding: 0px 5px;">293</li><li style="box-sizing: border-box; padding: 0px 5px;">294</li><li style="box-sizing: border-box; padding: 0px 5px;">295</li><li style="box-sizing: border-box; padding: 0px 5px;">296</li><li style="box-sizing: border-box; padding: 0px 5px;">297</li><li style="box-sizing: border-box; padding: 0px 5px;">298</li><li style="box-sizing: border-box; padding: 0px 5px;">299</li><li style="box-sizing: border-box; padding: 0px 5px;">300</li><li style="box-sizing: border-box; padding: 0px 5px;">301</li><li style="box-sizing: border-box; padding: 0px 5px;">302</li><li style="box-sizing: border-box; padding: 0px 5px;">303</li><li style="box-sizing: border-box; padding: 0px 5px;">304</li><li style="box-sizing: border-box; padding: 0px 5px;">305</li><li style="box-sizing: border-box; padding: 0px 5px;">306</li><li style="box-sizing: border-box; padding: 0px 5px;">307</li><li style="box-sizing: border-box; padding: 0px 5px;">308</li><li style="box-sizing: border-box; padding: 0px 5px;">309</li><li style="box-sizing: border-box; padding: 0px 5px;">310</li><li style="box-sizing: border-box; padding: 0px 5px;">311</li><li style="box-sizing: border-box; padding: 0px 5px;">312</li><li style="box-sizing: border-box; padding: 0px 5px;">313</li><li style="box-sizing: border-box; padding: 0px 5px;">314</li><li style="box-sizing: border-box; padding: 0px 5px;">315</li><li style="box-sizing: border-box; padding: 0px 5px;">316</li><li style="box-sizing: border-box; padding: 0px 5px;">317</li><li style="box-sizing: border-box; padding: 0px 5px;">318</li><li style="box-sizing: border-box; padding: 0px 5px;">319</li><li style="box-sizing: border-box; padding: 0px 5px;">320</li><li style="box-sizing: border-box; padding: 0px 5px;">321</li><li style="box-sizing: border-box; padding: 0px 5px;">322</li><li style="box-sizing: border-box; padding: 0px 5px;">323</li><li style="box-sizing: border-box; padding: 0px 5px;">324</li><li style="box-sizing: border-box; padding: 0px 5px;">325</li><li style="box-sizing: border-box; padding: 0px 5px;">326</li><li style="box-sizing: border-box; padding: 0px 5px;">327</li><li style="box-sizing: border-box; padding: 0px 5px;">328</li><li style="box-sizing: border-box; padding: 0px 5px;">329</li><li style="box-sizing: border-box; padding: 0px 5px;">330</li><li style="box-sizing: border-box; padding: 0px 5px;">331</li><li style="box-sizing: border-box; padding: 0px 5px;">332</li><li style="box-sizing: border-box; padding: 0px 5px;">333</li><li style="box-sizing: border-box; padding: 0px 5px;">334</li><li style="box-sizing: border-box; padding: 0px 5px;">335</li><li style="box-sizing: border-box; padding: 0px 5px;">336</li><li style="box-sizing: border-box; padding: 0px 5px;">337</li><li style="box-sizing: border-box; padding: 0px 5px;">338</li><li style="box-sizing: border-box; padding: 0px 5px;">339</li><li style="box-sizing: border-box; padding: 0px 5px;">340</li><li style="box-sizing: border-box; padding: 0px 5px;">341</li><li style="box-sizing: border-box; padding: 0px 5px;">342</li><li style="box-sizing: border-box; padding: 0px 5px;">343</li><li style="box-sizing: border-box; padding: 0px 5px;">344</li><li style="box-sizing: border-box; padding: 0px 5px;">345</li><li style="box-sizing: border-box; padding: 0px 5px;">346</li><li style="box-sizing: border-box; padding: 0px 5px;">347</li><li style="box-sizing: border-box; padding: 0px 5px;">348</li><li style="box-sizing: border-box; padding: 0px 5px;">349</li><li style="box-sizing: border-box; padding: 0px 5px;">350</li><li style="box-sizing: border-box; padding: 0px 5px;">351</li><li style="box-sizing: border-box; padding: 0px 5px;">352</li><li style="box-sizing: border-box; padding: 0px 5px;">353</li><li style="box-sizing: border-box; padding: 0px 5px;">354</li><li style="box-sizing: border-box; padding: 0px 5px;">355</li><li style="box-sizing: border-box; padding: 0px 5px;">356</li><li style="box-sizing: border-box; padding: 0px 5px;">357</li><li style="box-sizing: border-box; padding: 0px 5px;">358</li><li style="box-sizing: border-box; padding: 0px 5px;">359</li><li style="box-sizing: border-box; padding: 0px 5px;">360</li><li style="box-sizing: border-box; padding: 0px 5px;">361</li><li style="box-sizing: border-box; padding: 0px 5px;">362</li><li style="box-sizing: border-box; padding: 0px 5px;">363</li><li style="box-sizing: border-box; padding: 0px 5px;">364</li><li style="box-sizing: border-box; padding: 0px 5px;">365</li><li style="box-sizing: border-box; padding: 0px 5px;">366</li><li style="box-sizing: border-box; padding: 0px 5px;">367</li><li style="box-sizing: border-box; padding: 0px 5px;">368</li><li style="box-sizing: border-box; padding: 0px 5px;">369</li><li style="box-sizing: border-box; padding: 0px 5px;">370</li><li style="box-sizing: border-box; padding: 0px 5px;">371</li><li style="box-sizing: border-box; padding: 0px 5px;">372</li><li style="box-sizing: border-box; padding: 0px 5px;">373</li><li style="box-sizing: border-box; padding: 0px 5px;">374</li><li style="box-sizing: border-box; padding: 0px 5px;">375</li><li style="box-sizing: border-box; padding: 0px 5px;">376</li><li style="box-sizing: border-box; padding: 0px 5px;">377</li><li style="box-sizing: border-box; padding: 0px 5px;">378</li><li style="box-sizing: border-box; padding: 0px 5px;">379</li><li style="box-sizing: border-box; padding: 0px 5px;">380</li><li style="box-sizing: border-box; padding: 0px 5px;">381</li><li style="box-sizing: border-box; padding: 0px 5px;">382</li><li style="box-sizing: border-box; padding: 0px 5px;">383</li><li style="box-sizing: border-box; padding: 0px 5px;">384</li><li style="box-sizing: border-box; padding: 0px 5px;">385</li><li style="box-sizing: border-box; padding: 0px 5px;">386</li><li style="box-sizing: border-box; padding: 0px 5px;">387</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li><li style="box-sizing: border-box; padding: 0px 5px;">41</li><li style="box-sizing: border-box; padding: 0px 5px;">42</li><li style="box-sizing: border-box; padding: 0px 5px;">43</li><li style="box-sizing: border-box; padding: 0px 5px;">44</li><li style="box-sizing: border-box; padding: 0px 5px;">45</li><li style="box-sizing: border-box; padding: 0px 5px;">46</li><li style="box-sizing: border-box; padding: 0px 5px;">47</li><li style="box-sizing: border-box; padding: 0px 5px;">48</li><li style="box-sizing: border-box; padding: 0px 5px;">49</li><li style="box-sizing: border-box; padding: 0px 5px;">50</li><li style="box-sizing: border-box; padding: 0px 5px;">51</li><li style="box-sizing: border-box; padding: 0px 5px;">52</li><li style="box-sizing: border-box; padding: 0px 5px;">53</li><li style="box-sizing: border-box; padding: 0px 5px;">54</li><li style="box-sizing: border-box; padding: 0px 5px;">55</li><li style="box-sizing: border-box; padding: 0px 5px;">56</li><li style="box-sizing: border-box; padding: 0px 5px;">57</li><li style="box-sizing: border-box; padding: 0px 5px;">58</li><li style="box-sizing: border-box; padding: 0px 5px;">59</li><li style="box-sizing: border-box; padding: 0px 5px;">60</li><li style="box-sizing: border-box; padding: 0px 5px;">61</li><li style="box-sizing: border-box; padding: 0px 5px;">62</li><li style="box-sizing: border-box; padding: 0px 5px;">63</li><li style="box-sizing: border-box; padding: 0px 5px;">64</li><li style="box-sizing: border-box; padding: 0px 5px;">65</li><li style="box-sizing: border-box; padding: 0px 5px;">66</li><li style="box-sizing: border-box; padding: 0px 5px;">67</li><li style="box-sizing: border-box; padding: 0px 5px;">68</li><li style="box-sizing: border-box; padding: 0px 5px;">69</li><li style="box-sizing: border-box; padding: 0px 5px;">70</li><li style="box-sizing: border-box; padding: 0px 5px;">71</li><li style="box-sizing: border-box; padding: 0px 5px;">72</li><li style="box-sizing: border-box; padding: 0px 5px;">73</li><li style="box-sizing: border-box; padding: 0px 5px;">74</li><li style="box-sizing: border-box; padding: 0px 5px;">75</li><li style="box-sizing: border-box; padding: 0px 5px;">76</li><li style="box-sizing: border-box; padding: 0px 5px;">77</li><li style="box-sizing: border-box; padding: 0px 5px;">78</li><li style="box-sizing: border-box; padding: 0px 5px;">79</li><li style="box-sizing: border-box; padding: 0px 5px;">80</li><li style="box-sizing: border-box; padding: 0px 5px;">81</li><li style="box-sizing: border-box; padding: 0px 5px;">82</li><li style="box-sizing: border-box; padding: 0px 5px;">83</li><li style="box-sizing: border-box; padding: 0px 5px;">84</li><li style="box-sizing: border-box; padding: 0px 5px;">85</li><li style="box-sizing: border-box; padding: 0px 5px;">86</li><li style="box-sizing: border-box; padding: 0px 5px;">87</li><li style="box-sizing: border-box; padding: 0px 5px;">88</li><li style="box-sizing: border-box; padding: 0px 5px;">89</li><li style="box-sizing: border-box; padding: 0px 5px;">90</li><li style="box-sizing: border-box; padding: 0px 5px;">91</li><li style="box-sizing: border-box; padding: 0px 5px;">92</li><li style="box-sizing: border-box; padding: 0px 5px;">93</li><li style="box-sizing: border-box; padding: 0px 5px;">94</li><li style="box-sizing: border-box; padding: 0px 5px;">95</li><li style="box-sizing: border-box; padding: 0px 5px;">96</li><li style="box-sizing: border-box; padding: 0px 5px;">97</li><li style="box-sizing: border-box; padding: 0px 5px;">98</li><li style="box-sizing: border-box; padding: 0px 5px;">99</li><li style="box-sizing: border-box; padding: 0px 5px;">100</li><li style="box-sizing: border-box; padding: 0px 5px;">101</li><li style="box-sizing: border-box; padding: 0px 5px;">102</li><li style="box-sizing: border-box; padding: 0px 5px;">103</li><li style="box-sizing: border-box; padding: 0px 5px;">104</li><li style="box-sizing: border-box; padding: 0px 5px;">105</li><li style="box-sizing: border-box; padding: 0px 5px;">106</li><li style="box-sizing: border-box; padding: 0px 5px;">107</li><li style="box-sizing: border-box; padding: 0px 5px;">108</li><li style="box-sizing: border-box; padding: 0px 5px;">109</li><li style="box-sizing: border-box; padding: 0px 5px;">110</li><li style="box-sizing: border-box; padding: 0px 5px;">111</li><li style="box-sizing: border-box; padding: 0px 5px;">112</li><li style="box-sizing: border-box; padding: 0px 5px;">113</li><li style="box-sizing: border-box; padding: 0px 5px;">114</li><li style="box-sizing: border-box; padding: 0px 5px;">115</li><li style="box-sizing: border-box; padding: 0px 5px;">116</li><li style="box-sizing: border-box; padding: 0px 5px;">117</li><li style="box-sizing: border-box; padding: 0px 5px;">118</li><li style="box-sizing: border-box; padding: 0px 5px;">119</li><li style="box-sizing: border-box; padding: 0px 5px;">120</li><li style="box-sizing: border-box; padding: 0px 5px;">121</li><li style="box-sizing: border-box; padding: 0px 5px;">122</li><li style="box-sizing: border-box; padding: 0px 5px;">123</li><li style="box-sizing: border-box; padding: 0px 5px;">124</li><li style="box-sizing: border-box; padding: 0px 5px;">125</li><li style="box-sizing: border-box; padding: 0px 5px;">126</li><li style="box-sizing: border-box; padding: 0px 5px;">127</li><li style="box-sizing: border-box; padding: 0px 5px;">128</li><li style="box-sizing: border-box; padding: 0px 5px;">129</li><li style="box-sizing: border-box; padding: 0px 5px;">130</li><li style="box-sizing: border-box; padding: 0px 5px;">131</li><li style="box-sizing: border-box; padding: 0px 5px;">132</li><li style="box-sizing: border-box; padding: 0px 5px;">133</li><li style="box-sizing: border-box; padding: 0px 5px;">134</li><li style="box-sizing: border-box; padding: 0px 5px;">135</li><li style="box-sizing: border-box; padding: 0px 5px;">136</li><li style="box-sizing: border-box; padding: 0px 5px;">137</li><li style="box-sizing: border-box; padding: 0px 5px;">138</li><li style="box-sizing: border-box; padding: 0px 5px;">139</li><li style="box-sizing: border-box; padding: 0px 5px;">140</li><li style="box-sizing: border-box; padding: 0px 5px;">141</li><li style="box-sizing: border-box; padding: 0px 5px;">142</li><li style="box-sizing: border-box; padding: 0px 5px;">143</li><li style="box-sizing: border-box; padding: 0px 5px;">144</li><li style="box-sizing: border-box; padding: 0px 5px;">145</li><li style="box-sizing: border-box; padding: 0px 5px;">146</li><li style="box-sizing: border-box; padding: 0px 5px;">147</li><li style="box-sizing: border-box; padding: 0px 5px;">148</li><li style="box-sizing: border-box; padding: 0px 5px;">149</li><li style="box-sizing: border-box; padding: 0px 5px;">150</li><li style="box-sizing: border-box; padding: 0px 5px;">151</li><li style="box-sizing: border-box; padding: 0px 5px;">152</li><li style="box-sizing: border-box; padding: 0px 5px;">153</li><li style="box-sizing: border-box; padding: 0px 5px;">154</li><li style="box-sizing: border-box; padding: 0px 5px;">155</li><li style="box-sizing: border-box; padding: 0px 5px;">156</li><li style="box-sizing: border-box; padding: 0px 5px;">157</li><li style="box-sizing: border-box; padding: 0px 5px;">158</li><li style="box-sizing: border-box; padding: 0px 5px;">159</li><li style="box-sizing: border-box; padding: 0px 5px;">160</li><li style="box-sizing: border-box; padding: 0px 5px;">161</li><li style="box-sizing: border-box; padding: 0px 5px;">162</li><li style="box-sizing: border-box; padding: 0px 5px;">163</li><li style="box-sizing: border-box; padding: 0px 5px;">164</li><li style="box-sizing: border-box; padding: 0px 5px;">165</li><li style="box-sizing: border-box; padding: 0px 5px;">166</li><li style="box-sizing: border-box; padding: 0px 5px;">167</li><li style="box-sizing: border-box; padding: 0px 5px;">168</li><li style="box-sizing: border-box; padding: 0px 5px;">169</li><li style="box-sizing: border-box; padding: 0px 5px;">170</li><li style="box-sizing: border-box; padding: 0px 5px;">171</li><li style="box-sizing: border-box; padding: 0px 5px;">172</li><li style="box-sizing: border-box; padding: 0px 5px;">173</li><li style="box-sizing: border-box; padding: 0px 5px;">174</li><li style="box-sizing: border-box; padding: 0px 5px;">175</li><li style="box-sizing: border-box; padding: 0px 5px;">176</li><li style="box-sizing: border-box; padding: 0px 5px;">177</li><li style="box-sizing: border-box; padding: 0px 5px;">178</li><li style="box-sizing: border-box; padding: 0px 5px;">179</li><li style="box-sizing: border-box; padding: 0px 5px;">180</li><li style="box-sizing: border-box; padding: 0px 5px;">181</li><li style="box-sizing: border-box; padding: 0px 5px;">182</li><li style="box-sizing: border-box; padding: 0px 5px;">183</li><li style="box-sizing: border-box; padding: 0px 5px;">184</li><li style="box-sizing: border-box; padding: 0px 5px;">185</li><li style="box-sizing: border-box; padding: 0px 5px;">186</li><li style="box-sizing: border-box; padding: 0px 5px;">187</li><li style="box-sizing: border-box; padding: 0px 5px;">188</li><li style="box-sizing: border-box; padding: 0px 5px;">189</li><li style="box-sizing: border-box; padding: 0px 5px;">190</li><li style="box-sizing: border-box; padding: 0px 5px;">191</li><li style="box-sizing: border-box; padding: 0px 5px;">192</li><li style="box-sizing: border-box; padding: 0px 5px;">193</li><li style="box-sizing: border-box; padding: 0px 5px;">194</li><li style="box-sizing: border-box; padding: 0px 5px;">195</li><li style="box-sizing: border-box; padding: 0px 5px;">196</li><li style="box-sizing: border-box; padding: 0px 5px;">197</li><li style="box-sizing: border-box; padding: 0px 5px;">198</li><li style="box-sizing: border-box; padding: 0px 5px;">199</li><li style="box-sizing: border-box; padding: 0px 5px;">200</li><li style="box-sizing: border-box; padding: 0px 5px;">201</li><li style="box-sizing: border-box; padding: 0px 5px;">202</li><li style="box-sizing: border-box; padding: 0px 5px;">203</li><li style="box-sizing: border-box; padding: 0px 5px;">204</li><li style="box-sizing: border-box; padding: 0px 5px;">205</li><li style="box-sizing: border-box; padding: 0px 5px;">206</li><li style="box-sizing: border-box; padding: 0px 5px;">207</li><li style="box-sizing: border-box; padding: 0px 5px;">208</li><li style="box-sizing: border-box; padding: 0px 5px;">209</li><li style="box-sizing: border-box; padding: 0px 5px;">210</li><li style="box-sizing: border-box; padding: 0px 5px;">211</li><li style="box-sizing: border-box; padding: 0px 5px;">212</li><li style="box-sizing: border-box; padding: 0px 5px;">213</li><li style="box-sizing: border-box; padding: 0px 5px;">214</li><li style="box-sizing: border-box; padding: 0px 5px;">215</li><li style="box-sizing: border-box; padding: 0px 5px;">216</li><li style="box-sizing: border-box; padding: 0px 5px;">217</li><li style="box-sizing: border-box; padding: 0px 5px;">218</li><li style="box-sizing: border-box; padding: 0px 5px;">219</li><li style="box-sizing: border-box; padding: 0px 5px;">220</li><li style="box-sizing: border-box; padding: 0px 5px;">221</li><li style="box-sizing: border-box; padding: 0px 5px;">222</li><li style="box-sizing: border-box; padding: 0px 5px;">223</li><li style="box-sizing: border-box; padding: 0px 5px;">224</li><li style="box-sizing: border-box; padding: 0px 5px;">225</li><li style="box-sizing: border-box; padding: 0px 5px;">226</li><li style="box-sizing: border-box; padding: 0px 5px;">227</li><li style="box-sizing: border-box; padding: 0px 5px;">228</li><li style="box-sizing: border-box; padding: 0px 5px;">229</li><li style="box-sizing: border-box; padding: 0px 5px;">230</li><li style="box-sizing: border-box; padding: 0px 5px;">231</li><li style="box-sizing: border-box; padding: 0px 5px;">232</li><li style="box-sizing: border-box; padding: 0px 5px;">233</li><li style="box-sizing: border-box; padding: 0px 5px;">234</li><li style="box-sizing: border-box; padding: 0px 5px;">235</li><li style="box-sizing: border-box; padding: 0px 5px;">236</li><li style="box-sizing: border-box; padding: 0px 5px;">237</li><li style="box-sizing: border-box; padding: 0px 5px;">238</li><li style="box-sizing: border-box; padding: 0px 5px;">239</li><li style="box-sizing: border-box; padding: 0px 5px;">240</li><li style="box-sizing: border-box; padding: 0px 5px;">241</li><li style="box-sizing: border-box; padding: 0px 5px;">242</li><li style="box-sizing: border-box; padding: 0px 5px;">243</li><li style="box-sizing: border-box; padding: 0px 5px;">244</li><li style="box-sizing: border-box; padding: 0px 5px;">245</li><li style="box-sizing: border-box; padding: 0px 5px;">246</li><li style="box-sizing: border-box; padding: 0px 5px;">247</li><li style="box-sizing: border-box; padding: 0px 5px;">248</li><li style="box-sizing: border-box; padding: 0px 5px;">249</li><li style="box-sizing: border-box; padding: 0px 5px;">250</li><li style="box-sizing: border-box; padding: 0px 5px;">251</li><li style="box-sizing: border-box; padding: 0px 5px;">252</li><li style="box-sizing: border-box; padding: 0px 5px;">253</li><li style="box-sizing: border-box; padding: 0px 5px;">254</li><li style="box-sizing: border-box; padding: 0px 5px;">255</li><li style="box-sizing: border-box; padding: 0px 5px;">256</li><li style="box-sizing: border-box; padding: 0px 5px;">257</li><li style="box-sizing: border-box; padding: 0px 5px;">258</li><li style="box-sizing: border-box; padding: 0px 5px;">259</li><li style="box-sizing: border-box; padding: 0px 5px;">260</li><li style="box-sizing: border-box; padding: 0px 5px;">261</li><li style="box-sizing: border-box; padding: 0px 5px;">262</li><li style="box-sizing: border-box; padding: 0px 5px;">263</li><li style="box-sizing: border-box; padding: 0px 5px;">264</li><li style="box-sizing: border-box; padding: 0px 5px;">265</li><li style="box-sizing: border-box; padding: 0px 5px;">266</li><li style="box-sizing: border-box; padding: 0px 5px;">267</li><li style="box-sizing: border-box; padding: 0px 5px;">268</li><li style="box-sizing: border-box; padding: 0px 5px;">269</li><li style="box-sizing: border-box; padding: 0px 5px;">270</li><li style="box-sizing: border-box; padding: 0px 5px;">271</li><li style="box-sizing: border-box; padding: 0px 5px;">272</li><li style="box-sizing: border-box; padding: 0px 5px;">273</li><li style="box-sizing: border-box; padding: 0px 5px;">274</li><li style="box-sizing: border-box; padding: 0px 5px;">275</li><li style="box-sizing: border-box; padding: 0px 5px;">276</li><li style="box-sizing: border-box; padding: 0px 5px;">277</li><li style="box-sizing: border-box; padding: 0px 5px;">278</li><li style="box-sizing: border-box; padding: 0px 5px;">279</li><li style="box-sizing: border-box; padding: 0px 5px;">280</li><li style="box-sizing: border-box; padding: 0px 5px;">281</li><li style="box-sizing: border-box; padding: 0px 5px;">282</li><li style="box-sizing: border-box; padding: 0px 5px;">283</li><li style="box-sizing: border-box; padding: 0px 5px;">284</li><li style="box-sizing: border-box; padding: 0px 5px;">285</li><li style="box-sizing: border-box; padding: 0px 5px;">286</li><li style="box-sizing: border-box; padding: 0px 5px;">287</li><li style="box-sizing: border-box; padding: 0px 5px;">288</li><li style="box-sizing: border-box; padding: 0px 5px;">289</li><li style="box-sizing: border-box; padding: 0px 5px;">290</li><li style="box-sizing: border-box; padding: 0px 5px;">291</li><li style="box-sizing: border-box; padding: 0px 5px;">292</li><li style="box-sizing: border-box; padding: 0px 5px;">293</li><li style="box-sizing: border-box; padding: 0px 5px;">294</li><li style="box-sizing: border-box; padding: 0px 5px;">295</li><li style="box-sizing: border-box; padding: 0px 5px;">296</li><li style="box-sizing: border-box; padding: 0px 5px;">297</li><li style="box-sizing: border-box; padding: 0px 5px;">298</li><li style="box-sizing: border-box; padding: 0px 5px;">299</li><li style="box-sizing: border-box; padding: 0px 5px;">300</li><li style="box-sizing: border-box; padding: 0px 5px;">301</li><li style="box-sizing: border-box; padding: 0px 5px;">302</li><li style="box-sizing: border-box; padding: 0px 5px;">303</li><li style="box-sizing: border-box; padding: 0px 5px;">304</li><li style="box-sizing: border-box; padding: 0px 5px;">305</li><li style="box-sizing: border-box; padding: 0px 5px;">306</li><li style="box-sizing: border-box; padding: 0px 5px;">307</li><li style="box-sizing: border-box; padding: 0px 5px;">308</li><li style="box-sizing: border-box; padding: 0px 5px;">309</li><li style="box-sizing: border-box; padding: 0px 5px;">310</li><li style="box-sizing: border-box; padding: 0px 5px;">311</li><li style="box-sizing: border-box; padding: 0px 5px;">312</li><li style="box-sizing: border-box; padding: 0px 5px;">313</li><li style="box-sizing: border-box; padding: 0px 5px;">314</li><li style="box-sizing: border-box; padding: 0px 5px;">315</li><li style="box-sizing: border-box; padding: 0px 5px;">316</li><li style="box-sizing: border-box; padding: 0px 5px;">317</li><li style="box-sizing: border-box; padding: 0px 5px;">318</li><li style="box-sizing: border-box; padding: 0px 5px;">319</li><li style="box-sizing: border-box; padding: 0px 5px;">320</li><li style="box-sizing: border-box; padding: 0px 5px;">321</li><li style="box-sizing: border-box; padding: 0px 5px;">322</li><li style="box-sizing: border-box; padding: 0px 5px;">323</li><li style="box-sizing: border-box; padding: 0px 5px;">324</li><li style="box-sizing: border-box; padding: 0px 5px;">325</li><li style="box-sizing: border-box; padding: 0px 5px;">326</li><li style="box-sizing: border-box; padding: 0px 5px;">327</li><li style="box-sizing: border-box; padding: 0px 5px;">328</li><li style="box-sizing: border-box; padding: 0px 5px;">329</li><li style="box-sizing: border-box; padding: 0px 5px;">330</li><li style="box-sizing: border-box; padding: 0px 5px;">331</li><li style="box-sizing: border-box; padding: 0px 5px;">332</li><li style="box-sizing: border-box; padding: 0px 5px;">333</li><li style="box-sizing: border-box; padding: 0px 5px;">334</li><li style="box-sizing: border-box; padding: 0px 5px;">335</li><li style="box-sizing: border-box; padding: 0px 5px;">336</li><li style="box-sizing: border-box; padding: 0px 5px;">337</li><li style="box-sizing: border-box; padding: 0px 5px;">338</li><li style="box-sizing: border-box; padding: 0px 5px;">339</li><li style="box-sizing: border-box; padding: 0px 5px;">340</li><li style="box-sizing: border-box; padding: 0px 5px;">341</li><li style="box-sizing: border-box; padding: 0px 5px;">342</li><li style="box-sizing: border-box; padding: 0px 5px;">343</li><li style="box-sizing: border-box; padding: 0px 5px;">344</li><li style="box-sizing: border-box; padding: 0px 5px;">345</li><li style="box-sizing: border-box; padding: 0px 5px;">346</li><li style="box-sizing: border-box; padding: 0px 5px;">347</li><li style="box-sizing: border-box; padding: 0px 5px;">348</li><li style="box-sizing: border-box; padding: 0px 5px;">349</li><li style="box-sizing: border-box; padding: 0px 5px;">350</li><li style="box-sizing: border-box; padding: 0px 5px;">351</li><li style="box-sizing: border-box; padding: 0px 5px;">352</li><li style="box-sizing: border-box; padding: 0px 5px;">353</li><li style="box-sizing: border-box; padding: 0px 5px;">354</li><li style="box-sizing: border-box; padding: 0px 5px;">355</li><li style="box-sizing: border-box; padding: 0px 5px;">356</li><li style="box-sizing: border-box; padding: 0px 5px;">357</li><li style="box-sizing: border-box; padding: 0px 5px;">358</li><li style="box-sizing: border-box; padding: 0px 5px;">359</li><li style="box-sizing: border-box; padding: 0px 5px;">360</li><li style="box-sizing: border-box; padding: 0px 5px;">361</li><li style="box-sizing: border-box; padding: 0px 5px;">362</li><li style="box-sizing: border-box; padding: 0px 5px;">363</li><li style="box-sizing: border-box; padding: 0px 5px;">364</li><li style="box-sizing: border-box; padding: 0px 5px;">365</li><li style="box-sizing: border-box; padding: 0px 5px;">366</li><li style="box-sizing: border-box; padding: 0px 5px;">367</li><li style="box-sizing: border-box; padding: 0px 5px;">368</li><li style="box-sizing: border-box; padding: 0px 5px;">369</li><li style="box-sizing: border-box; padding: 0px 5px;">370</li><li style="box-sizing: border-box; padding: 0px 5px;">371</li><li style="box-sizing: border-box; padding: 0px 5px;">372</li><li style="box-sizing: border-box; padding: 0px 5px;">373</li><li style="box-sizing: border-box; padding: 0px 5px;">374</li><li style="box-sizing: border-box; padding: 0px 5px;">375</li><li style="box-sizing: border-box; padding: 0px 5px;">376</li><li style="box-sizing: border-box; padding: 0px 5px;">377</li><li style="box-sizing: border-box; padding: 0px 5px;">378</li><li style="box-sizing: border-box; padding: 0px 5px;">379</li><li style="box-sizing: border-box; padding: 0px 5px;">380</li><li style="box-sizing: border-box; padding: 0px 5px;">381</li><li style="box-sizing: border-box; padding: 0px 5px;">382</li><li style="box-sizing: border-box; padding: 0px 5px;">383</li><li style="box-sizing: border-box; padding: 0px 5px;">384</li><li style="box-sizing: border-box; padding: 0px 5px;">385</li><li style="box-sizing: border-box; padding: 0px 5px;">386</li><li style="box-sizing: border-box; padding: 0px 5px;">387</li></ul>

update_parameters();

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">l657~l686
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">void</span> AttitudeEstimatorQ::update_parameters(<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span> force)
{
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span> updated = force;

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (!updated) {
        orb_check(_params_sub, &updated);<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//查看参数是否更新;</span>
    }

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (updated) {<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//获取新参数;</span>
        parameter_update_s param_update;
        orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);

        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);
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span> mag_decl_deg = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0f</span>;
        param_get(_params_handles.mag_decl, &mag_decl_deg);
        update_mag_declination(math::radians(mag_decl_deg));
        int32_t mag_decl_auto_int;
        param_get(_params_handles.mag_decl_auto, &mag_decl_auto_int);
        _mag_decl_auto = mag_decl_auto_int != <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//自动磁偏角校正;</span>
        int32_t acc_comp_int;
        param_get(_params_handles.acc_comp, &acc_comp_int);
        _acc_comp = acc_comp_int != <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;
        param_get(_params_handles.bias_max, &_bias_max);<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//陀螺仪偏差上限;</span>
        param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//振动警告阈值;</span>
        param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
    }
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li></ul>

init();

k 为导航坐标系（NED）的 z 轴（D）在机体坐标系中的表示;

导航系中，D正方向朝下;
i 为导航坐标系（NED）的 x 轴（N）在机体坐标系;

i = (_mag - k * (_mag * k)); //施密特正交化;

$β 2 = α 2 - ( α 2 , β 1 ) ( β 1 , β 1 ) \cdot β 1$

//因向量 k 已归一化，故分母等于1;
j 为导航坐标系（NED）的 y 轴（E）在机体坐标系;

j = k % i //叉乘求正交向量;

$∣ ∣ ∣ ∣ ∣ i k [0] i [0] j k [1] i [1] k k [2] i [2] ∣ ∣ ∣ ∣ ∣ = ⎡ ⎣ ⎢ k [1] i [2] - i [1] k [2] i [0] k [2] - k [0] i [2] k [0] i [1] - i [0] k [1] ⎤ ⎦ ⎥$
构造旋转矩阵 R

R.set_row(0, i); R.set_row(1, j); R.set_row(2, k);

$R = ⎡ ⎣ ⎢ i j k ⎤ ⎦ ⎥$
转换为四元数 q ，根据设定校正磁偏，归一化;

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">l688~l728
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span> AttitudeEstimatorQ::init()
{
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Rotation matrix can be easily constructed from acceleration and mag field vectors</span>
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// 'k' is Earth Z axis (Down) unit vector in body frame</span>
    Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> k = -_accel;
    k.normalize();

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'</span>
    Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> i = (_mag - k * (_mag * k));
    i.normalize();

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'</span>
    Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> j = k % i;

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Fill rotation matrix</span>
    Matrix<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> R;
    R.set_row(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>, i);
    R.set_row(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>, j);
    R.set_row(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>, k);

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Convert to quaternion</span>
    _q.from_dcm(R);

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Compensate for magnetic declination</span>
    Quaternion decl_rotation;
    decl_rotation.from_yaw(_mag_decl);
    _q = decl_rotation * _q;

    _q.normalize();

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (PX4_ISFINITE(_q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>)) && PX4_ISFINITE(_q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>)) &&
        PX4_ISFINITE(_q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>)) && PX4_ISFINITE(_q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>)) &&
        _q.length() > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.95f</span> && _q.length() < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1.05f</span>) {
        _inited = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">true</span>;

    } <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">else</span> {
        _inited = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;
    }

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> _inited;
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li><li style="box-sizing: border-box; padding: 0px 5px;">41</li><li style="box-sizing: border-box; padding: 0px 5px;">42</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li><li style="box-sizing: border-box; padding: 0px 5px;">41</li><li style="box-sizing: border-box; padding: 0px 5px;">42</li></ul>

update();

init();//执行一次;

由加速度计和磁力计初始化旋转矩阵和四元数;
mag_earth = _q.conjugate(_mag);

不使用外部航向，或外部航向异常时，采用磁力计校准;
将磁力计读数从机体坐标系转换到导航坐标系;

$R n b *_m a g$
mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);

将磁力计的读数由向量换算到角度; 该角度减去磁偏，得到磁场偏差;
_mag_decl 由GPS得到;
**atan2f: 2 表示两个输入参数; 支持四象限内角度换算; 输出弧度值;
**_wrap_pi: 将(0~2pi)的角度映射到(-pi~pi);
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;

将磁场偏差转换到机体坐标系，并乘以其对应权重;

$R b n ⎡ ⎣ ⎢ 00 - m a g_e r r ⎤ ⎦ ⎥ \cdot_w m a g$
_q.normalize();

四元数归一化;
这里的归一化用于校正update_mag_declination后的偏差。
corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;

向量 k 是重力加速度向量(0,0,1)转换到机体坐标系;
_accel 是加速度计的读数;
_pos_acc 是由位置估计(GPS) 微分得到的加速度;
_accel - _pos_acc 表示飞行器加速度向量减去其水平分量;
k 与 (_accel - _pos_acc)叉乘，得到偏差;

$k = R b n ⎡ ⎣ ⎢ 001 ⎤ ⎦ ⎥$

$e = k \times [机体测量重力加速度]$
_gyro_bias += corr * (_w_gyro_bias * dt);

得到陀螺仪修正值
此处修正值为 mahony 滤波的 pi 控制器的积分部分;
因为 _gyro_bias 不归零，故不断累积;

$g y r o_b i a s + = [M a g * w m a g + A c c * w a c c] * w g y r o * d t$
_rates = _gyro + _gyro_bias;

_rates 表示角速度;

$ω = ω g + δ$
corr += _rates;

这里的 corr = (Ea+Em) + (Ea+Em)*dt + gyro;
个人认为这里的 corr 才是更新后的角速度;
_q += _q.derivative(corr) * dt;

更新四元数，derivative为求导函数，使用一阶龙格库塔求导。

$q ˙ = 1 2 q ω$

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">l730~l817
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">bool</span> AttitudeEstimatorQ::update(<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span> dt)
{
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (!_inited) {

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (!_data_good) {
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;
        }

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> init();
    }

    Quaternion q_last = _q;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//保存上一状态，以便恢复;</span>

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Angular rate of correction</span>
    Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> corr;<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//初始化元素为0;</span>

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//_ext_hdg_good表示外部航向数据可用;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_ext_hdg_mode > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span> && _ext_hdg_good) {
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_ext_hdg_mode == <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>) {
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Vision heading correction</span>
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//视觉航向校正;</span>
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Project heading to global frame and extract XY component</span>
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//将航向投影到导航坐标系，提取XY分量;</span>
            Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> vision_hdg_earth = _q.conjugate(_vision_hdg);
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span> vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>), vision_hdg_earth(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>)));
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Project correction to body frame</span>
            corr += _q.conjugate_inversed(Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>>(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0f</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0f</span>, -vision_hdg_err)) * _w_ext_hdg;
        }

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_ext_hdg_mode == <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>) {
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Mocap heading correction</span>
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Project heading to global frame and extract XY component</span>
            Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> mocap_hdg_earth = _q.conjugate(_mocap_hdg);
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span> mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>), mocap_hdg_earth(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>)));
            <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Project correction to body frame</span>
            corr += _q.conjugate_inversed(Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>>(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0f</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0f</span>, -mocap_hdg_err)) * _w_ext_hdg;
        }
    }

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (_ext_hdg_mode == <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>  || !_ext_hdg_good) {
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Magnetometer correction</span>
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Project mag field vector to global frame and extract XY component</span>
        Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> mag_earth = _q.conjugate(_mag);
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span> mag_err = _wrap_pi(atan2f(mag_earth(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>), mag_earth(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>)) - _mag_decl);
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Project magnetometer correction to body frame</span>
        corr += _q.conjugate_inversed(Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>>(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0f</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0f</span>, -mag_err)) * _w_mag;
    }

    _q.normalize();

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Accelerometer correction</span>
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Project 'k' unit vector of earth frame to body frame</span>
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));</span>
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Optimized version with dropped zeros</span>
    Vector<<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>> k(
        <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2.0f</span> * (_q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>) * _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>) - _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) * _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>)),
        <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2.0f</span> * (_q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>) * _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>) + _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) * _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>)),
        (_q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) * _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) - _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>) * _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>) - _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>) * _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>) + _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>) * _q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>))
    );

    corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Gyro bias estimation</span>
    _gyro_bias += corr * (_w_gyro_bias * dt);

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">for</span> (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> i = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>; i < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>; i++) {<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//陀螺仪最大偏差上限;</span>
        _gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
    }

    _rates = _gyro + _gyro_bias;

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Feed forward gyro</span>
    corr += _rates;

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Apply correction to state</span>
    _q += _q.derivative(corr) * dt;

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Normalize quaternion</span>
    _q.normalize();
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//判断四元数是否发散，若发散，则沿用之前的四元数;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (!(PX4_ISFINITE(_q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>)) && PX4_ISFINITE(_q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>)) &&
          PX4_ISFINITE(_q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>)) && PX4_ISFINITE(_q(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>)))) {
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Reset quaternion to last good state</span>
        _q = q_last;
        _rates.zero();
        _gyro_bias.zero();
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;
    }

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">true</span>;
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li><li style="box-sizing: border-box; padding: 0px 5px;">41</li><li style="box-sizing: border-box; padding: 0px 5px;">42</li><li style="box-sizing: border-box; padding: 0px 5px;">43</li><li style="box-sizing: border-box; padding: 0px 5px;">44</li><li style="box-sizing: border-box; padding: 0px 5px;">45</li><li style="box-sizing: border-box; padding: 0px 5px;">46</li><li style="box-sizing: border-box; padding: 0px 5px;">47</li><li style="box-sizing: border-box; padding: 0px 5px;">48</li><li style="box-sizing: border-box; padding: 0px 5px;">49</li><li style="box-sizing: border-box; padding: 0px 5px;">50</li><li style="box-sizing: border-box; padding: 0px 5px;">51</li><li style="box-sizing: border-box; padding: 0px 5px;">52</li><li style="box-sizing: border-box; padding: 0px 5px;">53</li><li style="box-sizing: border-box; padding: 0px 5px;">54</li><li style="box-sizing: border-box; padding: 0px 5px;">55</li><li style="box-sizing: border-box; padding: 0px 5px;">56</li><li style="box-sizing: border-box; padding: 0px 5px;">57</li><li style="box-sizing: border-box; padding: 0px 5px;">58</li><li style="box-sizing: border-box; padding: 0px 5px;">59</li><li style="box-sizing: border-box; padding: 0px 5px;">60</li><li style="box-sizing: border-box; padding: 0px 5px;">61</li><li style="box-sizing: border-box; padding: 0px 5px;">62</li><li style="box-sizing: border-box; padding: 0px 5px;">63</li><li style="box-sizing: border-box; padding: 0px 5px;">64</li><li style="box-sizing: border-box; padding: 0px 5px;">65</li><li style="box-sizing: border-box; padding: 0px 5px;">66</li><li style="box-sizing: border-box; padding: 0px 5px;">67</li><li style="box-sizing: border-box; padding: 0px 5px;">68</li><li style="box-sizing: border-box; padding: 0px 5px;">69</li><li style="box-sizing: border-box; padding: 0px 5px;">70</li><li style="box-sizing: border-box; padding: 0px 5px;">71</li><li style="box-sizing: border-box; padding: 0px 5px;">72</li><li style="box-sizing: border-box; padding: 0px 5px;">73</li><li style="box-sizing: border-box; padding: 0px 5px;">74</li><li style="box-sizing: border-box; padding: 0px 5px;">75</li><li style="box-sizing: border-box; padding: 0px 5px;">76</li><li style="box-sizing: border-box; padding: 0px 5px;">77</li><li style="box-sizing: border-box; padding: 0px 5px;">78</li><li style="box-sizing: border-box; padding: 0px 5px;">79</li><li style="box-sizing: border-box; padding: 0px 5px;">80</li><li style="box-sizing: border-box; padding: 0px 5px;">81</li><li style="box-sizing: border-box; padding: 0px 5px;">82</li><li style="box-sizing: border-box; padding: 0px 5px;">83</li><li style="box-sizing: border-box; padding: 0px 5px;">84</li><li style="box-sizing: border-box; padding: 0px 5px;">85</li><li style="box-sizing: border-box; padding: 0px 5px;">86</li><li style="box-sizing: border-box; padding: 0px 5px;">87</li><li style="box-sizing: border-box; padding: 0px 5px;">88</li><li style="box-sizing: border-box; padding: 0px 5px;">89</li><li style="box-sizing: border-box; padding: 0px 5px;">90</li><li style="box-sizing: border-box; padding: 0px 5px;">91</li><li style="box-sizing: border-box; padding: 0px 5px;">92</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li><li style="box-sizing: border-box; padding: 0px 5px;">41</li><li style="box-sizing: border-box; padding: 0px 5px;">42</li><li style="box-sizing: border-box; padding: 0px 5px;">43</li><li style="box-sizing: border-box; padding: 0px 5px;">44</li><li style="box-sizing: border-box; padding: 0px 5px;">45</li><li style="box-sizing: border-box; padding: 0px 5px;">46</li><li style="box-sizing: border-box; padding: 0px 5px;">47</li><li style="box-sizing: border-box; padding: 0px 5px;">48</li><li style="box-sizing: border-box; padding: 0px 5px;">49</li><li style="box-sizing: border-box; padding: 0px 5px;">50</li><li style="box-sizing: border-box; padding: 0px 5px;">51</li><li style="box-sizing: border-box; padding: 0px 5px;">52</li><li style="box-sizing: border-box; padding: 0px 5px;">53</li><li style="box-sizing: border-box; padding: 0px 5px;">54</li><li style="box-sizing: border-box; padding: 0px 5px;">55</li><li style="box-sizing: border-box; padding: 0px 5px;">56</li><li style="box-sizing: border-box; padding: 0px 5px;">57</li><li style="box-sizing: border-box; padding: 0px 5px;">58</li><li style="box-sizing: border-box; padding: 0px 5px;">59</li><li style="box-sizing: border-box; padding: 0px 5px;">60</li><li style="box-sizing: border-box; padding: 0px 5px;">61</li><li style="box-sizing: border-box; padding: 0px 5px;">62</li><li style="box-sizing: border-box; padding: 0px 5px;">63</li><li style="box-sizing: border-box; padding: 0px 5px;">64</li><li style="box-sizing: border-box; padding: 0px 5px;">65</li><li style="box-sizing: border-box; padding: 0px 5px;">66</li><li style="box-sizing: border-box; padding: 0px 5px;">67</li><li style="box-sizing: border-box; padding: 0px 5px;">68</li><li style="box-sizing: border-box; padding: 0px 5px;">69</li><li style="box-sizing: border-box; padding: 0px 5px;">70</li><li style="box-sizing: border-box; padding: 0px 5px;">71</li><li style="box-sizing: border-box; padding: 0px 5px;">72</li><li style="box-sizing: border-box; padding: 0px 5px;">73</li><li style="box-sizing: border-box; padding: 0px 5px;">74</li><li style="box-sizing: border-box; padding: 0px 5px;">75</li><li style="box-sizing: border-box; padding: 0px 5px;">76</li><li style="box-sizing: border-box; padding: 0px 5px;">77</li><li style="box-sizing: border-box; padding: 0px 5px;">78</li><li style="box-sizing: border-box; padding: 0px 5px;">79</li><li style="box-sizing: border-box; padding: 0px 5px;">80</li><li style="box-sizing: border-box; padding: 0px 5px;">81</li><li style="box-sizing: border-box; padding: 0px 5px;">82</li><li style="box-sizing: border-box; padding: 0px 5px;">83</li><li style="box-sizing: border-box; padding: 0px 5px;">84</li><li style="box-sizing: border-box; padding: 0px 5px;">85</li><li style="box-sizing: border-box; padding: 0px 5px;">86</li><li style="box-sizing: border-box; padding: 0px 5px;">87</li><li style="box-sizing: border-box; padding: 0px 5px;">88</li><li style="box-sizing: border-box; padding: 0px 5px;">89</li><li style="box-sizing: border-box; padding: 0px 5px;">90</li><li style="box-sizing: border-box; padding: 0px 5px;">91</li><li style="box-sizing: border-box; padding: 0px 5px;">92</li></ul>

update_mag_declination();

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">l819~l832
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">void</span> AttitudeEstimatorQ::update_mag_declination(<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span> new_declination)
{
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Apply initial declination or trivial rotations without changing estimation</span>
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//忽略微小旋转;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (!_inited || fabsf(new_declination - _mag_decl) < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0001f</span>) {
        _mag_decl = new_declination;

    } <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">else</span> {
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">// Immediately rotate current estimation to avoid gyro bias growth</span>
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//磁偏超过一定值后，校正姿态;</span>
        Quaternion decl_rotation;
        decl_rotation.from_yaw(new_declination - _mag_decl);
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//由磁偏角度转化为四元数;</span>
        _q = decl_rotation * _q;
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//四元数相乘表示角度相加;</span>
        _mag_decl = new_declination;
    }
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li></ul>

attitude_estimator_q_main();

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;">l834~l890
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> attitude_estimator_q_main(<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> argc, <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">char</span> *argv[])
{<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//外部调用接口;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (argc < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>) {
        warnx(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"usage: attitude_estimator_q {start|stop|status}"</span>);
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;
    }

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (!<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">strcmp</span>(argv[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>], <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"start"</span>)) {

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (attitude_estimator_q::instance != <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">nullptr</span>) {
            warnx(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"already running"</span>);
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;
        }
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//实例化，instance;</span>
        attitude_estimator_q::instance = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">new</span> AttitudeEstimatorQ;

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (attitude_estimator_q::instance == <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">nullptr</span>) {
            warnx(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"alloc failed"</span>);
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;
        }

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (OK != attitude_estimator_q::instance->start()) {
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">delete</span> attitude_estimator_q::instance;
            attitude_estimator_q::instance = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">nullptr</span>;
            warnx(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"start failed"</span>);
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;
        }

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;
    }

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (!<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">strcmp</span>(argv[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>], <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"stop"</span>)) {
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (attitude_estimator_q::instance == <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">nullptr</span>) {
            warnx(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"not running"</span>);
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;
        }
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//删除实例化对象，指针置空;</span>
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">delete</span> attitude_estimator_q::instance;
        attitude_estimator_q::instance = <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">nullptr</span>;
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;
    }
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//打印当前姿态信息;</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (!<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">strcmp</span>(argv[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>], <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"status"</span>)) {
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (attitude_estimator_q::instance) {
            attitude_estimator_q::instance->print();
            warnx(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"running"</span>);
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;

        } <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">else</span> {
            warnx(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"not running"</span>);
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;
        }
    }

    warnx(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">"unrecognized command"</span>);
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; opacity: 0.121905; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li><li style="box-sizing: border-box; padding: 0px 5px;">41</li><li style="box-sizing: border-box; padding: 0px 5px;">42</li><li style="box-sizing: border-box; padding: 0px 5px;">43</li><li style="box-sizing: border-box; padding: 0px 5px;">44</li><li style="box-sizing: border-box; padding: 0px 5px;">45</li><li style="box-sizing: border-box; padding: 0px 5px;">46</li><li style="box-sizing: border-box; padding: 0px 5px;">47</li><li style="box-sizing: border-box; padding: 0px 5px;">48</li><li style="box-sizing: border-box; padding: 0px 5px;">49</li><li style="box-sizing: border-box; padding: 0px 5px;">50</li><li style="box-sizing: border-box; padding: 0px 5px;">51</li><li style="box-sizing: border-box; padding: 0px 5px;">52</li><li style="box-sizing: border-box; padding: 0px 5px;">53</li><li style="box-sizing: border-box; padding: 0px 5px;">54</li><li style="box-sizing: border-box; padding: 0px 5px;">55</li><li style="box-sizing: border-box; padding: 0px 5px;">56</li><li style="box-sizing: border-box; padding: 0px 5px;">57</li><li style="box-sizing: border-box; padding: 0px 5px;">58</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li><li style="box-sizing: border-box; padding: 0px 5px;">41</li><li style="box-sizing: border-box; padding: 0px 5px;">42</li><li style="box-sizing: border-box; padding: 0px 5px;">43</li><li style="box-sizing: border-box; padding: 0px 5px;">44</li><li style="box-sizing: border-box; padding: 0px 5px;">45</li><li style="box-sizing: border-box; padding: 0px 5px;">46</li><li style="box-sizing: border-box; padding: 0px 5px;">47</li><li style="box-sizing: border-box; padding: 0px 5px;">48</li><li style="box-sizing: border-box; padding: 0px 5px;">49</li><li style="box-sizing: border-box; padding: 0px 5px;">50</li><li style="box-sizing: border-box; padding: 0px 5px;">51</li><li style="box-sizing: border-box; padding: 0px 5px;">52</li><li style="box-sizing: border-box; padding: 0px 5px;">53</li><li style="box-sizing: border-box; padding: 0px 5px;">54</li><li style="box-sizing: border-box; padding: 0px 5px;">55</li><li style="box-sizing: border-box; padding: 0px 5px;">56</li><li style="box-sizing: border-box; padding: 0px 5px;">57</li><li style="box-sizing: border-box; padding: 0px 5px;">58</li></ul>

qing101hua

关注

4
点赞
踩
85

收藏

觉得还不错? 一键收藏
4
评论
Mahony 互补滤波

版权声明：本文为博主原创文章，转载请注明，欢迎交流~Mahony互补滤波目录(?)[+]上接【互补滤波器】，继续学习互补滤波。。。。参考： Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs. PX4/Pixhawk
复制链接

扫一扫