--- a/src/compass_calib.c	Tue May 26 17:40:13 2015 +0200
+++ b/src/compass_calib.c	Fri May 22 14:50:40 2015 +0200
@@ -1,14 +1,14 @@
+//////////////////////////////////////////////////////////////////////////////
+/// compass_calib.c
+/// Calibrate hard-iron for magnetic compass measurements.
+/// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved.
+//////////////////////////////////////////////////////////////////////////////
+//  2015-05-22  [jDG] Make a smaller calibration code (15.6 --> 6.7 KB).
+
 #include "compass.h"
 
-static unsigned short int compass_N;
-
-static float Su, Sv, Sw;
-static float Suu, Svv, Sww, Suv, Suw, Svw;
-static float Suuu, Svvv, Swww;
-static float Suuv, Suuw, Svvu, Svvw, Swwu, Swwv;
-
 //////////////////////////////////////////////////////////////////////////////
-// mH: Crude work-around, needs to be made right
+// mH: Put compass data into bank 8 (stack) and bank 9 (variables)
 #ifndef UNIX
 #   pragma udata overlay bank8=0x800
     static char	  C_STACK[256];  // Overlay C-code data stack here.
@@ -19,11 +19,24 @@
         _endasm
 #   pragma udata overlay bank9_compass
 #else
-#       define RESET_C_STACK
+#   define RESET_C_STACK
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 
+static unsigned short int compass_N;
+
+static float Su, Sv, Sw;                    // First order moments.
+static float Suu, Svv, Sww, Suv, Suw, Svw;  // Second order moments.
+static float Saa;  // Suu + Svv + Sww
+static float Saau; // Suuu + Svvu + Swwu    // Third order moment.
+static float Saav; // Suuv + Svvv + Swwv
+static float Saaw; // Suuw + Svvw + Swww
+static float yu, yv, yw;                    // temp solution vector.
+static float uc, vc, wc;                    // temp sphere's center.
+
+//////////////////////////////////////////////////////////////////////////////
+
 void compass_reset_calibration()
 {
     RESET_C_STACK;
@@ -31,74 +44,103 @@
     compass_N = 0;
     Su = Sv = Sw = 0.0;
     Suu = Svv = Sww = Suv = Suw = Svw = 0.0;
-    Suuu = Svvv = Swww = 0.0;
-    Suuv = Suuw = Svvu = Svvw = Swwu = Swwv = 0.0;
-    compass_CX_f = compass_CY_f = compass_CZ_f = 0.0;
+    Saau = Saav = Saaw = 0.0;
+    compass_CX_f = compass_CY_f = compass_CZ_f = 0;
 }
 
+//////////////////////////////////////////////////////////////////////////////
+
 void compass_add_calibration()
 {
-    OVERLAY float u, v, w;
     RESET_C_STACK;
 
-    u = (compass_DX_f - compass_CX_f) / 32768.0f;
-    v = (compass_DY_f - compass_CY_f) / 32768.0f;
-    w = (compass_DZ_f - compass_CZ_f) / 32768.0f;
+    // Get filtered/calibrated magnetic direction:
+    yu = (compass_DX_f - compass_CX_f) / 32768.0f;
+    yv = (compass_DY_f - compass_CY_f) / 32768.0f;
+    yw = (compass_DZ_f - compass_CZ_f) / 32768.0f;
 
+    // Add to all moments:
     compass_N++;
-    Su += u;
-    Sv += v;
-    Sw += w;
-    Suv += u*v;
-    Suw += u*w;
-    Svw += v*w;
-    Suu  += u*u;
-    Suuu += u*u*u;
-    Suuv += v*u*u;
-    Suuw += w*u*u;
-    Svv  += v*v;
-    Svvv += v*v*v;
-    Svvu += u*v*v;
-    Svvw += w*v*v;
-    Sww  += w*w;
-    Swww += w*w*w;
-    Swwu += u*w*w;
-    Swwv += v*w*w;
+
+    Su += yu;
+    Sv += yv;
+    Sw += yw;
+
+    Suu += yu*yu;
+    Suv += yu*yv;
+    Suw += yu*yw;
+    Svv += yv*yv;
+    Svw += yv*yw;
+    Sww += yw*yw;
+
+    Saa = yu*yu + yv*yv + yw*yw;
+    Saau += yu * Saa;
+    Saav += yv * Saa;
+    Saaw += yw * Saa;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+static float compass_discriminent(PARAMETER char column)
+{
+    // Basic symetric matrix:
+    OVERLAY float a = Suu, d = Suv, g = Suw;
+    OVERLAY float b = Suv, e = Svv, h = Svw;
+    OVERLAY float c = Suw, f = Svw, i = Sww;
+
+    // Substitute a column, if asked to:
+    if( column==1 ) { a = yu; b = yv; c = yw; }
+    if( column==2 ) { d = yu; e = yv; f = yw; }
+    if( column==3 ) { g = yu; h = yv; i = yw; }
+
+    // Do the math:
+    return   a * (e * i - f * h)
+           - b * (d * i - f * g)
+           + c * (d * h - e * g);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+static float compass_dotc(PARAMETER float u, float v, float w)
+{
+    return u*uc + v*vc + w*wc;
 }
 
 //////////////////////////////////////////////////////////////////////////////
 
 void compass_solve_calibration()
 {
-    OVERLAY float yu, yv, yw;
     OVERLAY float delta;
-    OVERLAY float uc, vc, wc;
     RESET_C_STACK;
 
+    //---- Compute center of measured magnetic directions --------------------
+    uc = Su/compass_N;
+    vc = Sv/compass_N;
+    wc = Sw/compass_N;
+
     //---- Normalize partial sums --------------------------------------------
     //
-    // u, v, w should be centered on the mean value um, vm, wm:
-    // x = u + um, with um = Sx/N
+    // We measured the (u, v, w) values, and need the centered (x, y, z) ones
+    // around the sphere center's (uc, vc, wc) as:
+    // uc = Su / N;     The mean value
+    // x  = u - uc;     The differnce to the mean.
     //
     // So:
-    // (u + um)**2 = u**2 + 2u*um + um**2
-    // Su = 0, um = Sx/N
-    // Sxx = Suu + 2 um Su + N*(Sx/N)**2 = Suu + Sx**2/N
-    // Suu = Sxx - Sx**2/N
-    yu = Su/compass_N;
-    yv = Sv/compass_N;
-    yw = Sw/compass_N;
+    // x**2 = (u - uc)**2 = u**2 - 2u*uc + uc**2
+    //
+    // We need the Sxx sum of 2nd orders:
+    // Sxx = Suu - 2 uc Su + N*uc*(Su/N) = Suu - uc Su
+    Suu -= Su*uc;
+    Svv -= Sv*vc;
+    Sww -= Sw*wc;
 
-    Suu -= Su*yu;
-    Svv -= Sv*yv;
-    Sww -= Sw*yw;
-
-    // (u + um)(v + vm) = uv + u vm + v um + um vm
-    // Sxy = Suv + N * um vm
-    // Suv = Sxy - N * (Sx/N)(Sy/N);
-    Suv -= Su*yv;
-    Suw -= Su*yw;
-    Svw -= Sv*yw;
+    // (u - uc)(v - vc) = uv - u vc - v uc + uc vc
+    // Sxy = Suv - Su vc -   Sv uc + N uc vc
+    //     = Suv - Su vc - N vc uc + N uc vc
+    //     = Suv - Su vc
+    Suv -= Su*vc;
+    Suw -= Su*wc;
+    Svw -= Sv*wc;
 
     // (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3
     // Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3
@@ -115,116 +157,26 @@
     // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy
     //      = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N
     //      = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N
-    Suuu -= (3*Suu + Su*yu)*yu;
-    Suuv -= (Suu + Su*yu)*yv + 2*Suv*yu;
-    Suuw -= (Suu + Su*yu)*yw + 2*Suw*yu;
-
-    Svvu -= (Svv + Sv*yv)*yu + 2*Suv*yv;
-    Svvv -= (3*Svv + Sv*yv)*yv;
-    Svvw -= (Svv + Sv*yv)*yw + 2*Svw*yv;
-
-    Swwu -= (Sww + Sw*yw)*yu + 2*Suw*yw;
-    Swwv -= (Sww + Sw*yw)*yv + 2*Svw*yw;
-    Swww -= (3*Sww + Sw*yw)*yw;
+    Saa = Suu + Svv + Sww;
+    yu = Saau - Saa*uc - compass_dotc(Su*uc + 2*Suu, Sv*uc + 2*Suv, Sw*uc + 2*Suw);
+    yv = Saav - Saa*vc - compass_dotc(Su*vc + 2*Suv, Sv*vc + 2*Svv, Sw*vc + 2*Svw);
+    yw = Saaw - Saa*wc - compass_dotc(Su*wc + 2*Suw, Sv*wc + 2*Svw, Sw*wc + 2*Sww);
 
     //---- Solve the system --------------------------------------------------
     // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2
     // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2
     // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2
     // Note this is symetric, with a positiv diagonal, hence
-    // it always have a uniq solution.
-    yu = 0.5f * (Suuu + Svvu + Swwu);
-    yv = 0.5f * (Suuv + Svvv + Swwv);
-    yw = 0.5f * (Suuw + Svvw + Swww);
-    delta = Suu * (Svv * Sww - Svw * Svw)
-          - Suv * (Suv * Sww - Svw * Suw)
-          + Suw * (Suv * Svw - Svv * Suw);
-
-    uc = (yu  * (Svv * Sww - Svw * Svw)
-       -  yv  * (Suv * Sww - Svw * Suw)
-       +  yw  * (Suv * Svw - Svv * Suw) )/delta;
-    vc = (Suu * ( yv * Sww -  yw * Svw)
-       -  Suv * ( yu * Sww -  yw * Suw)
-       +  Suw * ( yu * Svw -  yv * Suw) )/delta;
-    wc = (Suu * (Svv * yw  - Svw * yv )
-       -  Suv * (Suv * yw  - Svw * yu )
-       +  Suw * (Suv * yv  - Svv * yu ) )/delta;
+    // discriminent is always not null.
+    delta = 0.5f / compass_discriminent(0);
 
-    // Back to uncentered coordinates:
-    // xc = um + uc
-    uc = Su/compass_N + compass_CX_f/32768.0f + uc;
-    vc = Sv/compass_N + compass_CY_f/32768.0f + vc;
-    wc = Sw/compass_N + compass_CZ_f/32768.0f + wc;
-
-    // Then save the new calibrated center:
-    compass_CX_f = (short)(32768 * uc);
-    compass_CY_f = (short)(32768 * vc);
-    compass_CZ_f = (short)(32768 * wc);
-}
-
-////////////////////////////// TEST CODE /////////////////////////////////////
+    // So computed new center, with offsetted values:
+    uc += compass_discriminent(1) * delta;
+    vc += compass_discriminent(2) * delta;
+    wc += compass_discriminent(3) * delta;
 
-#ifdef TEST_COMPASS_CALIBRATION
-
-#include <QtDebug>
-#include <stdio.h>
-
-#include <math.h>
-#include <stdlib.h>
-
-short compass_DX_f, compass_DY_f, compass_DZ_f;
-short compass_CX_f, compass_CY_f, compass_CZ_f;
-
-inline float uniform() {
-    return (rand() & 0xFFFF) / 65536.0f;
+    // Add correction due to already applyed calibration:
+    compass_CX_f += (short)(32768 * uc);
+    compass_CY_f += (short)(32768 * vc);
+    compass_CZ_f += (short)(32768 * wc);
 }
-inline float sqr(float x) {
-    return x*x;
-}
-
-static const float radius = 0.21f;
-static const float cx = 0.79f, cy = -0.46f, cz = 0.24f;
-// const float cx = 0, cy = 0, cz = 0;
-
-void check_compass_calib()
-{
-
-    // Starts with no calibration at all:
-    compass_CX_f = compass_CY_f = compass_CZ_f = 0;
-
-    // Try 10 recalibration passes:
-    for(int p=0; p<10; ++p)
-    {
-        compass_reset_calibration();
-
-        //---- Generates random points on a sphere -------------------------------
-        // of radius,center (cx, cy, cz):
-        for(int i=0; i<100; ++i)
-        {
-            float theta = uniform()*360.0f;
-            float phi   = uniform()*180.0f - 90.0f;
-
-            float x = cx + radius * cosf(phi)*cosf(theta);
-            float y = cy + radius * cosf(phi)*sinf(theta);
-            float z = cz + radius * sinf(phi);
-
-            compass_DX_f = short(32768 * x);
-            compass_DY_f = short(32768 * y);
-            compass_DZ_f = short(32768 * z);
-            compass_add_calibration();
-        }
-
-        compass_solve_calibration();
-        qDebug() << "Center ="
-                 << compass_CX_f/32768.0f
-                 << compass_CY_f/32768.0f
-                 << compass_CZ_f/32768.0f;
-
-        float r2 = sqr(compass_CX_f/32768.0f - cx)
-                 + sqr(compass_CY_f/32768.0f - cy)
-                 + sqr(compass_CZ_f/32768.0f - cz);
-        if( r2 > 0.01f*0.01f )
-            qWarning() << "    calibration error: " << sqrtf(r2);
-    }
-}
-#endif // TEST_COMPASS_CALIBRATION