PX4代码分析-(二)commander代码分析_px4 actuator_command___Vincent

一、commander的作用

commander相当于整个px4功能的入口， 该模块主要负责对地面站、遥控器以及其它模块发布的cmd命令的处理，还包括设置飞行模式、设置起飞点位置、解锁检测、失控检测、落地检测等功能的处理。

1、遥控器发送了切换land指令，首先是在driver中根据配置的通道信息解析了遥控器的指令并判断是land的指令，driver会生成一条VEHICLE_CMD_NAV_LAND的指令。commander模块接收到该指令后，首先会判断是否具备切换的条件，如果当前状态不能切换到该模式，则拒绝。如果可以，则发出相关的uorb，在navigator模块中进行飞行模式的切换。

2、在收到qgc发送的传感器校准指令时，会切换到校准函数，对陀螺仪、加速度计等传感器进行校准。

3、在处理其他模块发送的信息，如落地检测检测模块检测到已经落地，则commander模块中根据配置的参数进行自动上锁等动作。

二、commandor的思维导图

三、代码结构分析

1、代码位置：commander位于Firmware/src/modules/commander文件夹中。

2、构造函数：

在构造函数中，主要是读取设定的各种参数，然后初始化无人机的状态参数(_vehicle_status)，其中包含了机架类型等参数。

3、由头文件commander.hpp可知，class Commander : public ModuleBase<Commander>, public ModuleParams 所以类Commander是继承了ModuleBase类，所以Commander类中的run()函数是该类的主要的执行函数，所以接来下主要分析该函数。

Commander::run()
{
	bool sensor_fail_tune_played = false;
	const param_t param_airmode = param_find("MC_AIRMODE");//获取对应的参数
	const param_t param_rc_map_arm_switch = param_find("RC_MAP_ARM_SW");
	/* initialize */
	led_init();//led灯初始化，在boards/px4/对应的飞控文件中实现，主要是完成对应的gpio口的初始化
	buzzer_init();//蜂鸣器初始化，跟led一样
#if defined(BOARD_HAS_POWER_CONTROL)
	{
		// we need to do an initial publication to make sure uORB allocates the buffer, which cannot happen
		// in IRQ context.
		power_button_state_s button_state{};
		button_state.timestamp = hrt_absolute_time();
		button_state.event = 0xff;
		power_button_state_pub = orb_advertise(ORB_ID(power_button_state), &button_state);
		_power_button_state_sub.copy(&button_state);
	}
	if (board_register_power_state_notification_cb(power_button_state_notification_cb) != 0) {
		PX4_ERR("Failed to register power notification callback");
	}
#endif // BOARD_HAS_POWER_CONTROL
	get_circuit_breaker_params();//获取对应的参数
	/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
	//初始化任务mission的状态，在mavlink启动失败是，允许navigator导航使用存储的mission数据（mission都是存在sd卡中，通过dm_read()函数来读取）
	mission_init();
	bool param_init_forced = true;
	control_status_leds(&status, &armed, true, _battery_warning);//设置状态灯的状态
	thread_running = true;
	/* update vehicle status to find out vehicle type (required for preflight checks) */
	status.system_type = _param_mav_type.get();
	//判断无人机的类型
	if (is_rotary_wing(&status) || is_vtol(&status)) {
		status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
	} else if (is_fixed_wing(&status)) {
		status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
	} else if (is_ground_rover(&status)) {
		status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
	} else {
		status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
	}
	//根据status判断是否是垂直起降固定翼
	status.is_vtol = is_vtol(&status);
	status.is_vtol_tailsitter = is_vtol_tailsitter(&status);
	_boot_timestamp = hrt_absolute_time();
	// initially set to failed
	_last_lpos_fail_time_us = _boot_timestamp;
	_last_gpos_fail_time_us = _boot_timestamp;
	_last_lvel_fail_time_us = _boot_timestamp;
	// user adjustable duration required to assert arm/disarm via throttle/rudder stick
	//用户可调整的持续时间，以通过油门/方向舵杆进行防护/解除防护,就是设置通过油门解锁/上锁需要的时间
	uint32_t rc_arm_hyst = _param_rc_arm_hyst.get() * COMMANDER_MONITORING_LOOPSPERMSEC;
	int32_t airmode = 0;
	int32_t rc_map_arm_switch = 0;
	//为创建低优先级的线程初始化好运行环境
	/* initialize low priority thread */
	pthread_attr_t commander_low_prio_attr;
	pthread_attr_init(&commander_low_prio_attr);
	pthread_attr_setstacksize(&commander_low_prio_attr, PX4_STACK_ADJUSTED(3304));
#ifndef __PX4_QURT
	// This is not supported by QURT (yet).
	struct sched_param param;
	pthread_attr_getschedparam(&commander_low_prio_attr, &param);
	/* low priority */
	param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
	pthread_attr_setschedparam(&commander_low_prio_attr, &param);
#endif
	pthread_t commander_low_prio_thread;
	pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, nullptr);//创建新的线程，也就是会有两个线程在执行
	pthread_attr_destroy(&commander_low_prio_attr);
	status.system_id = _param_mav_sys_id.get();
	arm_auth_init(&mavlink_log_pub, &status.system_id);
	// run preflight immediately to find all relevant parameters, but don't report
	//立即运行飞行前检查以查找所有相关参数，但不要报告
	PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags, _arm_requirements.global_position, false, true,
				       hrt_elapsed_time(&_boot_timestamp));
	//各种初始化完成后进入循环，执行主要的程序
	while (!should_exit()) {
		transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
		/* update parameters */
		//更新需要的参数
		bool params_updated = _parameter_update_sub.updated();
		//如果参数更新了，会执行下面程序
		if (params_updated || param_init_forced) {
			// clear update
			parameter_update_s update;
			_parameter_update_sub.copy(&update);
			// update parameters from storage
			updateParams();
			// NAV_DLL_ACT value 4 Data Link Auto Recovery (CASA Outback Challenge rules) deleted 2020-03-10
			if (_param_nav_dll_act.get() == 4) {
				mavlink_log_critical(&mavlink_log_pub, "CASA Outback Challenge rules (NAV_DLL_ACT = 4) retired");
				_param_nav_dll_act.set(2); // value 2 Return mode
				_param_nav_dll_act.commit_no_notification();
			}
			// NAV_RCL_ACT value 4 RC Auto Recovery (CASA Outback Challenge rules) deleted 2020-03-10
			if (_param_nav_rcl_act.get() == 4) {
				mavlink_log_critical(&mavlink_log_pub, "CASA Outback Challenge rules (NAV_RCL_ACT = 4) retired");
				_param_nav_rcl_act.set(2); // value 2 Return mode
				_param_nav_rcl_act.commit_no_notification();
			}
			/* update parameters */
			if (!armed.armed) {//如果没有解锁，则执行下面程序
				status.system_type = _param_mav_type.get();
				const bool is_rotary = is_rotary_wing(&status) || (is_vtol(&status) && _vtol_status.vtol_in_rw_mode);
				const bool is_fixed = is_fixed_wing(&status) || (is_vtol(&status) && !_vtol_status.vtol_in_rw_mode);
				const bool is_ground = is_ground_rover(&status);
				/* disable manual override for all systems that rely on electronic stabilization */
				if (is_rotary) {
					status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
				} else if (is_fixed) {
					status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
				} else if (is_ground) {
					status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
				}
				/* set vehicle_status.is_vtol flag */
				status.is_vtol = is_vtol(&status);
				status.is_vtol_tailsitter = is_vtol_tailsitter(&status);
				/* check and update system / component ID */
				status.system_id = _param_mav_sys_id.get();
				status.component_id = _param_mav_comp_id.get();
				get_circuit_breaker_params();
				_status_changed = true;
			}
			status_flags.avoidance_system_required = _param_com_obs_avoid.get();
			status.rc_input_mode = _param_rc_in_off.get();//通过参数获取遥控的输入模式
			// percentage (* 0.01) needs to be doubled because RC total interval is 2, not 1
			_min_stick_change = _param_min_stick_change.get() * 0.02f;
			rc_arm_hyst = _param_rc_arm_hyst.get() * COMMANDER_MONITORING_LOOPSPERMSEC;
			//通过参数配置解锁所需要的条件
			_arm_requirements.arm_authorization = _param_arm_auth_required.get();
			_arm_requirements.esc_check = _param_escs_checks_required.get();
			_arm_requirements.global_position = !_param_arm_without_gps.get();
			_arm_requirements.mission = _param_arm_mission_required.get();
			/* flight mode slots */
			//配置6种飞行模式
			_flight_mode_slots[0] = _param_fltmode_1.get();
			_flight_mode_slots[1] = _param_fltmode_2.get();
			_flight_mode_slots[2] = _param_fltmode_3.get();
			_flight_mode_slots[3] = _param_fltmode_4.get();
			_flight_mode_slots[4] = _param_fltmode_5.get();
			_flight_mode_slots[5] = _param_fltmode_6.get();
			_auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);
			/* check for unsafe Airmode settings: yaw airmode requires the use of an arming switch */
			if (param_airmode != PARAM_INVALID && param_rc_map_arm_switch != PARAM_INVALID) {
				param_get(param_airmode, &airmode);
				param_get(param_rc_map_arm_switch, &rc_map_arm_switch);
				if (airmode == 2 && rc_map_arm_switch == 0) {
					airmode = 1; // change to roll/pitch airmode
					param_set(param_airmode, &airmode);
					mavlink_log_critical(&mavlink_log_pub, "Yaw Airmode requires the use of an Arm Switch")
				}
			}
			_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get());
			param_init_forced = false;
		}
		/* Update OA parameter */
		status_flags.avoidance_system_required = _param_com_obs_avoid.get();
#if defined(BOARD_HAS_POWER_CONTROL)
		/* handle power button state */
		if (_power_button_state_sub.updated()) {
			power_button_state_s button_state;
			if (_power_button_state_sub.copy(&button_state)) {
				if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) {
#if defined(CONFIG_BOARDCTL_POWEROFF)
					if (shutdown_if_allowed() && (px4_shutdown_request() == 0)) {
						while (1) { px4_usleep(1); }
					}
#endif // CONFIG_BOARDCTL_POWEROFF
				}
			}
		}
#endif // BOARD_HAS_POWER_CONTROL
		//这个应该是offboard模式控制相关的设置，待分析
		offboard_control_update();
		//下面是对供电的检查，如果通过usb供电，就不会解锁
		if (_system_power_sub.updated()) {
			system_power_s system_power{};
			_system_power_sub.copy(&system_power);
			if (hrt_elapsed_time(&system_power.timestamp) < 1_s) {
				if (system_power.servo_valid &&
				    !system_power.brick_valid &&
				    !system_power.usb_connected) {
					/* flying only on servo rail, this is unsafe */
					status_flags.condition_power_input_valid = false;
				} else {
					status_flags.condition_power_input_valid = true;
				}
#if defined(CONFIG_BOARDCTL_RESET)
				if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) {
					/* if the USB hardware connection went away, reboot */
					if (_system_power_usb_connected && !system_power.usb_connected) {
						/*
						 * Apparently the USB cable went away but we are still powered,
						 * so we bring the system back to a nominal state for flight.
						 * This is important to unload the USB stack of the OS which is
						 * a relatively complex piece of software that is non-essential
						 * for flight and continuing to run it would add a software risk
						 * without a need. The clean approach to unload it is to reboot.
						 */
						if (shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) {
							mavlink_log_critical(&mavlink_log_pub, "USB disconnected, rebooting for flight safety");
							while (1) { px4_usleep(1); }
						}
					}
				}
#endif // CONFIG_BOARDCTL_RESET
				_system_power_usb_connected = system_power.usb_connected;
			}
		}
		/* update safety topic */
		//更新有关安全开关
		if (_safety_sub.updated()) {
			const bool previous_safety_off = _safety.safety_off;
			if (_safety_sub.copy(&_safety)) {
				// disarm if safety is now on and still armed
				if (armed.armed && _safety.safety_switch_available && !_safety.safety_off) {
					bool safety_disarm_allowed = (status.hil_state == vehicle_status_s::HIL_STATE_OFF);
					// if land detector is available then prevent disarming via safety button if not landed
					if (hrt_elapsed_time(&_land_detector.timestamp) < 1_s) {
						bool maybe_landing = (_land_detector.landed || _land_detector.maybe_landed);
						if (!maybe_landing) {
							safety_disarm_allowed = false;
						}
					}
					if (safety_disarm_allowed) {
						if (TRANSITION_CHANGED == arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::SAFETY_BUTTON)) {
							_status_changed = true;
						}
					}
				}
				// Notify the user if the status of the safety switch changes
				if (_safety.safety_switch_available && previous_safety_off != _safety.safety_off) {
					if (_safety.safety_off) {
						set_tune(TONE_NOTIFY_POSITIVE_TUNE);
					} else {
						tune_neutral(true);
					}
					_status_changed = true;
				}
			}
		}
		/* update vtol vehicle status*/
		//更新垂直起降固定翼的状态
		if (_vtol_vehicle_status_sub.updated()) {
			/* vtol status changed */
			_vtol_vehicle_status_sub.copy(&_vtol_status);
			status.vtol_fw_permanent_stab = _vtol_status.fw_permanent_stab;
			/* Make sure that this is only adjusted if vehicle really is of type vtol */
			if (is_vtol(&status)) {
				// Check if there has been any change while updating the flags
				const auto new_vehicle_type = _vtol_status.vtol_in_rw_mode ?
							      vehicle_status_s::VEHICLE_TYPE_ROTARY_WING :
							      vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
				if (new_vehicle_type != status.vehicle_type) {
					status.vehicle_type = _vtol_status.vtol_in_rw_mode ?
							      vehicle_status_s::VEHICLE_TYPE_ROTARY_WING :
							      vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
					_status_changed = true;
				}
				if (status.in_transition_mode != _vtol_status.vtol_in_trans_mode) {
					status.in_transition_mode = _vtol_status.vtol_in_trans_mode;
					_status_changed = true;
				}
				if (status.in_transition_to_fw != _vtol_status.in_transition_to_fw) {
					status.in_transition_to_fw = _vtol_status.in_transition_to_fw;
					_status_changed = true;
				}
				if (status_flags.vtol_transition_failure != _vtol_status.vtol_transition_failsafe) {
					status_flags.vtol_transition_failure = _vtol_status.vtol_transition_failsafe;
					_status_changed = true;
				}
				const bool should_soft_stop = (status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
				if (armed.soft_stop != should_soft_stop) {
					armed.soft_stop = should_soft_stop;
					_status_changed = true;
				}
			}
		}
		//电调状态检查
		if (_esc_status_sub.updated()) {
			/* ESCs status changed */
			esc_status_s esc_status{};
			if (_esc_status_sub.copy(&esc_status)) {
				esc_status_check(esc_status);
			}
		}
		//检查位置估计和状态估计是否正常
		estimator_check(status_flags);
		/* Update land detector */
		//落地检查
		if (_land_detector_sub.updated()) {
			_was_landed = _land_detector.landed;
			_land_detector_sub.copy(&_land_detector);
			// Only take actions if armed
			if (armed.armed) {
				if (_was_landed != _land_detector.landed) {
					if (_land_detector.landed) {
						mavlink_log_info(&mavlink_log_pub, "Landing detected");
					} else {
						mavlink_log_info(&mavlink_log_pub, "Takeoff detected");
						_have_taken_off_since_arming = true;
						// Set all position and velocity test probation durations to takeoff value
						// This is a larger value to give the vehicle time to complete a failsafe landing
						// if faulty sensors cause loss of navigation shortly after takeoff.
						_gpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
						_lpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
						_lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
					}
				}
			}
		}
		// Auto disarm when landed or kill switch engaged
		//当落地或者停止开关接通时自动上锁
		if (armed.armed) {
			// Check for auto-disarm on landing or pre-flight
			if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) {
				if (_param_com_disarm_land.get() > 0 && _have_taken_off_since_arming) {
					_auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s);
					_auto_disarm_landed.set_state_and_update(_land_detector.landed, hrt_absolute_time());
				} else if (_param_com_disarm_preflight.get() > 0 && !_have_taken_off_since_arming) {
					_auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s);
					_auto_disarm_landed.set_state_and_update(true, hrt_absolute_time());
				}
				if (_auto_disarm_landed.get_state()) {
					arm_disarm(false, true, &mavlink_log_pub,
						   (_have_taken_off_since_arming ? arm_disarm_reason_t::AUTO_DISARM_LAND : arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT));
				}
			}
			// Auto disarm after 5 seconds if kill switch is engaged
			_auto_disarm_killed.set_state_and_update(armed.manual_lockdown || armed.lockdown, hrt_absolute_time());
			if (_auto_disarm_killed.get_state()) {
				if (armed.manual_lockdown) {
					arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::KILL_SWITCH);
				} else {
					arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::LOCKDOWN);
				}
			}
		} else {
			_auto_disarm_landed.set_state_and_update(false, hrt_absolute_time());
			_auto_disarm_killed.set_state_and_update(false, hrt_absolute_time());
		}
		if (_geofence_warning_action_on
		    && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
		    && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LOITER
		    && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
			// reset flag again when we switched out of it
			_geofence_warning_action_on = false;
		}
		_cpuload_sub.update(&_cpuload);
		//电池状态检查，包括在低电量时自动切换飞行模式
		battery_status_check();
		/* update subsystem info which arrives from outside of commander*/
		//更新来自commander外部的子系统信息
		subsystem_info_s info;
		while (_subsys_sub.update(&info))  {
			set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status);
			_status_changed = true;
		}
		/* If in INIT state, try to proceed to STANDBY state */
		if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
			arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
							     true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags,
							     _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
							     arm_disarm_reason_t::TRANSITION_TO_STANDBY);
			if (arming_ret == TRANSITION_DENIED) {
				/* do not complain if not allowed into standby */
				arming_ret = TRANSITION_NOT_CHANGED;
			}
		}
		/* start mission result check */
		const auto prev_mission_instance_count = _mission_result_sub.get().instance_count;
		//检查任务结果
		if (_mission_result_sub.update()) {
			const mission_result_s &mission_result = _mission_result_sub.get();
			// if mission_result is valid for the current mission
			const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp)
						       && (mission_result.instance_count > 0);
			status_flags.condition_auto_mission_available = mission_result_ok && mission_result.valid;
			if (mission_result_ok) {
				if (status.mission_failure != mission_result.failure) {
					status.mission_failure = mission_result.failure;
					_status_changed = true;
					if (status.mission_failure) {
						mavlink_log_critical(&mavlink_log_pub, "Mission cannot be completed");
					}
				}
				/* Only evaluate mission state if home is set */
				if (status_flags.condition_home_position_valid &&
				    (prev_mission_instance_count != mission_result.instance_count)) {
					if (!status_flags.condition_auto_mission_available) {
						/* the mission is invalid */
						tune_mission_fail(true);
					} else if (mission_result.warning) {
						/* the mission has a warning */
						tune_mission_fail(true);
					} else {
						/* the mission is valid */
						tune_mission_ok(true);
					}
				}
			}
		}
		// update manual_control_setpoint before geofence (which might check sticks or switches)
		_manual_control_setpoint_sub.update(&_manual_control_setpoint);
		/* start geofence result check */
		//检查电子围栏的结果
		_geofence_result_sub.update(&_geofence_result);
		const bool in_low_battery_failsafe = _battery_warning > battery_status_s::BATTERY_WARNING_LOW;
		// Geofence actions
		const bool geofence_action_enabled = _geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE;
		if (armed.armed
		    && geofence_action_enabled
		    && !in_low_battery_failsafe) {
			// check for geofence violation transition
			if (_geofence_result.geofence_violated && !_geofence_violated_prev) {
				switch (_geofence_result.geofence_action) {
				case (geofence_result_s::GF_ACTION_NONE) : {
						// do nothing
						break;
					}
				case (geofence_result_s::GF_ACTION_WARN) : {
						// do nothing, mavlink critical messages are sent by navigator
						break;
					}
				case (geofence_result_s::GF_ACTION_LOITER) : {
						if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags,
								&_internal_state)) {
							_geofence_loiter_on = true;
						}
						break;
					}
				case (geofence_result_s::GF_ACTION_RTL) : {
						if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
								&_internal_state)) {
							_geofence_rtl_on = true;
						}
						break;
					}
				case (geofence_result_s::GF_ACTION_LAND) : {
						if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
								&_internal_state)) {
							_geofence_land_on = true;
						}
						break;
					}
				case (geofence_result_s::GF_ACTION_TERMINATE) : {
						PX4_WARN("Flight termination because of geofence");
						mavlink_log_critical(&mavlink_log_pub, "Geofence violation! Flight terminated");
						armed.force_failsafe = true;
						_status_changed = true;
						break;
					}
				}
			}
			_geofence_violated_prev = _geofence_result.geofence_violated;
			// reset if no longer in LOITER or if manually switched to LOITER
			const bool in_loiter_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER;
			const bool manual_loiter_switch_on = _manual_control_setpoint.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON;
			if (!in_loiter_mode || manual_loiter_switch_on) {
				_geofence_loiter_on = false;
			}
			// reset if no longer in RTL or if manually switched to RTL
			const bool in_rtl_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL;
			const bool manual_return_switch_on = _manual_control_setpoint.return_switch == manual_control_setpoint_s::SWITCH_POS_ON;
			if (!in_rtl_mode || manual_return_switch_on) {
				_geofence_rtl_on = false;
			}
			// reset if no longer in LAND or if manually switched to LAND
			const bool in_land_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND;
			if (!in_land_mode) {
				_geofence_land_on = false;
			}
			_geofence_warning_action_on = _geofence_warning_action_on || (_geofence_loiter_on || _geofence_rtl_on
						      || _geofence_land_on);
		} else {
			// No geofence checks, reset flags
			_geofence_loiter_on = false;
			_geofence_rtl_on = false;
			_geofence_land_on = false;
			_geofence_warning_action_on = false;
			_geofence_violated_prev = false;
		}
		// abort auto mode or geofence reaction if sticks are moved significantly
		// but only if not in a low battery handling action
		const bool is_rotary_wing = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
		const bool override_auto_mode =
			(_param_rc_override.get() & OVERRIDE_AUTO_MODE_BIT) &&
			(_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF ||
			 _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND    ||
			 _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL 	  ||
			 _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION ||
			 _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER ||
			 _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET ||
			 _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND);
		const bool override_offboard_mode =
			(_param_rc_override.get() & OVERRIDE_OFFBOARD_MODE_BIT) &&
			_internal_state.main_state == commander_state_s::MAIN_STATE_OFFBOARD;
		if ((override_auto_mode || override_offboard_mode) && is_rotary_wing
		    && !in_low_battery_failsafe && !_geofence_warning_action_on) {
			// transition to previous state if sticks are touched
			if (hrt_elapsed_time(&_manual_control_setpoint.timestamp) < 1_s && // don't use uninitialized or old messages
			    ((fabsf(_manual_control_setpoint.x) > _min_stick_change) ||
			     (fabsf(_manual_control_setpoint.y) > _min_stick_change))) {
				// revert to position control in any case
				main_state_transition(status, commander_state_s::MAIN_STATE_POSCTL, status_flags, &_internal_state);
				mavlink_log_info(&mavlink_log_pub, "Pilot took over control using sticks");
			}
		}
		/* Check for mission flight termination */
		if (armed.armed && _mission_result_sub.get().flight_termination &&
		    !status_flags.circuit_breaker_flight_termination_disabled) {
			armed.force_failsafe = true;
			_status_changed = true;
			if (!_flight_termination_printed) {
				mavlink_log_critical(&mavlink_log_pub, "Geofence violation! Flight terminated");
				_flight_termination_printed = true;
			}
			if (_counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
				mavlink_log_critical(&mavlink_log_pub, "Flight termination active");
			}
		}
		/* RC input check */
		//遥控输入检查
		if (!status_flags.rc_input_blocked && _manual_control_setpoint.timestamp != 0 &&
		    (hrt_elapsed_time(&_manual_control_setpoint.timestamp) < (_param_com_rc_loss_t.get() * 1_s))) {
			/* handle the case where RC signal was regained */
			if (!status_flags.rc_signal_found_once) {
				status_flags.rc_signal_found_once = true;
				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, status_flags.rc_calibration_valid, status);
				_status_changed = true;
			} else {
				if (status.rc_signal_lost) {
					if (_rc_signal_lost_timestamp > 0) {
						mavlink_log_info(&mavlink_log_pub, "Manual control regained after %.1fs",
								 hrt_elapsed_time(&_rc_signal_lost_timestamp) * 1e-6);
					}
					set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, status_flags.rc_calibration_valid, status);
					_status_changed = true;
				}
			}
			status.rc_signal_lost = false;
			const bool in_armed_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
			const bool arm_switch_or_button_mapped =
				_manual_control_setpoint.arm_switch != manual_control_setpoint_s::SWITCH_POS_NONE;
			const bool arm_button_pressed = _param_arm_switch_is_button.get()
							&& (_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON);
			/* DISARM
			 * check if left stick is in lower left position or arm button is pushed or arm switch has transition from arm to disarm
			 * and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed)
			 * do it only for rotary wings in manual mode or fixed wing if landed.
			 * Disable stick-disarming if arming switch or button is mapped */
			//通过遥控器的油门进行解锁操作
			const bool stick_in_lower_left = _manual_control_setpoint.r < -STICK_ON_OFF_LIMIT
							 && (_manual_control_setpoint.z < 0.1f)
							 && !arm_switch_or_button_mapped;
			const bool arm_switch_to_disarm_transition = !_param_arm_switch_is_button.get() &&
					(_last_manual_control_setpoint_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
					(_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
			if (in_armed_state &&
			    (status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&
			    (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _land_detector.landed) &&
			    (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) {
				const bool manual_thrust_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL
								|| _internal_state.main_state == commander_state_s::MAIN_STATE_ACRO
								|| _internal_state.main_state == commander_state_s::MAIN_STATE_STAB
								|| _internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE;
				const bool rc_wants_disarm = (_stick_off_counter == rc_arm_hyst && _stick_on_counter < rc_arm_hyst)
							     || arm_switch_to_disarm_transition;
				if (rc_wants_disarm && (_land_detector.landed || manual_thrust_mode)) {
					arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
									     true /* fRunPreArmChecks */,
									     &mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
									     (arm_switch_to_disarm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK));
				}
				_stick_off_counter++;
			} else if (!(_param_arm_switch_is_button.get()
				     && _manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
				/* do not reset the counter when holding the arm button longer than needed */
				_stick_off_counter = 0;
			}
			/* ARM
			 * check if left stick is in lower right position or arm button is pushed or arm switch has transition from disarm to arm
			 * and we're in MANUAL mode.
			 * Disable stick-arming if arming switch or button is mapped */
			//通过遥控器的油门进行上锁操作
			const bool stick_in_lower_right = _manual_control_setpoint.r > STICK_ON_OFF_LIMIT && _manual_control_setpoint.z < 0.1f
							  && !arm_switch_or_button_mapped;
			/* allow a grace period for re-arming: preflight checks don't need to pass during that time,
			 * for example for accidential in-air disarming */
			const bool in_arming_grace_period = (_last_disarmed_timestamp != 0)
							    && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s);
			const bool arm_switch_to_arm_transition = !_param_arm_switch_is_button.get() &&
					(_last_manual_control_setpoint_arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF) &&
					(_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
					(_manual_control_setpoint.z < 0.1f || in_arming_grace_period);
			if (!in_armed_state &&
			    (status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&
			    (stick_in_lower_right || arm_button_pressed || arm_switch_to_arm_transition)) {
				if ((_stick_on_counter == rc_arm_hyst && _stick_off_counter < rc_arm_hyst) || arm_switch_to_arm_transition) {
					/* we check outside of the transition function here because the requirement
					 * for being in manual mode only applies to manual arming actions.
					 * the system can be armed in auto if armed via the GCS.
					 */
					if ((_internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL)
					    && (_internal_state.main_state != commander_state_s::MAIN_STATE_ACRO)
					    && (_internal_state.main_state != commander_state_s::MAIN_STATE_STAB)
					    && (_internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL)
					    && (_internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL)
					    && (_internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE)
					   ) {
						print_reject_arm("Not arming: Switch to a manual mode first");
					} else if (!status_flags.condition_home_position_valid &&
						   (_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL)) {
						print_reject_arm("Not arming: Geofence RTL requires valid home");
					} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
						arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_ARMED, &armed,
										     !in_arming_grace_period /* fRunPreArmChecks */,
										     &mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
										     (arm_switch_to_arm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK));
						if (arming_ret != TRANSITION_CHANGED) {
							px4_usleep(100000);
							print_reject_arm("Not arming: Preflight checks failed");
						}
					}
				}
				_stick_on_counter++;
			} else if (!(_param_arm_switch_is_button.get()
				     && _manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
				/* do not reset the counter when holding the arm button longer than needed */
				_stick_on_counter = 0;
			}
			_last_manual_control_setpoint_arm_switch = _manual_control_setpoint.arm_switch;
			if (arming_ret == TRANSITION_DENIED) {
				/*
				 * the arming transition can be denied to a number of reasons:
				 *  - pre-flight check failed (sensors not ok or not calibrated)
				 *  - safety not disabled
				 *  - system not in manual mode
				 */
				tune_negative(true);
			}
			/* evaluate the main state machine according to mode switches */
			//根据模式的改变估计主状态
			bool first_rc_eval = (_last_manual_control_setpoint.timestamp == 0) && (_manual_control_setpoint.timestamp > 0);
			transition_result_t main_res = set_main_state(status, &_status_changed);
			/* store last position lock state */
			_last_condition_local_altitude_valid = status_flags.condition_local_altitude_valid;
			_last_condition_local_position_valid = status_flags.condition_local_position_valid;
			_last_condition_global_position_valid = status_flags.condition_global_position_valid;
			/* play tune on mode change only if armed, blink LED always */
			if (main_res == TRANSITION_CHANGED || first_rc_eval) {
				tune_positive(armed.armed);
				_status_changed = true;
			} else if (main_res == TRANSITION_DENIED) {
				/* DENIED here indicates bug in the commander */
				mavlink_log_critical(&mavlink_log_pub, "Switching to this mode is currently not possible");
			}
			/* check throttle kill switch */
			if (_manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
				/* set lockdown flag */
				if (!armed.manual_lockdown) {
					const char kill_switch_string[] = "Kill-switch engaged";
					if (_land_detector.landed) {
						mavlink_log_info(&mavlink_log_pub, kill_switch_string);
					} else {
						mavlink_log_critical(&mavlink_log_pub, kill_switch_string);
					}
					_status_changed = true;
					armed.manual_lockdown = true;
				}
			} else if (_manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
				if (armed.manual_lockdown) {
					mavlink_log_info(&mavlink_log_pub, "Kill-switch disengaged");
					_status_changed = true;
					armed.manual_lockdown = false;
				}
			}
			/* no else case: do not change lockdown flag in unconfigured case */
		} else {
			// set RC lost
			if (status_flags.rc_signal_found_once && !status.rc_signal_lost) {
				// ignore RC lost during calibration
				if (!status_flags.condition_calibration_enabled && !status_flags.rc_input_blocked) {
					mavlink_log_critical(&mavlink_log_pub, "Manual control lost");
					status.rc_signal_lost = true;
					_rc_signal_lost_timestamp = _manual_control_setpoint.timestamp;
					set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, status);
					_status_changed = true;
				}
			}
		}
		// data link checks which update the status
		//数传检查
		data_link_check();
		//避障检查
		avoidance_check();
		// engine failure detection
		// TODO: move out of commander
		//引擎错误检查
		if (_actuator_controls_sub.updated()) {
			/* Check engine failure
			 * only for fixed wing for now
			 */
			if (!status_flags.circuit_breaker_engaged_enginefailure_check &&
			    status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !status.is_vtol && armed.armed) {
				actuator_controls_s actuator_controls{};
				_actuator_controls_sub.copy(&actuator_controls);
				const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
				const float current2throttle = _battery_current / throttle;
				if (((throttle > _param_ef_throttle_thres.get()) && (current2throttle < _param_ef_current2throttle_thres.get()))
				    || status.engine_failure) {
					const float elapsed = hrt_elapsed_time(&_timestamp_engine_healthy) / 1e6f;
					/* potential failure, measure time */
					if ((_timestamp_engine_healthy > 0) && (elapsed > _param_ef_time_thres.get())
					    && !status.engine_failure) {
						status.engine_failure = true;
						_status_changed = true;
						PX4_ERR("Engine Failure");
						set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, false, status);
					}
				}
			} else {
				/* no failure reset flag */
				_timestamp_engine_healthy = hrt_absolute_time();
				if (status.engine_failure) {
					status.engine_failure = false;
					_status_changed = true;
				}
			}
		}
		/* Reset main state to loiter or auto-mission after takeoff is completed.
		 * Sometimes, the mission result topic is outdated and the mission is still signaled
		 * as finished even though we only just started with the takeoff. Therefore, we also
		 * check the timestamp of the mission_result topic. */
		//在起飞任务完成后，转到悬停或者任务模式
		if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
		    && (_mission_result_sub.get().timestamp > _internal_state.timestamp)
		    && _mission_result_sub.get().finished) {
			const bool mission_available = (_mission_result_sub.get().timestamp > _boot_timestamp)
						       && (_mission_result_sub.get().instance_count > 0) && _mission_result_sub.get().valid;
			if ((_param_takeoff_finished_action.get() == 1) && mission_available) {
				main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &_internal_state);
			} else {
				main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &_internal_state);
			}
		}
		/* check if we are disarmed and there is a better mode to wait in */
		//检查是否上锁和是否有一个更好的模式在等待
		if (!armed.armed) {
			/* if there is no radio control but GPS lock the user might want to fly using
			 * just a tablet. Since the RC will force its mode switch setting on connecting
			 * we can as well just wait in a hold mode which enables tablet control.
			 */
			if (status.rc_signal_lost && (_internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL)
			    && status_flags.condition_home_position_valid) {
				main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &_internal_state);
			}
		}
		/* handle commands last, as the system needs to be updated to handle them */
		//解析command命令
		if (_cmd_sub.updated()) {
			/* got command */
			const unsigned last_generation = _cmd_sub.get_last_generation();
			vehicle_command_s cmd;
			if (_cmd_sub.copy(&cmd)) {
				if (_cmd_sub.get_last_generation() != last_generation + 1) {
					PX4_ERR("vehicle_command lost, generation %d -> %d", last_generation, _cmd_sub.get_last_generation());
				}
				//根据当前状态和解锁情况对command进行解析
				if (handle_command(&status, cmd, &armed, _command_ack_pub)) {
					_status_changed = true;
				}
			}
		}
		/* Check for failure detector status */
		//无人机失控状态检测
		const bool failure_detector_updated = _failure_detector.update(status);
		if (failure_detector_updated) {
			const uint8_t failure_status = _failure_detector.getStatus();
			if (failure_status != status.failure_detector_status) {
				status.failure_detector_status = failure_status;
				_status_changed = true;
			}
		}
		if (armed.armed &&
		    failure_detector_updated) {
			if (_failure_detector.isFailure()) {
				const hrt_abstime time_at_arm = armed.armed_time_ms * 1000;
				if (hrt_elapsed_time(&time_at_arm) < 500_ms) {
					// 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs
					if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
						arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::FAILURE_DETECTOR);
						_status_changed = true;
						mavlink_log_critical(&mavlink_log_pub, "ESCs did not respond to arm request");
					}
				}
				if (hrt_elapsed_time(&_time_at_takeoff) < (1_s * _param_com_lkdown_tko.get())) {
					// This handles the case where something fails during the early takeoff phase
					if (!_lockdown_triggered) {
						armed.lockdown = true;
						_lockdown_triggered = true;
						_status_changed = true;
						mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: lockdown");
					}
				} else if (!status_flags.circuit_breaker_flight_termination_disabled &&
					   !_flight_termination_triggered && !_lockdown_triggered) {
					armed.force_failsafe = true;
					_flight_termination_triggered = true;
					_status_changed = true;
					mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: terminate flight");
					set_tune_override(TONE_PARACHUTE_RELEASE_TUNE);
				}
			}
		}
		/* Get current timestamp */
		const hrt_abstime now = hrt_absolute_time();
		// automatically set or update home position
		//自动更新或者设置家的位置
		if (!_home_pub.get().manual_home) {
			const vehicle_local_position_s &local_position = _local_position_sub.get();
			// set the home position when taking off, but only if we were previously disarmed
			// and at least 500 ms from commander start spent to avoid setting home on in-air restart
			if (_should_set_home_on_takeoff && _was_landed && !_land_detector.landed &&
			    (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
				_should_set_home_on_takeoff = false;
				set_home_position();
			}
			if (!armed.armed) {
				if (status_flags.condition_home_position_valid) {
					if (_land_detector.landed && local_position.xy_valid && local_position.z_valid) {
						/* distance from home */
						float home_dist_xy = -1.0f;
						float home_dist_z = -1.0f;
						mavlink_wpm_distance_to_point_local(_home_pub.get().x, _home_pub.get().y, _home_pub.get().z,
										    local_position.x, local_position.y, local_position.z,
										    &home_dist_xy, &home_dist_z);
						if ((home_dist_xy > local_position.eph * 2.0f) || (home_dist_z > local_position.epv * 2.0f)) {
							/* update when disarmed, landed and moved away from current home position */
							set_home_position();
						}
					}
				} else {
					/* First time home position update - but only if disarmed */
					set_home_position();
					/* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin.
					 * This allows home altitude to be used in the calculation of height above takeoff location when GPS
					 * use has commenced after takeoff. */
					if (!status_flags.condition_home_position_valid) {
						set_home_position_alt_only();
					}
				}
			}
		}
		// check for arming state change
		//检查解锁状态改变
		if (_was_armed != armed.armed) {
			_status_changed = true;
			if (armed.armed) {
				if (!_land_detector.landed) { // check if takeoff already detected upon arming
					_have_taken_off_since_arming = true;
				}
			} else { // increase the flight uuid upon disarming
				const int32_t flight_uuid = _param_flight_uuid.get() + 1;
				_param_flight_uuid.set(flight_uuid);
				_param_flight_uuid.commit_no_notification();
				_last_disarmed_timestamp = hrt_absolute_time();
				_should_set_home_on_takeoff = true;
			}
		}
		if (!armed.armed) {
			/* Reset the flag if disarmed. */
			_have_taken_off_since_arming = false;
		}
		_was_armed = armed.armed;
		/* now set navigation state according to failsafe and main state */
		//根据失控和主状态设置导航状态
		bool nav_state_changed = set_nav_state(&status,
						       &armed,
						       &_internal_state,
						       &mavlink_log_pub,
						       (link_loss_actions_t)_param_nav_dll_act.get(),
						       _mission_result_sub.get().finished,
						       _mission_result_sub.get().stay_in_failsafe,
						       status_flags,
						       _land_detector.landed,
						       (link_loss_actions_t)_param_nav_rcl_act.get(),
						       (offboard_loss_actions_t)_param_com_obl_act.get(),
						       (offboard_loss_rc_actions_t)_param_com_obl_rc_act.get(),
						       (position_nav_loss_actions_t)_param_com_posctl_navl.get());
		if (nav_state_changed) {
			status.nav_state_timestamp = hrt_absolute_time();
		}
		if (status.failsafe != _failsafe_old) {
			_status_changed = true;
			if (status.failsafe) {
				mavlink_log_info(&mavlink_log_pub, "Failsafe mode activated");
			} else {
				mavlink_log_info(&mavlink_log_pub, "Failsafe mode deactivated");
			}
			_failsafe_old = status.failsafe;
		}
		//发布一些状态
		/* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags) at 2 Hz or immediately when changed */
		if (hrt_elapsed_time(&status.timestamp) >= 500_ms || _status_changed || nav_state_changed) {
			update_control_mode();
			status.timestamp = hrt_absolute_time();
			_status_pub.publish(status);
			switch ((PrearmedMode)_param_com_prearm_mode.get()) {
			case PrearmedMode::DISABLED:
				/* skip prearmed state  */
				armed.prearmed = false;
				break;
			case PrearmedMode::ALWAYS:
				/* safety is not present, go into prearmed
				* (all output drivers should be started / unlocked last in the boot process
				* when the rest of the system is fully initialized)
				*/
				armed.prearmed = (hrt_elapsed_time(&_boot_timestamp) > 5_s);
				break;
			case PrearmedMode::SAFETY_BUTTON:
				if (_safety.safety_switch_available) {
					/* safety switch is present, go into prearmed if safety is off */
					armed.prearmed = _safety.safety_off;
				} else {
					/* safety switch is not present, do not go into prearmed */
					armed.prearmed = false;
				}
				break;
			default:
				armed.prearmed = false;
				break;
			}
			armed.timestamp = hrt_absolute_time();
			_armed_pub.publish(armed);
			/* publish internal state for logging purposes */
			_internal_state.timestamp = hrt_absolute_time();
			_commander_state_pub.publish(_internal_state);
			/* publish vehicle_status_flags */
			status_flags.timestamp = hrt_absolute_time();
			// Evaluate current prearm status
			if (!armed.armed && !status_flags.condition_calibration_enabled) {
				bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, status, status_flags, true, false, true, 30_s);
				bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, status_flags, _safety, _arm_requirements, status, false);
				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, (preflight_check_res
						 && prearm_check_res), status);
			}
			_vehicle_status_flags_pub.publish(status_flags);
		}
		/* play arming and battery warning tunes */
		if (!_arm_tune_played && armed.armed &&
		    (_safety.safety_switch_available || (_safety.safety_switch_available && _safety.safety_off))) {
			/* play tune when armed */
			set_tune(TONE_ARMING_WARNING_TUNE);
			_arm_tune_played = true;
		} else if (!status_flags.usb_connected &&
			   (status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
			   (_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
			/* play tune on battery critical */
			set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
		} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
			   (_battery_warning == battery_status_s::BATTERY_WARNING_LOW)) {
			/* play tune on battery warning */
			set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
		} else if (status.failsafe) {
			tune_failsafe(true);
		} else {
			set_tune(TONE_STOP_TUNE);
		}
		/* reset arm_tune_played when disarmed */
		if (!armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) {
			//Notify the user that it is safe to approach the vehicle
			if (_arm_tune_played) {
				tune_neutral(true);
			}
			_arm_tune_played = false;
		}
		/* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */
		status_flags.condition_system_hotplug_timeout = (hrt_elapsed_time(&_boot_timestamp) > HOTPLUG_SENS_TIMEOUT);
		if (!sensor_fail_tune_played && (!status_flags.condition_system_sensors_initialized
						 && status_flags.condition_system_hotplug_timeout)) {
			set_tune_override(TONE_GPS_WARNING_TUNE);
			sensor_fail_tune_played = true;
			_status_changed = true;
		}
		_counter++;
		int blink_state = blink_msg_state();
		if (blink_state > 0) {
			/* blinking LED message, don't touch LEDs */
			if (blink_state == 2) {
				/* blinking LED message completed, restore normal state */
				control_status_leds(&status, &armed, true, _battery_warning);
			}
		} else {
			/* normal state */
			control_status_leds(&status, &armed, _status_changed, _battery_warning);
		}
		_status_changed = false;
		arm_auth_update(now, params_updated || param_init_forced);
		px4_usleep(COMMANDER_MONITORING_INTERVAL);
	}
	thread_should_exit = true;
	/* wait for threads to complete */
	int ret = pthread_join(commander_low_prio_thread, nullptr);
	if (ret) {
		warn("join failed: %d", ret);
	}
	rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_OFF);
	/* close fds */
	led_deinit();
	buzzer_deinit();
	thread_running = false;
}