-
Notifications
You must be signed in to change notification settings - Fork 19
/
l3g4200d.c
146 lines (123 loc) · 3.85 KB
/
l3g4200d.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
/*
l3g4200d lib 0x01
copyright (c) Davide Gironi, 2012
Released under GPLv3.
Please refer to LICENSE file for licensing information.
*/
#include <stdlib.h>
#include <avr/io.h>
#include <util/delay.h>
#include "l3g4200d.h"
//path to i2c fleury lib
#include L3G4200D_I2CFLEURYPATH
//offset variables
volatile double l3g4200d_offsetx = 0.0f;
volatile double l3g4200d_offsety = 0.0f;
volatile double l3g4200d_offsetz = 0.0f;
//reference temperature
int8_t l3g4200d_temperatureref = 0;
#if L3G4200D_CALIBRATED == 1 && L3G4200D_CALIBRATEDDOTEMPCOMP == 1
double l3g4200d_gtemp = 0; //temperature used for compensation
#endif
/*
* set reference temperature
*/
void l3g4200d_settemperatureref(void) {
i2c_start(L3G4200D_ADDR | I2C_WRITE);
i2c_write(L3G4200D_OUT_TEMP);
i2c_rep_start(L3G4200D_ADDR | I2C_READ);
uint8_t rawtemp = i2c_readNak();
i2c_stop();
l3g4200d_temperatureref = (int8_t) rawtemp;
#if L3G4200D_CALIBRATED == 1 && L3G4200D_CALIBRATEDDOTEMPCOMP == 1
l3g4200d_gtemp = (double)rawtemp;
#endif
}
/*
* get temperature variation
*/
int8_t l3g4200d_gettemperaturediff(void) {
i2c_start(L3G4200D_ADDR | I2C_WRITE);
i2c_write(L3G4200D_OUT_TEMP);
i2c_rep_start(L3G4200D_ADDR | I2C_READ);
uint8_t rawtemp = i2c_readNak();
i2c_stop();
return l3g4200d_temperatureref - (int8_t)rawtemp;
}
/*
* set offset variables
*/
void l3g4200d_setoffset(double offsetx, double offsety, double offsetz) {
l3g4200d_offsetx = offsetx;
l3g4200d_offsety = offsety;
l3g4200d_offsetz = offsetz;
}
/*
* get raw data
*/
void l3g4200d_getrawdata(int16_t *gxraw, int16_t *gyraw, int16_t *gzraw) {
uint8_t i = 0;
uint8_t buff[6];
i2c_start(L3G4200D_ADDR | I2C_WRITE);
i2c_write(L3G4200D_OUT_X_L | (1 << 7));
i2c_rep_start(L3G4200D_ADDR | I2C_READ);
for(i=0; i<6; i++) {
if(i==6-1)
buff[i] = i2c_readNak();
else
buff[i] = i2c_readAck();
}
i2c_stop();
*gxraw = ((buff[1] << 8) | buff[0]);
*gyraw = ((buff[3] << 8) | buff[2]);
*gzraw = ((buff[5] << 8) | buff[4]);
}
/*
* get converted data deg/sec
*/
void l3g4200d_getdata(double* gx, double* gy, double* gz) {
int16_t gxraw = 0;
int16_t gyraw = 0;
int16_t gzraw = 0;
l3g4200d_getrawdata(&gxraw, &gyraw, &gzraw);
#if L3G4200D_CALIBRATED == 1 && L3G4200D_CALIBRATEDDOTEMPCOMP == 1
l3g4200d_gtemp = l3g4200d_gtemp*0.95 + 0.05*l3g4200d_gettemperaturediff(); //filtered temperature compansation
#endif
#if L3G4200D_CALIBRATED == 1
#if L3G4200D_CALIBRATEDDOTEMPCOMP == 1
*gx = (gxraw - (double)((L3G4200D_TEMPCOMPX*l3g4200d_gtemp) + (double)l3g4200d_offsetx)) * (double)L3G4200D_GAINX;
*gy = (gyraw - (double)((L3G4200D_TEMPCOMPY*l3g4200d_gtemp) + (double)l3g4200d_offsety)) * (double)L3G4200D_GAINY;
*gz = (gzraw - (double)((L3G4200D_TEMPCOMPZ*l3g4200d_gtemp) + (double)l3g4200d_offsetz)) * (double)L3G4200D_GAINZ;
#else
*gx = (gxraw-(double)l3g4200d_offsetx) * (double)L3G4200D_GAINX;
*gy = (gyraw-(double)l3g4200d_offsety) * (double)L3G4200D_GAINY;
*gz = (gzraw-(double)l3g4200d_offsetz) * (double)L3G4200D_GAINZ;
#endif
#else
*gx = (gxraw-(double)l3g4200d_offsetx) * (double)L3G4200D_GAIN;
*gy = (gyraw-(double)l3g4200d_offsety) * (double)L3G4200D_GAIN;
*gz = (gzraw-(double)l3g4200d_offsetz) * (double)L3G4200D_GAIN;
#endif
}
/*
* init L3G4200D_
*/
void l3g4200d_init(void) {
//enable chip
i2c_start(L3G4200D_ADDR | I2C_WRITE);
i2c_write(L3G4200D_CTRL_REG1);
i2c_write(0x0F); //0x0F = 0b00001111, normal power mode, all axes enabled
i2c_stop();
//set range
i2c_start(L3G4200D_ADDR | I2C_WRITE);
i2c_write(L3G4200D_CTRL_REG4);
i2c_write(L3G4200D_RANGE<<4);
i2c_stop();
#if L3G4200D_CALIBRATED == 1
//init offset
l3g4200d_offsetx = L3G4200D_OFFSETX;
l3g4200d_offsety = L3G4200D_OFFSETY;
l3g4200d_offsetz = L3G4200D_OFFSETZ;
#endif
l3g4200d_settemperatureref();
}