PX4模块设计之二十三:自定义FlightTask
Posted lida2003
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了PX4模块设计之二十三:自定义FlightTask相关的知识,希望对你有一定的参考价值。
PX4模块设计之二十三:自定义FlightTask
- Step1 创建飞行模式文件夹
- Step2 创建飞行模式源代码和CMakeLists.txt文件
- Step3 更新CMakeLists.txt文件
- Step4 更新FlightTaskOrbit.hpp
- Step5 更新FlightTaskOrbit.cpp
- Step6 新增飞行模式到编译系统
- Step7 确定使用该飞行模式的位置
- Step8 MAVLink命令触发飞行模式
- 参考资料
目前已经有大量的飞控模式在PX4系统中实现,且成熟应用。详见以下章节介绍:
【1】PX4模块设计之九:PX4飞行模式简介
【2】PX4模块设计之二十二:FlightModeManager模块
实际应用时,各有各的需求以及硬件配置等情况,导致PX4系统内部的已有飞行模式并不满足需求。因此需要增加自定义的飞行模式。
这里就以PX4代码自带的Orbit飞行模式作为假设“新增”自定义飞行模式的角度来分析,如何自定义FlightTask(如何新增以及触发飞行模式)。
Step1 创建飞行模式文件夹
新建Orbit文件夹,PX4-Autopilot/src/modules/flight_mode_manager/tasks/Orbit
$ mkdir PX4-Autopilot/src/modules/flight_mode_manager/tasks/Orbit
Step2 创建飞行模式源代码和CMakeLists.txt文件
新建以"FlightTask"为前缀的飞行模式空文件源代码
$ touch PX4-Autopilot/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
$ touch PX4-Autopilot/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
$ touch PX4-Autopilot/src/modules/flight_mode_manager/tasks/Orbit/CMakeLists.txt
注:这里是PX4系统已经实现了,而刚新增的时候是空文件,实现代码在后。
Step3 更新CMakeLists.txt文件
从其他飞行模式中复制CMakeLists.txt文件,比如:Descend/CMakeLists.txt
主要修改这几行内容:
34 px4_add_library(FlightTaskOrbit
35 FlightTaskOrbit.cpp
36 )
37
38 target_link_libraries(FlightTaskOrbit PUBLIC FlightTaskManualAltitudeSmoothVel SlewRate)
39 target_include_directories(FlightTaskOrbit PUBLIC $CMAKE_CURRENT_SOURCE_DIR)
注1:新增FlightTaskOrbit依赖哪些库文件后面就跟链接库。由于Orbit依赖的是FlightTaskManualAltitudeSmoothVel+SlewRate类,且两个类均有对应的库文件,所以这里38行的写法与最纯粹的飞行模式的写法有所差异。
注2:通常来说新增最纯粹的飞行模式38行可以这样写:target_link_libraries(FlightTaskMyFlight PUBLIC FlightTask)。
Step4 更新FlightTaskOrbit.hpp
大部分飞行模式需要实现以下两个函数:
- bool activate(const trajectory_setpoint_s &last_setpoint) override;
- bool update() override;
Orbit飞行模式需要可以调整:半径、速度、高度
3. bool applyCommandParameters(const vehicle_command_s &command) override;
详细代码不再罗列,详见FlightTaskOrbit.hpp。
Step5 更新FlightTaskOrbit.cpp
详细代码不再罗列,详见FlightTaskOrbit.cpp。
注:早先这个飞行模式下代码只有三个文件,最新代码看到有flight_task_orbit_params.c,但是在px4_add_library里面确缺少这个c代码文件。这里只是介绍,如果文件数量有所变动,大家自行琢磨。
Step6 新增飞行模式到编译系统
通过在PX4-Autopilot/src/modules/flight_mode_manager/CMakeLists.txt新增Orbit目录到flight_tasks_all。
注:这里Orbit实际上PX4系统中已经有,且增加在NOT px4_constrained_flash_build中。如果自己新增的可以放在41行的位置。
Step7 确定使用该飞行模式的位置
通过start_flight_task找到一个可以放置该Orbit飞行模式的位置。
比如:放在vehicle_status_s::NAVIGATION_STATE_ORBIT
修改FlightModeManager.cpp代码如下:
304 if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT)
305 #if !defined(CONSTRAINED_FLASH)
306 error = switchTask(FlightTaskIndex::Orbit);
307 #endif // !CONSTRAINED_FLASH
306
注:鉴于holybro_kakuteF7使用了CONFIG_BOARD_CONSTRAINED_FLASH=y,所以没有编译相关代码,如果需要可以打开这个功能。(不同的板子或者编译目标对象,请结合实际情况调整,比如:px4_sitl_default是默认编译的,但是Orbit这个代码目前确实已经从PX4系统调用中删除了。
Step8 MAVLink命令触发飞行模式
这个具体可以参考Flight task architecture overview视频 – by Matthias Grob, Dennis Mannhart,PDF资料详见参考【5】。
大体分以下几个小步骤(PX4代码已经实现,大家可以自行参考)
- Create a new Mavlink MAV_CMD command: MAV_CMD_DO_ORBIT
- Add new mode to uORB messages
vehicle_command.msg: VEHICLE_CMD_DO_ORBIT
commander_state.msg: MAIN_STATE_ORBIT
vehicle_status.msg: NAVIGATION_STATE_ORBIT - Commander.cpp handle_command(): add case vehicle_command_s::VEHICLE_CMD_DO_ORBIT
- statemachine_helper.cpp main_state_transition(): case MAIN_STATE_ORBIT
- statemachine_helper.cpp set_nav_state(): case commander_state_s::MAIN_STATE_ORBIT
- Commander.cpp set_control_mode(): case vehicle_command_s::NAVIGATION_STATE_ORBIT
- mc_pos_control_main.cpp start_flight_task(): add case vehicle_status_s::NAVIGATION_STATE_ORBIT
注:7已经在上面Step7中介绍过。
参考资料
【1】PX4开源软件框架简明简介
【2】PX4模块设计之九:PX4飞行模式简介
【3】PX4模块设计之二十二:FlightModeManager模块
【4】PX4 Creating a Flight Task
【5】Slides-PX4-Dev-Summit-FlightTasks.pdf
以上是关于PX4模块设计之二十三:自定义FlightTask的主要内容,如果未能解决你的问题,请参考以下文章