Enabled manual override switch, work in progress. Added initial demix testing code to support delta mixing on the remote for convenient manual override

5 files changed, 49 insertions, 14 deletions

diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 40ea38cf7..f4eddbdd3 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -177,13 +177,13 @@ comms_handle_command(const void *buffer, size_t length)
 	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
 		system_state.fmu_channel_data[i] = cmd->servo_command[i];
 
-	/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
+	/* if the IO is armed and FMU gets disarmed, IO must also disarm */
 	if(system_state.arm_ok && !cmd->arm_ok) {
 		system_state.armed = false;
 	}
 
 	system_state.arm_ok = cmd->arm_ok;
-	system_state.mixer_use_fmu = true;
+	system_state.mixer_fmu_available = true;
 	system_state.fmu_data_received = true;
 
 
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index d4eace3df..0e0210373 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -59,6 +59,10 @@
 #define DEBUG
 #include "px4io.h"
 
+#define RC_FAILSAFE_TIMEOUT		2000000		/**< two seconds failsafe timeout */
+#define RC_CHANNEL_HIGH_THRESH		1600
+#define RC_CHANNEL_LOW_THRESH		1400
+
 void
 controls_main(void)
 {
@@ -80,7 +84,22 @@ controls_main(void)
 		if (fds[1].revents & POLLIN)
 			sbus_input();
 
-		/* XXX do ppm processing, bypass mode, etc. here */
+		/* force manual input override */
+		if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) {
+			system_state.mixer_use_fmu = false;
+		} else {
+			/* override not engaged, use FMU */
+			system_state.mixer_use_fmu = true;
+		}
+
+		/* detect rc loss event */
+		if (hrt_absolute_time() - system_state.rc_channels_timestamp > RC_FAILSAFE_TIMEOUT) {
+			system_state.rc_lost = true;
+		}
+
+		/* XXX detect fmu loss event */
+
+		/* XXX handle failsave events - RC loss and FMU loss - here */
 
 		/* do PWM output updates */
 		mixer_tick();
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index 483e9fe4d..d9626c1e3 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -97,7 +97,7 @@ mixer_tick(void)
 	/*
 	 * Decide which set of inputs we're using.
 	 */
-	if (system_state.mixer_use_fmu) {
+	if (system_state.mixer_use_fmu && system_state.mixer_fmu_available) {
 		/* we have recent control data from the FMU */
 		control_count = PX4IO_OUTPUT_CHANNELS;
 		control_values = &system_state.fmu_channel_data[0];
@@ -141,9 +141,10 @@ mixer_tick(void)
 
 	/*
 	 * Decide whether the servos should be armed right now.
+	 * A sufficient reason is armed state and either FMU or RC control inputs
 	 */
 
-	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0) && system_state.mixer_use_fmu;
+	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 483b9bcc8..9bfe3b1e4 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -76,43 +76,58 @@ struct sys_state_s
 	bool		dsm_input_ok;	/* valid Spektrum DSM data */
 	bool		sbus_input_ok;	/* valid Futaba S.Bus data */
 
-	/*
+	/**
 	 * Data from the remote control input(s)
 	 */
 	int		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_OUTPUT_CHANNELS];
 
-	/*
+	/**
 	 * Relay controls
 	 */
 	bool		relays[PX4IO_RELAY_CHANNELS];
 
-	/*
-	 * If true, we are using the FMU controls.
+	/**
+	 * If true, we are using the FMU controls, else RC input if available.
 	 */
 	bool		mixer_use_fmu;
 
-	/*
+	/**
+	 * 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;
 
-	/*
+	/**
 	 * If true, new control data from the FMU has been received.
 	 */
 	bool		fmu_data_received;
 
-	/*
+	/**
 	 * Current serial interface mode, per the serial_rx_mode parameter
 	 * in the config packet.
 	 */
 	uint8_t		serial_rx_mode;
+
+	/**
+	 * 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;
 };
 
 extern struct sys_state_s system_state;
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index d5bd103c1..916988af1 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -58,7 +58,7 @@ static struct hrt_call failsafe_call;
  * Count the number of times in a row that we see the arming button
  * held down.
  */
-static unsigned counter;
+static unsigned counter = 0;
 
 #define ARM_COUNTER_THRESHOLD	10
 #define DISARM_COUNTER_THRESHOLD	2