From baa49080547d740959f96174023a9cd5312924f1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 4 Oct 2013 13:00:12 +0200 Subject: [PATCH] Changes to pwm systemcmd, basic functionality there, needs polishing --- src/drivers/px4io/px4io.cpp | 104 +-------- src/systemcmds/pwm/pwm.c | 429 ++++++++++++++++++++++++------------ 2 files changed, 286 insertions(+), 247 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0fed99692b..add20c5511 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1748,6 +1748,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case PWM_SERVO_SET_MIN_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + warnx("Set min values"); set_min_values(pwm->values, pwm->channel_count); } break; @@ -2381,110 +2382,7 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "min")) { - if (argc < 3) { - errx(1, "min command needs at least one channel value (PWM)"); - } - - int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); - struct pwm_output_values pwm; - - if (iofd > 0) { - - pwm.channel_count = 0; - - for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++) - { - /* set channel to commanline argument or to 900 for non-provided channels */ - if (argc > i + 2) { - pwm.values[i] = atoi(argv[i+2]); - if (pwm.values[i] < 900 || pwm.values[i] > 1200) { - errx(1, "value out of range of 900 < value < 1200. Aborting."); - } - pwm.channel_count++; - } - } - - int ret = ioctl(iofd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm); - - if (ret != OK) - errx(ret, "failed setting min values"); - } else { - errx(1, "not loaded"); - } - exit(0); - } - - if (!strcmp(argv[1], "max")) { - - if (argc < 3) { - errx(1, "max command needs at least one channel value (PWM)"); - } - - int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); - struct pwm_output_values pwm; - - if (iofd > 0) { - - pwm.channel_count = 0; - - for (int i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++) - { - /* set channel to commanline argument or to 2100 for non-provided channels */ - if (argc > i + 2) { - pwm.values[i] = atoi(argv[i+2]); - if (pwm.values[i] < 1800 || pwm.values[i] > 2100) { - errx(1, "value out of range of 1800 < value < 2100. Aborting."); - } - pwm.channel_count++; - } - } - - int ret = ioctl(iofd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm); - - if (ret != OK) - errx(ret, "failed setting max values"); - } else { - errx(1, "not loaded"); - } - exit(0); - } - - if (!strcmp(argv[1], "idle") || !strcmp(argv[1], "disarmed")) { - - if (argc < 3) { - errx(1, "max command needs at least one channel value (PWM)"); - } - - int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); - struct pwm_output_values pwm; - - if (iofd > 0) { - - pwm.channel_count = 0; - - for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++) - { - /* set channel to commanline argument or to 0 for non-provided channels */ - if (argc > i + 2) { - pwm.values[i] = atoi(argv[i+2]); - if (pwm.values[i] < 900 || pwm.values[i] > 2100) { - errx(1, "value out of range of 900 < value < 2100. Aborting."); - } - pwm.channel_count++; - } - } - - int ret = ioctl(iofd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm); - - if (ret != OK) - errx(ret, "failed setting idle values"); - } else { - errx(1, "not loaded"); - } - exit(0); - } if (!strcmp(argv[1], "recovery")) { diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index c42a36c7f3..58df9cd159 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -71,16 +72,25 @@ usage(const char *reason) warnx("%s", reason); errx(1, "usage:\n" - "pwm [-v] [-d ] [-u ] [-c ] [-m chanmask ] [arm|disarm] [ ...]\n" - " -v Print information about the PWM device\n" - " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" - " PWM update rate for channels in \n" - " Channel group that should update at the alternate rate (may be specified more than once)\n" - " arm | disarm Arm or disarm the ouptut\n" - " ... PWM output values in microseconds to assign to the PWM outputs\n" - " Directly supply alt rate channel mask (debug use only)\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" "\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" + "set ... Directly set PWM values\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" + "\n" + "config min ... Configure minimum PWM values\n" + "config max ... Configure maximum PWM values\n" + "config disarmed ... Configure disarmed PWM values\n" + "\n" + "info Print information about the PWM device\n" ); } @@ -92,100 +102,53 @@ 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; int ch; int ret; char *ep; - unsigned group; int32_t set_mask = -1; + unsigned group; - if (argc < 2) + if (argc < 1) usage(NULL); - while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) { + while ((ch = getopt(argc, argv, "v:d:")) != EOF) { switch (ch) { - 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; - break; case 'd': + if (NULL == strstr(optarg, "/dev/")) { + warnx("device %s not valid", optarg); + usage(NULL); + } dev = optarg; - break; - - case 'u': - alt_rate = strtol(optarg, &ep, 0); - if (*ep != '\0') - usage("bad alt_rate value"); - break; - - case 'm': - set_mask = strtol(optarg, &ep, 0); - if (*ep != '\0') - usage("bad set_mask value"); + argv+=2; + argc-=2; break; case 'v': - print_info = true; + print_verbose = true; + argv+=1; + argc-=1; break; default: - usage(NULL); + break; } } - argc -= optind; - argv += optind; + + /* get rid of cmd name */ + argv+=1; + argc-=1; /* 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)"); - } - /* directly supplied channel mask */ - if (set_mask != -1) { - ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask); - if (ret != OK) - err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); - } + for (int argi=0; argi sizeof(channel) / sizeof(channel[0])) - err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0])); - channel[nchannels] = pwm_value; - nchannels++; + warnx("Outputs disarmed"); + exit(0); - continue; - } - usage("unrecognized option"); - } + } else if (!strcmp(argv[argi], "set")) { - /* 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"); + /* iterate remaining arguments */ + unsigned nchannels = 0; + unsigned channel[PWM_OUTPUT_MAX_CHANNELS] = {0}; - /* print current servo values */ - for (unsigned i = 0; i < count; i++) { - servo_position_t spos; + while (argc - argi > 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]); - ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); - if (ret == OK) { - printf("channel %u: %uus\n", i, spos); + nchannels++; + + continue; + } + usage("unrecognized option"); + } + + /* perform PWM output */ + if (nchannels) { + + /* 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 < 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; + 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 { - printf("%u: ERROR\n", i); + usage("no PWM values supplied"); } - } - /* print rate groups */ - for (unsigned i = 0; i < count; i++) { - uint32_t group_mask; + } else if (!strcmp(argv[argi], "config")) { - ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); - if (ret != OK) + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + + argi++; + + if (!strcmp(argv[argi], "rate")) { + + while ((ch = getopt(argc, argv, "m:c:")) != EOF) { + switch (ch) { + + case 'm': + set_mask = strtol(optarg, &ep, 0); + if (*ep != '\0') + usage("bad set_mask value"); + break; + argi+=2; + + 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; - if (group_mask != 0) { - printf("channel group %u: channels", i); - for (unsigned j = 0; j < 32; j++) - if (group_mask & (1 << j)) - printf(" %u", j); - printf("\n"); - } - } - fflush(stdout); - } - /* perform PWM output */ - if (nchannels) { + /* 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)"); + } - /* 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"); + /* directly supplied channel mask */ + if (set_mask != -1) { + ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask); + if (ret != OK) + err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); + } - warnx("Press CTRL-C or 'c' to abort."); + /* assign alternate rate to channel groups */ + if (alt_channels_set) { + uint32_t mask = 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); + for (group = 0; group < 32; group++) { + if ((1 << group) & alt_channel_groups) { + uint32_t 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_SELECT_UPDATE_RATE, mask); + if (ret != OK) + err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); + } + + + } else if (!strcmp(argv[argi], "min")) { + + /* 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++; + + continue; + } + usage("unrecognized option"); + } + 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) + errx(ret, "failed setting idle values"); + } + + + } else if (!strcmp(argv[argi], "max")) { + + /* 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++; + + continue; + } + usage("unrecognized option"); + } + 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) + errx(ret, "failed setting idle values"); + } + + + } else if (!strcmp(argv[argi], "disarmed")) { + + /* 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++; + + continue; + } + usage("unrecognized option"); + } + 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) + errx(ret, "failed setting idle values"); + } + + } else { + usage("specify rate, min, max or disarmed"); } - /* 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); + } 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"); + + /* print current servo values */ + 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("channel %u: %uus\n", i, spos); + } else { + printf("%u: ERROR\n", i); } } - /* rate limit to ~ 20 Hz */ - usleep(50000); + /* print rate groups */ + 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("channel group %u: channels", i); + for (unsigned j = 0; j < 32; j++) + if (group_mask & (1 << j)) + printf(" %u", j); + printf("\n"); + } + } + + } else { + usage("specify arm|disarm|set|config"); } } - exit(0); -} \ No newline at end of file +} + + +