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