Subversion Repositories Code-Repo

#include "defines.h"
#include "sensor_gyro_L3G.h"
#include "base_I2C.h"

static L3G_DATA *l3g_data_p;

void L3G_Init(L3G_DATA *data, char device, char sa0) {
    l3g_data_p = data;
    switch(device) {
        case L3G4200D_DEVICE:
            if (sa0 == L3G_SA0_LOW)
                l3g_data_p->address = L3G4200D_ADDRESS_SA0_LOW;
            else
                l3g_data_p->address = L3G4200D_ADDRESS_SA0_HIGH;
            break;
        case L3GD20_DEVICE:
            if (sa0 == L3G_SA0_LOW)
                l3g_data_p->address = L3GD20_ADDRESS_SA0_LOW;
            else
                l3g_data_p->address = L3GD20_ADDRESS_SA0_HIGH;
            break;
        default:
            break;
    }
}

void L3G_Begin() {
    char buffer[2];
    // Normal power mode, all axes enabled
    buffer[0] = L3G_CTRL_REG1;
    buffer[1] = 0x0F;
    I2C_Master_Send(l3g_data_p->address, 2, buffer);
    char result;
    do {
        result = I2C_Get_Status();
    } while (!result);
}

void L3G_Read(int* x, int* y, int* z) {
    char msg = L3G_OUT_X_L | 0x80;
    char buffer[6];
    char result;

    I2C_Master_Restart(l3g_data_p->address, msg, 6);
    do {
        result = I2C_Get_Status();
    } while (!result);
    I2C_Read_Buffer(buffer);

    // 0 = x_l, 1 = x_h, 2 = y_l, ...
    *x = buffer[1] << 8 | buffer[0];
    *y = buffer[3] << 8 | buffer[2];
    *z = buffer[5] << 8 | buffer[4];
}