I had a very similar-looking problem a few years ago, and posted a question in Electronics SE. @BrianDrummond figured it out and posted this answer:
Errors look like +/-256 +/- expected drift.
This suggests some kind of synchronising error - such as reading the LSByte of one sample and the MSByte of the next or vice versa. How does your gyro handle this, is there some way to temporarily put it on hold while you read data?
Also, examine the raw data around a few of these errors to see if it happens when a slowly drifting signal crosses an MSByte boundary. Obviously, focus on the Y Gyro, the errors are most clearly separated from the noise.
That turned out to be exactly the problem. My whole script (exploring clock frequency and low pass filter settings to no avail) is posted there but the key bits are:
pairs = ((0x3b, 0x3c), (0x3d, 0x3e), (0x3f, 0x40),
(0x43, 0x44), (0x45, 0x46), (0x47, 0x48))
def read_six():
vals = []
for ahi, alo in pairs:
high = bus.read_byte_data(address, ahi)
low = bus.read_byte_data(address, alo)
val = (high << 8) + low
if val >= 0x8000:
val = -((65535 - val) + 1)
vals.append(val)
return vals
I was reading every single byte separately, in the slowest, most awkward way imaginable. There was Python executing between the high byte and the low byte, which freed up the data buffer in the chip to have fresh data written to it.
The solution was to read all of the values, all fourteen bytes at once with a single read_i2c_block_data()
When the chip is being read with an I2C block read, I believe it keeps the data buffer locked and will not try to write to it for that short period of time. That way all of the bytes you read belong together and there is no mix-up.
def read_sevenblock():
# read fourteen bytes starting at address 0x3b. This will cycle through
# ax, ay, az, temp, gx, gy, and gz
fourteen_bytes = bus.read_i2c_block_data(0x68, 0x3b, 14) # Read them all at once
his, los = fourteen[0::2], fourteen[1::2] # high bytes are first
values = []
for hi, lo in zip(his, los):
value = (hi << 8) + lo # combine high and low byte
if value >= 0x8000:
value = -((65535 - value) + 1) # convert signed negative values
values.append(value)
return values
That's a year ago. For even better speed and integrity I am using the on-chip FIFO to buffer the data. I still use block reads but now I can use the steady cadence of digitization by the on-chip clock, while reading asynchronously. That's really important because if you want to use your data numerically, you need ideally even-spaced measurement times, or at least known times, and triggering each read with a Python loop won't do that.
Here is an extract of the script I use now, I've simplified it a bit. After you initialize your GPIO. The defaults are to read at a reasonable rate for three seconds then put all the values into a dictionary.
You'll have to split them up again the way you like.
import smbus, time
class MPU6050i2c(object):
def __init__(self):
self.bus = smbus.SMBus(1)
self.datadict = dict()
self.BWaccs = (260, 184, 94, 44, 21, 10, 5, None)
self.BWgyros = (256, 188, 98, 42, 20, 10, 5, None)
self.Fsamples = (8000, 1000, 1000, 1000, 1000, 1000, 1000, 8000)
self.I2C_clockdivs = range(16, 32)[7:] + range(16, 32)[:7]
# REGISTERS:
self.SMPLRT_DIV = 0x19
self.CONFIG = 0x1a
self.GYRO_CONFIG = 0x1b
self.ACCEL_CONFIG = 0x1c
self.FIFO_EN = 0x23
self.INT_ENABLE = 0x38
self.INT_STATUS = 0x3a
self.ACC_start = 0x3b
self.GYRO_start = 0x43
self.USER_CTRL = 0x6a
self.PWR_MGMT_1 = 0x6b
self.PWR_MGMT_2 = 0x6c
self.FIFO_R_W = 0x74
self.FIFO_COUNTH = 0x72 # two bytes
self.I2C_MST_CTRL = 0x24
# self.INT_PIN_CFG = 0x37 # this appears to be important
def setup(self, I2C_clockdiv=9, DLPF=2, divideby=100,
recordgyros=True, recordaccs=True, recordtemp=False,
n_gyro_scale=0, n_acc_scale=0, fifo_block_read=True,
chunkbytes=24, AD0=0):
if type(AD0) is str:
AD0 = AD0.lower()[:2]
if AD0 in (True, 1, 'hi'):
self.ADDR = 0x69 # this is different than the RTC!
else:
self.ADDR = 0x68 # be careful, this is the same as the RTC!
self.I2C_clockdiv = I2C_clockdiv
self.DLPF = DLPF
self.divideby = divideby
self.recordgyros = recordgyros
self.recordaccs = recordaccs
self.recordtemp = recordtemp
self.n_gyro_scale = n_gyro_scale
self.n_acc_scale = n_acc_scale
self.fifo_block_read = fifo_block_read
self.chunkbytes = chunkbytes
self.gyro_scale = 8 * self.n_gyro_scale
self.acc_scale = 8 * self.n_acc_scale
self.I2C_clock_freq = 8E+06 / float(self.I2C_clockdivs[self.I2C_clockdiv])
print "I2C_clock_freq (kHz): ", self.I2C_clock_freq
self.BWacc = self.BWaccs[self.DLPF]
self.BWgyro = self.BWgyros[self.DLPF]
self.Fsample = self.Fsamples[self.DLPF]
self.chunkrate = float(self.Fsample)/float(self.divideby)
self.nbytesperchunk = (self.recordaccs*6 +
self.recordgyros*6 +
self.recordtemp*2 )
self.byterate = self.nbytesperchunk * self.chunkrate
self.fifoen = (self.recordaccs * 0b00001000 +
self.recordgyros * 0b01110000 +
self.recordtemp * 0b10000000 )
self.gyro_scale = 8 * self.n_gyro_scale
self.acc_scale = 8 * self.n_acc_scale
def _cmdr(self, A):
return self.bus.read_byte_data(self.ADDR, A)
def _cmdw(self, A, B):
return self.bus.write_byte_data(self.ADDR, A, B)
def initialize(self):
self._cmdw(self.PWR_MGMT_1, 0x00) # wake up!
time.sleep(0.05)
self._cmdw(self.I2C_MST_CTRL, self.I2C_clockdiv) # set i2c 8 MHz clock didvie
self._cmdw(self.PWR_MGMT_2, 0x00) # dunno
self._cmdw(self.INT_ENABLE, 0x00) # bit 0 is for data ready interrupt
self._cmdw(self.CONFIG, 0x06) # bits 0-2 DLPF ### ????
self._cmdw(self.SMPLRT_DIV, self.divideby) # divide by 255
self._cmdw(self.GYRO_CONFIG, self.gyro_scale)
self._cmdw(self.ACCEL_CONFIG, self.acc_scale)
print "INT_STATUS: ", self._cmdr(self.INT_STATUS)
self._cmdw(self.USER_CTRL, 0x00) # disable everything
self._cmdw(self.USER_CTRL, 0x04) # FIFO reset (bit 2)
self._cmdw(self.USER_CTRL, 0x40) # FIFO enable (bit 6)
self._cmdw(self.FIFO_EN, self.fifoen)
print "INT_STATUS: ", self._cmdr(self.INT_STATUS)
def get_FIFO_count(self):
hi, lo = self.bus.read_i2c_block_data(self.ADDR, self.FIFO_COUNTH, 2)
return (hi << 8) + lo
def digitize(self, sampletime=3, maxdata=1000):
self.datadict = dict()
self.sampletime = sampletime
self.maxdata = maxdata
self.datadict = dict()
self.data = []
tstart = time.time()
ndata = 0
tnow = time.time()
values, fifocounts, tnows = [], [], []
while tnow - tstart <= self.sampletime and ndata <= self.maxdata:
fifocount = self.get_FIFO_count()
self.fifocount = fifocount
while fifocount < self.chunkbytes:
fifocount = self.get_FIFO_count()
# I wonder if we want this waiting loop so tight?
if self.fifo_block_read:
chunk = self.bus.read_i2c_block_data(self.ADDR,
self.FIFO_R_W,
self.chunkbytes)
else:
chunk = []
for j in range(24):
data = self.bus.read_byte_data(self.ADDR, self.FIFO_R_W)
chunk.append(data)
vals = []
for hi, lo in zip(chunk[0::2], chunk[1::2]):
val = (hi << 8) + lo
if val >= 0x8000:
val = -((65535 - val) + 1)
vals.append(val)
tnow = time.time()
values.append(vals)
fifocounts.append(fifocount)
tnows.append(tnow)
ndata += 1
self.datadict['values'] = values
self.datadict['fifocounts'] = fifocounts
self.datadict['tnows'] = tnows