Mercurial > public > hwos_code
changeset 282:7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
author | jDG |
---|---|
date | Fri, 22 May 2015 14:50:40 +0200 |
parents | eb758a5b44eb |
children | a2e19988e959 |
files | .hgignore src/Tests/compass_test.cpp src/Tests/compass_test.pro src/Tests/compass_trigo_test.cpp src/Tests/main_test.cpp src/compass.c src/compass.h src/compass_calib.c src/compass_ops.asm tools/crop_hwos_hex.bat |
diffstat | 10 files changed, 455 insertions(+), 163 deletions(-) [+] |
line wrap: on
line diff
--- 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/
--- /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(); +}
--- /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
--- /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 +}
--- /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(); +} +
--- 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;
--- 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);
--- 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
--- 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 ;
--- /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