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

Posted lida2003

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了PX4模块设计之二十三:自定义FlightTask相关的知识,希望对你有一定的参考价值。

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

以上是关于PX4模块设计之二十三:自定义FlightTask的主要内容,如果未能解决你的问题,请参考以下文章

PX4模块设计之二十二:FlightModeManager模块

PX4模块设计之二十六:BatteryStatus模块

PX4模块设计之二十九:RCUpdate模块

PX4模块设计之二十:PX4应用平台初始化

PX4模块设计之二十八:RCInput模块

PX4模块设计之二十五:DShot模块