|
|
|
|
|
|
|
#include "wmp.h"
|
|
|
|
#include "i2c.h"
|
|
|
|
#include "uart.h"
|
|
|
|
|
|
|
|
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;
|
|
|
|
|
|
|
|
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)
|
|
|
|
{
|
|
|
|
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)
|
|
|
|
{
|
|
|
|
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);
|
|
|
|
|
|
|
|
wmp_generation++;
|
|
|
|
if ((wmp_generation % 100) == 0) {
|
|
|
|
putstr("(");
|
|
|
|
puthex(wmp_roll);
|
|
|
|
putstr(", ");
|
|
|
|
puthex(wmp_pitch);
|
|
|
|
putstr(", ");
|
|
|
|
puthex(wmp_yaw);
|
|
|
|
putstr(")\r\n");
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|