多旋翼无人机

        本节我们一起来学习自动航迹点任务（Mission）和自动返航（ReturnToLand）的具体实现代码以及各个不同导航模式下所完成的功能。

一、航迹点任务（Mission）

        航迹点任务是无人机自动飞行中最为常用，也是最具有灵活性的飞行模式，使用者可以根据需要在地面上规划出无人机飞行的多个任务点，然后将这些任务点上传到无人机当中，然后通过遥控器或地面站命令使无人机切换至航迹点任务模式，之后无人机会根据当前任务进行飞行。具体的执行过程如下：

        1）首次切换到自动航迹点任务模式：取得需要执行的任务点mission_item，根据此任务点的经度、维度、高度和航向等信息设定无人机的当前飞行目标位置。

        2）根据当前目标位置向位置控制模块（mc_position_control）发送目标位置期望，即使无人机飞往目标点。

        3）判断无人机当前位置与目标任务点的距离是否小于到达半径进而判断无人机是否已经到达该任务点。

        4）如果无人机到达当前任务点，则取得下一个任务点，并执行当前任务命令（悬停、拍照等）。

        5）将下一个任务点作为无人机当前的飞行目标点，根据此任务点的经度、维度、高度和航向等信息设定无人机的当前飞行目标位置。

        6）重复步骤2，直到所有任务均已完成，最后在当前位置悬停。

        接下来我们来看看具体代码实现：

void Mission::on_activation()
{
	//首次切换到自动航迹点任务，设定任务点
	set_mission_items();
	//其他代码略
}

void Mission::on_active()
{
	//检查任务点是否有效
	check_mission_valid(false);
	//判断无人机是否已经到达当前任务点的位置
	if (is_mission_item_reached()) 
        {
		//通知MissionBlock任务点已到达
		set_mission_item_reached();
		//设定任务点
		set_mission_items();
	}
	//其他代码略
}

void Mission::set_mission_items()
{
	//读取下一个任务点
	if (prepare_mission_items(&_mission_item, &mission_item_next_position)) 
        {
	}
	//如果所有任务点均已完成
	else 
        {
		//设置无人机在当前位置悬停
		set_loiter_item (&_mission_item);
		return;
	}
	//执行任务点相关命令代码略
}

        我们可以在地面站中任意指定几个任务点，并上传到无人机当中，并滑动Start Mission按钮执行自动航迹点任务。如下图所示：

二、自动返航（ReturnToLand——RTL）

        与自动航迹点任务不同，自动返航模式属于保护性动作。我们在前面内容中已经介绍过，可以通过配置相关参数来设定无人机的安全保护动作，例如：低电量返航、遥控器信号丢失返航等。除此之外，自动返航也可以通过遥控器的辅助通道和地面站指令主动触发。自动返航动作一共有以下几个阶段：

        1）爬升（RTL_STATE_CLIMB）：从当前位置爬升到一定高度，为了避免返航图中与其他建筑物或树木等物体相撞，因此需要进行一定高度的爬升动作。

        2）返航（RTL_STATE_RETURN）：掉转航向，向起飞点（home_position）返航。

        3）下降（RTL_STATE_DESCEND）：到达起飞点后，无人机开始下降到悬停高度。

        4）悬停（RTL_STATE_LOITER）：在当前悬停高度悬停一定时间，如果设定的悬停时间小于0则不悬停。

        5）着陆（RTL_STATE_LAND）：从悬停高度继续下降，直到着陆为止。

        6）已着陆（RTL_STATE_LANDED）：完成返航动作。

        接下来我们来看看具体代码实现：

void RTL::on_active()
{
	//如果没有完成着陆，且到达当前返航任务点
	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached())
	{
		//设定返航状态
		advance_rtl();
		//设定返航任务点
		set_rtl_item();
	}
}

void RTL::set_rtl_item()
{
	switch (_rtl_state) 
	{
		//爬升
		case RTL_STATE_CLIMB: 
		{
			//设定当前位置与爬升高度
			_mission_item.lat = gpos.lat;
			_mission_item.lon = gpos.lon;
			_mission_item.altitude = return_alt;
			break;
		}
		//返航
		case RTL_STATE_RETURN: 
		{
			//设定返航目标点为起飞点，并设定航向
			_mission_item.lat = home.lat;
			_mission_item.lon = home.lon;
			_mission_item.altitude = return_alt;
			_mission_item.yaw = home.yaw;
			break;
		}
		//下降
		case RTL_STATE_DESCEND: 
		{
			//设定悬停高度为目标高度
			_mission_item.altitude = loiter_altitude;
			break;
		}
		//悬停
		case RTL_STATE_LOITER: 
		{
			//设定悬停时间
			_mission_item.time_inside = _param_land_delay.get();
			break;
		}
		//着陆
		case RTL_STATE_LAND: 
		{
			//设定起飞点为目标点
			_mission_item.lat = home.lat;
			_mission_item.lon = home.lon;
			_mission_item.yaw = home.yaw;
			_mission_item.altitude = home.alt;
			break;
		}
		//已着陆
		case RTL_STATE_LANDED: 
		{
			break;
		}
	}
	//其他代码略
}

void RTL::advance_rtl()
{
	switch (_rtl_state) 
	{
		//由爬升转为返航
		case RTL_STATE_CLIMB:
			_rtl_state = RTL_STATE_RETURN;
			break;
		//由返航转为下降
		case RTL_STATE_RETURN:
			_rtl_state = RTL_STATE_DESCEND;
			break;
		//由下降装为悬停或着陆
		case RTL_STATE_DESCEND:
			//如果悬停时间有效则转为悬停
			if (_param_land_delay.get() < -0.01f || _param_land_delay.get() > 0.01f) 
			{
				_rtl_state = RTL_STATE_LOITER;
			}
			//如果悬停时间无效则直接转为着陆
			else 
			{
				_rtl_state = RTL_STATE_LAND;
			}
			break;
		//由悬停转为着陆
		case RTL_STATE_LOITER:
			_rtl_state = RTL_STATE_LAND;
			break;
		//由着陆转为已着陆
		case RTL_STATE_LAND:
			_rtl_state = RTL_STATE_LANDED;
			break;
	}
	//其他代码略
}

三、其他导航模式

        除了自动航迹点任务和自动返航之外，导航模块中还有很多其他导航模式，例如：悬停（Loilter）、目标跟随（FollowTarget）、起飞（Takeoff）、着陆（Land）等等。这些导航模式从本质上讲，实现方式与上述两种导航模式没有区别，只不过是在不同的模式下，完成不同的目标点设定，并使无人机向目标点飞行，并执行相关指令动作。对于目标跟随模式，实际上并没有在无人机中实现目标识别和跟随的功能，而仅仅是通过地面站向无人机发送一个目标位置点，无人机将此目标点作为飞行目标，向此点飞行。所以从这个角度上来说无人机无法独立完成目标跟随动作，而是需要配合其他设备进行协同工作。例如在无人机上安装云台上并搭载一个摄像头，将摄像头所捕获的视频流通过无线电发送给地面站程序，地面站程序通过机器视觉等相关记录分析出需要跟随目标的位置，再通过地面站将目标位置发送给无人机，无人机才会向目标位置飞行，从而达到“目标跟随”。如下图所示：

        对于其他导航模式实际上功能虽然不同，但是从实现原理上讲也都大同小异，我们不作过多的介绍，请有兴趣的读者自行阅读源代码并学习相关内容。