Convert HIL and FMU drivers to the new multirate PWM interface.

3 files changed, 216 insertions, 128 deletions

diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp
index 861ed7924..8a471b61c 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;
@@ -786,59 +800,51 @@ 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);
+	unsigned pwm_rate = 0;	
+	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")) {
+		int ch;
 
-		} else if (!strcmp(argv[i], "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;
+		while ((ch = getopt(argc - 1, argv + 1, "u:")) != EOF) {
+			switch (ch) {
+			case 'u':
+				pwm_rate = strtol(optarg, nullptr, 0);
+				break;
 
-		} else if (!strcmp(argv[i], "mode_port2_pwm12")) {
-			new_mode = PORT2_8PWM;
+			case ':':
+				errx(1, "missing parameter");
 
-		} else if (!strcmp(argv[i], "mode_port2_pwm16")) {
-			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;
+			default:
+				errx(1, "unrecognised option");
 			}
-			// } else {
-			// 	fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n");
-			// }
 		}
-        }
+
+		new_mode = PORT1_FULL_PWM;
+
+	} else if (!strcmp(verb, "mode_pwm_serial")) {
+		new_mode = PORT1_PWM_AND_SERIAL;
+
+	} else if (!strcmp(verb, "mode_pwm_gpio")) {
+		new_mode = PORT1_PWM_AND_GPIO;
+
+	} else if (!strcmp(verb, "mode_port2_pwm8")) {
+		new_mode = PORT2_8PWM;
+
+	} else if (!strcmp(verb, "mode_port2_pwm12")) {
+		new_mode = PORT2_8PWM;
+
+	} else if (!strcmp(verb, "mode_port2_pwm16")) {
+		new_mode = PORT2_8PWM;
+	}
 
 	/* was a new mode set? */
 	if (new_mode != PORT_MODE_UNDEFINED) {
@@ -848,10 +854,15 @@ 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, pwm_rate);
 	}
 
-	/* 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");
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp
index 8e13f7c62..7f2c7a19b 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,56 @@ 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;
+	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)) {
+					// 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)
+						return -EINVAL;
+				} else {
+					if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK)
+						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 +406,29 @@ 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;
 			}
 
 			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 +585,6 @@ int
 PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret = OK;
-	int channel;
 
 	lock();
 
@@ -541,7 +598,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 +616,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,11 +632,16 @@ 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 MIXERIOCGETOUTPUTCOUNT:
 		if (_mode == MODE_4PWM) {
@@ -812,7 +876,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 +928,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 +1023,57 @@ 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);
+	unsigned pwm_rate = 0;
+	uint32_t alt_channels = 0;
+	bool alt_channels_set = false;
+	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")) {
+		int ch;
+		char *ap;
 
-		} else if (!strcmp(argv[i], "mode_gpio_serial")) {
-			new_mode = PORT_GPIO_AND_SERIAL;
+		while ((ch = getopt(argc - 1, argv + 1, "c:u:")) != EOF) {
+			switch (ch) {
+			case 'u':
+				pwm_rate = strtol(optarg, nullptr, 0);
+				break;
+
+			case 'c':
+				alt_channels = strtol(optarg, &ap, 0);
+				if (*ap == '\0')
+					alt_channels_set = true;
+				break;
 
-		} else if (!strcmp(argv[i], "mode_pwm_serial")) {
-			new_mode = PORT_PWM_AND_SERIAL;
+			case ':':
+				errx(1, "missing parameter");
 
-		} else if (!strcmp(argv[i], "mode_pwm_gpio")) {
-			new_mode = PORT_PWM_AND_GPIO;
+			default:
+				errx(1, "unrecognised option");
+			}
 		}
 
-		/* 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]);
+		new_mode = PORT_FULL_PWM;
 
-				} else {
-					errx(1, "missing argument for pwm update rate (-u)");
-					return 1;
-				}
+	} else if (!strcmp(verb, "mode_gpio_serial")) {
+		new_mode = PORT_GPIO_AND_SERIAL;
 
-			} else {
-				errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio");
-			}
-		}
+	} else if (!strcmp(verb, "mode_pwm_serial")) {
+		new_mode = PORT_PWM_AND_SERIAL;
+
+	} else if (!strcmp(verb, "mode_pwm_gpio")) {
+		new_mode = PORT_PWM_AND_GPIO;
 	}
 
 	/* was a new mode set? */
@@ -1023,12 +1084,28 @@ fmu_main(int argc, char *argv[])
 			return OK;
 
 		/* switch modes */
-		return fmu_new_mode(new_mode, pwm_update_rate_in_hz);
+		return fmu_new_mode(new_mode);
+
+		/* if new modes are PWM modes, respect the -u and -c options */
+		if ((new_mode == PORT_FULL_PWM) || (new_mode == PORT_PWM_AND_GPIO)) {
+			if (pwm_rate != 0) {
+				if (g_fmu->set_pwm_alt_rate(pwm_rate) != OK)
+					errx(1, "error setting alternate PWM rate");
+			}
+			if (alt_channels_set) {
+				if (g_fmu->set_pwm_alt_channels(alt_channels))
+					errx(1, "serror setting alternate PWM rate channels");
+			}
+		}
 	}
 
-	/* 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 [-u pwm_alt_rate_in_hz] [-c pwm_alt_rate_channel_mask], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
 	exit(1);
 }
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
index b09937300..6c09def9e 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -616,19 +616,19 @@ pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
 			if (mask == 0)
 				continue;
 
-			/* all channels in the group must be either high or low-rate */
-			uint32_t high = map & mask;
+			/* all channels in the group must be either default or alt-rate */
+			uint32_t alt = map & mask;
 
 			if (pass == 0) {
 				/* preflight */
-				if ((high != 0) && (high != mask)) {
+				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 (high != 0) {
+				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 {