From 8131d28a0faf7d33060cf067f5bd8dee41666fed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 21:35:50 +0200 Subject: Exported disarmed PWM values as IOCTLs --- src/drivers/px4io/px4io.cpp | 110 ++++++++++++++++++++++++++------------------ 1 file changed, 64 insertions(+), 46 deletions(-) (limited to 'src/drivers/px4io/px4io.cpp') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c88abe59a..bd5f33043 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -177,21 +177,6 @@ public: */ int set_failsafe_values(const uint16_t *vals, unsigned len); - /** - * Set the minimum PWM signals when armed - */ - int set_min_values(const uint16_t *vals, unsigned len); - - /** - * Set the maximum PWM signal when armed - */ - int set_max_values(const uint16_t *vals, unsigned len); - - /** - * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE - */ - int set_idle_values(const uint16_t *vals, unsigned len); - /** * Print IO status. * @@ -391,6 +376,21 @@ private: */ int mixer_send(const char *buf, unsigned buflen); + /** + * Set the minimum PWM signals when armed + */ + int set_min_values(const uint16_t *vals, unsigned len); + + /** + * Set the maximum PWM signal when armed + */ + int set_max_values(const uint16_t *vals, unsigned len); + + /** + * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE + */ + int set_idle_values(const uint16_t *vals, unsigned len); + /** * Handle a status update from IO. * @@ -1674,6 +1674,24 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; } + case PWM_SERVO_SET_DISARMED_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + set_idle_values(pwm->values, pwm->channel_count); + } + break; + + case PWM_SERVO_SET_MIN_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + set_min_values(pwm->values, pwm->channel_count); + } + break; + + case PWM_SERVO_SET_MAX_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + set_max_values(pwm->values, pwm->channel_count); + } + break; + case PWM_SERVO_GET_COUNT: *(unsigned *)arg = _max_actuators; break; @@ -2265,26 +2283,26 @@ px4io_main(int argc, char *argv[]) errx(1, "min command needs at least one channel value (PWM)"); } - if (g_dev != nullptr) { + int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); + struct pwm_output_values pwm; - /* set values for first 8 channels, fill unassigned channels with 900. */ - uint16_t min[8]; + if (iofd > 0) { - for (unsigned i = 0; i < sizeof(min) / sizeof(min[0]); i++) + 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) { - min[i] = atoi(argv[i+2]); - if (min[i] < 900 || min[i] > 1200) { + 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."); } - } else { - /* a zero value will the default */ - min[i] = 0; + pwm.channel_count++; } } - int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0])); + int ret = ioctl(iofd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm); if (ret != OK) errx(ret, "failed setting min values"); @@ -2300,26 +2318,26 @@ px4io_main(int argc, char *argv[]) errx(1, "max command needs at least one channel value (PWM)"); } - if (g_dev != nullptr) { + int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); + struct pwm_output_values pwm; + + if (iofd > 0) { - /* set values for first 8 channels, fill unassigned channels with 2100. */ - uint16_t max[8]; + pwm.channel_count = 0; - for (unsigned i = 0; i < sizeof(max) / sizeof(max[0]); i++) + 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) { - max[i] = atoi(argv[i+2]); - if (max[i] < 1800 || max[i] > 2100) { + 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."); } - } else { - /* a zero value will the default */ - max[i] = 0; + pwm.channel_count++; } } - int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0])); + int ret = ioctl(iofd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm); if (ret != OK) errx(ret, "failed setting max values"); @@ -2329,32 +2347,32 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "idle")) { + if (!strcmp(argv[1], "idle") || !strcmp(argv[1], "disarmed")) { if (argc < 3) { errx(1, "max command needs at least one channel value (PWM)"); } - if (g_dev != nullptr) { + int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); + struct pwm_output_values pwm; + + if (iofd > 0) { - /* set values for first 8 channels, fill unassigned channels with 0. */ - uint16_t idle[8]; + pwm.channel_count = 0; - for (unsigned i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) + 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) { - idle[i] = atoi(argv[i+2]); - if (idle[i] < 900 || idle[i] > 2100) { + 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."); } - } else { - /* a zero value will the default */ - idle[i] = 0; + pwm.channel_count++; } } - int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0])); + int ret = ioctl(iofd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm); if (ret != OK) errx(ret, "failed setting idle values"); -- cgit v1.2.3 From baa49080547d740959f96174023a9cd5312924f1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 4 Oct 2013 13:00:12 +0200 Subject: Changes to pwm systemcmd, basic functionality there, needs polishing --- src/drivers/px4io/px4io.cpp | 104 +---------- src/systemcmds/pwm/pwm.c | 431 +++++++++++++++++++++++++++++--------------- 2 files changed, 287 insertions(+), 248 deletions(-) (limited to 'src/drivers/px4io/px4io.cpp') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0fed99692..add20c551 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 c42a36c7f..58df9cd15 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"); - } - - /* assign alternate rate to channel groups */ - if (alt_channels_set) { - uint32_t mask = 0; - for (unsigned 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; - } - } + 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")) { + + /* iterate remaining arguments */ + unsigned nchannels = 0; + unsigned channel[PWM_OUTPUT_MAX_CHANNELS] = {0}; - /* 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 */ - 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); + 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]); + + 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"); + + /* 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"); + } + + /* assign alternate rate to channel groups */ + if (alt_channels_set) { + uint32_t mask = 0; + + 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"); } - } - fflush(stdout); - } - /* perform PWM output */ - if (nchannels) { + } else if (!strcmp(argv[argi], "info")) { - /* 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"); + /* 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"); - warnx("Press CTRL-C or 'c' to abort."); + /* print current servo values */ + for (unsigned i = 0; i < count; i++) { + servo_position_t spos; - 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); + 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); + } } - /* 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); + /* 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"); } } - /* rate limit to ~ 20 Hz */ - usleep(50000); + } else { + usage("specify arm|disarm|set|config"); } } - exit(0); -} \ No newline at end of file +} + + + -- cgit v1.2.3 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/drivers/px4io/px4io.cpp | 2 - src/systemcmds/pwm/pwm.c | 430 +++++++++++++++++++++++--------------------- 2 files changed, 222 insertions(+), 210 deletions(-) (limited to 'src/drivers/px4io/px4io.cpp') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index add20c551..ae5728af0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -957,8 +957,6 @@ PX4IO::set_idle_values(const uint16_t *vals, unsigned len) /* fail with error */ return E2BIG; - printf("Sending IDLE values\n"); - /* copy values to registers in IO */ return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len); } 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 From ea0aa49b546476ef9ca9904b32dc507d66f0ab44 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Oct 2013 16:24:49 +0200 Subject: pwm info provides more information, some fixes for setting rate/min/max/disarmed --- src/drivers/drv_pwm_output.h | 38 ++- src/drivers/hil/hil.cpp | 2 +- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- src/drivers/px4fmu/fmu.cpp | 14 +- src/drivers/px4io/px4io.cpp | 48 +++- src/modules/px4iofirmware/mixer.cpp | 4 +- src/modules/px4iofirmware/protocol.h | 4 +- src/modules/px4iofirmware/px4io.h | 2 +- src/modules/px4iofirmware/registers.c | 55 +++-- src/systemcmds/pwm/pwm.c | 419 ++++++++++++++++++---------------- 10 files changed, 345 insertions(+), 243 deletions(-) (limited to 'src/drivers/px4io/px4io.cpp') diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index fc916b522..76e98597a 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -101,38 +101,56 @@ ORB_DECLARE(output_pwm); /** disarm all servo outputs (stop generating pulses) */ #define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1) +/** get default servo update rate */ +#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) + /** set alternate servo update rate */ -#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) +#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 3) + +/** get alternate servo update rate */ +#define PWM_SERVO_GET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4) /** get the number of servos in *(unsigned *)arg */ -#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3) +#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 5) /** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */ -#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4) +#define PWM_SERVO_SET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 6) + +/** check the selected update rates */ +#define PWM_SERVO_GET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 7) /** set the 'ARM ok' bit, which activates the safety switch */ -#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 5) +#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 8) /** clear the 'ARM ok' bit, which deactivates the safety switch */ -#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6) +#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 9) /** start DSM bind */ -#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) +#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 10) #define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ #define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ /** power up DSM receiver */ -#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) +#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11) /** set the PWM value when disarmed - should be no PWM (zero) by default */ -#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 9) +#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 12) + +/** get the PWM value when disarmed */ +#define PWM_SERVO_GET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 13) /** set the minimum PWM value the output will send */ -#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 10) +#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 14) + +/** get the minimum PWM value the output will send */ +#define PWM_SERVO_GET_MIN_PWM _IOC(_PWM_SERVO_BASE, 15) /** set the maximum PWM value the output will send */ -#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 11) +#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 16) + +/** get the maximum PWM value the output will send */ +#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 17) /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 3e30e3292..6563c3446 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -517,7 +517,7 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) g_hil->set_pwm_rate(arg); break; - case PWM_SERVO_SELECT_UPDATE_RATE: + case PWM_SERVO_SET_SELECT_UPDATE_RATE: // HIL always outputs at the alternate (usually faster) rate break; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index d0de26a1a..b93f38cf6 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1071,7 +1071,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = OK; break; - case PWM_SERVO_SELECT_UPDATE_RATE: + case PWM_SERVO_SET_SELECT_UPDATE_RATE: ret = OK; break; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b1dd55dd7..3fc75eb29 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -685,14 +685,26 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) up_pwm_servo_arm(false); break; + case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: + *(uint32_t *)arg = _pwm_default_rate; + break; + case PWM_SERVO_SET_UPDATE_RATE: ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg); break; - case PWM_SERVO_SELECT_UPDATE_RATE: + case PWM_SERVO_GET_UPDATE_RATE: + *(uint32_t *)arg = _pwm_alt_rate; + break; + + case PWM_SERVO_SET_SELECT_UPDATE_RATE: ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); break; + case PWM_SERVO_GET_SELECT_UPDATE_RATE: + *(uint32_t *)arg = _pwm_alt_rate_channels; + break; + case PWM_SERVO_SET(5): case PWM_SERVO_SET(4): if (_mode < MODE_6PWM) { diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ae5728af0..ea3a73822 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -958,7 +958,7 @@ PX4IO::set_idle_values(const uint16_t *vals, unsigned len) return E2BIG; /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len); + return io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, vals, len); } @@ -1684,7 +1684,7 @@ PX4IO::print_status() printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); printf("\nidle values"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); + printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i)); printf("\n"); } @@ -1716,12 +1716,22 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0); break; + case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: + /* get the default update rate */ + *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE); + break; + case PWM_SERVO_SET_UPDATE_RATE: /* set the requested alternate rate */ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); break; - case PWM_SERVO_SELECT_UPDATE_RATE: { + case PWM_SERVO_GET_UPDATE_RATE: + /* get the alternative update rate */ + *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE); + break; + + case PWM_SERVO_SET_SELECT_UPDATE_RATE: { /* blindly clear the PWM update alarm - might be set for some other reason */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); @@ -1738,23 +1748,51 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; } + case PWM_SERVO_GET_SELECT_UPDATE_RATE: + + *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES); + break; + case PWM_SERVO_SET_DISARMED_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; set_idle_values(pwm->values, pwm->channel_count); + break; } + + case PWM_SERVO_GET_DISARMED_PWM: + + ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t)); + if (ret != OK) { + ret = -EIO; + } break; 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; } + + case PWM_SERVO_GET_MIN_PWM: + + ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t)); + if (ret != OK) { + ret = -EIO; + } break; case PWM_SERVO_SET_MAX_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; set_max_values(pwm->values, pwm->channel_count); + break; } + + case PWM_SERVO_GET_MAX_PWM: + + ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t)); + if (ret != OK) { + ret = -EIO; + } break; case PWM_SERVO_GET_COUNT: @@ -2448,5 +2486,5 @@ px4io_main(int argc, char *argv[]) bind(argc, argv); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'bind' or 'update'"); } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 30aff7d20..350b93488 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -307,9 +307,9 @@ mixer_tick(void) up_pwm_servo_set(i, r_page_servos[i]); } else if (mixer_servos_armed && should_always_enable_pwm) { - /* set the idle servo outputs. */ + /* set the disarmed servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) - up_pwm_servo_set(i, r_page_servo_idle[i]); + up_pwm_servo_set(i, r_page_servo_disarmed[i]); } } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 0e2cd1689..5e5396782 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -220,8 +220,8 @@ enum { /* DSM bind states */ /* PWM maximum values for certain ESCs */ #define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ -/* PWM idle values that are active, even when SAFETY_SAFE */ -#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +/* PWM disarmed values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 66c4ca906..6c9007a75 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -80,7 +80,7 @@ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ -extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */ +extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ /* * Register aliases. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 8cb21e54f..9d3c5ccfd 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -220,10 +220,10 @@ uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100 /** * PAGE 108 * - * idle PWM values for difficult ESCs + * disarmed PWM values for difficult ESCs * */ -uint16_t r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; +uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) @@ -293,16 +293,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - if (*values == 0) - /* set to default */ - r_page_servo_control_min[offset] = 900; - - else if (*values > 1200) + if (*values == 0) { + /* ignore 0 */ + } else if (*values > 1200) { r_page_servo_control_min[offset] = 1200; - else if (*values < 900) + } else if (*values < 900) { r_page_servo_control_min[offset] = 900; - else + } else { r_page_servo_control_min[offset] = *values; + } offset++; num_values--; @@ -315,16 +314,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - if (*values == 0) - /* set to default */ - r_page_servo_control_max[offset] = 2100; - - else if (*values > 2100) + if (*values == 0) { + /* ignore 0 */ + } else if (*values > 2100) { r_page_servo_control_max[offset] = 2100; - else if (*values < 1800) + } else if (*values < 1800) { r_page_servo_control_max[offset] = 1800; - else + } else { r_page_servo_control_max[offset] = *values; + } offset++; num_values--; @@ -332,21 +330,20 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; - case PX4IO_PAGE_IDLE_PWM: + case PX4IO_PAGE_DISARMED_PWM: /* copy channel data */ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - if (*values == 0) - /* set to default */ - r_page_servo_idle[offset] = 0; - - else if (*values < 900) - r_page_servo_idle[offset] = 900; - else if (*values > 2100) - r_page_servo_idle[offset] = 2100; - else - r_page_servo_idle[offset] = *values; + if (*values == 0) { + /* ignore 0 */ + } else if (*values < 900) { + r_page_servo_disarmed[offset] = 900; + } else if (*values > 2100) { + r_page_servo_disarmed[offset] = 2100; + } else { + r_page_servo_disarmed[offset] = *values; + } /* flag the failsafe values as custom */ r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; @@ -769,8 +766,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_CONTROL_MAX_PWM: SELECT_PAGE(r_page_servo_control_max); break; - case PX4IO_PAGE_IDLE_PWM: - SELECT_PAGE(r_page_servo_idle); + case PX4IO_PAGE_DISARMED_PWM: + SELECT_PAGE(r_page_servo_disarmed); break; default: diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 44e49dd05..25f8c4e75 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -77,21 +77,23 @@ usage(const char *reason) " arm Arm output\n" " disarm Disarm output\n" "\n" - " rate Configure PWM rates\n" - " [-c ] Channel group that should update at the alternate rate\n" + " rate Configure PWM rates\n" + " [-g ] 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" - " min ... Configure minimum PWM values\n" - " max ... Configure maximum PWM values\n" - " disarmed ... Configure disarmed PWM values\n" - " [-m ] Directly supply channel mask\n" + " min ... Configure minimum PWM values\n" + " max ... Configure maximum PWM values\n" + " disarmed ... Configure disarmed PWM values\n" + " [-c ] Supply channels (e.g. 1234)\n" + " [-m ] Directly supply channel mask (e.g. 0xF)\n" " [-a] Configure all outputs\n" " -p PWM value\n" "\n" " test ... Directly set PWM values\n" - " [-m ] Directly supply channel mask\n" + " [-c ] Supply channels (e.g. 1234)\n" + " [-m ] Directly supply channel mask (e.g. 0xF)\n" " [-a] Configure all outputs\n" " -p PWM value\n" "\n" @@ -123,7 +125,7 @@ pwm_main(int argc, char *argv[]) if (argc < 1) usage(NULL); - while ((ch = getopt(argc-1, &argv[1], "d:vc:m:ap:r:")) != EOF) { + while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { switch (ch) { case 'd': @@ -140,7 +142,7 @@ pwm_main(int argc, char *argv[]) case 'c': /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ - channels = strtol(optarg, &ep, 0); + channels = strtoul(optarg, &ep, 0); while ((single_ch = channels % 10)) { @@ -155,11 +157,12 @@ pwm_main(int argc, char *argv[]) usage("bad channel_group value"); alt_channel_groups |= (1 << group); alt_channels_set = true; + warnx("alt channels set, group: %d", group); break; case 'm': /* Read in mask directly */ - set_mask = strtol(optarg, &ep, 0); + set_mask = strtoul(optarg, &ep, 0); if (*ep != '\0') usage("bad set_mask value"); break; @@ -170,12 +173,12 @@ pwm_main(int argc, char *argv[]) } break; case 'p': - pwm_value = strtol(optarg, &ep, 0); + pwm_value = strtoul(optarg, &ep, 0); if (*ep != '\0') usage("bad PWM value provided"); break; case 'r': - alt_rate = strtol(optarg, &ep, 0); + alt_rate = strtoul(optarg, &ep, 0); if (*ep != '\0') usage("bad alternative rate provided"); break; @@ -205,241 +208,275 @@ pwm_main(int argc, char *argv[]) if (ret != OK) err(1, "PWM_SERVO_GET_COUNT"); - int argi = 1; /* leave away cmd name */ - - while(argi 0) { + ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); if (ret != OK) - err(1, "PWM_SERVO_ARM"); - - if (print_verbose) - warnx("Outputs armed"); - exit(0); + err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); + } - } else if (!strcmp(argv[argi], "disarm")) { - /* disarm, but do not revoke the SET_ARM_OK flag */ - ret = ioctl(fd, PWM_SERVO_DISARM, 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_DISARM"); + err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); + } - if (print_verbose) - warnx("Outputs disarmed"); - exit(0); + /* assign alternate rate to channel groups */ + if (alt_channels_set) { + uint32_t mask = 0; - }else if (!strcmp(argv[argi], "rate")) { + for (group = 0; group < 32; group++) { + if ((1 << group) & alt_channel_groups) { + uint32_t group_mask; - /* 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"); - } + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); + if (ret != OK) + err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); - /* 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"); + mask |= group_mask; + } } - /* assign alternate rate to channel groups */ - if (alt_channels_set) { - uint32_t mask = 0; + ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, mask); + if (ret != OK) + err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); + } + exit(0); - for (group = 0; group < 32; group++) { - if ((1 << group) & alt_channel_groups) { - uint32_t group_mask; + } else if (!strcmp(argv[1], "min")) { - ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); - if (ret != OK) - err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); - mask |= group_mask; - } - } + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; - ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask); - if (ret != OK) - err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1< 0) { - ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values); - if (ret != OK) - errx(ret, "failed setting idle values"); + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("User abort\n"); + exit(0); + } } - exit(0); + } + exit(0); - } else if (!strcmp(argv[argi], "test")) { - if (set_mask == 0) { - usage("no channels set"); - } - if (pwm_value == 0) - usage("no PWM value provided"); + } else if (!strcmp(argv[1], "info")) { - /* perform PWM output */ + printf("device: %s\n", dev); - /* Open console directly to grab CTRL-C signal */ - struct pollfd fds; - fds.fd = 0; /* stdin */ - fds.events = POLLIN; + uint32_t info_default_rate; + uint32_t info_alt_rate; + uint32_t info_alt_rate_mask; - warnx("Press CTRL-C or 'c' to abort."); + 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"); + } - while (1) { - for (unsigned i = 0; i < servo_count; i++) { - if (set_mask & 1< 0) { + 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"); + } - read(0, &c, 1); - if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("User abort\n"); - exit(0); - } - } - } - exit(0); + 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"); + } - } else if (!strcmp(argv[argi], "info")) { + /* print current servo values */ + for (unsigned i = 0; i < servo_count; i++) { + servo_position_t spos; - /* print current servo values */ - 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: %u us", i+1, 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); - } - } + if (info_alt_rate_mask & (1< Date: Mon, 7 Oct 2013 18:03:05 +0200 Subject: Trying to get rid of magic PWM numbers --- src/drivers/drv_pwm_output.h | 20 ++++++++++++++++++++ src/drivers/px4io/px4io.cpp | 6 +++--- src/modules/px4iofirmware/mixer.cpp | 4 ++-- src/modules/px4iofirmware/registers.c | 30 +++++++++++++++--------------- 4 files changed, 40 insertions(+), 20 deletions(-) (limited to 'src/drivers/px4io/px4io.cpp') diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 76e98597a..5a3a126d0 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -64,6 +64,26 @@ __BEGIN_DECLS */ #define PWM_OUTPUT_MAX_CHANNELS 16 +/** + * Minimum PWM in us + */ +#define PWM_MIN 900 + +/** + * Highest PWM allowed as the minimum PWM + */ +#define PWM_HIGHEST_MIN 1300 + +/** + * Maximum PWM in us + */ +#define PWM_MAX 2100 + +/** + * Lowest PWM allowed as the maximum PWM + */ +#define PWM_LOWEST_MAX 1700 + /** * Servo output signal type, value is actual servo output pulse * width in microseconds. diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ea3a73822..f9dc3773e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1818,7 +1818,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) /* TODO: we could go lower for e.g. TurboPWM */ unsigned channel = cmd - PWM_SERVO_SET(0); - if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) { + if ((channel >= _max_actuators) || (arg < PWM_MIN) || (arg > PWM_MAX)) { ret = -EINVAL; } else { /* send a direct PWM value */ @@ -2402,8 +2402,8 @@ px4io_main(int argc, char *argv[]) /* set channel to commandline argument or to 900 for non-provided channels */ if (argc > i + 2) { failsafe[i] = atoi(argv[i+2]); - if (failsafe[i] < 800 || failsafe[i] > 2200) { - errx(1, "value out of range of 800 < value < 2200. Aborting."); + if (failsafe[i] < PWM_MIN || failsafe[i] > PWM_AX) { + errx(1, "value out of range of %d < value < %d. Aborting.", PWM_MIN, PWM_MAX); } } else { /* a zero value will result in stopping to output any pulse */ diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 350b93488..352b93e85 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -242,8 +242,8 @@ mixer_tick(void) case ESC_RAMP: r_page_servos[i] = (outputs[i] - * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000 - + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000); + * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*PWM_MAX - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*PWM_MIN)/2/1000 + + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*PWM_MAX + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*PWM_MIN)/2/1000); break; case ESC_ON: diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 9d3c5ccfd..30e6f7de2 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -207,7 +207,7 @@ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 }; * minimum PWM values when armed * */ -uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; +uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN }; /** * PAGE 107 @@ -215,7 +215,7 @@ uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 90 * maximum PWM values when armed * */ -uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 }; +uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX }; /** * PAGE 108 @@ -223,7 +223,7 @@ uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100 * disarmed PWM values for difficult ESCs * */ -uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; +uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) @@ -295,10 +295,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* ignore 0 */ - } else if (*values > 1200) { - r_page_servo_control_min[offset] = 1200; - } else if (*values < 900) { - r_page_servo_control_min[offset] = 900; + } else if (*values > PWM_HIGHEST_MIN) { + r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; + } else if (*values < PWM_MIN) { + r_page_servo_control_min[offset] = PWM_MIN; } else { r_page_servo_control_min[offset] = *values; } @@ -316,10 +316,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* ignore 0 */ - } else if (*values > 2100) { - r_page_servo_control_max[offset] = 2100; - } else if (*values < 1800) { - r_page_servo_control_max[offset] = 1800; + } else if (*values > PWM_MAX) { + r_page_servo_control_max[offset] = PWM_MAX; + } else if (*values < PWM_LOWEST_MAX) { + r_page_servo_control_max[offset] = PWM_LOWEST_MAX; } else { r_page_servo_control_max[offset] = *values; } @@ -337,10 +337,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* ignore 0 */ - } else if (*values < 900) { - r_page_servo_disarmed[offset] = 900; - } else if (*values > 2100) { - r_page_servo_disarmed[offset] = 2100; + } else if (*values < PWM_MIN) { + r_page_servo_disarmed[offset] = PWM_MIN; + } else if (*values > PWM_MAX) { + r_page_servo_disarmed[offset] = PWM_MAX; } else { r_page_servo_disarmed[offset] = *values; } -- cgit v1.2.3 From 1b9e2af7425615130ddbcf79b89859c97a791a9c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 8 Oct 2013 17:03:57 +0200 Subject: Moved PWM ramp to systemlib --- src/drivers/px4io/px4io.cpp | 2 +- src/modules/px4iofirmware/mixer.cpp | 135 ++++++---------------------- src/modules/px4iofirmware/module.mk | 1 + src/modules/px4iofirmware/px4io.c | 6 ++ src/modules/px4iofirmware/px4io.h | 7 ++ src/modules/systemlib/pwm_limit/pwm_limit.c | 116 ++++++++++++++++++++++++ src/modules/systemlib/pwm_limit/pwm_limit.h | 79 ++++++++++++++++ 7 files changed, 237 insertions(+), 109 deletions(-) create mode 100644 src/modules/systemlib/pwm_limit/pwm_limit.c create mode 100644 src/modules/systemlib/pwm_limit/pwm_limit.h (limited to 'src/drivers/px4io/px4io.cpp') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f9dc3773e..18c9ef0bd 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2402,7 +2402,7 @@ px4io_main(int argc, char *argv[]) /* set channel to commandline argument or to 900 for non-provided channels */ if (argc > i + 2) { failsafe[i] = atoi(argv[i+2]); - if (failsafe[i] < PWM_MIN || failsafe[i] > PWM_AX) { + if (failsafe[i] < PWM_MIN || failsafe[i] > PWM_MAX) { errx(1, "value out of range of %d < value < %d. Aborting.", PWM_MIN, PWM_MAX); } } else { diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 352b93e85..5232f9b96 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -47,6 +47,7 @@ #include #include +#include #include extern "C" { @@ -59,12 +60,6 @@ extern "C" { */ #define FMU_INPUT_DROP_LIMIT_US 200000 -/* - * Time that the ESCs need to initialize - */ - #define ESC_INIT_TIME_US 1000000 - #define ESC_RAMP_TIME_US 2000000 - /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 #define PITCH 1 @@ -76,15 +71,6 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; -static uint64_t esc_init_time; - -enum esc_state_e { - ESC_OFF, - ESC_INIT, - ESC_RAMP, - ESC_ON -}; -static esc_state_e esc_state; /* selected control values and count for mixing */ enum mixer_source { @@ -165,6 +151,30 @@ mixer_tick(void) r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); } + /* + * Decide whether the servos should be armed right now. + * + * We must be armed, and we must have a PWM source; either raw from + * FMU or from the mixer. + * + * XXX correct behaviour for failsafe may require an additional case + * here. + */ + should_arm = ( + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + ) + ); + + should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + /* * Run the mixers. */ @@ -184,107 +194,17 @@ mixer_tick(void) float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; - uint16_t ramp_promille; - - /* update esc init state, but only if we are truely armed and not just PWM enabled */ - if (mixer_servos_armed && should_arm) { - - switch (esc_state) { - - /* after arming, some ESCs need an initalization period, count the time from here */ - case ESC_OFF: - esc_init_time = hrt_absolute_time(); - esc_state = ESC_INIT; - break; - - /* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */ - case ESC_INIT: - if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) { - esc_state = ESC_RAMP; - } - break; - - /* then ramp until the min speed is reached */ - case ESC_RAMP: - if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) { - esc_state = ESC_ON; - } - break; - - case ESC_ON: - default: - - break; - } - } else { - esc_state = ESC_OFF; - } - - /* do the calculations during the ramp for all at once */ - if (esc_state == ESC_RAMP) { - ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US; - } - - /* mix */ mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); - /* scale to PWM and update the servo outputs as required */ - for (unsigned i = 0; i < mixed; i++) { - - /* save actuator values for FMU readback */ - r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - - switch (esc_state) { - case ESC_INIT: - r_page_servos[i] = (outputs[i] * 600 + 1500); - break; - - case ESC_RAMP: - r_page_servos[i] = (outputs[i] - * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*PWM_MAX - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*PWM_MIN)/2/1000 - + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*PWM_MAX + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*PWM_MIN)/2/1000); - break; + pwm_limit.nchannels = mixed; - case ESC_ON: - r_page_servos[i] = (outputs[i] - * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 - + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); - break; + pwm_limit_calc(should_arm, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); - case ESC_OFF: - default: - break; - } - } for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; } - /* - * Decide whether the servos should be armed right now. - * - * We must be armed, and we must have a PWM source; either raw from - * FMU or from the mixer. - * - * XXX correct behaviour for failsafe may require an additional case - * here. - */ - should_arm = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and FMU is armed */ && ( - ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) - /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) - /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) - ) - ); - - should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); - if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); @@ -298,7 +218,6 @@ mixer_tick(void) mixer_servos_armed = false; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); isr_debug(5, "> PWM disabled"); - } if (mixer_servos_armed && should_arm) { diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 59f470a94..01869569f 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -14,6 +14,7 @@ SRCS = adc.c \ ../systemlib/mixer/mixer_group.cpp \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ + ../systemlib/pwm_limit/pwm_limit.c ifeq ($(BOARD),px4io-v1) SRCS += i2c.c diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index e70b3fe88..71d649029 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -50,6 +50,7 @@ #include #include +#include #include @@ -64,6 +65,8 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; +pwm_limit_t pwm_limit; + /* * a set of debug buffers to allow us to send debug information from ISRs */ @@ -174,6 +177,9 @@ user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); + /* initialize PWM limit lib */ + pwm_limit_init(&pwm_limit); + #if 0 /* not enough memory, lock down */ if (minfo.mxordblk < 500) { diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 6c9007a75..4fea0288c 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -46,6 +46,8 @@ #include "protocol.h" +#include + /* * Constants and limits. */ @@ -122,6 +124,11 @@ struct sys_state_s { extern struct sys_state_s system_state; +/* + * PWM limit structure + */ +extern pwm_limit_t pwm_limit; + /* * GPIO handling. */ diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c new file mode 100644 index 000000000..675247578 --- /dev/null +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Julian Oes + * + * 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_limit.c + * + * Lib to limit PWM output + * + * @author Julian Oes + */ + +#include "pwm_limit.h" +#include +#include +#include + +__EXPORT pwm_limit_init(pwm_limit_t *limit) +{ + limit->nchannels = 0; + limit->state = LIMIT_STATE_OFF; + limit->time_armed = 0; + return; +} + +__EXPORT void pwm_limit_calc(const bool armed, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output_requested, uint16_t *effective_pwm, pwm_limit_t *limit) +{ + /* first evaluate state changes */ + switch (limit->state) { + case LIMIT_STATE_OFF: + if (armed) + limit->state = LIMIT_STATE_RAMP; + limit->time_armed = hrt_absolute_time(); + break; + case LIMIT_STATE_INIT: + if (!armed) + limit->state = LIMIT_STATE_OFF; + else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US) + limit->state = LIMIT_STATE_RAMP; + break; + case LIMIT_STATE_RAMP: + if (!armed) + limit->state = LIMIT_STATE_OFF; + else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US) + limit->state = LIMIT_STATE_ON; + break; + case LIMIT_STATE_ON: + if (!armed) + limit->state = LIMIT_STATE_OFF; + break; + default: + break; + } + + unsigned progress; + uint16_t temp_pwm; + + /* then set effective_pwm based on state */ + switch (limit->state) { + case LIMIT_STATE_OFF: + case LIMIT_STATE_INIT: + for (unsigned i=0; inchannels; i++) { + effective_pwm[i] = disarmed_pwm[i]; + } + break; + case LIMIT_STATE_RAMP: + + progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US; + for (unsigned i=0; inchannels; i++) { + + temp_pwm = output_requested[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; + /* already follow user/controller input if higher than min_pwm */ + effective_pwm[i] = (disarmed_pwm[i]*(10000-progress) + (temp_pwm > min_pwm[i] ? temp_pwm : min_pwm[i])*progress)/10000; + + } + break; + case LIMIT_STATE_ON: + for (unsigned i=0; inchannels; i++) { + effective_pwm[i] = output_requested[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; + } + break; + default: + break; + } + return; +} diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h new file mode 100644 index 000000000..f8c6370e8 --- /dev/null +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Julian Oes + * + * 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_limit.h + * + * Lib to limit PWM output + * + * @author Julian Oes + */ + +#ifndef PWM_LIMIT_H_ +#define PWM_LIMIT_H_ + +#include +#include + +__BEGIN_DECLS + +/* + * time for the ESCs to initialize + * (this is not actually needed if PWM is sent right after boot) + */ +#define INIT_TIME_US 500000 +/* + * time to slowly ramp up the ESCs + */ +#define RAMP_TIME_US 2500000 + +typedef struct { + enum { + LIMIT_STATE_OFF = 0, + LIMIT_STATE_INIT, + LIMIT_STATE_RAMP, + LIMIT_STATE_ON + } state; + uint64_t time_armed; + unsigned nchannels; +} pwm_limit_t; + +__EXPORT void pwm_limit_init(pwm_limit_t *limit); + +__EXPORT void pwm_limit_calc(const bool armed, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output_requested, uint16_t *effective_pwm, pwm_limit_t *limit); + + +__END_DECLS + +#endif /* PWM_LIMIT_H_ */ -- cgit v1.2.3 From 6b079f41b27442d2efbe5467be2be0d8607b6746 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 10:21:37 +0200 Subject: Small warning fix --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/drivers/px4io/px4io.cpp') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 18c9ef0bd..8a6390ad7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2400,7 +2400,7 @@ px4io_main(int argc, char *argv[]) for (unsigned i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) { /* set channel to commandline argument or to 900 for non-provided channels */ - if (argc > i + 2) { + if ((unsigned)argc > i + 2) { failsafe[i] = atoi(argv[i+2]); if (failsafe[i] < PWM_MIN || failsafe[i] > PWM_MAX) { errx(1, "value out of range of %d < value < %d. Aborting.", PWM_MIN, PWM_MAX); -- cgit v1.2.3 From 1d3f25ee6c9983ec5da9de4d4f7b463f880f3a87 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 19 Oct 2013 10:43:41 +0200 Subject: pwm systemcmd can now set the failsafe values, fmu uses failsafe values as well now, fix to only send the appropriate number of pwm values to IO at once --- src/drivers/drv_pwm_output.h | 18 ++- src/drivers/px4fmu/fmu.cpp | 202 +++++++++++++++++++++------------- src/drivers/px4io/px4io.cpp | 128 +++++++-------------- src/modules/px4iofirmware/registers.c | 13 ++- src/systemcmds/pwm/pwm.c | 46 +++++++- 5 files changed, 233 insertions(+), 174 deletions(-) (limited to 'src/drivers/px4io/px4io.cpp') diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 3357e67a5..f0dd713b2 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -154,23 +154,29 @@ ORB_DECLARE(output_pwm); /** power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11) +/** set the PWM value for failsafe */ +#define PWM_SERVO_SET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 12) + +/** get the PWM value for failsafe */ +#define PWM_SERVO_GET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 13) + /** set the PWM value when disarmed - should be no PWM (zero) by default */ -#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 12) +#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 14) /** get the PWM value when disarmed */ -#define PWM_SERVO_GET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 13) +#define PWM_SERVO_GET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 15) /** set the minimum PWM value the output will send */ -#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 14) +#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 16) /** get the minimum PWM value the output will send */ -#define PWM_SERVO_GET_MIN_PWM _IOC(_PWM_SERVO_BASE, 15) +#define PWM_SERVO_GET_MIN_PWM _IOC(_PWM_SERVO_BASE, 17) /** set the maximum PWM value the output will send */ -#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 16) +#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 18) /** get the maximum PWM value the output will send */ -#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 17) +#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index e729612cc..b7b2f7b33 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-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 @@ -78,6 +78,12 @@ # include #endif +/* + * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side + */ + +#define CONTROL_INPUT_DROP_LIMIT_MS 20 + class PX4FMU : public device::CDev { public: @@ -130,9 +136,11 @@ private: actuator_controls_s _controls; pwm_limit_t _pwm_limit; + uint16_t _failsafe_pwm[_max_actuators]; uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; uint16_t _max_pwm[_max_actuators]; + unsigned _num_failsafe_set; unsigned _num_disarmed_set; static void task_main_trampoline(int argc, char *argv[]); @@ -218,7 +226,9 @@ PX4FMU::PX4FMU() : _armed(false), _pwm_on(false), _mixers(nullptr), + _failsafe_pwm({0}), _disarmed_pwm({0}), + _num_failsafe_set(0), _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { @@ -515,98 +525,103 @@ PX4FMU::task_main() /* sleep waiting for data, stopping to check for PPM * input at 100Hz */ - int ret = ::poll(&fds[0], 2, 10); + int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS); /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); usleep(1000000); continue; - } + } else if (ret == 0) { + /* timeout: no control data, switch to failsafe values */ +// warnx("no PWM: failsafe"); - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { - - /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); - - /* can we mix? */ - if (_mixers != nullptr) { - - unsigned num_outputs; - - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; - case MODE_4PWM: - num_outputs = 4; - break; - case MODE_6PWM: - num_outputs = 6; - break; - default: - num_outputs = 0; - break; - } + } else { - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); - outputs.timestamp = hrt_absolute_time(); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ - if (i >= outputs.noutputs || - !isfinite(outputs.output[i]) || - outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = -1.0f; + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { + + /* get controls - must always do this to avoid spinning */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + + /* can we mix? */ + if (_mixers != nullptr) { + + unsigned num_outputs; + + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + case MODE_4PWM: + num_outputs = 4; + break; + case MODE_6PWM: + num_outputs = 6; + break; + default: + num_outputs = 0; + break; } - } - uint16_t pwm_limited[num_outputs]; + /* do mixing */ + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (i >= outputs.noutputs || + !isfinite(outputs.output[i]) || + outputs.output[i] < -1.0f || + outputs.output[i] > 1.0f) { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = -1.0f; + } + } - pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + uint16_t pwm_limited[num_outputs]; - /* output actual limited values */ - for (unsigned i = 0; i < num_outputs; i++) { - controls_effective.control_effective[i] = (float)pwm_limited[i]; - } - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); + pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); - /* output to the servos */ - for (unsigned i = 0; i < num_outputs; i++) { - up_pwm_servo_set(i, pwm_limited[i]); - } + /* output actual limited values */ + for (unsigned i = 0; i < num_outputs; i++) { + controls_effective.control_effective[i] = (float)pwm_limited[i]; + } + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); - /* and publish for anyone that cares to see */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + /* output to the servos */ + for (unsigned i = 0; i < num_outputs; i++) { + up_pwm_servo_set(i, pwm_limited[i]); + } + + /* and publish for anyone that cares to see */ + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + } } - } - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + /* how about an arming update? */ + if (fds[1].revents & POLLIN) { + actuator_armed_s aa; - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); + /* get new value */ + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); - /* update the armed status and check that we're not locked down */ - bool set_armed = aa.armed && !aa.lockdown; - if (_armed != set_armed) - _armed = set_armed; + /* update the armed status and check that we're not locked down */ + bool set_armed = aa.armed && !aa.lockdown; + if (_armed != set_armed) + _armed = set_armed; - /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (aa.armed || _num_disarmed_set > 0); - if (_pwm_on != pwm_on) { - _pwm_on = pwm_on; - up_pwm_servo_arm(pwm_on); + /* update PWM status if armed or if disarmed PWM values are set */ + bool pwm_on = (aa.armed || _num_disarmed_set > 0); + if (_pwm_on != pwm_on) { + _pwm_on = pwm_on; + up_pwm_servo_arm(pwm_on); + } } } @@ -737,6 +752,45 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) *(uint32_t *)arg = _pwm_alt_rate_channels; break; + case PWM_SERVO_SET_FAILSAFE_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] > PWM_MAX) { + _failsafe_pwm[i] = PWM_MAX; + } else if (pwm->values[i] < PWM_MIN) { + _failsafe_pwm[i] = PWM_MIN; + } else { + _failsafe_pwm[i] = pwm->values[i]; + } + } + + /* + * update the counter + * this is needed to decide if disarmed PWM output should be turned on or not + */ + _num_failsafe_set = 0; + for (unsigned i = 0; i < _max_actuators; i++) { + if (_failsafe_pwm[i] > 0) + _num_failsafe_set++; + } + break; + } + case PWM_SERVO_GET_FAILSAFE_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _failsafe_pwm[i]; + } + pwm->channel_count = _max_actuators; + break; + } + case PWM_SERVO_SET_DISARMED_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values*)arg; /* discard if too many values are sent */ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d709f1cc8..b603ffc8d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -943,53 +943,6 @@ PX4IO::io_set_control_state() return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); } -int -PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) -{ - if (len > _max_actuators) - /* fail with error */ - return E2BIG; - - /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len); -} - -int -PX4IO::set_min_values(const uint16_t *vals, unsigned len) -{ - - if (len > _max_actuators) - /* fail with error */ - return E2BIG; - - /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len); -} - -int -PX4IO::set_max_values(const uint16_t *vals, unsigned len) -{ - - if (len > _max_actuators) - /* fail with error */ - return E2BIG; - - /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len); -} - -int -PX4IO::set_idle_values(const uint16_t *vals, unsigned len) -{ - - if (len > _max_actuators) - /* fail with error */ - return E2BIG; - - /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, vals, len); -} - int PX4IO::io_set_arming_state() @@ -1803,7 +1756,7 @@ PX4IO::print_status() printf("failsafe"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); - printf("\nidle values"); + printf("\ndisarmed values"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i)); printf("\n"); @@ -1874,15 +1827,39 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES); break; + case PWM_SERVO_SET_FAILSAFE_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count); + break; + } + + case PWM_SERVO_GET_FAILSAFE_PWM: + + ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t*)arg, _max_actuators); + if (ret != OK) { + ret = -EIO; + } + break; + case PWM_SERVO_SET_DISARMED_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - set_idle_values(pwm->values, pwm->channel_count); + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count); break; } case PWM_SERVO_GET_DISARMED_PWM: - ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t)); + ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, _max_actuators); if (ret != OK) { ret = -EIO; } @@ -1890,13 +1867,18 @@ 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; - set_min_values(pwm->values, pwm->channel_count); + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count); break; } case PWM_SERVO_GET_MIN_PWM: - ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t)); + ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, _max_actuators); if (ret != OK) { ret = -EIO; } @@ -1904,13 +1886,18 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case PWM_SERVO_SET_MAX_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - set_max_values(pwm->values, pwm->channel_count); + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count); break; } case PWM_SERVO_GET_MAX_PWM: - ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t)); + ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, _max_actuators); if (ret != OK) { ret = -EIO; } @@ -2518,37 +2505,6 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "failsafe")) { - - if (argc < 3) { - errx(1, "failsafe command needs at least one channel value (ppm)"); - } - - /* set values for first 8 channels, fill unassigned channels with 1500. */ - uint16_t failsafe[8]; - - for (unsigned i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) { - - /* set channel to commandline argument or to 900 for non-provided channels */ - if ((unsigned)argc > i + 2) { - failsafe[i] = atoi(argv[i+2]); - if (failsafe[i] < PWM_MIN || failsafe[i] > PWM_MAX) { - errx(1, "value out of range of %d < value < %d. Aborting.", PWM_MIN, PWM_MAX); - } - } else { - /* a zero value will result in stopping to output any pulse */ - failsafe[i] = 0; - } - } - - int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0])); - - if (ret != OK) - errx(ret, "failed setting failsafe values"); - exit(0); - } - - if (!strcmp(argv[1], "recovery")) { @@ -2616,5 +2572,5 @@ px4io_main(int argc, char *argv[]) bind(argc, argv); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'bind' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'"); } diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a338afe16..6a0532bee 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -199,7 +199,7 @@ uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI * * Disable pulses as default. */ -uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 }; +uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; /** * PAGE 106 @@ -276,8 +276,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - /* XXX range-check value? */ - r_page_servo_failsafe[offset] = *values; + if (*values == 0) { + /* ignore 0 */ + } else if (*values < PWM_MIN) { + r_page_servo_failsafe[offset] = PWM_MIN; + } else if (*values > PWM_MAX) { + r_page_servo_failsafe[offset] = PWM_MAX; + } else { + r_page_servo_failsafe[offset] = *values; + } /* flag the failsafe values as custom */ r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 25f8c4e75..3185e4371 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -72,20 +72,21 @@ usage(const char *reason) warnx("%s", reason); errx(1, "usage:\n" - "pwm arm|disarm|rate|min|max|disarmed|test|info ...\n" + "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n" "\n" " arm Arm output\n" " disarm Disarm output\n" "\n" - " rate Configure PWM rates\n" + " rate ... Configure PWM rates\n" " [-g ] 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" + " failsafe ... Configure failsafe PWM values\n" + " disarmed ... Configure disarmed PWM values\n" " min ... Configure minimum PWM values\n" " max ... Configure maximum PWM values\n" - " disarmed ... Configure disarmed PWM values\n" " [-c ] Supply channels (e.g. 1234)\n" " [-m ] Directly supply channel mask (e.g. 0xF)\n" " [-a] Configure all outputs\n" @@ -357,6 +358,35 @@ pwm_main(int argc, char *argv[]) } exit(0); + } else if (!strcmp(argv[1], "failsafe")) { + + 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<