615 lines
25 KiB
Python
615 lines
25 KiB
Python
"""
|
|
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:
|
|
|
|
G_SI = 9.80665
|
|
PI = 3.14159
|
|
|
|
# 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] = self.G_SI * 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] = (self.PI/180) * 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] = self.G_SI * 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] = (self.PI/180) * 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)
|
|
|
|
|
|
|
|
|