Exported disarmed PWM values as IOCTLs

2 files changed, 75 insertions, 47 deletions

diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index ec9d4ca09..6ed9320cb 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -79,6 +79,7 @@ typedef uint16_t	servo_position_t;
 struct pwm_output_values {
 	/** desired pulse widths for each of the supported channels */
 	servo_position_t	values[PWM_OUTPUT_MAX_CHANNELS];
+	int			channel_count;
 };
 
 /*
@@ -118,9 +119,18 @@ ORB_DECLARE(output_pwm);
 /** start DSM bind */
 #define DSM_BIND_START	_IOC(_PWM_SERVO_BASE, 7)
 
-/** Power up DSM receiver */
+/** power up DSM receiver */
 #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8)
 
+/** set the PWM value when disarmed - should be no PWM (zero) by default */
+#define PWM_SERVO_SET_DISARMED_PWM	_IOC(_PWM_SERVO_BASE, 9)
+
+/** set the minimum PWM value the output will send */
+#define PWM_SERVO_SET_MIN_PWM	_IOC(_PWM_SERVO_BASE, 10)
+
+/** set the maximum PWM value the output will send */
+#define PWM_SERVO_SET_MAX_PWM	_IOC(_PWM_SERVO_BASE, 11)
+
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
 
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index c88abe59a..bd5f33043 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -178,21 +178,6 @@ public:
 	int			set_failsafe_values(const uint16_t *vals, unsigned len);
 
 	/**
-	 * Set the minimum PWM signals when armed
-	 */
-	int 			set_min_values(const uint16_t *vals, unsigned len);
-
-	/**
-	 * Set the maximum PWM signal when armed
-	 */
-	int 			set_max_values(const uint16_t *vals, unsigned len);
-
-	/**
-	 * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE
-	 */
-	int 			set_idle_values(const uint16_t *vals, unsigned len);
-
-	/**
 	 * Print IO status.
 	 *
 	 * Print all relevant IO status information
@@ -392,6 +377,21 @@ private:
 	int			mixer_send(const char *buf, unsigned buflen);
 
 	/**
+	 * Set the minimum PWM signals when armed
+	 */
+	int 			set_min_values(const uint16_t *vals, unsigned len);
+
+	/**
+	 * Set the maximum PWM signal when armed
+	 */
+	int 			set_max_values(const uint16_t *vals, unsigned len);
+
+	/**
+	 * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE
+	 */
+	int 			set_idle_values(const uint16_t *vals, unsigned len);
+
+	/**
 	 * Handle a status update from IO.
 	 *
 	 * Publish IO status information if necessary.
@@ -1674,6 +1674,24 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		break;
 	}
 
+	case PWM_SERVO_SET_DISARMED_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		set_idle_values(pwm->values, pwm->channel_count);
+	}
+		break;
+
+	case PWM_SERVO_SET_MIN_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		set_min_values(pwm->values, pwm->channel_count);
+	}
+		break;
+
+	case PWM_SERVO_SET_MAX_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		set_max_values(pwm->values, pwm->channel_count);
+	}
+		break;
+
 	case PWM_SERVO_GET_COUNT:
 		*(unsigned *)arg = _max_actuators;
 		break;
@@ -2265,26 +2283,26 @@ px4io_main(int argc, char *argv[])
 			errx(1, "min command needs at least one channel value (PWM)");
 		}
 
-		if (g_dev != nullptr) {
+		int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0);
+		struct pwm_output_values pwm;
 
-			/* set values for first 8 channels, fill unassigned channels with 900. */
-			uint16_t min[8];
+		if (iofd > 0) {
 
-			for (unsigned i = 0; i < sizeof(min) / sizeof(min[0]); i++)
+			pwm.channel_count = 0;
+
+			for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++)
 			{
 				/* set channel to commanline argument or to 900 for non-provided channels */
 				if (argc > i + 2) {
-					min[i] = atoi(argv[i+2]);
-					if (min[i] < 900 || min[i] > 1200) {
+					pwm.values[i] = atoi(argv[i+2]);
+					if (pwm.values[i] < 900 || pwm.values[i] > 1200) {
 						errx(1, "value out of range of 900 < value < 1200. Aborting.");
 					}
-				} else {
-					/* a zero value will the default */
-					min[i] = 0;
+					pwm.channel_count++;
 				}
 			}
 
-			int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0]));
+			int ret = ioctl(iofd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm);
 
 			if (ret != OK)
 				errx(ret, "failed setting min values");
@@ -2300,26 +2318,26 @@ px4io_main(int argc, char *argv[])
 			errx(1, "max command needs at least one channel value (PWM)");
 		}
 
-		if (g_dev != nullptr) {
+		int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0);
+		struct pwm_output_values pwm;
+
+		if (iofd > 0) {
 
-			/* set values for first 8 channels, fill unassigned channels with 2100. */
-			uint16_t max[8];
+			pwm.channel_count = 0;
 
-			for (unsigned i = 0; i < sizeof(max) / sizeof(max[0]); i++)
+			for (int i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++)
 			{
 				/* set channel to commanline argument or to 2100 for non-provided channels */
 				if (argc > i + 2) {
-					max[i] = atoi(argv[i+2]);
-					if (max[i] < 1800 || max[i] > 2100) {
+					pwm.values[i] = atoi(argv[i+2]);
+					if (pwm.values[i] < 1800 || pwm.values[i] > 2100) {
 						errx(1, "value out of range of 1800 < value < 2100. Aborting.");
 					}
-				} else {
-					/* a zero value will the default */
-					max[i] = 0;
+					pwm.channel_count++;
 				}
 			}
 
-			int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0]));
+			int ret = ioctl(iofd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm);
 
 			if (ret != OK)
 				errx(ret, "failed setting max values");
@@ -2329,32 +2347,32 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "idle")) {
+	if (!strcmp(argv[1], "idle") || !strcmp(argv[1], "disarmed")) {
 
 		if (argc < 3) {
 			errx(1, "max command needs at least one channel value (PWM)");
 		}
 
-		if (g_dev != nullptr) {
+		int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0);
+		struct pwm_output_values pwm;
+
+		if (iofd > 0) {
 
-			/* set values for first 8 channels, fill unassigned channels with 0. */
-			uint16_t idle[8];
+			pwm.channel_count = 0;
 
-			for (unsigned i = 0; i < sizeof(idle) / sizeof(idle[0]); i++)
+			for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++)
 			{
 				/* set channel to commanline argument or to 0 for non-provided channels */
 				if (argc > i + 2) {
-					idle[i] = atoi(argv[i+2]);
-					if (idle[i] < 900 || idle[i] > 2100) {
+					pwm.values[i] = atoi(argv[i+2]);
+					if (pwm.values[i] < 900 || pwm.values[i] > 2100) {
 						errx(1, "value out of range of 900 < value < 2100. Aborting.");
 					}
-				} else {
-					/* a zero value will the default */
-					idle[i] = 0;
+					pwm.channel_count++;
 				}
 			}
 
-			int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0]));
+			int ret = ioctl(iofd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm);
 
 			if (ret != OK)
 				errx(ret, "failed setting idle values");