PX4模块设计之三十三:Sensors模块
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 thetopics 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 oron startup. The sensor drivers use the ioctl interface for parameter updates. For this to work properly, thesensor 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 modestopstatus 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_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)};
- 输出
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)};
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
uORB::PublicationMulti<differential_pressure_s> _diff_pres_pub{ORB_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
PX4模块设计之三十三:Sensors模块相关推荐
- BetaFlight模块设计之三十三:Pid模块分析
BetaFlight模块设计之三十三:Pid模块分析 Pid模块 1. Pid_audio子模块 2. Pid_init子模块 3. Pid算法子模块 3.1 TPA模式 3.2 飞行模式 3.3 L ...
- PX4模块设计之三十五:MulticopterAttitudeControl模块
PX4模块设计之三十五:MulticopterAttitudeControl模块 1. MulticopterAttitudeControl模块简介 2. 模块入口函数 2.1 主入口mc_att_c ...
- PX4模块设计之三十八:Navigator模块
PX4模块设计之三十八:Navigator模块 1. Navigator模块简介 2. 模块入口函数 2.1 主入口navigator_main 2.2 自定义子命令custom_command 3. ...
- PX4模块设计之三十四:ControlAllocator模块
PX4模块设计之三十四:ControlAllocator模块 1. ControlAllocator模块简介 2. 模块入口函数 2.1 主入口control_allocator_main 2.2 自 ...
- PX4模块设计之三十一:ManualControl模块
PX4模块设计之三十一:ManualControl模块 1. ManualControl模块简介 2. 模块入口函数 2.1 主入口manual_control_main 2.2 自定义子命令cust ...
- BetaFlight模块设计之三十五:RSSI信号强度链路稳定性分析
BetaFlight模块设计之三十五:RSSI信号强度&链路稳定性分析 1. RSSI信号强度 1.1 RSSI Value 1.2 RSSI dBm Value 2. 链路稳定性 3. RS ...
- BetaFlight模块设计之三十:Cli模块分析
BetaFlight模块设计之三十:Cli模块分析 Cli模块 Cli接口 Cli框架 Cli命令结构 主要函数分析 cliProcess函数 processCharacterInteractive函 ...
- BetaFlight模块设计之三十二:MSP协议模块分析
BetaFlight模块设计之三十二:MSP协议模块分析 1. MSP协议模块 1.1 MSP描述 1.2 MSP版本优缺点 1.3 MSP代码资源 2. MSP报文解析 2.1 MSP收包状态机 2 ...
- BetaFlight模块设计之三十四:OSD模块分析
BetaFlight模块设计之三十四:OSD模块分析 1. OSD模块 1.1 osd状态机子模块 1.2 osd_warnings检查子模块 1.3 osd_elements子模块 2. OSD设备 ...
最新文章
- Django安装使用基础
- JavaScript中HTML的DOM
- Spring 的优秀工具类盘点
- EditView设置边框
- 《命犯桃花》离斯蒂芬·金还很遥远
- P5491-[模板]二次剩余
- control层alert弹出框乱码_【ArcGIS for JS】动态图层的属性查询(11)
- 2017计算机湖北对口试题答案,2017年计算机专业对口考试试卷及答案
- 横断面数据提取工具_SDTP_CAD断面数据处理教程
- Win10 独立音量调整
- Jmeter接口测试及接口性能测试
- 图像处理 之 扫描全能王代替品
- Excel 如何批量查询快递单号
- U-boot简介及常用命令说明
- 阿里新推出“阿里云网盘”App,有机会干掉“百度网盘”吗?
- 【C语言】万字讲解 从零到精通 (文件操作与文件函数)
- 全新设计 水果忍者-穿靴子的猫官方中文版首发
- Python——第四天的Gut Punch
- 谷歌浏览器使用记住密码功能 导致input 输入框样式改变的解决办法
- 计算机组成原理实验存储器部件实验,实验4存储器部件实验.doc
热门文章
- [DeDe] - 织梦内容管理系统模板标签代码参考
- 同义词、序列、视图、索引
- Cesium结合Echarts的使用
- 零跑C01超低的售价,让梦想照进现实
- Clicks can be Cheating: Counterfactual Recommendation for Mitigating Clickbait Issue -- SIGIR‘21
- Shell入门笔记:Linux批量提取文件名/shel文件名提取日期/NCL批量读取文件(shell脚本结合)
- 极客头条 | 5月18日科技要闻:华为库存至少够缓冲一年;张朝阳质疑 5G 微波危害;苹果iOS 13不受支持机型曝光
- c语言find和mid函数的使用方法,excel中mid函数和find函数用法
- 百度飞桨公开课笔记——OPENCV库实现图像增广
- idea修改运行内存大小