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

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模块采用了ModuleBaseWorkQueue设计。

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_sub{this, 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_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)};
  • 输出
	uORB::Publication<vehicle_attitude_s> _vehicle_attitude_pub{ORB_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

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值