# HG changeset patch
# User jDG
# Date 1432299040 -7200
# Node ID 7d9edd3b8c86d1d662b89e3dde834d998bce50ea
# Parent  eb758a5b44eb52e9692d00c4d390aa4ae863b20a
Make a more compact COMPASS calibration code (<7KB), and add more tests.

diff -r eb758a5b44eb -r 7d9edd3b8c86 .hgignore
--- a/.hgignore	Tue May 26 17:40:13 2015 +0200
+++ b/.hgignore	Fri May 22 14:50:40 2015 +0200
@@ -4,3 +4,4 @@
 tools/o3pack.exe
 tools/o3pack_büro.bat
 hwos.X*
+src/Tests/build/
diff -r eb758a5b44eb -r 7d9edd3b8c86 src/Tests/compass_test.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Tests/compass_test.cpp	Fri May 22 14:50:40 2015 +0200
@@ -0,0 +1,132 @@
+//////////////////////////////////////////////////////////////////////////////
+/// compass_test.cpp
+/// Unit test for compass calibration.
+/// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved.
+//////////////////////////////////////////////////////////////////////////////
+//  HISTORY
+// 2015-05-23 jDG: Rewrite compass testing, to allow reducing code size.
+
+extern "C" {
+#   include "compass.h"
+}
+
+#include <gtest/gtest.h>
+
+#include <math.h>
+#include <iostream>
+
+//////////////////////////////////////////////////////////////////////////////
+
+inline float uniform() {
+    return (rand() & 0xFFFF) / 65536.0f;
+}
+inline float sqr(float x) {
+    return x*x;
+}
+
+static float radius = 0.21f;
+static float cx = 0, cy = 0, cz = 0;
+
+//////////////////////////////////////////////////////////////////////////////
+
+static void check_calib()
+{
+    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();
+    float r2 = sqr(compass_CX_f/32768.0f - cx)
+             + sqr(compass_CY_f/32768.0f - cy)
+             + sqr(compass_CZ_f/32768.0f - cz);
+
+    // Calibration error less than 2 bits:
+    EXPECT_NEAR(0, sqrtf(r2), 3.0f/32768.0f)
+        << "Center at (" << compass_CX_f/32768.0f << ", "
+                         << compass_CY_f/32768.0f << ", "
+                         << compass_CZ_f/32768.0f << ")."
+        << " Error = " << sqrtf(r2);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+TEST(compass, calibration_centered)
+{
+    compass_CX_f = compass_CY_f = compass_CZ_f = 0;
+
+    // Half-unit, centered, sphere:
+    radius = 0.5f;
+
+    // Try 10 recalibration passes:
+    for(int p=0; p<10; ++p)
+        check_calib();
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+TEST(compass, calibration_near_centered)
+{
+    // Put magnetic center elsewhere, but keep position+radius < 1.0, to
+    // avoid Q15 overflow...
+    radius = 0.21f;
+    cx = 0.019f, cy = -0.026f, cz = 0.004f;
+
+    // Try 10 recalibration passes:
+    for(int p=0; p<10; ++p)
+        check_calib();
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+TEST(compass, calibration_far_centered)
+{
+    // Put magnetic center elsewhere, but keep position+radius < 1.0, to
+    // avoid Q15 overflow...
+    radius = 0.21f;
+    cx = -0.79f, cy = 0.79f, cz = 0.79f;
+
+    // Try 10 recalibration passes:
+    for(int p=0; p<10; ++p)
+        check_calib();
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+TEST(compass, calibration_small_centered_signal)
+{
+    // use a very very small magnetic signal, centered:
+    radius = 0.001f;
+    cx = 0.000f, cy = 0.000f, cz = 0.000f;
+
+    // Try 10 recalibration passes:
+    for(int p=0; p<10; ++p)
+        check_calib();
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+TEST(compass, calibration_small_off_centered_signal)
+{
+    // Have a rather small sphere radius (20%), off-centered buy 80%
+    radius = 0.200f;
+    cx = 0.800f, cy = -0.800f, cz = 0.800f;
+
+    // Try 10 recalibration passes:
+    for(int p=0; p<10; ++p)
+        check_calib();
+}
diff -r eb758a5b44eb -r 7d9edd3b8c86 src/Tests/compass_test.pro
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Tests/compass_test.pro	Fri May 22 14:50:40 2015 +0200
@@ -0,0 +1,36 @@
+#-----------------------------------------------------------------------------
+#
+# Project created by QtCreator 2013-03-29T10:58:23
+#
+#-----------------------------------------------------------------------------
+
+TEMPLATE = app
+TARGET = compass_test
+
+CONFIG   *= qt
+CONFIG   -= app_bundle
+
+QT       *= core
+QT       -= gui
+
+DEFINES *= UNIX
+
+#-----------------------------------------------------------------------------
+# Need the GoogleTest 1.6.0 library here:
+GTEST=$$PWD/../../../../Dependencies/gtest-1.6.0
+!exists($$GTEST/include): error(Requires GoogleTest 1.6.0)
+INCLUDEPATH *= $$GTEST/include $$GTEST/gtest-1.6.0
+SOURCES *= $$GTEST/gtest-1.6.0/src/gtest-all.cc
+win32: DEFINES *= _VARIADIC_MAX=10
+
+#-----------------------------------------------------------------------------
+SOURCES += \
+    $$PWD/../compass.c \
+    $$PWD/../compass_calib.c \
+    compass_trigo_test.cpp \
+    compass_test.cpp \
+    main_test.cpp
+
+INCLUDEPATH *= $$PWD/..
+HEADERS += \
+    $$PWD/../compass.h
diff -r eb758a5b44eb -r 7d9edd3b8c86 src/Tests/compass_trigo_test.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Tests/compass_trigo_test.cpp	Fri May 22 14:50:40 2015 +0200
@@ -0,0 +1,140 @@
+//////////////////////////////////////////////////////////////////////////////
+/// compass_trigo_test.cpp
+/// Unit test for compass various operations.
+/// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved.
+//////////////////////////////////////////////////////////////////////////////
+//  HISTORY
+// 2015-05-23 jDG: Rewrite compass testing, to allow reducing code size.
+
+extern "C" {
+#   include "compass.h"
+}
+
+#include <gtest/gtest.h>
+
+//////////////////////////////////////////////////////////////////////////////
+// Fake assembleur fixed point multiplies.
+extern "C" Int16 compass_umul(void);
+extern "C" Int16 compass_imul(void);
+extern "C" Int16 compass_a, compass_b;
+
+// The (filtered) components of the magnetometer sensor:
+Int16 compass_DX_f;
+Int16 compass_DY_f;
+Int16 compass_DZ_f;
+
+// Found soft-iron calibration values, deduced from already filtered values.
+Int16 compass_CX_f;
+Int16 compass_CY_f;
+Int16 compass_CZ_f;
+
+// The (filtered) components of the accelerometer sensor:
+Int16 accel_DX_f;
+Int16 accel_DY_f;
+Int16 accel_DZ_f;
+
+// The compass result value.
+Int16 compass_heading;
+Int16 compass_roll;
+Int16 compass_pitch;
+
+Int16 compass_a, compass_b;
+Int16 compass_umul()
+{
+    unsigned int a = compass_a;
+    unsigned int b = compass_b;
+    a *= b;
+    a >>= 15;
+    return (Int16)a;
+}
+
+Int16 compass_imul()
+{
+    int a = compass_a;
+    int b = compass_b;
+    a *= b;
+    a >>= 15;
+    return (Int16)a;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+TEST(ops, multiply) {
+    // Check basic sign handling:
+    EXPECT_EQ(umul( 8000,  4000), (Int16)0x03D0);   // 8000/2**15 * 4000/2**15 = 0x3D0
+    EXPECT_EQ(imul(-8000,  4000), (Int16)0xFC2F);   // -976 = 0xFC2F
+    EXPECT_EQ(imul( 8000, -4000), (Int16)0xFC2F);   // -976 = 0xFC2F
+    EXPECT_EQ(imul(-8000, -4000), (Int16)0x03D0);   // +976 = 0x3D0
+}
+
+TEST(ops, divide) {
+    // Check basic divides:
+    EXPECT_EQ(udiv(32000, 32001), (Int16)32766);     // 0.99997 ~ 32766
+    EXPECT_EQ(udiv( 4000,  8000), (Int16)16384);
+    EXPECT_EQ(udiv( 2000,  8000), (Int16) 8192);
+    EXPECT_EQ(udiv( 1000,  8000), (Int16) 4096);
+    EXPECT_EQ(udiv(  500,  8000), (Int16) 2048);
+}
+
+TEST(trigo, atan) {
+    // Check angles returned by the SINGLE QUADRANT atan() function:
+    EXPECT_EQ(utan(100, 100), (Int16)4501);  // +1
+    EXPECT_EQ(utan( 90, 100), (Int16)4195);  // -4
+    EXPECT_EQ(utan( 80, 100), (Int16)3864);  // -2
+    EXPECT_EQ(utan( 70, 100), (Int16)3500);  // +1
+    EXPECT_EQ(utan( 60, 100), (Int16)3099);  // +3
+    EXPECT_EQ(utan( 50, 100), (Int16)2658);  // +1
+    EXPECT_EQ(utan( 40, 100), (Int16)2179);  // -1
+    EXPECT_EQ(utan( 30, 100), (Int16)1667);  // -3
+    EXPECT_EQ(utan( 20, 100), (Int16)1127);  // -4
+    EXPECT_EQ(utan( 10, 100), (Int16) 569);  // -2
+    EXPECT_EQ(utan(  0, 100), (Int16)   0);
+}
+
+TEST(trigo, cosx2h2) {
+    // Check ONE-OCTANT pseudo-cosinus function
+    // Note: cosxh(x**2, x**2+y**2) is computing cos(atan(y/x))
+    EXPECT_EQ(cosxh(12769, 13169), (Int16)32268);   // 113,  20 --> 10.0369° --> 32267 +1
+    EXPECT_EQ(cosxh(10000, 12500), (Int16)29310);   // 100,  50 --> 26.5650° --> 29309 +1
+    EXPECT_EQ(cosxh(10000, 20000), (Int16)23171);   // 100, 100 --> 45.0000° --> 23170 +1
+    EXPECT_EQ(cosxh( 2500, 12500), (Int16)14658);   //  50, 100 --> 63.4349° --> 14654 +4
+    EXPECT_EQ(cosxh(  400, 13169), (Int16) 5718);   //  20, 113 --> 79.9631° -->  5711 +7
+}
+
+TEST(trigo, sinCos) {
+    Int16 sin, cos;
+
+    //---- Check sincos() FIRST QUADRANT ---------------------------------
+    sincos( 20, 113, &sin, &cos);   // 80°
+    EXPECT_EQ(sin, (Int16)32269);   // +2
+    EXPECT_EQ(cos, (Int16) 5727);   // +16
+
+    sincos( 50, 100, &sin, &cos);   // 63°
+    EXPECT_EQ(sin, (Int16)29311);   // +2
+    EXPECT_EQ(cos, (Int16)14660);   // +6
+
+    sincos(100, 100, &sin, &cos);   // 45°
+    EXPECT_EQ(sin, (Int16)23173);   // +3
+    EXPECT_EQ(cos, (Int16)23173);   // +3
+
+    sincos(100,  50, &sin, &cos);   // 27°
+    EXPECT_EQ(sin, (Int16)14660);   // +6
+    EXPECT_EQ(cos, (Int16)29311);   // +2
+
+    sincos(113,  20, &sin, &cos);   // 10°
+    EXPECT_EQ(sin, (Int16) 5727);   // +16
+    EXPECT_EQ(cos, (Int16)32269);   // +2
+
+    //---- Check sincos() OTHER QUADRANTS --------------------------------
+    sincos(-20, 113, &sin, &cos);   // 90+80°
+    EXPECT_EQ(sin, (Int16) 32269);  // +2
+    EXPECT_EQ(cos, (Int16) -5727);  // +16
+
+    sincos(-20,-113, &sin, &cos);   // 180+80°
+    EXPECT_EQ(sin, (Int16)-32269);  // +2
+    EXPECT_EQ(cos, (Int16) -5727);  // +16
+
+    sincos( 20,-113, &sin, &cos);   // 270+80°
+    EXPECT_EQ(sin, (Int16)-32269);  // +2
+    EXPECT_EQ(cos, (Int16)  5727);  // +16
+}
diff -r eb758a5b44eb -r 7d9edd3b8c86 src/Tests/main_test.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Tests/main_test.cpp	Fri May 22 14:50:40 2015 +0200
@@ -0,0 +1,16 @@
+//////////////////////////////////////////////////////////////////////////////
+/// main_test.cpp
+/// Launch all defined GoogleTest unit tests defined.
+/// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved.
+//////////////////////////////////////////////////////////////////////////////
+//  HISTORY
+// 2015-05-23 jDG: Rewrite compass testing, to allow reducing code size.
+
+#include <gtest/gtest.h>
+
+int main(int argc, char *argv[])
+{
+    ::testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}
+
diff -r eb758a5b44eb -r 7d9edd3b8c86 src/compass.c
--- a/src/compass.c	Tue May 26 17:40:13 2015 +0200
+++ b/src/compass.c	Fri May 22 14:50:40 2015 +0200
@@ -1,8 +1,13 @@
+//////////////////////////////////////////////////////////////////////////////
+/// compass.c
+/// Compute north direction from magnetic/acceleration measures.
+/// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved.
 //////////////////////////////////////////////////////////////////////////////
 // HISTORY
 //  2012-12-01  [jDG] Creation
 //  2012-12-23  [jDG] Added filtering.
 //  2012-12-30  [jDG] Added calibration (spherical best fit).
+//  2015-05-22  [jDG] Minor cleanups. Smaller calibration code.
 
 #include "compass.h"
 
@@ -17,8 +22,9 @@
             LFSR    2, 0x800    \
         _endasm
 #   pragma udata overlay bank9_compass
+#   pragma code compass_run
 #else
-#       define RESET_C_STACK
+#   define RESET_C_STACK
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -210,7 +216,6 @@
 
     //---- Calculate sine and cosine of roll angle Phi -----------------------
     sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
-//    compass_roll = itan(sin, cos) / 100;
 
     //---- rotate by roll angle (-Phi) ---------------------------------------
     iBfy = imul(iBpy, cos) - imul(iBpz, sin);
@@ -219,7 +224,6 @@
 
     //---- calculate sin and cosine of pitch angle Theta ---------------------
     sincos(Gz, -accel_DX_f, &sin, &cos);     // NOTE: changed sin sign.
-//    compass_pitch = itan(sin, cos) / 100;
 
     /* correct cosine if pitch not in range -90 to 90 degrees */
     if( cos < 0 ) cos = -cos;
diff -r eb758a5b44eb -r 7d9edd3b8c86 src/compass.h
--- a/src/compass.h	Tue May 26 17:40:13 2015 +0200
+++ b/src/compass.h	Fri May 22 14:50:40 2015 +0200
@@ -50,3 +50,11 @@
 extern Angle itan(PARAMETER Int16 a, PARAMETER Int16 b);
 extern Angle cosxh(PARAMETER Int16 a, PARAMETER Int16 b);
 extern void  sincos(PARAMETER Int16 a, PARAMETER Int16 b, Int16* sin, Int16* cos);
+
+//////////////////////////////////////////////////////////////////////////////
+// The user functions
+extern void compass(void);
+extern void compass_reset_calibration(void);
+extern void compass_add_calibration(void);
+
+extern void compass_solve_calibration(void);
diff -r eb758a5b44eb -r 7d9edd3b8c86 src/compass_calib.c
--- 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
diff -r eb758a5b44eb -r 7d9edd3b8c86 src/compass_ops.asm
--- a/src/compass_ops.asm	Tue May 26 17:40:13 2015 +0200
+++ b/src/compass_ops.asm	Fri May 22 14:50:40 2015 +0200
@@ -33,7 +33,7 @@
     extern  compass_add_calibration
     extern  compass_solve_calibration
 
-compass code
+compass_ops code
 ;-----------------------------------------------------------------------------
 ; Filter compass values
 ;
diff -r eb758a5b44eb -r 7d9edd3b8c86 tools/crop_hwos_hex.bat
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tools/crop_hwos_hex.bat	Fri May 22 14:50:40 2015 +0200
@@ -0,0 +1,3 @@
+:: Crops hex file from MPLAP X to work with Tinybld
+:: Downloaded from http://srecord.sourceforge.net/download.html
+srec_cat ../hwos.X/dist/default/production/hwos.X.production.hex -intel -crop 0x00000 0x1E000 -o hwos_tiny.hex -intel
\ No newline at end of file