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

Posted 2022-09-09 lida2003

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

• 1. Sensors模块简介
• 2. 模块入口函数
• • 2.1 主入口sensors_main
  • 2.2 自定义子命令custom_command
  • 2.3 模块状态print_status【重载】
• 3. Sensors模块重要函数
• • 3.1 task_spawn
  • 3.2 instantiate
  • 3.3 init
  • 3.4 Run
• 4. 总结
• 5. 参考资料

1. Sensors模块简介

Sensors模块汇总了所有的飞控传感器，最终将处理后的传感数据以sensor_combined/airspeed/differential_pressure消息发布到系统中。

### Description
The sensors module is central to the whole system. It takes low-level output from drivers, turns
it into a more usable form, and publishes it for the rest of the system.

The provided functionality includes:
- Read the output from the sensor drivers (`sensor_gyro`, etc.).
  If there are multiple of the same type, do voting and failover handling.
  Then apply the board rotation and temperature calibration (if enabled). And finally publish the data; one of the
  topics is `sensor_combined`, used by many parts of the system.
- Make sure the sensor drivers get the updated calibration parameters (scale & offset) when the parameters change or
  on startup. The sensor drivers use the ioctl interface for parameter updates. For this to work properly, the
  sensor drivers must already be running when `sensors` is started.
- Do sensor consistency checks and publish the `sensors_status_imu` topic.

### Implementation
It runs in its own thread and polls on the currently selected gyro topic.

sensors <command> [arguments...]
 Commands:
   start
     [-h]        Start in HIL mode

   stop

   status        print status info

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

class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::ScheduledWorkItemModuleParams, public px4::WorkItem

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

2. 模块入口函数

2.1 主入口sensors_main

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

sensors_main
 └──> return Sensors::main(argc, argv)

2.2 自定义子命令custom_command

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

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

2.3 模块状态print_status【重载】

Sensors::print_status
 ├──> _voted_sensors_update.printStatus();
 ├──> <CONFIG_SENSORS_VEHICLE_MAGNETOMETER><_vehicle_magnetometer>
 │   ├──> PX4_INFO_RAW("\\n");
 │   └──> _vehicle_magnetometer->PrintStatus();
 ├──> <CONFIG_SENSORS_VEHICLE_AIR_DATA><_vehicle_air_data>
 │   ├──> PX4_INFO_RAW("\\n");
 │   └──> _vehicle_air_data->PrintStatus();
 ├──> <CONFIG_SENSORS_VEHICLE_AIRSPEED>
 │   ├──> PX4_INFO_RAW("\\n");
 │   ├──> PX4_INFO_RAW("Airspeed status:\\n");
 │   └──> _airspeed_validator.print();
 ├──> <CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW><_vehicle_optical_flow>
 │   ├──> PX4_INFO_RAW("\\n");
 │   └──> _vehicle_optical_flow->PrintStatus();
 ├──> <CONFIG_SENSORS_VEHICLE_GPS_POSITION><_vehicle_gps_position>
 │   ├──> PX4_INFO_RAW("\\n");
 │   └──> _vehicle_gps_position->PrintStatus();
 ├──> PX4_INFO_RAW("\\n");
 ├──> _vehicle_acceleration.PrintStatus();
 ├──> PX4_INFO_RAW("\\n");
 ├──> _vehicle_angular_velocity.PrintStatus();
 ├──> PX4_INFO_RAW("\\n");
 ├──> <for (auto &i : _vehicle_imu_list)><i != nullptr>
 │   ├──> PX4_INFO_RAW("\\n");
 │   └──> i->PrintStatus();
 └──> return 0;

3. Sensors模块重要函数

3.1 task_spawn

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

Sensors::task_spawn
 ├──> bool hil_enabled = false
 ├──> [命令行输入参数，解析hil_enabled]
 ├──> Sensors *instance = new Sensors(hil_enabled)
 ├──> <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中调用init进行ScheduleNow初次调用，后续对_vehicle_imu_sub成员变量进行事件回调注册(当有Sensors消息时，会调用SubscriptionCallbackWorkItem::ScheduleNow，再触发Sensors::Run过程)。

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

注：最多支持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
;

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
 │   ├──> <CONFIG_SENSORS_VEHICLE_AIR_DATA>
 │   │   ├──> const int n_baro = orb_group_count(ORB_ID(sensor_baro))
 │   │   └──> <n_baro != _n_baro>
 │   │       └──> _n_baro = n_baro; updated = true;
 │   ├──> <CONFIG_SENSORS_VEHICLE_GPS_POSITION>
 │   │   ├──> const int n_gps = orb_group_count(ORB_ID(sensor_gps))
 │   │   └──> <n_gps != _n_gps>
 │   │       └──> _n_gps = n_gps; updated = true;
 │   ├──> <CONFIG_SENSORS_VEHICLE_MAGNETOMETER>
 │   │   ├──> const int n_mag = orb_group_count(ORB_ID(sensor_mag))
 │   │   └──> <n_mag != _n_mag>
 │   │       └──> _n_mag = n_mag; updated = true;
 │   ├──> <CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW>
 │   │   ├──> const int n_optical_flow = orb_group_count(ORB_ID(sensor_optical_flow))
 │   │   └──> <n_optical_flow != _n_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();
 ├──> <else><_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;
 ├──> <CONFIG_SENSORS_VEHICLE_AIRSPEED>   // check analog airspeed
 │   ├──> adc_poll();      //发布differential_pressure消息
 │   └──> diff_pres_poll();      //发布airspeed消息
 └──> ScheduleDelayed(10_ms);  // backup schedule as a watchdog timeout

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_subORB_ID(parameter_update), 1_s;
uORB::Subscription _vcontrol_mode_subORB_ID(vehicle_control_mode);
uORB::Subscription _diff_pres_sub ORB_ID(differential_pressure);
uORB::Subscription _vehicle_air_data_subORB_ID(vehicle_air_data);
uORB::Subscription _adc_report_sub ORB_ID(adc_report);

• 输出

uORB::Publication<sensor_combined_s>      _sensor_pubORB_ID(sensor_combined);
uORB::Publication<airspeed_s>             _airspeed_pubORB_ID(airspeed);
uORB::PublicationMulti<differential_pressure_s> _diff_pres_pubORB_ID(differential_pressure);

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