Changes for multi-rate PWM output; default and alternate rates. ioctl protocol, PX4IO support.

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,13 +155,32 @@ __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.
  */
 __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.
  *

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
 {
@@ -1223,14 +1225,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 +1272,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 +1323,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 +1367,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 +1385,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 +1459,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;
@@ -1602,7 +1632,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 +1653,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 +1676,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);

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)
 {

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) */

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])

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 high or low-rate */
+			uint32_t high = map & mask;
+
+			if (pass == 0) {
+				/* preflight */
+				if ((high != 0) && (high != 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 (high != 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;
+}