Allow to in-air restore the FMU and IO arming state if only one of the two fails

5 files changed, 207 insertions, 41 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 6b1bc0f9b..f917b7275 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1260,6 +1260,8 @@ int commander_thread_main(int argc, char *argv[])
 	param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms);
 
 	param_t _param_sys_type = param_find("MAV_TYPE");
+	param_t _param_system_id = param_find("MAV_SYS_ID");
+	param_t _param_component_id = param_find("MAV_COMP_ID");
 
 	/* welcome user */
 	warnx("I am in command now!\n");
@@ -1444,6 +1446,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* parameters changed */
 			orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
 
+
 			/* update parameters */
 			if (!current_status.flag_system_armed) {
 				if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
@@ -1460,6 +1463,10 @@ int commander_thread_main(int argc, char *argv[])
 					current_status.flag_external_manual_override_ok = true;
 				}
 
+				/* check and update system / component ID */
+				param_get(_param_system_id, &(current_status.system_id));
+				param_get(_param_component_id, &(current_status.component_id));
+
 			} else {
 				warnx("ARMED, rejecting sys type change\n");
 			}
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 8bcf4bacb..069028c14 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -76,6 +76,7 @@
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/parameter_update.h>
@@ -243,6 +244,24 @@ private:
 	 */
 	int			mixer_send(const char *buf, unsigned buflen);
 
+	/**
+	 * Handle a status update from IO.
+	 *
+	 * Publish IO status information if necessary.
+	 *
+	 * @param status	The status register
+	 */
+	int			io_handle_status(uint16_t status);
+
+	/**
+	 * Handle an alarm update from IO.
+	 *
+	 * Publish IO alarm information if necessary.
+	 *
+	 * @param alarm		The status register
+	 */
+	int			io_handle_alarms(uint16_t alarms);
+
 };
 
 
@@ -332,17 +351,113 @@ PX4IO::init()
 	if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
 		_max_rc_input = RC_INPUT_MAX_CHANNELS;
 
-	/* dis-arm IO before touching anything */
-	io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 
-		PX4IO_P_SETUP_ARMING_ARM_OK |
-		PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
-		PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
+	/*
+	 * Check for IO flight state - if FMU was flagged to be in
+	 * armed state, FMU is recovering from an in-air reset.
+	 * Read back status and request the commander to arm
+	 * in this case.
+	 */
+
+	uint16_t reg;
 
-	/* publish RC config to IO */
-	ret = io_set_rc_config();
-	if (ret != OK) {
-		log("failed to update RC input config");
+	/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
+	ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &reg, sizeof(reg));
+	if (ret != OK)
 		return ret;
+
+	if (reg & PX4IO_P_SETUP_ARMING_ARM_OK) {
+
+		/* WARNING: COMMANDER app/vehicle status must be initialized.
+		 * If this fails (or the app is not started), worst-case IO
+		 * remains untouched (so manual override is still available).
+		 */
+
+		int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+		/* fill with initial values, clear updated flag */
+		vehicle_status_s status;
+		uint64_t try_start_time = hrt_absolute_time();
+		bool updated = false;
+		
+		/* keep checking for an update, ensure we got a recent state,
+		   not something that was published a long time ago. */
+		do {
+			orb_check(vstatus_sub, &updated);
+
+			if (updated) {
+				/* got data, copy and exit loop */
+				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+				break;
+			}
+
+			/* wait 10 ms */
+			usleep(10000);
+
+			/* abort after 10s */
+			if ((hrt_absolute_time() - try_start_time)/1000 > 10000) {
+				log("failed to recover from in-air restart (1), aborting IO driver init.");
+				return 1;
+			}
+
+		} while (true);
+
+		/* send command to arm system via command API */
+		vehicle_command_s cmd;
+		/* request arming */
+		cmd.param1 = 1.0f;
+		cmd.param2 = 0;
+		cmd.param3 = 0;
+		cmd.param4 = 0;
+		cmd.param5 = 0;
+		cmd.param6 = 0;
+		cmd.param7 = 0;
+		cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM;
+		cmd.target_system = status.system_id;
+		cmd.target_component = status.component_id;
+		cmd.source_system = status.system_id;
+		cmd.source_component = status.component_id;
+		/* ask to confirm command */
+		cmd.confirmation =  1;
+
+		/* send command once */
+		(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
+
+		/* spin here until IO's state has propagated into the system */
+		do {
+			orb_check(vstatus_sub, &updated);
+
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+			}
+
+			/* wait 10 ms */
+			usleep(10000);
+
+			/* abort after 10s */
+			if ((hrt_absolute_time() - try_start_time)/1000 > 10000) {
+				log("failed to recover from in-air restart (2), aborting IO driver init.");
+				return 1;
+			}
+
+		/* keep waiting for state change for 10 s */
+		} while (!status.flag_system_armed);
+
+	/* regular boot, no in-air restart, init IO */
+	} else {
+
+
+		/* dis-arm IO before touching anything */
+		io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 
+			PX4IO_P_SETUP_ARMING_ARM_OK |
+			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
+			PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
+
+			/* publish RC config to IO */
+		ret = io_set_rc_config();
+		if (ret != OK) {
+			log("failed to update RC input config");
+			return ret;
+		}
+
 	}
 
 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
@@ -647,6 +762,42 @@ PX4IO::io_set_rc_config()
 }
 
 int
+PX4IO::io_handle_status(uint16_t status)
+{
+	int ret = 1;
+	/**
+	 * WARNING: This section handles in-air resets.
+	 */
+
+	/* check for IO reset - force it back to armed if necessary */
+	if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)) {
+		/* set the arming flag */
+		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED);
+
+		/* set new status */
+		_status = status;
+		_status &= PX4IO_P_STATUS_FLAGS_ARMED;
+	} else {
+		ret = 0;
+
+		/* set new status */
+		_status = status;
+	}
+	return ret;
+}
+
+int
+PX4IO::io_handle_alarms(uint16_t alarms)
+{
+
+	/* XXX handle alarms */
+
+
+	/* set new alarms state */
+	_alarms = alarms;
+}
+
+int
 PX4IO::io_get_status()
 {
 	uint16_t	regs[4];
@@ -657,12 +808,8 @@ PX4IO::io_get_status()
 	if (ret != OK)
 		return ret;
 
-	_status = regs[0];
-	_alarms = regs[1];
-
-	/* XXX handle status */
-
-	/* XXX handle alarms */
+	io_handle_status(regs[0]);
+	io_handle_alarms(regs[1]);
 
 	/* only publish if battery has a valid minimum voltage */
 	if (regs[2] > 3300) {
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index f64b82ad1..4f1b067bd 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -64,9 +64,9 @@
  * packed.
  */
 
-#define PX4IO_CONTROL_CHANNELS	8
-#define PX4IO_INPUT_CHANNELS	12
-#define PX4IO_RELAY_CHANNELS	4
+#define PX4IO_CONTROL_CHANNELS			8
+#define PX4IO_INPUT_CHANNELS			12
+#define PX4IO_RELAY_CHANNELS			4
 
 /* Per C, this is safe for all 2's complement systems */
 #define REG_TO_SIGNED(_reg)	((int16_t)(_reg))
@@ -76,7 +76,7 @@
 #define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
 
 /* static configuration page */
-#define PX4IO_PAGE_CONFIG		0
+#define PX4IO_PAGE_CONFIG			0
 #define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_SOFTWARE_VERSION		1	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_BOOTLOADER_VERSION	2	/* get this how? */
@@ -88,11 +88,11 @@
 #define PX4IO_P_CONFIG_RELAY_COUNT		8	/* harcoded # of relay outputs */
 
 /* dynamic status page */
-#define PX4IO_PAGE_STATUS		1
+#define PX4IO_PAGE_STATUS			1
 #define PX4IO_P_STATUS_FREEMEM			0
 #define PX4IO_P_STATUS_CPULOAD			1
 
-#define PX4IO_P_STATUS_FLAGS			2	/* monitoring flags */
+#define PX4IO_P_STATUS_FLAGS			2	 /* monitoring flags */
 #define PX4IO_P_STATUS_FLAGS_ARMED		(1 << 0) /* arm-ok and locally armed */
 #define PX4IO_P_STATUS_FLAGS_OVERRIDE		(1 << 1) /* in manual override */
 #define PX4IO_P_STATUS_FLAGS_RC_OK		(1 << 2) /* RC input is valid */
@@ -103,7 +103,7 @@
 #define PX4IO_P_STATUS_FLAGS_RAW_PWM		(1 << 7) /* raw PWM from FMU is bypassing the mixer */
 #define PX4IO_P_STATUS_FLAGS_MIXER_OK		(1 << 8) /* mixer is OK */
 
-#define PX4IO_P_STATUS_ALARMS			3	/* alarm flags - alarms latch, write 1 to a bit to clear it */
+#define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* VBatt is very close to regulator dropout */
 #define PX4IO_P_STATUS_ALARMS_TEMPERATURE	(1 << 1) /* board temperature is high */
 #define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT	(1 << 2) /* servo current limit was exceeded */
@@ -115,29 +115,29 @@
 #define PX4IO_P_STATUS_IBATT			5	/* battery current in cA */
 
 /* array of post-mix actuator outputs, -10000..10000 */
-#define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_ACTUATORS			2	/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* array of PWM servo output values, microseconds */
-#define PX4IO_PAGE_SERVOS		3		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_SERVOS			3	/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* array of raw RC input values, microseconds */
-#define PX4IO_PAGE_RAW_RC_INPUT		4
+#define PX4IO_PAGE_RAW_RC_INPUT			4
 #define PX4IO_P_RAW_RC_COUNT			0	/* number of valid channels */
 #define PX4IO_P_RAW_RC_BASE			1	/* CONFIG_RC_INPUT_COUNT channels from here */
 
 /* array of scaled RC input values, -10000..10000 */
-#define PX4IO_PAGE_RC_INPUT		5
+#define PX4IO_PAGE_RC_INPUT			5
 #define PX4IO_P_RC_VALID			0	/* bitmask of valid controls */
 #define PX4IO_P_RC_BASE				1	/* CONFIG_RC_INPUT_COUNT controls from here */
 
 /* array of raw ADC values */
-#define PX4IO_PAGE_RAW_ADC_INPUT	6		/* 0..CONFIG_ADC_INPUT_COUNT-1 */
+#define PX4IO_PAGE_RAW_ADC_INPUT		6	/* 0..CONFIG_ADC_INPUT_COUNT-1 */
 
 /* setup page */
-#define PX4IO_PAGE_SETUP		100
+#define PX4IO_PAGE_SETUP			100
 #define PX4IO_P_SETUP_FEATURES			0
 
-#define PX4IO_P_SETUP_ARMING			1	/* arming controls */
+#define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
 #define PX4IO_P_SETUP_ARMING_ARM_OK		(1 << 0) /* OK to arm */
 #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK	(1 << 2) /* OK to switch to manual override via override RC channel */
 #define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK	(1 << 3) /* OK to perform position / vector control (= position lock) */
@@ -152,13 +152,13 @@
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
 /* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS		101		/* 0..CONFIG_CONTROL_COUNT */
+#define PX4IO_PAGE_CONTROLS			101	/* 0..CONFIG_CONTROL_COUNT */
 
 /* raw text load to the mixer parser - ignores offset */
-#define PX4IO_PAGE_MIXERLOAD		102
+#define PX4IO_PAGE_MIXERLOAD			102
 
 /* R/C channel config */
-#define PX4IO_PAGE_RC_CONFIG		103	/* R/C input configuration */
+#define PX4IO_PAGE_RC_CONFIG			103	/* R/C input configuration */
 #define PX4IO_P_RC_CONFIG_MIN			0	/* lowest input value */
 #define PX4IO_P_RC_CONFIG_CENTER		1	/* center input value */
 #define PX4IO_P_RC_CONFIG_MAX			2	/* highest input value */
@@ -170,10 +170,10 @@
 #define PX4IO_P_RC_CONFIG_STRIDE		6	/* spacing between channel config data */
 
 /* PWM output - overrides mixer */
-#define PX4IO_PAGE_DIRECT_PWM		104		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DIRECT_PWM			104	/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* PWM failsafe values - zero disables the output */
-#define PX4IO_PAGE_FAILSAFE_PWM		105		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_FAILSAFE_PWM			105	/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /**
  * As-needed mixer data upload.
@@ -187,8 +187,8 @@ struct px4io_mixdata {
 #define F2I_MIXER_MAGIC		0x6d74
 
 	uint8_t		action;
-#define F2I_MIXER_ACTION_RESET		0
-#define F2I_MIXER_ACTION_APPEND		1
+#define F2I_MIXER_ACTION_RESET			0
+#define F2I_MIXER_ACTION_APPEND			1
 
 	char		text[0];	/* actual text size may vary */
 };
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
index 014608416..be3bebada 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -290,11 +290,20 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_ARMING:
 
 			value &= PX4IO_P_SETUP_ARMING_VALID;
-			r_setup_arming = value;
 
-			/* update arming state - disarm if no longer OK */
-			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK))
+			/*
+			 * Update arming state - disarm if no longer OK.
+			 * This builds on the requirement that the FMU driver
+			 * asks about the FMU arming state on initialization,
+			 * so that an in-air reset of FMU can not lead to a
+			 * lockup of the IO arming state.
+			 */
+			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) {
 				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			}
+
+			r_setup_arming = value;
+
 			break;
 
 		case PX4IO_P_SETUP_PWM_RATES:
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index 06b4c5ca5..893e34537 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -156,7 +156,9 @@ struct vehicle_status_s
 	enum VEHICLE_FLIGHT_MODE flight_mode;		/**< current flight mode, as defined by mode switch */
 	enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode;	/**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
 	enum VEHICLE_MANUAL_SAS_MODE	manual_sas_mode;	/**< current stabilization mode */
-	int32_t system_type;				/**< system type, inspired by MAVLinks VEHICLE_TYPE enum */
+	int32_t system_type;				/**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
+	int32_t	system_id;				/**< system id, inspired by MAVLink's system ID field */
+	int32_t component_id;				/**< subsystem / component id, inspired by MAVLink's component ID field */
 
 	/* system flags - these represent the state predicates */
 
@@ -171,7 +173,7 @@ struct vehicle_status_s
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 
 	bool flag_preflight_gyro_calibration;		/**< true if gyro calibration is requested */
-	bool flag_preflight_mag_calibration;			/**< true if mag calibration is requested */
+	bool flag_preflight_mag_calibration;		/**< true if mag calibration is requested */
 	bool flag_preflight_accel_calibration;
 
 	bool rc_signal_found_once;
@@ -209,6 +211,7 @@ struct vehicle_status_s
 	bool flag_auto_flight_mode_ok;			/**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */
 	bool flag_external_manual_override_ok;		/**< external override non-fatal for system. Only true for fixed wing */
 	bool flag_valid_launch_position;		/**< indicates a valid launch position */
+	bool flag_valid_home_position;			/**< indicates a valid home position (a valid home position is not always a valid launch) */
 };
 
 /**