From 4524fe3e4888d569f855d1e7a82c8d5116636a0a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 20 Jan 2014 16:20:43 +1100
Subject: [PATCH] px4fmu: added PWM_SERVO_SET_COUNT API

this allows the balance between PWM channels and GPIOs to be changed
after the main flight code has started, which makes it possible to
change the balance with a parameter in APM
---
 src/drivers/drv_pwm_output.h |  4 ++++
 src/drivers/px4fmu/fmu.cpp   | 35 ++++++++++++++++++++++++++++++++++-
 2 files changed, 38 insertions(+), 1 deletion(-)

diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 51f916f37b..88da94b1ed 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -189,6 +189,10 @@ ORB_DECLARE(output_pwm);
 /** get the maximum PWM value the output will send */
 #define PWM_SERVO_GET_MAX_PWM	_IOC(_PWM_SERVO_BASE, 19)
 
+/** set the number of servos in (unsigned)arg - allows change of
+ * split between servos and GPIO */
+#define PWM_SERVO_SET_COUNT	_IOC(_PWM_SERVO_BASE, 20)
+
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
 
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 4b1b0ed0bb..c067d363b7 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1006,6 +1006,40 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 		break;
 
+	case PWM_SERVO_SET_COUNT: {
+		/* change the number of outputs that are enabled for
+		 * PWM. This is used to change the split between GPIO
+		 * and PWM under control of the flight config
+		 * parameters. Note that this does not allow for
+		 * changing a set of pins to be used for serial on
+		 * FMUv1 
+		 */
+		switch (arg) {
+		case 0:
+			set_mode(MODE_NONE);
+			break;
+
+		case 2:
+			set_mode(MODE_2PWM);
+			break;
+
+		case 4:
+			set_mode(MODE_4PWM);
+			break;
+
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+		case 6:
+			set_mode(MODE_6PWM);
+			break;
+#endif
+
+		default:
+			ret = -EINVAL;
+			break;
+		}
+		break;
+	}
+
 	case MIXERIOCRESET:
 		if (_mixers != nullptr) {
 			delete _mixers;
@@ -1443,7 +1477,6 @@ void
 sensor_reset(int ms)
 {
 	int	 fd;
-	int	 ret;
 
 	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);