diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-04 21:16:39 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-04 21:16:39 +0200 |
commit | e503c153617710452ec69c89c0d04b19cc710e7b (patch) | |
tree | d39448c73f22ab47c05acbc6271c3c4f18d18fb6 /apps/ardrone_interface/ardrone_interface.c | |
parent | 62682d805ec234ecb4f5396ca2c7a072ce3f753c (diff) | |
download | px4-firmware-e503c153617710452ec69c89c0d04b19cc710e7b.tar.gz px4-firmware-e503c153617710452ec69c89c0d04b19cc710e7b.tar.bz2 px4-firmware-e503c153617710452ec69c89c0d04b19cc710e7b.zip |
Checkpoint - this is worth an AR.Drone flight test. Fixed thrust scaling in sensors for manual input, kind of fixed AR.Drone motor interface, very reliable now
Diffstat (limited to 'apps/ardrone_interface/ardrone_interface.c')
-rw-r--r-- | apps/ardrone_interface/ardrone_interface.c | 52 |
1 files changed, 41 insertions, 11 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++; } |