PX4模块设计之二十三：自定义FlightTask

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

1. FlightTaskOrbit.hpp
2. FlightTaskOrbit.cpp
3. 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

大部分飞行模式需要实现以下两个函数：

1. bool activate(const trajectory_setpoint_s &last_setpoint) override;
2. 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代码已经实现，大家可以自行参考）

1. Create a new Mavlink MAV_CMD command: MAV_CMD_DO_ORBIT
2. 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
3. Commander.cpp handle_command(): add case vehicle_command_s::VEHICLE_CMD_DO_ORBIT
4. statemachine_helper.cpp main_state_transition(): case MAIN_STATE_ORBIT
5. statemachine_helper.cpp set_nav_state(): case commander_state_s::MAIN_STATE_ORBIT
6. Commander.cpp set_control_mode(): case vehicle_command_s::NAVIGATION_STATE_ORBIT
7. 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