| 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 |
}
|