l3g4200d.c
3.65 KB
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
#/*
* Copyright (C) 2014 Freie Universität Berlin
*
* This file is subject to the terms and conditions of the GNU Lesser
* General Public License v2.1. See the file LICENSE in the top level
* directory for more details.
*/
/**
* @ingroup drivers_l3g4200d
* @{
*
* @file
* @brief Device driver implementation for the L3G4200D gyroscope
*
* @author Hauke Petersen <hauke.petersen@fu-berlin.de>
* @author Peter Kietzmann <peter.kietzmann@haw-hamburg.de>
*
* @}
*/
#include <stdint.h>
#include "l3g4200d.h"
#include "l3g4200d-regs.h"
#include "periph/i2c.h"
#include "periph/gpio.h"
#define ENABLE_DEBUG (0)
#include "debug.h"
#define I2C_SPEED I2C_SPEED_FAST
#define MAX_VAL 0x7fff
int l3g4200d_init(l3g4200d_t *dev, i2c_t i2c, uint8_t address,
gpio_t int1_pin, gpio_t int2_pin,
l3g4200d_mode_t mode, l3g4200d_scale_t scale)
{
uint8_t tmp;
/* write device descriptor */
dev->i2c = i2c;
dev->addr = address;
dev->int1 = int1_pin;
dev->int2 = int2_pin;
/* set scale */
switch (scale) {
case L3G4200D_SCALE_250DPS:
dev->scale = 250;
break;
case L3G4200D_SCALE_500DPS:
dev->scale = 500;
break;
case L3G4200D_SCALE_2000DPS:
dev->scale = 2000;
break;
default:
dev->scale = 500;
break;
}
/* acquire exclusive access to the bus. */
i2c_acquire(dev->i2c);
/* initialize the I2C bus */
if (i2c_init_master(i2c, I2C_SPEED) < 0) {
/* Release the bus for other threads. */
i2c_release(dev->i2c);
return -1;
}
/* configure CTRL_REG1 */
tmp = ((mode & 0xf) << L3G4200D_CTRL1_MODE_POS) | L3G4200D_CTRL1_ALLON;
if (i2c_write_reg(dev->i2c, dev->addr, L3G4200D_REG_CTRL1, tmp) != 1) {
i2c_release(dev->i2c);
return -1;
}
tmp = ((scale & 0x3) << L3G4200D_CTRL4_FS_POS) | L3G4200D_CTRL4_BDU;
if (i2c_write_reg(dev->i2c, dev->addr, L3G4200D_REG_CTRL4, tmp) != 1) {
i2c_release(dev->i2c);
return -1;
}
i2c_release(dev->i2c);
return 0;
}
int l3g4200d_read(const l3g4200d_t *dev, l3g4200d_data_t *data)
{
uint8_t tmp[6];
int16_t res;
i2c_acquire(dev->i2c);
/* get acceleration in x direction */
i2c_read_regs(dev->i2c, dev->addr, L3G4200D_REG_OUT_X_L | L3G4200D_AUTOINC, tmp, 6);
i2c_release(dev->i2c);
/* parse and normalize data into result vector */
res = (tmp[1] << 8) | tmp[0];
data->acc_x = (int16_t)((dev->scale * res) / MAX_VAL);
res = (tmp[3] << 8) | tmp[2];
data->acc_y = (int16_t)((dev->scale * res) / MAX_VAL);
res = (tmp[5] << 8) | tmp[4];
data->acc_z = (int16_t)((dev->scale * res) / MAX_VAL);
return 0;
}
int l3g4200d_enable(const l3g4200d_t *dev)
{
uint8_t tmp;
int res;
i2c_acquire(dev->i2c);
res = i2c_read_reg(dev->i2c, dev->addr, L3G4200D_REG_CTRL1, &tmp);
if (res < 1) {
i2c_release(dev->i2c);
return res;
}
tmp |= L3G4200D_CTRL1_PD;
if (i2c_write_reg(dev->i2c, dev->addr, L3G4200D_REG_CTRL1, tmp) != 1) {
i2c_release(dev->i2c);
return -1;
}
i2c_release(dev->i2c);
return 0;
}
int l3g4200d_disable(const l3g4200d_t *dev)
{
uint8_t tmp;
int res;
i2c_acquire(dev->i2c);
res = i2c_read_reg(dev->i2c, dev->addr, L3G4200D_REG_CTRL1, &tmp);
if (res < 1) {
i2c_release(dev->i2c);
return res;
}
tmp &= ~L3G4200D_CTRL1_PD;
if (i2c_write_reg(dev->i2c, dev->addr, L3G4200D_REG_CTRL1, tmp) != 1) {
i2c_release(dev->i2c);
return -1;
}
i2c_release(dev->i2c);
return 0;
}