Disable multicopter attitude autotuner for fmu-v5 test build.

Scope VEHICLE_MODE_FLAG to the Commander class.

boards/px4/fmu-v5/test.px4board

@@ -5,6 +5,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n

msg/GeofenceResult.msg

@@ -1,14 +1,14 @@
-uint64 timestamp                        # time since system start (microseconds)
+uint64 timestamp                # time since system start (microseconds)
-uint8 GF_ACTION_NONE = 0                # no action on geofence violation
+uint8 GF_ACTION_NONE      = 0   # no action on geofence violation
-uint8 GF_ACTION_WARN = 1                # critical mavlink message
+uint8 GF_ACTION_WARN      = 1   # critical mavlink message
-uint8 GF_ACTION_LOITER = 2              # switch to AUTO|LOITER
+uint8 GF_ACTION_LOITER    = 2   # switch to AUTO|LOITER
-uint8 GF_ACTION_RTL = 3                 # switch to AUTO|RTL
+uint8 GF_ACTION_RTL       = 3   # switch to AUTO|RTL
-uint8 GF_ACTION_TERMINATE = 4           # flight termination
+uint8 GF_ACTION_TERMINATE = 4   # flight termination
-uint8 GF_ACTION_LAND = 5                # switch to AUTO|LAND
+uint8 GF_ACTION_LAND      = 5   # switch to AUTO|LAND
 
-uint8 geofence_violation_reason         # one of geofence_violation_reason_t::*
+uint8 geofence_violation_reason # one of geofence_violation_reason_t::*
 
-bool primary_geofence_breached          # true if the primary geofence is breached
+bool primary_geofence_breached  # true if the primary geofence is breached
-uint8 primary_geofence_action           # action to take when the primary geofence is breached
+uint8 primary_geofence_action   # action to take when the primary geofence is breached
 
-bool home_required                      # true if the geofence requires a valid home position
+bool home_required              # true if the geofence requires a valid home position

src/modules/commander/Commander.cpp

@@ -32,23 +32,22 @@
  ****************************************************************************/
 
 /**
- * @file commander.cpp
+ * @file Commander.cpp
- *
+ * Primary state machine and flight stack logic
- * Main state machine / business logic
- *
  */
 
 #include "Commander.hpp"
 
-/* commander module headers */
+/* Commander module headers */
 #include "Arming/ArmAuthorization/ArmAuthorization.h"
 #include "commander_helper.h"
 #include "esc_calibration.h"
-#define DEFINE_GET_PX4_CUSTOM_MODE
-#include "px4_custom_mode.h"
 #include "ModeUtil/control_mode.hpp"
 #include "ModeUtil/conversions.hpp"
 
+#define DEFINE_GET_PX4_CUSTOM_MODE
+#include "px4_custom_mode.h"
+
 /* PX4 headers */
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_tone_alarm.h>
@@ -69,23 +68,8 @@
 #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;
-
 // TODO: generate
-static constexpr bool operator ==(const actuator_armed_s &a, const actuator_armed_s &b)
+static constexpr bool operator == (const actuator_armed_s &a, const actuator_armed_s &b)
 {
 	return (a.armed == b.armed &&
 		a.prearmed == b.prearmed &&
@@ -95,9 +79,11 @@ static constexpr bool operator ==(const actuator_armed_s &a, const actuator_arme
 		a.force_failsafe == b.force_failsafe &&
 		a.in_esc_calibration_mode == b.in_esc_calibration_mode);
 }
+
 static_assert(sizeof(actuator_armed_s) == 16, "actuator_armed equality operator review");
 
 #if defined(BOARD_HAS_POWER_CONTROL)
+static orb_advert_t power_button_state_pub = nullptr;
 static orb_advert_t tune_control_pub = nullptr;
 
 static void play_power_button_down_tune()
@@ -114,7 +100,6 @@ static void stop_tune()
 	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
@@ -228,6 +213,56 @@ static bool broadcast_vehicle_command(const uint32_t cmd, const float param1 = N
 }
 #endif
 
+Commander::Commander() :
+	ModuleParams(nullptr)
+{
+	_vehicle_land_detected.landed = true;
+
+	_vehicle_status.system_id = 1;
+	_vehicle_status.component_id = 1;
+
+	_vehicle_status.system_type = 0;
+	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
+
+	_vehicle_status.nav_state = _user_mode_intention.get();
+	_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
+	_vehicle_status.nav_state_timestamp = hrt_absolute_time();
+
+	/* mark all signals lost as long as they haven't been found */
+	_vehicle_status.gcs_connection_lost = true;
+
+	_vehicle_status.power_input_valid = true;
+
+	// default for vtol is rotary wing
+	_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+
+	_param_mav_comp_id = param_find("MAV_COMP_ID");
+	_param_mav_sys_id = param_find("MAV_SYS_ID");
+	_param_mav_type = param_find("MAV_TYPE");
+	_param_rc_map_fltmode = param_find("RC_MAP_FLTMODE");
+
+	updateParameters();
+}
+
+Commander::~Commander()
+{
+	perf_free(_loop_perf);
+	perf_free(_preflight_check_perf);
+}
+
+Commander *Commander::instantiate(int argc, char *argv[])
+{
+	Commander *instance = new Commander();
+
+	if (instance) {
+		if (argc >= 2 && !strcmp(argv[1], "-h")) {
+			instance->enableHIL();
+		}
+	}
+
+	return instance;
+}
+
 int Commander::custom_command(int argc, char *argv[])
 {
 	if (!is_running()) {
@@ -471,22 +506,6 @@ int Commander::custom_command(int argc, char *argv[])
 	return print_usage("unknown command");
 }
 
-int Commander::print_status()
-{
-	PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
-	PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
-	PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
-	PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
-	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::shutdownIfAllowed()
 {
 	return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status,
@@ -638,45 +657,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
 	return arming_res;
 }
 
-Commander::Commander() :
+bool Commander::handleCommand(const vehicle_command_s &cmd)
-	ModuleParams(nullptr)
-{
-	_vehicle_land_detected.landed = true;
-
-	_vehicle_status.system_id = 1;
-	_vehicle_status.component_id = 1;
-
-	_vehicle_status.system_type = 0;
-	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
-
-	_vehicle_status.nav_state = _user_mode_intention.get();
-	_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
-	_vehicle_status.nav_state_timestamp = hrt_absolute_time();
-
-	/* mark all signals lost as long as they haven't been found */
-	_vehicle_status.gcs_connection_lost = true;
-
-	_vehicle_status.power_input_valid = true;
-
-	// default for vtol is rotary wing
-	_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
-
-	_param_mav_comp_id = param_find("MAV_COMP_ID");
-	_param_mav_sys_id = param_find("MAV_SYS_ID");
-	_param_mav_type = param_find("MAV_TYPE");
-	_param_rc_map_fltmode = param_find("RC_MAP_FLTMODE");
-
-	updateParameters();
-}
-
-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 != _vehicle_status.system_id) && (cmd.target_system != 0))
@@ -738,7 +719,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			uint8_t desired_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX;
 			transition_result_t main_ret = TRANSITION_NOT_CHANGED;
 
-			if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+			if (base_mode == VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) {
 				/* use autopilot-specific mode */
 				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
 					desired_nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
@@ -805,14 +786,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 			} else {
 				/* use base mode */
-				if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
+				if (base_mode == VEHICLE_MODE_FLAG_AUTO_ENABLED) {
 					desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
 
-				} else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+				} else if (base_mode == VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-					if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
+					if (base_mode == VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
 						desired_nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
 
-					} else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) {
+					} else if (base_mode == VEHICLE_MODE_FLAG_STABILIZE_ENABLED) {
 						desired_nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
 
 					} else {
@@ -913,7 +894,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 					_actuator_armed.force_failsafe = true;
 					_flight_termination_triggered = true;
 					PX4_WARN("forcing failsafe (termination)");
-					send_parachute_command();
+					sendParachuteCommand();
 				}
 
 			} else {
@@ -1126,13 +1107,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 			if (param1 == 0) {
 				// 0: Do nothing for autopilot
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 #if defined(CONFIG_BOARDCTL_RESET)
 
 			} else if ((param1 == 1) && shutdownIfAllowed() && (px4_reboot_request(false, 400_ms) == 0)) {
 				// 1: Reboot autopilot
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 				while (1) { px4_usleep(1); }
 
@@ -1142,7 +1123,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 			} else if ((param1 == 2) && shutdownIfAllowed() && (px4_shutdown_request(400_ms) == 0)) {
 				// 2: Shutdown autopilot
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 				while (1) { px4_usleep(1); }
 
@@ -1152,14 +1133,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 			} else if ((param1 == 3) && shutdownIfAllowed() && (px4_reboot_request(true, 400_ms) == 0)) {
 				// 3: Reboot autopilot and keep it in the bootloader until upgraded.
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 				while (1) { px4_usleep(1); }
 
 #endif // CONFIG_BOARDCTL_RESET
 
 			} else {
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
 			}
 		}
 
@@ -1170,7 +1151,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
 
 			} else {
 
@@ -1181,14 +1162,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
 						(cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal))
 				   ) {
 
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
 					break;
 
 				}
 
 				if ((int)(cmd.param1) == 1) {
 					/* gyro calibration */
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::GyroCalibration);
 
@@ -1200,19 +1181,19 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 				} else if ((int)(cmd.param2) == 1) {
 					/* magnetometer calibration */
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::MagCalibration);
 
 				} else if ((int)(cmd.param3) == 1) {
 					/* baro calibration */
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::BaroCalibration);
 
 				} else if ((int)(cmd.param4) == 1) {
 					/* RC calibration */
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					/* disable RC control input completely */
 					_vehicle_status.rc_calibration_in_progress = true;
 					mavlink_log_info(&_mavlink_log_pub, "Calibration: Disabling RC input\t");
@@ -1221,39 +1202,39 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 				} else if ((int)(cmd.param4) == 2) {
 					/* RC trim calibration */
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::RCTrimCalibration);
 
 				} else if ((int)(cmd.param5) == 1) {
 					/* accelerometer calibration */
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::AccelCalibration);
 
 				} else if ((int)(cmd.param5) == 2) {
 					// board offset calibration
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::LevelCalibration);
 
 				} else if ((int)(cmd.param5) == 4) {
 					// accelerometer quick calibration
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.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_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.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_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+						answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 						if (_safety.isButtonAvailable() && !_safety.isSafetyOff()) {
 							mavlink_log_critical(&_mavlink_log_pub, "ESC calibration denied! Press safety button first\t");
@@ -1267,7 +1248,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 						}
 
 					} else {
-						answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
+						answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
 					}
 
 				} else if ((int)(cmd.param4) == 0) {
@@ -1280,10 +1261,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 							     "Calibration: Restoring RC input");
 					}
 
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 				} else {
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
 				}
 			}
 
@@ -1295,10 +1276,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
 
 			} else {
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 				// parameter 1: Heading   (degrees)
 				// parameter 3: Latitude  (degrees)
 				// parameter 4: Longitude (degrees)
@@ -1330,28 +1311,28 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
 
 			} else {
 
 				if (((int)(cmd.param1)) == 0) {
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamLoadDefault);
 
 				} else if (((int)(cmd.param1)) == 1) {
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamSaveDefault);
 
 				} else if (((int)(cmd.param1)) == 2) {
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamResetAllConfig);
 
 				} else if (((int)(cmd.param1)) == 3) {
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamResetSensorFactory);
 
 				} else if (((int)(cmd.param1)) == 4) {
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamResetAll);
 				}
 			}
@@ -1361,7 +1342,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS:
 		_health_and_arming_checks.update(true);
-		answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+		answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 		break;
 
 	case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
@@ -1410,13 +1391,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	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_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
+		answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
 		break;
 	}
 
 	if (cmd_result != vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
 		/* already warned about unsupported commands in "default" case */
-		answer_command(cmd, cmd_result);
+		answerCommand(cmd, cmd_result);
 	}
 
 	return true;
@@ -1540,7 +1521,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 
 			_status_changed = true;
 			_actuator_armed.manual_lockdown = true;
-			send_parachute_command();
+			sendParachuteCommand();
 		}
 
 		break;
@@ -1678,7 +1659,7 @@ void Commander::run()
 
 		handleAutoDisarm();
 
-		battery_status_check();
+		batteryStatusCheck();
 
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (!_vehicle_status.calibration_enabled && _arm_state_machine.isInit()) {
@@ -1735,7 +1716,7 @@ void Commander::run()
 					PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation());
 				}
 
-				if (handle_command(cmd)) {
+				if (handleCommand(cmd)) {
 					_status_changed = true;
 				}
 			}
@@ -1818,7 +1799,7 @@ void Commander::run()
 		checkWorkerThread();
 
 		updateTunes();
-		control_status_leds(_status_changed, _battery_warning);
+		controlStatusLEDs(_status_changed, _battery_warning);
 
 		_status_changed = false;
 
@@ -2214,7 +2195,7 @@ bool Commander::handleModeIntentionAndFailsafe()
 
 		if (!_flight_termination_triggered) {
 			_flight_termination_triggered = true;
-			send_parachute_command();
+			sendParachuteCommand();
 		}
 
 		break;
@@ -2250,7 +2231,7 @@ void Commander::checkAndInformReadyForTakeoff()
 #endif // CONFIG_ARCH_BOARD_PX4_SITL
 }
 
-void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
+void Commander::controlStatusLEDs(bool changed, const uint8_t battery_warning)
 {
 	switch (blink_msg_state()) {
 	case 1:
@@ -2432,7 +2413,7 @@ void Commander::printRejectMode(uint8_t nav_state)
 	}
 }
 
-void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result)
+void Commander::answerCommand(const vehicle_command_s &cmd, uint8_t result)
 {
 	switch (result) {
 	case vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED:
@@ -2468,43 +2449,7 @@ void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result)
 	_vehicle_command_ack_pub.publish(command_ack);
 }
 
-int Commander::task_spawn(int argc, char *argv[])
+void Commander::enableHIL()
-{
-	_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()
 {
 	_vehicle_status.hil_state = vehicle_status_s::HIL_STATE_ON;
 }
@@ -2690,7 +2635,7 @@ void Commander::dataLinkCheck()
 	}
 }
 
-void Commander::battery_status_check()
+void Commander::batteryStatusCheck()
 {
 	// Handle shutdown request from emergency battery action
 	if (_battery_warning != _failsafe_flags.battery_warning) {
@@ -2788,7 +2733,7 @@ void Commander::offboardControlCheck()
 	}
 }
 
-void Commander::send_parachute_command()
+void Commander::sendParachuteCommand()
 {
 	vehicle_command_s vcmd{};
 	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE;
@@ -2807,6 +2752,17 @@ void Commander::send_parachute_command()
 	set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
 }
 
+int Commander::print_status()
+{
+	PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
+	PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
+	PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
+	PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
+	perf_print_counter(_loop_perf);
+	perf_print_counter(_preflight_check_perf);
+	return 0;
+}
+
 int Commander::print_usage(const char *reason)
 {
 	if (reason) {
@@ -2849,3 +2805,31 @@ The commander module contains the state machine for mode switching and failsafe
 
 	return 1;
 }
+
+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;
+}
+
+extern "C" __EXPORT int commander_main(int argc, char *argv[])
+{
+	return Commander::main(argc, argv);
+}

src/modules/commander/Commander.hpp

@@ -50,25 +50,21 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 
-// publications
 #include <uORB/Publication.hpp>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/actuator_test.h>
-#include <uORB/topics/failure_detector_status.h>
-#include <uORB/topics/vehicle_command_ack.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_status.h>
-
-// subscriptions
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
+
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/actuator_test.h>
+#include <uORB/topics/failure_detector_status.h>
 #include <uORB/topics/action_request.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/iridiumsbd_status.h>
+#include <uORB/topics/mavlink_log.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/offboard_control_mode.h>
@@ -78,10 +74,14 @@
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/tune_control.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 
 using math::constrain;
@@ -96,13 +96,13 @@ public:
 	~Commander();
 
 	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
+	static int custom_command(int argc, char *argv[]);
 
 	/** @see ModuleBase */
 	static Commander *instantiate(int argc, char *argv[]);
 
-	/** @see ModuleBase */
+	/** @see ModuleBase::print_status() */
-	static int custom_command(int argc, char *argv[]);
+	int print_status() override;
 
 	/** @see ModuleBase */
 	static int print_usage(const char *reason = nullptr);
@@ -110,79 +110,12 @@ public:
 	/** @see ModuleBase::run() */
 	void run() override;
 
-	/** @see ModuleBase::print_status() */
+	/** @see ModuleBase */
-	int print_status() override;
+	static int task_spawn(int argc, char *argv[]);
 
-	void enable_hil();
+	void enableHIL();
 
 private:
-	void answer_command(const vehicle_command_s &cmd, uint8_t result);
-
-	transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
-
-	transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false);
-
-	void battery_status_check();
-
-	void control_status_leds(bool changed, const uint8_t battery_warning);
-
-	/**
-	 * Checks the status of all available data links and handles switching between different system telemetry states.
-	 */
-	void dataLinkCheck();
-
-	void manualControlCheck();
-
-	void offboardControlCheck();
-
-	/**
-	 * @brief Handle incoming vehicle command relavant to Commander
-	 *
-	 * It ignores irrelevant vehicle commands defined inside the switch case statement
-	 * in the function.
-	 *
-	 * @param cmd 		Vehicle command to handle
-	 */
-	bool handle_command(const vehicle_command_s &cmd);
-
-	unsigned handleCommandActuatorTest(const vehicle_command_s &cmd);
-
-	void executeActionRequest(const action_request_s &action_request);
-
-	void printRejectMode(uint8_t nav_state);
-
-	void updateControlMode();
-
-	bool shutdownIfAllowed();
-
-	void send_parachute_command();
-
-	void checkForMissionUpdate();
-
-	void handlePowerButtonState();
-
-	void systemPowerUpdate();
-
-	void landDetectorUpdate();
-
-	void safetyButtonUpdate();
-
-	void vtolStatusUpdate();
-
-	void updateTunes();
-
-	void checkWorkerThread();
-
-	bool getPrearmState() const;
-
-	void handleAutoDisarm();
-
-	bool handleModeIntentionAndFailsafe();
-
-	void updateParameters();
-
-	void checkAndInformReadyForTakeoff();
-
 	enum class PrearmedMode {
 		DISABLED = 0,
 		SAFETY_BUTTON = 1,
@@ -194,108 +127,177 @@ private:
 		OFFBOARD_MODE_BIT = (1 << 1),
 	};
 
+	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, /*  | */
+	};
+
+	transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
+
+	transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false);
+
+	void answerCommand(const vehicle_command_s &cmd, uint8_t result);
+
+	void batteryStatusCheck();
+
+	void checkAndInformReadyForTakeoff();
+
+	void checkForMissionUpdate();
+
+	void checkWorkerThread();
+
+	void controlStatusLEDs(bool changed, const uint8_t battery_warning);
+
+	/** Checks the status of all available data links and handles switching between different system telemetry states. */
+	void dataLinkCheck();
+
+	void executeActionRequest(const action_request_s &action_request);
+
+	bool getPrearmState() const;
+
+	void handleAutoDisarm();
+
+	bool handleCommand(const vehicle_command_s &cmd);
+
+	unsigned handleCommandActuatorTest(const vehicle_command_s &cmd);
+
+	bool handleModeIntentionAndFailsafe();
+
+	void handlePowerButtonState();
+
+	void landDetectorUpdate();
+
+	void manualControlCheck();
+
+	void offboardControlCheck();
+
+	void printRejectMode(uint8_t nav_state);
+
+	void safetyButtonUpdate();
+
+	void sendParachuteCommand();
+
+	bool shutdownIfAllowed();
+
+	void systemPowerUpdate();
+
+	void updateControlMode();
+
+	void updateParameters();
+
+	void updateTunes();
+
+	void vtolStatusUpdate();
+
 	/* Decouple update interval and hysteresis counters, all depends on intervals */
 	static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
 	static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
 
-	vehicle_status_s        _vehicle_status{};
+	ArmStateMachine       _arm_state_machine{};
-
+	Failsafe              _failsafe_instance{this};
-	ArmStateMachine		_arm_state_machine{};
+	FailsafeBase          &_failsafe{_failsafe_instance};
-	Failsafe		_failsafe_instance{this};
+	FailureDetector       _failure_detector{this};
-	FailsafeBase		&_failsafe{_failsafe_instance};
+	HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status};
-	FailureDetector		_failure_detector{this};
+	Safety                _safety{};
-	HealthAndArmingChecks	_health_and_arming_checks{this, _vehicle_status};
+	UserModeIntention     _user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
-	Safety			_safety{};
+	WorkerThread          _worker_thread{};
-	UserModeIntention	_user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
-	WorkerThread 		_worker_thread{};
 
 	const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()};
-	HomePosition 		_home_position{_failsafe_flags};
 
+	HomePosition            _home_position{_failsafe_flags};
 
 	Hysteresis _auto_disarm_landed{false};
 	Hysteresis _auto_disarm_killed{false};
 
+	hrt_abstime _boot_timestamp{0};
+
 	hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0};
 	hrt_abstime _datalink_last_heartbeat_avoidance_system{0};
 	hrt_abstime _datalink_last_heartbeat_gcs{0};
 	hrt_abstime _datalink_last_heartbeat_onboard_controller{0};
 	hrt_abstime _datalink_last_heartbeat_parachute_system{0};
 
-	hrt_abstime _last_print_mode_reject_time{0};	///< To remember when last notification was sent
-
 	hrt_abstime _high_latency_datalink_heartbeat{0};
 	hrt_abstime _high_latency_datalink_lost{0};
 
-	hrt_abstime _boot_timestamp{0};
 	hrt_abstime _last_disarmed_timestamp{0};
-	hrt_abstime _overload_start{0};		///< time when CPU overload started
+	hrt_abstime _last_health_and_arming_check{0};
+	hrt_abstime _last_print_mode_reject_time{0};    ///< To remember when last notification was sent
+
+	hrt_abstime _led_overload_toggle{0};
+
+	hrt_abstime _overload_start{0};         ///< time when CPU overload started
 
 #if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
 	hrt_abstime _led_armed_state_toggle {0};
 #endif
-	hrt_abstime _led_overload_toggle {0};
 
-	hrt_abstime _last_health_and_arming_check{0};
+	actuator_armed_s        _actuator_armed{};
+	vehicle_control_mode_s  _vehicle_control_mode{};
+	vehicle_land_detected_s _vehicle_land_detected{};
+	vehicle_status_s        _vehicle_status{};
+	vtol_vehicle_status_s   _vtol_vehicle_status{};
 
-	uint8_t		_battery_warning{battery_status_s::BATTERY_WARNING_NONE};
+	uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
 
-	bool _failsafe_user_override_request{false}; ///< override request due to stick movements
+	bool _arm_tune_played{false};
-
-	bool _flight_termination_triggered{false};
-	bool _lockdown_triggered{false};
-
-	bool _open_drone_id_system_lost{true};
 	bool _avoidance_system_lost{false};
-	bool _onboard_controller_lost{false};
-	bool _parachute_system_lost{true};
 
-	bool _last_overload{false};
+	bool _failsafe_user_override_request{false}; // override request due to stick movements
-	bool _mode_switch_mapped{false};
+	bool _flight_termination_triggered{false};
+
+	bool _have_taken_off_since_arming{false};
 
 	bool _is_throttle_above_center{false};
 	bool _is_throttle_low{false};
 
-	bool _arm_tune_played{false};
+	bool _last_overload{false};
-	bool _was_armed{false};
+	bool _lockdown_triggered{false};
-	bool _have_taken_off_since_arming{false};
+
+	bool _mode_switch_mapped{false};
+
+	bool _onboard_controller_lost{false};
+	bool _open_drone_id_system_lost{true};
+
+	bool _parachute_system_lost{true};
+
 	bool _status_changed{true};
-
+	bool _was_armed{false};
-	vehicle_land_detected_s	_vehicle_land_detected{};
-
-	// commander publications
-	actuator_armed_s        _actuator_armed{};
-	vehicle_control_mode_s  _vehicle_control_mode{};
-	vtol_vehicle_status_s	_vtol_vehicle_status{};
 
 	// Subscriptions
-	uORB::Subscription					_action_request_sub{ORB_ID(action_request)};
+	uORB::Subscription _action_request_sub{ORB_ID(action_request)};
-	uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
+	uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
-	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
+	uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
-	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
+	uORB::Subscription _system_power_sub{ORB_ID(system_power)};
-	uORB::Subscription					_vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
-	uORB::Subscription					_vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
-	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
+	uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
-
-	uORB::SubscriptionInterval				_parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
-	uORB::SubscriptionMultiArray<telemetry_status_s>	_telemetry_status_subs{ORB_ID::telemetry_status};
 
 #if defined(BOARD_HAS_POWER_CONTROL)
-	uORB::Subscription					_power_button_state_sub {ORB_ID(power_button_state)};
+	uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)};
 #endif // BOARD_HAS_POWER_CONTROL
 
-	uORB::SubscriptionData<mission_result_s>		_mission_result_sub{ORB_ID(mission_result)};
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-	uORB::SubscriptionData<offboard_control_mode_s>		_offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
+	uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
+
+	uORB::SubscriptionData<mission_result_s>        _mission_result_sub{ORB_ID(mission_result)};
+	uORB::SubscriptionData<offboard_control_mode_s> _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 
 	// Publications
-	uORB::Publication<actuator_armed_s>			_actuator_armed_pub{ORB_ID(actuator_armed)};
+	uORB::Publication<actuator_armed_s>          _actuator_armed_pub{ORB_ID(actuator_armed)};
-	uORB::Publication<actuator_test_s>			_actuator_test_pub{ORB_ID(actuator_test)};
+	uORB::Publication<actuator_test_s>           _actuator_test_pub{ORB_ID(actuator_test)};
-	uORB::Publication<failure_detector_status_s>		_failure_detector_status_pub{ORB_ID(failure_detector_status)};
+	uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
-	uORB::Publication<vehicle_command_ack_s>		_vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::Publication<vehicle_command_ack_s>     _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
-	uORB::Publication<vehicle_control_mode_s>		_vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
+	uORB::Publication<vehicle_control_mode_s>    _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
-	uORB::Publication<vehicle_status_s>			_vehicle_status_pub{ORB_ID(vehicle_status)};
+	uORB::Publication<vehicle_status_s>          _vehicle_status_pub{ORB_ID(vehicle_status)};
 
 	orb_advert_t _mavlink_log_pub{nullptr};