quadrotor/wmp.c

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