Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 09:13:32 +08:00
Code Issues Actions 32 Packages Projects Releases Wiki Activity

WIP on ardrone control interface

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-03 21:34:54 +02:00
parent 79801b1578
commit 8a615a9741
3 changed files with 111 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ardrone_interface/ardrone_interface.c
+22 -15
View File
@@ -112,7 +112,6 @@ int ardrone_interface_main(int argc, char *argv[])
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);
thread_running = true;
exit(0);
}
@@ -142,7 +141,7 @@ static int ardrone_open_uart(struct termios *uart_config_original)
const char* uart_name = "/dev/ttyS1";
/* 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);
/* Try to set baud rate */
@@ -181,9 +180,11 @@ static int ardrone_open_uart(struct termios *uart_config_original)
int ardrone_interface_thread_main(int argc, char *argv[])
{
thread_running = true;
/* welcome user */
printf("[ardrone_interface] Control started, taking over motors\n");
/* File descriptors */
int gpios;
@@ -192,7 +193,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
bool motor_test_mode = false;
/* 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) {
motor_test_mode = true;
}
@@ -200,17 +201,27 @@ int ardrone_interface_thread_main(int argc, char *argv[])
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);
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)) {
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);
}
@@ -231,18 +242,14 @@ int ardrone_interface_thread_main(int argc, char *argv[])
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
printf("[ardrone_interface] Motors initialized - ready.\n");
fflush(stdout);
while (!thread_should_exit) {
if (motor_test_mode) {
/* set motors to idle speed */
ardrone_write_motor_commands(ardrone_write, 10, 0, 0, 0);
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);
ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
} else {
/* MAIN OPERATION MODE */
@@ -290,8 +297,8 @@ int ardrone_interface_thread_main(int argc, char *argv[])
if (led_counter == 12) led_counter = 0;
}
/* run at approximately 50 Hz */
usleep(20000);
/* run at approximately 100 Hz */
usleep(1000);
counter++;
}
apps/ardrone_interface/ardrone_motor_control.c
+77 -52
View File
@@ -47,8 +47,8 @@
#include "ardrone_motor_control.h"
static const 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_gpios = 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 {
uint16_t motor_value;
@@ -156,7 +156,6 @@ int ar_multiplexing_deinit(int fd)
int ar_select_motor(int fd, uint8_t motor)
{
int ret = 0;
unsigned long gpioset;
/*
* Four GPIOS:
* GPIO_EXT1
@@ -171,25 +170,40 @@ int ar_select_motor(int fd, uint8_t motor)
ret += ioctl(fd, GPIO_CLEAR, motor_gpios);
} else {
/* deselect all */
ret += ioctl(fd, GPIO_SET, motor_gpios);
/* select reqested motor */
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;
}
int ar_deselect_motor(int fd, uint8_t motor)
{
int ret = 0;
/*
* 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_pin)
int ar_init_motors(int ardrone_uart, int gpios)
{
/* Initialize multiplexing */
*gpios_pin = ar_multiplexing_init();
/* Write ARDrone commands on UART2 */
uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40};
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 errcounter = 0;
gpios = ar_multiplexing_init();
for (i = 1; i < 5; ++i) {
/* Initialize motors 1-4 */
initbuf[3] = i;
errcounter += ar_select_motor(*gpios_pin, i);
initbuf[3] = (uint8_t)i;
errcounter += ar_select_motor(gpios, i);
write(ardrone_uart, initbuf + 0, 1);
write(ardrone_uart, &(initbuf[0]), 1);
/* sleep 400 ms */
usleep(200000);
usleep(200000);
/* sleep 5 ms */
usleep(5000);
write(ardrone_uart, initbuf + 1, 1);
write(ardrone_uart, &(initbuf[1]), 1);
/* wait 50 ms */
usleep(5000);
write(ardrone_uart, &(initbuf[2]), 1);
/* wait 50 ms */
usleep(50000);
write(ardrone_uart, initbuf + 2, 1);
write(ardrone_uart, &(initbuf[3]), 1);
/* wait 50 ms */
usleep(50000);
write(ardrone_uart, initbuf + 3, 1);
write(ardrone_uart, &(initbuf[4]), 1);
/* wait 50 ms */
usleep(50000);
write(ardrone_uart, initbuf + 4, 1);
/* wait 50 ms */
usleep(50000);
/* 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(ardrone_uart, multicastbuf + 3, 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, multicastbuf + 4, 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, multicastbuf + 5, 1);
/* 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_pin, 0);
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(ardrone_uart, &(multicastbuf[3]), 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, &(multicastbuf[4]), 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, &(multicastbuf[5]), 1);
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);
}
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) {
const unsigned int min_motor_interval = 20000;
const unsigned int min_motor_interval = 4900;
static uint64_t last_motor_time = 0;
if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
uint8_t buf[5] = {0};
ar_get_motor_packet(buf, motor1, motor2, motor3, motor4);
int ret;
if ((ret = write(ardrone_fd, buf, sizeof(buf))) > 0) {
ret = write(ardrone_fd, buf, sizeof(buf));
if (ret == sizeof(buf)) {
return OK;
} else {
return ret;
apps/ardrone_interface/ardrone_motor_control.h
+12 -1
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.
*
* @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);
/**
* 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);
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.
*/
int ar_init_motors(int ardrone_uart, int *gpios_pin);
int ar_init_motors(int ardrone_uart, int gpio);
/**
* Set LED pattern.