Skip to content

Instantly share code, notes, and snippets.

@algas

algas/m5stack_icm20948.py

Last active September 2, 2023 02:11
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save algas/c3cd20db98b3cbfca41124f988ee013b to your computer and use it in GitHub Desktop.
Save algas/c3cd20db98b3cbfca41124f988ee013b to your computer and use it in GitHub Desktop.
Download ZIP
M5Stack Fire 9DoF IMU (ICM-20948)
Raw
m5stack_icm20948.py
'''
Copyright 2020 SparkFun Electronics
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.
'''
from m5stack import *
from m5ui import *
from uiflow import *
setScreenColor(0x222222)
import i2c_bus
# The name of this device
_DEFAULT_NAME = "Qwiic ICM20948"
# Some devices have multiple availabel addresses - this is a list of these addresses.
# NOTE: The first address in this list is considered the default I2C address for the
# device.
_AVAILABLE_I2C_ADDRESS = [0x69, 0x68]
# define our valid chip IDs
_validChipIDs = [0xEA]
# Internal Sensor IDs, used in various functions as arguments to know who to affect
ICM_20948_Internal_Acc = (1 << 0)
ICM_20948_Internal_Gyr = (1 << 1)
ICM_20948_Internal_Mag = (1 << 2)
ICM_20948_Internal_Tmp = (1 << 3)
ICM_20948_Internal_Mst = (1 << 4) # I2C Master Ineternal
# Sample mode options
ICM_20948_Sample_Mode_Continuous = 0x00
ICM_20948_Sample_Mode_Cycled = 0x01
# Accel full scale range options [AGB2_REG_ACCEL_CONFIG]
gpm2 = 0x00 # G forces Plus or Minus (aka "gpm")
gpm4 = 0x01
gpm8 = 0x02
gpm16 = 0x03
# Gyro full scale range options [AGB2_REG_GYRO_CONFIG_1]
dps250 = 0x00 #degrees per second (aka "dps")
dps500 = 0x01
dps1000 = 0x02
dps2000 = 0x03
# Accelerometer low pass filter configuration options
# Format is dAbwB_nXbwZ - A is integer part of 3db BW, B is fraction. X is integer part of nyquist bandwidth, Y is fraction
acc_d246bw_n265bw = 0x00
acc_d246bw_n265bw_1 = 0x01
acc_d111bw4_n136bw = 0x02
acc_d50bw4_n68bw8 = 0x03
acc_d23bw9_n34bw4 = 0x04
acc_d11bw5_n17bw = 0x05
acc_d5bw7_n8bw3 = 0x06
acc_d473bw_n499bw = 0x07
# Gryo low pass filter configuration options
# Format is dAbwB_nXbwZ - A is integer part of 3db BW, B is fraction. X is integer part of nyquist bandwidth, Y is fraction
gyr_d196bw6_n229bw8 = 0x00
gyr_d151bw8_n187bw6 = 0x01
gyr_d119bw5_n154bw3 = 0x02
gyr_d51bw2_n73bw3 = 0x03
gyr_d23bw9_n35bw9 = 0x04
gyr_d11bw6_n17bw8 = 0x05
gyr_d5bw7_n8bw9 = 0x06
gyr_d361bw4_n376bw5 = 0x07
# Magnetometer specific stuff
MAG_AK09916_I2C_ADDR = 0x0C
MAG_AK09916_WHO_AM_I = 0x4809
MAG_REG_WHO_AM_I = 0x00
AK09916_mode_power_down = 0x00
AK09916_mode_single = (0x01 << 0)
AK09916_mode_cont_10hz = (0x01 << 1)
AK09916_mode_cont_20hz = (0x02 << 1)
AK09916_mode_cont_50hz = (0x03 << 1)
AK09916_mode_cont_100hz = (0x04 << 1)
AK09916_mode_self_test = (0x01 << 4)
#Magnetometer Registers (aka sub-addresses when reading as I2C Master)
AK09916_REG_WIA1 = 0x00
AK09916_REG_WIA2 = 0x01
AK09916_REG_ST1 = 0x10
AK09916_REG_HXL = 0x11
AK09916_REG_HXH = 0x12
AK09916_REG_HYL = 0x13
AK09916_REG_HYH = 0x14
AK09916_REG_HZL = 0x15
AK09916_REG_HZH = 0x16
AK09916_REG_ST2 = 0x18
AK09916_REG_CNTL2 = 0x31
AK09916_REG_CNTL3 = 0x32
class QwiicIcm20948(object):
device_name = _DEFAULT_NAME
available_addresses = _AVAILABLE_I2C_ADDRESS
# Generalized
REG_BANK_SEL = 0x7F
# Gyroscope and Accelerometer
# User Bank 0
AGB0_REG_WHO_AM_I = 0x00
AGB0_REG_USER_CTRL = 0x03
AGB0_REG_LP_CONFIG = 0x05
AGB0_REG_PWR_MGMT_1 = 0x06
AGB0_REG_PWR_MGMT_2 = 0x07
AGB0_REG_INT_PIN_CONFIG = 0x0F
AGB0_REG_INT_ENABLE = 0x10
AGB0_REG_INT_ENABLE_1 = 0x11
AGB0_REG_INT_ENABLE_2 = 0x12
AGB0_REG_INT_ENABLE_3 = 0x13
AGB0_REG_I2C_MST_STATUS = 0x17
AGB0_REG_INT_STATUS = 0x19
AGB0_REG_INT_STATUS_1 = 0x1A
AGB0_REG_INT_STATUS_2 = 0x1B
AGB0_REG_INT_STATUS_3 = 0x1C
AGB0_REG_DELAY_TIMEH = 0x28
AGB0_REG_DELAY_TIMEL = 0x29
AGB0_REG_ACCEL_XOUT_H = 0x2D
AGB0_REG_ACCEL_XOUT_L = 0x2E
AGB0_REG_ACCEL_YOUT_H = 0x2F
AGB0_REG_ACCEL_YOUT_L = 0x30
AGB0_REG_ACCEL_ZOUT_H = 0x31
AGB0_REG_ACCEL_ZOUT_L = 0x32
AGB0_REG_GYRO_XOUT_H = 0x33
AGB0_REG_GYRO_XOUT_L = 0x34
AGB0_REG_GYRO_YOUT_H = 0x35
AGB0_REG_GYRO_YOUT_L = 0x36
AGB0_REG_GYRO_ZOUT_H = 0x37
AGB0_REG_GYRO_ZOUT_L = 0x38
AGB0_REG_TEMP_OUT_H = 0x39
AGB0_REG_TEMP_OUT_L = 0x3A
AGB0_REG_EXT_SLV_SENS_DATA_00 = 0x3B
AGB0_REG_EXT_SLV_SENS_DATA_01 = 0x3C
AGB0_REG_EXT_SLV_SENS_DATA_02 = 0x3D
AGB0_REG_EXT_SLV_SENS_DATA_03 = 0x3E
AGB0_REG_EXT_SLV_SENS_DATA_04 = 0x3F
AGB0_REG_EXT_SLV_SENS_DATA_05 = 0x40
AGB0_REG_EXT_SLV_SENS_DATA_06 = 0x41
AGB0_REG_EXT_SLV_SENS_DATA_07 = 0x42
AGB0_REG_EXT_SLV_SENS_DATA_08 = 0x43
AGB0_REG_EXT_SLV_SENS_DATA_09 = 0x44
AGB0_REG_EXT_SLV_SENS_DATA_10 = 0x45
AGB0_REG_EXT_SLV_SENS_DATA_11 = 0x46
AGB0_REG_EXT_SLV_SENS_DATA_12 = 0x47
AGB0_REG_EXT_SLV_SENS_DATA_13 = 0x48
AGB0_REG_EXT_SLV_SENS_DATA_14 = 0x49
AGB0_REG_EXT_SLV_SENS_DATA_15 = 0x4A
AGB0_REG_EXT_SLV_SENS_DATA_16 = 0x4B
AGB0_REG_EXT_SLV_SENS_DATA_17 = 0x4C
AGB0_REG_EXT_SLV_SENS_DATA_18 = 0x4D
AGB0_REG_EXT_SLV_SENS_DATA_19 = 0x4E
AGB0_REG_EXT_SLV_SENS_DATA_20 = 0x4F
AGB0_REG_EXT_SLV_SENS_DATA_21 = 0x50
AGB0_REG_EXT_SLV_SENS_DATA_22 = 0x51
AGB0_REG_EXT_SLV_SENS_DATA_23 = 0x52
AGB0_REG_FIFO_EN_1 = 0x66
AGB0_REG_FIFO_EN_2 = 0x67
AGB0_REG_FIFO_MODE = 0x69
AGB0_REG_FIFO_COUNT_H = 0x70
AGB0_REG_FIFO_COUNT_L = 0x71
AGB0_REG_FIFO_R_W = 0x72
AGB0_REG_DATA_RDY_STATUS = 0x74
AGB0_REG_FIFO_CFG = 0x76
AGB0_REG_MEM_START_ADDR = 0x7C # Hmm Invensense thought they were sneaky not listing these locations on the datasheet...
AGB0_REG_MEM_R_W = 0x7D # These three locations seem to be able to access some memory within the device
AGB0_REG_MEM_BANK_SEL = 0x7E # And that location is also where the DMP image gets loaded
AGB0_REG_REG_BANK_SEL = 0x7F
# Bank 1
AGB1_REG_SELF_TEST_X_GYRO = 0x02
AGB1_REG_SELF_TEST_X_GYRO = 0x02
AGB1_REG_SELF_TEST_Y_GYRO = 0x03
AGB1_REG_SELF_TEST_Z_GYRO = 0x04
AGB1_REG_SELF_TEST_X_ACCEL = 0x0E
AGB1_REG_SELF_TEST_Y_ACCEL = 0x0F
AGB1_REG_SELF_TEST_Z_ACCEL = 0x10
AGB1_REG_XA_OFFS_H = 0x14
AGB1_REG_XA_OFFS_L = 0x15
AGB1_REG_YA_OFFS_H = 0x17
AGB1_REG_YA_OFFS_L = 0x18
AGB1_REG_ZA_OFFS_H = 0x1A
AGB1_REG_ZA_OFFS_L = 0x1B
AGB1_REG_TIMEBASE_CORRECTION_PLL = 0x28
AGB1_REG_REG_BANK_SEL = 0x7F
# Bank 2
AGB2_REG_GYRO_SMPLRT_DIV = 0x00
AGB2_REG_GYRO_CONFIG_1 = 0x01
AGB2_REG_GYRO_CONFIG_2 = 0x02
AGB2_REG_XG_OFFS_USRH = 0x03
AGB2_REG_XG_OFFS_USRL = 0x04
AGB2_REG_YG_OFFS_USRH = 0x05
AGB2_REG_YG_OFFS_USRL = 0x06
AGB2_REG_ZG_OFFS_USRH = 0x07
AGB2_REG_ZG_OFFS_USRL = 0x08
AGB2_REG_ODR_ALIGN_EN = 0x09
AGB2_REG_ACCEL_SMPLRT_DIV_1 = 0x10
AGB2_REG_ACCEL_SMPLRT_DIV_2 = 0x11
AGB2_REG_ACCEL_INTEL_CTRL = 0x12
AGB2_REG_ACCEL_WOM_THR = 0x13
AGB2_REG_ACCEL_CONFIG_1 = 0x14
AGB2_REG_ACCEL_CONFIG_2 = 0x15
AGB2_REG_FSYNC_CONFIG = 0x52
AGB2_REG_TEMP_CONFIG = 0x53
AGB2_REG_MOD_CTRL_USR = 0x54
AGB2_REG_REG_BANK_SEL = 0x7F
# Bank 3
AGB3_REG_I2C_MST_ODR_CONFIG = 0x00
AGB3_REG_I2C_MST_CTRL = 0x01
AGB3_REG_I2C_MST_DELAY_CTRL = 0x02
AGB3_REG_I2C_SLV0_ADDR = 0x03
AGB3_REG_I2C_SLV0_REG = 0x04
AGB3_REG_I2C_SLV0_CTRL = 0x05
AGB3_REG_I2C_SLV0_DO = 0x06
AGB3_REG_I2C_SLV1_ADDR = 0x07
AGB3_REG_I2C_SLV1_REG = 0x08
AGB3_REG_I2C_SLV1_CTRL = 0x09
AGB3_REG_I2C_SLV1_DO = 0x0A
AGB3_REG_I2C_SLV2_ADDR = 0x0B
AGB3_REG_I2C_SLV2_REG = 0x0C
AGB3_REG_I2C_SLV2_CTRL = 0x0D
AGB3_REG_I2C_SLV2_DO = 0x0E
AGB3_REG_I2C_SLV3_ADDR = 0x0F
AGB3_REG_I2C_SLV3_REG = 0x10
AGB3_REG_I2C_SLV3_CTRL = 0x11
AGB3_REG_I2C_SLV3_DO = 0x12
AGB3_REG_I2C_SLV4_ADDR = 0x13
AGB3_REG_I2C_SLV4_REG = 0x14
AGB3_REG_I2C_SLV4_CTRL = 0x15
AGB3_REG_I2C_SLV4_DO = 0x16
AGB3_REG_I2C_SLV4_DI = 0x17
AGB3_REG_REG_BANK_SEL = 0x7F
# Magnetometer
M_REG_WIA2 = 0x01
M_REG_ST1 = 0x10
M_REG_HXL = 0x11
M_REG_HXH = 0x12
M_REG_HYL = 0x13
M_REG_HYH = 0x14
M_REG_HZL = 0x15
M_REG_HZH = 0x16
M_REG_ST2 = 0x18
M_REG_CNTL2 = 0x31
M_REG_CNTL3 = 0x32
M_REG_TS1 = 0x33
M_REG_TS2 = 0x34
# Constructor
def __init__(self, lcd=None, address=None, bus_port=None, frequency=None):
self.lcd = lcd
port = bus_port if bus_port is not None else i2c_bus.PORTA
freq = frequency if frequency is not None else 100000
i2c0 = i2c_bus.easyI2C(port, 0x00, freq=freq)
addr = address if address is not None else self.available_addresses[0]
i2c0.addr=(addr)
if i2c0.available():
self._i2c = i2c0
self.print('init success', 0, 0, 0xffffff)
else:
self._i2c = None
print('init failure')
def setBank(self, bank):
if 3 < bank:
print("Invalid Bank value: %d" % bank)
return False
bank = ((bank << 4) & 0x30)
return self.writeByte(self.REG_BANK_SEL, bank)
def readByte(self, address):
return self._i2c.read_u8(address)
def writeByte(self, address, data):
return self._i2c.write_u8(address, data)
def readByteWithBank(self, bank, address):
self.setBank(bank)
return self.readByte(address)
def writeByteWithBank(self, bank, address, data):
self.setBank(bank)
return self.writeByte(address, data)
def readBlock(self, address, numbytes):
return self._i2c.read_reg(address, numbytes)
def print(self, data, x, y, color, rotate=0):
self.lcd.print(data, x, y, color, rotate=rotate)
def clear(self):
self.lcd.clear()
def swReset(self):
# Read the Power Management Register, store in local variable "register"
self.setBank(0)
register = self.readByte(self.AGB0_REG_PWR_MGMT_1)
# Set the device reset bit [7]
register |= (1<<7)
# Write register
self.setBank(0)
return self.writeByte(self.AGB0_REG_PWR_MGMT_1, register)
def sleep(self, on):
# Read the Power Management Register, store in local variable "register"
self.setBank(0)
register = self.readByte(self.AGB0_REG_PWR_MGMT_1)
# Set/clear the sleep bit [6] as needed
if on:
register |= (1<<6) # set bit
else:
register &= ~(1<<6) # clear bit
# Write register
self.setBank(0)
return self.writeByte(self.AGB0_REG_PWR_MGMT_1, register)
def lowPower(self, on):
# Read the Power Management Register, store in local variable "register"
self.setBank(0)
register = self.readByte(self.AGB0_REG_PWR_MGMT_1)
# Set/clear the low power mode bit [5] as needed
if on:
register |= (1<<5) # set bit
else:
register &= ~(1<<5) # clear bit
# Write register
self.setBank(0)
return self.writeByte(self.AGB0_REG_PWR_MGMT_1, register)
def setSampleMode(self, sensors, mode):
# check for valid sensor ID from user of this function
if ((sensors & (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr | ICM_20948_Internal_Mst)) == False):
print("Invalid Sensor ID")
return False
# Read the LP CONFIG Register, store in local variable "register"
self.setBank(0)
register = self.readByte(self.AGB0_REG_LP_CONFIG)
if (sensors & ICM_20948_Internal_Acc):
# Set/clear the sensor specific sample mode bit as needed
if mode == ICM_20948_Sample_Mode_Cycled:
register |= (1<<5) # set bit
elif mode == ICM_20948_Sample_Mode_Continuous:
register &= ~(1<<5) # clear bit
if (sensors & ICM_20948_Internal_Gyr):
# Set/clear the sensor specific sample mode bit as needed
if mode == ICM_20948_Sample_Mode_Cycled:
register |= (1<<4) # set bit
elif mode == ICM_20948_Sample_Mode_Continuous:
register &= ~(1<<4) # clear bit
if (sensors & ICM_20948_Internal_Mst):
# Set/clear the sensor specific sample mode bit as needed
if mode == ICM_20948_Sample_Mode_Cycled:
register |= (1<<6) # set bit
elif mode == ICM_20948_Sample_Mode_Continuous:
register &= ~(1<<6) # clear bit
# Write register
self.setBank(0)
return self.writeByte(self.AGB0_REG_LP_CONFIG, register)
def setFullScaleRangeAccel(self, mode):
# Read the Accel Config Register, store in local variable "register"
self.setBank(2)
register = self.readByte(self.AGB2_REG_ACCEL_CONFIG_1)
register &= ~(0b00000110) # clear bits 2:1 (0b0000.0XX0)
register |= (mode << 1) # place mode select into bits 2:1 of AGB2_REG_ACCEL_CONFIG
# Write register
self.setBank(2)
return self.writeByte(self.AGB2_REG_ACCEL_CONFIG_1, register)
def setFullScaleRangeGyro(self, mode):
# Read the Gyro Config Register, store in local variable "register"
self.setBank(2)
register = self.readByte(self.AGB2_REG_GYRO_CONFIG_1)
register &= ~(0b00000110) # clear bits 2:1 (0b0000.0XX0)
register |= (mode << 1) # place mode select into bits 2:1 of AGB2_REG_GYRO_CONFIG_1
# Write register
self.setBank(2)
return self.writeByte(self.AGB2_REG_GYRO_CONFIG_1, register)
def setDLPFcfgAccel(self, dlpcfg):
# Read the Accel Config Register, store in local variable "register"
self.setBank(2)
register = self.readByte(self.AGB2_REG_ACCEL_CONFIG_1)
register &= ~(0b00111000) # clear bits 5:3 (0b00XX.X000)
register |= (dlpcfg << 3) # place dlpcfg select into bits 5:3 of AGB2_REG_ACCEL_CONFIG_1
# Write register
self.setBank(2)
return self.writeByte(self.AGB2_REG_ACCEL_CONFIG_1, register)
def setDLPFcfgGyro(self, dlpcfg):
# Read the gyro Config Register, store in local variable "register"
self.setBank(2)
register = self.readByte(self.AGB2_REG_GYRO_CONFIG_1)
register &= ~(0b00111000) # clear bits 5:3 (0b00XX.X000)
register |= (dlpcfg << 3) # place dlpcfg select into bits 5:3 of AGB2_REG_GYRO_CONFIG_1
# Write register
self.setBank(2)
return self.writeByte(self.AGB2_REG_GYRO_CONFIG_1, register)
def enableDlpfAccel(self, on):
# Read the AGB2_REG_ACCEL_CONFIG_1, store in local variable "register"
self.setBank(2)
register = self.readByte(self.AGB2_REG_ACCEL_CONFIG_1)
# Set/clear the ACCEL_FCHOICE bit [0] as needed
if on:
register |= (1<<0) # set bit
else:
register &= ~(1<<0) # clear bit
# Write register
self.setBank(2)
return self.writeByte(self.AGB2_REG_ACCEL_CONFIG_1, register)
def enableDlpfGyro(self, on):
# Read the AGB2_REG_GYRO_CONFIG_1, store in local variable "register"
self.setBank(2)
register = self.readByte(self.AGB2_REG_GYRO_CONFIG_1)
# Set/clear the GYRO_FCHOICE bit [0] as needed
if on:
register |= (1<<0) # set bit
else:
register &= ~(1<<0) # clear bit
# Write register
self.setBank(2)
return self.writeByte(self.AGB2_REG_GYRO_CONFIG_1, register)
def dataReady(self):
self.setBank(0)
register = self.readByte(self.AGB0_REG_INT_STATUS_1)
# check bit [0]
if (register & (1<<0)):
return True
else:
return False
def ToSignedInt(self, input):
if input > 32767:
input -= 65536
return input
def getAgmt(self):
# Read all of the readings starting at AGB0_REG_ACCEL_XOUT_H
numbytes = 14 + 9 # Read Accel, gyro, temp, and 9 bytes of mag
self.setBank(0)
buff = self.readBlock(self.AGB0_REG_ACCEL_XOUT_H, numbytes)
self.axRaw = ((buff[0] << 8) | (buff[1] & 0xFF))
self.ayRaw = ((buff[2] << 8) | (buff[3] & 0xFF))
self.azRaw = ((buff[4] << 8) | (buff[5] & 0xFF))
self.gxRaw = ((buff[6] << 8) | (buff[7] & 0xFF))
self.gyRaw = ((buff[8] << 8) | (buff[9] & 0xFF))
self.gzRaw = ((buff[10] << 8) | (buff[11] & 0xFF))
self.tmpRaw = ((buff[12] << 8) | (buff[13] & 0xFF))
self.magStat1 = buff[14]
self.mxRaw = ((buff[16] << 8) | (buff[15] & 0xFF)) # Mag data is read little endian
self.myRaw = ((buff[18] << 8) | (buff[17] & 0xFF))
self.mzRaw = ((buff[20] << 8) | (buff[19] & 0xFF))
self.magStat2 = buff[22]
# Convert all values to signed (because python treats all ints as 32 bit ints
# and does not see the MSB as the sign of our 16 bit int raw value)
self.axRaw = self.ToSignedInt(self.axRaw)
self.ayRaw = self.ToSignedInt(self.ayRaw)
self.azRaw = self.ToSignedInt(self.azRaw)
self.gxRaw = self.ToSignedInt(self.gxRaw)
self.gyRaw = self.ToSignedInt(self.gyRaw)
self.gzRaw = self.ToSignedInt(self.gzRaw)
self.mxRaw = self.ToSignedInt(self.mxRaw)
self.myRaw = self.ToSignedInt(self.myRaw)
self.mzRaw = self.ToSignedInt(self.mzRaw)
# check for data read error
if buff:
return True
else:
return False
def startupMagnetometer(self):
self.i2cMasterPassthrough(False)
self.i2cMasterEnable(True)
tries = 0
maxTries = 5
while (tries < maxTries):
if (self.magWhoIAm()):
break
self.i2cMasterReset()
tries += 1
if (tries == maxTries):
print("Mag ID fail. Tries: %d\n", tries)
return False
mag_reg_ctrl2 = 0x00
mag_reg_ctrl2 |= AK09916_mode_cont_100hz
self.writeMag(AK09916_REG_CNTL2, mag_reg_ctrl2)
return self.i2cMasterConfigureSlave(0, MAG_AK09916_I2C_ADDR, AK09916_REG_ST1, 9, True, True, False, False, False)
def i2cMasterPassthrough(self, passthrough):
# Read the AGB0_REG_INT_PIN_CONFIG, store in local variable "register"
self.setBank(0)
register = self.readByte(self.AGB0_REG_INT_PIN_CONFIG)
# Set/clear the BYPASS_EN bit [1] as needed
if passthrough:
register |= (1<<1) # set bit
else:
register &= ~(1<<1) # clear bit
# Write register
self.setBank(0)
return self.writeByte(self.AGB0_REG_INT_PIN_CONFIG, register)
def i2cMasterEnable(self, enable):
self.i2cMasterPassthrough(False) # Disable BYPASS_EN
# Setup Master Clock speed as 345.6 kHz, and NSP (aka next slave read) to "stop between reads"
# Read the AGB3_REG_I2C_MST_CTRL, store in local variable "register"
self.setBank(3)
register = self.readByte(self.AGB3_REG_I2C_MST_CTRL)
register &= ~(0x0F) # clear bits for master clock [3:0]
register |= (0x07) # set bits for master clock [3:0], 0x07 corresponds to 345.6 kHz, good for up to 400 kHz
register |= (1<<4) # set bit [4] for NSR (next slave read). 0 = restart between reads. 1 = stop between reads.
# Write register
self.setBank(3)
self.writeByte(self.AGB3_REG_I2C_MST_CTRL, register)
# enable/disable Master I2C
# Read the AGB0_REG_USER_CTRL, store in local variable "register"
self.setBank(0)
register = self.readByte(self.AGB0_REG_USER_CTRL)
# Set/clear the I2C_MST_EN bit [5] as needed
if enable:
register |= (1<<5) # set bit
else:
register &= ~(1<<5) # clear bit
# Write register
self.setBank(0)
return self.writeByte(self.AGB0_REG_USER_CTRL, register)
def magWhoIAm(self):
whoiam1 = self.readMag(AK09916_REG_WIA1)
whoiam2 = self.readMag(AK09916_REG_WIA2)
if ((whoiam1 == (MAG_AK09916_WHO_AM_I >> 8)) and (whoiam2 == (MAG_AK09916_WHO_AM_I & 0xFF))):
return True
else:
return False
def i2cMasterReset(self):
# Read the AGB0_REG_USER_CTRL, store in local variable "register"
self.setBank(0)
register = self.readByte(self.AGB0_REG_USER_CTRL)
# Set the I2C_MST_RST bit [1]
register |= (1<<1) # set bit
# Write register
self.setBank(0)
return self.writeByte(self.AGB0_REG_USER_CTRL, register)
def i2cMasterConfigureSlave(self, slave, addr, reg, len, Rw, enable, data_only, grp, swap):
# Adjust slave address, reg (aka sub-address), and control as needed for each slave slot (0-3)
slv_addr_reg = 0x00
slv_reg_reg = 0x00
slv_ctrl_reg = 0x00
if slave == 0:
slv_addr_reg = self.AGB3_REG_I2C_SLV0_ADDR
slv_reg_reg = self.AGB3_REG_I2C_SLV0_REG
slv_ctrl_reg = self.AGB3_REG_I2C_SLV0_CTRL
elif slave == 1:
slv_addr_reg = self.AGB3_REG_I2C_SLV1_ADDR
slv_reg_reg = self.AGB3_REG_I2C_SLV1_REG
slv_ctrl_reg = self.AGB3_REG_I2C_SLV1_CTRL
elif slave == 2:
slv_addr_reg = self.AGB3_REG_I2C_SLV2_ADDR
slv_reg_reg = self.AGB3_REG_I2C_SLV2_REG
slv_ctrl_reg = self.AGB3_REG_I2C_SLV2_CTRL
elif slave == 3:
slv_addr_reg = self.AGB3_REG_I2C_SLV3_ADDR
slv_reg_reg = self.AGB3_REG_I2C_SLV3_REG
slv_ctrl_reg = self.AGB3_REG_I2C_SLV3_CTRL
else:
return False
self.setBank(3)
# Set the slave address and the Rw flag
address = addr
if Rw:
address |= (1<<7) # set bit# set RNW bit [7]
self.writeByte(slv_addr_reg, address)
# Set the slave sub-address (reg)
subAddress = reg
self.writeByte(slv_reg_reg, subAddress)
# Set up the control info
ctrl_reg_slvX = 0x00
ctrl_reg_slvX |= len
ctrl_reg_slvX |= (enable << 7)
ctrl_reg_slvX |= (swap << 6)
ctrl_reg_slvX |= (data_only << 5)
ctrl_reg_slvX |= (grp << 4)
return self.writeByte(slv_ctrl_reg, ctrl_reg_slvX)
def ICM_20948_i2c_master_slv4_txn(self, addr, reg, data, Rw, send_reg_addr):
# Thanks MikeFair! // https://github.com/kriswiner/MPU9250/issues/86
if Rw:
addr |= 0x80
self.setBank(3)
self.writeByte(self.AGB3_REG_I2C_SLV4_ADDR, addr)
self.setBank(3)
self.writeByte(self.AGB3_REG_I2C_SLV4_REG, reg)
ctrl_register_slv4 = 0x00
ctrl_register_slv4 |= (1<<7) # EN bit [7] (set)
ctrl_register_slv4 &= ~(1<<6) # INT_EN bit [6] (cleared)
ctrl_register_slv4 &= ~(0x0F) # DLY bits [4:0] (cleared = 0)
if(send_reg_addr):
ctrl_register_slv4 &= ~(1<<5) # REG_DIS bit [5] (cleared)
else:
ctrl_register_slv4 |= (1<<5) # REG_DIS bit [5] (set)
txn_failed = False
if (Rw == False):
self.setBank(3)
self.writeByte(self.AGB3_REG_I2C_SLV4_DO, data)
# Kick off txn
self.setBank(3)
self.writeByte(self.AGB3_REG_I2C_SLV4_CTRL, ctrl_register_slv4)
max_cycles = 1000
count = 0
slave4Done = False
while (slave4Done == False):
self.setBank(0)
i2c_mst_status = self.readByte(self.AGB0_REG_I2C_MST_STATUS)
if i2c_mst_status & (1<<6): # Check I2C_SLAVE_DONE bit [6]
slave4Done = True
if count > max_cycles:
slave4Done = True
count += 1
if i2c_mst_status & (1<<4): # Check I2C_SLV4_NACK bit [4]
txn_failed = True
if count > max_cycles:
txn_failed = True
if txn_failed:
return False
if Rw:
self.setBank(3)
return self.readByte(self.AGB3_REG_I2C_SLV4_DI)
return True # if we get here, then it was a successful write
def i2cMasterSingleW(self, addr, reg, data):
data1 = self.ICM_20948_i2c_master_slv4_txn(addr, reg, data, False, True)
return data1
def writeMag(self, reg, data):
data = self.i2cMasterSingleW(MAG_AK09916_I2C_ADDR, reg, data)
return data
def i2cMasterSingleR(self, addr, reg):
data = self.ICM_20948_i2c_master_slv4_txn(addr, reg, 0, True, True)
return data
def readMag(self, reg):
data = self.i2cMasterSingleR(MAG_AK09916_I2C_ADDR, reg)
return data
def begin(self):
# are we who we need to be?
self.setBank(0)
chipID = self.readByte(self.AGB0_REG_WHO_AM_I)
if not chipID in _validChipIDs:
print("Invalid Chip ID: 0x%.2X" % chipID)
return False
# software reset
self.swReset()
wait(1)
# set sleep mode off
self.sleep(False)
# set lower power mode off
self.lowPower(False)
# set sample mode to continuous for both accel and gyro
self.setSampleMode((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous)
# set full scale range for both accel and gryo (separate functions)
self.setFullScaleRangeAccel(gpm2)
self.setFullScaleRangeGyro(dps250)
# set low pass filter for both accel and gyro (separate functions)
self.setDLPFcfgAccel(acc_d473bw_n499bw)
self.setDLPFcfgGyro(gyr_d361bw4_n376bw5)
# disable digital low pass filters on both accel and gyro
self.enableDlpfAccel(False)
self.enableDlpfGyro(False)
self.startupMagnetometer()
return True
IMU = QwiicIcm20948(lcd)
IMU.print('success to begin' if IMU.begin() else 'failure to begin', 0,20,0xffffff)
def buttonA_wasPressed():
if IMU.dataReady():
IMU.clear()
IMU.getAgmt()
IMU.print('pushA', 0, 40, 0xffffff)
IMU.print('ax:{ax}, ay:{ay}, az:{az}'.format(ax=IMU.axRaw,ay=IMU.ayRaw,az=IMU.azRaw), 0, 60, 0xffffff)
IMU.print('gx:{gx}, gy:{gy}, gz:{gz}'.format(gx=IMU.gxRaw,gy=IMU.gyRaw,gz=IMU.gzRaw), 0, 80, 0xffffff)
IMU.print('mx:{mx}, my:{my}, mz:{mz}'.format(mx=IMU.mxRaw,my=IMU.myRaw,mz=IMU.mzRaw), 0, 100, 0xffffff)
else:
IMU.print('pushA', 0, 40, 0xffffff)
pass
btnA.wasPressed(buttonA_wasPressed)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment