scripts/libraries: Replace drivers with upstream versions.

This commit is contained in:
iabdalkader 2023-10-02 14:08:38 +02:00
parent 72c2509907
commit 2b5b7963bb
13 changed files with 50 additions and 652 deletions

View File

@ -1,9 +1,17 @@
# Relative humidity and temperature sensor example.
# Note Arduino Nano BLE 33 Sense Rev2 uses the HS3003.
import time
import hts221
from hts221 import HTS221
from machine import Pin, I2C
bus = I2C(1, scl=Pin(15), sda=Pin(14))
hts = hts221.HTS221(bus)
try:
hts = HTS221(bus)
except OSError:
from hs3003 import HS3003
hts = HS3003(bus)
while True:
rH = hts.humidity()

View File

@ -0,0 +1,15 @@
# IMU example for Arduino Nano BLE 33 Sense (REV1 and REV2).
import time
import imu
from machine import Pin, I2C
bus = I2C(1, scl=Pin(15), sda=Pin(14))
imu = imu.IMU(bus)
while (True):
print('Accelerometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*imu.accel()))
print('Gyroscope: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*imu.gyro()))
print('Magnetometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*imu.magnet()))
print("")
time.sleep_ms(100)

View File

@ -1,14 +0,0 @@
import time
import lsm9ds1
from machine import Pin, I2C
bus = I2C(1, scl=Pin(15), sda=Pin(14))
lsm = lsm9ds1.LSM9DS1(bus)
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.read_accel()))
print("Magnetometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}".format(*lsm.read_magnet()))
print("Gyroscope: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}".format(*lsm.read_gyro()))
print("")
time.sleep_ms(500)

View File

@ -1,7 +1,7 @@
# HTS221 + BLE example.
# BLE temperature sensor example.
import time
import hts221
from hts221 import HTS221
from board import LED
from machine import Pin, I2C
from ubluepy import Service, Characteristic, UUID, Peripheral, constants
@ -45,7 +45,12 @@ periph.setConnectionHandler(event_handler)
periph.advertise(device_name="Temperature Sensor", services=[service])
bus = I2C(1, scl=Pin(15), sda=Pin(14))
hts = hts221.HTS221(bus)
try:
hts = HTS221(bus)
except OSError:
from hs3003 import HS3003
hts = HS3003(bus)
while True:
if notif_enabled:

View File

@ -1,13 +1,15 @@
# LSM9DS1 Gyro example.
# LSM6DSOX Basic Example.
import time
from lsm6dsox import LSM6DSOX
from machine import Pin, I2C
from machine import Pin
from machine import I2C
lsm = LSM6DSOX(I2C(0, scl=Pin(13), sda=Pin(12)))
# Or init in SPI mode.
# lsm = LSM6DSOX(SPI(5), cs=Pin(10))
while True:
print("Accelerometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}".format(*lsm.read_accel()))
print("Gyroscope: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}".format(*lsm.read_gyro()))
print("Accelerometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}".format(*lsm.accel()))
print("Gyroscope: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}".format(*lsm.gyro()))
print("")
time.sleep_ms(100)

View File

@ -4,9 +4,9 @@
# NOTE: The pre-trained models (UCF files) for the examples can be found here:
# https://github.com/STMicroelectronics/STMems_Machine_Learning_Core/tree/master/application_examples/lsm6dsox
from lsm6dsox import LSM6DSOX
from machine import Pin
from machine import I2C
from lsm6dsox import LSM6DSOX
INT_MODE = True # Run in interrupt mode.
INT_FLAG = False # Set True on interrupt.
@ -27,9 +27,7 @@ i2c = I2C(0, scl=Pin(13), sda=Pin(12))
UCF_FILE = "lsm6dsox_vibration_monitoring.ucf"
UCF_LABELS = {0: "no vibration", 1: "low vibration", 2: "high vibration"}
# NOTE: Selected data rate and scale must match the MLC data rate and scale.
lsm = LSM6DSOX(
i2c, gyro_odr=26, accel_odr=26, gyro_scale=2000, accel_scale=4, ucf=UCF_FILE
)
lsm = LSM6DSOX(i2c, gyro_odr=26, accel_odr=26, gyro_scale=2000, accel_scale=4, ucf=UCF_FILE)
# Head gestures example
# UCF_FILE = "lsm6dsox_head_gestures.ucf"
@ -43,8 +41,8 @@ while True:
if INT_MODE:
if INT_FLAG:
INT_FLAG = False
print(UCF_LABELS[lsm.read_mlc_output()[0]])
print(UCF_LABELS[lsm.mlc_output()[0]])
else:
buf = lsm.read_mlc_output()
buf = lsm.mlc_output()
if buf is not None:
print(UCF_LABELS[buf[0]])

View File

@ -4,10 +4,10 @@ from lsm6dsox import LSM6DSOX
from machine import Pin
from machine import SPI
lsm = LSM6DSOX(SPI(5), cs_pin=Pin("PF6", Pin.OUT_PP, Pin.PULL_UP))
lsm = LSM6DSOX(SPI(5), cs=Pin("PF6", Pin.OUT_PP, Pin.PULL_UP))
while True:
print("Accelerometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}".format(*lsm.read_accel()))
print("Gyroscope: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}".format(*lsm.read_gyro()))
print("Accelerometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}".format(*lsm.accel()))
print("Gyroscope: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}".format(*lsm.gyro()))
print("")
time.sleep_ms(100)

View File

@ -27,7 +27,7 @@ UCF_LABELS = {0: "no vibration", 1: "low vibration", 2: "high vibration"}
# NOTE: Selected data rate and scale must match the MLC data rate and scale.
lsm = LSM6DSOX(
SPI(5),
cs_pin=Pin("PF6", Pin.OUT_PP, Pin.PULL_UP),
cs=Pin("PF6", Pin.OUT_PP, Pin.PULL_UP),
gyro_odr=26,
accel_odr=26,
gyro_scale=2000,
@ -48,8 +48,8 @@ while True:
if INT_MODE:
if INT_FLAG:
INT_FLAG = False
print(UCF_LABELS[lsm.read_mlc_output()[0]])
print(UCF_LABELS[lsm.mlc_output()[0]])
else:
buf = lsm.read_mlc_output()
buf = lsm.mlc_output()
if buf is not None:
print(UCF_LABELS[buf[0]])

View File

@ -1,61 +0,0 @@
# This file is part of the OpenMV project.
#
# Copyright (c) 2013-2021 Ibrahim Abdelkader <iabdalkader@openmv.io>
# Copyright (c) 2013-2021 Kwabena W. Agyeman <kwagyeman@openmv.io>
#
# This work is licensed under the MIT license, see the file LICENSE for details.
#
# HTS221 driver based on public domain driver.
import time
import struct
class HTS221:
def __init__(self, i2c, data_rate=1, dev_addr=0x5F):
self.bus = i2c
self.odr = data_rate
self.slv_addr = dev_addr
# Set configuration register
# Humidity and temperature average configuration
self.bus.writeto_mem(self.slv_addr, 0x10, b"\x1B")
# Set control register
# PD | BDU | ODR
cfg = 0x80 | 0x04 | (self.odr & 0x3)
self.bus.writeto_mem(self.slv_addr, 0x20, bytes([cfg]))
# TODO needed ?
time.sleep_ms(100)
# Read Calibration values from non-volatile memory of the device
# Humidity Calibration values
self.H0 = self.read_reg(0x30, 1) / 2
self.H1 = self.read_reg(0x31, 1) / 2
self.H2 = self.read_reg(0x36, 2)
self.H3 = self.read_reg(0x3A, 2)
# Temperature Calibration values
raw = self.read_reg(0x35, 1)
self.T0 = ((raw & 0x03) * 256) + self.read_reg(0x32, 1)
self.T1 = ((raw & 0x0C) * 64) + self.read_reg(0x33, 1)
self.T2 = self.read_reg(0x3C, 2)
self.T3 = self.read_reg(0x3E, 2)
def read_reg(self, reg_addr, size):
fmt = "B" if size == 1 else "H"
reg_addr = reg_addr if size == 1 else reg_addr | 0x80
return struct.unpack(fmt, self.bus.readfrom_mem(self.slv_addr, reg_addr, size))[0]
def humidity(self):
rH = self.read_reg(0x28, 2)
return (self.H1 - self.H0) * (rH - self.H2) / (self.H3 - self.H2) + self.H0
def temperature(self):
temp = self.read_reg(0x2A, 2)
if temp > 32767:
temp -= 65536
return ((self.T1 - self.T0) / 8.0) * (temp - self.T2) / (self.T3 - self.T2) + (
self.T0 / 8.0
)

View File

@ -1,94 +0,0 @@
# LPS22HB/HH pressure seneor micropython drive
# ver: 2.0
# License: MIT
# Author: shaoziyang (shaoziyang@micropython.org.cn)
# v1.0 2016.4
# v2.0 2019.7
class LPS22H:
LPS22_CTRL_REG1 = const(0x10)
LPS22_CTRL_REG2 = const(0x11)
LPS22_STATUS = const(0x27)
LPS22_TEMP_OUT_L = const(0x2B)
LPS22_PRESS_OUT_XL = const(0x28)
LPS22_PRESS_OUT_L = const(0x29)
def __init__(self, i2c, addr=0x5C):
self.i2c = i2c
self.addr = addr
self.tb = bytearray(1)
self.rb = bytearray(1)
self.oneshot = False
self.irq_v = [0, 0]
# ODR=1 EN_LPFP=1 BDU=1
self.setreg(LPS22_CTRL_REG1, 0x1A)
self.oneshot_mode(False)
def oneshot_mode(self, oneshot=None):
if oneshot is None:
return self.oneshot
else:
self.getreg(LPS22_CTRL_REG1)
self.oneshot = oneshot
if oneshot:
self.rb[0] &= 0x0F
else:
self.rb[0] |= 0x10
self.setreg(LPS22_CTRL_REG1, self.rb[0])
def int16(self, d):
return d if d < 0x8000 else d - 0x10000
def setreg(self, reg, dat):
self.tb[0] = dat
self.i2c.writeto_mem(self.addr, reg, self.tb)
def getreg(self, reg):
self.i2c.readfrom_mem_into(self.addr, reg, self.rb)
return self.rb[0]
def get2reg(self, reg):
return self.getreg(reg) + self.getreg(reg + 1) * 256
def ONE_SHOT(self, b):
if self.oneshot:
self.setreg(LPS22_CTRL_REG2, self.getreg(LPS22_CTRL_REG2) | 0x01)
self.getreg(0x28 + b * 2)
while 1:
if self.getreg(LPS22_STATUS) & b:
return
def temperature(self):
self.ONE_SHOT(2)
try:
return self.int16(self.get2reg(LPS22_TEMP_OUT_L)) / 100
except MemoryError:
return self.temperature_irq()
def pressure(self):
self.ONE_SHOT(1)
try:
return (self.getreg(LPS22_PRESS_OUT_XL) + self.get2reg(LPS22_PRESS_OUT_L) * 256) / 4096
except MemoryError:
return self.pressure_irq()
def altitude(self):
return (
(((1013.25 / self.pressure()) ** (1 / 5.257)) - 1.0)
* (self.temperature() + 273.15)
/ 0.0065
)
def temperature_irq(self):
self.ONE_SHOT(2)
return self.int16(self.get2reg(LPS22_TEMP_OUT_L)) // 100
def pressure_irq(self):
self.ONE_SHOT(1)
return self.get2reg(LPS22_PRESS_OUT_L) >> 4
def get_irq(self):
self.irq_v[0] = self.temperature_irq()
self.irq_v[1] = self.pressure_irq()
return self.irq_v

View File

@ -1,271 +0,0 @@
"""
LSM6DSOX STMicro driver for MicroPython based on LSM9DS1:
Source repo: https://github.com/hoihu/projects/tree/master/raspi-hat
The MIT License (MIT)
Copyright (c) 2021 Damien P. George
Copyright (c) 2021-2022 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
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
Basic example usage:
import time
from lsm6dsox import LSM6DSOX
from machine import Pin, SPI, I2C
# Init in I2C mode.
lsm = LSM6DSOX(I2C(0, scl=Pin(13), sda=Pin(12)))
# Or init in SPI mode.
#lsm = LSM6DSOX(SPI(5), cs_pin=Pin(10))
while (True):
print('Accelerometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*lsm.read_accel()))
print('Gyroscope: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*lsm.read_gyro()))
print("")
time.sleep_ms(100)
"""
import array
from micropython import const
class LSM6DSOX:
_CTRL3_C = const(0x12)
_CTRL1_XL = const(0x10)
_CTRL8_XL = const(0x17)
_CTRL9_XL = const(0x18)
_CTRL2_G = const(0x11)
_CTRL7_G = const(0x16)
_OUTX_L_G = const(0x22)
_OUTX_L_XL = const(0x28)
_MLC_STATUS = const(0x38)
_DEFAULT_ADDR = const(0x6A)
_WHO_AM_I_REG = const(0x0F)
_FUNC_CFG_ACCESS = const(0x01)
_FUNC_CFG_BANK_USER = const(0)
_FUNC_CFG_BANK_HUB = const(1)
_FUNC_CFG_BANK_EMBED = const(2)
_MLC0_SRC = const(0x70)
_MLC_INT1 = const(0x0D)
_TAP_CFG0 = const(0x56)
_EMB_FUNC_EN_A = const(0x04)
_EMB_FUNC_EN_B = const(0x05)
def __init__(
self,
bus,
cs_pin=None,
address=_DEFAULT_ADDR,
gyro_odr=104,
accel_odr=104,
gyro_scale=2000,
accel_scale=4,
ucf=None,
):
"""Initializes Gyro and Accelerator.
accel_odr: (0, 1.6Hz, 3.33Hz, 6.66Hz, 12.5Hz, 26Hz, 52Hz, 104Hz, 208Hz, 416Hz, 888Hz)
gyro_odr: (0, 1.6Hz, 3.33Hz, 6.66Hz, 12.5Hz, 26Hz, 52Hz, 104Hz, 208Hz, 416Hz, 888Hz)
gyro_scale: (245dps, 500dps, 1000dps, 2000dps)
accel_scale: (+/-2g, +/-4g, +/-8g, +-16g)
ucf: MLC program to load.
"""
self.bus = bus
self.cs_pin = cs_pin
self.address = address
self._use_i2c = hasattr(self.bus, "readfrom_mem")
if not self._use_i2c and cs_pin is None:
raise ValueError("A CS pin must be provided in SPI mode")
# check the id of the Accelerometer/Gyro
if self.__read_reg(_WHO_AM_I_REG) != 108:
raise OSError("No LSM6DS device was found at address 0x%x" % (self.address))
# allocate scratch buffer for efficient conversions and memread op's
self.scratch_int = array.array("h", [0, 0, 0])
SCALE_GYRO = {250: 0, 500: 1, 1000: 2, 2000: 3}
SCALE_ACCEL = {2: 0, 4: 2, 8: 3, 16: 1}
# XL_HM_MODE = 0 by default. G_HM_MODE = 0 by default.
ODR = {
0: 0x00,
1.6: 0x08,
3.33: 0x09,
6.66: 0x0A,
12.5: 0x01,
26: 0x02,
52: 0x03,
104: 0x04,
208: 0x05,
416: 0x06,
888: 0x07,
}
gyro_odr = round(gyro_odr, 2)
accel_odr = round(accel_odr, 2)
# Sanity checks
if not gyro_odr in ODR:
raise ValueError("Invalid sampling rate: %d" % accel_odr)
if not gyro_scale in SCALE_GYRO:
raise ValueError("invalid gyro scaling: %d" % gyro_scale)
if not accel_odr in ODR:
raise ValueError("Invalid sampling rate: %d" % accel_odr)
if not accel_scale in SCALE_ACCEL:
raise ValueError("invalid accelerometer scaling: %d" % accel_scale)
# Soft-reset the device.
self.reset()
# Load and configure MLC if UCF file is provided
if ucf is not None:
self.load_mlc(ucf)
# Set Gyroscope datarate and scale.
# Note output from LPF2 second filtering stage is selected. See Figure 18.
self.__write_reg(_CTRL1_XL, (ODR[accel_odr] << 4) | (SCALE_ACCEL[accel_scale] << 2) | 2)
# Enable LPF2 and HPF fast-settling mode, ODR/4
self.__write_reg(_CTRL8_XL, 0x09)
# Set Gyroscope datarate and scale.
self.__write_reg(_CTRL2_G, (ODR[gyro_odr] << 4) | (SCALE_GYRO[gyro_scale] << 2) | 0)
self.gyro_scale = 32768 / gyro_scale
self.accel_scale = 32768 / accel_scale
def __read_reg(self, reg, size=1):
if self._use_i2c:
buf = self.bus.readfrom_mem(self.address, reg, size)
else:
try:
self.cs_pin(0)
self.bus.write(bytes([reg | 0x80]))
buf = self.bus.read(size)
finally:
self.cs_pin(1)
if size == 1:
return int(buf[0])
return [int(x) for x in buf]
def __write_reg(self, reg, val):
if self._use_i2c:
self.bus.writeto_mem(self.address, reg, bytes([val]))
else:
try:
self.cs_pin(0)
self.bus.write(bytes([reg, val]))
finally:
self.cs_pin(1)
def __read_reg_into(self, reg, buf):
if self._use_i2c:
self.bus.readfrom_mem_into(self.address, reg, buf)
else:
try:
self.cs_pin(0)
self.bus.write(bytes([reg | 0x80]))
self.bus.readinto(buf)
finally:
self.cs_pin(1)
def reset(self):
self.__write_reg(_CTRL3_C, self.__read_reg(_CTRL3_C) | 0x1)
for i in range(0, 10):
if (self.__read_reg(_CTRL3_C) & 0x01) == 0:
return
time.sleep_ms(10)
raise OSError("Failed to reset LSM6DS device.")
def set_mem_bank(self, bank):
cfg = self.__read_reg(_FUNC_CFG_ACCESS) & 0x3F
self.__write_reg(_FUNC_CFG_ACCESS, cfg | (bank << 6))
def set_embedded_functions(self, enable, emb_ab=None):
self.set_mem_bank(_FUNC_CFG_BANK_EMBED)
if enable:
self.__write_reg(_EMB_FUNC_EN_A, emb_ab[0])
self.__write_reg(_EMB_FUNC_EN_B, emb_ab[1])
else:
emb_a = self.__read_reg(_EMB_FUNC_EN_A)
emb_b = self.__read_reg(_EMB_FUNC_EN_B)
self.__write_reg(_EMB_FUNC_EN_A, (emb_a & 0xC7))
self.__write_reg(_EMB_FUNC_EN_B, (emb_b & 0xE6))
emb_ab = (emb_a, emb_b)
self.set_mem_bank(_FUNC_CFG_BANK_USER)
return emb_ab
def load_mlc(self, ucf):
# Load MLC config from file
with open(ucf, "r") as ucf_file:
for l in ucf_file:
if l.startswith("Ac"):
v = [int(v, 16) for v in l.strip().split(" ")[1:3]]
self.__write_reg(v[0], v[1])
emb_ab = self.set_embedded_functions(False)
# Disable I3C interface
self.__write_reg(_CTRL9_XL, self.__read_reg(_CTRL9_XL) | 0x01)
# Enable Block Data Update
self.__write_reg(_CTRL3_C, self.__read_reg(_CTRL3_C) | 0x40)
# Route signals on interrupt pin 1
self.set_mem_bank(_FUNC_CFG_BANK_EMBED)
self.__write_reg(_MLC_INT1, self.__read_reg(_MLC_INT1) & 0x01)
self.set_mem_bank(_FUNC_CFG_BANK_USER)
# Configure interrupt pin mode
self.__write_reg(_TAP_CFG0, self.__read_reg(_TAP_CFG0) | 0x41)
self.set_embedded_functions(True, emb_ab)
def read_mlc_output(self):
buf = None
if self.__read_reg(_MLC_STATUS) & 0x1:
self.__read_reg(0x1A, size=12)
self.set_mem_bank(_FUNC_CFG_BANK_EMBED)
buf = self.__read_reg(_MLC0_SRC, 8)
self.set_mem_bank(_FUNC_CFG_BANK_USER)
return buf
def read_gyro(self):
"""Returns gyroscope vector in degrees/sec."""
mv = memoryview(self.scratch_int)
f = self.gyro_scale
self.__read_reg_into(_OUTX_L_G, mv)
return (mv[0] / f, mv[1] / f, mv[2] / f)
def read_accel(self):
"""Returns acceleration vector in gravity units (9.81m/s^2)."""
mv = memoryview(self.scratch_int)
f = self.accel_scale
self.__read_reg_into(_OUTX_L_XL, mv)
return (mv[0] / f, mv[1] / f, mv[2] / f)

View File

@ -1,189 +0,0 @@
"""
The MIT License (MIT)
Copyright (c) 2013, 2014 Damien P. George
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
LSM9DS1 - 9DOF inertial sensor of STMicro driver for MicroPython.
The sensor contains an accelerometer / gyroscope / magnetometer
Uses the internal FIFO to store up to 16 gyro/accel data, use the iter_accel_gyro generator to access it.
Source repo: https://github.com/hoihu/projects/tree/master/raspi-hat
Example usage:
import time
from lsm9ds1 import LSM9DS1
from machine import Pin, I2C
lsm = 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.read_accel()))
print('Magnetometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*lsm.read_magnet()))
print('Gyroscope: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}'.format(*lsm.read_gyro()))
print("")
time.sleep_ms(100)
"""
import array
class LSM9DS1:
WHO_AM_I = const(0xF)
CTRL_REG1_G = const(0x10)
INT_GEN_SRC_G = const(0x14)
OUT_TEMP = const(0x15)
OUT_G = const(0x18)
CTRL_REG4_G = const(0x1E)
STATUS_REG = const(0x27)
OUT_XL = const(0x28)
FIFO_CTRL_REG = const(0x2E)
FIFO_SRC = const(0x2F)
OFFSET_REG_X_M = const(0x05)
CTRL_REG1_M = const(0x20)
OUT_M = const(0x28)
SCALE_GYRO = [(245, 0), (500, 1), (2000, 3)]
SCALE_ACCEL = [(2, 0), (4, 2), (8, 3), (16, 1)]
def __init__(self, i2c, address_gyro=0x6B, address_magnet=0x1E):
self.i2c = i2c
self.address_gyro = address_gyro
self.address_magnet = address_magnet
# check id's of accelerometer/gyro and magnetometer
if (self.read_id_magnet() != b"=") or (self.read_id_gyro() != b"h"):
raise OSError(
"Invalid LSM9DS1 device, using address {}/{}".format(address_gyro, 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):
"""Initializes 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) | ((self.SCALE_GYRO[scale_gyro][1] & 0x3) << 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
# 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) | ((self.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])
# fifo: use continuous 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 / self.SCALE_GYRO[scale_gyro][0]
self.scale_accel = 32768 / self.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
mv[2] = 0x00 # ctrl3: continuous 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)
def calibrate_magnet(self, offset):
"""
offset is a magnet vecor that will be substracted by the magnetometer
for each measurement. It is written to the magnetometer's offset register
"""
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])
def read_id_gyro(self):
return self.i2c.readfrom_mem(self.address_gyro, WHO_AM_I, 1)
def read_id_magnet(self):
return self.i2c.readfrom_mem(self.address_magnet, WHO_AM_I, 1)
def read_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)
return (mv[0] / f, mv[1] / f, mv[2] / f)
def read_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)
return (mv[0] / f, mv[1] / f, mv[2] / f)
def read_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)
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"
)
if fifo_state & 0x3F:
# print("Available samples=%d" % (fifo_state & 0x1f))
yield self.read_gyro(), self.read_accel()
else:
break

View File

@ -3,7 +3,6 @@
# Drivers
require("hts221")
require("lps22h")
require("lsm9ds1")
require("bmm150")
require("bmi270")
require("hs3003")