多旋翼无人机

        在本节我们来学习Commander中的3种状态切换过程，其中包括无人机的解锁状态处理、主状态处理和导航状态处理。这3种状态是无人机中非常重要的状态，直接影响飞机的控制导航逻辑和控制逻辑，进而影响飞机的实际飞行效果。下面我们就来分别对这3种飞行状态的处理过程进行分析：

一、arming_state解锁/锁定状态

        解锁状态在PX4飞控程序中的定义名称为Arming state表示飞机当前的解锁和锁定状态，它被定义成vehicle_status.msg这个uORB中的uint8 arming_state属性，实际上arming_state并不仅仅用于表示“锁定”和“解锁”这两个状态，它一共有6个状态也定义在vehicle_status.msg这个UORB当中，其内容如下：

uint8 ARMING_STATE_INIT = 0             //初始化
uint8 ARMING_STATE_STANDBY = 1          //准备就绪
uint8 ARMING_STATE_ARMED = 2            //已解锁
uint8 ARMING_STATE_STANDBY_ERROR = 3    //就绪错误
uint8 ARMING_STATE_REBOOT = 4           //重启
uint8 ARMING_STATE_IN_AIR_RESTORE = 5   //空中恢复

        实际上，Arming state是为了表示无人机在整个飞行过程中可能出现的基本状态，其中只有Arming state为ARMING_STATE_ARMED时，才表示飞控解锁，也就是电机解锁。用于表示飞控实际对电机“锁定”和“解锁”状态的变量并非Arming state，而是actuator_armed.msg这个uORB的bool armed属性。注意这个变量是一个布尔型变量，它只表示无人机系统“解锁”和“锁定”这两个状态。

        上述的6个状态切换图如下：

        在PX4程序中使用了一个二位数组来表示此状态机的切换关系，如下：

static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = 
{
	//                                 INIT,  STANDBY, ARMED, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
	{ /*ARMING_STATE_INIT */           true,  true,    false, true,          false,  false },
	{ /*ARMING_STATE_STANDBY */        true,  true,    true,  false,         false,  false },
	{ /*ARMING_STATE_ARMED */          false, true,    true,  false,         false,  true  },
	{ /*ARMING_STATE_STANDBY_ERROR */  true,  true,    true,  true,          false,  false },
	{ /*ARMING_STATE_REBOOT */         true,  true,    false, true,          true,   true  },
	{ /*ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,         false,  false }
};

        在这个表示Arming state的二位数组中，每一行表示无人机需要切换的新的状态，每一列表示当前状态。从当前状态切换到新状态时的限制条件就是arming_transitions这个二位数组的值，如果为true表示允许切换，如果为false表示不允许切换。

transition_result_t arming_state_transition(vehicle_status_s *status,
				const arming_state_t new_arming_state,
				actuator_armed_s *armed,
				vehicle_status_flags_s *status_flags,
				...)
{
	//允许切换标识
	transition_result_t ret = TRANSITION_DENIED;
	//当前状态
	arming_state_t current_arming_state = status->arming_state;
	//如果需要切换的新状态与当前状态相同，则不处理
	if (new_arming_state == current_arming_state)
	{
		ret = TRANSITION_NOT_CHANGED;
	}
	else
	{
		//检查状态切换结果
		bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
		//如果允许切换
		if (valid_transition)
		{
			//设定“解锁”或“锁定”状态
			armed->armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED);
			//完成状态切换
			ret = TRANSITION_CHANGED;
			//更新Arming state为新状态
			status->arming_state = new_arming_state;
		}
	}
	return ret;
}

二、main_state主状态

        主状态就是我们之前所介绍的无人机的飞行模式。切换主状态就是切换飞行模式，使无人机进入不同的飞行控制逻辑当中。当然，飞控的主状态不可以任意切换，切换到不同的主状态需要不同的前提条件。每一种状态的条件前提我们可以按下表进行判断：

        用于表示主状态的变量被定义为commander_state.msg这个uORB中的uint8 main_state属性。其实际值与所表示的状态对应关系如下：

uint8 MAIN_STATE_MANUAL = 0                    //手动模式
uint8 MAIN_STATE_ALTCTL = 1                    //高度控制模式
uint8 MAIN_STATE_POSCTL = 2                    //位置控制模式
uint8 MAIN_STATE_AUTO_MISSION = 3              //任务模式
uint8 MAIN_STATE_AUTO_LOITER = 4               //悬停模式
uint8 MAIN_STATE_AUTO_RTL = 5                  //返航模式
uint8 MAIN_STATE_ACRO = 6                      //特技模式
uint8 MAIN_STATE_OFFBOARD = 7                  //离线模式
uint8 MAIN_STATE_STAB = 8                      //增稳模式
uint8 MAIN_STATE_RATTITUDE = 9                 //手动特技模式
uint8 MAIN_STATE_AUTO_TAKEOFF = 10             //起飞模式
uint8 MAIN_STATE_AUTO_LAND = 11                //着陆模式
uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12       //目标跟随模式

        在主状态切换函数main_state_transition()处理了主状态的切换逻辑，具体代码如下：

transition_result_t main_state_transition(const vehicle_status_s &status,
		const main_state_t new_main_state,
		const vehicle_status_flags_s &status_flags,
		commander_state_s *internal_state)
{
	//切换结果，允许或拒绝
	transition_result_t ret = TRANSITION_DENIED;
	//需要切换的新主状态
	switch (new_main_state)
	{
		case commander_state_s::MAIN_STATE_MANUAL:
		case commander_state_s::MAIN_STATE_STAB:
		case commander_state_s::MAIN_STATE_ACRO:
		case commander_state_s::MAIN_STATE_RATTITUDE:
			ret = TRANSITION_CHANGED;
			break;
		case commander_state_s::MAIN_STATE_ALTCTL:
			if (status_flags.condition_local_altitude_valid
					|| status_flags.condition_global_position_valid)
				ret = TRANSITION_CHANGED;
			break;
		case commander_state_s::MAIN_STATE_POSCTL:
			if (status_flags.condition_local_position_valid
					|| status_flags.condition_global_position_valid)
				ret = TRANSITION_CHANGED;
			break;
		case commander_state_s::MAIN_STATE_AUTO_LOITER:
			if (status_flags.condition_global_position_valid)
				ret = TRANSITION_CHANGED;
			break;
		case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
			if (status.is_rotary_wing)
				ret = TRANSITION_CHANGED;
			break;
		case commander_state_s::MAIN_STATE_AUTO_MISSION:
			if (status_flags.condition_global_position_valid
					&& status_flags.condition_auto_mission_available)
				ret = TRANSITION_CHANGED;
			break;
		case commander_state_s::MAIN_STATE_AUTO_RTL:
			if (status_flags.condition_global_position_valid
					&& status_flags.condition_home_position_valid)
				ret = TRANSITION_CHANGED;
			break;
		case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
		case commander_state_s::MAIN_STATE_AUTO_LAND:
			if (status_flags.condition_local_position_valid)
				ret = TRANSITION_CHANGED;
			break;
		case commander_state_s::MAIN_STATE_AUTO_PRECLAND:
			if (status_flags.condition_local_position_valid
					&& status_flags.condition_global_position_valid
					&& status.is_rotary_wing)
				ret = TRANSITION_CHANGED;
			break;
		case commander_state_s::MAIN_STATE_OFFBOARD:
			if (!status_flags.offboard_control_signal_lost)
				ret = TRANSITION_CHANGED;
			break;
		case commander_state_s::MAIN_STATE_MAX:
		default:
			break;
	}
	//如果允许切换主状态，则修改主状态的值
	if (ret == TRANSITION_CHANGED)
		if (internal_state->main_state != new_main_state)
			internal_state->main_state = new_main_state;
		else
			ret = TRANSITION_NOT_CHANGED;
	return ret;
}

        以上代码中所处理的逻辑与上述表格中的内容一致，不再赘述。

三、导航状态

        导航状态被定义在vehicle_status.msg这个uORB的uint8 nav_state属性，其实际值与导航状态的对应关系如下：

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         //按目标精准着陆

        与主状态切换不同，无人机会根据当前状态与不同的条件对导航状态进行降级评估，也就是说如果当前飞行模式的必要条件如果不满足，则会切入到一个较为安全的导航状态。在正常情况下，导航状态值会被保持与主状态一致。

switch (internal_state->main_state)
{
	case commander_state_s::MAIN_STATE_ACRO:
		status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
		break;
	case commander_state_s::MAIN_STATE_MANUAL:
		status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
		break;
	case commander_state_s::MAIN_STATE_RATTITUDE:
		status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE;
		break;
	case commander_state_s::MAIN_STATE_STAB:
		status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
		break;
	case commander_state_s::MAIN_STATE_ALTCTL:
		status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
		break;
	...
	default:
		status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
		break;
}

        但是，在实际飞行中，会遇到很多异常情况，飞控程序会根据预先对相关保护参数的设定值修改导航状态，例如，当数传信号丢失后的处理方法如下：

void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
		const vehicle_status_flags_s &status_flags,
		commander_state_s *internal_state,
		const link_loss_actions_t link_loss_act,
		uint8_t auto_recovery_nav_state)
{
	//全局位置和起飞点有效时，自动恢复保护
	if (link_loss_act == link_loss_actions_t::AUTO_RECOVER
			&& status_flags.condition_global_position_valid
			&& status_flags.condition_home_position_valid)
	{
		status->nav_state = auto_recovery_nav_state;
	}
	//全局位置有效时，自动悬停
	else if (link_loss_act == link_loss_actions_t::AUTO_LOITER
			&& status_flags.condition_global_position_valid)
	{
		status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;

	}
	//全局位置和起飞点有效时，自动返航
	else if (link_loss_act == link_loss_actions_t::AUTO_RTL
			&& status_flags.condition_global_position_valid
			&& status_flags.condition_home_position_valid)
	{
		main_state_transition(*status, commander_state_s::MAIN_STATE_AUTO_RTL,
				status_flags, internal_state);
		status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
	}
	//本地位置有效时，自动降落
	else if (link_loss_act == link_loss_actions_t::AUTO_LAND
			&& status_flags.condition_local_position_valid)
	{
		status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;

	}
	//自毁
	else if (link_loss_act == link_loss_actions_t::TERMINATE)
	{
		status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
		armed->force_failsafe = true;

	}
	//锁定
	else if (link_loss_act == link_loss_actions_t::LOCKDOWN)
	{
		armed->lockdown = true;
	}
	//全局位置和起飞点有效时，自动返航
	else if (status_flags.condition_global_position_valid
			&& status_flags.condition_home_position_valid)
	{
		status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
	}
	//本地位置有效时自动降落
	else if (status_flags.condition_local_position_valid)
	{
		status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
	}
	//高度有效时
	else if (status_flags.condition_local_altitude_valid)
	{
		//多旋翼自动下降
		if (status->is_rotary_wing)
		{
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
		}
		//自动降落当GPS失效
		else
		{
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
		}
	}
	//自毁
	else
	{
		status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
	}
}