def __init__(self, i2c, mpu6500=None, ak8963=None):
self.Kp = 100 #比例增益支配率收敛到加速度计/磁强计
self.Ki = 100 #积分增益支配率的陀螺仪偏见的衔接
self.halfT = 0.004 #采样周期的一半
self.q0 = 1
self.q1 = 0
self.q2 = 0
self.q3 = 0 #四元数的元素 ,代表估计方向
self.HalfEXInt = 0
self.HalfEYInt = 0
self.HalfEZInt = 0 #按比例缩小积分误差
self.vals = {}
self.AX = KalmanFiter(TestPool=100) #滤波，加速度计，需要稳定性
self.AY = KalmanFiter(TestPool=100)
self.AZ = KalmanFiter(TestPool=100)
self.GX = KalmanFiter(PredictPool=100) #角速度，需要及时
self.GY = KalmanFiter(PredictPool=100)
self.GZ = KalmanFiter(PredictPool=100)
self.MX = KalmanFiter(TestPool=100) #滤波，加速度计，需要稳定性
self.MY = KalmanFiter(TestPool=100)
self.MZ = KalmanFiter(TestPool=100)
if mpu6500 is None:
self.mpu6500 = MPU6500(i2c)
else:
self.mpu6500 = mpu6500
if ak8963 is None:
self.ak8963 = AK8963(i2c)
else:
self.ak8963 = ak8963
def __init__(self, i2c, mpu6500=None, ak8963=None):
if mpu6500 is None:
self.mpu6500 = MPU6500(i2c)
else:
self.mpu6500 = mpu6500
if ak8963 is None:
self.ak8963 = AK8963(i2c)
else:
self.ak8963 = ak8963
def senMPU():
global accel, acX, acY, acZ, gyro, gX, gY, gZ, mag, mX, mY, mZ
bypass = MPU9250(i2c_gy91)
if MPUCalibrat:
ak8963 = AK8963(i2c_gy91, offset=offsetMag, scale=scaleMag)
#Pendent d'actualització de la llibreria a la versio 4.0
#mpu6500 = MPU6500(i2c_gy91, offset=offsetGyro, gyro_sf=SF_DEG_S)
mpu6500 = MPU6500(i2c_gy91, gyro_sf=SF_DEG_S)
else:
ak8963 = AK8963(i2c_gy91)
mpu6500 = MPU6500(i2c_gy91, gyro_sf=SF_DEG_S)
mpu = mpu9250.MPU9250(i2c_gy91, ak8963=ak8963, mpu6500=mpu6500)
#Reorientacio dels eixos del accelerometre i el giroscopi per a que tinguin la mateixa disposicio que el magnetometre
#Veure eixos al datasheet MPU9250
accel, gyro = orientate((1, 0, 2), (False, False, True), mpu.acceleration,
mpu.gyro)
acX, acY, acZ = accel
gX, gY, gZ = gyro
mag = mpu.magnetic
mX, mY, mZ = mag
def __init__(self, i2c, mpu6500=None, ak8963=None):
if mpu6500 is None:
self.mpu6500 = MPU6500(i2c)
else:
self.mpu6500 = mpu6500
# Enable I2C bypass to access AK8963 directly.
char = self.mpu6500._register_char(_INT_PIN_CFG)
char &= ~_I2C_BYPASS_MASK # clear I2C bits
char |= _I2C_BYPASS_EN
self.mpu6500._register_char(_INT_PIN_CFG, char)
if ak8963 is None:
self.ak8963 = AK8963(i2c)
else:
self.ak8963 = ak8963
def calMPU():
#Calibració magnetometre
bypass = MPU9250(i2c_gy91)
ak8963 = AK8963(i2c_gy91)
offsetMag, scaleMag = ak8963.calibrate(count=256, delay=200)
#Calibracio Giroscopi
mpu6500 = MPU6500(i2c_gy91)
offsetGyro = mpu6500.calibrate(count=256, delay=0)
#Guardar dades de calibracio
logCal = open('/sd/cal.py', 'w')
logCal.write('offsetMag = {} \nscaleMag = {} \noffsetGyro = {}'.format(
offsetMag, scaleMag, offsetGyro))
logCal.close()
MPUCalibrat = True
return offsetMag, scaleMag, offsetGyro
def __init__(self,
i2c,
accel_scale_factor=SF_G,
accel_func_scale=ACCEL_FS_SEL_2G,
gyro_scale_factor=SF_DEG_S,
gyro_func_scale=GYRO_FS_SEL_250DPS,
offset_default=(0, 0, 0),
scale_default=(1, 1, 1)):
""" SF scale factor -> facteur de mise à l'échelle (unites)
accel_scale_factor :
SF_G = 1
SF_M_S2 = 9.80665 # 1 g = 9.80665 m/s2 ie. standard gravity
gyro_scale_factor :
SF_DEG_S = 1
SF_RAD_S = 0.017453292519943 # 1 deg/s is 0.017453292519943 rad/s
FS functionning scale : plage de mesure
gyro_func_scale :
ACCEL_FS_SEL_2G
ACCEL_FS_SEL_4G
ACCEL_FS_SEL_8G
ACCEL_FS_SEL_16G
gyro_func_scale (deg/s):
GYRO_FS_SEL_250DPS
GYRO_FS_SEL_500DPS
GYRO_FS_SEL_1000DPS
GYRO_FS_SEL_2000DPS
offset_default, offset_default : obtenu par la calibration
"""
self.mpu6500 = MPU6500(i2c,
accel_sf=accel_scale_factor,
accel_fs=accel_func_scale,
gyro_sf=gyro_scale_factor,
gyro_fs=gyro_func_scale)
self.ak8963 = AK8963(i2c, offset=offset_default, scale=scale_default)
#self.ak8963 = AK8963(i2c)
sensor = MPU9250(i2c, mpu6500=self.mpu6500, ak8963=self.ak8963)
print("MPU9250 id: " + hex(sensor.whoami))
import display
Image = display.Image
display = display.Display()
import button
button_a = button.Button(35)
button_b = button.Button(27)
import temperature
__adc = machine.ADC(machine.Pin(34, machine.Pin.IN))
__adc.atten(machine.ADC.ATTN_11DB)
temperature = temperature.Temperature(__adc).temperature
try:
from mpu9250 import MPU9250
from mpu6500 import MPU6500
__i2c = machine.I2C(scl=machine.Pin(22), sda=machine.Pin(21), freq=200000)
__dev = __i2c.scan()
# print("dev ", __dev)
if 104 in __dev:
print("1.4 version")
__sensor = MPU9250(__i2c, MPU6500(__i2c, 0x68))
if 105 in __dev:
print("1.2 version No compass")
__sensor = MPU9250(__i2c, MPU6500(__i2c, 0x69))
import accelerometer
accelerometer = accelerometer.Direction(__sensor)
import compass
compass = compass.Compass(__sensor)
except Exception as e:
print("MPU9250 Error", e)
def __init__(self, fifo_mode=False):
self.mpu6500 = MPU6500(0x68, fifo_mode)
self.ak8963 = AK8963()
import utime
from machine import I2C, Pin, freq
from mpu9250 import MPU9250
from mpu6500 import MPU6500, SF_G, SF_DEG_S, GYRO_FS_SEL_1000DPS
# import ADXL345 as ADXL345
import math
freq(160000000)
i2c = I2C(scl=Pin(5, Pin.PULL_UP), sda=Pin(4, Pin.PULL_UP), freq=400000)
mpu6500 = MPU6500(i2c, gyro_sf=SF_DEG_S)
sensor = MPU9250(i2c, mpu6500=mpu6500)
# adx = ADXL345.ADXL345(i2c)
print()
print('\nOFSET', sensor.mpu6500.calibrate())
sensor.mpu6500.calibrate(delay=10)
print("MPU9250 id: " + hex(sensor.whoami))
angx = 0
angy = 0
angz = 0
dt2 = 0
gy1 = (0, 0, 0)
gy2 = (0, 0, 0)
accelerationX = sensor.acceleration[0]
accelerationY = sensor.acceleration[1]
accelerationZ = sensor.acceleration[2]
pitch0 = 180 * math.atan(accelerationX/math.sqrt(accelerationY *
import utime
from machine import I2C, Pin
from ak8963 import AK8963
from mpu9250 import MPU9250
from mpu6500 import MPU6500, SF_G, SF_DEG_S
i2c = I2C(scl=Pin(12), sda=Pin(14))
ak8963 = AK8963(i2c,
offset=(-11.6288, 20.7088, -55.9863),
scale=(0.951743, 1.06654, 0.988447))
mpu6500 = MPU6500(i2c, accel_sf=SF_G, gyro_sf=SF_DEG_S)
sensor = MPU9250(i2c, mpu6500=mpu6500, ak8963=ak8963)
print("MPU9250 id: " + hex(sensor.whoami))
data_file = open('data.txt', 'a+')
while True:
data_file.write('Acceleration: {}\n'.format(sensor.acceleration))
data_file.write('Gyro: {}\n'.format(sensor.gyro))
data_file.write('Magnetic: {}\n'.format(sensor.magnetic))
data_file.flush()
utime.sleep_ms(166)
from mpu9250 import MPU9250
from mpu6500 import MPU6500, SF_DEG_S
from fusion import Fusion
# pylint: enable=import-error
micropython.alloc_emergency_exception_buf(100)
tft = m5stack.Display()
tft.font(tft.FONT_Default)
tft.text(60, 90, "HEADING:")
tft.text(88, 110, "PITCH:")
tft.text(102, 130, "ROLL:")
i2c = I2C(scl=Pin(22), sda=Pin(21))
mpu6500 = MPU6500(i2c, gyro_sf=SF_DEG_S) # Fusion expects deg/s
sensor = MPU9250(i2c, mpu6500=mpu6500)
imu = Fusion()
def update_imu(timer):
imu.update(sensor.acceleration, sensor.gyro, sensor.magnetic)
def display_imu(timer):
tft.text(180, 90, "{:3.1f}\r".format(imu.heading))
tft.text(180, 110, "{:3.1f}\r".format(imu.pitch))
tft.text(180, 130, "{:3.1f}\r".format(imu.roll))
timer_0 = Timer(0)
def setUp(self):
self.mpu6500 = MPU6500()
return (1 / (ind + 1)) * (avg * ind + val)
# Mount the SD card for recording data
sd = SD()
os.mount(sd, '/sd')
pycom.heartbeat(False)
# Initialise the file
f = open('/sd/gyro_calibration.txt', 'w')
f.write("Offset Values for MPU9250 gyroscope:\n") # Write an introduction
f.close()
# Initialise the I2C connection
i2c = I2C(0, I2C.MASTER, baudrate=100000)
gyroSensor = MPU6500(i2c)
samples = 256 # Number of samples to average
delay = 200 # Delay between samples (ms)
counter = 1
(Gx, Gy, Gz) = gyroSensor.gyro
ax = Gx
ay = Gy
az = Gz
print(counter)
while counter < samples:
(Gx, Gy, Gz) = gyroSensor.gyro