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
|
#include "driver/mpu9250.h"
#include "arch.h"
#if defined(MULTIPASS_ARCH_HAS_I2C) && !defined(DRIVER_SOFTI2C)
#include "driver/i2c.h"
#else
#include "driver/soft_i2c.h"
#endif
void MPU9250::init()
{
// reset to default
txbuf[0] = 107;
txbuf[1] = 0x80;
i2c.xmit(address, 2, txbuf, 0, rxbuf);
arch.delay_ms(1);
// enable I2C passthrough to magnetometer
txbuf[0] = 55;
txbuf[1] = 1<<1;
i2c.xmit(address, 2, txbuf, 0, rxbuf);
// reset magnetometer
txbuf[0] = 0x0b;
txbuf[1] = 1;
i2c.xmit(0x0c, 2, txbuf, 0, rxbuf);
arch.delay_ms(1);
// put magnetometer into continuous measurement mode
txbuf[0] = 0x0a;
txbuf[1] = 2;
i2c.xmit(0x0c, 2, txbuf, 0, rxbuf);
}
void MPU9250::setGyroEnable(bool x, bool y, bool z)
{
txbuf[0] = 108;
i2c.xmit(address, 1, txbuf, 1, txbuf + 1); // not a typo
txbuf[1] &= 0x07;
if (!x) {
txbuf[1] |= 4;
}
if (!y) {
txbuf[1] |= 2;
}
if (!z) {
txbuf[1] |= 1;
}
i2c.xmit(address, 2, txbuf, 0, rxbuf);
}
void MPU9250::setAccelEnable(bool x, bool y, bool z)
{
txbuf[0] = 108;
i2c.xmit(address, 1, txbuf, 1, txbuf + 1); // not a typo
txbuf[1] &= 0xf8;
if (!x) {
txbuf[1] |= 1<<5;
}
if (!y) {
txbuf[1] |= 1<<4;
}
if (!z) {
txbuf[1] |= 1<<3;
}
i2c.xmit(address, 2, txbuf, 0, rxbuf);
}
void MPU9250::setPower(bool sleep, bool gyroStandby)
{
txbuf[0] = 107;
if (sleep) {
txbuf[1] = 1<<6;
}
else if (gyroStandby) {
txbuf[1] = 1<<4;
}
i2c.xmit(address, 2, txbuf, 0, rxbuf);
}
signed int MPU9250::getWordReg(unsigned char const regBase)
{
txbuf[0] = regBase;
i2c.xmit(address, 1, txbuf, 2, rxbuf);
return ((signed int)rxbuf[0] << 8) + rxbuf[1];
}
float MPU9250::getTemperature()
{
return (float)getWordReg(65) / 333.87 + 21;
}
void MPU9250::getMagnet(int *x, int *y, int *z)
{
txbuf[0] = 0x02;
i2c.xmit(0x0c, 0, txbuf, 8, rxbuf);
if ((rxbuf[0] & 0x01) && !(rxbuf[7] & 0x08)) {
*x = ((signed int)rxbuf[2] << 8) + rxbuf[1];
*y = ((signed int)rxbuf[4] << 8) + rxbuf[3];
*z = ((signed int)rxbuf[6] << 8) + rxbuf[5];
}
else {
*x = *y = *z = 0;
}
}
MPU9250 mpu9250;
|
