Subversion Repositories Code-Repo

Rev

Rev 159 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
159 Kevin 1 
#include "defines.h"
2 
#include "sensor_gyro_L3G.h"
3 
#include "base_I2C.h"
4 
 
5 
static L3G_DATA *l3g_data_p;
6 
 
7 
void L3G_Init(L3G_DATA *data, char device, char sa0) {
8 
    l3g_data_p = data;
9 
    switch(device) {
10 
        case L3G4200D_DEVICE:
11 
            if (sa0 == L3G_SA0_LOW)
12 
                l3g_data_p->address = L3G4200D_ADDRESS_SA0_LOW;
13 
            else
14 
                l3g_data_p->address = L3G4200D_ADDRESS_SA0_HIGH;
15 
            break;
16 
        case L3GD20_DEVICE:
17 
            if (sa0 == L3G_SA0_LOW)
18 
                l3g_data_p->address = L3GD20_ADDRESS_SA0_LOW;
19 
            else
20 
                l3g_data_p->address = L3GD20_ADDRESS_SA0_HIGH;
21 
            break;
22 
        default:
23 
            break;
24 
    }
25 
}
26 
 
27 
void L3G_Begin() {
28 
    char buffer[2];
29 
    // Normal power mode, all axes enabled
30 
    buffer[0] = L3G_CTRL_REG1;
31 
    buffer[1] = 0x0F;
32 
    I2C_Master_Send(l3g_data_p->address, 2, buffer);
33 
    char result;
34 
    do {
35 
        result = I2C_Get_Status();
36 
    } while (!result);
37 
}
38 
 
39 
void L3G_Read(int* x, int* y, int* z) {
40 
    char msg = L3G_OUT_X_L | 0x80;
41 
    char buffer[6];
42 
    char result;
43 
 
44 
    I2C_Master_Restart(l3g_data_p->address, msg, 6);
45 
    do {
46 
        result = I2C_Get_Status();
47 
    } while (!result);
48 
    I2C_Read_Buffer(buffer);
49 
 
50 
    // 0 = x_l, 1 = x_h, 2 = y_l, ...
51 
    *x = buffer[1] << 8 | buffer[0];
52 
    *y = buffer[3] << 8 | buffer[2];
53 
    *z = buffer[5] << 8 | buffer[4];
54 
}