Fix up FMU input timeout handling.

Fix the FMU auto OK LED status.
Strip out unused fields from the system state structure.

5 files changed, 8 insertions, 96 deletions

diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index 7ccf915b0..f394233f4 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -66,7 +66,7 @@ extern "C" {
 #define OVERRIDE 4
 
 /* current servo arm/disarm state */
-bool mixer_servos_armed = false;
+static bool mixer_servos_armed = false;
 
 /* selected control values and count for mixing */
 enum mixer_source {
@@ -89,7 +89,7 @@ mixer_tick(void)
 	/* check that we are receiving fresh data from the FMU */
 	if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
 
-		/* too many frames without FMU input, time to go to failsafe */
+		/* too long without FMU input, time to go to failsafe */
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
 		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_FMU_OK;
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 682348a60..501691cd2 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -185,5 +185,3 @@ struct px4io_mixdata {
 
 /* maximum size is limited by the link frame size XXX use config values to set this */
 #define F2I_MIXER_MAX_TEXT	(64 - sizeof(struct px4io_mixdata))
-
-#pragma pack(pop)
\ No newline at end of file
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 2e2c50a3a..d8cdafb4b 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -101,107 +101,16 @@ extern uint16_t			r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
 
 /*
  * System state structure.
- *
- * XXX note that many fields here are deprecated and replaced by 
- *     registers.
  */
 struct sys_state_s {
 
-//	bool		armed;			/* IO armed */
-//	bool		arm_ok;			/* FMU says OK to arm */
-//	uint16_t	servo_rate;		/* output rate of servos in Hz */
-
-	/**
-	 * Remote control input(s) channel mappings
-	 */
-	uint8_t		rc_map[4];
-
-	/**
-	 * Remote control channel attributes
-	 */
-	uint16_t	rc_min[4];
-	uint16_t	rc_trim[4];
-	uint16_t	rc_max[4];
-	int16_t		rc_rev[4];
-	uint16_t	rc_dz[4];
-
-	/**
-	 * Data from the remote control input(s)
-	 */
-//	unsigned	rc_channels;
-//	uint16_t	rc_channel_data[PX4IO_INPUT_CHANNELS];
 	uint64_t	rc_channels_timestamp;
 
 	/**
-	 * Control signals from FMU.
-	 */
-//	uint16_t	fmu_channel_data[PX4IO_CONTROL_CHANNELS];
-
-	/**
-	 * Mixed servo outputs
-	 */
-//	uint16_t	servos[IO_SERVO_COUNT];
-
-	/**
-	 * Relay controls
-	 */
-//	bool		relays[PX4IO_RELAY_CHANNELS];
-
-	/**
-	 * If true, we are using the FMU controls, else RC input if available.
-	 */
-//	bool		mixer_manual_override;
-
-	/**
-	 * If true, FMU input is available.
-	 */
-//	bool		mixer_fmu_available;
-
-	/**
-	 * If true, state that should be reported to FMU has been updated.
-	 */
-//	bool		fmu_report_due;
-
-	/**
 	 * Last FMU receive time, in microseconds since system boot
 	 */
 	uint64_t	fmu_data_received_time;
 
-	/**
-	 * If true, the RC signal has been lost for more than a timeout interval
-	 */
-//	bool		rc_lost;
-
-	/**
-	 * If true, the connection to FMU has been lost for more than a timeout interval
-	 */
-//	bool		fmu_lost;
-
-	/**
-	 * If true, FMU is ready for autonomous position control. Only used for LED indication
-	 */
-	bool vector_flight_mode_ok;
-
-	/**
-	 * If true, IO performs an on-board manual override with the RC channel values
-	 */
-//	bool manual_override_ok;
-
-	/*
-	 * Measured battery voltage in mV
-	 */
-//	uint16_t	battery_mv;
-
-	/*
-	 * ADC IN5 measurement
-	 */
-//	uint16_t	adc_in5;
-
-	/*
-	 * Power supply overcurrent status bits.
-	 */
-//	uint8_t		overcurrent;
-
 };
 
 extern struct sys_state_s system_state;
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
index 59684f1ee..0dd8fe28d 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -42,6 +42,8 @@
 #include <stdbool.h>
 #include <stdlib.h>
 
+#include <drivers/drv_hrt.h>
+
 #include "px4io.h"
 #include "protocol.h"
 
@@ -140,6 +142,8 @@ uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
 void
 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
 {
+	system_state.fmu_data_received_time = hrt_absolute_time();
+
 	switch (page) {
 
 		/* handle bulk controls input */
@@ -157,6 +161,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		}
 
 		/* XXX we should cause a mixer tick ASAP */
+		system_state.fmu_data_received_time = hrt_absolute_time();
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
 		break;
 
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index a14051f76..540636e19 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -151,7 +151,7 @@ safety_check_button(void *arg)
 	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
 		pattern = LED_PATTERN_FMU_ARMED;
 
-	} else if (system_state.vector_flight_mode_ok) {
+	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) {
 		pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
 	}