quadrotor/wmp.c

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