# mpu6050_test22_2019mar1901 tlfong01 2019mar20hkt1652
# Rpi3B+ stretch linux 4.14.34-v7+ arm python3.5.3

# *** Description ***

# 1. Setup I2C channel (for Rpi23+ stretch, speed is NOT adjustable, but fixed at 100k baud)
# 2. Reset MPU
# 3. Wakeup MPU
# 4. Read and print default value 0x68 in register WhoImI 0x75
# 5. Write something to registers gyro/accelConfig and read back

# *** Imports ***

import datetime
from   time import sleep
import inspect
import smbus
# *** I2C Init ***

i2cCh0    = smbus.SMBus(1)
i2cCh1    = i2cCh0

i2cChList = [i2cCh0, i2cCh1]

# *** Device Address ***

mpu6050Addr0 = 0x68  # address pin AD0 biased to ground
mpu6050Addr1 = 0x69  # address pin AD0 biased to +5V

i2cDvAddrList = [mpu6050Addr0, mpu6050Addr1]

# *** Register Addresses ***

regAddrDict = {
‘WHO_AM_I’      : 0x75,
‘PWR_MGMT_1’    : 0x6b,
‘PWR_MGMT_2’    : 0x6c,
‘SMPRT_DIV’     : 0x19,
‘LP_WAKE_CTRL’  : 0x99,
‘GYRO_XOUT_H’   : 0x43,
‘GYRO_XOUT_L’   : 0x44,
‘GYRO_YOUT_H’   : 0x45,
‘GYRO_YOUT_L’   : 0x46,
‘GYRO_ZOUT_H’   : 0x47,
‘GYRO_ZOUT_L’   : 0x48,
‘ACCEL_XOUT_H’  : 0x3b,
‘ACCEL_XOUT_L’  : 0x3c,
‘ACCEL_YOUT_H’  : 0x3d,
‘ACCEL_YOUT_L’  : 0x3e,
‘ACCEL_ZOUT_H’  : 0x3f,
‘ACCEL_ZOUT_L’  : 0x40,
‘TEMP_OUT_H’    : 0x41,
‘TEMP_OUT_L’    : 0x42,
‘SELF_TEST_X’   : 0x0d,
‘SELF_TEST_X’   : 0x0d,
‘XG_OFFS_USRH’  : 0x13,
‘XG_OFFS_USRL’  : 0x14,
‘YG_OFFS_USRH’  : 0x15,
‘YG_OFFS_USRL’  : 0x16,
‘ZG_OFFS_USRH’  : 0x17,
‘ZG_OFFS_USRL’  : 0x18,
‘XA_OFFS_USRH’  : 0x06,
‘XA_OFFS_USRL’  : 0x07,
‘YA_OFFS_USRH’  : 0x08,
‘YA_OFFS_USRL’  : 0x09,
‘ZA_OFFS_USRH’  : 0x0a,
‘ZA_OFFS_USRL’  : 0x0b,
‘GYRO_CONFIG’   : 0x1b,
‘ACCEL_CONFIG’  : 0x1c,
}

controlByteDict = {
‘Reset’                 : 0x80,
‘Wakeup’                : 0x00,
‘SampleRateDivider0x04’ : 0x04,
‘GyroConfig01’          : 0xe8, # trigger x, y, z, with full scale 0x01
‘AccelConfig01’         : 0xec, # trigger x, y, z, with full scale 0x11

‘0x00’: 0x00,
‘0x01’: 0x01,
‘0x03’: 0x03,
‘0x04’: 0x04,
‘0x07’: 0x07,
‘0x08’: 0x08,
‘0x10’: 0x10,
‘0x40’: 0x40,
‘0x55’: 0x55,
‘0x5a’: 0x5a,
‘0x66’: 0x66,
‘0x77’: 0x77,
‘0x80’: 0x80,
‘0x85’: 0x85,
‘0x88’: 0x88,
‘0x90’: 0x90,
‘0x95’: 0x95,
‘0x9a’: 0x9a,
‘0x9d’: 0x9d,
‘0x9f’: 0x9f,
‘0xa0’: 0xa0,
‘0xa4’: 0xa4,
‘0xa7’: 0xa7,
‘0xb0’: 0xb0,
‘0xc0’: 0xc0,
‘0xd0’: 0xd0,
‘0xe8’: 0xe8,
‘0x0f’: 0x0f,
‘0xf0’: 0xf0,
‘0xff’: 0xff,
}

mpuDict = {
‘Title’: ‘mpuDict’,
‘Dict’: {
‘0’:  {‘Name’: ‘MPU0’, ‘I2cChNum’: 0, ‘I2cDvAddrNum’: 0,
‘SampleRateDividerName’: ‘0x04’,
‘GyroOffsetXhigh’:  0x00, ‘GyroOffsetXlow’:  0x00,
‘GyroOffsetYhigh’:  0x00, ‘GyroOffsetYlow’:  0x00,
‘GyroOffsetZhigh’:  0x00, ‘GyroOffsetZlow’:  0x00,
‘AccelOffsetXhigh’: 0x00, ‘AccelOffsetXlow’: 0x00,
‘AccelOffsetYhigh’: 0x00, ‘AccelOffsetYlow’: 0x00,
‘AccelOffsetZhigh’: 0x00, ‘AccelOffsetZlow’: 0x00,
},
‘1’:  {‘Name’: ‘MPU1’, ‘I2cChNum’: 0, ‘I2cDvAddrNum’: 1,
‘SampleRateDividerName’: ‘0x04’,
‘GyroOffsetXhigh’:  0x00, ‘GyroOffsetXlow’:  0x00,
‘GyroOffsetYhigh’:  0x00, ‘GyroOffsetYlow’:  0x00,
‘GyroOffsetZhigh’:  0x00, ‘GyroOffsetZlow’:  0x00,
‘AccelOffsetXhigh’: 0x00, ‘AccelOffsetXlow’: 0x00,
‘AccelOffsetYhigh’: 0x00, ‘AccelOffsetYlow’: 0x00,
‘AccelOffsetZhigh’: 0x00, ‘AccelOffsetZlow’: 0x00,
},
}}

# *** Print Functions ***

def printDateTime():
timeNow = datetime.datetime.now()
timeNowStr = str(timeNow)[0:16]
print(‘      Date Time now =’, timeNowStr)
return

def convertByteToFourCharPadString(num):
s = hex(num)
s = (s[2:])
if (len(s) != 2):
s = ‘0’ + s
s = ‘0x’ + s
return s

# *** Set Reset Bit(s) Functions ***

def setOneBit(byte, bitPosition):
newByte = byte | (0x01 << bitPosition)
return newByte

def resetOneBit(byte, bitPosition):
newByte = byte & (~(0x01 << bitPosition))
return newByte

def setManyBits(byte, noChangeBitsMask, setManyBitsMask):
newByte = (byte & noChangeBitsMask) | setManyBitsMask
return newByte

def checkOneBit(byte, bitPosition):
if (byte & (0x01 << bitPosition)) >= 1:
return True
else:
return False

def testSetResetOneBitSetManyBits():
byte = 0x00
print(‘byte     =’, convertByteToFourCharPadString(byte))
newByte = setOneBit(byte, 3)
print(‘new byte =’, convertByteToFourCharPadString(newByte))
newByte = resetOneBit(byte, 3)
print(‘new byte =’, convertByteToFourCharPadString(newByte))
byte = 0x55
print(‘byte     =’, convertByteToFourCharPadString(byte))
newByte = setManyBits(byte, 0xf0, 0x02)
print(‘new byte =’, convertByteToFourCharPadString(newByte))
byte = 0x55
print(‘byte     =’, convertByteToFourCharPadString(byte))
status = checkOneBit(byte, 1)
print(‘status   =’, status)
return

# *** General I2C Device Read Write Print Device/Register Functions ***

def writeDevTwoBytes(i2cPort, devAddrByte, dataByte1, dataByte2):
i2cPort.write_byte_data(devAddrByte, dataByte1, dataByte2)
return

def readDevRegOneByte(i2cPort, devAddrByte, regAddrByte):
readByte = i2cPort.read_byte_data(devAddrByte, regAddrByte)
return readByte

def readDevRegOneWord(i2cPort, devAddrByte, regAddrByte):
readByteHigh = i2cPort.read_byte_data(devAddrByte, regAddrByte)
readByteLow  = i2cPort.read_byte_data(devAddrByte, regAddrByte + 1)
readWord     = (readByteHigh << 8) | readByteLow
if (readWord > 32768):
readWord = readWord – 65536
return readWord

def writeDevRegOneByte(i2cPort, devAddrByte, regName, writeByteName):
regAddrByte = regAddrDict[regName]
writeByte = controlByteDict[writeByteName]
writeDevTwoBytes(i2cPort, devAddrByte, regAddrByte, writeByte)
return

def printDevRegOneByte(i2cPort, devAddrByte, regName, printTitle):
regAddrByte = regAddrDict[regName]
readByte = readDevRegOneByte(i2cPort, devAddrByte, regAddrByte)
print(printTitle, convertByteToFourCharPadString(readByte))

# *** MPU6050 Specific Register Write, Read, Print Functions ***

def operationDelay():
sleep(0.001) # 1mS
return

def readPrintRegByte(i2cCh, mpu6050Addr, regName, printTitle):
regAddrByte = regAddrDict[regName]
print(‘        regName                =’, regName)
print(‘        regAddr                =’, hex(regAddrByte))
readByte = readDevRegOneByte(i2cCh, mpu6050Addr, regAddrByte)
print(‘     ‘, printTitle, ‘ =’, convertByteToFourCharPadString(readByte))
return

def readPrintRegWord(i2cCh, mpu6050Addr, regName, printTitle):
regAddrByte = regAddrDict[regName]
print(‘        regName                =’, regName)
print(‘        regAddr                =’, hex(regAddrByte))
readWord = readDevRegOneWord(i2cCh, mpu6050Addr, regAddrByte)
print(‘     ‘, printTitle, ‘ =’, readWord)
return

def writereadPrintRegByte(i2cCh, mpu6050Addr, regName, writeByteName, printTitle):
regAddrByte = regAddrDict[regName]
writeByte = controlByteDict[writeByteName]
print(‘        regName                =’, regName)
print(‘        regAddr                =’, hex(regAddrByte))
print(‘        writeByte              =’, convertByteToFourCharPadString(writeByte))
writeDevRegOneByte(i2cCh, mpu6050Addr, regName, writeByteName)
operationDelay()
readPrintRegByte(i2cCh, mpu6050Addr, regName, printTitle)
return

# *** MPU6050 System Functions ***

def resetMpu(i2cCh, mpu6050Addr):
print(‘      Begin’, inspect.stack()[0][3])
writeDevRegOneByte(i2cCh, mpu6050Addr, ‘PWR_MGMT_1’, ‘Reset’)
operationDelay()
readPrintRegByte(i2cCh, mpu6050Addr, ‘PWR_MGMT_1’, ‘  powerMgmt1           ‘)
print(‘      End  ‘, inspect.stack()[0][3])
return

def wakeupMpu(i2cCh, mpu6050Addr):
print(‘      Begin’, inspect.stack()[0][3])
writeDevRegOneByte(i2cCh, mpu6050Addr, ‘PWR_MGMT_1’, ‘Wakeup’)
operationDelay()
readPrintRegByte(i2cCh, mpu6050Addr, ‘PWR_MGMT_1’, ‘  powerMgmt1           ‘)
print(‘      End  ‘, inspect.stack()[0][3])
return

# *** MPU6050 Test Functions ***

def readPrintRegWhoAmI(i2cCh, mpu6050Addr):
print(‘      Begin’, inspect.stack()[0][3])
readPrintRegByte(i2cCh, mpu6050Addr, ‘WHO_AM_I’, ‘  readByte             ‘)
print(‘      End  ‘, inspect.stack()[0][3])
return

def writeReadPrintRegGyroConfig(i2cCh, mpu6050Addr):
print(‘      Begin’, inspect.stack()[0][3])
writereadPrintRegByte(i2cCh, mpu6050Addr, ‘GYRO_CONFIG’, ‘GyroConfig01’, ‘  readByte             ‘)
print(‘      End  ‘, inspect.stack()[0][3])
return

def writeReadPrintRegAccelConfig(i2cCh, mpu6050Addr):
print(‘      Begin’, inspect.stack()[0][3])
writereadPrintRegByte(i2cCh, mpu6050Addr, ‘ACCEL_CONFIG’, ‘AccelConfig01’, ‘  readByte             ‘)
print(‘      End  ‘, inspect.stack()[0][3])
return

def readPrintRegGyroOutputByteX(i2cCh, mpu6050Addr):
print(‘      Begin’, inspect.stack()[0][3])
readPrintRegByte(i2cCh, mpu6050Addr, ‘GYRO_YOUT_H’, ‘  readByte             ‘)
print(‘      End  ‘, inspect.stack()[0][3])
return

def readPrintRegGyroOutputWordX(i2cCh, mpu6050Addr):
print(‘      Begin’, inspect.stack()[0][3])
readPrintRegWord(i2cCh, mpu6050Addr, ‘GYRO_YOUT_H’, ‘  readByte             ‘)
print(‘      End  ‘, inspect.stack()[0][3])
return

def testMpu(mpuNum):
print(‘\n*** Begin test **********************************************\n’)
print(‘    Begin’, inspect.stack()[0][3])
printDateTime()
i2cCh = i2cChList[mpuDict[‘Dict’][str(mpuNum)][‘I2cChNum’]]
mpu6050Addr = i2cDvAddrList[mpuDict[‘Dict’][str(mpuNum)][‘I2cDvAddrNum’]]
print(‘\n      *** mpu6050Addr =’, hex(mpu6050Addr), ‘***’)
print()
resetMpu(i2cCh, mpu6050Addr)
wakeupMpu(i2cCh, mpu6050Addr)
readPrintRegWhoAmI(i2cCh, mpu6050Addr)
#writeReadPrintRegGyroConfig(i2cCh, mpu6050Addr)
#writeReadPrintRegAccelConfig(i2cCh, mpu6050Addr)
readPrintRegGyroOutputByteX(i2cCh, mpu6050Addr)
readPrintRegGyroOutputWordX(i2cCh, mpu6050Addr)
print(‘    End  ‘, inspect.stack()[0][3])
print(‘\n*** End   test **********************************************’)
return

# *** Init Function Execution ***
# *** Main Function Execution ***

testMpu(0)
testMpu(1)

# *** End ***

”’ *** Sample output ***

”’