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 calibrateCompass():
if _mpu9250:
i2c = I2C(scl=Pin(22), sda=Pin(21), freq=400000)
print("Calibrating compass: keep turning BPI:BIT for 15 seconds.")
ak8963 = AK8963(i2c)
offset, scale = ak8963.calibrate(count=150, delay=100)
print("Calibration completed.")
print("AK8963 offset:")
print(offset)
print("AK8963 scale:")
print(scale)
_mpu9250 = MPU9250(i2c, 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 __init__(self):
self.i2c = I2C(-1, scl=Pin(22), sda=Pin(21), freq=400000)
# activate magnetometer and bypass
self.i2c.writeto_mem(104, 0x6B, b'\x01')
self.i2c.writeto_mem(104, 0x37, b'\x02')
self.i2c.writeto_mem(104, 0x6a, b'\x00')
try:
f = open('compass_calibraten', 'r')
data = ujson.load(f)
f.close()
print('Using offset {} and scale {}'.format(
data['offset'], data['scale']))
except OSError:
data = {'offset': (0, 0, 0), 'scale': (1, 1, 1)}
self.sensor = AK8963(self.i2c,
offset=data['offset'],
scale=data['scale'])
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))
def __init__(self, fifo_mode=False):
self.mpu6500 = MPU6500(0x68, fifo_mode)
self.ak8963 = AK8963()
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)
import pycom
from machine import I2C, Pin
from mpu9250 import MPU9250
from ak8963 import AK8963
# Returned Values:
# Offset = (5.19961, -4.03418, -56.78174)
# Scale = (1.067856, 1.114284, 0.8575542)
i2c = I2C(0, I2C.MASTER, baudrate=100000)
ak8963 = AK8963(i2c)
offset, scale = ak8963.calibrate(count=256, delay=200)
#sensor = MPU9250(i2c, ak8963=ak8963)
print("Offset: ", offset)
print("Scale: ", scale)
offset = j['offset']
scale = j['scale']
except Exception as e:
# whatever failed above - let's go with the defaults
log.warning(e, 'unable to parse {}, using defaults'.format(FILE_MAG_CAL))
finally:
log.info('magnetometer calibration offset: {}, scale: {}'.format(
offset, scale))
# imu
from mpu9250 import MPU9250
dummy = MPU9250(
i2c) # this opens the "bybass" to access to the AK8963 directly
from ak8963 import AK8963
# use bypass to pass calibration data
ak8963 = AK8963(i2c, offset=offset, scale=scale)
# finally initialize inertial measurement unit
imu = MPU9250(i2c, ak8963=ak8963)
log.debug(imu)
import uasyncio as asyncio
from fusion_async import Fusion
async def read_coro():
# TODO: validate sleepy time
await asyncio.sleep_ms(10)
# go with what is set right now
return imu.acceleration, imu.gyro, imu.magnetic