extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]) 告之函数入口 函数attitude_estimator_q_main attitude_estimator_q_main.cpp的工作流程图 by 蜗牛拉火车 函数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()，得 到初始四元 数 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) 不为ture 防止积分饱和 处理 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 取消主题订阅 函数结束 函数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得到初始 四元数