Merge pull request #224 from PX4/pwm-multirate

Pwm multirate

11 files changed, 696 insertions, 247 deletions

diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h
index f3f753ebe..07831f20c 100644
--- a/apps/drivers/drv_pwm_output.h
+++ b/apps/drivers/drv_pwm_output.h
@@ -36,7 +36,7 @@
  *
  * Servo values can be set with the PWM_SERVO_SET ioctl, by writing a
  * pwm_output_values structure to the device, or by publishing to the
- * output_pwm ObjDev.
+ * output_pwm ORB topic.
  * Writing a value of 0 to a channel suppresses any output for that
  * channel.
  */
@@ -60,7 +60,7 @@ __BEGIN_DECLS
 #define PWM_OUTPUT_DEVICE_PATH	"/dev/pwm_output"
 
 /**
- * Maximum number of PWM output channels in the system.
+ * Maximum number of PWM output channels supported by the device.
  */
 #define PWM_OUTPUT_MAX_CHANNELS	16
 
@@ -82,14 +82,14 @@ struct pwm_output_values {
 };
 
 /*
- * ObjDev tag for PWM outputs.
+ * ORB tag for PWM outputs.
  */
 ORB_DECLARE(output_pwm);
 
 /*
  * ioctl() definitions
  *
- * Note that ioctls and ObjDev updates should not be mixed, as the
+ * Note that ioctls and ORB updates should not be mixed, as the
  * behaviour of the system in this case is not defined.
  */
 #define _PWM_SERVO_BASE		0x2a00
@@ -100,20 +100,14 @@ ORB_DECLARE(output_pwm);
 /** disarm all servo outputs (stop generating pulses) */
 #define PWM_SERVO_DISARM	_IOC(_PWM_SERVO_BASE, 1)
 
-/** set update rate in Hz */
-#define PWM_SERVO_SET_UPDATE_RATE	_IOC(_PWM_SERVO_BASE, 2)
+/** set alternate servo update rate */
+#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
 
 /** get the number of servos in *(unsigned *)arg */
 #define PWM_SERVO_GET_COUNT	_IOC(_PWM_SERVO_BASE, 3)
 
-/** set debug level for servo IO */
-#define PWM_SERVO_SET_DEBUG	_IOC(_PWM_SERVO_BASE, 4)
-
-/** enable in-air restart */
-#define PWM_SERVO_INAIR_RESTART_ENABLE	_IOC(_PWM_SERVO_BASE, 5)
-
-/** disable in-air restart */
-#define PWM_SERVO_INAIR_RESTART_DISABLE	_IOC(_PWM_SERVO_BASE, 6)
+/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
+#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
 
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
@@ -121,6 +115,10 @@ ORB_DECLARE(output_pwm);
 /** get a single specific servo value */
 #define PWM_SERVO_GET(_servo)	_IOC(_PWM_SERVO_BASE, 0x40 + _servo)
 
+/** get the _n'th rate group's channels; *(uint32_t *)arg returns a bitmap of channels
+ *  whose update rates must be the same.
+ */
+#define PWM_SERVO_GET_RATEGROUP(_n) _IOC(_PWM_SERVO_BASE, 0x60 + _n)
 
 /*
  * Low-level PWM output interface.
@@ -157,7 +155,7 @@ __EXPORT extern void	up_pwm_servo_deinit(void);
 __EXPORT extern void	up_pwm_servo_arm(bool armed);
 
 /**
- * Set the servo update rate
+ * Set the servo update rate for all rate groups.
  *
  * @param rate		The update rate in Hz to set.
  * @return		OK on success, -ERANGE if an unsupported update rate is set.
@@ -165,6 +163,25 @@ __EXPORT extern void	up_pwm_servo_arm(bool armed);
 __EXPORT extern int	up_pwm_servo_set_rate(unsigned rate);
 
 /**
+ * Get a bitmap of output channels assigned to a given rate group.
+ *
+ * @param group		The rate group to query. Rate groups are assigned contiguously
+ *			starting from zero.
+ * @return		A bitmap of channels assigned to the rate group, or zero if
+ *			the group number has no channels.
+ */
+__EXPORT extern uint32_t up_pwm_servo_get_rate_group(unsigned group);
+
+/**
+ * Set the update rate for a given rate group.
+ *
+ * @param group		The rate group whose update rate will be changed.
+ * @param rate		The update rate in Hz.
+ * @return		OK if the group was adjusted, -ERANGE if an unsupported update rate is set.
+ */
+__EXPORT extern int	up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate);
+
+/**
  * Set the current output value for a channel.
  *
  * @param channel	The channel to set.
diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp
index 861ed7924..d9aa772d4 100644
--- a/apps/drivers/hil/hil.cpp
+++ b/apps/drivers/hil/hil.cpp
@@ -158,6 +158,7 @@ HIL::HIL() :
 	CDev("hilservo", PWM_OUTPUT_DEVICE_PATH/*"/dev/hil" XXXL*/),
 	_mode(MODE_NONE),
 	_update_rate(50),
+	_current_update_rate(0),
 	_task(-1),
 	_t_actuators(-1),
 	_t_armed(-1),
@@ -511,9 +512,14 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_SET_UPDATE_RATE:
+		// HIL always outputs at the alternate (usually faster) rate 
 		g_hil->set_pwm_rate(arg);
 		break;
 
+	case PWM_SERVO_SELECT_UPDATE_RATE:
+		// HIL always outputs at the alternate (usually faster) rate 
+		break;
+
 	case PWM_SERVO_SET(2):
 	case PWM_SERVO_SET(3):
 		if (_mode != MODE_4PWM) {
@@ -549,6 +555,14 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}
 
+	case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): {
+		// no restrictions on output grouping
+		unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
+
+		*(uint32_t *)arg = (1 << channel);
+		break;
+	}
+
 	case MIXERIOCGETOUTPUTCOUNT:
 		if (_mode == MODE_4PWM) {
 			*(unsigned *)arg = 4;
@@ -641,7 +655,7 @@ enum PortMode {
 PortMode g_port_mode;
 
 int
-hil_new_mode(PortMode new_mode, int update_rate)
+hil_new_mode(PortMode new_mode)
 {
 	// uint32_t gpio_bits;
 	
@@ -699,8 +713,6 @@ hil_new_mode(PortMode new_mode, int update_rate)
 
 	/* (re)set the PWM output mode */
 	g_hil->set_mode(servo_mode);
-	if ((servo_mode != HIL::MODE_NONE) && (update_rate != 0))
-		g_hil->set_pwm_rate(update_rate);
 
 	return OK;
 }
@@ -786,59 +798,34 @@ int
 hil_main(int argc, char *argv[])
 {
 	PortMode new_mode = PORT_MODE_UNDEFINED;
-	int pwm_update_rate_in_hz = 0;
-
-	if (!strcmp(argv[1], "test"))
-		test();
-
-	if (!strcmp(argv[1], "fake"))
-		fake(argc - 1, argv + 1);
+	const char *verb = argv[1];
 
 	if (hil_start() != OK)
-		errx(1, "failed to start the FMU driver");
+		errx(1, "failed to start the HIL driver");
 
 	/*
 	 * Mode switches.
-	 *
-	 * XXX use getopt?
 	 */
-	for (int i = 1; i < argc; i++) {   /* argv[0] is "fmu" */
 
-		if (!strcmp(argv[i], "mode_pwm")) {
-			new_mode = PORT1_FULL_PWM;
+ 	// this was all cut-and-pasted from the FMU driver; it's junk
+	if (!strcmp(verb, "mode_pwm")) {
+		new_mode = PORT1_FULL_PWM;
 
-		} else if (!strcmp(argv[i], "mode_pwm_serial")) {
-			new_mode = PORT1_PWM_AND_SERIAL;
+	} else if (!strcmp(verb, "mode_pwm_serial")) {
+		new_mode = PORT1_PWM_AND_SERIAL;
 
-		} else if (!strcmp(argv[i], "mode_pwm_gpio")) {
-			new_mode = PORT1_PWM_AND_GPIO;
-    
-		} else if (!strcmp(argv[i], "mode_port2_pwm8")) {
-			new_mode = PORT2_8PWM;
+	} else if (!strcmp(verb, "mode_pwm_gpio")) {
+		new_mode = PORT1_PWM_AND_GPIO;
 
-		} else if (!strcmp(argv[i], "mode_port2_pwm12")) {
-			new_mode = PORT2_8PWM;
+	} else if (!strcmp(verb, "mode_port2_pwm8")) {
+		new_mode = PORT2_8PWM;
 
-		} else if (!strcmp(argv[i], "mode_port2_pwm16")) {
-			new_mode = PORT2_8PWM;
-		}
+	} else if (!strcmp(verb, "mode_port2_pwm12")) {
+		new_mode = PORT2_8PWM;
 
-		/* look for the optional pwm update rate for the supported modes */
-		if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) {
-			// if (new_mode == PORT1_FULL_PWM || new_mode == PORT1_PWM_AND_GPIO) {
-			// XXX all modes have PWM settings
-			if (argc > i + 1) {
-				pwm_update_rate_in_hz = atoi(argv[i + 1]);
-				printf("pwm update rate: %d Hz\n", pwm_update_rate_in_hz);
-			} else {
-				fprintf(stderr, "missing argument for pwm update rate (-u)\n");
-				return 1;
-			}
-			// } else {
-			// 	fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n");
-			// }
-		}
-        }
+	} else if (!strcmp(verb, "mode_port2_pwm16")) {
+		new_mode = PORT2_8PWM;
+	}
 
 	/* was a new mode set? */
 	if (new_mode != PORT_MODE_UNDEFINED) {
@@ -848,12 +835,17 @@ hil_main(int argc, char *argv[])
 			return OK;
 
 		/* switch modes */
-		return hil_new_mode(new_mode, pwm_update_rate_in_hz);
+		return hil_new_mode(new_mode);
 	}
 
-	/* test, etc. here */
+	if (!strcmp(verb, "test"))
+		test();
+
+	if (!strcmp(verb, "fake"))
+		fake(argc - 1, argv + 1);
+
 
 	fprintf(stderr, "HIL: unrecognized command, try:\n");
-	fprintf(stderr, "  mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n");
+	fprintf(stderr, "  mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n");
 	return -EINVAL;
 }
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp
index 8e13f7c62..476adb7f0 100644
--- a/apps/drivers/px4fmu/fmu.cpp
+++ b/apps/drivers/px4fmu/fmu.cpp
@@ -90,14 +90,18 @@ public:
 	virtual int	init();
 
 	int		set_mode(Mode mode);
-	int		set_pwm_rate(unsigned rate);
+
+	int		set_pwm_alt_rate(unsigned rate);
+	int		set_pwm_alt_channels(uint32_t channels);
 
 private:
 	static const unsigned _max_actuators = 4;
 
 	Mode		_mode;
-	int 		_update_rate;
-	int 		_current_update_rate;
+	unsigned	_pwm_default_rate;
+	unsigned	_pwm_alt_rate;
+	uint32_t	_pwm_alt_rate_channels;
+	unsigned	_current_update_rate;
 	int		_task;
 	int		_t_actuators;
 	int		_t_armed;
@@ -121,6 +125,7 @@ private:
 					 uint8_t control_index,
 					 float &input);
 
+	int		set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
 	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
 
 	struct GPIOConfig {
@@ -163,7 +168,10 @@ PX4FMU	*g_fmu;
 PX4FMU::PX4FMU() :
 	CDev("fmuservo", "/dev/px4fmu"),
 	_mode(MODE_NONE),
-	_update_rate(50),
+	_pwm_default_rate(50),
+	_pwm_alt_rate(50),
+	_pwm_alt_rate_channels(0),
+	_current_update_rate(0),
 	_task(-1),
 	_t_actuators(-1),
 	_t_armed(-1),
@@ -263,27 +271,31 @@ PX4FMU::set_mode(Mode mode)
 	 * are presented on the output pins.
 	 */
 	switch (mode) {
-	case MODE_2PWM:
-		debug("MODE_2PWM");
-		/* multi-port with flow control lines as PWM */
-		/* XXX magic numbers */
-		up_pwm_servo_init(0x3);
-		_update_rate = 50;	/* default output rate */
-		break;
+	case MODE_2PWM:	// multi-port with flow control lines as PWM
+	case MODE_4PWM: // multi-port as 4 PWM outs
+		debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4);
+		
+		/* default output rates */
+		_pwm_default_rate = 50;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
 
-	case MODE_4PWM:
-		debug("MODE_4PWM");
-		/* multi-port as 4 PWM outs */
 		/* XXX magic numbers */
-		up_pwm_servo_init(0xf);
-		_update_rate = 50;	/* default output rate */
+		up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf);
+		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
 		break;
 
 	case MODE_NONE:
 		debug("MODE_NONE");
-		/* disable servo outputs and set a very low update rate */
+
+		_pwm_default_rate = 10;	/* artificially reduced output rate */
+		_pwm_alt_rate = 10;
+		_pwm_alt_rate_channels = 0;
+
+		/* disable servo outputs - no need to set rates */
 		up_pwm_servo_deinit();
-		_update_rate = 10;
+
 		break;
 
 	default:
@@ -295,15 +307,63 @@ PX4FMU::set_mode(Mode mode)
 }
 
 int
-PX4FMU::set_pwm_rate(unsigned rate)
+PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate)
 {
-	if ((rate > 500) || (rate < 10))
-		return -EINVAL;
+	debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate);
+
+	for (unsigned pass = 0; pass < 2; pass++) {
+		for (unsigned group = 0; group < _max_actuators; group++) {
+
+			// get the channel mask for this rate group
+			uint32_t mask = up_pwm_servo_get_rate_group(group);
+			if (mask == 0)
+				continue;
+
+			// all channels in the group must be either default or alt-rate
+			uint32_t alt = rate_map & mask;
+
+			if (pass == 0) {
+				// preflight
+				if ((alt != 0) && (alt != mask)) {
+					warn("rate group %u mask %x bad overlap %x", group, mask, alt);
+					// not a legal map, bail
+					return -EINVAL;
+				}
+			} else {
+				// set it - errors here are unexpected
+				if (alt != 0) {
+					if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) {
+						warn("rate group set alt failed");
+						return -EINVAL;
+					}
+				} else {
+					if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
+						warn("rate group set default failed");
+						return -EINVAL;
+					}
+				}
+			}
+		}
+	}
+	_pwm_alt_rate_channels = rate_map;
+	_pwm_default_rate = default_rate;
+	_pwm_alt_rate = alt_rate;
 
-	_update_rate = rate;
 	return OK;
 }
 
+int
+PX4FMU::set_pwm_alt_rate(unsigned rate)
+{
+	return set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, rate);
+}
+
+int
+PX4FMU::set_pwm_alt_channels(uint32_t channels)
+{
+	return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate);
+}
+
 void
 PX4FMU::task_main()
 {
@@ -353,24 +413,30 @@ PX4FMU::task_main()
 	/* loop until killed */
 	while (!_task_should_exit) {
 
-		/* handle update rate changes */
-		if (_current_update_rate != _update_rate) {
-			int update_rate_in_ms = int(1000 / _update_rate);
+		/*
+		 * Adjust actuator topic update rate to keep up with
+		 * the highest servo update rate configured.
+		 *
+		 * We always mix at max rate; some channels may update slower.
+		 */
+		unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
+		if (_current_update_rate != max_rate) {
+			_current_update_rate = max_rate;
+			int update_rate_in_ms = int(1000 / _current_update_rate);
 
 			/* reject faster than 500 Hz updates */
 			if (update_rate_in_ms < 2) {
 				update_rate_in_ms = 2;
-				_update_rate = 500;
+				_current_update_rate = 500;
 			}
 			/* reject slower than 50 Hz updates */
 			if (update_rate_in_ms > 20) {
 				update_rate_in_ms = 20;
-				_update_rate = 50;
+				_current_update_rate = 50;
 			}
 
+			debug("adjusted actuator update interval to %ums", update_rate_in_ms);
 			orb_set_interval(_t_actuators, update_rate_in_ms);
-			up_pwm_servo_set_rate(_update_rate);
-			_current_update_rate = _update_rate;
 		}
 
 		/* sleep waiting for data, stopping to check for PPM
@@ -527,7 +593,6 @@ int
 PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret = OK;
-	int channel;
 
 	lock();
 
@@ -541,7 +606,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_SET_UPDATE_RATE:
-		set_pwm_rate(arg);
+		ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
+		break;
+
+	case PWM_SERVO_SELECT_UPDATE_RATE:
+		ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
 		break;
 
 	case PWM_SERVO_SET(2):
@@ -555,9 +624,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_SET(0):
 	case PWM_SERVO_SET(1):
 		if (arg < 2100) {
-			channel = cmd - PWM_SERVO_SET(0);
-			up_pwm_servo_set(channel, arg);
-
+			up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
 		} else {
 			ret = -EINVAL;
 		}
@@ -573,12 +640,18 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 		/* FALLTHROUGH */
 	case PWM_SERVO_GET(0):
-	case PWM_SERVO_GET(1): {
-			channel = cmd - PWM_SERVO_GET(0);
-			*(servo_position_t *)arg = up_pwm_servo_get(channel);
-			break;
-		}
+	case PWM_SERVO_GET(1):
+		*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
+		break;
+
+	case PWM_SERVO_GET_RATEGROUP(0):
+	case PWM_SERVO_GET_RATEGROUP(1):
+	case PWM_SERVO_GET_RATEGROUP(2):
+	case PWM_SERVO_GET_RATEGROUP(3):
+		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
+		break;
 
+	case PWM_SERVO_GET_COUNT:	
 	case MIXERIOCGETOUTPUTCOUNT:
 		if (_mode == MODE_4PWM) {
 			*(unsigned *)arg = 4;
@@ -812,7 +885,7 @@ enum PortMode {
 PortMode g_port_mode;
 
 int
-fmu_new_mode(PortMode new_mode, int update_rate)
+fmu_new_mode(PortMode new_mode)
 {
 	uint32_t gpio_bits;
 	PX4FMU::Mode servo_mode;
@@ -864,9 +937,6 @@ fmu_new_mode(PortMode new_mode, int update_rate)
 	/* (re)set the PWM output mode */
 	g_fmu->set_mode(servo_mode);
 
-	if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0))
-		g_fmu->set_pwm_rate(update_rate);
-
 	return OK;
 }
 
@@ -962,57 +1032,31 @@ int
 fmu_main(int argc, char *argv[])
 {
 	PortMode new_mode = PORT_MODE_UNSET;
-	int pwm_update_rate_in_hz = 0;
-
-	if (!strcmp(argv[1], "test"))
-		test();
-
-	if (!strcmp(argv[1], "fake"))
-		fake(argc - 1, argv + 1);
+	const char *verb = argv[1];
 
 	if (fmu_start() != OK)
 		errx(1, "failed to start the FMU driver");
 
 	/*
 	 * Mode switches.
-	 *
-	 * XXX use getopt?
 	 */
-	for (int i = 1; i < argc; i++) {   /* argv[0] is "fmu" */
-		if (!strcmp(argv[i], "mode_gpio")) {
-			new_mode = PORT_FULL_GPIO;
+	if (!strcmp(verb, "mode_gpio")) {
+		new_mode = PORT_FULL_GPIO;
 
-		} else if (!strcmp(argv[i], "mode_serial")) {
-			new_mode = PORT_FULL_SERIAL;
+	} else if (!strcmp(verb, "mode_serial")) {
+		new_mode = PORT_FULL_SERIAL;
 
-		} else if (!strcmp(argv[i], "mode_pwm")) {
-			new_mode = PORT_FULL_PWM;
+	} else if (!strcmp(verb, "mode_pwm")) {
+		new_mode = PORT_FULL_PWM;
 
-		} else if (!strcmp(argv[i], "mode_gpio_serial")) {
-			new_mode = PORT_GPIO_AND_SERIAL;
+	} else if (!strcmp(verb, "mode_gpio_serial")) {
+		new_mode = PORT_GPIO_AND_SERIAL;
 
-		} else if (!strcmp(argv[i], "mode_pwm_serial")) {
-			new_mode = PORT_PWM_AND_SERIAL;
-
-		} else if (!strcmp(argv[i], "mode_pwm_gpio")) {
-			new_mode = PORT_PWM_AND_GPIO;
-		}
-
-		/* look for the optional pwm update rate for the supported modes */
-		if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) {
-			if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) {
-				if (argc > i + 1) {
-					pwm_update_rate_in_hz = atoi(argv[i + 1]);
-
-				} else {
-					errx(1, "missing argument for pwm update rate (-u)");
-					return 1;
-				}
+	} else if (!strcmp(verb, "mode_pwm_serial")) {
+		new_mode = PORT_PWM_AND_SERIAL;
 
-			} else {
-				errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio");
-			}
-		}
+	} else if (!strcmp(verb, "mode_pwm_gpio")) {
+		new_mode = PORT_PWM_AND_GPIO;
 	}
 
 	/* was a new mode set? */
@@ -1023,12 +1067,17 @@ fmu_main(int argc, char *argv[])
 			return OK;
 
 		/* switch modes */
-		return fmu_new_mode(new_mode, pwm_update_rate_in_hz);
+		int ret = fmu_new_mode(new_mode);
+		exit(ret == OK ? 0 : 1);
 	}
 
-	/* test, etc. here */
+	if (!strcmp(verb, "test"))
+		test();
+
+	if (!strcmp(verb, "fake"))
+		fake(argc - 1, argv + 1);
 
 	fprintf(stderr, "FMU: unrecognised command, try:\n");
-	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
+	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
 	exit(1);
 }
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 27c885ed7..4f6938a14 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -86,6 +86,8 @@
 #include "uploader.h"
 #include <debug.h>
 
+#define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
+#define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
 
 class PX4IO : public device::I2C
 {
@@ -98,6 +100,17 @@ public:
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
 	virtual ssize_t		write(file *filp, const char *buffer, size_t len);
 
+	/**
+	* Set the update rate for actuator outputs from FMU to IO.
+	*
+	* @param rate    The rate in Hz actuator outpus are sent to IO.
+	*      Min 10 Hz, max 400 Hz
+	*/
+	int      set_update_rate(int rate);
+
+	/**
+	* Print the current status of IO
+	*/
 	void			print_status();
 
 private:
@@ -1223,14 +1236,15 @@ PX4IO::print_status()
 		((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
 		((flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  ? " INIT_OK" : " INIT_FAIL"));
 	uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
-	printf("alarms 0x%04x%s%s%s%s%s%s\n",
+	printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
 		alarms,
 		((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW)     ? " VBATT_LOW" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE)   ? " TEMPERATURE" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT)   ? " ACC_CURRENT" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST)      ? " FMU_LOST" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST)       ? " RC_LOST" : ""));
+		((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST)       ? " RC_LOST" : ""),
+		((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR)     ? " PWM_ERROR" : ""));
 	printf("vbatt %u ibatt %u\n",
 		io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
 		io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT));
@@ -1269,10 +1283,10 @@ PX4IO::print_status()
 		((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK)   ? " VECTOR_FLIGHT_OK" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""));
-	printf("rates 0x%04x lowrate %u highrate %u relays 0x%04x\n",
+	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
-		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_LOWRATE),
-		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE),
+		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
+		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
 	printf("vbatt scale %u ibatt scale %u ibatt bias %u\n",
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE),
@@ -1320,36 +1334,39 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0);
 		break;
 
-	case PWM_SERVO_INAIR_RESTART_ENABLE:
-		/* set the 'in-air restart' bit */
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK);
+	case PWM_SERVO_SET_UPDATE_RATE:
+		/* set the requested alternate rate */
+		if ((arg >= 50) && (arg <= 400)) {	/* TODO: we could go higher for e.g. TurboPWM */
+			ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
+		} else {
+			ret = -EINVAL;
+		}
 		break;
 
-	case PWM_SERVO_INAIR_RESTART_DISABLE:
-		/* unset the 'in-air restart' bit */
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0);
-		break;
+	case PWM_SERVO_SELECT_UPDATE_RATE: {
 
-	case PWM_SERVO_SET_UPDATE_RATE:
-		/* set the requested rate */
-		if ((arg >= 50) && (arg <= 400)) {
-			ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE, arg);
-		} else {
+		/* blindly clear the PWM update alarm - might be set for some other reason */
+		io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
+
+		/* attempt to set the rate map */
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg);
+
+		/* check that the changes took */
+		uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
+		if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) {
 			ret = -EINVAL;
+			io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
 		}
 		break;
+	}
 
 	case PWM_SERVO_GET_COUNT:
 		*(unsigned *)arg = _max_actuators;
 		break;
 
-	case PWM_SERVO_SET_DEBUG:
-		/* set the debug level */
-		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg);
-		break;
-
-	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS): {
+	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): {
 
+		/* TODO: we could go lower for e.g. TurboPWM */
 		unsigned channel = cmd - PWM_SERVO_SET(0);
 		if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) {
 			ret = -EINVAL;
@@ -1361,7 +1378,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		break;
 	}
 
-	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS): {
+	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): {
 
 		unsigned channel = cmd - PWM_SERVO_GET(0);
 
@@ -1379,6 +1396,16 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		break;
 	}
 
+	case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): {
+
+		unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
+
+		uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
+
+		*(uint32_t *)arg = value;
+		break;
+	}
+
 	case GPIO_RESET:
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0);
 		break;
@@ -1443,6 +1470,20 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		break;
 	}
 
+	case PX4IO_SET_DEBUG:
+		/* set the debug level */
+		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg);
+		break;
+
+	case PX4IO_INAIR_RESTART_ENABLE:
+		/* set/clear the 'in-air restart' bit */
+		if (arg) {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK);
+		} else {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0);
+		}
+		break;
+
 	default:
 		/* not a recognized value */
 		ret = -ENOTTY;
@@ -1466,12 +1507,51 @@ PX4IO::write(file *filp, const char *buffer, size_t len)
 	return count * 2;
 }
 
+int
+PX4IO::set_update_rate(int rate)
+{
+	int interval_ms = 1000 / rate;
+	if (interval_ms < 5) {
+		interval_ms = 5;
+		warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
+	}
+
+	if (interval_ms > 100) {
+		interval_ms = 100;
+		warnx("update rate too low, limiting to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
+	}
+
+	_update_interval = interval_ms;
+	return 0;
+}
+
+
 extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
 
 namespace
 {
 
 void
+start(int argc, char *argv[])
+{
+	if (g_dev != nullptr)
+		errx(1, "already loaded");
+
+	/* create the driver - it will set g_dev */
+	(void)new PX4IO();
+
+	if (g_dev == nullptr)
+		errx(1, "driver alloc failed");
+
+	if (OK != g_dev->init()) {
+		delete g_dev;
+		errx(1, "driver init failed");
+	}
+
+	exit(0);
+}
+
+void
 test(void)
 {
 	int		fd;
@@ -1565,32 +1645,20 @@ px4io_main(int argc, char *argv[])
 	if (argc < 2)
 		goto out;
 
-	if (!strcmp(argv[1], "start")) {
-
-		if (g_dev != nullptr)
-			errx(1, "already loaded");
-
-		/* create the driver - it will set g_dev */
-		(void)new PX4IO();
+	if (!strcmp(argv[1], "start"))
+		start(argc - 1, argv + 1);
 
-		if (g_dev == nullptr)
-			errx(1, "driver alloc failed");
+	if (!strcmp(argv[1], "limit")) {
 
-		if (OK != g_dev->init()) {
-			delete g_dev;
-			errx(1, "driver init failed");
-		}
+		if (g_dev != nullptr) {
 
-		/* look for the optional pwm update rate for the supported modes */
-		if ((argc > 2) && (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0)) {
-			if (argc > 2 + 1) {
-#warning implement this 
+			if ((argc > 2)) {
+				g_dev->set_update_rate(atoi(argv[2]));
 			} else {
-				fprintf(stderr, "missing argument for pwm update rate (-u)\n");
+				errx(1, "missing argument (50 - 200 Hz)");
 				return 1;
 			}
 		}
-
 		exit(0);
 	}
 
@@ -1602,7 +1670,7 @@ px4io_main(int argc, char *argv[])
 			 * We can cheat and call the driver directly, as it
 		 	 * doesn't reference filp in ioctl()
 			 */
-			g_dev->ioctl(NULL, PWM_SERVO_INAIR_RESTART_ENABLE, 0);
+			g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
 		} else {
 			errx(1, "not loaded");
 		}
@@ -1623,16 +1691,16 @@ px4io_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 
-			if (g_dev != nullptr) {
-				printf("[px4io] loaded\n");
-				g_dev->print_status();
-			} else {
-				printf("[px4io] not loaded\n");
-			}
-
-			exit(0);
+		if (g_dev != nullptr) {
+			printf("[px4io] loaded\n");
+			g_dev->print_status();
+		} else {
+			printf("[px4io] not loaded\n");
 		}
 
+		exit(0);
+	}
+
 	if (!strcmp(argv[1], "debug")) {
 		if (argc <= 2) {
 			printf("usage: px4io debug LEVEL\n");
@@ -1646,7 +1714,7 @@ px4io_main(int argc, char *argv[])
 		/* we can cheat and call the driver directly, as it
 		 * doesn't reference filp in ioctl()
 		 */
-		int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level);
+		int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level);
 		if (ret != 0) {
 			printf("SET_DEBUG failed - %d\n", ret);
 			exit(1);
@@ -1721,5 +1789,5 @@ px4io_main(int argc, char *argv[])
 		monitor();
 
 	out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug' or 'update'");
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit' or 'update'");
 }
diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c
index 2b8641f00..d7316e1f7 100644
--- a/apps/drivers/stm32/drv_pwm_servo.c
+++ b/apps/drivers/stm32/drv_pwm_servo.c
@@ -68,10 +68,6 @@
 #include <stm32_gpio.h>
 #include <stm32_tim.h>
 
-
-/* default rate (in Hz) of PWM updates */
-static uint32_t	pwm_update_rate = 50;
-
 #define REG(_tmr, _reg)	(*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg))
 
 #define rCR1(_tmr)    	REG(_tmr, STM32_GTIM_CR1_OFFSET)
@@ -93,6 +89,10 @@ static uint32_t	pwm_update_rate = 50;
 #define rDCR(_tmr)    	REG(_tmr, STM32_GTIM_DCR_OFFSET)
 #define rDMAR(_tmr)   	REG(_tmr, STM32_GTIM_DMAR_OFFSET)
 
+static void		pwm_timer_init(unsigned timer);
+static void		pwm_timer_set_rate(unsigned timer, unsigned rate);
+static void		pwm_channel_init(unsigned channel);
+
 static void
 pwm_timer_init(unsigned timer)
 {
@@ -113,11 +113,8 @@ pwm_timer_init(unsigned timer)
 	/* configure the timer to free-run at 1MHz */
 	rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1;
 
-	/* and update at the desired rate */
-	rARR(timer) = (1000000 / pwm_update_rate) - 1;
-
-	/* generate an update event; reloads the counter and all registers */
-	rEGR(timer) = GTIM_EGR_UG;
+	/* default to updating at 50Hz */
+	pwm_timer_set_rate(timer, 50);
 
 	/* note that the timer is left disabled - arming is performed separately */
 }
@@ -272,19 +269,41 @@ up_pwm_servo_deinit(void)
 }
 
 int
-up_pwm_servo_set_rate(unsigned rate)
+up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
 {
-	if ((rate < 50) || (rate > 400))
+	/* limit update rate to 1..10000Hz; somewhat arbitrary but safe */
+	if (rate < 1)
+		return -ERANGE;
+	if (rate > 10000)
 		return -ERANGE;
 
-	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
-		if (pwm_timers[i].base != 0)
-			pwm_timer_set_rate(i, rate);
-	}
+	if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0))
+		return ERROR;
+
+	pwm_timer_set_rate(group, rate);
 
 	return OK;
 }
 
+int
+up_pwm_servo_set_rate(unsigned rate)
+{
+	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++)
+		up_pwm_servo_set_rate_group_update(i, rate);
+}
+
+uint32_t
+up_pwm_servo_get_rate_group(unsigned group)
+{
+	unsigned channels = 0;
+
+	for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
+		if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group))
+			channels |= (1 << i);
+	}
+	return channels;
+}
+
 void
 up_pwm_servo_arm(bool armed)
 {
diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index ec69cdd64..f38593d2a 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -93,13 +93,11 @@ mixer_tick(void)
 
 		/* too long without FMU input, time to go to failsafe */
 		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
-			lowsyslog("AP RX timeout");
+			isr_debug(1, "AP RX timeout");
 		}
 		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
 
-		/* XXX this is questionable - vehicle may not make sense for direct control */
-		r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
 	} else {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
 		r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST;
@@ -179,7 +177,10 @@ mixer_tick(void)
 	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
 	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
 		/* there is valid input */ (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));
+		/* 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) ))
+	);
 
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 14d221b5b..8d8b7e941 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -76,7 +76,7 @@
 #define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
 
 /* static configuration page */
-#define PX4IO_PAGE_CONFIG			0
+#define PX4IO_PAGE_CONFIG		0
 #define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_SOFTWARE_VERSION		1	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_BOOTLOADER_VERSION	2	/* get this how? */
@@ -88,7 +88,7 @@
 #define PX4IO_P_CONFIG_RELAY_COUNT		8	/* harcoded # of relay outputs */
 
 /* dynamic status page */
-#define PX4IO_PAGE_STATUS			1
+#define PX4IO_PAGE_STATUS		1
 #define PX4IO_P_STATUS_FREEMEM			0
 #define PX4IO_P_STATUS_CPULOAD			1
 
@@ -112,28 +112,33 @@
 #define PX4IO_P_STATUS_ALARMS_ACC_CURRENT	(1 << 3) /* accessory current limit was exceeded */
 #define PX4IO_P_STATUS_ALARMS_FMU_LOST		(1 << 4) /* timed out waiting for controls from FMU */
 #define PX4IO_P_STATUS_ALARMS_RC_LOST		(1 << 5) /* timed out waiting for RC input */
+#define PX4IO_P_STATUS_ALARMS_PWM_ERROR		(1 << 6) /* PWM configuration or output was bad */
 
 #define PX4IO_P_STATUS_VBATT			4	/* battery voltage in mV */
 #define PX4IO_P_STATUS_IBATT			5	/* battery current in cA */
 
 /* array of post-mix actuator outputs, -10000..10000 */
-#define PX4IO_PAGE_ACTUATORS			2	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* array of PWM servo output values, microseconds */
-#define PX4IO_PAGE_SERVOS			3	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_SERVOS		3		/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* array of raw RC input values, microseconds */
-#define PX4IO_PAGE_RAW_RC_INPUT			4
+#define PX4IO_PAGE_RAW_RC_INPUT		4
 #define PX4IO_P_RAW_RC_COUNT			0	/* number of valid channels */
 #define PX4IO_P_RAW_RC_BASE			1	/* CONFIG_RC_INPUT_COUNT channels from here */
 
 /* array of scaled RC input values, -10000..10000 */
-#define PX4IO_PAGE_RC_INPUT			5
+#define PX4IO_PAGE_RC_INPUT		5
 #define PX4IO_P_RC_VALID			0	/* bitmask of valid controls */
 #define PX4IO_P_RC_BASE				1	/* CONFIG_RC_INPUT_COUNT controls from here */
 
 /* array of raw ADC values */
-#define PX4IO_PAGE_RAW_ADC_INPUT		6	/* 0..CONFIG_ADC_INPUT_COUNT-1 */
+#define PX4IO_PAGE_RAW_ADC_INPUT	6		/* 0..CONFIG_ADC_INPUT_COUNT-1 */
+
+/* PWM servo information */
+#define PX4IO_PAGE_PWM_INFO		7
+#define PX4IO_RATE_MAP_BASE			0	/* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
 
 /* setup page */
 #define PX4IO_PAGE_SETUP			100
@@ -146,8 +151,8 @@
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
-#define PX4IO_P_SETUP_PWM_LOWRATE		3	/* 'low' PWM frame output rate in Hz */
-#define PX4IO_P_SETUP_PWM_HIGHRATE		4	/* 'high' PWM frame output rate in Hz */
+#define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
+#define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
 #define PX4IO_P_SETUP_VBATT_SCALE		6	/* battery voltage correction factor (float) */
 #define PX4IO_P_SETUP_IBATT_SCALE		7	/* battery current scaling factor (float) */
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 7b4b07c2c..202e9d9d9 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -94,8 +94,8 @@ extern uint16_t			r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
 #define r_setup_features	r_page_setup[PX4IO_P_SETUP_FEATURES]
 #define r_setup_arming		r_page_setup[PX4IO_P_SETUP_ARMING]
 #define r_setup_pwm_rates	r_page_setup[PX4IO_P_SETUP_PWM_RATES]
-#define r_setup_pwm_lowrate	r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE]
-#define r_setup_pwm_highrate	r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE]
+#define r_setup_pwm_defaultrate	r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE]
+#define r_setup_pwm_altrate	r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE]
 #define r_setup_relays		r_page_setup[PX4IO_P_SETUP_RELAYS]
 
 #define r_control_values	(&r_page_controls[0])
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
index 645c1565d..6c09def9e 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -48,6 +48,7 @@
 #include "protocol.h"
 
 static int	registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
+static void	pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate);
 
 /**
  * PAGE 0
@@ -116,11 +117,12 @@ uint16_t		r_page_rc_input[] = {
 };
 
 /**
- * PAGE 6
+ * Scratch page; used for registers that are constructed as-read.
  *
- * Raw ADC input.
+ * PAGE 6 Raw ADC input.
+ * PAGE 7 PWM rate maps.
  */
-uint16_t		r_page_adc[ADC_CHANNEL_COUNT];
+uint16_t		r_page_scratch[32];
 
 /**
  * PAGE 100
@@ -132,8 +134,8 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_FEATURES]		= 0,
 	[PX4IO_P_SETUP_ARMING]			= 0,
 	[PX4IO_P_SETUP_PWM_RATES]		= 0,
-	[PX4IO_P_SETUP_PWM_LOWRATE]		= 50,
-	[PX4IO_P_SETUP_PWM_HIGHRATE]		= 200,
+	[PX4IO_P_SETUP_PWM_DEFAULTRATE]		= 50,
+	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
 	[PX4IO_P_SETUP_RELAYS]			= 0,
 	[PX4IO_P_SETUP_VBATT_SCALE]		= 10000,
 	[PX4IO_P_SETUP_IBATT_SCALE]		= 0,
@@ -321,26 +323,23 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 		case PX4IO_P_SETUP_PWM_RATES:
 			value &= PX4IO_P_SETUP_RATES_VALID;
-			r_setup_pwm_rates = value;
-			/* XXX re-configure timers */
+			pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate);
 			break;
 
-		case PX4IO_P_SETUP_PWM_LOWRATE:
+		case PX4IO_P_SETUP_PWM_DEFAULTRATE:
 			if (value < 50)
 				value = 50;
 			if (value > 400)
 				value = 400;
-			r_setup_pwm_lowrate = value;
-			/* XXX re-configure timers */
+			pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
 			break;
 
-		case PX4IO_P_SETUP_PWM_HIGHRATE:
+		case PX4IO_P_SETUP_PWM_ALTRATE:
 			if (value < 50)
 				value = 50;
 			if (value > 400)
 				value = 400;
-			r_setup_pwm_highrate = value;
-			/* XXX re-configure timers */
+			pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
 			break;
 
 		case PX4IO_P_SETUP_RELAYS:
@@ -529,10 +528,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 		break;
 
 	case PX4IO_PAGE_RAW_ADC_INPUT:
-		r_page_adc[0] = adc_measure(ADC_VBATT);
-		r_page_adc[1] = adc_measure(ADC_IN5);
+		memset(r_page_scratch, 0, sizeof(r_page_scratch));
+		r_page_scratch[0] = adc_measure(ADC_VBATT);
+		r_page_scratch[1] = adc_measure(ADC_IN5);
 
-		SELECT_PAGE(r_page_adc);
+		SELECT_PAGE(r_page_scratch);
+		break;
+
+	case PX4IO_PAGE_PWM_INFO:
+		memset(r_page_scratch, 0, sizeof(r_page_scratch));
+		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+			r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i);
+
+		SELECT_PAGE(r_page_scratch);
 		break;
 
 	/*
@@ -593,3 +601,44 @@ last_offset = offset;
 
 	return 0;
 }
+
+/*
+ * Helper function to handle changes to the PWM rate control registers.
+ */
+static void
+pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
+{
+	for (unsigned pass = 0; pass < 2; pass++) {
+		for (unsigned group = 0; group < IO_SERVO_COUNT; group++) {
+
+			/* get the channel mask for this rate group */
+			uint32_t mask = up_pwm_servo_get_rate_group(group);
+			if (mask == 0)
+				continue;
+
+			/* all channels in the group must be either default or alt-rate */
+			uint32_t alt = map & mask;
+
+			if (pass == 0) {
+				/* preflight */
+				if ((alt != 0) && (alt != mask)) {
+					/* not a legal map, bail with an alarm */
+					r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
+					return;
+				}
+			} else {
+				/* set it - errors here are unexpected */
+				if (alt != 0) {
+					if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK)
+						r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
+				} else {
+					if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK)
+						r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
+				}
+			}
+		}
+	}
+	r_setup_pwm_rates = map;
+	r_setup_pwm_defaultrate = defaultrate;
+	r_setup_pwm_altrate = altrate;
+}
diff --git a/apps/systemcmds/pwm/Makefile b/apps/systemcmds/pwm/Makefile
new file mode 100644
index 000000000..60885aa0a
--- /dev/null
+++ b/apps/systemcmds/pwm/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build the pwm tool.
+#
+
+APPNAME		 = pwm
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/pwm/pwm.c b/apps/systemcmds/pwm/pwm.c
new file mode 100644
index 000000000..bf4a1b579
--- /dev/null
+++ b/apps/systemcmds/pwm/pwm.c
@@ -0,0 +1,207 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file pwm.c
+ *
+ * PWM servo output configuration and monitoring tool.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/i2c.h>
+#include <nuttx/mtd.h>
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+#include "drivers/drv_pwm_output.h"
+
+static void	usage(const char *reason);
+__EXPORT int	pwm_main(int argc, char *argv[]);
+
+
+static void
+usage(const char *reason)
+{
+	if (reason != NULL)
+		warnx("%s", reason);
+	errx(1, 
+		"usage:\n"
+		"pwm [-v] [-d <device>] [-u <alt_rate>] [-c <alt_channel_mask>] [arm|disarm] [<channel_value> ...]\n"
+		"  -v                 Print information about the PWM device\n"
+		"  <device>           PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
+		"  <alt_rate>         PWM update rate for channels in <alt_channel_mask>\n"
+		"  <alt_channel_mask> Bitmask of channels to update at the alternate rate\n"
+		"  arm | disarm       Arm or disarm the ouptut\n"
+		"  <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n");
+}
+
+int
+pwm_main(int argc, char *argv[])
+{
+	const char *dev = PWM_OUTPUT_DEVICE_PATH;
+	unsigned alt_rate = 0;
+	uint32_t alt_channels;
+	bool alt_channels_set = false;
+	bool print_info = false;
+	int ch;
+	int ret;
+	char *ep;
+
+	while ((ch = getopt(argc, argv, "c:d:u:v")) != EOF) {
+		switch (ch) {
+		case 'c':
+			alt_channels = strtol(optarg, &ep, 0);
+			if (*ep != '\0')
+				usage("bad alt_channel_mask value");
+			alt_channels_set = true;
+			break;
+
+		case 'd':
+			dev = optarg;
+			break;
+
+		case 'u':
+			alt_rate = strtol(optarg, &ep, 0);
+			if (*ep != '\0')
+				usage("bad alt_rate value");
+
+		case 'v':
+			print_info = true;
+			break;
+
+		default:
+			usage(NULL);
+		}
+	}
+	argc -= optind;
+	argv += optind;
+
+	/* open for ioctl only */
+	int fd = open(dev, 0);
+	if (fd < 0)
+		err(1, "can't open %s", dev);
+
+	/* change alternate PWM rate */
+	if (alt_rate > 0) {
+		ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
+		if (ret != OK)
+			err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
+	}
+
+	/* assign alternate rate to channels */
+	if (alt_channels_set) {
+		ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, alt_channels);
+		if (ret != OK)
+			err(1, "PWM_SERVO_SELECT_UPDATE_RATE (check mask vs. device capabilities)");
+	}
+
+	/* iterate remaining arguments */
+	unsigned channel = 0;
+	while (argc--) {
+		const char *arg = argv[0];
+		argv++;
+		if (!strcmp(arg, "arm")) {
+			ret = ioctl(fd, PWM_SERVO_ARM, 0);
+			if (ret != OK)
+				err(1, "PWM_SERVO_ARM");
+			continue;
+		}
+		if (!strcmp(arg, "disarm")) {
+			ret = ioctl(fd, PWM_SERVO_DISARM, 0);
+			if (ret != OK)
+				err(1, "PWM_SERVO_DISARM");
+			continue;
+		}
+		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++;
+			continue;
+		}
+		usage("unrecognised option");
+	}
+
+	/* print verbose info */
+	if (print_info) {
+		/* get the number of servo channels */
+		unsigned count;
+		ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count);
+		if (ret != OK)
+			err(1, "PWM_SERVO_GET_COUNT");
+
+		/* print current servo values */
+		printf("PWM output values:\n");
+		for (unsigned i = 0; i < count; i++) {
+			servo_position_t spos;
+
+			ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
+			if (ret == OK) {
+				printf("%u: %uus\n", i, spos);
+			} else {
+				printf("%u: ERROR\n", i);
+			}
+		}
+
+		/* print rate groups */
+		printf("Available alt_channel_mask groups:\n");
+		for (unsigned i = 0; i < count; i++) {
+			uint32_t group_mask;
+
+			ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
+			if (ret != OK)
+				break;
+			if (group_mask != 0)
+				printf(" 0x%x", group_mask);
+		}
+		printf("\n");
+		fflush(stdout);
+	}
+	exit(0);
+}
\ No newline at end of file