From 62732758a8b1b8604486a0a20d3bea08de032f5f Mon Sep 17 00:00:00 2001 From: Gavan Fantom Date: Fri, 7 Oct 2011 23:39:28 +0000 Subject: [PATCH] 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 | 2 +- dcm.c | 153 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ dcm.h | 9 ++++ main.c | 1 + matrix.c | 147 ++++++++++++++++++++++++++++++++++++++++++++++++++++ matrix.h | 19 +++++++ uart.c | 25 +++++++++ uart.h | 1 + wmp.c | 90 ++++++++++++++++++++++++++------ wmp.h | 1 + 10 files changed, 431 insertions(+), 17 deletions(-) create mode 100644 dcm.c create mode 100644 dcm.h create mode 100644 matrix.c create mode 100644 matrix.h diff --git a/Makefile b/Makefile index 1452c2f..23cc81d 100644 --- a/Makefile +++ b/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 diff --git a/dcm.c b/dcm.c new file mode 100644 index 0000000..28b1231 --- /dev/null +++ b/dcm.c @@ -0,0 +1,153 @@ +/* dcm.c */ + +#ifdef WE_HAVE_SQRT +#include +#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"); +} diff --git a/dcm.h b/dcm.h new file mode 100644 index 0000000..164dd7a --- /dev/null +++ b/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); diff --git a/main.c b/main.c index cbf306a..54986f5 100644 --- a/main.c +++ b/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."); diff --git a/matrix.c b/matrix.c new file mode 100644 index 0000000..29a4614 --- /dev/null +++ b/matrix.c @@ -0,0 +1,147 @@ +/* matrix.c */ + +#ifdef MATRIX_DEBUG +#include +#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 */ +} diff --git a/matrix.h b/matrix.h new file mode 100644 index 0000000..f7566e5 --- /dev/null +++ b/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); diff --git a/uart.c b/uart.c index 1009fd4..99b5235 100644 --- a/uart.c +++ b/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; diff --git a/uart.h b/uart.h index 6617a96..cff52fc 100644 --- a/uart.h +++ b/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); diff --git a/wmp.c b/wmp.c index 8f174ca..6790956 100644 --- a/wmp.c +++ b/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"); +} diff --git a/wmp.h b/wmp.h index 236ad07..e9a3bc8 100644 --- a/wmp.h +++ b/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 */