Last active
September 2, 2023 02:11
-
-
Save algas/c3cd20db98b3cbfca41124f988ee013b to your computer and use it in GitHub Desktop.
M5Stack Fire 9DoF IMU (ICM-20948)
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
''' | |
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