The unified diff between revisions [65df00aa..] and [4cc7246c..] is displayed below. It can also be downloaded as a raw diff.
# # old_revision [65df00aa2705ce33fd74f4dd706d2879fe99b2b0] # new_revision [4cc7246c1b6c809c9dc15997798f6deed15b3631] # # add_file "dcm.c" # content [16b59bc009e1e04c2932e132fa351a06fa789e3a] # # add_file "dcm.h" # content [8cf2b18d961bdb55d2b83556e9d9cab91a58285a] # # add_file "matrix.c" # content [7aac09561911cbe10901d55f177c6c7e5177729c] # # add_file "matrix.h" # content [8a673aeabd2806fd044f0d9d14418d4c2d39ffac] # # patch "Makefile" # from [bd708913eaf7c1bac787cd82ff2b7e23c57ddc59] # to [aa804f03d53484ad86061695d7e033b091c87a88] # # patch "main.c" # from [5c11844a2fa7481ccddb49faaace19b7c21a1c7f] # to [48ab0f2657065a72781d68205f5b7ede8bfd60c4] # # patch "uart.c" # from [7c57b3658cbce3a90ff7773d9e4a0b1d626c68af] # to [37a2e0459886f7f9c4e4a5361e21d50902dbe3f7] # # patch "uart.h" # from [0b8aacd8b3674937065175bb8fd6ce1feafd477e] # to [4acdb1f160b933d1653b3ac765f110cd71332112] # # patch "wmp.c" # from [9acbe9ae30b2b7c95b8a700f4f7bb97c38097e21] # to [76b67ab3d8729319797c8f0f7ca2839524a76a20] # # patch "wmp.h" # from [1c8449a5057b64d7441e74d3ea43ef2818f9ffe8] # to [12983abdeae8f48c789d10feb223483cfd6e31eb] # ============================================================ --- Makefile bd708913eaf7c1bac787cd82ff2b7e23c57ddc59 +++ Makefile aa804f03d53484ad86061695d7e033b091c87a88 @@ -3,7 +3,7 @@ SSRCS=crt0.s 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 ============================================================ --- /dev/null +++ dcm.c 16b59bc009e1e04c2932e132fa351a06fa789e3a @@ -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"); +} ============================================================ --- /dev/null +++ dcm.h 8cf2b18d961bdb55d2b83556e9d9cab91a58285a @@ -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 5c11844a2fa7481ccddb49faaace19b7c21a1c7f +++ main.c 48ab0f2657065a72781d68205f5b7ede8bfd60c4 @@ -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."); ============================================================ --- /dev/null +++ matrix.c 7aac09561911cbe10901d55f177c6c7e5177729c @@ -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 */ +} ============================================================ --- /dev/null +++ matrix.h 8a673aeabd2806fd044f0d9d14418d4c2d39ffac @@ -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 7c57b3658cbce3a90ff7773d9e4a0b1d626c68af +++ uart.c 37a2e0459886f7f9c4e4a5361e21d50902dbe3f7 @@ -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 0b8aacd8b3674937065175bb8fd6ce1feafd477e +++ uart.h 4acdb1f160b933d1653b3ac765f110cd71332112 @@ -7,6 +7,7 @@ void putint(unsigned int n); 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 9acbe9ae30b2b7c95b8a700f4f7bb97c38097e21 +++ wmp.c 76b67ab3d8729319797c8f0f7ca2839524a76a20 @@ -2,7 +2,10 @@ #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}; i2c_result wmp_result; @@ -81,21 +84,31 @@ unsigned int wmp_roll; 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 @@ void wmp_event_handler(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 1c8449a5057b64d7441e74d3ea43ef2818f9ffe8 +++ wmp.h 12983abdeae8f48c789d10feb223483cfd6e31eb @@ -18,5 +18,6 @@ void wmp_event_handler(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 */