Pixhawk学习4——Commander相关分析

最新推荐文章于 2023-12-20 22:36:38 发布
最新推荐文章于 2023-12-20 22:36:38 发布
阅读量946 收藏 2
点赞数
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/victoryqian528/article/details/103372591
版权

Commander文件夹中的内容的作用主要为Pixhawk飞行模式的切换。该段程序会根据遥控器上的开关状态以及飞机飞行状态和各传感器性能状态进行判断，最终实现飞行模式的切换。
飞行模式切换主要涉及到以下几个文件：
src/modules/sensors/sensors.cpp
src/modules/commander/state_machine_helper.cpp
src/modules/commander/commander.cpp
几个文件之间的关系框图如下：
该图为涉及到commander的整个函数流程图

1、sensors.cpp中，发布遥控器信息

		/* only publish manual control if the signal is still present */
		if (!signal_lost) {

			/* initialize manual setpoint */
			struct manual_control_setpoint_s manual = {};
			/* set mode slot to unassigned */
			manual.mode_slot = manual_control_setpoint_s::MODE_SLOT_NONE;
			/* set the timestamp to the last signal time */
			manual.timestamp = rc_input.timestamp_last_signal;

			/* limit controls */
			manual.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
			manual.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
			manual.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
			manual.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
			manual.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
			manual.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
			manual.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
			manual.aux3 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
			manual.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
			manual.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);

			if (_parameters.rc_map_flightmode > 0) {

				/* the number of valid slots equals the index of the max marker minus one */
				const int num_slots = manual_control_setpoint_s::MODE_SLOT_MAX;

				/* the half width of the range of a slot is the total range
				 * divided by the number of slots, again divided by two
				 */
				const float slot_width_half = 2.0f / num_slots / 2.0f;

				/* min is -1, max is +1, range is 2. We offset below min and max */
				const float slot_min = -1.0f - 0.05f;
				const float slot_max = 1.0f + 0.05f;

				/* the slot gets mapped by first normalizing into a 0..1 interval using min
				 * and max. Then the right slot is obtained by multiplying with the number of
				 * slots. And finally we add half a slot width to ensure that integer rounding
				 * will take us to the correct final index.
				 */
				manual.mode_slot = (((((_rc.channels[_parameters.rc_map_flightmode - 1] - slot_min) * num_slots) + slot_width_half) /
						     (slot_max - slot_min)) + (1.0f / num_slots));

				if (manual.mode_slot >= num_slots) {
					manual.mode_slot = num_slots - 1;
				}
			}

			/* mode switches */
			manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th,
					     _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
			manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE,
						  _parameters.rc_rattitude_th,
						  _parameters.rc_rattitude_inv);
			manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th,
					       _parameters.rc_posctl_inv);
			manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th,
					       _parameters.rc_return_inv);
			manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th,
					       _parameters.rc_loiter_inv);
			manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th,
					     _parameters.rc_acro_inv);
			manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD,
						 _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
			manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,
					     _parameters.rc_killswitch_th, _parameters.rc_killswitch_inv);
			manual.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION,
						   _parameters.rc_trans_th, _parameters.rc_trans_inv);
			manual.gear_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_GEAR,
					     _parameters.rc_gear_th, _parameters.rc_gear_inv);

			/* publish manual_control_setpoint topic */
			if (_manual_control_pub != nullptr) {
				orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual);

			} else {
				_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
			}

			/* copy from mapped manual control to control group 3 */
			struct actuator_controls_s actuator_group_3 = {};

			actuator_group_3.timestamp = rc_input.timestamp_last_signal;

			actuator_group_3.control[0] = manual.y;
			actuator_group_3.control[1] = manual.x;
			actuator_group_3.control[2] = manual.r;
			actuator_group_3.control[3] = manual.z;
			actuator_group_3.control[4] = manual.flaps;
			actuator_group_3.control[5] = manual.aux1;
			actuator_group_3.control[6] = manual.aux2;
			actuator_group_3.control[7] = manual.aux3;

			/* publish actuator_controls_3 topic */
			if (_actuator_group_3_pub != nullptr) {
				orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);

			} else {
				_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
			}

			/* Update parameters from RC Channels (tuning with RC) if activated */
			static hrt_abstime last_rc_to_param_map_time = 0;

			if (hrt_elapsed_time(&last_rc_to_param_map_time) > 1e6) {
				set_params_from_rc();
				last_rc_to_param_map_time = hrt_absolute_time();
			}
		}

2、commander.cpp中，commander_thread_main(int argc, char *argv[])函数里提取遥控器信息

	/* Subscribe to manual control data */
	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	memset(&sp_man, 0, sizeof(sp_man));

3、commander.cpp中，判断遥控器开关是否有效。

transition_result_t
set_main_state_rc(struct vehicle_status_s *status_local)
{
	/* set main state according to RC switches */
	transition_result_t res = TRANSITION_DENIED;

	// XXX this should not be necessary any more, we should be able to
	// just delete this and respond to mode switches
	/* if offboard is set already by a mavlink command, abort */
	if (status_flags.offboard_control_set_by_command) {
		return main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state);
	}

	/* manual setpoint has not updated, do not re-evaluate it */
	if (((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man.timestamp)) ||
		((_last_sp_man.offboard_switch == sp_man.offboard_switch) &&
		 (_last_sp_man.return_switch == sp_man.return_switch) &&
		 (_last_sp_man.mode_switch == sp_man.mode_switch) &&
		 (_last_sp_man.acro_switch == sp_man.acro_switch) &&
		 (_last_sp_man.rattitude_switch == sp_man.rattitude_switch) &&
		 (_last_sp_man.posctl_switch == sp_man.posctl_switch) &&
		 (_last_sp_man.loiter_switch == sp_man.loiter_switch) &&
		 (_last_sp_man.mode_slot == sp_man.mode_slot))) {

		// update these fields for the geofence system

		if (!warning_action_on) {
			_last_sp_man.timestamp = sp_man.timestamp;
			_last_sp_man.x = sp_man.x;
			_last_sp_man.y = sp_man.y;
			_last_sp_man.z = sp_man.z;
			_last_sp_man.r = sp_man.r;
		}

		/* no timestamp change or no switch change -> nothing changed */
		return TRANSITION_NOT_CHANGED;
	}

	_last_sp_man = sp_man;

	/* offboard switch overrides main switch */
	if (sp_man.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
		res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state);

		if (res == TRANSITION_DENIED) {
			print_reject_mode(status_local, "OFFBOARD");
			/* mode rejected, continue to evaluate the main system mode */

		} else {
			/* changed successfully or already in this state */
			return res;
		}
	}

	/* RTL switch overrides main switch */
	if (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
		warnx("RTL switch changed and ON!");
		res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state);

		if (res == TRANSITION_DENIED) {
			print_reject_mode(status_local, "AUTO RTL");

			/* fallback to LOITER if home position not set */
			res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);
		}

		if (res != TRANSITION_DENIED) {
			/* changed successfully or already in this state */
			return res;
		}

		/* if we get here mode was rejected, continue to evaluate the main system mode */
	}

	/* we know something has changed - check if we are in mode slot operation */
	if (sp_man.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) {

		if (sp_man.mode_slot >= sizeof(_flight_mode_slots) / sizeof(_flight_mode_slots[0])) {
			warnx("m slot overflow");
			return TRANSITION_DENIED;
		}

		int new_mode = _flight_mode_slots[sp_man.mode_slot];

		if (new_mode < 0) {
			/* slot is unused */
			res = TRANSITION_NOT_CHANGED;

		} else {
			res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);

			/* ensure that the mode selection does not get stuck here */
			int maxcount = 5;

			/* enable the use of break */
			/* fallback strategies, give the user the closest mode to what he wanted */
			while (res == TRANSITION_DENIED && maxcount > 0) {

				maxcount--;

				if (new_mode == commander_state_s::MAIN_STATE_AUTO_MISSION) {

					/* fall back to loiter */
					new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
					print_reject_mode(status_local, "AUTO MISSION");
					res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);

					if (res != TRANSITION_DENIED) {
						break;
					}
				}

				if (new_mode == commander_state_s::MAIN_STATE_AUTO_RTL) {

					/* fall back to position control */
					new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
					print_reject_mode(status_local, "AUTO RTL");
					res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);

					if (res != TRANSITION_DENIED) {
						break;
					}
				}

				if (new_mode == commander_state_s::MAIN_STATE_AUTO_LAND) {

					/* fall back to position control */
					new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
					print_reject_mode(status_local, "AUTO LAND");
					res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);

					if (res != TRANSITION_DENIED) {
						break;
					}
				}

				if (new_mode == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) {

					/* fall back to position control */
					new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
					print_reject_mode(status_local, "AUTO TAKEOFF");
					res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);

					if (res != TRANSITION_DENIED) {
						break;
					}
				}

				if (new_mode == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET) {

					/* fall back to position control */
					new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
					print_reject_mode(status_local, "AUTO FOLLOW");
					res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);

					if (res != TRANSITION_DENIED) {
						break;
					}
				}

				if (new_mode == commander_state_s::MAIN_STATE_AUTO_LOITER) {

					/* fall back to position control */
					new_mode = commander_state_s::MAIN_STATE_POSCTL;
					print_reject_mode(status_local, "AUTO HOLD");
					res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);

					if (res != TRANSITION_DENIED) {
						break;
					}
				}

				if (new_mode == commander_state_s::MAIN_STATE_POSCTL) {

					/* fall back to altitude control */
					new_mode = commander_state_s::MAIN_STATE_ALTCTL;
					print_reject_mode(status_local, "POSITION CONTROL");
					res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);

					if (res != TRANSITION_DENIED) {
						break;
					}
				}

				if (new_mode == commander_state_s::MAIN_STATE_ALTCTL) {

					/* fall back to stabilized */
					new_mode = commander_state_s::MAIN_STATE_STAB;
					print_reject_mode(status_local, "ALTITUDE CONTROL");
					res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);

					if (res != TRANSITION_DENIED) {
						break;
					}
				}

				if (new_mode == commander_state_s::MAIN_STATE_STAB) {

					/* fall back to manual */
					new_mode = commander_state_s::MAIN_STATE_MANUAL;
					print_reject_mode(status_local, "STABILIZED");
					res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);

					if (res != TRANSITION_DENIED) {
						break;
					}
				}
			}
		}

		return res;
	}

	/* offboard and RTL switches off or denied, check main mode switch */
	switch (sp_man.mode_switch) {
	case manual_control_setpoint_s::SWITCH_POS_NONE:
		res = TRANSITION_NOT_CHANGED;
		break;

	case manual_control_setpoint_s::SWITCH_POS_OFF:		// MANUAL
		if (sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {

			/* manual mode is stabilized already for multirotors, so switch to acro
			 * for any non-manual mode
			 */
			// XXX: put ACRO and STAB on separate switches
			if (status.is_rotary_wing && !status.is_vtol) {
				res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, &status_flags, &internal_state);
			} else if (!status.is_rotary_wing) {
				res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);
			} else {
				res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
			}

		}
		else if(sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){
			/* Similar to acro transitions for multirotors.  FW aircraft don't need a
			 * rattitude mode.*/
			if (status.is_rotary_wing) {
				res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, &status_flags, &internal_state);
			} else {
				res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);
			}
		}else {
			res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
		}

		// TRANSITION_DENIED is not possible here
		break;

	case manual_control_setpoint_s::SWITCH_POS_MIDDLE:		// ASSIST
		if (sp_man.posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
			res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);

			if (res != TRANSITION_DENIED) {
				break;	// changed successfully or already in this state
			}

			print_reject_mode(status_local, "POSITION CONTROL");
		}

		// fallback to ALTCTL
		res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags, &internal_state);

		if (res != TRANSITION_DENIED) {
			break;	// changed successfully or already in this mode
		}

		if (sp_man.posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
			print_reject_mode(status_local, "ALTITUDE CONTROL");
		}

		// fallback to MANUAL
		res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
		// TRANSITION_DENIED is not possible here
		break;

	case manual_control_setpoint_s::SWITCH_POS_ON:			// AUTO
		if (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
			res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);

			if (res != TRANSITION_DENIED) {
				break;	// changed successfully or already in this state
			}

			print_reject_mode(status_local, "AUTO PAUSE");

		} else {
			res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state);

			if (res != TRANSITION_DENIED) {
				break;	// changed successfully or already in this state
			}

			print_reject_mode(status_local, "AUTO MISSION");

			// fallback to LOITER if home position not set
			res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);

			if (res != TRANSITION_DENIED) {
				break;  // changed successfully or already in this state
			}
		}

		// fallback to POSCTL
		res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);

		if (res != TRANSITION_DENIED) {
			break;  // changed successfully or already in this state
		}

		// fallback to ALTCTL
		res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags, &internal_state);

		if (res != TRANSITION_DENIED) {
			break;	// changed successfully or already in this state
		}

		// fallback to MANUAL
		res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
		// TRANSITION_DENIED is not possible here
		break;

	default:
		break;
	}

	return res;
}

其中，多次调用了state_machine_helper.cpp中的transition_result_t main_state_transition(……)函数，该函数根据飞行模式，检查飞行器状态（高度估计、位置估计等等反馈数据的状态），反馈是否可以将新的飞行模式赋值给internal_state->main_state。

transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev,
		      status_flags_s *status_flags, struct commander_state_s *internal_state)
{
	transition_result_t ret = TRANSITION_DENIED;

	/* transition may be denied even if the same state is requested because conditions may have changed */
	switch (new_main_state) {
	case commander_state_s::MAIN_STATE_MANUAL:
	case commander_state_s::MAIN_STATE_STAB:
		ret = TRANSITION_CHANGED;
		break;

	case commander_state_s::MAIN_STATE_ACRO:
	case commander_state_s::MAIN_STATE_RATTITUDE:
		if (status->is_rotary_wing) {
			ret = TRANSITION_CHANGED;
		}

		break;

	case commander_state_s::MAIN_STATE_ALTCTL:

		/* need at minimum altitude estimate */
		if (status_flags->condition_local_altitude_valid ||
		    status_flags->condition_global_position_valid) {
			ret = TRANSITION_CHANGED;
		}

		break;

	case commander_state_s::MAIN_STATE_POSCTL:

		/* need at minimum local position estimate */
		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:

		/* need global position estimate */
		if (status_flags->condition_global_position_valid) {
			ret = TRANSITION_CHANGED;
		}

		break;

	case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
	case commander_state_s::MAIN_STATE_AUTO_MISSION:
	case commander_state_s::MAIN_STATE_AUTO_RTL:
	case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
	case commander_state_s::MAIN_STATE_AUTO_LAND:

		/* need global position and home position */
		if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
			ret = TRANSITION_CHANGED;
		}

		break;

	case commander_state_s::MAIN_STATE_OFFBOARD:

		/* need offboard signal */
		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) {
			main_state_prev = internal_state->main_state;
			internal_state->main_state = new_main_state;
			internal_state->timestamp = hrt_absolute_time();

		} else {
			ret = TRANSITION_NOT_CHANGED;
		}
	}

	return ret;
}

internal_state->main_state共有以下14种形式：

#define MAIN_STATE_MANUAL 0
#define MAIN_STATE_ALTCTL 1
#define MAIN_STATE_POSCTL 2
#define MAIN_STATE_AUTO_MISSION 3
#define MAIN_STATE_AUTO_LOITER 4
#define MAIN_STATE_AUTO_RTL 5
#define MAIN_STATE_ACRO 6
#define MAIN_STATE_OFFBOARD 7
#define MAIN_STATE_STAB 8
#define MAIN_STATE_RATTITUDE 9
#define MAIN_STATE_AUTO_TAKEOFF 10
#define MAIN_STATE_AUTO_LAND 11
#define MAIN_STATE_AUTO_FOLLOW_TARGET 12
#define MAIN_STATE_MAX 13

4、根据internal_state->main_state值，commander_thread_main(int argc, char *argv[])函数里再调用state_machine_helper.cpp中的set_nav_state()函数，根据检查飞行器状态（传感器执行机构：遥控器、发动机、GPS等等），最终确认控制方式status->nav_state的值。

/**
 * Check failsafe and main status and set navigation status for navigator accordingly
 */
bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
		   orb_advert_t *mavlink_log_pub,
		   const bool data_link_loss_enabled, const bool mission_finished,
		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed,
		   const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act)
{
	navigation_state_t nav_state_old = status->nav_state;

	bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED
		      || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
	bool old_failsafe = status->failsafe;
	status->failsafe = false;

	/* evaluate main state to decide in normal (non-failsafe) mode */
	switch (internal_state->main_state) {
	case commander_state_s::MAIN_STATE_ACRO:
	case commander_state_s::MAIN_STATE_MANUAL:
	case commander_state_s::MAIN_STATE_RATTITUDE:
	case commander_state_s::MAIN_STATE_STAB:
	case commander_state_s::MAIN_STATE_ALTCTL:

		/* require RC for all manual modes */
		if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed) {
			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);

			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;

			} 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) {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;

			} else {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
			}

		} else {
			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;
			}
		}

		break;

	case commander_state_s::MAIN_STATE_POSCTL: {
			const bool rc_lost = rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd);

			if (rc_lost && armed) {
				enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);

				if (status_flags->condition_global_position_valid &&
				    status_flags->condition_home_position_valid &&
				    !status_flags->gps_failure) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;

				} else if (status_flags->condition_local_position_valid &&
					   !status_flags->gps_failure) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;

				} else if (status_flags->condition_local_altitude_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;

				} else {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
				}

				/* As long as there is RC, we can fallback to ALTCTL, or STAB. */
				/* A local position estimate is enough for POSCTL for multirotors,
				 * this enables POSCTL using e.g. flow.
				 * For fixedwing, a global position is needed. */

			} else if (((status->is_rotary_wing && !status_flags->condition_local_position_valid) ||
				    (!status->is_rotary_wing && !status_flags->condition_global_position_valid))
				   && armed) {
				enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);

				if (status_flags->condition_local_altitude_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;

				} else {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
				}

			} else {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
			}
		}
		break;

	case commander_state_s::MAIN_STATE_AUTO_MISSION:

		/* go into failsafe
		 * - if commanded to do so
		 * - if we have an engine failure
		 * - if we have vtol transition failure
		 * - depending on datalink, RC and if the mission is finished */

		/* first look at the commands */
		if (status->engine_failure_cmd) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;

		} else if (status_flags->data_link_lost_cmd) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;

		} else if (status_flags->gps_failure_cmd) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps_cmd);

		} else if (status_flags->rc_signal_lost_cmd) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;

		} else if (status_flags->vtol_transition_failure_cmd) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;

			/* finished handling commands which have priority, now handle failures */

		} else if (status_flags->gps_failure) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);

		} else if (status->engine_failure) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;

		} else if (status_flags->vtol_transition_failure) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;

		} else if (status->mission_failure) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;

			/* datalink loss enabled:
			 * check for datalink lost: this should always trigger RTGS */

		} else if (data_link_loss_enabled && status->data_link_lost) {
			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);

			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;

			} 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) {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;

			} else {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
			}

			/* datalink loss disabled:
			 * check if both, RC and datalink are lost during the mission
			 * or all links are lost after the mission finishes in air: this should always trigger RCRECOVER */

		} else if (!data_link_loss_enabled && status->rc_signal_lost && status->data_link_lost && !landed && mission_finished) {
			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);

			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;

			} 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) {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;

			} else {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
			}

			/* stay where you are if you should stay in failsafe, otherwise everything is perfect */

		} else if (!stay_in_failsafe) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
		}

		break;

	case commander_state_s::MAIN_STATE_AUTO_LOITER:

		/* go into failsafe on a engine failure */
		if (status->engine_failure) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;

		} else if (status_flags->gps_failure) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);

			/* also go into failsafe if just datalink is lost */

		} else if (status->data_link_lost && data_link_loss_enabled) {
			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);

			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;

			} 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) {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;

			} else {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
			}

			/* go into failsafe if RC is lost and datalink loss is not set up and rc loss is not disabled */

		} else if (status->rc_signal_lost && rc_loss_enabled && !data_link_loss_enabled) {
			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;

			} 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) {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;

			} else {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
			}

			/* don't bother if RC is lost if datalink is connected */

		} else if (status->rc_signal_lost) {

			/* this mode is ok, we don't need RC for loitering */
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;

		} else {
			/* everything is perfect */
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
		}

		break;

	case commander_state_s::MAIN_STATE_AUTO_RTL:

		/* require global position and home, also go into failsafe on an engine failure */

		if (status->engine_failure) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;

		} else if (status_flags->gps_failure) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);

		} else if ((!status_flags->condition_global_position_valid ||
			    !status_flags->condition_home_position_valid)) {
			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_home);

			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) {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;

			} else {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
			}

		} else {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
		}

		break;

	case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:

		/* require global position and home */

		if (status->engine_failure) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;

		} else if (!status_flags->condition_global_position_valid) {
			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);

			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) {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;

			} else {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
			}

		} else {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;
		}

		break;

	case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:

		/* require global position and home */

		if (status->engine_failure) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;

		} else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid ||
				!status_flags->condition_home_position_valid)) {
			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);

			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) {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;

			} else {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
			}

		} else {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
		}

		break;

	case commander_state_s::MAIN_STATE_AUTO_LAND:

		/* require global position and home */

		if (status->engine_failure) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;

		} else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid ||
				!status_flags->condition_home_position_valid)) {
			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);

			if (status_flags->condition_local_altitude_valid) {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;

			} else {
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
			}

		} else {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
		}

		break;

	case commander_state_s::MAIN_STATE_OFFBOARD:

		/* require offboard control, otherwise stay where you are */
		if (status_flags->offboard_control_signal_lost && !status->rc_signal_lost) {
			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);

			if (status_flags->offboard_control_loss_timeout && offb_loss_rc_act < 5 && offb_loss_rc_act >= 0) {
				if (offb_loss_rc_act == 3 && status_flags->condition_global_position_valid
				    && status_flags->condition_home_position_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;

				} else if (offb_loss_rc_act == 0 && status_flags->condition_global_position_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;

				} else if (offb_loss_rc_act == 1 && status_flags->condition_local_altitude_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;

				} else if (offb_loss_rc_act == 2) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;

				} else if (offb_loss_rc_act == 4 && status_flags->condition_global_position_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;

				} else if (status_flags->condition_local_altitude_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;

				} else {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
				}

			} else {
				if (status_flags->condition_global_position_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;

				} else if (status_flags->condition_local_altitude_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;

				} else {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
				}
			}

		} else if (status_flags->offboard_control_signal_lost && status->rc_signal_lost) {
			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);

			if (status_flags->offboard_control_loss_timeout && offb_loss_act < 3 && offb_loss_act >= 0) {
				if (offb_loss_act == 2 && status_flags->condition_global_position_valid
				    && status_flags->condition_home_position_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;

				} else if (offb_loss_act == 1 && status_flags->condition_global_position_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;

				} else if (offb_loss_act == 0 && status_flags->condition_global_position_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;

				} else if (status_flags->condition_local_altitude_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;

				} else {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
				}

			} else {
				if (status_flags->condition_global_position_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;

				} else if (status_flags->condition_local_altitude_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;

				} else {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
				}
			}

		} else {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
		}

	default:
		break;
	}

	return status->nav_state != nav_state_old;
}

status->nav_state的值有以下21中形式：

#define NAVIGATION_STATE_MANUAL 0
#define NAVIGATION_STATE_ALTCTL 1
#define NAVIGATION_STATE_POSCTL 2
#define NAVIGATION_STATE_AUTO_MISSION 3
#define NAVIGATION_STATE_AUTO_LOITER 4
#define NAVIGATION_STATE_AUTO_RTL 5
#define NAVIGATION_STATE_AUTO_RCRECOVER 6
#define NAVIGATION_STATE_AUTO_RTGS 7
#define NAVIGATION_STATE_AUTO_LANDENGFAIL 8
#define NAVIGATION_STATE_AUTO_LANDGPSFAIL 9
#define NAVIGATION_STATE_ACRO 10
#define NAVIGATION_STATE_UNUSED 11
#define NAVIGATION_STATE_DESCEND 12
#define NAVIGATION_STATE_TERMINATION 13
#define NAVIGATION_STATE_OFFBOARD 14
#define NAVIGATION_STATE_STAB 15
#define NAVIGATION_STATE_RATTITUDE 16
#define NAVIGATION_STATE_AUTO_TAKEOFF 17
#define NAVIGATION_STATE_AUTO_LAND 18
#define NAVIGATION_STATE_AUTO_FOLLOW_TARGET 19
#define NAVIGATION_STATE_MAX 20

5、最后，commander_thread_main(int argc, char *argv[])函数里调用set_control_mode()函数，根据status->nav_state值，确定控制模式 control_mode.flag_xxx。

void
set_control_mode()
{
	/* set vehicle_control_mode according to set_navigation_state */
	control_mode.flag_armed = armed.armed;
	control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);
	control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
	control_mode.flag_control_offboard_enabled = false;

	switch (status.nav_state) {
	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
		control_mode.flag_control_manual_enabled = true;
		control_mode.flag_control_auto_enabled = false;
		control_mode.flag_control_rates_enabled = stabilization_required();
		control_mode.flag_control_attitude_enabled = stabilization_required();
		control_mode.flag_control_rattitude_enabled = false;
		control_mode.flag_control_altitude_enabled = false;
		control_mode.flag_control_climb_rate_enabled = false;
		control_mode.flag_control_position_enabled = false;
		control_mode.flag_control_velocity_enabled = false;
		control_mode.flag_control_acceleration_enabled = false;
		control_mode.flag_control_termination_enabled = false;
		break;

	case vehicle_status_s::NAVIGATION_STATE_STAB:
		control_mode.flag_control_manual_enabled = true;
		control_mode.flag_control_auto_enabled = false;
		control_mode.flag_control_rates_enabled = true;
		control_mode.flag_control_attitude_enabled = true;
		control_mode.flag_control_rattitude_enabled = true;
		control_mode.flag_control_altitude_enabled = false;
		control_mode.flag_control_climb_rate_enabled = false;
		control_mode.flag_control_position_enabled = false;
		control_mode.flag_control_velocity_enabled = false;
		control_mode.flag_control_acceleration_enabled = false;
		control_mode.flag_control_termination_enabled = false;
		/* override is not ok in stabilized mode */
		control_mode.flag_external_manual_override_ok = false;
		break;

	case vehicle_status_s::NAVIGATION_STATE_RATTITUDE:
		control_mode.flag_control_manual_enabled = true;
		control_mode.flag_control_auto_enabled = false;
		control_mode.flag_control_rates_enabled = true;
		control_mode.flag_control_attitude_enabled = true;
		control_mode.flag_control_rattitude_enabled = true;
		control_mode.flag_control_altitude_enabled = false;
		control_mode.flag_control_climb_rate_enabled = false;
		control_mode.flag_control_position_enabled = false;
		control_mode.flag_control_velocity_enabled = false;
		control_mode.flag_control_acceleration_enabled = false;
		control_mode.flag_control_termination_enabled = false;
		break;

	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
		control_mode.flag_control_manual_enabled = true;
		control_mode.flag_control_auto_enabled = false;
		control_mode.flag_control_rates_enabled = true;
		control_mode.flag_control_attitude_enabled = true;
		control_mode.flag_control_rattitude_enabled = false;
		control_mode.flag_control_altitude_enabled = true;
		control_mode.flag_control_climb_rate_enabled = true;
		control_mode.flag_control_position_enabled = false;
		control_mode.flag_control_velocity_enabled = false;
		control_mode.flag_control_acceleration_enabled = false;
		control_mode.flag_control_termination_enabled = false;
		break;

	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
		control_mode.flag_control_manual_enabled = true;
		control_mode.flag_control_auto_enabled = false;
		control_mode.flag_control_rates_enabled = true;
		control_mode.flag_control_attitude_enabled = true;
		control_mode.flag_control_rattitude_enabled = false;
		control_mode.flag_control_altitude_enabled = true;
		control_mode.flag_control_climb_rate_enabled = true;
		control_mode.flag_control_position_enabled = !status.in_transition_mode;
		control_mode.flag_control_velocity_enabled = !status.in_transition_mode;
		control_mode.flag_control_acceleration_enabled = false;
		control_mode.flag_control_termination_enabled = false;
		break;

	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
	case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
		/* override is not ok for the RTL and recovery mode */
		control_mode.flag_external_manual_override_ok = false;
		/* fallthrough */
	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
		control_mode.flag_control_manual_enabled = false;
		control_mode.flag_control_auto_enabled = true;
		control_mode.flag_control_rates_enabled = true;
		control_mode.flag_control_attitude_enabled = true;
		control_mode.flag_control_rattitude_enabled = false;
		control_mode.flag_control_altitude_enabled = true;
		control_mode.flag_control_climb_rate_enabled = true;
		control_mode.flag_control_position_enabled = !status.in_transition_mode;
		control_mode.flag_control_velocity_enabled = !status.in_transition_mode;
		control_mode.flag_control_acceleration_enabled = false;
		control_mode.flag_control_termination_enabled = false;
		break;

	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
		control_mode.flag_control_manual_enabled = false;
		control_mode.flag_control_auto_enabled = false;
		control_mode.flag_control_rates_enabled = true;
		control_mode.flag_control_attitude_enabled = true;
		control_mode.flag_control_rattitude_enabled = false;
		control_mode.flag_control_altitude_enabled = false;
		control_mode.flag_control_climb_rate_enabled = true;
		control_mode.flag_control_position_enabled = false;
		control_mode.flag_control_velocity_enabled = false;
		control_mode.flag_control_acceleration_enabled = false;
		control_mode.flag_control_termination_enabled = false;
		break;

	case vehicle_status_s::NAVIGATION_STATE_ACRO:
		control_mode.flag_control_manual_enabled = true;
		control_mode.flag_control_auto_enabled = false;
		control_mode.flag_control_rates_enabled = true;
		control_mode.flag_control_attitude_enabled = false;
		control_mode.flag_control_rattitude_enabled = false;
		control_mode.flag_control_altitude_enabled = false;
		control_mode.flag_control_climb_rate_enabled = false;
		control_mode.flag_control_position_enabled = false;
		control_mode.flag_control_velocity_enabled = false;
		control_mode.flag_control_acceleration_enabled = false;
		control_mode.flag_control_termination_enabled = false;
		break;

	case vehicle_status_s::NAVIGATION_STATE_DESCEND:
		/* TODO: check if this makes sense */
		control_mode.flag_control_manual_enabled = false;
		control_mode.flag_control_auto_enabled = true;
		control_mode.flag_control_rates_enabled = true;
		control_mode.flag_control_attitude_enabled = true;
		control_mode.flag_control_rattitude_enabled = false;
		control_mode.flag_control_position_enabled = false;
		control_mode.flag_control_velocity_enabled = false;
		control_mode.flag_control_acceleration_enabled = false;
		control_mode.flag_control_altitude_enabled = false;
		control_mode.flag_control_climb_rate_enabled = true;
		control_mode.flag_control_termination_enabled = false;
		break;

	case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
		/* disable all controllers on termination */
		control_mode.flag_control_manual_enabled = false;
		control_mode.flag_control_auto_enabled = false;
		control_mode.flag_control_rates_enabled = false;
		control_mode.flag_control_attitude_enabled = false;
		control_mode.flag_control_rattitude_enabled = false;
		control_mode.flag_control_position_enabled = false;
		control_mode.flag_control_velocity_enabled = false;
		control_mode.flag_control_acceleration_enabled = false;
		control_mode.flag_control_altitude_enabled = false;
		control_mode.flag_control_climb_rate_enabled = false;
		control_mode.flag_control_termination_enabled = true;
		break;

	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
		control_mode.flag_control_manual_enabled = false;
		control_mode.flag_control_auto_enabled = false;
		control_mode.flag_control_offboard_enabled = true;

		/*
		 * The control flags depend on what is ignored according to the offboard control mode topic
		 * Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position)
		 */
		control_mode.flag_control_rates_enabled = !offboard_control_mode.ignore_bodyrate ||
			!offboard_control_mode.ignore_attitude ||
			!offboard_control_mode.ignore_position ||
			!offboard_control_mode.ignore_velocity ||
			!offboard_control_mode.ignore_acceleration_force;

		control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude ||
			!offboard_control_mode.ignore_position ||
			!offboard_control_mode.ignore_velocity ||
			!offboard_control_mode.ignore_acceleration_force;

		control_mode.flag_control_rattitude_enabled = false;

		control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force &&
		  !status.in_transition_mode;

		control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity ||
			!offboard_control_mode.ignore_position) && !status.in_transition_mode &&
			!control_mode.flag_control_acceleration_enabled;

		control_mode.flag_control_climb_rate_enabled = (!offboard_control_mode.ignore_velocity ||
			!offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled;

		control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode &&
		  !control_mode.flag_control_acceleration_enabled;

		control_mode.flag_control_altitude_enabled = (!offboard_control_mode.ignore_velocity ||
			!offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled;

		break;

	default:
		break;
	}
}

6、总结
个人认为，该部分程序如果理顺了其实比较简单，无非就是根据遥控器的开关量，结合飞机飞行状态（飞行反馈和传感器执行结构状态）来切换制定的飞行模式，然后进行相应的飞行控制模式。上述分析中1、2、5都比较好理解。源码中将3和4分开考虑，且3中又调用了另一个.cpp文件中的子函数，而且在set_main_state_rc和main_state_transition两个函数中反复case各种模式，使得程序非常不清晰。3和4是模式切换的核心，但却经历了两次“是否可以改变模式”的判断，个人感觉比较冗余，完全可以将3和4中的判断飞行反馈量状态和传感器执行机构状态合起来考虑，这样整个代码才会比较清晰。

MWM0909
关注
  • 0
    点赞
  • 踩
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
Total commander 11 的配置文件
11-21
可以用在最新的Total commander 11.03的32位或者64位，从官网下载后直接Copy到安排目录就完成了，懂的都懂，wincmd.key
PX4模式切换流程图commander.cpp.pdf
04-26
PX4模式切换流程图commander.cpp
Pixhawk原生固件PX4之commander函数
FantasyJXF的博客
02-09 2969
commander好长……
px4仿真无法起飞问题（Failsafe enabled: no datalink）
weixin_43886742的博客
09-03 3257
报错信息 问题描述：使用JMAVSim和gazebo仿真px4起飞时报错如下： WARN [commander] Failsafe enabled: no datalink 说不安全 解决方法 打开QGC 就可以起飞了
【px4】运行mavsdk中的offboard example
weixin_44537885的博客
07-23 1178
mavsdk实现offboard插件
【PX4_BUG】MAVROS控制无人机offboard模式无法起飞，报错CMD: Unexpected command 176, result 0，重复Offboard enabled
最新发布
在后厂村路努力科研搬砖的无人机控制方向练习生一名～
12-20 1810
主要介绍在通过MAVROS功能包的offboard模式控制gazebo中的飞机起飞到高度两米时遇到无法起飞的BUG，重复报错INFO [commander] Failsafe mode deactivated，INFO [commander] Failsafe mode activated，重复报错CMD: Unexpected command 176, result 0，重复报错Offboard enabled，介绍了解决方法。
阿木社区 PX4 commander解析以及流程图
06-01
Commder 是Px4 源码中的飞行控制命令切换模块，比如由自稳模式切换到，定高，定 点，自动模式。所有模式能不能成功切换，都在这个模块中做了相应的飞行条件检查。自稳 模式的切换，不需要太多的飞行条件。如果是定高...
Commander Badges_commander_
09-30
描述中的"commander badge free zip"指出这是一个免费的压缩文件，包含了有关“commander”级别的徽章。ZIP文件是一种常见的文件压缩格式，它能将多个文件打包成一个单一的文件，便于存储和传输。在这个案例中，用户...
飞扬时空版 Total Commander 10.5
07-12
飞扬时空版 Total Commander 10.5
failsafe,简单、复杂的故障处理.zip
09-26
故障安全是一个轻量级的、零依赖的库，用于处理Java 8中的失败，用一个简洁的API来处理日常用例和处理所有其他事务的灵活性。它的工作原理是用一个或多个弹性策略包装可执行逻辑，这些策略可以根据需要组合和组合。
ubuntu18+jetson nano +px4+ros ＜——＞QGC+ubuntu20+ros(关于仿真和实物运行的持续记录心得)
weixin_41967369的博客
05-27 3132
使用地面电脑控制搭载有jetson nano的pixhwark 2.4.8 无人机；仿真到真机搭建，学习记录不断更新以及期间遇到的问题！！！
源码编译 MAVROS
Xianfeng Design
06-12 1197
介绍MAVROS的编译使用，以及其过程中遇到的问题解决
pixhawk px4 commander.cpp
czyv587的博客
08-27 4183
对于复杂的函数，要做的就是看函数的输入是什么、来自哪里，经过处理后得到什么、给谁用，这样就可以把程序逻辑理清(中间的分析就是看函数如何处理的)extern "C" __EXPORT int commander_main(int argc, char *argv[]);argc和argv是commander_main函数的形参，它们是程序的“命令行参数”。agrc(argument count的缩写，
R Commander指南：交互式统计分析
《Using the R Commander》是学习R语言和统计分析的一个优秀起点，尤其是对于那些希望通过图形化界面来探索R的用户。它不仅提供了统计分析的基础知识，还强调了在实践中应用统计思想的重要性，是教育和自我学习的...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值