From 8567134d64f5d8e3c7aba7cebfdf56ffe56b44ed Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 7 Jun 2013 19:41:41 +0200
Subject: [PATCH] Made pwm command sending continously, improved failsafe logic

---
 src/modules/px4iofirmware/mixer.cpp | 16 +++++-----
 src/systemcmds/pwm/pwm.c            | 48 +++++++++++++++++++++++++----
 2 files changed, 49 insertions(+), 15 deletions(-)

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index a2193b526b..123eb67786 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -98,7 +98,7 @@ mixer_tick(void)
 		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
 			isr_debug(1, "AP RX timeout");
 		}
-		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
 
 	} else {
@@ -112,12 +112,11 @@ mixer_tick(void)
 	 * Decide which set of controls we're using.
 	 */
 
-	/* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
+	/* do not mix if RAW_PWM mode is on and FMU is good */
 	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
-	        !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+	        (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
 
-		/* don't actually mix anything - we already have raw PWM values or
-		 not a valid mixer. */
+		/* don't actually mix anything - we already have raw PWM values */
 		source = MIX_NONE;
 
 	} else {
@@ -196,10 +195,9 @@ mixer_tick(void)
 	bool should_arm = (
 	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
 	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
-		/* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
-		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
-		/* FMU is available or FMU is not available but override is an option */
-		((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
+		/* there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) ||
+		/* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) && 
+		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
 	);
 
 	if (should_arm && !mixer_servos_armed) {
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index e150b5a74b..570ca6aa93 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -180,7 +180,8 @@ pwm_main(int argc, char *argv[])
 	}
 
 	/* iterate remaining arguments */
-	unsigned channel = 0;
+	unsigned nchannels = 0;
+	unsigned channel[8] = {0};
 	while (argc--) {
 		const char *arg = argv[0];
 		argv++;
@@ -204,13 +205,15 @@ pwm_main(int argc, char *argv[])
 		}
 		unsigned pwm_value = strtol(arg, &ep, 0);
 		if (*ep == '\0') {
-			ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value);
-			if (ret != OK)
-				err(1, "PWM_SERVO_SET(%d)", channel);
-			channel++;
+			channel[nchannels] = pwm_value;
+			nchannels++;
+
+			if (nchannels >= sizeof(channel) / sizeof(channel[0]))
+				err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0]));
+
 			continue;
 		}
-		usage("unrecognised option");
+		usage("unrecognized option");
 	}
 
 	/* print verbose info */
@@ -250,5 +253,38 @@ pwm_main(int argc, char *argv[])
 		}
 		fflush(stdout);
 	}
+
+	/* perform PWM output */
+	if (nchannels) {
+
+		/* Open console directly to grab CTRL-C signal */
+		int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
+		if (!console)
+			err(1, "failed opening console");
+
+		warnx("Press CTRL-C or 'c' to abort.");
+
+		while (1) {
+			for (int i = 0; i < nchannels; i++) {
+				ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]);
+				if (ret != OK)
+					err(1, "PWM_SERVO_SET(%d)", i);
+			}
+
+			/* abort on user request */
+			char c;
+			if (read(console, &c, 1) == 1) {
+				if (c == 0x03 || c == 0x63) {
+					warnx("User abort\n");
+					close(console);
+					exit(0);
+				}
+			}
+
+			/* rate limit to ~ 20 Hz */
+			usleep(50000);
+		}
+	}
+
 	exit(0);
 }
\ No newline at end of file