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

Posted 2022-09-09 lida2003

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

• 1. AttitudeEstimatorQ模块简介
• 2. 模块入口函数
• • 2.1 主入口attitude_estimator_q_main
  • 2.2 自定义子命令custom_command
• 3. AttitudeEstimatorQ模块重要函数
• • 3.1 task_spawn
  • 3.2 instantiate
  • 3.3 init
  • 3.4 Run
• 4. 总结
• 5. 参考资料

1. AttitudeEstimatorQ模块简介

### Description
Attitude estimator q.

AttitudeEstimatorQ <command> [arguments...]
 Commands:
   start

   stop

   status        print status info

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

class AttitudeEstimatorQ : public ModuleBase<AttitudeEstimatorQ>, public ModuleParams, public px4::WorkItem

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

2. 模块入口函数

2.1 主入口attitude_estimator_q_main

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

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

2.2 自定义子命令custom_command

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

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

3. AttitudeEstimatorQ模块重要函数

3.1 task_spawn

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

AttitudeEstimatorQ::task_spawn
 ├──> AttitudeEstimatorQ *instance = new AttitudeEstimatorQ()
 ├──> <instance>
 │   ├──> _object.store(instance);
 │   ├──> _task_id = task_id_is_work_queue
 │   └──> <instance->init()>
 │       └──> return PX4_OK
 ├──> <else>
 │   └──> PX4_ERR("alloc failed")
 ├──> delete instance
 ├──> _object.store(nullptr)
 ├──> _task_id = -1
 └──> return PX4_ERROR

3.2 instantiate

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

3.3 init

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

AttitudeEstimatorQ::init
 ├──> <!_sensors_sub.registerCallback()>
 │   ├──> PX4_ERR("callback registration failed")
 │   └──> return false;
 └──> return true;

uORB::SubscriptionCallbackWorkItem _sensors_subthis, ORB_ID(sensor_combined);

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

4. 总结

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

• 输入

	uORB::SubscriptionCallbackWorkItem _sensors_subthis, ORB_ID(sensor_combined);

	uORB::SubscriptionInterval _parameter_update_subORB_ID(parameter_update), 1_s;

	uORB::Subscription _vehicle_attitude_subORB_ID(vehicle_attitude);
	uORB::Subscription _vehicle_gps_position_subORB_ID(vehicle_gps_position);
	uORB::Subscription _vehicle_local_position_subORB_ID(vehicle_local_position);
	uORB::Subscription _vehicle_magnetometer_subORB_ID(vehicle_magnetometer);
	uORB::Subscription _vehicle_mocap_odometry_subORB_ID(vehicle_mocap_odometry);
	uORB::Subscription _vehicle_visual_odometry_subORB_ID(vehicle_visual_odometry);

• 输出

	uORB::Publication<vehicle_attitude_s> _vehicle_attitude_pubORB_ID(vehicle_attitude);

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
;

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