view src/compass_calib.c @ 267:f64afa14ef07
BUGFIX: Analog sensors were ignored in 1.76
1.77 release
| author |
heinrichsweikamp |
| date |
Thu, 16 Apr 2015 11:30:00 +0200 (2015-04-16) |
| parents |
a4bff632e97b |
| children |
7d9edd3b8c86 |
line source
#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;
compass_CX_f = compass_CY_f = compass_CZ_f = 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
