merged

1 files changed, 41 insertions, 28 deletions

diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index 0fd4ac717..b8ea308f3 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -48,7 +48,11 @@
 #include <unistd.h>
 #include <fcntl.h>
 
+#include <debug.h>
+
 #include <drivers/drv_pwm_output.h>
+#include <drivers/drv_hrt.h>
+
 #include <systemlib/mixer/mixer.h>
 
 extern "C" {
@@ -56,10 +60,9 @@ extern "C" {
 }
 
 /*
- * 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
 
 /* current servo arm/disarm state */
 bool mixer_servos_armed = false;
@@ -80,37 +83,45 @@ mixer_tick(void)
 {
 	bool should_arm;
 
+	/* 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 */
+		system_state.mixer_manual_override = true;
+		system_state.mixer_fmu_available = false;
+		lib_lowprintf("\nRX timeout\n");
+	}
+
 	/*
 	 * Decide which set of inputs we're using.
 	 */
-	if (system_state.mixer_use_fmu) {
-		/* we have recent control data from the FMU */
-		control_count = PX4IO_OUTPUT_CHANNELS;
-		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++;
-
-			/* too many frames without FMU input, time to go to failsafe */
-			if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
-				system_state.mixer_use_fmu = false;
-			}
-
+	/* this is for planes, where manual override makes sense */
+	if(system_state.manual_override_ok) {
+		/* if everything is ok */
+		if (!system_state.mixer_manual_override && 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];
+		/* when override is on or the fmu is not available */
+		} else if (system_state.rc_channels > 0) {
+			control_count = system_state.rc_channels;
+			control_values = &system_state.rc_channel_data[0];
 		} else {
-			fmu_input_drops = 0;
-			system_state.fmu_data_received = false;
-		}
+			/* we have no control input (no FMU, no RC) */
 
-	} else if (system_state.rc_channels > 0) {
-		/* we have control data from an R/C input */
-		control_count = system_state.rc_channels;
-		control_values = &system_state.rc_channel_data[0];
+			// XXX builtin failsafe would activate here
+			control_count = 0;
+		}
 
+	/* this is for multicopters, etc. where manual override does not make sense */
 	} else {
-		/* we have no control input */
-		/* XXX builtin failsafe would activate here */
-		control_count = 0;
+		/* if the fmu is available whe are good */
+		if(system_state.mixer_fmu_available) {
+			control_count = PX4IO_OUTPUT_CHANNELS;
+			control_values = &system_state.fmu_channel_data[0];
+		/* we better shut everything off */
+		} else {
+			control_count = 0;
+		}
 	}
 
 	/*
@@ -144,7 +155,9 @@ 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);
 
 	if (should_arm && !mixer_servos_armed) {
@@ -216,4 +229,4 @@ mixer_handle_text(const void *buffer, size_t length)
 
 		break;
 	}
-}
\ No newline at end of file
+}