Mahony互补滤波

目录(?)[+]

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

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

by luoshi006

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

应用场景
名词解释
- 陀螺仪
- 加速度计
- 磁力计
- 坐标系
- 姿态表示
mahony 滤波原理
- mahony 滤波主要过程
- 预备知识
- 预测
- 校正
  - 加速度计校正
  - 磁力计校正
    - 无 GPS 校准时
    - 有 GPS 校准时
- 更新四元数
源码分析
- 主程序运行流程图
- 函数功能简述
- 源码分析
  - 头文件
  - using
  - namespace attitude_estimator_q
  - 类定义 class AttitudeEstimatorQ
  - 构造函数
  - 析构函数
  - start
  - print
  - task_main_trampoline
  - task_main
  - update_parameters
  - init
  - update
  - update_mag_declination
  - attitude_estimator_q_main

应用场景

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

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

名词解释

陀螺仪

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

加速度计

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

磁力计

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

坐标系

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

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

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

姿态表示

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

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

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

mahony 滤波原理

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

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

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

mahony 滤波主要过程

q^˙=12q^⊗P(Ω¯¯¯+δ)−−−−−−−−(a)

δ=kP⋅e+kI⋅∫e−−−−−−−−(b)

e=v¯×v^−−−−−−−−−−−−(c)

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

PI 调节器中：[20160901更新]

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

预备知识

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

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

Θ˙=Wbω

=⎡⎣⎢100tanθsinϕcosϕsinϕ/cosθtanθcosϕ−sinϕcosϕ/cosθ⎤⎦⎥bω

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

Reb˙=Reb[bω]×

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

q˙be(t)=12⎡⎣⎢⎢⎢⎢0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωz−ωyωx0⎤⎦⎥⎥⎥⎥qbe(t)−−−−−−−（1）

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

预测

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

qbe(k)=qbe(k−1)+q˙be(k)Δt

校正

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

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

加速度计校正

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

y(k)=RCT+RCy(k−1)+TT+RCx(k)

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

Δqaccˆ=Δqacc¯¯¯¯¯¯¯¯||Δqacc¯¯¯¯¯¯¯¯||

计算偏差：

e=Δqaccˆ×v

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

⎡⎣⎢vxvyvz⎤⎦⎥=Cbn⎡⎣⎢001⎤⎦⎥

=⎡⎣⎢2(q1q3−q0q2)2(q2q3+q0q1)q20−q21−q22+q23⎤⎦⎥

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

磁力计校正

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

1. 无 GPS 校准时：

计算误差：

e=Δqmagˆ×w

式中，w 计算过程如下：

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

⎡⎣⎢hxhyhz⎤⎦⎥=Cnb⎡⎣⎢mxmymz⎤⎦⎥

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

bx=h2x+h2y−−−−−−√

bz=hz

再次变换到机体坐标系：

⎡⎣⎢wxwywz⎤⎦⎥=Cbn⎡⎣⎢bx0bz⎤⎦⎥

2. 有 GPS 校准时：

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

⎡⎣⎢wxwywz⎤⎦⎥=Cbn⎡⎣⎢00mag⎤⎦⎥

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

更新四元数

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

e=eacc+emag

由该误差值得到修正值：

δ=KP⋅e+KI⋅∫edt

修正后的角速度值：

ω=ωgyro+δ

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

q˙=f(q,ω)

q(t+T)=q(t)+T⋅f(q,ω)

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

q0(t+T)=q0(t)+T2[−ωxq1(t)−ωyq2(t)−ωzq3(t)]

四元数规范化：

q=q0+q1i+q2j+q3kq20+q21+q22+q23−−−−−−−−−−−−−√

源码分析

该部分源码直接截取 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)⋅β1

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

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

∣∣∣∣∣ik[0]i[0]jk[1]i[1]kk[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=⎡⎣⎢ijk⎤⎦⎥
转换为四元数 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);

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

Rnb∗_mag
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;

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

Rbn⎡⎣⎢00−mag_err⎤⎦⎥⋅_wmag
_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=Rbn⎡⎣⎢001⎤⎦⎥

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

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

gyro_bias+=[Mag∗wmag+Acc∗wacc]∗wgyro∗dt
_rates = _gyro + _gyro_bias;

_rates 表示角速度;

ω=ωg+δ
corr += _rates;

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

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

q˙=12qω

<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>

应用场景
名词解释
- 陀螺仪
- 加速度计
- 磁力计
- 坐标系
- 姿态表示
mahony 滤波原理
- mahony 滤波主要过程
- 预备知识
- 预测
- 校正
  - 加速度计校正
  - 磁力计校正
    - 无 GPS 校准时
    - 有 GPS 校准时
- 更新四元数
源码分析
- 主程序运行流程图
- 函数功能简述
- 源码分析
  - 头文件
  - using
  - namespace attitude_estimator_q
  - 类定义 class AttitudeEstimatorQ
  - 构造函数
  - 析构函数
  - start
  - print
  - task_main_trampoline
  - task_main
  - update_parameters
  - init
  - update
  - update_mag_declination
  - attitude_estimator_q_main

Mahony 互补滤波相关推荐

stm32 MPU6050 姿态解算 Mahony互补滤波算法
文章目录 0.介绍 1,理论分析 1.1 MPU6050 1.2 Mahony算法原理 2,代码实现 1.1 MPU6050初始化及数据读取 1.2 Mahony算法c语言实现 1.3 将代码移植到你 ...
基于RflySim平台的mahony(含磁罗盘)互补滤波在pixhawk仿真及实物实验（带实验数据）
写在前面本案例实验采用RflySim平台,该平台可以高效快速编写代码,使用simulink模型搭建,可以见代码直接生成对应的C代码,并一键将代码烧录Pixhawk中,是一种快速开发平台,RflySi ...
mahony 互补滤波器
by luoshi006 上接[互补滤波器],继续学习互补滤波.... 参考: Keeping a Good Attitude: A Quaternion-Based Orientation Filt ...
对比直立车模控制中的互补滤波、Karlman滤波和参考滤波方案
前几天,有参加智能车竞赛的同学在公众号上询问一个关于直立车模控制中角度融合算法的问题.感觉这是所有初次准备智能车模竞赛的同学都会碰到的问题. 卓大大,请问您可以针对卡尔曼滤波.互补滤波.清华滤波的原理 ...
四旋翼姿态解算——互补滤波算法及理论推导
转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...
【51单片机快速入门指南】4.3.2： MPU6050：一阶互补滤波、二阶互补滤波和卡尔曼滤波获取欧拉角
目录源码 MPU6050_Filter.c MPU6050_Filter.h 使用方法测试程序一阶互补滤波效果二阶互补滤波效果卡尔曼滤波效果总结普中51-单核-A2 STC89C5 ...
STC51单片机15——MPU6050六轴数据融合，互补滤波，时间常数可调，可稳定运行，串口显示角度值
51单片机用模拟IIC的方式读取MPU6050的原始数据,之后经过换算转成三轴加速度和三轴角速度.设定定时器,以固定的频率采集以上得到的数据,并加入互补滤波,去除加速度的噪声以及陀螺仪的零飘.注意,本 ...
陀螺仪与磁力计的姿态融合——互补滤波(续)
大家好,我是学电子的小白白~ 上一篇我们讲了陀螺仪与加速度计的数据融合,本篇文章是互补滤波的续篇,接着讲陀螺仪与磁力计的数据融合. 1)为什么需要磁力计的数据先回忆一下上一篇内容,加速度计是如何修正 ...
MPU6050加速度、角速度的解算以及互补滤波使用
MPU6050加速度.角速度的解算以及互补滤波使用 MPU6050加速度的解算 MPU6050角速度的解算互补滤波使用(数据融合) 数字低通滤波器数字高通滤波器 MPU6050加速度的解算先根据 ...
树莓派pico mpu6050 一阶互补滤波四元数法解算姿态角
micro-python:一阶互补滤波&四元数法代码 2.系统方案 2.1.组成本系统由供电部分, 主控部分, 姿态传感器与通信部份组成 2.2.供电部分电池为一节14500锂电池, 容 ...

Mahony 互补滤波

应用场景

名词解释

陀螺仪

加速度计

磁力计

坐标系

姿态表示

mahony 滤波原理

mahony 滤波主要过程

预备知识

预测

校正

加速度计校正

磁力计校正

1. 无 GPS 校准时：

2. 有 GPS 校准时：

更新四元数

源码分析

主程序运行流程图：

函数功能简述

源码分析

头文件

using @@@

namespace attitude_estimator_q

类定义: class AttitudeEstimatorQ

构造函数

析构函数

start();

print();

task_main_trampoline();

task_main();

update_parameters();

init();

update();

update_mag_declination();

attitude_estimator_q_main();

Mahony 互补滤波相关推荐

最新文章

热门文章