IO firmware supports termination failsafe

3 files changed, 28 insertions, 3 deletions

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 606c639f9..1eacda97a 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -155,6 +155,16 @@ mixer_tick(void)
 	}
 
 	/*
+	 * Check if failsafe termination is set - if yes,
+	 * set the force failsafe flag once entering the first
+	 * failsafe condition.
+	 */
+	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) &&
+		(source == MIX_FAILSAFE)) {
+		r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
+	}
+
+	/*
 	 * Check if we should force failsafe - and do it if we have to
 	 */
 	if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 050783687..eae7f9567 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -180,6 +180,7 @@
 #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 6) /* Disable the IO-internal evaluation of the RC */
 #define PX4IO_P_SETUP_ARMING_LOCKDOWN		(1 << 7) /* If set, the system operates normally, but won't actuate any servos */
 #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE	(1 << 8) /* If set, the system will always output the failsafe values */
+#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE	(1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 0da778b6f..7a5a5e484 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -190,7 +190,8 @@ volatile uint16_t	r_page_setup[] =
 					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
 					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
 					 PX4IO_P_SETUP_ARMING_LOCKDOWN | \
-					 PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)
+					 PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
+					 PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -518,6 +519,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 			}
 
+			/*
+			 * If the failsafe termination flag is set, do not allow the autopilot to unset it
+			 */
+			value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);
+
+			/*
+			 * If failsafe termination is enabled and force failsafe bit is set, do not allow
+			 * the autopilot to clear it.
+			 */
+			if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) {
+				value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
+			}
+
 			r_setup_arming = value;
 
 			break;