You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
101 lines
2.0 KiB
101 lines
2.0 KiB
|
|
#include "wmp.h" |
|
#include "i2c.h" |
|
|
|
bool wmp_init(void) |
|
{ |
|
if (!i2c_send_start()) |
|
return FALSE; |
|
if (!i2c_send_address(0x53, TRUE)) |
|
return FALSE; |
|
if (!i2c_send_data(0xfe)) |
|
return FALSE; |
|
if (!i2c_send_data(0x04)) |
|
return FALSE; |
|
i2c_send_stop(); |
|
return TRUE; |
|
} |
|
|
|
unsigned char wmp_calibration_data[0x20]; |
|
|
|
bool wmp_read_calibration_data(void) |
|
{ |
|
int i; |
|
|
|
if (!i2c_send_start()) |
|
return FALSE; |
|
if (!i2c_send_address(0x53, TRUE)) |
|
return FALSE; |
|
if (!i2c_send_data(0x20)) |
|
return FALSE; |
|
i2c_send_stop(); |
|
|
|
if (!i2c_send_start()) |
|
return FALSE; |
|
if (!i2c_send_address(0x53, FALSE)) |
|
return FALSE; |
|
for (i = 0; i < 0x20; i++) { |
|
unsigned int data; |
|
if (!i2c_receive_data(&data, (i == 0x1f))) |
|
return FALSE; |
|
wmp_calibration_data[i] = data; |
|
} |
|
i2c_send_stop(); |
|
return TRUE; |
|
} |
|
|
|
unsigned int wmp_yaw; |
|
unsigned int wmp_pitch; |
|
unsigned int wmp_roll; |
|
|
|
bool wmp_yaw_fast; |
|
bool wmp_pitch_fast; |
|
bool wmp_roll_fast; |
|
|
|
/* 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) |
|
|
|
bool wmp_sample(void) |
|
{ |
|
int i; |
|
unsigned int b[6]; |
|
|
|
if (!i2c_send_start()) |
|
return FALSE; |
|
if (!i2c_send_address(0x52, TRUE)) |
|
return FALSE; |
|
if (!i2c_send_data(0x00)) |
|
return FALSE; |
|
i2c_send_stop(); |
|
|
|
if (!i2c_send_start()) |
|
return FALSE; |
|
if (!i2c_send_address(0x52, FALSE)) |
|
return FALSE; |
|
for (i = 0; i < 6; i++) { |
|
if (!i2c_receive_data(&(b[i]), (i == 5))) |
|
return FALSE; |
|
} |
|
i2c_send_stop(); |
|
|
|
wmp_yaw = ((b[3]>>2)<<8) + b[0]; |
|
wmp_pitch = ((b[4]>>2)<<8) + b[1]; |
|
wmp_roll = ((b[5]>>2)<<8) + b[2]; |
|
|
|
/* XXX We don't take into account the fast/slow mode flag here */ |
|
wmp_yaw_fast = !(b[3] & 0x2); |
|
wmp_pitch_fast = !(b[3] & 0x1); |
|
wmp_roll_fast = !(b[4] & 0x2); |
|
|
|
return TRUE; |
|
} |
|
|
|
|