Calibrate delay function now optional

Author: peterhinch

README.md

@@ -27,22 +27,22 @@ if you use a 6 degrees of freedom (DOF) sensor, yaw will be invalid.
 
 For 9DOF sensors. Accepts three 3-tuples (x, y, z) of accelerometer, gyro and magnetometer data
 and updates the filters. This should be called periodically with a frequency 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: Microtesla.
+required response speed. Units:  
+accel: Typically G. Values are normalised in the algorithm: units are irrelevant.  
+gyro: Degrees per second.  
+magnetometer: Typically Microtesla but values are normalised in the algorithm: units are irrelevant.
 
 ```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.
+speed. Units:  
+accel: Typically G. Values are normalised in the algorithm so units are irrelevant.  
+gyro: Degrees per second.  
 
-```calibrate(getxyz, stopfunc, waitfunc)```
+```calibrate(getxyz, stopfunc, waitfunc = None)```
 
-The first argument is a function returning a tuple of magnetic x,y,z values from the sensor.
+The first argument is a function returning a tuple of magnetic x,y,z values from the sensor.  
 The second is a function returning ```True``` when calibration is deemed complete: this could
 be a timer or an input from the user.  
 The third is a function providing a delay. Some hardware may require a delay between magnetometer

fusion.py

@@ -1,5 +1,6 @@
 # Sensor fusion for the micropython board. 29th May 2015
 # Ported to MicroPython/Pyboard by Peter Hinch.
+# 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.4 calibrate method added
@@ -18,8 +19,8 @@
 arguments:
 getxyz must return current magnetometer (x, y, z) tuple from the sensor
 stopfunc (responding to time or user input) tells it to stop
-waitfunc provides a delay between readings. typically pyb.delay(100) but can be altered in threaded environment
-Sets magbias to the mean values of x,y,z
+waitfunc provides an optional delay between readings to accommodate hardware or to avoid hogging
+the CPU in a threaded environment. It sets magbias to the mean values of x,y,z
 '''
 class Fusion(object):
     '''
@@ -34,11 +35,12 @@ def __init__(self):
         GyroMeasError = radians(40)         # Original code indicates this leads to a 2 sec response time
         self.beta = sqrt(3.0 / 4.0) * GyroMeasError  # compute beta (see README)
 
-    def calibrate(self, getxyz, stopfunc, waitfunc):
+    def calibrate(self, getxyz, stopfunc, waitfunc = None):
         magmax = list(getxyz())             # Initialise max and min lists with current values
         magmin = magmax[:]
         while not stopfunc():
-            waitfunc()
+            if waitfunc is not None:
+                waitfunc()
             magxyz = tuple(getxyz())
             for x in range(3):
                 magmax[x] = max(magmax[x], magxyz[x])
@@ -117,8 +119,8 @@ def update_nomag(self, accel, gyro):    # 3-tuples (x, y, z) for accel, gyro
         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)
+        mx, my, mz = (mag[x] - self.magbias[x] for x in range(3)) # Units irrelevant (normalised)
+        ax, ay, az = accel                  # Units irrelevant (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