text: /****************************************************************************
   *
   *   Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
   *
   * Redistribution and use in source and binary forms, with or without
   * modification, are permitted provided that the following conditions
   * are met:
   *
   * 1. Redistributions of source code must retain the above copyright
   *    notice, this list of conditions and the following disclaimer.
   * 2. Redistributions in binary form must reproduce the above copyright
   *    notice, this list of conditions and the following disclaimer in
   *    the documentation and/or other materials provided with the
   *    distribution.
   * 3. Neither the name PX4 nor the names of its contributors may be
   *    used to endorse or promote products derived from this software
   *    without specific prior written permission.
   *
   * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
   * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
   * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
   * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
   * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
   * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
   * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
   * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
   * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
   * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
   * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
   * POSSIBILITY OF SUCH DAMAGE.
   *
   ****************************************************************************/
  
  /**
   * @file commander.cpp
   *
   * Main state machine / business logic
   *
   * @TODO This application is currently in a rewrite process. Main changes:
   *			- Calibration routines are moved into the event system
   *			- Commander is rewritten as class
   *			- State machines will be model driven
   */
  
  #include "Commander.hpp"
  
  /* commander module headers */
  #include "Arming/PreFlightCheck/PreFlightCheck.hpp"
  #include "Arming/ArmAuthorization/ArmAuthorization.h"
  #include "Arming/HealthFlags/HealthFlags.h"
  #include "commander_helper.h"
  #include "esc_calibration.h"
  #include "px4_custom_mode.h"
  #include "state_machine_helper.h"
  
  /* PX4 headers */
  #include <dataman/dataman.h>
  #include <drivers/drv_hrt.h>
  #include <drivers/drv_tone_alarm.h>
  #include <lib/geo/geo.h>
  #include <mathlib/mathlib.h>
  #include <navigator/navigation.h>
  #include <px4_platform_common/events.h>
  #include <px4_platform_common/px4_config.h>
  #include <px4_platform_common/defines.h>
  #include <px4_platform_common/external_reset_lockout.h>
  #include <px4_platform_common/posix.h>
  #include <px4_platform_common/shutdown.h>
  #include <px4_platform_common/tasks.h>
  #include <px4_platform_common/time.h>
  #include <circuit_breaker/circuit_breaker.h>
  #include <systemlib/mavlink_log.h>
  
  #include <math.h>
  #include <float.h>
  #include <cstring>
  #include <matrix/math.hpp>
  
  #include <uORB/topics/mavlink_log.h>
  #include <uORB/topics/tune_control.h>
  
  typedef enum VEHICLE_MODE_FLAG {
  	VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
  	VEHICLE_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
  	VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
  	VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
  	VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
  	VEHICLE_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
  	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */
  	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */
  	VEHICLE_MODE_FLAG_ENUM_END = 129, /*  | */
  } VEHICLE_MODE_FLAG;
  
  #if defined(BOARD_HAS_POWER_CONTROL)
  static orb_advert_t tune_control_pub = nullptr;
  static void play_power_button_down_tune()
  {
  	tune_control_s tune_control{};
  	tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT - 20;
  	tune_control.tune_id = tune_control_s::TUNE_ID_POWER_OFF;
  	tune_control.timestamp = hrt_absolute_time();
  	orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control);
  }
  
  static void stop_tune()
  {
  	tune_control_s tune_control{};
  	tune_control.tune_override = true;
  	tune_control.timestamp = hrt_absolute_time();
  	orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control);
  }
  
  static orb_advert_t power_button_state_pub = nullptr;
  static int power_button_state_notification_cb(board_power_button_state_notification_e request)
  {
  	// Note: this can be called from IRQ handlers, so we publish a message that will be handled
  	// on the main thread of commander.
  	power_button_state_s button_state{};
  	button_state.timestamp = hrt_absolute_time();
  	const int ret = PWR_BUTTON_RESPONSE_SHUT_DOWN_PENDING;
  
  	switch (request) {
  	case PWR_BUTTON_IDEL:
  		button_state.event = power_button_state_s::PWR_BUTTON_STATE_IDEL;
  		break;
  
  	case PWR_BUTTON_DOWN:
  		button_state.event = power_button_state_s::PWR_BUTTON_STATE_DOWN;
  		play_power_button_down_tune();
  		break;
  
  	case PWR_BUTTON_UP:
  		button_state.event = power_button_state_s::PWR_BUTTON_STATE_UP;
  		stop_tune();
  		break;
  
  	case PWR_BUTTON_REQUEST_SHUT_DOWN:
  		button_state.event = power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN;
  		break;
  
  	default:
  		PX4_ERR("unhandled power button state: %i", (int)request);
  		return ret;
  	}
  
  	if (power_button_state_pub != nullptr) {
  		orb_publish(ORB_ID(power_button_state), power_button_state_pub, &button_state);
  
  	} else {
  		PX4_ERR("power_button_state_pub not properly initialized");
  	}
  
  	return ret;
  }
  #endif // BOARD_HAS_POWER_CONTROL
  
  #ifndef CONSTRAINED_FLASH
  static bool send_vehicle_command(const uint32_t cmd, const float param1 = NAN, const float param2 = NAN,
  				 const float param3 = NAN,  const float param4 = NAN, const double param5 = static_cast<double>(NAN),
  				 const double param6 = static_cast<double>(NAN), const float param7 = NAN)
  {
  	vehicle_command_s vcmd{};
  	vcmd.command = cmd;
  	vcmd.param1 = param1;
  	vcmd.param2 = param2;
  	vcmd.param3 = param3;
  	vcmd.param4 = param4;
  	vcmd.param5 = param5;
  	vcmd.param6 = param6;
  	vcmd.param7 = param7;
  
  	uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
  	vcmd.source_system = vehicle_status_sub.get().system_id;
  	vcmd.target_system = vehicle_status_sub.get().system_id;
  	vcmd.source_component = vehicle_status_sub.get().component_id;
  	vcmd.target_component = vehicle_status_sub.get().component_id;
  
  	uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
  	vcmd.timestamp = hrt_absolute_time();
  	return vcmd_pub.publish(vcmd);
  }
  
  static bool broadcast_vehicle_command(const uint32_t cmd, const float param1 = NAN, const float param2 = NAN,
  				      const float param3 = NAN,  const float param4 = NAN, const double param5 = static_cast<double>(NAN),
  				      const double param6 = static_cast<double>(NAN), const float param7 = NAN)
  {
  	vehicle_command_s vcmd{};
  	vcmd.command = cmd;
  	vcmd.param1 = param1;
  	vcmd.param2 = param2;
  	vcmd.param3 = param3;
  	vcmd.param4 = param4;
  	vcmd.param5 = param5;
  	vcmd.param6 = param6;
  	vcmd.param7 = param7;
  
  	uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
  	vcmd.source_system = vehicle_status_sub.get().system_id;
  	vcmd.target_system = 0;
  	vcmd.source_component = vehicle_status_sub.get().component_id;
  	vcmd.target_component = 0;
  
  	uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
  	vcmd.timestamp = hrt_absolute_time();
  	return vcmd_pub.publish(vcmd);
  }
  #endif
  
  int Commander::custom_command(int argc, char *argv[])
  {
  	if (!is_running()) {
  		print_usage("not running");
  		return 1;
  	}
  
  #ifndef CONSTRAINED_FLASH
  
  	if (!strcmp(argv[0], "calibrate")) {
  		if (argc > 1) {
  			if (!strcmp(argv[1], "gyro")) {
  				// gyro calibration: param1 = 1
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 1.f, 0.f, 0.f, 0.f, 0.0, 0.0, 0.f);
  
  			} else if (!strcmp(argv[1], "mag")) {
  				if (argc > 2 && (strcmp(argv[2], "quick") == 0)) {
  					// magnetometer quick calibration: VEHICLE_CMD_FIXED_MAG_CAL_YAW
  					send_vehicle_command(vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW);
  
  				} else {
  					// magnetometer calibration: param2 = 1
  					send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.0, 0.0, 0.f);
  				}
  
  			} else if (!strcmp(argv[1], "baro")) {
  				// baro calibration: param3 = 1
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 1.f, 0.f, 0.0, 0.0, 0.f);
  
  			} else if (!strcmp(argv[1], "accel")) {
  				if (argc > 2 && (strcmp(argv[2], "quick") == 0)) {
  					// accelerometer quick calibration: param5 = 3
  					send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 4.0, 0.0, 0.f);
  
  				} else {
  					// accelerometer calibration: param5 = 1
  					send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 1.0, 0.0, 0.f);
  				}
  
  			} else if (!strcmp(argv[1], "level")) {
  				// board level calibration: param5 = 2
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 2.0, 0.0, 0.f);
  
  			} else if (!strcmp(argv[1], "airspeed")) {
  				// airspeed calibration: param6 = 2
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.0, 2.0, 0.f);
  
  			} else if (!strcmp(argv[1], "esc")) {
  				// ESC calibration: param7 = 1
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.0, 0.0, 1.f);
  
  			} else {
  				PX4_ERR("argument %s unsupported.", argv[1]);
  				return 1;
  			}
  
  			return 0;
  
  		} else {
  			PX4_ERR("missing argument");
  		}
  	}
  
  	if (!strcmp(argv[0], "check")) {
  		uORB::Subscription vehicle_status_sub{ORB_ID(vehicle_status)};
  		vehicle_status_s vehicle_status{};
  		vehicle_status_sub.copy(&vehicle_status);
  
  		uORB::Subscription vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
  		vehicle_status_flags_s vehicle_status_flags{};
  		vehicle_status_flags_sub.copy(&vehicle_status_flags);
  
  		uORB::Subscription vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
  		vehicle_control_mode_s vehicle_control_mode{};
  		vehicle_control_mode_sub.copy(&vehicle_control_mode);
  
  		bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags,
  					   vehicle_control_mode,
  					   true, true, 30_s);
  		PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED");
  
  		bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, vehicle_status_flags, vehicle_control_mode, safety_s{},
  					PreFlightCheck::arm_requirements_t{}, vehicle_status);
  		PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED");
  
  		print_health_flags(vehicle_status);
  
  		return 0;
  	}
  
  	if (!strcmp(argv[0], "arm")) {
  		float param2 = 0.f;
  
  		// 21196: force arming/disarming (e.g. allow arming to override preflight checks and disarming in flight)
  		if (argc > 1 && !strcmp(argv[1], "-f")) {
  			param2 = 21196.f;
  		}
  
  		send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
  				     static_cast<float>(vehicle_command_s::ARMING_ACTION_ARM),
  				     param2);
  
  		return 0;
  	}
  
  	if (!strcmp(argv[0], "disarm")) {
  		float param2 = 0.f;
  
  		// 21196: force arming/disarming (e.g. allow arming to override preflight checks and disarming in flight)
  		if (argc > 1 && !strcmp(argv[1], "-f")) {
  			param2 = 21196.f;
  		}
  
  		send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
  				     static_cast<float>(vehicle_command_s::ARMING_ACTION_DISARM),
  				     param2);
  
  		return 0;
  	}
  
  	if (!strcmp(argv[0], "takeoff")) {
  		// switch to takeoff mode and arm
  		send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF);
  		send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
  				     static_cast<float>(vehicle_command_s::ARMING_ACTION_ARM),
  				     0.f);
  
  		return 0;
  	}
  
  	if (!strcmp(argv[0], "land")) {
  		send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_LAND);
  
  		return 0;
  	}
  
  	if (!strcmp(argv[0], "transition")) {
  		uORB::Subscription vehicle_status_sub{ORB_ID(vehicle_status)};
  		vehicle_status_s vehicle_status{};
  		vehicle_status_sub.copy(&vehicle_status);
  		send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION,
  				     (float)(vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ?
  					     vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW :
  					     vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC), 0.0f);
  
  		return 0;
  	}
  
  	if (!strcmp(argv[0], "mode")) {
  		if (argc > 1) {
  
  			if (!strcmp(argv[1], "manual")) {
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_MANUAL);
  
  			} else if (!strcmp(argv[1], "altctl")) {
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_ALTCTL);
  
  			} else if (!strcmp(argv[1], "posctl")) {
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_POSCTL);
  
  			} else if (!strcmp(argv[1], "auto:mission")) {
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
  						     PX4_CUSTOM_SUB_MODE_AUTO_MISSION);
  
  			} else if (!strcmp(argv[1], "auto:loiter")) {
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
  						     PX4_CUSTOM_SUB_MODE_AUTO_LOITER);
  
  			} else if (!strcmp(argv[1], "auto:rtl")) {
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
  						     PX4_CUSTOM_SUB_MODE_AUTO_RTL);
  
  			} else if (!strcmp(argv[1], "acro")) {
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_ACRO);
  
  			} else if (!strcmp(argv[1], "offboard")) {
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_OFFBOARD);
  
  			} else if (!strcmp(argv[1], "stabilized")) {
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_STABILIZED);
  
  			} else if (!strcmp(argv[1], "auto:takeoff")) {
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
  						     PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF);
  
  			} else if (!strcmp(argv[1], "auto:land")) {
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
  						     PX4_CUSTOM_SUB_MODE_AUTO_LAND);
  
  			} else if (!strcmp(argv[1], "auto:precland")) {
  				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
  						     PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND);
  
  			} else {
  				PX4_ERR("argument %s unsupported.", argv[1]);
  			}
  
  			return 0;
  
  		} else {
  			PX4_ERR("missing argument");
  		}
  	}
  
  	if (!strcmp(argv[0], "lockdown")) {
  
  		if (argc < 2) {
  			Commander::print_usage("not enough arguments, missing [on, off]");
  			return 1;
  		}
  
  		bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION,
  						strcmp(argv[1], "off") ? 2.0f : 0.0f /* lockdown */, 0.0f);
  
  		return (ret ? 0 : 1);
  	}
  
  	if (!strcmp(argv[0], "pair")) {
  
  		// GCS pairing request handled by a companion
  		bool ret = broadcast_vehicle_command(vehicle_command_s::VEHICLE_CMD_START_RX_PAIR, 10.f);
  
  		return (ret ? 0 : 1);
  	}
  
  	if (!strcmp(argv[0], "set_ekf_origin")) {
  		if (argc > 3) {
  
  			double latitude  = atof(argv[1]);
  			double longitude = atof(argv[2]);
  			float  altitude  = atof(argv[3]);
  
  			// Set the ekf NED origin global coordinates.
  			bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN,
  							0.f, 0.f, 0.0, 0.0, latitude, longitude, altitude);
  			return (ret ? 0 : 1);
  
  		} else {
  			PX4_ERR("missing argument");
  			return 0;
  		}
  	}
  
  	if (!strcmp(argv[0], "poweroff")) {
  
  		bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN,
  						2.0f);
  
  		return (ret ? 0 : 1);
  	}
  
  
  #endif
  
  	return print_usage("unknown command");
  }
  
  int Commander::print_status()
  {
  	PX4_INFO("arming: %s", _arm_state_machine.arming_state_names[_status.arming_state]);
  	PX4_INFO("navigation: %s", nav_state_names[_status.nav_state]);
  	perf_print_counter(_loop_perf);
  	perf_print_counter(_preflight_check_perf);
  	return 0;
  }
  
  extern "C" __EXPORT int commander_main(int argc, char *argv[])
  {
  	return Commander::main(argc, argv);
  }
  
  bool Commander::shutdown_if_allowed()
  {
  	return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety,
  			vehicle_status_s::ARMING_STATE_SHUTDOWN,
  			_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, _arm_requirements,
  			hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::shutdown);
  }
  
  static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason)
  {
  	switch (calling_reason) {
  	case arm_disarm_reason_t::transition_to_standby: return "";
  
  	case arm_disarm_reason_t::rc_stick: return "RC";
  
  	case arm_disarm_reason_t::rc_switch: return "RC (switch)";
  
  	case arm_disarm_reason_t::command_internal: return "internal command";
  
  	case arm_disarm_reason_t::command_external: return "external command";
  
  	case arm_disarm_reason_t::mission_start: return "mission start";
  
  	case arm_disarm_reason_t::safety_button: return "safety button";
  
  	case arm_disarm_reason_t::auto_disarm_land: return "landing";
  
  	case arm_disarm_reason_t::auto_disarm_preflight: return "auto preflight disarming";
  
  	case arm_disarm_reason_t::kill_switch: return "kill-switch";
  
  	case arm_disarm_reason_t::lockdown: return "lockdown";
  
  	case arm_disarm_reason_t::failure_detector: return "failure detector";
  
  	case arm_disarm_reason_t::shutdown: return "shutdown request";
  
  	case arm_disarm_reason_t::unit_test: return "unit tests";
  
  	case arm_disarm_reason_t::rc_button: return "RC (button)";
  	}
  
  	return "";
  };
  
  using battery_fault_reason_t = events::px4::enums::battery_fault_reason_t;
  static_assert(battery_status_s::BATTERY_FAULT_COUNT == (static_cast<uint8_t>(battery_fault_reason_t::_max) + 1)
  	      , "Battery fault flags mismatch!");
  
  static constexpr const char *battery_fault_reason_str(battery_fault_reason_t battery_fault_reason)
  {
  	switch (battery_fault_reason) {
  	case battery_fault_reason_t::deep_discharge: return "under voltage";
  
  	case battery_fault_reason_t::voltage_spikes: return "over voltage";
  
  	case battery_fault_reason_t::cell_fail: return "cell fault";
  
  	case battery_fault_reason_t::over_current: return "over current";
  
  	case battery_fault_reason_t::fault_temperature: return "critical temperature";
  
  	case battery_fault_reason_t::under_temperature: return "under temperature";
  
  	case battery_fault_reason_t::incompatible_voltage: return "voltage mismatch";
  
  	case battery_fault_reason_t::incompatible_firmware: return "incompatible firmware";
  
  	case battery_fault_reason_t::incompatible_model: return "incompatible model";
  
  	case battery_fault_reason_t::hardware_fault: return "hardware fault";
  
  	case battery_fault_reason_t::over_temperature: return "near temperature limit";
  
  	}
  
  	return "";
  };
  
  using battery_mode_t = events::px4::enums::battery_mode_t;
  static_assert(battery_status_s::BATTERY_MODE_COUNT == (static_cast<uint8_t>(battery_mode_t::_max) + 1)
  	      , "Battery mode flags mismatch!");
  static constexpr const char *battery_mode_str(battery_mode_t battery_mode)
  {
  	switch (battery_mode) {
  	case battery_mode_t::autodischarging: return "auto discharging";
  
  	case battery_mode_t::hotswap: return "hot-swap";
  
  	default: return "unknown";
  	}
  }
  
  using esc_fault_reason_t = events::px4::enums::esc_fault_reason_t;
  static_assert(esc_report_s::ESC_FAILURE_COUNT == (static_cast<uint8_t>(esc_fault_reason_t::_max) + 1)
  	      , "ESC fault flags mismatch!");
  static constexpr const char *esc_fault_reason_str(esc_fault_reason_t esc_fault_reason)
  {
  	switch (esc_fault_reason) {
  	case esc_fault_reason_t::over_current: return "over current";
  
  	case esc_fault_reason_t::over_voltage: return "over voltage";
  
  	case esc_fault_reason_t::motor_over_temp: return "motor critical temperature";
  
  	case esc_fault_reason_t::over_rpm: return "over RPM";
  
  	case esc_fault_reason_t::inconsistent_cmd: return "control failure";
  
  	case esc_fault_reason_t::motor_stuck: return "motor stall";
  
  	case esc_fault_reason_t::failure_generic: return "hardware failure";
  
  	case esc_fault_reason_t::motor_warn_temp: return "motor over temperature";
  
  	case esc_fault_reason_t::esc_warn_temp: return "over temperature";
  
  	case esc_fault_reason_t::esc_over_temp: return "critical temperature";
  
  	}
  
  	return "";
  };
  
  
  
  using navigation_mode_t = events::px4::enums::navigation_mode_t;
  
  static inline navigation_mode_t navigation_mode(uint8_t main_state)
  {
  	switch (main_state) {
  	case commander_state_s::MAIN_STATE_MANUAL: return navigation_mode_t::manual;
  
  	case commander_state_s::MAIN_STATE_ALTCTL: return navigation_mode_t::altctl;
  
  	case commander_state_s::MAIN_STATE_POSCTL: return navigation_mode_t::posctl;
  
  	case commander_state_s::MAIN_STATE_AUTO_MISSION: return navigation_mode_t::auto_mission;
  
  	case commander_state_s::MAIN_STATE_AUTO_LOITER: return navigation_mode_t::auto_loiter;
  
  	case commander_state_s::MAIN_STATE_AUTO_RTL: return navigation_mode_t::auto_rtl;
  
  	case commander_state_s::MAIN_STATE_ACRO: return navigation_mode_t::acro;
  
  	case commander_state_s::MAIN_STATE_OFFBOARD: return navigation_mode_t::offboard;
  
  	case commander_state_s::MAIN_STATE_STAB: return navigation_mode_t::stab;
  
  	case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: return navigation_mode_t::auto_takeoff;
  
  	case commander_state_s::MAIN_STATE_AUTO_LAND: return navigation_mode_t::auto_land;
  
  	case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: return navigation_mode_t::auto_follow_target;
  
  	case commander_state_s::MAIN_STATE_AUTO_PRECLAND: return navigation_mode_t::auto_precland;
  
  	case commander_state_s::MAIN_STATE_ORBIT: return navigation_mode_t::orbit;
  
  	case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff;
  	}
  
  	static_assert(commander_state_s::MAIN_STATE_MAX - 1 == (int)navigation_mode_t::auto_vtol_takeoff,
  		      "enum definition mismatch");
  
  	return navigation_mode_t::unknown;
  }
  
  static constexpr const char *main_state_str(uint8_t main_state)
  {
  	switch (main_state) {
  	case commander_state_s::MAIN_STATE_MANUAL: return "Manual";
  
  	case commander_state_s::MAIN_STATE_ALTCTL: return "Altitude";
  
  	case commander_state_s::MAIN_STATE_POSCTL: return "Position";
  
  	case commander_state_s::MAIN_STATE_AUTO_MISSION: return "Mission";
  
  	case commander_state_s::MAIN_STATE_AUTO_LOITER: return "Hold";
  
  	case commander_state_s::MAIN_STATE_AUTO_RTL: return "RTL";
  
  	case commander_state_s::MAIN_STATE_ACRO: return "Acro";
  
  	case commander_state_s::MAIN_STATE_OFFBOARD: return "Offboard";
  
  	case commander_state_s::MAIN_STATE_STAB: return "Stabilized";
  
  	case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: return "Takeoff";
  
  	case commander_state_s::MAIN_STATE_AUTO_LAND: return "Land";
  
  	case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: return "Follow target";
  
  	case commander_state_s::MAIN_STATE_AUTO_PRECLAND: return "Precision land";
  
  	case commander_state_s::MAIN_STATE_ORBIT: return "Orbit";
  
  	default: return "Unknown";
  	}
  }
  
  transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks)
  {
  	// allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming
  	if (calling_reason == arm_disarm_reason_t::rc_switch
  	    && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) {
  		run_preflight_checks = false;
  	}
  
  	if (run_preflight_checks && !_armed.armed) {
  		if (_vehicle_control_mode.flag_control_manual_enabled) {
  			if (_vehicle_control_mode.flag_control_climb_rate_enabled &&
  			    !_status.rc_signal_lost && _is_throttle_above_center) {
  				mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center\t");
  				events::send(events::ID("commander_arm_denied_throttle_center"),
  				{events::Log::Critical, events::LogInternal::Info},
  				"Arming denied: throttle above center");
  				tune_negative(true);
  				return TRANSITION_DENIED;
  			}
  
  			if (!_vehicle_control_mode.flag_control_climb_rate_enabled &&
  			    !_status.rc_signal_lost && !_is_throttle_low && _status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER) {
  				mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t");
  				events::send(events::ID("commander_arm_denied_throttle_high"),
  				{events::Log::Critical, events::LogInternal::Info},
  				"Arming denied: high throttle");
  				tune_negative(true);
  				return TRANSITION_DENIED;
  			}
  
  		} else if (calling_reason == arm_disarm_reason_t::rc_stick
  			   || calling_reason == arm_disarm_reason_t::rc_switch
  			   || calling_reason == arm_disarm_reason_t::rc_button) {
  			mavlink_log_critical(&_mavlink_log_pub, "Arming denied: switch to manual mode first\t");
  			events::send(events::ID("commander_arm_denied_not_manual"),
  			{events::Log::Critical, events::LogInternal::Info},
  			"Arming denied: switch to manual mode first");
  			tune_negative(true);
  			return TRANSITION_DENIED;
  		}
  
  		if ((_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL)
  		    && !_status_flags.home_position_valid) {
  			mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Geofence RTL requires valid home\t");
  			events::send(events::ID("commander_arm_denied_geofence_rtl"),
  			{events::Log::Critical, events::LogInternal::Info},
  			"Arming denied: Geofence RTL requires valid home");
  			tune_negative(true);
  			return TRANSITION_DENIED;
  		}
  	}
  
  	transition_result_t arming_res = _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety,
  					 vehicle_status_s::ARMING_STATE_ARMED, _armed, run_preflight_checks,
  					 &_mavlink_log_pub, _status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
  					 calling_reason);
  
  	if (arming_res == TRANSITION_CHANGED) {
  		mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason));
  		events::send<events::px4::enums::arm_disarm_reason_t>(events::ID("commander_armed_by"), events::Log::Info,
  				"Armed by {1}", calling_reason);
  
  		_status_changed = true;
  
  	} else if (arming_res == TRANSITION_DENIED) {
  		tune_negative(true);
  	}
  
  	return arming_res;
  }
  
  transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool forced)
  {
  	if (!forced) {
  		const bool landed = (_vehicle_land_detected.landed || _vehicle_land_detected.maybe_landed || is_ground_rover(_status));
  		const bool mc_manual_thrust_mode = _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
  						   && _vehicle_control_mode.flag_control_manual_enabled
  						   && !_vehicle_control_mode.flag_control_climb_rate_enabled;
  		const bool commanded_by_rc = (calling_reason == arm_disarm_reason_t::rc_stick)
  					     || (calling_reason == arm_disarm_reason_t::rc_switch)
  					     || (calling_reason == arm_disarm_reason_t::rc_button);
  
  		if (!landed && !(mc_manual_thrust_mode && commanded_by_rc)) {
  			if (calling_reason != arm_disarm_reason_t::rc_stick) {
  				mavlink_log_critical(&_mavlink_log_pub, "Disarming denied! Not landed\t");
  				events::send(events::ID("commander_disarming_denied_not_landed"),
  				{events::Log::Critical, events::LogInternal::Info},
  				"Disarming denied, not landed");
  			}
  
  			return TRANSITION_DENIED;
  		}
  	}
  
  	transition_result_t arming_res = _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety,
  					 vehicle_status_s::ARMING_STATE_STANDBY, _armed, false,
  					 &_mavlink_log_pub, _status_flags, _arm_requirements,
  					 hrt_elapsed_time(&_boot_timestamp), calling_reason);
  
  	if (arming_res == TRANSITION_CHANGED) {
  		mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason));
  		events::send<events::px4::enums::arm_disarm_reason_t>(events::ID("commander_disarmed_by"), events::Log::Info,
  				"Disarmed by {1}", calling_reason);
  
  		if (_param_com_force_safety.get()) {
  			_safety_handler.enableSafety();
  		}
  
  		_status_changed = true;
  
  	} else if (arming_res == TRANSITION_DENIED) {
  		tune_negative(true);
  	}
  
  	return arming_res;
  }
  
  Commander::Commander() :
  	ModuleParams(nullptr),
  	_failure_detector(this)
  {
  	_vehicle_land_detected.landed = true;
  
  	// XXX for now just set sensors as initialized
  	_status_flags.system_sensors_initialized = true;
  
  	// We want to accept RC inputs as default
  	_status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
  	_status.nav_state_timestamp = hrt_absolute_time();
  	_status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
  
  	/* mark all signals lost as long as they haven't been found */
  	_status.rc_signal_lost = true;
  	_status.data_link_lost = true;
  
  	_status_flags.offboard_control_signal_lost = true;
  
  	_status_flags.power_input_valid = true;
  	_status_flags.rc_calibration_valid = true;
  
  	// default for vtol is rotary wing
  	_vtol_status.vtol_in_rw_mode = true;
  
  	_vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME);
  	_vehicle_gps_position_valid.set_hysteresis_time_from(true, GPS_VALID_TIME);
  }
  
  Commander::~Commander()
  {
  	perf_free(_loop_perf);
  	perf_free(_preflight_check_perf);
  }
  
  bool
  Commander::handle_command(const vehicle_command_s &cmd)
  {
  	/* only handle commands that are meant to be handled by this system and component, or broadcast */
  	if (((cmd.target_system != _status.system_id) && (cmd.target_system != 0))
  	    || ((cmd.target_component != _status.component_id) && (cmd.target_component != 0))) {
  		return false;
  	}
  
  	/* result of the command */
  	unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
  
  	/* request to set different system mode */
  	switch (cmd.command) {
  	case vehicle_command_s::VEHICLE_CMD_DO_REPOSITION: {
  
  			// Just switch the flight mode here, the navigator takes care of
  			// doing something sensible with the coordinates. Its designed
  			// to not require navigator and command to receive / process
  			// the data at the exact same time.
  
  			// Check if a mode switch had been requested
  			if ((((uint32_t)cmd.param2) & 1) > 0) {
  				transition_result_t main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER,
  							       _status_flags, _internal_state);
  
  				if ((main_ret != TRANSITION_DENIED)) {
  					cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  
  				} else {
  					cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
  					mavlink_log_critical(&_mavlink_log_pub, "Reposition command rejected\t");
  					/* EVENT
  					 * @description Check for a valid position estimate
  					 */
  					events::send(events::ID("commander_reposition_rejected"),
  					{events::Log::Error, events::LogInternal::Info},
  					"Reposition command rejected");
  				}
  
  			} else {
  				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  			}
  		}
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: {
  			uint8_t base_mode = (uint8_t)cmd.param1;
  			uint8_t custom_main_mode = (uint8_t)cmd.param2;
  			uint8_t custom_sub_mode = (uint8_t)cmd.param3;
  
  			uint8_t desired_main_state = commander_state_s::MAIN_STATE_MAX;
  			transition_result_t main_ret = TRANSITION_NOT_CHANGED;
  
  			if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) {
  				/* use autopilot-specific mode */
  				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
  					desired_main_state = commander_state_s::MAIN_STATE_MANUAL;
  
  				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
  					desired_main_state = commander_state_s::MAIN_STATE_ALTCTL;
  
  				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
  					desired_main_state = commander_state_s::MAIN_STATE_POSCTL;
  
  				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
  					if (custom_sub_mode > 0) {
  
  						switch (custom_sub_mode) {
  						case PX4_CUSTOM_SUB_MODE_AUTO_LOITER:
  							desired_main_state = commander_state_s::MAIN_STATE_AUTO_LOITER;
  							break;
  
  						case PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
  							desired_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION;
  							break;
  
  						case PX4_CUSTOM_SUB_MODE_AUTO_RTL:
  							desired_main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
  							break;
  
  						case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF:
  							desired_main_state = commander_state_s::MAIN_STATE_AUTO_TAKEOFF;
  							break;
  
  						case PX4_CUSTOM_SUB_MODE_AUTO_LAND:
  							desired_main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
  							break;
  
  						case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET:
  							desired_main_state = commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET;
  							break;
  
  						case PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND:
  							desired_main_state = commander_state_s::MAIN_STATE_AUTO_PRECLAND;
  							break;
  
  						default:
  							main_ret = TRANSITION_DENIED;
  							mavlink_log_critical(&_mavlink_log_pub, "Unsupported auto mode\t");
  							events::send(events::ID("commander_unsupported_auto_mode"), events::Log::Error,
  								     "Unsupported auto mode");
  							break;
  						}
  
  					} else {
  						desired_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION;
  					}
  
  				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
  					desired_main_state = commander_state_s::MAIN_STATE_ACRO;
  
  				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) {
  					desired_main_state = commander_state_s::MAIN_STATE_STAB;
  
  				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
  					desired_main_state = commander_state_s::MAIN_STATE_OFFBOARD;
  				}
  
  			} else {
  				/* use base mode */
  				if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
  					desired_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION;
  
  				} else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
  					if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
  						desired_main_state = commander_state_s::MAIN_STATE_POSCTL;
  
  					} else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) {
  						desired_main_state = commander_state_s::MAIN_STATE_STAB;
  
  					} else {
  						desired_main_state = commander_state_s::MAIN_STATE_MANUAL;
  					}
  				}
  			}
  
  			if (desired_main_state != commander_state_s::MAIN_STATE_MAX) {
  				main_ret = main_state_transition(_status, desired_main_state, _status_flags, _internal_state);
  			}
  
  			if (main_ret != TRANSITION_DENIED) {
  				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  
  			} else {
  				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
  			}
  		}
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM: {
  
  			// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
  			// for logic state parameters
  			const int8_t arming_action = static_cast<int8_t>(lroundf(cmd.param1));
  
  			if (arming_action != vehicle_command_s::ARMING_ACTION_ARM
  			    && arming_action != vehicle_command_s::ARMING_ACTION_DISARM) {
  				mavlink_log_critical(&_mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f\t", (double)cmd.param1);
  				events::send<float>(events::ID("commander_unsupported_arm_disarm_param"), events::Log::Error,
  						    "Unsupported ARM_DISARM param: {1:.3}", cmd.param1);
  
  			} else {
  				// Arm is forced (checks skipped) when param2 is set to a magic number.
  				const bool forced = (static_cast<int>(lroundf(cmd.param2)) == 21196);
  				const bool cmd_from_io = (static_cast<int>(roundf(cmd.param3)) == 1234);
  
  				if (!forced) {
  					// Flick to in-air restore first if this comes from an onboard system and from IO
  					if (cmd.source_system == _status.system_id && cmd.source_component == _status.component_id
  					    && cmd_from_io && (arming_action == vehicle_command_s::ARMING_ACTION_ARM)) {
  						_status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
  					}
  				}
  
  				transition_result_t arming_res = TRANSITION_DENIED;
  				arm_disarm_reason_t arm_disarm_reason = cmd.from_external ? arm_disarm_reason_t::command_external :
  									arm_disarm_reason_t::command_internal;
  
  				if (arming_action == vehicle_command_s::ARMING_ACTION_ARM) {
  					arming_res = arm(arm_disarm_reason, cmd.from_external || !forced);
  
  				} else if (arming_action == vehicle_command_s::ARMING_ACTION_DISARM) {
  					arming_res = disarm(arm_disarm_reason, forced);
  
  				}
  
  				if (arming_res == TRANSITION_DENIED) {
  					cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
  
  				} else {
  					cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  
  					/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
  					if ((arming_action == vehicle_command_s::ARMING_ACTION_ARM) && (arming_res == TRANSITION_CHANGED) &&
  					    (hrt_absolute_time() > (_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && !_home_pub.get().manual_home) {
  
  						set_home_position();
  					}
  				}
  			}
  		}
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: {
  			if (cmd.param1 > 1.5f) {
  				// Test termination command triggers lockdown but not actual termination.
  				if (!_lockdown_triggered) {
  					_armed.lockdown = true;
  					_lockdown_triggered = true;
  					PX4_WARN("forcing lockdown (motors off)");
  				}
  
  			} else if (cmd.param1 > 0.5f) {
  				// Trigger real termination.
  				if (!_flight_termination_triggered) {
  					_armed.force_failsafe = true;
  					_flight_termination_triggered = true;
  					PX4_WARN("forcing failsafe (termination)");
  					send_parachute_command();
  				}
  
  			} else {
  				_armed.force_failsafe = false;
  				_armed.lockdown = false;
  
  				_lockdown_triggered = false;
  				_flight_termination_triggered = false;
  
  				PX4_WARN("disabling failsafe and lockdown");
  			}
  
  			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  		}
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_DO_SET_HOME: {
  			if (_param_com_home_en.get()) {
  				bool use_current = cmd.param1 > 0.5f;
  
  				if (use_current) {
  					/* use current position */
  					if (set_home_position()) {
  						cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  
  					} else {
  						cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
  					}
  
  				} else {
  					float yaw = matrix::wrap_2pi(math::radians(cmd.param4));
  					yaw = PX4_ISFINITE(yaw) ? yaw : (float)NAN;
  					const double lat = cmd.param5;
  					const double lon = cmd.param6;
  					const float alt = cmd.param7;
  
  					if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) {
  						const vehicle_local_position_s &local_pos = _local_position_sub.get();
  
  						if (local_pos.xy_global && local_pos.z_global) {
  							/* use specified position */
  							home_position_s home{};
  							home.timestamp = hrt_absolute_time();
  
  							fillGlobalHomePos(home, lat, lon, alt);
  
  							home.manual_home = true;
  
  							// update local projection reference including altitude
  							MapProjection ref_pos{local_pos.ref_lat, local_pos.ref_lon};
  							float home_x;
  							float home_y;
  							ref_pos.project(lat, lon, home_x, home_y);
  							const float home_z = -(alt - local_pos.ref_alt);
  							fillLocalHomePos(home, home_x, home_y, home_z, yaw);
  
  							/* mark home position as set */
  							_status_flags.home_position_valid = _home_pub.update(home);
  
  							cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  
  						} else {
  							cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
  						}
  
  					} else {
  						cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
  					}
  				}
  
  			} else {
  				// COM_HOME_EN disabled
  				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
  			}
  		}
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: {
  			/* switch to RTL which ends the mission */
  			if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags,
  					_internal_state)) {
  				mavlink_log_info(&_mavlink_log_pub, "Returning to launch\t");
  				events::send(events::ID("commander_rtl"), events::Log::Info, "Returning to launch");
  				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  
  			} else {
  				mavlink_log_critical(&_mavlink_log_pub, "Return to launch denied\t");
  				/* EVENT
  				 * @description Check for a valid position estimate
  				 */
  				events::send(events::ID("commander_rtl_denied"), {events::Log::Critical, events::LogInternal::Info},
  					     "Return to launch denied");
  				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
  			}
  		}
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
  			/* ok, home set, use it to take off */
  			if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, _status_flags,
  					_internal_state)) {
  				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  
  			} else if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) {
  				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  
  			} else {
  				mavlink_log_critical(&_mavlink_log_pub, "Takeoff denied! Please disarm and retry\t");
  				/* EVENT
  				 * @description Check for a valid position estimate
  				 */
  				events::send(events::ID("commander_takeoff_denied"), {events::Log::Critical, events::LogInternal::Info},
  					     "Takeoff denied! Please disarm and retry");
  				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
  			}
  		}
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_NAV_VTOL_TAKEOFF:
  
  		/* ok, home set, use it to take off */
  		if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF,
  				_status_flags,
  				_internal_state)) {
  			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  
  		} else if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF) {
  			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  
  		} else {
  			mavlink_log_critical(&_mavlink_log_pub, "VTOL Takeoff denied! Please disarm and retry");
  			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
  		}
  
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
  			if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags,
  					_internal_state)) {
  				mavlink_log_info(&_mavlink_log_pub, "Landing at current position\t");
  				events::send(events::ID("commander_landing_current_pos"), events::Log::Info,
  					     "Landing at current position");
  				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  
  			} else {
  				mavlink_log_critical(&_mavlink_log_pub, "Landing denied! Please land manually\t");
  				/* EVENT
  				 * @description Check for a valid position estimate
  				 */
  				events::send(events::ID("commander_landing_current_pos_denied"), {events::Log::Critical, events::LogInternal::Info},
  					     "Landing denied! Please land manually");
  				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
  			}
  		}
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: {
  			if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_PRECLAND, _status_flags,
  					_internal_state)) {
  				mavlink_log_info(&_mavlink_log_pub, "Precision landing\t");
  				events::send(events::ID("commander_landing_prec_land"), events::Log::Info,
  					     "Landing using precision landing");
  				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  
  			} else {
  				mavlink_log_critical(&_mavlink_log_pub, "Precision landing denied! Please land manually\t");
  				/* EVENT
  				 * @description Check for a valid position estimate
  				 */
  				events::send(events::ID("commander_landing_prec_land_denied"), {events::Log::Critical, events::LogInternal::Info},
  					     "Precision landing denied! Please land manually");
  				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
  			}
  		}
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_MISSION_START: {
  
  			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
  
  			// check if current mission and first item are valid
  			if (_status_flags.auto_mission_available) {
  
  				// requested first mission item valid
  				if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {
  
  					// switch to AUTO_MISSION and ARM
  					if ((TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags,
  							_internal_state))
  					    && (TRANSITION_DENIED != arm(arm_disarm_reason_t::mission_start))) {
  
  						cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  
  					} else {
  						cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
  						mavlink_log_critical(&_mavlink_log_pub, "Mission start denied\t");
  						/* EVENT
  						 * @description Check for a valid position estimate
  						 */
  						events::send(events::ID("commander_mission_start_denied"), {events::Log::Critical, events::LogInternal::Info},
  							     "Mission start denied");
  					}
  				}
  
  			} else {
  				mavlink_log_critical(&_mavlink_log_pub, "Mission start denied! No valid mission\t");
  				events::send(events::ID("commander_mission_start_denied_no_mission"), {events::Log::Critical, events::LogInternal::Info},
  					     "Mission start denied! No valid mission");
  			}
  		}
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: {
  			// if no high latency telemetry exists send a failed acknowledge
  			if (_high_latency_datalink_heartbeat > _boot_timestamp) {
  				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED;
  				mavlink_log_critical(&_mavlink_log_pub, "Control high latency failed! Telemetry unavailable\t");
  				events::send(events::ID("commander_ctrl_high_latency_failed"), {events::Log::Critical, events::LogInternal::Info},
  					     "Control high latency failed! Telemetry unavailable");
  			}
  		}
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_DO_ORBIT:
  
  		transition_result_t main_ret;
  
  		if (_status.in_transition_mode) {
  			main_ret = TRANSITION_DENIED;
  
  		} else if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
  			// for fixed wings the behavior of orbit is the same as loiter
  			main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER,
  							 _status_flags, _internal_state);
  
  		} else {
  			// Switch to orbit state and let the orbit task handle the command further
  			main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_ORBIT, _status_flags,
  							 _internal_state);
  		}
  
  		if ((main_ret != TRANSITION_DENIED)) {
  			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  
  		} else {
  			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
  			mavlink_log_critical(&_mavlink_log_pub, "Orbit command rejected");
  		}
  
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_DO_MOTOR_TEST:
  		cmd_result = handle_command_motor_test(cmd);
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_ACTUATOR_TEST:
  		cmd_result = handle_command_actuator_test(cmd);
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
  
  			const int param1 = cmd.param1;
  
  			if (param1 == 0) {
  				// 0: Do nothing for autopilot
  				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  
  #if defined(CONFIG_BOARDCTL_RESET)
  
  			} else if ((param1 == 1) && shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) {
  				// 1: Reboot autopilot
  				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  
  				while (1) { px4_usleep(1); }
  
  #endif // CONFIG_BOARDCTL_RESET
  
  #if defined(BOARD_HAS_POWER_CONTROL)
  
  			} else if ((param1 == 2) && shutdown_if_allowed() && (px4_shutdown_request(400_ms) == 0)) {
  				// 2: Shutdown autopilot
  				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  
  				while (1) { px4_usleep(1); }
  
  #endif // BOARD_HAS_POWER_CONTROL
  
  #if defined(CONFIG_BOARDCTL_RESET)
  
  			} else if ((param1 == 3) && shutdown_if_allowed() && (px4_reboot_request(true, 400_ms) == 0)) {
  				// 3: Reboot autopilot and keep it in the bootloader until upgraded.
  				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  
  				while (1) { px4_usleep(1); }
  
  #endif // CONFIG_BOARDCTL_RESET
  
  			} else {
  				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
  			}
  		}
  
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
  
  			if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
  			    || _status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN || _worker_thread.isBusy()) {
  
  				// reject if armed or shutting down
  				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
  
  			} else {
  
  				/* try to go to INIT/PREFLIGHT arming state */
  				if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, safety_s{},
  						vehicle_status_s::ARMING_STATE_INIT, _armed,
  						false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags,
  						PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT
  						30_s, // time since boot not relevant for switching to ARMING_STATE_INIT
  						(cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal))
  				   ) {
  
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
  					break;
  
  				}
  
  				if ((int)(cmd.param1) == 1) {
  					/* gyro calibration */
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  					_status_flags.calibration_enabled = true;
  					_worker_thread.startTask(WorkerThread::Request::GyroCalibration);
  
  				} else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
  					   (int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
  					   (int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
  					/* temperature calibration: handled in events module */
  					break;
  
  				} else if ((int)(cmd.param2) == 1) {
  					/* magnetometer calibration */
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  					_status_flags.calibration_enabled = true;
  					_worker_thread.startTask(WorkerThread::Request::MagCalibration);
  
  				} else if ((int)(cmd.param3) == 1) {
  					/* baro calibration */
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  					_status_flags.calibration_enabled = true;
  					_worker_thread.startTask(WorkerThread::Request::BaroCalibration);
  
  				} else if ((int)(cmd.param4) == 1) {
  					/* RC calibration */
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  					/* disable RC control input completely */
  					_status_flags.rc_calibration_in_progress = true;
  					mavlink_log_info(&_mavlink_log_pub, "Calibration: Disabling RC input\t");
  					events::send(events::ID("commander_calib_rc_off"), events::Log::Info,
  						     "Calibration: Disabling RC input");
  
  				} else if ((int)(cmd.param4) == 2) {
  					/* RC trim calibration */
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  					_status_flags.calibration_enabled = true;
  					_worker_thread.startTask(WorkerThread::Request::RCTrimCalibration);
  
  				} else if ((int)(cmd.param5) == 1) {
  					/* accelerometer calibration */
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  					_status_flags.calibration_enabled = true;
  					_worker_thread.startTask(WorkerThread::Request::AccelCalibration);
  
  				} else if ((int)(cmd.param5) == 2) {
  					// board offset calibration
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  					_status_flags.calibration_enabled = true;
  					_worker_thread.startTask(WorkerThread::Request::LevelCalibration);
  
  				} else if ((int)(cmd.param5) == 4) {
  					// accelerometer quick calibration
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  					_status_flags.calibration_enabled = true;
  					_worker_thread.startTask(WorkerThread::Request::AccelCalibrationQuick);
  
  				} else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) {
  					// TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017)
  					/* airspeed calibration */
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  					_status_flags.calibration_enabled = true;
  					_worker_thread.startTask(WorkerThread::Request::AirspeedCalibration);
  
  				} else if ((int)(cmd.param7) == 1) {
  					/* do esc calibration */
  					if (check_battery_disconnected(&_mavlink_log_pub)) {
  						answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  						_status_flags.calibration_enabled = true;
  						_armed.in_esc_calibration_mode = true;
  						_worker_thread.startTask(WorkerThread::Request::ESCCalibration);
  
  					} else {
  						answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
  					}
  
  				} else if ((int)(cmd.param4) == 0) {
  					/* RC calibration ended - have we been in one worth confirming? */
  					if (_status_flags.rc_calibration_in_progress) {
  						/* enable RC control input */
  						_status_flags.rc_calibration_in_progress = false;
  						mavlink_log_info(&_mavlink_log_pub, "Calibration: Restoring RC input\t");
  						events::send(events::ID("commander_calib_rc_on"), events::Log::Info,
  							     "Calibration: Restoring RC input");
  					}
  
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  
  				} else {
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
  				}
  			}
  
  			break;
  		}
  
  	case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: {
  			// Magnetometer quick calibration using world magnetic model and known heading
  			if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
  			    || (_status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN)
  			    || _worker_thread.isBusy()) {
  
  				// reject if armed or shutting down
  				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
  
  			} else {
  				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  				// parameter 1: Heading   (degrees)
  				// parameter 3: Latitude  (degrees)
  				// parameter 4: Longitude (degrees)
  
  				// assume vehicle pointing north (0 degrees) if heading isn't specified
  				const float heading_radians = PX4_ISFINITE(cmd.param1) ? math::radians(roundf(cmd.param1)) : 0.f;
  
  				float latitude = NAN;
  				float longitude = NAN;
  
  				if (PX4_ISFINITE(cmd.param3) && PX4_ISFINITE(cmd.param4)) {
  					// invalid if both lat & lon are 0 (current mavlink spec)
  					if ((fabsf(cmd.param3) > 0) && (fabsf(cmd.param4) > 0)) {
  						latitude = cmd.param3;
  						longitude = cmd.param4;
  					}
  				}
  
  				_status_flags.calibration_enabled = true;
  				_worker_thread.setMagQuickData(heading_radians, latitude, longitude);
  				_worker_thread.startTask(WorkerThread::Request::MagCalibrationQuick);
  			}
  
  			break;
  		}
  
  	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
  
  			if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
  			    || _status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN
  			    || _worker_thread.isBusy()) {
  
  				// reject if armed or shutting down
  				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
  
  			} else {
  
  				if (((int)(cmd.param1)) == 0) {
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  					_worker_thread.startTask(WorkerThread::Request::ParamLoadDefault);
  
  				} else if (((int)(cmd.param1)) == 1) {
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  					_worker_thread.startTask(WorkerThread::Request::ParamSaveDefault);
  
  				} else if (((int)(cmd.param1)) == 2) {
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  					_worker_thread.startTask(WorkerThread::Request::ParamResetAll);
  
  				} else if (((int)(cmd.param1)) == 3) {
  					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
  					_worker_thread.startTask(WorkerThread::Request::ParamResetSensorFactory);
  				}
  			}
  
  			break;
  		}
  
  	case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
  	case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
  	case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
  	case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
  	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL:
  	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE:
  	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
  	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
  	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN:
  	case vehicle_command_s::VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
  	case vehicle_command_s::VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
  	case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION:
  	case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL:
  	case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL:
  	case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST:
  	case vehicle_command_s::VEHICLE_CMD_OBLIQUE_SURVEY:
  	case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL:
  	case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE:
  	case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_ZOOM:
  	case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_FOCUS:
  	case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
  	case vehicle_command_s::VEHICLE_CMD_DO_LAND_START:
  	case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND:
  	case vehicle_command_s::VEHICLE_CMD_LOGGING_START:
  	case vehicle_command_s::VEHICLE_CMD_LOGGING_STOP:
  	case vehicle_command_s::VEHICLE_CMD_NAV_DELAY:
  	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI:
  	case vehicle_command_s::VEHICLE_CMD_NAV_ROI:
  	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION:
  	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
  	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
  	case vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE:
  	case vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN:
  	case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
  	case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
  	case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR:
  	case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR:
  		/* ignore commands that are handled by other parts of the system */
  		break;
  
  	default:
  		/* Warn about unsupported commands, this makes sense because only commands
  		 * to this component ID (or all) are passed by mavlink. */
  		answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
  		break;
  	}
  
  	if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
  		/* already warned about unsupported commands in "default" case */
  		answer_command(cmd, cmd_result);
  	}
  
  	return true;
  }
  
  unsigned
  Commander::handle_command_motor_test(const vehicle_command_s &cmd)
  {
  	if (_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) {
  		return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
  	}
  
  	if (_param_com_mot_test_en.get() != 1) {
  		return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
  	}
  
  	test_motor_s test_motor{};
  	test_motor.timestamp = hrt_absolute_time();
  	test_motor.motor_number = (int)(cmd.param1 + 0.5f) - 1;
  
  	int throttle_type = (int)(cmd.param2 + 0.5f);
  
  	if (throttle_type != 0) { // 0: MOTOR_TEST_THROTTLE_PERCENT
  		return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
  	}
  
  	int motor_count = (int)(cmd.param5 + 0.5);
  
  	if (motor_count > 1) {
  		return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
  	}
  
  	test_motor.action = test_motor_s::ACTION_RUN;
  	test_motor.value = math::constrain(cmd.param3 / 100.f, 0.f, 1.f);
  
  	if (test_motor.value < FLT_EPSILON) {
  		// the message spec is not clear on whether 0 means stop, but it should be closer to what a user expects
  		test_motor.value = -1.f;
  	}
  
  	test_motor.timeout_ms = (int)(cmd.param4 * 1000.f + 0.5f);
  
  	// enforce a timeout and a maximum limit
  	if (test_motor.timeout_ms == 0 || test_motor.timeout_ms > 3000) {
  		test_motor.timeout_ms = 3000;
  	}
  
  	test_motor.driver_instance = 0; // the mavlink command does not allow to specify the instance, so set to 0 for now
  	_test_motor_pub.publish(test_motor);
  
  	return vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  }
  
  unsigned
  Commander::handle_command_actuator_test(const vehicle_command_s &cmd)
  {
  	if (_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) {
  		return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
  	}
  
  	if (_param_com_mot_test_en.get() != 1) {
  		return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
  	}
  
  	actuator_test_s actuator_test{};
  	actuator_test.timestamp = hrt_absolute_time();
  	actuator_test.function = (int)(cmd.param5 + 0.5);
  
  	if (actuator_test.function < 1000) {
  		const int first_motor_function = 1; // from MAVLink ACTUATOR_OUTPUT_FUNCTION
  		const int first_servo_function = 33;
  
  		if (actuator_test.function >= first_motor_function
  		    && actuator_test.function < first_motor_function + actuator_test_s::MAX_NUM_MOTORS) {
  			actuator_test.function = actuator_test.function - first_motor_function + actuator_test_s::FUNCTION_MOTOR1;
  
  		} else if (actuator_test.function >= first_servo_function
  			   && actuator_test.function < first_servo_function + actuator_test_s::MAX_NUM_SERVOS) {
  			actuator_test.function = actuator_test.function - first_servo_function + actuator_test_s::FUNCTION_SERVO1;
  
  		} else {
  			return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
  		}
  
  	} else {
  		actuator_test.function -= 1000;
  	}
  
  	actuator_test.value = cmd.param1;
  
  	actuator_test.action = actuator_test_s::ACTION_DO_CONTROL;
  	int timeout_ms = (int)(cmd.param2 * 1000.f + 0.5f);
  
  	if (timeout_ms <= 0) {
  		actuator_test.action = actuator_test_s::ACTION_RELEASE_CONTROL;
  
  	} else {
  		actuator_test.timeout_ms = timeout_ms;
  	}
  
  	// enforce a timeout and a maximum limit
  	if (actuator_test.timeout_ms == 0 || actuator_test.timeout_ms > 3000) {
  		actuator_test.timeout_ms = 3000;
  	}
  
  	_actuator_test_pub.publish(actuator_test);
  	return vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
  }
  
  void Commander::executeActionRequest(const action_request_s &action_request)
  {
  	arm_disarm_reason_t arm_disarm_reason{};
  
  	// Silently ignore RC actions during RC calibration
  	if (_status_flags.rc_calibration_in_progress
  	    && (action_request.source == action_request_s::SOURCE_RC_STICK_GESTURE
  		|| action_request.source == action_request_s::SOURCE_RC_SWITCH
  		|| action_request.source == action_request_s::SOURCE_RC_BUTTON
  		|| action_request.source == action_request_s::SOURCE_RC_MODE_SLOT)) {
  		return;
  	}
  
  	switch (action_request.source) {
  	case action_request_s::SOURCE_RC_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::rc_stick; break;
  
  	case action_request_s::SOURCE_RC_SWITCH: arm_disarm_reason = arm_disarm_reason_t::rc_switch; break;
  
  	case action_request_s::SOURCE_RC_BUTTON: arm_disarm_reason = arm_disarm_reason_t::rc_button; break;
  	}
  
  	switch (action_request.action) {
  	case action_request_s::ACTION_DISARM: disarm(arm_disarm_reason); break;
  
  	case action_request_s::ACTION_ARM: arm(arm_disarm_reason); break;
  
  	case action_request_s::ACTION_TOGGLE_ARMING:
  		if (_armed.armed) {
  			disarm(arm_disarm_reason);
  
  		} else {
  			arm(arm_disarm_reason);
  		}
  
  		break;
  
  	case action_request_s::ACTION_UNKILL:
  		if (arm_disarm_reason == arm_disarm_reason_t::rc_switch && _armed.manual_lockdown) {
  			mavlink_log_info(&_mavlink_log_pub, "Kill-switch disengaged\t");
  			events::send(events::ID("commander_kill_sw_disengaged"), events::Log::Info, "Kill-switch disengaged");
  			_status_changed = true;
  			_armed.manual_lockdown = false;
  		}
  
  		break;
  
  	case action_request_s::ACTION_KILL:
  		if (arm_disarm_reason == arm_disarm_reason_t::rc_switch && !_armed.manual_lockdown) {
  			const char kill_switch_string[] = "Kill-switch engaged\t";
  			events::LogLevels log_levels{events::Log::Info};
  
  			if (_vehicle_land_detected.landed) {
  				mavlink_log_info(&_mavlink_log_pub, kill_switch_string);
  
  			} else {
  				mavlink_log_critical(&_mavlink_log_pub, kill_switch_string);
  				log_levels.external = events::Log::Critical;
  			}
  
  			events::send(events::ID("commander_kill_sw_engaged"), log_levels, "Kill-switch engaged");
  
  			_status_changed = true;
  			_armed.manual_lockdown = true;
  			send_parachute_command();
  		}
  
  		break;
  
  	case action_request_s::ACTION_SWITCH_MODE:
  
  		// if there's never been a mode change force RC switch as initial state
  		if (action_request.source == action_request_s::SOURCE_RC_MODE_SLOT
  		    && !_armed.armed && (_internal_state.main_state_changes == 0)
  		    && (action_request.mode == commander_state_s::MAIN_STATE_ALTCTL
  			|| action_request.mode == commander_state_s::MAIN_STATE_POSCTL)) {
  			_internal_state.main_state = action_request.mode;
  			_internal_state.main_state_changes++;
  		}
  
  		int ret = main_state_transition(_status, action_request.mode, _status_flags, _internal_state);
  
  		if (ret == transition_result_t::TRANSITION_DENIED) {
  			print_reject_mode(action_request.mode);
  		}
  
  		break;
  	}
  }
  
  /**
  * @brief This function initializes the home position an altitude of the vehicle. This happens first time we get a good GPS fix and each
  *		 time the vehicle is armed with a good GPS fix.
  **/
  bool
  Commander::set_home_position()
  {
  	// Need global and local position fix to be able to set home
  	// but already set the home position in local coordinates if available
  	// in case the global position is only valid after takeoff
  	if (_param_com_home_en.get() && _status_flags.local_position_valid) {
  
  		// Set home position in local coordinates
  		const vehicle_local_position_s &lpos = _local_position_sub.get();
  		_heading_reset_counter = lpos.heading_reset_counter;
  
  		home_position_s home{};
  		home.timestamp = hrt_absolute_time();
  		home.manual_home = false;
  		fillLocalHomePos(home, lpos);
  
  		if (_status_flags.global_position_valid) {
  
  			const vehicle_global_position_s &gpos = _global_position_sub.get();
  
  			// Ensure that the GPS accuracy is good enough for intializing home
  			if (isGPosGoodForInitializingHomePos(gpos)) {
  				fillGlobalHomePos(home, gpos);
  				setHomePosValid();
  			}
  		}
  
  		_home_pub.update(home);
  	}
  
  	return _status_flags.home_position_valid;
  }
  
  bool
  Commander::set_in_air_home_position()
  {
  	if (_param_com_home_en.get()
  	    && _status_flags.local_position_valid
  	    && _status_flags.global_position_valid) {
  
  		const vehicle_global_position_s &gpos = _global_position_sub.get();
  		home_position_s home{};
  
  		// Ensure that the GPS accuracy is good enough for intializing home
  		if (isGPosGoodForInitializingHomePos(gpos)) {
  			home = _home_pub.get();
  			home.timestamp = hrt_absolute_time();
  			const vehicle_local_position_s &lpos = _local_position_sub.get();
  
  			if (_home_pub.get().valid_lpos) {
  				// Back-compute lon, lat and alt of home position given the home
  				// and current positions in local frame
  				MapProjection ref_pos{gpos.lat, gpos.lon};
  				double home_lat;
  				double home_lon;
  				ref_pos.reproject(home.x - lpos.x, home.y - lpos.y, home_lat, home_lon);
  				const float home_alt = gpos.alt + home.z;
  				fillGlobalHomePos(home, home_lat, home_lon, home_alt);
  
  			} else {
  				// Home position in local frame is unknowm, set
  				// home as current position
  				fillLocalHomePos(home, lpos);
  				fillGlobalHomePos(home, gpos);
  			}
  
  			setHomePosValid();
  			_home_pub.update(home);
  		}
  	}
  
  	return _status_flags.home_position_valid;
  }
  
  bool
  Commander::isGPosGoodForInitializingHomePos(const vehicle_global_position_s &gpos) const
  {
  	return (gpos.eph <= _param_com_home_h_t.get())
  	       && (gpos.epv <= _param_com_home_v_t.get());
  }
  
  void
  Commander::fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos) const
  {
  	fillLocalHomePos(home, lpos.x, lpos.y, lpos.z, lpos.heading);
  }
  
  void
  Commander::fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading) const
  {
  	home.x = x;
  	home.y = y;
  	home.z = z;
  	home.valid_lpos = true;
  
  	home.yaw = heading;
  }
  
  void Commander::fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos) const
  {
  	fillGlobalHomePos(home, gpos.lat, gpos.lon, gpos.alt);
  }
  
  void Commander::fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt) const
  {
  	home.lat = lat;
  	home.lon = lon;
  	home.valid_hpos = true;
  	home.alt = alt;
  	home.valid_alt = true;
  }
  
  void Commander::setHomePosValid()
  {
  	// play tune first time we initialize HOME
  	if (!_status_flags.home_position_valid) {
  		tune_home_set(true);
  	}
  
  	// mark home position as set
  	_status_flags.home_position_valid = true;
  }
  
  bool
  Commander::set_home_position_alt_only()
  {
  	const vehicle_local_position_s &lpos = _local_position_sub.get();
  
  	if (_param_com_home_en.get() && !_home_pub.get().valid_alt && lpos.z_global) {
  		// handle special case where we are setting only altitude using local position reference
  		home_position_s home{};
  		home.alt = lpos.ref_alt;
  		home.valid_alt = true;
  
  		home.timestamp = hrt_absolute_time();
  
  		return _home_pub.update(home);
  	}
  
  	return false;
  }
  
  void
  Commander::updateHomePositionYaw(float yaw)
  {
  	if (_param_com_home_en.get()) {
  		home_position_s home = _home_pub.get();
  
  		home.yaw = yaw;
  		home.timestamp = hrt_absolute_time();
  
  		_home_pub.update(home);
  	}
  }
  
  void
  Commander::run()
  {
  	bool sensor_fail_tune_played = false;
  
  	const param_t param_airmode = param_find("MC_AIRMODE");
  	const param_t param_man_arm_gesture = param_find("MAN_ARM_GESTURE");
  	const param_t param_rc_map_arm_sw = param_find("RC_MAP_ARM_SW");
  
  	/* initialize */
  	led_init();
  	buzzer_init();
  
  #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);
  
  		tune_control_s tune_control{};
  		button_state.timestamp = hrt_absolute_time();
  		tune_control_pub = orb_advertise(ORB_ID(tune_control), &tune_control);
  	}
  
  	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();
  
  	bool param_init_forced = 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.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;
  
  	_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, _vehicle_control_mode,
  				       false, true, hrt_elapsed_time(&_boot_timestamp));
  
  	while (!should_exit()) {
  
  		perf_begin(_loop_perf);
  
  		/* update parameters */
  		const 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();
  
  			/* 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();
  
  			_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();
  			_arm_requirements.geofence = _param_geofence_action.get() > geofence_result_s::GF_ACTION_NONE;
  
  			_auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);
  			_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
  
  			// disable arm gesture if an arm switch is configured
  			if (param_man_arm_gesture != PARAM_INVALID && param_rc_map_arm_sw != PARAM_INVALID) {
  				int32_t man_arm_gesture = 0, rc_map_arm_sw = 0;
  				param_get(param_man_arm_gesture, &man_arm_gesture);
  				param_get(param_rc_map_arm_sw, &rc_map_arm_sw);
  
  				if (rc_map_arm_sw > 0 && man_arm_gesture == 1) {
  					man_arm_gesture = 0; // disable arm gesture
  					param_set(param_man_arm_gesture, &man_arm_gesture);
  					mavlink_log_critical(&_mavlink_log_pub, "Arm stick gesture disabled if arm switch in use\t")
  					/* EVENT
  					* @description <param>MAN_ARM_GESTURE</param> is now set to disable arm/disarm stick gesture.
  					*/
  					events::send(events::ID("rc_update_arm_stick_gesture_disabled_with_switch"), {events::Log::Info, events::LogInternal::Disabled},
  						     "Arm stick gesture disabled if arm switch in use");
  				}
  			}
  
  			// check for unsafe Airmode settings: yaw airmode requires disabling the stick arm gesture
  			if (param_airmode != PARAM_INVALID && param_man_arm_gesture != PARAM_INVALID) {
  				int32_t airmode = 0, man_arm_gesture = 0;
  				param_get(param_airmode, &airmode);
  				param_get(param_man_arm_gesture, &man_arm_gesture);
  
  				if (airmode == 2 && man_arm_gesture == 1) {
  					airmode = 1; // change to roll/pitch airmode
  					param_set(param_airmode, &airmode);
  					mavlink_log_critical(&_mavlink_log_pub, "Yaw Airmode requires disabling the stick arm gesture\t")
  					/* EVENT
  					 * @description <param>MC_AIRMODE</param> is now set to roll/pitch airmode.
  					 */
  					events::send(events::ID("commander_airmode_requires_no_arm_gesture"), {events::Log::Error, events::LogInternal::Disabled},
  						     "Yaw Airmode requires disabling the stick arm gesture");
  				}
  			}
  
  			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 (shutdown_if_allowed() && (px4_shutdown_request() == 0)) {
  						while (1) { px4_usleep(1); }
  					}
  				}
  			}
  		}
  
  #endif // BOARD_HAS_POWER_CONTROL
  
  		offboard_control_update();
  
  		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.power_input_valid = false;
  
  				} else {
  					_status_flags.power_input_valid = true;
  				}
  
  				_system_power_usb_connected = system_power.usb_connected;
  			}
  		}
  
  		/* Update land detector */
  		if (_vehicle_land_detected_sub.updated()) {
  			const bool was_landed = _vehicle_land_detected.landed;
  			_vehicle_land_detected_sub.copy(&_vehicle_land_detected);
  
  			// Only take actions if armed
  			if (_armed.armed) {
  				if (!was_landed && _vehicle_land_detected.landed) {
  					mavlink_log_info(&_mavlink_log_pub, "Landing detected\t");
  					events::send(events::ID("commander_landing_detected"), events::Log::Info, "Landing detected");
  					_status.takeoff_time = 0;
  
  				} else if (was_landed && !_vehicle_land_detected.landed) {
  					mavlink_log_info(&_mavlink_log_pub, "Takeoff detected\t");
  					events::send(events::ID("commander_takeoff_detected"), events::Log::Info, "Takeoff detected");
  					_status.takeoff_time = hrt_absolute_time();
  					_have_taken_off_since_arming = true;
  				}
  
  				// automatically set or update home position
  				if (_param_com_home_en.get() && !_home_pub.get().manual_home) {
  					// 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 && !_vehicle_land_detected.landed &&
  					    (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
  						if (was_landed) {
  							_should_set_home_on_takeoff = !set_home_position();
  
  						} else if (_param_com_home_in_air.get()) {
  							_should_set_home_on_takeoff = !set_in_air_home_position();
  						}
  					}
  				}
  			}
  		}
  
  		_safety_handler.safetyButtonHandler();
  
  		/* update safety topic */
  		const bool safety_updated = _safety_sub.updated();
  
  		if (safety_updated) {
  			const bool previous_safety_valid = (_safety.timestamp != 0);
  			const bool previous_safety_off = _safety.safety_off;
  
  			if (_safety_sub.copy(&_safety)) {
  				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, _safety.safety_switch_available, _safety.safety_off,
  						 _safety.safety_switch_available, _status);
  
  				// disarm if safety is now on and still armed
  				if (_armed.armed && _safety.safety_switch_available && !_safety.safety_off
  				    && (_status.hil_state == vehicle_status_s::HIL_STATE_OFF)) {
  					disarm(arm_disarm_reason_t::safety_button);
  				}
  
  				// Notify the user if the status of the safety switch changes
  				if (previous_safety_valid && _safety.safety_switch_available && previous_safety_off != _safety.safety_off) {
  
  					if (_safety.safety_off) {
  						set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE);
  
  					} 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_check();
  
  		} else if (_param_escs_checks_required.get() != 0) {
  
  			if (!_status_flags.escs_error) {
  
  				if ((_last_esc_status_updated != 0) && (hrt_elapsed_time(&_last_esc_status_updated) > 700_ms)) {
  					/* Detect timeout after first telemetry packet received
  					 * Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that
  					 */
  
  					mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry timeout\t");
  					events::send(events::ID("commander_esc_telemetry_timeout"), events::Log::Critical,
  						     "ESCs telemetry timeout");
  					_status_flags.escs_error = true;
  
  				} else if (_last_esc_status_updated == 0 && hrt_elapsed_time(&_boot_timestamp) > 5000_ms) {
  					/* Detect if esc telemetry is not connected after reboot */
  					mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry not connected\t");
  					events::send(events::ID("commander_esc_telemetry_not_con"), events::Log::Critical,
  						     "ESCs telemetry not connected");
  					_status_flags.escs_error = true;
  				}
  			}
  		}
  
  		estimator_check();
  
  		// 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(_vehicle_land_detected.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()) {
  					if (_have_taken_off_since_arming) {
  						disarm(arm_disarm_reason_t::auto_disarm_land);
  
  					} else {
  						disarm(arm_disarm_reason_t::auto_disarm_preflight);
  					}
  				}
  			}
  
  			// Auto disarm after 5 seconds if kill switch is engaged
  			bool auto_disarm = _armed.manual_lockdown;
  
  			// auto disarm if locked down to avoid user confusion
  			//  skipped in HITL where lockdown is enabled for safety
  			if (_status.hil_state != vehicle_status_s::HIL_STATE_ON) {
  				auto_disarm |= _armed.lockdown;
  			}
  
  			_auto_disarm_killed.set_state_and_update(auto_disarm, hrt_absolute_time());
  
  			if (_auto_disarm_killed.get_state()) {
  				if (_armed.manual_lockdown) {
  					disarm(arm_disarm_reason_t::kill_switch, true);
  
  				} else {
  					disarm(arm_disarm_reason_t::lockdown, true);
  				}
  			}
  
  		} 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;
  		}
  
  		if (_battery_status_subs.updated()) {
  			battery_status_check();
  		}
  
  		/* If in INIT state, try to proceed to STANDBY state */
  		if (!_status_flags.calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
  
  			_arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _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);
  		}
  
  		/* start mission result check */
  		if (_mission_result_sub.updated()) {
  			const mission_result_s &mission_result = _mission_result_sub.get();
  
  			const auto prev_mission_instance_count = mission_result.instance_count;
  			_mission_result_sub.update();
  
  			// 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.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) {
  						// navigator sends out the exact reason
  						mavlink_log_critical(&_mavlink_log_pub, "Mission cannot be completed\t");
  						events::send(events::ID("commander_mission_cannot_be_completed"), {events::Log::Critical, events::LogInternal::Info},
  							     "Mission cannot be completed");
  					}
  				}
  
  				/* Only evaluate mission state if home is set */
  				if (_status_flags.home_position_valid &&
  				    (prev_mission_instance_count != mission_result.instance_count)) {
  
  					if (!_status_flags.auto_mission_available) {
  						/* the mission is invalid */
  						tune_mission_fail(true);
  
  					} else if (mission_result.warning) {
  						/* the mission has a warning */
  						tune_mission_warn(true);
  
  					} else {
  						/* the mission is valid */
  						tune_mission_ok(true);
  					}
  				}
  			}
  
  			// Transition main state to loiter or auto-mission after takeoff is completed.
  			if (_armed.armed && !_vehicle_land_detected.landed
  			    && (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
  				_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
  			    && (mission_result.timestamp >= _status.nav_state_timestamp)
  			    && mission_result.finished) {
  
  				if ((_param_takeoff_finished_action.get() == 1) && _status_flags.auto_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);
  				}
  			}
  		}
  
  		/* start geofence result check */
  		_geofence_result_sub.update(&_geofence_result);
  		_status.geofence_violated = _geofence_result.geofence_violated;
  
  		const bool in_low_battery_failsafe_delay = _battery_failsafe_timestamp != 0;
  
  		// 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_delay) {
  
  			// 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");
  
  						if (!_flight_termination_triggered && !_lockdown_triggered) {
  							_flight_termination_triggered = true;
  							mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated\t");
  							events::send(events::ID("commander_geofence_termination"), {events::Log::Alert, events::LogInternal::Warning},
  								     "Geofence violation! Flight terminated");
  							_armed.force_failsafe = true;
  							_status_changed = true;
  							send_parachute_command();
  						}
  
  						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;
  
  			if (!in_loiter_mode) {
  				_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;
  
  			if (!in_rtl_mode) {
  				_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;
  		}
  
  		/* Check for mission flight termination */
  		if (_armed.armed && _mission_result_sub.get().flight_termination &&
  		    !_status_flags.circuit_breaker_flight_termination_disabled) {
  
  
  			if (!_flight_termination_triggered && !_lockdown_triggered) {
  				// navigator only requests flight termination on GPS failure
  				mavlink_log_critical(&_mavlink_log_pub, "GPS failure: Flight terminated\t");
  				events::send(events::ID("commander_mission_termination"), {events::Log::Alert, events::LogInternal::Warning},
  					     "GPS failure: Flight terminated");
  				_flight_termination_triggered = true;
  				_armed.force_failsafe = true;
  				_status_changed = true;
  				send_parachute_command();
  			}
  
  			if (hrt_elapsed_time(&_last_termination_message_sent) > 4_s) {
  				_last_termination_message_sent = hrt_absolute_time();
  				mavlink_log_critical(&_mavlink_log_pub, "Flight termination active\t");
  				events::send(events::ID("commander_mission_termination_active"), {events::Log::Alert, events::LogInternal::Warning},
  					     "Flight termination active");
  			}
  		}
  
  		manual_control_setpoint_s manual_control_setpoint;
  
  		if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
  			if (manual_control_setpoint.valid) {
  				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 (_last_valid_manual_control_setpoint > 0) {
  							float elapsed = hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6f;
  							mavlink_log_info(&_mavlink_log_pub, "Manual control regained after %.1fs\t", (double)elapsed);
  							events::send<float>(events::ID("commander_rc_regained"), events::Log::Info,
  									    "Manual control regained after {1:.1} s", elapsed);
  						}
  
  						set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true,
  								 _status_flags.rc_calibration_valid, _status);
  						_status_changed = true;
  					}
  				}
  
  				const bool mode_switch_mapped = (_param_rc_map_fltmode.get() > 0) || (_param_rc_map_mode_sw.get() > 0);
  				const bool is_mavlink = manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC;
  
  				if (!_armed.armed && (is_mavlink || !mode_switch_mapped) && (_internal_state.main_state_changes == 0)) {
  					// if there's never been a mode change force position control as initial state
  					_internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
  					_internal_state.main_state_changes++;
  				}
  
  				_status.rc_signal_lost = false;
  				_is_throttle_above_center = manual_control_setpoint.z > 0.6f;
  				_is_throttle_low = manual_control_setpoint.z < 0.1f;
  				_last_valid_manual_control_setpoint = manual_control_setpoint.timestamp;
  
  			} else {
  				if (_status_flags.rc_signal_found_once && !_status.rc_signal_lost) {
  					mavlink_log_critical(&_mavlink_log_pub, "Manual control lost\t");
  					events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info},
  						     "Manual control lost");
  					_status.rc_signal_lost = true;
  					set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true,
  							 false, _status);
  					_status_changed = true;
  				}
  			}
  
  
  			const bool override_enabled =
  				((_param_com_rc_override.get() & static_cast<int32_t>(RcOverrideBits::AUTO_MODE_BIT))
  				 && _vehicle_control_mode.flag_control_auto_enabled)
  				|| ((_param_com_rc_override.get() & static_cast<int32_t>(RcOverrideBits::OFFBOARD_MODE_BIT))
  				    && _vehicle_control_mode.flag_control_offboard_enabled);
  
  			// Abort autonomous mode and switch to position mode if sticks are moved significantly
  			// but only if actually in air.
  			if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
  			    && !in_low_battery_failsafe_delay && !_geofence_warning_action_on
  			    && _armed.armed
  			    && !_status_flags.rc_calibration_in_progress
  			    && manual_control_setpoint.valid
  			    && manual_control_setpoint.sticks_moving
  			    && override_enabled) {
  				const transition_result_t posctl_result =
  					main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state);
  
  				if (posctl_result == TRANSITION_CHANGED) {
  					tune_positive(true);
  					mavlink_log_info(&_mavlink_log_pub, "Pilot took over position control using sticks\t");
  					events::send(events::ID("commander_rc_override_pos"), events::Log::Info,
  						     "Pilot took over position control using sticks");
  					_status_changed = true;
  
  				} else if (posctl_result == TRANSITION_DENIED) {
  					// If transition to POSCTL was denied, then we can try again with ALTCTL.
  					const transition_result_t altctl_result =
  						main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, _internal_state);
  
  					if (altctl_result == TRANSITION_CHANGED) {
  						tune_positive(true);
  						mavlink_log_info(&_mavlink_log_pub, "Pilot took over altitude control using sticks\t");
  						events::send(events::ID("commander_rc_override_alt"), events::Log::Info,
  							     "Pilot took over altitude control using sticks");
  						_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;
  				}
  			}
  		}
  
  		/* 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.global_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 */
  		if (_vehicle_command_sub.updated()) {
  			/* got command */
  			const unsigned last_generation = _vehicle_command_sub.get_last_generation();
  			vehicle_command_s cmd;
  
  			if (_vehicle_command_sub.copy(&cmd)) {
  				if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
  					PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation());
  				}
  
  				if (handle_command(cmd)) {
  					_status_changed = true;
  				}
  			}
  		}
  
  		if (_action_request_sub.updated()) {
  			const unsigned last_generation = _action_request_sub.get_last_generation();
  			action_request_s action_request;
  
  			if (_action_request_sub.copy(&action_request)) {
  				if (_action_request_sub.get_last_generation() != last_generation + 1) {
  					PX4_ERR("action_request lost, generation %u -> %u", last_generation, _action_request_sub.get_last_generation());
  				}
  
  				executeActionRequest(action_request);
  			}
  		}
  
  		/* Check for failure detector status */
  		if (_failure_detector.update(_status, _vehicle_control_mode)) {
  			_status.failure_detector_status = _failure_detector.getStatus().value;
  			auto fd_status_flags = _failure_detector.getStatusFlags();
  			_status_changed = true;
  
  			if (_armed.armed) {
  				if (fd_status_flags.arm_escs) {
  					// 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs
  					if (hrt_elapsed_time(&_status.armed_time) < 500_ms) {
  						disarm(arm_disarm_reason_t::failure_detector);
  						mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request\t");
  						events::send(events::ID("commander_fd_escs_not_arming"), events::Log::Critical, "ESCs did not respond to arm request");
  					}
  				}
  
  				if (fd_status_flags.roll || fd_status_flags.pitch || fd_status_flags.alt || fd_status_flags.ext) {
  					const bool is_right_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get());
  
  					if (is_right_after_takeoff && !_lockdown_triggered) {
  						// This handles the case where something fails during the early takeoff phase
  						_armed.lockdown = true;
  						_lockdown_triggered = true;
  						mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: lockdown\t");
  						/* EVENT
  						 * @description
  						 * When a critical failure is detected right after takeoff, the system turns off the motors.
  						 * Failures include an exceeding tilt angle, altitude failure or an external failure trigger.
  						 *
  						 * <profile name="dev">
  						 * This can be configured with the parameter <param>COM_LKDOWN_TKO</param>.
  						 * </profile>
  						 */
  						events::send(events::ID("commander_fd_lockdown"), {events::Log::Emergency, events::LogInternal::Warning},
  							     "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;
  						mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: terminate flight\t");
  						/* EVENT
  						 * @description
  						 * Critical failures include an exceeding tilt angle, altitude failure or an external failure trigger.
  						 *
  						 * <profile name="dev">
  						 * Flight termination can be disabled with the parameter <param>CBRK_FLIGHTTERM</param>.
  						 * </profile>
  						 */
  						events::send(events::ID("commander_fd_terminate"), {events::Log::Emergency, events::LogInternal::Warning},
  							     "Critical failure detected: terminate flight");
  						send_parachute_command();
  					}
  				}
  
  				if (fd_status_flags.imbalanced_prop
  				    && !_imbalanced_propeller_check_triggered) {
  					_status_changed = true;
  					_imbalanced_propeller_check_triggered = true;
  					imbalanced_prop_failsafe(&_mavlink_log_pub, _status, _status_flags, &_internal_state,
  								 (imbalanced_propeller_action_t)_param_com_imb_prop_act.get());
  				}
  			}
  		}
  
  		// Publish wind speed warning if enabled via parameter
  		if (_param_com_wind_warn.get() > FLT_EPSILON && !_vehicle_land_detected.landed) {
  			checkWindAndWarn();
  		}
  
  		/* Get current timestamp */
  		const hrt_abstime now = hrt_absolute_time();
  
  		// automatically set or update home position
  		if (_param_com_home_en.get() && !_home_pub.get().manual_home) {
  			const vehicle_local_position_s &local_position = _local_position_sub.get();
  
  			if (!_armed.armed) {
  				if (_home_pub.get().valid_lpos) {
  					if (_vehicle_land_detected.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.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 (!_vehicle_land_detected.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;
  			_imbalanced_propeller_check_triggered = false;
  		}
  
  		/* now set navigation state according to failsafe and main state */
  		bool nav_state_changed = set_nav_state(_status,
  						       _armed,
  						       _internal_state,
  						       &_mavlink_log_pub,
  						       static_cast<link_loss_actions_t>(_param_nav_dll_act.get()),
  						       _mission_result_sub.get().finished,
  						       _mission_result_sub.get().stay_in_failsafe,
  						       _status_flags,
  						       _vehicle_land_detected.landed,
  						       static_cast<link_loss_actions_t>(_param_nav_rcl_act.get()),
  						       static_cast<offboard_loss_actions_t>(_param_com_obl_act.get()),
  						       static_cast<quadchute_actions_t>(_param_com_qc_act.get()),
  						       static_cast<offboard_loss_rc_actions_t>(_param_com_obl_rc_act.get()),
  						       static_cast<position_nav_loss_actions_t>(_param_com_posctl_navl.get()),
  						       _param_com_rcl_act_t.get(),
  						       _param_com_rcl_except.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\t");
  				events::send(events::ID("commander_failsafe_activated"), events::Log::Info, "Failsafe mode activated");
  
  			} else {
  				mavlink_log_info(&_mavlink_log_pub, "Failsafe mode deactivated\t");
  				events::send(events::ID("commander_failsafe_deactivated"), events::Log::Info, "Failsafe mode deactivated");
  			}
  
  			_failsafe_old = _status.failsafe;
  		}
  
  		/* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags, failure_detector_status) 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);
  
  			// Evaluate current prearm status
  			if (!_armed.armed && !_status_flags.calibration_enabled) {
  				perf_begin(_preflight_check_perf);
  				bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, _vehicle_control_mode,
  							   false, true, hrt_elapsed_time(&_boot_timestamp));
  				perf_end(_preflight_check_perf);
  
  				// skip arm authorization check until actual arming attempt
  				PreFlightCheck::arm_requirements_t arm_req = _arm_requirements;
  				arm_req.arm_authorization = false;
  				bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, _status_flags, _vehicle_control_mode, _safety, arm_req,
  							_status, false);
  
  				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, (preflight_check_res
  						 && prearm_check_res), _status);
  			}
  
  			/* publish vehicle_status_flags */
  			_status_flags.timestamp = hrt_absolute_time();
  			_vehicle_status_flags_pub.publish(_status_flags);
  
  			/* publish failure_detector data */
  			failure_detector_status_s fd_status{};
  			fd_status.timestamp = hrt_absolute_time();
  			fd_status.fd_roll = _failure_detector.getStatusFlags().roll;
  			fd_status.fd_pitch = _failure_detector.getStatusFlags().pitch;
  			fd_status.fd_alt = _failure_detector.getStatusFlags().alt;
  			fd_status.fd_ext = _failure_detector.getStatusFlags().ext;
  			fd_status.fd_arm_escs = _failure_detector.getStatusFlags().arm_escs;
  			fd_status.fd_high_wind = _failure_detector.getStatusFlags().high_wind;
  			fd_status.fd_battery = _failure_detector.getStatusFlags().battery;
  			fd_status.fd_imbalanced_prop = _failure_detector.getStatusFlags().imbalanced_prop;
  			fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
  			_failure_detector_status_pub.publish(fd_status);
  		}
  
  		/* 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(tune_control_s::TUNE_ID_ARMING_WARNING);
  			_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(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST);
  
  		} 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(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW);
  
  		} else if (_status.failsafe && _armed.armed) {
  			tune_failsafe(true);
  
  		} else {
  			set_tune(tune_control_s::TUNE_ID_STOP);
  		}
  
  		/* 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.system_hotplug_timeout = (hrt_elapsed_time(&_boot_timestamp) > HOTPLUG_SENS_TIMEOUT);
  
  		if (!sensor_fail_tune_played && (!_status_flags.system_sensors_initialized
  						 && _status_flags.system_hotplug_timeout)) {
  
  			set_tune_override(tune_control_s::TUNE_ID_GPS_WARNING);
  			sensor_fail_tune_played = true;
  			_status_changed = true;
  		}
  
  		// check if the worker has finished
  		if (_worker_thread.hasResult()) {
  			int ret = _worker_thread.getResultAndReset();
  			_armed.in_esc_calibration_mode = false;
  
  			if (_status_flags.calibration_enabled) { // did we do a calibration?
  				_status_flags.calibration_enabled = false;
  
  				if (ret == 0) {
  					tune_positive(true);
  
  				} else {
  					tune_negative(true);
  				}
  			}
  		}
  
  		control_status_leds(_status_changed, _battery_warning);
  
  		_status_changed = false;
  
  		/* store last position lock state */
  		_last_local_altitude_valid = _status_flags.local_altitude_valid;
  		_last_local_position_valid = _status_flags.local_position_valid;
  		_last_global_position_valid = _status_flags.global_position_valid;
  
  		_was_armed = _armed.armed;
  
  		arm_auth_update(now, params_updated || param_init_forced);
  
  		px4_indicate_external_reset_lockout(LockoutComponent::Commander, _armed.armed);
  
  		perf_end(_loop_perf);
  
  		// sleep if there are no vehicle_commands or action_requests to process
  		if (!_vehicle_command_sub.updated() && !_action_request_sub.updated()) {
  			px4_usleep(COMMANDER_MONITORING_INTERVAL);
  		}
  	}
  
  	rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_OFF);
  
  	/* close fds */
  	led_deinit();
  	buzzer_deinit();
  }
  
  void
  Commander::get_circuit_breaker_params()
  {
  	_status_flags.circuit_breaker_engaged_power_check = circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(),
  			CBRK_SUPPLY_CHK_KEY);
  	_status_flags.circuit_breaker_engaged_usb_check = circuit_breaker_enabled_by_val(_param_cbrk_usb_chk.get(),
  			CBRK_USB_CHK_KEY);
  	_status_flags.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled_by_val(_param_cbrk_airspd_chk.get(),
  			CBRK_AIRSPD_CHK_KEY);
  	_status_flags.circuit_breaker_engaged_enginefailure_check = circuit_breaker_enabled_by_val(_param_cbrk_enginefail.get(),
  			CBRK_ENGINEFAIL_KEY);
  	_status_flags.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val(_param_cbrk_flightterm.get(),
  			CBRK_FLIGHTTERM_KEY);
  	_status_flags.circuit_breaker_engaged_posfailure_check = circuit_breaker_enabled_by_val(_param_cbrk_velposerr.get(),
  			CBRK_VELPOSERR_KEY);
  	_status_flags.circuit_breaker_vtol_fw_arming_check = circuit_breaker_enabled_by_val(_param_cbrk_vtolarming.get(),
  			CBRK_VTOLARMING_KEY);
  }
  
  void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
  {
  	switch (blink_msg_state()) {
  	case 1:
  		// blinking LED message, don't touch LEDs
  		return;
  
  	case 2:
  		// blinking LED message completed, restore normal state
  		changed = true;
  		break;
  
  	default:
  		break;
  	}
  
  	const hrt_abstime time_now_us = hrt_absolute_time();
  
  	if (_cpuload_sub.updated()) {
  		cpuload_s cpuload;
  
  		if (_cpuload_sub.copy(&cpuload)) {
  
  			bool overload = (cpuload.load > 0.95f) || (cpuload.ram_usage > 0.98f);
  
  			if (_overload_start == 0 && overload) {
  				_overload_start = time_now_us;
  
  			} else if (!overload) {
  				_overload_start = 0;
  			}
  		}
  	}
  
  	const bool overload = (_overload_start != 0);
  
  	// driving the RGB led
  	if (changed || _last_overload != overload) {
  		uint8_t led_mode = led_control_s::MODE_OFF;
  		uint8_t led_color = led_control_s::COLOR_WHITE;
  		bool set_normal_color = false;
  
  		uint64_t overload_warn_delay = (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 1_ms : 250_ms;
  
  		// set mode
  		if (overload && (time_now_us >= _overload_start + overload_warn_delay)) {
  			led_mode = led_control_s::MODE_BLINK_FAST;
  			led_color = led_control_s::COLOR_PURPLE;
  
  		} else if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
  			led_mode = led_control_s::MODE_ON;
  			set_normal_color = true;
  
  		} else if (!_status_flags.system_sensors_initialized && _status_flags.system_hotplug_timeout) {
  			led_mode = led_control_s::MODE_BLINK_FAST;
  			led_color = led_control_s::COLOR_RED;
  
  		} else if (_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
  			led_mode = led_control_s::MODE_BREATHE;
  			set_normal_color = true;
  
  		} else if (!_status_flags.system_sensors_initialized && !_status_flags.system_hotplug_timeout) {
  			led_mode = led_control_s::MODE_BREATHE;
  			set_normal_color = true;
  
  		} else if (_status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
  			// if in init status it should not be in the error state
  			led_mode = led_control_s::MODE_OFF;
  
  		} else {
  			// STANDBY_ERROR and other states
  			led_mode = led_control_s::MODE_BLINK_NORMAL;
  			led_color = led_control_s::COLOR_RED;
  		}
  
  		if (set_normal_color) {
  			// set color
  			if (_status.failsafe) {
  				led_color = led_control_s::COLOR_PURPLE;
  
  			} else if (battery_warning == battery_status_s::BATTERY_WARNING_LOW) {
  				led_color = led_control_s::COLOR_AMBER;
  
  			} else if (battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
  				led_color = led_control_s::COLOR_RED;
  
  			} else {
  				if (_status_flags.home_position_valid && _status_flags.global_position_valid) {
  					led_color = led_control_s::COLOR_GREEN;
  
  				} else {
  					led_color = led_control_s::COLOR_BLUE;
  				}
  			}
  		}
  
  		if (led_mode != led_control_s::MODE_OFF) {
  			rgbled_set_color_and_mode(led_color, led_mode);
  		}
  	}
  
  	_last_overload = overload;
  
  #if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
  
  	if (_armed.armed) {
  		if (_status.failsafe) {
  			BOARD_ARMED_LED_OFF();
  
  			if (time_now_us >= _led_armed_state_toggle + 250_ms) {
  				_led_armed_state_toggle = time_now_us;
  				BOARD_ARMED_STATE_LED_TOGGLE();
  			}
  
  		} else {
  			BOARD_ARMED_STATE_LED_OFF();
  
  			// armed, solid
  			BOARD_ARMED_LED_ON();
  		}
  
  	} else if (_armed.ready_to_arm) {
  		BOARD_ARMED_LED_OFF();
  
  		// ready to arm, blink at 1Hz
  		if (time_now_us >= _led_armed_state_toggle + 1_s) {
  			_led_armed_state_toggle = time_now_us;
  			BOARD_ARMED_STATE_LED_TOGGLE();
  		}
  
  	} else {
  		BOARD_ARMED_LED_OFF();
  
  		// not ready to arm, blink at 10Hz
  		if (time_now_us >= _led_armed_state_toggle + 100_ms) {
  			_led_armed_state_toggle = time_now_us;
  			BOARD_ARMED_STATE_LED_TOGGLE();
  		}
  	}
  
  #endif
  
  	// give system warnings on error LED
  	if (overload) {
  		if (time_now_us >= _led_overload_toggle + 50_ms) {
  			_led_overload_toggle = time_now_us;
  			BOARD_OVERLOAD_LED_TOGGLE();
  		}
  
  	} else {
  		BOARD_OVERLOAD_LED_OFF();
  	}
  }
  
  bool Commander::check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
  				      const hrt_abstime &data_timestamp_us, hrt_abstime &last_fail_time_us,
  				      const bool was_valid)
  {
  	bool valid = was_valid;
  	const bool data_stale = ((hrt_elapsed_time(&data_timestamp_us) > _param_com_pos_fs_delay.get() * 1_s)
  				 || (data_timestamp_us == 0));
  	const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy);
  	const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy);
  
  	// Check accuracy with hysteresis in both test level and time
  	if (level_check_pass) {
  		if (!was_valid) {
  			// check if probation period has elapsed
  			if (hrt_elapsed_time(&last_fail_time_us) > 1_s) {
  				valid = true;
  			}
  		}
  
  	} else {
  		// level check failed
  		if (was_valid) {
  			// FAILURE! no longer valid
  			valid = false;
  		}
  
  		last_fail_time_us = hrt_absolute_time();
  	}
  
  	if (was_valid != valid) {
  		_status_changed = true;
  	}
  
  	return valid;
  }
  
  void
  Commander::update_control_mode()
  {
  	_vehicle_control_mode = {};
  
  	/* set vehicle_control_mode according to set_navigation_state */
  	_vehicle_control_mode.flag_armed = _armed.armed;
  
  	switch (_status.nav_state) {
  	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
  		_vehicle_control_mode.flag_control_manual_enabled = true;
  		_vehicle_control_mode.flag_control_rates_enabled = stabilization_required();
  		_vehicle_control_mode.flag_control_attitude_enabled = stabilization_required();
  		break;
  
  	case vehicle_status_s::NAVIGATION_STATE_STAB:
  		_vehicle_control_mode.flag_control_manual_enabled = true;
  		_vehicle_control_mode.flag_control_rates_enabled = true;
  		_vehicle_control_mode.flag_control_attitude_enabled = true;
  		break;
  
  	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
  		_vehicle_control_mode.flag_control_manual_enabled = true;
  		_vehicle_control_mode.flag_control_rates_enabled = true;
  		_vehicle_control_mode.flag_control_attitude_enabled = true;
  		_vehicle_control_mode.flag_control_altitude_enabled = true;
  		_vehicle_control_mode.flag_control_climb_rate_enabled = true;
  		break;
  
  	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
  		_vehicle_control_mode.flag_control_manual_enabled = true;
  		_vehicle_control_mode.flag_control_rates_enabled = true;
  		_vehicle_control_mode.flag_control_attitude_enabled = true;
  		_vehicle_control_mode.flag_control_altitude_enabled = true;
  		_vehicle_control_mode.flag_control_climb_rate_enabled = true;
  		_vehicle_control_mode.flag_control_position_enabled = true;
  		_vehicle_control_mode.flag_control_velocity_enabled = true;
  		break;
  
  	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
  	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
  	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
  	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
  	case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
  	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
  	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
  	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
  	case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
  		_vehicle_control_mode.flag_control_auto_enabled = true;
  		_vehicle_control_mode.flag_control_rates_enabled = true;
  		_vehicle_control_mode.flag_control_attitude_enabled = true;
  		_vehicle_control_mode.flag_control_altitude_enabled = true;
  		_vehicle_control_mode.flag_control_climb_rate_enabled = true;
  		_vehicle_control_mode.flag_control_position_enabled = true;
  		_vehicle_control_mode.flag_control_velocity_enabled = true;
  		break;
  
  	case vehicle_status_s::NAVIGATION_STATE_ACRO:
  		_vehicle_control_mode.flag_control_manual_enabled = true;
  		_vehicle_control_mode.flag_control_rates_enabled = true;
  		break;
  
  	case vehicle_status_s::NAVIGATION_STATE_DESCEND:
  		_vehicle_control_mode.flag_control_auto_enabled = true;
  		_vehicle_control_mode.flag_control_rates_enabled = true;
  		_vehicle_control_mode.flag_control_attitude_enabled = true;
  		_vehicle_control_mode.flag_control_climb_rate_enabled = true;
  		break;
  
  	case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
  		/* disable all controllers on termination */
  		_vehicle_control_mode.flag_control_termination_enabled = true;
  		break;
  
  	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
  		_vehicle_control_mode.flag_control_offboard_enabled = true;
  
  		if (_offboard_control_mode_sub.get().position) {
  			_vehicle_control_mode.flag_control_position_enabled = true;
  			_vehicle_control_mode.flag_control_velocity_enabled = true;
  			_vehicle_control_mode.flag_control_altitude_enabled = true;
  			_vehicle_control_mode.flag_control_climb_rate_enabled = true;
  			_vehicle_control_mode.flag_control_acceleration_enabled = true;
  			_vehicle_control_mode.flag_control_rates_enabled = true;
  			_vehicle_control_mode.flag_control_attitude_enabled = true;
  
  		} else if (_offboard_control_mode_sub.get().velocity) {
  			_vehicle_control_mode.flag_control_velocity_enabled = true;
  			_vehicle_control_mode.flag_control_altitude_enabled = true;
  			_vehicle_control_mode.flag_control_climb_rate_enabled = true;
  			_vehicle_control_mode.flag_control_acceleration_enabled = true;
  			_vehicle_control_mode.flag_control_rates_enabled = true;
  			_vehicle_control_mode.flag_control_attitude_enabled = true;
  
  		} else if (_offboard_control_mode_sub.get().acceleration) {
  			_vehicle_control_mode.flag_control_acceleration_enabled = true;
  			_vehicle_control_mode.flag_control_rates_enabled = true;
  			_vehicle_control_mode.flag_control_attitude_enabled = true;
  
  		} else if (_offboard_control_mode_sub.get().attitude) {
  			_vehicle_control_mode.flag_control_rates_enabled = true;
  			_vehicle_control_mode.flag_control_attitude_enabled = true;
  
  		} else if (_offboard_control_mode_sub.get().body_rate) {
  			_vehicle_control_mode.flag_control_rates_enabled = true;
  		}
  
  		break;
  
  	case vehicle_status_s::NAVIGATION_STATE_ORBIT:
  		_vehicle_control_mode.flag_control_manual_enabled = false;
  		_vehicle_control_mode.flag_control_auto_enabled = false;
  		_vehicle_control_mode.flag_control_rates_enabled = true;
  		_vehicle_control_mode.flag_control_attitude_enabled = true;
  		_vehicle_control_mode.flag_control_altitude_enabled = true;
  		_vehicle_control_mode.flag_control_climb_rate_enabled = true;
  		_vehicle_control_mode.flag_control_position_enabled = true;
  		_vehicle_control_mode.flag_control_velocity_enabled = true;
  		break;
  
  	default:
  		break;
  	}
  
  	_vehicle_control_mode.flag_multicopter_position_control_enabled =
  		(_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
  		&& (_vehicle_control_mode.flag_control_altitude_enabled
  		    || _vehicle_control_mode.flag_control_climb_rate_enabled
  		    || _vehicle_control_mode.flag_control_position_enabled
  		    || _vehicle_control_mode.flag_control_velocity_enabled
  		    || _vehicle_control_mode.flag_control_acceleration_enabled);
  
  	_vehicle_control_mode.timestamp = hrt_absolute_time();
  	_control_mode_pub.publish(_vehicle_control_mode);
  }
  
  bool
  Commander::stabilization_required()
  {
  	return (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ||		// is a rotary wing, or
  		_status.vtol_fw_permanent_stab || 	// is a VTOL in fixed wing mode and stabilisation is on, or
  		(_vtol_status.vtol_in_trans_mode && 	// is currently a VTOL transitioning AND
  		 _status.vehicle_type ==
  		 vehicle_status_s::VEHICLE_TYPE_FIXED_WING));	// is a fixed wing, ie: transitioning back to rotary wing mode
  }
  
  void
  Commander::print_reject_mode(uint8_t main_state)
  {
  	if (hrt_elapsed_time(&_last_print_mode_reject_time) > 1_s) {
  
  		mavlink_log_critical(&_mavlink_log_pub, "Switching to %s is currently not available\t", main_state_str(main_state));
  		/* EVENT
  		 * @description Check for a valid position estimate
  		 */
  		events::send<events::px4::enums::navigation_mode_t>(events::ID("commander_modeswitch_not_avail"), {events::Log::Critical, events::LogInternal::Info},
  				"Switching to mode '{1}' is currently not possible", navigation_mode(main_state));
  
  		/* only buzz if armed, because else we're driving people nuts indoors
  		they really need to look at the leds as well. */
  		tune_negative(_armed.armed);
  
  		_last_print_mode_reject_time = hrt_absolute_time();
  	}
  }
  
  void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result)
  {
  	switch (result) {
  	case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED:
  		tune_negative(true);
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_RESULT_FAILED:
  		tune_negative(true);
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
  		tune_negative(true);
  		break;
  
  	case vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED:
  		tune_negative(true);
  		break;
  
  	default:
  		break;
  	}
  
  	/* publish ACK */
  	vehicle_command_ack_s command_ack{};
  	command_ack.command = cmd.command;
  	command_ack.result = result;
  	command_ack.target_system = cmd.source_system;
  	command_ack.target_component = cmd.source_component;
  	command_ack.timestamp = hrt_absolute_time();
  	_command_ack_pub.publish(command_ack);
  }
  
  int Commander::task_spawn(int argc, char *argv[])
  {
  	_task_id = px4_task_spawn_cmd("commander",
  				      SCHED_DEFAULT,
  				      SCHED_PRIORITY_DEFAULT + 40,
  				      3250,
  				      (px4_main_t)&run_trampoline,
  				      (char *const *)argv);
  
  	if (_task_id < 0) {
  		_task_id = -1;
  		return -errno;
  	}
  
  	// wait until task is up & running
  	if (wait_until_running() < 0) {
  		_task_id = -1;
  		return -1;
  	}
  
  	return 0;
  }
  
  Commander *Commander::instantiate(int argc, char *argv[])
  {
  	Commander *instance = new Commander();
  
  	if (instance) {
  		if (argc >= 2 && !strcmp(argv[1], "-h")) {
  			instance->enable_hil();
  		}
  	}
  
  	return instance;
  }
  
  void Commander::enable_hil()
  {
  	_status.hil_state = vehicle_status_s::HIL_STATE_ON;
  }
  
  void Commander::data_link_check()
  {
  	for (auto &telemetry_status :  _telemetry_status_subs) {
  		telemetry_status_s telemetry;
  
  		if (telemetry_status.update(&telemetry)) {
  
  			// handle different radio types
  			switch (telemetry.type) {
  			case telemetry_status_s::LINK_TYPE_USB:
  				// set (but don't unset) telemetry via USB as active once a MAVLink connection is up
  				_status_flags.usb_connected = true;
  				break;
  
  			case telemetry_status_s::LINK_TYPE_IRIDIUM: {
  					iridiumsbd_status_s iridium_status;
  
  					if (_iridiumsbd_status_sub.update(&iridium_status)) {
  						_high_latency_datalink_heartbeat = iridium_status.last_heartbeat;
  
  						if (_status.high_latency_data_link_lost) {
  							if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_param_com_hldl_reg_t.get() * 1_s)) {
  								_status.high_latency_data_link_lost = false;
  								_status_changed = true;
  							}
  						}
  
  						const bool present = true;
  						const bool enabled = true;
  						const bool ok = (iridium_status.last_heartbeat > 0); // maybe at some point here an additional check should be made
  
  						set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SATCOM, present, enabled, ok, _status);
  					}
  
  					break;
  				}
  			}
  
  			if (telemetry.heartbeat_type_gcs) {
  				// Initial connection or recovery from data link lost
  				if (_status.data_link_lost) {
  					_status.data_link_lost = false;
  					_status_changed = true;
  
  					if (_datalink_last_heartbeat_gcs != 0) {
  						mavlink_log_info(&_mavlink_log_pub, "Data link regained\t");
  						events::send(events::ID("commander_dl_regained"), events::Log::Info, "Data link regained");
  					}
  
  					if (!_armed.armed && !_status_flags.calibration_enabled) {
  						// make sure to report preflight check failures to a connecting GCS
  						PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, _vehicle_control_mode,
  									       true, false, hrt_elapsed_time(&_boot_timestamp));
  					}
  				}
  
  				_datalink_last_heartbeat_gcs = telemetry.timestamp;
  			}
  
  			if (telemetry.heartbeat_type_onboard_controller) {
  				if (_onboard_controller_lost) {
  					_onboard_controller_lost = false;
  					_status_changed = true;
  
  					if (_datalink_last_heartbeat_onboard_controller != 0) {
  						mavlink_log_info(&_mavlink_log_pub, "Onboard controller regained\t");
  						events::send(events::ID("commander_onboard_ctrl_regained"), events::Log::Info, "Onboard controller regained");
  					}
  				}
  
  				_datalink_last_heartbeat_onboard_controller = telemetry.timestamp;
  			}
  
  			if (telemetry.heartbeat_type_parachute) {
  				if (_parachute_system_lost) {
  					_parachute_system_lost = false;
  
  					if (_datalink_last_heartbeat_parachute_system != 0) {
  						mavlink_log_info(&_mavlink_log_pub, "Parachute system regained\t");
  						events::send(events::ID("commander_parachute_regained"), events::Log::Info, "Parachute system regained");
  					}
  				}
  
  				bool healthy = telemetry.parachute_system_healthy;
  
  				_datalink_last_heartbeat_parachute_system = telemetry.timestamp;
  				_status_flags.parachute_system_present = true;
  				_status_flags.parachute_system_healthy = healthy;
  				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, true, true, healthy, _status);
  			}
  
  			if (telemetry.heartbeat_component_obstacle_avoidance) {
  				if (_avoidance_system_lost) {
  					_avoidance_system_lost = false;
  					_status_changed = true;
  				}
  
  				_datalink_last_heartbeat_avoidance_system = telemetry.timestamp;
  				_status_flags.avoidance_system_valid = telemetry.avoidance_system_healthy;
  			}
  		}
  	}
  
  
  	// GCS data link loss failsafe
  	if (!_status.data_link_lost) {
  		if ((_datalink_last_heartbeat_gcs != 0)
  		    && hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_param_com_dl_loss_t.get() * 1_s)) {
  
  			_status.data_link_lost = true;
  			_status.data_link_lost_counter++;
  
  			mavlink_log_info(&_mavlink_log_pub, "Connection to ground station lost\t");
  			events::send(events::ID("commander_gcs_lost"), {events::Log::Warning, events::LogInternal::Info},
  				     "Connection to ground station lost");
  
  			_status_changed = true;
  		}
  	}
  
  	// ONBOARD CONTROLLER data link loss failsafe
  	if ((_datalink_last_heartbeat_onboard_controller > 0)
  	    && (hrt_elapsed_time(&_datalink_last_heartbeat_onboard_controller) > (_param_com_obc_loss_t.get() * 1_s))
  	    && !_onboard_controller_lost) {
  
  		mavlink_log_critical(&_mavlink_log_pub, "Connection to mission computer lost\t");
  		events::send(events::ID("commander_mission_comp_lost"), events::Log::Critical, "Connection to mission computer lost");
  		_onboard_controller_lost = true;
  		_status_changed = true;
  	}
  
  	// Parachute system
  	if ((hrt_elapsed_time(&_datalink_last_heartbeat_parachute_system) > 3_s)
  	    && !_parachute_system_lost) {
  		mavlink_log_critical(&_mavlink_log_pub, "Parachute system lost");
  		_status_flags.parachute_system_present = false;
  		_status_flags.parachute_system_healthy = false;
  		_parachute_system_lost = true;
  		_status_changed = true;
  		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, false, true, false, _status);
  	}
  
  	// AVOIDANCE SYSTEM state check (only if it is enabled)
  	if (_status_flags.avoidance_system_required && !_onboard_controller_lost) {
  		// if heartbeats stop
  		if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0)
  		    && (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) {
  
  			_avoidance_system_lost = true;
  			_status_flags.avoidance_system_valid = false;
  		}
  	}
  
  	// high latency data link loss failsafe
  	if (_high_latency_datalink_heartbeat > 0
  	    && hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_param_com_hldl_loss_t.get() * 1_s)) {
  		_high_latency_datalink_lost = hrt_absolute_time();
  
  		if (!_status.high_latency_data_link_lost) {
  			_status.high_latency_data_link_lost = true;
  			mavlink_log_critical(&_mavlink_log_pub, "High latency data link lost\t");
  			events::send(events::ID("commander_high_latency_lost"), events::Log::Critical, "High latency data link lost");
  			_status_changed = true;
  		}
  	}
  }
  
  void Commander::avoidance_check()
  {
  	for (auto &dist_sens_sub : _distance_sensor_subs) {
  		distance_sensor_s distance_sensor;
  
  		if (dist_sens_sub.update(&distance_sensor)) {
  			if ((distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
  			    (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {
  
  				_valid_distance_sensor_time_us = distance_sensor.timestamp;
  			}
  		}
  	}
  
  	const bool cp_enabled =  _param_cp_dist.get() > 0.f;
  
  	const bool distance_sensor_valid = hrt_elapsed_time(&_valid_distance_sensor_time_us) < 500_ms;
  	const bool cp_healthy = _status_flags.avoidance_system_valid || distance_sensor_valid;
  
  	const bool sensor_oa_present = cp_healthy || _status_flags.avoidance_system_required || cp_enabled;
  
  	const bool auto_mode = _vehicle_control_mode.flag_control_auto_enabled;
  	const bool pos_ctl_mode = (_vehicle_control_mode.flag_control_manual_enabled
  				   && _vehicle_control_mode.flag_control_position_enabled);
  
  	const bool sensor_oa_enabled = ((auto_mode && _status_flags.avoidance_system_required) || (pos_ctl_mode && cp_enabled));
  	const bool sensor_oa_healthy = ((auto_mode && _status_flags.avoidance_system_valid) || (pos_ctl_mode && cp_healthy));
  
  	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE, sensor_oa_present, sensor_oa_enabled,
  			 sensor_oa_healthy, _status);
  }
  
  void Commander::battery_status_check()
  {
  	int battery_required_count{0};
  	bool battery_has_fault = false;
  	// There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest
  	// option is to check if ANY of them have a warning, and specifically find which one has the most
  	// urgent warning.
  	uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE;
  	// To make sure that all connected batteries are being regularly reported, we check which one has the
  	// oldest timestamp.
  	hrt_abstime oldest_update = hrt_absolute_time();
  	float worst_battery_time_s{NAN};
  
  	_battery_current = 0.0f;
  
  	for (auto &battery_sub : _battery_status_subs) {
  		int index = battery_sub.get_instance();
  		battery_status_s battery;
  
  		if (!battery_sub.copy(&battery)) {
  			continue;
  		}
  
  		if (battery.is_required) {
  			battery_required_count++;
  		}
  
  		if (_armed.armed) {
  
  			if ((_last_connected_batteries & (1 << index)) && !battery.connected) {
  				mavlink_log_critical(&_mavlink_log_pub, "Battery %d disconnected. Land now! \t", index + 1);
  				events::send<uint8_t>(events::ID("commander_battery_disconnected"), {events::Log::Emergency, events::LogInternal::Warning},
  						      "Battery {1} disconnected. Land now!", index + 1);
  				// trigger a battery failsafe action if a battery disconnects in flight
  				worst_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
  			}
  
  			if ((battery.mode > 0) && (battery.mode != _last_battery_mode[index])) {
  
  				mavlink_log_critical(&_mavlink_log_pub, "Battery %d is in %s mode! \t", index + 1,
  						     battery_mode_str(static_cast<battery_mode_t>(battery.mode)));
  				events::send<uint8_t, events::px4::enums::battery_mode_t>(events::ID("commander_battery_mode"), {events::Log::Critical, events::LogInternal::Warning},
  						"Battery {1} mode: {2}. Land now!", index + 1, static_cast<battery_mode_t>(battery.mode));
  			}
  		}
  
  		_last_battery_mode[index] = battery.mode;
  
  		if (battery.connected) {
  			_last_connected_batteries |= 1 << index;
  
  			if (battery.warning > worst_warning) {
  				worst_warning = battery.warning;
  			}
  
  			if (battery.timestamp < oldest_update) {
  				oldest_update = battery.timestamp;
  			}
  
  			if (battery.faults > 0) {
  				// MAVLink supported faults, can be checked on the ground station
  				battery_has_fault = true;
  
  				if (battery.faults != _last_battery_fault[index] || battery.custom_faults != _last_battery_custom_fault[index]) {
  					for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(battery_fault_reason_t::_max);
  					     fault_index++) {
  						if (battery.faults & (1 << fault_index)) {
  							mavlink_log_emergency(&_mavlink_log_pub, "Battery %d: %s. %s \t", index + 1,
  									      battery_fault_reason_str(static_cast<battery_fault_reason_t>(fault_index)),  _armed.armed ? "Land now!" : "");
  
  							events::px4::enums::suggested_action_t action = _armed.armed ? events::px4::enums::suggested_action_t::land :
  									events::px4::enums::suggested_action_t::none;
  
  							/* EVENT
  							 * @description
  							 * The battery reported a failure which might be dangerous to fly.
  							 * Manufacturer error code: {4}
  							 */
  							events::send<uint8_t, battery_fault_reason_t, events::px4::enums::suggested_action_t, uint32_t>
  							(events::ID("commander_battery_fault"), {events::Log::Emergency, events::LogInternal::Warning},
  							 "Battery {1}: {2}. {3}", index + 1, static_cast<battery_fault_reason_t>(fault_index), action, battery.custom_faults);
  						}
  					}
  				}
  			}
  
  			_last_battery_fault[index] = battery.faults;
  			_last_battery_custom_fault[index] = battery.custom_faults;
  
  			if (PX4_ISFINITE(battery.time_remaining_s)
  			    && (!PX4_ISFINITE(worst_battery_time_s)
  				|| (PX4_ISFINITE(worst_battery_time_s) && (battery.time_remaining_s < worst_battery_time_s)))) {
  				worst_battery_time_s = battery.time_remaining_s;
  			}
  
  			// Sum up current from all batteries.
  			_battery_current += battery.current_filtered_a;
  		}
  	}
  
  	rtl_time_estimate_s rtl_time_estimate{};
  
  	// Compare estimate of RTL time to estimate of remaining flight time
  	if (_rtl_time_estimate_sub.copy(&rtl_time_estimate)
  	    && (hrt_absolute_time() - rtl_time_estimate.timestamp) < 2_s
  	    && rtl_time_estimate.valid
  	    && _armed.armed
  	    && !_vehicle_land_detected.ground_contact // not in any landing stage
  	    && !_rtl_time_actions_done
  	    && PX4_ISFINITE(worst_battery_time_s)
  	    && rtl_time_estimate.safe_time_estimate >= worst_battery_time_s
  	    && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
  	    && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
  		// Try to trigger RTL
  		if (main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags,
  					  _internal_state) == TRANSITION_CHANGED) {
  			mavlink_log_emergency(&_mavlink_log_pub, "Remaining flight time low, returning to land\t");
  			events::send(events::ID("commander_remaining_flight_time_rtl"), {events::Log::Critical, events::LogInternal::Info},
  				     "Remaining flight time low, returning to land");
  
  		} else {
  			mavlink_log_emergency(&_mavlink_log_pub, "Remaining flight time low, land now!\t");
  			events::send(events::ID("commander_remaining_flight_time_land"), {events::Log::Critical, events::LogInternal::Info},
  				     "Remaining flight time low, land now!");
  		}
  
  		_rtl_time_actions_done = true;
  	}
  
  	bool battery_warning_level_increased_while_armed = false;
  	bool update_internal_battery_state = false;
  
  	if (_armed.armed) {
  		if (worst_warning > _battery_warning) {
  			battery_warning_level_increased_while_armed = true;
  			update_internal_battery_state = true;
  		}
  
  	} else {
  		if (_battery_warning != worst_warning) {
  			update_internal_battery_state = true;
  		}
  	}
  
  	if (update_internal_battery_state) {
  		_battery_warning = worst_warning;
  	}
  
  	_status_flags.battery_healthy =
  		// All connected batteries are regularly being published
  		(hrt_elapsed_time(&oldest_update) < 5_s)
  		// There is at least one connected battery (in any slot)
  		&& (math::countSetBits(_last_connected_batteries) >= battery_required_count)
  		// No currently-connected batteries have any warning
  		&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE)
  		// No currently-connected batteries have any fault
  		&& (!battery_has_fault);
  
  	// execute battery failsafe if the state has gotten worse while we are armed
  	if (battery_warning_level_increased_while_armed) {
  		uint8_t failsafe_action = get_battery_failsafe_action(_internal_state, _battery_warning,
  					  (low_battery_action_t)_param_com_low_bat_act.get());
  
  		warn_user_about_battery(&_mavlink_log_pub, _battery_warning,
  					failsafe_action, _param_com_bat_act_t.get(),
  					main_state_str(failsafe_action), navigation_mode(failsafe_action));
  		_battery_failsafe_timestamp = hrt_absolute_time();
  
  		// Switch to loiter to wait for the reaction delay
  		if (_param_com_bat_act_t.get() > 0.f
  		    && failsafe_action != commander_state_s::MAIN_STATE_MAX) {
  			main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state);
  		}
  	}
  
  	if (_battery_failsafe_timestamp != 0
  	    && hrt_elapsed_time(&_battery_failsafe_timestamp) > _param_com_bat_act_t.get() * 1_s
  	    && (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER
  		|| _vehicle_control_mode.flag_control_auto_enabled)) {
  		_battery_failsafe_timestamp = 0;
  		uint8_t failsafe_action = get_battery_failsafe_action(_internal_state, _battery_warning,
  					  (low_battery_action_t)_param_com_low_bat_act.get());
  
  		if (failsafe_action != commander_state_s::MAIN_STATE_MAX) {
  			_internal_state.main_state = failsafe_action;
  			_internal_state.main_state_changes++;
  			_internal_state.timestamp = hrt_absolute_time();
  		}
  	}
  
  	// Handle shutdown request from emergency battery action
  	if (update_internal_battery_state) {
  
  		if (_battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
  #if defined(BOARD_HAS_POWER_CONTROL)
  
  			if (shutdown_if_allowed() && (px4_shutdown_request(400_ms) == 0)) {
  				mavlink_log_critical(&_mavlink_log_pub, "Dangerously low battery! Shutting system down\t");
  				events::send(events::ID("commander_low_bat_shutdown"), {events::Log::Emergency, events::LogInternal::Warning},
  					     "Dangerously low battery! Shutting system down");
  
  				while (1) { px4_usleep(1); }
  
  			} else {
  				mavlink_log_critical(&_mavlink_log_pub, "System does not support shutdown\t");
  				/* EVENT
  				 * @description Cannot shut down, most likely the system does not support it.
  				 */
  				events::send(events::ID("commander_low_bat_shutdown_failed"), {events::Log::Emergency, events::LogInternal::Error},
  					     "Dangerously low battery! System shut down failed");
  			}
  
  #endif // BOARD_HAS_POWER_CONTROL
  		}
  	}
  }
  
  void Commander::estimator_check()
  {
  	// Check if quality checking of position accuracy and consistency is to be performed
  	const bool run_quality_checks = !_status_flags.circuit_breaker_engaged_posfailure_check;
  
  	_local_position_sub.update();
  	_global_position_sub.update();
  
  	const vehicle_local_position_s &lpos = _local_position_sub.get();
  
  	if (lpos.heading_reset_counter != _heading_reset_counter) {
  		if (_status_flags.home_position_valid) {
  			updateHomePositionYaw(_home_pub.get().yaw + lpos.delta_heading);
  		}
  
  		_heading_reset_counter = lpos.heading_reset_counter;
  	}
  
  	const bool mag_fault_prev = _estimator_status_flags_sub.get().cs_mag_fault;
  	const bool gnss_heading_fault_prev = _estimator_status_flags_sub.get().cs_gps_yaw_fault;
  
  	// use primary estimator_status
  	if (_estimator_selector_status_sub.updated()) {
  		estimator_selector_status_s estimator_selector_status;
  
  		if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
  			if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) {
  				_estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance);
  				_estimator_status_flags_sub.ChangeInstance(estimator_selector_status.primary_instance);
  			}
  		}
  	}
  
  	if (_estimator_status_flags_sub.update()) {
  		const estimator_status_flags_s &estimator_status_flags = _estimator_status_flags_sub.get();
  
  		_status_flags.dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning
  					       || estimator_status_flags.cs_inertial_dead_reckoning;
  
  		if (!(estimator_status_flags.cs_inertial_dead_reckoning || estimator_status_flags.cs_wind_dead_reckoning)) {
  			// position requirements (update if not dead reckoning)
  			bool gps             = estimator_status_flags.cs_gps;
  			bool optical_flow    = estimator_status_flags.cs_opt_flow;
  			bool vision_position = estimator_status_flags.cs_ev_pos;
  
  			_status_flags.position_reliant_on_gps             =  gps && !optical_flow && !vision_position;
  			_status_flags.position_reliant_on_optical_flow    = !gps &&  optical_flow && !vision_position;
  			_status_flags.position_reliant_on_vision_position = !gps && !optical_flow &&  vision_position;
  		}
  
  		// Check for a magnetomer fault and notify the user
  		if (!mag_fault_prev && estimator_status_flags.cs_mag_fault) {
  			mavlink_log_critical(&_mavlink_log_pub, "Compass needs calibration - Land now!\t");
  			events::send(events::ID("commander_stopping_mag_use"), events::Log::Critical,
  				     "Stopping compass use! Land now and calibrate the compass");
  			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, true, true, false, _status);
  		}
  
  		if (!gnss_heading_fault_prev && estimator_status_flags.cs_gps_yaw_fault) {
  			mavlink_log_critical(&_mavlink_log_pub, "GNSS heading not reliable - Land now!\t");
  			events::send(events::ID("commander_stopping_gnss_heading_use"), events::Log::Critical,
  				     "GNSS heading not reliable. Land now!");
  			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, false, _status);
  		}
  	}
  
  
  	/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
  	* for a short time interval after takeoff.
  	* Most of the time, the drone can recover from a bad initial yaw using GPS-inertial
  	* heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but
  	* if this does not fix the issue we need to stop using a position controlled
  	* mode to prevent flyaway crashes.
  	*/
  	bool pre_flt_fail_innov_heading = false;
  	bool pre_flt_fail_innov_vel_horiz = false;
  
  	if (_estimator_status_sub.updated()) {
  
  		estimator_status_s estimator_status;
  
  		if (_estimator_status_sub.copy(&estimator_status)) {
  
  			pre_flt_fail_innov_heading = estimator_status.pre_flt_fail_innov_heading;
  			pre_flt_fail_innov_vel_horiz = estimator_status.pre_flt_fail_innov_vel_horiz;
  
  			if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
  
  				if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
  					_nav_test_failed = false;
  					_nav_test_passed = false;
  
  				} else {
  					if (!_nav_test_passed) {
  						// Both test ratios need to pass/fail together to change the nav test status
  						const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f)
  									     && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON);
  
  						const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f);
  
  						if (innovation_pass) {
  							_time_last_innov_pass = hrt_absolute_time();
  
  							// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
  							const bool sufficient_time = (_status.takeoff_time != 0) && (hrt_elapsed_time(&_status.takeoff_time) > 30_s);
  							const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f);
  
  							// Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds
  							if (hrt_elapsed_time(&_time_last_innov_fail) > 10_s
  							    && (sufficient_time || sufficient_speed)) {
  								_nav_test_passed = true;
  								_nav_test_failed = false;
  							}
  
  						} else if (innovation_fail) {
  							_time_last_innov_fail = hrt_absolute_time();
  
  							if (!_nav_test_failed && hrt_elapsed_time(&_time_last_innov_pass) > 2_s) {
  								// if the innovation test has failed continuously, declare the nav as failed
  								_nav_test_failed = true;
  								mavlink_log_emergency(&_mavlink_log_pub, "Navigation failure! Land and recalibrate sensors\t");
  								events::send(events::ID("commander_navigation_failure"), events::Log::Emergency,
  									     "Navigation failure! Land and recalibrate the sensors");
  							}
  						}
  					}
  				}
  			}
  		}
  	}
  
  	// run position and velocity accuracy checks
  	// Check if quality checking of position accuracy and consistency is to be performed
  	if (run_quality_checks) {
  		float lpos_eph_threshold_adj = _param_com_pos_fs_eph.get();
  
  		// relax local position eph threshold in operator controlled position mode
  		if (_internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL &&
  		    ((_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL)
  		     || (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL))) {
  
  			// Set the allowable position uncertainty based on combination of flight and estimator state
  			// When we are in a operator demanded position control mode and are solely reliant on optical flow,
  			// do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift
  			if (_status_flags.position_reliant_on_optical_flow) {
  				lpos_eph_threshold_adj = INFINITY;
  			}
  		}
  
  		bool xy_valid = lpos.xy_valid && !_nav_test_failed;
  		bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed;
  
  		if (!_armed.armed) {
  			if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) {
  				xy_valid = false;
  			}
  
  			if (pre_flt_fail_innov_vel_horiz) {
  				v_xy_valid = false;
  			}
  		}
  
  		const vehicle_global_position_s &gpos = _global_position_sub.get();
  
  		_status_flags.global_position_valid =
  			check_posvel_validity(xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
  					      _last_gpos_fail_time_us, _status_flags.global_position_valid);
  
  		_status_flags.local_position_valid =
  			check_posvel_validity(xy_valid, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp,
  					      _last_lpos_fail_time_us, _status_flags.local_position_valid);
  
  		_status_flags.local_velocity_valid =
  			check_posvel_validity(v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
  					      _last_lvel_fail_time_us, _status_flags.local_velocity_valid);
  	}
  
  
  	// altitude
  	_status_flags.local_altitude_valid = lpos.z_valid
  					     && (hrt_elapsed_time(&lpos.timestamp) < (_param_com_pos_fs_delay.get() * 1_s));
  
  
  	// attitude
  	vehicle_attitude_s attitude{};
  	_vehicle_attitude_sub.copy(&attitude);
  	const matrix::Quatf q{attitude.q};
  	const bool no_element_larger_than_one = (fabsf(q(0)) <= 1.f)
  						&& (fabsf(q(1)) <= 1.f)
  						&& (fabsf(q(2)) <= 1.f)
  						&& (fabsf(q(3)) <= 1.f);
  	const bool norm_in_tolerance = (fabsf(1.f - q.norm()) <= 1e-6f);
  
  	const bool attitude_valid = (hrt_elapsed_time(&attitude.timestamp) < 1_s)
  				    && norm_in_tolerance && no_element_larger_than_one;
  
  	if (_status_flags.attitude_valid && !attitude_valid) {
  		PX4_ERR("attitude estimate no longer valid");
  	}
  
  	_status_flags.attitude_valid = attitude_valid;
  
  
  	// angular velocity
  	vehicle_angular_velocity_s angular_velocity{};
  	_vehicle_angular_velocity_sub.copy(&angular_velocity);
  	const bool condition_angular_velocity_time_valid = (angular_velocity.timestamp != 0)
  			&& (hrt_elapsed_time(&angular_velocity.timestamp) < 1_s);
  	const bool condition_angular_velocity_finite = PX4_ISFINITE(angular_velocity.xyz[0])
  			&& PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]);
  	const bool angular_velocity_valid = condition_angular_velocity_time_valid
  					    && condition_angular_velocity_finite;
  
  	if (_status_flags.angular_velocity_valid && !angular_velocity_valid) {
  		const char err_str[] {"angular velocity no longer valid"};
  
  		if (!condition_angular_velocity_time_valid) {
  			PX4_ERR("%s (timeout)", err_str);
  
  		} else if (!condition_angular_velocity_finite) {
  			PX4_ERR("%s (non-finite values)", err_str);
  		}
  	}
  
  	_status_flags.angular_velocity_valid = angular_velocity_valid;
  
  
  	// gps
  	const bool condition_gps_position_was_valid = _status_flags.gps_position_valid;
  
  	if (_vehicle_gps_position_sub.updated()) {
  		vehicle_gps_position_s vehicle_gps_position;
  
  		if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)) {
  
  			bool time = (vehicle_gps_position.timestamp != 0) && (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 1_s);
  
  			bool fix = vehicle_gps_position.fix_type >= 2;
  			bool eph = vehicle_gps_position.eph < _param_com_pos_fs_eph.get();
  			bool epv = vehicle_gps_position.epv < _param_com_pos_fs_epv.get();
  			bool evh = vehicle_gps_position.s_variance_m_s < _param_com_vel_fs_evh.get();
  
  			_vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh, hrt_absolute_time());
  			_status_flags.gps_position_valid = _vehicle_gps_position_valid.get_state();
  
  			_vehicle_gps_position_timestamp_last = vehicle_gps_position.timestamp;
  		}
  
  	} else {
  		const hrt_abstime now_us = hrt_absolute_time();
  
  		if (now_us > _vehicle_gps_position_timestamp_last + GPS_VALID_TIME) {
  			_vehicle_gps_position_valid.set_state_and_update(false, now_us);
  			_status_flags.gps_position_valid = false;
  		}
  	}
  
  	if (condition_gps_position_was_valid && !_status_flags.gps_position_valid) {
  		PX4_DEBUG("GPS no longer valid");
  	}
  }
  
  void
  Commander::offboard_control_update()
  {
  	bool offboard_available = false;
  
  	if (_offboard_control_mode_sub.updated()) {
  		const offboard_control_mode_s old = _offboard_control_mode_sub.get();
  
  		if (_offboard_control_mode_sub.update()) {
  			const offboard_control_mode_s &ocm = _offboard_control_mode_sub.get();
  
  			if (old.position != ocm.position ||
  			    old.velocity != ocm.velocity ||
  			    old.acceleration != ocm.acceleration ||
  			    old.attitude != ocm.attitude ||
  			    old.body_rate != ocm.body_rate ||
  			    old.actuator != ocm.actuator) {
  
  				_status_changed = true;
  			}
  
  			if (ocm.position || ocm.velocity || ocm.acceleration || ocm.attitude || ocm.body_rate || ocm.actuator) {
  				offboard_available = true;
  			}
  		}
  	}
  
  	if (_offboard_control_mode_sub.get().position && !_status_flags.local_position_valid) {
  		offboard_available = false;
  
  	} else if (_offboard_control_mode_sub.get().velocity && !_status_flags.local_velocity_valid) {
  		offboard_available = false;
  
  	} else if (_offboard_control_mode_sub.get().acceleration && !_status_flags.local_velocity_valid) {
  		// OFFBOARD acceleration handled by position controller
  		offboard_available = false;
  	}
  
  	_offboard_available.set_state_and_update(offboard_available, hrt_absolute_time());
  
  	const bool offboard_lost = !_offboard_available.get_state();
  
  	if (_status_flags.offboard_control_signal_lost != offboard_lost) {
  		_status_flags.offboard_control_signal_lost = offboard_lost;
  		_status_changed = true;
  	}
  }
  
  void Commander::esc_status_check()
  {
  	esc_status_s esc_status{};
  
  	_esc_status_sub.copy(&esc_status);
  
  	if (esc_status.esc_count > 0) {
  
  		char esc_fail_msg[50];
  		esc_fail_msg[0] = '\0';
  
  		int online_bitmask = (1 << esc_status.esc_count) - 1;
  
  		// Check if ALL the ESCs are online
  		if (online_bitmask == esc_status.esc_online_flags) {
  
  			_status_flags.escs_error = false;
  			_last_esc_online_flags = esc_status.esc_online_flags;
  
  		} else if (_last_esc_online_flags == esc_status.esc_online_flags)  {
  
  			// Avoid checking the status if the flags are the same or if the mixer has not yet been loaded in the ESC driver
  
  			_status_flags.escs_error = true;
  
  		} else if (esc_status.esc_online_flags < _last_esc_online_flags) {
  
  			// Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing Offline ESCs warnings at boot
  
  			for (int index = 0; index < esc_status.esc_count; index++) {
  				if ((esc_status.esc_online_flags & (1 << index)) == 0) {
  					snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", index + 1);
  					esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
  					events::px4::enums::suggested_action_t action = _armed.armed ? events::px4::enums::suggested_action_t::land :
  							events::px4::enums::suggested_action_t::none;
  					events::send<uint8_t, events::px4::enums::suggested_action_t>(events::ID("commander_esc_offline"),
  							events::Log::Critical, "ESC{1} offline. {2}", index + 1, action);
  				}
  			}
  
  			mavlink_log_critical(&_mavlink_log_pub, "%soffline. %s\t", esc_fail_msg, _armed.armed ? "Land now!" : "");
  
  			_last_esc_online_flags = esc_status.esc_online_flags;
  			_status_flags.escs_error = true;
  		}
  
  		_status_flags.escs_failure = false;
  
  		for (int index = 0; index < esc_status.esc_count; index++) {
  
  			_status_flags.escs_failure |= esc_status.esc[index].failures > 0;
  
  			if (esc_status.esc[index].failures != _last_esc_failure[index]) {
  
  				for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(esc_fault_reason_t::_max);
  				     fault_index++) {
  					if (esc_status.esc[index].failures & (1 << fault_index)) {
  
  						esc_fault_reason_t fault_reason_index = static_cast<esc_fault_reason_t>(fault_index);
  
  						const char *user_action = nullptr;
  						events::px4::enums::suggested_action_t action = events::px4::enums::suggested_action_t::none;
  
  						if (fault_reason_index == esc_fault_reason_t::motor_warn_temp
  						    || fault_reason_index == esc_fault_reason_t::esc_warn_temp) {
  							user_action = "Reduce throttle";
  							action = events::px4::enums::suggested_action_t::reduce_throttle;
  
  						} else {
  							user_action = "Land now!";
  							action = events::px4::enums::suggested_action_t::land;
  						}
  
  						mavlink_log_emergency(&_mavlink_log_pub, "ESC%d: %s. %s \t", index + 1,
  								      esc_fault_reason_str(fault_reason_index),  _armed.armed ? user_action : "");
  
  						events::send<uint8_t, events::px4::enums::esc_fault_reason_t, events::px4::enums::suggested_action_t>
  						(events::ID("commander_esc_fault"), {events::Log::Emergency, events::LogInternal::Warning},
  						 "ESC {1}: {2}. {3}", index + 1, fault_reason_index, action);
  					}
  				}
  			}
  
  			_last_esc_failure[index] = esc_status.esc[index].failures;
  		}
  
  	}
  
  	_last_esc_status_updated = esc_status.timestamp;
  }
  
  void Commander::send_parachute_command()
  {
  	vehicle_command_s vcmd{};
  	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE;
  	vcmd.param1 = static_cast<float>(vehicle_command_s::PARACHUTE_ACTION_RELEASE);
  
  	uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
  	vcmd.source_system = vehicle_status_sub.get().system_id;
  	vcmd.target_system = vehicle_status_sub.get().system_id;
  	vcmd.source_component = vehicle_status_sub.get().component_id;
  	vcmd.target_component = 161; // MAV_COMP_ID_PARACHUTE
  
  	uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
  	vcmd.timestamp = hrt_absolute_time();
  	vcmd_pub.publish(vcmd);
  
  	set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
  }
  
  void Commander::checkWindAndWarn()
  {
  	wind_s wind_estimate;
  
  	if (_wind_sub.update(&wind_estimate)) {
  		const matrix::Vector2f wind(wind_estimate.windspeed_north, wind_estimate.windspeed_east);
  
  		// publish a warning if it's the first since in air or 60s have passed since the last warning
  		const bool warning_timeout_passed = _last_wind_warning == 0 || hrt_elapsed_time(&_last_wind_warning) > 60_s;
  
  		if (wind.longerThan(_param_com_wind_warn.get()) && warning_timeout_passed) {
  			mavlink_log_critical(&_mavlink_log_pub, "High wind speed detected (%.1f m/s), landing advised\t", (double)wind.norm());
  
  			events::send<float>(events::ID("commander_high_wind_warning"),
  			{events::Log::Warning, events::LogInternal::Info},
  			"High wind speed detected ({1:.1m/s}), landing advised", wind.norm());
  			_last_wind_warning = hrt_absolute_time();
  		}
  	}
  }
  
  int Commander::print_usage(const char *reason)
  {
  	if (reason) {
  		PX4_INFO("%s", reason);
  	}
  
  	PRINT_MODULE_DESCRIPTION(
  		R"DESCR_STR(
  ### Description
  The commander module contains the state machine for mode switching and failsafe behavior.
  )DESCR_STR");
  
  	PRINT_MODULE_USAGE_NAME("commander", "system");
  	PRINT_MODULE_USAGE_COMMAND("start");
  	PRINT_MODULE_USAGE_PARAM_FLAG('h', "Enable HIL mode", true);
  #ifndef CONSTRAINED_FLASH
  	PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run sensor calibration");
  	PRINT_MODULE_USAGE_ARG("mag|baro|accel|gyro|level|esc|airspeed", "Calibration type", false);
  	PRINT_MODULE_USAGE_ARG("quick", "Quick calibration (accel only, not recommended)", false);
  	PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks");
  	PRINT_MODULE_USAGE_COMMAND("arm");
  	PRINT_MODULE_USAGE_PARAM_FLAG('f', "Force arming (do not run preflight checks)", true);
  	PRINT_MODULE_USAGE_COMMAND("disarm");
  	PRINT_MODULE_USAGE_PARAM_FLAG('f', "Force disarming (disarm in air)", true);
  	PRINT_MODULE_USAGE_COMMAND("takeoff");
  	PRINT_MODULE_USAGE_COMMAND("land");
  	PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition");
  	PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode");
  	PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland",
  			"Flight mode", false);
  	PRINT_MODULE_USAGE_COMMAND("pair");
  	PRINT_MODULE_USAGE_COMMAND("lockdown");
  	PRINT_MODULE_USAGE_ARG("on|off", "Turn lockdown on or off", false);
  	PRINT_MODULE_USAGE_COMMAND("set_ekf_origin");
  	PRINT_MODULE_USAGE_ARG("lat, lon, alt", "Origin Latitude, Longitude, Altitude", false);
  	PRINT_MODULE_USAGE_COMMAND_DESCR("lat|lon|alt", "Origin latitude longitude altitude");
  	PRINT_MODULE_USAGE_COMMAND_DESCR("poweroff", "Power off board (if supported)");
  #endif
  	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
  
  	return 1;
  }
  