Skip to content

Commit f3f2d65

Browse files
ivoszzdeadprogram
ivoszz
authored and
deadprogram
committed
mpu6886: initial implementation
1 parent deca190 commit f3f2d65

File tree

3 files changed

+330
-0
lines changed

3 files changed

+330
-0
lines changed

‎examples/mpu6886/main.go‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
// Connects to an MPU6886 I2C accelerometer/gyroscope.
2+
package main
3+
4+
import (
5+
"machine"
6+
"time"
7+
8+
"tinygo.org/x/drivers/mpu6886"
9+
)
10+
11+
func main() {
12+
machine.I2C0.Configure(machine.I2CConfig{})
13+
14+
accel := mpu6886.New(machine.I2C0)
15+
accel.Configure(mpu6886.Config{})
16+
17+
for {
18+
x, y, z, _ := accel.ReadAcceleration()
19+
println(x, y, z)
20+
time.Sleep(time.Millisecond * 100)
21+
}
22+
}

‎mpu6886/mpu6886.go‎

Lines changed: 192 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,192 @@
1+
// Package mpu6886 provides a driver for the MPU6886 accelerometer and gyroscope
2+
// made by InvenSense.
3+
//
4+
// Datasheet:
5+
// https://m5stack.oss-cn-shenzhen.aliyuncs.com/resource/docs/datasheet/core/MPU-6886-000193%2Bv1.1_GHIC_en.pdf
6+
package mpu6886 // import "tinygo.org/x/drivers/mpu6886"
7+
8+
import (
9+
"errors"
10+
"time"
11+
12+
"tinygo.org/x/drivers"
13+
)
14+
15+
const WhoAmI = 0x19
16+
17+
var errNotConnected = errors.New("mpu6886: failed to communicate with a sensor")
18+
19+
// Device wraps an I2C connection to a MPU6886 device.
20+
type Device struct {
21+
bus drivers.I2C
22+
Address uint16
23+
aRange uint8
24+
gRange uint8
25+
}
26+
27+
// Config contains settings for filtering, sampling, and modes of operation
28+
type Config struct {
29+
AccelRange uint8
30+
GyroRange uint8
31+
}
32+
33+
// New creates a new MPU6886 connection. The I2C bus must already be
34+
// configured.
35+
//
36+
// This function only creates the Device object, it does not touch the device.
37+
func New(bus drivers.I2C) *Device {
38+
return &Device{bus: bus, Address: DefaultAddress}
39+
}
40+
41+
// Connected returns whether a MPU6886 has been found.
42+
// It does a "who am I" request and checks the response.
43+
func (d *Device) Connected() bool {
44+
data := []byte{0}
45+
d.bus.Tx(d.Address, []byte{WHO_AM_I}, data)
46+
return data[0] == WhoAmI
47+
}
48+
49+
// Configure sets up the device for communication.
50+
func (d *Device) Configure(config Config) (err error) {
51+
if config.AccelRange < 4 {
52+
d.aRange = config.AccelRange
53+
}
54+
if config.GyroRange < 4 {
55+
d.gRange = config.GyroRange
56+
}
57+
58+
if !d.Connected() {
59+
return errNotConnected
60+
}
61+
// This initialization sequence is borrowed from Arduino M5Stack library
62+
// Zero register
63+
if err = d.bus.Tx(d.Address, []byte{PWR_MGMT_1, 0x00}, nil); err != nil {
64+
return
65+
}
66+
time.Sleep(10 * time.Millisecond)
67+
// Set DEVICE_RESET bit
68+
if err = d.bus.Tx(d.Address, []byte{PWR_MGMT_1, 0x80}, nil); err != nil {
69+
return
70+
}
71+
time.Sleep(10 * time.Millisecond)
72+
// Set CLKSEL to 1 - Auto selects the best available clock source
73+
if err = d.bus.Tx(d.Address, []byte{PWR_MGMT_1, 0x01}, nil); err != nil {
74+
return
75+
}
76+
time.Sleep(10 * time.Millisecond)
77+
// Set ACCEL_FS_SEL
78+
if err = d.bus.Tx(d.Address, []byte{ACCEL_CONFIG, d.aRange << 3}, nil); err != nil {
79+
return
80+
}
81+
time.Sleep(time.Millisecond)
82+
// Set FS_SEL
83+
if err = d.bus.Tx(d.Address, []byte{GYRO_CONFIG, d.gRange << 3}, nil); err != nil {
84+
return
85+
}
86+
time.Sleep(time.Millisecond)
87+
// default: 0x80, set DLPF_CFG to 001 (Low Pass Filter)
88+
if err = d.bus.Tx(d.Address, []byte{CONFIG, 0x01}, nil); err != nil {
89+
return
90+
}
91+
time.Sleep(time.Millisecond)
92+
// Set sample rate divisor, sample rate is ~ 170 Hz
93+
if err = d.bus.Tx(d.Address, []byte{SMPLRT_DIV, 0x05}, nil); err != nil {
94+
return
95+
}
96+
time.Sleep(time.Millisecond)
97+
// Set Interupt pin
98+
if err = d.bus.Tx(d.Address, []byte{INT_PIN_CFG, 0x22}, nil); err != nil {
99+
return
100+
}
101+
time.Sleep(time.Millisecond)
102+
// Enable DATA_RDY_INT_EN
103+
if err = d.bus.Tx(d.Address, []byte{INT_ENABLE, 0x01}, nil); err != nil {
104+
return
105+
}
106+
time.Sleep(100 * time.Millisecond)
107+
return nil
108+
}
109+
110+
// ReadTemperature returns the temperature in Celsius millidegrees (°C/1000).
111+
func (d *Device) ReadTemperature() (t int32, err error) {
112+
data := make([]byte, 2)
113+
if err = d.bus.Tx(d.Address, []byte{TEMP_OUT_H}, data); err != nil {
114+
return
115+
}
116+
rawTemperature := int32(int16((uint16(data[0]) << 8) | uint16(data[1])))
117+
// The formula to convert to degrre of Celsius is
118+
// T_C = T_raw / 326.8 + 25.0
119+
// This formula should not overflow
120+
t = rawTemperature*10000/3268 + 25000
121+
return
122+
}
123+
124+
// ReadAcceleration reads the current acceleration from the device and returns
125+
// it in µg (micro-gravity). When one of the axes is pointing straight to Earth
126+
// and the sensor is not moving the returned value will be around 1000000 or
127+
// -1000000.
128+
func (d *Device) ReadAcceleration() (x int32, y int32, z int32, err error) {
129+
data := make([]byte, 6)
130+
if err = d.bus.Tx(d.Address, []byte{ACCEL_XOUT_H}, data); err != nil {
131+
return
132+
}
133+
// Now do two things:
134+
// 1. merge the two values to a 16-bit number (and cast to a 32-bit integer)
135+
// 2. scale the value to bring it in the -1000000..1000000 range.
136+
// This is done with a trick. What we do here is essentially multiply by
137+
// 1000000 and divide by 16384 to get the original scale, but to avoid
138+
// overflow we do it at 1/64 of the value:
139+
// 1000000 / 64 = 15625
140+
// 16384 / 64 = 256
141+
divider := int32(1)
142+
switch d.aRange {
143+
case AFS_RANGE_2_G:
144+
divider = 256
145+
case AFS_RANGE_4_G:
146+
divider = 128
147+
case AFS_RANGE_8_G:
148+
divider = 64
149+
case AFS_RANGE_16_G:
150+
divider = 32
151+
}
152+
x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / divider
153+
y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / divider
154+
z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / divider
155+
return
156+
}
157+
158+
// ReadRotation reads the current rotation from the device and returns it in
159+
// µ°/s (micro-degrees/sec). This means that if you were to do a complete
160+
// rotation along one axis and while doing so integrate all values over time,
161+
// you would get a value close to 360000000.
162+
func (d *Device) ReadRotation() (x int32, y int32, z int32, err error) {
163+
data := make([]byte, 6)
164+
if err = d.bus.Tx(d.Address, []byte{GYRO_XOUT_H}, data); err != nil {
165+
return
166+
}
167+
// First the value is converted from a pair of bytes to a signed 16-bit
168+
// value and then to a signed 32-bit value to avoid integer overflow.
169+
// Then the value is scaled to µ°/s (micro-degrees per second).
170+
// This is done in the following steps:
171+
// 1. Multiply by 250 * 1000_000
172+
// 2. Divide by 32768
173+
// The following calculation (x * 15625 / 2048 * 1000) is essentially the
174+
// same but avoids overflow. First both operations are divided by 16 leading
175+
// to multiply by 15625000 and divide by 2048, and then part of the multiply
176+
// is done after the divide instead of before.
177+
divider := int32(1)
178+
switch d.gRange {
179+
case GFS_RANGE_250:
180+
divider = 2048
181+
case GFS_RANGE_500:
182+
divider = 1024
183+
case GFS_RANGE_1000:
184+
divider = 512
185+
case GFS_RANGE_2000:
186+
divider = 256
187+
}
188+
x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / divider * 1000
189+
y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / divider * 1000
190+
z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / divider * 1000
191+
return
192+
}

‎mpu6886/registers.go‎

Lines changed: 116 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
1+
package mpu6886
2+
3+
// Constants/addresses used for I2C.
4+
5+
// The I2C address which this device listens to.
6+
const (
7+
DefaultAddress = 0x68
8+
SecondaryAddress = 0x69
9+
)
10+
11+
// Registers. Names, addresses and comments copied from the datasheet.
12+
const (
13+
XG_OFFS_TC_H = 0x04
14+
XG_OFFS_TC_L = 0x05
15+
YG_OFFS_TC_H = 0x07
16+
YG_OFFS_TC_L = 0x08
17+
ZG_OFFS_TC_H = 0x0A
18+
ZG_OFFS_TC_L = 0x0B
19+
20+
// Self test registers
21+
SELF_TEST_X_ACCEL = 0x0D
22+
SELF_TEST_Y_ACCEL = 0x0E
23+
SELF_TEST_Z_ACCEL = 0x0F
24+
25+
XG_OFFS_USRH = 0x13
26+
XG_OFFS_USRL = 0x14
27+
YG_OFFS_USRH = 0x15
28+
YG_OFFS_USRL = 0x16
29+
ZG_OFFS_USRH = 0x17
30+
ZG_OFFS_USRL = 0x18
31+
32+
SMPLRT_DIV = 0x19
33+
CONFIG = 0x1A
34+
GYRO_CONFIG = 0x1B
35+
ACCEL_CONFIG = 0x1C
36+
ACCEL_CONFIG_2 = 0x1D
37+
LP_MODE_CFG = 0x1E
38+
ACCEL_WOM_X_THR = 0x20
39+
ACCEL_WOM_Y_THR = 0x21
40+
ACCEL_WOM_Z_THR = 0x22
41+
FIFO_EN = 0x23
42+
FSYNC_INT = 0x36
43+
44+
// Interrupt configuration
45+
INT_PIN_CFG = 0x37
46+
INT_ENABLE = 0x38
47+
FIFO_WM_INT_STATUS = 0x39
48+
INT_STATUS = 0x3A
49+
50+
// Accelerometer measurements
51+
ACCEL_XOUT_H = 0x3B
52+
ACCEL_XOUT_L = 0x3C
53+
ACCEL_YOUT_H = 0x3D
54+
ACCEL_YOUT_L = 0x3E
55+
ACCEL_ZOUT_H = 0x3F
56+
ACCEL_ZOUT_L = 0x40
57+
58+
// Temperature measurement
59+
TEMP_OUT_H = 0x41
60+
TEMP_OUT_L = 0x42
61+
62+
// Gyroscope measurements
63+
GYRO_XOUT_H = 0x43
64+
GYRO_XOUT_L = 0x44
65+
GYRO_YOUT_H = 0x45
66+
GYRO_YOUT_L = 0x46
67+
GYRO_ZOUT_H = 0x47
68+
GYRO_ZOUT_L = 0x48
69+
70+
SELF_TEST_X_GYRO = 0x50
71+
SELF_TEST_Y_GYRO = 0x51
72+
SELF_TEST_Z_GYRO = 0x52
73+
74+
E_ID0 = 0x53
75+
E_ID1 = 0x54
76+
E_ID2 = 0x55
77+
E_ID3 = 0x56
78+
E_ID4 = 0x57
79+
E_ID5 = 0x58
80+
E_ID6 = 0x59
81+
82+
FIFO_WM_TH1 = 0x60
83+
FIFO_WM_TH2 = 0x61
84+
SIGNAL_PATH_RESET = 0x68
85+
ACCEL_INTEL_CTRL = 0x69
86+
USER_CTRL = 0x6A
87+
PWR_MGMT_1 = 0x6B
88+
PWR_MGMT_2 = 0x6C
89+
I2C_IF = 0x70
90+
FIFO_COUNTH = 0x72
91+
FIFO_COUNTL = 0x73
92+
FIFO_R_W = 0x74
93+
WHO_AM_I = 0x75
94+
95+
XA_OFFSET_H = 0x77
96+
XA_OFFSET_L = 0x78
97+
YA_OFFSET_H = 0x7A
98+
YA_OFFSET_L = 0x7B
99+
ZA_OFFSET_H = 0x7D
100+
ZA_OFFSET_L = 0x7E
101+
)
102+
103+
// Accelerometer and gyroscope ranges
104+
const (
105+
AFS_RANGE_2_G = iota
106+
AFS_RANGE_4_G
107+
AFS_RANGE_8_G
108+
AFS_RANGE_16_G
109+
)
110+
111+
const (
112+
GFS_RANGE_250 = iota
113+
GFS_RANGE_500
114+
GFS_RANGE_1000
115+
GFS_RANGE_2000
116+
)

0 commit comments

Comments
 (0)