Quadrotor from scratch
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

102 lines
2.0 KiB

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