diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-03 21:34:54 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-03 21:34:54 +0200 |
commit | 8a615a9741ad1dd7406a804349329345573da5b5 (patch) | |
tree | fc7828bba5a2148801a445091ec3897f49398eae /apps/ardrone_interface | |
parent | 79801b15789217f15ad3fbd15938c3f6b09425a5 (diff) | |
download | px4-firmware-8a615a9741ad1dd7406a804349329345573da5b5.tar.gz px4-firmware-8a615a9741ad1dd7406a804349329345573da5b5.tar.bz2 px4-firmware-8a615a9741ad1dd7406a804349329345573da5b5.zip |
WIP on ardrone control interface
Diffstat (limited to 'apps/ardrone_interface')
-rw-r--r-- | apps/ardrone_interface/ardrone_interface.c | 37 | ||||
-rw-r--r-- | apps/ardrone_interface/ardrone_motor_control.c | 123 | ||||
-rw-r--r-- | apps/ardrone_interface/ardrone_motor_control.h | 13 |
3 files changed, 108 insertions, 65 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 5b7678a3c..c3293d66e 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -112,7 +112,6 @@ int ardrone_interface_main(int argc, char *argv[]) thread_should_exit = false; ardrone_interface_task = task_create("ardrone_interface", SCHED_PRIORITY_MAX - 15, 4096, ardrone_interface_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; exit(0); } @@ -142,7 +141,7 @@ static int ardrone_open_uart(struct termios *uart_config_original) const char* uart_name = "/dev/ttyS1"; /* open uart */ - printf("[ardrone_interface] UART is /dev/ttyS1, baud rate is 115200"); + printf("[ardrone_interface] UART is /dev/ttyS1, baud rate is 115200\n"); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -181,9 +180,11 @@ static int ardrone_open_uart(struct termios *uart_config_original) int ardrone_interface_thread_main(int argc, char *argv[]) { + thread_running = true; + /* welcome user */ printf("[ardrone_interface] Control started, taking over motors\n"); - + /* File descriptors */ int gpios; @@ -192,7 +193,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) bool motor_test_mode = false; /* read commandline arguments */ - for (int i = 1; i < argc; i++) { + for (int i = 0; i < argc && argv[i]; i++) { if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) { motor_test_mode = true; } @@ -200,17 +201,27 @@ int ardrone_interface_thread_main(int argc, char *argv[]) struct termios uart_config_original; + if (motor_test_mode) { + printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n"); + } + + /* initialize multiplexing, deactivate all outputs */ + gpios = ar_multiplexing_init(); + + /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ ardrone_write = ardrone_open_uart(&uart_config_original); if (ardrone_write < 0) { fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); + thread_running = false; exit(ERROR); } /* initialize motors */ - if (OK != ar_init_motors(ardrone_write, &gpios)) { + if (OK != ar_init_motors(ardrone_write, gpios)) { close(ardrone_write); fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + thread_running = false; exit(ERROR); } @@ -231,18 +242,14 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + printf("[ardrone_interface] Motors initialized - ready.\n"); + fflush(stdout); + while (!thread_should_exit) { if (motor_test_mode) { /* set motors to idle speed */ - ardrone_write_motor_commands(ardrone_write, 10, 0, 0, 0); - sleep(2); - ardrone_write_motor_commands(ardrone_write, 0, 10, 0, 0); - sleep(2); - ardrone_write_motor_commands(ardrone_write, 0, 0, 10, 0); - sleep(2); - ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 10); - sleep(2); + ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10); } else { /* MAIN OPERATION MODE */ @@ -290,8 +297,8 @@ int ardrone_interface_thread_main(int argc, char *argv[]) if (led_counter == 12) led_counter = 0; } - /* run at approximately 50 Hz */ - usleep(20000); + /* run at approximately 100 Hz */ + usleep(1000); counter++; } diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index e96d343fc..d40f0066a 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -47,8 +47,8 @@ #include "ardrone_motor_control.h" -static const unsigned long motor_gpios = GPIO_EXT_1 | GPIO_EXT_2 | GPIO_MULTI_1 | GPIO_MULTI_2; -static const unsigned long motor_gpio[4] = { GPIO_EXT_1, GPIO_EXT_2, GPIO_MULTI_1, GPIO_MULTI_2 }; +static unsigned long motor_gpios = GPIO_EXT_1 | GPIO_EXT_2 | GPIO_MULTI_1 | GPIO_MULTI_2; +static unsigned long motor_gpio[4] = { GPIO_EXT_1, GPIO_EXT_2, GPIO_MULTI_1, GPIO_MULTI_2 }; typedef union { uint16_t motor_value; @@ -156,7 +156,6 @@ int ar_multiplexing_deinit(int fd) int ar_select_motor(int fd, uint8_t motor) { int ret = 0; - unsigned long gpioset; /* * Four GPIOS: * GPIO_EXT1 @@ -171,25 +170,40 @@ int ar_select_motor(int fd, uint8_t motor) ret += ioctl(fd, GPIO_CLEAR, motor_gpios); } else { - /* deselect all */ - ret += ioctl(fd, GPIO_SET, motor_gpios); - /* select reqested motor */ ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]); - - /* deselect all others */ - // gpioset = motor_gpios ^ motor_gpio[motor - 1]; - // ret += ioctl(fd, GPIO_SET, gpioset); } + fsync(fd); + return ret; } -int ar_init_motors(int ardrone_uart, int *gpios_pin) +int ar_deselect_motor(int fd, uint8_t motor) { - /* Initialize multiplexing */ - *gpios_pin = ar_multiplexing_init(); + int ret = 0; + /* + * Four GPIOS: + * GPIO_EXT1 + * GPIO_EXT2 + * GPIO_UART2_CTS + * GPIO_UART2_RTS + */ + + if (motor == 0) { + /* deselect motor 1-4 */ + ret += ioctl(fd, GPIO_SET, motor_gpios); + + } else { + /* deselect reqested motor */ + ret = ioctl(fd, GPIO_SET, motor_gpio[motor - 1]); + } + return ret; +} + +int ar_init_motors(int ardrone_uart, int gpios) +{ /* Write ARDrone commands on UART2 */ uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40}; uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0}; @@ -201,64 +215,74 @@ int ar_init_motors(int ardrone_uart, int *gpios_pin) int i; int errcounter = 0; + gpios = ar_multiplexing_init(); + for (i = 1; i < 5; ++i) { /* Initialize motors 1-4 */ - initbuf[3] = i; - errcounter += ar_select_motor(*gpios_pin, i); + initbuf[3] = (uint8_t)i; + errcounter += ar_select_motor(gpios, i); - write(ardrone_uart, initbuf + 0, 1); + write(ardrone_uart, &(initbuf[0]), 1); - /* sleep 400 ms */ - usleep(200000); - usleep(200000); + /* sleep 5 ms */ + usleep(5000); - write(ardrone_uart, initbuf + 1, 1); + write(ardrone_uart, &(initbuf[1]), 1); /* wait 50 ms */ - usleep(50000); + usleep(5000); - write(ardrone_uart, initbuf + 2, 1); + write(ardrone_uart, &(initbuf[2]), 1); /* wait 50 ms */ usleep(50000); - write(ardrone_uart, initbuf + 3, 1); + write(ardrone_uart, &(initbuf[3]), 1); /* wait 50 ms */ usleep(50000); - write(ardrone_uart, initbuf + 4, 1); + write(ardrone_uart, &(initbuf[4]), 1); /* wait 50 ms */ usleep(50000); - /* enable multicast */ - write(ardrone_uart, multicastbuf + 0, 1); - /* wait 1 ms */ - usleep(1000); - - write(ardrone_uart, multicastbuf + 1, 1); - /* wait 1 ms */ - usleep(1000); - - write(ardrone_uart, multicastbuf + 2, 1); - /* wait 1 ms */ - usleep(1000); + /* wait 5 ms */ + usleep(50000); - write(ardrone_uart, multicastbuf + 3, 1); - /* wait 1 ms */ - usleep(1000); + /* enforce immediate write */ + fsync(ardrone_uart); - write(ardrone_uart, multicastbuf + 4, 1); - /* wait 1 ms */ - usleep(1000); + ar_deselect_motor(gpios, i); - write(ardrone_uart, multicastbuf + 5, 1); - /* wait 5 ms */ - usleep(50000); + usleep(500000); + /* sleep 500 ms */ } /* start the multicast part */ - errcounter += ar_select_motor(*gpios_pin, 0); + errcounter += ar_select_motor(gpios, 0); + + /* enable multicast */ + write(ardrone_uart, &(multicastbuf[0]), 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, &(multicastbuf[1]), 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, &(multicastbuf[2]), 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, &(multicastbuf[3]), 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, &(multicastbuf[4]), 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, &(multicastbuf[5]), 1); if (errcounter != 0) { - fprintf(stderr, "[ar motors] init sequence incomplete, failed %d times", -errcounter); + fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter); fflush(stdout); } return errcounter; @@ -285,13 +309,14 @@ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t } int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) { - const unsigned int min_motor_interval = 20000; + const unsigned int min_motor_interval = 4900; static uint64_t last_motor_time = 0; if (hrt_absolute_time() - last_motor_time > min_motor_interval) { uint8_t buf[5] = {0}; ar_get_motor_packet(buf, motor1, motor2, motor3, motor4); int ret; - if ((ret = write(ardrone_fd, buf, sizeof(buf))) > 0) { + ret = write(ardrone_fd, buf, sizeof(buf)); + if (ret == sizeof(buf)) { return OK; } else { return ret; diff --git a/apps/ardrone_interface/ardrone_motor_control.h b/apps/ardrone_interface/ardrone_motor_control.h index 8af143742..664419707 100644 --- a/apps/ardrone_interface/ardrone_motor_control.h +++ b/apps/ardrone_interface/ardrone_motor_control.h @@ -49,9 +49,20 @@ void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, u /** * Select a motor in the multiplexing. + * + * @param fd GPIO file descriptor + * @param motor Motor number, from 1 to 4, 0 selects all */ int ar_select_motor(int fd, uint8_t motor); +/** + * Deselect a motor in the multiplexing. + * + * @param fd GPIO file descriptor + * @param motor Motor number, from 1 to 4, 0 deselects all + */ +int ar_deselect_motor(int fd, uint8_t motor); + void ar_enable_broadcast(int fd); int ar_multiplexing_init(void); @@ -69,7 +80,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor /** * Initialize the motors. */ -int ar_init_motors(int ardrone_uart, int *gpios_pin); +int ar_init_motors(int ardrone_uart, int gpio); /** * Set LED pattern. |