From ad6be40966a1d6f04f3c6a3499341aae93f77efc Mon Sep 17 00:00:00 2001 From: Yurii Soldak Date: Mon, 30 Aug 2021 20:10:46 +0200 Subject: [PATCH] lsm6dox: minimal correct implementation --- examples/lsm6dsox/main.go | 108 ++++++++++++++++++++++++++++++++++ lsm6dsox/lsm6dsox.go | 120 ++++++++++++++++++++++++++++++++++++++ lsm6dsox/registers.go | 71 ++++++++++++++++++++++ 3 files changed, 299 insertions(+) create mode 100644 examples/lsm6dsox/main.go create mode 100644 lsm6dsox/lsm6dsox.go create mode 100644 lsm6dsox/registers.go diff --git a/examples/lsm6dsox/main.go b/examples/lsm6dsox/main.go new file mode 100644 index 0000000..e962d29 --- /dev/null +++ b/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 +} diff --git a/lsm6dsox/lsm6dsox.go b/lsm6dsox/lsm6dsox.go new file mode 100644 index 0000000..050728f --- /dev/null +++ b/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 +} diff --git a/lsm6dsox/registers.go b/lsm6dsox/registers.go new file mode 100644 index 0000000..b57c586 --- /dev/null +++ b/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 +)