mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
scripts/libraries: Replace drivers with upstream versions.
This commit is contained in:
parent
72c2509907
commit
2b5b7963bb
@ -1,9 +1,17 @@
|
|||||||
|
# Relative humidity and temperature sensor example.
|
||||||
|
# Note Arduino Nano BLE 33 Sense Rev2 uses the HS3003.
|
||||||
|
|
||||||
import time
|
import time
|
||||||
import hts221
|
from hts221 import HTS221
|
||||||
from machine import Pin, I2C
|
from machine import Pin, I2C
|
||||||
|
|
||||||
bus = I2C(1, scl=Pin(15), sda=Pin(14))
|
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:
|
while True:
|
||||||
rH = hts.humidity()
|
rH = hts.humidity()
|
||||||
|
|||||||
@ -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)
|
||||||
@ -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)
|
|
||||||
@ -1,7 +1,7 @@
|
|||||||
# HTS221 + BLE example.
|
# BLE temperature sensor example.
|
||||||
|
|
||||||
import time
|
import time
|
||||||
import hts221
|
from hts221 import HTS221
|
||||||
from board import LED
|
from board import LED
|
||||||
from machine import Pin, I2C
|
from machine import Pin, I2C
|
||||||
from ubluepy import Service, Characteristic, UUID, Peripheral, constants
|
from ubluepy import Service, Characteristic, UUID, Peripheral, constants
|
||||||
@ -45,7 +45,12 @@ periph.setConnectionHandler(event_handler)
|
|||||||
periph.advertise(device_name="Temperature Sensor", services=[service])
|
periph.advertise(device_name="Temperature Sensor", services=[service])
|
||||||
|
|
||||||
bus = I2C(1, scl=Pin(15), sda=Pin(14))
|
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:
|
while True:
|
||||||
if notif_enabled:
|
if notif_enabled:
|
||||||
|
|||||||
@ -1,13 +1,15 @@
|
|||||||
# LSM9DS1 Gyro example.
|
# LSM6DSOX Basic Example.
|
||||||
import time
|
import time
|
||||||
from lsm6dsox import LSM6DSOX
|
from lsm6dsox import LSM6DSOX
|
||||||
|
from machine import Pin
|
||||||
from machine import Pin, I2C
|
from machine import I2C
|
||||||
|
|
||||||
lsm = LSM6DSOX(I2C(0, scl=Pin(13), sda=Pin(12)))
|
lsm = LSM6DSOX(I2C(0, scl=Pin(13), sda=Pin(12)))
|
||||||
|
# Or init in SPI mode.
|
||||||
|
# lsm = LSM6DSOX(SPI(5), cs=Pin(10))
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
print("Accelerometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}".format(*lsm.read_accel()))
|
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.read_gyro()))
|
print("Gyroscope: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}".format(*lsm.gyro()))
|
||||||
print("")
|
print("")
|
||||||
time.sleep_ms(100)
|
time.sleep_ms(100)
|
||||||
|
|||||||
@ -4,9 +4,9 @@
|
|||||||
# NOTE: The pre-trained models (UCF files) for the examples can be found here:
|
# 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
|
# https://github.com/STMicroelectronics/STMems_Machine_Learning_Core/tree/master/application_examples/lsm6dsox
|
||||||
|
|
||||||
|
from lsm6dsox import LSM6DSOX
|
||||||
from machine import Pin
|
from machine import Pin
|
||||||
from machine import I2C
|
from machine import I2C
|
||||||
from lsm6dsox import LSM6DSOX
|
|
||||||
|
|
||||||
INT_MODE = True # Run in interrupt mode.
|
INT_MODE = True # Run in interrupt mode.
|
||||||
INT_FLAG = False # Set True on interrupt.
|
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_FILE = "lsm6dsox_vibration_monitoring.ucf"
|
||||||
UCF_LABELS = {0: "no vibration", 1: "low vibration", 2: "high vibration"}
|
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.
|
# NOTE: Selected data rate and scale must match the MLC data rate and scale.
|
||||||
lsm = LSM6DSOX(
|
lsm = LSM6DSOX(i2c, gyro_odr=26, accel_odr=26, gyro_scale=2000, accel_scale=4, ucf=UCF_FILE)
|
||||||
i2c, gyro_odr=26, accel_odr=26, gyro_scale=2000, accel_scale=4, ucf=UCF_FILE
|
|
||||||
)
|
|
||||||
|
|
||||||
# Head gestures example
|
# Head gestures example
|
||||||
# UCF_FILE = "lsm6dsox_head_gestures.ucf"
|
# UCF_FILE = "lsm6dsox_head_gestures.ucf"
|
||||||
@ -43,8 +41,8 @@ while True:
|
|||||||
if INT_MODE:
|
if INT_MODE:
|
||||||
if INT_FLAG:
|
if INT_FLAG:
|
||||||
INT_FLAG = False
|
INT_FLAG = False
|
||||||
print(UCF_LABELS[lsm.read_mlc_output()[0]])
|
print(UCF_LABELS[lsm.mlc_output()[0]])
|
||||||
else:
|
else:
|
||||||
buf = lsm.read_mlc_output()
|
buf = lsm.mlc_output()
|
||||||
if buf is not None:
|
if buf is not None:
|
||||||
print(UCF_LABELS[buf[0]])
|
print(UCF_LABELS[buf[0]])
|
||||||
|
|||||||
@ -4,10 +4,10 @@ from lsm6dsox import LSM6DSOX
|
|||||||
from machine import Pin
|
from machine import Pin
|
||||||
from machine import SPI
|
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:
|
while True:
|
||||||
print("Accelerometer: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}".format(*lsm.read_accel()))
|
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.read_gyro()))
|
print("Gyroscope: x:{:>8.3f} y:{:>8.3f} z:{:>8.3f}".format(*lsm.gyro()))
|
||||||
print("")
|
print("")
|
||||||
time.sleep_ms(100)
|
time.sleep_ms(100)
|
||||||
|
|||||||
@ -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.
|
# NOTE: Selected data rate and scale must match the MLC data rate and scale.
|
||||||
lsm = LSM6DSOX(
|
lsm = LSM6DSOX(
|
||||||
SPI(5),
|
SPI(5),
|
||||||
cs_pin=Pin("PF6", Pin.OUT_PP, Pin.PULL_UP),
|
cs=Pin("PF6", Pin.OUT_PP, Pin.PULL_UP),
|
||||||
gyro_odr=26,
|
gyro_odr=26,
|
||||||
accel_odr=26,
|
accel_odr=26,
|
||||||
gyro_scale=2000,
|
gyro_scale=2000,
|
||||||
@ -48,8 +48,8 @@ while True:
|
|||||||
if INT_MODE:
|
if INT_MODE:
|
||||||
if INT_FLAG:
|
if INT_FLAG:
|
||||||
INT_FLAG = False
|
INT_FLAG = False
|
||||||
print(UCF_LABELS[lsm.read_mlc_output()[0]])
|
print(UCF_LABELS[lsm.mlc_output()[0]])
|
||||||
else:
|
else:
|
||||||
buf = lsm.read_mlc_output()
|
buf = lsm.mlc_output()
|
||||||
if buf is not None:
|
if buf is not None:
|
||||||
print(UCF_LABELS[buf[0]])
|
print(UCF_LABELS[buf[0]])
|
||||||
|
|||||||
@ -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
|
|
||||||
)
|
|
||||||
@ -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
|
|
||||||
@ -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)
|
|
||||||
@ -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
|
|
||||||
@ -3,7 +3,6 @@
|
|||||||
# Drivers
|
# Drivers
|
||||||
require("hts221")
|
require("hts221")
|
||||||
require("lps22h")
|
require("lps22h")
|
||||||
require("lsm9ds1")
|
|
||||||
require("bmm150")
|
require("bmm150")
|
||||||
require("bmi270")
|
require("bmi270")
|
||||||
require("hs3003")
|
require("hs3003")
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user