lsm6ds3tr: initial implementation

examples/lsm6ds3tr/main.go

@@ -0,0 +1,37 @@
+// Connects to an LSM6DS3TR I2C a 6 axis Inertial Measurement Unit (IMU)
+package main
+
+import (
+	"machine"
+	"time"
+
+	"tinygo.org/x/drivers/lsm6ds3tr"
+)
+
+func main() {
+	machine.I2C0.Configure(machine.I2CConfig{})
+
+	accel := lsm6ds3tr.New(machine.I2C0)
+	err := accel.Configure(lsm6ds3tr.Configuration{})
+	if err != nil {
+		for {
+			println("Failed to configure", err.Error())
+			time.Sleep(time.Second)
+		}
+	}
+
+	for {
+		if !accel.Connected() {
+			println("LSM6DS3TR not connected")
+			time.Sleep(time.Second)
+			continue
+		}
+		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)
+	}
+}

lsm6ds3tr/lsm6ds3tr.go

@@ -0,0 +1,189 @@
+// Package lsm6ds3tr implements a driver for the LSM6DS3TR
+// a 6 axis Inertial Measurement Unit (IMU)
+//
+// Datasheet: https://www.st.com/resource/en/datasheet/lsm6ds3tr.pdf
+//
+package lsm6ds3tr // import "tinygo.org/x/drivers/lsm6ds3tr"
+
+import (
+	"errors"
+
+	"tinygo.org/x/drivers"
+)
+
+type AccelRange uint8
+type AccelSampleRate uint8
+type AccelBandwidth uint8
+
+type GyroRange uint8
+type GyroSampleRate uint8
+
+// Device wraps an I2C connection to a LSM6DS3TR device.
+type Device struct {
+	bus             drivers.I2C
+	Address         uint16
+	accelRange      AccelRange
+	accelSampleRate AccelSampleRate
+	gyroRange       GyroRange
+	gyroSampleRate  GyroSampleRate
+	buf             [6]uint8
+}
+
+// Configuration for LSM6DS3TR device.
+type Configuration struct {
+	AccelRange       AccelRange
+	AccelSampleRate  AccelSampleRate
+	AccelBandWidth   AccelBandwidth
+	GyroRange        GyroRange
+	GyroSampleRate   GyroSampleRate
+	IsPedometer      bool
+	ResetStepCounter bool
+}
+
+var errNotConnected = errors.New("lsm6ds3tr: failed to communicate with acel/gyro sensor")
+
+// New creates a new LSM6DS3TR 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,
+	}
+}
+
+// Configure sets up the device for communication.
+func (d *Device) doConfigure(cfg Configuration) (err error) {
+
+	// Verify unit communication
+	if !d.Connected() {
+		return errNotConnected
+	}
+
+	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.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
+	}
+
+	data := d.buf[:1]
+
+	// Configure accelerometer
+	data[0] = uint8(d.accelRange) | uint8(d.accelSampleRate)
+	err = d.bus.WriteRegister(uint8(d.Address), CTRL1_XL, data)
+	if err != nil {
+		return
+	}
+
+	// Set ODR bit
+	err = d.bus.ReadRegister(uint8(d.Address), CTRL4_C, data)
+	if err != nil {
+		return
+	}
+	data[0] = data[0] &^ BW_SCAL_ODR_ENABLED
+	data[0] |= BW_SCAL_ODR_ENABLED
+	err = d.bus.WriteRegister(uint8(d.Address), CTRL4_C, data)
+	if err != nil {
+		return
+	}
+
+	// Configure gyroscope
+	data[0] = uint8(d.gyroRange) | uint8(d.gyroSampleRate)
+	err = d.bus.WriteRegister(uint8(d.Address), CTRL2_G, data)
+	if err != nil {
+		return
+	}
+
+	return nil
+}
+
+// Connected returns whether a LSM6DS3TR has been found.
+// It does a "who am I" request and checks the response.
+func (d *Device) Connected() bool {
+	data := d.buf[:1]
+	d.bus.ReadRegister(uint8(d.Address), WHO_AM_I, data)
+	return data[0] == 0x6A
+}
+
+// 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, y, z int32, err error) {
+	data := d.buf[:6]
+	err = d.bus.ReadRegister(uint8(d.Address), OUTX_L_XL, data)
+	if err != nil {
+		return
+	}
+	// 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(data[1])<<8)|uint16(data[0]))) * k
+	y = int32(int16((uint16(data[3])<<8)|uint16(data[2]))) * k
+	z = int32(int16((uint16(data[5])<<8)|uint16(data[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, y, z int32, err error) {
+	data := d.buf[:6]
+	err = d.bus.ReadRegister(uint8(d.Address), OUTX_L_G, data)
+	if err != nil {
+		return
+	}
+	// k comes from "Table 3. Mechanical characteristics" 3 of the datasheet * 1000
+	k := int32(4375) // 125DPS
+	if d.gyroRange == GYRO_245DPS {
+		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(data[1])<<8)|uint16(data[0]))) * k
+	y = int32(int16((uint16(data[3])<<8)|uint16(data[2]))) * k
+	z = int32(int16((uint16(data[5])<<8)|uint16(data[4]))) * k
+	return
+}
+
+// ReadTemperature returns the temperature in celsius milli degrees (°C/1000)
+func (d *Device) ReadTemperature() (t int32, err error) {
+	data := d.buf[:2]
+	err = d.bus.ReadRegister(uint8(d.Address), OUT_TEMP_L, data)
+	if err != nil {
+		return
+	}
+	// From "Table 5. Temperature sensor characteristics"
+	// temp = value/256 + 25
+	t = 25000 + (int32(int16((int16(data[1])<<8)|int16(data[0])))*125)/32
+	return
+}

lsm6ds3tr/lsm6ds3tr_generic.go

@@ -0,0 +1,9 @@
+//go:build !xiao_ble
+// +build !xiao_ble
+
+package lsm6ds3tr
+
+// Configure sets up the device for communication.
+func (d *Device) Configure(cfg Configuration) error {
+	return d.doConfigure(cfg)
+}

lsm6ds3tr/lsm6ds3tr_xiao_ble.go

@@ -0,0 +1,35 @@
+//go:build xiao_ble
+// +build xiao_ble
+
+package lsm6ds3tr
+
+import (
+	"device/nrf"
+	"machine"
+	"time"
+)
+
+// Configure sets up the device for communication.
+func (d *Device) Configure(cfg Configuration) error {
+
+	// Following lines are XIAO BLE Sense specific, they have nothing to do with sensor per se
+	// Implementation adapted from https://github.com/Seeed-Studio/Seeed_Arduino_LSM6DS3/blob/master/LSM6DS3.cpp#L68-L77
+
+	// Special mode for IMU power pin on this board.
+	// Can not use pin.Configure() directly due to special mode and 32 bit size
+	pinConfig := uint32(nrf.GPIO_PIN_CNF_DIR_Output<<nrf.GPIO_PIN_CNF_DIR_Pos) |
+		uint32(nrf.GPIO_PIN_CNF_INPUT_Disconnect<<nrf.GPIO_PIN_CNF_INPUT_Pos) |
+		uint32(nrf.GPIO_PIN_CNF_PULL_Disabled<<nrf.GPIO_PIN_CNF_PULL_Pos) |
+		uint32(nrf.GPIO_PIN_CNF_DRIVE_H0H1<<nrf.GPIO_PIN_CNF_DRIVE_Pos) |
+		uint32(nrf.GPIO_PIN_CNF_SENSE_Disabled<<nrf.GPIO_PIN_CNF_SENSE_Pos)
+	nrf.P1.PIN_CNF[8].Set(pinConfig) // LSM_PWR == P1_08
+
+	// Enable IMU
+	machine.LSM_PWR.High()
+
+	// Wait a moment
+	time.Sleep(10 * time.Millisecond)
+
+	// Common initialisation code
+	return d.doConfigure(cfg)
+}

lsm6ds3tr/registers.go

@@ -0,0 +1,79 @@
+package lsm6ds3tr
+
+// 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
+
+	GYRO_125DPS  GyroRange = 0x02
+	GYRO_245DPS  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
+)