zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

WIP on ardrone control interface

sbg
Lorenz Meier 13 years ago
parent
79801b1578
commit
8a615a9741
3 changed files with 108 additions and 65 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 35
      apps/ardrone_interface/ardrone_interface.c
  2. 103
      apps/ardrone_interface/ardrone_motor_control.c
  3. 13
      apps/ardrone_interface/ardrone_motor_control.h

35
apps/ardrone_interface/ardrone_interface.c
Unescape View File

@ -112,7 +112,6 @@ int ardrone_interface_main(int argc, char *argv[])
thread_should_exit = false; 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); 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); exit(0);
} }
@ -142,7 +141,7 @@ static int ardrone_open_uart(struct termios *uart_config_original)
const char* uart_name = "/dev/ttyS1"; const char* uart_name = "/dev/ttyS1";
/* open uart */ /* 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); uart = open(uart_name, O_RDWR | O_NOCTTY);
/* Try to set baud rate */ /* Try to set baud rate */
@ -181,6 +180,8 @@ static int ardrone_open_uart(struct termios *uart_config_original)
int ardrone_interface_thread_main(int argc, char *argv[]) int ardrone_interface_thread_main(int argc, char *argv[])
{ {
thread_running = true;
/* welcome user */ /* welcome user */
printf("[ardrone_interface] Control started, taking over motors\n"); printf("[ardrone_interface] Control started, taking over motors\n");
@ -192,7 +193,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
bool motor_test_mode = false; bool motor_test_mode = false;
/* read commandline arguments */ /* 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) { if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
motor_test_mode = true; motor_test_mode = true;
} }
@ -200,17 +201,27 @@ int ardrone_interface_thread_main(int argc, char *argv[])
struct termios uart_config_original; 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); ardrone_write = ardrone_open_uart(&uart_config_original);
if (ardrone_write < 0) { if (ardrone_write < 0) {
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
thread_running = false;
exit(ERROR); exit(ERROR);
} }
/* initialize motors */ /* initialize motors */
if (OK != ar_init_motors(ardrone_write, &gpios)) { if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write); close(ardrone_write);
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
thread_running = false;
exit(ERROR); 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 state_sub = orb_subscribe(ORB_ID(vehicle_status));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
printf("[ardrone_interface] Motors initialized - ready.\n");
fflush(stdout);
while (!thread_should_exit) { while (!thread_should_exit) {
if (motor_test_mode) { if (motor_test_mode) {
/* set motors to idle speed */ /* set motors to idle speed */
ardrone_write_motor_commands(ardrone_write, 10, 0, 0, 0); ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
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);
} else { } else {
/* MAIN OPERATION MODE */ /* MAIN OPERATION MODE */
@ -290,8 +297,8 @@ int ardrone_interface_thread_main(int argc, char *argv[])
if (led_counter == 12) led_counter = 0; if (led_counter == 12) led_counter = 0;
} }
/* run at approximately 50 Hz */ /* run at approximately 100 Hz */
usleep(20000); usleep(1000);
counter++; counter++;
} }

103
apps/ardrone_interface/ardrone_motor_control.c
Unescape View File

@ -47,8 +47,8 @@
#include "ardrone_motor_control.h" #include "ardrone_motor_control.h"
static const unsigned long motor_gpios = 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 const unsigned long motor_gpio[4] = { 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 { typedef union {
uint16_t motor_value; uint16_t motor_value;
@ -156,7 +156,6 @@ int ar_multiplexing_deinit(int fd)
int ar_select_motor(int fd, uint8_t motor) int ar_select_motor(int fd, uint8_t motor)
{ {
int ret = 0; int ret = 0;
unsigned long gpioset;
/* /*
* Four GPIOS: * Four GPIOS:
* GPIO_EXT1 * GPIO_EXT1
@ -171,25 +170,40 @@ int ar_select_motor(int fd, uint8_t motor)
ret += ioctl(fd, GPIO_CLEAR, motor_gpios); ret += ioctl(fd, GPIO_CLEAR, motor_gpios);
} else { } else {
/* deselect all */
ret += ioctl(fd, GPIO_SET, motor_gpios);
/* select reqested motor */ /* select reqested motor */
ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]); 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; return ret;
} }
int ar_init_motors(int ardrone_uart, int *gpios_pin) int ar_deselect_motor(int fd, uint8_t motor)
{ {
/* Initialize multiplexing */ int ret = 0;
*gpios_pin = ar_multiplexing_init(); /*
* 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 */ /* Write ARDrone commands on UART2 */
uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40}; uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40};
uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0}; 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 i;
int errcounter = 0; int errcounter = 0;
gpios = ar_multiplexing_init();
for (i = 1; i < 5; ++i) { for (i = 1; i < 5; ++i) {
/* Initialize motors 1-4 */ /* Initialize motors 1-4 */
initbuf[3] = i; initbuf[3] = (uint8_t)i;
errcounter += ar_select_motor(*gpios_pin, i); errcounter += ar_select_motor(gpios, i);
write(ardrone_uart, initbuf + 0, 1); write(ardrone_uart, &(initbuf[0]), 1);
/* sleep 400 ms */ /* sleep 5 ms */
usleep(200000); usleep(5000);
usleep(200000);
write(ardrone_uart, initbuf + 1, 1); write(ardrone_uart, &(initbuf[1]), 1);
/* wait 50 ms */ /* wait 50 ms */
usleep(50000); usleep(5000);
write(ardrone_uart, initbuf + 2, 1); write(ardrone_uart, &(initbuf[2]), 1);
/* wait 50 ms */ /* wait 50 ms */
usleep(50000); usleep(50000);
write(ardrone_uart, initbuf + 3, 1); write(ardrone_uart, &(initbuf[3]), 1);
/* wait 50 ms */ /* wait 50 ms */
usleep(50000); usleep(50000);
write(ardrone_uart, initbuf + 4, 1); write(ardrone_uart, &(initbuf[4]), 1);
/* wait 50 ms */ /* wait 50 ms */
usleep(50000); usleep(50000);
/* wait 5 ms */
usleep(50000);
/* enforce immediate write */
fsync(ardrone_uart);
ar_deselect_motor(gpios, i);
usleep(500000);
/* sleep 500 ms */
}
/* start the multicast part */
errcounter += ar_select_motor(gpios, 0);
/* enable multicast */ /* enable multicast */
write(ardrone_uart, multicastbuf + 0, 1); write(ardrone_uart, &(multicastbuf[0]), 1);
/* wait 1 ms */ /* wait 1 ms */
usleep(1000); usleep(1000);
write(ardrone_uart, multicastbuf + 1, 1); write(ardrone_uart, &(multicastbuf[1]), 1);
/* wait 1 ms */ /* wait 1 ms */
usleep(1000); usleep(1000);
write(ardrone_uart, multicastbuf + 2, 1); write(ardrone_uart, &(multicastbuf[2]), 1);
/* wait 1 ms */ /* wait 1 ms */
usleep(1000); usleep(1000);
write(ardrone_uart, multicastbuf + 3, 1); write(ardrone_uart, &(multicastbuf[3]), 1);
/* wait 1 ms */ /* wait 1 ms */
usleep(1000); usleep(1000);
write(ardrone_uart, multicastbuf + 4, 1); write(ardrone_uart, &(multicastbuf[4]), 1);
/* wait 1 ms */ /* wait 1 ms */
usleep(1000); usleep(1000);
write(ardrone_uart, multicastbuf + 5, 1); write(ardrone_uart, &(multicastbuf[5]), 1);
/* wait 5 ms */
usleep(50000);
}
/* start the multicast part */
errcounter += ar_select_motor(*gpios_pin, 0);
if (errcounter != 0) { 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); fflush(stdout);
} }
return errcounter; 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) { 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; static uint64_t last_motor_time = 0;
if (hrt_absolute_time() - last_motor_time > min_motor_interval) { if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
uint8_t buf[5] = {0}; uint8_t buf[5] = {0};
ar_get_motor_packet(buf, motor1, motor2, motor3, motor4); ar_get_motor_packet(buf, motor1, motor2, motor3, motor4);
int ret; int ret;
if ((ret = write(ardrone_fd, buf, sizeof(buf))) > 0) { ret = write(ardrone_fd, buf, sizeof(buf));
if (ret == sizeof(buf)) {
return OK; return OK;
} else { } else {
return ret; return ret;

13
apps/ardrone_interface/ardrone_motor_control.h
Unescape View File

@ -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. * 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); 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); void ar_enable_broadcast(int fd);
int ar_multiplexing_init(void); 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. * 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. * Set LED pattern.

Write Preview
Loading…
Cancel
Save