drivers: add mpu6050 additional functionality

mpu6050/mpu6050.go

@@ -20,7 +20,7 @@ type Device struct {
 //
 // This function only creates the Device object, it does not touch the device.
 func New(bus drivers.I2C) Device {
-	return Device{bus, Address}
+	return Device{bus: bus, Address: Address}
 }
 
 // Connected returns whether a MPU6050 has been found.

mpu6050/mpu6050_stored.go

@@ -0,0 +1,210 @@
+package mpu6050
+
+import (
+	"errors"
+
+	"tinygo.org/x/drivers"
+)
+
+// DeviceStored reads all IMU data in a single i2c transaction
+// to reduce bus usage. It is a better alternative to a simple Device
+// if getting gyroscopes+acceleration measurements together many times in
+// a second.
+type DeviceStored struct {
+	Device
+	gyroRange  int16
+	accelRange int16
+	// buf is bufgfer for
+	buf [1]byte
+	// data contains IMU readings
+	data [14]byte
+}
+
+// NewStored creates a versatile instance of a mpu6050 device handle.
+func NewStored(bus drivers.I2C) DeviceStored {
+	return DeviceStored{Device: New(bus), accelRange: 2, gyroRange: 250}
+}
+
+type Config struct {
+	// Set to one of ACCEL_RANGE_X register values
+	AccRange byte
+	// Set to one of GYRO_RANGE_x register values
+	GyroRange byte
+	// SetSampleRate sets the sample rate for the FIFO,
+	// register ouput and DMP. The sample rate is determined
+	// by:
+	//		SR = Gyroscope Output Rate / (1 + srDiv)
+	SampleRatio byte
+	ClkSel      byte
+}
+
+// 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 (d *DeviceStored) Configure(data Config) (err error) {
+	// setClockSource
+	if err = d.SetClockSource(data.ClkSel); err != nil {
+		return err
+	}
+	// setSampleRate
+	if err = d.SetSampleRate(data.SampleRatio); err != nil {
+		return err
+	}
+	// setFullScaleGyroRange
+	if err = d.SetGyroRange(data.GyroRange); err != nil {
+		return err
+	}
+	// setFullScaleAccelRange
+	if err = d.SetAccelRange(data.AccRange); err != nil {
+		return err
+	}
+	// setSleep
+	if err = d.SetSleep(false); 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.
+func (d *DeviceStored) Acceleration() (ax, ay, az int32) {
+	return int32(int16((uint16(d.buf[0])<<8)|uint16(d.data[1]))) * 15625 / 512 * int32(d.accelRange),
+		int32(int16((uint16(d.data[2])<<8)|uint16(d.data[3]))) * 15625 / 512 * int32(d.accelRange),
+		int32(int16((uint16(d.data[4])<<8)|uint16(d.data[5]))) * 15625 / 512 * int32(d.accelRange)
+}
+
+// Rotations 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
+// rotation along one axis and while doing so integrate all values over time,
+// you would get a value close to 360000000.
+func (d *DeviceStored) Rotation() (gx, gy, gz int32) {
+	return int32(int16((uint16(d.data[8])<<8)|uint16(d.data[9]))) * 15625 / 2048 * int32(d.gyroRange) * 4,
+		int32(int16((uint16(d.data[10])<<8)|uint16(d.data[11]))) * 15625 / 2048 * int32(d.gyroRange) * 4,
+		int32(int16((uint16(d.data[12])<<8)|uint16(d.data[13]))) * 15625 / 2048 * int32(d.gyroRange) * 4
+}
+
+// Temperature returns the temperature of the device in centigrade.
+func (d *DeviceStored) Temperature() (Celsius int16) {
+	return ((int16(d.data[6])<<8)|int16(d.data[7]))/340 + 37 // float64(temp/340) + 36.53
+}
+
+// Get reads IMU data and stores it inside DeviceStored. The data can then be accessed through Rotation and Acceleration
+// methods.
+func (d *DeviceStored) Get() error {
+	d.bus.ReadRegister(uint8(d.Address), ACCEL_XOUT_H, d.data[:14])
+	return nil
+}
+
+// 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 (d *DeviceStored) SetSampleRate(srDiv byte) (err error) {
+	// setSampleRate
+	d.buf[0] = srDiv
+	if err = d.bus.WriteRegister(uint8(d.Address), SMPLRT_DIV, d.buf[:1]); err != nil {
+		return err
+	}
+	return nil
+}
+
+// SetClockSource configures the source of the clock
+// for the peripheral.
+func (d *DeviceStored) SetClockSource(clkSel byte) (err error) {
+	if err = d.bus.ReadRegister(uint8(d.Address), PWR_MGMT_1, d.buf[:1]); err != nil {
+		return err
+	}
+
+	d.buf[0] = (d.buf[0] & (^CLK_SEL_Msk)) | clkSel // Write CLKSEL field
+	if err = d.bus.WriteRegister(uint8(d.Address), PWR_MGMT_1, d.buf[:1]); 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 *DeviceStored) SetGyroRange(gyroRange byte) (err error) {
+	switch gyroRange {
+	case GYRO_RANGE_250:
+		p.gyroRange = 250
+	case GYRO_RANGE_500:
+		p.gyroRange = 500
+	case GYRO_RANGE_1000:
+		p.gyroRange = 1000
+	case GYRO_RANGE_2000:
+		p.gyroRange = 2000
+	default:
+		return errors.New("invalid gyro FSR input")
+	}
+	// setFullScaleGyroRange
+	if err = p.bus.ReadRegister(uint8(p.Address), GYRO_CONFIG, p.buf[:1]); err != nil {
+		return err
+	}
+	p.buf[0] = (p.buf[0] & (^G_FS_SEL)) | (gyroRange << G_FS_Pos) // Write FS_SEL field
+
+	if err = p.bus.WriteRegister(uint8(p.Address), GYRO_CONFIG, p.buf[:1]); 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 (d *DeviceStored) SetAccelRange(accRange byte) (err error) {
+	switch accRange {
+	case ACCEL_RANGE_2:
+		d.accelRange = 2
+	case ACCEL_RANGE_4:
+		d.accelRange = 4
+	case ACCEL_RANGE_8:
+		d.accelRange = 8
+	case ACCEL_RANGE_16:
+		d.accelRange = 16
+	default:
+		return errors.New("invalid accelerometer FSR input")
+	}
+
+	if err = d.bus.ReadRegister(uint8(d.Address), ACCEL_CONFIG, d.buf[:1]); err != nil {
+		return err
+	}
+	d.buf[0] = (d.buf[0] & (^AFS_SEL)) | (accRange << AFS_Pos) // Write only FS_SEL field
+
+	if err = d.bus.WriteRegister(uint8(d.Address), ACCEL_CONFIG, d.buf[:1]); 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 (d *DeviceStored) SetSleep(sleep bool) (err error) {
+	if err = d.bus.ReadRegister(uint8(d.Address), PWR_MGMT_1, d.buf[:1]); err != nil {
+		return err
+	}
+	if sleep {
+		d.buf[0] = (d.buf[0] & (^SLEEP_Msk)) | (1 << SLEEP_Pos) // Set CLK_SEL bits only
+	} else {
+		d.buf[0] = (d.buf[0] & (^SLEEP_Msk))
+	}
+
+	if err = d.bus.WriteRegister(uint8(d.Address), PWR_MGMT_1, d.buf[:1]); err != nil {
+		return err
+	}
+	return nil
+}

mpu6050/registers.go

@@ -107,3 +107,34 @@ const (
 	FIFO_R_W        = 0x74 // FIFO read/write
 	WHO_AM_I        = 0x75 // Who am I
 )
+
+//MPU 6050 MASKS
+const (
+	G_FS_SEL    uint8 = 0x18
+	AFS_SEL     uint8 = 0x18
+	CLK_SEL_Msk uint8 = 0x07
+	SLEEP_Msk   uint8 = 0x40
+)
+
+//MPU 6050 SHIFTS
+const (
+	AFS_Pos   uint8 = 3
+	G_FS_Pos  uint8 = 3
+	SLEEP_Pos uint8 = 6
+)
+
+// Gyroscope ranges for Init configuration
+const (
+	GYRO_RANGE_250 byte = iota
+	GYRO_RANGE_500
+	GYRO_RANGE_1000
+	GYRO_RANGE_2000
+)
+
+// Accelerometer ranges for Init configuration
+const (
+	ACCEL_RANGE_2 byte = iota
+	ACCEL_RANGE_4
+	ACCEL_RANGE_8
+	ACCEL_RANGE_16
+)