From 8131d28a0faf7d33060cf067f5bd8dee41666fed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 21:35:50 +0200 Subject: [PATCH 01/48] 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/48] 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/48] 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/48] 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/48] 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/48] 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/48] 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/48] 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/48] 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/48] 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/48] 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/48] 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/48] 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/48] 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/48] 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/48] 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 71ac33596836519a341001bb48a8835b8af75cd3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Oct 2013 21:43:11 +0200 Subject: [PATCH 17/48] Small improvements to autoland, ensure that throttle can be shut down close to touch down. Depends on accurate land WP altitude --- src/lib/external_lgpl/tecs/tecs.h | 5 +++ .../fw_pos_control_l1_main.cpp | 32 +++++++++++++++---- .../fw_pos_control_l1_params.c | 1 + 3 files changed, 31 insertions(+), 7 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 2ae6b28bb4..4a98c8e974 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -94,6 +94,11 @@ public: // Rate of change of velocity along X body axis in m/s^2 float get_VXdot(void) { return _vel_dot; } + + float get_speed_weight() { + return _spdWeight; + } + // log data on internal state of the controller. Called at 10Hz // void log_data(DataFlash_Class &dataflash, uint8_t msgid); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index cd4a0d58ef..3697af225e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -196,6 +196,8 @@ private: float throttle_max; float throttle_cruise; + float throttle_land_max; + float loiter_hold_radius; } _parameters; /**< local copies of interesting parameters */ @@ -227,6 +229,8 @@ private: param_t throttle_max; param_t throttle_cruise; + param_t throttle_land_max; + param_t loiter_hold_radius; } _parameter_handles; /**< handles for interesting parameters */ @@ -342,6 +346,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_min = param_find("FW_THR_MIN"); _parameter_handles.throttle_max = param_find("FW_THR_MAX"); _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); + _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -404,6 +409,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max)); param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); + param_get(_parameter_handles.time_const, &(_parameters.time_const)); param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); @@ -625,6 +632,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); float altitude_error = _global_triplet.current.altitude - _global_pos.alt; + /* no throttle limit as default */ + float throttle_max = 1.0f; + /* AUTONOMOUS FLIGHT */ // XXX this should only execute if auto AND safety off (actuators active), @@ -634,11 +644,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); + /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ + _tecs.set_speed_weight(_parameters.speed_weight); + /* execute navigation once we have a setpoint */ if (_setpoint_valid) { - float altitude_error = _global_triplet.current.altitude - _global_pos.alt; - /* current waypoint (the one currently heading for) */ math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); @@ -712,16 +723,23 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - if (altitude_error > -20.0f) { - float flare_angle_rad = math::radians(15.0f);//math::radians(global_triplet.current.param1) + if (altitude_error > -10.0f) { + + float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + + /* set the speed weight to 0.0 to push the system to control altitude with pitch */ + _tecs.set_speed_weight(0.0f); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, - true, flare_angle_rad, + false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + /* kill the throttle if param requests it */ + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f)); @@ -785,7 +803,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _loiter_hold = true; } - float altitude_error = _loiter_hold_alt - _global_pos.alt; + altitude_error = _loiter_hold_alt - _global_pos.alt; math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon); @@ -862,7 +880,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } _att_sp.pitch_body = _tecs.get_pitch_demand(); - _att_sp.thrust = _tecs.get_throttle_demand(); + _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); return setpoint; } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index d210ec7126..9b64cb047a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -72,6 +72,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); +PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 0.0f); PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); From 013579cffd70f46788a356043563340731beabae Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Oct 2013 07:54:04 +0200 Subject: [PATCH 18/48] More improvements on landing --- .../fw_pos_control_l1_main.cpp | 72 ++++++++++++++++--- .../fw_pos_control_l1_params.c | 2 +- 2 files changed, 64 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3697af225e..348a17ba63 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -727,15 +727,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (altitude_error > -10.0f) { float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + float land_pitch_min = math::radians(5.0f); + float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - /* set the speed weight to 0.0 to push the system to control altitude with pitch */ - _tecs.set_speed_weight(0.0f); + // /* set the speed weight to 0.0 to push the system to control altitude with pitch */ + // _tecs.set_speed_weight(0.0f); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, _parameters.airspeed_min, _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + 0.0f, _parameters.throttle_max, throttle_land, + land_pitch_min, math::radians(_parameters.pitch_limit_max)); /* kill the throttle if param requests it */ throttle_max = math::min(throttle_max, _parameters.throttle_land_max); @@ -814,7 +816,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _att_sp.yaw_body = _l1_control.nav_bearing(); /* climb with full throttle if the altitude error is bigger than 5 meters */ - bool climb_out = (altitude_error > 5); + bool climb_out = (altitude_error > 3); float min_pitch; @@ -842,6 +844,43 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _att_sp.roll_reset_integral = true; } + } else if (0/* easy mode enabled */) { + + /** EASY FLIGHT **/ + + if (0/* switched from another mode to easy */) { + _seatbelt_hold_heading = _att.yaw; + } + + if (0/* easy on and manual control yaw non-zero */) { + _seatbelt_hold_heading = _att.yaw + _manual.yaw; + } + + /* climb out control */ + bool climb_out = false; + + /* user wants to climb out */ + if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + climb_out = true; + } + + /* if in seatbelt mode, set airspeed based on manual control */ + + // XXX check if ground speed undershoot should be applied here + float seatbelt_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.throttle; + + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, + seatbelt_airspeed, + _airspeed.indicated_airspeed_m_s, eas2tas, + false, _parameters.pitch_limit_min, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + _parameters.pitch_limit_min, _parameters.pitch_limit_max); + } else if (0/* seatbelt mode enabled */) { /** SEATBELT FLIGHT **/ @@ -861,13 +900,28 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; + /* user switched off throttle */ + if (_manual.throttle < 0.1f) { + throttle_max = 0.0f; + /* switch to pure pitch based altitude control, give up speed */ + _tecs.set_speed_weight(0.0f); + } + + /* climb out control */ + bool climb_out = false; + + /* user wants to climb out */ + if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + climb_out = true; + } + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + _att_sp.roll_body = _manual.roll; + _att_sp.yaw_body = _manual.yaw; _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, - false, _parameters.pitch_limit_min, + climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 9b64cb047a..c39df9ae31 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -72,7 +72,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); -PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 0.0f); +PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); From 95aba0d70eae6a9aba55d31f223b852df1f82dcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Oct 2013 09:36:20 +0200 Subject: [PATCH 19/48] Almost perfect landing approach, needs touch-down fine tuning --- .../fw_pos_control_l1_main.cpp | 47 +++++++++++++++---- src/modules/mavlink/waypoints.c | 37 ++++++++++----- 2 files changed, 62 insertions(+), 22 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 348a17ba63..219c6ffa6b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -635,6 +635,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* no throttle limit as default */ float throttle_max = 1.0f; + /* not in non-abort mode for landing yet */ + bool land_noreturn = false; + /* AUTONOMOUS FLIGHT */ // XXX this should only execute if auto AND safety off (actuators active), @@ -717,23 +720,39 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) { - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + /* switch to heading hold for the last meters, continue heading hold after */ + + float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), next_wp.getX(), next_wp.getY()); + + if (wp_distance < 5.0f || land_noreturn) { + + /* heading hold, along the line connecting this and the last waypoint */ + float target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + + if (altitude_error > -20.0f) + land_noreturn = true; + + } else { + + /* normal navigation */ + _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + } + _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - if (altitude_error > -10.0f) { + float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + float land_pitch_min = math::radians(5.0f); + float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + float airspeed_land = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; - float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) - float land_pitch_min = math::radians(5.0f); - float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + if (altitude_error > -5.0f) { - // /* set the speed weight to 0.0 to push the system to control altitude with pitch */ - // _tecs.set_speed_weight(0.0f); - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, _parameters.airspeed_min, + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, 0.0f, _parameters.throttle_max, throttle_land, @@ -743,7 +762,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio throttle_max = math::min(throttle_max, _parameters.throttle_land_max); /* limit roll motion to prevent wings from touching the ground first */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f)); + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + + } else if (altitude_error > -20.0f) { + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } else { diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index ddad5f0df6..adaf814047 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -399,6 +399,13 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (time_elapsed) { if (cur_wp->autocontinue) { + + /* safeguard against invalid missions with last wp autocontinue on */ + if (wpm->current_active_wp_id == wpm->size - 1) { + /* stop handling missions here */ + cur_wp->autocontinue = false; + } + cur_wp->current = 0; float navigation_lat = -1.0f; @@ -419,13 +426,16 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ navigation_frame = MAV_FRAME_LOCAL_NED; } + /* guard against missions without final land waypoint */ /* only accept supported navigation waypoints, skip unknown ones */ do { + /* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */ if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) { + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) { /* this is a navigation waypoint */ navigation_frame = cur_wp->frame; @@ -434,17 +444,20 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ navigation_alt = cur_wp->z; } - if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { - /* the last waypoint was reached, if auto continue is - * activated keep the system loitering there. - */ - cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; - cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius - cur_wp->frame = navigation_frame; - cur_wp->x = navigation_lat; - cur_wp->y = navigation_lon; - cur_wp->z = navigation_alt; - cur_wp->autocontinue = false; + if (wpm->current_active_wp_id == wpm->size - 1) { + + /* if we're not landing at the last nav waypoint, we're falling back to loiter */ + if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) { + /* the last waypoint was reached, if auto continue is + * activated AND it is NOT a land waypoint, keep the system loitering there. + */ + cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; + cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius + cur_wp->frame = navigation_frame; + cur_wp->x = navigation_lat; + cur_wp->y = navigation_lon; + cur_wp->z = navigation_alt; + } /* we risk an endless loop for missions without navigation waypoints, abort. */ break; From 5fafe45745238ae377ed748c6f561a6cbf02221a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Oct 2013 10:14:42 +0200 Subject: [PATCH 20/48] Updated docs --- Documentation/fixed_wing_control.odg | Bin 0 -> 12258 bytes Documentation/l1_control.odg | Bin 11753 -> 0 bytes 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 Documentation/fixed_wing_control.odg delete mode 100644 Documentation/l1_control.odg diff --git a/Documentation/fixed_wing_control.odg b/Documentation/fixed_wing_control.odg new file mode 100644 index 0000000000000000000000000000000000000000..0918edcac86ac046cda417e534551bb91480c219 GIT binary patch literal 12258 zcmd6NcT`l%((jOigwLT_piGisGy;f0RY$lfJmw4y)-o54lV!y zaD8C|00281JE)VpJrrzj53_-QoooIQT&f4DJEwmT>N`H;&8AX)KT0CBK~V& z;=gK(e|Pl9KW;D^xUCeEwUd**I1uRS>dNaX$csQ)0r|zm#DL#hnf_F@g#1g--Wdt| zu4M@ULSfMBt~&zxdHI1qbgv`)r$Jro{u+`!66y$rJAKddKgIaf=2t8P0`ae)uk-Oo zy1s`lz{e*9{NwkJs2%NWVE@$PwJr|?VP_9^vax{uC8uDxjom+mxz_#O(_f>uLRwnF z{uTExgCfAkCkV6xT7sRxJT5j+SLPp)|Iv^C-dnMM>#Zdc?D}6V`1yd>I!p|Ipo{-9 zQ3aT!f6U73;ggp9K8O$`^m|`;EWu7tX#qZdK^}fS9)1BGegSbl0dXNQN#JkvBrPH0 ze>D)%5fBs?77-T`kp%u|{KEs((#FXKZpCBijQs9N`hkvssG6{d_MgcA(f(I6h&u!Z zb(9wTH|;PL|8x+!Xx^K4+OIO)Adg;e|Y`Zv+a02 z@g1FPAPnDAg|N8B=7Aub;Z9OaAR#88wB(=T_-E#S{Q4Kd_W}810m8<{{=T9Cei1wX z0N@C9x=y3x^&&KFH0!tkBJ$cckJ&xQ@MJhw&1SC*JM176ez7b?nF&yBL{XG|}89=EcK4)%*Yg z^isRFHYANVsnAN_2YTZjQaQ{3kIma)l6P~*=zDhb^r%fhg;#PAA#V2_2AWKYF zx%V9LTZ-?GQNBd0H~>VXR4*~j4+FebUO|ITiRujBM|bYU?fCd^lwlZ(qYl9at#)6$d4gF`F073RVH+n*ix(kWs&jSc3_#-b;9o54ZFDR2MtRfO=^q zvBhdt;jJ1px8f|`$cV`mH=^#-fLiy11wZ{gBDS(X^v_sd>V?WWcce2I*h+(E! z^ynL#q4)SRw+5X7&6O;OJ6yiiN!h>E#|q6jZnjxfcAfwYt7N6mHoWRZL-^?+#7$QO zrN6ihYE9y|^=gMoa$(J4o`Fpv9Frl4fzBbO2`A2Cw6=4jRdF>_w!>wGMQlRAethWo zwTp5$qrDnchp2ILTwK@HAXj_cBA0*8iF}(&3^q>P_=Ih>^A-J$RCT=Uw#{n&;106K zqqsT<^hB8dbnr|R{Vo}95bZ=O|FD@DH`d&@Q()0zVY2f9G}9HoA72E?b1bW$8({5( z_2v}VvEyqM?9T+UlKc$ATWV7Gz@J28dA*>PGv;r=;(Vr1uad%H(Z`Onki>y>Gj+Hh zYqs*SqiuANA@6W^A6O2Wn|PVdIZEQi?O@Se01am_S`Xn3%+-E;>H<-7e;}0@Y`Y$N z;*vcfH&85wrlOwkJmI#Qgx2Ka%$aWwK8BpARnvvy;G@nVru3V?fQFuc28hn{OHupE zwk%-Z93M2)#O(!OE4w^_-G1!bw?zJ)L*#a_1K;u_Dsx^!65!)-Ho-8v3RAcCEVRa-{hNHY;VbT(@Z}<5D_~9WvRSaFi^PR=ZbMBK!qT)nX zY_>TC>rSJ@l>4wZk9sLXb>^ccT|7Kby97}(*1Z)4fWdJSyBTrBf_F1dI0)|P_X>w( z&qCT>e{%6i{Q3a(MrJ8j-U*S(L>t3l76c_9=PM^L%}(Bn#X+6 z*17pT;cXe{r&+)Q6)fy7)rZT37yy7RGXU`S3+W92002S2ovy$8-0WbRrjwD2wd9_I z`b4R!e94wEWUS=^x)D@(nC`C$s8{J2dQ|Q~`>G#V_Qot3eWloo4w75~?qQuIGMq%2 zXocPGef}jU&Ly7)8eeCY>fz1{RH|$CbZo&JDyCrfA~TCDgNH{`sAuNW4*(TpJilo= z$|Fj=_U(j{Pe`{dsMW?*fT~qWWVt($=`IN-mUZ!2&a`H7@C9~?jD~~fVNOWm;>NPq z&KYsd(Vo!TN~c5zCA>EZtF*VJH;oNn7JqO?oE~%ED6GmW_n4@8!*L_MV9ZK$_T8~k zdOy=0&pM=b3tOk>Tu_TiL}OgZ+kM634h$-4>=59N zoK-L7x|_*M(;leG^)xSSzkay>9lnBaQYj_+050M zP~=9me8Vd?-q;J+*_{p|N@q20&Wucx9l27BpYthA+xx4&Iq;t`@_EA}zaHp?&?bV? zXtUq!8VVJq>7L;X9o4#9aj6b?!pJHg=0sJgozTM*IG_aT@}Bg$ z4fmeVeA-*IFH*PHNd)RSZ!%J?vMdU`HO-4D`y9g7q=-DU`>M9Y_0Eez);uzoOY$A* zBi{E(Sc>n?-&7-)XRJS)In#+03_x6~cG&Oly^H7^QS=&mqimtBG3_SK`KiCqnya-Z zOnX*^$c=V|H_~hOApsV#TmK;52_hf>mL3b(kP1QdF35%fx#rMro7fR|UY5oTMSR7) zq-8jFPszUA$S~W{Q{mBri4C^*>lQm2R&Z~EbjFV=aRJk#GOfPn;H;fj0}n4uvL%6YSt zjCOEl2D|L7ekLDV&IL-I2t8SD?i{TVM5YjVSOAZquZT))28Wo#xJpb_kV`*%GVTRO z4#@6Zrr-&prXF3s*hq2FS^Iw7GfQ`$)e*uzT=~cfMJ{d|nWvMN-{eJPJYF0OXiyA` zlQYnQB92nI;05_l8che3;}m&%j8{(Q zvnK=0u%}v*!LF~PTx_FK4czhXb9}O0B45k0dza-=u;l~rUajjDKXT!OQBa-*pO7&+ zCxcCwL5F6QOkVSYYv$&+x)&8bFe$P(IzGK>MjIvXLUL8=Us_7eO)+rk1t0q|vX;EmwKOWuCuQuOAVp>1GA0gxR zwwQgR4EFd%VSm$5*aeP3e@YrlED4uP3P6jf>}+o(r)3EL(^Kh;zCh9|eIoo9x<X%8`GJX5FX12An4Ybfo99r?yS=a6?Lvvvu;7|;?7GF0PAfu_>7I`QVA zgTebRX#f)5o%pXGC8^jSRx>Bok)cF9H}b2$yQnw^Lh)=!w2 zEBzGLx^{1Ih^z>OiyGC>Uj}OCk~ej$3~Tx-ys^()z?Palo(^utOt-jwrB}C&911&n zljIwZ`iYvmOV<5Lp;P&m4m{=RY-0BEY}?x@yq+ULk|nd&=aO;E}`EZy2~G9%p~(Bc`*b3dEP zt#7QYe?v)_*ih;SbvD_qJY~ftc@@SLJ(%Br{>eXrdOcWQv6r4&-30IC%jqy7{_^bY z(j;nDFPB&@!+Ri$fn~Lup4^_GTs`(Gp**PfQ|k3_tKsGSNk>SCL+WDugd&g^#y)RFWNd$9ag+K1@CK7jc~M#kR2Y&0OsjM{)1K{QS-q#DNHlx5ABWk9NJUJ|`V)l|9E}ls73R zpMP`ICG@QB%cZqDT~Zf3!IJQ#Lvt;c?o^gp9D4I>ZZH0O9H=>yA!l@5HC%j%ZlNBE zlJnFMoukk}ni6Od9Hfy;WO-i&7(s72xNtjkn;F{=&yvB9huNDaeQ!}TG`8aTySs|1 z#H>S#6od+X%2+4nkMR1zPvF?=vj-=C;@`cNS!$_Gm z)|ZtUOzMb+$8%5cCu6EB=q5)-^*>UQRy5ERPk0D2uh_x_vl>$COw}`DwnB?0Gkm+U z8=mWyvJ@}ag6XHpH|?5i64jjboYu%MCMgh)K&P1DKADJcU_;@4F{*6?Mi>4IO>wpr z8l3&e0f#W+!<8nm{F|@(TBqf$aD|INF>)#Y4XAFyqfv&u_6=94E#(F%LN801H=3+= z!2^Gs_Y%oNKxn)-ea0;VNq?R|^WGU9@*;~enfg&mxo=mUGrDc^!bpt$I$4^Ct)ObH zy6U@pH4ZSEEn$&V*-`5!jk19m5t4Pw4K)V&Z;qW{w1i9~utN^aI}!+n1?1 z=c+GX(m#5bWZ_Rqi!KQgxB&e(Zjq<3C1n3MVXnXnw`Q2f>X|Vo^?90wR-F0SOd$Ey^V#V z4Qg^NPCD|8%|BX~!Sa+p-l0IGsUG*m@Yn=!8;L;_KYib%`uX%avJ*l?VIXD85w}X_ zNP^LthNiKV1X?< zETr}>k+7N^4-F37BAWfEmUc?^X52;~_Lm)rN=!?1ZNsm`VN66?59pS}dKl3iy3&i% zr9xHN3lV5GA>wjh9T@nn>5Y5%>)x}gZoEXPOC%%q?{mSXkQs{d=f1a-%i4)<5ok#? z8K8Fu74KZ(4lWHA(~Szaz~}pZ1N>$MCDz(0O@Rwmoi%kS8=!@89_0sO{br#b?|ZdI zDs8@d1Wjc!AA7*ALL6HiAeKBHyG9N=@@q9`w_5+lti-0Ad`rW zuD2vwtOqen;#C-d+T;!ssEbhUjxitjjj~dAs+ny)$wJAJwx~T6D$44>PjVHQU$j61cE&)oWEv>hF)&_DGK5ju<%_nQ$BMA-f z(l&Nz-g$dTxtbzgd}kiVQIXaAWVKfYNu0amb?AM~ z=pk>d?Gp(B0I9?ujQ+=AkLP30F;)x!;QFEj0027H&UO}Xuno)+==9^3*B)*a@<3ey zAD0>z007`CDavU90Dvk000n@J3IG6}+)WP$0MIv-qcziF!)Dc(hyo_DfH(ZTgo*Ln!5ZFL4QcMOqrvKr7a@bcLFA%x}1BBG@wl zrD31-JlN&mQJIBIsnOFfHXU5x9DV4voUfCYFEnV0qA>9#+#5M&H)x5PXcS&TE(ssG zdWoHEed#$dp7Yli9@qh`F2-sAa|_qMmCa-CmY0`L9=CX3UG6Ur9xfUZmOhD+YKHF?I z$h92wZG1k}bl#Y2wrZ;+YP`EIx1{uB(aHYUi{LcrzSq0+-8c~t!P6#Wr8FDj{3OJ` z3i)0d^JI4pt@8S&a#XK+bMer;!=HYuH`0jTyKxf6wHS~YWoO!$YILcxR@1C2Bryx4 z7$1>N71M59*K%0D@Vd0>#m%d^EpsK}Daw;3-Dq&~w9h96}wCTqCIRr(~&dsD=I@2$=}8AmQ3leFrG8YbgAeBpo_8RafO;NV7?VawwZK&w4`)jU9Lp-q2FJRy?;4$0?-2hfO`n|BO9wf zPwpHAfHrnuE2tw-*2c*WZ0`vCdt>U~7~3Q4o$c+QaOeM_of859vj@YWu>Yam^|_q0 zqXiiGU+idq7bg;7g@ii(Uo5ZRJpZ1a>q!5*zu#G1r{t&A?+yUjf8z@bfk0t@9D4#G z&Pe2+d)jE z<`L;Qm3!P>?@qPc=+bsFCNpKg31w@d8zO<0XUO?xb}crxnH^+OoZURbJQE1p^f-uq zY56V==DwKNtB#5e8dTVQ6&19+L(5g#+&q}zs3520-t6QbvMtUM<)*rSIC61valG>B z;`FOVsoV3RU~UUKU8-JI&M$E>Bfh=vb$o3(#vxDNIw~FoW{ibNz~lCZ%c0ay#$~*#+pZZL`&r@n!BzK&p6l6rHd(-Q*K&Oe|C04o!5ZR(Bowyy&sD5cm)LzfK=(1 z3;)SZKv$`4#BBcIhamUTB;2po;esvMff?Rf?P-Z*qN_wxg%)9s;^~582ke)+M9?_y z4cT@^Yg-*b0++-CniuH$Ma`eZ9&x`V+N@S_<*|<$>9H$2bk)HYb(htu#pBd3stXZa zjJ|5AJsO*R@+ClETsbqKQMJT`eLf6d5_h-F^7Iw+p=X|sFUEM7bKD4%b3IWB3-Cq} zUMBxMW~gC|MNJ^NE?uuOy~D*uj`2v*gN%-z`9QfLWg@&9j-j*yd_Iy`uep&x0lN0Z z#5>5bU=o6whe_RXokq^g;~JWwE6I+2hDt_n7?3FWjT~<|5g&_gmTZWZRdeF_aaq0> zQ3RR^-oAfdDB?-kWQok8U%Yu!5TBknqPOcL#7b1e|JDsWx?&{)e!}O44{8ZA(*;o3 z{g^e>&h6qjR}~moUy*A?@xLnNY7ETy=);6vq~*gqdpT6YlFf~Gc0rd{wnUsl{jiRQ zwU~il73WP%XS*O*>fr{N%=DTbJTG2GlI2Qra&HR#wJkZ z$jdF`;n2&=@#zz@ra_+wRjdtjG1Uw*n1=i`KM z?E1Ed`V>)|DYx+$1$z?Rd2pmT7NGl}$BA__xyVA#_N_hHD2X$SKY;ip@~#Zh+1uxi z+{B!Gi%w2Dsy#z&OcPjP*o%1PF*fBh(0Z-|)w4H$20=Dp8wfkn7tH}+WVfW>Jc zF{7>u&-B&#{>1ovQILUj-m;~T0$%Bzk*&iU5}lht;}^9j1TH(%AY+HGsiZDy+^EDNu|Fx8pcmXDdX*e*U#HCbXZ}zvSxwz?8Rh-AR^VK3;A6HO*f8KiB@*yiptRR^gQ@m#td!{I)@%iJo)#?>; zM#PD@X2se%A|H^c#|H|{zFNtq)ubdQ6DdT3<3+pzRS((~w4YNIbB}Q`C@0WqrOhNK zO@DQ->L$e@=kh}ZO_;JL7p9u^ZC{;sdtY5lK@N=YD5@WLIPq*!gC||l%VypAn~ElM zYV5G-ur|UKmFK)66}KZocuQE(y}2ql@~jUWp($K*Tk~8r_ZVC=0N-y zx%e>tLZP`Li-?ZwXW-iDWmgc0xmIR>%AwE3KaepX@HG)0rRzG-ZU$3(oMwmXnWp3@ z?07gnl~L*=kE+qgU)^ocP63+V0Z2`8jwz{eP{EXCQl0O)a_<-F)NFfHCw`qdIH7j$ zy8p(55C)%MUQHiSZcEE&9(-ga_D0~g8*VbDcjwOLtT7#rX}HXNHk7F8$er^|YK74N*7L~$@VyYz-CT9(Nprq$9?V^y{MAkOq&r#(o& z)1-bwQ8_Eh@a2_8|6%(BmW;Jk#WlTMwaKOhZt1joEL^TM9_NZwOzc?uo-7Th5OZx& z2jta6d*(h<4s`YgR?|MFnReMxHO?y0P~oe0^}M$l6u{%vv)S#5YmE>PIL}efqq=JDclG#78+&%?R@bpU8L5u zilIH{18?1QlQphQa{^)&KrLO}~{p!{5xUj=ipIOglt0?nD` zTY)b>8FV^W#WxEAr8X@(2V`7lY@0$uiuw;-ShgKj53jltl4ZVs7G2Z|669k800x3T zK8t?$fkO@e0Mzd5+~rYHmj~K`;Wm$;j^FDAinZUu+Zphhj=7k&e2epNSU15V&FNx%Z!mfKQOes4Xj)LzX~d>$ za@TDkDVFE)>70%UB{qFXxm8^OrA%Zri_I$9!)0d|9~SSUzQole9(33wTzmT46G#T{ zzC)w)@#H6Q4)JQ$&uDd05-Z^aHh4n<52ntp@xD@FUKIETN%`IWN$J#p*Xm zKPlZmMM?P$%1?^-2g;8*d+qxNPrD|0KV|ecD1Srw{@Z-|YX>*$E@Q3#wUHWx7xL!$r%3aRCSyvyZU|?RW5dnw*AanpA?z)U0@P7c)gApMB literal 0 HcmV?d00001 diff --git a/Documentation/l1_control.odg b/Documentation/l1_control.odg deleted file mode 100644 index 69910c677459aae4664a08b1bbeb6aaed48721ad..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 11753 zcmeIYcT`hb6E+?}5RhIK&`_n9&^yvadPjN)0YXV23B3u@K|llnkuJSS?;uK15Ru+N zdM_eH`fuUAzIyNLeSi1+{{CjIvsQA>%rmoRpG@|%_h_nMUL^+rZ~%Z%Jyp3hOzt)g z003}#;Q#;tN0=iN?coFkJ2^SPAYe2M0q1drTX7@6C>V+x;RJ90}SAg!z% z{uB2vlfuW#%g6pMS{@?q?9Vm5((A#{@df>=N4j814A@)49(p|gy?>GWbTAS*g+v^9w?tE zm=7et#b;>=wG!sz;uYcpSwV%kz@ox}d?FAo5o=x{A*(-K|Mc>Q*Z=&sQI{`13JrtM zeNPp_@)Da1f^dPOB^mgI7}B}SEh%k1=k=$Nm3>RAY)dy{HWS$Nkov*;lLe8}!}v&L z(N|#_9VzNxV}R9rK7~VehXb+Q8l_jnJ+9ER?M4lV6pORGmfT}AN59BjO?Tb$UORUR z`zpGhzL`aZ^;%CYK-)7^qHmziq14>%#l0ONQe|lJQ-GDRhagVMCR^%mhQ=-mfe-2Bx`U}C#un++Os67@%!yw)4j1ta>ms?RJtUCm>@ zeU4O|5`?^XibmDQ7ObL$zf|BRZ$Z~S>_=1U5ho^!DFc%QU1-%teKUw#6q26`Zcm7s z=Lrz50|mQ$+_C%;uFq}s++SMxMqbE3DJQY#_95>6$~~w0JKWT+ zso(T8{aAJUHeUA<-z45bF9#~KK9VMW=jP*$znAgCiB!Gs)j}%=NxVGpDb^%OS-Oe7 zl4Wx=YlulWW9=&VfXdyDZtrc^gN{5REy0MJ75An|xAE&DzTvTp+pLS!PC4 z7g~A2ZxFDu;B~yy51Uau_Fuc%ywRpxI>Bv|Rv1o^O~c2)VS42HB`h%7#$X($3|SAK zE&Upm!UaiTq&O-y9gx`{td_6(B<%jwf2;4`FCV8b-bTvTttKIbt^qiVOQJC?Gt!QZ-3qnna!1$MI>%j{%!V zLhcrMndtnis}0?@%ece4+jKG%2##W)JOkV0%z*h?5WTUaGVzG~llkir90hx_xpyd2 zZ!3v5MFai9E3ZiBUM*3z|CZmDbgOU}6kuD%a(pAvEPr)UO>j}m#XPg5*kEbwi5Xd4 z<Nm3sws@^)Zo5n|nHJcxBNq>?%!s)fa3NfblmUU=%H&t|m`9jmeIszP>_#@B z^>VnJp&2DFmLN{1WJ*?5Gk3=2$==rH1aX(Y*iWHJX^X4t_y|@c(?w+_J9n@T+QICPGgCpyjuD@t=G00;t(zFhLTJ34HdP9P`Vk@)l)sDdl<_s45L z;A3O5KGSWes*1lRzS!kisO{k_Q>!_Ob>%8R0p`aLebnNu?1@c#3k3|_#p@m(u^ zCZka`f#NA}ga@~)>9gIt+&oX!a{&ibA8O`Q+>%geVA-+?)`*4qrP~IjeO0Y<{zB?VAd(47v;ASpd z)Qiyw?%MG zsa;PEZ^tu9oW<@Fx8%mI1f3Jodda*-iLN^{*4a~48UvLsdN*_*nbxb|za1>>y|uZY zcPtj&MYo;jm9>^issP~E;R?B>!PlvuH$3pe~6_s<&vdpt3C{4PPC&@#z zdl7vub&-cUi7`S25tD~*1p(r^*TgXSv=EsVXOH^})#t-j9hh@o9?RX)&NsfUNboA- zddf3(Iz8ba4)e+?K~;k=^Y#o3-rAl32~q~(XVx}Nl%5pKcyqzlu>s};mrpG56RwJav|C;q(H!CZ#F6|1ph98jyekYr54+8&4N#>1k z=zaVnvzN{bZY3MT6g&P@C7vlueZc(C$7>yZ7<8wOdU0CrHO20XjXOei-8jJpn)bLB zLkg)SL+f-#CUOh1!rV-%p4W0gpWY2Tyfam(DQ*xefScnF^_<&|z>1La2rgzcmr%L! zytOeK$$R(=e(n_`7qTNzQO0aS?I!qe#;v>XA(~-NH7|=7tJWJ7t<^0+1_!U|Eg$O{ zJ)5Hkhp-a9DMxYKQa0P~58Wa${W=!>M&SM!wNSAg>jf=g!fRUud6%~!zE%o-JU*|f zqO!fkph$$h+MOvPb5}p_fx;P#QWK>`mwI+A93mRU!`mFoT|}Y)KEB|Um!L|7ZDZp? zO{$1Ulh)^k_NxA-m*E`CUAFQOUll0+jzeD#m-po*$PP7oK`pxh&@1tlf)s{kDb@lPe8KAm2SH=eZ38T>-HI8 z>Q2a6*TIQtF!9Z*vTw%3Ggld~9d&5(>v2dEZ9QWU>v^o6)DLt$V@U3Pe)1w*!_9N& zrR3Bq!!f4OYbFk_=Fsi}V|jSF2CKg9+~;aajYF56$!|x1a|b$aTC1Ac#e`%F;I%Bv z!I`b0%FqJAZA&c6whctQO+>CFWpBzGHHl5Hb`&o_T16sFB5l!EhnB)+aRanYkvuh5C)W_tpLt|gza4s z$-tF9;y2Wy%7Gsk8LZNnWH)SaNO~lc1Bo&EL?}n<+CD$f$NH8#_-x8ChE0h(C*n=_>pdv8 zsRo1UrW_Uouj|&D(sH1L*$M{kdKlG1A2#cRDJBKo8bIT35Obd?>(HEY_t+|%HH9(2zA)gy+WHq{PLPpSYlUjVoZ>_awD|^~l@^e9(J&(I%GS@oErJLS`D+pM`ikL3I z8%AFoYu&)A`=Ikk=kaLH`#S%dQ&h*@uV}ni2lVkmcZ63?y_ks(&`Eb3grW5Kg`<)N z=MHoVz>3!sl;w>3>9@xE1`bC*g^9wIYBss$0?Z&EBb~)jHE9eC^T7_I8UAQKxRoQ# z`yzL)UC3n2_L^ET(r)n6v{2-mZEm@!>AR$wdmZkA@U0EBq2vbd?U$;#kbvoXiA3nA zL#DpK>1!}1u9k%9b&LkggGXpPPwW|Eog3D^CIjGoFB>TNZnY|ZHVam4z_mMIHFCSY|~|eS91GRq*3mvGP(E>S{@i?d6c-n{#NKazVI=xt)SE z?SV&>kikjac<<1;z>;mqGc(7zd-sKJ9q(%&OZX?V`Q+>+_1CaA)ek%5t4$eYMbCeu ze_NCA&?%)SSNt9WXN^X*?D+l*mYuHm1GnZ|2U3aM-C5k1%y!;g-JyLGyfaokg!PIj zjcaxS?GZmMWIMdAuykktqf2K>v|-)1jA=qj>Yibz;mN%mK_65n4_`K~e}AXgE6vv= zy)c*3$^c!YC*5=EVi|8k-6RNO)ZBKu%b!QTIFPf2QgOl?U}E>90--sYSd!+-;1@Ao_VrltH%<(H(Z>%`)SR~)@=%P zJUuiWU3bVwv=&)%0nQn-EXUdRPcomy_~Na({0R?KO*fyYN}}E}$~@rrPGdv{<&rL`LQ9H>;Kx zL?hZb+^2u9k1Y}jxW;t$q#`9twkRx2U?Fh6s_H$cFmK?54)+_ zeIvK298)li@2S>XSN!mHTCB0>P*K`&lJ=|bMoFt|4eeArpvAA)ll)3J3FPw^wHylW zeBcn(169O8EGfoi$k%Y4iCI*uaNgWN-B_d5!nRAK=N@Fk!C%UwwR_S-k`>zeumf9h zvYZ%v_sdRH(qjOQJV7w0HC>d4s%Uu%_^v#;S$`Pr2f^e+jo6)|cWq1Wt_po2e*JMQ z8BccjQ4B0vG<-mMSujU6E@Fgd+ZKzM3dqPkO`;)Y^o6_)>p?HJKa{#Xt1XEB6Sf=Y zea@{!_2_Eao{T%W0g9X-+YKTB*|vcjOYCrd42J+Olb|pqe|v{?_RgzF&=Ux;yc(HS z1Y@fjzLllRhb7ALS60IEd6+G7_IDL|b=;I?>OlLZ9i<`9wk#H>7$2QnSg^<4{-y0<5j`Qw)4^z`%_Sod+m zRC|H(ZnSxX6t-YU-D&-P@i1SHaeem)_sC-@?ENUHpyUGd&2BVPj~Jki=BzSSAfMh4 zgf8(CF>AAvv0WG&Xusej_i@Nhl@6x-cC=sdLbjl^A)XwP>wl0D?c04QWh6Ht9uyZw z?x!!H;@g+A*9XZm9muFswI8-WAyJ25odPdf>1w|J3T9V5n;p@3tN zC(}=Er&#A`K=@Z6QIf(mN;THO%~f5DgPq;OVtztirw5IBIzUGcGOO7cpR|bE`VyE$ z(GZ9FzE^XS>sg&DUZ8=hEmVYvev5|B9?gBL0~L2}Ptg>qdwU>TTfC$)l(?ttMdjX; zqH=mfK+EO0=N?EUhxjr$1pHqlpG zDDA$E-}H%!sr@eN9%w=`Cq(Obw0H4Nv@CFdi*`!KR8TZU>Ht;uKI>Dbd^bxpM~Bu5 zmFm`q`+;Uc}ufCFoP}2cLbK@>I9s^B$^`VAGT4d4lX{m*J)v-;w=*kBMQ*?e zu3Nl*oTn8+%c)BXWv8j;o&fRFO#ad8=tdAo4ppq{Q}%V?OU%%H$3-c9qRhghB^Iw{ zw++-Vn_A(%mh`ew1C9OEMV^}92>(3mv%|irI>Y18I~N_BTP=qdbN@sFT|4IlC^@rzT;1|I#%qIOg(S$xi~t0#y(a=Q0G zQ;qU(LR6&22u=)TX}JzJ=7-o`dZ;JncpYecFzrs1FoI7gfsI`V+_oT@o{O0;OOg-! zCJWw`Qjsf_CLe;IqB4>XL0zdTu3x3@L^Z{C$0Q$WwaGgqMb-NOgI2-*N_!xA;jG&u zorDG!lkew`^G0owR>FJ|d2_rhf!0y0w?{(W&of7_KT7zA%JrLK_Wy4EDC_@MnK&i7 zc~#xHXf(?8@<>5hL0!H?)*|4q$DO|%HUNMC06-1_w}#pLd18a&J2V2}-~@(49sUpPF3)3JP?lii|FEO|U7SdS4HAm_ z|FOLM%l~_NE+hTl{(fh5nUbGYzdQJ0`He3y1Oj#V=4(lsm)Y>Sgzgbm`fjAc#0Q#2 zH!RDpxMH_{+^CaLiEF=gpPaQ|D%Ic}eF*I?rN^U2)tE-5aOqe!R7GbEz@%}D?7Z0!(-eQTvY=}91o;Dg1GflmlDis6glMO?|cw$?HSgQDCcW+}rV z?HD{=I+OT>d9#Ia^zOQ^q*%k&2qtgmP0FmVsvK@Q&}Wev@ek(4mkg~j*S$7durkhP zCemCzn0L!iY*U1~W$BA6ypOBxdqokZdf3(#_HJ2pi6={AebcmhGdS#FPJ{DVYanaq z2$9~CyO>R%t`W#@>2=jOFULKOqDEhfy-I}^*t>s7rx%3b$NPrEJSO(~7)ERqi)kXW z9UJ3QQJNflW=Gu-24zpZ)|4coy(`I7Q#$2GUF$EfJITOh2{_M%fwmR>Lt6h&8Wg2T*IXEU=2&ZADAR$>^l8vHn8CNdnqTW(+!TB+RFsI1(Lfj zAVRzFWd0%A>>5N|&8v}iC5Ai4+g&L%+s5=F@>kj_b$KtINKd-AG9}8lT2UrCTRf8p z1d$h%apFmNH2at{V_n?S3@H5IvpVJA>PCg-HuF{_e{jb7_=(Dy6P4-A6vUZ|YfA$1 z>{Ef2dqm`sgC#!84i#_GDPf!+LU0qzeUFpISQEGPosWDZxxIq@?A-oH6diKUmf1OD zivmpS?Zci4w3QhTjRncDUMExH<9@hf1rU!O`ivXx2iE>vIqzDURy_SO3!RhPDo~3z z-*~;MkeVRBT1If5I6<_8EOzesSoi0)$m|=V&R+AySls;7UXM$B6tf?uurmcMXyx0N zg^A8PnSWao)AZZ;zy>XfZ)5XFG4psD$0NMM-ll@$XBE-z70tL6Ok|s=P1aM3m4K<`bHq-XDW3*u=fXGsa{LpEf2^$oqc8GLNOv!>7qbv zB1+aQsN7Qxx(XiJmS%Pd28E1JZ@0Vt2 zY2TD1Z4J&0E=|s7KgPWqwv;^x!KYfME(NoK?t_TS$Uk?xNgt@;D$xe-pFiYJ+zr)u zY*|&z^q$DnenI@&ErLZzmW4!>3OvQpDy>}6vI%~5rfD6eW=eU(^*T#--!m~w!$$`SMlr>izSt+pNk zAn6l)*;neq467eH$!rTN=pfngM>0p2#PLU2v}fxkOiA5j%pbh6as8^PO;j7_-C!~w z`dnO1T^r(7Xf^|>B)V?}ucqlulU}_~R)iCFC!ScWTnY7~1~L9FTRncog*Kd_TB-KE zEE0M>uPOJ7;}i3VK$O|s!i(znF#j87{x40}4Yo*XG>T6ek8bPloC%pbFQv#e1Ych^ zIF9`cjM&4c&FpAk+Vx*kC<;FlrMR7)qz^ECJEXG>z!Ge43l#%hXWXWnsyc4uCj0JCvZWKo(%>AGC_9%=Ag;@^*5}_UdWJ{!)*1mfGM4Zvqf}} zI{MMKH3QOag80`BOH`55Ihq5g_RO;)z^zIQsTza;K`FM8lK**WuHfML4)SIG-m$yR z`;C&;)cy~elXbS#itef+T#J(f;cIceaYCOih((F(>~bX4S8|bW1rGMqZ*JC)t7ya= zTA%nnOW;U#NE;ZKcVqD~YsSEk6lWh&R%5&Aa9`RKE$7DhwNSTa+sid^X?kxF+oMCF z+>6KoKEgPg-Y**1@<~3mxA(pkh4wdE5W#skNo`Aw#F#V4mnsR{#z@3~Q@W)CmgKsZS>LGX-q?c>WksjVt+&0soGj@z zwjZlPWad(5wAxe?V<)m-k-+Ouc#x0$FgSQ2sg}l!jp}~s)g(9Qwndr9< zEg$LKaMNG)`r0=T#o>BX*KM|}qq@LZ(t_zhq|$N$?4LZRx?Ck)>C)b)z3d$1|FKH? z-Fk-t005{d=*n;@t1I$2g5fZ0DC+y!aj{M-{4E`R(;)}LmVa?BF7qb1zd8K`p!k9u z7av1M&`+8)-!Ug&Cu`pta?)fYIk3_DQJ*FBy}QF0haZ?C{UD_^1DX~Ref)USExChD zK$0moA)OtSSOV)t%CGA2DQ6;Mm|&}z7RxTK51IT9x|3F)b6s^9gX>Iv^Mqt@e|2tj zIUE}oV->4boy4q@6kmxffZ>1QqX-Z*(t?q(Y<%NPwQn-+VE}I`2JXJ2*lMqFH$Xlo1REyk{+Z^K$mmO{>&}9=_c}pqpER6 zCH8gZeXSMR`I+Ri==Iiy`!`u|C1^?#a5&C#``%=23D1oHr+hFklUe{+>?FMc08C?E zCiMyiIpCk$Gyf_-nl=A!_4gjkpQ3fy;`wJs<`1hMQ}}B;z-8mqPwBt<*EXuZTmHJ` zciArRQz-HN(me3P@Sk=Be~JU;KkpCz9p$Ih9~|tb+`aYRl;gh`*H* z-+=wf(Ecq>)o*ZqWo-WzXY)5WzcRRg#<}EhKZW`Be<$g$jP9S2E(y_3G5-zHPloqz zQR07t@{{rXf%4;>z4ZNqwq0_*pVIvslz(7=|LcAF>i`*m|DCFz{O_-!{aPz88Q@Pb s1pdkge|Y~fq+iRyWhMP7_gMd-u4<}aVPC2d1Bd}aR{?+&wo9Y`4}A~OqW}N^ From 40610c7d484d077dc10726628c3adbe01edd91df Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Oct 2013 10:38:51 +0200 Subject: [PATCH 21/48] Fixes, but approach needs proper design --- .../fw_pos_control_l1_main.cpp | 66 ++++++++++++++----- src/modules/mavlink/waypoints.c | 12 ++-- 2 files changed, 57 insertions(+), 21 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 219c6ffa6b..9ed74f0caf 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -158,6 +158,12 @@ private: float _launch_alt; bool _launch_valid; + /* land states */ + /* not in non-abort mode for landing yet */ + bool land_noreturn; + /* heading hold */ + float target_bearing; + /* throttle and airspeed states */ float _airspeed_error; ///< airspeed error to setpoint in m/s bool _airspeed_valid; ///< flag if a valid airspeed estimate exists @@ -329,7 +335,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_error(0.0f), _airspeed_valid(false), _groundspeed_undershoot(0.0f), - _global_pos_valid(false) + _global_pos_valid(false), + land_noreturn(false) { _nav_capabilities.turn_distance = 0.0f; @@ -635,9 +642,6 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* no throttle limit as default */ float throttle_max = 1.0f; - /* not in non-abort mode for landing yet */ - bool land_noreturn = false; - /* AUTONOMOUS FLIGHT */ // XXX this should only execute if auto AND safety off (actuators active), @@ -722,15 +726,26 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), next_wp.getX(), next_wp.getY()); - - if (wp_distance < 5.0f || land_noreturn) { + float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY()); + //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); + if (wp_distance < 15.0f || land_noreturn) { /* heading hold, along the line connecting this and the last waypoint */ - float target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + + + // if (global_triplet.previous_valid) { + // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + // } else { + + if (!land_noreturn) + target_bearing = _att.yaw; + //} + + warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); - if (altitude_error > -20.0f) + if (altitude_error > -5.0f) land_noreturn = true; } else { @@ -739,6 +754,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); } + /* do not go down too early */ + if (wp_distance > 50.0f) { + altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; + } + + _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -748,15 +769,21 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) float land_pitch_min = math::radians(5.0f); float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; + float airspeed_land = _parameters.airspeed_min; + float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; - if (altitude_error > -5.0f) { + if (altitude_error > -4.0f) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + /* land with minimal speed */ + + /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ + _tecs.set_speed_weight(2.0f); + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, 0.0f, _parameters.throttle_max, throttle_land, - land_pitch_min, math::radians(_parameters.pitch_limit_max)); + math::radians(-10.0f), math::radians(15.0f)); /* kill the throttle if param requests it */ throttle_max = math::min(throttle_max, _parameters.throttle_land_max); @@ -764,9 +791,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - } else if (altitude_error > -20.0f) { + } else if (wp_distance < 60.0f && altitude_error > -20.0f) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + /* minimize speed to approach speed */ + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -774,6 +803,8 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { + /* normal cruise speed */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), @@ -866,6 +897,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } } + /* reset land state */ + if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { + land_noreturn = false; + } + if (was_circle_mode && !_l1_control.circle_mode()) { /* just kicked out of loiter, reset roll integrals */ _att_sp.roll_reset_integral = true; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index adaf814047..7e4a2688f6 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -398,13 +398,13 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (time_elapsed) { - if (cur_wp->autocontinue) { + /* safeguard against invalid missions with last wp autocontinue on */ + if (wpm->current_active_wp_id == wpm->size - 1) { + /* stop handling missions here */ + cur_wp->autocontinue = false; + } - /* safeguard against invalid missions with last wp autocontinue on */ - if (wpm->current_active_wp_id == wpm->size - 1) { - /* stop handling missions here */ - cur_wp->autocontinue = false; - } + if (cur_wp->autocontinue) { cur_wp->current = 0; From 1d3f25ee6c9983ec5da9de4d4f7b463f880f3a87 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 19 Oct 2013 10:43:41 +0200 Subject: [PATCH 22/48] 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 23/48] 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 24/48] 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 25/48] 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 26/48] 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 4b63c5488582241c7e3af45dce74c112dbfa60bd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 27 Oct 2013 14:48:12 +0100 Subject: [PATCH 27/48] l1: fix constrain of sine_eta1 --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 196ded26ca..27d76f959c 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -190,7 +190,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float xtrackErr = vector_A_to_airplane % vector_AB; float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f); /* limit output to 45 degrees */ - sine_eta1 = math::constrain(sine_eta1, -M_PI_F / 4.0f, +M_PI_F / 4.0f); + sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071 float eta1 = asinf(sine_eta1); eta = eta1 + eta2; /* bearing from current position to L1 point */ From 9064f8bf09dd91388b9fd3e66568d086bf1be69b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Oct 2013 15:49:26 +1100 Subject: [PATCH 28/48] px4io: fixed the io_reg_{set,get} errors this fixes the PX4IO state machine to avoid the io errors we were seeing. There are still some open questions with this code, but it now seems to give zero errors, which is an improvement! --- src/modules/px4iofirmware/i2c.c | 6 ------ src/modules/px4iofirmware/px4io.c | 10 ++-------- src/modules/px4iofirmware/serial.c | 20 +++----------------- 3 files changed, 5 insertions(+), 31 deletions(-) diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 10aeb5c9fa..79b6546b39 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -149,12 +149,6 @@ interface_init(void) #endif } -void -interface_tick() -{ -} - - /* reset the I2C bus used to recover from lockups diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index e70b3fe880..e28106971a 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -159,9 +159,6 @@ user_start(int argc, char *argv[]) /* start the FMU interface */ interface_init(); - /* add a performance counter for the interface */ - perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface"); - /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); @@ -203,11 +200,6 @@ user_start(int argc, char *argv[]) /* track the rate at which the loop is running */ perf_count(loop_perf); - /* kick the interface */ - perf_begin(interface_perf); - interface_tick(); - perf_end(interface_perf); - /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); @@ -218,6 +210,7 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); +#if 0 /* check for debug activity */ show_debug_messages(); @@ -234,6 +227,7 @@ user_start(int argc, char *argv[]) (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } +#endif } } diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 94d7407dff..e9adc8cd69 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -74,9 +74,6 @@ static DMA_HANDLE rx_dma; static int serial_interrupt(int irq, void *context); static void dma_reset(void); -/* if we spend this many ticks idle, reset the DMA */ -static unsigned idle_ticks; - static struct IOPacket dma_packet; /* serial register accessors */ @@ -150,16 +147,6 @@ interface_init(void) debug("serial init"); } -void -interface_tick() -{ - /* XXX look for stuck/damaged DMA and reset? */ - if (idle_ticks++ > 100) { - dma_reset(); - idle_ticks = 0; - } -} - static void rx_handle_packet(void) { @@ -230,9 +217,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) /* disable UART DMA */ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); - /* reset the idle counter */ - idle_ticks = 0; - /* handle the received packet */ rx_handle_packet(); @@ -308,6 +292,7 @@ serial_interrupt(int irq, void *context) /* it was too short - possibly truncated */ perf_count(pc_badidle); + dma_reset(); return 0; } @@ -343,7 +328,8 @@ dma_reset(void) sizeof(dma_packet), DMA_CCR_MINC | DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS); + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIVERYHI); /* start receive DMA ready for the next packet */ stm32_dmastart(rx_dma, rx_dma_callback, NULL, false); From 75a0c18a9e56ac64e043cce00223ea8a627d24a5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Oct 2013 15:49:57 +1100 Subject: [PATCH 29/48] px4io: FMU half of px4io error fixes --- src/drivers/px4io/px4io_serial.cpp | 28 +++++++++++++++++----------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 236cb918dc..43318ca849 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include @@ -262,7 +263,8 @@ PX4IO_serial::init() up_enable_irq(PX4IO_SERIAL_VECTOR); /* enable UART in DMA mode, enable error and line idle interrupts */ - /* rCR3 = USART_CR3_EIE;*/ + rCR3 = USART_CR3_EIE; + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; /* create semaphores */ @@ -497,22 +499,20 @@ PX4IO_serial::_wait_complete() DMA_SCR_PBURST_SINGLE | DMA_SCR_MBURST_SINGLE); stm32_dmastart(_tx_dma, nullptr, nullptr, false); + //rCR1 &= ~USART_CR1_TE; + //rCR1 |= USART_CR1_TE; rCR3 |= USART_CR3_DMAT; perf_end(_pc_dmasetup); - /* compute the deadline for a 5ms timeout */ + /* compute the deadline for a 10ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); -#if 0 - abstime.tv_sec++; /* long timeout for testing */ -#else - abstime.tv_nsec += 10000000; /* 0ms timeout */ - if (abstime.tv_nsec > 1000000000) { + abstime.tv_nsec += 10*1000*1000; + if (abstime.tv_nsec >= 1000*1000*1000) { abstime.tv_sec++; - abstime.tv_nsec -= 1000000000; + abstime.tv_nsec -= 1000*1000*1000; } -#endif /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ int ret; @@ -619,8 +619,8 @@ PX4IO_serial::_do_interrupt() */ if (_rx_dma_status == _dma_status_waiting) { _abort_dma(); - perf_count(_pc_uerrs); + perf_count(_pc_uerrs); /* complete DMA as though in error */ _do_rx_dma_callback(DMA_STATUS_TEIF); @@ -642,6 +642,12 @@ PX4IO_serial::_do_interrupt() unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { perf_count(_pc_badidle); + + /* stop the receive DMA */ + stm32_dmastop(_rx_dma); + + /* complete the short reception */ + _do_rx_dma_callback(DMA_STATUS_TEIF); return; } @@ -670,4 +676,4 @@ PX4IO_serial::_abort_dma() stm32_dmastop(_rx_dma); } -#endif /* PX4IO_SERIAL_BASE */ \ No newline at end of file +#endif /* PX4IO_SERIAL_BASE */ From 52ee477137a293d40fc552f4100c52d19b142fc1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Oct 2013 15:50:33 +1100 Subject: [PATCH 30/48] lsm303d: try to reset the lsm303d if it goes bad in flight this is based on earlier work by Julian Oes --- src/drivers/lsm303d/lsm303d.cpp | 32 +++++++++++++++++++++++++++++--- 1 file changed, 29 insertions(+), 3 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 8bed8a8df0..60601e22c8 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -241,11 +241,18 @@ private: perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; + perf_counter_t _reg7_resets; + perf_counter_t _reg1_resets; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; math::LowPassFilter2p _accel_filter_z; + // expceted values of reg1 and reg7 to catch in-flight + // brownouts of the sensor + uint8_t _reg7_expected; + uint8_t _reg1_expected; + /** * Start automatic measurement. */ @@ -434,9 +441,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), + _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), + _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ) + _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _reg1_expected(0), + _reg7_expected(0) { // enable debug() calls _debug_enabled = true; @@ -526,10 +537,12 @@ void LSM303D::reset() { /* enable accel*/ - write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); + _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; + write_reg(ADDR_CTRL_REG1, _reg1_expected); /* enable mag */ - write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + _reg7_expected = REG7_CONT_MODE_M; + write_reg(ADDR_CTRL_REG7, _reg7_expected); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); @@ -1133,6 +1146,7 @@ LSM303D::accel_set_samplerate(unsigned frequency) } modify_reg(ADDR_CTRL_REG1, clearbits, setbits); + _reg1_expected = (_reg1_expected & ~clearbits) | setbits; return OK; } @@ -1210,6 +1224,12 @@ LSM303D::mag_measure_trampoline(void *arg) void LSM303D::measure() { + if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) { + perf_count(_reg1_resets); + reset(); + return; + } + /* status register and data as read back from the device */ #pragma pack(push, 1) @@ -1282,6 +1302,12 @@ LSM303D::measure() void LSM303D::mag_measure() { + if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) { + perf_count(_reg7_resets); + reset(); + return; + } + /* status register and data as read back from the device */ #pragma pack(push, 1) struct { From 1336d625a83ba3f1afc207b64ec248dd1e15848a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Oct 2013 14:47:37 +0100 Subject: [PATCH 31/48] Hotfix: Announcing important messages via audio --- src/modules/commander/commander.cpp | 58 ++++++++++++++--------------- 1 file changed, 29 insertions(+), 29 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9814a7bccf..44a1442968 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -379,7 +379,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } // TODO remove debug code - //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); + //mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ arming_res = TRANSITION_NOT_CHANGED; @@ -496,11 +496,11 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); + mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); + mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } @@ -521,13 +521,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe tune_negative(); if (result == VEHICLE_CMD_RESULT_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command); + mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command); } else if (result == VEHICLE_CMD_RESULT_FAILED) { - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command); + mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command); } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command); + mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); } } @@ -874,10 +874,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "[cmd] LANDED"); + mavlink_log_critical(mavlink_fd, "#audio: LANDED"); } else { - mavlink_log_critical(mavlink_fd, "[cmd] IN AIR"); + mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } } @@ -955,7 +955,7 @@ int commander_thread_main(int argc, char *argv[]) //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); + mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; battery_tune_played = false; @@ -967,7 +967,7 @@ int commander_thread_main(int argc, char *argv[]) /* critical battery voltage, this is rather an emergency, change state machine */ if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); + mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; battery_tune_played = false; @@ -1069,12 +1069,12 @@ int commander_thread_main(int argc, char *argv[]) /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time"); + mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained"); + mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; } } @@ -1143,7 +1143,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (res == TRANSITION_DENIED) { warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } /* fill current_status according to mode switches */ @@ -1159,12 +1159,12 @@ int commander_thread_main(int argc, char *argv[]) } else if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } @@ -1189,7 +1189,7 @@ int commander_thread_main(int argc, char *argv[]) if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); } /* check which state machines for changes, clear "changed" flag */ @@ -1506,7 +1506,7 @@ print_reject_mode(const char *msg) if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; char s[80]; - sprintf(s, "[cmd] WARNING: reject %s", msg); + sprintf(s, "#audio: warning: reject %s", msg); mavlink_log_critical(mavlink_fd, s); tune_negative(); } @@ -1520,7 +1520,7 @@ print_reject_arm(const char *msg) if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; char s[80]; - sprintf(s, "[cmd] %s", msg); + sprintf(s, "#audio: %s", msg); mavlink_log_critical(mavlink_fd, s); tune_negative(); } @@ -1617,14 +1617,14 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c if (res == TRANSITION_CHANGED) { if (control_mode->flag_control_position_enabled) { - mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD"); + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD"); } else { if (status->condition_landed) { - mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)"); + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)"); } else { - mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD"); + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD"); } } } @@ -1660,22 +1660,22 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); tune_negative(); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); tune_negative(); break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command); tune_negative(); break; case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); tune_negative(); break; @@ -1814,14 +1814,14 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR"); /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) - mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } @@ -1834,14 +1834,14 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + mavlink_log_critical(mavlink_fd, "#audio: parameters save error"); /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) - mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } From 0fa03e65ab3ab0e173e487b3e5f5321780f3afff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Oct 2013 15:21:50 +0100 Subject: [PATCH 32/48] Cleanup of Doxygen tags --- src/drivers/drv_accel.h | 4 +++- src/drivers/drv_airspeed.h | 5 ++++- src/drivers/drv_baro.h | 4 +++- src/drivers/drv_gps.h | 4 +++- src/drivers/drv_gyro.h | 4 +++- src/drivers/drv_rc_input.h | 3 ++- src/drivers/drv_sensor.h | 4 +++- src/drivers/drv_tone_alarm.h | 4 +++- 8 files changed, 24 insertions(+), 8 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 8a4f684284..7d93ed9383 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Accelerometer driver interface. + * @file drv_accel.h + * + * Accelerometer driver interface. */ #ifndef _DRV_ACCEL_H diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h index 7bb9ee2af1..78f31495d9 100644 --- a/src/drivers/drv_airspeed.h +++ b/src/drivers/drv_airspeed.h @@ -32,7 +32,10 @@ ****************************************************************************/ /** - * @file Airspeed driver interface. + * @file drv_airspeed.h + * + * Airspeed driver interface. + * * @author Simon Wilks */ diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index aa251889f7..e2f0137ae2 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Barometric pressure sensor driver interface. + * @file drv_baro.h + * + * Barometric pressure sensor driver interface. */ #ifndef _DRV_BARO_H diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index 398cf48701..06e3535b30 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file GPS driver interface. + * @file drv_gps.h + * + * GPS driver interface. */ #ifndef _DRV_GPS_H diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index fefcae50b9..2ae8c70582 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Gyroscope driver interface. + * @file drv_gyro.h + * + * Gyroscope driver interface. */ #ifndef _DRV_GYRO_H diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 4decc324ef..78ffad9d7b 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -32,8 +32,9 @@ ****************************************************************************/ /** - * @file R/C input interface. + * @file drv_rc_input.h * + * R/C input interface. */ #ifndef _DRV_RC_INPUT_H diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 3a89cab9dd..8e8b2c1b95 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Common sensor API and ioctl definitions. + * @file drv_sensor.h + * + * Common sensor API and ioctl definitions. */ #ifndef _DRV_SENSOR_H diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 2fab37dd21..cb5fd53a5f 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -31,7 +31,9 @@ * ****************************************************************************/ -/* +/** + * @file drv_tone_alarm.h + * * Driver for the PX4 audio alarm port, /dev/tone_alarm. * * The tone_alarm driver supports a set of predefined "alarm" From a06b3e50ab4d4c73fd400ec2891b1ab7a9eb8451 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 29 Oct 2013 13:38:44 +0100 Subject: [PATCH 33/48] Only read 5 values, then return --- src/examples/px4_simple_app/px4_simple_app.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 7f655964d9..44e6dc7f35 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -71,7 +71,7 @@ int px4_simple_app_main(int argc, char *argv[]) int error_counter = 0; - while (true) { + for (int i = 0; i < 5; i++) { /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ int poll_ret = poll(fds, 1, 1000); From 2293aa4e0ae809e31ba1aa71492253460a5e3aab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Oct 2013 21:22:05 +0100 Subject: [PATCH 34/48] 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 35/48] 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 36/48] 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 37/48] 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 38/48] 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 39/48] 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 40/48] 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 41/48] 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 42/48] 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 43/48] 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 44/48] 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 45/48] 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 46/48] 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 47/48] 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); From 2fd1aed6be03de42d09c062871838ee7e852aa4a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Nov 2013 12:02:45 +0100 Subject: [PATCH 48/48] Disable the segway app, as its GCC 4.7 incompatible and not generally used --- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index f9061c1100..3068270865 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -79,7 +79,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -MODULES += modules/segway +#MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index ed2f2da5ed..761fb8d9d6 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -78,7 +78,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -MODULES += modules/segway +#MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control @@ -120,7 +120,7 @@ MODULES += lib/conversion #MODULES += examples/math_demo # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/hello_sky -#MODULES += examples/px4_simple_app +MODULES += examples/px4_simple_app # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/daemon