Made PX4IO FMU timeout based on IOs HRT, updating mixers now on every FMU update and not at fixed rate, this is WIP and currently does not support mixing with RC-only

4 files changed, 15 insertions, 20 deletions

diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 0fcf952ab..9a786234e 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -183,7 +183,7 @@ comms_handle_command(const void *buffer, size_t length)
 	system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok;
 	system_state.manual_override_ok = cmd->manual_override_ok;
 	system_state.mixer_fmu_available = true;
-	system_state.fmu_data_received = true;
+	system_state.fmu_data_received_time = hrt_absolute_time();
 
 	/* set PWM update rate if changed (after limiting) */
 	uint16_t new_servo_rate = cmd->servo_rate;
@@ -201,6 +201,9 @@ comms_handle_command(const void *buffer, size_t length)
 		system_state.servo_rate = new_servo_rate;
 	}
 
+	/* update servo values immediately */
+	mixer_tick();
+
 	/* XXX do relay changes here */	
 	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
 		system_state.relays[i] = cmd->relay_state[i];
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index 9db9a0fa5..4bd6fe1a0 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -61,8 +61,8 @@
 #include "px4io.h"
 
 #define RC_FAILSAFE_TIMEOUT		2000000		/**< two seconds failsafe timeout */
-#define RC_CHANNEL_HIGH_THRESH		1600
-#define RC_CHANNEL_LOW_THRESH		1400
+#define RC_CHANNEL_HIGH_THRESH		1700
+#define RC_CHANNEL_LOW_THRESH		1300
 
 static void	ppm_input(void);
 
@@ -133,8 +133,7 @@ controls_main(void)
 			system_state.fmu_report_due = true;
 		}
 
-		/* do PWM output updates */
-		mixer_tick();
+		/* PWM output updates are performed directly on each comms update */
 	}
 }
 
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index 5a644906b..318183ef5 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -53,10 +53,9 @@
 #include "px4io.h"
 
 /*
- * Count of periodic calls in which we have no FMU input.
+ * Maximum interval in us before FMU signal is considered lost
  */
-static unsigned fmu_input_drops;
-#define FMU_INPUT_DROP_LIMIT	20
+#define FMU_INPUT_DROP_LIMIT_US		200000
 
 /*
  * Update a mixer based on the current control signals.
@@ -91,17 +90,11 @@ mixer_tick(void)
 		control_values = &system_state.fmu_channel_data[0];
 
 		/* check that we are receiving fresh data from the FMU */
-		if (!system_state.fmu_data_received) {
-			fmu_input_drops++;
+		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 */
-			if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
-				system_state.mixer_manual_override = true;
-				system_state.mixer_fmu_available = false;
-			}
-		} else {
-			fmu_input_drops = 0;
-			system_state.fmu_data_received = false;
+			system_state.mixer_manual_override = true;
+			system_state.mixer_fmu_available = false;
 		}
 
 	} else if (system_state.rc_channels > 0 && system_state.manual_override_ok) {
@@ -109,7 +102,7 @@ mixer_tick(void)
 		control_count = system_state.rc_channels;
 		control_values = &system_state.rc_channel_data[0];
 	} else {
-		/* we have no control input */
+		/* we have no control input (no FMU, no RC) */
 
 		// XXX builtin failsafe would activate here 
 		control_count = 0;
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 84f5ba029..c8699c6c6 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -106,9 +106,9 @@ struct sys_state_s
 	bool		fmu_report_due;
 
 	/**
-	 * If true, new control data from the FMU has been received.
+	 * Last FMU receive time, in microseconds since system boot
 	 */
-	bool		fmu_data_received;
+	uint64_t	fmu_data_received_time;
 
 	/**
 	 * Current serial interface mode, per the serial_rx_mode parameter