自定义飞行模式

添加自定义模式

本文主要参考APM官网
(https://ardupilot.org/dev/docs/apmcopter-adding-a-new-flight-mode.html)
以及怒飞垂云的博客
(http://www.nufeichuiyun.com/?p=277)

APM的代码量非常大，想要完全搞明白不太可能，快速入门的方法就是仿照着大牛们写过的程序，自己试着写一下，比如说我现在要添加一个最简单的矩形航线，我们可以先查看github上所提供的所有模式，然后比较一下，看自己想添加的模式和哪个相近，我这里添加的模式就可以仿照circle这个模式来写。

1 修改飞行模式流程（以ArduCopter为例）

下面是ArduCopter的架构视图

下图显示了手动模式（例如Stabilize, Acro, Drift）的体系结构

下图显示了自主模式（即RTL，Guided，Auto）的体系结构

在mode.h中为该模式定义一个新类。复制一个类似的现有模式的类定义并仅更改类名称（即，将“类ModeStabilize”复制并重命名为“类ModeNewMode”）可能是最容易的。
新类应从Mode类继承 run()和init().所以说每个类的定义大致如下：

public:
   // inherit constructor
   using Mode::Mode;
   bool init(bool ignore_checks) override;
   void run() override;

protected:
   const char *name() const override { return "NEWMODE"; }
   const char *name4() const override { return "NEWM"; }

这些是必须有的。

还有一些返回true / false的简单方法，您可能想覆盖该控制功能，例如是否可以在新模式下布防Copter：

bool requires_GPS() const override { return false/true; }
bool has_manual_throttle() const override { return true/false; }
bool allows_arming(bool from_gcs) const override { return true; };
bool is_autopilot() const override { return false; }

其中requires_GPS()和has_manual_throttle()两个函数返回值依据飞行模式来确定，一般情况下手动模式（例如Stabilize, Acro）是不需要GPS但需要油门。
一般情况下自动模式（RTL, Guided, Auto）需要GPS不需要油门。
当然某些是除外的，这里笔者是为了方便理解这样归类的。

1.1 init()函数

基于类似的模式（例如mode_stabilize.cpp 或mode_circle.cpp）创建一个新的mode_ <新飞行模式> .cpp文件 。这个新文件可能应该实现init()将在飞机首次进入模式时调用的方法。如果飞机可以进入该模式，则此函数应返回true，否则返回false。以下是mode_rtl.cpp的init方法的摘录，其中显示了除非设置了原始位置，否则飞机无法进入RTL模式。

// circle_init - initialise circle controller flight mode
bool Copter::ModeCircle::init(bool ignore_checks)
{
    if (copter.position_ok() || ignore_checks) {
        pilot_yaw_override = false;

        // initialize speeds and accelerations
        pos_control->set_speed_xy(wp_nav->get_speed_xy());
        pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
        pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
        pos_control->set_accel_z(g.pilot_accel_z);

        // initialise circle controller including setting the circle center based on vehicle speed
        copter.circle_nav->init();

        return true;
    }else{
        return false;
    }
}
}

1.2 run()函数

以下是mode_stabilize.cpp的更新方法（称为每秒400次）的摘录，该更新方法对用户的输入进行解码，然后将新目标发送给姿态控制器。

void ModeStabilize::run()
{
    // convert pilot input to lean angles
    float target_roll, target_pitch;
    get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);

    // get pilot's desired yaw rate
    float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

    // code that sets motor spool state omitted

    // call attitude controller
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);

    // output pilot's throttle
    attitude_control->set_throttle_out(get_pilot_desired_throttle(), true, g.throttle_filt);

2 实战

笔者根据APM官网所给的教程以及参考开头提到的博客自己设计了两个简单的飞行模式，一个是按照举行路线飞，另一个是按照正六边形路线飞。以下是具体实现步骤：

1先复制一份Circle.cpp

（1）将mode_Circle.cpp文件复制，在这个代码的基础上修改，生成自己的模式。
（2）先找到mode_Circle.cpp文件，可以根据这里的函数查看它是在哪里定义的（Ctrl+f3），复制一份放在要仿照的class的下面，将class的名称改为自定义的名称:


然后按照自己的需求修改成员变量和成员函数。

2修改函数

然后返回到Rectangle.cpp，将modeCircle全部替换成ModeRec,删减及修改函数。
下面是mode_Rec.cpp

#include "Copter.h"

/*
 * Init and run calls for guided flight mode
 */

//init()和run()是两个必备的

// guided_init - initialise guided controller
//初始化时应该计算飞行路径
bool Copter::ModeRec::init(bool ignore_checks)
{

    if (copter.position_ok() || ignore_checks) {
        // initialise yaw
        auto_yaw.set_mode_to_default(false);

        //将当前航点清零
        path_num=0;
        //产生要飞行的路径
        generate_path();

        // start in position control mode
        //产生路径之后飞控就会启动位置控制引导
        //飞机按照规划路径走
        pos_control_start();
        return true;
    }else{
        return false;
    }
}

void Copter:: ModeRec::generate_path()
{
    float radius_cm=1000.0;
    //停止点作为第0航点
    wp_nav->get_wp_stopping_point(path[0]);
    path[1]=path[0]+Vector3f(1.0f,0,0)*radius_cm;
    path[2]=path[1]+Vector3f(0.0,-1.0f,0.0)*radius_cm;
    path[3]=path[2]+Vector3f(-2.0f,0.0,0.0)*radius_cm;
    path[4]=path[3]+Vector3f(0.0,2.0f,0.0)*radius_cm;
    path[5]=path[4]+Vector3f(2.0f,0.0,0.0)*radius_cm;
    path[6]=path[1];
}

// initialise guided mode's position controller
void Copter::ModeRec::pos_control_start()
{

    // initialise waypoint and spline controller
    //航点导航库的初始化
    wp_nav->wp_and_spline_init();

    // no need to check return status because terrain data is not used

    //将当前航点设为目标停止点
    wp_nav->set_wp_destination(path[0], false);

    // initialise yaw
    //航向控制设为默认
    auto_yaw.set_mode_to_default(false);
}


// guided_run - runs the guided controller
// should be called at 100hz or more
void Copter::ModeRec::run()
{
    if(path_num<6){
            //判断是否到达目标航点
           if(wp_nav->reached_wp_destination()){
               //到达目标航点，切换到下一行点
               path_num++;
               wp_nav->set_wp_destination(path[path_num],false);

           }

        }

       pos_control_run();
 }


// guided_pos_control_run - runs the guided position controller
// called from guided_run
void Copter::ModeRec::pos_control_run()
{
    // if not auto armed or motors not enabled set throttle to zero and exit immediately
    if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
        zero_throttle_and_relax_ac();
        return;
    }

    // process pilot's yaw input
    float target_yaw_rate = 0;
    if (!copter.failsafe.radio) {
        // get pilot's desired yaw rate
        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
        if (!is_zero(target_yaw_rate)) {
            auto_yaw.set_mode(AUTO_YAW_HOLD);
        }
    }

    // set motors to full range
    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

    // run waypoint controller
    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());

    // call z-axis position controller (wpnav should have already updated it's alt target)
    pos_control->update_z_controller();

    // call attitude controller
    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
        // roll & pitch from waypoint controller, yaw rate from pilot
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
    } else if (auto_yaw.mode() == AUTO_YAW_RATE) {
        // roll & pitch from waypoint controller, yaw rate from mavlink command or mission item
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.rate_cds());
    } else {
        // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);
    }
}

这里主要增加了generate()这个函数，用来产生要飞行的路径，起始点的位置为path[0],且这也是停止点的位置。不同的飞行路径只需要根据几何关系修改generate_path()即可。
例如，若要规划一个正六边形的路径只需要改成下面这样即可：

void Copter:: ModeSixB::generate_path()
{
    float radius_cm=1000.0;
    //停止点作为第0航点
    wp_nav->get_wp_stopping_point(path[0]);
    path[1]=path[0]+Vector3f(cosf(radians(30)),-sinf(radians(30)),0)*radius_cm;
    path[2]=path[0]+Vector3f(0.0,-1.0f,0.0)*radius_cm;
    path[3]=path[0]+Vector3f(-cosf(radians(30)),-sinf(radians(30)),0)*radius_cm;
    path[4]=path[0]+Vector3f(-cosf(radians(30)),sinf(radians(30)),0)*radius_cm;
    path[5]=path[0]+Vector3f(0.0,1.0f,0.0)*radius_cm;
    path[6]=path[0]+Vector3f(cosf(radians(30)),sinf(radians(30)),0)*radius_cm;
    path[7]=path[1];
}

这样还没完，还需要其他的修改。接下来要考虑如何切换飞行模式，其他的飞行模式都在mode.cpp中有所定义，以case形式。如下图所示：

在最下面添加case REC: 下面的名称仿照上面的，查找上面的FOLLOW的定义：

这样就可以了。我们还以FOLLOW为例，我们还需要查看mode_follow的定义，依旧按住F3跳到定义出，可以看出这是在copter.cpp找那个定义的：

我们只需要在下面跟着定义一下名称。
你以为这样就完了么？不，还差最后一步，我们需要在地面站中显示出这个模式，我们需要在GCS_Mavlink.cpp中 switch…case…一下这个模式，如下图所示：

这样就完成了所有的步骤。效果图如下：