--- /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
+}