-
Notifications
You must be signed in to change notification settings - Fork 357
/
mpu9250.go
98 lines (86 loc) · 2.35 KB
/
mpu9250.go
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
// Package sensors provides a stratux interface to sensors used for AHRS calculations.
package sensors
import (
"../goflying/mpu9250"
"github.com/kidoman/embd"
)
const (
mpu9250GyroRange = 250 // mpu9250GyroRange is the default range to use for the Gyro.
mpu9250AccelRange = 4 // mpu9250AccelRange is the default range to use for the Accel.
mpu9250UpdateFreq = 50 // mpu9250UpdateFreq is the rate at which to update the sensor values.
)
// MPU9250 represents an InvenSense MPU9250 attached to the I2C bus and satisfies
// the IMUReader interface.
type MPU9250 struct {
mpu *mpu9250.MPU9250
}
// NewMPU9250 returns an instance of the MPU9250 IMUReader, connected to an
// MPU9250 attached on the I2C bus with either valid address.
func NewMPU9250(i2cbus *embd.I2CBus) (*MPU9250, error) {
var (
m MPU9250
mpu *mpu9250.MPU9250
err error
)
mpu, err = mpu9250.NewMPU9250(i2cbus, mpu9250GyroRange, mpu9250AccelRange, mpu9250UpdateFreq, true, false)
if err != nil {
return nil, err
}
// Set Gyro (Accel) LPFs to 20 (21) Hz to filter out prop/glareshield vibrations above 1200 (1260) RPM
mpu.SetGyroLPF(21)
mpu.SetAccelLPF(21)
m.mpu = mpu
return &m, nil
}
// Read returns the average (since last reading) time, Gyro X-Y-Z, Accel X-Y-Z, Mag X-Y-Z,
// error reading Gyro/Accel, and error reading Mag.
func (m *MPU9250) Read() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, GAError, MAGError error) {
var (
data *mpu9250.MPUData
i int8
)
data = new(mpu9250.MPUData)
for data.N == 0 && i < 5 {
data = <-m.mpu.CAvg
T = data.T.UnixNano()
G1 = data.G1
G2 = data.G2
G3 = data.G3
A1 = data.A1
A2 = data.A2
A3 = data.A3
M1 = data.M1
M2 = data.M2
M3 = data.M3
GAError = data.GAError
MAGError = data.MagError
i++
}
return
}
// ReadOne returns the most recent time, Gyro X-Y-Z, Accel X-Y-Z, Mag X-Y-Z,
// error reading Gyro/Accel, and error reading Mag.
func (m *MPU9250) ReadOne() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, GAError, MAGError error) {
var (
data *mpu9250.MPUData
)
data = new(mpu9250.MPUData)
data = <-m.mpu.C
T = data.T.UnixNano()
G1 = data.G1
G2 = data.G2
G3 = data.G3
A1 = data.A1
A2 = data.A2
A3 = data.A3
M1 = data.M1
M2 = data.M2
M3 = data.M3
GAError = data.GAError
MAGError = data.MagError
return
}
// Close stops reading the MPU.
func (m *MPU9250) Close() {
m.mpu.CloseMPU()
}