PX4自动任务飞行AUTO_MISSION的实现流程

第一步，进入方式。

    与之前分析的POSCTL模式类似，主要有遥控器开关和指令两种模式，实现过程完全相同。

    最终都是调用了main_state_transition()函数将新模式写入到internal_state.main_state中。

第二步，导航模式和控制模式的选择。

    在commander的主循环中，set_nav_state()来设定导航模式。由于自动任务飞行需要使用各种传感器来提供信息，如果一切正常则设置导航模式为status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; 如果，有任何的传感器状态异常，则切换到其他的导航模式中。由于set_nav_state()函数是在主循环中不断运行的，传感器状态也在不断更新，也就是说正常进行自主任务飞行的过程中一旦发生了传感器异常就会立即切换到应急处置的对应导航模式中。

    在set_control_mode()函数中根据nva_state的值设定了控制相关的flag，与位置控制类似。

    按照一定周期发布了vehicle_control_mode和vehicle_status两个消息。

第三步，导航过程的实现。

    在上一篇POSCTL的分析中提到，在进入了navigator后根据导航模式_vstatus.nav_state选择对应的导航模式实例。对POSCTL而言_navigation_mode = nullptr，因此没有进行任何具体的操作；而对AUTO_MISSION而言_navigation_mode = &_mission，是一个类型为Mission的实例。Mission这个类继承自MissionBlock类，后者又继承了NavigatorMode类。

   在_navigation_mode没有被赋值为&_mission时，执行的是run(false)，进而执行on_inactive()函数；当_navigation_mode被赋值为&_mission时，如果是首次从其他模式切换过来，则要执行on_activation()函数，否则执行on_active()函数。这几个函数是声明在NavigatorMode类中的虚函数，具体的实现在各自派生类的具体定义中。因此，Mission::on_active()是自动任务飞行中导航的实现函数。

    在Mission::on_active()中，首先判断是否重新上传了飞行任务，如果有则重新设定当前的航段。

	/* check if anything has changed */
	bool onboard_updated = false;
	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);


	if (onboard_updated) {
		update_onboard_mission();
	}


	bool offboard_updated = false;
	orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);


	if (offboard_updated) {
		update_offboard_mission();
	}


	/* reset the current offboard mission if needed */
	if (need_to_reset_mission(true)) {
		reset_offboard_mission(_offboard_mission);
		update_offboard_mission();
		_navigator->reset_cruising_speed();
		offboard_updated = true;
	}


	/* reset mission items if needed */
	if (onboard_updated || offboard_updated) {
		set_mission_items();
	}

    然后判断当前航段是否结束，如果结束的话，就设定状态为到达当前航点，并推进任务流程、重新设定当前的航段。

	/* lets check if we reached the current mission item */
	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {

		/* If we just completed a takeoff which was inserted before the right waypoint,
		   there is no need to report that we reached it because we didn't. */
		if (_work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
			set_mission_item_reached();
		}

		if (_mission_item.autocontinue) {
			/* switch to next waypoint if 'autocontinue' flag set */
			advance_mission();
			set_mission_items();
		}

	} 

    其中，到达航点的判定函数MissionBlock::is_mission_item_reached()又要根据不同的设定情况分别进行处理。

    最后再回到navigator中发布三点航段的信息和任务信息。

		if (_pos_sp_triplet_updated) {
			_pos_sp_triplet.timestamp = hrt_absolute_time();
			publish_position_setpoint_triplet();
			_pos_sp_triplet_updated = false;
		}

		if (_mission_result_updated) {
			publish_mission_result();
			_mission_result_updated = false;
		}

    因此，navigator在这里的作用就是：从航线信息里顺次读取执行的航段；处理各种特殊的情形；在航段更新之后重新加载新的航段信息。

第四步，位置控制的实现。

    在mc_pos_control的主循环中，首先调用poll_subscriptions()获取从navigator发布的三点航段信息_pos_sp_triplet。然后在do_control()中进入control_non_manual()的分支，实现非手动模式的位置控制。

    在control_non_manual()中，首先调用了control_auto()利用当前位置和航段首尾点，经过投影获得控制期望的位置_pos_sp；然后调用函数control_position()利用当前位置_pos和期望位置_pos_sp得到期望速度_vel_sp。control_position()函数与手动模式中相同。

    再回到mc_pos_control的主循环中，将期望位置_pos_sp和期望速度_vel_sp写入_local_pos_sp的对应位置，并通过消息vehicle_local_position_setpoint发布出去。

第五步，姿态控制。

    后续分析。

第六步，控制信号输出。

    与手动模式相同。

另外，具体代码在实现过程中考虑到很多边边角角的地方，需要对照代码去逐一分析。