File content as of revision 1:6909d797972f:

#include "GY80.h"

GY80::GY80(PinName sda, PinName scl): Wire( sda, scl)
{
    Wire.frequency(I2C_FREQ);
    Accel_Init();
    Gyro_Init();
    Magn_Init();
}

void GY80::Accel_Init()
{    
    byte data[2];
    data[0] = 0x2D; // Power register
    data[1] = 0x08; //Measurement mode
    Wire.write(ACCEL_ADDRESS, data, 2);
    wait_ms(1);

    data[0] = 0x31; // Data format register
    data[1] = 0x08; //Set to full resolution
    Wire.write(ACCEL_ADDRESS, data, 2);
    wait_ms(1);

    // Because our main loop runs at 50Hz we adjust the output data rate to 50Hz (25Hz bandwidth)
    data[0] = 0x2C; // Rate
    data[1] = 0x0A; //Set to 50Hz, normal operation, 0x0A 100hz 
    Wire.write(ACCEL_ADDRESS, data, 2);
    wait_ms(1);
}

void GY80::Gyro_Init()
{
    byte data[2];

    data[0] = 0x20; //L3G4200D_CTRL_REG1
    data[1] = 0x0; // normal power mode, all axes enable, 100Hz
    Wire.write(GYRO_ADDRESS, data, 2);
    wait_ms(1);


    data[0] = 0x23; // L3G4200D_CTRL_REG4
    data[1] = 0x20; //2000 dps full scale 
    Wire.write(GYRO_ADDRESS, data, 2);
    wait_ms(1);

    data[0] = 0x21; // L3G4200D_CTRL_REG2
    data[1] = 0x20; //High pass cutoff freq
    Wire.write(GYRO_ADDRESS, data, 2);
    wait_ms(1);  

    data[0] = 0x24; // L3G4200D_CTRL_REG5
    data[1] = 0x02; //Low Pass Filter
    Wire.write(GYRO_ADDRESS, data, 2);
    wait_ms(1);
}

void GY80::Magn_Init()
{   
    byte data[2];
    data[0] = 0x02;
    data[1] = 0x00; // Set continuous mode (default 10Hz)
    Wire.write(MAGN_ADDRESS, data, 2);
    wait_ms(1);

    data[0] = 0x00;
    data[1] = 0x18;
    Wire.write(MAGN_ADDRESS, data, 2);
    wait_ms(1);
}

void GY80::Read_Accel(float* accel_v)
{
    byte buff[6];
    buff[0] = 0x32; // Send address to read from
    Wire.write(ACCEL_ADDRESS, buff, 1);

    if (Wire.read(ACCEL_ADDRESS, buff,6) == 0)  // All bytes received?
    {
        Convert(accel_v, buff);
    }
    // Why aren't we scaling accelerometer? I think the DCM paper talks a little about this... ??
    accel_v[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
    accel_v[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
    accel_v[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
}


void GY80::Read_Gyro(float* gyro_v)
{
    byte buff[6];

    buff[0] = 0xA8; // 0x28 | (1 << 7) Send address to read from 
    Wire.write(GYRO_ADDRESS, buff, 1);
    // Request 6 bytes
    if (Wire.read(GYRO_ADDRESS, buff,6) == 0)  // All bytes received?
    {
        Convert(gyro_v, buff);
    }
    gyro_v[0] = DEG2RAD((gyro[0] - GYRO_X_OFFSET) * GYRO_GAIN_X); 
    gyro_v[1] = DEG2RAD((gyro[1] - GYRO_Y_OFFSET) * GYRO_GAIN_Y);
    gyro_v[2] = DEG2RAD((gyro[2] - GYRO_Z_OFFSET) * GYRO_GAIN_Z);
}

void GY80::Read_Magn(float* magn_v)
{
    byte buff[6];

    buff[0] = 0x03; // Send address to read from
    Wire.write(MAGN_ADDRESS, buff, 1);

    // Request 6 bytes
    if (Wire.read(MAGN_ADDRESS, buff,6) == 0)  // All bytes received?
    {
        Convert(magn_v, buff);
    }
    magn_v[0] = (mag[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
    magn_v[1] = (mag[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE;
    magn_v[2] = (mag[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE;
}

void GY80::Convert(float* value, byte* raw)
{
    value[0] = ((signed short) raw[1] << 8) | raw[0];  // Y axis (internal sensor x axis)
    value[1] = ((signed short) raw[3] << 8) | raw[2];  // X axis (internal sensor y axis)
    value[2] = ((signed short) raw[5] << 8) | raw[4];  // Z axis (internal sensor z axis)
}