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
|
#ifndef MPU6000_H
#define MPU6000_H
#include <spi/spi.h>
#include <gpio/gpio.h>
#include <os/time.h>
class MPU6000 {
private:
Pin cs;
SPI_t& spi;
uint8_t raw_data[14];
void read_register(uint8_t reg, uint8_t len, uint8_t *data) {
cs.off();
spi.transfer_byte(reg | (1 << 7)); // Read bit
while(len--) {
*data++ = spi.transfer_byte();
}
cs.on();
}
void write_register(uint8_t reg, uint8_t data) {
cs.off();
spi.transfer_byte(reg);
spi.transfer_byte(data);
cs.on();
}
public:
MPU6000(Pin cs_pin, SPI_t& spi_bus) : cs(cs_pin), spi(spi_bus) {}
float gyro [3] = {0,0,0}; //X Y Z
float accel [3] = {0,0,0}; //X Y Z
void init() {
spi.reg.CR1 = (1 << 9) | (1 << 8) | (1 << 6) | (2 << 3) | (1 << 2) | (1 << 1) | (1 << 0); // SSM On | SSI 1 | SPE On | BR /8 | CPOL CK 1 Idle | CPHA Second clock data capture
spi.reg.CR2 = (1 << 12) | ( 7 << 8); // FRXTH RXNE if FIFO >= 8bit | DS 8-bit
write_register(0x6B, (1 << 7)); // Device Reset
Time::sleep(150);
uint8_t who_am_i = 0x00;
while (who_am_i != 0x68) {
read_register(0x75, 1, &who_am_i); //WHO_AM_I register
}
write_register(0x19, 4); // SAMPLE DIV 4
Time::sleep(1);
write_register(0x1A, (3 << 0)); // CONFIG - DLPF_CFG 42hz
Time::sleep(1);
write_register(0x1B, (3 << 3)); // GYRO_CFG, 2000 deg/s
Time::sleep(1);
write_register(0x1C, (3 << 3)); // ACCEL_CFG, +-16g
Time::sleep(1);
write_register(0x6A, (1 << 4)); // USER_CTRL - I2C Off
Time::sleep(1);
write_register(0x6B, (3 << 0)); // PWR_MGMT_1 - CLKSEL PLL_Z
Time::sleep(1);
write_register(0x6C, 0); // PWR_MGMT_2
Time::sleep(150);
}
void update() {
int16_t temp;
spi.reg.CR1 = (1 << 9) | (1 << 8) | (1 << 6) | (2 << 3) | (1 << 2) | (1 << 1) | (1 << 0); // SSM On | SSI 1 | SPE On | BR /8 | CPOL CK 1 Idle | CPHA Second clock data capture
spi.reg.CR2 = (1 << 12) | ( 7 << 8); // FRXTH RXNE if FIFO >= 8bit | DS 8-bit
read_register(0x3B, 14, raw_data); // Accel 0-5, Temp 6-7, Gyro 8-13
temp = raw_data[0] << 8;
temp |= raw_data[1];
accel[0] = float(temp) * 2 / 32768.0f;
temp = raw_data[2] << 8;
temp |= raw_data[3];
accel[1] = float(temp) * 2 / 32768.0f;
temp = raw_data[4] << 8;
temp |= raw_data[5];
accel[2] = float(temp) * 2 / 32768.0f;
temp = raw_data[8] << 8;
temp |= raw_data[9];
gyro[0] = float(temp) * 2000 / 32768.0f;
temp = raw_data[10] << 8;
temp |= raw_data[11];
gyro[1] = float(temp) * 2000 / 32768.0f;
temp = raw_data[12] << 8;
temp |= raw_data[13];
gyro[2] = float(temp) * 2000 / 32768.0f;
}
};
#endif
|
