The unified diff between revisions [4cc7246c..] and [23a3e9a5..] is displayed below. It can also be downloaded as a raw diff.
# # old_revision [4cc7246c1b6c809c9dc15997798f6deed15b3631] # new_revision [23a3e9a50b4034343e3bd217d2c225dcaec064dd] # # add_file "fisqrt.c" # content [65553ed1491b6b3d4a266cc821c3f117c623a203] # # add_file "fisqrt.h" # content [362a8a7cb02214b241e95c9baa328416f02a48b5] # # patch "Makefile" # from [aa804f03d53484ad86061695d7e033b091c87a88] # to [d61625cad3fe90d5ffd933b03b2f322afc177d10] # # patch "dcm.c" # from [16b59bc009e1e04c2932e132fa351a06fa789e3a] # to [35a44d5b49a5c9bf56678ae7af0b611fde405621] # # patch "dcm.h" # from [8cf2b18d961bdb55d2b83556e9d9cab91a58285a] # to [91aed9ca7b3b59898347ab5f47e7aa364a483928] # # patch "main.c" # from [48ab0f2657065a72781d68205f5b7ede8bfd60c4] # to [e1a823b4962f3e8dc43b519a1f57745854ae6689] # # patch "wmp.c" # from [76b67ab3d8729319797c8f0f7ca2839524a76a20] # to [1ab4017652548447277e83cf1697442cb7c6949b] # ============================================================ --- Makefile aa804f03d53484ad86061695d7e033b091c87a88 +++ Makefile d61625cad3fe90d5ffd933b03b2f322afc177d10 @@ -4,6 +4,7 @@ CSRCS=main.c i2c.c wmp.c timer.c interru 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 16b59bc009e1e04c2932e132fa351a06fa789e3a +++ dcm.c 35a44d5b49a5c9bf56678ae7af0b611fde405621 @@ -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 ome 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); + } else if (f < 100.0f && f > 0.01f) { #ifdef WE_HAVE_SQRT - } else if (f < 100.0f && f > 0.01f) { 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 8cf2b18d961bdb55d2b83556e9d9cab91a58285a +++ dcm.h 91aed9ca7b3b59898347ab5f47e7aa364a483928 @@ -7,3 +7,6 @@ void dcm_send_packet(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); + ============================================================ --- /dev/null +++ fisqrt.c 65553ed1491b6b3d4a266cc821c3f117c623a203 @@ -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; +} ============================================================ --- /dev/null +++ fisqrt.h 362a8a7cb02214b241e95c9baa328416f02a48b5 @@ -0,0 +1,3 @@ +/* fisqrt.h */ + +float fisqrt(float n); ============================================================ --- main.c 48ab0f2657065a72781d68205f5b7ede8bfd60c4 +++ main.c e1a823b4962f3e8dc43b519a1f57745854ae6689 @@ -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 76b67ab3d8729319797c8f0f7ca2839524a76a20 +++ wmp.c 1ab4017652548447277e83cf1697442cb7c6949b @@ -3,11 +3,28 @@ #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_roll_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;