import smbus
import time

bus=smbus.SMBus(1)

class MPU9250():   
    MPU9150_RA_MAG_ADDRESS		=0x0C
    MPU9150_RA_MAG_XOUT_L		=0x03
    MPU9150_RA_MAG_XOUT_H		=0x04
    MPU9150_RA_MAG_YOUT_L		=0x05
    MPU9150_RA_MAG_YOUT_H		=0x06
    MPU9150_RA_MAG_ZOUT_L		=0x07
    MPU9150_RA_MAG_ZOUT_H		=0x08

    MPU9250_ADDRESS_AD0_LOW     =0x68 # address pin low (GND), default for InvenSense evaluation board
    MPU9250_ADDRESS_AD0_HIGH    =0x69 # address pin high (VCC)
    MPU9250_DEFAULT_ADDRESS     = MPU9250_ADDRESS_AD0_LOW

    MPU9250_RA_XG_OFFS_TC       =0x00 #[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
    MPU9250_RA_YG_OFFS_TC       =0x01 #[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
    MPU9250_RA_ZG_OFFS_TC       =0x02 #[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
    MPU9250_RA_X_FINE_GAIN      =0x03 #[7:0] X_FINE_GAIN
    MPU9250_RA_Y_FINE_GAIN      =0x04 #[7:0] Y_FINE_GAIN
    MPU9250_RA_Z_FINE_GAIN      =0x05 #[7:0] Z_FINE_GAIN
    MPU9250_RA_XA_OFFS_H        =0x06 #[15:0] XA_OFFS
    MPU9250_RA_XA_OFFS_L_TC     =0x07
    MPU9250_RA_YA_OFFS_H        =0x08 #[15:0] YA_OFFS
    MPU9250_RA_YA_OFFS_L_TC     =0x09
    MPU9250_RA_ZA_OFFS_H        =0x0A #[15:0] ZA_OFFS
    MPU9250_RA_ZA_OFFS_L_TC     =0x0B
    MPU9250_RA_XG_OFFS_USRH     =0x13 #[15:0] XG_OFFS_USR
    MPU9250_RA_XG_OFFS_USRL     =0x14
    MPU9250_RA_YG_OFFS_USRH     =0x15 #[15:0] YG_OFFS_USR
    MPU9250_RA_YG_OFFS_USRL     =0x16
    MPU9250_RA_ZG_OFFS_USRH     =0x17 #[15:0] ZG_OFFS_USR
    MPU9250_RA_ZG_OFFS_USRL     =0x18
    MPU9250_RA_SMPLRT_DIV       =0x19
    MPU9250_RA_CONFIG           =0x1A
    MPU9250_RA_GYRO_CONFIG      =0x1B
    MPU9250_RA_ACCEL_CONFIG     =0x1C
    MPU9250_RA_FF_THR           =0x1D
    MPU9250_RA_FF_DUR           =0x1E
    MPU9250_RA_MOT_THR          =0x1F
    MPU9250_RA_MOT_DUR          =0x20
    MPU9250_RA_ZRMOT_THR        =0x21
    MPU9250_RA_ZRMOT_DUR        =0x22
    MPU9250_RA_FIFO_EN          =0x23
    MPU9250_RA_I2C_MST_CTRL     =0x24
    MPU9250_RA_I2C_SLV0_ADDR    =0x25
    MPU9250_RA_I2C_SLV0_REG     =0x26
    MPU9250_RA_I2C_SLV0_CTRL    =0x27
    MPU9250_RA_I2C_SLV1_ADDR    =0x28
    MPU9250_RA_I2C_SLV1_REG     =0x29
    MPU9250_RA_I2C_SLV1_CTRL    =0x2A
    MPU9250_RA_I2C_SLV2_ADDR    =0x2B
    MPU9250_RA_I2C_SLV2_REG     =0x2C
    MPU9250_RA_I2C_SLV2_CTRL    =0x2D
    MPU9250_RA_I2C_SLV3_ADDR    =0x2E
    MPU9250_RA_I2C_SLV3_REG     =0x2F
    MPU9250_RA_I2C_SLV3_CTRL    =0x30
    MPU9250_RA_I2C_SLV4_ADDR    =0x31
    MPU9250_RA_I2C_SLV4_REG     =0x32
    MPU9250_RA_I2C_SLV4_DO      =0x33
    MPU9250_RA_I2C_SLV4_CTRL    =0x34
    MPU9250_RA_I2C_SLV4_DI      =0x35
    MPU9250_RA_I2C_MST_STATUS   =0x36
    MPU9250_RA_INT_PIN_CFG      =0x37
    MPU9250_RA_INT_ENABLE       =0x38
    MPU9250_RA_DMP_INT_STATUS   =0x39
    MPU9250_RA_INT_STATUS       =0x3A
    MPU9250_RA_ACCEL_XOUT_H     =0x3B
    MPU9250_RA_ACCEL_XOUT_L     =0x3C
    MPU9250_RA_ACCEL_YOUT_H     =0x3D
    MPU9250_RA_ACCEL_YOUT_L     =0x3E
    MPU9250_RA_ACCEL_ZOUT_H     =0x3F
    MPU9250_RA_ACCEL_ZOUT_L     =0x40
    MPU9250_RA_TEMP_OUT_H       =0x41
    MPU9250_RA_TEMP_OUT_L       =0x42
    MPU9250_RA_GYRO_XOUT_H      =0x43
    MPU9250_RA_GYRO_XOUT_L      =0x44
    MPU9250_RA_GYRO_YOUT_H      =0x45
    MPU9250_RA_GYRO_YOUT_L      =0x46
    MPU9250_RA_GYRO_ZOUT_H      =0x47
    MPU9250_RA_GYRO_ZOUT_L      =0x48
    MPU9250_RA_EXT_SENS_DATA_00 =0x49
    MPU9250_RA_EXT_SENS_DATA_01 =0x4A
    MPU9250_RA_EXT_SENS_DATA_02 =0x4B
    MPU9250_RA_EXT_SENS_DATA_03 =0x4C
    MPU9250_RA_EXT_SENS_DATA_04 =0x4D
    MPU9250_RA_EXT_SENS_DATA_05 =0x4E
    MPU9250_RA_EXT_SENS_DATA_06 =0x4F
    MPU9250_RA_EXT_SENS_DATA_07 =0x50
    MPU9250_RA_EXT_SENS_DATA_08 =0x51
    MPU9250_RA_EXT_SENS_DATA_09 =0x52
    MPU9250_RA_EXT_SENS_DATA_10 =0x53
    MPU9250_RA_EXT_SENS_DATA_11 =0x54
    MPU9250_RA_EXT_SENS_DATA_12 =0x55
    MPU9250_RA_EXT_SENS_DATA_13 =0x56
    MPU9250_RA_EXT_SENS_DATA_14 =0x57
    MPU9250_RA_EXT_SENS_DATA_15 =0x58
    MPU9250_RA_EXT_SENS_DATA_16 =0x59
    MPU9250_RA_EXT_SENS_DATA_17 =0x5A
    MPU9250_RA_EXT_SENS_DATA_18 =0x5B
    MPU9250_RA_EXT_SENS_DATA_19 =0x5C
    MPU9250_RA_EXT_SENS_DATA_20 =0x5D
    MPU9250_RA_EXT_SENS_DATA_21 =0x5E
    MPU9250_RA_EXT_SENS_DATA_22 =0x5F
    MPU9250_RA_EXT_SENS_DATA_23 =0x60
    MPU9250_RA_MOT_DETECT_STATUS    =0x61
    MPU9250_RA_I2C_SLV0_DO      =0x63
    MPU9250_RA_I2C_SLV1_DO      =0x64
    MPU9250_RA_I2C_SLV2_DO      =0x65
    MPU9250_RA_I2C_SLV3_DO      =0x66
    MPU9250_RA_I2C_MST_DELAY_CTRL   =0x67
    MPU9250_RA_SIGNAL_PATH_RESET    =0x68
    MPU9250_RA_MOT_DETECT_CTRL      =0x69
    MPU9250_RA_USER_CTRL        =0x6A
    MPU9250_RA_PWR_MGMT_1       =0x6B
    MPU9250_RA_PWR_MGMT_2       =0x6C
    MPU9250_RA_BANK_SEL         =0x6D
    MPU9250_RA_MEM_START_ADDR   =0x6E
    MPU9250_RA_MEM_R_W          =0x6F
    MPU9250_RA_DMP_CFG_1        =0x70
    MPU9250_RA_DMP_CFG_2        =0x71
    MPU9250_RA_FIFO_COUNTH      =0x72
    MPU9250_RA_FIFO_COUNTL      =0x73
    MPU9250_RA_FIFO_R_W         =0x74
    MPU9250_RA_WHO_AM_I         =0x75

    MPU9250_TC_PWR_MODE_BIT     =7
    MPU9250_TC_OFFSET_BIT       =6
    MPU9250_TC_OFFSET_LENGTH    =6
    MPU9250_TC_OTP_BNK_VLD_BIT  =0

    MPU9250_VDDIO_LEVEL_VLOGIC  =0
    MPU9250_VDDIO_LEVEL_VDD     =1

    MPU9250_CFG_EXT_SYNC_SET_BIT    =5
    MPU9250_CFG_EXT_SYNC_SET_LENGTH =3
    MPU9250_CFG_DLPF_CFG_BIT    =2
    MPU9250_CFG_DLPF_CFG_LENGTH =3

    MPU9250_EXT_SYNC_DISABLED       =0x0
    MPU9250_EXT_SYNC_TEMP_OUT_L     =0x1
    MPU9250_EXT_SYNC_GYRO_XOUT_L    =0x2
    MPU9250_EXT_SYNC_GYRO_YOUT_L    =0x3
    MPU9250_EXT_SYNC_GYRO_ZOUT_L    =0x4
    MPU9250_EXT_SYNC_ACCEL_XOUT_L   =0x5
    MPU9250_EXT_SYNC_ACCEL_YOUT_L   =0x6
    MPU9250_EXT_SYNC_ACCEL_ZOUT_L   =0x7

    MPU9250_DLPF_BW_256         =0x00
    MPU9250_DLPF_BW_188         =0x01
    MPU9250_DLPF_BW_98          =0x02
    MPU9250_DLPF_BW_42          =0x03
    MPU9250_DLPF_BW_20          =0x04
    MPU9250_DLPF_BW_10          =0x05
    MPU9250_DLPF_BW_5           =0x06

    MPU9250_GCONFIG_FS_SEL_BIT      =4
    MPU9250_GCONFIG_FS_SEL_LENGTH   =2

    MPU9250_GYRO_FS_250         =0x00
    MPU9250_GYRO_FS_500         =0x01
    MPU9250_GYRO_FS_1000        =0x02
    MPU9250_GYRO_FS_2000        =0x03

    MPU9250_ACONFIG_XA_ST_BIT          = 7
    MPU9250_ACONFIG_YA_ST_BIT          = 6
    MPU9250_ACONFIG_ZA_ST_BIT          = 5
    MPU9250_ACONFIG_AFS_SEL_BIT         =4
    MPU9250_ACONFIG_AFS_SEL_LENGTH      =2
    MPU9250_ACONFIG_ACCEL_HPF_BIT       =2
    MPU9250_ACONFIG_ACCEL_HPF_LENGTH    =3

    MPU9250_ACCEL_FS_2          =0x00
    MPU9250_ACCEL_FS_4          =0x01
    MPU9250_ACCEL_FS_8          =0x02
    MPU9250_ACCEL_FS_16         =0x03

    MPU9250_DHPF_RESET          =0x00
    MPU9250_DHPF_5              =0x01
    MPU9250_DHPF_2P5            =0x02
    MPU9250_DHPF_1P25           =0x03
    MPU9250_DHPF_0P63           =0x04
    MPU9250_DHPF_HOLD           =0x07

    MPU9250_TEMP_FIFO_EN_BIT    =7
    MPU9250_XG_FIFO_EN_BIT      =6
    MPU9250_YG_FIFO_EN_BIT      =5
    MPU9250_ZG_FIFO_EN_BIT      =4
    MPU9250_ACCEL_FIFO_EN_BIT   =3
    MPU9250_SLV2_FIFO_EN_BIT    =2
    MPU9250_SLV1_FIFO_EN_BIT    =1
    MPU9250_SLV0_FIFO_EN_BIT    =0

    MPU9250_MULT_MST_EN_BIT     =7
    MPU9250_WAIT_FOR_ES_BIT     =6
    MPU9250_SLV_3_FIFO_EN_BIT   =5
    MPU9250_I2C_MST_P_NSR_BIT   =4
    MPU9250_I2C_MST_CLK_BIT     =3
    MPU9250_I2C_MST_CLK_LENGTH  =4

    MPU9250_CLOCK_DIV_348       =0x0
    MPU9250_CLOCK_DIV_333       =0x1
    MPU9250_CLOCK_DIV_320       =0x2
    MPU9250_CLOCK_DIV_308       =0x3
    MPU9250_CLOCK_DIV_296       =0x4
    MPU9250_CLOCK_DIV_286       =0x5
    MPU9250_CLOCK_DIV_276       =0x6
    MPU9250_CLOCK_DIV_267       =0x7
    MPU9250_CLOCK_DIV_258       =0x8
    MPU9250_CLOCK_DIV_500       =0x9
    MPU9250_CLOCK_DIV_471       =0xA
    MPU9250_CLOCK_DIV_444       =0xB
    MPU9250_CLOCK_DIV_421       =0xC
    MPU9250_CLOCK_DIV_400       =0xD
    MPU9250_CLOCK_DIV_381       =0xE
    MPU9250_CLOCK_DIV_364       =0xF

    MPU9250_I2C_SLV_RW_BIT      =7
    MPU9250_I2C_SLV_ADDR_BIT    =6
    MPU9250_I2C_SLV_ADDR_LENGTH =7
    MPU9250_I2C_SLV_EN_BIT      =7
    MPU9250_I2C_SLV_BYTE_SW_BIT =6
    MPU9250_I2C_SLV_REG_DIS_BIT =5
    MPU9250_I2C_SLV_GRP_BIT     =4
    MPU9250_I2C_SLV_LEN_BIT     =3
    MPU9250_I2C_SLV_LEN_LENGTH  =4

    MPU9250_I2C_SLV4_RW_BIT        = 7
    MPU9250_I2C_SLV4_ADDR_BIT      = 6
    MPU9250_I2C_SLV4_ADDR_LENGTH   = 7
    MPU9250_I2C_SLV4_EN_BIT        = 7
    MPU9250_I2C_SLV4_INT_EN_BIT    = 6
    MPU9250_I2C_SLV4_REG_DIS_BIT   = 5
    MPU9250_I2C_SLV4_MST_DLY_BIT   = 4
    MPU9250_I2C_SLV4_MST_DLY_LENGTH =5

    MPU9250_MST_PASS_THROUGH_BIT    =7
    MPU9250_MST_I2C_SLV4_DONE_BIT   =6
    MPU9250_MST_I2C_LOST_ARB_BIT   = 5
    MPU9250_MST_I2C_SLV4_NACK_BIT  = 4
    MPU9250_MST_I2C_SLV3_NACK_BIT   =3
    MPU9250_MST_I2C_SLV2_NACK_BIT   =2
    MPU9250_MST_I2C_SLV1_NACK_BIT   =1
    MPU9250_MST_I2C_SLV0_NACK_BIT   =0

    MPU9250_INTCFG_INT_LEVEL_BIT        =7
    MPU9250_INTCFG_INT_OPEN_BIT         =6
    MPU9250_INTCFG_LATCH_INT_EN_BIT     =5
    MPU9250_INTCFG_INT_RD_CLEAR_BIT     =4
    MPU9250_INTCFG_FSYNC_INT_LEVEL_BIT  =3
    MPU9250_INTCFG_FSYNC_INT_EN_BIT     =2
    MPU9250_INTCFG_I2C_BYPASS_EN_BIT    =1
    MPU9250_INTCFG_CLKOUT_EN_BIT        =0

    MPU9250_INTMODE_ACTIVEHIGH  =0x00
    MPU9250_INTMODE_ACTIVELOW   =0x01

    MPU9250_INTDRV_PUSHPULL     =0x00
    MPU9250_INTDRV_OPENDRAIN    =0x01

    MPU9250_INTLATCH_50USPULSE  =0x00
    MPU9250_INTLATCH_WAITCLEAR  =0x01

    MPU9250_INTCLEAR_STATUSREAD =0x00
    MPU9250_INTCLEAR_ANYREAD    =0x01

    MPU9250_INTERRUPT_FF_BIT           = 7
    MPU9250_INTERRUPT_MOT_BIT           =6
    MPU9250_INTERRUPT_ZMOT_BIT          =5
    MPU9250_INTERRUPT_FIFO_OFLOW_BIT    =4
    MPU9250_INTERRUPT_I2C_MST_INT_BIT   =3
    MPU9250_INTERRUPT_PLL_RDY_INT_BIT   =2
    MPU9250_INTERRUPT_DMP_INT_BIT       =1
    MPU9250_INTERRUPT_DATA_RDY_BIT      =0

    # TODO: figure out what these actually do
    # UMPL source code is not very obivous
    MPU9250_DMPINT_5_BIT           = 5
    MPU9250_DMPINT_4_BIT           = 4
    MPU9250_DMPINT_3_BIT           = 3
    MPU9250_DMPINT_2_BIT           = 2
    MPU9250_DMPINT_1_BIT           = 1
    MPU9250_DMPINT_0_BIT           = 0

    MPU9250_MOTION_MOT_XNEG_BIT    = 7
    MPU9250_MOTION_MOT_XPOS_BIT    = 6
    MPU9250_MOTION_MOT_YNEG_BIT    = 5
    MPU9250_MOTION_MOT_YPOS_BIT    = 4
    MPU9250_MOTION_MOT_ZNEG_BIT    = 3
    MPU9250_MOTION_MOT_ZPOS_BIT    = 2
    MPU9250_MOTION_MOT_ZRMOT_BIT   = 0

    MPU9250_DELAYCTRL_DELAY_ES_SHADOW_BIT   =7
    MPU9250_DELAYCTRL_I2C_SLV4_DLY_EN_BIT   =4
    MPU9250_DELAYCTRL_I2C_SLV3_DLY_EN_BIT   =3
    MPU9250_DELAYCTRL_I2C_SLV2_DLY_EN_BIT   =2
    MPU9250_DELAYCTRL_I2C_SLV1_DLY_EN_BIT   =1
    MPU9250_DELAYCTRL_I2C_SLV0_DLY_EN_BIT   =0

    MPU9250_PATHRESET_GYRO_RESET_BIT   = 2
    MPU9250_PATHRESET_ACCEL_RESET_BIT  = 1
    MPU9250_PATHRESET_TEMP_RESET_BIT  =  0

    MPU9250_DETECT_ACCEL_ON_DELAY_BIT     =  5
    MPU9250_DETECT_ACCEL_ON_DELAY_LENGTH  =  2
    MPU9250_DETECT_FF_COUNT_BIT           =  3
    MPU9250_DETECT_FF_COUNT_LENGTH        =  2
    MPU9250_DETECT_MOT_COUNT_BIT          =  1
    MPU9250_DETECT_MOT_COUNT_LENGTH       =  2

    MPU9250_DETECT_DECREMENT_RESET = 0x0
    MPU9250_DETECT_DECREMENT_1     = 0x1
    MPU9250_DETECT_DECREMENT_2     = 0x2
    MPU9250_DETECT_DECREMENT_4     = 0x3

    MPU9250_USERCTRL_DMP_EN_BIT            = 7
    MPU9250_USERCTRL_FIFO_EN_BIT           = 6
    MPU9250_USERCTRL_I2C_MST_EN_BIT        = 5
    MPU9250_USERCTRL_I2C_IF_DIS_BIT        = 4
    MPU9250_USERCTRL_DMP_RESET_BIT        =  3
    MPU9250_USERCTRL_FIFO_RESET_BIT       =  2
    MPU9250_USERCTRL_I2C_MST_RESET_BIT    =  1
    MPU9250_USERCTRL_SIG_COND_RESET_BIT   =  0

    MPU9250_PWR1_DEVICE_RESET_BIT  = 7
    MPU9250_PWR1_SLEEP_BIT         = 6
    MPU9250_PWR1_CYCLE_BIT         = 5
    MPU9250_PWR1_TEMP_DIS_BIT      = 3
    MPU9250_PWR1_CLKSEL_BIT        = 2
    MPU9250_PWR1_CLKSEL_LENGTH     = 3

    MPU9250_CLOCK_INTERNAL        =  0x00
    MPU9250_CLOCK_PLL_XGYRO       =  0x01
    MPU9250_CLOCK_PLL_YGYRO       =  0x02
    MPU9250_CLOCK_PLL_ZGYRO       =  0x03
    MPU9250_CLOCK_PLL_EXT32K      =  0x04
    MPU9250_CLOCK_PLL_EXT19M      =  0x05
    MPU9250_CLOCK_KEEP_RESET      =  0x07

    MPU9250_PWR2_LP_WAKE_CTRL_BIT      = 7
    MPU9250_PWR2_LP_WAKE_CTRL_LENGTH   = 2
    MPU9250_PWR2_STBY_XA_BIT           = 5
    MPU9250_PWR2_STBY_YA_BIT           = 4
    MPU9250_PWR2_STBY_ZA_BIT           = 3
    MPU9250_PWR2_STBY_XG_BIT           = 2
    MPU9250_PWR2_STBY_YG_BIT           = 1
    MPU9250_PWR2_STBY_ZG_BIT           = 0

    MPU9250_WAKE_FREQ_1P25    =  0x0
    MPU9250_WAKE_FREQ_2P5     =  0x1
    MPU9250_WAKE_FREQ_5       =  0x2
    MPU9250_WAKE_FREQ_10      =  0x3

    MPU9250_BANKSEL_PRFTCH_EN_BIT     =  6
    MPU9250_BANKSEL_CFG_USER_BANK_BIT =  5
    MPU9250_BANKSEL_MEM_SEL_BIT       =  4
    MPU9250_BANKSEL_MEM_SEL_LENGTH    =  5

    MPU9250_WHO_AM_I_BIT      =  6
    MPU9250_WHO_AM_I_LENGTH   =  8

    MPU9250_DMP_MEMORY_BANKS       = 8
    MPU9250_DMP_MEMORY_BANK_SIZE   = 256
    MPU9250_DMP_MEMORY_CHUNK_SIZE  = 16
    Sample_Num_Mdate = 5000
    
    MPU9250_CLOCK_PLL_XGYRO = 0x01
    def __init__(self):
        self.setClockSource(self.MPU9250_CLOCK_PLL_XGYRO)
        self.setFullScaleGyroRange(self.MPU9250_GYRO_FS_250)
        self.setFullScaleAccelRange(self.MPU9250_ACCEL_FS_2)
        self.setSleepEnabled(False)
        
    def setClockSource(self,source):
        return self.writeBits(devAddr,self.MPU9250_RA_PWR_MGMT_1, self.MPU9250_PWR1_CLKSEL_BIT,self.MPU9250_PWR1_CLKSEL_LENGTH,source)
        
    def setFullScaleGyroRange(self,range):
        pass
        
    def setFullScaleAccelRange(self,range):
        pass
        
    def setSleepEnabled(self,enabled):
        pass
                
    def getMotion9(self):
        ax, ay, az, gx, gy, gz = self.getMotion6()
        self.writeByte()
        pass
        
    def getMotion6(self):
        pass
        
#I2Cdev
    def writeByte(self,):
        return self.writeBytes(devAddr, regAddr, 1, &data)
        
    def writeBytes(devAddr, regAddr, length, data)£º
        pass
        
    def writeBits(self, devAddr, regAddr, bitStart, length, data):                
        if self.readByte(devAddr, regAddr) != 0:
            mask = ((1<<length)-1) << (bitStart - length + 1)
            data <<= (bitStart - length + 1)
            data &= mask
            b &= ~(mask)
            b |= data
            return self.writeByte(devAddr, regAddr, b)
        else:
            return False
            
    def readByte(self, devAddr, regAddr, data, timeout):
        readBytes(devAddr, regAddr, 1, data, timeout)
        
    def readBytes(self, devAddr, regAddr, length, data, timeout):
        count = 0
        BUFFER_LENGTH = 32        
        k = 0
        t1 = 1000*time.time()        
        while k < length£º
            bus.write_byte(devAddr, regAddr)
            bus.
            
            if length <= BUFFER_LENGTH:
                k = k + length
            else:
                k = k + BUFFER_LENGTH
            
        
        
if __name__=="__main__":
    mpu = MPU9250()    
        
        
        