PX4模块设计之三十八:Navigator模块
PX4模块设计之三十八:Navigator模块
- 1. Navigator模块简介
- 2. 模块入口函数
- 2.1 主入口navigator_main
- 2.2 自定义子命令custom_command
- 3. Navigator模块重要函数
- 3.1 task_spawn
- 3.2 instantiate
- 3.3 init
- 3.4 Run
- 4. 总结
- 5. 参考资料
1. Navigator模块简介
### Description
Module that is responsible for autonomous flight modes. This includes missions (read from dataman),
takeoff and RTL.
It is also responsible for geofence violation checking.### Implementation
The different internal modes are implemented as separate classes that inherit from a common base class `NavigatorMode`.
The member `_navigation_mode` contains the current active mode.Navigator publishes position setpoint triplets (`position_setpoint_triplet_s`), which are then used by the position
controller.navigator <command> [arguments...]Commands:startfencefile load a geofence file from SD card, stored at etc/geofence.txtfake_traffic publishes 4 fake transponder_report_s uORB messagesstopstatus print status info
注:print_usage函数是具体对应实现。
class Navigator : public ModuleBase<Navigator>, public ModuleParams
注:Navigator模块采用了ModuleBase, 但没有使用WorkQueue设计,因此在实现上需要实现instantiate方法。
2. 模块入口函数
2.1 主入口navigator_main
同样继承了ModuleBase,由ModuleBase的main来完成模块入口。
navigator_main└──> return Navigator::main(argc, argv)
2.2 自定义子命令custom_command
模块除支持start/stop/status命令,自定义命令支持以下子命令:
- fencefile: load a geofence file from SD card, stored at etc/geofence.txt
- fake_traffic: publishes 4 fake transponder_report_s uORB messages
Navigator::custom_command├──> <!is_running()>│ ├──> print_usage("not running")│ └──> return 1├──> <!strcmp(argv[0], "fencefile")>│ ├──> get_instance()->load_fence_from_file(GEOFENCE_FILENAME)│ └──> return 0├──> <!strcmp(argv[0], "fake_traffic")>│ ├──> get_instance()->fake_traffic("LX007", 500, 1.0f, -1.0f, 100.0f, 90.0f, 0.001f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT)│ ├──> get_instance()->fake_traffic("LX55", 1000, 0, 0, 100.0f, 90.0f, 0.001f, transponder_report_s::ADSB_EMITTER_TYPE_SMALL)│ ├──> get_instance()->fake_traffic("LX20", 15000, 1.0f, -1.0f, 280.0f, 90.0f, 0.001f, transponder_report_s::ADSB_EMITTER_TYPE_LARGE)│ ├──> get_instance()->fake_traffic("UAV", 10, 1.0f, -2.0f, 10.0f, 10.0f, 0.01f, transponder_report_s::ADSB_EMITTER_TYPE_UAV)│ └──> return 0└──> return print_usage("unknown command")
3. Navigator模块重要函数
3.1 task_spawn
这里直接使用px4_task_spawn_cmd创建任务。
Navigator::task_spawn├──> _task_id = px4_task_spawn_cmd("navigator",SCHED_DEFAULT,SCHED_PRIORITY_NAVIGATION,PX4_STACK_ADJUSTED(1952),(px4_main_t)&run_trampoline,(char *const *)argv)├──> <_task_id < 0>│ ├──> _task_id = -1│ └──> return -errno└──> return 0
注:鉴于ModuleBase::run_trampoline会直接调用instantiate初始化任务,Navigator模块就必须对Navigator::instantiate方法进行重载实现。
3.2 instantiate
初始化Navigator类实例。
Navigator::instantiate├──> Navigator *instance = new Navigator()├──> <instance == nullptr>│ └──> PX4_ERR("alloc failed")└──> return instance
3.3 init
注:鉴于该模块采用任务,而之前大量的模块使用了WorkQueue的设计方法,在task_spawn里会调用init这个方法,为了对比,保留这个方法。
3.4 Run
根据输入参数及业务逻辑计算输出量,并发布消息。
Navigator::Run├──> [Try to load the geofence: /fs/microsd/etc/geofence.txt]├──> params_update├──> [wakeup source(s) setup]│ ├──> fds[0].fd = _local_pos_sub;│ ├──> fds[0].events = POLLIN;│ ├──> fds[1].fd = _vehicle_status_sub;│ ├──> fds[1].events = POLLIN;│ ├──> fds[2].fd = _mission_sub;│ ├──> fds[2].events = POLLIN;│ └──> orb_set_interval(_local_pos_sub, 50); // rate-limit position subscription to 20 Hz / 50 ms└──> <!should_exit()>├──> int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); // wait for up to 1000ms for data├──> <pret < 0> // this is undesirable but not much we can do - might want to flag unhappy status│ ├──> PX4_ERR("poll error %d, %d", pret, errno);│ ├──> px4_usleep(10000);│ └──> continue;├──> [update vehicle_local_position]├──> [update vehicle_status]├──> [update mission]├──> [update vehicle_gps_position]├──> [update vehicle_global_position]├──> [update parameter_update]├──> [update vehicle_land_detected]├──> [update position_controller_status]├──> [update home_position]├──> <_vehicle_command_sub.updated()>│ └──> [execute commands] 【发布vehicle_roi/vehicle_command/vehicle_command_ack】├──> check_traffic()├──> geofence_breach_check(have_geofence_position_data) 【发布geofence_result消息】├──> [judge navigation new mode] // Do not execute any state machine while we are disarmed├──> [reset triplet, when we have a new navigation mode]├──> [iterate through navigation modes and set active/inactive for each]├──> <_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once> // if nothing is running, set position setpoint triplet invalid once│ ├──> _pos_sp_triplet_published_invalid_once = true;│ └──> reset_triplets();├──> <_pos_sp_triplet_updated>│ └──> publish_position_setpoint_triplet(); 【发布position_setpoint_triplet消息】└──> <_mission_result_updated>└──> publish_mission_result(); 【发布mission_result消息】
4. 总结
具体逻辑业务后续再做深入,从模块代码角度:
- 输入
uORB::SubscriptionData<position_controller_status_s> _position_controller_status_sub{ORB_ID(position_controller_status)};uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */
uORB::Subscription _gps_pos_sub{ORB_ID(vehicle_gps_position)}; /**< gps position subscription */
uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */
uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _pos_ctrl_landing_status_sub{ORB_ID(position_controller_landing_status)}; /**< position controller landing status subscription */
uORB::Subscription _traffic_sub{ORB_ID(transponder_report)}; /**< traffic subscription */
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; /**< vehicle commands (onboard and offboard) */
- 输出
uORB::Publication<geofence_result_s> _geofence_result_pub{ORB_ID(geofence_result)};
uORB::Publication<mission_result_s> _mission_result_pub{ORB_ID(mission_result)};
uORB::Publication<position_setpoint_triplet_s> _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)};
uORB::Publication<vehicle_command_ack_s> _vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<vehicle_command_s> _vehicle_cmd_pub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_roi_s> _vehicle_roi_pub{ORB_ID(vehicle_roi)};
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模块设计之三十八:Navigator模块相关推荐
- BetaFlight模块设计之三十:Cli模块分析
BetaFlight模块设计之三十:Cli模块分析 Cli模块 Cli接口 Cli框架 Cli命令结构 主要函数分析 cliProcess函数 processCharacterInteractive函 ...
- PX4模块设计之三十五:MulticopterAttitudeControl模块
PX4模块设计之三十五:MulticopterAttitudeControl模块 1. MulticopterAttitudeControl模块简介 2. 模块入口函数 2.1 主入口mc_att_c ...
- 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 ...
- PX4模块设计之三十三:Sensors模块
PX4模块设计之三十三:Sensors模块 1. Sensors模块简介 2. 模块入口函数 2.1 主入口sensors_main 2.2 自定义子命令custom_command 2.3 模块状态 ...
- BetaFlight模块设计之三十五:RSSI信号强度链路稳定性分析
BetaFlight模块设计之三十五:RSSI信号强度&链路稳定性分析 1. RSSI信号强度 1.1 RSSI Value 1.2 RSSI dBm Value 2. 链路稳定性 3. RS ...
- 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设备 ...
- BetaFlight模块设计之三十三:Pid模块分析
BetaFlight模块设计之三十三:Pid模块分析 Pid模块 1. Pid_audio子模块 2. Pid_init子模块 3. Pid算法子模块 3.1 TPA模式 3.2 飞行模式 3.3 L ...
最新文章
- [转载]交换机背板带宽计算方法
- linux ip_conntrack 连接满导致网络丢包
- 虚拟机安装(Cent OS)
- Python 技术篇-含中文编码的代码运行方法,(unicode error) ‘utf-8‘ codec can‘t decode问题原因及解决方法
- onenote怎么同步到电脑_详解onenote保存与同步④:本地笔记奇葩的丢失经历
- 邮件header中的subject或者from解码
- 大话移动通信pdf_移动通信是怎么实现的?
- TensorFlow高阶 API: keras教程-使用tf.keras搭建mnist手写数字识别网络
- ionic4监听返回事件 AppMinimize navController
- 29岁“退休程序员”郭宇:有钱的人不一定自由,自由的人不一定有钱
- 在启动时从配置文件中读取对象
- 微信小程序开发学习笔记005--微信小程序组件详解
- 简单排序(插入排序法)
- 【转】如何学会独立思考?
- Microsoft Security Essentials升级到2.0测试版 失败时的解决办法
- 百度网盘分享无提取码文件的方法
- 实现Codeblock支持C99标准
- java实现微信公众号的模板消息推送
- 漫画:为什么不能打断程序员?
- Pug 介绍和在 Vue 中使用
热门文章
- 2022年全球市场合成孔径声呐系统总体规模、主要生产商、主要地区、产品和应用细分研究报告
- Vue3中Element-Plus的el-upload限制只上传一个文件(最简单明了)
- java c rsa解秘_Java的RSA加密,.net如何解密
- 微信点击链接下载APP或者文件后自动调用手机自带默认浏览器或提示选择浏览器打开如何实现
- Unity HybridRender 视锥体裁剪
- 阿里云总裁胡晓明40个精彩回答 涉及CDN、价格战、生态竞争等
- 前端的Docker入门与实践
- win8英雄联盟连接不上服务器未响应,Win8系统英雄联盟出现连接失败如何解决
- CMake Error (find_package):Could not find a package configuration
- 面试阿里,HashMap 这一篇就够了