Merge remote-tracking branch 'upstream/termination_failsafe' into obcfailsafe

src/drivers/drv_pwm_output.h

@@ -205,10 +205,13 @@ ORB_DECLARE(output_pwm);
 #define PWM_SERVO_GET_DISABLE_LOCKDOWN		_IOC(_PWM_SERVO_BASE, 22)
 
 /** force safety switch off (to disable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
+#define PWM_SERVO_SET_FORCE_SAFETY_OFF		_IOC(_PWM_SERVO_BASE, 23)
 
 /** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */
-#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24)
+#define PWM_SERVO_SET_FORCE_FAILSAFE		_IOC(_PWM_SERVO_BASE, 24)
+
+/** make failsafe non-recoverable (termination) if it occurs */
+#define PWM_SERVO_SET_TERMINATION_FAILSAFE	_IOC(_PWM_SERVO_BASE, 25)
 
 /*
  *

src/drivers/px4io/px4io.cpp

@@ -1176,6 +1176,14 @@ PX4IO::io_set_arming_state()
 		clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
 	}
 
+	// XXX this is for future support in the commander
+	// but can be removed if unneeded
+	// if (armed.termination_failsafe) {
+	// 	set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
+	// } else {
+	// 	clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
+	// }
+
 	if (armed.ready_to_arm) {
 		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 
@@ -2039,7 +2047,8 @@ PX4IO::print_status(bool extended_status)
 	       ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)	? " INAIR_RESTART_OK" : ""),
 	       ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)	? " ALWAYS_PWM_ENABLE" : ""),
 	       ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)		? " LOCKDOWN" : ""),
-	       ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)		? " FORCE_FAILSAFE" : "")
+	       ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)		? " FORCE_FAILSAFE" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "")
 	       );
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
@@ -2263,6 +2272,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		}
 		break;
 
+	case PWM_SERVO_SET_TERMINATION_FAILSAFE:
+		/* if failsafe occurs, do not allow the system to recover */
+		if (arg == 0) {
+			/* clear termination failsafe flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, 0);
+		} else {
+			/* set termination failsafe flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);
+		}
+		break;
+
 	case DSM_BIND_START:
 
 		/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */

src/modules/px4iofirmware/mixer.cpp

@@ -139,7 +139,9 @@ mixer_tick(void)
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
 		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
-		     !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
+		     !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
+		     /* do not enter manual override if we asked for termination failsafe and FMU is lost */
+		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) {
 
 		 	/* if allowed, mix from RC inputs directly */
 			source = MIX_OVERRIDE;
@@ -154,6 +156,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
 	 */

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 */

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;

src/systemcmds/pwm/pwm.c

@@ -654,9 +654,28 @@ pwm_main(int argc, char *argv[])
 			}
 		}
 		exit(0);
+	} else if (!strcmp(argv[1], "terminatefail")) {
+
+		if (argc < 3) {
+			errx(1, "arg missing [on|off]");
+		} else {
+
+			if (!strcmp(argv[2], "on")) {
+				/* force failsafe */
+				ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 1);
+			} else {
+				/* force failsafe */
+				ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 0);
+			}
+
+			if (ret != OK) {
+				warnx("FAILED setting termination failsafe %s", argv[2]);
+			}
+		}
+		exit(0);
 	}
 
-	usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info|forcefail");
+	usage("specify arm|disarm|rate|failsafe\n\t\tdisarmed|min|max|test|info|forcefail|terminatefail");
 	return 0;
 }