PX4代码分析-(四) FlightModeManager代码分析

一、Flight_mode代码结构分析及模块作用

      Flight_mode模块从字面意思是飞行模式，但是我觉得可以理解为制导模块。该模块和navigator模块构成了PX4的导航和制导功能。

      上一篇博客介绍了navigator模块，它会生成_pos_sp_triplet导航点，这个导航点为什么不直接作为目标位置，我的理解，有两个原因：

1. Navigator主要针对自动飞行模式，生成的航点都是根据自动飞行模式生成点，不能处理手动飞行时的功能；
2. Navigator生成的航点可能比较粗糙，比如飞mission模式时，在地图上标注的相邻点可能间隔很远，为了飞得更平顺，需要生成更平顺的航迹点。

由代码结构可以看出，flight_mode_manager模块是由一个FlightModeManager主函数及多个不同的tasks组成的。跟navigator模块不一样的是，navigator中的多个任务模式之间是独立，功能不复用。而flight_mode中的task之间很多都是重复的，通过继承的方式实现功能的复用。

    Flight_mode模块订阅_vehicle_status这个话题，然后根据这个话题中的nav_state（导航状态）切换到不同的task。最后在不同的task中根据任务的具体内容生成trajectory_setpoint，并将其发布，最终在pid控制中以此作为目标位置进行控制。

还是老样子，本次只分析Flight_mode的代码执行流程，具体的任务模式都是相对独立的模块，待以后详细分析

二、Flight_mode功能结构图

三、代码解析

      本次主要对FlightModeManager主文件中的重要函数进行注释分析

void FlightModeManager::Run()
{
	if (should_exit()) {
		_vehicle_local_position_sub.unregisterCallback();
		exit_and_cleanup();
		return;
	}

	perf_begin(_loop_perf);

	// Check if parameters have changed
	//检查参数是否更新
	if (_parameter_update_sub.updated()) {
		// clear update
		parameter_update_s param_update;
		_parameter_update_sub.copy(&param_update);
		updateParams();
	}

	// generate setpoints on local position changes
	//当位置发生变化时，生成期望的位置点
	vehicle_local_position_s vehicle_local_position;

	if (_vehicle_local_position_sub.update(&vehicle_local_position)) {//首先判断本地位置是否更新
		const hrt_abstime time_stamp_now = vehicle_local_position.timestamp_sample;
		// Guard against too small (< 0.2ms) and too large (> 100ms) dt's.
		//对微分时间进行限制
		const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) / 1e6f), 0.0002f, 0.1f);
		_time_stamp_last_loop = time_stamp_now;

		_home_position_sub.update();//订阅最新的home位置数据
		_vehicle_control_mode_sub.update();//订阅控制模式
		_vehicle_land_detected_sub.update();//订阅落地检测的状态，包含是否落地等
		_vehicle_status_sub.update();//飞机状态

		start_flight_task();//飞行任务切换

		if (_vehicle_command_sub.updated()) {//对指令进行处理
			handleCommand();
		}

		tryApplyCommandIfAny();

		if (isAnyTaskActive()) {
			generateTrajectorySetpoint(dt, vehicle_local_position);//调用不同的任务，生成航迹位置点
		}

	}

	perf_end(_loop_perf);
}

void FlightModeManager::start_flight_task()
{
	// Do not run any flight task for VTOLs in fixed-wing mode
	//在垂直起降固定翼模式时的固定翼飞行状态时，不执行任何飞行任务
	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
		switchTask(FlightTaskIndex::None);
		return;
	}

	// Only run transition flight task if altitude control is enabled (e.g. in Altitdue, Position, Auto flight mode)
	//当高度控制打开后，只执行transition飞行任务
	if (_vehicle_status_sub.get().in_transition_mode && _vehicle_control_mode_sub.get().flag_control_altitude_enabled) {
		switchTask(FlightTaskIndex::Transition);
		return;
	}

	bool found_some_task = false;
	bool matching_task_running = true;
	bool task_failure = false;
	const bool nav_state_descend = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND);

	// Follow me
	//当导航状态为跟随时，切换到自动跟随任务
	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
		found_some_task = true;
		FlightTaskError error = FlightTaskError::InvalidTask;

#if !defined(CONSTRAINED_FLASH)
		error = switchTask(FlightTaskIndex::AutoFollowTarget);
#endif // !CONSTRAINED_FLASH

		if (error != FlightTaskError::NoError) {
			matching_task_running = false;
			task_failure = true;
		}
	}

	// Orbit
	//环绕状态
	if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT)
	    && !_command_failed) {
		found_some_task = true;
		FlightTaskError error = FlightTaskError::InvalidTask;

#if !defined(CONSTRAINED_FLASH)
		error = switchTask(FlightTaskIndex::Orbit);
#endif // !CONSTRAINED_FLASH

		if (error != FlightTaskError::NoError) {
			matching_task_running = false;
			task_failure = true;
		}
	}

	// Navigator interface for autonomous modes
	//导航的自动模式
	if (_vehicle_control_mode_sub.get().flag_control_auto_enabled
	    && !nav_state_descend) {
		found_some_task = true;

		if (switchTask(FlightTaskIndex::Auto) != FlightTaskError::NoError) {
			matching_task_running = false;
			task_failure = true;
		}
	}

	// Manual position control
	//手动控制模式
	if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL) || task_failure) {
		found_some_task = true;
		FlightTaskError error = FlightTaskError::NoError;

		switch (_param_mpc_pos_mode.get()) {
		case 0:
			error = switchTask(FlightTaskIndex::ManualPosition);
			break;

		case 3:
			error = switchTask(FlightTaskIndex::ManualPositionSmoothVel);
			break;

		case 4:
		default:
			if (_param_mpc_pos_mode.get() != 4) {
				PX4_ERR("MPC_POS_MODE %" PRId32 " invalid, resetting", _param_mpc_pos_mode.get());
				_param_mpc_pos_mode.set(4);
				_param_mpc_pos_mode.commit();
			}

			error = switchTask(FlightTaskIndex::ManualAcceleration);
			break;
		}

		task_failure = (error != FlightTaskError::NoError);
		matching_task_running = matching_task_running && !task_failure;
	}

	// Manual altitude control
	//手动高度控制
	if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL) || task_failure) {
		found_some_task = true;
		FlightTaskError error = FlightTaskError::NoError;

		switch (_param_mpc_pos_mode.get()) {
		case 0:
			error = switchTask(FlightTaskIndex::ManualAltitude);
			break;

		case 3:
		default:
			error = switchTask(FlightTaskIndex::ManualAltitudeSmoothVel);
			break;
		}

		task_failure = (error != FlightTaskError::NoError);
		matching_task_running = matching_task_running && !task_failure;
	}

	// Emergency descend
	//紧急下降
	if (nav_state_descend || task_failure) {
		found_some_task = true;

		FlightTaskError error = switchTask(FlightTaskIndex::Descend);

		task_failure = (error != FlightTaskError::NoError);
		matching_task_running = matching_task_running && !task_failure;
	}

	if (task_failure) {
		// For some reason no task was able to start, go into failsafe flighttask
		found_some_task = (switchTask(FlightTaskIndex::Failsafe) == FlightTaskError::NoError);
	}

	if (!found_some_task) {
		switchTask(FlightTaskIndex::None);
	}

	if (!matching_task_running && _vehicle_control_mode_sub.get().flag_armed && !_no_matching_task_error_printed) {
		PX4_ERR("Matching flight task was not able to run, Nav state: %" PRIu8 ", Task: %" PRIu32,
			_vehicle_status_sub.get().nav_state, static_cast<uint32_t>(_current_task.index));
	}

	_no_matching_task_error_printed = !matching_task_running;
}


void FlightModeManager::generateTrajectorySetpoint(const float dt,
		const vehicle_local_position_s &vehicle_local_position)
{
	// If the task fails sned out empty NAN setpoints and the controller will emergency failsafe
	trajectory_setpoint_s setpoint = FlightTask::empty_trajectory_setpoint;
	vehicle_constraints_s constraints = FlightTask::empty_constraints;
	//update函数作为当前任务的执行函数，在该函数中根据任务的不同计算航迹点
	if (_current_task.task->updateInitialize() && _current_task.task->update()) {
		// setpoints and constraints for the position controller from flighttask
		setpoint = _current_task.task->getTrajectorySetpoint();
		constraints = _current_task.task->getConstraints();
	}

	// limit altitude according to land detector
	//根据降落检测状态对高度进行限制
	limitAltitude(setpoint, vehicle_local_position);
	//读取起飞状态
	if (_takeoff_status_sub.updated()) {
		takeoff_status_s takeoff_status;

		if (_takeoff_status_sub.copy(&takeoff_status)) {
			_takeoff_state = takeoff_status.takeoff_state;
		}
	}
	//当起飞状态还在爬升阶段或者还没到爬升阶段时，将当前任务进行释放
	if (_takeoff_state < takeoff_status_s::TAKEOFF_STATE_RAMPUP) {
		// reactivate the task which will reset the setpoint to current state
		_current_task.task->reActivate();
	}


	setpoint.timestamp = hrt_absolute_time();
	_trajectory_setpoint_pub.publish(setpoint);//发布航迹点

	constraints.timestamp = hrt_absolute_time();
	_vehicle_constraints_pub.publish(constraints);

	// if there's any change in landing gear setpoint publish it
	//跟固定翼起落架相关的，发布起落架相关的函数
	landing_gear_s landing_gear = _current_task.task->getGear();

	if (landing_gear.landing_gear != _old_landing_gear_position
	    && landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) {

		landing_gear.timestamp = hrt_absolute_time();
		_landing_gear_pub.publish(landing_gear);
	}

	_old_landing_gear_position = landing_gear.landing_gear;
}


//任务切换函数，根据任务的index切换到不同的任务
FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
{
	// switch to the running task, nothing to do
	//如果待切换的任务和当前执行的任务相同，则什么都不做
	if (new_task_index == _current_task.index) {
		return FlightTaskError::NoError;
	}

	// Save current setpoints for the next FlightTask
	//将当前任务的目标点保存，作为下一个任务的目标点，这样可以保证飞行的平滑
	trajectory_setpoint_s last_setpoint = FlightTask::empty_trajectory_setpoint;
	ekf_reset_counters_s last_reset_counters{};

	if (isAnyTaskActive()) {
		last_setpoint = _current_task.task->getTrajectorySetpoint();
		last_reset_counters = _current_task.task->getResetCounters();
	}
	//对待切换的任务进行初始化
	if (_initTask(new_task_index)) {
		// invalid task
		return FlightTaskError::InvalidTask;
	}

	if (!isAnyTaskActive()) {
		// no task running
		return FlightTaskError::NoError;
	}

	// activation failed
	//updateInitialize和activate函数就是对应的任务首次执行函数，类似于初始化作用
	if (!_current_task.task->updateInitialize() || !_current_task.task->activate(last_setpoint)) {
		_current_task.task->~FlightTask();
		_current_task.task = nullptr;
		_current_task.index = FlightTaskIndex::None;
		return FlightTaskError::ActivationFailed;
	}

	_current_task.task->setResetCounters(last_reset_counters);
	_command_failed = false;

	return FlightTaskError::NoError;
}