def _init_mpu(self):
self.mpu = Mpu()
self.mpu.add_callback(self.on_move)
def run():
global plotItem, y, mpu, kalman, dt, yaw_int, exp
while True:
# gyro_xout, gyro_yout, gyro_zout, accel_xout_scaled, accel_yout_scaled, accel_zout_scaled, x_rotation, y_rotation = mpu.read()
gyro_xout, gyro_yout, gyro_zout = mpu.read()
# kalman.filterOnce(np.array([[gyro_zout]]))
# yaw, yaw_speed = kalman.x
exp.filterOnce(gyro_zout)
y[:-1] = y[1:]
y[-1] = yaw_int
yaw_int += (exp.state-1.5) * dt
plotItem.plot(t, y, clear=True)
pg.QtGui.QApplication.processEvents()
time.sleep(dt)
if __name__ == '__main__':
dt = 0.01
yaw_int = 0
kalman, exp = init(dt)
win = pg.GraphicsWindow(title='mpu6050 test')
plotItem = win.addPlot(title='yaw')
plotItem.setYRange(-90, 90)
t = np.arange(100)
y = np.zeros(100)
mpu = Mpu()
try:
run()
except KeyboardInterrupt:
exit(-1)
