embox

1
/**
2
 * @file
3
 * @brief
4
 *
5
 * @date 06.02.2020
6
 * @author Alexander Kalmuk
7
 */
8
#include <unistd.h>
9
#include <util/log.h>
10

11
#include "l3g4200d.h"
12
#include "l3g4200d_transfer.h"
13

14
#define L3G4200D_WHO_AM_I     0xf
15
#define L3G4200D_CTRL_REG(i)  (0x20 + i)
16
#define L3G4200D_STATUS_REG   0x27
17
#define L3G4200D_OUT_X_L      0x28
18
#define L3G4200D_OUT_X_H      0x29
19
#define L3G4200D_OUT_Y_L      0x2A
20
#define L3G4200D_OUT_Y_H      0x2B
21
#define L3G4200D_OUT_Z_L      0x2C
22
#define L3G4200D_OUT_Z_H      0x2D
23

24
#define L3G4200D_WHO_AM_I_VALUE 0xd3
25

26
extern struct l3g4200d_dev l3g4200d_dev0;
27

28
int16_t l3g4200d_get_angular_rate_x(void) {
29
	uint8_t l, h;
30
	struct l3g4200d_dev *dev = &l3g4200d_dev0;
31
	l3g4200d_readb(dev, L3G4200D_OUT_X_L, &l);
32
	l3g4200d_readb(dev, L3G4200D_OUT_X_H, &h);
33
	return l | ((int16_t) h << 8);
34
}
35

36
int16_t l3g4200d_get_angular_rate_y(void) {
37
	uint8_t l, h;
38
	struct l3g4200d_dev *dev = &l3g4200d_dev0;
39
	l3g4200d_readb(dev, L3G4200D_OUT_Y_L, &l);
40
	l3g4200d_readb(dev, L3G4200D_OUT_Y_H, &h);
41
	return l | ((int16_t) h << 8);
42
}
43

44
int16_t l3g4200d_get_angular_rate_z(void) {
45
	uint8_t l, h;
46
	struct l3g4200d_dev *dev = &l3g4200d_dev0;
47
	l3g4200d_readb(dev, L3G4200D_OUT_Z_L, &l);
48
	l3g4200d_readb(dev, L3G4200D_OUT_Z_H, &h);
49
	return l | ((int16_t) h << 8);
50
}
51

52
int l3g4200d_init(void) {
53
	int i;
54
	uint8_t tmp;
55
	struct l3g4200d_dev *dev = &l3g4200d_dev0;
56

57
	if (l3g4200d_hw_init(dev) < 0) {
58
		log_error("L3G4200D hw init failed!");
59
		return -1;
60
	}
61

62
	if (l3g4200d_readb(dev, L3G4200D_WHO_AM_I, &tmp) < 0) {
63
		return -1;
64
	}
65
	log_info("WHO_AM_I = 0x%02x", tmp);
66
	if (tmp != L3G4200D_WHO_AM_I_VALUE) {
67
		log_error("L3G4200D Device ID mismatch! %2x", tmp);
68
		return -1;
69
	}
70

71
	tmp = 0x7; /* Enable X, Y, Z */
72
	tmp |= (1 << 3); /* Enable power */
73
	l3g4200d_writeb(dev, L3G4200D_CTRL_REG(0), tmp);
74

75
	for (i = 0; i < 5; i++) {
76
		l3g4200d_readb(dev, L3G4200D_CTRL_REG(i), &tmp);
77
		log_info("CTRL_REG%d = 0x%02x", i + 1, tmp);
78
	}
79
	l3g4200d_readb(dev, L3G4200D_STATUS_REG, &tmp);
80
	log_info("STATUS_REG = 0x%02x", tmp);
81

82
	return 0;
83
}
84