File content as of revision 42:96671b71aac5:

// Code for reading from the L3GD20H gyro with I2C.

#include <mbed.h>
#include "l3g.h"
#include "pc_serial.h"

I2C i2c(p9, p10);

const int address = 0xD6;

int32_t l3gInit()
{
    int whoami = l3gReadReg(0x0F);
    if (whoami != 0xD7)
    {    
        pc.printf("l3g whoami error: %d\n", whoami);
        return -1;  // wrong ID or no response
    }
    
    // CTRL1: 800 Hz output data rate,
    // low-pass filter cutoff 100 Hz
    l3gWriteReg(0x20, 0xFF);

    // CTRL4: 2000 dps full scale
    l3gWriteReg(0x23, 0x20);

    // CTRL5: High-pass filter disabled
    l3gWriteReg(0x24, 0x00);

    return 0;  // success
}

int32_t l3gReadReg(char reg)
{
    int result = i2c.write(address, &reg, 1); 
    if (result != 0)
    {
        return -1;  // error
    }

    char data;
    result = i2c.read(address, &data, 1);
    if (result != 0)
    {
        return -2;  // error
    }
  
    return data;
}

int32_t l3gWriteReg(char reg, char value)
{
    char data[2];
    data[0] = reg;
    data[1] = value;
    int result = i2c.write(address, data, 2); 
    if (result != 0)
    {
        return -1;  // error
    }
    
    return 0;
}

int32_t l3gZAvailable()
{
    // Read STATUS
    int32_t result = l3gReadReg(0x27);
    if (result < 0)
    {
        return result;
    }
    
    // Read the ZDA bit.
    return result >> 2 & 1;
}

int32_t l3gZRead()
{
    char reg = 0x80 | 0x2C;  // OUT_Z_L, with slave address updating
    int result = i2c.write(address, &reg, 1);
    if (result != 0)
    {
        return -500001;
    }
    
    char data[2];
    result = i2c.read(address, data, 2);
    if(result != 0)
    {
        return -500002;
    }
    
    return (int16_t)(data[1] << 8 | data[0]);
}

// The stuff below doesn't actually have anything to do with the L3G.

#define L3G_CAL_COUNT_MAX 1000
int8_t l3gCalBuffer[L3G_CAL_COUNT_MAX];
uint32_t l3gCalCount = 0;
uint32_t l3gCalReplayIndex = 0;

int32_t l3gCalibrate()
{
    int32_t reading = l3gZRead();
    
    if (l3gCalCount < L3G_CAL_COUNT_MAX)
    {
        int8_t c;
        if (reading > 127) { c = 127; }
        else if (reading < -128) { c = -128; }
        else { c = reading; }
        l3gCalBuffer[l3gCalCount++] = c;
    }
    
    return reading;
}

bool l3gCalibrateDone()
{
    return l3gCalCount >= L3G_CAL_COUNT_MAX;
}

void l3gCalibrateReset()
{
    l3gCalCount = 0;
}

int32_t l3gZReadCalibrated()
{
    int8_t zeroRate = 0;
    if (l3gCalCount)
    {
      if (l3gCalReplayIndex >= l3gCalCount) { l3gCalReplayIndex = 0; }
      zeroRate = l3gCalBuffer[l3gCalReplayIndex++];
    }
    
    return l3gZRead() - zeroRate;
}