PX4模块设计之三十二：AttitudeEstimatorQ模块

1. AttitudeEstimatorQ模块简介

### Description
Attitude estimator q.

AttitudeEstimatorQ  [arguments...]
 Commands:
   start

   stop

   status        print status info
1
2
3
4
5
6
7
8
9
10

注：print_usage函数是具体对应实现。

class AttitudeEstimatorQ : public ModuleBase, public ModuleParams, public px4::WorkItem
1

注：AttitudeEstimatorQ模块采用了ModuleBase和WorkQueue设计。

2. 模块入口函数

2.1 主入口attitude_estimator_q_main

同样继承了ModuleBase，由ModuleBase的main来完成模块入口。

attitude_estimator_q_main
 └──> return AttitudeEstimatorQ::main(argc, argv)
1
2

2.2 自定义子命令custom_command

模块仅支持start/stop/status命令，不支持其他自定义命令。

AttitudeEstimatorQ::custom_command
 └──> return print_usage("unknown command");
1
2

3. AttitudeEstimatorQ模块重要函数

3.1 task_spawn

这里主要初始化了AttitudeEstimatorQ对象，后续通过WorkQueue来完成进行轮询。

AttitudeEstimatorQ::task_spawn
 ├──> AttitudeEstimatorQ *instance = new AttitudeEstimatorQ()
 ├──> 
 │   ├──> _object.store(instance);
 │   ├──> _task_id = task_id_is_work_queue
 │   └──> init()>
 │       └──> return PX4_OK
 ├──> 
 │   └──> PX4_ERR("alloc failed")
 ├──> delete instance
 ├──> _object.store(nullptr)
 ├──> _task_id = -1
 └──> return PX4_ERROR
1
2
3
4
5
6
7
8
9
10
11
12
13

3.2 instantiate

注：鉴于该模块不采用任务(线程)，所以ModuleBase::run_trampoline无需执行，所以可以不实现。

3.3 init

在task_spawn中使用，对_sensors_sub成员变量进行事件回调注册(当有_sensors_sub消息时，会调用SubscriptionCallbackWorkItem::ScheduleNow，再触发AttitudeEstimatorQ::Run过程)。

AttitudeEstimatorQ::init
 ├──> 
 │   ├──> PX4_ERR("callback registration failed")
 │   └──> return false;
 └──> return true;
1
2
3
4
5

uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
1

3.4 Run

将输入消息处理后，更新发布最新计算的vehicle_attitude消息。

AttitudeEstimatorQ::Run
 ├──> [优雅退出处理]
 └──> <_sensors_sub.updated()>
     ├──> _data_good = true;
     ├──> _ext_hdg_good = false;
     ├──> update_parameters();
     ├──> update_sensors();
     ├──> update_magnetometer();
     ├──> update_visual_odometry();
     ├──> update_motion_capture_odometry();
     ├──> update_gps_position();
     ├──> update_vehicle_local_position();
     └──> update_vehicle_attitude();  // 发布vehicle_attitude消息
1
2
3
4
5
6
7
8
9
10
11
12
13

4. 总结

深入姿态计算需要进一步研读内部逻辑和算法（后续有时间我们再进行深入），从模块代码角度：

• 输入

	uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};

	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
	uORB::Subscription _vehicle_magnetometer_sub{ORB_ID(vehicle_magnetometer)};
	uORB::Subscription _vehicle_mocap_odometry_sub{ORB_ID(vehicle_mocap_odometry)};
	uORB::Subscription _vehicle_visual_odometry_sub{ORB_ID(vehicle_visual_odometry)};
1
2
3
4
5
6
7
8
9
10

• 输出

	uORB::Publication _vehicle_attitude_pub{ORB_ID(vehicle_attitude)};
1

struct vehicle_attitude_s {

	uint64_t timestamp;
	uint64_t timestamp_sample;
	float q[4];
	float delta_q_reset[4];
	uint8_t quat_reset_counter;
	uint8_t _padding0[7]; // required for logger
};
1
2
3
4
5
6
7
8
9

5. 参考资料

【1】PX4开源软件框架简明简介
【2】PX4模块设计之十一：Built-In框架
【3】PX4模块设计之十二：High Resolution Timer设计
【4】PX4模块设计之十三：WorkQueue设计
【5】PX4模块设计之十七：ModuleBase模块
【6】PX4模块设计之三十：Hysteresis类
【7】PX4 modules_main