extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[])
告之函数入口
函数attitude_estimator_q_main
函数start
（定义attitude_estimator_q
函数句柄、开启任务等）
函数task_main_trampoline
函数task_main
1.订阅所需要的topics；
2.采集配置参数(update_parameters),包括权重，是
否利用GPS进行补偿，陀螺仪最大偏差，航向角测
量模式，空速测量模式等；
进入while大循环
配置阻塞时间（1ms读取一次se
nsor_combined的数据）
更新配置参数（update_p
arameters）
采集传感器数据:(ax,ay,az)(w
x,wy,wz)(mx,my,mz),其中加
速度和角速度数据要进行低
通滤波处理
依据不同外设
1.用vision或mocap更新航向
2.更新airspeed
更新GPS数据：
1. 如果mag_decl_auto配置不为0，利用GP
S更新磁偏角；
2. 如果acc_comp配置不为0，则利用GPS
测得速度，转换到机体坐标系下，并存储在
_pos_acc中以待后用
测量循环一次所需时间，作为dt
，如果超过0.02s,取0.02s
函数update()
函数内部
第一次进入
，执行函数
Init()，得
到初始四元
数
二次及以后
进入函数
依据不同外设：利
用vision或mocap
对航向角进行修正
利用(mx,my,mz)对
航向修正，得到修
正量e(yaw)
利用(ax,ay,az)对陀
螺仪修正，得到修
正量e(pitch roll)
修正积分项ei=
[e(yaw)+e(pitch roll)]*dt
比例项ep=
e(yaw)+e(pitch roll)
防止积分饱和
处理
w(coor)=w+ep
+sum(ei)
利用w(coor)对四
元数进行更新
四元数归一化
函数内部
由四元数得到
欧拉角
将得到的欧拉角、
补偿后的角速度、
加速度、四元数、
转换矩阵、陀螺仪
补偿项等存入vehi
cle_attitude_s类型
的att变量中
通过att将数据
发布
将得到的四元数，补偿
后的角速度，加速度存
入control_state_s类型
的变量control_state中
通过变量contr
ol_state将数据
发布
函数内部
取消主题订阅
_task_should_exit
==ture
（在析构函数中置为ture）
不为ture
为ture
函数结束
Init()函数
处理方法
单独列出
函数Init()
[ax,ay,az]归一化作
为初始的D(NED坐
标系)方向，记为k
[mx,my,mz]与k做正
交化处理后作为N(N
ED坐标系)方向，记
为i
j=i x k作为E(NED
坐标系)方向
初始方向余弦
R=（i ，j，k）
通过函数from_dc
m()，由R得到初始
四元数
attitude_estimator_q_main.cpp的工作流程图
by 蜗牛拉火车
资源评论
