PX4模块设计之三十三：Sensors模块

3. Sensors模块重要函数

3.1 task_spawn

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

Sensors::task_spawn
 ├──> bool hil_enabled = false
 ├──> [命令行输入参数，解析hil_enabled]
 ├──> Sensors *instance = new Sensors(hil_enabled)
 ├──> 
 │   ├──> _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
14
15

3.2 instantiate

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

3.3 init

在task_spawn中调用init进行ScheduleNow初次调用，后续对_vehicle_imu_sub成员变量进行事件回调注册(当有Sensors消息时，会调用SubscriptionCallbackWorkItem::ScheduleNow，再触发Sensors::Run过程)。

Sensors::init
 ├──> _vehicle_imu_sub[0].registerCallback()  // 这里有点奇怪，为什么只是注册一个IMU；假如多个IMU，第0个损坏且数据不发生变化，怎么办？？？
 ├──> ScheduleNow()
 └──> return true
1
2
3
4

注：最多支持4个IMU设备。

uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[MAX_SENSOR_COUNT] {
	{this, ORB_ID(vehicle_imu), 0},
	{this, ORB_ID(vehicle_imu), 1},
	{this, ORB_ID(vehicle_imu), 2},
	{this, ORB_ID(vehicle_imu), 3}
};
1
2
3
4
5
6

3.4 Run

在没有解锁时进行传感器遍历和添加；当解锁以后就不在添加传感器，仅对当前已经确认支持的传感设备进行数据更新。

Sensors::Run
 ├──> [优雅退出处理]
 ├──> <_vcontrol_mode_sub.updated()><_vcontrol_mode_sub.copy(&vcontrol_mode)>
 │   └──> _armed = vcontrol_mode.flag_armed
 ├──> <(!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) || (_last_config_update == 0)>   // keep adding sensors as long as we are not armed
 │   ├──> bool updated = false
 │   ├──> 
 │   │   ├──> const int n_baro = orb_group_count(ORB_ID(sensor_baro))
 │   │   └──> 
 │   │       └──> _n_baro = n_baro; updated = true;
 │   ├──> 
 │   │   ├──> const int n_gps = orb_group_count(ORB_ID(sensor_gps))
 │   │   └──> 
 │   │       └──> _n_gps = n_gps; updated = true;
 │   ├──> 
 │   │   ├──> const int n_mag = orb_group_count(ORB_ID(sensor_mag))
 │   │   └──> 
 │   │       └──> _n_mag = n_mag; updated = true;
 │   ├──> 
 │   │   ├──> const int n_optical_flow = orb_group_count(ORB_ID(sensor_optical_flow))
 │   │   └──> 
 │   │       └──> _n_optical_flow = n_optical_flow; updated = true;
 │   ├──> const int n_accel = orb_group_count(ORB_ID(sensor_accel));
 │   ├──> const int n_gyro  = orb_group_count(ORB_ID(sensor_gyro));
 │   ├──> <(n_accel != _n_accel) || (n_gyro != _n_gyro) || updated>
 │   │   ├──> _n_accel = n_accel;
 │   │   ├──> _n_gyro = n_gyro;
 │   │   └──> parameters_update();
 │   ├──> _voted_sensors_update.initializeSensors();
 │   ├──> InitializeVehicleIMU();
 │   └──> _last_config_update = hrt_absolute_time();
 ├──> <_parameter_update_sub.updated()>  // when not adding sensors poll for param updates
 │   ├──> _parameter_update_sub.copy(&pupdate);
 │   ├──> parameters_update();
 │   └──> updateParams();
 ├──> _voted_sensors_update.sensorsPoll(_sensor_combined);
 ├──> <_sensor_combined.timestamp != _sensor_combined_prev_timestamp>
 │   ├──> _voted_sensors_update.setRelativeTimestamps(_sensor_combined);
 │   ├──> _sensor_pub.publish(_sensor_combined);                                 // 发布sensor_combined消息
 │   └──> _sensor_combined_prev_timestamp = _sensor_combined.timestamp;
 ├──>    // check analog airspeed
 │   ├──> adc_poll();      //发布differential_pressure消息
 │   └──> diff_pres_poll();      //发布airspeed消息
 └──> ScheduleDelayed(10_ms);  // backup schedule as a watchdog timeout
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

4. 总结

该模块主要处理多IMU数据，空速计，气压差传感数据，从代码角度：

• 输入

uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[MAX_SENSOR_COUNT] {{this, ORB_ID(vehicle_imu), 0},{this, ORB_ID(vehicle_imu), 1},{this, ORB_ID(vehicle_imu), 2},{this, ORB_ID(vehicle_imu), 3}};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _diff_pres_sub {ORB_ID(differential_pressure)};
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _adc_report_sub {ORB_ID(adc_report)};
1
2
3
4
5
6

• 输出

uORB::Publication      _sensor_pub{ORB_ID(sensor_combined)};
uORB::Publication             _airspeed_pub{ORB_ID(airspeed)};
uORB::PublicationMulti _diff_pres_pub{ORB_ID(differential_pressure)};
1
2
3

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