--- a/src/compass.c	Thu Oct 11 21:06:29 2018 +0200
+++ b/src/compass.c	Thu Nov 22 19:47:26 2018 +0100
@@ -1,6 +1,7 @@
 //////////////////////////////////////////////////////////////////////////////
 /// compass.c
-/// Compute north direction from magnetic/acceleration measures.
+/// Compute north direction from magnetic/acceleration measures
+///
 /// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved.
 //////////////////////////////////////////////////////////////////////////////
 // HISTORY
@@ -12,189 +13,197 @@
 #include "compass.h"
 
 //////////////////////////////////////////////////////////////////////////////
-// mH: Crude work-around, needs to be made right
+// mH: crude work-around, needs to be fixed up
 #ifndef UNIX
-#   pragma udata overlay bank8=0x800
-    static char	  C_STACK[256];  // Overlay C-code data stack here.
-#       define RESET_C_STACK    \
-        _asm                    \
-            LFSR    1, 0x800    \
-            LFSR    2, 0x800    \
-        _endasm
-#   pragma udata overlay bank9_compass
-#   pragma code compass_run
+#	pragma udata overlay bank8=0x800
+	static char C_STACK[256];			// overlay C-code data stack here
+#	define RESET_C_STACK		\
+		_asm					\
+			LFSR	1, 0x800	\
+			LFSR	2, 0x800	\
+		_endasm
+#	pragma udata overlay bank9_compass
+#	pragma code compass_run
 #else
-#   define RESET_C_STACK
+#	define RESET_C_STACK
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error
 //
-#define K1  (5701)  // Needs K1/2**16
-#define K2  (1645)  // Needs K2/2**48  WAS NEGATIV
-#define K3  ( 446)  // Needs K3/2**80
+#define K1	(5701)			// needs K1/2**16
+#define K2	(1645)			// needs K2/2**48  WAS NEGATIV
+#define K3	( 446)			// needs K3/2**80
+
 
 //////////////////////////////////////////////////////////////////////////////
-// Interface to assembleur multiplies
+// Interface to assembler multiplies
+
 Int16 umul(PARAMETER Int16 a, PARAMETER Int16 b)
 {
-    extern Int16 compass_umul(void);
-    extern Int16 compass_a, compass_b;
-    compass_a = a;
-    compass_b = b;
-    return compass_umul();
+	extern Int16 compass_umul(void);
+	extern Int16 compass_a, compass_b;
+	compass_a = a;
+	compass_b = b;
+	return compass_umul();
 }
 
 Int16 imul(PARAMETER Int16 a, PARAMETER Int16 b)
 {
-    extern Int16 compass_imul(void);
-    extern Int16 compass_a, compass_b;
-    compass_a = a;
-    compass_b = b;
-    return compass_imul();
+	extern Int16 compass_imul(void);
+	extern Int16 compass_a, compass_b;
+	compass_a = a;
+	compass_b = b;
+	return compass_imul();
 }
 
 //////////////////////////////////////////////////////////////////////////////
 /// Returns a / b * 2**16
 ///
-/// A 16/16 -> 16 bits divide, returning a scalled result.
+/// A 16/16 -> 16 bits divide, returning a scaled result.
 /// Used to multiply fractional numbers in the range 0..1,
 /// represented as 0..32767.
+
 Int16 udiv(PARAMETER Int16 a, PARAMETER Int16 b)
 {
-    OVERLAY Int16 d, r;
+	OVERLAY Int16 d, r;
 
-    //---- Pre-scale both numerator and denominator --------------------------
-    while( (((a>>8) | (b>>8)) & 0xC0) == 0 )
-    {
-        a <<= 1;
-        b <<= 1;
-    }
+	//---- Pre-scale both numerator and denominator --------------------------
+	while( (((a>>8) | (b>>8)) & 0xC0) == 0 )
+	{
+		a <<= 1;
+		b <<= 1;
+	}
 
-    //---- Make division trials ----------------------------------------------
-    d = 0x4000;             // Starts with 0.5, because 1.0 is sign bit.
-    b >>= 1;                // Hence pre-shift b.
-    r = 0;
-    do {
-        if( a >= b ) {      // a is big enough ?
-            a -= b;         // then count d times b out of it.
-            r |= d;         // and accumulate that bit.
-        }
-        b >>= 1;            // then loop trying twice smaller.
-        d >>= 1;
-    } while( b );
-    return r;
+	//---- Make division trials ----------------------------------------------
+	d = 0x4000;				// start with 0.5, because 1.0 is sign bit
+	b >>= 1;				// hence pre-shift b
+	r = 0;
+	do {
+		if( a >= b )		// a is big enough ?
+		{
+			a -= b;			// then count d times b out of it
+			r |= d;			// and accumulate that bit
+		}
+		b >>= 1;			// then loop trying twice smaller
+		d >>= 1;
+	} while( b );
+	return r;
 }
 
 //////////////////////////////////////////////////////////////////////////////
 /// Computes atan(y/x) in Angle, for x, y in range 0..32767
 ///
 /// Results a single quadrant Angle, in the range 0 .. Q_PI/2
+
 Angle utan(PARAMETER Int16 y, PARAMETER Int16 x)
 {
-    OVERLAY Int16 ratio, angle, x2, x3;
+	OVERLAY Int16 ratio, angle, x2, x3;
 
-    //---- Handle zero divisor -----------------------------------------------
-    if( x == 0 )
-        return (y == 0) ? 0 : Q_PIO2;
+	//---- Handle zero divisor -----------------------------------------------
+	if( x == 0 )
+		return (y == 0) ? 0 : Q_PIO2;
 
-    //---- Make it half-quadrant : 0 .. 45 deg -------------------------------
-    ratio = (x > y) ? udiv(y, x) : udiv(x, y);
+	//---- Make it half-quadrant : 0 .. 45 deg -------------------------------
+	ratio = (x > y) ? udiv(y, x) : udiv(x, y);
 
-    //---- Then apply the polynomial approximation ---------------------------
-    angle = umul(K1, ratio);            // r*K1 / 2**16
-    x2 = umul(ratio, ratio);            // r**2 / 2**16
-    x3 = umul(x2, ratio);               // r**3 / 2**32
-    angle -= umul(x3, K2);              // K2*r**3 / 2**48: NEGATIV.
+	//---- Then apply the polynomial approximation ---------------------------
+	angle = umul(K1, ratio);			// r*K1 / 2**16
+	x2 = umul(ratio, ratio);			// r**2 / 2**16
+	x3 = umul(x2, ratio);				// r**3 / 2**32
+	angle -= umul(x3, K2);				// K2*r**3 / 2**48: NEGATIV.
 
-    x3 = umul(x3, x2);                  // r**5 / 2**64
-    angle += umul(x3, K3);              // K3*r**5 / 2**80
+	x3 = umul(x3, x2);					// r**5 / 2**64
+	angle += umul(x3, K3);				// K3*r**5 / 2**80
 
-    //---- Recover the full quadrant -----------------------------------------
-    return (x < y) ? (Angle)(Q_PIO2 - angle)
-                   : (Angle)(angle);
+	//---- Recover the full quadrant -----------------------------------------
+	return (x < y) ? (Angle)(Q_PIO2 - angle)
+				   : (Angle)(angle);
 }
 
 //////////////////////////////////////////////////////////////////////////////
 /// Computes atan2(y/x) in Angle, for x, y in range -32768 to 32767
 ///
 /// Results a four quadrant Angle, in the range -Q_PI .. +Q_PI
+
 Angle itan(PARAMETER Int16 y, PARAMETER Int16 x)
 {
-    // Beware: -32768 is not properly handled (sgn error).
-    if( x == -32768 ) x = -32767;
-    if( y == -32768 ) y = -32767;
+	// Beware: -32768 is not properly handled (sign error)
+	if( x == -32768 ) x = -32767;
+	if( y == -32768 ) y = -32767;
 
-    if( x >= 0 )
-        if( y >= 0 )    // First quadrant: 0..90 deg.
-            return utan(y,x);
-        else            // Fourth quadrant: 0..-90 deg
-            return -utan(-y,x);
-    else
-        if( y >= 0 )    // Second quadrant: 90..180 deg
-            return Q_PI - utan(y, -x);
-        else            // Third quadrant: -90..-180 deg;
-            return -Q_PI + utan(-y, -x);
+	if( x >= 0 )
+		if( y >= 0 )					// first  quadrant: 0..90 deg.
+			return utan(y,x);
+		else							// fourth quadrant: 0..-90 deg
+			return -utan(-y,x);
+	else
+		if( y >= 0 )					// second quadrant: 90..180 deg
+			return Q_PI - utan(y, -x);
+		else							// third  quadrant: -90..-180 deg;
+			return -Q_PI + utan(-y, -x);
 }
 
 //////////////////////////////////////////////////////////////////////////////
 /// Computes cos(theta) = sqrtf(x2/h2),
 /// when theta = atan(y/x) and h2=x*x+y*y
 ///
+
 Int16 cosxh(PARAMETER Int16 x2, PARAMETER Int16 h2)
 {
-    OVERLAY Int16 r = 0;
-    OVERLAY Int16 d = 0x4000;
+	OVERLAY Int16 r = 0;
+	OVERLAY Int16 d = 0x4000;
 
-    do {
-        OVERLAY Int16 a = r + d;
-        a = umul(a, a);
-        a = umul(a, h2);
-        if( a <= x2 ) r += d;
-        d >>= 1;
-    } while( d );
+	do {
+		OVERLAY Int16 a = r + d;
+		a = umul(a, a);
+		a = umul(a, h2);
+		if( a <= x2 ) r += d;
+		d >>= 1;
+	} while( d );
 
-    return r;
+	return r;
 }
 
 //////////////////////////////////////////////////////////////////////////////
 /// Computes both sin and cos of angle y/x,
 /// with h = sqrt(x**2+y**2).
 ///
+
 void sincos(PARAMETER Int16 x, PARAMETER Int16 y, Int16* sin, Int16* cos)
 {
-    OVERLAY Int16 x2, y2, h2;
+	OVERLAY Int16 x2, y2, h2;
 
-    //---- Fold into one quadant ---------------------------------------------
-    OVERLAY char neg = 0;
-    if( x < 0 )
-    {
-        neg |= 1;
-        x = -x;
-    }
-    if( y < 0 )
-    {
-        neg |= 2;
-        y = -y;
-    }
+	//---- Fold into one quadant ---------------------------------------------
+	OVERLAY char neg = 0;
+	if( x < 0 )
+	{
+		neg |= 1;
+		x = -x;
+	}
+	if( y < 0 )
+	{
+		neg |= 2;
+		y = -y;
+	}
 
-    //---- Pre-scale both numerator and denominator ----------------------
-    while( (((x>>8) | (y>>8)) & 0xE0) == 0 )
-    {
-        x <<= 1;
-        y <<= 1;
-    }
+	//---- Pre-scale both numerator and denominator ----------------------
+	while( (((x>>8) | (y>>8)) & 0xE0) == 0 )
+	{
+		x <<= 1;
+		y <<= 1;
+	}
 
-    //---- Uses trig() to do the stuff one on quadrant -------------------
-    x2 = umul(x,x);
-    y2 = umul(y,y);
-    h2 = x2 + y2;
-    x2 = cosxh(x2, h2);
+	//---- Uses trig() to do the stuff one on quadrant -------------------
+	x2 = umul(x,x);
+	y2 = umul(y,y);
+	h2 = x2 + y2;
+	x2 = cosxh(x2, h2);
 
-    //---- Results back in four quadrants --------------------------------
-    *cos = (neg & 1) ? -x2 : x2;
-    y2 = cosxh(y2, h2);
-    *sin = (neg & 2) ? -y2 : y2;
+	//---- Results back in four quadrants --------------------------------
+	*cos = (neg & 1) ? -x2 : x2;
+	y2   = cosxh(y2, h2);
+	*sin = (neg & 2) ? -y2 : y2;
 }
 
 //////////////////////////////////////////////////////////////////////////////
@@ -202,47 +211,48 @@
 
 void compass(void)
 {
-    OVERLAY Int16 sin, cos;
-    OVERLAY Int16 iBfx, iBfy, Gz;
-    OVERLAY Int16 iBpx, iBpy, iBpz;
-    RESET_C_STACK;
+	OVERLAY Int16 sin, cos;
+	OVERLAY Int16 iBfx, iBfy, Gz;
+	OVERLAY Int16 iBpx, iBpy, iBpz;
+
+	RESET_C_STACK;
 
-    //---- Make hard iron correction -----------------------------------------
-    // Measured magnetometer orientation, measured ok.
-    // From matthias drawing: (X,Y,Z) --> (X,Y,Z) : no rotation.
-    iBpx = compass_DX_f - compass_CX_f; // X
-    iBpy = compass_DY_f - compass_CY_f; // Y
-    iBpz = compass_DZ_f - compass_CZ_f; // Z
+	//---- Make hard iron correction -----------------------------------------
+	// Measured magnetometer orientation, measured ok
+	// From matthias drawing: (X,Y,Z) --> (X,Y,Z) : no rotation
+	iBpx = compass_DX_f - compass_CX_f; // X
+	iBpy = compass_DY_f - compass_CY_f; // Y
+	iBpz = compass_DZ_f - compass_CZ_f; // Z
 
-    //---- Calculate sine and cosine of roll angle Phi -----------------------
-    sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
+	//---- Calculate sine and cosine of roll angle Phi -----------------------
+	sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
 
-    //---- rotate by roll angle (-Phi) ---------------------------------------
-    iBfy = imul(iBpy, cos) - imul(iBpz, sin);
-    iBpz = imul(iBpy, sin) + imul(iBpz, cos);
-    Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos);
+	//---- rotate by roll angle (-Phi) ---------------------------------------
+	iBfy = imul(iBpy, cos) - imul(iBpz, sin);
+	iBpz = imul(iBpy, sin) + imul(iBpz, cos);
+	Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos);
 
-    //---- calculate sin and cosine of pitch angle Theta ---------------------
-    sincos(Gz, -accel_DX_f, &sin, &cos);     // NOTE: changed sin sign.
+	//---- calculate sin and cosine of pitch angle Theta ---------------------
+	sincos(Gz, -accel_DX_f, &sin, &cos);	 // NOTE: changed sin sign
 
-    /* correct cosine if pitch not in range -90 to 90 degrees */
-    if( cos < 0 ) cos = -cos;
+	/* correct cosine if pitch not in range -90 to 90 degrees */
+	if( cos < 0 ) cos = -cos;
 
-    ///---- de-rotate by pitch angle Theta -----------------------------------
-    iBfx = imul(iBpx, cos) + imul(iBpz, sin);
+	///---- de-rotate by pitch angle Theta -----------------------------------
+	iBfx = imul(iBpx, cos) + imul(iBpz, sin);
 
-    //---- Detect uncalibrated compass ---------------------------------------
-    if( !compass_CX_f && !compass_CY_f && !compass_CZ_f )
-    {
-        compass_heading = -1;
-        return;
-    }
+	//---- Detect uncalibrated compass ---------------------------------------
+	if( !compass_CX_f && !compass_CY_f && !compass_CZ_f )
+	{
+		compass_heading = -1;
+		return;
+	}
 
-    //---- calculate current yaw = e-compass angle Psi -----------------------
-    // Result in degree (no need of 0.01 deg precision...
-    compass_heading = itan(-iBfy, iBfx) / 100;
+	//---- calculate current yaw = e-compass angle Psi -----------------------
+	// Result in degree (no need of 0.01 deg precision...
+	compass_heading = itan(-iBfy, iBfx) / 100;
 
-    // Result in 0..360 range:
-    if( compass_heading < 0 )
-        compass_heading += 360;
+	// Result in 0..360 range:
+	if( compass_heading < 0 )
+		compass_heading += 360;
 }