Checkpoint - this is worth an AR.Drone flight test. Fixed thrust scaling in sensors for manual input, kind of fixed AR.Drone motor interface, very reliable now

5 files changed, 107 insertions, 63 deletions

diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index 51feac539..a865992ed 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -217,14 +217,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 		exit(ERROR);
 	}
 
-	/* initialize motors */
-	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);
-	}
-
 	/* Led animation */
 	int counter = 0;
 	int led_counter = 0;
@@ -245,6 +237,44 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	printf("[ardrone_interface] Motors initialized - ready.\n");
 	fflush(stdout);
 
+	/* initialize motors */
+	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);
+	}
+
+	ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
+
+
+	// XXX Re-done initialization to make sure it is accepted by the motors
+	// XXX should be removed after more testing, but no harm
+
+	/* close uarts */
+	close(ardrone_write);
+	ar_multiplexing_deinit(gpios);
+
+	/* 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;
+		exit(ERROR);
+	}
+
+	/* initialize motors */
+	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);
+	}
+
 	while (!thread_should_exit) {
 
 		if (motor_test_mode) {
@@ -267,7 +297,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 			}
 		}
 
-		if (counter % 22 == 0) {
+		if (counter % 16 == 0) {
 			if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
 
 			if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
@@ -297,8 +327,8 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 			if (led_counter == 12) led_counter = 0;
 		}
 
-		/* run at approximately 100 Hz */
-		usleep(1000);
+		/* run at approximately 200 Hz */
+		usleep(5000);
 
 		counter++;
 	}
diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c
index 1e4b48c07..380ae9c15 100644
--- a/apps/ardrone_interface/ardrone_motor_control.c
+++ b/apps/ardrone_interface/ardrone_motor_control.c
@@ -55,6 +55,8 @@ typedef union {
 	uint8_t bytes[2];
 } motor_union_t;
 
+#define UART_TRANSFER_TIME_BYTE_US (9+50) /**< 9 us per byte at 115200k plus overhead */
+
 /**
  * @brief Generate the 8-byte motor set packet
  *
@@ -217,69 +219,80 @@ int ar_init_motors(int ardrone_uart, int gpios)
 	int i;
 	int errcounter = 0;
 
+
+	/* initial setup run */
 	for (i = 1; i < 5; ++i) {
 		/* Initialize motors 1-4 */
-		initbuf[3] = (uint8_t)i;
 		errcounter += ar_select_motor(gpios, i);
 
-		write(ardrone_uart, &(initbuf[0]), 1);
+		write(ardrone_uart, &(initbuf[3]), 1);
+		fsync(ardrone_uart);
 
-		/* sleep 5 ms */
-		usleep(5000);
+		/*
+		 * write 0xE0 - request status
+		 * receive one status byte
+		 */
+		write(ardrone_uart, &(initbuf[0]), 1);
+		fsync(ardrone_uart);
+		usleep(UART_TRANSFER_TIME_BYTE_US*1);
 
+		/*
+		 * write 0x91 - request checksum
+		 * receive 120 status bytes
+		 */
 		write(ardrone_uart, &(initbuf[1]), 1);
-		/* wait 50 ms */
-		usleep(5000);
+		fsync(ardrone_uart);
+		usleep(UART_TRANSFER_TIME_BYTE_US*120);
 
+		/*
+		 * write 0xA1 - set status OK
+		 * receive one status byte - should be A0
+		 * to confirm status is OK
+		 */
 		write(ardrone_uart, &(initbuf[2]), 1);
-		/* wait 50 ms */
-		usleep(50000);
+		fsync(ardrone_uart);
+		usleep(UART_TRANSFER_TIME_BYTE_US*1);
 
+		/*
+		 * set as motor i, where i = 1..4
+		 * receive nothing
+		 */
+		initbuf[3] = (uint8_t)i;
 		write(ardrone_uart, &(initbuf[3]), 1);
-		/* wait 50 ms */
-		usleep(50000);
+		fsync(ardrone_uart);
 
+		/*
+		 * write 0x40 - check version
+		 * receive 11 bytes encoding the version
+		 */
 		write(ardrone_uart, &(initbuf[4]), 1);
-		/* wait 50 ms */
-		usleep(50000);
-
-		/* wait 5 ms */
-		usleep(50000);
-
-		/* enforce immediate write */
 		fsync(ardrone_uart);
+		usleep(UART_TRANSFER_TIME_BYTE_US*11);
 
 		ar_deselect_motor(gpios, i);
-
-		usleep(500000);
-		/* sleep 500 ms */
+		/* sleep 400 ms */
+		usleep(400000);
 	}
 
 	/* start the multicast part */
 	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 six times A0 - enable broadcast
+	 * receive nothing
+	 */
+	// for (int i = 0; i < sizeof(multicastbuf); i++) {
+	write(ardrone_uart, multicastbuf, 6);
+	fsync(ardrone_uart);
 
-	write(ardrone_uart, &(multicastbuf[3]), 1);
-	/* wait 1 ms */
-	usleep(1000);
+	usleep(200000);
 
-	write(ardrone_uart, &(multicastbuf[4]), 1);
-	/* wait 1 ms */
-	usleep(1000);
+	write(ardrone_uart, multicastbuf, 6);
+	fsync(ardrone_uart);
 
-	write(ardrone_uart, &(multicastbuf[5]), 1);
+	/* set motors to zero speed */
+	ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
+	fsync(ardrone_uart);
 
 	if (errcounter != 0) {
 		fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
@@ -316,6 +329,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
 		ar_get_motor_packet(buf, motor1, motor2, motor3, motor4);
 		int ret;
 		ret = write(ardrone_fd, buf, sizeof(buf));
+		fsync(ardrone_fd);
 		if (ret == sizeof(buf)) {
 			return OK;
 		} else {
diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.c b/apps/px4/attitude_estimator_bm/attitude_bm.c
index 29908ddd5..8face5aea 100644
--- a/apps/px4/attitude_estimator_bm/attitude_bm.c
+++ b/apps/px4/attitude_estimator_bm/attitude_bm.c
@@ -147,18 +147,18 @@ void attitude_blackmagic_init(void)
 //	};
 
 	static m_elem kal_gain[12 * 9] = {
-		0.001f , 0    ,   0    ,   0    ,   0    ,   0    ,   0   ,    0    ,   0,
-		0   ,    0.001f , 0   ,    0   ,    0   ,    0   ,    0   ,    0   ,    0,
-		0   ,    0    ,   0.001f , 0   ,    0   ,    0   ,    0   ,    0   ,    0,
-		0   ,    0    ,   0   ,    0.015f, 	0   ,    0   ,    0   ,    0   ,    0,
-		0   ,    0   ,    0   ,    0    ,   0.015f, 	 0   ,    0   ,    0   ,    0,
-		0   ,    0    ,   0   ,    0    ,   0   ,    0.015f, 	  0   ,    0   ,    0,
+		0.0007f , 0    ,   0    ,   0    ,   0    ,   0    ,   0   ,    0    ,   0,
+		0   ,    0.0007f , 0   ,    0   ,    0   ,    0   ,    0   ,    0   ,    0,
+		0   ,    0    ,   0.0007f , 0   ,    0   ,    0   ,    0   ,    0   ,    0,
+		0   ,    0    ,   0   ,    0.01f, 	0   ,    0   ,    0   ,    0   ,    0,
+		0   ,    0   ,    0   ,    0    ,   0.01f, 	 0   ,    0   ,    0   ,    0,
+		0   ,    0    ,   0   ,    0    ,   0   ,    0.01f, 	  0   ,    0   ,    0,
 		0.0000f , +0.00002f, 0   ,    0 , 		0, 		 0,  	  0,  	   0    ,   0,
 		-0.00002f, 0    ,   0   ,    0 , 		0, 		 0,  	  0,  	   0, 	    0,
 		0,    	 0 ,	  0   ,    0,  	    0,		 0,  	  0,  	   0, 	    0,
-		0  ,     0    ,   0   ,    0   ,    0    ,   0   ,    0.6f ,   0   ,    0,
-		0   ,    0   ,    0   ,    0   ,    0    ,   0   ,    0    ,   0.6f ,   0,
-		0   ,    0   ,    0   ,    0   ,    0   ,    0   ,    0    ,   0    ,   0.6f
+		0  ,     0    ,   0   ,    0   ,    0    ,   0   ,    0.4f ,   0   ,    0,
+		0   ,    0   ,    0   ,    0   ,    0    ,   0   ,    0    ,   0.4f ,   0,
+		0   ,    0   ,    0   ,    0   ,    0   ,    0   ,    0    ,   0    ,   0.4f
 	};
 	//offset update only correct if not upside down.
 
diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
index 1174a7524..0f6b6044f 100644
--- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
+++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
@@ -87,9 +87,9 @@ int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *eul
 	gyro_values.z =  raw->gyro_rad_s[2];
 
 	float_vect3 accel_values;
-	accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 120;
-	accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 120;
-	accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 120;
+	accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 100;
+	accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 100;
+	accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 100;
 
 	float_vect3 mag_values;
 	mag_values.x = raw->magnetometer_ga[0]*1500.0f;
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 205f88093..79f9c19e7 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -914,7 +914,7 @@ Sensors::ppm_poll()
 	if (manual_control.yaw >  1.0f) manual_control.yaw =  1.0f;
 	
 	/* throttle input */
-	manual_control.throttle = (_rc.chan[_rc.function[THROTTLE]].scaled+1.0f)/2.0f;
+	manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled/2.0f;
 	if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
 	if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;