mirror of
https://github.com/tinygo-org/drivers.git
synced 2026-07-26 10:38:41 +00:00
lsm9ds1: minimal implementation
This commit is contained in:
Yurii Soldak
Ron Evans
@@ -0,0 +1,103 @@
|
||||
// LSM9DS1, 9 axis Inertial Measurement Unit (IMU)
|
||||
package main
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"machine"
|
||||
"time"
|
||||
|
||||
"tinygo.org/x/drivers/lsm9ds1"
|
||||
)
|
||||
|
||||
const (
|
||||
PLOTTER = false
|
||||
SHOW_ACCELERATION = true
|
||||
SHOW_ROTATION = true
|
||||
SHOW_MAGNETIC_FIELD = true
|
||||
SHOW_TEMPERATURE = true
|
||||
)
|
||||
|
||||
func main() {
|
||||
|
||||
// I2C configure
|
||||
machine.I2C0.Configure(machine.I2CConfig{})
|
||||
|
||||
// LSM9DS1 setup
|
||||
device := lsm9ds1.New(machine.I2C0)
|
||||
err := device.Configure(lsm9ds1.Configuration{
|
||||
AccelRange: lsm9ds1.ACCEL_2G,
|
||||
AccelSampleRate: lsm9ds1.ACCEL_SR_119,
|
||||
GyroRange: lsm9ds1.GYRO_250DPS,
|
||||
GyroSampleRate: lsm9ds1.GYRO_SR_119,
|
||||
MagRange: lsm9ds1.MAG_4G,
|
||||
MagSampleRate: lsm9ds1.MAG_SR_40,
|
||||
})
|
||||
if err != nil {
|
||||
for {
|
||||
println("Failed to configure", err.Error())
|
||||
time.Sleep(time.Second)
|
||||
}
|
||||
}
|
||||
|
||||
for {
|
||||
|
||||
if con, err := device.Connected(); !con || err != nil {
|
||||
println("LSM9DS1 not connected")
|
||||
time.Sleep(time.Second)
|
||||
continue
|
||||
}
|
||||
|
||||
ax, ay, az, _ := device.ReadAcceleration()
|
||||
gx, gy, gz, _ := device.ReadRotation()
|
||||
mx, my, mz, _ := device.ReadMagneticField()
|
||||
t, _ := device.ReadTemperature()
|
||||
|
||||
if PLOTTER {
|
||||
printPlotter(ax, ay, az, gx, gy, gz, mx, my, mz, t)
|
||||
time.Sleep(time.Millisecond * 100)
|
||||
} else {
|
||||
printMonitor(ax, ay, az, gx, gy, gz, mx, my, mz, t)
|
||||
time.Sleep(time.Millisecond * 1000)
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Arduino IDE's Serial Plotter
|
||||
func printPlotter(ax, ay, az, gx, gy, gz, mx, my, mz, t int32) {
|
||||
if SHOW_ACCELERATION {
|
||||
fmt.Printf("AX:%f, AY:%f, AZ:%f,", axis(ax), axis(ay), axis(az))
|
||||
}
|
||||
if SHOW_ROTATION {
|
||||
fmt.Printf("GX:%f, GY:%f, GZ:%f,", axis(gx), axis(gy), axis(gz))
|
||||
}
|
||||
if SHOW_MAGNETIC_FIELD {
|
||||
fmt.Printf("MX:%d, MY:%d, MZ:%d,", mx, my, mz)
|
||||
}
|
||||
if SHOW_TEMPERATURE {
|
||||
fmt.Printf("T:%f", float32(t)/1000)
|
||||
}
|
||||
println()
|
||||
}
|
||||
|
||||
// Any Serial Monitor
|
||||
func printMonitor(ax, ay, az, gx, gy, gz, mx, my, mz, t int32) {
|
||||
if SHOW_ACCELERATION {
|
||||
fmt.Printf("Acceleration (g): %f, %f, %f\r\n", axis(ax), axis(ay), axis(az))
|
||||
}
|
||||
if SHOW_ROTATION {
|
||||
fmt.Printf("Rotation (dps): %f, %f, %f\r\n", axis(gx), axis(gy), axis(gz))
|
||||
}
|
||||
if SHOW_MAGNETIC_FIELD {
|
||||
fmt.Printf("Magnetic field (nT): %d, %d, %d\r\n", mx, my, mz)
|
||||
}
|
||||
if SHOW_TEMPERATURE {
|
||||
fmt.Printf("Temperature C: %f\r\n", float32(t)/1000)
|
||||
}
|
||||
println()
|
||||
}
|
||||
|
||||
func axis(raw int32) float32 {
|
||||
return float32(raw) / 1000000
|
||||
}
|
||||
@@ -0,0 +1,227 @@
|
||||
// LSM9DS1, 9 axis Inertial Measurement Unit (IMU)
|
||||
//
|
||||
// Datasheet: https://www.st.com/resource/en/datasheet/lsm6ds3.pdf
|
||||
//
|
||||
package lsm9ds1 // import "tinygo.org/x/drivers/lsm9ds1"
|
||||
|
||||
import (
|
||||
"errors"
|
||||
|
||||
"tinygo.org/x/drivers"
|
||||
)
|
||||
|
||||
type AccelRange uint8
|
||||
type AccelSampleRate uint8
|
||||
type AccelBandwidth uint8
|
||||
|
||||
type GyroRange uint8
|
||||
type GyroSampleRate uint8
|
||||
|
||||
type MagRange uint8
|
||||
type MagSampleRate uint8
|
||||
|
||||
// Device wraps connection to a LSM9DS1 device.
|
||||
type Device struct {
|
||||
bus drivers.I2C
|
||||
AccelAddress uint8
|
||||
MagAddress uint8
|
||||
accelMultiplier int32
|
||||
gyroMultiplier int32
|
||||
magMultiplier int32
|
||||
dataBufferSix []uint8
|
||||
dataBufferTwo []uint8
|
||||
}
|
||||
|
||||
// Configuration for LSM9DS1 device.
|
||||
type Configuration struct {
|
||||
AccelRange AccelRange
|
||||
AccelSampleRate AccelSampleRate
|
||||
AccelBandWidth AccelBandwidth
|
||||
GyroRange GyroRange
|
||||
GyroSampleRate GyroSampleRate
|
||||
MagRange MagRange
|
||||
MagSampleRate MagSampleRate
|
||||
}
|
||||
|
||||
var errNotConnected = errors.New("lsm9ds1: failed to communicate with either acel/gyro or magnet sensor")
|
||||
|
||||
// New creates a new LSM9DS1 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,
|
||||
AccelAddress: ACCEL_ADDRESS,
|
||||
MagAddress: MAG_ADDRESS,
|
||||
dataBufferSix: make([]uint8, 6),
|
||||
dataBufferTwo: make([]uint8, 2),
|
||||
}
|
||||
}
|
||||
|
||||
// Connected returns whether both sensor on LSM9DS1 has been found.
|
||||
// It does two "who am I" requests and checks the responses.
|
||||
// In a rare case of an I2C bus issue, it can also return an error.
|
||||
// Case of boolean false and error nil means I2C is up,
|
||||
// but "who am I" responses have unexpected values.
|
||||
func (d *Device) Connected() (connected bool, err error) {
|
||||
data1, data2 := []byte{0}, []byte{0}
|
||||
err = d.bus.ReadRegister(d.AccelAddress, WHO_AM_I, data1)
|
||||
if err != nil {
|
||||
return false, err
|
||||
}
|
||||
err = d.bus.ReadRegister(d.MagAddress, WHO_AM_I_M, data2)
|
||||
if err != nil {
|
||||
return false, err
|
||||
}
|
||||
return data1[0] == 0x68 && data2[0] == 0x3D, nil
|
||||
}
|
||||
|
||||
// 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) {
|
||||
err = d.bus.ReadRegister(uint8(d.AccelAddress), OUT_X_L_XL, d.dataBufferSix)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
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, y, z int32, err error) {
|
||||
err = d.bus.ReadRegister(uint8(d.AccelAddress), OUT_X_L_G, d.dataBufferSix)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
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
|
||||
}
|
||||
|
||||
// ReadMagneticField reads the current magnetic field from the device and returns
|
||||
// it in nT (nanotesla). 1 G (gauss) = 100_000 nT (nanotesla).
|
||||
func (d *Device) ReadMagneticField() (x, y, z int32, err error) {
|
||||
err = d.bus.ReadRegister(uint8(d.MagAddress), OUT_X_L_M, d.dataBufferSix)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
x = int32(int16((int16(d.dataBufferSix[1])<<8)|int16(d.dataBufferSix[0]))) * d.magMultiplier
|
||||
y = int32(int16((int16(d.dataBufferSix[3])<<8)|int16(d.dataBufferSix[2]))) * d.magMultiplier
|
||||
z = int32(int16((int16(d.dataBufferSix[5])<<8)|int16(d.dataBufferSix[4]))) * d.magMultiplier
|
||||
return
|
||||
}
|
||||
|
||||
// ReadTemperature returns the temperature in Celsius milli degrees (°C/1000)
|
||||
func (d *Device) ReadTemperature() (t int32, err error) {
|
||||
err = d.bus.ReadRegister(uint8(d.AccelAddress), OUT_TEMP_L, d.dataBufferTwo)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
// 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
|
||||
}
|
||||
|
||||
// --- end of public methods --------------------------------------------------
|
||||
|
||||
// doConfigure is called by public Configure methods after all
|
||||
// necessary board-specific initialisations are taken care of
|
||||
func (d *Device) doConfigure(cfg Configuration) (err error) {
|
||||
|
||||
// Verify unit communication
|
||||
if con, err := d.Connected(); !con || err != nil {
|
||||
return errNotConnected
|
||||
}
|
||||
|
||||
// Multipliers come from "Table 3. Sensor 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 = 732
|
||||
}
|
||||
switch cfg.GyroRange {
|
||||
case GYRO_250DPS:
|
||||
d.gyroMultiplier = 8750
|
||||
case GYRO_500DPS:
|
||||
d.gyroMultiplier = 17500
|
||||
case GYRO_2000DPS:
|
||||
d.gyroMultiplier = 70000
|
||||
}
|
||||
switch cfg.MagRange {
|
||||
case MAG_4G:
|
||||
d.magMultiplier = 14
|
||||
case MAG_8G:
|
||||
d.magMultiplier = 29
|
||||
case MAG_12G:
|
||||
d.magMultiplier = 43
|
||||
case MAG_16G:
|
||||
d.magMultiplier = 58
|
||||
}
|
||||
|
||||
data := make([]byte, 1)
|
||||
|
||||
// Configure accelerometer
|
||||
// Sample rate & measurement range
|
||||
data[0] = uint8(cfg.AccelSampleRate)<<5 | uint8(cfg.AccelRange)<<3
|
||||
err = d.bus.WriteRegister(d.AccelAddress, CTRL_REG6_XL, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
// Configure gyroscope
|
||||
// Sample rate & measurement range
|
||||
data[0] = uint8(cfg.GyroSampleRate)<<5 | uint8(cfg.GyroRange)<<3
|
||||
err = d.bus.WriteRegister(d.AccelAddress, CTRL_REG1_G, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
// Configure magnetometer
|
||||
|
||||
// Temperature compensation enabled
|
||||
// High-performance mode XY axis
|
||||
// Sample rate
|
||||
data[0] = 0b10000000 | 0b01000000 | uint8(cfg.MagSampleRate)<<2
|
||||
err = d.bus.WriteRegister(d.MagAddress, CTRL_REG1_M, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
// Measurement range
|
||||
data[0] = uint8(cfg.MagRange) << 5
|
||||
err = d.bus.WriteRegister(d.MagAddress, CTRL_REG2_M, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
// Continuous-conversion mode
|
||||
// https://electronics.stackexchange.com/questions/237397/continuous-conversion-vs-single-conversion-mode
|
||||
data[0] = 0b00000000
|
||||
err = d.bus.WriteRegister(d.MagAddress, CTRL_REG3_M, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
// High-performance mode Z axis
|
||||
data[0] = 0b00001000
|
||||
err = d.bus.WriteRegister(d.MagAddress, CTRL_REG4_M, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
return nil
|
||||
}
|
||||
@@ -0,0 +1,9 @@
|
||||
//go:build !nano_33_ble
|
||||
// +build !nano_33_ble
|
||||
|
||||
package lsm9ds1
|
||||
|
||||
// Configure sets up the device for communication.
|
||||
func (d *Device) Configure(cfg Configuration) error {
|
||||
return d.doConfigure(cfg)
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
//go:build nano_33_ble
|
||||
// +build nano_33_ble
|
||||
|
||||
// Nano 33 BLE [Sense] has LSM9DS1 unit on-board.
|
||||
// This custom Configure function powers unit up
|
||||
// and enables I2C, so unit can can be accessed.
|
||||
package lsm9ds1
|
||||
|
||||
import (
|
||||
"machine"
|
||||
"time"
|
||||
)
|
||||
|
||||
// Configure sets up the device for communication.
|
||||
func (d *Device) Configure(cfg Configuration) error {
|
||||
// Following lines are Nano 33 BLE specific, they have nothing to do with sensor per se
|
||||
machine.LSM_PWR.Configure(machine.PinConfig{Mode: machine.PinOutput})
|
||||
machine.LSM_PWR.High()
|
||||
machine.I2C_PULLUP.Configure(machine.PinConfig{Mode: machine.PinOutput})
|
||||
machine.I2C_PULLUP.High()
|
||||
// Wait a moment
|
||||
time.Sleep(10 * time.Millisecond)
|
||||
// Common initialisation code
|
||||
return d.doConfigure(cfg)
|
||||
}
|
||||
@@ -0,0 +1,102 @@
|
||||
package lsm9ds1
|
||||
|
||||
// Constants/addresses used for I2C.
|
||||
|
||||
const (
|
||||
|
||||
// Constants/addresses used for I2C.
|
||||
ACCEL_ADDRESS = 0x6B
|
||||
MAG_ADDRESS = 0x1E
|
||||
|
||||
// Table 21. Accelerometer and gyroscope register address map
|
||||
WHO_AM_I = 0x0F // value 0x68
|
||||
CTRL_REG1_G = 0x10
|
||||
OUT_X_L_G = 0x18
|
||||
OUT_X_H_G = 0x19
|
||||
OUT_Y_L_G = 0x1A
|
||||
OUT_Y_H_G = 0x1B
|
||||
OUT_Z_L_G = 0x1C
|
||||
OUT_Z_H_G = 0x1D
|
||||
OUT_TEMP_L = 0x15
|
||||
OUT_TEMP_H = 0x16
|
||||
CTRL_REG6_XL = 0x20
|
||||
STATUS_REG = 0x27
|
||||
OUT_X_L_XL = 0x28
|
||||
OUT_X_H_XL = 0x29
|
||||
OUT_Y_L_XL = 0x2A
|
||||
OUT_Y_H_XL = 0x2B
|
||||
OUT_Z_L_XL = 0x2C
|
||||
OUT_Z_H_XL = 0x2D
|
||||
|
||||
// Table 22. Magnetic sensor register address map
|
||||
OFFSET_X_REG_L_M = 0x05
|
||||
OFFSET_X_REG_H_M = 0x06
|
||||
OFFSET_Y_REG_L_M = 0x07
|
||||
OFFSET_Y_REG_H_M = 0x08
|
||||
OFFSET_Z_REG_L_M = 0x09
|
||||
OFFSET_Z_REG_H_M = 0x0A
|
||||
WHO_AM_I_M = 0x0F // value 0x3D
|
||||
CTRL_REG1_M = 0x20 // TEMP_COMP OM1 OM0 DO2 DO1 DO0 FAST_ODR ST
|
||||
CTRL_REG2_M = 0x21 // 0 FS1 FS0 0 REBOOT SOFT_RST 0 0
|
||||
CTRL_REG3_M = 0x22 // 0 LP 0 0 SIM MD1 MD0
|
||||
CTRL_REG4_M = 0x23 // 0 0 0 0 OMZ1 OMZ0 BLE 0
|
||||
STATUS_REG_M = 0x27
|
||||
OUT_X_L_M = 0x28
|
||||
OUT_X_H_M = 0x29
|
||||
OUT_Y_L_M = 0x2A
|
||||
OUT_Y_H_M = 0x2B
|
||||
OUT_Z_L_M = 0x2C
|
||||
OUT_Z_H_M = 0x2D
|
||||
|
||||
// Table 67. CTRL_REG6_XL register description
|
||||
ACCEL_2G AccelRange = 0b00
|
||||
ACCEL_4G AccelRange = 0b10
|
||||
ACCEL_8G AccelRange = 0b11
|
||||
ACCEL_16G AccelRange = 0b01
|
||||
|
||||
// Table 68. ODR register setting (accelerometer only mode)
|
||||
ACCEL_SR_OFF AccelSampleRate = 0b000
|
||||
ACCEL_SR_10 AccelSampleRate = 0b001
|
||||
ACCEL_SR_50 AccelSampleRate = 0b010
|
||||
ACCEL_SR_119 AccelSampleRate = 0b011
|
||||
ACCEL_SR_238 AccelSampleRate = 0b100
|
||||
ACCEL_SR_476 AccelSampleRate = 0b101
|
||||
ACCEL_SR_952 AccelSampleRate = 0b110
|
||||
|
||||
// Table 67. CTRL_REG6_XL register description
|
||||
ACCEL_BW_50 AccelBandwidth = 0b11
|
||||
ACCEL_BW_105 AccelBandwidth = 0b10
|
||||
ACCEL_BW_211 AccelBandwidth = 0b01
|
||||
ACCEL_BW_408 AccelBandwidth = 0b00
|
||||
|
||||
// Table 45. CTRL_REG1_G register description
|
||||
GYRO_250DPS GyroRange = 0b00
|
||||
GYRO_500DPS GyroRange = 0b01
|
||||
GYRO_2000DPS GyroRange = 0b11
|
||||
|
||||
// Table 9. Gyroscope operating modes
|
||||
// Table 46. ODR and BW configuration setting (after LPF1)
|
||||
GYRO_SR_OFF GyroSampleRate = 0b000
|
||||
GYRO_SR_15 GyroSampleRate = 0b001
|
||||
GYRO_SR_60 GyroSampleRate = 0b010
|
||||
GYRO_SR_119 GyroSampleRate = 0b011
|
||||
GYRO_SR_238 GyroSampleRate = 0b100
|
||||
GYRO_SR_476 GyroSampleRate = 0b101
|
||||
GYRO_SR_952 GyroSampleRate = 0b110
|
||||
|
||||
// Table 114. Full-scale selection
|
||||
MAG_4G MagRange = 0b00
|
||||
MAG_8G MagRange = 0b01
|
||||
MAG_12G MagRange = 0b10
|
||||
MAG_16G MagRange = 0b11
|
||||
|
||||
// Table 111. Output data rate configuration
|
||||
MAG_SR_06 MagSampleRate = 0b000
|
||||
MAG_SR_1 MagSampleRate = 0b001
|
||||
MAG_SR_2 MagSampleRate = 0b010
|
||||
MAG_SR_5 MagSampleRate = 0b011
|
||||
MAG_SR_10 MagSampleRate = 0b100
|
||||
MAG_SR_20 MagSampleRate = 0b101
|
||||
MAG_SR_40 MagSampleRate = 0b110
|
||||
MAG_SR_80 MagSampleRate = 0b111
|
||||
)
|
||||
Reference in New Issue
Block a user