前言

APM是一款功能齐全的开源多轴飞行器无人机控制器。历经多年的完善与发展，其手动模式的功能较为齐全和完善。自动模式逐渐成为开发重点。此文章将对自动模式进行简单的功能添加与改写。

贴出官方网站方便学习：学习 ArduPilot — 简介 — 开发文档https://ardupilot.org/dev/docs/learning-ardupilot-introduction.html

一、自动模式简介

自动模式是飞行系统中，最复杂的系统。包括：任务读取保存，任务执行逻辑，航迹规划，位置控制，姿态控制。之间几个模式的互相调用，接口，都是我们需要学习和开发的内容。

模式调用逻辑

//对模式进行初始化，准备执行任务
bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
{
case AUTO:
success = auto_init(ignore_checks);
break;
}

//以下为主循环调用，频率为400hz
void Copter::fast_loop()
{
// 运行姿态控制器
update_flight_mode();
}

//运行自动模式
void Copter::update_flight_mode()
{
switch (control_mode) {
case AUTO:
auto_run();
break;
}

//模式退出逻辑
void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t
new_control_mode)
{
if (old_control_mode == AUTO) {
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
}

二、添加自动降落功能

1.飞控主循环调用逻辑

2.功能添加

原理：本功能添加较为简单，在主循环函数中添加功能函数并在线程中调用即可。

以下为原生代码的部分


const AP_Scheduler::Task Copter::scheduler_tasks[] = {
    SCHED_TASK(rc_loop,              100,    130),
    SCHED_TASK(throttle_loop,         50,     75),
    SCHED_TASK(update_GPS,            50,    200),
//分别声明函数名，调用频率，最大停止时间

编译自己要添加的功能函数

void Copter::oneminute()
{
	if (control_mode == AUTO)
	 {
	 	long start_time;
	 	long t;
	 	
	 	if (auto_mode == Auto_TakeOff)
	 	{
	 		start_time = time(0);
	 		while(t <= 61)
	 		{
	 			t = time - start_time;
	 			if (t >= 60)
	 			{
	 				auto_mode = Auto_Land;
				 }
			 }
		 }
	 }
}

大概思路：1、首先判断飞行模式是否为自动模式，若为自动模式则进入函数

2、判断是否执行起飞指令，若是则记录起始时间并进入循环函数

3、若计时小于61执行循环函数

4、判断计时是否达到60s，若是则执行降落命令

添加线程

const AP_Scheduler::Task Copter::scheduler_tasks[] = {
    SCHED_TASK(rc_loop,              100,    130),
    SCHED_TASK(throttle_loop,         50,     75),
    SCHED_TASK(update_GPS,            50,    200),
    SCHED_TASK(oneminute,             50,    200),//添加到此处

添加声明

最后在copter.h文件中添加声明

    void fast_loop();
    void rc_loop();
    void throttle_loop();
    void update_batt_compass(void);
    void fourhundred_hz_logging();
    void ten_hz_logging_loop();
    void twentyfive_hz_logging();
    void three_hz_loop();
    void one_hz_loop();
    void update_GPS(void);
    void init_simple_bearing();
    void update_simple_mode(void);
    void update_super_simple_bearing(bool force_update);
    void read_AHRS(void);
    void update_altitude();
    void oneminute();//添加到此处