From 3ae5895183b191a3b551f7119aba88edf040f2ef Mon Sep 17 00:00:00 2001 From: Daniel Esteban Date: Mon, 26 Aug 2019 08:56:48 +0200 Subject: [PATCH] Initial support for LSM6DS3 IMU (#83) * Initial support for LSM6DS3 IMU --- Makefile | 1 + README.md | 1 + examples/lsm6ds3/main.go | 30 +++++++ lsm6ds3/lsm6ds3.go | 183 +++++++++++++++++++++++++++++++++++++++ lsm6ds3/registers.go | 83 ++++++++++++++++++ 5 files changed, 298 insertions(+) create mode 100644 examples/lsm6ds3/main.go create mode 100644 lsm6ds3/lsm6ds3.go create mode 100644 lsm6ds3/registers.go diff --git a/Makefile b/Makefile index 7315899..c7a502e 100644 --- a/Makefile +++ b/Makefile @@ -28,6 +28,7 @@ smoke-test: tinygo build -size short -o ./build/test.elf -target=microbit ./examples/hd44780/text/main.go tinygo build -size short -o ./build/test.elf -target=microbit ./examples/hub75/main.go tinygo build -size short -o ./build/test.elf -target=circuitplay-express ./examples/lis3dh/main.go + tinygo build -size short -o ./build/test.elf -target=arduino-nano33 ./examples/lsm6ds3/main.go tinygo build -size short -o ./build/test.elf -target=itsybitsy-m0 ./examples/mag3110/main.go tinygo build -size short -o ./build/test.elf -target=microbit ./examples/microbitmatrix/main.go tinygo build -size short -o ./build/test.elf -target=itsybitsy-m0 ./examples/mma8653/main.go diff --git a/README.md b/README.md index 70eb8a6..a0aa8a1 100644 --- a/README.md +++ b/README.md @@ -69,6 +69,7 @@ func main() { | [GPS module](https://www.u-blox.com/en/product/neo-6-series) | I2C/UART | | [HUB75 RGB led matrix](https://cdn-learn.adafruit.com/downloads/pdf/32x16-32x32-rgb-led-matrix.pdf) | SPI | | [LIS3DH accelerometer](https://www.st.com/resource/en/datasheet/lis3dh.pdf) | I2C | +| [LSM6DS3 accelerometer](https://www.st.com/resource/en/datasheet/lsm6ds3.pdf) | I2C | | [MAG3110 magnetometer](https://www.nxp.com/docs/en/data-sheet/MAG3110.pdf) | I2C | | [BBC micro:bit LED matrix](https://github.com/bbcmicrobit/hardware/blob/master/SCH_BBC-Microbit_V1.3B.pdf) | GPIO | | [Microphone - PDM](https://cdn-learn.adafruit.com/assets/assets/000/049/977/original/MP34DT01-M.pdf) | I2S/PDM | diff --git a/examples/lsm6ds3/main.go b/examples/lsm6ds3/main.go new file mode 100644 index 0000000..88b85d1 --- /dev/null +++ b/examples/lsm6ds3/main.go @@ -0,0 +1,30 @@ +// Connects to an LSM6DS3 I2C a 6 axis Inertial Measurement Unit (IMU) +package main + +import ( + "machine" + "time" + + "tinygo.org/x/drivers/lsm6ds3" +) + +func main() { + machine.I2C0.Configure(machine.I2CConfig{}) + + accel := lsm6ds3.New(machine.I2C0) + accel.Configure(lsm6ds3.Configuration{}) + if !accel.Connected() { + println("LSM6DS3 not connected") + return + } + + for { + x, y, z := accel.ReadAcceleration() + println("Acceleration:", float32(x)/1000000, float32(y)/1000000, float32(z)/1000000) + x, y, z = accel.ReadRotation() + println("Gyroscope:", float32(x)/1000000, float32(y)/1000000, float32(z)/1000000) + x, _ = accel.ReadTemperature() + println("Degrees C", float32(x)/1000, "\n\n") + time.Sleep(time.Millisecond * 1000) + } +} diff --git a/lsm6ds3/lsm6ds3.go b/lsm6ds3/lsm6ds3.go new file mode 100644 index 0000000..d6a3e73 --- /dev/null +++ b/lsm6ds3/lsm6ds3.go @@ -0,0 +1,183 @@ +// Package lsm6ds3 implements a driver for the LSM6DS3 a 6 axis Inertial +// Measurement Unit (IMU) +// +// Datasheet: https://www.st.com/resource/en/datasheet/lsm6ds3.pdf +// +package lsm6ds3 // import "tinygo.org/x/drivers/lsm6ds3" + +import ( + "machine" +) + +type AccelRange uint8 +type AccelSampleRate uint8 +type AccelBandwidth uint8 + +type GyroRange uint8 +type GyroSampleRate uint8 + +// Device wraps an I2C connection to a LSM6DS3 device. +type Device struct { + bus machine.I2C + Address uint16 + accelRange AccelRange + accelSampleRate AccelSampleRate + accelBandWidth AccelBandwidth + gyroRange GyroRange + gyroSampleRate GyroSampleRate + dataBufferSix []uint8 + dataBufferTwo []uint8 +} + +// Configuration for LSM6DS3 device. +type Configuration struct { + AccelRange AccelRange + AccelSampleRate AccelSampleRate + AccelBandWidth AccelBandwidth + GyroRange GyroRange + GyroSampleRate GyroSampleRate + IsPedometer bool + ResetStepCounter bool +} + +// New creates a new LSM6DS3 connection. The I2C bus must already be +// configured. +// +// This function only creates the Device object, it does not touch the device. +func New(bus machine.I2C) Device { + return Device{bus: bus, Address: Address} +} + +// Configure sets up the device for communication. +func (d *Device) Configure(cfg Configuration) { + if cfg.AccelRange != 0 { + d.accelRange = cfg.AccelRange + } else { + d.accelRange = ACCEL_2G + } + + if cfg.AccelSampleRate != 0 { + d.accelSampleRate = cfg.AccelSampleRate + } else { + d.accelSampleRate = ACCEL_SR_104 + } + + if cfg.AccelBandWidth != 0 { + d.accelBandWidth = cfg.AccelBandWidth + } else { + d.accelBandWidth = ACCEL_BW_100 + } + + if cfg.GyroRange != 0 { + d.gyroRange = cfg.GyroRange + } else { + d.gyroRange = GYRO_2000DPS + } + + if cfg.GyroSampleRate != 0 { + d.gyroSampleRate = cfg.GyroSampleRate + } else { + d.gyroSampleRate = GYRO_SR_104 + } + + d.dataBufferSix = make([]uint8, 6) + d.dataBufferTwo = make([]uint8, 2) + + if cfg.IsPedometer { // CONFIGURE AS PEDOMETER + // Configure accelerometer: 2G + 26Hz + d.bus.WriteRegister(uint8(d.Address), CTRL1_XL, []byte{uint8(ACCEL_2G) | uint8(ACCEL_SR_26)}) + + // Configure Zen_G, Yen_G, Xen_G, reset steps + if cfg.ResetStepCounter { + d.bus.WriteRegister(uint8(d.Address), CTRL10_C, []byte{0x3E}) + } else { + d.bus.WriteRegister(uint8(d.Address), CTRL10_C, []byte{0x3C}) + } + + // Enable pedometer + d.bus.WriteRegister(uint8(d.Address), TAP_CFG, []byte{0x40}) + } else { // NORMAL USE + // Configure accelerometer + data := make([]uint8, 1) + data[0] = uint8(d.accelRange) | uint8(d.accelSampleRate) | uint8(d.accelBandWidth) + d.bus.WriteRegister(uint8(d.Address), CTRL1_XL, data) + + // Set ODR bit + d.bus.ReadRegister(uint8(d.Address), CTRL4_C, data) + data[0] = data[0] &^ BW_SCAL_ODR_ENABLED + data[0] |= BW_SCAL_ODR_ENABLED + d.bus.WriteRegister(uint8(d.Address), CTRL4_C, data) + + // Configure gyroscope + data[0] = uint8(d.gyroRange) | uint8(d.gyroSampleRate) + d.bus.WriteRegister(uint8(d.Address), CTRL2_G, data) + } +} + +// Connected returns whether a LSM6DS3 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] == 0x69 +} + +// 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_XL, d.dataBufferSix) + // k comes from "Table 3. Mechanical characteristics" 3 of the datasheet * 1000 + k := int32(61) // 2G + if d.accelRange == ACCEL_4G { + k = 122 + } else if d.accelRange == ACCEL_8G { + k = 244 + } else if d.accelRange == ACCEL_16G { + k = 488 + } + x = int32(int16((uint16(d.dataBufferSix[1])<<8)|uint16(d.dataBufferSix[0]))) * k + y = int32(int16((uint16(d.dataBufferSix[3])<<8)|uint16(d.dataBufferSix[2]))) * k + z = int32(int16((uint16(d.dataBufferSix[5])<<8)|uint16(d.dataBufferSix[4]))) * k + 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) + // k comes from "Table 3. Mechanical characteristics" 3 of the datasheet * 1000 + k := int32(4375) // 125DPS + if d.gyroRange == GYRO_250DPS { + k = 8750 + } else if d.gyroRange == GYRO_500DPS { + k = 17500 + } else if d.gyroRange == GYRO_1000DPS { + k = 35000 + } else if d.gyroRange == GYRO_2000DPS { + k = 70000 + } + x = int32(int16((uint16(d.dataBufferSix[1])<<8)|uint16(d.dataBufferSix[0]))) * k + y = int32(int16((uint16(d.dataBufferSix[3])<<8)|uint16(d.dataBufferSix[2]))) * k + z = int32(int16((uint16(d.dataBufferSix[5])<<8)|uint16(d.dataBufferSix[4]))) * k + 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 5. Temperature sensor characteristics" + // temp = value/16 + 25 + t := 25000 + (int32(int16((int16(d.dataBufferTwo[1])<<8)|int16(d.dataBufferTwo[0])))*125)/2 + return t, nil +} + +// ReadSteps returns the steps of the pedometer +func (d *Device) ReadSteps() int32 { + d.bus.ReadRegister(uint8(d.Address), STEP_COUNTER_L, d.dataBufferTwo) + return int32(int16((uint16(d.dataBufferTwo[1]) << 8) | uint16(d.dataBufferTwo[0]))) +} diff --git a/lsm6ds3/registers.go b/lsm6ds3/registers.go new file mode 100644 index 0000000..abb51b1 --- /dev/null +++ b/lsm6ds3/registers.go @@ -0,0 +1,83 @@ +package lsm6ds3 + +// Constants/addresses used for I2C. + +// The I2C address which this device listens to. +const Address = 0x6A + +const ( + WHO_AM_I = 0x0F + STATUS = 0x1E + CTRL1_XL = 0x10 + CTRL2_G = 0x11 + CTRL3_C = 0x12 + CTRL4_C = 0x13 + CTRL5_C = 0x14 + CTRL6_C = 0x15 + CTRL7_G = 0x16 + CTRL8_XL = 0x17 + CTRL9_XL = 0x18 + CTRL10_C = 0x19 + 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_XL = 0x28 + OUTX_H_XL = 0x29 + OUTY_L_XL = 0x2A + OUTY_H_XL = 0x2B + OUTZ_L_XL = 0x2C + OUTZ_H_XL = 0x2D + OUT_TEMP_L = 0x20 + OUT_TEMP_H = 0x21 + BW_SCAL_ODR_DISABLED = 0x00 + BW_SCAL_ODR_ENABLED = 0x80 + STEP_TIMESTAMP_L = 0x49 + STEP_TIMESTAMP_H = 0x4A + STEP_COUNTER_L = 0x4B + STEP_COUNTER_H = 0x4C + STEP_COUNT_DELTA = 0x15 + TAP_CFG = 0x58 + INT1_CTRL = 0x0D + + 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 + ACCEL_SR_13330 AccelSampleRate = 0xB0 + + ACCEL_BW_50 AccelBandwidth = 0x03 + ACCEL_BW_100 AccelBandwidth = 0x02 + ACCEL_BW_200 AccelBandwidth = 0x01 + ACCEL_BW_400 AccelBandwidth = 0x00 + + //GYRO_125DPS GyroRange = 0x01 + 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 +)