每天读一点儿APM(PIX)代码之二：飞行模式FlightMode

本文对apm的飞行模式管理和切换等机制进行介绍，最后通过实例，分别修改apm飞控端代码以及missionplanner代码，将apm的飞行模式由6个扩展到了9个。因为作者最近一直在修改apm的固定翼程序，所以讲更多的以ArduPlane为例子进行介绍，不过ArduCopter与之思路是相同的，很容易触类旁通。

一．飞行模式的本质

Apm开发团队建议开发者通过扩展飞行模式的方式对apm功能进行扩展，植入自己所需要的飞行逻辑，由此可见飞行模式之重要性。实际上，FlightMode在apm中就是一个枚举类型，不同的状态即对应与不同的飞行模式。可见defines.h。

enum FlightMode {
    MANUAL        = 0,
    CIRCLE        = 1,
    STABILIZE     = 2,
    TRAINING      = 3,
    ACRO          = 4,
    FLY_BY_WIRE_A = 5,
    FLY_BY_WIRE_B = 6,
    CRUISE        = 7,
    AUTOTUNE      = 8,
    AUTO          = 10,
    RTL           = 11,
    LOITER        = 12,
    GUIDED        = 15,
    INITIALISING  = 16,
TAKEOFF  = 17
};
           

在飞控的任务表中（scheduler_tasks），定义了一个关键的不断循环的调度函数updata_flight_mode ,该函数会根据FlightMode的不同值，执行不同的逻辑功能，由此体现出不同飞行模式的不同。

二．飞行模式的切换

在ArduPlane的飞控任务表（scheduler_tasks）中，有一个read_control_switch的函数（对于多旋翼arduCopter是从rc_loop调用），它检测输入的飞行模式控制通道的变化，并改变FlightMode状态。请见control_modes.ino.在该函数中，先后执行两块逻辑功能:

首先执行readSwitch(),从输入通道中直接判断出对应与飞行模式列表（flight_modes）中的序列值（ArduCopter没有将这个逻辑单独封装为函数，直接调用了）。flight_modes是飞控板载rom中存储的，飞行模式集合的首元素的指针。这一飞行模式集合实际上也是apm的一种固件参数，即可以通过MissionPlanner的参数设置进行修改，也可以通过特别开发的可视化界面修改。APM的rom中最多可以存储255个飞行模式。

然后，获知当前输入所代表的飞行模式后，调用set_mode()函数，修改飞行模式。该函数的主要功能可以分为三部分：检查飞行模式切换是否被允许；将当前飞行模式的各种状态归零；切换到新的飞行模式，并进行新模式所必需的初始化工作(ArduPlane中主要是修改油门的自动控制方式，ArduCopter中则是调用飞行模式的Init())。

需要注意的是，切换模式中采用了两种特殊机制，以保证模式切换过程的平滑稳定:

1.只有当前输入的模式与前输入模式不同是，才会修改飞行模式。这个机制解决了飞控逻辑中，又算法自发修改了飞行模式，与遥控输入飞行模式不符时带来的冲突。

2.只有连续两个周期检查到输入的飞行模式发生变化时，才会修改飞行模式，这实际上是对信号输入进行了时间平滑，可以解决信号瞬间改变导致飞行模式突变的问题，这种方法在数字电路中十分常用。

三．飞行模式作了什么？

不同的飞行模式如何对飞行器产生不同的控制功能呢？可以从update_flight_mode()中找到答案。在固定翼ArduPlane中，这一函数被写入飞控任务表，而在多悬翼ArduCopter中 ,这一函数在fastloop循环内。

switch (control_mode) {
        case ACRO:
            #if FRAME_CONFIG == HELI_FRAME
                heli_acro_run();
            #else
                acro_run();
            #endif
            break;
        case STABILIZE:
            #if FRAME_CONFIG == HELI_FRAME
                heli_stabilize_run();
            #else
                stabilize_run();
            #endif
            break;
        case ALT_HOLD:
            althold_run();
            break;
        case AUTO:
            auto_run();
            break;
        case CIRCLE:
            circle_run();
            break;
....
....
           

如图所示 ，该函数内部，根据不同的飞行模式，调用相应的功能，因此 。ArduPlane是直接执行功能代码，ArduCopter则将不同模式的功能代码统一封装在xxx_run()函数中调用，其本质是一样的。更详细的分析，请参看下一篇文章。

至此，本文对Apm飞行模式的定义、调度、切换、执行等进行了介绍。本人将apm的6种飞行模式扩展到了9种，并且可以通过遥控器上两个三端开关进行调节，相关的源代码，可以在github上查看和下载。下面对修改过程做简单介绍：

对ArduPlane片上程序的修改：

1.在parameters.h中添加新增的 3个飞行模式参数和新增的模式控制通道

k_param_flight_mode7,
k_param_flight_mode8,
k_param_flight_mode9
以及
k_param_flight_mode_channel2
           

 2.修改readSwitch函数，根据两个三段开关正好可以拟合9种变量值

static uint8_t readSwitch(void)
{
uint16_t pulsewidth = hal.rcin->read(g.flight_mode_channel - 1);
uint16_t pulsewidth2 = hal.rcin->read(g.flight_mode_channel2 - 1);
if (pulsewidth <= 900 || pulsewidth >= 2200) return 255;
if (pulsewidth2 <= 900 || pulsewidth >= 2200) return 255;
if (pulsewidth < 1300){
if (pulsewidth2 < 1300) return 0;
else if (pulsewidth2 < 1700)	return 1;
else return 2;
}
else if (pulsewidth < 1700){
if (pulsewidth2 < 1300) return 3;
else if (pulsewidth2 < 1700) return 4;
else return 5;
}
else{
if (pulsewidth2 < 1300) return 6;
else if (pulsewidth2 < 1700) return 7;
else return 7;
}
return 0;
}
           

3.在Parameters.ino中添加新参数的定义，如

// @Param: FLTMODE8
// @DisplayName: FlightMode9
// @Description: Flight mode for switch position 8 (1750 to 2049)
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOT<pre name="code" class="csharp">(int)(float)MainV2.comPort.MAV.param["FLTMODE_CH2"];
           

UNE,10:Auto,11:RTL,12:Loiter,15:Guided,17:TAKEOFF// @User: StandardGSCALAR(flight_mode9, "FLTMODE9", FLIGHT_MODE_9),

4.在config.h中修改两个飞行模式通道的默认值为5和6

#ifndef FLIGHT_MODE_CHANNEL
 # define FLIGHT_MODE_CHANNEL    5
#endif
#ifndef FLIGHT_MODE_CHANNEL2
# define FLIGHT_MODE_CHANNEL2    6
#endif
           

片上程序修改完毕后，已经可以在地面站通过参数设置修改新增的参数来使用9种飞行模式，也可以如下修改MissionPlaner代码，通过界面使用9种飞行模式:

1.APM地面站的飞行模式设置界面在GCSViews\ConfigurationView下，由ConfigFlightModes这一界面类实现。所以我们首先修改它，并添加飞行模式6到9.

2.在上述类中，有一个定时器不断循环，检查飞控获得的遥控输入通道，根据此计算当前飞行模式，显示在界面中，见timer_Tick()函数。修改其中的readSwitch函数，使得从两个通道计算飞行模式的逻辑与修改过的apm单片机上相同。

此处，(int)(float)MainV2.comPort.MAV.param["FLTMODE_CH2"];这句可以获取飞控中的参数值。

3.MissionPlanner软件采用为了实现全球化，通过资源指定界面中的文字。因此需要修改资源文件ConfigFlightModes.zh-Hans.resx修改界面上的相关文字。

上述修改，都在下属github中托管，欢迎大家参考:

Arduplane:https://github.com/xttyyy/arduplane3.2.1-takeoff-8mode/点击打开链接

ArduPlane.hex是修改后的的ArduPlane3.21版本固件，可以直接烧录到APM中.

Missionplanner:https://github.com/xttyyy/MissionPlanner/点击打开链接

至此，飞控片上代码和地面站已经修改完毕，可以正确的使用9种飞行模式了.下一期，将会以固定翼为例，分析apm代码对飞行器的稳定控制，并添加一个直接的起飞模式，简化apm固定翼的自动起飞。