def setup():
senMPU()
global ori
ori = Fusion()
Timing = True
Calibrate = False
def getmag():
senMPU()
return mag
if Calibrate:
print("Calibrant. Presionar KEY un cop acabat.")
ori.calibrate(getmag, sw, lambda: pyb.delay(100))
print(ori.magbias)
if Timing:
'''
mag = mpu.magnetic # Don't include blocking read in time
accel = mpu.acceleration # or i2c
gyro = mpu.gyro
'''
senMPU()
start = time.ticks_us() # Measure computation time only
ori.update(accel, gyro, mag, 0.005) # 1.97mS on Pyboard
t = time.ticks_diff(time.ticks_us(), start)
print("Temps de mostreig (uS):", t)
# Code for Pyboard switch
#sw = pyb.Switch()
# Choose test to run
Calibrate = True
Timing = True
def getmag(): # Return (x, y, z) tuple (blocking read)
return imu.mag.xyz
if Calibrate:
print("Calibrating. Press switch when done.")
fuse.calibrate(getmag, sw, lambda: pyb.delay(100))
print(fuse.magbias)
if Timing:
mag = imu.mag.xyz # Don't include blocking read in time
accel = imu.accel.xyz # or i2c
gyro = imu.gyro.xyz
start = time.ticks_us() # Measure computation time only
fuse.update(accel, gyro, mag) # 1.97mS on Pyboard
t = time.ticks_diff(time.ticks_us(), start)
print("Update time (uS):", t)
count = 0
while True:
fuse.update(imu.accel.xyz, imu.gyro.xyz,
imu.mag.xyz) # Note blocking mag read
import utime as time
from mpu9250 import MPU9250
from fusion import Fusion
from machine import I2C
i2c = I2C(sda = 21, scl = 22)
imu = MPU9250(i2c)
fuse = Fusion()
def getmag(): # Return (x, y, z) tuple (blocking read)
return imu.mag.xyz
if buttonA.isPressed():
print("Calibrating. Press switch when done.")
fuse.calibrate(getmag, BtnB.press, lambda : time.sleep(0.1))
print(fuse.magbias)
if False:
mag = imu.magnetic # Don't include blocking read in time
accel = imu.acceleration # or i2c
gyro = imu.gyro
start = time.ticks_us() # Measure computation time only
fuse.update(accel, gyro, mag) # 1.97mS on Pyboard
t = time.ticks_diff(time.ticks_us(), start)
print("Update time (uS):", t)
lcd.clear()
lcd.font(lcd.FONT_Small)
lcd.setTextColor(lcd.WHITE, lcd.BLACK)
class TCCompass:
def __init__(self, imu, timeout=1000):
# load configuration file
with open('calibration.conf', mode='r') as f:
mpu_conf = f.readline()
mcnf = pickle.loads(mpu_conf)
self.MPU_Centre = mcnf[0][0]
self.MPU_TR = mcnf[1]
self.counter = pyb.millis()
self.timeout = timeout
# setup MPU9250
self.imu = imu
self.gyrobias = (-1.394046, 1.743511, 0.4735878)
# setup fusions
self.fuse = Fusion()
self.update()
self.hrp = [self.fuse.heading, self.fuse.roll, self.fuse.pitch]
def process(self):
self.update()
if pyb.elapsed_millis(self.counter) >= self.timeout:
self.hrp = [self.fuse.heading, self.fuse.roll, self.fuse.pitch]
self.counter = pyb.millis()
return True
return False
def update(self):
accel = self.imu.accel.xyz
gyroraw = self.imu.gyro.xyz
gyro = [
gyroraw[0] - self.gyrobias[0], gyroraw[1] - self.gyrobias[1],
gyroraw[2] - self.gyrobias[2]
]
self.mag = self.imu.mag.xyz
self.fuse.update(
accel, gyro, self.adjust_mag(self.mag, self.MPU_Centre,
self.MPU_TR))
def getmag(self):
return self.imu.mag.xyz
@staticmethod
def adjust_mag(mag, centre, TR):
mx_raw = mag[0] - centre[0]
my_raw = mag[1] - centre[1]
mz_raw = mag[2] - centre[2]
mx = mx_raw * TR[0][0] + my_raw * TR[0][1] + mz_raw * TR[0][2]
my = mx_raw * TR[1][0] + my_raw * TR[1][1] + mz_raw * TR[1][2]
mz = mx_raw * TR[2][0] + my_raw * TR[2][1] + mz_raw * TR[2][2]
return (mx, my, mz)
def Calibrate(self):
print("Calibrating. Press switch when done.")
sw = pyb.Switch()
self.fuse.calibrate(self.getmag, sw, lambda: pyb.delay(100))
print(self.fuse.magbias)
def gyrocal(self):
xa = 0
ya = 0
za = 0
for x in range(0, 100):
xyz = self.imu.gyro.xyz
xa += xyz[0]
ya += xyz[1]
za += xyz[2]
pyb.delay(1000)
print(xa / 100, ya / 100, za / 100)
@property
def heading(self):
a = self.hrp[0]
if a < 0:
a += 360
return a
@property
def roll(self):
return self.hrp[1]
@property
def pitch(self):
return self.hrp[2]
@property
def output(self):
outstring = [
CMP("1,{},{}".format(self.mag, self.hrp[0])).msg,
HDG(self.hrp[0]).msg,
XDR(self.hrp[2], self.hrp[1])
]
for x in range(0, 2):
outstring[x] = outstring[x].replace('(', '')
outstring[x] = outstring[x].replace(')', '')
return outstring
imu = MPU9150('X')
fuse = Fusion()
# Choose test to run
Calibrate = True
Timing = False
def getmag(): # Return (x, y, z) tuple (blocking read)
return imu.mag.xyz
if Calibrate:
print("Calibrating. Press switch when done.")
sw = pyb.Switch()
fuse.calibrate(getmag, sw, lambda : pyb.delay(100))
print(fuse.magbias)
if Timing:
mag = imu.mag.xyz # Don't include blocking read in time
accel = imu.accel.xyz # or i2c
gyro = imu.gyro.xyz
start = pyb.micros()
fuse.update(accel, gyro, mag) # 1.65mS on Pyboard
t = pyb.elapsed_micros(start)
print("Update time (uS):", t)
count = 0
while True:
fuse.update(imu.accel.xyz, imu.gyro.xyz, imu.mag.xyz) # Note blocking mag read
if count % 50 == 0:
def TimeDiff(start, end):
return (start - end) / 1000000
# Expect a timestamp. Use supplied differencing function.
fuse = Fusion(TimeDiff)
def getmag(): # Return (x, y, z) magnetometer vector.
imudata = next(get_data)
return imudata[2]
print(intro)
fuse.calibrate(getmag, lambda: not calibrate, lambda: time.sleep(0.01))
print('Cal done. Magnetometer bias vector:', fuse.magbias)
print('Heading Pitch Roll')
count = 0
while True:
try:
data = next(get_data)
except StopIteration: # A file is finite.
break # A real app would probably run forever.
fuse.update(*data)
if count % 25 == 0:
print("{:8.3f} {:8.3f} {:8.3f}".format(fuse.heading, fuse.pitch,
fuse.roll))
time.sleep(0.02)
count += 1
def rotate(self, q):
q1 = np.quaternion(q[0], q[1], q[2], q[3])
q2 = np.conjugate(q1)
v = np.quaternion(0, self.x, self.z, self.y)
rot = (q1 * v) * q2
return Nodes(rot.x, rot.z, rot.y)
def project(self, win_width, win_height, fov, viewer_distance):
transformation = [120, 220]
factor = fov / (viewer_distance + self.z)
x = self.x * factor + win_width / 1.6
y = -self.y * factor + win_height / 1.6
return Nodes(x - transformation[0], y - transformation[1], self.z)
if __name__ == '__main__':
i = 200
get_data = gdata()
print('Intro')
while (i > 0):
getmag = next(get_data)
fuse.calibrate(getmag)
i -= 1
print('Cal done. Magnetometer bias vector:', fuse.magbias, fuse.scale)
get_centre = init_quat()
centre = next(get_centre)
fuse.set_centre(centre)
print("Centre initialised")
main()
if is_micropython:
def TimeDiff(start, end):
return time.ticks_diff(start, end)/1000000
else: # Cpython cheat: test data does not roll over
def TimeDiff(start, end):
return (start - end)/1000000
# Expect a timestamp. Use supplied differencing function.
fuse = Fusion(TimeDiff)
def getmag(): # Return (x, y, z) magnetometer vector.
imudata = next(get_data)
return imudata[2]
print(intro)
fuse.calibrate(getmag, lambda : not calibrate, lambda : time.sleep(0.01))
print('Cal done. Magnetometer bias vector:', fuse.magbias)
print('Heading Pitch Roll')
count = 0
while True:
try:
data = next(get_data)
except StopIteration: # A file is finite.
break # A real app would probably run forever.
fuse.update(*data)
if count % 25 == 0:
print("{:8.3f} {:8.3f} {:8.3f}".format(fuse.heading, fuse.pitch, fuse.roll))
time.sleep(0.02)
count += 1
def calibrate(mag):
fuse = Fusion()
print("Calibrating. Press switch when done.")
sw = switch
fuse.calibrate(mag, sw, lambda: time.sleep(0.1))
print(fuse.magbias)
