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

1 功能目标

上节课,实现了一键自动飞行五角星航线的功能,但是五角星的大小是在飞控代码中是固定不变的,如果想调整五角星的大小,需要重新编译固件,非常不方便。
本节课,将五角星的大小定义为一个参数,并且可以在MissionPlanner中修改它,从而控制五角星的大小。

2 添加代码

使用Vscode打开ardupilot文件夹。进入ArduCopter文件夹,在上一节源码的基础上修改。
1、添加参数变量;
更改Parameters.h文件,在ParametersG2类定义中添加自定义变量star_radius_cm,代码如下

  1. /*
  2. 2nd block of parameters, to avoid going past 256 top level keys
  3. */
  4. class ParametersG2 {
  5. ......
  6. AP_Float pilot_y_rate;
  7. ......
  8. AP_Float star_radius_cm;
  9. };

2、添加参数定义;
更改Parameters.cpp文件,参照其它参数,将我们的自定义参数star_radius_cm加入ParametersG2::var_info参数组,其中,”STAR_R_CM”为自定义参数star_radius_cm在MP中显示名称,52为自定义参数在参数组序号值(各版本源码值不同,按实际填写,在现有参数序列后加1),1000.0为我们自定义参数的默认值,即半径1000cm。
代码如下

  1. const AP_Param::GroupInfo ParametersG2::var_info[] = {
  2. ......
  3. AP_GROUPINFO("SURFTRAK_MODE", 51, ParametersG2, surftrak_mode, (uint8_t)Copter::SurfaceTracking::Surface::GROUND),
  4. AP_GROUPINFO("STAR_R_CM", 52, ParametersG2, star_radius_cm, 1000.0),
  5. AP_GROUPEND
  6. };

3、使用参数控制五角星大小;
更改mode_DrawStar.cpp文件,将原有设置半径的值(1000.0)改为自定义参数star_radius_cm,代码如下

  1. void ModeDrawStar::generate_path(){
  2. //float radius_cm = 1000.0;
  3. float radius_cm = g2.star_radius_cm;
  4. ......
  5. }

3 仿真查看结果

仿真飞行,在MissionPlanner中可以直接看到并修改此参数。
1、启动copter仿真,会重新编译代码,使用MP链接仿真器。
2、按上一节步骤先进行一次仿真模拟,会以默认半径值(1000cm)进行五角星航线飞行。
3、仿真器界面进行控制,先切换到Lotier模式,再向前推进一段距离,再切换到Lotier模式。

  1. // 切换到Lotier模式
  2. mode 5
  3. // 更改升降量 向前推进
  4. rc 2 1100
  5. // 回到中位
  6. rc 2 1500
  7. // 切换到Lotier模式
  8. mode 5

4、MP参数列表中找到并修改我们自定义参数的值,从1000改为2000,写入参数,如图
image.png
5、仿真器界面进行控制,切换到五角星航线模式,以修改过的半径再进行一次五角星航线飞行。
image.png