From 8131d28a0faf7d33060cf067f5bd8dee41666fed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 21:35:50 +0200 Subject: [PATCH 01/35] Exported disarmed PWM values as IOCTLs --- src/drivers/drv_pwm_output.h | 12 +++- src/drivers/px4io/px4io.cpp | 110 ++++++++++++++++++++--------------- 2 files changed, 75 insertions(+), 47 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index ec9d4ca098..6ed9320cb1 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -79,6 +79,7 @@ typedef uint16_t servo_position_t; struct pwm_output_values { /** desired pulse widths for each of the supported channels */ servo_position_t values[PWM_OUTPUT_MAX_CHANNELS]; + int channel_count; }; /* @@ -118,9 +119,18 @@ ORB_DECLARE(output_pwm); /** start DSM bind */ #define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) -/** Power up DSM receiver */ +/** power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) +/** set the PWM value when disarmed - should be no PWM (zero) by default */ +#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 9) + +/** set the minimum PWM value the output will send */ +#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 10) + +/** set the maximum PWM value the output will send */ +#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 11) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c88abe59a8..bd5f330436 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; - /* set values for first 8 channels, fill unassigned channels with 2100. */ - uint16_t max[8]; + if (iofd > 0) { - for (unsigned i = 0; i < sizeof(max) / sizeof(max[0]); i++) + 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) { - 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; - /* set values for first 8 channels, fill unassigned channels with 0. */ - uint16_t idle[8]; + if (iofd > 0) { - for (unsigned i = 0; i < sizeof(idle) / sizeof(idle[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 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"); From baa49080547d740959f96174023a9cd5312924f1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 4 Oct 2013 13:00:12 +0200 Subject: [PATCH 02/35] Changes to pwm systemcmd, basic functionality there, needs polishing --- src/drivers/px4io/px4io.cpp | 104 +-------- src/systemcmds/pwm/pwm.c | 429 ++++++++++++++++++++++++------------ 2 files changed, 286 insertions(+), 247 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0fed99692b..add20c5511 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1748,6 +1748,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case PWM_SERVO_SET_MIN_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + warnx("Set min values"); set_min_values(pwm->values, pwm->channel_count); } break; @@ -2381,110 +2382,7 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "min")) { - if (argc < 3) { - errx(1, "min command needs at least one channel value (PWM)"); - } - - int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); - struct pwm_output_values pwm; - - if (iofd > 0) { - - pwm.channel_count = 0; - - for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++) - { - /* set channel to commanline argument or to 900 for non-provided channels */ - if (argc > i + 2) { - pwm.values[i] = atoi(argv[i+2]); - if (pwm.values[i] < 900 || pwm.values[i] > 1200) { - errx(1, "value out of range of 900 < value < 1200. Aborting."); - } - pwm.channel_count++; - } - } - - int ret = ioctl(iofd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm); - - if (ret != OK) - errx(ret, "failed setting min values"); - } else { - errx(1, "not loaded"); - } - exit(0); - } - - if (!strcmp(argv[1], "max")) { - - if (argc < 3) { - errx(1, "max command needs at least one channel value (PWM)"); - } - - int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); - struct pwm_output_values pwm; - - if (iofd > 0) { - - pwm.channel_count = 0; - - for (int i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++) - { - /* set channel to commanline argument or to 2100 for non-provided channels */ - if (argc > i + 2) { - pwm.values[i] = atoi(argv[i+2]); - if (pwm.values[i] < 1800 || pwm.values[i] > 2100) { - errx(1, "value out of range of 1800 < value < 2100. Aborting."); - } - pwm.channel_count++; - } - } - - int ret = ioctl(iofd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm); - - if (ret != OK) - errx(ret, "failed setting max values"); - } else { - errx(1, "not loaded"); - } - exit(0); - } - - if (!strcmp(argv[1], "idle") || !strcmp(argv[1], "disarmed")) { - - if (argc < 3) { - errx(1, "max command needs at least one channel value (PWM)"); - } - - int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); - struct pwm_output_values pwm; - - if (iofd > 0) { - - pwm.channel_count = 0; - - for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++) - { - /* set channel to commanline argument or to 0 for non-provided channels */ - if (argc > i + 2) { - pwm.values[i] = atoi(argv[i+2]); - if (pwm.values[i] < 900 || pwm.values[i] > 2100) { - errx(1, "value out of range of 900 < value < 2100. Aborting."); - } - pwm.channel_count++; - } - } - - int ret = ioctl(iofd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm); - - if (ret != OK) - errx(ret, "failed setting idle values"); - } else { - errx(1, "not loaded"); - } - exit(0); - } if (!strcmp(argv[1], "recovery")) { diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index c42a36c7f3..58df9cd159 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -71,16 +72,25 @@ usage(const char *reason) warnx("%s", reason); errx(1, "usage:\n" - "pwm [-v] [-d ] [-u ] [-c ] [-m chanmask ] [arm|disarm] [ ...]\n" - " -v Print information about the PWM device\n" - " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" - " PWM update rate for channels in \n" - " Channel group that should update at the alternate rate (may be specified more than once)\n" - " arm | disarm Arm or disarm the ouptut\n" - " ... PWM output values in microseconds to assign to the PWM outputs\n" - " Directly supply alt rate channel mask (debug use only)\n" + "pwm [-v] [-d ] set|config|arm|disarm|info ...\n" + "" + " -v Print verbose information\n" + " -d PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" "\n" - "When -c is specified, any channel groups not listed with -c will update at the default rate.\n" + "arm Arm output\n" + "disarm Disarm output\n" + "\n" + "set ... Directly set PWM values\n" + "\n" + "config rate Configure PWM rates\n" + " [-c ] Channel group that should update at the alternate rate\n" + " [-m ] Directly supply alt rate channel mask\n" + "\n" + "config min ... Configure minimum PWM values\n" + "config max ... Configure maximum PWM values\n" + "config disarmed ... Configure disarmed PWM values\n" + "\n" + "info Print information about the PWM device\n" ); } @@ -92,100 +102,53 @@ pwm_main(int argc, char *argv[]) unsigned alt_rate = 0; uint32_t alt_channel_groups = 0; bool alt_channels_set = false; - bool print_info = false; + bool print_verbose = false; int ch; int ret; char *ep; - unsigned group; int32_t set_mask = -1; + unsigned group; - if (argc < 2) + if (argc < 1) usage(NULL); - while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) { + while ((ch = getopt(argc, argv, "v:d:")) != EOF) { switch (ch) { - case 'c': - group = strtoul(optarg, &ep, 0); - if ((*ep != '\0') || (group >= 32)) - usage("bad channel_group value"); - alt_channel_groups |= (1 << group); - alt_channels_set = true; - break; case 'd': + if (NULL == strstr(optarg, "/dev/")) { + warnx("device %s not valid", optarg); + usage(NULL); + } dev = optarg; - break; - - case 'u': - alt_rate = strtol(optarg, &ep, 0); - if (*ep != '\0') - usage("bad alt_rate value"); - break; - - case 'm': - set_mask = strtol(optarg, &ep, 0); - if (*ep != '\0') - usage("bad set_mask value"); + argv+=2; + argc-=2; break; case 'v': - print_info = true; + print_verbose = true; + argv+=1; + argc-=1; break; default: - usage(NULL); + break; } } - argc -= optind; - argv += optind; + + /* get rid of cmd name */ + argv+=1; + argc-=1; /* open for ioctl only */ int fd = open(dev, 0); if (fd < 0) err(1, "can't open %s", dev); - /* change alternate PWM rate */ - if (alt_rate > 0) { - ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); - if (ret != OK) - err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); - } - /* directly supplied channel mask */ - if (set_mask != -1) { - ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask); - if (ret != OK) - err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); - } + for (int argi=0; argi sizeof(channel) / sizeof(channel[0])) - err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0])); - channel[nchannels] = pwm_value; - nchannels++; + warnx("Outputs disarmed"); + exit(0); - continue; - } - usage("unrecognized option"); - } + } else if (!strcmp(argv[argi], "set")) { - /* print verbose info */ - if (print_info) { - /* get the number of servo channels */ - unsigned count; - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count); - if (ret != OK) - err(1, "PWM_SERVO_GET_COUNT"); + /* iterate remaining arguments */ + unsigned nchannels = 0; + unsigned channel[PWM_OUTPUT_MAX_CHANNELS] = {0}; - /* print current servo values */ - for (unsigned i = 0; i < count; i++) { - servo_position_t spos; + while (argc - argi > 1) { + argi++; + unsigned pwm_value = strtol(argv[argi], &ep, 0); + if (*ep == '\0') { + if (nchannels > sizeof(channel) / sizeof(channel[0])) + err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0])); + //XXX check for sane values ? + channel[nchannels] = pwm_value; + if (print_verbose) + warnx("Set channel %d: %d us", nchannels+1, channel[nchannels]); - ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); - if (ret == OK) { - printf("channel %u: %uus\n", i, spos); + nchannels++; + + continue; + } + usage("unrecognized option"); + } + + /* perform PWM output */ + if (nchannels) { + + /* Open console directly to grab CTRL-C signal */ + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + + warnx("Press CTRL-C or 'c' to abort."); + + while (1) { + for (unsigned i = 0; i < nchannels; i++) { + ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]); + if (ret != OK) + err(1, "PWM_SERVO_SET(%d)", i); + } + + /* abort on user request */ + char c; + ret = poll(&fds, 1, 0); + if (ret > 0) { + + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("User abort\n"); + exit(0); + } + } + } } else { - printf("%u: ERROR\n", i); + usage("no PWM values supplied"); } - } - /* print rate groups */ - for (unsigned i = 0; i < count; i++) { - uint32_t group_mask; + } else if (!strcmp(argv[argi], "config")) { - ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); - if (ret != OK) + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + + argi++; + + if (!strcmp(argv[argi], "rate")) { + + while ((ch = getopt(argc, argv, "m:c:")) != EOF) { + switch (ch) { + + case 'm': + set_mask = strtol(optarg, &ep, 0); + if (*ep != '\0') + usage("bad set_mask value"); + break; + argi+=2; + + case 'c': + group = strtoul(optarg, &ep, 0); + if ((*ep != '\0') || (group >= 32)) + usage("bad channel_group value"); + alt_channel_groups |= (1 << group); + alt_channels_set = true; + argi+=2; + break; + } + } + argi++; + if (argi >= argc) + usage("no alt_rate value supplied"); + + alt_rate = strtol(argv[argi], &ep, 0); + if (*ep != '\0') + usage("bad alt_rate value"); break; - if (group_mask != 0) { - printf("channel group %u: channels", i); - for (unsigned j = 0; j < 32; j++) - if (group_mask & (1 << j)) - printf(" %u", j); - printf("\n"); - } - } - fflush(stdout); - } - /* perform PWM output */ - if (nchannels) { + /* change alternate PWM rate */ + if (alt_rate > 0) { + ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); + if (ret != OK) + err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); + } - /* Open console directly to grab CTRL-C signal */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - if (!console) - err(1, "failed opening console"); + /* directly supplied channel mask */ + if (set_mask != -1) { + ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask); + if (ret != OK) + err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); + } - warnx("Press CTRL-C or 'c' to abort."); + /* assign alternate rate to channel groups */ + if (alt_channels_set) { + uint32_t mask = 0; - while (1) { - for (int i = 0; i < nchannels; i++) { - ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]); - if (ret != OK) - err(1, "PWM_SERVO_SET(%d)", i); + for (group = 0; group < 32; group++) { + if ((1 << group) & alt_channel_groups) { + uint32_t group_mask; + + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); + if (ret != OK) + err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); + + mask |= group_mask; + } + } + + ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask); + if (ret != OK) + err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); + } + + + } else if (!strcmp(argv[argi], "min")) { + + /* iterate remaining arguments */ + while (argc - argi > 1) { + argi++; + unsigned pwm_value = strtol(argv[argi], &ep, 0); + if (*ep == '\0') { + if (pwm_values.channel_count > PWM_OUTPUT_MAX_CHANNELS) + err(1, "too many pwm values (max %d)", PWM_OUTPUT_MAX_CHANNELS); + //XXX check for sane values ? + pwm_values.values[pwm_values.channel_count] = pwm_value; + pwm_values.channel_count++; + + continue; + } + usage("unrecognized option"); + } + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + } else { + + ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + if (ret != OK) + errx(ret, "failed setting idle values"); + } + + + } else if (!strcmp(argv[argi], "max")) { + + /* iterate remaining arguments */ + while (argc - argi > 1) { + argi++; + unsigned pwm_value = strtol(argv[argi], &ep, 0); + if (*ep == '\0') { + if (pwm_values.channel_count > PWM_OUTPUT_MAX_CHANNELS) + err(1, "too many pwm values (max %d)", PWM_OUTPUT_MAX_CHANNELS); + //XXX check for sane values ? + pwm_values.values[pwm_values.channel_count] = pwm_value; + pwm_values.channel_count++; + + continue; + } + usage("unrecognized option"); + } + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + } else { + + ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + if (ret != OK) + errx(ret, "failed setting idle values"); + } + + + } else if (!strcmp(argv[argi], "disarmed")) { + + /* iterate remaining arguments */ + while (argc - argi > 1) { + argi++; + unsigned pwm_value = strtol(argv[argi], &ep, 0); + if (*ep == '\0') { + if (pwm_values.channel_count > PWM_OUTPUT_MAX_CHANNELS) + err(1, "too many pwm values (max %d)", PWM_OUTPUT_MAX_CHANNELS); + //XXX check for sane values ? + pwm_values.values[pwm_values.channel_count] = pwm_value; + pwm_values.channel_count++; + + continue; + } + usage("unrecognized option"); + } + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + } else { + + ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values); + if (ret != OK) + errx(ret, "failed setting idle values"); + } + + } else { + usage("specify rate, min, max or disarmed"); } - /* abort on user request */ - char c; - if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("User abort\n"); - close(console); - exit(0); + } else if (!strcmp(argv[argi], "info")) { + + /* get the number of servo channels */ + unsigned count; + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count); + if (ret != OK) + err(1, "PWM_SERVO_GET_COUNT"); + + /* print current servo values */ + for (unsigned i = 0; i < count; i++) { + servo_position_t spos; + + ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); + if (ret == OK) { + printf("channel %u: %uus\n", i, spos); + } else { + printf("%u: ERROR\n", i); } } - /* rate limit to ~ 20 Hz */ - usleep(50000); + /* print rate groups */ + for (unsigned i = 0; i < count; i++) { + uint32_t group_mask; + + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); + if (ret != OK) + break; + if (group_mask != 0) { + printf("channel group %u: channels", i); + for (unsigned j = 0; j < 32; j++) + if (group_mask & (1 << j)) + printf(" %u", j); + printf("\n"); + } + } + + } else { + usage("specify arm|disarm|set|config"); } } - exit(0); -} \ No newline at end of file +} + + + From 9ff521711861fce857b6c17c2ec87eaa2073376e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 4 Oct 2013 17:20:34 +0200 Subject: [PATCH 03/35] Some more interface changes, needs testing and cleanup --- src/drivers/px4io/px4io.cpp | 2 - src/systemcmds/pwm/pwm.c | 496 ++++++++++++++++++------------------ 2 files changed, 255 insertions(+), 243 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index add20c5511..ae5728af00 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 58df9cd159..44e49dd054 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"); - } - - /* 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); - } - } - } + /* 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 PWM values supplied"); + usage("no alternative rate provided"); } - } else if (!strcmp(argv[argi], "config")) { + /* 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"); + } + + /* 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"); + } + exit(0); + + } else if (!strcmp(argv[argi], "min")) { + + 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}; - 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; - } + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1<= 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)"); - } - - /* 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"); } + 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"); + } + exit(0); + + } else if (!strcmp(argv[argi], "max")) { + + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); + + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1< 0) { + + 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"); - /* 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; } From ea0aa49b546476ef9ca9904b32dc507d66f0ab44 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Oct 2013 16:24:49 +0200 Subject: [PATCH 04/35] 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 | 521 ++++++++++++++------------ 10 files changed, 396 insertions(+), 294 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index fc916b5225..76e98597ae 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 3e30e32927..6563c3446e 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 d0de26a1ad..b93f38cf6b 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 b1dd55dd78..3fc75eb29f 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 ae5728af00..ea3a738224 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 30aff7d20a..350b93488e 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 0e2cd16898..5e5396782b 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 66c4ca9066..6c9007a75a 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 8cb21e54fb..9d3c5ccfd9 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 */ + if (*values == 0) { + /* ignore 0 */ + } else if (*values > 2100) { r_page_servo_control_max[offset] = 2100; - - 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 44e49dd054..25f8c4e753 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 */ + if (!strcmp(argv[1], "arm")) { + /* tell IO that its ok to disable its safety with the switch */ + ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + if (ret != OK) + err(1, "PWM_SERVO_SET_ARM_OK"); + /* tell IO that the system is armed (it will output values if safety is off) */ + ret = ioctl(fd, PWM_SERVO_ARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_ARM"); - while(argi 0) { + ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); if (ret != OK) - err(1, "PWM_SERVO_SET_ARM_OK"); - /* tell IO that the system is armed (it will output values if safety is off) */ - ret = ioctl(fd, PWM_SERVO_ARM, 0); - if (ret != OK) - err(1, "PWM_SERVO_ARM"); - - if (print_verbose) - warnx("Outputs armed"); - exit(0); - - } else if (!strcmp(argv[argi], "disarm")) { - /* disarm, but do not revoke the SET_ARM_OK flag */ - ret = ioctl(fd, PWM_SERVO_DISARM, 0); - if (ret != OK) - err(1, "PWM_SERVO_DISARM"); - - if (print_verbose) - warnx("Outputs disarmed"); - exit(0); - - }else if (!strcmp(argv[argi], "rate")) { - - /* change alternate PWM rate */ - if (alt_rate > 0) { - ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); - if (ret != OK) - err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); - } else { - usage("no alternative rate provided"); - } - - /* 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"); - } - - /* 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"); - } - exit(0); - - } else if (!strcmp(argv[argi], "min")) { - - 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< 0) { - - 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")) { - - /* 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: %uus\n", i, spos); - } else { - printf("%u: ERROR\n", i); - } - } - - /* print rate groups */ - for (unsigned i = 0; i < servo_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"); - } - } - exit(0); - + err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); } - argi++; + + /* directly supplied channel mask */ + if (set_mask > 0) { + ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask); + if (ret != OK) + err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); + } + + /* 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_SET_SELECT_UPDATE_RATE, mask); + if (ret != OK) + err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); + } + exit(0); + + } else if (!strcmp(argv[1], "min")) { + + 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< 0) { + + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("User abort\n"); + exit(0); + } + } + } + exit(0); + + + } else if (!strcmp(argv[1], "info")) { + + printf("device: %s\n", dev); + + uint32_t info_default_rate; + uint32_t info_alt_rate; + uint32_t info_alt_rate_mask; + + ret = ioctl(fd, PWM_SERVO_GET_DEFAULT_UPDATE_RATE, (unsigned long)&info_default_rate); + if (ret != OK) { + err(1, "PWM_SERVO_GET_DEFAULT_UPDATE_RATE"); + } + + ret = ioctl(fd, PWM_SERVO_GET_UPDATE_RATE, (unsigned long)&info_alt_rate); + if (ret != OK) { + err(1, "PWM_SERVO_GET_UPDATE_RATE"); + } + + ret = ioctl(fd, PWM_SERVO_GET_SELECT_UPDATE_RATE, (unsigned long)&info_alt_rate_mask); + if (ret != OK) { + err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE"); + } + + struct pwm_output_values disarmed_pwm; + struct pwm_output_values min_pwm; + struct pwm_output_values max_pwm; + + ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (unsigned long)&disarmed_pwm); + if (ret != OK) { + err(1, "PWM_SERVO_GET_DISARMED_PWM"); + } + ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (unsigned long)&min_pwm); + if (ret != OK) { + err(1, "PWM_SERVO_GET_MIN_PWM"); + } + ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (unsigned long)&max_pwm); + if (ret != OK) { + err(1, "PWM_SERVO_GET_MAX_PWM"); + } + + /* print current servo values */ + for (unsigned i = 0; i < 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); + + if (info_alt_rate_mask & (1< Date: Mon, 7 Oct 2013 16:34:07 +0200 Subject: [PATCH 05/35] Adapt startup scripts to new pwm systemcmd interface --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 12 ++++++++---- ROMFS/px4fmu_common/init.d/11_dji_f450 | 13 ++++++++----- ROMFS/px4fmu_common/init.d/15_tbs_discovery | 13 ++++++++----- ROMFS/px4fmu_common/init.d/16_3dr_iris | 13 ++++++++----- ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 2 +- .../init.d/rc.custom_dji_f330_mkblctrl | 10 ++++------ 6 files changed, 37 insertions(+), 26 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index d21759507d..e7a62a4b80 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -61,9 +61,6 @@ then sh /etc/init.d/rc.io # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 - px4io min 1200 1200 1200 1200 - px4io max 1800 1800 1800 1800 else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -81,7 +78,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index 388f40a477..91847b06bf 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -44,10 +44,6 @@ then usleep 5000 sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 - px4io min 1200 1200 1200 1200 - px4io max 1800 1800 1800 1800 else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -65,7 +61,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals (for DJI ESCs) +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery index cbfde6d3c0..65be56df8c 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -44,10 +44,6 @@ then usleep 5000 sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 - px4io min 1200 1200 1200 1200 - px4io max 1800 1800 1800 1800 else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -65,7 +61,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals (for DJI ESCs) +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index d47ecb3932..3434735fd7 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -44,10 +44,6 @@ then usleep 5000 sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 - px4io min 1200 1200 1200 1200 - px4io max 1800 1800 1800 1800 else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -65,7 +61,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 index ae41f2a014..eae37098bd 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -53,7 +53,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -c 1234 -r 400 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl index 51bc61c9e8..a63d0e5f1d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -83,10 +83,6 @@ then usleep 5000 sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 - px4io min 1200 1200 1200 1200 - px4io max 1800 1800 1800 1800 else fmu mode_pwm # Start MAVLink (on UART1 / ttyS0) @@ -107,9 +103,11 @@ else fi # -# Set PWM output frequency +# Set disarmed, min and max PWM signals # -#pwm -u 400 -m 0xff +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 # # Start common for all multirotors apps From 19879432ad6cf709af25192401829719defd2983 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Oct 2013 18:03:05 +0200 Subject: [PATCH 06/35] 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(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 76e98597ae..5a3a126d04 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 ea3a738224..f9dc3773e2 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 350b93488e..352b93e858 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 9d3c5ccfd9..30e6f7de27 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; } From 1b9e2af7425615130ddbcf79b89859c97a791a9c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 8 Oct 2013 17:03:57 +0200 Subject: [PATCH 07/35] Moved PWM ramp to systemlib --- src/drivers/px4io/px4io.cpp | 2 +- src/modules/px4iofirmware/mixer.cpp | 143 +++++--------------- 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, 241 insertions(+), 113 deletions(-) create mode 100644 src/modules/systemlib/pwm_limit/pwm_limit.c create mode 100644 src/modules/systemlib/pwm_limit/pwm_limit.h diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f9dc3773e2..18c9ef0bd0 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 352b93e858..5232f9b963 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,102 +151,6 @@ mixer_tick(void) r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); } - /* - * Run the mixers. - */ - if (source == MIX_FAILSAFE) { - - /* copy failsafe values to the servo outputs */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { - r_page_servos[i] = r_page_servo_failsafe[i]; - - /* safe actuators for FMU feedback */ - r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f; - } - - - } else if (source != MIX_NONE) { - - 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; - - 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; - - 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. * @@ -285,6 +175,36 @@ mixer_tick(void) && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + /* + * Run the mixers. + */ + if (source == MIX_FAILSAFE) { + + /* copy failsafe values to the servo outputs */ + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + r_page_servos[i] = r_page_servo_failsafe[i]; + + /* safe actuators for FMU feedback */ + r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f; + } + + + } else if (source != MIX_NONE) { + + float outputs[PX4IO_SERVO_COUNT]; + unsigned mixed; + + /* mix */ + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + + pwm_limit.nchannels = mixed; + + 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); + + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) + r_page_servos[i] = 0; + } + 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 59f470a94e..01869569f6 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 e70b3fe880..71d6490291 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 6c9007a75a..4fea0288c4 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 0000000000..6752475780 --- /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 0000000000..f8c6370e81 --- /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_ */ From b25b9d37d53d3caab7c7eb2eed6f5cdbdd3f8804 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Oct 2013 09:00:22 +0200 Subject: [PATCH 08/35] Small function definition correction --- src/modules/systemlib/pwm_limit/pwm_limit.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 6752475780..f67b5edf25 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -45,7 +45,7 @@ #include #include -__EXPORT pwm_limit_init(pwm_limit_t *limit) +__EXPORT void pwm_limit_init(pwm_limit_t *limit) { limit->nchannels = 0; limit->state = LIMIT_STATE_OFF; From e2fef6b374e14f8c0f383557cf3569f8265ba034 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 10:16:45 +0200 Subject: [PATCH 09/35] Use unsigned for channel counts --- src/drivers/drv_pwm_output.h | 2 +- src/drivers/hil/hil.cpp | 2 +- src/modules/uORB/topics/actuator_outputs.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 5a3a126d04..3357e67a50 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -99,7 +99,7 @@ typedef uint16_t servo_position_t; struct pwm_output_values { /** desired pulse widths for each of the supported channels */ servo_position_t values[PWM_OUTPUT_MAX_CHANNELS]; - int channel_count; + unsigned channel_count; }; /* diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 6563c3446e..c1d73dd87f 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -404,7 +404,7 @@ HIL::task_main() for (unsigned i = 0; i < num_outputs; i++) { /* last resort: catch NaN, INF and out-of-band errors */ - if (i < (unsigned)outputs.noutputs && + if (i < outputs.noutputs && isfinite(outputs.output[i]) && outputs.output[i] >= -1.0f && outputs.output[i] <= 1.0f) { diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h index 30895ca83f..4461404230 100644 --- a/src/modules/uORB/topics/actuator_outputs.h +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -60,7 +60,7 @@ struct actuator_outputs_s { uint64_t timestamp; /**< output timestamp in us since system boot */ float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ - int noutputs; /**< valid outputs */ + unsigned noutputs; /**< valid outputs */ }; /** From 3dc2bdfa22f0ac152392299628e037e3a6120c2e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 10:19:50 +0200 Subject: [PATCH 10/35] Changed pwm_limit interface a bit --- src/modules/px4iofirmware/mixer.cpp | 4 +--- src/modules/systemlib/pwm_limit/pwm_limit.c | 19 ++++++++++--------- src/modules/systemlib/pwm_limit/pwm_limit.h | 8 +++----- 3 files changed, 14 insertions(+), 17 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 5232f9b963..05897b4ce6 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -197,9 +197,7 @@ mixer_tick(void) /* mix */ mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); - pwm_limit.nchannels = mixed; - - 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); + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index f67b5edf25..3187a4fa2e 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -45,15 +45,14 @@ #include #include -__EXPORT void pwm_limit_init(pwm_limit_t *limit) +void 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) +void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { /* first evaluate state changes */ switch (limit->state) { @@ -89,24 +88,26 @@ __EXPORT void pwm_limit_calc(const bool armed, const uint16_t *disarmed_pwm, con switch (limit->state) { case LIMIT_STATE_OFF: case LIMIT_STATE_INIT: - for (unsigned i=0; inchannels; i++) { + for (unsigned i=0; itime_armed)*10000 / RAMP_TIME_US; - for (unsigned i=0; inchannels; i++) { + for (unsigned i=0; i min_pwm[i] ? temp_pwm : min_pwm[i])*progress)/10000; - + output[i] = (float)progress/10000.0f * output[i]; } 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; + for (unsigned i=0; i #include -__BEGIN_DECLS - /* * time for the ESCs to initialize * (this is not actually needed if PWM is sent right after boot) @@ -66,13 +64,13 @@ typedef struct { LIMIT_STATE_ON } state; uint64_t time_armed; - unsigned nchannels; } pwm_limit_t; +__BEGIN_DECLS + __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); - +__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit); __END_DECLS From 96111a67a65486ce8b99987e51fb11da54789379 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 10:21:22 +0200 Subject: [PATCH 11/35] Tought the fmu driver the new pwm limit interface --- src/drivers/px4fmu/fmu.cpp | 146 ++++++++++++++++++++++++++++++----- src/drivers/px4fmu/module.mk | 3 +- 2 files changed, 127 insertions(+), 22 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 3fc75eb29f..7f30487bff 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -59,11 +59,12 @@ #include #include -# include +#include #include #include #include +#include #include #include @@ -72,7 +73,7 @@ #include #include -#include + #ifdef HRT_PPM_CHANNEL # include #endif @@ -100,7 +101,7 @@ public: int set_pwm_alt_channels(uint32_t channels); private: - static const unsigned _max_actuators = 4; + static const unsigned _max_actuators = 6; Mode _mode; unsigned _pwm_default_rate; @@ -122,6 +123,11 @@ private: actuator_controls_s _controls; + pwm_limit_t _pwm_limit; + uint16_t _disarmed_pwm[_max_actuators]; + uint16_t _min_pwm[_max_actuators]; + uint16_t _max_pwm[_max_actuators]; + static void task_main_trampoline(int argc, char *argv[]); void task_main() __attribute__((noreturn)); @@ -203,7 +209,10 @@ PX4FMU::PX4FMU() : _primary_pwm_device(false), _task_should_exit(false), _armed(false), - _mixers(nullptr) + _mixers(nullptr), + _disarmed_pwm({0}), + _min_pwm({PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN}), + _max_pwm({PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX}) { _debug_enabled = true; } @@ -457,6 +466,9 @@ PX4FMU::task_main() rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; #endif + /* initialize PWM limit lib */ + pwm_limit_init(&_pwm_limit); + log("starting"); /* loop until killed */ @@ -530,32 +542,35 @@ PX4FMU::task_main() outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); outputs.timestamp = hrt_absolute_time(); - // XXX output actual limited values - memcpy(&controls_effective, &_controls, sizeof(controls_effective)); - - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); - /* 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) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - } else { + 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] = 900; + outputs.output[i] = -1.0f; } + } - /* output to the servo */ - up_pwm_servo_set(i, outputs.output[i]); + uint16_t pwm_limited[num_outputs]; + + pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + + /* 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); + + /* 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 */ @@ -705,6 +720,95 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) *(uint32_t *)arg = _pwm_alt_rate_channels; break; + case PWM_SERVO_SET_DISARMED_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) { + _disarmed_pwm[i] = PWM_MAX; + } else if (pwm->values[i] < PWM_MIN) { + _disarmed_pwm[i] = PWM_MIN; + } else { + _disarmed_pwm[i] = pwm->values[i]; + } + } + break; + } + case PWM_SERVO_GET_DISARMED_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _disarmed_pwm[i]; + } + pwm->channel_count = _max_actuators; + break; + } + + case PWM_SERVO_SET_MIN_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_HIGHEST_MIN) { + _min_pwm[i] = PWM_HIGHEST_MIN; + } else if (pwm->values[i] < PWM_MIN) { + _min_pwm[i] = PWM_MIN; + } else { + _min_pwm[i] = pwm->values[i]; + } + } + break; + } + case PWM_SERVO_GET_MIN_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _min_pwm[i]; + } + pwm->channel_count = _max_actuators; + arg = (unsigned long)&pwm; + break; + } + + case PWM_SERVO_SET_MAX_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_LOWEST_MAX) { + _max_pwm[i] = PWM_LOWEST_MAX; + } else if (pwm->values[i] > PWM_MAX) { + _max_pwm[i] = PWM_MAX; + } else { + _max_pwm[i] = pwm->values[i]; + } + } + break; + } + case PWM_SERVO_GET_MAX_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _max_pwm[i]; + } + pwm->channel_count = _max_actuators; + arg = (unsigned long)&pwm; + break; + } + case PWM_SERVO_SET(5): case PWM_SERVO_SET(4): if (_mode < MODE_6PWM) { @@ -1148,7 +1252,7 @@ test(void) } } else { // and use write interface for the other direction - int ret = write(fd, servos, sizeof(servos)); + ret = write(fd, servos, sizeof(servos)); if (ret != (int)sizeof(servos)) err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); } diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index 05bc7a5b33..d918abd572 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -3,4 +3,5 @@ # MODULE_COMMAND = fmu -SRCS = fmu.cpp +SRCS = fmu.cpp \ + ../../modules/systemlib/pwm_limit/pwm_limit.c From 6b079f41b27442d2efbe5467be2be0d8607b6746 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 10:21:37 +0200 Subject: [PATCH 12/35] Small warning fix --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 18c9ef0bd0..8a6390ad78 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); From 9cd3c40606f023a7943b1418a748abb046e36ecb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 10:48:49 +0200 Subject: [PATCH 13/35] Set the PWM values only once or continuous if specified --- src/systemcmds/pwm/pwm.c | 63 +++++++++++++++++++++++++--------------- 1 file changed, 40 insertions(+), 23 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 25f8c4e753..b3975de9d0 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -72,7 +72,7 @@ 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|min|max|disarmed|set|info ...\n" "\n" " arm Arm output\n" " disarm Disarm output\n" @@ -91,10 +91,11 @@ usage(const char *reason) " [-a] Configure all outputs\n" " -p PWM value\n" "\n" - " test ... Directly set PWM values\n" + " set ... Directly set PWM values\n" " [-c ] Supply channels (e.g. 1234)\n" " [-m ] Directly supply channel mask (e.g. 0xF)\n" " [-a] Configure all outputs\n" + " [-e] Repeat setting the values until stopped\n" " -p PWM value\n" "\n" " info Print information about the PWM device\n" @@ -113,6 +114,7 @@ pwm_main(int argc, char *argv[]) uint32_t alt_channel_groups = 0; bool alt_channels_set = false; bool print_verbose = false; + bool repeated_pwm_output = false; int ch; int ret; char *ep; @@ -125,7 +127,7 @@ pwm_main(int argc, char *argv[]) if (argc < 1) usage(NULL); - while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { + while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:e")) != EOF) { switch (ch) { case 'd': @@ -182,6 +184,9 @@ pwm_main(int argc, char *argv[]) if (*ep != '\0') usage("bad alternative rate provided"); break; + case 'e': + repeated_pwm_output = true; + break; default: break; } @@ -357,7 +362,7 @@ pwm_main(int argc, char *argv[]) } exit(0); - } else if (!strcmp(argv[1], "test")) { + } else if (!strcmp(argv[1], "set")) { if (set_mask == 0) { usage("no channels set"); @@ -367,14 +372,38 @@ pwm_main(int argc, char *argv[]) /* perform PWM output */ - /* Open console directly to grab CTRL-C signal */ - struct pollfd fds; - fds.fd = 0; /* stdin */ - fds.events = POLLIN; + if (repeated_pwm_output) { - warnx("Press CTRL-C or 'c' to abort."); + /* Open console directly to grab CTRL-C signal */ + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; - while (1) { + warnx("Press CTRL-C or 'c' to abort."); + + while (1) { + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1< 0) { + + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("User abort\n"); + exit(0); + } + } + } + } else { + /* only set the values once */ for (unsigned i = 0; i < servo_count; i++) { if (set_mask & 1< 0) { - - read(0, &c, 1); - if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("User abort\n"); - exit(0); - } - } } exit(0); @@ -476,7 +493,7 @@ pwm_main(int argc, char *argv[]) exit(0); } - usage("specify arm|disarm|rate|min|max|disarmed|test|info"); + usage("specify arm|disarm|rate|min|max|disarmed|set|info"); return 0; } From 326f241185f45d9e2d4377e8096a8a2f05f65b0d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 12:11:25 +0200 Subject: [PATCH 14/35] Enable PWM when disarmed on the fmu side --- src/drivers/px4fmu/fmu.cpp | 27 +++++++++++++++++++++++---- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 7f30487bff..8e9e8103ff 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -118,6 +118,7 @@ private: volatile bool _task_should_exit; bool _armed; + bool _pwm_on; MixerGroup *_mixers; @@ -127,6 +128,7 @@ private: uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; uint16_t _max_pwm[_max_actuators]; + unsigned _num_disarmed_set; static void task_main_trampoline(int argc, char *argv[]); void task_main() __attribute__((noreturn)); @@ -209,10 +211,12 @@ PX4FMU::PX4FMU() : _primary_pwm_device(false), _task_should_exit(false), _armed(false), + _pwm_on(false), _mixers(nullptr), _disarmed_pwm({0}), _min_pwm({PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN}), - _max_pwm({PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX}) + _max_pwm({PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX}), + _num_disarmed_set(0) { _debug_enabled = true; } @@ -585,11 +589,16 @@ PX4FMU::task_main() /* get new value */ orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); - /* update PWM servo armed status if armed and not locked down */ + /* update the armed status and check that we're not locked down */ bool set_armed = aa.armed && !aa.lockdown; - if (set_armed != _armed) { + if (_armed != set_armed) _armed = set_armed; - up_pwm_servo_arm(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); } } @@ -738,6 +747,16 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) _disarmed_pwm[i] = pwm->values[i]; } } + + /* + * update the counter + * this is needed to decide if disarmed PWM output should be turned on or not + */ + _num_disarmed_set = 0; + for (unsigned i = 0; i < _max_actuators; i++) { + if (_disarmed_pwm[i] > 0) + _num_disarmed_set++; + } break; } case PWM_SERVO_GET_DISARMED_PWM: { From 3cbe1ee1a8308e2efc017374a6d297761c6c5226 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 16:33:52 +0200 Subject: [PATCH 15/35] Revert "Set the PWM values only once or continuous if specified" This reverts commit 9cd3c40606f023a7943b1418a748abb046e36ecb. --- src/systemcmds/pwm/pwm.c | 63 +++++++++++++++------------------------- 1 file changed, 23 insertions(+), 40 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index b3975de9d0..25f8c4e753 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -72,7 +72,7 @@ usage(const char *reason) warnx("%s", reason); errx(1, "usage:\n" - "pwm arm|disarm|rate|min|max|disarmed|set|info ...\n" + "pwm arm|disarm|rate|min|max|disarmed|test|info ...\n" "\n" " arm Arm output\n" " disarm Disarm output\n" @@ -91,11 +91,10 @@ usage(const char *reason) " [-a] Configure all outputs\n" " -p PWM value\n" "\n" - " set ... Directly set PWM values\n" + " test ... Directly set PWM values\n" " [-c ] Supply channels (e.g. 1234)\n" " [-m ] Directly supply channel mask (e.g. 0xF)\n" " [-a] Configure all outputs\n" - " [-e] Repeat setting the values until stopped\n" " -p PWM value\n" "\n" " info Print information about the PWM device\n" @@ -114,7 +113,6 @@ pwm_main(int argc, char *argv[]) uint32_t alt_channel_groups = 0; bool alt_channels_set = false; bool print_verbose = false; - bool repeated_pwm_output = false; int ch; int ret; char *ep; @@ -127,7 +125,7 @@ pwm_main(int argc, char *argv[]) if (argc < 1) usage(NULL); - while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:e")) != EOF) { + while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { switch (ch) { case 'd': @@ -184,9 +182,6 @@ pwm_main(int argc, char *argv[]) if (*ep != '\0') usage("bad alternative rate provided"); break; - case 'e': - repeated_pwm_output = true; - break; default: break; } @@ -362,7 +357,7 @@ pwm_main(int argc, char *argv[]) } exit(0); - } else if (!strcmp(argv[1], "set")) { + } else if (!strcmp(argv[1], "test")) { if (set_mask == 0) { usage("no channels set"); @@ -372,38 +367,14 @@ pwm_main(int argc, char *argv[]) /* perform PWM output */ - if (repeated_pwm_output) { + /* Open console directly to grab CTRL-C signal */ + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; - /* 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."); - warnx("Press CTRL-C or 'c' to abort."); - - while (1) { - for (unsigned i = 0; i < servo_count; i++) { - if (set_mask & 1< 0) { - - read(0, &c, 1); - if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("User abort\n"); - exit(0); - } - } - } - } else { - /* only set the values once */ + while (1) { for (unsigned i = 0; i < servo_count; i++) { if (set_mask & 1< 0) { + + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("User abort\n"); + exit(0); + } + } } exit(0); @@ -493,7 +476,7 @@ pwm_main(int argc, char *argv[]) exit(0); } - usage("specify arm|disarm|rate|min|max|disarmed|set|info"); + usage("specify arm|disarm|rate|min|max|disarmed|test|info"); return 0; } From 5d36971689566e2142a16643a77337f2e3613c35 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Oct 2013 16:59:21 +0200 Subject: [PATCH 16/35] Base max actuators on board revision --- src/drivers/px4fmu/fmu.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8e9e8103ff..e729612cc3 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -101,7 +101,12 @@ public: int set_pwm_alt_channels(uint32_t channels); private: +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + static const unsigned _max_actuators = 4; +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) static const unsigned _max_actuators = 6; +#endif Mode _mode; unsigned _pwm_default_rate; @@ -214,10 +219,13 @@ PX4FMU::PX4FMU() : _pwm_on(false), _mixers(nullptr), _disarmed_pwm({0}), - _min_pwm({PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN}), - _max_pwm({PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX}), _num_disarmed_set(0) { + for (unsigned i = 0; i < _max_actuators; i++) { + _min_pwm[i] = PWM_MIN; + _max_pwm[i] = PWM_MAX; + } + _debug_enabled = true; } From 1d3f25ee6c9983ec5da9de4d4f7b463f880f3a87 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 19 Oct 2013 10:43:41 +0200 Subject: [PATCH 17/35] 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(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 3357e67a50..f0dd713b27 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 e729612cc3..b7b2f7b33d 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) { + } else { - /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { - /* can we mix? */ - if (_mixers != nullptr) { + /* get controls - must always do this to avoid spinning */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); - unsigned num_outputs; + /* can we mix? */ + if (_mixers != nullptr) { - 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; - } + unsigned 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; + 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; } + + /* 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; + } + } + + uint16_t pwm_limited[num_outputs]; + + pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + + /* 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); + + /* 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); } - - uint16_t pwm_limited[num_outputs]; - - pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); - - /* 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); - - /* 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 d709f1cc89..b603ffc8d5 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 a338afe161..6a0532beec 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 25f8c4e753..3185e43714 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< Date: Sat, 19 Oct 2013 10:44:38 +0200 Subject: [PATCH 18/35] Use new pwm cmds in rc.custom_io_esc --- ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 28 ++++++++++----------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc index e645d9d549..999422767a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc @@ -62,19 +62,6 @@ then usleep 5000 sh /etc/init.d/rc.io - - if [ $ESC_MAKER = afro ] - then - # Set PWM values for Afro ESCs - px4io idle 1050 1050 1050 1050 - px4io min 1080 1080 1080 1080 - px4io max 1860 1860 1860 1860 - else - # Set PWM values for typical ESCs - px4io idle 900 900 900 900 - px4io min 1110 1100 1100 1100 - px4io max 1800 1800 1800 1800 - fi else fmu mode_pwm # Start MAVLink (on UART1 / ttyS0) @@ -84,6 +71,19 @@ else set EXIT_ON_END yes fi +if [ $ESC_MAKER = afro ] +then + # Set PWM values for Afro ESCs + pwm disarmed -c 1234 -p 1050 + pwm min -c 1234 -p 1080 + pwm max -c 1234 -p 1860 +else + # Set PWM values for typical ESCs + pwm disarmed -c 1234 -p 900 + pwm min -c 1234 -p 980 + pwm max -c 1234 -p 1800 +fi + # # Load mixer # @@ -105,7 +105,7 @@ fi # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -r 400 -c 1234 # # Start common for all multirotors apps From ba77836000eaa28041969577482e3e4074d11e1b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 19 Oct 2013 10:44:58 +0200 Subject: [PATCH 19/35] Small indentation fix --- ROMFS/px4fmu_common/init.d/rcS | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 93e76184d4..5fb62af481 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -235,13 +235,13 @@ then set MODE custom fi - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 22 - then - set FRAME_GEOMETRY w - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi + # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame + if param compare SYS_AUTOSTART 22 + then + set FRAME_GEOMETRY w + sh /etc/init.d/rc.custom_io_esc + set MODE custom + fi if param compare SYS_AUTOSTART 30 then From 342a7bf55b815241b98e775e16833ce5e9a48974 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 22 Oct 2013 22:21:10 +0200 Subject: [PATCH 20/35] esc_calib: get disarmed/max values from PWM device, more informative output --- src/systemcmds/esc_calib/esc_calib.c | 115 +++++++++++++++++---------- 1 file changed, 72 insertions(+), 43 deletions(-) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 608c9fff17..1ea02d81e5 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -72,12 +72,13 @@ usage(const char *reason) { if (reason != NULL) warnx("%s", reason); - errx(1, - "usage:\n" - "esc_calib [-d ] \n" - " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" - " Provide channels (e.g.: 1 2 3 4)\n" - ); + + errx(1, + "usage:\n" + "esc_calib [-d ] \n" + " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " Provide channels (e.g.: 1 2 3 4)\n" + ); } @@ -98,12 +99,12 @@ esc_calib_main(int argc, char *argv[]) if (argc < 2) usage(NULL); - while ((ch = getopt(argc-1, argv, "d:")) != EOF) { + while ((ch = getopt(argc - 1, argv, "d:")) != EOF) { switch (ch) { - + case 'd': dev = optarg; - argc-=2; + argc -= 2; break; default: @@ -111,7 +112,7 @@ esc_calib_main(int argc, char *argv[]) } } - if(argc < 2) { + if (argc < 2) { usage("no channels provided"); } @@ -122,121 +123,149 @@ esc_calib_main(int argc, char *argv[]) if (*ep == '\0') { if (channel_number > MAX_CHANNELS || channel_number <= 0) { err(1, "invalid channel number: %d", channel_number); + } else { - channels_selected[channel_number-1] = true; + channels_selected[channel_number - 1] = true; } } } printf("\nATTENTION, please remove or fix propellers before starting calibration!\n" - "\n" - "Make sure\n" - "\t - that the ESCs are not powered\n" - "\t - that safety is off (two short blinks)\n" - "\t - that the controllers are stopped\n" - "\n" - "Do you want to start calibration now: y or n?\n"); + "\n" + "Make sure\n" + "\t - that the ESCs are not powered\n" + "\t - that safety is off (two short blinks)\n" + "\t - that the controllers are stopped\n" + "\n" + "Do you want to start calibration now: y or n?\n"); /* wait for user input */ while (1) { - + ret = poll(&fds, 1, 0); + if (ret > 0) { read(0, &c, 1); + if (c == 'y' || c == 'Y') { - + break; + } else if (c == 0x03 || c == 0x63 || c == 'q') { printf("ESC calibration exited\n"); exit(0); + } else if (c == 'n' || c == 'N') { printf("ESC calibration aborted\n"); exit(0); + } else { printf("Unknown input, ESC calibration aborted\n"); exit(0); - } + } } + /* rate limit to ~ 20 Hz */ usleep(50000); } /* open for ioctl only */ int fd = open(dev, 0); + if (fd < 0) err(1, "can't open %s", dev); - - /* Wait for user confirmation */ - printf("\nHigh PWM set\n" - "\n" - "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n" - "\n"); + /* get max PWM value setting */ + uint16_t pwm_max = 0; + ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, &pwm_max); + + if (ret != OK) + err(1, "PWM_SERVO_GET_MAX_PWM"); + + /* get disarmed PWM value setting */ + uint16_t pwm_disarmed = 0; + ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, &pwm_disarmed); + + if (ret != OK) + err(1, "PWM_SERVO_GET_DISARMED_PWM"); + + /* wait for user confirmation */ + printf("\nHigh PWM set: %d\n" + "\n" + "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n" + "\n", pwm_max); fflush(stdout); while (1) { + /* set max PWM */ + for (unsigned i = 0; i < MAX_CHANNELS; i++) { + if (channels_selected[i]) { + ret = ioctl(fd, PWM_SERVO_SET(i), pwm_max); - /* First set high PWM */ - for (unsigned i = 0; i 0) { read(0, &c, 1); + if (c == 13) { - + break; + } else if (c == 0x03 || c == 0x63 || c == 'q') { warnx("ESC calibration exited"); exit(0); } } + /* rate limit to ~ 20 Hz */ usleep(50000); } - /* we don't need any more user input */ - - - printf("Low PWM set, hit ENTER when finished\n" - "\n"); + printf("Low PWM set: %d\n" + "\n" + "Hit ENTER when finished\n" + "\n", pwm_disarmed); while (1) { - /* Then set low PWM */ - for (unsigned i = 0; i 0) { read(0, &c, 1); + if (c == 13) { - + break; + } else if (c == 0x03 || c == 0x63 || c == 'q') { printf("ESC calibration exited\n"); exit(0); } } + /* rate limit to ~ 20 Hz */ usleep(50000); } - printf("ESC calibration finished\n"); exit(0); From 2733a54fe201e060ad08aac8dfd02caeed12b08c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 26 Oct 2013 14:43:34 +0400 Subject: [PATCH 21/35] missionlib: waypoint yaw fix --- src/modules/mavlink/missionlib.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index e8d707948a..d37b18a195 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -220,7 +220,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.lon = param6_lon_y * 1e7f; sp.altitude = param7_alt_z; sp.altitude_is_relative = false; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F); set_special_fields(param1, param2, param3, param4, command, &sp); /* Initialize setpoint publication if necessary */ From 2293aa4e0ae809e31ba1aa71492253460a5e3aab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Oct 2013 21:22:05 +0100 Subject: [PATCH 22/35] Fixed min value check, works for fixed wing now --- src/modules/systemlib/pwm_limit/pwm_limit.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 3187a4fa2e..4cc618dddd 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -100,7 +100,7 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ temp_pwm = output[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; + effective_pwm[i] = (temp_pwm > min_pwm[i]) ? temp_pwm : ((disarmed_pwm[i]*(10000-progress) + min_pwm[i])*progress) / 10000; output[i] = (float)progress/10000.0f * output[i]; } break; From 44f88bf0a741fe530dea1c8b3137f30117a7b4b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Oct 2013 09:03:19 +0100 Subject: [PATCH 23/35] Fix to allow setting again zero disarmed PWM values after boot --- src/modules/px4iofirmware/registers.c | 45 +++++++++++++++++---------- 1 file changed, 29 insertions(+), 16 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 6a0532beec..40597adf1f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -338,26 +338,39 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num break; case PX4IO_PAGE_DISARMED_PWM: + { + /* flag for all outputs */ + bool all_disarmed_off = true; - /* copy channel data */ - while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - if (*values == 0) { - /* ignore 0 */ - } 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; + if (*values == 0) { + /* 0 means disabling always PWM */ + r_page_servo_disarmed[offset] = 0; + } else if (*values < PWM_MIN) { + r_page_servo_disarmed[offset] = PWM_MIN; + all_disarmed_off = false; + } else if (*values > PWM_MAX) { + r_page_servo_disarmed[offset] = PWM_MAX; + all_disarmed_off = false; + } else { + r_page_servo_disarmed[offset] = *values; + all_disarmed_off = false; + } + + offset++; + num_values--; + values++; } - /* flag the failsafe values as custom */ - r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; - - offset++; - num_values--; - values++; + if (all_disarmed_off) { + /* disable PWM output if disarmed */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE); + } else { + /* enable PWM output always */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; + } } break; From f0466143de18aedb5ada6f01b3c6435c9a0dc82a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Oct 2013 09:04:03 +0100 Subject: [PATCH 24/35] Minor warning and no error in case of zero value for disarmed --- src/systemcmds/pwm/pwm.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 3185e43714..09a9344008 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -335,7 +335,7 @@ pwm_main(int argc, char *argv[]) usage("no channels set"); } if (pwm_value == 0) - usage("no PWM value provided"); + warnx("reading disarmed value of zero, disabling disarmed PWM"); struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; @@ -343,7 +343,7 @@ pwm_main(int argc, char *argv[]) if (set_mask & 1< Date: Wed, 30 Oct 2013 09:14:17 +0100 Subject: [PATCH 25/35] Fixed pwm limit to apply the proper limits / scaling --- src/modules/systemlib/pwm_limit/pwm_limit.c | 22 ++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 4cc618dddd..cac3dc82a3 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -97,10 +97,26 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US; for (unsigned i=0; i 0) { - temp_pwm = output[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] = (temp_pwm > min_pwm[i]) ? temp_pwm : ((disarmed_pwm[i]*(10000-progress) + min_pwm[i])*progress) / 10000; + /* safeguard against overflows */ + uint16_t disarmed = disarmed_pwm[i]; + if (disarmed > min_pwm[i]) + disarmed = min_pwm[i]; + + uint16_t disarmed_min_diff = min_pwm[i] - disarmed; + ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; + } else { + + /* no disarmed pwm value set, choose min pwm */ + ramp_min_pwm = min_pwm[i]; + } + + effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; output[i] = (float)progress/10000.0f * output[i]; } break; From 34d2f318ac8a72cce63e3e14e004daee45001011 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Oct 2013 09:14:57 +0100 Subject: [PATCH 26/35] Fixed commander and IO startup sequence, in-air restart is operational in this shape --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 2 ++ ROMFS/px4fmu_common/init.d/11_dji_f450 | 2 ++ ROMFS/px4fmu_common/init.d/15_tbs_discovery | 2 ++ ROMFS/px4fmu_common/init.d/16_3dr_iris | 2 ++ ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 2 ++ ROMFS/px4fmu_common/init.d/rc.multirotor | 5 ----- 6 files changed, 10 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index 764a88a24e..81ea292aae 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -59,6 +59,8 @@ then mavlink start usleep 5000 + commander start + sh /etc/init.d/rc.io # Set PWM values for DJI ESCs else diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index 91847b06bf..4dbf76cee7 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -43,6 +43,8 @@ then mavlink start usleep 5000 + commander start + sh /etc/init.d/rc.io else # Start MAVLink (on UART1 / ttyS0) diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery index 65be56df8c..bd6189a6d3 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -43,6 +43,8 @@ then mavlink start usleep 5000 + commander start + sh /etc/init.d/rc.io else # Start MAVLink (on UART1 / ttyS0) diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index d36699b9a5..d8cc0e9132 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -43,6 +43,8 @@ then mavlink start usleep 5000 + commander start + sh /etc/init.d/rc.io else # Start MAVLink (on UART1 / ttyS0) diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc index 999422767a..d750cc87ad 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc @@ -61,6 +61,8 @@ then mavlink start -d /dev/ttyS1 -b 115200 usleep 5000 + commander start + sh /etc/init.d/rc.io else fmu mode_pwm diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index e3638e3d1f..dfff07198a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -13,11 +13,6 @@ sh /etc/init.d/rc.sensors # sh /etc/init.d/rc.logging -# -# Start the commander. -# -commander start - # # Start GPS interface (depends on orb) # From 727342a5168bb23501c50287acb8edfe6d80e157 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Oct 2013 22:34:51 +0100 Subject: [PATCH 27/35] Teached the FMU driver that stopping is also an option --- src/drivers/px4fmu/fmu.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 6d4019f249..dd475bb6ca 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1093,6 +1093,20 @@ fmu_start(void) return ret; } +int +fmu_stop(void) +{ + int ret = OK; + + if (g_fmu != nullptr) { + + delete g_fmu; + g_fmu = nullptr; + } + + return ret; +} + void test(void) { @@ -1224,6 +1238,12 @@ fmu_main(int argc, char *argv[]) PortMode new_mode = PORT_MODE_UNSET; const char *verb = argv[1]; + if (!strcmp(verb, "stop")) { + fmu_stop(); + errx(0, "FMU driver stopped"); + } + + if (fmu_start() != OK) errx(1, "failed to start the FMU driver"); From 9820ed9de36418a4ff518c8833b199bb4c074a4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Oct 2013 08:23:32 +0100 Subject: [PATCH 28/35] Actually allow full range in FMU driver --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 3920b5fb8b..4bd27f9076 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -908,7 +908,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* FALLTHROUGH */ case PWM_SERVO_SET(1): case PWM_SERVO_SET(0): - if (arg < 2100) { + if (arg <= 2100) { up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); } else { ret = -EINVAL; From 3c8c091e76c264fd803c066bc3a23cfdfc9738b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Oct 2013 08:23:44 +0100 Subject: [PATCH 29/35] esc_calib on steroids --- src/systemcmds/esc_calib/esc_calib.c | 91 +++++++++++++++++++++++++--- 1 file changed, 84 insertions(+), 7 deletions(-) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 1ea02d81e5..0ac9e9e17b 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -62,6 +62,8 @@ #include "systemlib/err.h" #include "drivers/drv_pwm_output.h" +#include + static void usage(const char *reason); __EXPORT int esc_calib_main(int argc, char *argv[]); @@ -92,6 +94,9 @@ esc_calib_main(int argc, char *argv[]) int ret; char c; + int low = -1; + int high = -1; + struct pollfd fds; fds.fd = 0; /* stdin */ fds.events = POLLIN; @@ -107,6 +112,16 @@ esc_calib_main(int argc, char *argv[]) argc -= 2; break; + case 'l': + low = strtol(optarg, NULL, 0); + argc -= 2; + break; + + case 'h': + high = strtol(optarg, NULL, 0); + argc -= 2; + break; + default: usage(NULL); } @@ -131,6 +146,26 @@ esc_calib_main(int argc, char *argv[]) } } + /* make sure no other source is publishing control values now */ + struct actuator_controls_s actuators; + int act_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + + /* clear changed flag */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, act_sub, &actuators); + + /* wait 50 ms */ + usleep(50000); + + /* now expect nothing changed on that topic */ + bool orb_updated; + orb_check(act_sub, &orb_updated); + + if (orb_updated) { + errx(1, "ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n" + "\tmultirotor_att_control stop\n" + "\tfw_att_control stop\n"); + } + printf("\nATTENTION, please remove or fix propellers before starting calibration!\n" "\n" "Make sure\n" @@ -179,17 +214,59 @@ esc_calib_main(int argc, char *argv[]) /* get max PWM value setting */ uint16_t pwm_max = 0; - ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, &pwm_max); - if (ret != OK) - err(1, "PWM_SERVO_GET_MAX_PWM"); + if (high > 0 && high > low && high < 2200) { + pwm_max = high; + } else { + ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, &pwm_max); + + if (ret != OK) + err(1, "PWM_SERVO_GET_MAX_PWM"); + } + + /* bound to sane values */ + if (pwm_max > 2200) + pwm_max = 2200; + + if (pwm_max < 1700) + pwm_max = 1700; /* get disarmed PWM value setting */ uint16_t pwm_disarmed = 0; - ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, &pwm_disarmed); + if (low > 0 && low < high && low > 800) { + pwm_disarmed = low; + } else { + ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, &pwm_disarmed); + + if (ret != OK) + err(1, "PWM_SERVO_GET_DISARMED_PWM"); + + if (pwm_disarmed == 0) { + ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, &pwm_disarmed); + + if (ret != OK) + err(1, "PWM_SERVO_GET_MIN_PWM"); + } + } + + /* bound to sane values */ + if (pwm_disarmed > 1300) + pwm_disarmed = 1300; + + if (pwm_disarmed < 800) + pwm_disarmed = 800; + + /* tell IO that its ok to disable its safety with the switch */ + ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); if (ret != OK) - err(1, "PWM_SERVO_GET_DISARMED_PWM"); + err(1, "PWM_SERVO_SET_ARM_OK"); + /* tell IO that the system is armed (it will output values if safety is off) */ + ret = ioctl(fd, PWM_SERVO_ARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_ARM"); + + warnx("Outputs armed"); /* wait for user confirmation */ printf("\nHigh PWM set: %d\n" @@ -205,7 +282,7 @@ esc_calib_main(int argc, char *argv[]) ret = ioctl(fd, PWM_SERVO_SET(i), pwm_max); if (ret != OK) - err(1, "PWM_SERVO_SET(%d)", i); + err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_max); } } @@ -242,7 +319,7 @@ esc_calib_main(int argc, char *argv[]) ret = ioctl(fd, PWM_SERVO_SET(i), pwm_disarmed); if (ret != OK) - err(1, "PWM_SERVO_SET(%d)", i); + err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_disarmed); } } From 7d443eb3325cfff469c88864fdc96b68612d36c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Oct 2013 09:03:37 +0100 Subject: [PATCH 30/35] Commandline parsing fixes --- src/systemcmds/esc_calib/esc_calib.c | 44 ++++++++++++++++++---------- 1 file changed, 28 insertions(+), 16 deletions(-) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 0ac9e9e17b..d524ab23be 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -77,7 +77,7 @@ usage(const char *reason) errx(1, "usage:\n" - "esc_calib [-d ] \n" + "esc_calib [-l ] [-h ] [-d ] \n" " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" " Provide channels (e.g.: 1 2 3 4)\n" ); @@ -101,25 +101,32 @@ esc_calib_main(int argc, char *argv[]) fds.fd = 0; /* stdin */ fds.events = POLLIN; - if (argc < 2) - usage(NULL); + if (argc < 2) { + usage("no channels provided"); + } - while ((ch = getopt(argc - 1, argv, "d:")) != EOF) { + int arg_consumed = 0; + + while ((ch = getopt(argc, &argv[0], "l:h:d:")) != -1) { switch (ch) { case 'd': dev = optarg; - argc -= 2; + arg_consumed += 2; break; case 'l': - low = strtol(optarg, NULL, 0); - argc -= 2; + low = strtoul(optarg, &ep, 0); + if (*ep != '\0') + usage("bad low pwm value"); + arg_consumed += 2; break; case 'h': - high = strtol(optarg, NULL, 0); - argc -= 2; + high = strtoul(optarg, &ep, 0); + if (*ep != '\0') + usage("bad high pwm value"); + arg_consumed += 2; break; default: @@ -127,14 +134,12 @@ esc_calib_main(int argc, char *argv[]) } } - if (argc < 2) { - usage("no channels provided"); - } - - while (--argc) { + while ((--argc - arg_consumed) > 0) { const char *arg = argv[argc]; unsigned channel_number = strtol(arg, &ep, 0); + warnx("adding channel #%d", channel_number); + if (*ep == '\0') { if (channel_number > MAX_CHANNELS || channel_number <= 0) { err(1, "invalid channel number: %d", channel_number); @@ -257,11 +262,11 @@ esc_calib_main(int argc, char *argv[]) if (pwm_disarmed < 800) pwm_disarmed = 800; - /* tell IO that its ok to disable its safety with the switch */ + /* tell IO/FMU that its ok to disable its safety with the switch */ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); if (ret != OK) err(1, "PWM_SERVO_SET_ARM_OK"); - /* tell IO that the system is armed (it will output values if safety is off) */ + /* tell IO/FMU that the system is armed (it will output values if safety is off) */ ret = ioctl(fd, PWM_SERVO_ARM, 0); if (ret != OK) err(1, "PWM_SERVO_ARM"); @@ -343,6 +348,13 @@ esc_calib_main(int argc, char *argv[]) usleep(50000); } + /* disarm */ + ret = ioctl(fd, PWM_SERVO_DISARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_DISARM"); + + warnx("Outputs disarmed"); + printf("ESC calibration finished\n"); exit(0); From 88351f3da178be1c73dad47557d894943e484e34 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 31 Oct 2013 09:20:44 +0100 Subject: [PATCH 31/35] esc_calib: Changed cmdline interface (now same as for the pwm systecmd), read in the number of channels available, don't make the esc_calib dependant on min/max/disarmed values --- src/systemcmds/esc_calib/esc_calib.c | 160 ++++++++++++--------------- 1 file changed, 68 insertions(+), 92 deletions(-) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index d524ab23be..c402061971 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -67,8 +67,6 @@ static void usage(const char *reason); __EXPORT int esc_calib_main(int argc, char *argv[]); -#define MAX_CHANNELS 14 - static void usage(const char *reason) { @@ -76,12 +74,15 @@ usage(const char *reason) warnx("%s", reason); errx(1, - "usage:\n" - "esc_calib [-l ] [-h ] [-d ] \n" - " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" - " Provide channels (e.g.: 1 2 3 4)\n" - ); - + "usage:\n" + "esc_calib\n" + " [-d PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " [-l Low PWM value in us (default: %dus)\n" + " [-h High PWM value in us (default: %dus)\n" + " [-c ] Supply channels (e.g. 1234)\n" + " [-m ] Directly supply channel mask (e.g. 0xF)\n" + " [-a] Use all outputs\n" + , PWM_MIN, PWM_MAX); } int @@ -89,13 +90,18 @@ esc_calib_main(int argc, char *argv[]) { char *dev = PWM_OUTPUT_DEVICE_PATH; char *ep; - bool channels_selected[MAX_CHANNELS] = {false}; int ch; int ret; char c; - int low = -1; - int high = -1; + unsigned max_channels = 0; + + uint32_t set_mask = 0; + unsigned long channels; + unsigned single_ch = 0; + + uint16_t pwm_high = PWM_MAX; + uint16_t pwm_low = PWM_MIN; struct pollfd fds; fds.fd = 0; /* stdin */ @@ -107,7 +113,7 @@ esc_calib_main(int argc, char *argv[]) int arg_consumed = 0; - while ((ch = getopt(argc, &argv[0], "l:h:d:")) != -1) { + while ((ch = getopt(argc, argv, "d:c:m:al:h:")) != EOF) { switch (ch) { case 'd': @@ -115,41 +121,49 @@ esc_calib_main(int argc, char *argv[]) arg_consumed += 2; break; + case 'c': + /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ + channels = strtoul(optarg, &ep, 0); + + while ((single_ch = channels % 10)) { + + set_mask |= 1<<(single_ch-1); + channels /= 10; + } + break; + + case 'm': + /* Read in mask directly */ + set_mask = strtoul(optarg, &ep, 0); + if (*ep != '\0') + usage("bad set_mask value"); + break; + + case 'a': + /* Choose all channels */ + for (unsigned i = 0; i PWM_HIGHEST_MIN) + usage("low PWM invalid"); break; - case 'h': - high = strtoul(optarg, &ep, 0); - if (*ep != '\0') - usage("bad high pwm value"); - arg_consumed += 2; + /* Read in custom high value */ + pwm_high = strtoul(optarg, &ep, 0); + if (*ep != '\0' || pwm_high > PWM_MAX || pwm_high < PWM_LOWEST_MAX) + usage("high PWM invalid"); break; - default: usage(NULL); } } - while ((--argc - arg_consumed) > 0) { - const char *arg = argv[argc]; - unsigned channel_number = strtol(arg, &ep, 0); - - warnx("adding channel #%d", channel_number); - - if (*ep == '\0') { - if (channel_number > MAX_CHANNELS || channel_number <= 0) { - err(1, "invalid channel number: %d", channel_number); - - } else { - channels_selected[channel_number - 1] = true; - - } - } - } + if (set_mask == 0) + usage("no channels chosen"); /* make sure no other source is publishing control values now */ struct actuator_controls_s actuators; @@ -217,50 +231,10 @@ esc_calib_main(int argc, char *argv[]) if (fd < 0) err(1, "can't open %s", dev); - /* get max PWM value setting */ - uint16_t pwm_max = 0; - - if (high > 0 && high > low && high < 2200) { - pwm_max = high; - } else { - ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, &pwm_max); - - if (ret != OK) - err(1, "PWM_SERVO_GET_MAX_PWM"); - } - - /* bound to sane values */ - if (pwm_max > 2200) - pwm_max = 2200; - - if (pwm_max < 1700) - pwm_max = 1700; - - /* get disarmed PWM value setting */ - uint16_t pwm_disarmed = 0; - - if (low > 0 && low < high && low > 800) { - pwm_disarmed = low; - } else { - ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, &pwm_disarmed); - - if (ret != OK) - err(1, "PWM_SERVO_GET_DISARMED_PWM"); - - if (pwm_disarmed == 0) { - ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, &pwm_disarmed); - - if (ret != OK) - err(1, "PWM_SERVO_GET_MIN_PWM"); - } - } - - /* bound to sane values */ - if (pwm_disarmed > 1300) - pwm_disarmed = 1300; - - if (pwm_disarmed < 800) - pwm_disarmed = 800; + /* get number of channels available on the device */ + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&max_channels); + if (ret != OK) + err(1, "PWM_SERVO_GET_COUNT"); /* tell IO/FMU that its ok to disable its safety with the switch */ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); @@ -273,21 +247,23 @@ esc_calib_main(int argc, char *argv[]) warnx("Outputs armed"); + /* wait for user confirmation */ printf("\nHigh PWM set: %d\n" "\n" "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n" - "\n", pwm_max); + "\n", pwm_high); fflush(stdout); while (1) { /* set max PWM */ - for (unsigned i = 0; i < MAX_CHANNELS; i++) { - if (channels_selected[i]) { - ret = ioctl(fd, PWM_SERVO_SET(i), pwm_max); + for (unsigned i = 0; i < max_channels; i++) { + + if (set_mask & 1< Date: Tue, 20 Aug 2013 14:19:22 +0200 Subject: [PATCH 32/35] Fixed small typo --- src/systemcmds/config/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 188dafa4ef..80689f20c8 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -272,7 +272,7 @@ do_accel(int argc, char *argv[]) } } else { - errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 4 G\n\t"); + errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 4' to set measurement range to 4 G\n\t"); } int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); From eac640739b8eb63343e01566d4c093179e3b657f Mon Sep 17 00:00:00 2001 From: runepx4 Date: Thu, 31 Oct 2013 10:23:37 +0100 Subject: [PATCH 33/35] Added 8 rotor Coaxial Rotor mixer --- ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix | 7 +++++++ src/modules/systemlib/mixer/mixer.h | 1 + src/modules/systemlib/mixer/mixer_multirotor.cpp | 15 +++++++++++++++ src/modules/systemlib/mixer/multi_tables | 13 ++++++++++++- 4 files changed, 35 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix new file mode 100644 index 0000000000..51cebb7858 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a Coaxial Octocopter in the X configuration. All controls +are mixed 100%. + +R: 8c 10000 10000 10000 0 diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 723bf9f3b7..1c889a8119 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -449,6 +449,7 @@ public: HEX_PLUS, /**< hex in + configuration */ OCTA_X, OCTA_PLUS, + OCTA_COX, MAX_GEOMETRY }; diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index b89f341b60..bf77795d5c 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -130,6 +130,16 @@ const MultirotorMixer::Rotor _config_octa_plus[] = { { 1.000000, 0.000000, -1.00 }, { -1.000000, 0.000000, -1.00 }, }; +const MultirotorMixer::Rotor _config_octa_cox[] = { + { -0.707107, 0.707107, 1.00 }, + { 0.707107, 0.707107, -1.00 }, + { 0.707107, -0.707107, 1.00 }, + { -0.707107, -0.707107, -1.00 }, + { 0.707107, 0.707107, 1.00 }, + { -0.707107, 0.707107, -1.00 }, + { -0.707107, -0.707107, 1.00 }, + { 0.707107, -0.707107, -1.00 }, +}; const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], @@ -139,6 +149,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_hex_plus[0], &_config_octa_x[0], &_config_octa_plus[0], + &_config_octa_cox[0], }; const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_x */ @@ -149,6 +160,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 6, /* hex_plus */ 8, /* octa_x */ 8, /* octa_plus */ + 8, /* octa_cox */ }; } @@ -240,6 +252,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "8x")) { geometry = MultirotorMixer::OCTA_X; + + } else if (!strcmp(geomname, "8c")) { + geometry = MultirotorMixer::OCTA_COX; } else { debug("unrecognised geometry '%s'", geomname); diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 683c630409..050bf2f47f 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -74,7 +74,18 @@ set octa_plus { 90 CW } -set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus} +set octa_cox { + 45 CCW + -45 CW + -135 CCW + 135 CW + -45 CCW + 45 CW + 135 CCW + -135 CW +} + +set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus octa_cox} proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]} From 25bf1abecffeb0b4c29386ef6a019b7a60c23899 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 31 Oct 2013 10:29:06 +0100 Subject: [PATCH 34/35] pwm_output: Allow PWM values from 900us to 2100us but use a default of 1000us to 2000us --- src/drivers/drv_pwm_output.h | 18 +++++++++++++---- src/drivers/px4fmu/fmu.cpp | 28 +++++++++++++-------------- src/drivers/px4io/px4io.cpp | 2 +- src/modules/px4iofirmware/registers.c | 28 +++++++++++++-------------- src/systemcmds/esc_calib/esc_calib.c | 10 +++++----- 5 files changed, 48 insertions(+), 38 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index efd6afb4bd..51f916f37b 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -65,9 +65,14 @@ __BEGIN_DECLS #define PWM_OUTPUT_MAX_CHANNELS 16 /** - * Minimum PWM in us + * Lowest minimum PWM in us */ -#define PWM_MIN 900 +#define PWM_LOWEST_MIN 900 + +/** + * Default minimum PWM in us + */ +#define PWM_DEFAULT_MIN 1000 /** * Highest PWM allowed as the minimum PWM @@ -75,9 +80,14 @@ __BEGIN_DECLS #define PWM_HIGHEST_MIN 1300 /** - * Maximum PWM in us + * Highest maximum PWM in us */ -#define PWM_MAX 2100 +#define PWM_HIGHEST_MAX 2100 + +/** + * Default maximum PWM in us + */ +#define PWM_DEFAULT_MAX 2000 /** * Lowest PWM allowed as the maximum PWM diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4bd27f9076..0441566e98 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -232,8 +232,8 @@ PX4FMU::PX4FMU() : _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { - _min_pwm[i] = PWM_MIN; - _max_pwm[i] = PWM_MAX; + _min_pwm[i] = PWM_DEFAULT_MIN; + _max_pwm[i] = PWM_DEFAULT_MAX; } _debug_enabled = true; @@ -762,10 +762,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) 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 if (pwm->values[i] > PWM_HIGHEST_MAX) { + _failsafe_pwm[i] = PWM_HIGHEST_MAX; + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _failsafe_pwm[i] = PWM_LOWEST_MIN; } else { _failsafe_pwm[i] = pwm->values[i]; } @@ -801,10 +801,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) for (unsigned i = 0; i < pwm->channel_count; i++) { if (pwm->values[i] == 0) { /* ignore 0 */ - } else if (pwm->values[i] > PWM_MAX) { - _disarmed_pwm[i] = PWM_MAX; - } else if (pwm->values[i] < PWM_MIN) { - _disarmed_pwm[i] = PWM_MIN; + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _disarmed_pwm[i] = PWM_HIGHEST_MAX; + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _disarmed_pwm[i] = PWM_LOWEST_MIN; } else { _disarmed_pwm[i] = pwm->values[i]; } @@ -842,8 +842,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* ignore 0 */ } else if (pwm->values[i] > PWM_HIGHEST_MIN) { _min_pwm[i] = PWM_HIGHEST_MIN; - } else if (pwm->values[i] < PWM_MIN) { - _min_pwm[i] = PWM_MIN; + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _min_pwm[i] = PWM_LOWEST_MIN; } else { _min_pwm[i] = pwm->values[i]; } @@ -872,8 +872,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* ignore 0 */ } else if (pwm->values[i] < PWM_LOWEST_MAX) { _max_pwm[i] = PWM_LOWEST_MAX; - } else if (pwm->values[i] > PWM_MAX) { - _max_pwm[i] = PWM_MAX; + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _max_pwm[i] = PWM_HIGHEST_MAX; } else { _max_pwm[i] = pwm->values[i]; } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 63e775857c..08e697b9f8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1985,7 +1985,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 < PWM_MIN) || (arg > PWM_MAX)) { + if ((channel >= _max_actuators) || (arg < PWM_LOWEST_MIN) || (arg > PWM_HIGHEST_MAX)) { ret = -EINVAL; } else { diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 40597adf1f..86a40bc228 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, 0, 0, 0, 0, 0, 0, 0 }; * minimum PWM values when armed * */ -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 }; +uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN }; /** * PAGE 107 @@ -215,7 +215,7 @@ uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_MIN, PWM_MIN, PWM_ * maximum PWM values when armed * */ -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 }; +uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX }; /** * PAGE 108 @@ -278,10 +278,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num 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 if (*values < PWM_LOWEST_MIN) { + r_page_servo_failsafe[offset] = PWM_LOWEST_MIN; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX; } else { r_page_servo_failsafe[offset] = *values; } @@ -304,8 +304,8 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } 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 if (*values < PWM_LOWEST_MIN) { + r_page_servo_control_min[offset] = PWM_LOWEST_MIN; } else { r_page_servo_control_min[offset] = *values; } @@ -323,8 +323,8 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* ignore 0 */ - } else if (*values > PWM_MAX) { - r_page_servo_control_max[offset] = PWM_MAX; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_control_max[offset] = PWM_HIGHEST_MAX; } else if (*values < PWM_LOWEST_MAX) { r_page_servo_control_max[offset] = PWM_LOWEST_MAX; } else { @@ -348,11 +348,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* 0 means disabling always PWM */ r_page_servo_disarmed[offset] = 0; - } else if (*values < PWM_MIN) { - r_page_servo_disarmed[offset] = PWM_MIN; + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_disarmed[offset] = PWM_LOWEST_MIN; all_disarmed_off = false; - } else if (*values > PWM_MAX) { - r_page_servo_disarmed[offset] = PWM_MAX; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX; all_disarmed_off = false; } else { r_page_servo_disarmed[offset] = *values; diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index c402061971..b237b31be3 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -82,7 +82,7 @@ usage(const char *reason) " [-c ] Supply channels (e.g. 1234)\n" " [-m ] Directly supply channel mask (e.g. 0xF)\n" " [-a] Use all outputs\n" - , PWM_MIN, PWM_MAX); + , PWM_DEFAULT_MIN, PWM_DEFAULT_MAX); } int @@ -100,8 +100,8 @@ esc_calib_main(int argc, char *argv[]) unsigned long channels; unsigned single_ch = 0; - uint16_t pwm_high = PWM_MAX; - uint16_t pwm_low = PWM_MIN; + uint16_t pwm_high = PWM_DEFAULT_MAX; + uint16_t pwm_low = PWM_DEFAULT_MIN; struct pollfd fds; fds.fd = 0; /* stdin */ @@ -148,13 +148,13 @@ esc_calib_main(int argc, char *argv[]) case 'l': /* Read in custom low value */ - if (*ep != '\0' || pwm_low < PWM_MIN || pwm_low > PWM_HIGHEST_MIN) + if (*ep != '\0' || pwm_low < PWM_LOWEST_MIN || pwm_low > PWM_HIGHEST_MIN) usage("low PWM invalid"); break; case 'h': /* Read in custom high value */ pwm_high = strtoul(optarg, &ep, 0); - if (*ep != '\0' || pwm_high > PWM_MAX || pwm_high < PWM_LOWEST_MAX) + if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX) usage("high PWM invalid"); break; default: From 5781b58640a7bb3937f9eff979f99535ab1169e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Nov 2013 09:05:28 +0100 Subject: [PATCH 35/35] Minor bugfix to commander, emits arming sound now on the right occasions. Fixes an annoying issue where the arming sound would go off constantly if the safety was re-engaged in arming mode, something that we consider to be ok operationally --- src/modules/commander/commander.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 44a1442968..ed090271ca 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -199,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode */ int commander_thread_main(int argc, char *argv[]); -void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); +void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed); void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); @@ -843,6 +843,12 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(safety), safety_sub, &safety); + + // XXX this would be the right approach to do it, but do we *WANT* this? + // /* disarm if safety is now on and still armed */ + // if (safety.safety_switch_available && !safety.safety_off) { + // (void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + // } } /* update global position estimate */ @@ -1219,7 +1225,7 @@ int commander_thread_main(int argc, char *argv[]) } /* play arming and battery warning tunes */ - if (!arm_tune_played && armed.armed) { + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { /* play tune when armed */ if (tune_arm() == OK) arm_tune_played = true; @@ -1240,7 +1246,7 @@ int commander_thread_main(int argc, char *argv[]) } /* reset arm_tune_played when disarmed */ - if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) { arm_tune_played = false; } @@ -1309,7 +1315,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) +control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed) { /* driving rgbled */ if (changed) { @@ -1356,11 +1362,11 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ - if (armed->armed) { + if (actuator_armed->armed) { /* armed, solid */ led_on(LED_BLUE); - } else if (armed->ready_to_arm) { + } else if (actuator_armed->ready_to_arm) { /* ready to arm, blink at 1Hz */ if (leds_counter % 20 == 0) led_toggle(LED_BLUE);