Move PWM rate configuration, etc. into a separate utility and out of the individual drivers.

5 files changed, 288 insertions, 92 deletions

diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp
index 8a471b61c..d9aa772d4 100644
--- a/apps/drivers/hil/hil.cpp
+++ b/apps/drivers/hil/hil.cpp
@@ -655,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;
 	
@@ -713,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;
 }
@@ -800,7 +798,6 @@ int
 hil_main(int argc, char *argv[])
 {
 	PortMode new_mode = PORT_MODE_UNDEFINED;
-	unsigned pwm_rate = 0;	
 	const char *verb = argv[1];
 
 	if (hil_start() != OK)
@@ -812,22 +809,6 @@ hil_main(int argc, char *argv[])
 
  	// this was all cut-and-pasted from the FMU driver; it's junk
 	if (!strcmp(verb, "mode_pwm")) {
-		int ch;
-
-		while ((ch = getopt(argc - 1, argv + 1, "u:")) != EOF) {
-			switch (ch) {
-			case 'u':
-				pwm_rate = strtol(optarg, nullptr, 0);
-				break;
-
-			case ':':
-				errx(1, "missing parameter");
-
-			default:
-				errx(1, "unrecognised option");
-			}
-		}
-
 		new_mode = PORT1_FULL_PWM;
 
 	} else if (!strcmp(verb, "mode_pwm_serial")) {
@@ -854,7 +835,7 @@ hil_main(int argc, char *argv[])
 			return OK;
 
 		/* switch modes */
-		return hil_new_mode(new_mode, pwm_rate);
+		return hil_new_mode(new_mode);
 	}
 
 	if (!strcmp(verb, "test"))
@@ -865,6 +846,6 @@ hil_main(int argc, char *argv[])
 
 
 	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 7f2c7a19b..476adb7f0 100644
--- a/apps/drivers/px4fmu/fmu.cpp
+++ b/apps/drivers/px4fmu/fmu.cpp
@@ -309,6 +309,8 @@ PX4FMU::set_mode(Mode mode)
 int
 PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate)
 {
+	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++) {
 
@@ -323,17 +325,22 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 			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)
+					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)
+					if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
+						warn("rate group set default failed");
 						return -EINVAL;
+					}
 				}
 			}
 		}
@@ -428,6 +435,7 @@ PX4FMU::task_main()
 				_current_update_rate = 50;
 			}
 
+			debug("adjusted actuator update interval to %ums", update_rate_in_ms);
 			orb_set_interval(_t_actuators, update_rate_in_ms);
 		}
 
@@ -643,6 +651,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		*(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;
@@ -1023,9 +1032,6 @@ int
 fmu_main(int argc, char *argv[])
 {
 	PortMode new_mode = PORT_MODE_UNSET;
-	unsigned pwm_rate = 0;
-	uint32_t alt_channels = 0;
-	bool alt_channels_set = false;
 	const char *verb = argv[1];
 
 	if (fmu_start() != OK)
@@ -1041,29 +1047,6 @@ fmu_main(int argc, char *argv[])
 		new_mode = PORT_FULL_SERIAL;
 
 	} else if (!strcmp(verb, "mode_pwm")) {
-		int ch;
-		char *ap;
-
-		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;
-
-			case ':':
-				errx(1, "missing parameter");
-
-			default:
-				errx(1, "unrecognised option");
-			}
-		}
-
 		new_mode = PORT_FULL_PWM;
 
 	} else if (!strcmp(verb, "mode_gpio_serial")) {
@@ -1084,19 +1067,8 @@ fmu_main(int argc, char *argv[])
 			return OK;
 
 		/* switch modes */
-		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");
-			}
-		}
+		int ret = fmu_new_mode(new_mode);
+		exit(ret == OK ? 0 : 1);
 	}
 
 	if (!strcmp(verb, "test"))
@@ -1106,6 +1078,6 @@ fmu_main(int argc, char *argv[])
 		fake(argc - 1, argv + 1);
 
 	fprintf(stderr, "FMU: unrecognised command, try:\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");
+	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 803e74469..9854402f0 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -1502,6 +1502,26 @@ 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;
@@ -1595,34 +1615,8 @@ 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 (g_dev == nullptr)
-			errx(1, "driver alloc failed");
-
-		if (OK != g_dev->init()) {
-			delete g_dev;
-			errx(1, "driver init failed");
-		}
-
-		/* 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 
-			} else {
-				fprintf(stderr, "missing argument for pwm update rate (-u)\n");
-				return 1;
-			}
-		}
-
-		exit(0);
-	}
+	if (!strcmp(argv[1], "start"))
+		start(argc - 1, argv + 1);
 
 	if (!strcmp(argv[1], "recovery")) {
 
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