|
|
|
|
|
|
|
#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");
|
|
|
|
}
|