Mpu6050 rewrite

examples/mpu6050/main.go

@@ -11,12 +11,24 @@ import (
 func main() {
 	machine.I2C0.Configure(machine.I2CConfig{})
 
-	accel := mpu6050.New(machine.I2C0)
-	accel.Configure()
+	mpuDevice := mpu6050.New(machine.I2C0)
+	mpuDevice.Init(mpu6050.IMUConfig{
+		AccRange:  mpu6050.ACCEL_RANGE_16,
+		GyroRange: mpu6050.GYRO_RANGE_2000,
+	})
 
 	for {
-		x, y, z := accel.ReadAcceleration()
-		println(x, y, z)
 		time.Sleep(time.Millisecond * 100)
+		err := mpuDevice.Update()
+		if err != nil {
+			println("error reading from mpu6050:", err.Error())
+			continue
+		}
+		print("acceleration: ")
+		println(mpuDevice.Acceleration())
+		print("angular velocity:")
+		println(mpuDevice.AngularVelocity())
+		print("temperature centigrade:")
+		println(mpuDevice.Temperature() / 1000)
 	}
 }

mpu6050/mpu6050.go

@@ -1,95 +1,251 @@
-// 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"
+package mpu6050
+
+import (
+	"encoding/binary"
+	"fmt"
+
+	"tinygo.org/x/drivers"
+)
 
-import "tinygo.org/x/drivers"
+const DefaultAddress = 0x68
 
-// Device wraps an I2C connection to a MPU6050 device.
-type Device struct {
-	bus     drivers.I2C
-	Address uint16
+type IMUConfig struct {
+	// Use ACCEL_RANGE_2 through ACCEL_RANGE_16.
+	AccRange byte
+	// Use GYRO_RANGE_250 through GYRO_RANGE_2000
+	GyroRange   byte
+	sampleRatio byte // TODO(soypat): expose these as configurable.
+	clkSel      byte
 }
 
-// New creates a new MPU6050 connection. The I2C bus must already be
-// configured.
-//
-// This function only creates the Device object, it does not touch the device.
-func New(bus drivers.I2C) Device {
-	return Device{bus, Address}
+// Dev contains MPU board abstraction for usage
+type Dev struct {
+	conn    drivers.I2C
+	aRange  int32 //Gyroscope FSR acording to SetAccelRange input
+	gRange  int32 //Gyroscope FSR acording to SetGyroRange input
+	data    [14]byte
+	address byte
+}
+
+// New instantiates and initializes a MPU6050 struct without writing/reading
+// i2c bus. Typical I2C MPU6050 address is 0x68.
+func New(bus drivers.I2C, addr uint16) *Dev {
+	p := &Dev{}
+	p.address = uint8(addr)
+	p.conn = bus
+	return p
+}
+
+// Init configures the necessary registers for using the
+// MPU6050. It sets the range of both the accelerometer
+// and the gyroscope, the sample rate, the clock source
+// and wakes up the peripheral.
+func (p *Dev) Init(data IMUConfig) (err error) {
+
+	// setSleep
+	if err = p.SetSleep(false); err != nil {
+		return fmt.Errorf("set sleep: %w", err)
+	}
+	// setClockSource
+	if err = p.SetClockSource(data.clkSel); err != nil {
+		return fmt.Errorf("set clksrc: %w", err)
+	}
+	// setSampleRate
+	if err = p.SetSampleRate(data.sampleRatio); err != nil {
+		return fmt.Errorf("set sample: %w", err)
+	}
+	// setFullScaleGyroRange
+	if err = p.SetGyroRange(data.GyroRange); err != nil {
+		return fmt.Errorf("set gyro: %w", err)
+	}
+	// setFullScaleAccelRange
+	if err = p.SetAccelRange(data.AccRange); err != nil {
+		return fmt.Errorf("set accelrange: %w", err)
+	}
+	return nil
 }
 
-// Connected returns whether a MPU6050 has been found.
-// It does a "who am I" request and checks the response.
-func (d Device) Connected() bool {
-	data := []byte{0}
-	d.bus.ReadRegister(uint8(d.Address), WHO_AM_I, data)
-	return data[0] == 0x68
+// Update fetches the latest data from the MPU6050
+func (p *Dev) Update() (err error) {
+	if err = p.read(ACCEL_XOUT_H, p.data[:]); err != nil {
+		return err
+	}
+	return nil
 }
 
-// Configure sets up the device for communication.
-func (d Device) Configure() error {
-	return d.SetClockSource(CLOCK_INTERNAL)
+// Get treats the MPU6050 like an ADC and returns the raw reading of the channel.
+// Channels 0-2 are acceleration, 3-5 are gyroscope and 6 is the temperature.
+func (d *Dev) RawReading(channel int) int16 {
+	if channel > 6 {
+		// Bad value.
+		return 0
+	}
+	return convertWord(d.data[channel*2:])
 }
 
-// ReadAcceleration reads the current acceleration from the device and returns
-// it in µg (micro-gravity). When one of the axes is pointing straight to Earth
+func convertWord(buf []byte) int16 {
+	return int16(binary.BigEndian.Uint16(buf))
+}
+
+func (d *Dev) ChannelLen() int { return 7 }
+
+// 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.
-func (d Device) ReadAcceleration() (x int32, y int32, z int32) {
-	data := make([]byte, 6)
-	d.bus.ReadRegister(uint8(d.Address), ACCEL_XOUT_H, data)
-	// Now do two things:
-	// 1. merge the two values to a 16-bit number (and cast to a 32-bit integer)
-	// 2. scale the value to bring it in the -1000000..1000000 range.
-	//    This is done with a trick. What we do here is essentially multiply by
-	//    1000000 and divide by 16384 to get the original scale, but to avoid
-	//    overflow we do it at 1/64 of the value:
-	//      1000000 / 64 = 15625
-	//      16384   / 64 = 256
-	x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / 256
-	y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / 256
-	z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / 256
-	return
-}
-
-// ReadRotation reads the current rotation from the device and returns it in
-// µ°/s (micro-degrees/sec). This means that if you were to do a complete
+func (d *Dev) Acceleration() (ax, ay, az int32) {
+	const accelOffset = 0
+	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
+// µ°/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 360000000.
-func (d Device) ReadRotation() (x int32, y int32, z int32) {
-	data := make([]byte, 6)
-	d.bus.ReadRegister(uint8(d.Address), GYRO_XOUT_H, data)
-	// First the value is converted from a pair of bytes to a signed 16-bit
-	// value and then to a signed 32-bit value to avoid integer overflow.
-	// Then the value is scaled to µ°/s (micro-degrees per second).
-	// This is done in the following steps:
-	// 1. Multiply by 250 * 1000_000
-	// 2. Divide by 32768
-	// The following calculation (x * 15625 / 2048 * 1000) is essentially the
-	// same but avoids overflow. First both operations are divided by 16 leading
-	// to multiply by 15625000 and divide by 2048, and then part of the multiply
-	// is done after the divide instead of before.
-	x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / 2048 * 1000
-	y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / 2048 * 1000
-	z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / 2048 * 1000
-	return
-}
-
-// SetClockSource allows the user to configure the clock source.
-func (d Device) SetClockSource(source uint8) error {
-	return d.bus.WriteRegister(uint8(d.Address), PWR_MGMT_1, []uint8{source})
-}
-
-// SetFullScaleGyroRange allows the user to configure the scale range for the gyroscope.
-func (d Device) SetFullScaleGyroRange(rng uint8) error {
-	return d.bus.WriteRegister(uint8(d.Address), GYRO_CONFIG, []uint8{rng})
-}
-
-// SetFullScaleAccelRange allows the user to configure the scale range for the accelerometer.
-func (d Device) SetFullScaleAccelRange(rng uint8) error {
-	return d.bus.WriteRegister(uint8(d.Address), ACCEL_CONFIG, []uint8{rng})
+// you would get a value close to 6.3 radians (360 degrees).
+func (d *Dev) AngularVelocity() (gx, gy, gz int32) {
+	const angvelOffset = 8
+	gx = int32(convertWord(d.data[angvelOffset+0:])) * 2663 / 5000 * d.gRange
+	gy = int32(convertWord(d.data[angvelOffset+2:])) * 2663 / 5000 * d.gRange
+	gz = int32(convertWord(d.data[angvelOffset+4:])) * 2663 / 5000 * d.gRange
+	return gx, gy, gz
+}
+
+// Temperature returns the temperature of the device in milli-centigrade.
+func (d *Dev) Temperature() (Celsius int32) {
+	const tempOffset = 6
+	return 1000*int32(convertWord(d.data[tempOffset:]))/340 + 37*1000
+}
+
+// SetSampleRate sets the sample rate for the FIFO,
+// register ouput and DMP. The sample rate is determined
+// by:
+//
+//	SR = Gyroscope Output Rate / (1 + srDiv)
+//
+// The Gyroscope Output Rate is 8kHz when the DLPF is
+// 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 *Dev) SetSampleRate(srDiv byte) (err error) {
+	// setSampleRate
+	var sr [1]byte
+	sr[0] = srDiv
+	if err = p.write8(SMPRT_DIV, sr[0]); err != nil {
+		return err
+	}
+	return nil
+}
+
+// SetClockSource configures the source of the clock
+// for the peripheral.
+func (p *Dev) 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
+}
+
+// SetGyroRange 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 *Dev) SetGyroRange(gyroRange byte) (err error) {
+	switch gyroRange {
+	case GYRO_RANGE_250:
+		p.gRange = 250
+	case GYRO_RANGE_500:
+		p.gRange = 500
+	case GYRO_RANGE_1000:
+		p.gRange = 1000
+	case GYRO_RANGE_2000:
+		p.gRange = 2000
+	default:
+		return fmt.Errorf("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
+}
+
+// SetAccelRange 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 *Dev) SetAccelRange(accRange byte) (err error) {
+	switch accRange {
+	case ACCEL_RANGE_2:
+		p.aRange = 2
+	case ACCEL_RANGE_4:
+		p.aRange = 4
+	case ACCEL_RANGE_8:
+		p.aRange = 8
+	case ACCEL_RANGE_16:
+		p.aRange = 16
+	default:
+		return fmt.Errorf("invalid accelerometer FSR input")
+	}
+	// setFullScaleAccelRange
+	var aConfig [1]byte
+
+	if err = p.read(ACCEL_CONFIG, aConfig[:]); err != nil {
+		return err
+	}
+	aConfig[0] = (aConfig[0] & (^AFS_SEL)) | (accRange << AFS_SHIFT) // Escribo solo el campo de FS_sel
+
+	if err = p.write8(ACCEL_CONFIG, aConfig[0]); err != nil {
+		return err
+	}
+	return nil
+}
+
+// SetSleep 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 *Dev) SetSleep(sleep bool) (err error) {
+	// setSleepBit
+	var pwrMgt [1]byte
+
+	if err = p.read(PWR_MGMT_1, pwrMgt[:]); err != nil {
+		return err
+	}
+	if sleep {
+		pwrMgt[0] = (pwrMgt[0] & (^SLEEP_MASK)) | (1 << SLEEP_SHIFT) // Escribo solo el campo de clk_sel
+	} else {
+		pwrMgt[0] = (pwrMgt[0] & (^SLEEP_MASK))
+	}
+	//Envio el registro modificado al periferico
+	if err = p.write8(PWR_MGMT_1, pwrMgt[0]); err != nil {
+		return err
+	}
+	return nil
+}
+
+func DefaultConfig() IMUConfig {
+	return IMUConfig{
+		AccRange:    ACCEL_RANGE_16,
+		GyroRange:   GYRO_RANGE_2000,
+		sampleRatio: 0, // TODO add const values.
+		clkSel:      0,
+	}
 }