Mercurial > public > hwos_code
annotate src/compass_calib.c @ 223:940155e9aefb
PC config, 1.72 release
| author | heinrichsweikamp |
|---|---|
| date | Wed, 14 Jan 2015 11:37:19 +0100 |
| parents | a4bff632e97b |
| children | 7d9edd3b8c86 |
| rev | line source |
|---|---|
| 0 | 1 #include "compass.h" |
| 2 | |
| 3 static unsigned short int compass_N; | |
| 4 | |
| 5 static float Su, Sv, Sw; | |
| 6 static float Suu, Svv, Sww, Suv, Suw, Svw; | |
| 7 static float Suuu, Svvv, Swww; | |
| 8 static float Suuv, Suuw, Svvu, Svvw, Swwu, Swwv; | |
| 9 | |
| 10 ////////////////////////////////////////////////////////////////////////////// | |
| 11 // mH: Crude work-around, needs to be made right | |
| 12 #ifndef UNIX | |
| 13 # pragma udata overlay bank8=0x800 | |
| 14 static char C_STACK[256]; // Overlay C-code data stack here. | |
| 15 # define RESET_C_STACK \ | |
| 16 _asm \ | |
| 17 LFSR 1, 0x800 \ | |
| 18 LFSR 2, 0x800 \ | |
| 19 _endasm | |
| 20 # pragma udata overlay bank9_compass | |
| 21 #else | |
| 22 # define RESET_C_STACK | |
| 23 #endif | |
| 24 | |
| 25 ////////////////////////////////////////////////////////////////////////////// | |
| 26 | |
| 27 void compass_reset_calibration() | |
| 28 { | |
| 29 RESET_C_STACK; | |
| 30 | |
| 31 compass_N = 0; | |
| 32 Su = Sv = Sw = 0.0; | |
| 33 Suu = Svv = Sww = Suv = Suw = Svw = 0.0; | |
| 34 Suuu = Svvv = Swww = 0.0; | |
| 35 Suuv = Suuw = Svvu = Svvw = Swwu = Swwv = 0.0; | |
|
96
a4bff632e97b
auto-reset compass filtering data before calibration
heinrichsweikamp
parents:
0
diff
changeset
|
36 compass_CX_f = compass_CY_f = compass_CZ_f = 0.0; |
| 0 | 37 } |
| 38 | |
| 39 void compass_add_calibration() | |
| 40 { | |
| 41 OVERLAY float u, v, w; | |
| 42 RESET_C_STACK; | |
| 43 | |
| 44 u = (compass_DX_f - compass_CX_f) / 32768.0f; | |
| 45 v = (compass_DY_f - compass_CY_f) / 32768.0f; | |
| 46 w = (compass_DZ_f - compass_CZ_f) / 32768.0f; | |
| 47 | |
| 48 compass_N++; | |
| 49 Su += u; | |
| 50 Sv += v; | |
| 51 Sw += w; | |
| 52 Suv += u*v; | |
| 53 Suw += u*w; | |
| 54 Svw += v*w; | |
| 55 Suu += u*u; | |
| 56 Suuu += u*u*u; | |
| 57 Suuv += v*u*u; | |
| 58 Suuw += w*u*u; | |
| 59 Svv += v*v; | |
| 60 Svvv += v*v*v; | |
| 61 Svvu += u*v*v; | |
| 62 Svvw += w*v*v; | |
| 63 Sww += w*w; | |
| 64 Swww += w*w*w; | |
| 65 Swwu += u*w*w; | |
| 66 Swwv += v*w*w; | |
| 67 } | |
| 68 | |
| 69 ////////////////////////////////////////////////////////////////////////////// | |
| 70 | |
| 71 void compass_solve_calibration() | |
| 72 { | |
| 73 OVERLAY float yu, yv, yw; | |
| 74 OVERLAY float delta; | |
| 75 OVERLAY float uc, vc, wc; | |
| 76 RESET_C_STACK; | |
| 77 | |
| 78 //---- Normalize partial sums -------------------------------------------- | |
| 79 // | |
| 80 // u, v, w should be centered on the mean value um, vm, wm: | |
| 81 // x = u + um, with um = Sx/N | |
| 82 // | |
| 83 // So: | |
| 84 // (u + um)**2 = u**2 + 2u*um + um**2 | |
| 85 // Su = 0, um = Sx/N | |
| 86 // Sxx = Suu + 2 um Su + N*(Sx/N)**2 = Suu + Sx**2/N | |
| 87 // Suu = Sxx - Sx**2/N | |
| 88 yu = Su/compass_N; | |
| 89 yv = Sv/compass_N; | |
| 90 yw = Sw/compass_N; | |
| 91 | |
| 92 Suu -= Su*yu; | |
| 93 Svv -= Sv*yv; | |
| 94 Sww -= Sw*yw; | |
| 95 | |
| 96 // (u + um)(v + vm) = uv + u vm + v um + um vm | |
| 97 // Sxy = Suv + N * um vm | |
| 98 // Suv = Sxy - N * (Sx/N)(Sy/N); | |
| 99 Suv -= Su*yv; | |
| 100 Suw -= Su*yw; | |
| 101 Svw -= Sv*yw; | |
| 102 | |
| 103 // (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3 | |
| 104 // Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3 | |
| 105 // Su = 0, um = Sx/N: | |
| 106 // Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3 | |
| 107 // = Sxxx - 3 Sx*Suu/N - Sx**3/N**2 | |
| 108 | |
| 109 // (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm) | |
| 110 // Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm) | |
| 111 // | |
| 112 // Su = 0, Sv = 0, vm = Sy/N: | |
| 113 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm | |
| 114 // | |
| 115 // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy | |
| 116 // = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N | |
| 117 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N | |
| 118 Suuu -= (3*Suu + Su*yu)*yu; | |
| 119 Suuv -= (Suu + Su*yu)*yv + 2*Suv*yu; | |
| 120 Suuw -= (Suu + Su*yu)*yw + 2*Suw*yu; | |
| 121 | |
| 122 Svvu -= (Svv + Sv*yv)*yu + 2*Suv*yv; | |
| 123 Svvv -= (3*Svv + Sv*yv)*yv; | |
| 124 Svvw -= (Svv + Sv*yv)*yw + 2*Svw*yv; | |
| 125 | |
| 126 Swwu -= (Sww + Sw*yw)*yu + 2*Suw*yw; | |
| 127 Swwv -= (Sww + Sw*yw)*yv + 2*Svw*yw; | |
| 128 Swww -= (3*Sww + Sw*yw)*yw; | |
| 129 | |
| 130 //---- Solve the system -------------------------------------------------- | |
| 131 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2 | |
| 132 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2 | |
| 133 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2 | |
| 134 // Note this is symetric, with a positiv diagonal, hence | |
| 135 // it always have a uniq solution. | |
| 136 yu = 0.5f * (Suuu + Svvu + Swwu); | |
| 137 yv = 0.5f * (Suuv + Svvv + Swwv); | |
| 138 yw = 0.5f * (Suuw + Svvw + Swww); | |
| 139 delta = Suu * (Svv * Sww - Svw * Svw) | |
| 140 - Suv * (Suv * Sww - Svw * Suw) | |
| 141 + Suw * (Suv * Svw - Svv * Suw); | |
| 142 | |
| 143 uc = (yu * (Svv * Sww - Svw * Svw) | |
| 144 - yv * (Suv * Sww - Svw * Suw) | |
| 145 + yw * (Suv * Svw - Svv * Suw) )/delta; | |
| 146 vc = (Suu * ( yv * Sww - yw * Svw) | |
| 147 - Suv * ( yu * Sww - yw * Suw) | |
| 148 + Suw * ( yu * Svw - yv * Suw) )/delta; | |
| 149 wc = (Suu * (Svv * yw - Svw * yv ) | |
| 150 - Suv * (Suv * yw - Svw * yu ) | |
| 151 + Suw * (Suv * yv - Svv * yu ) )/delta; | |
| 152 | |
| 153 // Back to uncentered coordinates: | |
| 154 // xc = um + uc | |
| 155 uc = Su/compass_N + compass_CX_f/32768.0f + uc; | |
| 156 vc = Sv/compass_N + compass_CY_f/32768.0f + vc; | |
| 157 wc = Sw/compass_N + compass_CZ_f/32768.0f + wc; | |
| 158 | |
| 159 // Then save the new calibrated center: | |
| 160 compass_CX_f = (short)(32768 * uc); | |
| 161 compass_CY_f = (short)(32768 * vc); | |
| 162 compass_CZ_f = (short)(32768 * wc); | |
| 163 } | |
| 164 | |
| 165 ////////////////////////////// TEST CODE ///////////////////////////////////// | |
| 166 | |
| 167 #ifdef TEST_COMPASS_CALIBRATION | |
| 168 | |
| 169 #include <QtDebug> | |
| 170 #include <stdio.h> | |
| 171 | |
| 172 #include <math.h> | |
| 173 #include <stdlib.h> | |
| 174 | |
| 175 short compass_DX_f, compass_DY_f, compass_DZ_f; | |
| 176 short compass_CX_f, compass_CY_f, compass_CZ_f; | |
| 177 | |
| 178 inline float uniform() { | |
| 179 return (rand() & 0xFFFF) / 65536.0f; | |
| 180 } | |
| 181 inline float sqr(float x) { | |
| 182 return x*x; | |
| 183 } | |
| 184 | |
| 185 static const float radius = 0.21f; | |
| 186 static const float cx = 0.79f, cy = -0.46f, cz = 0.24f; | |
| 187 // const float cx = 0, cy = 0, cz = 0; | |
| 188 | |
| 189 void check_compass_calib() | |
| 190 { | |
| 191 | |
| 192 // Starts with no calibration at all: | |
| 193 compass_CX_f = compass_CY_f = compass_CZ_f = 0; | |
| 194 | |
| 195 // Try 10 recalibration passes: | |
| 196 for(int p=0; p<10; ++p) | |
| 197 { | |
| 198 compass_reset_calibration(); | |
| 199 | |
| 200 //---- Generates random points on a sphere ------------------------------- | |
| 201 // of radius,center (cx, cy, cz): | |
| 202 for(int i=0; i<100; ++i) | |
| 203 { | |
| 204 float theta = uniform()*360.0f; | |
| 205 float phi = uniform()*180.0f - 90.0f; | |
| 206 | |
| 207 float x = cx + radius * cosf(phi)*cosf(theta); | |
| 208 float y = cy + radius * cosf(phi)*sinf(theta); | |
| 209 float z = cz + radius * sinf(phi); | |
| 210 | |
| 211 compass_DX_f = short(32768 * x); | |
| 212 compass_DY_f = short(32768 * y); | |
| 213 compass_DZ_f = short(32768 * z); | |
| 214 compass_add_calibration(); | |
| 215 } | |
| 216 | |
| 217 compass_solve_calibration(); | |
| 218 qDebug() << "Center =" | |
| 219 << compass_CX_f/32768.0f | |
| 220 << compass_CY_f/32768.0f | |
| 221 << compass_CZ_f/32768.0f; | |
| 222 | |
| 223 float r2 = sqr(compass_CX_f/32768.0f - cx) | |
| 224 + sqr(compass_CY_f/32768.0f - cy) | |
| 225 + sqr(compass_CZ_f/32768.0f - cz); | |
| 226 if( r2 > 0.01f*0.01f ) | |
| 227 qWarning() << " calibration error: " << sqrtf(r2); | |
| 228 } | |
| 229 } | |
| 230 #endif // TEST_COMPASS_CALIBRATION |