The unified diff between revisions [d8ed90db..] and [4cc7246c..] 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 [d8ed90db2d4284a290224447c40a0d9cef3fbc31] # new_revision [4cc7246c1b6c809c9dc15997798f6deed15b3631] # # add_file "wmp.c" # content [76b67ab3d8729319797c8f0f7ca2839524a76a20] # ============================================================ --- /dev/null +++ wmp.c 76b67ab3d8729319797c8f0f7ca2839524a76a20 @@ -0,0 +1,214 @@ + +#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; +unsigned int wmp_generation; + +struct i2c_transaction wmp_init_transaction = { + (0x53 << 1) + 0, /* write */ + 2, + wmp_init_command, + &wmp_result, + NULL +}; + +unsigned char wmp_read_cal_command[1] = {0x20}; + +struct i2c_transaction wmp_read_cal_transaction2; + +struct i2c_transaction wmp_read_cal_transaction = { + (0x53 << 1) + 0, /* write */ + 1, + wmp_read_cal_command, + &wmp_result, + &wmp_read_cal_transaction2 +}; + +struct i2c_transaction wmp_read_cal_transaction2 = { + (0x53 << 1) + 1, /* read */ + 0x20, + wmp_calibration_data, + &wmp_result, + NULL +}; + +unsigned char wmp_sample_command[1] = {0x00}; + +unsigned char wmp_sample_data[6]; + +struct i2c_transaction wmp_sample_transaction2; + +struct i2c_transaction wmp_sample_transaction = { + (0x52 << 1) + 0, /* write */ + 1, + wmp_sample_command, + &wmp_result, + &wmp_sample_transaction2 +}; + +struct i2c_transaction wmp_sample_transaction2 = { + (0x52 << 1) + 1, /* read */ + 6, + wmp_sample_data, + &wmp_result, + NULL +}; + + +bool wmp_init(void) +{ + if (!i2c_start_transaction(&wmp_init_transaction)) + return FALSE; + while (i2c_busy()) ; + return (wmp_result == I2C_SUCCESS); +} + +unsigned char wmp_calibration_data[0x20]; + +bool wmp_read_calibration_data(void) +{ + if (!i2c_start_transaction(&wmp_read_cal_transaction)) + return FALSE; + while (i2c_busy()); + return (wmp_result == I2C_SUCCESS); +} + +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 (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) +{ + if (!i2c_start_transaction(&wmp_sample_transaction)) + return FALSE; + + while (i2c_busy()); + + if (wmp_result != I2C_SUCCESS) + return FALSE; + + 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]; + + /* XXX We don't take into account the fast/slow mode flag here */ + wmp_yaw_fast = !(wmp_sample_data[3] & 0x2); + wmp_pitch_fast = !(wmp_sample_data[3] & 0x1); + wmp_roll_fast = !(wmp_sample_data[4] & 0x2); + + return TRUE; +} + +bool wmp_start_sample(void) +{ + return i2c_start_transaction(&wmp_sample_transaction); +} + +void wmp_event_handler(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]; + + /* XXX We don't take into account the fast/slow mode flag here */ + wmp_yaw_fast = !(wmp_sample_data[3] & 0x2); + wmp_pitch_fast = !(wmp_sample_data[3] & 0x1); + wmp_roll_fast = !(wmp_sample_data[4] & 0x2); + + 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"); +}