Track gyro using a DCM.

This brings in an implementation of some general matrix manipulation routines,
not all of which are used at teh moment.

Output the DCM on the UART at 50Hz for the host to display it usefully.

Makefile

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

dcm.c

@@ -0,0 +1,153 @@
+/* dcm.c */
+
+#ifdef WE_HAVE_SQRT
+#include <math.h>
+#endif
+#include "matrix.h"
+#include "dcm.h"
+#include "uart.h"
+
+/* Implementation of the DCM IMU concept as described by Premerlani
+ * and Bizard
+ */
+
+float dcm[3*3] = {1, 0, 0,
+		  0, 1, 0,
+		  0, 0, 1};
+
+float delta_t = 0.01;
+
+void dcm_update(float omega_x, float omega_y, float omega_z)
+{
+	float tx = delta_t * omega_x;
+	float ty = delta_t * omega_y;
+	float tz = delta_t * omega_z;
+
+	float update_matrix[3*3] = { 0, -tz,  ty,
+				    tz,   0, -tx,
+				   -ty,  tx,   0};
+	float temp_matrix[3*3];
+
+	matrix_multiply(temp_matrix, dcm, update_matrix, 3, 3, 3);
+	matrix_add(dcm, dcm, temp_matrix, 3, 3);
+
+	dcm_normalise();
+}
+
+void dcm_normalise(void)
+{
+	float error;
+	float tmp[6];
+	int i;
+
+	/* dot product of first two rows */
+	error = dcm[0]*dcm[3] + dcm[1]*dcm[4] + dcm[2]*dcm[5];
+
+	/* printf("error is %f\n", error); */
+
+	tmp[0] = dcm[3];
+	tmp[1] = dcm[4];
+	tmp[2] = dcm[5];
+	tmp[3] = dcm[0];
+	tmp[4] = dcm[1];
+	tmp[5] = dcm[2];
+
+	for (i = 0; i < 6; i++)
+		dcm[i] = dcm[i] - (tmp[i] * (0.5f * error));
+
+	/* third row = cross product of first two rows */
+	dcm[6] = dcm[1]*dcm[5] - dcm[2]*dcm[4];
+	dcm[7] = dcm[2]*dcm[3] - dcm[0]*dcm[5];
+	dcm[8] = dcm[0]*dcm[4] - dcm[1]*dcm[3];
+
+	if (!(dcm_renormalise(dcm+0) &&
+	      dcm_renormalise(dcm+3) &&
+	      dcm_renormalise(dcm+6))) {
+		/* Shit. I've been shot. */
+		dcm[0] = dcm[4] = dcm[8] = 1.0f;
+		dcm[1] = dcm[2] = dcm[3] = 0.0f;
+		dcm[5] = dcm[6] = dcm[7] = 0.0f;
+	}
+}
+
+bool dcm_renormalise(float *v)
+{
+	float f = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
+
+	/* printf("f is %f\n", f); */
+
+	if (f < 1.5625f && f > 0.64f) {
+		f = 0.5 * (3 - f);
+#ifdef WE_HAVE_SQRT
+	} else if (f < 100.0f && f > 0.01f) {
+		f = 1.0 / sqrt(f);
+		/* XXX log this event? */
+#endif
+	} else {
+		putstr("problem\r\n");
+		return FALSE;
+	}
+
+	v[0] = v[0] * f;
+	v[1] = v[1] * f;
+	v[2] = v[2] * f;
+
+	return TRUE;
+}
+
+void dcm_dump(void)
+{
+	putstr("dcm: ");
+	putint_s((int)(dcm[0] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[1] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[2] * 100000.0f));
+	putstr("\r\n     ");
+	putint_s((int)(dcm[3] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[4] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[5] * 100000.0f));
+	putstr("\r\n     ");
+	putint_s((int)(dcm[6] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[7] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[8] * 100000.0f));
+	putstr("\r\n");
+}
+
+void puthexfloat(float f)
+{
+	union {
+		float f;
+		unsigned int i;
+	} u;
+
+	u.f = f;
+	puthex(u.i);
+}
+
+void dcm_send_packet(void)
+{
+	putstr("D:(");
+	puthexfloat(dcm[0]);
+	putstr(",");
+	puthexfloat(dcm[1]);
+	putstr(",");
+	puthexfloat(dcm[2]);
+	putstr(",");
+	puthexfloat(dcm[3]);
+	putstr(",");
+	puthexfloat(dcm[4]);
+	putstr(",");
+	puthexfloat(dcm[5]);
+	putstr(",");
+	puthexfloat(dcm[6]);
+	putstr(",");
+	puthexfloat(dcm[7]);
+	putstr(",");
+	puthexfloat(dcm[8]);
+	putstr(")\r\n");
+}

dcm.h

@@ -0,0 +1,9 @@
+/* dcm.h */
+
+#include "types.h"
+
+void dcm_update(float omega_x, float omega_y, float omega_z);
+void dcm_normalise(void);
+bool dcm_renormalise(float *v);
+void dcm_dump(void);
+void dcm_send_packet(void);

main.c

@@ -279,6 +279,7 @@ void menu_handler(void)
 			putstr("Initialising timer... ");
 			timer_set_period(10*TIMER_MS);
 			reply("done");
+			wmp_start_zero();
 			break;
 		default:
 			reply("Unrecognised command.");

matrix.c

@@ -0,0 +1,147 @@
+/* matrix.c */
+
+#ifdef MATRIX_DEBUG
+#include <stdio.h>
+#endif
+
+/* dest[r][c] = m1[r][n] * m2[n][c] */
+void matrix_multiply(float *dest, float *m1, float *m2, int r, int c, int n)
+{
+	int i, j, k;
+
+	for (i = 0; i < r; i++) {
+		for (j = 0; j < c; j++) {
+			dest[i*c+j] = 0;
+			for (k = 0; k < n; k++) {
+				dest[i*c+j] += m1[i*n+k] * m2[k*c+j];
+			}
+		}
+	}
+}
+
+/* dest[r][c] = m1[r][n] * m2[c][n] */
+void matrix_multiply_t(float *dest, float *m1, float *m2, int r, int c, int n)
+{
+	int i, j, k;
+
+	for (i = 0; i < r; i++) {
+		for (j = 0; j < c; j++) {
+			dest[i*c+j] = 0;
+			for (k = 0; k < n; k++) {
+				dest[i*c+j] += m1[i*n+k] * m2[j*n+k];
+			}
+		}
+	}
+}
+
+/* dest[r][c] = m1[r][c] + m2[r][c] */
+void matrix_add(float *dest, float *m1, float *m2, int r, int c)
+{
+	int i, j;
+
+	for (i = 0; i < r; i++) {
+		for (j = 0; j < c; j++) {
+			dest[i*c+j] = m1[i*c+j] + m2[i*c+j];
+		}
+	}
+}
+
+#ifdef MATRIX_DEBUG
+void dump_matrix(float *m, int r, int c)
+{
+	int i, j;
+
+	for (i = 0; i < r; i++) {
+		printf("(");
+		for (j = 0; j < c; j++) {
+			printf("%f", *(m++));
+			if (j < c-1)
+				printf(" ");
+		}
+		printf(")\n");
+	}
+	printf("\n");
+}
+#endif
+
+/* Invert src into dst.
+ * NOTE: destroys the src matrix
+ */
+void matrix_invert(float *dst, float *src, int size)
+{
+	int i, j, k;
+
+	/* Initialise answer with identity matrix */
+	for (i = 0; i < size*size; i++)
+		dst[i] = 0.0;
+	for (i = 0; i < size; i++)
+		dst[i*(size+1)] = 1.0;
+
+	/* Manipulate the matrix into row-echelon form */
+	for (i = 0; i < size; i++) {
+		float max;
+		int maxi;
+
+		/* find a pivot */
+		max = src[i*size+i];
+		maxi = i;
+		for (j = i+1; j < size; j++) {
+			if (src[j*size+i] > max) {
+				max = src[j*size+i];
+				maxi = j;
+			}
+		}
+		if (maxi != i) {
+			/* swap rows */
+			for (j = 0; j < size; j++) {
+				float tmp;
+
+				tmp = src[i*size+j];
+				src[i*size+j] = src[maxi*size+j];
+				src[maxi*size+j] = tmp;
+				tmp = dst[i*size+j];
+				dst[i*size+j] = dst[maxi*size+j];
+				dst[maxi*size+j] = tmp;
+			}
+		}
+		for (j = size-1; j > i; j--) {
+			float factor;
+
+			factor = src[j*size+i] / max;
+			for (k = 0; k < size; k++) {
+				src[j*size+k] = src[j*size+k] -
+						    src[i*size+k] * factor;
+				dst[j*size+k] = dst[j*size+k] -
+						    dst[i*size+k] * factor;
+			}
+		}
+	}
+
+	/* Back-substitute to get src into diagonal form */
+	for (i = size-1; i > 0; i--) {
+		for (j = 0; j < i; j++) {
+			float factor;
+
+			factor = src[j*size+i] / src[i*size+i];
+			for (k = 0; k < size; k++) {
+				src[j*size+k] = src[j*size+k] -
+						    src[i*size+k] * factor;
+				dst[j*size+k] = dst[j*size+k] -
+						    dst[i*size+k] * factor;
+			}
+		}
+	}
+
+	/* Divide each row by its diagonal element */
+	for (i = 0; i < size; i++) {
+		float factor;
+
+		factor = src[i*size+i];
+		for (j = 0; j < size; j++) {
+			src[i*size+j] = src[i*size+j] / factor;
+			dst[i*size+j] = dst[i*size+j] / factor;
+		}
+	}
+
+	/* src should now be the identiy matrix while dst holds the answer */
+}

matrix.h

@@ -0,0 +1,19 @@
+/* matrix.h */
+
+/* dest[r][c] = m1[r][n] * m2[n][c] */
+void matrix_multiply(float *dest, float *m1, float *m2, int r, int c, int n);
+
+/* dest[r][c] = m1[r][n] * m2[c][n] */
+void matrix_multiply_t(float *dest, float *m1, float *m2, int r, int c, int n);
+
+/* dest[r][c] = m1[r][c] + m2[r][c] */
+void matrix_add(float *dest, float *m1, float *m2, int r, int c);
+
+#ifdef MATRIX_DEBUG
+void dump_matrix(float *m, int r, int c);
+#endif
+
+/* Invert src into dst.
+ * NOTE: destroys the src matrix
+ */
+void matrix_invert(float *dst, float *src, int size);

uart.c

@@ -155,6 +155,31 @@ void putint(unsigned int n) {
 	putstr(s+i);
 }
 
+void putint_s(int n) {
+	char s[12];
+	int i;
+	int neg;
+
+	/* OK, technically, this might not work properly for the most
+	 * negative possible number. Oh well.
+	 */
+	neg = (n < 0);
+	if (neg)
+		n = -n;
+
+	i = 11;
+	s[i] = '\0';
+
+	do {
+		s[--i] = n % 10 + '0';
+	} while ((n /= 10) > 0);
+
+	if (neg)
+		s[--i] = '-';
+
+	putstr(s+i);
+}
+
 void puthex(unsigned int n) {
 	char s[9];
 	int i;

uart.h

@@ -7,6 +7,7 @@ void init_uart(void);
 void putch(char c);
 void putstr(char *s);
 void putint(unsigned int n);
+void putint_s(int n);
 void puthex(unsigned int n);
 bool getch(char *c);
 

wmp.c

@@ -2,6 +2,9 @@
 #include "wmp.h"
 #include "i2c.h"
 #include "uart.h"
+#include "dcm.h"
+
+#define WMP_ZERO_COUNT 100
 
 unsigned char wmp_init_command[2] = {0xfe, 0x04};
 
@@ -81,21 +84,31 @@ unsigned int wmp_yaw;
 unsigned int wmp_pitch;
 unsigned int wmp_roll;
 
+unsigned int wmp_yaw_zero;
+unsigned int wmp_pitch_zero;
+unsigned int wmp_roll_zero;
+
 bool wmp_yaw_fast;
 bool wmp_pitch_fast;
 bool wmp_roll_fast;
 
+bool wmp_update;
+bool wmp_zero;
+
+#define TWO_PI 6.28318531f
+#define DEG_TO_RAD (TWO_PI/360.0f)
+
 /* There's considerable debate about these values, and they may vary
  * between different models of the Wii Motion Plus. It would be nice
  * to be able to use the calibration data stored on the device itself
  * but we don't know the format yet.
  */
-#define SLOW_YAW_STEP	(1000/20)
-#define SLOW_PITCH_STEP (1000/20)
-#define SLOW_ROLL_STEP  (1000/20)
-#define FAST_YAW_STEP	(1000/4)
-#define FAST_PITCH_STEP (1000/4)
-#define FAST_ROLL_STEP  (1000/4)
+#define SLOW_YAW_STEP	(20 / DEG_TO_RAD)
+#define SLOW_PITCH_STEP (20 / DEG_TO_RAD)
+#define SLOW_ROLL_STEP  (20 / DEG_TO_RAD)
+#define FAST_YAW_STEP	(4 / DEG_TO_RAD)
+#define FAST_PITCH_STEP (4 / DEG_TO_RAD)
+#define FAST_ROLL_STEP  (4 / DEG_TO_RAD)
 
 bool wmp_sample(void)
 {
@@ -128,6 +141,8 @@ bool wmp_start_sample(void)
 
 void wmp_event_handler(void)
 {
+	float yaw, pitch, roll;
+
 	if (wmp_result != I2C_SUCCESS)
 		return;
 
@@ -142,15 +157,58 @@ void wmp_event_handler(void)
 	wmp_pitch_fast = !(wmp_sample_data[3] & 0x1);
 	wmp_roll_fast = !(wmp_sample_data[4] & 0x2);
 
-	wmp_generation++;
-	if ((wmp_generation % 100) == 0) {
-		putstr("(");
-		puthex(wmp_roll);
-		putstr(", ");
-		puthex(wmp_pitch);
-		putstr(", ");
-		puthex(wmp_yaw);
-		putstr(")\r\n");
-
+	if (wmp_update) {
+		int tmp_yaw = wmp_yaw;
+		int tmp_pitch = wmp_pitch;
+		int tmp_roll = wmp_roll;
+
+		tmp_yaw -= wmp_yaw_zero;
+		tmp_pitch -= wmp_pitch_zero;
+		tmp_roll -= wmp_roll_zero;
+
+		if (wmp_yaw_fast)
+			yaw = ((float)tmp_yaw) / FAST_YAW_STEP;
+		else
+			yaw = ((float)tmp_yaw) / SLOW_YAW_STEP;
+
+		if (wmp_pitch_fast)
+			pitch = ((float)tmp_pitch) / FAST_PITCH_STEP;
+		else
+			pitch = ((float)tmp_pitch) / SLOW_PITCH_STEP;
+
+		if (wmp_roll_fast)
+			roll = ((float)tmp_roll) / FAST_ROLL_STEP;
+		else
+			roll = ((float)tmp_roll) / SLOW_ROLL_STEP;
+
+		dcm_update(roll, pitch, yaw);
+
+		wmp_generation++;
+
+		if ((wmp_generation % 2) == 0)
+			dcm_send_packet();
+
+	} else if (wmp_zero) {
+		wmp_yaw_zero += wmp_yaw;
+		wmp_pitch_zero += wmp_pitch;
+		wmp_roll_zero += wmp_roll;
+		wmp_generation++;
+		if (wmp_generation >= WMP_ZERO_COUNT) {
+			wmp_zero = FALSE;
+			wmp_update = TRUE;
+			wmp_generation = 0;
+			wmp_yaw_zero /= WMP_ZERO_COUNT;
+			wmp_pitch_zero /= WMP_ZERO_COUNT;
+			wmp_roll_zero /= WMP_ZERO_COUNT;
+			putstr("Zero finished\r\n");
+		}
 	}
 }
+
+void wmp_start_zero(void)
+{
+	wmp_zero = TRUE;
+	wmp_update = FALSE;
+	wmp_generation = 0;
+	putstr("Starting zero\r\n");
+}

wmp.h

@@ -18,5 +18,6 @@ bool wmp_sample(void);
 bool wmp_read_calibration_data(void);
 bool wmp_start_sample(void);
 void wmp_event_handler(void);
+void wmp_start_zero(void);
 
 #endif /* __WMP_H */