"""
MPU9250 driver code is placed under the BSD license.
Copyright (c) 2014, Emlid Limited, www.emlid.com
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
    notice, this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.
    * Neither the name of the Emlid Limited nor the names of its contributors
    may be used to endorse or promote products derived from this software
    without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""

import spidev
import time
import struct
import array

class MPU9250:

    # MPU9250 registers
    __MPUREG_XG_OFFS_TC = 0x00
    __MPUREG_YG_OFFS_TC = 0x01
    __MPUREG_ZG_OFFS_TC = 0x02
    __MPUREG_X_FINE_GAIN = 0x03
    __MPUREG_Y_FINE_GAIN = 0x04
    __MPUREG_Z_FINE_GAIN = 0x05
    __MPUREG_XA_OFFS_H = 0x06
    __MPUREG_XA_OFFS_L = 0x07
    __MPUREG_YA_OFFS_H = 0x08
    __MPUREG_YA_OFFS_L = 0x09
    __MPUREG_ZA_OFFS_H = 0x0A
    __MPUREG_ZA_OFFS_L = 0x0B
    __MPUREG_PRODUCT_ID = 0x0C
    __MPUREG_SELF_TEST_X = 0x0D
    __MPUREG_SELF_TEST_Y = 0x0E
    __MPUREG_SELF_TEST_Z = 0x0F
    __MPUREG_SELF_TEST_A = 0x10
    __MPUREG_XG_OFFS_USRH = 0x13
    __MPUREG_XG_OFFS_USRL = 0x14
    __MPUREG_YG_OFFS_USRH = 0x15
    __MPUREG_YG_OFFS_USRL = 0x16
    __MPUREG_ZG_OFFS_USRH = 0x17
    __MPUREG_ZG_OFFS_USRL = 0x18
    __MPUREG_SMPLRT_DIV = 0x19
    __MPUREG_CONFIG = 0x1A
    __MPUREG_GYRO_CONFIG = 0x1B
    __MPUREG_ACCEL_CONFIG = 0x1C
    __MPUREG_ACCEL_CONFIG_2      = 0x1D
    __MPUREG_LP_ACCEL_ODR        = 0x1E
    __MPUREG_MOT_THR             = 0x1F
    __MPUREG_FIFO_EN             = 0x23
    __MPUREG_I2C_MST_CTRL        = 0x24
    __MPUREG_I2C_SLV0_ADDR       = 0x25
    __MPUREG_I2C_SLV0_REG        = 0x26
    __MPUREG_I2C_SLV0_CTRL       = 0x27
    __MPUREG_I2C_SLV1_ADDR       = 0x28
    __MPUREG_I2C_SLV1_REG        = 0x29
    __MPUREG_I2C_SLV1_CTRL       = 0x2A
    __MPUREG_I2C_SLV2_ADDR       = 0x2B
    __MPUREG_I2C_SLV2_REG        = 0x2C
    __MPUREG_I2C_SLV2_CTRL       = 0x2D
    __MPUREG_I2C_SLV3_ADDR       = 0x2E
    __MPUREG_I2C_SLV3_REG        = 0x2F
    __MPUREG_I2C_SLV3_CTRL       = 0x30
    __MPUREG_I2C_SLV4_ADDR       = 0x31
    __MPUREG_I2C_SLV4_REG        = 0x32
    __MPUREG_I2C_SLV4_DO         = 0x33
    __MPUREG_I2C_SLV4_CTRL       = 0x34
    __MPUREG_I2C_SLV4_DI         = 0x35
    __MPUREG_I2C_MST_STATUS      = 0x36
    __MPUREG_INT_PIN_CFG = 0x37
    __MPUREG_INT_ENABLE = 0x38
    __MPUREG_ACCEL_XOUT_H = 0x3B
    __MPUREG_ACCEL_XOUT_L = 0x3C
    __MPUREG_ACCEL_YOUT_H = 0x3D
    __MPUREG_ACCEL_YOUT_L = 0x3E
    __MPUREG_ACCEL_ZOUT_H = 0x3F
    __MPUREG_ACCEL_ZOUT_L = 0x40
    __MPUREG_TEMP_OUT_H = 0x41
    __MPUREG_TEMP_OUT_L = 0x42
    __MPUREG_GYRO_XOUT_H = 0x43
    __MPUREG_GYRO_XOUT_L = 0x44
    __MPUREG_GYRO_YOUT_H = 0x45
    __MPUREG_GYRO_YOUT_L = 0x46
    __MPUREG_GYRO_ZOUT_H = 0x47
    __MPUREG_GYRO_ZOUT_L = 0x48
    __MPUREG_EXT_SENS_DATA_00    = 0x49
    __MPUREG_EXT_SENS_DATA_01    = 0x4A
    __MPUREG_EXT_SENS_DATA_02    = 0x4B
    __MPUREG_EXT_SENS_DATA_03    = 0x4C
    __MPUREG_EXT_SENS_DATA_04    = 0x4D
    __MPUREG_EXT_SENS_DATA_05    = 0x4E
    __MPUREG_EXT_SENS_DATA_06    = 0x4F
    __MPUREG_EXT_SENS_DATA_07    = 0x50
    __MPUREG_EXT_SENS_DATA_08    = 0x51
    __MPUREG_EXT_SENS_DATA_09    = 0x52
    __MPUREG_EXT_SENS_DATA_10    = 0x53
    __MPUREG_EXT_SENS_DATA_11    = 0x54
    __MPUREG_EXT_SENS_DATA_12    = 0x55
    __MPUREG_EXT_SENS_DATA_13    = 0x56
    __MPUREG_EXT_SENS_DATA_14    = 0x57
    __MPUREG_EXT_SENS_DATA_15    = 0x58
    __MPUREG_EXT_SENS_DATA_16    = 0x59
    __MPUREG_EXT_SENS_DATA_17    = 0x5A
    __MPUREG_EXT_SENS_DATA_18    = 0x5B
    __MPUREG_EXT_SENS_DATA_19    = 0x5C
    __MPUREG_EXT_SENS_DATA_20    = 0x5D
    __MPUREG_EXT_SENS_DATA_21    = 0x5E
    __MPUREG_EXT_SENS_DATA_22    = 0x5F
    __MPUREG_EXT_SENS_DATA_23    = 0x60
    __MPUREG_I2C_SLV0_DO         = 0x63
    __MPUREG_I2C_SLV1_DO         = 0x64
    __MPUREG_I2C_SLV2_DO         = 0x65
    __MPUREG_I2C_SLV3_DO         = 0x66
    __MPUREG_I2C_MST_DELAY_CTRL  = 0x67
    __MPUREG_SIGNAL_PATH_RESET   = 0x68
    __MPUREG_MOT_DETECT_CTRL     = 0x69
    __MPUREG_USER_CTRL = 0x6A
    __MPUREG_PWR_MGMT_1 = 0x6B
    __MPUREG_PWR_MGMT_2 = 0x6C
    __MPUREG_BANK_SEL = 0x6D
    __MPUREG_MEM_START_ADDR = 0x6E
    __MPUREG_MEM_R_W = 0x6F
    __MPUREG_DMP_CFG_1 = 0x70
    __MPUREG_DMP_CFG_2 = 0x71
    __MPUREG_FIFO_COUNTH = 0x72
    __MPUREG_FIFO_COUNTL = 0x73
    __MPUREG_FIFO_R_W = 0x74
    __MPUREG_WHOAMI = 0x75
    __MPUREG_XA_OFFSET_H         = 0x77
    __MPUREG_XA_OFFSET_L         = 0x78
    __MPUREG_YA_OFFSET_H         = 0x7A
    __MPUREG_YA_OFFSET_L         = 0x7B
    __MPUREG_ZA_OFFSET_H         = 0x7D
    __MPUREG_ZA_OFFSET_L         = 0x7E

    # ---- AK8963 Reg In MPU9250 -----------------------------------------------

    __AK8963_I2C_ADDR             = 0x0c #0x18
    __AK8963_Device_ID            = 0x48

    # Read-only Reg
    __AK8963_WIA                  = 0x00
    __AK8963_INFO                 = 0x01
    __AK8963_ST1                  = 0x02
    __AK8963_HXL                  = 0x03
    __AK8963_HXH                  = 0x04
    __AK8963_HYL                  = 0x05
    __AK8963_HYH                  = 0x06
    __AK8963_HZL                  = 0x07
    __AK8963_HZH                  = 0x08
    __AK8963_ST2                  = 0x09

    # Write/Read Reg
    __AK8963_CNTL1                = 0x0A
    __AK8963_CNTL2                = 0x0B
    __AK8963_ASTC                 = 0x0C
    __AK8963_TS1                  = 0x0D
    __AK8963_TS2                  = 0x0E
    __AK8963_I2CDIS               = 0x0F

    # Read-only Reg ( ROM )
    __AK8963_ASAX                 = 0x10
    __AK8963_ASAY                 = 0x11
    __AK8963_ASAZ                 = 0x12

    # Configuration bits MPU9250
    __BIT_SLEEP = 0x40
    __BIT_H_RESET = 0x80
    __BITS_CLKSEL = 0x07
    __MPU_CLK_SEL_PLLGYROX = 0x01
    __MPU_CLK_SEL_PLLGYROZ = 0x03
    __MPU_EXT_SYNC_GYROX = 0x02
    __BITS_FS_250DPS              = 0x00
    __BITS_FS_500DPS              = 0x08
    __BITS_FS_1000DPS             = 0x10
    __BITS_FS_2000DPS             = 0x18
    __BITS_FS_2G                  = 0x00
    __BITS_FS_4G                  = 0x08
    __BITS_FS_8G                  = 0x10
    __BITS_FS_16G                 = 0x18
    __BITS_FS_MASK                = 0x18
    __BITS_DLPF_CFG_256HZ_NOLPF2  = 0x00
    __BITS_DLPF_CFG_188HZ         = 0x01
    __BITS_DLPF_CFG_98HZ          = 0x02
    __BITS_DLPF_CFG_42HZ          = 0x03
    __BITS_DLPF_CFG_20HZ          = 0x04
    __BITS_DLPF_CFG_10HZ          = 0x05
    __BITS_DLPF_CFG_5HZ           = 0x06
    __BITS_DLPF_CFG_2100HZ_NOLPF  = 0x07
    __BITS_DLPF_CFG_MASK          = 0x07
    __BIT_INT_ANYRD_2CLEAR        = 0x10
    __BIT_RAW_RDY_EN              = 0x01
    __BIT_I2C_IF_DIS              = 0x10

    __READ_FLAG   = 0x80

    # ---- Sensitivity ---------------------------------------------------------

    __MPU9250A_2g       = (0.000061035156) # 0.000061035156 g/LSB
    __MPU9250A_4g       = (0.000122070312) # 0.000122070312 g/LSB
    __MPU9250A_8g       = (0.000244140625) # 0.000244140625 g/LSB
    __MPU9250A_16g      = (0.000488281250) # 0.000488281250 g/LSB

    __MPU9250G_250dps   = (0.007633587786) # 0.007633587786 dps/LSB
    __MPU9250G_500dps   = (0.015267175572) # 0.015267175572 dps/LSB
    __MPU9250G_1000dps  = (0.030487804878) # 0.030487804878 dps/LSB
    __MPU9250G_2000dps  = (0.060975609756) # 0.060975609756 dps/LSB

    __MPU9250M_4800uT   = (0.6)            # 0.6 uT/LSB

    __MPU9250T_85degC   = (0.002995177763) # 0.002995177763 degC/LSB

    __Magnetometer_Sensitivity_Scale_Factor = (0.15)

    def __init__(self, spi_bus_number = 0, spi_dev_number = 1):
        self.bus = spidev.SpiDev()
        self.spi_bus_number = spi_bus_number
        self.spi_dev_number = spi_dev_number
        self.gyro_divider = 0.0
        self.acc_divider = 0.0
        self.calib_data = [0.0, 0.0, 0.0]
        self.magnetometer_ASA = [0.0, 0.0, 0.0]
        self.temperature = 0.0
        self.gyroscope_data = [0.0, 0.0, 0.0]
        self.accelerometer_data = [0.0, 0.0, 0.0]
        self.magnetometer_data = [0.0, 0.0, 0.0]

# -----------------------------------------------------------------------------------------------
#                                     REGISTER READ & WRITE
# usage: use these methods to read and write MPU9250 registers over SPI
# -----------------------------------------------------------------------------------------------

    def WriteReg(self, reg_address, data):
        self.bus.open(self.spi_bus_number, self.spi_dev_number)
        tx = [reg_address, data]
        rx = self.bus.xfer2(tx)
        self.bus.close()
        return rx

# -----------------------------------------------------------------------------------------------

    def ReadReg(self, reg_address):
        self.bus.open(self.spi_bus_number, self.spi_dev_number)
        tx = [reg_address | self.__READ_FLAG, 0x00]
        rx = self.bus.xfer2(tx)
        self.bus.close()
        return rx[1]

# -----------------------------------------------------------------------------------------------

    def ReadRegs(self, reg_address, length):
        self.bus.open(self.spi_bus_number, self.spi_dev_number)
        tx = [0] * (length + 1)
        tx[0] = reg_address | self.__READ_FLAG

        rx = self.bus.xfer2(tx)

        self.bus.close()
        return rx[1:len(rx)]

# -----------------------------------------------------------------------------------------------
#                                 TEST CONNECTION
# usage: call this function to know if SPI and MPU9250 are working correctly.
# returns true if mpu9250 answers
# -----------------------------------------------------------------------------------------------

    def testConnection(self):
        response = self.ReadReg(self.__MPUREG_WHOAMI)
        if (response == 0x71):
            return True
        else:
            return False

# -----------------------------------------------------------------------------------------------
#                                     INITIALIZATION
# usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
# low pass filter value; suitable values are:
# BITS_DLPF_CFG_256HZ_NOLPF2
# BITS_DLPF_CFG_188HZ
# BITS_DLPF_CFG_98HZ
# BITS_DLPF_CFG_42HZ
# BITS_DLPF_CFG_20HZ
# BITS_DLPF_CFG_10HZ
# BITS_DLPF_CFG_5HZ
# BITS_DLPF_CFG_2100HZ_NOLPF
# returns 1 if an error occurred
# -----------------------------------------------------------------------------------------------

    def initialize(self, sample_rate_div = 1, low_pass_filter = 0x01):
        MPU_InitRegNum = 17
        MPU_Init_Data = [[0, 0]] * MPU_InitRegNum

        MPU_Init_Data = [
        [0x80, self.__MPUREG_PWR_MGMT_1],   # Reset Device
        [0x01, self.__MPUREG_PWR_MGMT_1],          # Clock Source
        [0x00, self.__MPUREG_PWR_MGMT_2],          # Enable Acc & Gyro
        [low_pass_filter, self.__MPUREG_CONFIG],   # Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz
        [0x18, self.__MPUREG_GYRO_CONFIG],         # +-2000dps
        [0x08, self.__MPUREG_ACCEL_CONFIG],        # +-4G
        [0x09, self.__MPUREG_ACCEL_CONFIG_2],      # Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz
        [0x30, self.__MPUREG_INT_PIN_CFG],
        #[0x40, self.__MPUREG_I2C_MST_CTRL],       # I2C Speed 348 kHz
        #[0x20, self.__MPUREG_USER_CTRL],          # Enable AUX
        [0x20, self.__MPUREG_USER_CTRL],           # I2C Master mode
        [0x0D, self.__MPUREG_I2C_MST_CTRL],        #  I2C configuration multi-master  IIC 400KHz

        [self.__AK8963_I2C_ADDR, self.__MPUREG_I2C_SLV0_ADDR],  #Set the I2C slave addres of AK8963 and set for write.
        #[0x09, self.__MPUREG_I2C_SLV4_CTRL],
        #[0x81, self.__MPUREG_I2C_MST_DELAY_CTRL], #Enable I2C delay

        [self.__AK8963_CNTL2, self.__MPUREG_I2C_SLV0_REG], #I2C slave 0 register address from where to begin data transfer
        [0x01, self.__MPUREG_I2C_SLV0_DO], # Reset AK8963
        [0x81, self.__MPUREG_I2C_SLV0_CTRL],  #Enable I2C and set 1 byte

        [self.__AK8963_CNTL1, self.__MPUREG_I2C_SLV0_REG], #I2C slave 0 register address from where to begin data transfer
        [0x12, self.__MPUREG_I2C_SLV0_DO], # Register value to continuous measurement in 16bit
        [0x81, self.__MPUREG_I2C_SLV0_CTRL]  #Enable I2C and set 1 byte
        ]

        for i in range(0, MPU_InitRegNum):
            self.WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0])
            time.sleep(0.01) # I2C must slow down the write speed, otherwise it won't work

        self.set_acc_scale(self.__BITS_FS_16G)
        self.set_gyro_scale(self.__BITS_FS_2000DPS)

        self.calib_mag()

# -----------------------------------------------------------------------------------------------
#                                 ACCELEROMETER SCALE
# usage: call this function at startup, after initialization, to set the right range for the
# accelerometers. Suitable ranges are:
# BITS_FS_2G
# BITS_FS_4G
# BITS_FS_8G
# BITS_FS_16G
# returns the range set (2,4,8 or 16)
# -----------------------------------------------------------------------------------------------

    def set_acc_scale(self, scale):
        self.WriteReg(self.__MPUREG_ACCEL_CONFIG, scale)
        if (scale == self.__BITS_FS_2G):
            self.acc_divider = 16384.0
        elif (scale == self.__BITS_FS_4G):
            self.acc_divider = 8192.0
        elif (scale == self.__BITS_FS_8G):
            self.acc_divider = 4096.0
        elif (scale == self.__BITS_FS_16G):
            self.acc_divider = 2048.0

        temp_scale = self.ReadReg(self.__MPUREG_ACCEL_CONFIG)
        if (temp_scale == self.__BITS_FS_2G):
            temp_scale = 2
        elif (temp_scale == self.__BITS_FS_4G):
            temp_scale = 4
        elif (temp_scale == self.__BITS_FS_8G):
            temp_scale = 8
        elif (temp_scale == self.__BITS_FS_16G):
            temp_scale = 16

        return temp_scale

# -----------------------------------------------------------------------------------------------
#                                 GYROSCOPE SCALE
# usage: call this function at startup, after initialization, to set the right range for the
# gyroscopes. Suitable ranges are:
# BITS_FS_250DPS
# BITS_FS_500DPS
# BITS_FS_1000DPS
# BITS_FS_2000DPS
# returns the range set (250,500,1000 or 2000)
# -----------------------------------------------------------------------------------------------

    def set_gyro_scale(self, scale):
        self.WriteReg(self.__MPUREG_GYRO_CONFIG, scale)
        if (scale == self.__BITS_FS_250DPS):
            self.gyro_divider = 131.0
        elif (scale == self.__BITS_FS_500DPS):
            self.gyro_divider = 65.6
        elif (scale == self.__BITS_FS_1000DPS):
            self.gyro_divider = 32.8
        elif (scale == self.__BITS_FS_2000DPS):
            self.gyro_divider = 16.4

        temp_scale = self.ReadReg(self.__MPUREG_GYRO_CONFIG)
        if (temp_scale == self.__BITS_FS_250DPS):
            temp_scale = 250
        elif (temp_scale == self.__BITS_FS_500DPS):
            temp_scale = 500
        elif (temp_scale == self.__BITS_FS_1000DPS):
            temp_scale = 1000
        elif (temp_scale == self.__BITS_FS_2000DPS):
            temp_scale = 2000

        return temp_scale

# -----------------------------------------------------------------------------------------------
#                                 WHO AM I?
# usage: call this function to know if SPI is working correctly. It checks the I2C address of the
# mpu9250 which should be 104 when in SPI mode.
# returns the I2C address (104)
# -----------------------------------------------------------------------------------------------

    def whoami(self):
        return self.ReadReg(self.__MPUREG_WHOAMI)

# -----------------------------------------------------------------------------------------------
#                                 READ ACCELEROMETER
# usage: call this function to read accelerometer data. Axis represents selected axis:
# 0 -> X axis
# 1 -> Y axis
# 2 -> Z axis
# -----------------------------------------------------------------------------------------------

    def read_acc(self):
        response = self.ReadRegs(self.__MPUREG_ACCEL_XOUT_H, 6)

        for i in range(0, 3):
            data = self.byte_to_float(response[i*2:i*2+2])
            self.accelerometer_data[i] = data/self.acc_divider

# -----------------------------------------------------------------------------------------------
#                                 READ GYROSCOPE
# usage: call this function to read gyroscope data. Axis represents selected axis:
# 0 -> X axis
# 1 -> Y axis
# 2 -> Z axis
# -----------------------------------------------------------------------------------------------

    def read_gyro(self):
        response = self.ReadRegs(self.__MPUREG_GYRO_XOUT_H, 6)

        for i in range(0, 3):
            data = self.byte_to_float(response[i*2:i*2+2])
            self.gyroscope_data[i] = data/self.gyro_divider

# -----------------------------------------------------------------------------------------------
#                                 READ TEMPERATURE
# usage: call this function to read temperature data.
# returns the value in Celsius
# -----------------------------------------------------------------------------------------------

    def read_temp(self):
        response = self.ReadRegs(self.__MPUREG_TEMP_OUT_H, 2)

        #temp = response[0]*256.0 + response[1]
        temp = self.byte_to_float(response)
        self.temperature = (temp/340.0)+36.53

# -----------------------------------------------------------------------------------------------
#                                 READ ACCELEROMETER CALIBRATION
# usage: call this function to read accelerometer data. Axis represents selected axis:
# 0 -> X axis
# 1 -> Y axis
# 2 -> Z axis
# returns Factory Trim value
# -----------------------------------------------------------------------------------------------

    def calib_acc(self):
        temp_scale = self.ReadReg(self.__MPUREG_ACCEL_CONFIG)
        self.set_acc_scale(self.__BITS_FS_8G)

        response = self.ReadRegs(self.__MPUREG_SELF_TEST_X, 4)

        self.calib_data[0] = ((response[0] & 11100000) >> 3) | ((response[3] & 00110000) >> 4)
        self.calib_data[1] = ((response[1] & 11100000) >> 3) | ((response[3] & 00001100) >> 2)
        self.calib_data[2] = ((response[2] & 11100000) >> 3) | ((response[3] & 00000011))

        self.set_acc_scale(temp_scale)

# -----------------------------------------------------------------------------------------------

    def AK8963_whoami(self):
        self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG) #Set the I2C slave addres of AK8963 and set for read.
        self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_WIA) #I2C slave 0 register address from where to begin data transfer
        self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x81) #Read 1 byte from the magnetometer

        #self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x81) # Enable I2C and set bytes
        time.sleep(0.01)

        return self.ReadReg(self.__MPUREG_EXT_SENS_DATA_00) # Read I2C

# -----------------------------------------------------------------------------------------------

    def calib_mag(self):
        # Set the I2C slave addres of AK8963 and set for read.
        self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG)
        # I2C slave 0 register address from where to begin data transfer
        self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_ASAX)
        # Read 3 bytes from the magnetometer
        self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x83)

        time.sleep(0.01)

        response = self.ReadRegs(self.__MPUREG_EXT_SENS_DATA_00, 3)

        for i in range(0, 3):
            self.magnetometer_ASA[i] = ((float(response[i]) - 128)/256 + 1) * self.__Magnetometer_Sensitivity_Scale_Factor

# -----------------------------------------------------------------------------------------------

    def read_mag(self):
        # Set the I2C slave addres of AK8963 and set for read.
        self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG)
        # I2C slave 0 register address from where to begin data transfer
        self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_HXL)
        # Read 6 bytes from the magnetometer
        self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x87)

        time.sleep(0.01)

        response = self.ReadRegs(self.__MPUREG_EXT_SENS_DATA_00, 7)
        #must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
        for i in range(0, 3):
            data = self.byte_to_float_le(response[i*2:i*2+2])
            self.magnetometer_data[i] = data * self.magnetometer_ASA[i]

# -----------------------------------------------------------------------------------------------

    def read_all(self):
        # Send I2C command at first
        # Set the I2C slave addres of AK8963 and set for read.
        self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG)
        # I2C slave 0 register address from where ; //Read 7 bytes from the magnetometerto begin data transfer
        self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_HXL)
        # Read 7 bytes from the magnetometer
        self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x87)
        # must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.

        # time.sleep(0.001)
        response = self.ReadRegs(self.__MPUREG_ACCEL_XOUT_H, 21);

        # Get Accelerometer values
        for i in range(0, 3):
            data = self.byte_to_float(response[i*2:i*2+2])
            self.accelerometer_data[i] = data/self.acc_divider

        # Get temperature
        i = 3
        temp = self.byte_to_float(response[i*2:i*2+2])
        self.temperature = (temp/340.0)+36.53

        # Get gyroscope values
        for i in range(4, 7):
            data = self.byte_to_float(response[i*2:i*2+2])
            self.gyroscope_data[i-4] = data/self.gyro_divider

        # Get magnetometer values
        for i in range(7, 10):
            data = self.byte_to_float_le(response[i*2:i*2+2])
            self.magnetometer_data[i-7] = data * self.magnetometer_ASA[i-7]

# -----------------------------------------------------------------------------------------------
#                                          GET VALUES
# usage: call this functions to read and get values
# returns accel, gyro and mag values
# -----------------------------------------------------------------------------------------------

    def getMotion9(self):
        self.read_all()
        m9a = self.accelerometer_data
        m9g = self.gyroscope_data
        m9m = self.magnetometer_data

        return m9a, m9g, m9m

# -----------------------------------------------------------------------------------------------

    def getMotion6(self):
        self.read_acc()
        self.read_gyro()

        m6a = self.accelerometer_data
        m6g = self.gyroscope_data

        return m6a, m6g

# -----------------------------------------------------------------------------------------------

    def byte_to_float(self, input_buffer):
        byte_array = array.array("B", input_buffer)
        signed_16_bit_int, = struct.unpack(">h", byte_array)
        return float(signed_16_bit_int)

# -----------------------------------------------------------------------------------------------

    def byte_to_float_le(self, input_buffer):
        byte_array = array.array("B", input_buffer)
        signed_16_bit_int, = struct.unpack("<h", byte_array)
        return float(signed_16_bit_int)




