姿态解算流程图
2022-02-16 18:39:36 0 举报
Pixhawk姿态解算流程图
作者其他创作
大纲/内容
修正积分项ei=[e(yaw)+e(pitch roll)]*dt比例项ep=e(yaw)+e(pitch roll)
函数内部
函数update()
函数结束
通过函数from_dcm()，由R得到初始四元数
不为ture
四元数归一化
更新GPS数据：1. 如果mag_decl_auto配置不为0，利用GPS更新磁偏角；2. 如果acc_comp配置不为0，则利用GPS测得速度，转换到机体坐标系下，并存储在_pos_acc中以待后用
第一次进入，执行函数Init()，得到初始四元数
w(coor)=w+ep+sum(ei)其中：w为陀螺仪测得的角速度
attitude_estimator_q_main.cpp的工作流程图
通过变量control_state将数据发布
函数task_main
二次及以后进入函数
配置阻塞时间（1s读取一次sensor_combined的数据）
利用w(coor)对四元数进行更新
函数attitude_estimator_q_main
将得到的四元数，补偿后的角速度，加速度存入control_state_s类型的变量control_state中
通过att将数据发布
j=i x k作为E(NED坐标系)方向
函数task_main_trampoline
extern \"C\
初始方向余弦R=（i ，j，k）
由四元数得到欧拉角
依据不同外设：利用vision或mocap对航向角进行修正
更新配置参数（update_parameters）
by 蜗牛拉火车
Init()函数处理方法单独列出
防止积分饱和处理
取消主题订阅
依据不同外设1.用vision或mocap更新航向2.更新airspeed
进入while大循环
_task_should_exit==ture（在析构函数中置为ture）
函数start（定义attitude_estimator_q函数句柄、开启任务等）
为ture
函数Init()
将得到的欧拉角、补偿后的角速度、加速度、四元数、转换矩阵、陀螺仪补偿项等存入vehicle_attitude_s类型的att变量中
0 条评论
回复 删除
下一页
