95 lines
2.9 KiB
Python
95 lines
2.9 KiB
Python
"""
|
|
ICM42670P
|
|
|
|
Micropython library for the ICM42670P(Accelerometer+Gyroscope)
|
|
=======================================================
|
|
|
|
#Preliminary composition 20220716
|
|
|
|
dahanzimin From the Mixly Team
|
|
"""
|
|
import time
|
|
from micropython import const
|
|
|
|
ICM42670_REG_DEVICE_ID = const(0x00)
|
|
ICM42670_REG_RESET = const(0x02)
|
|
ICM42670_REG_DATA = const(0x09)
|
|
ICM42670_REG_PWR_MGMT0 = const(0x1F)
|
|
ICM42670_REG_GYRO_CONFIG0 = const(0x20)
|
|
ICM42670_REG_ACCEL_CONFIG0 = const(0x21)
|
|
ICM42670_REG_APEX_DATA0 = const(0x31)
|
|
ICM42670_REG_WHO_AM_I = const(0x75)
|
|
|
|
AccRange_16g = 0
|
|
AccRange_8g = 1
|
|
AccRange_4g = 2
|
|
AccRange_2g = 3
|
|
|
|
GyrRange_2000dps = 0
|
|
GyrRange_1000dps = 1
|
|
GyrRange_500dps = 2
|
|
GyrRange_250dps = 3
|
|
|
|
Acc_Gyr_Odr_1600Hz = 0x05
|
|
Acc_Gyr_Odr_800Hz = 0x06
|
|
Acc_Gyr_Odr_400Hz = 0x07
|
|
Acc_Gyr_Odr_200Hz = 0x08
|
|
Acc_Gyr_Odr_100Hz = 0x09
|
|
Acc_Gyr_Odr_50Hz = 0x0A
|
|
Acc_Gyr_Odr_25Hz = 0x0B
|
|
Acc_Gyr_Odr_12_5Hz = 0x0C
|
|
|
|
class ICM42670:
|
|
def __init__(self, i2c_bus, addr=0x68, AccRange=AccRange_16g, GyrRange=GyrRange_2000dps, Acc_Gyr_Odr=Acc_Gyr_Odr_100Hz):
|
|
self._device = i2c_bus
|
|
self._address = addr
|
|
if self._chip_id() != 0x67:
|
|
raise AttributeError("Cannot find a ICM42670")
|
|
|
|
self._wreg(ICM42670_REG_RESET,0x10) #Software reset enabled
|
|
time.sleep(0.1)
|
|
self._wreg(ICM42670_REG_GYRO_CONFIG0,(GyrRange << 4) | Acc_Gyr_Odr) #Gyr-500HZ/2000dps
|
|
self._wreg(ICM42670_REG_ACCEL_CONFIG0,(AccRange << 4) | Acc_Gyr_Odr) #ACC-100HZ/16G
|
|
self._wreg(ICM42670_REG_PWR_MGMT0,0x1E)
|
|
time.sleep(0.1)
|
|
self.acc_lsb_div= 2 ** (11 + AccRange)
|
|
self.gyr_lsb_div= 2 ** (14 + GyrRange)
|
|
|
|
def _wreg(self, reg, val):
|
|
'''Write memory address'''
|
|
self._device.writeto_mem(self._address,reg,val.to_bytes(1, 'little'))
|
|
|
|
def _rreg(self, reg,nbytes=1):
|
|
'''Read memory address'''
|
|
return self._device.readfrom_mem(self._address, reg, nbytes)[0] if nbytes<=1 else self._device.readfrom_mem(self._address, reg, nbytes)[0:nbytes]
|
|
|
|
def _chip_id(self):
|
|
return self._rreg(ICM42670_REG_WHO_AM_I)
|
|
|
|
def u2s(self,n):
|
|
return n if n < (1 << 15) else n - (1 << 16)
|
|
|
|
def getdata(self):
|
|
_buffer=self._rreg(ICM42670_REG_DATA,14)
|
|
tmp= float(self.u2s(_buffer[0]<<8|_buffer[1]))/128+25
|
|
acc_x=float(self.u2s(_buffer[2]<<8|_buffer[3]))/self.acc_lsb_div
|
|
acc_y=float(self.u2s(_buffer[4]<<8|_buffer[5]))/self.acc_lsb_div
|
|
acc_z=float(self.u2s(_buffer[6]<<8|_buffer[7]))/self.acc_lsb_div
|
|
gyr_x=float(self.u2s(_buffer[8]<<8|_buffer[9]))/self.gyr_lsb_div
|
|
gyr_y=float(self.u2s(_buffer[10]<<8|_buffer[11]))/self.gyr_lsb_div
|
|
gyr_z=float(self.u2s(_buffer[12]<<8|_buffer[13]))/self.gyr_lsb_div
|
|
return (acc_x,acc_y,acc_z),(gyr_x,gyr_y,gyr_z),round(tmp,2)
|
|
|
|
def accelerometer(self):
|
|
return self.getdata()[0]
|
|
|
|
def strength(self):
|
|
from math import sqrt
|
|
return sqrt(self.accelerometer()[0]**2+self.accelerometer()[1]**2+self.accelerometer()[2]**2)
|
|
|
|
def gyroscope(self):
|
|
return self.getdata()[1]
|
|
|
|
def temperature(self):
|
|
return self.getdata()[2]
|