Merge pull request #224 from PX4/pwm-multirate

Pwm multirate

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,27 +100,25 @@ 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)
 
 /** 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/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;
 }