Take interleaved accelerometer and gyro readings, and integrate them into

a single attitude estimation.

A bunch of debug stuff, too.

Makefile

@@ -4,6 +4,7 @@ NAME=quad
 
 SSRCS=crt0.s
 CSRCS=main.c i2c.c wmp.c timer.c interrupt.c uart.c event.c matrix.c dcm.c
+CSRCS+=fisqrt.c
 
 COPTIM?=-O1
 CFLAGS=-march=armv4t -msoft-float $(COPTIM) -Wall -Werror -Wextra

dcm.c

@@ -2,11 +2,20 @@
 
 #ifdef WE_HAVE_SQRT
 #include <math.h>
+#else
+#include "fisqrt.h"
 #endif
 #include "matrix.h"
 #include "dcm.h"
 #include "uart.h"
 
+#define GRAVITY 9.80665f
+
+#define KP_ROLLPITCH 0.05967
+#define KI_ROLLPITCH 0.00001278
+
+#define ERROR_LIMIT 1.17f
+
 /* Implementation of the DCM IMU concept as described by Premerlani
  * and Bizard
  */
@@ -15,10 +24,17 @@ float dcm[3*3] = {1, 0, 0,
 		  0, 1, 0,
 		  0, 0, 1};
 
+float omega_p[3] = {0.0, 0.0, 0.0};
+float omega_i[3] = {0.0, 0.0, 0.0};
+
 float delta_t = 0.01;
 
-void dcm_update(float omega_x, float omega_y, float omega_z)
+void dcm_update(float x, float y, float z)
 {
+	float omega_x = x + omega_i[0] + omega_p[0];
+	float omega_y = y + omega_i[1] + omega_p[1];
+	float omega_z = z + omega_i[2] + omega_p[2];
+
 	float tx = delta_t * omega_x;
 	float ty = delta_t * omega_y;
 	float tz = delta_t * omega_z;
@@ -34,6 +50,36 @@ void dcm_update(float omega_x, float omega_y, float omega_z)
 	dcm_normalise();
 }
 
+void dcm_setvector(float x, float y, float z)
+{
+	/* We're given the Z axis */
+	dcm[6] = x;
+	dcm[7] = y;
+	dcm[8] = z;
+
+	/* Second row = cross product of unit X and third rows */
+	dcm[3] = 0.0;
+	dcm[4] = -dcm[8];
+	dcm[5] = dcm[7];
+
+	/* First row = cross product of third and second rows */
+	dcm[0] = dcm[7]*dcm[5] - dcm[8]*dcm[4];
+	dcm[1] = dcm[8]*dcm[3] - dcm[6]*dcm[5];
+	dcm[2] = dcm[6]*dcm[4] - dcm[7]*dcm[3];
+
+	/* Second row = cross product of third and first rows */
+	dcm[3] = dcm[7]*dcm[2] - dcm[8]*dcm[1];
+	dcm[4] = dcm[8]*dcm[0] - dcm[6]*dcm[2];
+	dcm[5] = dcm[6]*dcm[1] - dcm[7]*dcm[0];
+
+	dcm_renormalise(dcm+0);
+	dcm_renormalise(dcm+3);
+	dcm_renormalise(dcm+6);
+#if 0
+	dcm_normalise();
+#endif
+}
+
 void dcm_normalise(void)
 {
 	float error;
@@ -78,11 +124,14 @@ bool dcm_renormalise(float *v)
 
 	if (f < 1.5625f && f > 0.64f) {
 		f = 0.5 * (3 - f);
-#ifdef WE_HAVE_SQRT
 	} else if (f < 100.0f && f > 0.01f) {
+#ifdef WE_HAVE_SQRT
 		f = 1.0 / sqrt(f);
-		/* XXX log this event? */
+#else
+		f = fisqrt(f);
 #endif
+		/* XXX log this event? */
+		putstr("sqrt\r\n");
 	} else {
 		putstr("problem\r\n");
 		return FALSE;
@@ -95,6 +144,64 @@ bool dcm_renormalise(float *v)
 	return TRUE;
 }
 
+void dcm_drift_correction(float x, float y, float z)
+{
+	float mag;
+	float weight;
+	float error[3];
+	int i;
+
+	mag = (1.0/fisqrt(x*x+y*y+z*z)) / GRAVITY;
+
+	mag = 1-mag;
+	if (mag < 0.0)
+		mag = -mag;
+
+	weight = 1 - 3*mag;
+
+	if (weight < 0.0)
+		weight = 0.0;
+	if (weight > 1.0)
+		weight = 1.0;
+
+	/* error = cross product of dcm last row and acceleration vector */
+	/* third row = cross product of first two rows */
+	error[0] = dcm[7]*z - dcm[8]*y;
+	error[1] = dcm[8]*x - dcm[6]*z;
+	error[2] = dcm[6]*y - dcm[7]*x;
+
+	for (i = 0; i < 3; i++) {
+		if (error[i] > ERROR_LIMIT)
+			error[i] = ERROR_LIMIT;
+		if (error[i] < -ERROR_LIMIT)
+			error[i] = -ERROR_LIMIT;
+	}
+
+	for (i = 0; i < 3; i++) {
+		omega_p[i] = error[i] * (KP_ROLLPITCH * weight);
+		omega_i[i] += error[i] * (KI_ROLLPITCH * weight);
+	}
+
+	putstr("w: ");
+	putint_s((int)(weight * 100000.0f));
+	putstr("\r\n");
+#if 0
+	putstr("p: ");
+	putint_s((int)(omega_p[0] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(omega_p[1] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(omega_p[2] * 100000.0f));
+	putstr(" i: ");
+	putint_s((int)(omega_i[0] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(omega_i[1] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(omega_i[2] * 100000.0f));
+	putstr("\r\n");
+#endif
+}
+
 void dcm_dump(void)
 {
 	putstr("dcm: ");

dcm.h

@@ -7,3 +7,6 @@ void dcm_normalise(void);
 bool dcm_renormalise(float *v);
 void dcm_dump(void);
 void dcm_send_packet(void);
+void dcm_setvector(float x, float y, float z);
+void dcm_drift_correction(float x, float y, float z);
+

fisqrt.c

@@ -0,0 +1,18 @@
+/* Implementation of fast inverse square root.
+ * See http://en.wikipedia.org/wiki/Fast_inverse_square_root
+ */
+
+float fisqrt(float n)
+{
+	long i;
+	float x2, y;
+
+	x2 = n * 0.5f;
+	y = n;
+	i = *(long *)&y;
+	i = 0x5f3759df - (i>>1);
+	y = *(float *)&i;
+	y = y * (1.5f - (x2*y*y));
+
+	return y;
+}

fisqrt.h

@@ -0,0 +1,3 @@
+/* fisqrt.h */
+
+float fisqrt(float n);

main.c

@@ -277,7 +277,9 @@ void menu_handler(void)
 			break;
 		case 'P':
 			putstr("Initialising timer... ");
-			timer_set_period(10*TIMER_MS);
+			/* We want a 100Hz loop but two samples per iteration.
+			 * So, we go for 200Hz. */
+			timer_set_period(5*TIMER_MS);
 			reply("done");
 			wmp_start_zero();
 			break;

wmp.c

@@ -3,10 +3,27 @@
 #include "i2c.h"
 #include "uart.h"
 #include "dcm.h"
+#include "fisqrt.h"
 
 #define WMP_ZERO_COUNT 100
 
-unsigned char wmp_init_command[2] = {0xfe, 0x04};
+
+#define ACCEL_ZERO_X 520
+#define ACCEL_ZERO_Y 516
+#define ACCEL_ZERO_Z 514
+
+/*
+516, 510, 710
+-4, -6, 196
+16, 36, 38416   = 38468
+sqrt(38468) = 196.1326...
+... somehow once we scale by gravity we get almost exactly 0.05.
+*/
+
+#define ACCEL_SCALE 0.05
+
+/* Nunchuck pass-through mode */
+unsigned char wmp_init_command[2] = {0xfe, 0x05};
 
 i2c_result wmp_result;
 unsigned int wmp_generation;
@@ -92,6 +109,10 @@ bool wmp_yaw_fast;
 bool wmp_pitch_fast;
 bool wmp_roll_fast;
 
+int accel_x;
+int accel_y;
+int accel_z;
+
 bool wmp_update;
 bool wmp_zero;
 
@@ -139,15 +160,10 @@ bool wmp_start_sample(void)
 	return i2c_start_transaction(&wmp_sample_transaction);
 }
 
-void wmp_event_handler(void)
+void wmp_process_gyro_sample(void)
 {
 	float yaw, pitch, roll;
 
-	if (wmp_result != I2C_SUCCESS)
-		return;
-
-	wmp_result = I2C_IN_PROGRESS;
-
 	wmp_yaw   = ((wmp_sample_data[3]>>2)<<8) + wmp_sample_data[0];
 	wmp_pitch = ((wmp_sample_data[4]>>2)<<8) + wmp_sample_data[1];
 	wmp_roll  = ((wmp_sample_data[5]>>2)<<8) + wmp_sample_data[2];
@@ -185,8 +201,10 @@ void wmp_event_handler(void)
 
 		wmp_generation++;
 
+#if 1
 		if ((wmp_generation % 2) == 0)
 			dcm_send_packet();
+#endif
 
 	} else if (wmp_zero) {
 		wmp_yaw_zero += wmp_yaw;
@@ -205,6 +223,66 @@ void wmp_event_handler(void)
 	}
 }
 
+void wmp_process_accel_sample(void)
+{
+	float x, y, z;
+#if 0
+	float invmag;
+#endif
+
+	accel_x   = (wmp_sample_data[2]<<2) + ((wmp_sample_data[5]>>3) & 0x02);
+	accel_y   = (wmp_sample_data[3]<<2) + ((wmp_sample_data[5]>>4) & 0x02);
+	accel_z   = ((wmp_sample_data[4]<<2) & 0x3f8) +
+					      ((wmp_sample_data[5]>>5) & 0x06);
+
+	x = (accel_x - ACCEL_ZERO_X) * ACCEL_SCALE;
+	y = (accel_y - ACCEL_ZERO_Y) * ACCEL_SCALE;
+	z = (accel_z - ACCEL_ZERO_Z) * ACCEL_SCALE;
+
+#if 0
+	invmag = fisqrt(x*x + y*y + z*z);
+
+	x = x * invmag;
+	y = y * invmag;
+	z = z * invmag;
+#endif
+
+#if 0
+	accel_x = (x * 512.0 + 1000.0);
+	accel_y = (y * 512.0 + 1000.0);
+	accel_z = (z * 512.0 + 1000.0);
+#endif
+
+#if 0
+	putstr("(");
+	putint(accel_x);
+	putstr(", ");
+	putint(accel_y);
+	putstr(", ");
+	putint(accel_z);
+	putstr(")\r\n");
+#endif
+
+	/* The minus signs are needed because something is upside down.
+	 * It might actually be the WMP, but we're defining coordinates based
+	 * on that so we'll just fudge it here.
+	 */
+	dcm_drift_correction(x, -y, -z);
+}
+
+void wmp_event_handler(void)
+{
+	if (wmp_result != I2C_SUCCESS)
+		return;
+
+	wmp_result = I2C_IN_PROGRESS;
+
+	if (wmp_sample_data[5] & 0x02)
+		wmp_process_gyro_sample();
+	else
+		wmp_process_accel_sample();
+}
+
 void wmp_start_zero(void)
 {
 	wmp_zero = TRUE;