WIP on ardrone control interface

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,6 +180,8 @@ 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");
 
@@ -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

@@ -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);
 
+		/* 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 */
-		write(ardrone_uart, multicastbuf + 0, 1);
+	write(ardrone_uart, &(multicastbuf[0]), 1);
 	/* wait 1 ms */
 	usleep(1000);
 
-		write(ardrone_uart, multicastbuf + 1, 1);
+	write(ardrone_uart, &(multicastbuf[1]), 1);
 	/* wait 1 ms */
 	usleep(1000);
 
-		write(ardrone_uart, multicastbuf + 2, 1);
+	write(ardrone_uart, &(multicastbuf[2]), 1);
 	/* wait 1 ms */
 	usleep(1000);
 
-		write(ardrone_uart, multicastbuf + 3, 1);
+	write(ardrone_uart, &(multicastbuf[3]), 1);
 	/* wait 1 ms */
 	usleep(1000);
 
-		write(ardrone_uart, multicastbuf + 4, 1);
+	write(ardrone_uart, &(multicastbuf[4]), 1);
 	/* wait 1 ms */
 	usleep(1000);
 
-		write(ardrone_uart, multicastbuf + 5, 1);
-		/* wait 5 ms */
-		usleep(50000);
-	}
-
-	/* start the multicast part */
-	errcounter += ar_select_motor(*gpios_pin, 0);
+	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

@@ -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.