aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--apps/ardrone_interface/ardrone_interface.c37
-rw-r--r--apps/ardrone_interface/ardrone_motor_control.c123
-rw-r--r--apps/ardrone_interface/ardrone_motor_control.h13
3 files changed, 108 insertions, 65 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index 5b7678a3c..c3293d66e 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -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++;
}
diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c
index e96d343fc..d40f0066a 100644
--- a/apps/ardrone_interface/ardrone_motor_control.c
+++ b/apps/ardrone_interface/ardrone_motor_control.c
@@ -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_init_motors(int ardrone_uart, int *gpios_pin)
+int ar_deselect_motor(int fd, uint8_t motor)
{
- /* Initialize multiplexing */
- *gpios_pin = ar_multiplexing_init();
+ 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)
+{
/* 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(50000);
+ usleep(5000);
- write(ardrone_uart, initbuf + 2, 1);
+ write(ardrone_uart, &(initbuf[2]), 1);
/* wait 50 ms */
usleep(50000);
- write(ardrone_uart, initbuf + 3, 1);
+ write(ardrone_uart, &(initbuf[3]), 1);
/* wait 50 ms */
usleep(50000);
- write(ardrone_uart, initbuf + 4, 1);
+ 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);
+ /* wait 5 ms */
+ usleep(50000);
- write(ardrone_uart, multicastbuf + 3, 1);
- /* wait 1 ms */
- usleep(1000);
+ /* enforce immediate write */
+ fsync(ardrone_uart);
- write(ardrone_uart, multicastbuf + 4, 1);
- /* wait 1 ms */
- usleep(1000);
+ ar_deselect_motor(gpios, i);
- write(ardrone_uart, multicastbuf + 5, 1);
- /* wait 5 ms */
- usleep(50000);
+ 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;
diff --git a/apps/ardrone_interface/ardrone_motor_control.h b/apps/ardrone_interface/ardrone_motor_control.h
index 8af143742..664419707 100644
--- a/apps/ardrone_interface/ardrone_motor_control.h
+++ b/apps/ardrone_interface/ardrone_motor_control.h
@@ -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.
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-09 17:07:14 +0000