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