diff options
Diffstat (limited to 'src/systemcmds/esc_calib/esc_calib.c')
| -rw-r--r-- | src/systemcmds/esc_calib/esc_calib.c | 170 |
1 files changed, 96 insertions, 74 deletions
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 188fa4e37..1ea02d81e 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,6 +46,7 @@ #include <stdbool.h> #include <unistd.h> #include <fcntl.h> +#include <poll.h> #include <sys/mount.h> #include <sys/ioctl.h> #include <sys/stat.h> @@ -63,41 +65,46 @@ static void usage(const char *reason); __EXPORT int esc_calib_main(int argc, char *argv[]); -#define MAX_CHANNELS 8 +#define MAX_CHANNELS 14 static void usage(const char *reason) { if (reason != NULL) warnx("%s", reason); - errx(1, - "usage:\n" - "esc_calib [-d <device>] <channels>\n" - " <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" - " <channels> Provide channels (e.g.: 1 2 3 4)\n" - ); + + errx(1, + "usage:\n" + "esc_calib [-d <device>] <channels>\n" + " <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " <channels> Provide channels (e.g.: 1 2 3 4)\n" + ); } int esc_calib_main(int argc, char *argv[]) { - const char *dev = PWM_OUTPUT_DEVICE_PATH; + char *dev = PWM_OUTPUT_DEVICE_PATH; char *ep; bool channels_selected[MAX_CHANNELS] = {false}; int ch; int ret; char c; + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + if (argc < 2) usage(NULL); - while ((ch = getopt(argc, argv, "d:")) != EOF) { + while ((ch = getopt(argc - 1, argv, "d:")) != EOF) { switch (ch) { - + case 'd': dev = optarg; - argc--; + argc -= 2; break; default: @@ -105,7 +112,7 @@ esc_calib_main(int argc, char *argv[]) } } - if(argc < 1) { + if (argc < 2) { usage("no channels provided"); } @@ -116,135 +123,150 @@ 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; } } } - /* Wait for confirmation */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - if (!console) - err(1, "failed opening console"); - - warnx("ATTENTION, please remove or fix props before starting calibration!\n" - "\n" - "Also press the arming switch first for safety off\n" - "\n" - "Do you really want to start calibration: y or n?\n"); + 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"); /* wait for user input */ while (1) { - - if (read(console, &c, 1) == 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') { - warnx("ESC calibration exited"); - close(console); + printf("ESC calibration exited\n"); exit(0); + } else if (c == 'n' || c == 'N') { - warnx("ESC calibration aborted"); - close(console); + printf("ESC calibration aborted\n"); exit(0); + } else { - warnx("Unknown input, ESC calibration aborted"); - close(console); + 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); - // XXX arming not necessaire at the moment - // /* Then arm */ - // ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); - // if (ret != OK) - // err(1, "PWM_SERVO_SET_ARM_OK"); + /* get max PWM value setting */ + uint16_t pwm_max = 0; + ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, &pwm_max); - // ret = ioctl(fd, PWM_SERVO_ARM, 0); - // if (ret != OK) - // err(1, "PWM_SERVO_ARM"); + 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 */ - warnx("Set high PWM\n" - "Connect battery now and hit ENTER after the ESCs confirm the first calibration step"); + /* 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<MAX_CHANNELS; i++) { - if(channels_selected[i]) { - ret = ioctl(fd, PWM_SERVO_SET(i), 2100); if (ret != OK) err(1, "PWM_SERVO_SET(%d)", i); } } - if (read(console, &c, 1) == 1) { + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + if (c == 13) { - + break; + } else if (c == 0x03 || c == 0x63 || c == 'q') { warnx("ESC calibration exited"); - close(console); exit(0); } } + /* rate limit to ~ 20 Hz */ usleep(50000); } - /* we don't need any more user input */ - - - warnx("Set low PWM, hit ENTER when finished"); + 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<MAX_CHANNELS; i++) { - if(channels_selected[i]) { - ret = ioctl(fd, PWM_SERVO_SET(i), 900); + /* set disarmed PWM */ + for (unsigned i = 0; i < MAX_CHANNELS; i++) { + if (channels_selected[i]) { + ret = ioctl(fd, PWM_SERVO_SET(i), pwm_disarmed); + if (ret != OK) err(1, "PWM_SERVO_SET(%d)", i); } } - if (read(console, &c, 1) == 1) { + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + if (c == 13) { - + break; + } else if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("ESC calibration exited"); - close(console); + printf("ESC calibration exited\n"); exit(0); } } + /* rate limit to ~ 20 Hz */ usleep(50000); } - - warnx("ESC calibration finished"); - close(console); - - // XXX disarming not necessaire at the moment - /* Now disarm again */ - // ret = ioctl(fd, PWM_SERVO_DISARM, 0); - - + printf("ESC calibration finished\n"); exit(0); -}
\ No newline at end of file +} |