File content as of revision 0:66fe7a32bd59:

#include "mbed.h"

/*************************
    L3G4200D Registers
*************************/
#define WHO_AM_I 0x0F
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24
#define REFERENCE 0x25
#define OUT_TEMP 0x26
#define STATUS_REG 0x27
#define OUT_X_L 0x28
#define OUT_X_H 0x29
#define OUT_Y_L 0x2A
#define OUT_Y_H 0x2B
#define OUT_Z_L 0x2C
#define OUT_Z_H 0x2D
#define FIFO_CTRL_REG 0x2E
#define FIFO_SRC_REG 0x2F
#define INT1_CFG 0x30
#define INT1_SRC 0x31
#define INT1_TSH_XH 0x32
#define INT1_TSH_XL 0x33
#define INT1_TSH_YH 0x34
#define INT1_TSH_YL 0x35
#define INT1_TSH_ZH 0x36
#define INT1_TSH_ZL 0x37
#define INT1_DURATION 0x38
#define xHigh 2  //axis x 0b00000010
#define yHigh 8  //axis y 0b00001000
#define zHigh 32 //axis z 0b00100000
#define GYRO_CALIBRATION_COUNT  64       //how many samples to take for calibration
#define GYRO_SAMPLE_PERIOD  .001f       //differnece of ODR period and time required to read registers
#define GYRO_SAMPLE_COUNT       1       //number of samples to average

DigitalOut chipSelect(PTD4);
DigitalIn INT1(D0);
Serial pc(USBTX, USBRX);


SPI L3G4200D(PTD6, PTD7, PTD5);

// 16-bit two's comp gyro readings 
int16_t gyro[3] = {0,0,0}, gyroBias[3] = {0,0,0};

int readRegister(int address)
{
    int toRead;
    
    address |= 0x80;  // This tells the L3G4200D we're reading;
    
    chipSelect.write(0);
    L3G4200D.write(address);
    toRead = L3G4200D.write(0x00);
    chipSelect.write(1);
    
    return toRead;
}

void writeRegister(int address, int data)
{
    address &= 0x7F;  // This to tell the L3G4200D we're writing
    
    chipSelect.write(0);
    L3G4200D.write(address);
    L3G4200D.write(data);
    chipSelect.write(1);
}


void setRef()
{
    readRegister(REFERENCE);
    writeRegister(INT1_CFG, 0x6A); //Enable XH, YH and ZH interrupt generation
                                     //Interrupt latched
    //Gyro.writeReg(L3G_INT1_CFG, 0x80);
}

void getGyroValues()
{
    gyro[0] = (readRegister(0x29)&0xFF)<<8;
    gyro[0] |= (readRegister(0x28)&0xFF);
    //gyro[0] = gyro[0] - gyroBias[0];
    
    gyro[1] = (readRegister(0x2B)&0xFF)<<8;
    gyro[1] |= (readRegister(0x2A)&0xFF);
    //gyro[1] = gyro[1] - gyroBias[1];
    
    gyro[2] = (readRegister(0x2D)&0xFF)<<8;
    gyro[2] |= (readRegister(0x2C)&0xFF);
    //gyro[2] = gyro[2] - gyroBias[2];
}

void getGyroBias()
{
    float accumulator[3] = {0,0,0}, tempStore[3];
    int sampleCount  = 0;
    
    //Summation of 64 readings
    while (sampleCount < GYRO_CALIBRATION_COUNT) 
    {
        //Make sure the accelerometer has had enough time
        //to take a new sample.
        wait(GYRO_SAMPLE_PERIOD);
        
        //get accelerometer data
        getGyroValues();
        
        for(int i = 0; i < 3; i++)
        {  
            //add current sample to previous samples
            accumulator[i] += tempStore[i];
        }
        
        sampleCount++;
    }
    for(int i = 0; i < 3; i++)
    {  
        //divide by number of samples
        tempStore[i] = accumulator[i] / GYRO_CALIBRATION_COUNT;   
        gyroBias[i] = (int16_t)tempStore[i];   
    } 
}

void getGyroAvg()
{
    float accumulator[3] = {0,0,0}, tempStore[3];
    int sampleCount  = 0;
    
    //Summation of 64 readings
    while (sampleCount < GYRO_SAMPLE_COUNT) 
    {
        //Make sure the accelerometer has had enough time
        //to take a new sample.
        wait(GYRO_SAMPLE_PERIOD);
        
        //get accelerometer data
        getGyroValues();
        
        for(int i = 0; i < 3; i++)
        {  
            //add current sample to previous samples
            accumulator[i] += tempStore[i];
        }
        
        sampleCount++;
    }
    for(int i = 0; i < 3; i++)
    {  
        //divide by number of samples
        tempStore[i] = accumulator[i] / GYRO_CALIBRATION_COUNT;   
        gyro[i] = (int16_t)tempStore[i];   
    } 
}

void setupL3G4200D()
{
    L3G4200D.format(8, 3);          //Set SPI mode to CPOL = 1 and CPHA = 1  
                                    //8-bit transmissions
                                    
    L3G4200D.frequency(10000000);   //10MHz SPI frequency
    
    chipSelect.write(1);  
    wait(.1);
    
    chipSelect.write(0);
    L3G4200D.write(0);  //Work around for clock not being set high when initiating SPI
    chipSelect.write(1);
  
    //writeRegister(CTRL_REG1, 0xCF); //normal power mode, all axes enabled, 800 Hz
    writeRegister(CTRL_REG1, 0xCF); //normal power mode, all axes enabled, 100 Hz
    
    writeRegister(CTRL_REG2, 0x00); //High-pass filter disabled
    writeRegister(CTRL_REG3, 0x80); //Interrupt driven to INT1 pad  
    writeRegister(CTRL_REG4, 0x00); //250 dps full scale
    writeRegister(CTRL_REG5, 0x05); //Data in DataReg and FIFO are high-pass filtered
                                      //High-pass-filtered data are used for interrupt
                                      //generation
    
    writeRegister(INT1_TSH_XH, 0x00); //X HIGH threshold 
    writeRegister(INT1_TSH_XL, 0x60); //X LOW threshold
    writeRegister(INT1_TSH_YH, 0x00); //Y HIGH threshold
    writeRegister(INT1_TSH_YL, 0x60); //Y LOW threshold
    writeRegister(INT1_TSH_ZH, 0x00); //Z HIGH threshold
    writeRegister(INT1_TSH_ZL, 0x60); //Z LOW threshold
    
    writeRegister(INT1_DURATION, 0x01); //Duration = 10ms
    
        
    setRef();           //Device must remain stationary while setting the gyro reference level
    
    //getGyroBias();
}

void checkGyro()
{
    int interrSource = readRegister(INT1_SRC);
    int compare = interrSource&xHigh;
    
    //X-Axis
    if(compare == 2)
    {
        //getGyroAvg();
        getGyroValues();
        pc.printf("X: %i\n", gyro[0]);     
        
        compare = interrSource&yHigh;
        //Y-Axis
        if(compare == 8)
        {
            pc.printf("Y: %i\n", gyro[1]); 
        }
        compare = interrSource&zHigh;
        //Z-Axis
        if(compare == 32)
        {
            pc.printf("Z: %i\n", gyro[2]);
        }
    }
    
    else
    {
        
        compare = interrSource&yHigh;
        //Y-Axis
        if(compare == 8)
        {
     
            //getGyroAvg();
            getGyroValues();   
            pc.printf("Y: %i\n", gyro[1]); 
            
            //Z-Axis
            compare = interrSource&zHigh;
            if(compare == 32)
            {
                pc.printf("Z: %i\n", gyro[2]);
            }
        
        }
        else
        { 
            //Z-Axis
            compare = interrSource&zHigh;
            if(compare == 32)
            {
                //getGyroAvg();
                getGyroValues();   
            
                pc.printf("Z: %i\n", gyro[2]);
            }
        }
    }
}