--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/compass_calib.c	Wed Apr 24 19:22:45 2013 +0200
@@ -0,0 +1,229 @@
+#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
+#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
+#else
+#       define RESET_C_STACK
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+
+void compass_reset_calibration()
+{
+    RESET_C_STACK;
+
+    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;
+}
+
+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;
+
+    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;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+void compass_solve_calibration()
+{
+    OVERLAY float yu, yv, yw;
+    OVERLAY float delta;
+    OVERLAY float uc, vc, wc;
+    RESET_C_STACK;
+
+    //---- Normalize partial sums --------------------------------------------
+    //
+    // u, v, w should be centered on the mean value um, vm, wm:
+    // x = u + um, with um = Sx/N
+    //
+    // 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;
+
+    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 + 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
+    // Su = 0, um = Sx/N:
+    // Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3
+    //      = Sxxx - 3 Sx*Suu/N - Sx**3/N**2
+
+    // (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm)
+    // Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm)
+    //
+    // Su = 0, Sv = 0, vm = Sy/N:
+    // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm
+    //
+    // 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;
+
+    //---- 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;
+
+    // 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 /////////////////////////////////////
+
+#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;
+}
+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