多旋翼无人机

        在本节我们来一起学习关于PX4飞控当中自动航线的实现原理。此部分代码在PX4的Firmware/src/modules/navigator模块当中，此模块名为Navigator，从字面上解释即为“导航”。实际上，这是一个字面直译，相当于我们在日常生活中所说的路径规划，例如在开车时我们需要从A点行驶到B点，车载导航系统会自动为驾驶员规划好一条或几条被选路线。在无人机中往往称为自动航线规划与自动飞行。而从另一个层面来说，导航原理实际上指的是将各个传感器和测量仪的读数进行有效融合并估算出系统当前的状态、姿态、速度和位置等相关内容。但是从普通理解上将，我们在本节所讨论的Navigator“导航”只是航线规划和自动飞行，而数据融合和状态估计我们会在后续内容中讲解。

        Navigator模块中要使用的核心状态集合我们成为导航状态，这部分内容我们已经在命令响应状态机中讲解过了，我们再来复习一下：

uint8 NAVIGATION_STATE_MANUAL = 0                 //手动模式
uint8 NAVIGATION_STATE_ALTCTL = 1                 //高度控制模式
uint8 NAVIGATION_STATE_POSCTL = 2                 //位置控制模式
uint8 NAVIGATION_STATE_AUTO_MISSION = 3           //自动任务模式
uint8 NAVIGATION_STATE_AUTO_LOITER = 4            //自动悬停模式
uint8 NAVIGATION_STATE_AUTO_RTL = 5               //自动返航模式
uint8 NAVIGATION_STATE_AUTO_RCRECOVER = 6         //遥控器恢复
uint8 NAVIGATION_STATE_AUTO_RTGS = 7              //数传信号丢失自动返回地面站
uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8       //自动降落当引擎失效
uint8 NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9       //自动降落当GPS失效
uint8 NAVIGATION_STATE_ACRO = 10                  //特级模式
uint8 NAVIGATION_STATE_UNUSED = 11                //预留
uint8 NAVIGATION_STATE_DESCEND = 12               //下降模式
uint8 NAVIGATION_STATE_TERMINATION = 13           //自毁模式
uint8 NAVIGATION_STATE_OFFBOARD = 14              //离线模式
uint8 NAVIGATION_STATE_STAB = 15                  //增稳模式
uint8 NAVIGATION_STATE_RATTITUDE = 16             //手动特技模式
uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17          //自动起飞模式
uint8 NAVIGATION_STATE_AUTO_LAND = 18             //自动着陆模式
uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19    //目标跟随模式
uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20         //按目标精准着陆

        在每一种导航状态在Navigator都会做出响应的处理，实际上在PX4程序中是采用的C++的多态机制来实现不同功能的。在Navigator中定义了一个NavigatorMode类，之后又定义了很多用于处理不同模式的子类，这些子类都继承了NavigatorMode，在Navigator中，根据不同状态来执行不同模式下的处理函数。这些类的继承关系如下：

        实际上Navigator中实现了一个叫MissionBlock的类继承了NavigatorMode，然后将Mission、Loiter、RTL等这些不同导航模式下的具体执行类继承了MissionBlock。MissionBlock的子类一共有：DataLinkLoss、EngineFailure、FollowTarget、GpsFailure、Land、Loiter、Mission、PrecLand、RCLoss、RTL、Takeoff等11个。分别负责在响应的状态的执行功能。例如：Loiter负责悬停；RTL负责返航；Mission负责执行自动任务等。

        整个导航模块的主功能函数在Navigator::run()中执行，主要负责订阅并获取相关的uORB内容，包括全局坐标系（vehicle_global_position）和本地坐标系（vehicle_local_position）的位置数据、GPS导航（vehicle_gps_position）位置数据和无人机状态（vehicle_status）等相关数据。在主循环中接收无人机命令（vehicle_command）这个uORB的相关指令并做出响应的响应。然后在根据最终的导航状态（vehicle_status中的nav_state）完成导航系统。

        在本节中，我们主要来分析一下MissionBlock的多个子类，在不同导航状态下的切换方式：

//导航模式-新
NavigatorMode *navigation_mode_new { nullptr };

//根据导航状态进行不同处理
switch (_vstatus.nav_state) {
//任务模式
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
	navigation_mode_new = &_mission;
	break;
//自动悬停模式
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
	navigation_mode_new = &_loiter;
	break;
//遥控器恢复模式
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
	navigation_mode_new = &_rcLoss;
	break;
//自动返航模式
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: {
	//返航类型
	switch (rtl_type()) {
	//着陆
	case RTL::RTL_LAND:
		if (rtl_activated) {
			mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL LAND activated");
		}
		//任务执行后着陆，任务模式
		if (on_mission_landing() && !get_land_detected()->landed) {
			navigation_mode_new = &_mission;
		//自动返航模式
		} else {
			navigation_mode_new = &_rtl;
		}
		break;
	default:
		if (rtl_activated) {
			mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL HOME activated");
		}
		navigation_mode_new = &_rtl;
		break;
	}
	break;
}
//自动起飞模式
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
	_pos_sp_triplet_published_invalid_once = false;
	navigation_mode_new = &_takeoff;
	break;
//自动着陆模式
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
	_pos_sp_triplet_published_invalid_once = false;
	navigation_mode_new = &_land;
	break;
//跟随目标模式
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
	_pos_sp_triplet_published_invalid_once = false;
	navigation_mode_new = &_follow_target;
	break;
// 其他模式略
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
case vehicle_status_s::NAVIGATION_STATE_STAB:
default:
	navigation_mode_new = nullptr;
	break;
}
_navigation_mode = navigation_mode_new;
for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
	_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
}

       我们可以看到，NavigatorMode *navigation_mode_new是一个基类的对象指针，而不同的子类_mission、_rtl、_follow_target等，都是NavigatorMode的子类。在C++中使用基类对象指针指向子类对象后，执行基类的run()从而实现了多态的效果。需要注意的是：run()函数是在NavigatorMode类中定义的，但是它并不是一个虚函数。多态的效果实际上是通过run()函数内部调用其他虚函数而实现的。我们再来看看NavigatorMode类中的几个虚函数的定义，以及run()函数的内容：

class NavigatorMode
{
public:
	//执行函数
	void run(bool active);
	//模式无效时调用此功能
	virtual void on_inactive();
	//当模式激活时，此函数被调用一次
	virtual void on_activation();
	//当模式变为非活动状态时，此功能被调用一次
	virtual void on_inactivation();
	//模式激活时调用此功能
	virtual void on_active();
private:
	bool _active { false };
};

void NavigatorMode::run(bool active)
{
	if (active) 
	{
		if (!_active) 
			on_activation();
		else 
			on_active();
	} 
	else 
	{
		if (_active) 
			on_inactivation();
		else 
			on_inactive();
	}
	_active = active;
}

        代码中除了run()函数之外的其他函数on_inactive()、on_activation()、on_inactivation()、on_active()都是虚函数，因此当run()函数中执行这些函数时，实际执行的是子类当中的函数（子类中重写这些虚函数）从而实现了多态效果。注意，run()函数会在Navigator::run()中被重复执行，因此on_active()函数也是重复执行的，其他函数只是在模式切换时执行一次。