Skip to content

Commit 8887ad9

Browse files
committed
add writeMasked; add range types; unexport config methods; less allocs; less spanish
1 parent c4c9bdb commit 8887ad9

File tree

3 files changed

+232
-119
lines changed

3 files changed

+232
-119
lines changed

examples/mpu6050/main.go

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,8 @@ func main() {
1414
mpuDevice := mpu6050.New(machine.I2C0, mpu6050.DefaultAddress)
1515

1616
err := mpuDevice.Configure(mpu6050.Config{
17-
AccRange: mpu6050.ACCEL_RANGE_16,
18-
GyroRange: mpu6050.GYRO_RANGE_2000,
17+
AccelRange: mpu6050.ACCEL_RANGE_16,
18+
GyroRange: mpu6050.GYRO_RANGE_2000,
1919
})
2020
if err != nil {
2121
panic(err.Error())
@@ -31,7 +31,7 @@ func main() {
3131
println(mpuDevice.Acceleration())
3232
print("angular velocity:")
3333
println(mpuDevice.AngularVelocity())
34-
print("temperature centigrade:")
34+
print("temperature celsius:")
3535
println(mpuDevice.Temperature() / 1000)
3636
}
3737
}

mpu6050/mpu6050.go

Lines changed: 85 additions & 93 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,10 @@
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"
28

39
import (
410
"encoding/binary"
@@ -9,11 +15,19 @@ import (
915

1016
const DefaultAddress = 0x68
1117

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+
1226
type Config struct {
1327
// Use ACCEL_RANGE_2 through ACCEL_RANGE_16.
14-
AccRange byte
28+
AccelRange RangeAccel
1529
// Use GYRO_RANGE_250 through GYRO_RANGE_2000
16-
GyroRange byte
30+
GyroRange RangeGyro
1731
sampleRatio byte // TODO(soypat): expose these as configurable.
1832
clkSel byte
1933
}
@@ -23,9 +37,12 @@ type Device struct {
2337
conn drivers.I2C
2438
aRange int32 //Gyroscope FSR acording to SetAccelRange input
2539
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
2946
address byte
3047
}
3148

@@ -44,67 +61,72 @@ func New(bus drivers.I2C, addr uint16) *Device {
4461
// and wakes up the peripheral.
4562
func (p *Device) Configure(data Config) (err error) {
4663
if err = p.Sleep(false); err != nil {
47-
return errors.New("set sleep: " + err.Error())
64+
return err
4865
}
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
5168
}
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
5471
}
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
5774
}
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
6077
}
6178
return nil
6279
}
6380

81+
func (d Device) Connected() bool {
82+
data := []byte{0}
83+
d.read(_WHO_AM_I, data)
84+
return data[0] == 0x68
85+
}
86+
6487
// Update fetches the latest data from the MPU6050
6588
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 {
6790
return err
6891
}
6992
return nil
7093
}
7194

7295
// 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.
7698
func (d *Device) Acceleration() (ax, ay, az int32) {
7799
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
81103
return ax, ay, az
82104
}
83105

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
85107
// µ°/rad (micro-radians/sec). This means that if you were to do a complete
86108
// rotation along one axis and while doing so integrate all values over time,
87109
// you would get a value close to 6.3 radians (360 degrees).
88110
func (d *Device) AngularVelocity() (gx, gy, gz int32) {
89111
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
94116
return gx, gy, gz
95117
}
96118

97119
// Temperature returns the temperature of the device in milli-centigrade.
98120
func (d *Device) Temperature() (Celsius int32) {
99121
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
101123
}
102124

103125
func convertWord(buf []byte) int16 {
104126
return int16(binary.BigEndian.Uint16(buf))
105127
}
106128

107-
// SetSampleRate sets the sample rate for the FIFO,
129+
// setSampleRate sets the sample rate for the FIFO,
108130
// register ouput and DMP. The sample rate is determined
109131
// by:
110132
//
@@ -114,7 +136,7 @@ func convertWord(buf []byte) int16 {
114136
// disabled and 1kHz otherwise. The maximum sample rate
115137
// for the accelerometer is 1kHz, if a higher sample rate
116138
// 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) {
118140
// setSampleRate
119141
var sr [1]byte
120142
sr[0] = srDiv
@@ -124,108 +146,78 @@ func (p *Device) SetSampleRate(srDiv byte) (err error) {
124146
return nil
125147
}
126148

127-
// SetClockSource configures the source of the clock
149+
// setClockSource configures the source of the clock
128150
// 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)
141153
}
142154

143-
// SetRangeGyro configures the full scale range of the gyroscope.
155+
// setRangeGyro configures the full scale range of the gyroscope.
144156
// 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) {
148158
switch gyroRange {
149-
case GYRO_RANGE_250:
159+
case RangeGyro250:
150160
p.gRange = 250
151-
case GYRO_RANGE_500:
161+
case RangeGyro500:
152162
p.gRange = 500
153-
case GYRO_RANGE_1000:
163+
case RangeGyro1000:
154164
p.gRange = 1000
155-
case GYRO_RANGE_2000:
165+
case RangeGyro2000:
156166
p.gRange = 2000
157167
default:
158168
return errors.New("invalid gyroscope FSR input")
159169
}
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)
172171
}
173172

174-
// SetRangeAccel configures the full scale range of the accelerometer.
173+
// setRangeAccel configures the full scale range of the accelerometer.
175174
// It has four possible values +- 2g, 4g, 8g, 16g.
176175
// The function takes values of accRange from 0-3 where 0 means the
177176
// 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) {
179178
switch accRange {
180-
case ACCEL_RANGE_2:
179+
case RangeAccel2:
181180
p.aRange = 2
182-
case ACCEL_RANGE_4:
181+
case RangeAccel4:
183182
p.aRange = 4
184-
case ACCEL_RANGE_8:
183+
case RangeAccel8:
185184
p.aRange = 8
186-
case ACCEL_RANGE_16:
185+
case RangeAccel16:
187186
p.aRange = 16
188187
default:
189188
return errors.New("invalid accelerometer FSR input")
190189
}
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)
202191
}
203192

204193
// Sleep sets the sleep bit on the power managment 1 field.
205194
// When the recieved bool is true, it sets the bit to 1 thus putting
206195
// the peripheral in sleep mode.
207196
// When false is recieved the bit is set to 0 and the peripheral wakes up.
208197
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 {
212204
return err
213205
}
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
221213
}
222-
return nil
214+
return 0
223215
}
224216

225217
func DefaultConfig() Config {
226218
return Config{
227-
AccRange: ACCEL_RANGE_16,
228-
GyroRange: GYRO_RANGE_2000,
219+
AccelRange: RangeAccel16,
220+
GyroRange: RangeGyro2000,
229221
sampleRatio: 0, // TODO add const values.
230222
clkSel: 0,
231223
}

0 commit comments

Comments
 (0)