Fix up AR.drone motor GPIO config and initialisation

2 files changed, 7 insertions, 7 deletions

diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index c3293d66e..51feac539 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -205,12 +205,12 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 		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);
 
+	/* 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;
diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c
index d40f0066a..1e4b48c07 100644
--- a/apps/ardrone_interface/ardrone_motor_control.c
+++ b/apps/ardrone_interface/ardrone_motor_control.c
@@ -118,6 +118,7 @@ int ar_multiplexing_init()
 		return -1;
 	}
 
+	/* configure all motor select GPIOs as outputs */
 	if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) {
 		warn("GPIO: output set fail");
 		close(fd);
@@ -174,8 +175,6 @@ int ar_select_motor(int fd, uint8_t motor)
 		ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]);
 	}
 
-	fsync(fd);
-
 	return ret;
 }
 
@@ -208,6 +207,9 @@ int ar_init_motors(int ardrone_uart, int gpios)
 	uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40};
 	uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0};
 
+	/* deselect all motors */
+	ar_deselect_motor(gpios, 0);
+
 	/* initialize all motors
 	 * - select one motor at a time
 	 * - configure motor
@@ -215,8 +217,6 @@ int ar_init_motors(int ardrone_uart, int gpios)
 	int i;
 	int errcounter = 0;
 
-	gpios = ar_multiplexing_init();
-
 	for (i = 1; i < 5; ++i) {
 		/* Initialize motors 1-4 */
 		initbuf[3] = (uint8_t)i;