diff options
| -rw-r--r-- | apps/ardrone_interface/ardrone_interface.c | 26 | ||||
| -rw-r--r-- | apps/ardrone_interface/ardrone_motor_control.c | 5 |
2 files changed, 15 insertions, 16 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 220c3d3b5..c4cceaf90 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -205,18 +205,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n"); } - /* 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); - } - /* Led animation */ int counter = 0; int led_counter = 0; @@ -237,6 +225,18 @@ int ardrone_interface_thread_main(int argc, char *argv[]) printf("[ardrone_interface] Motors initialized - ready.\n"); fflush(stdout); + /* 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); @@ -253,7 +253,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* close uarts */ close(ardrone_write); - ar_multiplexing_deinit(gpios); + //ar_multiplexing_deinit(gpios); /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ ardrone_write = ardrone_open_uart(&uart_config_original); diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index 2644a25cb..dd7a5655d 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -226,9 +226,7 @@ int ar_init_motors(int ardrone_uart, int gpios) for (i = 1; i < 5; ++i) { /* Initialize motors 1-4 */ errcounter += ar_select_motor(gpios, i); - - write(ardrone_uart, &(initbuf[3]), 1); - fsync(ardrone_uart); + usleep(200); /* * write 0xE0 - request status @@ -278,6 +276,7 @@ int ar_init_motors(int ardrone_uart, int gpios) /* start the multicast part */ errcounter += ar_select_motor(gpios, 0); + usleep(200); /* * first round |