V0.1 Alpha

peterhinch · peterhinch · commit e3961b15714b · 2015-05-21T07:45:50.000+01:00
diff --git a/README.md b/README.md
@@ -1,2 +1,43 @@
 # micropython-fusion
-Sensor fusion calculating yaw, pitch and roll from the outputs of motion tracking devices
+Sensor fusion calculating yaw, pitch and roll from the outputs of motion tracking devices. This
+uses the Madgwick algorithm, widely used in multicopter designs for its speed and quality. An
+update takes about 1.6mS on the Pyboard
+
+### Fusion Class
+
+The module supports this one class. A Fusion object needs to be periodically updated with data
+from the sensor. When queried by the ``angle`` method, it returns yaw, pitch and roll values in
+degrees. Note that if you use a 6 degrees of freedom (DOF) sensor, yaw will be invalid.
+
+Methods
+-------
+
+```angle()```
+Return yaw, pitch and roll in degrees
+
+```update(accel, gyro, mag)```
+For 9DOF sensors. Accepts 3-tuples (x, y, z) of accelerometer, gyro and magnetometer data and
+updates the filters. This should be called periodically, depending on the required response
+speed. Units:
+accel: Typically G. Values are normalised in the algorithm so units are irrelevant.
+gyro: Degrees per second.
+magnetometer: Microteslas.
+
+```update_nomag(accel, gyro)```
+For 6DOF sensors. Accepts 3-tuples (x, y, z) of accelerometer and gyro data and
+updates the filters. This should be called periodically, depending on the required response
+speed. Units:
+accel: Typically G. Values are normalised in the algorithm so units are irrelevant.
+gyro: Degrees per second.
+
+### Notes for beginners
+
+If you're designing a machine using a motion sensing device consider the effects of vibration.
+This can be surprisingly high (use the sensor to measure it). No amount of digital filtering
+will remove it because it is likely to contain frequencies above the sampling rate of the sensor:
+these will be aliased down into the filter passband and affect the results. It's normally
+neccessary to isolate the sensor with a mechanical filter, typically a mass supported on very
+soft rubber mounts.
+
+Update rate - TODO
+
diff --git a/fusion.py b/fusion.py
@@ -0,0 +1,283 @@
+# Sensor fusion for the micropython board. 21st May 2015
+# Ported to Python by Peter Hinch
+# V0.1 Experimental
+
+import pyb 
+from math import sqrt, atan2, asin, degrees, radians
+'''
+Supports 6 and 9 degrees of freedom sensors. Tested with InvenSense MPU-9150 9DOF sensor.
+Source https://github.com/xioTechnologies/Open-Source-AHRS-With-x-IMU.git
+also https://github.com/kriswiner/MPU-9250.git
+Ported to Python. Integrator timing adapted for pyboard.
+User should repeatedly call the appropriate update method and extract the yaw pitch and roll angles as
+reuired using the angles method.
+'''
+class Fusion(object):
+    '''
+    Class provides sensor fusion allowing yaw, pitch and roll to be extracted. This uses the Madgwick algorithm.
+    The update method must be called peiodically. The calculations take 1.6mS on the Pyboard. Suggested update
+    rate 50Hz (20mS)
+    '''
+    magbias = [0, 0, 0]                     # local bias factors: average value of each axis when rotated 2*pi radians
+    declination = 0                         # Option to add offset for true north
+    def __init__(self):
+        self.start_time = None              # Record time between updates
+        self.q = [1.0, 0.0, 0.0, 0.0]       # vector to hold quaternion
+        GyroMeasError = radians(40)         # Original code indicates this leads to a 2 sec response time
+        self.beta = sqrt(3.0 / 4.0) * GyroMeasError  # compute beta
+
+    def angles(self):
+        yaw   = degrees(atan2(2.0 * (self.q[1] * self.q[2] + self.q[0] * self.q[3]),
+            self.q[0] * self.q[0] + self.q[1] * self.q[1] - self.q[2] * self.q[2] - self.q[3] * self.q[3]))
+        pitch = degrees(-asin(2.0 * (self.q[1] * self.q[3] - self.q[0] * self.q[2])))
+        roll  = degrees(atan2(2.0 * (self.q[0] * self.q[1] + self.q[2] * self.q[3]),
+            self.q[0] * self.q[0] - self.q[1] * self.q[1] - self.q[2] * self.q[2] + self.q[3] * self.q[3]))
+        return yaw + self.declination, pitch, roll
+
+    def update(self, accel, gyro, mag):     # 3-tuples (x, y, z) for accel, gyro and mag data
+        mx, my, mz = (mag[x] - self.magbias[x] for x in range(3)) # Units uT
+        ax, ay, az = accel                  # Units G (but later normalised)
+        gx, gy, gz = (radians(x) for x in gyro)  # Units deg/s
+        if self.start_time is None:
+            self.start_time = pyb.micros()  # First run
+        q1, q2, q3, q4 = (self.q[x] for x in range(4))   # short name local variable for readability
+        # Auxiliary variables to avoid repeated arithmetic
+        _2q1 = 2.0 * q1
+        _2q2 = 2.0 * q2
+        _2q3 = 2.0 * q3
+        _2q4 = 2.0 * q4
+        _2q1q3 = 2.0 * q1 * q3
+        _2q3q4 = 2.0 * q3 * q4
+        q1q1 = q1 * q1
+        q1q2 = q1 * q2
+        q1q3 = q1 * q3
+        q1q4 = q1 * q4
+        q2q2 = q2 * q2
+        q2q3 = q2 * q3
+        q2q4 = q2 * q4
+        q3q3 = q3 * q3
+        q3q4 = q3 * q4
+        q4q4 = q4 * q4
+
+        # Normalise accelerometer measurement
+        norm = sqrt(ax * ax + ay * ay + az * az)
+        if (norm == 0.0):
+            return # handle NaN
+        norm = 1.0/norm
+        ax *= norm
+        ay *= norm
+        az *= norm
+
+        # Normalise magnetometer measurement
+        norm = sqrt(mx * mx + my * my + mz * mz)
+        if (norm == 0.0):
+            return # handle NaN
+        norm = 1.0/norm
+        mx *= norm
+        my *= norm
+        mz *= norm
+
+        # Reference direction of Earth's magnetic field
+        _2q1mx = 2.0 * q1 * mx
+        _2q1my = 2.0 * q1 * my
+        _2q1mz = 2.0 * q1 * mz
+        _2q2mx = 2.0 * q2 * mx
+        hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4
+        hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4
+        _2bx = sqrt(hx * hx + hy * hy)
+        _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4
+        _4bx = 2.0 * _2bx
+        _4bz = 2.0 * _2bz
+
+        # Gradient descent algorithm corrective step
+        s1 = (-_2q3 * (2.0 * q2q4 - _2q1q3 - ax) + _2q2 * (2.0 * q1q2 + _2q3q4 - ay) - _2bz * q3 * 
+            (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * 
+            (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + 
+            _2bz * (0.5 - q2q2 - q3q3) - mz))
+        s2 = (_2q4 * (2.0 * q2q4 - _2q1q3 - ax) + _2q1 * (2.0 * q1q2 + _2q3q4 - ay) - 4.0 * q2 * 
+            (1.0 - 2.0 * q2q2 - 2.0 * q3q3 - az) + _2bz * q4 * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * 
+            (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * 
+            (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
+        s3 = (-_2q1 * (2.0 * q2q4 - _2q1q3 - ax) + _2q4 * (2.0 * q1q2 + _2q3q4 - ay) - 4.0 * q3 * 
+            (1.0 - 2.0 * q2q2 - 2.0 * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5 - q3q3 - q4q4) + 
+            _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + 
+            _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
+        s4 = (_2q2 * (2.0 * q2q4 - _2q1q3 - ax) + _2q3 * (2.0 * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) *
+            (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) *
+            (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) +
+            _2bz * (0.5 - q2q2 - q3q3) - mz))
+        norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4)    # normalise step magnitude
+        norm = 1.0/norm
+        s1 *= norm
+        s2 *= norm
+        s3 *= norm
+        s4 *= norm
+
+        # Compute rate of change of quaternion
+        qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1
+        qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2
+        qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3
+        qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4
+
+        # Integrate to yield quaternion
+        deltat = pyb.elapsed_micros(self.start_time) / 1000000
+        self.start_time = pyb.micros()
+        q1 += qDot1 * deltat
+        q2 += qDot2 * deltat
+        q3 += qDot3 * deltat
+        q4 += qDot4 * deltat
+        norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4)    # normalise quaternion
+        norm = 1.0/norm
+        self.q = q1 * norm, q2 * norm, q3 * norm, q4 * norm
+
+    def update_nomag(self, accel, gyro):    # 3-tuples (x, y, z) for accel, gyro
+        ax, ay, az = accel                  # Units G (but later normalised)
+        gx, gy, gz = (radians(x) for x in gyro) # Units deg/s
+        if self.start_time is None:
+            self.start_time = pyb.micros()  # First run
+        q1, q2, q3, q4 = (self.q[x] for x in range(4))   # short name local variable for readability
+        # Auxiliary variables to avoid repeated arithmetic
+        _2q1 = 2 * q1
+        _2q2 = 2 * q2
+        _2q3 = 2 * q3
+        _2q4 = 2 * q4
+        _4q1 = 4 * q1
+        _4q2 = 4 * q2
+        _4q3 = 4 * q3
+        _8q2 = 8 * q2
+        _8q3 = 8 * q3
+        q1q1 = q1 * q1
+        q2q2 = q2 * q2
+        q3q3 = q3 * q3
+        q4q4 = q4 * q4
+
+        # Normalise accelerometer measurement
+        norm = sqrt(ax * ax + ay * ay + az * az)
+        if (norm == 0):
+            return # handle NaN
+        norm = 1 / norm        # use reciprocal for division
+        ax *= norm
+        ay *= norm
+        az *= norm
+
+        # Gradient decent algorithm corrective step
+        s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay
+        s2 = _4q2 * q4q4 - _2q4 * ax + 4 * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az
+        s3 = 4 * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az
+        s4 = 4 * q2q2 * q4 - _2q2 * ax + 4 * q3q3 * q4 - _2q3 * ay
+        norm = 1 / sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4)    # normalise step magnitude
+        s1 *= norm
+        s2 *= norm
+        s3 *= norm
+        s4 *= norm
+
+        # Compute rate of change of quaternion
+        qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1
+        qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2
+        qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3
+        qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4
+
+        # Integrate to yield quaternion
+        deltat = pyb.elapsed_micros(self.start_time) / 1000000
+        self.start_time = pyb.micros()
+        q1 += qDot1 * deltat
+        q2 += qDot2 * deltat
+        q3 += qDot3 * deltat
+        q4 += qDot4 * deltat
+        norm = 1 / sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4)    # normalise quaternion
+        self.q = q1 * norm, q2 * norm, q3 * norm, q4 * norm
+
+    def Update(self, accel, gyro, mag):     # 3-tuples (x, y, z) for accel, gyro and mag data
+        mx, my, mz = (mag[x] - self.magbias[x] for x in range(3)) # Units uT
+        ax, ay, az = accel                  # Units G (but later normalised)
+        gx, gy, gz = (radians(x) for x in gyro)  # Units deg/s
+        if self.start_time is None:
+            self.start_time = pyb.micros()  # First run
+        q1, q2, q3, q4 = (self.q[x] for x in range(4))   # short name local variable for readability
+        # Auxiliary variables to avoid repeated arithmetic
+        _2q1 = 2 * q1
+        _2q2 = 2 * q2
+        _2q3 = 2 * q3
+        _2q4 = 2 * q4
+        _2q1q3 = 2 * q1 * q3
+        _2q3q4 = 2 * q3 * q4
+        q1q1 = q1 * q1
+        q1q2 = q1 * q2
+        q1q3 = q1 * q3
+        q1q4 = q1 * q4
+        q2q2 = q2 * q2
+        q2q3 = q2 * q3
+        q2q4 = q2 * q4
+        q3q3 = q3 * q3
+        q3q4 = q3 * q4
+        q4q4 = q4 * q4
+
+        # Normalise accelerometer measurement
+        norm = sqrt(ax * ax + ay * ay + az * az)
+        if (norm == 0):
+            return # handle NaN
+        norm = 1 / norm                     # use reciprocal for division
+        ax *= norm
+        ay *= norm
+        az *= norm
+
+        # Normalise magnetometer measurement
+        norm = sqrt(mx * mx + my * my + mz * mz)
+        if (norm == 0):
+            return                          # handle NaN
+        norm = 1 / norm                     # use reciprocal for division
+        mx *= norm
+        my *= norm
+        mz *= norm
+
+        # Reference direction of Earth's magnetic field
+        _2q1mx = 2 * q1 * mx
+        _2q1my = 2 * q1 * my
+        _2q1mz = 2 * q1 * mz
+        _2q2mx = 2 * q2 * mx
+        hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4
+        hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4
+        _2bx = sqrt(hx * hx + hy * hy)
+        _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4
+        _4bx = 2 * _2bx
+        _4bz = 2 * _2bz
+
+        # Gradient descent algorithm corrective step
+        s1 = (-_2q3 * (2 * q2q4 - _2q1q3 - ax) + _2q2 * (2 * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5 - q3q3 - q4q4)
+             + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
+             + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
+
+        s2 = (_2q4 * (2 * q2q4 - _2q1q3 - ax) + _2q1 * (2 * q1q2 + _2q3q4 - ay) - 4 * q2 * (1 - 2 * q2q2 - 2 * q3q3 - az)
+             + _2bz * q4 * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4)
+             + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
+
+        s3 = (-_2q1 * (2 * q2q4 - _2q1q3 - ax) + _2q4 * (2 * q1q2 + _2q3q4 - ay) - 4 * q3 * (1 - 2 * q2q2 - 2 * q3q3 - az)
+             + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx)
+             + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
+             + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
+
+        s4 = (_2q2 * (2 * q2q4 - _2q1q3 - ax) + _2q3 * (2 * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5 - q3q3 - q4q4)
+              + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
+              + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
+
+        norm = 1 / sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4)    # normalise step magnitude
+        s1 *= norm
+        s2 *= norm
+        s3 *= norm
+        s4 *= norm
+
+        # Compute rate of change of quaternion
+        qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1
+        qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2
+        qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3
+        qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4
+
+        # Integrate to yield quaternion
+        deltat = pyb.elapsed_micros(self.start_time) / 1000000
+        self.start_time = pyb.micros()
+        q1 += qDot1 * deltat
+        q2 += qDot2 * deltat
+        q3 += qDot3 * deltat
+        q4 += qDot4 * deltat
+        norm = 1 / sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4)    # normalise quaternion
+        self.q = q1 * norm, q2 * norm, q3 * norm, q4 * norm
diff --git a/fusionlcd.py b/fusionlcd.py
@@ -0,0 +1,47 @@
+import gc
+from mpu9150 import MPU9150
+from fusion import Fusion
+from usched import Sched, wait, Poller
+from lcdthread import LCD, PINLIST          # Library supporting Hitachi LCD module
+
+imu = MPU9150('Y', 1, True)
+imu.gyro_range(0)
+imu.accel_range(0)
+
+fuse = Fusion()
+
+def makefilt(delta, T = None):              # Build a single pole IIR filter with time conatant T
+    if T is None:                           # For testing
+        alpha, beta = 0, 1
+    else:
+        alpha = 1 - delta/T
+        beta = 1 - alpha
+    output = 0
+    def runfilt(inp):
+        nonlocal output
+        output = alpha * output + beta * inp
+        return output
+    return runfilt
+
+gc.disable()
+# In systems with a continuously running loop, to avoid occasional delays
+# of around 2mS do a manual gc on each pass
+
+def lcd_thread(mylcd, imu):
+    mylcd[0] = "{:8s}{:8s}{:8s}".format("Yaw","Pitch","Roll")
+    count = 0
+    while True:
+        yield from wait(0.02) # IMU updates at 50Hz
+        count += 1
+        if count % 25 == 0:
+            yaw, pitch, roll = fuse.angles()
+            mylcd[1] = "{:7.0f} {:7.0f} {:7.0f}".format(yaw, pitch, roll)
+        fuse.Update(imu.get_accel(), imu.get_gyro(), imu.get_mag()) # Note blocking mag read
+#        fuse.update_nomag(imu.get_accel(), imu.get_gyro())
+        gc.collect()
+
+objSched = Sched()
+lcd0 = LCD(PINLIST, objSched, cols = 24)
+objSched.add_thread(lcd_thread(lcd0, imu))
+objSched.run()
+gc.enable()
diff --git a/fusiontest.py b/fusiontest.py
