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_accel_LSM303.h"
3 
#include "base_I2C.h"
4 
 
5 
static LSM303_DATA *lsm303_data_p;
6 
 
7 
void LSM303_Init(LSM303_DATA* data, char device, char sa0) {
8 
    lsm303_data_p = data;
9 
    lsm303_data_p->device = device;
10 
    switch (device) {
11 
        case LSM303DLH_DEVICE:
12 
        case LSM303DLM_DEVICE:
13 
            if (sa0 == LSM303_SA0_A_LOW)
14 
                lsm303_data_p->acc_address = ACC_ADDRESS_SA0_A_LOW;
15 
            else
16 
                lsm303_data_p->acc_address = ACC_ADDRESS_SA0_A_HIGH;
17 
            break;
18 
        case LSM303DLHC_DEVICE:
19 
            lsm303_data_p->acc_address = ACC_ADDRESS_SA0_A_HIGH;
20 
            break;
21 
        default:
22 
            break;
23 
    }
24 
}
25 
 
26 
void LSM303_Begin() {
27 
    // Enable Accelerometer
28 
    // 0x27 = 0b00100111
29 
    // Normal power mode, all axes enabled
30 
    LSM303_Write_A_Reg(LSM303_CTRL_REG1_A, 0x27);
31 
 
32 
    // Enable Magnetometer
33 
    // 0x00 = 0b00000000
34 
    // Continuous conversion mode
35 
    LSM303_Write_M_Reg(LSM303_MR_REG_M, 0x00);
36 
}
37 
 
38 
void LSM303_Write_A_Reg(char reg, char value) {
39 
    char buffer[2];
40 
    buffer[0] = reg;
41 
    buffer[1] = value;
42 
    I2C_Master_Send(lsm303_data_p->acc_address, 2, buffer);
43 
    char result;
44 
    do {
45 
        result = I2C_Get_Status();
46 
    } while (!result);
47 
}
48 
 
49 
void LSM303_Write_M_Reg(char reg, char value) {
50 
    char buffer[2];
51 
    buffer[0] = reg;
52 
    buffer[1] = value;
53 
    I2C_Master_Send(lsm303_data_p->mag_address, 2, buffer);
54 
    char result;
55 
    do {
56 
        result = I2C_Get_Status();
57 
    } while (!result);
58 
}
59 
 
60 
void LSM303_Set_Mag_Gain(enum magGain value) {
61 
    char buffer[2];
62 
    buffer[0] = LSM303_CRB_REG_M;
63 
    buffer[1] = (char)value;
64 
    I2C_Master_Send(lsm303_data_p->mag_address, 2, buffer);
65 
    char result;
66 
    do {
67 
        result = I2C_Get_Status();
68 
    } while (!result);
69 
}
70 
 
71 
void LSM303_Read_Acc(int* x, int* y, int* z) {
72 
    char buffer[6];
73 
    char value = LSM303_OUT_X_L_A | 0x80;
74 
    I2C_Master_Restart(lsm303_data_p->acc_address, value, 6);
75 
    char result;
76 
    do {
77 
        result = I2C_Get_Status();
78 
    } while (!result);
79 
    I2C_Read_Buffer(buffer);
80 
 
81 
    // 0 = x_l, 1 = x_h, 2 = y_l, ...
82 
    *x = buffer[1] << 8 | buffer[0];
83 
    *y = buffer[3] << 8 | buffer[2];
84 
    *z = buffer[5] << 8 | buffer[4];
85 
}
86 
 
87 
void LSM303_Read_Mag(int* x, int* y, int* z) {
88 
    char buffer[6];
89 
    char value = LSM303_OUT_X_H_M;
90 
    I2C_Master_Restart(lsm303_data_p->mag_address, value, 6);
91 
    char result;
92 
    do {
93 
        result = I2C_Get_Status();
94 
    } while (!result);
95 
    I2C_Read_Buffer(buffer);
96 
 
97 
    *x = buffer[0] << 8 | buffer[1];
98 
 
99 
    if (lsm303_data_p->device == LSM303DLH_DEVICE) {
100 
        // DLH: register address for Y comes before Z
101 
        *y = buffer[2] << 8 | buffer[3];
102 
        *z = buffer[4] << 8 | buffer[5];
103 
    } else {
104 
        // DLM, DLHC: register address for Z comes before Y
105 
        *z = buffer[2] << 8 | buffer[3];
106 
        *y = buffer[4] << 8 | buffer[5];
107 
    }
108 
}