lsm9ds1: Refactor driver.

iabdalkader · dpgeorge · commit bf8b3c04de67 · 2023-02-18T15:54:04.000+11:00
Changes are:
- fix typos
- simplify the driver init code
- support setting the magnetometer ODR separately
- update manifest
diff --git a/micropython/drivers/imu/lsm9ds1/lsm9ds1.py b/micropython/drivers/imu/lsm9ds1/lsm9ds1.py
@@ -2,6 +2,7 @@
 The MIT License (MIT)
 
 Copyright (c) 2013, 2014 Damien P. George
+Copyright (c) 2022-2023 Ibrahim Abdelkader <iabdalkader@openmv.io>
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
@@ -32,13 +33,13 @@
 from lsm9ds1 import LSM9DS1
 from machine import Pin, I2C
 
-lsm = LSM9DS1(I2C(1, scl=Pin(15), sda=Pin(14)))
+imu = LSM9DS1(I2C(1, scl=Pin(15), sda=Pin(14)))
 
 while (True):
-    #for g,a in lsm.iter_accel_gyro(): print(g,a)    # using fifo
-    print('Accelerometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*lsm.accel()))
-    print('Magnetometer:  x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*lsm.magnet()))
-    print('Gyroscope:     x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*lsm.gyro()))
+    #for g,a in imu.iter_accel_gyro(): print(g,a)    # using fifo
+    print('Accelerometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*imu.accel()))
+    print('Magnetometer:  x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*imu.magnet()))
+    print('Gyroscope:     x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*imu.gyro()))
     print("")
     time.sleep_ms(100)
 """
@@ -58,129 +59,142 @@
 _OFFSET_REG_X_M = const(0x05)
 _CTRL_REG1_M = const(0x20)
 _OUT_M = const(0x28)
-_SCALE_GYRO = const(((245, 0), (500, 1), (2000, 3)))
-_SCALE_ACCEL = const(((2, 0), (4, 2), (8, 3), (16, 1)))
+_ACCEL_SCALE = const((2, 16, 4, 8))
+_GYRO_SCALE = const((245, 500, 2000))
+_MAGNET_SCALE = const((4, 8, 12, 16))
+_ODR_IMU = const((0, 14.9, 59.5, 119, 238, 476, 952))
+_ODR_MAGNET = const((0.625, 1.25, 2.5, 5, 10, 20, 40, 80))
 
 
 class LSM9DS1:
-    def __init__(self, i2c, address_gyro=0x6B, address_magnet=0x1E):
-        self.i2c = i2c
-        self.address_gyro = address_gyro
+    def __init__(
+        self,
+        bus,
+        address_imu=0x6B,
+        address_magnet=0x1E,
+        gyro_odr=952,
+        gyro_scale=245,
+        accel_odr=952,
+        accel_scale=4,
+        magnet_odr=80,
+        magnet_scale=4,
+    ):
+        """Initalizes Gyro, Accelerometer and Magnetometer.
+        bus: IMU bus
+        address_imu: IMU I2C address.
+        address_magnet: Magnetometer I2C address.
+        gyro_odr: (0, 14.9Hz, 59.5Hz, 119Hz, 238Hz, 476Hz, 952Hz)
+        gyro_scale: (245dps, 500dps, 2000dps )
+        accel_odr: (0, 14.9Hz, 59.5Hz, 119Hz, 238Hz, 476Hz, 952Hz)
+        accel_scale: (+/-2g, +/-4g, +/-8g, +-16g)
+        magnet_odr: (0.625Hz, 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz)
+        magnet_scale: (+/-4, +/-8, +/-12, +/-16)
+        """
+        self.bus = bus
+        self.address_imu = address_imu
         self.address_magnet = address_magnet
-        # check id's of accelerometer/gyro and magnetometer
+
+        # Sanity checks
+        if not gyro_odr in _ODR_IMU:
+            raise ValueError("Invalid gyro sampling rate: %d" % gyro_odr)
+        if not gyro_scale in _GYRO_SCALE:
+            raise ValueError("Invalid gyro scaling: %d" % gyro_scale)
+
+        if not accel_odr in _ODR_IMU:
+            raise ValueError("Invalid accelerometer sampling rate: %d" % accel_odr)
+        if not accel_scale in _ACCEL_SCALE:
+            raise ValueError("Invalid accelerometer scaling: %d" % accel_scale)
+
+        if not magnet_odr in _ODR_MAGNET:
+            raise ValueError("Invalid magnet sampling rate: %d" % magnet_odr)
+        if not magnet_scale in _MAGNET_SCALE:
+            raise ValueError("Invalid magnet scaling: %d" % magnet_scale)
+
         if (self.magent_id() != b"=") or (self.gyro_id() != b"h"):
             raise OSError(
-                "Invalid LSM9DS1 device, using address {}/{}".format(address_gyro, address_magnet)
+                "Invalid LSM9DS1 device, using address {}/{}".format(address_imu, address_magnet)
             )
-        # allocate scratch buffer for efficient conversions and memread op's
-        self.scratch = array.array("B", [0, 0, 0, 0, 0, 0])
-        self.scratch_int = array.array("h", [0, 0, 0])
-        self.init_gyro_accel()
-        self.init_magnetometer()
-
-    def init_gyro_accel(self, sample_rate=6, scale_gyro=0, scale_accel=0):
-        """Initalizes Gyro and Accelerator.
-        sample rate: 0-6 (off, 14.9Hz, 59.5Hz, 119Hz, 238Hz, 476Hz, 952Hz)
-        scale_gyro: 0-2 (245dps, 500dps, 2000dps )
-        scale_accel: 0-3 (+/-2g, +/-4g, +/-8g, +-16g)
-        """
-        assert sample_rate <= 6, "invalid sampling rate: %d" % sample_rate
-        assert scale_gyro <= 2, "invalid gyro scaling: %d" % scale_gyro
-        assert scale_accel <= 3, "invalid accelerometer scaling: %d" % scale_accel
-
-        i2c = self.i2c
-        addr = self.address_gyro
-        mv = memoryview(self.scratch)
-        # angular control registers 1-3 / Orientation
-        mv[0] = ((sample_rate & 0x07) << 5) | ((_SCALE_GYRO[scale_gyro][1] & 0x3) << 3)
+
+        mv = memoryview(bytearray(6))
+
+        # Configure Gyroscope.
+        mv[0] = (_ODR_IMU.index(gyro_odr) << 5) | ((_GYRO_SCALE.index(gyro_scale)) << 3)
         mv[1:4] = b"\x00\x00\x00"
-        i2c.writeto_mem(addr, _CTRL_REG1_G, mv[:5])
-        # ctrl4 - enable x,y,z, outputs, no irq latching, no 4D
-        # ctrl5 - enable all axes, no decimation
+        self.bus.writeto_mem(self.address_imu, _CTRL_REG1_G, mv[:5])
+
+        # Configure Accelerometer
+        mv[0] = 0x38  # ctrl4 - enable x,y,z, outputs, no irq latching, no 4D
+        mv[1] = 0x38  # ctrl5 - enable all axes, no decimation
         # ctrl6 - set scaling and sample rate of accel
-        # ctrl7,8 - leave at default values
-        # ctrl9 - FIFO enabled
-        mv[0] = mv[1] = 0x38
-        mv[2] = ((sample_rate & 7) << 5) | ((_SCALE_ACCEL[scale_accel][1] & 0x3) << 3)
-        mv[3] = 0x00
-        mv[4] = 0x4
-        mv[5] = 0x2
-        i2c.writeto_mem(addr, _CTRL_REG4_G, mv[:6])
+        mv[2] = (_ODR_IMU.index(accel_odr) << 5) | ((_ACCEL_SCALE.index(accel_scale)) << 3)
+        mv[3] = 0x00  # ctrl7 - leave at default values
+        mv[4] = 0x4  # ctrl8 - leave at default values
+        mv[5] = 0x2  # ctrl9 - FIFO enabled
+        self.bus.writeto_mem(self.address_imu, _CTRL_REG4_G, mv)
 
         # fifo: use continous mode (overwrite old data if overflow)
-        i2c.writeto_mem(addr, _FIFO_CTRL_REG, b"\x00")
-        i2c.writeto_mem(addr, _FIFO_CTRL_REG, b"\xc0")
-
-        self.scale_gyro = 32768 / _SCALE_GYRO[scale_gyro][0]
-        self.scale_accel = 32768 / _SCALE_ACCEL[scale_accel][0]
+        self.bus.writeto_mem(self.address_imu, _FIFO_CTRL_REG, b"\x00")
+        self.bus.writeto_mem(self.address_imu, _FIFO_CTRL_REG, b"\xc0")
 
-    def init_magnetometer(self, sample_rate=7, scale_magnet=0):
-        """
-        sample rates = 0-7 (0.625, 1.25, 2.5, 5, 10, 20, 40, 80Hz)
-        scaling = 0-3 (+/-4, +/-8, +/-12, +/-16 Gauss)
-        """
-        assert sample_rate < 8, "invalid sample rate: %d (0-7)" % sample_rate
-        assert scale_magnet < 4, "invalid scaling: %d (0-3)" % scale_magnet
-        i2c = self.i2c
-        addr = self.address_magnet
-        mv = memoryview(self.scratch)
-        mv[0] = 0x40 | (sample_rate << 2)  # ctrl1: high performance mode
-        mv[1] = scale_magnet << 5  # ctrl2: scale, normal mode, no reset
+        # Configure Magnetometer
+        mv[0] = 0x40 | (magnet_odr << 2)  # ctrl1: high performance mode
+        mv[1] = _MAGNET_SCALE.index(magnet_scale) << 5  # ctrl2: scale, normal mode, no reset
         mv[2] = 0x00  # ctrl3: continous conversion, no low power, I2C
         mv[3] = 0x08  # ctrl4: high performance z-axis
         mv[4] = 0x00  # ctr5: no fast read, no block update
-        i2c.writeto_mem(addr, _CTRL_REG1_M, mv[:5])
-        self.scale_factor_magnet = 32768 / ((scale_magnet + 1) * 4)
+        self.bus.writeto_mem(self.address_magnet, _CTRL_REG1_M, mv[:5])
+
+        self.gyro_scale = 32768 / gyro_scale
+        self.accel_scale = 32768 / accel_scale
+        self.scale_factor_magnet = 32768 / ((_MAGNET_SCALE.index(magnet_scale) + 1) * 4)
+
+        # Allocate scratch buffer for efficient conversions and memread op's
+        self.scratch_int = array.array("h", [0, 0, 0])
 
     def calibrate_magnet(self, offset):
         """
-        offset is a magnet vecor that will be substracted by the magnetometer
+        offset is a magnet vector that will be subtracted by the magnetometer
         for each measurement. It is written to the magnetometer's offset register
         """
+        import struct
+
         offset = [int(i * self.scale_factor_magnet) for i in offset]
-        mv = memoryview(self.scratch)
-        mv[0] = offset[0] & 0xFF
-        mv[1] = offset[0] >> 8
-        mv[2] = offset[1] & 0xFF
-        mv[3] = offset[1] >> 8
-        mv[4] = offset[2] & 0xFF
-        mv[5] = offset[2] >> 8
-        self.i2c.writeto_mem(self.address_magnet, _OFFSET_REG_X_M, mv[:6])
+        self.bus.writeto_mem(self.address_magnet, _OFFSET_REG_X_M, struct.pack("<HHH", offset))
 
     def gyro_id(self):
-        return self.i2c.readfrom_mem(self.address_gyro, _WHO_AM_I, 1)
+        return self.bus.readfrom_mem(self.address_imu, _WHO_AM_I, 1)
 
     def magent_id(self):
-        return self.i2c.readfrom_mem(self.address_magnet, _WHO_AM_I, 1)
+        return self.bus.readfrom_mem(self.address_magnet, _WHO_AM_I, 1)
 
     def magnet(self):
         """Returns magnetometer vector in gauss.
         raw_values: if True, the non-scaled adc values are returned
         """
         mv = memoryview(self.scratch_int)
         f = self.scale_factor_magnet
-        self.i2c.readfrom_mem_into(self.address_magnet, _OUT_M | 0x80, mv)
+        self.bus.readfrom_mem_into(self.address_magnet, _OUT_M | 0x80, mv)
         return (mv[0] / f, mv[1] / f, mv[2] / f)
 
     def gyro(self):
         """Returns gyroscope vector in degrees/sec."""
         mv = memoryview(self.scratch_int)
-        f = self.scale_gyro
-        self.i2c.readfrom_mem_into(self.address_gyro, _OUT_G | 0x80, mv)
+        f = self.gyro_scale
+        self.bus.readfrom_mem_into(self.address_imu, _OUT_G | 0x80, mv)
         return (mv[0] / f, mv[1] / f, mv[2] / f)
 
     def accel(self):
         """Returns acceleration vector in gravity units (9.81m/s^2)."""
         mv = memoryview(self.scratch_int)
-        f = self.scale_accel
-        self.i2c.readfrom_mem_into(self.address_gyro, _OUT_XL | 0x80, mv)
+        f = self.accel_scale
+        self.bus.readfrom_mem_into(self.address_imu, _OUT_XL | 0x80, mv)
         return (mv[0] / f, mv[1] / f, mv[2] / f)
 
     def iter_accel_gyro(self):
         """A generator that returns tuples of (gyro,accelerometer) data from the fifo."""
         while True:
             fifo_state = int.from_bytes(
-                self.i2c.readfrom_mem(self.address_gyro, _FIFO_SRC, 1), "big"
+                self.bus.readfrom_mem(self.address_imu, _FIFO_SRC, 1), "big"
             )
             if fifo_state & 0x3F:
                 # print("Available samples=%d" % (fifo_state & 0x1f))
diff --git a/micropython/drivers/imu/lsm9ds1/manifest.py b/micropython/drivers/imu/lsm9ds1/manifest.py
@@ -1 +1,2 @@
+metadata(description="Driver for ST LSM9DS1 IMU.", version="1.0.0")
 module("lsm9ds1.py", opt=3)

Original file line number	Diff line number	Diff line change
`@@ -1 +1,2 @@`
	`1`	`+metadata(description="Driver for ST LSM9DS1 IMU.", version="1.0.0")`
`1`	`2`	`module("lsm9ds1.py", opt=3)`