Mercurial > public > hwos_code

comparison src/compass_calib.c @ 628:cd58f7fc86db

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
3.05 stable work
author heinrichsweikamp
date Thu, 19 Sep 2019 12:01:29 +0200
parents c40025d8e750
children 4050675965ea
comparison
equal deleted inserted replaced
627:bf5fee575701 628:cd58f7fc86db
6 // Copyright (c) 2012-2019, JD Gascuel, HeinrichsWeikamp, all rights reserved. 6 // Copyright (c) 2012-2019, JD Gascuel, HeinrichsWeikamp, all rights reserved.
7 // 7 //
8 ////////////////////////////////////////////////////////////////////////////// 8 //////////////////////////////////////////////////////////////////////////////
9 9
10 // 2015-05-22 [jDG] Make a smaller calibration code (15.6 --> 6.7 KB). 10 // 2015-05-22 [jDG] Make a smaller calibration code (15.6 --> 6.7 KB).
11 // 2019-05-14 [rl] make it even smaller again by another 2.000 byte 11 // 2019-05-14 [rl] make smaller again
12 12
13 #include "configuration.inc" 13 #include "configuration.inc"
14 #include "compass.h" 14 #include "compass.h"
15 15
16 16
37 37
38 ////////////////////////////////////////////////////////////////////////////// 38 //////////////////////////////////////////////////////////////////////////////
39 39
40 static unsigned short int compass_N; 40 static unsigned short int compass_N;
41 41
42 static float Su, Sv, Sw; // first order moments 42 static float Su, Sv, Sw; // first order moments
43 static float Suu, Svv, Sww, Suv, Suw, Svw; // second order moments 43 static float Suu, Svv, Sww, Suv, Suw, Svw; // second order moments
44 static float Saa; // Suu + Svv + Sww 44 static float Saa; // Suu + Svv + Sww
45 static float Saau; // Suuu + Svvu + Swwu third order moment 45 static float Saau; // Suuu + Svvu + Swwu third order moment
46 static float Saav; // Suuv + Svvv + Swwv third order moment 46 static float Saav; // Suuv + Svvv + Swwv third order moment
47 static float Saaw; // Suuw + Svvw + Swww third order moment 47 static float Saaw; // Suuw + Svvw + Swww third order moment
48 48
49 static float yu, yv, yw; // temp solution vector 49 static float yu, yv, yw; // temp solution vector
50 static float uc, vc, wc; // temp sphere's center 50 static float uc, vc, wc; // temp sphere's center
51 51
52 static float yh, uh, S0, S1, S2, S3; // transfer vars for compass_solve_helper() 52 static float yh,uh,S0,S1,S2,S3; // transfer vars for compass_solve_helper()
53 static float discriminant, delta; // transfer vars for calc_discriminant() 53
54 54 //////////////////////////////////////////////////////////////////////////////
55 55
56 ////////////////////////////////////////////////////////////////////////////// 56 void compass_reset_calibration()
57
58 #ifndef UNIX
59 # pragma code compass_calib
60 #endif
61
62 //////////////////////////////////////////////////////////////////////////////
63
64 void compass_reset_calibration() // 202 byte
65 { 57 {
66 RESET_C_STACK; 58 RESET_C_STACK;
67 59
68 compass_N = 0; 60 compass_N = 0;
69 Su = Sv = Sw = 0.0; 61 Su = Sv = Sw = 0.0;
70 Suu = Svv = Sww = 0.0; 62 Suu = Svv = Sww = 0.0;
71 Suv = Suw = Svw = 0.0; 63 Suv = Suw = Svw = 0.0;
72 Saau = Saav = Saaw = 0.0; 64 Saau = Saav = Saaw = 0.0;
73 compass_CX_f = compass_CY_f = compass_CZ_f = 0; // int16 65 compass_CX_f = compass_CY_f = compass_CZ_f = 0;
74 } 66 }
75 67
76 ////////////////////////////////////////////////////////////////////////////// 68 //////////////////////////////////////////////////////////////////////////////
77 69
78 void compass_add_calibration() // 1488 byte 70 void compass_add_calibration()
79 { 71 {
80 overlay float SQR_yu, SQR_yv, SQR_yw; // squared values;
81
82 RESET_C_STACK; 72 RESET_C_STACK;
83 73
84 // get filtered/calibrated magnetic direction // 276 byte 74 // get filtered/calibrated magnetic direction
85 yu = (compass_DX_f - compass_CX_f) / 32768.0f; 75 yu = (compass_DX_f - compass_CX_f) / 32768.0f;
86 yv = (compass_DY_f - compass_CY_f) / 32768.0f; 76 yv = (compass_DY_f - compass_CY_f) / 32768.0f;
87 yw = (compass_DZ_f - compass_CZ_f) / 32768.0f; 77 yw = (compass_DZ_f - compass_CZ_f) / 32768.0f;
88 78
89 // compute squared values // 156 byte 79 // add to all moments
90 SQR_yu = yu*yu;
91 SQR_yv = yv*yv;
92 SQR_yw = yw*yw;
93
94 // increment count
95 compass_N++; 80 compass_N++;
96 81
97 // add to all moments // 156 byte
98 Su += yu; 82 Su += yu;
99 Sv += yv; 83 Sv += yv;
100 Sw += yw; 84 Sw += yw;
101 85
102 // // 156 byte 86 Suu += yu*yu;
103 Suu += SQR_yu;
104 Svv += SQR_yv;
105 Sww += SQR_yw;
106
107 // // 312 byte
108 Suv += yu*yv; 87 Suv += yu*yv;
109 Suw += yu*yw; 88 Suw += yu*yw;
89 Svv += yv*yv;
110 Svw += yv*yw; 90 Svw += yv*yw;
111 91 Sww += yw*yw;
112 // // 104 byte 92
113 Saa = SQR_yu + SQR_yv + SQR_yw; 93 Saa = yu*yu + yv*yv + yw*yw;
114 94
115
116 // // 312 byte
117 Saau += yu * Saa; 95 Saau += yu * Saa;
118 Saav += yv * Saa; 96 Saav += yv * Saa;
119 Saaw += yw * Saa; 97 Saaw += yw * Saa;
120 } 98 }
121 99
122 ////////////////////////////////////////////////////////////////////////////// 100 //////////////////////////////////////////////////////////////////////////////
123 101
124 static void calc_discriminant(PARAMETER char column) 102 static float compass_discriminent(PARAMETER char column)
125 { 103 {
126 // basic symmetric matrix 104 // basic symmetric matrix
127 overlay float a = Suu, d = Suv, g = Suw; 105 OVERLAY float a = Suu, d = Suv, g = Suw;
128 overlay float b = Suv, e = Svv, h = Svw; 106 OVERLAY float b = Suv, e = Svv, h = Svw;
129 overlay float c = Suw, f = Svw, i = Sww; 107 OVERLAY float c = Suw, f = Svw, i = Sww;
108 OVERLAY float result;
130 109
131 // substitute a column, if asked to 110 // substitute a column, if asked to
132 if( column==1 ) { a = yu; b = yv; c = yw; } 111 if( column==1 ) { a = yu; b = yv; c = yw; }
133 if( column==2 ) { d = yu; e = yv; f = yw; } 112 if( column==2 ) { d = yu; e = yv; f = yw; }
134 if( column==3 ) { g = yu; h = yv; i = yw; } 113 if( column==3 ) { g = yu; h = yv; i = yw; }
135 114
136 // do the math in a couple of single terms to reduce 115 // do the math in a couple of single terms to reduce
137 // the amount of required ".tmpdata" memory 116 // the amount of required ".tmpdata" memory
138 discriminant = a * (e * i - f * h); 117 result = a * (e * i - f * h);
139 discriminant -= b * (d * i - f * g); 118 result -= b * (d * i - f * g);
140 discriminant += c * (d * h - e * g); 119 result += c * (d * h - e * g);
141 120
142 // apply delta if column = 1/2/3 121 return result;
143 if( column ) discriminant *= delta; 122 }
144 } 123
145 124 //////////////////////////////////////////////////////////////////////////////
146 ////////////////////////////////////////////////////////////////////////////// 125
147 126 static void compass_solve_helper(void)
148 static void compass_solve_helper(void) // 338 byte
149 { 127 {
150 // computes: 128 // computes:
151 // 129 //
152 // yh = S0 - Saa*uh - compass_dotc(Su*uh + 2*S1, Sv*uh + 2*S2, Sw*uh + 2*S3); 130 // yh = S0 - Saa*uh - compass_dotc(Su*uh + 2*S1, Sv*uh + 2*S2, Sw*uh + 2*S3);
153 // 131 //
161 yh = S0; 139 yh = S0;
162 yh -= Saa*uh; 140 yh -= Saa*uh;
163 yh -= (Su*uh + 2*S1) * uc; 141 yh -= (Su*uh + 2*S1) * uc;
164 yh -= (Sv*uh + 2*S2) * vc; 142 yh -= (Sv*uh + 2*S2) * vc;
165 yh -= (Sw*uh + 2*S3) * wc; 143 yh -= (Sw*uh + 2*S3) * wc;
166 } 144
167 145 return;
168 ////////////////////////////////////////////////////////////////////////////// 146 }
169 147
170 void compass_solve_calibration() // 1738 byte 148 //////////////////////////////////////////////////////////////////////////////
171 { 149
172 overlay float inv_compass_N; 150 void compass_solve_calibration()
151 {
152 OVERLAY float delta;
173 153
174 RESET_C_STACK; 154 RESET_C_STACK;
175 155
176 // compute center of measured magnetic directions // 200 byte 156 //---- Compute center of measured magnetic directions --------------------
177 inv_compass_N = 1 / compass_N; 157 uc = Su/compass_N;
178 uc = Su * inv_compass_N; 158 vc = Sv/compass_N;
179 vc = Sv * inv_compass_N; 159 wc = Sw/compass_N;
180 wc = Sw * inv_compass_N; 160
181 161 //---- Normalize partial sums --------------------------------------------
182 // normalize partial sums
183 // 162 //
184 // We measured the (u, v, w) values, and need the centered (x, y, z) ones 163 // We measured the (u, v, w) values, and need the centered (x, y, z) ones
185 // around the sphere center's (uc, vc, wc) as: 164 // around the sphere center's (uc, vc, wc) as:
186 // uc = Su / N; The mean value 165 // uc = Su / N; The mean value
187 // x = u - uc; The difference to the mean. 166 // x = u - uc; The difference to the mean.
220 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N 199 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N
221 200
222 Saa = Suu + Svv + Sww; 201 Saa = Suu + Svv + Sww;
223 202
224 // compute yu = Saau - Saa*uc - compass_dotc(Su*uc + 2*Suu, Sv*uc + 2*Suv, Sw*uc + 2*Suw); 203 // compute yu = Saau - Saa*uc - compass_dotc(Su*uc + 2*Suu, Sv*uc + 2*Suv, Sw*uc + 2*Suw);
225 // 204 uh = uc; S0 = Saau; S1 = Suu; S2 = Suv; S3 = Suw;
226 uh = uc; S0 = Saau; S1 = Suu; S2 = Suv; S3 = Suw; compass_solve_helper(); yu = yh; 205 compass_solve_helper();
206 yu = yh;
227 207
228 // compute yv = Saav - Saa*vc - compass_dotc(Su*vc + 2*Suv, Sv*vc + 2*Svv, Sw*vc + 2*Svw); 208 // compute yv = Saav - Saa*vc - compass_dotc(Su*vc + 2*Suv, Sv*vc + 2*Svv, Sw*vc + 2*Svw);
229 // 209 uh = vc; S0 = Saav; S1 = Suv; S2 = Svv; S3 = Svw;
230 uh = vc; S0 = Saav; S1 = Suv; S2 = Svv; S3 = Svw; compass_solve_helper(); yv = yh; 210 compass_solve_helper();
211 yv = yh;
231 212
232 // compute yw = Saaw - Saa*wc - compass_dotc(Su*wc + 2*Suw, Sv*wc + 2*Svw, Sw*wc + 2*Sww); 213 // compute yw = Saaw - Saa*wc - compass_dotc(Su*wc + 2*Suw, Sv*wc + 2*Svw, Sw*wc + 2*Sww);
233 // 214 uh = wc; S0 = Saaw; S1 = Suw; S2 = Svw; S3 = Sww;
234 uh = wc; S0 = Saaw; S1 = Suw; S2 = Svw; S3 = Sww; compass_solve_helper(); yw = yh; 215 compass_solve_helper();
216 yw = yh;
235 217
236 218
237 //---- Solve the system -------------------------------------------------- 219 //---- Solve the system --------------------------------------------------
238 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2 220 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2
239 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2 221 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2
240 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2 222 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2
241 // 223 //
242 // Note this is symmetric, with a positive diagonal, hence discriminant is always not null 224 // Note this is symmetric, with a positive diagonal, hence discriminant is always not null
243 225
244 //compute delta value 226 delta = 0.5f / compass_discriminent(0);
245 calc_discriminant(0); delta = 0.5f / discriminant; 227
246 228 // computed new center, with offset values:
247 // compute new center, with offset values 229 uc += compass_discriminent(1) * delta;
248 calc_discriminant(1); uc += discriminant; 230 vc += compass_discriminent(2) * delta;
249 calc_discriminant(2); vc += discriminant; 231 wc += compass_discriminent(3) * delta;
250 calc_discriminant(3); wc += discriminant; 232
251 233 // add correction due to already applied calibration:
252 // add correction due to already applied calibration
253 compass_CX_f += (short)(32768 * uc); 234 compass_CX_f += (short)(32768 * uc);
254 compass_CY_f += (short)(32768 * vc); 235 compass_CY_f += (short)(32768 * vc);
255 compass_CZ_f += (short)(32768 * wc); 236 compass_CZ_f += (short)(32768 * wc);
256 } 237 }
257 238