From 9ff521711861fce857b6c17c2ec87eaa2073376e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 4 Oct 2013 17:20:34 +0200 Subject: Some more interface changes, needs testing and cleanup --- src/systemcmds/pwm/pwm.c | 430 ++++++++++++++++++++++++----------------------- 1 file changed, 222 insertions(+), 208 deletions(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 58df9cd15..44e49dd05 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -72,25 +72,33 @@ usage(const char *reason) warnx("%s", reason); errx(1, "usage:\n" - "pwm [-v] [-d ] set|config|arm|disarm|info ...\n" - "" - " -v Print verbose information\n" - " -d PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + "pwm arm|disarm|rate|min|max|disarmed|test|info ...\n" "\n" - "arm Arm output\n" - "disarm Disarm output\n" + " arm Arm output\n" + " disarm Disarm output\n" "\n" - "set ... Directly set PWM values\n" + " rate Configure PWM rates\n" + " [-c ] Channel group that should update at the alternate rate\n" + " [-m ] Directly supply channel mask\n" + " [-a] Configure all outputs\n" + " -r PWM rate (50 to 400 Hz)\n" "\n" - "config rate Configure PWM rates\n" - " [-c ] Channel group that should update at the alternate rate\n" - " [-m ] Directly supply alt rate channel mask\n" + " min ... Configure minimum PWM values\n" + " max ... Configure maximum PWM values\n" + " disarmed ... Configure disarmed PWM values\n" + " [-m ] Directly supply channel mask\n" + " [-a] Configure all outputs\n" + " -p PWM value\n" "\n" - "config min ... Configure minimum PWM values\n" - "config max ... Configure maximum PWM values\n" - "config disarmed ... Configure disarmed PWM values\n" + " test ... Directly set PWM values\n" + " [-m ] Directly supply channel mask\n" + " [-a] Configure all outputs\n" + " -p PWM value\n" "\n" - "info Print information about the PWM device\n" + " info Print information about the PWM device\n" + "\n" + " -v Print verbose information\n" + " -d PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" ); } @@ -106,13 +114,16 @@ pwm_main(int argc, char *argv[]) int ch; int ret; char *ep; - int32_t set_mask = -1; + uint32_t set_mask = 0; unsigned group; + unsigned long channels; + unsigned single_ch = 0; + unsigned pwm_value = 0; if (argc < 1) usage(NULL); - while ((ch = getopt(argc, argv, "v:d:")) != EOF) { + while ((ch = getopt(argc-1, &argv[1], "d:vc:m:ap:r:")) != EOF) { switch (ch) { case 'd': @@ -121,32 +132,82 @@ pwm_main(int argc, char *argv[]) usage(NULL); } dev = optarg; - argv+=2; - argc-=2; break; case 'v': print_verbose = true; - argv+=1; - argc-=1; break; + case 'c': + /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ + channels = strtol(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; + break; + + case 'm': + /* Read in mask directly */ + set_mask = strtol(optarg, &ep, 0); + if (*ep != '\0') + usage("bad set_mask value"); + break; + + case 'a': + for (unsigned i = 0; i 0) { + warnx("Chose channels: "); + printf(" "); + for (unsigned i = 0; i 1) { - argi++; - unsigned pwm_value = strtol(argv[argi], &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])); - //XXX check for sane values ? - channel[nchannels] = pwm_value; - if (print_verbose) - warnx("Set channel %d: %d us", nchannels+1, channel[nchannels]); - - nchannels++; - - continue; - } - usage("unrecognized option"); + /* 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)"); + } else { + usage("no alternative rate provided"); } - /* perform PWM output */ - if (nchannels) { + /* directly supplied channel mask */ + if (set_mask > 0) { + ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask); + if (ret != OK) + err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); + } else { + usage("no channel/channel groups selected"); + } - /* Open console directly to grab CTRL-C signal */ - struct pollfd fds; - fds.fd = 0; /* stdin */ - fds.events = POLLIN; + /* assign alternate rate to channel groups */ + if (alt_channels_set) { + uint32_t mask = 0; - warnx("Press CTRL-C or 'c' to abort."); + for (group = 0; group < 32; group++) { + if ((1 << group) & alt_channel_groups) { + uint32_t group_mask; - while (1) { - for (unsigned i = 0; i < nchannels; i++) { - ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]); + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); if (ret != OK) - err(1, "PWM_SERVO_SET(%d)", i); - } - - /* abort on user request */ - char c; - ret = poll(&fds, 1, 0); - if (ret > 0) { + err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); - read(0, &c, 1); - if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("User abort\n"); - exit(0); - } + mask |= group_mask; } } - } else { - usage("no PWM values supplied"); - } - - } else if (!strcmp(argv[argi], "config")) { - struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; - - argi++; + ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask); + if (ret != OK) + err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); + } + exit(0); - if (!strcmp(argv[argi], "rate")) { + } else if (!strcmp(argv[argi], "min")) { - while ((ch = getopt(argc, argv, "m:c:")) != EOF) { - switch (ch) { + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); - case 'm': - set_mask = strtol(optarg, &ep, 0); - if (*ep != '\0') - usage("bad set_mask value"); - break; - argi+=2; + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; - case 'c': - group = strtoul(optarg, &ep, 0); - if ((*ep != '\0') || (group >= 32)) - usage("bad channel_group value"); - alt_channel_groups |= (1 << group); - alt_channels_set = true; - argi+=2; - break; - } - } - argi++; - if (argi >= argc) - usage("no alt_rate value supplied"); - - alt_rate = strtol(argv[argi], &ep, 0); - if (*ep != '\0') - usage("bad alt_rate value"); - break; - - /* 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)"); + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1< 1) { - argi++; - unsigned pwm_value = strtol(argv[argi], &ep, 0); - if (*ep == '\0') { - if (pwm_values.channel_count > PWM_OUTPUT_MAX_CHANNELS) - err(1, "too many pwm values (max %d)", PWM_OUTPUT_MAX_CHANNELS); - //XXX check for sane values ? - pwm_values.values[pwm_values.channel_count] = pwm_value; - pwm_values.channel_count++; + } else if (!strcmp(argv[argi], "disarmed")) { - continue; - } - usage("unrecognized option"); - } - if (pwm_values.channel_count == 0) { - usage("no PWM values added"); - } else { + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - if (ret != OK) - errx(ret, "failed setting idle values"); - } + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1< 1) { - argi++; - unsigned pwm_value = strtol(argv[argi], &ep, 0); - if (*ep == '\0') { - if (pwm_values.channel_count > PWM_OUTPUT_MAX_CHANNELS) - err(1, "too many pwm values (max %d)", PWM_OUTPUT_MAX_CHANNELS); - //XXX check for sane values ? - pwm_values.values[pwm_values.channel_count] = pwm_value; - pwm_values.channel_count++; + ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values); + if (ret != OK) + errx(ret, "failed setting idle values"); + } + exit(0); - continue; - } - usage("unrecognized option"); - } - if (pwm_values.channel_count == 0) { - usage("no PWM values added"); - } else { + } else if (!strcmp(argv[argi], "test")) { - ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); - if (ret != OK) - errx(ret, "failed setting idle values"); - } + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); + /* perform PWM output */ - } else if (!strcmp(argv[argi], "disarmed")) { + /* Open console directly to grab CTRL-C signal */ + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; - /* iterate remaining arguments */ - while (argc - argi > 1) { - argi++; - unsigned pwm_value = strtol(argv[argi], &ep, 0); - if (*ep == '\0') { - if (pwm_values.channel_count > PWM_OUTPUT_MAX_CHANNELS) - err(1, "too many pwm values (max %d)", PWM_OUTPUT_MAX_CHANNELS); - //XXX check for sane values ? - pwm_values.values[pwm_values.channel_count] = pwm_value; - pwm_values.channel_count++; + warnx("Press CTRL-C or 'c' to abort."); - continue; + while (1) { + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1< 0) { - } else { - usage("specify rate, min, max or disarmed"); + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("User abort\n"); + exit(0); + } + } } + exit(0); - } else if (!strcmp(argv[argi], "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"); + } else if (!strcmp(argv[argi], "info")) { /* 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); @@ -405,7 +418,7 @@ pwm_main(int argc, char *argv[]) } /* 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); @@ -419,12 +432,13 @@ pwm_main(int argc, char *argv[]) printf("\n"); } } + exit(0); - } else { - usage("specify arm|disarm|set|config"); } + argi++; } - exit(0); + usage("specify arm|disarm|set|config|test"); + return 0; } -- cgit v1.2.3