fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 09:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-04 21:16:39 +02:00
parent 62682d805e
commit e503c15361
5 changed files with 115 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ardrone_interface/ardrone_interface.c
+41 -11
View File
@@ -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++;
}