diff --git a/micropython/drivers/imu/lsm9ds1/lsm9ds1.py b/micropython/drivers/imu/lsm9ds1/lsm9ds1.py index 5d9942a7..8042ecc6 100644 --- 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 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 @@ import time 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,100 +59,113 @@ _FIFO_SRC = const(0x2F) _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 + mv = memoryview(bytearray(6)) - 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) + # 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.bus.writeto_mem(self.address_imu, _FIFO_CTRL_REG, b"\x00") + self.bus.writeto_mem(self.address_imu, _FIFO_CTRL_REG, b"\xc0") - self.scale_gyro = 32768 / _SCALE_GYRO[scale_gyro][0] - self.scale_accel = 32768 / _SCALE_ACCEL[scale_accel][0] - - 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("