diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c
index 3af467fac3..bfe33ee8a7 100644
--- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c
+++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c
@@ -104,10 +104,10 @@ int up_pwm_servo_init(uint32_t channel_mask)
 	return OK;
 }
 
-void up_pwm_servo_deinit(void)
+void up_pwm_servo_deinit(uint32_t channel_mask)
 {
 	/* disable the timers */
-	up_pwm_servo_arm(false);
+	up_pwm_servo_arm(false, channel_mask);
 }
 
 int up_pwm_servo_set_rate_group_update(unsigned channel, unsigned rate)
@@ -154,8 +154,8 @@ uint32_t up_pwm_servo_get_rate_group(unsigned group)
 }
 
 void
-up_pwm_servo_arm(bool armed)
+up_pwm_servo_arm(bool armed, uint32_t channel_mask)
 {
-	io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS);
-	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
+	io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask);
+	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask);
 }
diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c
index 9534f51df3..d44ee5457c 100644
--- a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c
+++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c
@@ -105,10 +105,10 @@ int up_pwm_servo_init(uint32_t channel_mask)
 	return OK;
 }
 
-void up_pwm_servo_deinit(void)
+void up_pwm_servo_deinit(uint32_t channel_mask)
 {
 	/* disable the timers */
-	up_pwm_servo_arm(false);
+	up_pwm_servo_arm(false, channel_mask);
 }
 
 int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
@@ -155,8 +155,8 @@ uint32_t up_pwm_servo_get_rate_group(unsigned group)
 }
 
 void
-up_pwm_servo_arm(bool armed)
+up_pwm_servo_arm(bool armed, uint32_t channel_mask)
 {
-	io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS);
-	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
+	io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask);
+	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask);
 }
diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c
index 1f5a2c6678..c46b74161c 100644
--- a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c
+++ b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c
@@ -102,10 +102,10 @@ int up_pwm_servo_init(uint32_t channel_mask)
 	return OK;
 }
 
-void up_pwm_servo_deinit(void)
+void up_pwm_servo_deinit(uint32_t channel_mask)
 {
 	/* disable the timers */
-	up_pwm_servo_arm(false);
+	up_pwm_servo_arm(false, channel_mask);
 }
 
 int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
@@ -152,8 +152,8 @@ uint32_t up_pwm_servo_get_rate_group(unsigned group)
 }
 
 void
-up_pwm_servo_arm(bool armed)
+up_pwm_servo_arm(bool armed, uint32_t channel_mask)
 {
-	io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS);
-	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
+	io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask);
+	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask);
 }
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c
index aa929e6e7e..fa5304a15a 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c
+++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c
@@ -109,10 +109,10 @@ int up_pwm_servo_init(uint32_t channel_mask)
 	return OK;
 }
 
-void up_pwm_servo_deinit(void)
+void up_pwm_servo_deinit(uint32_t channel_mask)
 {
 	/* disable the timers */
-	up_pwm_servo_arm(false);
+	up_pwm_servo_arm(false, channel_mask);
 }
 
 int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
@@ -158,8 +158,8 @@ uint32_t up_pwm_servo_get_rate_group(unsigned group)
 }
 
 void
-up_pwm_servo_arm(bool armed)
+up_pwm_servo_arm(bool armed, uint32_t channel_mask)
 {
-	io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS);
-	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
+	io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask);
+	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask);
 }
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 821390e435..e5145b82f9 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -317,8 +317,14 @@ __EXPORT extern int	up_pwm_servo_init(uint32_t channel_mask);
 
 /**
  * De-initialise the PWM servo outputs.
+ *
+ * @param channel_mask  Bitmask of channels (LSB = channel 0) to enable.
+ *      This allows some of the channels to remain configured
+ *      as GPIOs or as another function.
+ *      A value of 0 is ALL channels
+ *
  */
-__EXPORT extern void	up_pwm_servo_deinit(void);
+__EXPORT extern void	up_pwm_servo_deinit(uint32_t channel_mask);
 
 /**
  * Arm or disarm servo outputs.
@@ -330,8 +336,14 @@ __EXPORT extern void	up_pwm_servo_deinit(void);
  *
  * @param armed		If true, outputs are armed; if false they
  *			are disarmed.
+ *
+ * @param channel_mask  Bitmask of channels (LSB = channel 0) to enable.
+ *      This allows some of the channels to remain configured
+ *      as GPIOs or as another function.
+ *      A value of 0 is ALL channels
+ *
  */
-__EXPORT extern void	up_pwm_servo_arm(bool armed);
+__EXPORT extern void	up_pwm_servo_arm(bool armed, uint32_t channel_mask);
 
 /**
  * Set the servo update rate for all rate groups.
diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp
index 4caf38ed07..f01de16c86 100644
--- a/src/drivers/pwm_out/PWMOut.cpp
+++ b/src/drivers/pwm_out/PWMOut.cpp
@@ -69,7 +69,7 @@ PWMOut::PWMOut(int instance, uint8_t output_base) :
 PWMOut::~PWMOut()
 {
 	/* make sure servos are off */
-	up_pwm_servo_deinit(); // TODO: review for multi
+	up_pwm_servo_deinit(_pwm_mask);
 
 	/* clean up the alternate device node */
 	unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
@@ -331,18 +331,20 @@ int PWMOut::set_mode(Mode mode)
 		_num_outputs = 0;
 		_mixing_output.setMaxNumOutputs(_num_outputs);
 		update_params();
-
-		if (old_mask != _pwm_mask) {
-			/* disable servo outputs - no need to set rates */
-			up_pwm_servo_deinit(); // TODO: review for multi
-		}
-
 		break;
 
 	default:
 		return -EINVAL;
 	}
 
+	if (old_mask != _pwm_mask) {
+		/* disable servo outputs - no need to set rates */
+		if (old_mask != 0) {
+			up_pwm_servo_deinit(old_mask);
+			_pwm_on = false;
+		}
+	}
+
 	_mode = mode;
 	return OK;
 }
@@ -563,7 +565,7 @@ void PWMOut::update_pwm_out_state(bool on)
 		_pwm_initialized = true;
 	}
 
-	up_pwm_servo_arm(on);
+	up_pwm_servo_arm(on, _pwm_mask);
 }
 
 bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 43e35efded..6eefd229f6 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -353,14 +353,14 @@ mixer_tick()
 
 	if (needs_to_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
-		up_pwm_servo_arm(true);
+		up_pwm_servo_arm(true, 0);
 		mixer_servos_armed = true;
 		atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
 		isr_debug(5, "> PWM enabled");
 
 	} else if (!needs_to_arm && mixer_servos_armed) {
 		/* armed but need to disarm */
-		up_pwm_servo_arm(false);
+		up_pwm_servo_arm(false, 0);
 		mixer_servos_armed = false;
 		atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED));
 		isr_debug(5, "> PWM disabled");