mirror of
https://github.com/tinygo-org/drivers.git
synced 2026-07-26 10:38:41 +00:00
lsm6ds3tr: initial implementation
This commit is contained in:
Yurii Soldak
Ron Evans
@@ -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)
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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)
|
||||
}
|
||||
@@ -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)
|
||||
}
|
||||
@@ -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
|
||||
)
|
||||
Reference in New Issue
Block a user