File content as of revision 26:5674b8978551:

// MPU6000 MBED library developed for swimmers

#include "mbed.h"
#include "mpu6000.h"

// Constructor
MPU6000::MPU6000(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {}

// Function that writes data to register
void MPU6000::regWrite(uint8_t ADDR, uint8_t DATA)
{
    // CS low -> start communicating
    cs = 0;
    // Send register address first
    spi.write(ADDR);
    // Send value to be written
    spi.write(DATA);
    // Wait a bit, since chip select is only software defined
    wait_us(1);
    // CS high -> stop communicating
    cs = 1;  
}

// Function that reads data from register
uint8_t MPU6000::regRead(uint8_t ADDR)
{
    // CS low -> start communicating
    cs = 0;
    // Write register address to read from ored with read flag
    spi.write((ADDR | READ));
    // Write dummy data to get respond clocked out
    int DATA = spi.write(0x00);
    // Wait a bit
    wait_us(1);
    // CS high -> stop communicating
    cs = 1;   
    // Return what has been read
    return DATA;
}

// Initializes MPU6000
uint8_t MPU6000::init()
{
    // Set chipselect high
    cs = 1;
    // Set spi bit length and format (use mode 3)
    spi.format(8, 3);
    // Set spi frquency to 1MHz (1MHz is max of MPU6000)
    spi.frequency(1000000);  
    // Disable I2C first
    regWrite(USER_CTRL, 0x10);
    // Reboot
    regWrite(PWR_MGMT_1, 0x80);
    // Wait a bit for MPU to reset
    wait_ms(150);
    // Read WHOAMI register
    uint8_t name = regRead(WHO_AM_I);
    // Select gyro pll for clk (manufacturers recommendation)
    regWrite(PWR_MGMT_1, 0x03);
    // Disable I2C again (after reset registers clear themselves)
    regWrite(USER_CTRL, 0x10);
    // Set gyro full scale range to 500 dps (degrees per second)
    regWrite(GYRO_CONFIG, 0x10); 
    // Set gyro full scale range to +- 4g
    regWrite(ACCEL_CONFIG, 0x08); 
    // Wait a bit
    wait_ms (150);
    // Return name
    return name;
}

// Reads current ACC values and stores them to provided buffer
void MPU6000::valRead(bool device, short int &x, short int &y, short int &z)
{
    uint8_t bufHigh, bufLow;
    // Switch based on selected device
    switch(device)
    {
        // Handle accelerometer selection
        case ACCEL:
            {
                // Wait a bit
                wait_us(5);
                // CS low -> start communicating
                cs = 0;
                // Write ACC_X high byte address, rest of values will come until we remain clocking -> burst mode
                spi.write(ACCEL_XOUT_H | READ);
                // ACC_X high byte
                bufHigh = spi.write(0x00);
                // ACC_X low byte
                bufLow = spi.write(0x00);
                // Merge ACC_X and add it to buffer
                x = (short int)((bufHigh << 8) | bufLow);
                // ACC_Y high byte
                bufHigh = spi.write(0x00);
                // ACC_Y low byte
                bufLow = spi.write(0x00);
                // Merge ACC_Y and add it to buffer
                y = (short int)((bufHigh << 8) | bufLow); 
                // ACC_Z high byte
                bufHigh = spi.write(0x00);
                // ACC_Y low byte
                bufLow = spi.write(0x00);
                // Merge ACC_Z and add it to buffer
                z = (short int)((bufHigh << 8) | bufLow);
                // CS high -> stop communicating
                cs = 1;
                break;
            }
        // Handle gyro selection
        case GYRO:
            {
                // Wait a bit
                wait_us(5);
                // CS low -> start communicating
                cs = 0;
                // Write GYRO_X high byte address, rest of values will come until we remain clocking -> burst mode
                spi.write(GYRO_XOUT_H | READ);
                // GYRO_X high byte
                bufHigh = spi.write(0x00);
                // GYRO_X low byte
                bufLow = spi.write(0x00);
                // Merge GYRO_X and add it to buffer
                x = (short int)((bufHigh << 8) | bufLow);
                // GYRO_Y high byte
                bufHigh = spi.write(0x00);
                // GYRO_Y low byte
                bufLow = spi.write(0x00);
                // Merge GYRO_Y and add it to buffer
                y = (short int)((bufHigh << 8) | bufLow); 
                // GYRO_Z high byte
                bufHigh = spi.write(0x00);
                // GYRO_Z low byte
                bufLow = spi.write(0x00);
                // Merge GYRO_Z and add it to buffer
                z = (short int)((bufHigh << 8) | bufLow);
                // CS high -> stop communicating
                cs = 1;
                break;
            }
    }
}