|
目前已经有大量的飞控模式在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"为前缀的飞行模式空文件源代码
- FlightTaskOrbit.hpp
- FlightTaskOrbit.cpp
- CMakeLists.txt
$ 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
|
