文章目录
1. 上电启动2. 进入主循run3. 拆分函数分析4. SwitchTask5. 形成控制点
我们把上一节commander的内容一起作简单的总结，一般我们上电从启动脚本rcS开始，把各个模块拉起来，此时，系统进入未解锁且未选择导航状态的待机态，等待健康检查与模式请求。
到这里，commander会建立状态机、合并各类健康/故障信息。此时，commander会充当一个裁判员，发布最终裁决，导航状态（nav_state）
手动类：ACRO / STABILIZED(MANUAL) / ALTCTL（定高）/ POSCTL（定点）
自动类：AUTO_MISSION / AUTO_RTL / AUTO_LAND / AUTO_TAKEOFF / HOLD(LOITER) 等
离板：OFFBOARD
并同时发布对应的控制回路开关vehicle_control_mode（哪些回路开启：位置/速度/姿态/油门等开关位）
下面是模式对应的数字序号
uint8 nav_state # Currently active mode
uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode
uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode
uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode
uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode
uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode
uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode
uint8 NAVIGATION_STATE_POSITION_SLOW = 6
uint8 NAVIGATION_STATE_FREE5 = 7
uint8 NAVIGATION_STATE_ALTITUDE_CRUISE = 8 # Altitude with Cruise mode
uint8 NAVIGATION_STATE_FREE3 = 9
uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode
uint8 NAVIGATION_STATE_FREE2 = 11
uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control)
uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode
uint8 NAVIGATION_STATE_OFFBOARD = 14
uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode
uint8 NAVIGATION_STATE_FREE1 = 16
uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff
uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land
uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow
uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target
uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle
uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter
uint8 NAVIGATION_STATE_EXTERNAL1 = 23
uint8 NAVIGATION_STATE_EXTERNAL2 = 24
uint8 NAVIGATION_STATE_EXTERNAL3 = 25
uint8 NAVIGATION_STATE_EXTERNAL4 = 26
uint8 NAVIGATION_STATE_EXTERNAL5 = 27
uint8 NAVIGATION_STATE_EXTERNAL6 = 28
uint8 NAVIGATION_STATE_EXTERNAL7 = 29
uint8 NAVIGATION_STATE_EXTERNAL8 = 30
uint8 NAVIGATION_STATE_MAX = 31
到这里，进入飞行管理层，此时会根据你的自己的飞行模式进行选择，你可以从遥控器、QGC、offboard等方式选择飞行模式，假如是自动类的，那么Navigator会接入，例如Mission（自动航线）发布“任务点三元组” position_setpoint_triplet，假如是其他手动类的，FlightTasks会根据你的操控生成trajectory_setpoint，然后给到位置控制器，这个FlightTasks是属于多旋翼控制类的，相当于mc_pos_control 里的任务插件，把“意图/摇杆/离板输入”细化成可跟踪的 trajectory_setpoint。
值得注意的是！！有两种模式是不会进入FlightTasks的，特技arco和STABILIZED模式是直接从姿态控制器出发的，和APM一样的。
一般我们设定，CH1→Roll（横滚/副翼）CH2→Pitch（俯仰/升降）CH3→Throttle（油门CH4→Yaw（偏航/方向） CH5一般为模式切换，最多有6种模式，这个模式切换通道可以参考（模式切换介绍）
1. 上电启动
和以往的的启动文件一样，在ROMFS/px4fmu_common/init.d/rcS中会设定rc.vehicle_setup，在这里会分流（按机型分流）rc.mc_apps / rc.fw_apps / rc.vtol_apps。这里面会真正 start 各个模块，我们多旋翼用rc.mc_apps，同时在rcS中
在rc.mc_apps里会对飞行模式管理进行启动
flight_mode_manager start
和以往一样，会跳到src/modules/flight_mode_manager/FlightModeManager.cpp
extern "C" __EXPORT int flight_mode_manager_main(int argc, char *argv[])
{
return FlightModeManager::main(argc, argv);
}
然后又返回platforms/common/include/px4_platform_common/module.h 开始一步一步启动，这个过程可以参考上一节中commander的启动方式，一摸一样的，在这里不详细放出来看。
但是我们得知道该模块的继承类
class FlightModeManager : public ModuleBase<FlightModeManager>, public ModuleParams, public px4::WorkItem
{
2. 进入主循run
run代码主要做三件事：
1）处理参数变更 & 指令；
2）按当前 nav_state 选择/切换合适的 FlightTask；
3）计算并发布新的 trajectory_setpoint（位置/速度/加速度/航向参考）
void FlightModeManager::Run()
{
if (should_exit()) {
_vehicle_local_position_sub.unregisterCallback();
exit_and_cleanup();
return;
}
perf_begin(_loop_perf);
// Check if parameters have changed
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(¶m_update);
updateParams();
}
// generate setpoints on local position changes
vehicle_local_position_s vehicle_local_position;
if (_vehicle_local_position_sub.update(&vehicle_local_position)) {
const hrt_abstime time_stamp_now = vehicle_local_position.timestamp_sample;
// Guard against too small (< 0.2ms) and too large (> 100ms) dt's.
const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) / 1e6f), 0.0002f, 0.1f);
_time_stamp_last_loop = time_stamp_now;
_vehicle_control_mode_sub.update();
_vehicle_land_detected_sub.update();
_vehicle_status_sub.update();
start_flight_task();
if (_vehicle_command_sub.updated()) {
handleCommand();
}
tryApplyCommandIfAny();
if (isAnyTaskActive()) {
generateTrajectorySetpoint(dt, vehicle_local_position);
}
}
perf_end(_loop_perf);
}
我们可以从上面清楚看到，进入了该函数
start_flight_task();
我们重点学习一下这个函数：
这条函数根据 Commander 下发的 nav_state 和控制回路开关，选择并切换合适的 FlightTask（多旋翼位置侧的“行为插件”）
// 作用：根据 Commander 的最终导航状态 nav_state 与控制回路开关，选择/切换合适的 FlightTask；
// 若切换失败，则按“降级链”依次尝试更保守的任务（POSCTL→ALTCTL→DESCEND→FAILSAFE→None）。
void FlightModeManager::start_flight_task()
{
// ① 早退条件：以下场景不运行多旋翼 FlightTask
// - VTOL 处于固定翼模式（vehicle_type == FIXED_WING）
// - 使用 EXTERNAL1~EXTERNAL8 这类外部自定义模式（由外部处理）
if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)
|| ((_vehicle_status_sub.get().nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1)
&& (_vehicle_status_sub.get().nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL8))) {
switchTask(FlightTaskIndex::None); // 不运行任何 FlightTask
return;
}
// ② VTOL 过渡优先：仅当处于过渡 in_transition_mode 且 已开启高度环 时，启用 Transition 任务
// （例如从多旋翼向固定翼过渡，需要能控 Z 轴）
if (_vehicle_status_sub.get().in_transition_mode && _vehicle_control_mode_sub.get().flag_control_altitude_enabled) {
switchTask(FlightTaskIndex::Transition);
return;
}
// ③ 控制切换过程中的状态变量
bool found_some_task = false; // 是否至少尝试/选择了某个任务（无论成功与否）
bool matching_task_running = true; // “期望匹配的任务”最终是否成功在运行（用于打印一次性错误）
bool task_failure = false; // 最近一次 switchTask 是否失败（失败会触发降级链）
const bool nav_state_descend = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND);
// ④ Follow-me 自动跟随（AUTO_FOLLOW_TARGET）
if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
found_some_task = true;
FlightTaskError error = FlightTaskError::InvalidTask;
#if !defined(CONSTRAINED_FLASH) // 某些小闪存板会裁掉大任务（如 Follow/Orbit）
error = switchTask(FlightTaskIndex::AutoFollowTarget);
#endif // !CONSTRAINED_FLASH
if (error != FlightTaskError::NoError) {
matching_task_running = false; // 期望任务没跑起来
task_failure = true; // 标记失败，后续走降级
}
}
// ⑤ 环绕（ORBIT），且上次命令未失败（避免反复尝试）
if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT)
&& !_command_failed) {
found_some_task = true;
FlightTaskError error = FlightTaskError::InvalidTask;
#if !defined(CONSTRAINED_FLASH)
error = switchTask(FlightTaskIndex::Orbit);
#endif // !CONSTRAINED_FLASH
if (error != FlightTaskError::NoError) {
matching_task_running = false;
task_failure = true; // 失败 → 触发后续降级
}
}
// ⑥ 通用自动模式对接（Navigator）：只要自动控制开且不是 Descend，就尝试 Auto 任务
if (_vehicle_control_mode_sub.get().flag_control_auto_enabled
&& !nav_state_descend) {
found_some_task = true;
if (switchTask(FlightTaskIndex::Auto) != FlightTaskError::NoError) {
matching_task_running = false;
task_failure = true; // 自动类失败 → 触发降级到手飞/更保守任务
}
}
// ⑦ Position Slow（更保守/限速更严的位控）
if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW) {
found_some_task = true;
FlightTaskError error = switchTask(FlightTaskIndex::ManualAccelerationSlow);
task_failure = (error != FlightTaskError::NoError);
}
// ⑧ 手飞位控（POSCTL）或 上一步失败需降级 到位控
if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL) || task_failure) {
found_some_task = true;
FlightTaskError error = FlightTaskError::NoError;
// MPC_POS_MODE 决定手飞位控的风格
// - 0：经典位置型（ManualPosition）
// - 4：加速度/jerk 限制型（ManualAcceleration，默认为 4；其他值也强制回 4）
switch (_param_mpc_pos_mode.get()) {
case 0:
error = switchTask(FlightTaskIndex::ManualPosition);
break;
case 4:
default:
if (_param_mpc_pos_mode.get() != 4) {
PX4_ERR("MPC_POS_MODE %" PRId32 " invalid, resetting", _param_mpc_pos_mode.get());
_param_mpc_pos_mode.set(4); // 强制回默认
_param_mpc_pos_mode.commit();
}
error = switchTask(FlightTaskIndex::ManualAcceleration);
break;
}
task_failure = (error != FlightTaskError::NoError); // 位控是否成功
matching_task_running = matching_task_running && !task_failure; // 用于后面一次性报错
}
// ⑨ 手飞定高（ALTCTL）或 上一步失败需继续降级 到定高
if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL) || task_failure) {
found_some_task = true;
FlightTaskError error = FlightTaskError::NoError;
// MPC_POS_MODE 也影响定高的风格
// - 0：经典定高（ManualAltitude）
// - 3/其他：平滑速度型定高（ManualAltitudeSmoothVel，默认）
switch (_param_mpc_pos_mode.get()) {
case 0:
error = switchTask(FlightTaskIndex::ManualAltitude);
break;
case 3:
default:
error = switchTask(FlightTaskIndex::ManualAltitudeSmoothVel);
break;
}
task_failure = (error != FlightTaskError::NoError); // 定高是否成功
matching_task_running = matching_task_running && !task_failure;
}
// ⑩ 应急下降（DESCEND）或 上一步失败继续降级 到应急下降
if (nav_state_descend || task_failure) {
found_some_task = true;
FlightTaskError error = switchTask(FlightTaskIndex::Descend); // 极简策略：控制下沉/限位
task_failure = (error != FlightTaskError::NoError);
matching_task_running = matching_task_running && !task_failure;
}
// ⑪ 如果上述都失败 → 进入 Failsafe（最后兜底任务）
if (task_failure) {
// Failsafe 成功则 found_some_task=true；否则保持 false
found_some_task = (switchTask(FlightTaskIndex::Failsafe) == FlightTaskError::NoError);
}
// ⑫ 如果没有找到任何需要运行的任务（例如 ACRO/STAB 或 FW），明确设为 None
if (!found_some_task) {
switchTask(FlightTaskIndex::None);
}
// ⑬ 仅在已上锁且第一次失败时打印错误，避免刷屏
if (!matching_task_running && _vehicle_control_mode_sub.get().flag_armed && !_no_matching_task_error_printed) {
PX4_ERR("Matching flight task was not able to run, Nav state: %" PRIu8 ", Task: %" PRIu32,
_vehicle_status_sub.get().nav_state, static_cast<uint32_t>(_current_task.index));
}
// 记录这次是否已打印过，以抑制重复报错
_no_matching_task_error_printed = !matching_task_running;
}
3. 拆分函数分析
如果vehicle_type == FIXED_WING，那么不用多旋翼 FlightTask。
EXTERNAL1~8：预留给外部自定义模式，FMM不介入。
结果：直接切 None，后面都不执行。
if (vehicle_type == FIXED_WING
|| nav_state ∈ [EXTERNAL1, EXTERNAL8]) { switchTask(None); return; }
过渡优先：VTOL 过渡期的特殊任务，先用 Transition 任务稳定 Z 轴，过渡时至少得能控高度，才能安全换形态。
if (in_transition_mode && flag_control_altitude_enabled) {
switchTask(Transition); return;
}
三个“流程状态位”，其中task_failure 是“降级开关”：一旦置真，后面会尝试更保守的任务。
bool found_some_task=false; // 是否尝试/命中了某类任务
bool matching_task_running=true; // 期望的任务是否成功在跑（用于一次性报错）
bool task_failure=false; // 最近一次 switchTask 是否失败（触发降级用）
bool nav_state_descend = (nav_state == DESCEND);
FollowMe 分支（自动跟随）
if (nav_state == AUTO_FOLLOW_TARGET) { switchTask(AutoFollowTarget); ... }
Orbit 分支（绕圈）
if (nav_state == ORBIT && !_command_failed) { switchTask(Orbit); ... }
“自动类总入口”：Auto 任务（对接 Navigator），只要 Commander 打开“自动控制”（flag_control_auto_enabled=1），且不是应急下降，就先尝试通用 Auto：由 Navigator 提供 position_setpoint_triplet 等任务点，Auto 负责连续化为 trajectory_setpoint。
失败 → 交给手飞链路继续降级。
if (flag_control_auto_enabled && !nav_state_descend) {
if (switchTask(Auto) != NoError) task_failure=true;
}
Position Slow（“慢速位控”）专门的“限速更严/响应更平”的位控模式（一般用于训练/安全场景）。
if (nav_state == POSITION_SLOW) { switchTask(ManualAccelerationSlow); ... }
POSCTL 分支（手飞位控）+ 可被“失败”强行触发，两种手感：
ManualPosition（老派，“像在拖拽位置/速度”）
ManualAcceleration（现代默认，“像在给加速度/jerk 指令”，更顺滑/可预期）
if (nav_state == POSCTL || task_failure) {
switch (MPC_POS_MODE) {
case 0: switchTask(ManualPosition); break; // 经典“位置型”手感
case 4:
default:
// 非法值强制回4（现代默认）：加速度/jerk 限制的手感，更平顺
if (MPC_POS_MODE != 4) { 重置为4并commit; }
switchTask(ManualAcceleration); break;
}
task_failure = (error != NoError);
matching_task_running &= !task_failure;
}
ALTCTL 分支（手飞定高）+ 继续降级，若位控失败，再退到只控高度的手飞（水平不走位置环）
if (nav_state == ALTCTL || task_failure) {
switch (MPC_POS_MODE) {
case 0: switchTask(ManualAltitude); break; // 经典定高（Z位置）
case 3:
default: switchTask(ManualAltitudeSmoothVel); break; // 平滑速度型定高（默认）
}
task_failure = (error != NoError);
matching_task_running &= !task_failure;
}
DESCEND 分支（应急下降）+ 再不行继续降
if (nav_state_descend || task_failure) {
switchTask(Descend);
task_failure = (error != NoError);
matching_task_running &= !task_failure;
}
FAILSAFE 兜底
if (task_failure) { found_some_task = (switchTask(Failsafe) == NoError); }
None（完全不需要 FlightTask 的场合
if (!found_some_task) { switchTask(None); }
4. SwitchTask
上面run中会根据选择多次“调用（call）switchTask(…)。不是“跳转到别的线程/流程”，而是在本函数里同步地调用switchTask()去切换/激活某个 FlightTask 插件，然后根据它的返回码继续往下执行当前函数逻辑。
FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
{
// switch to the running task, nothing to do
if (new_task_index == _current_task.index) {
return FlightTaskError::NoError;
}
// Save current setpoints for the next FlightTask
trajectory_setpoint_s last_setpoint = FlightTask::empty_trajectory_setpoint;
if (isAnyTaskActive()) {
last_setpoint = _current_task.task->getTrajectorySetpoint();
}
if (_initTask(new_task_index)) {
// invalid task
return FlightTaskError::InvalidTask;
}
if (!isAnyTaskActive()) {
// no task running
return FlightTaskError::NoError;
}
// activation failed
if (!_current_task.task->updateInitialize() || !_current_task.task->activate(last_setpoint)) {
_current_task.task->~FlightTask();
_current_task.task = nullptr;
_current_task.index = FlightTaskIndex::None;
return FlightTaskError::ActivationFailed;
}
_command_failed = false;
return FlightTaskError::NoError;
}
其中会初始化新的任务，这里的各种任务就是我们文章第一幅图那些模式序号
int FlightModeManager::_initTask(FlightTaskIndex task_index)
{
// disable the old task if there is any
if (_current_task.task) {
_current_task.task->~FlightTask();
_current_task.task = nullptr;
_current_task.index = FlightTaskIndex::None;
}
switch (task_index) {
case FlightTaskIndex::None:
// already disabled task
break;
case FlightTaskIndex::Auto:
_current_task.task = new (&_task_union.Auto) FlightTaskAuto();
break;
case FlightTaskIndex::Descend:
_current_task.task = new (&_task_union.Descend) FlightTaskDescend();
break;
case FlightTaskIndex::Failsafe:
_current_task.task = new (&_task_union.Failsafe) FlightTaskFailsafe();
break;
case FlightTaskIndex::ManualAcceleration:
_current_task.task = new (&_task_union.ManualAcceleration) FlightTaskManualAcceleration();
break;
case FlightTaskIndex::ManualAccelerationSlow:
_current_task.task = new (&_task_union.ManualAccelerationSlow) FlightTaskManualAccelerationSlow();
break;
case FlightTaskIndex::ManualAltitude:
_current_task.task = new (&_task_union.ManualAltitude) FlightTaskManualAltitude();
break;
case FlightTaskIndex::ManualAltitudeSmoothVel:
_current_task.task = new (&_task_union.ManualAltitudeSmoothVel) FlightTaskManualAltitudeSmoothVel();
break;
case FlightTaskIndex::ManualPosition:
_current_task.task = new (&_task_union.ManualPosition) FlightTaskManualPosition();
break;
case FlightTaskIndex::Transition:
_current_task.task = new (&_task_union.Transition) FlightTaskTransition();
break;
case FlightTaskIndex::AutoFollowTarget:
_current_task.task = new (&_task_union.AutoFollowTarget) FlightTaskAutoFollowTarget();
break;
case FlightTaskIndex::Orbit:
_current_task.task = new (&_task_union.Orbit) FlightTaskOrbit();
break;
default:
// invalid task
return 1;
}
// task construction succeeded
_current_task.index = task_index;
return 0;
}
5. 形成控制点
在之前run的代码中中最后会形成控制点，这个代码就两个部分：
成功：从任务拿 trajectory_setpoint + constraints → 打时间戳 → 发布 →（若起落架变化）下发起落架命令。
失败：发布 NaN setpoint/constraints，让控制器触发紧急保护；若仍在起飞前，会reActivate 以防 setpoint 跳变。
generateTrajectorySetpoint(dt, vehicle_local_position);
我们关注比较重要的代码：
//更新任务初始化&&任务更新
if (_current_task.task->updateInitialize() && _current_task.task->update())
{
// setpoints and constraints for the position controller from flighttask
///飞行任务中位置控制器的设定点和约束
setpoint = _current_task.task->getTrajectorySetpoint();
constraints = _current_task.task->getConstraints();
}
其中(_current_task.task->updateInitialize()和_current_task.task->update()都是指向的是虚函数，是指向基类 FlightTask 的指针，其子类是是各个控制模式下的对象，例如FlightTaskManualPosition。
大概讲到这里，后续具体到哪个模式可以看CSDN博客，这里以位置控制为例挺详细
© 版权声明
文章版权归作者所有，未经允许请勿转载。
相关文章
暂无评论...
