Greg Brush / WiiMP

Connect to a Wii Motion Plus (compatible) unit with no pass through or other controllers connected. The Motion Plus is a 3-axis gyroscope add-on w/Invensense IDG655 & IXZ650 MEMS gyros communicating via I2C @ 100KHz clock (some may do 400KHz)

WiiMP.c

Committer:
gbrush
Date:
2011-03-14
Revision:
0:506079387c48
Child:
1:727906cb5051

File content as of revision 0:506079387c48:

#include "WiiMP.h"



WiiMP::WiiMP(PinName data, PinName clk):_i2c(data, clk) {
    _i2c.frequency(100000); // Some report success at 400000, but 100000 should be guaranteed
    Error = true;
    
    //initialize whatever is at 0xA4 (if anything) -- will turn off MP+ if it's already on
    unsigned char cmd[] = {0xF0, 0x55};
    _i2c.write(WIIEXT_ADDR, (const char*)cmd, sizeof(cmd));
    wait_ms(I2C_READ_DELAY);
    
    // Instruct WiiMP to change address from 0xA6 to 0xA4 and initialize
    cmd[0] = 0xfe; cmd[1]= 0x04;
    if (_i2c.write(WMP_ADDR, (const char*)cmd, sizeof(cmd)) == I2C_ACK) {
        Error = false;
    }
    
}

bool WiiMP::Read(int* Yaw,int* Roll,int* Pitch) {

    int i;
    char readBuf[WIIMP_READLEN];
    
    if (Error) {
        return false;
    }
    
    const unsigned char cmd[] = {0x00};
    if (_i2c.write(WIIEXT_ADDR, (const char*)cmd, sizeof(cmd)) == I2C_ACK) {
        wait_ms(I2C_READ_DELAY);
        if (_i2c.read(WIIEXT_ADDR, readBuf, sizeof(readBuf)) == I2C_ACK) {
            //init values
            *Yaw = 0; *Roll = 0; *Pitch = 0;
   
            for (i = 0; i < WIIMP_READLEN; ++i) {
                readBuf[i] = (readBuf[i] ^ 0x17) + 0x17;
            }
            *Yaw   = ((readBuf[YawH] >> 2) << 8) + readBuf[YawL];
            *Roll  = ((readBuf[RollH] >> 2) << 8) + readBuf[RollL];
            *Pitch = ((readBuf[PitchH] >> 2) << 8) + readBuf[PitchL];
                        
        }
        else
        {
            return false;
        }
    } else {
        return false;
    }
    return true;
}