多旋翼无人机

        在上一节中，我们已经知道在不同的导航状态下有着不同的执行模式，而我们在本节中主要来分析其中比较重要的一个执行模式，自动飞行任务Mission，也称为航迹点任务飞行模式。当飞控进入自动任务模式之后Navigator就会切换到Mission模式，并进行相应的自动飞行任务。在C++的类设计中，Mission类继承了MissionBlock类，而MissionBlock类继承了NavigatorMode类，它们的关系如下：

       在NavigatorMode类实现了几个虚函数on_inactive()、on_activation()、on_inactivation()、on_active()，每个不同的执行模式下具有不同的内容。而MissionBlock类主要实现了一些关于航迹点飞行的公用处理函数，例如：is_mission_item_reached()、reset_mission_item_reached()、mission_item_to_position_setpoint()、set_loiter_item()、set_takeoff_item()、set_land_item()等等。在本节我们主要来介绍与自动飞行任务Mission相关的一些函数的具体内容。我们可以将Mission类的核心功能概括为一下3点：

1.设定当前飞行的目标航迹点。

2.判断无人机是否已经到达目标航迹点。

3.到达目标点后执行指定动作，并设定下一个目标航迹点。

        实际上，自动飞行任务的程序需要处理的功能远远不止以上3点，但是核心功能就是上述内容，其他的功能就是在这些核心功能的基础上做了很多扩展功能，来提高程序的稳定性、扩展性等等。在navigation.h文件中定义了航迹点结构体的具体内容：

struct mission_item_s {
	double lat;						//航迹点纬度
	double lon;						//航迹点经度
	union {
		struct {
			union {
				float time_inside;		//在航迹点半径范围内停留时间
				float pitch_min;		//固定翼起飞最小俯仰角
				float circle_radius;            //电子围栏圆周半径
			};
			float acceptance_radius;		//航迹点到达半径
			float loiter_radius;			//固定翼盘旋半径
			float yaw;				//航向角
			float altitude;				//航迹点高度
		};
	};
	uint16_t nav_cmd;					//导航命令
	int16_t do_jump_mission_index;			        //任务跳转索引号
	uint16_t do_jump_repeat_count;			        //重复执行跳转数
	union {
		uint16_t do_jump_current_count;		        //任务跳转当前索引号
		uint16_t vertex_count;				//多边形电子围栏顶点个数
		uint16_t land_precision;			//精准着陆方式
	};
};

        为了使用方便，MissionBlock类中已经实现了2个函数mission_item_to_position_setpoint()函数用于设定一个任务点作为飞行的目标点和is_mission_item_reached()函数用于判断无人机的当前位置是否已经到达了目标点。这两个函数是自动飞行最为关键的两个函数，例如无人机在空中飞行，其当前的坐标为A（x0、y0），此时我们希望无人机自动飞往B（x1、y1）点，于是我们就可以调用mission_item_to_position_setpoint()函数将无人机的目标航点设置成x1、y1，并设置此航迹点的到达半径为R米。如图所示：

        当无人机在A点时我们为无人机指定了一个目标航迹点B，于是无人机开始向B点飞行，之后飞控程序会不停的对当前位置和目标航迹点位置的距离进行判断，如果距离小于指定的到达半径，则表示已经到达目标点。于是，当无人机从A点向B点飞行，途中经过C点时，飞控程序的is_mission_item_reached()函数会返回true，表示已经到达目标点B，并且执行后续功能：如果还有其他航迹点，则转向其他航迹点执行，否则继续向B点飞行，直到完全与B点重合为止。

        我们首先来看一看mission_item_to_position_setpoint()函数的具体实现内容：

bool mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp)
{
	//设置经度纬度
	sp->lat = item.lat;
	sp->lon = item.lon;
	//根据不同条件设置高度为海拔高度还是相对与起飞点（HomePosition）高度
	sp->alt = item.altitude_is_relative ? item.altitude + _navigator->get_home_position()->alt : item.altitude;
	//设置航向
	sp->yaw = item.yaw;
	//设置到达半径
	sp->acceptance_radius = item.acceptance_radius;
	//设置导航命令
	switch (item.nav_cmd)
	{
		//起飞
		case NAV_CMD_TAKEOFF:
			sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
			break;

		//着陆
		case NAV_CMD_LAND:
			sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
			break;

		//悬停
			case NAV_CMD_LOITER:
				sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
				break;

		//位置（航迹点）
		default:
			sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
			break;
	}
	sp->valid = true;
	return sp->valid;
}

        而对于到点判断函数is_mission_item_reached()来说，需要根据不同的导航命令来判断飞机当前位置是否已经到达目标点，例如普通航迹点飞行的到点判断，与起飞和降落的到点判断逻辑都不相同，因此需要根据实际情况进行不同的处理，具体代码如下：

bool MissionBlock::is_mission_item_reached()
{
	bool _waypoint_position_reached = false;

	//如果没有着陆，并且没有到达当前点
	if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached)
	{
		float dist = -1.0f;
		float dist_xy = -1.0f;
		float dist_z = -1.0f;
		//计算海拔高度或者是相对与起飞点的高度
		float altitude_amsl = _mission_item.altitude_is_relative
		? _mission_item.altitude + _navigator->get_home_position()->alt
		: _mission_item.altitude;
		//计算无人机当前位置与目标位置的距离
		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
				_navigator->get_global_position()->lat,
				_navigator->get_global_position()->lon,
				_navigator->get_global_position()->alt,
				&dist_xy, &dist_z);

		//普通航迹点任务
		if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT)
		{
			//取得当前航迹点的到达半径
			float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius);
			//判断无人机与目标航迹点的距离是否已经小于到达半径，如果小于则表示已经达到此航迹点
			if (dist >= 0.0f && dist <= mission_acceptance_radius
					&& dist_z <= _navigator->get_altitude_acceptance_radius())
			{
				_waypoint_position_reached = true;
			}
		}
		//起飞
		else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF)
		{
			//略
		}
		//着陆
		else if (_mission_item.nav_cmd == NAV_CMD_LAND)
		{
			//略
		}
		//其他导航命令判断逻辑代码略
	}
	return _waypoint_position_reached;
}

        实际上，目标点设置和到点判断两个函数中的代码逻辑很复杂，内容也很多，我们在上面的代码中已经将大部分的代码省略掉了，希望读者能够从本质上理解和掌握自动飞行任务的实现原理和实际飞行效果，而不要被大量的代码实现细节所误导，因此希望读者不要过于陷入细节，从大方向上懂得整个程序的运行过程。

        另外，如果我们将航迹点的到达半径设置的非常小，无人机很可能一直在目标点附近徘徊，而判断一直没有到达，因为在普通GPS定点飞行过程中，飞机的控制精度不是特别精准，在位置测量个估计上会有一些误差；如果我们将航迹点的到达半径设置的非常大，则会导致飞机刚刚进入到达半径附近就会飞往下一个航迹点，如图所示：

        在无人机起飞前我们设定好的航迹点为A-C-E-G。实际上，无人机在到达B点后就会判断已经到达当前航迹点，于是会从B点直接飞往E点；而当达到D点后同样会判断已经达到当前航迹点，于是直接飞往G点。因此无人机的飞行轨迹并非是有各个航迹点所连接起来的航线，而是跟每一个航迹点的到达半径相关。因此达到半径需要根据实际情况进行设置，不能太大，也不能太小。