Skip to content

Commit 3bdab45

Browse files
ysoldakdeadprogram
ysoldak
authored and
deadprogram
committed
lsm6ds3tr: initial implementation
1 parent 9f67836 commit 3bdab45

File tree

5 files changed

+349
-0
lines changed

5 files changed

+349
-0
lines changed

‎examples/lsm6ds3tr/main.go‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
// Connects to an LSM6DS3TR I2C a 6 axis Inertial Measurement Unit (IMU)
2+
package main
3+
4+
import (
5+
"machine"
6+
"time"
7+
8+
"tinygo.org/x/drivers/lsm6ds3tr"
9+
)
10+
11+
func main() {
12+
machine.I2C0.Configure(machine.I2CConfig{})
13+
14+
accel := lsm6ds3tr.New(machine.I2C0)
15+
err := accel.Configure(lsm6ds3tr.Configuration{})
16+
if err != nil {
17+
for {
18+
println("Failed to configure", err.Error())
19+
time.Sleep(time.Second)
20+
}
21+
}
22+
23+
for {
24+
if !accel.Connected() {
25+
println("LSM6DS3TR not connected")
26+
time.Sleep(time.Second)
27+
continue
28+
}
29+
x, y, z, _ := accel.ReadAcceleration()
30+
println("Acceleration:", float32(x)/1000000, float32(y)/1000000, float32(z)/1000000)
31+
x, y, z, _ = accel.ReadRotation()
32+
println("Gyroscope:", float32(x)/1000000, float32(y)/1000000, float32(z)/1000000)
33+
x, _ = accel.ReadTemperature()
34+
println("Degrees C", float32(x)/1000, "\n\n")
35+
time.Sleep(time.Millisecond * 1000)
36+
}
37+
}

‎lsm6ds3tr/lsm6ds3tr.go‎

Lines changed: 189 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,189 @@
1+
// Package lsm6ds3tr implements a driver for the LSM6DS3TR
2+
// a 6 axis Inertial Measurement Unit (IMU)
3+
//
4+
// Datasheet: https://www.st.com/resource/en/datasheet/lsm6ds3tr.pdf
5+
//
6+
package lsm6ds3tr // import "tinygo.org/x/drivers/lsm6ds3tr"
7+
8+
import (
9+
"errors"
10+
11+
"tinygo.org/x/drivers"
12+
)
13+
14+
type AccelRange uint8
15+
type AccelSampleRate uint8
16+
type AccelBandwidth uint8
17+
18+
type GyroRange uint8
19+
type GyroSampleRate uint8
20+
21+
// Device wraps an I2C connection to a LSM6DS3TR device.
22+
type Device struct {
23+
bus drivers.I2C
24+
Address uint16
25+
accelRange AccelRange
26+
accelSampleRate AccelSampleRate
27+
gyroRange GyroRange
28+
gyroSampleRate GyroSampleRate
29+
buf [6]uint8
30+
}
31+
32+
// Configuration for LSM6DS3TR device.
33+
type Configuration struct {
34+
AccelRange AccelRange
35+
AccelSampleRate AccelSampleRate
36+
AccelBandWidth AccelBandwidth
37+
GyroRange GyroRange
38+
GyroSampleRate GyroSampleRate
39+
IsPedometer bool
40+
ResetStepCounter bool
41+
}
42+
43+
var errNotConnected = errors.New("lsm6ds3tr: failed to communicate with acel/gyro sensor")
44+
45+
// New creates a new LSM6DS3TR connection. The I2C bus must already be configured.
46+
//
47+
// This function only creates the Device object, it does not touch the device.
48+
func New(bus drivers.I2C) *Device {
49+
return &Device{
50+
bus: bus,
51+
Address: Address,
52+
}
53+
}
54+
55+
// Configure sets up the device for communication.
56+
func (d *Device) doConfigure(cfg Configuration) (err error) {
57+
58+
// Verify unit communication
59+
if !d.Connected() {
60+
return errNotConnected
61+
}
62+
63+
if cfg.AccelRange != 0 {
64+
d.accelRange = cfg.AccelRange
65+
} else {
66+
d.accelRange = ACCEL_2G
67+
}
68+
69+
if cfg.AccelSampleRate != 0 {
70+
d.accelSampleRate = cfg.AccelSampleRate
71+
} else {
72+
d.accelSampleRate = ACCEL_SR_104
73+
}
74+
75+
if cfg.GyroRange != 0 {
76+
d.gyroRange = cfg.GyroRange
77+
} else {
78+
d.gyroRange = GYRO_2000DPS
79+
}
80+
81+
if cfg.GyroSampleRate != 0 {
82+
d.gyroSampleRate = cfg.GyroSampleRate
83+
} else {
84+
d.gyroSampleRate = GYRO_SR_104
85+
}
86+
87+
data := d.buf[:1]
88+
89+
// Configure accelerometer
90+
data[0] = uint8(d.accelRange) | uint8(d.accelSampleRate)
91+
err = d.bus.WriteRegister(uint8(d.Address), CTRL1_XL, data)
92+
if err != nil {
93+
return
94+
}
95+
96+
// Set ODR bit
97+
err = d.bus.ReadRegister(uint8(d.Address), CTRL4_C, data)
98+
if err != nil {
99+
return
100+
}
101+
data[0] = data[0] &^ BW_SCAL_ODR_ENABLED
102+
data[0] |= BW_SCAL_ODR_ENABLED
103+
err = d.bus.WriteRegister(uint8(d.Address), CTRL4_C, data)
104+
if err != nil {
105+
return
106+
}
107+
108+
// Configure gyroscope
109+
data[0] = uint8(d.gyroRange) | uint8(d.gyroSampleRate)
110+
err = d.bus.WriteRegister(uint8(d.Address), CTRL2_G, data)
111+
if err != nil {
112+
return
113+
}
114+
115+
return nil
116+
}
117+
118+
// Connected returns whether a LSM6DS3TR has been found.
119+
// It does a "who am I" request and checks the response.
120+
func (d *Device) Connected() bool {
121+
data := d.buf[:1]
122+
d.bus.ReadRegister(uint8(d.Address), WHO_AM_I, data)
123+
return data[0] == 0x6A
124+
}
125+
126+
// ReadAcceleration reads the current acceleration from the device and returns
127+
// it in µg (micro-gravity). When one of the axes is pointing straight to Earth
128+
// and the sensor is not moving the returned value will be around 1000000 or
129+
// -1000000.
130+
func (d *Device) ReadAcceleration() (x, y, z int32, err error) {
131+
data := d.buf[:6]
132+
err = d.bus.ReadRegister(uint8(d.Address), OUTX_L_XL, data)
133+
if err != nil {
134+
return
135+
}
136+
// k comes from "Table 3. Mechanical characteristics" 3 of the datasheet * 1000
137+
k := int32(61) // 2G
138+
if d.accelRange == ACCEL_4G {
139+
k = 122
140+
} else if d.accelRange == ACCEL_8G {
141+
k = 244
142+
} else if d.accelRange == ACCEL_16G {
143+
k = 488
144+
}
145+
x = int32(int16((uint16(data[1])<<8)|uint16(data[0]))) * k
146+
y = int32(int16((uint16(data[3])<<8)|uint16(data[2]))) * k
147+
z = int32(int16((uint16(data[5])<<8)|uint16(data[4]))) * k
148+
return
149+
}
150+
151+
// ReadRotation reads the current rotation from the device and returns it in
152+
// µ°/s (micro-degrees/sec). This means that if you were to do a complete
153+
// rotation along one axis and while doing so integrate all values over time,
154+
// you would get a value close to 360000000.
155+
func (d *Device) ReadRotation() (x, y, z int32, err error) {
156+
data := d.buf[:6]
157+
err = d.bus.ReadRegister(uint8(d.Address), OUTX_L_G, data)
158+
if err != nil {
159+
return
160+
}
161+
// k comes from "Table 3. Mechanical characteristics" 3 of the datasheet * 1000
162+
k := int32(4375) // 125DPS
163+
if d.gyroRange == GYRO_245DPS {
164+
k = 8750
165+
} else if d.gyroRange == GYRO_500DPS {
166+
k = 17500
167+
} else if d.gyroRange == GYRO_1000DPS {
168+
k = 35000
169+
} else if d.gyroRange == GYRO_2000DPS {
170+
k = 70000
171+
}
172+
x = int32(int16((uint16(data[1])<<8)|uint16(data[0]))) * k
173+
y = int32(int16((uint16(data[3])<<8)|uint16(data[2]))) * k
174+
z = int32(int16((uint16(data[5])<<8)|uint16(data[4]))) * k
175+
return
176+
}
177+
178+
// ReadTemperature returns the temperature in celsius milli degrees (°C/1000)
179+
func (d *Device) ReadTemperature() (t int32, err error) {
180+
data := d.buf[:2]
181+
err = d.bus.ReadRegister(uint8(d.Address), OUT_TEMP_L, data)
182+
if err != nil {
183+
return
184+
}
185+
// From "Table 5. Temperature sensor characteristics"
186+
// temp = value/256 + 25
187+
t = 25000 + (int32(int16((int16(data[1])<<8)|int16(data[0])))*125)/32
188+
return
189+
}

‎lsm6ds3tr/lsm6ds3tr_generic.go‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
//go:build !xiao_ble
2+
// +build !xiao_ble
3+
4+
package lsm6ds3tr
5+
6+
// Configure sets up the device for communication.
7+
func (d *Device) Configure(cfg Configuration) error {
8+
return d.doConfigure(cfg)
9+
}

‎lsm6ds3tr/lsm6ds3tr_xiao_ble.go‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
//go:build xiao_ble
2+
// +build xiao_ble
3+
4+
package lsm6ds3tr
5+
6+
import (
7+
"device/nrf"
8+
"machine"
9+
"time"
10+
)
11+
12+
// Configure sets up the device for communication.
13+
func (d *Device) Configure(cfg Configuration) error {
14+
15+
// Following lines are XIAO BLE Sense specific, they have nothing to do with sensor per se
16+
// Implementation adapted from https://github.com/Seeed-Studio/Seeed_Arduino_LSM6DS3/blob/master/LSM6DS3.cpp#L68-L77
17+
18+
// Special mode for IMU power pin on this board.
19+
// Can not use pin.Configure() directly due to special mode and 32 bit size
20+
pinConfig := uint32(nrf.GPIO_PIN_CNF_DIR_Output<<nrf.GPIO_PIN_CNF_DIR_Pos) |
21+
uint32(nrf.GPIO_PIN_CNF_INPUT_Disconnect<<nrf.GPIO_PIN_CNF_INPUT_Pos) |
22+
uint32(nrf.GPIO_PIN_CNF_PULL_Disabled<<nrf.GPIO_PIN_CNF_PULL_Pos) |
23+
uint32(nrf.GPIO_PIN_CNF_DRIVE_H0H1<<nrf.GPIO_PIN_CNF_DRIVE_Pos) |
24+
uint32(nrf.GPIO_PIN_CNF_SENSE_Disabled<<nrf.GPIO_PIN_CNF_SENSE_Pos)
25+
nrf.P1.PIN_CNF[8].Set(pinConfig) // LSM_PWR == P1_08
26+
27+
// Enable IMU
28+
machine.LSM_PWR.High()
29+
30+
// Wait a moment
31+
time.Sleep(10 * time.Millisecond)
32+
33+
// Common initialisation code
34+
return d.doConfigure(cfg)
35+
}

‎lsm6ds3tr/registers.go‎

Lines changed: 79 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
package lsm6ds3tr
2+
3+
// Constants/addresses used for I2C.
4+
5+
// The I2C address which this device listens to.
6+
const Address = 0x6A
7+
8+
const (
9+
WHO_AM_I = 0x0F
10+
STATUS = 0x1E
11+
CTRL1_XL = 0x10
12+
CTRL2_G = 0x11
13+
CTRL3_C = 0x12
14+
CTRL4_C = 0x13
15+
CTRL5_C = 0x14
16+
CTRL6_C = 0x15
17+
CTRL7_G = 0x16
18+
CTRL8_XL = 0x17
19+
CTRL9_XL = 0x18
20+
CTRL10_C = 0x19
21+
OUTX_L_G = 0x22
22+
OUTX_H_G = 0x23
23+
OUTY_L_G = 0x24
24+
OUTY_H_G = 0x25
25+
OUTZ_L_G = 0x26
26+
OUTZ_H_G = 0x27
27+
OUTX_L_XL = 0x28
28+
OUTX_H_XL = 0x29
29+
OUTY_L_XL = 0x2A
30+
OUTY_H_XL = 0x2B
31+
OUTZ_L_XL = 0x2C
32+
OUTZ_H_XL = 0x2D
33+
OUT_TEMP_L = 0x20
34+
OUT_TEMP_H = 0x21
35+
BW_SCAL_ODR_DISABLED = 0x00
36+
BW_SCAL_ODR_ENABLED = 0x80
37+
STEP_TIMESTAMP_L = 0x49
38+
STEP_TIMESTAMP_H = 0x4A
39+
STEP_COUNTER_L = 0x4B
40+
STEP_COUNTER_H = 0x4C
41+
STEP_COUNT_DELTA = 0x15
42+
TAP_CFG = 0x58
43+
INT1_CTRL = 0x0D
44+
45+
ACCEL_2G AccelRange = 0x00
46+
ACCEL_4G AccelRange = 0x08
47+
ACCEL_8G AccelRange = 0x0C
48+
ACCEL_16G AccelRange = 0x04
49+
50+
ACCEL_SR_OFF AccelSampleRate = 0x00
51+
ACCEL_SR_13 AccelSampleRate = 0x10
52+
ACCEL_SR_26 AccelSampleRate = 0x20
53+
ACCEL_SR_52 AccelSampleRate = 0x30
54+
ACCEL_SR_104 AccelSampleRate = 0x40
55+
ACCEL_SR_208 AccelSampleRate = 0x50
56+
ACCEL_SR_416 AccelSampleRate = 0x60
57+
ACCEL_SR_833 AccelSampleRate = 0x70
58+
ACCEL_SR_1666 AccelSampleRate = 0x80
59+
ACCEL_SR_3332 AccelSampleRate = 0x90
60+
ACCEL_SR_6664 AccelSampleRate = 0xA0
61+
62+
GYRO_125DPS GyroRange = 0x02
63+
GYRO_245DPS GyroRange = 0x00
64+
GYRO_500DPS GyroRange = 0x04
65+
GYRO_1000DPS GyroRange = 0x08
66+
GYRO_2000DPS GyroRange = 0x0C
67+
68+
GYRO_SR_OFF GyroSampleRate = 0x00
69+
GYRO_SR_13 GyroSampleRate = 0x10
70+
GYRO_SR_26 GyroSampleRate = 0x20
71+
GYRO_SR_52 GyroSampleRate = 0x30
72+
GYRO_SR_104 GyroSampleRate = 0x40
73+
GYRO_SR_208 GyroSampleRate = 0x50
74+
GYRO_SR_416 GyroSampleRate = 0x60
75+
GYRO_SR_833 GyroSampleRate = 0x70
76+
GYRO_SR_1666 GyroSampleRate = 0x80
77+
GYRO_SR_3332 GyroSampleRate = 0x90
78+
GYRO_SR_6664 GyroSampleRate = 0xA0
79+
)

0 commit comments

Comments
 (0)