def InitMPU6050(self):
# Accelerometer Values and Device
self._mpuIdx = ['x', 'y', 'z']
self._a = [0.0, 0.0, 0.0] # Acceleration (g's)
self._w = [0.0, 0.0, 0.0] # Angular rate (deg/s)
self._wFilt = [0.0, 0.0, 0.0] # Filtered Angular rate (deg/s)
self._theta = [0.0, 0.0, 0.0] # Angle (deg)
self._thetaFilt = [0.0, 0.0, 0.0] # Filtered Angle (deg)
self._a_off = [0.66, -0.26, 10.22 - 9.81]
self._w_off = [-1.07, 0.67, -1.55]
#self._a_off = [0.0, 0.0, 0.0]
#self._w_off = [0.0, 0.0, 0.0]
self._thetaXFilt = ComplementaryFilter(alpha=0.05) # alpha = 0.02
self._thetaXFilt2 = Filter(timeConst=0.0)
self._wXFilt = Filter(timeConst=0.0)
self._mpu6050 = mpu6050(0x68) # 0x68 - default I2C slave addr
self._mpu6050.set_accel_range(mpu6050.ACCEL_RANGE_2G)
self._mpu6050.set_gyro_range(mpu6050.GYRO_RANGE_250DEG)
return
