8000 lsm9ds1: Refactor driver. · micropython/micropython-lib@bf8b3c0 · GitHub
[go: up one dir, main page]
Skip to content

Commit bf8b3c0

Browse files
iabdalkaderdpgeorge
iabdalkader
authored and
dpgeorge
committed
lsm9ds1: Refactor driver.
Changes are: - fix typos - simplify the driver init code - support setting the magnetometer ODR separately - update manifest
1 parent e88aa3a commit bf8b3c0

File tree

2 files changed

+95
-80
lines changed

2 files changed

+95
-80
lines changed

‎micropython/drivers/imu/lsm9ds1/lsm9ds1.py

Lines changed: 94 additions & 80 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22
The MIT License (MIT)
33
44
Copyright (c) 2013, 2014 Damien P. George
5+
Copyright (c) 2022-2023 Ibrahim Abdelkader <iabdalkader@openmv.io>
56
67
Permission is hereby granted, free of charge, to any person obtaining a copy
78
of this software and associated documentation files (the "Software"), to deal
@@ -32,13 +33,13 @@
3233
from lsm9ds1 import LSM9DS1
3334
from machine import Pin, I2C
3435
35-
lsm = LSM9DS1(I2C(1, scl=Pin(15), sda=Pin(14)))
36+
imu = LSM9DS1(I2C(1, scl=Pin(15), sda=Pin(14)))
3637
3738
while (True):
38-
#for g,a in lsm.iter_accel_gyro(): print(g,a) # using fifo
39-
print('Accelerometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*lsm.accel()))
40-
print('Magnetometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*lsm.magnet()))
41-
print('Gyroscope: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*lsm.gyro()))
39+
#for g,a in imu.iter_accel_gyro(): print(g,a) # using fifo
40+
print('Accelerometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*imu.accel()))
41+
print('Magnetometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*imu.magnet()))
42+
print('Gyroscope: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*imu.gyro()))
4243
print("")
4344
time.sleep_ms(100)
4445
"""
@@ -58,129 +59,142 @@
5859
_OFFSET_REG_X_M = const(0x05)
5960
_CTRL_REG1_M = const(0x20)
6061
_OUT_M = const(0x28)
61-
_SCALE_GYRO = const(((245, 0), (500, 1), (2000, 3)))
62-
_SCALE_ACCEL = const(((2, 0), (4, 2), (8, 3), (16, 1)))
62+
_ACCEL_SCALE = const((2, 16, 4, 8))
63+
_GYRO_SCALE = const((245, 500, 2000))
64+
_MAGNET_SCALE = const((4, 8, 12, 16))
65+
_ODR_IMU = const((0, 14.9, 59.5, 119, 238, 476, 952))
66+
_ODR_MAGNET = const((0.625, 1.25, 2.5, 5, 10, 20, 40, 80))
6367

6468

6569
class LSM9DS1:
66-
def __init__(self, i2c, address_gyro=0x6B, address_magnet=0x1E):
67-
self.i2c = i2c
68-
self.address_gyro = address_gyro
70+
def __init__(
71+
self,
72+
bus,
73+
address_imu=0x6B,
74+
address_magnet=0x1E,
75+
gyro_odr=952,
76+
gyro_scale=245,
77+
accel_odr=952,
78+
accel_scale=4,
79+
magnet_odr=80,
80+
magnet_scale=4,
81+
):
82+
"""Initalizes Gyro, Accelerometer and Magnetometer.
83+
bus: IMU bus
84+
address_imu: IMU I2C address.
85+
address_magnet: Magnetometer I2C address.
86+
gyro_odr: (0, 14.9Hz, 59.5Hz, 119Hz, 238Hz, 476Hz, 952Hz)
87+
gyro_scale: (245dps, 500dps, 2000dps )
88+
accel_odr: (0, 14.9Hz, 59.5Hz, 119Hz, 238Hz, 476Hz, 952Hz)
89+
accel_scale: (+/-2g, +/-4g, +/-8g, +-16g)
90+
magnet_odr: (0.625Hz, 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz)
91+
magnet_scale: (+/-4, +/-8, +/-12, +/-16)
92+
"""
93+
self.bus = bus
94+
self.address_imu = address_imu
6995
self.address_magnet = address_magnet
70-
# check id's of accelerometer/gyro and magnetometer
96+
97+
# Sanity checks
98+
if not gyro_odr in _ODR_IMU:
99+
raise ValueError("Invalid gyro sampling rate: %d" % gyro_odr)
100+
if not gyro_scale in _GYRO_SCALE:
101+
raise ValueError("Invalid gyro scaling: %d" % gyro_scale)
102+
103+
if not accel_odr in _ODR_IMU:
104+
raise ValueError("Invalid accelerometer sampling rate: %d" % accel_odr)
105+
if not accel_scale in _ACCEL_SCALE:
106+
raise ValueError("Invalid accelerometer scaling: %d" % accel_scale)
107+
108+
if not magnet_odr in _ODR_MAGNET:
109+
raise ValueError("Invalid magnet sampling rate: %d" % magnet_odr)
110+
if not magnet_scale in _MAGNET_SCALE:
111+
raise ValueError("Invalid magnet scaling: %d" % magnet_scale)
112+
71113
if (self.magent_id() != b"=") or (self.gyro_id() != b"h"):
72114
raise OSError(
73-
"Invalid LSM9DS1 device, using address {}/{}".format(address_gyro, address_magnet)
115+
"Invalid LSM9DS1 device, using address {}/{}".format(address_imu, address_magnet)
74116
)
75-
# allocate scratch buffer for efficient conversions and memread op's
76-
self.scratch = array.array("B", [0, 0, 0, 0, 0, 0])
77-
self.scratch_int = array.array("h", [0, 0, 0])
78-
self.init_gyro_accel()
79-
self.init_magnetometer()
80-
81-
def init_gyro_accel(self, sample_rate=6, scale_gyro=0, scale_accel=0):
82-
"""Initalizes Gyro and Accelerator.
83-
sample rate: 0-6 (off, 14.9Hz, 59.5Hz, 119Hz, 238Hz, 476Hz, 952Hz)
84-
scale_gyro: 0-2 (245dps, 500dps, 2000dps )
85-
scale_accel: 0-3 (+/-2g, +/-4g, +/-8g, +-16g)
86-
"""
87-
assert sample_rate <= 6, "invalid sampling rate: %d" % sample_rate
88-
assert scale_gyro <= 2, "invalid gyro scaling: %d" % scale_gyro
89-
assert scale_accel <= 3, "invalid accelerometer scaling: %d" % scale_accel
90-
91-
i2c = self.i2c
92-
addr = self.address_gyro
93-
mv = memoryview(self.scratch)
94-
# angular control registers 1-3 / Orientation
95-
mv[0] = ((sample_rate & 0x07) << 5) | ((_SCALE_GYRO[scale_gyro][1] & 0x3) << 3)
117+
118+
mv = memoryview(bytearray(6))
119+
120+
# Configure Gyroscope.
121+
mv[0] = (_ODR_IMU.index(gyro_odr) << 5) | ((_GYRO_SCALE.index(gyro_scale)) << 3)
96122
mv[1:4] = b"\x00\x00\x00"
97-
i2c.writeto_mem(addr, _CTRL_REG1_G, mv[:5])
98-
# ctrl4 - enable x,y,z, outputs, no irq latching, no 4D
99-
# ctrl5 - enable all axes, no decimation
123+
self.bus.writeto_mem(self.address_imu, _CTRL_REG1_G, mv[:5])
124+
125+
# Configure Accelerometer
126+
mv[0] = 0x38 # ctrl4 - enable x,y,z, outputs, no irq latching, no 4D
127+
mv[1] = 0x38 # ctrl5 - enable all axes, no decimation
100128
# ctrl6 - set scaling and sample rate of accel
101-
# ctrl7,8 - leave at default values
102-
# ctrl9 - FIFO enabled
103-
mv[0] = mv[1] = 0x38
104-
mv[2] = ((sample_rate & 7) << 5) | ((_SCALE_ACCEL[scale_accel][1] & 0x3) << 3)
105-
mv[3] = 0x00
106-
mv[4] = 0x4
107-
mv[5] = 0x2
108-
i2c.writeto_mem(addr, _CTRL_REG4_G, mv[:6])
129+
mv[2] = (_ODR_IMU.index(accel_odr) << 5) | ((_ACCEL_SCALE.index(accel_scale)) << 3)
130+
mv[3] = 0x00 # ctrl7 - leave at default values
131+
mv[4] = 0x4 # ctrl8 - leave at default values
132+
mv[5] = 0x2 # ctrl9 - FIFO enabled
133+
self.bus.writeto_mem(self.address_imu, _CTRL_REG4_G, mv)
109134

110135
# fifo: use continous mode (overwrite old data if overflow)
111-
i2c.writeto_mem(addr, _FIFO_CTRL_REG, b"\x00")
112-
i2c.writeto_mem(addr, _FIFO_CTRL_REG, b"\xc0")
113-
114-
self.scale_gyro = 32768 / _SCALE_GYRO[scale_gyro][0]
115-
self.scale_accel = 32768 / _SCALE_ACCEL[scale_accel][0]
136+
self.bus.writeto_mem(self.address_imu, _FIFO_CTRL_REG, b"\x00")
137+
self.bus.writeto_mem(self.address_imu, _FIFO_CTRL_REG, b"\xc0")
116138

117-
def init_magnetometer(self, sample_rate=7, scale_magnet=0):
118-
"""
119-
sample rates = 0-7 (0.625, 1.25, 2.5, 5, 10, 20, 40, 80Hz)
120-
scaling = 0-3 (+/-4, +/-8, +/-12, +/-16 Gauss)
121-
"""
122-
assert sample_rate < 8, "invalid sample rate: %d (0-7)" % sample_rate
123-
assert scale_magnet < 4, "invalid scaling: %d (0-3)" % scale_magnet
124-
i2c = self.i2c
125-
addr = self.address_magnet
126-
mv = memoryview(self.scratch)
127-
mv[0] = 0x40 | (sample_rate << 2) # ctrl1: high performance mode
128-
mv[1] = scale_magnet << 5 # ctrl2: scale, normal mode, no reset
139+
# Configure Magnetometer
140+
mv[0] = 0x40 | (magnet_odr << 2) # ctrl1: high performance mode
141+
mv[1] = _MAGNET_SCALE.index(magnet_scale) << 5 # ctrl2: scale, normal mode, no reset
129142
mv[2] = 0x00 # ctrl3: continous conversion, no low power, I2C
130143
mv[3] = 0x08 # ctrl4: high performance z-axis
131144
mv[4] = 0x00 # ctr5: no fast read, no block update
132-
i2c.writeto_mem(addr, _CTRL_REG1_M, mv[:5])
133-
self.scale_factor_magnet = 32768 / ((scale_magnet + 1) * 4)
145+
self.bus.writeto_mem(self.address_magnet, _CTRL_REG1_M, mv[:5])
146+
147+
self.gyro_scale = 32768 / gyro_scale
148+
self.accel_scale = 32768 / accel_scale
149+
self.scale_factor_magnet = 32768 / ((_MAGNET_SCALE.index(magnet_scale) + 1) * 4)
150+
151+
# Allocate scratch buffer for efficient conversions and memread op's
152+
self.scratch_int = array.array("h", [0, 0, 0])
134153

135154
def calibrate_magnet(self, offset):
136155
"""
137-
offset is a magnet vecor that will be substracted by the magnetometer
156+
offset is a magnet vector that will be subtracted by the magnetometer
138157
for each measurement. It is written to the magnetometer's offset register
139158
"""
159+
import struct
160+
140161
offset = [int(i * self.scale_factor_magnet) for i in offset]
141-
mv = memoryview(self.scratch)
142-
mv[0] = offset[0] & 0xFF
143-
mv[1] = offset[0] >> 8
144-
mv[2] = offset[1] & 0xFF
145-
mv[3] = offset[1] >> 8
146-
mv[4] = offset[2] & 0xFF
147-
mv[5] = offset[2] >> 8
148-
self.i2c.writeto_mem(self.address_magnet, _OFFSET_REG_X_M, mv[:6])
162+
self.bus.writeto_mem(self.address_magnet, _OFFSET_REG_X_M, struct.pack("<HHH", offset))
149163

150164
def gyro_id(self):
151-
return self.i2c.readfrom_mem(self.address_gyro, _WHO_AM_I, 1)
165+
return self.bus.readfrom_mem(self.address_imu, _WHO_AM_I, 1)
152166

153167
def magent_id(self):
154-
return self.i2c.readfrom_mem(self.address_magnet, _WHO_AM_I, 1)
168+
return self.bus.readfrom_mem(self.address_magnet, _WHO_AM_I, 1)
155169

156170
def magnet(self):
157171
"""Returns magnetometer vector in gauss.
158172
raw_values: if True, the non-scaled adc values are returned
159173
"""
160174
mv = memoryview(self.scratch_int)
161175
f = self.scale_factor_magnet
162-
self.i2c.readfrom_mem_into(self.address_magnet, _OUT_M | 0x80, mv)
176+
self.bus.readfrom_mem_into(self.address_magnet, _OUT_M | 0x80, mv)
163177
return (mv[0] / f, mv[1] / f, mv[2] / f)
164178

165179
def gyro(self):
166180
"""Returns gyroscope vector in degrees/sec."""
167181
mv = memoryview(self.scratch_int)
168-
f = self.scale_gyro
169-
self.i2c.readfrom_mem_into(self.address_gyro, _OUT_G | 0x80, mv)
182+
f = self.gyro_scale
183+
self.bus.readfrom_mem_into(self.address_imu, _OUT_G | 0x80, mv)
170184
return (mv[0] / f, mv[1] / f, mv[2] / f)
171185

172186
def accel(self):
173187
"""Returns acceleration vector in gravity units (9.81m/s^2)."""
174188
mv = memoryview(self.scratch_int)
175-
f = self.scale_accel
176-
self.i2c.readfrom_mem_into(self.address_gyro, _OUT_XL | 0x80, mv)
189+
f = self.accel_scale
190+
self.bus.readfrom_mem_into(self.address_imu, _OUT_XL | 0x80, mv)
177191
return (mv[0] / f, mv[1] / f, mv[2] / f)
178192

179193
def iter_accel_gyro(self):
180194
"""A generator that returns tuples of (gyro,accelerometer) data from the fifo."""
181195
while True:
182196
fifo_state = int.from_bytes(
183-
self.i2c.readfrom_mem(self.address_gyro, _FIFO_SRC, 1), "big"
197+
self.bus.readfrom_mem(self.address_imu, _FIFO_SRC, 1), "big"
184198
)
185199
if fifo_state & 0x3F:
186200
# print("Available samples=%d" % (fifo_state & 0x1f))

‎micropython/drivers/imu/lsm9ds1/manifest.py

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1 +1,2 @@
1+
metadata(description="Driver for ST LSM9DS1 IMU.", version="1.0.0")
12
module("lsm9ds1.py", opt=3)

0 commit comments

Comments
 (0)
0