"""
This code is based on Martijn's(martijn@mrtijn.nl) project mpu6050.
Follow the MIT protocol.
https://github.com/m-rtijn/mpu6050
"""
from smbus2 import SMBus
import time
[Doku]
class MPU6050():
I2C_ADDRESS = 0x68
# Global Variables
GRAVITIY_MS2 = 9.80665
address = None
bus = None
# Scale Modifiers
ACCEL_SCALE_MODIFIER_2G = 16384.0
ACCEL_SCALE_MODIFIER_4G = 8192.0
ACCEL_SCALE_MODIFIER_8G = 4096.0
ACCEL_SCALE_MODIFIER_16G = 2048.0
GYRO_SCALE_MODIFIER_250DEG = 131.0
GYRO_SCALE_MODIFIER_500DEG = 65.5
GYRO_SCALE_MODIFIER_1000DEG = 32.8
GYRO_SCALE_MODIFIER_2000DEG = 16.4
# Pre-defined ranges
ACCEL_RANGE_2G = 0x00
ACCEL_RANGE_4G = 0x08
ACCEL_RANGE_8G = 0x10
ACCEL_RANGE_16G = 0x18
GYRO_RANGE_250DEG = 0x00
GYRO_RANGE_500DEG = 0x08
GYRO_RANGE_1000DEG = 0x10
GYRO_RANGE_2000DEG = 0x18
FILTER_BW_256=0x00
FILTER_BW_188=0x01
FILTER_BW_98=0x02
FILTER_BW_42=0x03
FILTER_BW_20=0x04
FILTER_BW_10=0x05
FILTER_BW_5=0x06
# MPU-6050 Registers
PWR_MGMT_1 = 0x6B
PWR_MGMT_2 = 0x6C
ACCEL_XOUT0 = 0x3B
ACCEL_YOUT0 = 0x3D
ACCEL_ZOUT0 = 0x3F
TEMP_OUT0 = 0x41
GYRO_XOUT0 = 0x43
GYRO_YOUT0 = 0x45
GYRO_ZOUT0 = 0x47
ACCEL_CONFIG = 0x1C
GYRO_CONFIG = 0x1B
MPU_CONFIG = 0x1A
# Bypass Mode
REG_INT_PIN_CFG = 0x37 # INT_PIN_CFG
REG_USER_CTRL = 0x6A # USER_CTRL
REG_WHO_AM_I = 0x75 # WHO_AM_I
def __init__(self, address=I2C_ADDRESS, bus=1):
self.address = address
self.bus = SMBus(bus)
# Wake up the MPU-6050 since it starts in sleep mode
try:
self.bus.write_byte_data(self.address, self.PWR_MGMT_1, 0x00)
except:
raise OSError(f"MPU6050 not found addr: 0x{self.address:0x}")
# I2C communication methods
[Doku]
def read_i2c_word(self, register):
"""Read two i2c registers and combine them.
register -- the first register to read from.
Returns the combined read results.
"""
# Read the data from the registers
high = self.bus.read_byte_data(self.address, register)
low = self.bus.read_byte_data(self.address, register + 1)
value = (high << 8) + low
if (value >= 0x8000):
return -((65535 - value) + 1)
else:
return value
# MPU-6050 Methods
[Doku]
def get_temp(self):
"""Reads the temperature from the onboard temperature sensor of the MPU-6050.
Returns the temperature in degrees Celcius.
"""
raw_temp = self.read_i2c_word(self.TEMP_OUT0)
# Get the actual temperature using the formule given in the
# MPU-6050 Register Map and Descriptions revision 4.2, page 30
actual_temp = (raw_temp / 340.0) + 36.53
return actual_temp
[Doku]
def set_accel_range(self, accel_range):
"""Sets the range of the accelerometer to range.
accel_range -- the range to set the accelerometer to. Using a
pre-defined range is advised.
"""
# First change it to 0x00 to make sure we write the correct value later
self.bus.write_byte_data(self.address, self.ACCEL_CONFIG, 0x00)
# Write the new range to the ACCEL_CONFIG register
self.bus.write_byte_data(self.address, self.ACCEL_CONFIG, accel_range)
[Doku]
def read_accel_range(self, raw = False):
"""Reads the range the accelerometer is set to.
If raw is True, it will return the raw value from the ACCEL_CONFIG
register
If raw is False, it will return an integer: -1, 2, 4, 8 or 16. When it
returns -1 something went wrong.
"""
raw_data = self.bus.read_byte_data(self.address, self.ACCEL_CONFIG)
if raw is True:
return raw_data
elif raw is False:
if raw_data == self.ACCEL_RANGE_2G:
return 2
elif raw_data == self.ACCEL_RANGE_4G:
return 4
elif raw_data == self.ACCEL_RANGE_8G:
return 8
elif raw_data == self.ACCEL_RANGE_16G:
return 16
else:
return -1
[Doku]
def get_accel_data(self, g = False):
"""Gets and returns the X, Y and Z values from the accelerometer.
If g is True, it will return the data in g
If g is False, it will return the data in m/s^2
Returns a dictionary with the measurement results.
"""
x = self.read_i2c_word(self.ACCEL_XOUT0)
y = self.read_i2c_word(self.ACCEL_YOUT0)
z = self.read_i2c_word(self.ACCEL_ZOUT0)
accel_scale_modifier = None
accel_range = self.read_accel_range(True)
if accel_range == self.ACCEL_RANGE_2G:
accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_2G
elif accel_range == self.ACCEL_RANGE_4G:
accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_4G
elif accel_range == self.ACCEL_RANGE_8G:
accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_8G
elif accel_range == self.ACCEL_RANGE_16G:
accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_16G
else:
print("Unkown range - accel_scale_modifier set to self.ACCEL_SCALE_MODIFIER_2G")
accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_2G
x = x / accel_scale_modifier
y = y / accel_scale_modifier
z = z / accel_scale_modifier
if g is True:
return [x, y, z]
elif g is False:
x = x * self.GRAVITIY_MS2
y = y * self.GRAVITIY_MS2
z = z * self.GRAVITIY_MS2
return [x, y, z]
[Doku]
def set_gyro_range(self, gyro_range):
"""Sets the range of the gyroscope to range.
gyro_range -- the range to set the gyroscope to. Using a pre-defined
range is advised.
"""
# First change it to 0x00 to make sure we write the correct value later
self.bus.write_byte_data(self.address, self.GYRO_CONFIG, 0x00)
# Write the new range to the ACCEL_CONFIG register
self.bus.write_byte_data(self.address, self.GYRO_CONFIG, gyro_range)
[Doku]
def set_filter_range(self, filter_range=FILTER_BW_256):
"""Sets the low-pass bandpass filter frequency"""
# Keep the current EXT_SYNC_SET configuration in bits 3, 4, 5 in the MPU_CONFIG register
EXT_SYNC_SET = self.bus.read_byte_data(self.address, self.MPU_CONFIG) & 0b00111000
return self.bus.write_byte_data(self.address, self.MPU_CONFIG, EXT_SYNC_SET | filter_range)
[Doku]
def read_gyro_range(self, raw = False):
"""Reads the range the gyroscope is set to.
If raw is True, it will return the raw value from the GYRO_CONFIG
register.
If raw is False, it will return 250, 500, 1000, 2000 or -1. If the
returned value is equal to -1 something went wrong.
"""
raw_data = self.bus.read_byte_data(self.address, self.GYRO_CONFIG)
if raw is True:
return raw_data
elif raw is False:
if raw_data == self.GYRO_RANGE_250DEG:
return 250
elif raw_data == self.GYRO_RANGE_500DEG:
return 500
elif raw_data == self.GYRO_RANGE_1000DEG:
return 1000
elif raw_data == self.GYRO_RANGE_2000DEG:
return 2000
else:
return -1
[Doku]
def get_gyro_data(self):
"""Gets and returns the X, Y and Z values from the gyroscope.
Returns the read values in a dictionary.
"""
x = self.read_i2c_word(self.GYRO_XOUT0)
y = self.read_i2c_word(self.GYRO_YOUT0)
z = self.read_i2c_word(self.GYRO_ZOUT0)
gyro_scale_modifier = None
gyro_range = self.read_gyro_range(True)
if gyro_range == self.GYRO_RANGE_250DEG:
gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_250DEG
elif gyro_range == self.GYRO_RANGE_500DEG:
gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_500DEG
elif gyro_range == self.GYRO_RANGE_1000DEG:
gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_1000DEG
elif gyro_range == self.GYRO_RANGE_2000DEG:
gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_2000DEG
else:
print("Unkown range - gyro_scale_modifier set to self.GYRO_SCALE_MODIFIER_250DEG")
gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_250DEG
x = x / gyro_scale_modifier
y = y / gyro_scale_modifier
z = z / gyro_scale_modifier
return [x, y, z]
[Doku]
def get_all_data(self):
"""Reads and returns all the available data."""
temp = self.get_temp()
accel = self.get_accel_data()
gyro = self.get_gyro_data()
return [accel, gyro, temp]
[Doku]
def enable_bypass(self):
"""
Enable MPU6050 bypass mode
Returns:
[uc,ic] : USER_CTRL and INT_PIN_CFG register values
"""
self.bus.write_byte_data(self.address, self.REG_USER_CTRL, 0x00)
time.sleep(0.002)
# open I2C bypass mode, connect SDA/SCL to auxiliary I2C device
self.bus.write_byte_data(self.address, self.REG_INT_PIN_CFG, 0x02)
time.sleep(0.002)
uc = self.bus.read_byte_data(self.address, self.REG_USER_CTRL)
ic = self.bus.read_byte_data(self.address, self.REG_INT_PIN_CFG)
print(f"Bypass enabled: USER_CTRL=0x{uc:02X}, INT_PIN_CFG=0x{ic:02X}")
return [uc,ic]
if __name__ == "__main__":
# mpu = MPU6050(0x68)
mpu = MPU6050()
print(mpu.get_temp())
accel_data = mpu.get_accel_data()
print(accel_data[0])
print(accel_data[1])
print(accel_data[2])
gyro_data = mpu.get_gyro_data()
print(gyro_data[0])
print(gyro_data[1])
print(gyro_data[2])
bypassDate = mpu.enable_bypass()
print(bypassDate[0])
print(bypassDate[1])