quadrotor/wmp.c

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

	}
}