File content as of revision 3:c058c2f600bb:

//InvenSense MPU-9250
//Driver for the nrf51822 mcu
//maximum I2C speed is 400kHz

//TODO don't forget tap/double-tap functionality and calibration

#include "MPU9250.h"
#include "mbed.h"

bool MPU9250::readRegister(int addr, uint8_t* data, int length = 1) {
    if (i2c.write(deviceAddress,(char *)&addr, 1, true)) {
        if (i2c.read(deviceAddress,(char *)data, length, false)) {
            return true;
        }
    }
    
    return false;
}

bool MPU9250::writeRegister(int addr, uint8_t data) {
    if (i2c.write(deviceAddress,(char *)&addr, 1, true)) {
        bool ack = i2c.write(data);
        i2c.stop();
        return ack;
    }
        
    return false;
}

bool MPU9250::readMagRegister(int addr, uint8_t* data, int length = 1) {
    if (i2c.write(MPU9250_REG_ID,(char *)&addr, 1, true)) {
        if (i2c.read(MPU9250_REG_ID,(char *)data, length, false)) {
            return true;
        }
    }
    
    i2c.stop();
    
    return false;
}

bool MPU9250::writeMagRegister(int addr, uint8_t data) {
    if (i2c.write(MPU9250_REG_ID,(char *)&addr, 1, true)) {
        bool ack = i2c.write(data);
        i2c.stop();
        return ack;
    }
    
    return false;
}

MPU9250::MPU9250(int SCLPin, int SDAPin, int addr) : i2c((PinName)SDAPin,(PinName)SCLPin) {
    deviceAddress = addr;
    //magnetometer has it's own non-changeable address defined in: MPU9250_REG_ID   
}
        
bool MPU9250::init() {
    //configure interrupts and disable FSYNC pin interrupt (not connected)
    // ACTL 0 = INT pin active high
    // OPEN 0 = INT pin is push-pull
    // LATCH_INT_EN 1 = keep INT high until cleared
    // INT_ANYRD_2CLEAR 1 = INT is cleared if any read operation is performed
    // ACTL_FSYNC 0 = FSYNC is active high
    // FSYNC_INT_MODE_EN 0 = disable FSYNC input
    // BYPASS_EN 1 = set I2C master pins to bypass when I2C master is disabled
    // RESERVED = keep at 0
    writeRegister(MPU9250_REG_INTCFG, 0b00110010); //(flags MSB to LSB: ACTL,OPEN,LATCH_INT_EN,INT_ANYRD_2CLEAR,ACTL_FSYNC,FSYNC_INT_MODE_EN,BYPASS_EN,RESERVED)

    //configure gyroscope range
    writeRegister(MPU9250_REG_GYROCFG,MPU9250_GYROSCALE_2000);
    
    //configure accelerometer range
    writeRegister(MPU9250_REG_ACCELCFG,MPU9250_ACCSCALE_8);
    
    //read first magnetometer data
    writeMagRegister(MPU9250_MAG_REG_CNTL, 0x01); //magnetometer single-measurement
    
    //set interrupt conditions and enable interrupts
    writeRegister(MPU9250_REG_WOMT,  0b00000011); //Set wake-on-motion threshold, range is 0mg to 1020mg (LSB = 4mg)
    writeRegister(MPU9250_REG_INTEN, 0b01000000); //Enable wake-on-motion interrupt
    
    
    return wakeup();
}

MPU9250_GYRO_DATA MPU9250::readGyro() {
    MPU9250_GYRO_DATA data;
    uint8_t* dataBytePtr = (uint8_t *)&data;

    data.isValid = readRegister(MPU9250_REG_GYRO_X_H, dataBytePtr, 6);

    return data;
}

MPU9250_ACCEL_DATA MPU9250::readAccel() {
    MPU9250_ACCEL_DATA data;
    uint8_t* dataBytePtr = (uint8_t *)&data;

    data.isValid = readRegister(MPU9250_REG_ACCEL_X_H, dataBytePtr, 6);

    return data;
}

MPU9250_ACCEL_DATA MPU9250::readAccelNew() {
    //MPU9250_ACCEL_DATA data;
    //uint8_t* dataBytePtr = (uint8_t *)&data;

    //data.isValid = readRegister(MPU9250_REG_ACCEL_X_H, dataBytePtr, 6);

    //return data;

    //EXAMPLE>
    MPU9250_ACCEL_DATA data; 
    uint8_t oneByte;
    
    //read one byte at a time and transfer it to the struct ourselves instead of working with pointers
    //addr = register address
    
    //#define MPU9250_REG_ACCEL_X_H 0x3B
    i2c.write(deviceAddress,MPU9250_REG_ACCEL_X_H, 1, true);
    i2c.read(deviceAddress, oneByte, 1, false);
    data.x = (oneByte << 8); //high byte
    
    i2c.write(deviceAddress,MPU9250_REG_ACCEL_X_L, 1, true);
    i2c.read(deviceAddress, oneByte, 1, false);
    data.x = data.x | oneByte; //low byte    
    
    i2c.stop();
    return data;
    //EXAMPLE<
}

MPU9250_TEMP_DATA MPU9250::readTemp() {
    MPU9250_TEMP_DATA data;
    uint8_t* dataBytePtr = (uint8_t *)&data;

    data.isValid = readRegister(MPU9250_REG_TEMP_H, dataBytePtr, 2);
    
    //TODO conv from sensor to celsius: ((sensoroutput-roomtempoffset)/tempsensitivity)+21
    
    return data;
}

MPU9250_MAG_DATA MPU9250::readMag() {
    MPU9250_MAG_DATA data;
    uint8_t magDataReady;
    uint8_t* dataBytePtr = (uint8_t *)&data;
    
    //wait for magnetometer reading to be finished
    do { readMagRegister(MPU9250_MAG_REG_STATUS,&magDataReady); }
    while (!(magDataReady&0x01));
    
    //read magnetometer data:
    data.isValid = readMagRegister(MPU9250_MAG_REG_X_L, dataBytePtr, 6);
    
    //high and low nibble need to be reversed because of register map order
    data.x = SHORT_SWAP_BYTE(data.x);
    data.y = SHORT_SWAP_BYTE(data.y);
    data.z = SHORT_SWAP_BYTE(data.z);
    
    return data;
}

bool MPU9250::wakeup() {
    return writeRegister(MPU9250_REG_PWR1,0);
}

bool MPU9250::standby() {
    //turns off sensing but keeps the gyroscope powered
    return writeRegister(MPU9250_REG_PWR1, 0b00010000);
}

bool MPU9250::sleep() {
    //TODO
    return writeRegister(MPU9250_REG_PWR1, 0b01000000);
}