add writeMasked; add range types; unexport config methods; less allocs; less spanish

Author: soypat

examples/mpu6050/main.go

@@ -14,8 +14,8 @@ func main() {
 	mpuDevice := mpu6050.New(machine.I2C0, mpu6050.DefaultAddress)
 
 	err := mpuDevice.Configure(mpu6050.Config{
-		AccRange:  mpu6050.ACCEL_RANGE_16,
-		GyroRange: mpu6050.GYRO_RANGE_2000,
+		AccelRange: mpu6050.ACCEL_RANGE_16,
+		GyroRange:  mpu6050.GYRO_RANGE_2000,
 	})
 	if err != nil {
 		panic(err.Error())
@@ -31,7 +31,7 @@ func main() {
 		println(mpuDevice.Acceleration())
 		print("angular velocity:")
 		println(mpuDevice.AngularVelocity())
-		print("temperature centigrade:")
+		print("temperature celsius:")
 		println(mpuDevice.Temperature() / 1000)
 	}
 }

mpu6050/mpu6050.go

@@ -1,19 +1,34 @@
-package mpu6050
+// Package mpu6050 provides a driver for the MPU6050 accelerometer and gyroscope
+// made by InvenSense.
+//
+// Datasheets:
+// https://store.invensense.com/datasheets/invensense/MPU-6050_DataSheet_V3%204.pdf
+// https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
+package mpu6050 // import "tinygo.org/x/drivers/mpu6050"
 
 import (
 	"encoding/binary"
 	"errors"
+	"fmt"
 
 	"tinygo.org/x/drivers"
 )
 
 const DefaultAddress = 0x68
 
+// RangeAccel defines the range of the accelerometer.
+// Allowed values are 2, 4, 8 and 16 with the unit g (gravity).
+type RangeAccel uint8
+
+// RangeGyro defines the range of the gyroscope.
+// Allowed values are 250, 500, 1000 and 2000 with the unit °/s (degree per second).
+type RangeGyro uint8
+
 type Config struct {
 	// Use ACCEL_RANGE_2 through ACCEL_RANGE_16.
-	AccRange byte
+	AccelRange RangeAccel
 	// Use GYRO_RANGE_250 through GYRO_RANGE_2000
-	GyroRange   byte
+	GyroRange   RangeGyro
 	sampleRatio byte // TODO(soypat): expose these as configurable.
 	clkSel      byte
 }
@@ -23,9 +38,12 @@ type Device struct {
 	conn   drivers.I2C
 	aRange int32 //Gyroscope FSR acording to SetAccelRange input
 	gRange int32 //Gyroscope FSR acording to SetGyroRange input
-	// RawData contains the accelerometer, gyroscope and temperature RawData read
-	// in the last call via the Update method.
-	RawData [14]byte
+	// data contains the accelerometer, gyroscope and temperature data read
+	// in the last call via the Update method. The data is stored as seven 16bit unsigned
+	// integers in big endian format:
+	//
+	//	| ax | ay | az | temp | gx | gy | gz |
+	data    [14]byte
 	address byte
 }
 
@@ -44,67 +62,74 @@ func New(bus drivers.I2C, addr uint16) *Device {
 // and wakes up the peripheral.
 func (p *Device) Configure(data Config) (err error) {
 	if err = p.Sleep(false); err != nil {
-		return errors.New("set sleep: " + err.Error())
+		// We have a special error case for what is probably the first write
+		// to the device.
+		return fmt.Errorf("during attempt to wake: %w", err)
 	}
-	if err = p.SetClockSource(data.clkSel); err != nil {
-		return errors.New("set clksrc: " + err.Error())
+	if err = p.setClockSource(data.clkSel); err != nil {
+		return err
 	}
-	if err = p.SetSampleRate(data.sampleRatio); err != nil {
-		return errors.New("set sampleratio: " + err.Error())
+	if err = p.setSampleRate(data.sampleRatio); err != nil {
+		return err
 	}
-	if err = p.SetRangeGyro(data.GyroRange); err != nil {
-		return errors.New("set gyrorange: " + err.Error())
+	if err = p.setRangeGyro(data.GyroRange); err != nil {
+		return err
 	}
-	if err = p.SetRangeAccel(data.AccRange); err != nil {
-		return errors.New("set accelrange: " + err.Error())
+	if err = p.setRangeAccel(data.AccelRange); err != nil {
+		return err
 	}
 	return nil
 }
 
+func (d Device) Connected() bool {
+	data := []byte{0}
+	d.read(_WHO_AM_I, data)
+	return data[0] == 0x68
+}
+
 // Update fetches the latest data from the MPU6050
 func (p *Device) Update() (err error) {
-	if err = p.read(_ACCEL_XOUT_H, p.RawData[:]); err != nil {
+	if err = p.read(_ACCEL_XOUT_H, p.data[:]); err != nil {
 		return err
 	}
 	return nil
 }
 
 // Acceleration returns last read acceleration in µg (micro-gravity).
-// When one of the axes is pointing straight to Earth
-// and the sensor is not moving the returned value will be around 1000000 or
-// -1000000.
+// When one of the axes is pointing straight to Earth and the sensor is not
+// moving the returned value will be around 1000000 or -1000000.
 func (d *Device) Acceleration() (ax, ay, az int32) {
 	const accelOffset = 0
-	ax = int32(convertWord(d.RawData[accelOffset+0:])) * 15625 / 512 * d.aRange
-	ay = int32(convertWord(d.RawData[accelOffset+2:])) * 15625 / 512 * d.aRange
-	az = int32(convertWord(d.RawData[accelOffset+4:])) * 15625 / 512 * d.aRange
+	ax = int32(convertWord(d.data[accelOffset+0:])) * 15625 / 512 * d.aRange
+	ay = int32(convertWord(d.data[accelOffset+2:])) * 15625 / 512 * d.aRange
+	az = int32(convertWord(d.data[accelOffset+4:])) * 15625 / 512 * d.aRange
 	return ax, ay, az
 }
 
-// Rotations reads the current rotation from the device and returns it in
+// AngularVelocity reads the current angular velocity from the device and returns it in
 // µ°/rad (micro-radians/sec). This means that if you were to do a complete
 // rotation along one axis and while doing so integrate all values over time,
 // you would get a value close to 6.3 radians (360 degrees).
 func (d *Device) AngularVelocity() (gx, gy, gz int32) {
 	const angvelOffset = 8
-	_ = d.RawData[angvelOffset+5] // This line fails to compile if RawData is too short.
-	gx = int32(convertWord(d.RawData[angvelOffset+0:])) * 4363 / 8192 * d.gRange
-	gy = int32(convertWord(d.RawData[angvelOffset+2:])) * 4363 / 8192 * d.gRange
-	gz = int32(convertWord(d.RawData[angvelOffset+4:])) * 4363 / 8192 * d.gRange
+	_ = d.data[angvelOffset+5] // This line fails to compile if RawData is too short.
+	gx = int32(convertWord(d.data[angvelOffset+0:])) * 4363 / 8192 * d.gRange
+	gy = int32(convertWord(d.data[angvelOffset+2:])) * 4363 / 8192 * d.gRange
+	gz = int32(convertWord(d.data[angvelOffset+4:])) * 4363 / 8192 * d.gRange
 	return gx, gy, gz
 }
 
 // Temperature returns the temperature of the device in milli-centigrade.
 func (d *Device) Temperature() (Celsius int32) {
 	const tempOffset = 6
-	return 1506*int32(convertWord(d.RawData[tempOffset:]))/512 + 37*1000
+	return 1506*int32(convertWord(d.data[tempOffset:]))/512 + 37*1000
 }
 
 func convertWord(buf []byte) int16 {
 	return int16(binary.BigEndian.Uint16(buf))
 }
 
-// SetSampleRate sets the sample rate for the FIFO,
+// setSampleRate sets the sample rate for the FIFO,
 // register ouput and DMP. The sample rate is determined
 // by:
 //
@@ -114,7 +139,7 @@ func convertWord(buf []byte) int16 {
 // disabled and 1kHz otherwise. The maximum sample rate
 // for the accelerometer is 1kHz, if a higher sample rate
 // is chosen, the same accelerometer sample will be output.
-func (p *Device) SetSampleRate(srDiv byte) (err error) {
+func (p *Device) setSampleRate(srDiv byte) (err error) {
 	// setSampleRate
 	var sr [1]byte
 	sr[0] = srDiv
@@ -124,108 +149,78 @@ func (p *Device) SetSampleRate(srDiv byte) (err error) {
 	return nil
 }
 
-// SetClockSource configures the source of the clock
+// setClockSource configures the source of the clock
 // for the peripheral.
-func (p *Device) SetClockSource(clkSel byte) (err error) {
-	// setClockSource
-	var pwrMgt [1]byte
-
-	if err = p.read(_PWR_MGMT_1, pwrMgt[:]); err != nil {
-		return err
-	}
-	pwrMgt[0] = (pwrMgt[0] & (^_CLK_SEL_MASK)) | clkSel // Escribo solo el campo de clk_sel
-	if err = p.write8(_PWR_MGMT_1, pwrMgt[0]); err != nil {
-		return err
-	}
-	return nil
+func (p *Device) setClockSource(clkSel byte) (err error) {
+	return p.writeMasked(_PWR_MGMT_1, _CLK_SEL_MASK, clkSel)
 }
 
-// SetRangeGyro configures the full scale range of the gyroscope.
+// setRangeGyro configures the full scale range of the gyroscope.
 // It has four possible values +- 250°/s, 500°/s, 1000°/s, 2000°/s.
-// The function takes values of gyroRange from 0-3 where 0 means the
-// lowest FSR (250°/s) and 3 is the highest FSR (2000°/s).
-func (p *Device) SetRangeGyro(gyroRange byte) (err error) {
+func (p *Device) setRangeGyro(gyroRange RangeGyro) (err error) {
 	switch gyroRange {
-	case GYRO_RANGE_250:
+	case RangeGyro250:
 		p.gRange = 250
-	case GYRO_RANGE_500:
+	case RangeGyro500:
 		p.gRange = 500
-	case GYRO_RANGE_1000:
+	case RangeGyro1000:
 		p.gRange = 1000
-	case GYRO_RANGE_2000:
+	case RangeGyro2000:
 		p.gRange = 2000
 	default:
 		return errors.New("invalid gyroscope FSR input")
 	}
-	// setFullScaleGyroRange
-	var gConfig [1]byte
-
-	if err = p.read(_GYRO_CONFIG, gConfig[:]); err != nil {
-		return err
-	}
-	gConfig[0] = (gConfig[0] & (^_G_FS_SEL)) | (gyroRange << _G_FS_SHIFT) // Escribo solo el campo de FS_sel
-
-	if err = p.write8(_GYRO_CONFIG, gConfig[0]); err != nil {
-		return err
-	}
-	return nil
+	return p.writeMasked(_GYRO_CONFIG, _G_FS_SEL, uint8(gyroRange)<<_G_FS_SHIFT)
 }
 
-// SetRangeAccel configures the full scale range of the accelerometer.
+// setRangeAccel configures the full scale range of the accelerometer.
 // It has four possible values +- 2g, 4g, 8g, 16g.
 // The function takes values of accRange from 0-3 where 0 means the
 // lowest FSR (2g) and 3 is the highest FSR (16g)
-func (p *Device) SetRangeAccel(accRange byte) (err error) {
+func (p *Device) setRangeAccel(accRange RangeAccel) (err error) {
 	switch accRange {
-	case ACCEL_RANGE_2:
+	case RangeAccel2:
 		p.aRange = 2
-	case ACCEL_RANGE_4:
+	case RangeAccel4:
 		p.aRange = 4
-	case ACCEL_RANGE_8:
+	case RangeAccel8:
 		p.aRange = 8
-	case ACCEL_RANGE_16:
+	case RangeAccel16:
 		p.aRange = 16
 	default:
 		return errors.New("invalid accelerometer FSR input")
 	}
-
-	var aConfig [1]byte
-	if err = p.read(_ACCEL_CONFIG, aConfig[:]); err != nil {
-		return err
-	}
-	aConfig[0] = (aConfig[0] & (^_AFS_SEL)) | (accRange << _AFS_SHIFT)
-
-	if err = p.write8(_ACCEL_CONFIG, aConfig[0]); err != nil {
-		return err
-	}
-	return nil
+	return p.writeMasked(_ACCEL_CONFIG, _AFS_SEL, uint8(accRange)<<_AFS_SHIFT)
 }
 
 // Sleep sets the sleep bit on the power managment 1 field.
 // When the recieved bool is true, it sets the bit to 1 thus putting
 // the peripheral in sleep mode.
 // When false is recieved the bit is set to 0 and the peripheral wakes up.
 func (p *Device) Sleep(sleepEnabled bool) (err error) {
-	// setSleepBit
-	var pwrMgt [1]byte
-	if err = p.read(_PWR_MGMT_1, pwrMgt[:]); err != nil {
+	return p.writeMasked(_PWR_MGMT_1, _SLEEP_MASK, b2u8(sleepEnabled)<<_SLEEP_SHIFT)
+}
+
+func (d *Device) writeMasked(reg byte, mask byte, value byte) error {
+	var b [1]byte
+	if err := d.read(reg, b[:]); err != nil {
 		return err
 	}
-	if sleepEnabled {
-		pwrMgt[0] = (pwrMgt[0] & (^_SLEEP_MASK)) | (1 << _SLEEP_SHIFT) // Overwrite only Sleep
-	} else {
-		pwrMgt[0] = (pwrMgt[0] & (^_SLEEP_MASK))
-	}
-	if err = p.write8(_PWR_MGMT_1, pwrMgt[0]); err != nil {
-		return err
+	b[0] = (b[0] &^ mask) | value&mask
+	return d.write8(reg, b[0])
+}
+
+func b2u8(b bool) byte {
+	if b {
+		return 1
 	}
-	return nil
+	return 0
 }
 
 func DefaultConfig() Config {
 	return Config{
-		AccRange:    ACCEL_RANGE_16,
-		GyroRange:   GYRO_RANGE_2000,
+		AccelRange:  RangeAccel16,
+		GyroRange:   RangeGyro2000,
 		sampleRatio: 0, // TODO add const values.
 		clkSel:      0,
 	}