diff options
Diffstat (limited to 'apps/ardrone_interface')
-rw-r--r-- | apps/ardrone_interface/ardrone_interface.c | 52 | ||||
-rw-r--r-- | apps/ardrone_interface/ardrone_motor_control.c | 92 |
2 files changed, 94 insertions, 50 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 51feac539..a865992ed 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -217,14 +217,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) exit(ERROR); } - /* initialize motors */ - 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); - } - /* Led animation */ int counter = 0; int led_counter = 0; @@ -245,6 +237,44 @@ int ardrone_interface_thread_main(int argc, char *argv[]) printf("[ardrone_interface] Motors initialized - ready.\n"); fflush(stdout); + /* initialize motors */ + 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); + } + + ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); + + + // XXX Re-done initialization to make sure it is accepted by the motors + // XXX should be removed after more testing, but no harm + + /* close uarts */ + close(ardrone_write); + ar_multiplexing_deinit(gpios); + + /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ + ardrone_write = ardrone_open_uart(&uart_config_original); + + /* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */ + gpios = ar_multiplexing_init(); + + 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)) { + close(ardrone_write); + fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + thread_running = false; + exit(ERROR); + } + while (!thread_should_exit) { if (motor_test_mode) { @@ -267,7 +297,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } } - if (counter % 22 == 0) { + if (counter % 16 == 0) { if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); @@ -297,8 +327,8 @@ int ardrone_interface_thread_main(int argc, char *argv[]) if (led_counter == 12) led_counter = 0; } - /* run at approximately 100 Hz */ - usleep(1000); + /* run at approximately 200 Hz */ + usleep(5000); counter++; } diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index 1e4b48c07..380ae9c15 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -55,6 +55,8 @@ typedef union { uint8_t bytes[2]; } motor_union_t; +#define UART_TRANSFER_TIME_BYTE_US (9+50) /**< 9 us per byte at 115200k plus overhead */ + /** * @brief Generate the 8-byte motor set packet * @@ -217,69 +219,80 @@ int ar_init_motors(int ardrone_uart, int gpios) int i; int errcounter = 0; + + /* initial setup run */ for (i = 1; i < 5; ++i) { /* Initialize motors 1-4 */ - initbuf[3] = (uint8_t)i; errcounter += ar_select_motor(gpios, i); - write(ardrone_uart, &(initbuf[0]), 1); + write(ardrone_uart, &(initbuf[3]), 1); + fsync(ardrone_uart); - /* sleep 5 ms */ - usleep(5000); + /* + * write 0xE0 - request status + * receive one status byte + */ + write(ardrone_uart, &(initbuf[0]), 1); + fsync(ardrone_uart); + usleep(UART_TRANSFER_TIME_BYTE_US*1); + /* + * write 0x91 - request checksum + * receive 120 status bytes + */ write(ardrone_uart, &(initbuf[1]), 1); - /* wait 50 ms */ - usleep(5000); + fsync(ardrone_uart); + usleep(UART_TRANSFER_TIME_BYTE_US*120); + /* + * write 0xA1 - set status OK + * receive one status byte - should be A0 + * to confirm status is OK + */ write(ardrone_uart, &(initbuf[2]), 1); - /* wait 50 ms */ - usleep(50000); + fsync(ardrone_uart); + usleep(UART_TRANSFER_TIME_BYTE_US*1); + /* + * set as motor i, where i = 1..4 + * receive nothing + */ + initbuf[3] = (uint8_t)i; write(ardrone_uart, &(initbuf[3]), 1); - /* wait 50 ms */ - usleep(50000); + fsync(ardrone_uart); + /* + * write 0x40 - check version + * receive 11 bytes encoding the version + */ write(ardrone_uart, &(initbuf[4]), 1); - /* wait 50 ms */ - usleep(50000); - - /* wait 5 ms */ - usleep(50000); - - /* enforce immediate write */ fsync(ardrone_uart); + usleep(UART_TRANSFER_TIME_BYTE_US*11); ar_deselect_motor(gpios, i); - - usleep(500000); - /* sleep 500 ms */ + /* sleep 400 ms */ + usleep(400000); } /* start the multicast part */ 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 six times A0 - enable broadcast + * receive nothing + */ + // for (int i = 0; i < sizeof(multicastbuf); i++) { + write(ardrone_uart, multicastbuf, 6); + fsync(ardrone_uart); - write(ardrone_uart, &(multicastbuf[3]), 1); - /* wait 1 ms */ - usleep(1000); + usleep(200000); - write(ardrone_uart, &(multicastbuf[4]), 1); - /* wait 1 ms */ - usleep(1000); + write(ardrone_uart, multicastbuf, 6); + fsync(ardrone_uart); - write(ardrone_uart, &(multicastbuf[5]), 1); + /* set motors to zero speed */ + ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0); + fsync(ardrone_uart); if (errcounter != 0) { fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter); @@ -316,6 +329,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor ar_get_motor_packet(buf, motor1, motor2, motor3, motor4); int ret; ret = write(ardrone_fd, buf, sizeof(buf)); + fsync(ardrone_fd); if (ret == sizeof(buf)) { return OK; } else { |