V0.5 plus readme changes.

Author: peterhinch

README.md

@@ -1,7 +1,14 @@
 # micropython-fusion
+
 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
+update takes about 1.6mS on the Pyboard. The code is intended to be independent of the sensor
+device: testing was done with the InvenSense MPU-9150.
+
+## Disclaimer
+
+I should point out that I'm unfamiliar with aircraft conventions and would appreciate feedback
+if my observations about coordinate conventions are incorrect.
 
 # fusion module
 
@@ -15,12 +22,12 @@ if you use a 6 degrees of freedom (DOF) sensor, yaw will be invalid.
 
 ```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:
+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: Microteslas.
+magnetometer: Microtesla.
 
 ```update_nomag(accel, gyro)```
 
@@ -33,8 +40,8 @@ gyro: Degrees per second.
 ```calibrate(getxyz, stopfunc)```
 
 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: a timer or an
-input from the user. It updates the ```magbias``` property.
+The second is a function returning ```True``` when calibration is deemed complete: this could
+be a timer or an input from the user. The method updates the ```magbias``` property.
 
 Calibration is performed by rotating the unit around each orthogonal axis while the routine
 runs, the aim being to compensate for offsets caused by static local magnetic fields.
@@ -45,17 +52,19 @@ Three read-only properties provide access to the angles. These are in degrees.
 
 ```yaw```
 
-Angle relative to North (anticlockwise)
+Angle relative to North. Better terminology is "heading" since it is ground referenced: yaw
+is also used to mean the angle of an aircraft's fuselage relative to its direction of motion.
 
 ```pitch```
 
-Angle of aircraft nose relative to ground (+ve is towards ground)
+Angle of aircraft nose relative to ground (conventionally +ve is towards ground). Also known
+as "elevation".
 
 ```roll```
 
-Angle of aircraft wings to ground anticlockwise (left wing down)
+Angle of aircraft wings to ground, also known as "bank".
 
-### Notes for beginners
+### Notes for constructors
 
 If you're developing 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
@@ -64,27 +73,31 @@ these will be aliased down into the filter passband and affect the results. It's
 neccessary to isolate the sensor with a mechanical filter, typically a mass supported on very
 soft rubber mounts.
 
-You may want to take control of garbage collection (GC). In systems with continuously running
-control loops there is a case for doing an explicit GC on each iteration: this tends to make the
-GC time shorter and ensures it occurs at a predictable time. See the MicroPython ```gc``` module.
-
 If using a magnetoemeter consider the fact that the Earth's magnetic field is small: the field
 detected may be influenced by ferrous metals in the machine being controlled or by currents in
 nearby wires. If the latter are variable there is little chance of compensating for them, but
 constant magnetic offsets may be addressed by calibration. This involves rotating the machine
 around each of three orthogonal axes while running the fusion object's ```calibrate``` method.
 
-Update rate - TODO
+The local coordinate system for the sensor is usually defined as follows, assuming the vehicle
+is on the ground:  
+z Vertical axis, vector points towards ground  
+x Axis towards the front of the vehicle, vector points in normal direction of travel  
+y Vector points left from pilot's point of view (I think)  
+
+You may want to take control of garbage collection (GC). In systems with continuously running
+control loops there is a case for doing an explicit GC on each iteration: this tends to make the
+GC time shorter and ensures it occurs at a predictable time. See the MicroPython ```gc``` module.
 
 # Background notes
 
-These are blatantly plagiarised as this isn't my field, but in my defence I have quoted sources.
+These are blatantly plagiarised as this isn't my field. I have quoted sources.
 
 ### Yaw 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. I gather Tait-Bryan angles are earth-relative.  
+the Earth. The angles emitted by the Madgwick algorithm (Tait-Bryan angles) are earth-relative.  
 See http://en.wikipedia.org/wiki/Euler_angles#Tait.E2.80.93Bryan_angles
 
 The following adapted from https://github.com/kriswiner/MPU-9250.git  
@@ -97,11 +110,17 @@ is positive, up toward the sky is negative.
 Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
 These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
 Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation
-the rotations must be applied in the correct order which for this configuration is yaw, pitch, and then roll.
-For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
+the rotations must be applied in the correct order which for this configuration is yaw, pitch,
+and then roll. For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+which has additional links.
+
+I have seen sources which contradict the above directions for yaw (heading) and roll (bank).
 
 ### Beta
 
+The Madgwick algorithm has a "magic number" Beta which determines the tradeoff between accuracy
+and response speed.
+
 Source: https://github.com/kriswiner/MPU-9250.git  
 There is a tradeoff in the beta parameter between accuracy and response speed.
 In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s)

fusion.py

@@ -1,6 +1,7 @@
-# Sensor fusion for the micropython board. 22nd May 2015
-# Ported to Python by Peter Hinch
-# V0.2 Experimental. This is alpha quality code and needs further testing.
+# Sensor fusion for the micropython board. 24th May 2015
+# Ported to MicroPython/Pyboard by Peter Hinch.
+# V0.5 angles method replaced by yaw pitch and roll properties
+# V0.4 calibrate method added
 
 import pyb 
 from math import sqrt, atan2, asin, degrees, radians
@@ -15,8 +16,7 @@
 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)
+    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 (anticlockwise rotation)
     def __init__(self):
@@ -51,14 +51,6 @@ def roll(self):
         return 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]))
 
-    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_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

fusionlcd.py

@@ -12,6 +12,14 @@
 
 Calibrate = False
 
+def scale(func, template, *vecs):
+    res = []
+    for v in vecs:
+        res.append(tuple(map(func, template, v)))
+    return res
+
+rot = (-1, 1, 1)
+
 def lcd_thread(mylcd, imu):
     if Calibrate:
         sw = pyb.Switch()
@@ -27,8 +35,9 @@ def lcd_thread(mylcd, imu):
         count += 1
         if count % 25 == 0:
             mylcd[1] = "{:7.0f} {:7.0f} {:7.0f}".format(fuse.yaw, fuse.pitch, fuse.roll)
-        mx, my, mz = imu.get_mag() # Note blocking mag read
-        fuse.update(imu.get_accel(), imu.get_gyro(), (mx, my, mz))
+#        vectors = scale(lambda x, y: x*y, rot, imu.get_mag())
+        fuse.update(imu.get_accel(), imu.get_gyro(), imu.get_mag())
+#        fuse.update(imu.get_accel(), imu.get_gyro(), (mx, my, mz))
 #        fuse.update_nomag(imu.get_accel(), imu.get_gyro())
 
 objSched = Sched()

fusiontest.py

@@ -9,13 +9,23 @@
 fuse = Fusion()
 
 Calibrate = False
+Timing = True
 
 if Calibrate:
     print("Calibrating. Press switch when done.")
     sw = pyb.Switch()
     fuse.calibrate(imu.get_mag, sw)
     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()
+    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:
     pyb.delay(20)