22、多旋翼飞行器半自主控制与故障安全逻辑设计

多旋翼飞行器半自主控制与故障安全逻辑设计

1. 半自主控制模式设计实验

首先是半自主控制模式的设计实验，此实验包含模式切换控制器设计、仿真和飞行测试等步骤。

1.1 模式切换控制器设计

指定 RC 发射器的三位置开关作为模式切换开关，选择 CH5。根据 CH5 的 PWM 值范围来确定多旋翼飞行器的工作模式：
- 当 PWM 值在 [1000, 1400) 时，飞行器处于稳定模式。
- 当 PWM 值在 [1400, 1600) 时，飞行器处于高度保持模式。
- 当 PWM 值在 [1600, 2000) 时，飞行器处于悬停模式。

以下是模式切换的代码实现：

function control_mode = fcn(ch5)
%0:Stabilize mode
%1: Altitude hold mode
%2: loiter mlde
if ch5<1400
    control_mode=0;
elseif ch5<1600
    control_mode=1;
else
    control_mode=2;
end

1.2 仿真步骤

仿真分为软件在环（SIL）仿真和硬件在环（HIL）仿真。

• SIL 仿真

  1. 编写位置控制模型 ：将死区和位置控制器添加到稳定模式模型中，并将输入模式从 0 改为 2，表示悬停模型可