1
- package mpu6050
1
+ // Package mpu6050 provides a driver for the MPU6050 accelerometer and gyroscope
2
+ // made by InvenSense.
3
+ //
4
+ // Datasheets:
5
+ // https://store.invensense.com/datasheets/invensense/MPU-6050_DataSheet_V3%204.pdf
6
+ // https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
7
+ package mpu6050 // import "tinygo.org/x/drivers/mpu6050"
2
8
3
9
import (
4
10
"encoding/binary"
@@ -9,11 +15,19 @@ import (
9
15
10
16
const DefaultAddress = 0x68
11
17
18
+ // RangeAccel defines the range of the accelerometer.
19
+ // Allowed values are 2, 4, 8 and 16 with the unit g (gravity).
20
+ type RangeAccel uint8
21
+
22
+ // RangeGyro defines the range of the gyroscope.
23
+ // Allowed values are 250, 500, 1000 and 2000 with the unit °/s (degree per second).
24
+ type RangeGyro uint8
25
+
12
26
type Config struct {
13
27
// Use ACCEL_RANGE_2 through ACCEL_RANGE_16.
14
- AccRange byte
28
+ AccelRange RangeAccel
15
29
// Use GYRO_RANGE_250 through GYRO_RANGE_2000
16
- GyroRange byte
30
+ GyroRange RangeGyro
17
31
sampleRatio byte // TODO(soypat): expose these as configurable.
18
32
clkSel byte
19
33
}
@@ -23,9 +37,12 @@ type Device struct {
23
37
conn drivers.I2C
24
38
aRange int32 //Gyroscope FSR acording to SetAccelRange input
25
39
gRange int32 //Gyroscope FSR acording to SetGyroRange input
26
- // RawData contains the accelerometer, gyroscope and temperature RawData read
27
- // in the last call via the Update method.
28
- RawData [14 ]byte
40
+ // data contains the accelerometer, gyroscope and temperature data read
41
+ // in the last call via the Update method. The data is stored as seven 16bit unsigned
42
+ // integers in big endian format:
43
+ //
44
+ // | ax | ay | az | temp | gx | gy | gz |
45
+ data [14 ]byte
29
46
address byte
30
47
}
31
48
@@ -44,67 +61,72 @@ func New(bus drivers.I2C, addr uint16) *Device {
44
61
// and wakes up the peripheral.
45
62
func (p * Device ) Configure (data Config ) (err error ) {
46
63
if err = p .Sleep (false ); err != nil {
47
- return errors . New ( "set sleep: " + err . Error ())
64
+ return err
48
65
}
49
- if err = p .SetClockSource (data .clkSel ); err != nil {
50
- return errors . New ( "set clksrc: " + err . Error ())
66
+ if err = p .setClockSource (data .clkSel ); err != nil {
67
+ return err
51
68
}
52
- if err = p .SetSampleRate (data .sampleRatio ); err != nil {
53
- return errors . New ( "set sampleratio: " + err . Error ())
69
+ if err = p .setSampleRate (data .sampleRatio ); err != nil {
70
+ return err
54
71
}
55
- if err = p .SetRangeGyro (data .GyroRange ); err != nil {
56
- return errors . New ( "set gyrorange: " + err . Error ())
72
+ if err = p .setRangeGyro (data .GyroRange ); err != nil {
73
+ return err
57
74
}
58
- if err = p .SetRangeAccel (data .AccRange ); err != nil {
59
- return errors . New ( "set accelrange: " + err . Error ())
75
+ if err = p .setRangeAccel (data .AccelRange ); err != nil {
76
+ return err
60
77
}
61
78
return nil
62
79
}
63
80
81
+ func (d Device ) Connected () bool {
82
+ data := []byte {0 }
83
+ d .read (_WHO_AM_I , data )
84
+ return data [0 ] == 0x68
85
+ }
86
+
64
87
// Update fetches the latest data from the MPU6050
65
88
func (p * Device ) Update () (err error ) {
66
- if err = p .read (_ACCEL_XOUT_H , p .RawData [:]); err != nil {
89
+ if err = p .read (_ACCEL_XOUT_H , p .data [:]); err != nil {
67
90
return err
68
91
}
69
92
return nil
70
93
}
71
94
72
95
// Acceleration returns last read acceleration in µg (micro-gravity).
73
- // When one of the axes is pointing straight to Earth
74
- // and the sensor is not moving the returned value will be around 1000000 or
75
- // -1000000.
96
+ // When one of the axes is pointing straight to Earth and the sensor is not
97
+ // moving the returned value will be around 1000000 or -1000000.
76
98
func (d * Device ) Acceleration () (ax , ay , az int32 ) {
77
99
const accelOffset = 0
78
- ax = int32 (convertWord (d .RawData [accelOffset + 0 :])) * 15625 / 512 * d .aRange
79
- ay = int32 (convertWord (d .RawData [accelOffset + 2 :])) * 15625 / 512 * d .aRange
80
- az = int32 (convertWord (d .RawData [accelOffset + 4 :])) * 15625 / 512 * d .aRange
100
+ ax = int32 (convertWord (d .data [accelOffset + 0 :])) * 15625 / 512 * d .aRange
101
+ ay = int32 (convertWord (d .data [accelOffset + 2 :])) * 15625 / 512 * d .aRange
102
+ az = int32 (convertWord (d .data [accelOffset + 4 :])) * 15625 / 512 * d .aRange
81
103
return ax , ay , az
82
104
}
83
105
84
- // Rotations reads the current rotation from the device and returns it in
106
+ // AngularVelocity reads the current angular velocity from the device and returns it in
85
107
// µ°/rad (micro-radians/sec). This means that if you were to do a complete
86
108
// rotation along one axis and while doing so integrate all values over time,
87
109
// you would get a value close to 6.3 radians (360 degrees).
88
110
func (d * Device ) AngularVelocity () (gx , gy , gz int32 ) {
89
111
const angvelOffset = 8
90
- _ = d .RawData [angvelOffset + 5 ] // This line fails to compile if RawData is too short.
91
- gx = int32 (convertWord (d .RawData [angvelOffset + 0 :])) * 4363 / 8192 * d .gRange
92
- gy = int32 (convertWord (d .RawData [angvelOffset + 2 :])) * 4363 / 8192 * d .gRange
93
- gz = int32 (convertWord (d .RawData [angvelOffset + 4 :])) * 4363 / 8192 * d .gRange
112
+ _ = d .data [angvelOffset + 5 ] // This line fails to compile if RawData is too short.
113
+ gx = int32 (convertWord (d .data [angvelOffset + 0 :])) * 4363 / 8192 * d .gRange
114
+ gy = int32 (convertWord (d .data [angvelOffset + 2 :])) * 4363 / 8192 * d .gRange
115
+ gz = int32 (convertWord (d .data [angvelOffset + 4 :])) * 4363 / 8192 * d .gRange
94
116
return gx , gy , gz
95
117
}
96
118
97
119
// Temperature returns the temperature of the device in milli-centigrade.
98
120
func (d * Device ) Temperature () (Celsius int32 ) {
99
121
const tempOffset = 6
100
- return 1506 * int32 (convertWord (d .RawData [tempOffset :]))/ 512 + 37 * 1000
122
+ return 1506 * int32 (convertWord (d .data [tempOffset :]))/ 512 + 37 * 1000
101
123
}
102
124
103
125
func convertWord (buf []byte ) int16 {
104
126
return int16 (binary .BigEndian .Uint16 (buf ))
105
127
}
106
128
107
- // SetSampleRate sets the sample rate for the FIFO,
129
+ // setSampleRate sets the sample rate for the FIFO,
108
130
// register ouput and DMP. The sample rate is determined
109
131
// by:
110
132
//
@@ -114,7 +136,7 @@ func convertWord(buf []byte) int16 {
114
136
// disabled and 1kHz otherwise. The maximum sample rate
115
137
// for the accelerometer is 1kHz, if a higher sample rate
116
138
// is chosen, the same accelerometer sample will be output.
117
- func (p * Device ) SetSampleRate (srDiv byte ) (err error ) {
139
+ func (p * Device ) setSampleRate (srDiv byte ) (err error ) {
118
140
// setSampleRate
119
141
var sr [1 ]byte
120
142
sr [0 ] = srDiv
@@ -124,108 +146,78 @@ func (p *Device) SetSampleRate(srDiv byte) (err error) {
124
146
return nil
125
147
}
126
148
127
- // SetClockSource configures the source of the clock
149
+ // setClockSource configures the source of the clock
128
150
// for the peripheral.
129
- func (p * Device ) SetClockSource (clkSel byte ) (err error ) {
130
- // setClockSource
131
- var pwrMgt [1 ]byte
132
-
133
- if err = p .read (_PWR_MGMT_1 , pwrMgt [:]); err != nil {
134
- return err
135
- }
136
- pwrMgt [0 ] = (pwrMgt [0 ] & (^ _CLK_SEL_MASK )) | clkSel // Escribo solo el campo de clk_sel
137
- if err = p .write8 (_PWR_MGMT_1 , pwrMgt [0 ]); err != nil {
138
- return err
139
- }
140
- return nil
151
+ func (p * Device ) setClockSource (clkSel byte ) (err error ) {
152
+ return p .writeMasked (_PWR_MGMT_1 , _CLK_SEL_MASK , clkSel )
141
153
}
142
154
143
- // SetRangeGyro configures the full scale range of the gyroscope.
155
+ // setRangeGyro configures the full scale range of the gyroscope.
144
156
// It has four possible values +- 250°/s, 500°/s, 1000°/s, 2000°/s.
145
- // The function takes values of gyroRange from 0-3 where 0 means the
146
- // lowest FSR (250°/s) and 3 is the highest FSR (2000°/s).
147
- func (p * Device ) SetRangeGyro (gyroRange byte ) (err error ) {
157
+ func (p * Device ) setRangeGyro (gyroRange RangeGyro ) (err error ) {
148
158
switch gyroRange {
149
- case GYRO_RANGE_250 :
159
+ case RangeGyro250 :
150
160
p .gRange = 250
151
- case GYRO_RANGE_500 :
161
+ case RangeGyro500 :
152
162
p .gRange = 500
153
- case GYRO_RANGE_1000 :
163
+ case RangeGyro1000 :
154
164
p .gRange = 1000
155
- case GYRO_RANGE_2000 :
165
+ case RangeGyro2000 :
156
166
p .gRange = 2000
157
167
default :
158
168
return errors .New ("invalid gyroscope FSR input" )
159
169
}
160
- // setFullScaleGyroRange
161
- var gConfig [1 ]byte
162
-
163
- if err = p .read (_GYRO_CONFIG , gConfig [:]); err != nil {
164
- return err
165
- }
166
- gConfig [0 ] = (gConfig [0 ] & (^ _G_FS_SEL )) | (gyroRange << _G_FS_SHIFT ) // Escribo solo el campo de FS_sel
167
-
168
- if err = p .write8 (_GYRO_CONFIG , gConfig [0 ]); err != nil {
169
- return err
170
- }
171
- return nil
170
+ return p .writeMasked (_GYRO_CONFIG , _G_FS_SEL , uint8 (gyroRange )<< _G_FS_SHIFT )
172
171
}
173
172
174
- // SetRangeAccel configures the full scale range of the accelerometer.
173
+ // setRangeAccel configures the full scale range of the accelerometer.
175
174
// It has four possible values +- 2g, 4g, 8g, 16g.
176
175
// The function takes values of accRange from 0-3 where 0 means the
177
176
// lowest FSR (2g) and 3 is the highest FSR (16g)
178
- func (p * Device ) SetRangeAccel (accRange byte ) (err error ) {
177
+ func (p * Device ) setRangeAccel (accRange RangeAccel ) (err error ) {
179
178
switch accRange {
180
- case ACCEL_RANGE_2 :
179
+ case RangeAccel2 :
181
180
p .aRange = 2
182
- case ACCEL_RANGE_4 :
181
+ case RangeAccel4 :
183
182
p .aRange = 4
184
- case ACCEL_RANGE_8 :
183
+ case RangeAccel8 :
185
184
p .aRange = 8
186
- case ACCEL_RANGE_16 :
185
+ case RangeAccel16 :
187
186
p .aRange = 16
188
187
default :
189
188
return errors .New ("invalid accelerometer FSR input" )
190
189
}
191
-
192
- var aConfig [1 ]byte
193
- if err = p .read (_ACCEL_CONFIG , aConfig [:]); err != nil {
194
- return err
195
- }
196
- aConfig [0 ] = (aConfig [0 ] & (^ _AFS_SEL )) | (accRange << _AFS_SHIFT )
197
-
198
- if err = p .write8 (_ACCEL_CONFIG , aConfig [0 ]); err != nil {
199
- return err
200
- }
201
- return nil
190
+ return p .writeMasked (_ACCEL_CONFIG , _AFS_SEL , uint8 (accRange )<< _AFS_SHIFT )
202
191
}
203
192
204
193
// Sleep sets the sleep bit on the power managment 1 field.
205
194
// When the recieved bool is true, it sets the bit to 1 thus putting
206
195
// the peripheral in sleep mode.
207
196
// When false is recieved the bit is set to 0 and the peripheral wakes up.
208
197
func (p * Device ) Sleep (sleepEnabled bool ) (err error ) {
209
- // setSleepBit
210
- var pwrMgt [1 ]byte
211
- if err = p .read (_PWR_MGMT_1 , pwrMgt [:]); err != nil {
198
+ return p .writeMasked (_PWR_MGMT_1 , _SLEEP_MASK , b2u8 (sleepEnabled )<< _SLEEP_SHIFT )
199
+ }
200
+
201
+ func (d * Device ) writeMasked (reg byte , mask byte , value byte ) error {
202
+ var b [1 ]byte
203
+ if err := d .read (reg , b [:]); err != nil {
212
204
return err
213
205
}
214
- if sleepEnabled {
215
- pwrMgt [ 0 ] = ( pwrMgt [0 ] & ( ^ _SLEEP_MASK )) | ( 1 << _SLEEP_SHIFT ) // Overwrite only Sleep
216
- } else {
217
- pwrMgt [ 0 ] = ( pwrMgt [ 0 ] & ( ^ _SLEEP_MASK ))
218
- }
219
- if err = p . write8 ( _PWR_MGMT_1 , pwrMgt [ 0 ]); err != nil {
220
- return err
206
+ b [ 0 ] = ( b [ 0 ] &^ mask ) | value & mask
207
+ return d . write8 ( reg , b [0 ])
208
+ }
209
+
210
+ func b2u8 ( b bool ) byte {
211
+ if b {
212
+ return 1
221
213
}
222
- return nil
214
+ return 0
223
215
}
224
216
225
217
func DefaultConfig () Config {
226
218
return Config {
227
- AccRange : ACCEL_RANGE_16 ,
228
- GyroRange : GYRO_RANGE_2000 ,
219
+ AccelRange : RangeAccel16 ,
220
+ GyroRange : RangeGyro2000 ,
229
221
sampleRatio : 0 , // TODO add const values.
230
222
clkSel : 0 ,
231
223
}
0 commit comments