通过本节课，讲解如何添加自定义的飞行模式，帮助大家深入了解APM固件二次开发的流程。

1 功能目标

添加一种“五角星航线模式”飞行模式：切入此模式后，飞控自动按照“五角星航线”进行自动飞行。

2 添加代码

说明：飞行模式定义于mode.h文件中，Mode为基类型，类型ModeGuided即对应引导模式，以此为参照，创建我们自己的飞行模式，名称为DRAWSTAR，
类型名为ModeDrawStar。

使用Vscode打开ardupilot文件夹。（4.0.x版本源码）
进入ArduCopter文件夹
1、更改config.h文件 （大概第273行，不同版本源码位置不同)，添加MODE_DRAWSTAR_ENABLED定义，代码如下

  1. //////////////////////////////////////////////////////////////////////////////
  2. // Drawstar mode - draw five-pointed star ----waypoint
  3. #ifndef MODE_DRAWSTAR_ENABLED
  4. # define MODE_DRAWSTAR_ENABLED ENABLED
  5. #endif

2、更改mode.h文件 在enumclassNumber : uint8_t { }中，参照其它类型，添加DRAWSTAR，代码如下，
此处设定DRAWSTAR模式为29，不同版本代码的值不同，在源代码最下方依次序添加

  1. TURTLE =       28,  // Flip over after crash
  2. DRAWSTAR  =    29,  // five-points star waypoints

3、更改mode.h文件 参照ModeGuided类型，在其后添加ModeDrawStar类型定义，代码如下

  1. class ModeDrawStar : public Mode {
  3. public:
  4.     // inherit constructor
  5.     using Mode::Mode;
  6.     Number mode_number() const override { return Number::DRAWSTAR; }
  8.     bool init(bool ignore_checks) override;
  9.     void run() override;
  11.     bool requires_GPS() const override { return true; }  // 此模式需要有GPS定位
  12.     bool has_manual_throttle() const override { return false; }  // 此模式不允许手动控制油门
  13.     bool allows_arming(AP_Arming::Method method) const override { return false; }  // 不允许在此模式下解锁
  14.     bool is_autopilot() const override { return true; }  // 此模式为自动飞行控制
  15.     bool has_user_takeoff(bool must_navigate) const override { return false; }  // 不允许在此模式下直接起飞（必须是在空中切到此模式）
  16.     bool in_guided_mode() const override { return true; }  // 此模式是一种引导的模式
  18. protected:
  20.     const char *name() const override { return "DRAW_STAR"; }
  21.     const char *name4() const override { return "STAR"; }
  23. private:
  25.     Vector3f path[10];  //航点
  26.     int path_num;       //当前航点
  28.     void generate_path();   //生成航线
  30.     // wp controller
  31.     void pos_control_start();   //
  32.     void pos_control_run();     //
  34. };

4、更改copter.h文件 （大概第208行，不同版本源码位置不同)，添加对ModeDrawStar的友元类声明，代码如下

  1. friend class ModeGuided;
  2. friend class ModeDrawStar;

5、更改copter.h文件 （大概第945行，不同版本源码位置不同)，参照其它类型，创建ModeDrawStar类型对象 mode_drawstar，代码如下

  1. #if MODE_DRAWSTAR_ENABLED == ENABLED
  2.     ModeDrawStar mode_drawstar;
  3. #endif

6、更改GCS_Mavlink.cpp文件 （大概第32行，不同版本源码位置不同)，参照其它类型，添加对我们自定义类型DRAWSTAR的处理，代码如下

  1.     case Mode::Number::GUIDED:
  2.     case Mode::Number::DRAWSTAR:
  3.     case Mode::Number::CIRCLE:

7、创建mode_DrawStar.cpp文件 代码如下

  1. #include "Copter.h"
  3. #if MODE_DRAWSTAR_ENABLED == ENABLED
  5. // init - initialise guided controller
  6. bool ModeDrawStar::init(bool ignore_checks)
  7. {
  8.     // start in velaccel control mode
  9.     path_num = 0; //
  11.     generate_path();    //
  12.     pos_control_start(); //
  14.     return true;
  15. }
  17. void ModeDrawStar::generate_path(){
  18.     float radius_cm = 1000.0;
  20.     wp_nav->get_wp_stopping_point(path[0]);
  22.     path[1] = path[0] + Vector3f(1.0f, 0, 0) * radius_cm;
  23.     path[2] = path[0] + Vector3f(-cosf(radians(36.0f)), -sinf(radians(36.0f)), 0) * radius_cm;
  24.     path[3] = path[0] + Vector3f(sinf(radians(18.0f)), cosf(radians(18.0f)), 0) * radius_cm;
  25.     path[4] = path[0] + Vector3f(sinf(radians(18.0f)), -cosf(radians(18.0f)), 0) * radius_cm;
  26.     path[5] = path[0] + Vector3f(-cosf(radians(36.0f)), sinf(radians(36.0f)), 0) * radius_cm;
  27.     path[6] = path[1];
  28. }
  31. // run - runs the guided controller
  32. // should be called at 100hz or more
  33. void ModeDrawStar::run()
  34. {
  35.     if (path_num < 6) {  // 五角星航线尚未走完
  36.         if (wp_nav->reached_wp_destination()) {  // 到达某个端点
  37.             path_num++;
  38.             wp_nav->set_wp_destination(path[path_num], false);  // 将下一个航点位置设置为导航控制模块的目标位置
  39.         }
  40.     } else if ((path_num == 6) && wp_nav->reached_wp_destination()) {  // 五角星航线运行完成，自动进入Loiter模式
  41.         gcs().send_text(MAV_SEVERITY_INFO, "Draw star finished, now go into loiter mode");
  42.         copter.set_mode(Mode::Number::LOITER, ModeReason::MISSION_END);  // 切换到loiter模式
  43.     }
  45.     pos_control_run();  // 位置控制器
  46.  }
  49. // 更改自guided模式的wp_control_start     initialise guided mode's waypoint navigation controller
  50. void ModeDrawStar::pos_control_start()
  51. {    
  52.     // initialise waypoint and spline controller
  53.     wp_nav->wp_and_spline_init();
  55.     // initialise wpnav to stopping point
  56.     //Vector3f stopping_point;
  57.     //wp_nav->get_wp_stopping_point(stopping_point);
  59.     // no need to check return status because terrain data is not used
  60.     wp_nav->set_wp_destination(path[0], false);
  62.     // initialise yaw
  63.     auto_yaw.set_mode_to_default(false);
  64. }
  66. // 源自guided模式的wp_control_run                                      run guided mode's waypoint navigation controller
  67. void ModeDrawStar::pos_control_run()
  68. {
  69.     // process pilot's yaw input
  70.     float target_yaw_rate = 0;
  71.     if (!copter.failsafe.radio && use_pilot_yaw()) {
  72.         // get pilot's desired yaw rate
  73.         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
  74.         if (!is_zero(target_yaw_rate)) {
  75.             auto_yaw.set_mode(AUTO_YAW_HOLD);
  76.         }
  77.     }
  79.     // if not armed set throttle to zero and exit immediately
  80.     if (is_disarmed_or_landed()) {
  81.         // do not spool down tradheli when on the ground with motor interlock enabled
  82.         make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
  83.         return;
  84.     }
  86.     // set motors to full range
  87.     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
  89.     // run waypoint controller
  90.     copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
  92.     // call z-axis position controller (wpnav should have already updated it's alt target)
  93.     pos_control->update_z_controller();
  95.     // call attitude controller
  96.     if (auto_yaw.mode() == AUTO_YAW_HOLD) {
  97.         // roll & pitch from waypoint controller, yaw rate from pilot
  98.         attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
  99.     } else if (auto_yaw.mode() == AUTO_YAW_RATE) {
  100.         // roll & pitch from waypoint controller, yaw rate from mavlink command or mission item
  101.         attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), auto_yaw.rate_cds());
  102.     } else {
  103.         // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
  104.         attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
  105.     }
  106. }
  108. #endif

3 仿真查看结果

1、启动copter仿真，会重新编译代码，使用MP链接仿真器
2、仿真器界面进行控制，切换到我们自定义的飞行模式DRAWSTAR（29）

  1. // 解锁
  2. // 注意，在AUTO模式下是禁止解锁的，如果无法解锁，请先检查一下飞控是否处于AUTO模式
  3. // 可通过“mode loiter”命令将飞控切到loiter模式，然后解锁起飞，之后再切到其他模式
  4. arm throttle
  6. // 将油门推到一个较高的位置，起飞
  7. rc 3 1800
  9. // 将油门放到中位
  10. rc 3 1500
  12. // 切换到五角星航线模式
  13. mode 29  // 注意，对于新的master版本例程，此处应为mode 29  不同版本数字不同

3、MP查看仿真结果
image.png