# mpu6050_test15_2019mar1901 tlfong01 2019mar19hkt2209
# 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

# *** Device Address ***

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

# *** Register Addresses ***

whoAmIAddr         = 0x75
powerMgmt1Addr     = 0x6b
gyroConfigAddr     = 0x1b
accelConfigAddr    = 0x1c

# *** Control Bytes ***

resetByte        = 0x80
wakeupByte       = 0x00
gyroConfigByte   = 0xe8 # trigger x, y, z, with full scale 0x01
accelConfigByte  = 0xec # trigger x, y, z, with full scale 0x11

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,
    }

# *** 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

# *** 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 writeDevRegOneByteOld(i2cPort, devAddrByte, regAddrByte, writeByte):
    writeDevTwoBytes(i2cPort, devAddrByte, regAddrByte, writeByte)
    return

def writeDevRegOneByte(i2cPort, devAddrByte, regName, writeByte):
    regAddrByte = regAddrDict[regName]
    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 readPrintRegOld(i2cCh,  mpu6050Addr, regAddrByte, printTitle):
    print('        regAddr                =', hex(regAddrByte))    
    readByte = readDevRegOneByte(i2cCh, mpu6050Addr, regAddrByte)
    print('     ', printTitle, ' =', convertByteToFourCharPadString(readByte))
    return

def readPrintReg(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 writeReadPrintRegOld(i2cCh, mpu6050Addr, regAddrByte, writeByte, printTitle):
    print('        regAddr                =', hex(regAddrByte))
    print('        writeByte              =', convertByteToFourCharPadString(writeByte))
    writeDevRegOneByte(i2cCh, mpu6050Addr, regAddrByte, writeByte)
    operationDelay()
    readPrintReg(i2cCh, mpu6050Addr, regAddrByte, printTitle)
    return

def writeReadPrintReg(i2cCh, mpu6050Addr, regName, writeByte, printTitle):
    regAddrByte = regAddrDict[regName]
    print('        regName                =', regName)          
    print('        regAddr                =', hex(regAddrByte))
    print('        writeByte              =', convertByteToFourCharPadString(writeByte))
    writeDevRegOneByte(i2cCh, mpu6050Addr, regName, writeByte)
    operationDelay()
    readPrintReg(i2cCh, mpu6050Addr, regName, printTitle)
    return

# *** MPU6050 System Functions ***

def resetMpu(i2cCh, mpu6050Addr):
    print('      Begin', inspect.stack()[0][3])  
    writeDevRegOneByte(i2cCh, mpu6050Addr, 'PWR_MGMT_1', resetByte)
    operationDelay()
    readPrintReg(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', wakeupByte)
    operationDelay()
    readPrintReg(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])  
    readPrintReg(i2cCh, mpu6050Addr, 'WHO_AM_I', '  readByte             ')
    print('      End  ', inspect.stack()[0][3])
    return

def writeReadPrintRegGyroConfig(i2cCh, mpu6050Addr):
    print('      Begin', inspect.stack()[0][3])  
    writeReadPrintReg(i2cCh, mpu6050Addr, 'GYRO_CONFIG', gyroConfigByte, '  readByte             ')
    print('      End  ', inspect.stack()[0][3])
    return

def writeReadPrintRegAccelConfig(i2cCh, mpu6050Addr):
    print('      Begin', inspect.stack()[0][3])  
    writeReadPrintReg(i2cCh, mpu6050Addr, 'ACCEL_CONFIG', accelConfigByte, '  readByte             ')
    print('      End  ', inspect.stack()[0][3])
    return

def testMpu(i2cCh, mpu6050Addr):
    print('\n\n      *** mpu6050Addr =', hex(mpu6050Addr), '***')
    print()
    resetMpu(i2cCh, mpu6050Addr)
    wakeupMpu(i2cCh, mpu6050Addr)
    readPrintRegWhoAmI(i2cCh, mpu6050Addr)
    writeReadPrintRegGyroConfig(i2cCh, mpu6050Addr)
    writeReadPrintRegAccelConfig(i2cCh, mpu6050Addr)
    print('\n\n      *** End of test ***')
    return

# *** Init Function Execution ***

printDateTime()
i2cCh0 = smbus.SMBus(1)

# *** Main Function Execution ***

testMpu(i2cCh0, mpu6050Addr0)
testMpu(i2cCh0, mpu6050Addr1)

# *** End ***

'''
>>> 
== RESTART: /home/pi/Python_Programs/test1191/mpu6050_test15_2019mar1701.py ==
      Date Time now = 2019-03-19 22:09


      *** mpu6050Addr = 0x68 ***

      Begin resetMpu
        regName                = PWR_MGMT_1
        regAddr                = 0x6b
        powerMgmt1             = 0x40
      End   resetMpu
      Begin wakeupMpu
        regName                = PWR_MGMT_1
        regAddr                = 0x6b
        powerMgmt1             = 0x00
      End   wakeupMpu
      Begin readPrintRegWhoAmI
        regName                = WHO_AM_I
        regAddr                = 0x75
        readByte               = 0x68
      End   readPrintRegWhoAmI
      Begin writeReadPrintRegGyroConfig
        regName                = GYRO_CONFIG
        regAddr                = 0x1b
        writeByte              = 0xe8
        regName                = GYRO_CONFIG
        regAddr                = 0x1b
        readByte               = 0xe8
      End   writeReadPrintRegGyroConfig
      Begin writeReadPrintRegAccelConfig
        regName                = ACCEL_CONFIG
        regAddr                = 0x1c
        writeByte              = 0xec
        regName                = ACCEL_CONFIG
        regAddr                = 0x1c
        readByte               = 0xec
      End   writeReadPrintRegAccelConfig


      *** End of test ***


      *** mpu6050Addr = 0x69 ***

      Begin resetMpu
        regName                = PWR_MGMT_1
        regAddr                = 0x6b
        powerMgmt1             = 0x40
      End   resetMpu
      Begin wakeupMpu
        regName                = PWR_MGMT_1
        regAddr                = 0x6b
        powerMgmt1             = 0x00
      End   wakeupMpu
      Begin readPrintRegWhoAmI
        regName                = WHO_AM_I
        regAddr                = 0x75
        readByte               = 0x68
      End   readPrintRegWhoAmI
      Begin writeReadPrintRegGyroConfig
        regName                = GYRO_CONFIG
        regAddr                = 0x1b
        writeByte              = 0xe8
        regName                = GYRO_CONFIG
        regAddr                = 0x1b
        readByte               = 0xe8
      End   writeReadPrintRegGyroConfig
      Begin writeReadPrintRegAccelConfig
        regName                = ACCEL_CONFIG
        regAddr                = 0x1c
        writeByte              = 0xec
        regName                = ACCEL_CONFIG
        regAddr                = 0x1c
        readByte               = 0xec
      End   writeReadPrintRegAccelConfig


      *** End of test ***
>>> 
'''