blob: 1d2ac884327a8476dcc87ef168d5bb7b5aaf5797 (
plain)
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
|
#ifndef L3GD20_H
#define L3GD20_H
#include <math.h>
#include <gpio/pin.h>
#include <spi/spi.h>
class L3GD20 {
private:
Pin& cs;
SPI_t& spi;
public:
float x, y, z;
L3GD20(Pin& cs_pin, SPI_t& spi_bus) : cs(cs_pin), spi(spi_bus) {}
void init() {
spi.reg.CR1 = (1 << 9) | (1 << 8) | (1 << 6) | (3 << 3) | (1 << 2) | (1 << 1) | (1 << 0); // SSM, SSI, SPE, 84/16MHz, MSTR, CPOL, CPHA
cs.off();
spi.transfer_byte(0x40 | 0x20);
spi.transfer_byte(0xff);
cs.on();
}
void update() {
int16_t raw_x, raw_y, raw_z;
spi.reg.CR1 = 0x37f;
cs.off();
spi.transfer_byte(0xc0 | 0x28);
raw_x = spi.transfer_byte();
raw_x |= spi.transfer_byte() << 8;
raw_y = spi.transfer_byte();
raw_y |= spi.transfer_byte() << 8;
raw_z = spi.transfer_byte();
raw_z |= spi.transfer_byte() << 8;
cs.on();
x = float(raw_x) * (2.0 * M_PI * 250.0 / 360.0 / 32768.0);
y = float(raw_y) * (2.0 * M_PI * 250.0 / 360.0 / 32768.0);
z = float(raw_z) * (2.0 * M_PI * 250.0 / 360.0 / 32768.0);
}
};
#endif
|
