From 434de4e949c893f53b0d7a3feb1c2588542cd9af Mon Sep 17 00:00:00 2001
From: Thiago0B <thiago0b12@hotmail.com>
Date: Mon, 11 Nov 2013 22:02:40 -0200
Subject: Fix user abort behave in test

Now the pwm ouput return to the last value before test (useful and safer when testing ESCs).
---
 src/systemcmds/pwm/pwm.c | 19 +++++++++++++++++++
 1 file changed, 19 insertions(+)

diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index 09a934400..898c04cd5 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -395,6 +395,17 @@ pwm_main(int argc, char *argv[])
 		if (pwm_value == 0)
 			usage("no PWM value provided");
 
+		/* get current servo values */
+		struct pwm_output_values last_spos;
+
+		for (unsigned i = 0; i < servo_count; i++) {
+
+
+			ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]);
+			if (ret != OK)
+				err(1, "PWM_SERVO_GET(%d)", i);
+		}
+
 		/* perform PWM output */
 
 		/* Open console directly to grab CTRL-C signal */
@@ -420,6 +431,14 @@ pwm_main(int argc, char *argv[])
 
 			read(0, &c, 1);
 				if (c == 0x03 || c == 0x63 || c == 'q') {
+					/* reset output to the last value */
+					for (unsigned i = 0; i < servo_count; i++) {
+									if (set_mask & 1<<i) {
+										ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
+										if (ret != OK)
+											err(1, "PWM_SERVO_SET(%d)", i);
+									}
+								}
 					warnx("User abort\n");
 					exit(0);
 				}
-- 
cgit v1.2.3