The unified diff between revisions [4cc7246c..] and [23a3e9a5..] is displayed below. It can also be downloaded as a raw diff.
This diff has been restricted to the following files: 'wmp.c'
# # old_revision [4cc7246c1b6c809c9dc15997798f6deed15b3631] # new_revision [23a3e9a50b4034343e3bd217d2c225dcaec064dd] # # patch "wmp.c" # from [76b67ab3d8729319797c8f0f7ca2839524a76a20] # to [1ab4017652548447277e83cf1697442cb7c6949b] # ============================================================ --- 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;