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

1. FlightModeManager简介

FlightModeManager主要就是控制飞行模式的。

### Description
This implements the setpoint generation for all modes. It takes the current mode state of the vehicle as input
and outputs setpoints for controllers.

flight_mode_manager <command> [arguments...]
 Commands:
   start

   stop

   status        print status info

2. FlightModeManager启动

参考PX4模块设计之十：PX4启动过程，执行rcS启动脚本：

• Step1：ROMFS/px4fmu_common/init.d/rcS:407: . ${R}etc/init.d/rc.vehicle_setup
• Step2：ROMFS/px4fmu_common/init.d/rc.vehicle_setup:39: . ${R}etc/init.d/rc.mc_apps
• Step3：ROMFS/px4fmu_common/init.d/rc.mc_apps:78:flight_mode_manager start

3. FlightModeManager模块重要实现

3.1 custom_command

无额外具体实现子命令。

int FlightModeManager::custom_command(int argc, char *argv[])
{
	return print_usage("unknown command");
}

3.2 print_usage

略，详见命令打印简介。

3.3 instantiate

不使用任务实现，采用PX4 WorkQueue。

3.4 task_spawn

启动FlightModeManager模块任务（采用WorkQueue；不直接使用任务，创建线程）。

FlightModeManager::task_spawn
 ├──> FlightModeManager *instance = new FlightModeManager();
 ├──> <instance>
 │   ├──> _object.store(instance);
 │   ├──> _task_id = task_id_is_work_queue;
 │   └──> <instance->init()>
 │       └──>return PX4_OK;
 ├──> <else>
 │   └──> PX4_ERR("alloc failed");
 ├──> delete instance;
 ├──> _object.store(nullptr);
 ├──> _task_id = -1;
 └──> return PX4_ERROR;

3.4 init

初始化FlightModeManager，注册SubscriptionCallbackWorkItem _vehicle_local_position_sub钩子（call），最终触发px4::WorkItem，进而WorkQueue创建任务，调用FlightModeManager::Run。

FlightModeManager::init
 ├──> !_vehicle_local_position_sub.registerCallback()
 ├──> _vehicle_local_position_sub.set_interval_us(20_ms);
 └──> _vehicle_local_position_sub.set_interval_us(20_ms);

3.5 Run

每次WorkQueue调用，都会执行到该函数，对飞行姿态进行调整。

FlightModeManager::Run
 ├──> <should_exit()>
 │   ├──> _vehicle_local_position_sub.unregisterCallback()
 │   ├──> exit_and_cleanup()
 │   └──> return
 ├──> perf_begin(_loop_perf)
 ├──> <_parameter_update_sub.updated()>
 │   ├──> _parameter_update_sub.copy(&param_update)
 │   └──> updateParams()
 ├──> <_vehicle_local_position_sub.update(&vehicle_local_position)>
 │   ├──> const hrt_abstime time_stamp_now = vehicle_local_position.timestamp_sample
 │   ├──> const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) / 1e6f), 0.0002f, 0.1f)
 │   ├──> _time_stamp_last_loop = time_stamp_now
 │   ├──> _home_position_sub.update()
 │   ├──> _vehicle_control_mode_sub.update()
 │   ├──> _vehicle_land_detected_sub.update()
 │   ├──> _vehicle_status_sub.update()
 │   ├──> start_flight_task()
 │   ├──> <_vehicle_command_sub.updated()>
 │   │   └──> handleCommand()
 │   └──> <isAnyTaskActive()>
 │       └──> generateTrajectorySetpoint(dt, vehicle_local_position)
 └──> perf_end(_loop_perf)

3.6 start_flight_task

FlightModeManager::start_flight_task
 ├──> <_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING>
 │   ├──> switchTask(FlightTaskIndex::None) // Do not run any flight task for VTOLs in fixed-wing mode
 │   └──> return 
 │
 │    // 飞行模式切换
 ├──> <_vehicle_status_sub.get().in_transition_mode && _vehicle_control_mode_sub.get().flag_control_altitude_enabled>
 │   └──> [Only run transition flight task if altitude control is enabled (e.g. in Altitdue, Position, Auto flight mode)]
 ├──> <_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET>
 │   └──> [Auto-follow me]
 ├──> <_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND>
 │   └──> [Emergency descend]
 ├──> <_vehicle_control_mode_sub.get().flag_control_auto_enabled>
 │   └──> [Auto related maneuvers]
 ├──> <_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || task_failure>
 │   └──> [manual position control]
 ├──> <_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || task_failure>
 │   └──> [manual altitude control]
 ├──> <_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT>
 │   └──> should_disable_task = false;
 │
 ├──> <task_failure>
 │   ├──> FlightTaskError error = switchTask(FlightTaskIndex::Failsafe);
 │   └──> <error != FlightTaskError::NoError>
 │       └──> switchTask(FlightTaskIndex::None)
 ├──> <should_disable_task && !task_failure>
 │   └──> switchTask(FlightTaskIndex::None)
 └──> _last_vehicle_nav_state = _vehicle_status_sub.get().nav_state

4. FlightModeManager飞行模式继承关系

根据类继承关系，分析如下：

ListNode
 └──> ModuleParams
     └──> FlightTask
         ├──> 【HK1】FlightTaskAuto
         ├──> FlightTaskAutoFollowTarget
         ├──> 【HK2】FlightTaskDescend
         ├──> 【HK3】FlightTaskFailsafe
         ├──> 【HK9】FlightTaskTransition
         └──> 【HK5】FlightTaskManualAltitude
             ├──> 【HK6】FlightTaskManualAltitudeSmoothVel
             |   ├──> 【HK4】FlightTaskManualAcceleration
             |   └──> FlightTaskOrbit
             └──> 【HK7】FlightTaskManualPosition
                 └──> 【HK8】FlightTaskManualPositionSmoothVel 

注：HKn:holybro_kakutef7使用，n表示第几种。

5. FlightModeManager模块工程结构

FlightModeManager模块工程结构如下图所示：

.
├── CMakeLists.txt
├── FlightModeManager.cpp
├── FlightModeManager.hpp
└── tasks
    ├── CMakeLists.txt
    ├── Auto                                            // 【HK1】
    │   ├── CMakeLists.txt
    │   ├── FlightTaskAuto.cpp
    │   └── FlightTaskAuto.hpp
    ├── AutoFollowTarget
    │   ├── CMakeLists.txt
    │   ├── FlightTaskAutoFollowTarget.cpp
    │   ├── FlightTaskAutoFollowTarget.hpp
    │   ├── follow_target_estimator
    │   └── follow_target_params.c
    ├── Descend                                         // 【HK2】
    │   ├── CMakeLists.txt
    │   ├── FlightTaskDescend.cpp
    │   └── FlightTaskDescend.hpp
    ├── Failsafe                                         // 【HK3】
    │   ├── CMakeLists.txt
    │   ├── FlightTaskFailsafe.cpp
    │   └── FlightTaskFailsafe.hpp
    ├── FlightTask
    │   ├── CMakeLists.txt
    │   ├── FlightTask.cpp
    │   └── FlightTask.hpp
    ├── ManualAcceleration                                // 【HK4】
    │   ├── CMakeLists.txt
    │   ├── FlightTaskManualAcceleration.cpp
    │   └── FlightTaskManualAcceleration.hpp
    ├── ManualAltitude                                    // 【HK5】
    │   ├── CMakeLists.txt
    │   ├── FlightTaskManualAltitude.cpp
    │   └── FlightTaskManualAltitude.hpp
    ├── ManualAltitudeSmoothVel                            // 【HK6】
    │   ├── CMakeLists.txt
    │   ├── FlightTaskManualAltitudeSmoothVel.cpp
    │   └── FlightTaskManualAltitudeSmoothVel.hpp
    ├── ManualPosition                                     // 【HK7】
    │   ├── CMakeLists.txt
    │   ├── FlightTaskManualPosition.cpp
    │   └── FlightTaskManualPosition.hpp 
    ├── ManualPositionSmoothVel                            // 【HK8】
    │   ├── CMakeLists.txt
    │   ├── FlightTaskManualPositionSmoothVel.cpp
    │   └── FlightTaskManualPositionSmoothVel.hpp
    ├── Orbit
    │   ├── CMakeLists.txt
    │   ├── FlightTaskOrbit.cpp
    │   └── FlightTaskOrbit.hpp
    └── Transition                                         // 【HK9】
        ├── CMakeLists.txt
        ├── FlightTaskTransition.cpp
        └── FlightTaskTransition.hpp

注：HKn:holybro_kakutef7使用，n表示第几种。

6. holybro_kakutef7支持的飞行模式

当前holybro_kakutef7代码中，使用如下飞行模式：
【HK1】Auto
【HK2】Descend
【HK3】Failsafe
【HK4】ManualAcceleration
【HK5】ManualAltitude
【HK6】ManualAltitudeSmoothVel
【HK7】ManualPosition
【HK8】ManualPositionSmoothVel
【HK9】Transition

7. 参考资料

【1】PX4开源软件框架简明简介
【2】PX4模块设计之十七：ModuleBase模块
【3】PX4模块设计之十三：WorkQueue设计
【4】PX4模块设计之十：PX4启动过程