1 files changed, 336 insertions, 127 deletions

diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index c42a36c7f..b3975de9d 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -45,6 +45,7 @@
 #include <stdbool.h>
 #include <unistd.h>
 #include <fcntl.h>
+#include <poll.h>
 #include <sys/mount.h>
 #include <sys/ioctl.h>
 #include <sys/stat.h>
@@ -71,16 +72,36 @@ usage(const char *reason)
 		warnx("%s", reason);
 	errx(1, 
 		"usage:\n"
-		"pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [-m chanmask ] [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"
-		"  <channel_group>    Channel group that should update at the alternate rate (may be specified more than once)\n"
-		"  arm | disarm       Arm or disarm the ouptut\n"
-		"  <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n"
-		"  <chanmask>         Directly supply alt rate channel mask (debug use only)\n"
+		"pwm arm|disarm|rate|min|max|disarmed|set|info  ...\n"
 		"\n"
-		"When -c is specified, any channel groups not listed with -c will update at the default rate.\n"
+		"  arm                      Arm output\n"
+		"  disarm                   Disarm output\n"
+		"\n"
+		"  rate                     Configure PWM rates\n"
+		"    [-g <channel group>]   Channel group that should update at the alternate rate\n"
+		"    [-m <chanmask> ]       Directly supply channel mask\n"
+		"    [-a]                   Configure all outputs\n"
+		"    -r <alt_rate>          PWM rate (50 to 400 Hz)\n"
+		"\n"
+		"  min ...           	    Configure minimum PWM values\n"
+		"  max ...           	    Configure maximum PWM values\n"
+		"  disarmed ...      	    Configure disarmed PWM values\n"
+		"    [-c <channels>]        Supply channels (e.g. 1234)\n"
+		"    [-m <chanmask> ]       Directly supply channel mask (e.g. 0xF)\n"
+		"    [-a]                   Configure all outputs\n"
+		"    -p <pwm value>         PWM value\n"
+		"\n"
+		"  set ...                  Directly set PWM values\n"
+		"    [-c <channels>]        Supply channels (e.g. 1234)\n"
+		"    [-m <chanmask> ]       Directly supply channel mask (e.g. 0xF)\n"
+		"    [-a]                   Configure all outputs\n"
+		"    [-e]                   Repeat setting the values until stopped\n"
+		"    -p <pwm value>         PWM value\n"
+		"\n"
+		"  info                     Print information about the PWM device\n"
+		"\n"
+		"    -v                     Print verbose information\n"
+		"    -d <device>            PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
 		);
 
 }
@@ -92,152 +113,370 @@ pwm_main(int argc, char *argv[])
 	unsigned alt_rate = 0;
 	uint32_t alt_channel_groups = 0;
 	bool alt_channels_set = false;
-	bool print_info = false;
+	bool print_verbose = false;
+	bool repeated_pwm_output = false;
 	int ch;
 	int ret;
 	char *ep;
+	uint32_t set_mask = 0;
 	unsigned group;
-	int32_t set_mask = -1;
+	unsigned long channels;
+	unsigned single_ch = 0;
+	unsigned pwm_value = 0;
 
-	if (argc < 2)
+	if (argc < 1)
 		usage(NULL);
 
-	while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) {
+	while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:e")) != EOF) {
 		switch (ch) {
+
+		case 'd':
+			if (NULL == strstr(optarg, "/dev/")) {
+				warnx("device %s not valid", optarg);
+				usage(NULL);
+			}
+			dev = optarg;
+			break;
+
+		case 'v':
+			print_verbose = true;
+			break;
+
 		case 'c':
+			/* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
+			channels = strtoul(optarg, &ep, 0);
+
+			while ((single_ch = channels % 10)) {
+
+				set_mask |= 1<<(single_ch-1);
+				channels /= 10;
+			}
+			break;
+
+		case 'g':
 			group = strtoul(optarg, &ep, 0);
 			if ((*ep != '\0') || (group >= 32))
 				usage("bad channel_group value");
 			alt_channel_groups |= (1 << group);
 			alt_channels_set = true;
+			warnx("alt channels set, group: %d", group);
 			break;
 
-		case 'd':
-			dev = optarg;
+		case 'm':
+			/* Read in mask directly */
+			set_mask = strtoul(optarg, &ep, 0);
+			if (*ep != '\0')
+				usage("bad set_mask value");
 			break;
 
-		case 'u':
-			alt_rate = strtol(optarg, &ep, 0);
+		case 'a':
+			for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
+				set_mask |= 1<<i;
+			}
+			break;
+		case 'p':
+			pwm_value = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad alt_rate value");
+				usage("bad PWM value provided");
 			break;
-
-		case 'm':
-			set_mask = strtol(optarg, &ep, 0);
+		case 'r':
+			alt_rate = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad set_mask value");
+				usage("bad alternative rate provided");
 			break;
-
-		case 'v':
-			print_info = true;
+		case 'e':
+			repeated_pwm_output = true;
 			break;
-
 		default:
-			usage(NULL);
+			break;
+		}
+	}
+
+	if (print_verbose && set_mask > 0) {
+		warnx("Chose channels: ");
+		printf("    ");
+		for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
+			if (set_mask & 1<<i)
+				printf("%d ", i+1);
 		}
+		printf("\n");
 	}
-	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);
+	/* get the number of servo channels */
+	unsigned servo_count;
+	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	if (ret != OK)
+		err(1, "PWM_SERVO_GET_COUNT");
+
+	if (!strcmp(argv[1], "arm")) {
+		/* tell IO that its ok to disable its safety with the switch */
+		ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
 		if (ret != OK)
-			err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
-	}
+			err(1, "PWM_SERVO_SET_ARM_OK");
+		/* tell IO that the system is armed (it will output values if safety is off) */
+		ret = ioctl(fd, PWM_SERVO_ARM, 0);
+		if (ret != OK)
+			err(1, "PWM_SERVO_ARM");
+
+		if (print_verbose)
+			warnx("Outputs armed");
+		exit(0);
 
-	/* directly supplied channel mask */
-	if (set_mask != -1) {
-		ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask);
+	} else if (!strcmp(argv[1], "disarm")) {
+		/* disarm, but do not revoke the SET_ARM_OK flag */
+		ret = ioctl(fd, PWM_SERVO_DISARM, 0);
 		if (ret != OK)
-			err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
-	}
+			err(1, "PWM_SERVO_DISARM");
+
+		if (print_verbose)
+			warnx("Outputs disarmed");
+		exit(0);
+
+	} else if (!strcmp(argv[1], "rate")) {
+
+		/* 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 channel groups */
-	if (alt_channels_set) {
-		uint32_t mask = 0;
+		/* directly supplied channel mask */
+		if (set_mask > 0) {
+			ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask);
+			if (ret != OK)
+				err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE");
+		}
 
-		for (unsigned group = 0; group < 32; group++) {
-			if ((1 << group) & alt_channel_groups) {
-				uint32_t group_mask;
+		/* assign alternate rate to channel groups */
+		if (alt_channels_set) {
+			uint32_t mask = 0;
 
-				ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
-				if (ret != OK)
-					err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
+			for (group = 0; group < 32; group++) {
+				if ((1 << group) & alt_channel_groups) {
+					uint32_t group_mask;
 
-				mask |= group_mask;
+					ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
+					if (ret != OK)
+						err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
+
+					mask |= group_mask;
+				}
 			}
+
+			ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, mask);
+			if (ret != OK)
+				err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE");
 		}
+		exit(0);
 
-		ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask);
-		if (ret != OK)
-			err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
-	}
+	} else if (!strcmp(argv[1], "min")) {
+
+		if (set_mask == 0) {
+			usage("no channels set");
+		}
+		if (pwm_value == 0)
+			usage("no PWM value provided");
 
-	/* iterate remaining arguments */
-	unsigned nchannels = 0;
-	unsigned channel[8] = {0};
-	while (argc--) {
-		const char *arg = argv[0];
-		argv++;
-		if (!strcmp(arg, "arm")) {
-			/* tell IO that its ok to disable its safety with the switch */
-			ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
+
+		for (unsigned i = 0; i < servo_count; i++) {
+			if (set_mask & 1<<i) {
+				pwm_values.values[i] = pwm_value;
+				if (print_verbose)
+					warnx("Channel %d: min PWM: %d", i+1, pwm_value);
+			}
+			pwm_values.channel_count++;
+		}
+
+		if (pwm_values.channel_count == 0) {
+			usage("no PWM values added");
+		} else {
+
+			ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
 			if (ret != OK)
-				err(1, "PWM_SERVO_SET_ARM_OK");
-			/* tell IO that the system is armed (it will output values if safety is off) */
-			ret = ioctl(fd, PWM_SERVO_ARM, 0);
+				errx(ret, "failed setting min values");
+		}
+		exit(0);
+
+	} else if (!strcmp(argv[1], "max")) {
+
+		if (set_mask == 0) {
+			usage("no channels set");
+		}
+		if (pwm_value == 0)
+			usage("no PWM value provided");
+
+		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
+
+		for (unsigned i = 0; i < servo_count; i++) {
+			if (set_mask & 1<<i) {
+				pwm_values.values[i] = pwm_value;
+				if (print_verbose)
+					warnx("Channel %d: max PWM: %d", i+1, pwm_value);
+			}
+			pwm_values.channel_count++;
+		}
+
+		if (pwm_values.channel_count == 0) {
+			usage("no PWM values added");
+		} else {
+
+			ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
 			if (ret != OK)
-				err(1, "PWM_SERVO_ARM");
-			continue;
+				errx(ret, "failed setting max values");
 		}
-		if (!strcmp(arg, "disarm")) {
-			/* disarm, but do not revoke the SET_ARM_OK flag */
-			ret = ioctl(fd, PWM_SERVO_DISARM, 0);
+		exit(0);
+
+	} else if (!strcmp(argv[1], "disarmed")) {
+
+		if (set_mask == 0) {
+			usage("no channels set");
+		}
+		if (pwm_value == 0)
+			usage("no PWM value provided");
+
+		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
+
+		for (unsigned i = 0; i < servo_count; i++) {
+			if (set_mask & 1<<i) {
+				pwm_values.values[i] = pwm_value;
+				if (print_verbose)
+					warnx("Channel %d: disarmed PWM: %d", i+1, pwm_value);
+			}
+			pwm_values.channel_count++;
+		}
+
+		if (pwm_values.channel_count == 0) {
+			usage("no PWM values added");
+		} else {
+
+			ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values);
 			if (ret != OK)
-				err(1, "PWM_SERVO_DISARM");
-			continue;
+				errx(ret, "failed setting disarmed values");
 		}
-		unsigned pwm_value = strtol(arg, &ep, 0);
-		if (*ep == '\0') {
-			if (nchannels > sizeof(channel) / sizeof(channel[0]))
-				err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0]));
+		exit(0);
 
-			channel[nchannels] = pwm_value;
-			nchannels++;
+	} else if (!strcmp(argv[1], "set")) {
 
-			continue;
+		if (set_mask == 0) {
+			usage("no channels set");
 		}
-		usage("unrecognized option");
-	}
+		if (pwm_value == 0)
+			usage("no PWM value provided");
 
-	/* 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");
+		/* perform PWM output */
+
+		if (repeated_pwm_output) {
+
+			/* Open console directly to grab CTRL-C signal */
+			struct pollfd fds;
+			fds.fd = 0; /* stdin */
+			fds.events = POLLIN;
+
+			warnx("Press CTRL-C or 'c' to abort.");
+
+			while (1) {
+				for (unsigned i = 0; i < servo_count; i++) {
+					if (set_mask & 1<<i) {
+						ret = ioctl(fd, PWM_SERVO_SET(i), pwm_value);
+						if (ret != OK)
+							err(1, "PWM_SERVO_SET(%d)", i);
+					}
+				}
+
+				/* abort on user request */
+				char c;
+				ret = poll(&fds, 1, 0);
+				if (ret > 0) {
+
+				read(0, &c, 1);
+					if (c == 0x03 || c == 0x63 || c == 'q') {
+						warnx("User abort\n");
+						exit(0);
+					}
+				}
+			}
+		} else {
+			/* only set the values once */
+			for (unsigned i = 0; i < servo_count; i++) {
+				if (set_mask & 1<<i) {
+					ret = ioctl(fd, PWM_SERVO_SET(i), pwm_value);
+					if (ret != OK)
+						err(1, "PWM_SERVO_SET(%d)", i);
+				}
+			}
+		}
+		exit(0);
+
+
+	} else if (!strcmp(argv[1], "info")) {
+
+		printf("device: %s\n", dev);
+
+		uint32_t info_default_rate;
+		uint32_t info_alt_rate;
+		uint32_t info_alt_rate_mask;
+
+		ret = ioctl(fd, PWM_SERVO_GET_DEFAULT_UPDATE_RATE, (unsigned long)&info_default_rate);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_DEFAULT_UPDATE_RATE");
+		}
+
+		ret = ioctl(fd, PWM_SERVO_GET_UPDATE_RATE, (unsigned long)&info_alt_rate);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_UPDATE_RATE");
+		}
+
+		ret = ioctl(fd, PWM_SERVO_GET_SELECT_UPDATE_RATE, (unsigned long)&info_alt_rate_mask);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE");
+		}
+
+		struct pwm_output_values disarmed_pwm;
+		struct pwm_output_values min_pwm;
+		struct pwm_output_values max_pwm;
+
+		ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (unsigned long)&disarmed_pwm);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_DISARMED_PWM");
+		}
+		ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (unsigned long)&min_pwm);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_MIN_PWM");
+		}
+		ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (unsigned long)&max_pwm);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_MAX_PWM");
+		}
 
 		/* print current servo values */
-		for (unsigned i = 0; i < count; i++) {
+		for (unsigned i = 0; i < servo_count; i++) {
 			servo_position_t spos;
 
 			ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
 			if (ret == OK) {
-				printf("channel %u: %uus\n", i, spos);
+				printf("channel %u: %u us", i+1, spos);
+
+				if (info_alt_rate_mask & (1<<i))
+					printf(" (alternative rate: %d Hz", info_alt_rate);
+				else
+					printf(" (default rate: %d Hz", info_default_rate);
+
+
+				printf(" disarmed; %d us, min: %d us, max: %d us)", disarmed_pwm.values[i], min_pwm.values[i], max_pwm.values[i]);
+				printf("\n");
 			} else {
 				printf("%u: ERROR\n", i);
 			}
 		}
-
 		/* print rate groups */
-		for (unsigned i = 0; i < count; i++) {
+		for (unsigned i = 0; i < servo_count; i++) {
 			uint32_t group_mask;
 
 			ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
@@ -247,44 +486,14 @@ pwm_main(int argc, char *argv[])
 				printf("channel group %u: channels", i);
 				for (unsigned j = 0; j < 32; j++)
 					if (group_mask & (1 << j))
-						printf(" %u", j);
+						printf(" %u", j+1);
 				printf("\n");
 			}
 		}
-		fflush(stdout);
-	}
-
-	/* perform PWM output */
-	if (nchannels) {
-
-		/* Open console directly to grab CTRL-C signal */
-		int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
-		if (!console)
-			err(1, "failed opening console");
-
-		warnx("Press CTRL-C or 'c' to abort.");
+		exit(0);
 
-		while (1) {
-			for (int i = 0; i < nchannels; i++) {
-				ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]);
-				if (ret != OK)
-					err(1, "PWM_SERVO_SET(%d)", i);
-			}
-
-			/* abort on user request */
-			char c;
-			if (read(console, &c, 1) == 1) {
-				if (c == 0x03 || c == 0x63 || c == 'q') {
-					warnx("User abort\n");
-					close(console);
-					exit(0);
-				}
-			}
-
-			/* rate limit to ~ 20 Hz */
-			usleep(50000);
-		}
 	}
+	usage("specify arm|disarm|rate|min|max|disarmed|set|info");
+	return 0;
+}
 
-	exit(0);
-}
\ No newline at end of file