Test programs fixed for new MPU9x50 interface. Yaw replaced with Heading.

Author: peterhinch

README.md

@@ -1,6 +1,6 @@
 # micropython-fusion
 
-Sensor fusion calculating yaw, pitch and roll from the outputs of motion tracking devices. This
+Sensor fusion calculating heading, 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. The original Madgwick study indicated that an update
 rate of 10-50Hz was adequate for accurate results, suggesting that the performance of this
@@ -18,8 +18,8 @@ if my observations about coordinate conventions are incorrect.
 ## Fusion class
 
 The module supports this one class. A Fusion object needs to be periodically updated with data
-from the sensor. It provides yaw, pitch and roll values (in degrees) as properties. Note that
-if you use a 6 degrees of freedom (DOF) sensor, yaw will be invalid.
+from the sensor. It provides heading, pitch and roll values (in degrees) as properties. Note that
+if you use a 6 degrees of freedom (DOF) sensor, heading will be invalid.
 
 ### Methods
 
@@ -57,11 +57,10 @@ runs, the aim being to compensate for offsets caused by static local magnetic fi
 
 Three read-only properties provide access to the angles. These are in degrees.
 
-**yaw**
+**heading**
 
-Angle relative to North. Better terminology is "heading" (used in the original Madgwick study)
-since it is ground referenced: yaw is also used to mean the angle of an aircraft's fuselage
-relative to its direction of motion.
+Angle relative to North. Note some sources use the term "yaw". As this is also used to mean
+the angle of an aircraft's fuselage relative to its direction of motion, I have avoided it.
 
 **pitch**
 
@@ -103,11 +102,12 @@ GC time shorter and ensures it occurs at a predictable time. See the MicroPython
 
 These are blatantly plagiarised as this isn't my field. I have quoted sources.
 
-### Yaw Pitch and Roll
+### Heading Pitch and Roll
 
 Perhaps better titled heading, elevation and bank: there seems to be ambiguity about the concept
 of yaw, whether this is measured relative to the aircraft's local coordinate system or that of
-the Earth. The angles emitted by the Madgwick algorithm (Tait-Bryan angles) are earth-relative.  
+the Earth: the original Madgwick study uses the term "heading", a convention I have retained as
+the angles emitted by the Madgwick algorithm (Tait-Bryan angles) are earth-relative.  
 See [Wikipedia article](http://en.wikipedia.org/wiki/Euler_angles#Tait.E2.80.93Bryan_angles)
 
 The following adapted from https://github.com/kriswiner/MPU-9250.git  

fusion.py

@@ -1,8 +1,9 @@
-# Sensor fusion for the micropython board. 29th May 2015
+# Sensor fusion for the micropython board. 25th June 2015
 # Ported to MicroPython/Pyboard by Peter Hinch.
+# V0.7 Yaw replaced with heading
 # V0.65 waitfunc now optional
 # V0.6 calibrate altered to work round MicroPython map() bug, waitfunc added
-# V0.5 angles method replaced by yaw pitch and roll properties
+# V0.5 angles method replaced by heading pitch and roll properties
 # V0.4 calibrate method added
 
 import pyb 
@@ -12,7 +13,7 @@
 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 6 or 9 DOF update method and extract yaw pitch and roll angles as
+User should repeatedly call the appropriate 6 or 9 DOF update method and extract heading pitch and roll angles as
 required.
 Calibrate method:
 The sensor should be slowly rotated around each orthogonal axis while this runs.
@@ -24,10 +25,10 @@
 '''
 class Fusion(object):
     '''
-    Class provides sensor fusion allowing yaw, pitch and roll to be extracted. This uses the Madgwick algorithm.
+    Class provides sensor fusion allowing heading, 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.
     '''
-    declination = 0                         # Optional offset for true north. A +ve value adds to yaw
+    declination = 0                         # Optional offset for true north. A +ve value adds to heading
     def __init__(self):
         self.magbias = (0, 0, 0)            # local magnetic bias factors: set from calibration
         self.start_time = None              # Time between updates
@@ -48,7 +49,7 @@ def calibrate(self, getxyz, stopfunc, waitfunc = None):
         self.magbias = tuple(map(lambda a, b: (a +b)/2, magmin, magmax))
 
     @property
-    def yaw(self):
+    def heading(self):
         return self.declination + 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]))
 

fusionlcd.py

@@ -1,6 +1,7 @@
 # Fusionlcd.py Test for sensor fusion on Pyboard. Uses LCD display and threaded library.
 # Author Peter Hinch
-# V0.65 2nd June 2015
+# For libraries see https://github.com/peterhinch/Micropython-scheduler.git
+# V0.7 25th June 2015 Adapted for new MPU9x50 interface
 
 import pyb
 from mpu9150 import MPU9150
@@ -20,9 +21,7 @@
 """
 switch = pyb.Pin('Y7', pyb.Pin.IN, pull=pyb.Pin.PULL_UP) # Switch to ground on Y7
 
-imu = MPU9150('X', 1, True)                # Attached to 'X' bus, 1 device, disable interruots
-imu.gyro_range(0)
-imu.accel_range(0)
+imu = MPU9150('X')                # Attached to 'X' bus, 1 device, disable interruots
 
 fuse = Fusion()
 
@@ -34,16 +33,16 @@ def lcd_thread(mylcd, imu, sw):
         mylcd[0] = "Calibrate. Push switch"
         mylcd[1] = "when done"
         yield from wait(0.1)
-        fuse.calibrate(imu.get_mag, lambda : not sw.value(), waitfunc)
+        fuse.calibrate(imu.mag.xyz, lambda : not sw.value(), waitfunc)
         print(fuse.magbias)
     mylcd[0] = "{:5s}{:5s} {:5s}".format("Yaw","Pitch","Roll")
     count = 0
     while True:
         yield from wait(0.02) # IMU updates at 50Hz
         count += 1
         if count % 25 == 0:
-            mylcd[1] = "{:4.0f} {:4.0f}  {:4.0f}".format(fuse.yaw, fuse.pitch, fuse.roll)
-        fuse.update(imu.get_accel(), imu.get_gyro(), imu.get_mag())
+            mylcd[1] = "{:4.0f} {:4.0f}  {:4.0f}".format(fuse.heading, fuse.pitch, fuse.roll)
+        fuse.update(imu.accel.xyz, imu.gyro.xyz, imu.mag.xyz)
 # For 6DOF sensors
 #        fuse.update_nomag(imu.get_accel(), imu.get_gyro())
 

fusiontest.py

@@ -1,40 +1,41 @@
 # Simple test program for sensor fusion on Pyboard
 # Author Peter Hinch
-# 29th May 2015
-# V0.6
+# V0.7 25th June 2015 Adapted for new MPU9x50 interface
+
 import pyb
 from mpu9150 import MPU9150
 from fusion import Fusion
 
-imu = MPU9150('X', 1, False)
-imu.gyro_range(0)
-imu.accel_range(0)
+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(imu.get_mag, sw, lambda : pyb.delay(100))
+    fuse.calibrate(getmag, sw, lambda : pyb.delay(100))
     print(fuse.magbias)
 
 if Timing:
-    mag = imu.get_mag() # Don't include blocking read in time
-    accel = imu.get_accel() # or i2c
-    gyro = imu.get_gyro()
+    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.get_accel(), imu.get_gyro(), imu.get_mag()) # Note blocking mag read
+    fuse.update(imu.accel.xyz, imu.gyro.xyz, imu.mag.xyz) # Note blocking mag read
     if count % 50 == 0:
-        print("Yaw, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(fuse.yaw, fuse.pitch, fuse.roll))
+        print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(fuse.heading, fuse.pitch, fuse.roll))
     pyb.delay(20)
     count += 1