fusion_r_syn.py

# fusion_r_syn.py Test for sensor fusion remote device
# simulated by captured data mpudata
# Author Peter Hinch
# Released under the MIT License (MIT)
# Copyright (c) 2018 Peter Hinch
# Run under MicroPython on Unix or other target, or CPython 3.4 or later

try:
    import utime as time
except ImportError:
    import time
try:
    import ujson as json
except ImportError:
    import json
from fusion import Fusion
from deltat import is_micropython

intro = '''
This demo reads data from a file created by recording IMU data from a Pyboard
equipped with an IMU device. The data (in JSON format) contains the 3 IMU
vectors followed by a timestamp in μs.

Initially the board was rotated around each orthogonal axis to calibrate the
magnetometer. A special line was inserted into the file to signal the end of
cal.

The device was then partially rotated in heading, then pitch, then roll: this
data is displayed when calibration is complete.

Calibration takes 8 seconds.
'''

# gdata produces synthetic remote data from a file.
# In an application this would do a blocking data read from a remote device.
# The change in calibration mode might be returned from the remote as here or
# handled at the fusion device by user input.

# Initially we're in cal mode
calibrate = True
def gdata():
    global calibrate
    # Return [[ax, ay, az], [gx, gy, gz], [mx, my, mz], timestamp]
    # from whatever the device supplies (in this case JSON)
    with open('mpudata', 'r') as f:
        line = f.readline()  # An app would do a blocking read of remote data
        while line:
            if line.strip() == 'cal_end':
                calibrate = False
            else:
                yield json.loads(line)  # Convert foreign data format.
            line = f.readline()  # Blocking read.

get_data = gdata()

# Test of supplying a timediff
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