lsm6dox: minimal correct implementation

examples/lsm6dsox/main.go

@@ -0,0 +1,108 @@
+// Connects to an LSM6DSOX I2C a 6 axis Inertial Measurement Unit (IMU)
+package main
+
+import (
+	"fmt"
+	"machine"
+	"time"
+
+	"tinygo.org/x/drivers/lsm6dsox"
+)
+
+const (
+	PLOTTER           = true
+	SHOW_ACCELERATION = false
+	SHOW_ROTATION     = true
+	SHOW_TEMPERATURE  = false
+
+	// calibration is naive, good enough for a demo: do not shake a second after flashing
+	GYRO_CALIBRATION = true
+)
+
+var cal = [...]float32{0, 0, 0}
+
+func main() {
+
+	machine.I2C0.Configure(machine.I2CConfig{})
+
+	device := lsm6dsox.New(machine.I2C0)
+	device.Configure(lsm6dsox.Configuration{
+		AccelRange:      lsm6dsox.ACCEL_2G,
+		AccelSampleRate: lsm6dsox.ACCEL_SR_104,
+		GyroRange:       lsm6dsox.GYRO_250DPS,
+		GyroSampleRate:  lsm6dsox.GYRO_SR_104,
+	})
+
+	for {
+
+		if !device.Connected() {
+			println("LSM6DSOX not connected")
+			time.Sleep(time.Second)
+			continue
+		}
+
+		// heuristic: after successful calibration the value can't be 0
+		if GYRO_CALIBRATION && cal[0] == 0 {
+			calibrateGyro(device)
+		}
+
+		ax, ay, az := device.ReadAcceleration()
+		gx, gy, gz := device.ReadRotation()
+		t, _ := device.ReadTemperature()
+
+		if PLOTTER {
+			printPlotter(ax, ay, az, gx, gy, gz, t)
+			time.Sleep(time.Millisecond * 100)
+		} else {
+			printMonitor(ax, ay, az, gx, gy, gz, t)
+			time.Sleep(time.Millisecond * 1000)
+		}
+
+	}
+
+}
+
+func calibrateGyro(device *lsm6dsox.Device) {
+	for i := 0; i < 100; i++ {
+		gx, gy, gz := device.ReadRotation()
+		cal[0] += float32(gx) / 1000000
+		cal[1] += float32(gy) / 1000000
+		cal[2] += float32(gz) / 1000000
+		time.Sleep(time.Millisecond * 10)
+	}
+	cal[0] /= 100
+	cal[1] /= 100
+	cal[2] /= 100
+}
+
+// Arduino IDE's Serial Plotter
+func printPlotter(ax, ay, az, gx, gy, gz, t int32) {
+	if SHOW_ACCELERATION {
+		fmt.Printf("AX:%f, AY:%f, AZ:%f,", axis(ax, 0), axis(ay, 0), axis(az, 0))
+	}
+	if SHOW_ROTATION {
+		fmt.Printf("GX:%f, GY:%f, GZ:%f,", axis(gx, cal[0]), axis(gy, cal[1]), axis(gz, cal[2]))
+	}
+	if SHOW_TEMPERATURE {
+		fmt.Printf("T:%f", float32(t)/1000)
+	}
+	println()
+}
+
+// Any Serial Monitor
+func printMonitor(ax, ay, az, gx, gy, gz, t int32) {
+	if SHOW_ACCELERATION {
+		fmt.Printf("Acceleration: %f, %f, %f\r\n", axis(ax, 0), axis(ay, 0), axis(az, 0))
+	}
+	if SHOW_ROTATION {
+		fmt.Printf("Rotation: %f, %f, %f\r\n", axis(gx, cal[0]), axis(gy, cal[1]), axis(gz, cal[2]))
+	}
+	if SHOW_TEMPERATURE {
+		fmt.Printf("Temperature C: %f\r\n", float32(t)/1000)
+	}
+	println()
+}
+
+func axis(raw int32, cal float32) float32 {
+	return float32(raw)/1000000 - cal
+}

lsm6dsox/lsm6dsox.go

@@ -0,0 +1,120 @@
+// Package lsm6dsox implements a driver for the LSM6DSOX
+// a 6 axis Inertial Measurement Unit (IMU)
+//
+// Datasheet: https://www.st.com/resource/en/datasheet/lsm6dsox.pdf
+//
+package lsm6dsox // import "tinygo.org/x/drivers/lsm6dsox"
+
+import "tinygo.org/x/drivers"
+
+type AccelRange uint8
+type AccelSampleRate uint8
+
+type GyroRange uint8
+type GyroSampleRate uint8
+
+// Device wraps an I2C connection to a LSM6DSOX device.
+type Device struct {
+	bus             drivers.I2C
+	Address         uint16
+	dataBufferSix   []uint8
+	dataBufferTwo   []uint8
+	accelMultiplier int32
+	gyroMultiplier  int32
+}
+
+// Configuration for LSM6DSOX device.
+type Configuration struct {
+	AccelRange      AccelRange
+	AccelSampleRate AccelSampleRate
+	GyroRange       GyroRange
+	GyroSampleRate  GyroSampleRate
+}
+
+// New creates a new LSM6DSOX 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:           bus,
+		Address:       Address,
+		dataBufferSix: make([]uint8, 6),
+		dataBufferTwo: make([]uint8, 2),
+	}
+}
+
+// Configure sets up the device for communication.
+func (d *Device) Configure(cfg Configuration) {
+
+	// Multipliers come from "Table 2. Mechanical characteristics" of the datasheet * 1000
+	switch cfg.AccelRange {
+	case ACCEL_2G:
+		d.accelMultiplier = 61
+	case ACCEL_4G:
+		d.accelMultiplier = 122
+	case ACCEL_8G:
+		d.accelMultiplier = 244
+	case ACCEL_16G:
+		d.accelMultiplier = 488
+	}
+	switch cfg.GyroRange {
+	case GYRO_250DPS:
+		d.gyroMultiplier = 8750
+	case GYRO_500DPS:
+		d.gyroMultiplier = 17500
+	case GYRO_1000DPS:
+		d.gyroMultiplier = 35000
+	case GYRO_2000DPS:
+		d.gyroMultiplier = 70000
+	}
+
+	data := make([]uint8, 1)
+	// Configure accelerometer
+	data[0] = uint8(cfg.AccelRange) | uint8(cfg.AccelSampleRate)
+	d.bus.WriteRegister(uint8(d.Address), CTRL1_XL, data)
+	// Configure gyroscope
+	data[0] = uint8(cfg.GyroRange) | uint8(cfg.GyroSampleRate)
+	d.bus.WriteRegister(uint8(d.Address), CTRL2_G, data)
+}
+
+// Connected returns whether a LSM6DSOX 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] == 0x6C
+}
+
+// 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
+// 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) {
+	d.bus.ReadRegister(uint8(d.Address), OUTX_L_A, d.dataBufferSix)
+	x = int32(int16((uint16(d.dataBufferSix[1])<<8)|uint16(d.dataBufferSix[0]))) * d.accelMultiplier
+	y = int32(int16((uint16(d.dataBufferSix[3])<<8)|uint16(d.dataBufferSix[2]))) * d.accelMultiplier
+	z = int32(int16((uint16(d.dataBufferSix[5])<<8)|uint16(d.dataBufferSix[4]))) * d.accelMultiplier
+	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
+// 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) {
+	d.bus.ReadRegister(uint8(d.Address), OUTX_L_G, d.dataBufferSix)
+	x = int32(int16((uint16(d.dataBufferSix[1])<<8)|uint16(d.dataBufferSix[0]))) * d.gyroMultiplier
+	y = int32(int16((uint16(d.dataBufferSix[3])<<8)|uint16(d.dataBufferSix[2]))) * d.gyroMultiplier
+	z = int32(int16((uint16(d.dataBufferSix[5])<<8)|uint16(d.dataBufferSix[4]))) * d.gyroMultiplier
+	return
+}
+
+// ReadTemperature returns the temperature in celsius milli degrees (°C/1000)
+func (d *Device) ReadTemperature() (int32, error) {
+	d.bus.ReadRegister(uint8(d.Address), OUT_TEMP_L, d.dataBufferTwo)
+
+	// From "Table 4. Temperature sensor characteristics"
+	// temp = value/256 + 25
+	t := 25000 + (int32(int16((int16(d.dataBufferTwo[1])<<8)|int16(d.dataBufferTwo[0])))*125)/32
+	return t, nil
+}

lsm6dsox/registers.go

@@ -0,0 +1,71 @@
+package lsm6dsox
+
+// Constants/addresses used for I2C.
+
+// The I2C address which this device listens to.
+const Address = 0x6A
+
+const (
+	INT1_CTRL  = 0x0D
+	INT2_CTRL  = 0x0E
+	WHO_AM_I   = 0x0F
+	CTRL1_XL   = 0x10 // Accelerometer control register 1 (r/w)
+	CTRL2_G    = 0x11 // Gyroscope control register 2 (r/w)
+	CTRL3_C    = 0x12
+	CTRL4_C    = 0x13
+	CTRL5_C    = 0x14
+	CTRL6_C    = 0x15
+	CTRL7_G    = 0x16
+	CTRL8_XL   = 0x17
+	CTRL9_XL   = 0x18
+	CTRL10_C   = 0x19
+	STATUS_REG = 0x1E
+	OUT_TEMP_L = 0x20
+	OUT_TEMP_H = 0x21
+	OUTX_L_G   = 0x22
+	OUTX_H_G   = 0x23
+	OUTY_L_G   = 0x24
+	OUTY_H_G   = 0x25
+	OUTZ_L_G   = 0x26
+	OUTZ_H_G   = 0x27
+	OUTX_L_A   = 0x28
+	OUTX_H_A   = 0x29
+	OUTY_L_A   = 0x2A
+	OUTY_H_A   = 0x2B
+	OUTZ_L_A   = 0x2C
+	OUTZ_H_A   = 0x2D
+
+	ACCEL_2G  AccelRange = 0x00
+	ACCEL_4G  AccelRange = 0x08
+	ACCEL_8G  AccelRange = 0x0C
+	ACCEL_16G AccelRange = 0x04
+
+	ACCEL_SR_OFF  AccelSampleRate = 0x00
+	ACCEL_SR_13   AccelSampleRate = 0x10
+	ACCEL_SR_26   AccelSampleRate = 0x20
+	ACCEL_SR_52   AccelSampleRate = 0x30
+	ACCEL_SR_104  AccelSampleRate = 0x40
+	ACCEL_SR_208  AccelSampleRate = 0x50
+	ACCEL_SR_416  AccelSampleRate = 0x60
+	ACCEL_SR_833  AccelSampleRate = 0x70
+	ACCEL_SR_1666 AccelSampleRate = 0x80
+	ACCEL_SR_3332 AccelSampleRate = 0x90
+	ACCEL_SR_6664 AccelSampleRate = 0xA0
+
+	GYRO_250DPS  GyroRange = 0x00
+	GYRO_500DPS  GyroRange = 0x04
+	GYRO_1000DPS GyroRange = 0x08
+	GYRO_2000DPS GyroRange = 0x0C
+
+	GYRO_SR_OFF  GyroSampleRate = 0x00
+	GYRO_SR_13   GyroSampleRate = 0x10
+	GYRO_SR_26   GyroSampleRate = 0x20
+	GYRO_SR_52   GyroSampleRate = 0x30
+	GYRO_SR_104  GyroSampleRate = 0x40
+	GYRO_SR_208  GyroSampleRate = 0x50
+	GYRO_SR_416  GyroSampleRate = 0x60
+	GYRO_SR_833  GyroSampleRate = 0x70
+	GYRO_SR_1666 GyroSampleRate = 0x80
+	GYRO_SR_3332 GyroSampleRate = 0x90
+	GYRO_SR_6664 GyroSampleRate = 0xA0
+)