| ISF 2.1 Intelligent Sensing Framework for Kinetis with Processor Expert

- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
magnetic.c
Go to the documentation of this file.
1 // Copyright (c) 2014, Freescale Semiconductor, Inc.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of Freescale Semiconductor, Inc. nor the
12 // names of its contributors may be used to endorse or promote products
13 // derived from this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 // DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY
19 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 //
26 // This file contains magnetic calibration functions. It is STRONGLY RECOMMENDED
27 // that the casual developer NOT TOUCH THIS FILE. The mathematics behind this file
28 // is extremely complex, and it will be very easy (almost inevitable) that you screw it
29 // up.
30 //
31
32 #include "math.h"
33 #include "stdlib.h"
34 #include "matrix.h"
35 #include "math_constants.h"
36 #include "magnetic.h"
37
38 #define ONETHIRD 0.33333333F // one third
39 #define ONESIXTH 0.166666667F // one sixth
40
41 // function resets the magnetometer buffer and magnetic calibration
42 void fInitMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer)
43 {
44 int8 j, k; // loop counters
45
46 // initialize the calibration hard and soft iron estimate to null
47 f3x3matrixAeqI(pthisMagCal->finvW);
48 pthisMagCal->fV[X] = pthisMagCal->fV[Y] = pthisMagCal->fV[Z] = 0.0F;
49 pthisMagCal->fB = DEFAULTB;
50 pthisMagCal->fFourBsq = 4.0F * pthisMagCal->fB * pthisMagCal->fB;
51 pthisMagCal->fFitErrorpc = 1000.0F;
52 pthisMagCal->iValidMagCal = 0;
53 pthisMagCal->iCalInProgress = 0;
54 pthisMagCal->iMagCalHasRun = 0;
55
56 // set magnetic buffer index to invalid value -1 to denote invalid
57 pthisMagBuffer->iMagBufferCount = 0;
58 for (j = 0; j < MAGBUFFSIZEX; j++)
59 {
60 for (k = 0; k < MAGBUFFSIZEY; k++)
61 {
62 pthisMagBuffer->index[j][k] = -1;
63 }
64 }
65
66 // initialize the array of (MAGBUFFSIZEX - 1) elements of 100 * tangents used for buffer indexing
67 // entries cover the range 100 * tan(-PI/2 + PI/MAGBUFFSIZEX), 100 * tan(-PI/2 + 2*PI/MAGBUFFSIZEX) to
68 // 100 * tan(-PI/2 + (MAGBUFFSIZEX - 1) * PI/MAGBUFFSIZEX).
69 // for MAGBUFFSIZEX=12, the entries range in value from -373 to +373
70 for (j = 0; j < (MAGBUFFSIZEX - 1); j++)
71 {
72 pthisMagBuffer->tanarray[j] = (int16) (100.0F * tanf(PI * (-0.5F + (float) (j + 1) / MAGBUFFSIZEX)));
73 }
74
75 return;
76 }
77
78 // function updates the magnetic measurement buffer with most recent magnetic data (typically 25Hz)
79 void iUpdateMagnetometerBuffer(struct MagneticBuffer *pthisMagBuffer, struct AccelSensor *pthisAccel,
80 struct MagSensor *pthisMag, int32 loopcounter)
81 {
82 // local variables
83 int32 idelta; // absolute vector distance
84 int32 i; // counter
85 int16 itanj, itank; // indexing accelerometer ratios
86 int8 j, k, l, m; // counters
87 int8 itooclose; // flag denoting measurement is too close to existing ones
88
89 // calculate the magnetometer buffer bins from the accelerometer tangent ratios
90 if (pthisAccel->iGp[X] == 0) return;
91 itanj = (100 * (int32)pthisAccel->iGp[Y]) / ((int32)pthisAccel->iGp[X]);
92 itank = (100 * (int32)pthisAccel->iGp[Z]) / ((int32)pthisAccel->iGp[X]);
93 // map tangent ratios to bins j and k using equal angle bins: C guarantees left to right execution of the test
94 // and add an offset of MAGBUFFSIZEX bins to k to mimic atan2 on this ratio
95 // j will vary from 0 to MAGBUFFSIZEX - 1 and k from 0 to 2 * MAGBUFFSIZEX - 1
96 j = k = 0;
97 while ((j < (MAGBUFFSIZEX - 1) && (itanj >= pthisMagBuffer->tanarray[j]))) j++;
98 while ((k < (MAGBUFFSIZEX - 1) && (itank >= pthisMagBuffer->tanarray[k]))) k++;
99 if (pthisAccel->iGp[X] < 0) k += MAGBUFFSIZEX;
100
101 // case 1: buffer is full and this bin has a measurement: over-write without increasing number of measurements
102 // this is the most common option at run time
103 if ((pthisMagBuffer->iMagBufferCount == MAXMEASUREMENTS) && (pthisMagBuffer->index[j][k] != -1))
104 {
105 // store the fast (unaveraged at typically 200Hz) integer magnetometer reading into the buffer bin j, k
106 for (i = X; i <= Z; i++)
107 {
108 pthisMagBuffer->iBp[i][j][k] = pthisMag->iBp[i];
109 }
110 pthisMagBuffer->index[j][k] = loopcounter;
111 return;
112 } // end case 1
113
114 // case 2: the buffer is full and this bin does not have a measurement: store and retire the oldest
115 // this is the second most common option at run time
116 if ((pthisMagBuffer->iMagBufferCount == MAXMEASUREMENTS) && (pthisMagBuffer->index[j][k] == -1))
117 {
118 // store the fast (unaveraged at typically 200Hz) integer magnetometer reading into the buffer bin j, k
119 for (i = X; i <= Z; i++)
120 {
121 pthisMagBuffer->iBp[i][j][k] = pthisMag->iBp[i];
122 }
123 pthisMagBuffer->index[j][k] = loopcounter;
124
125 // set l and m to the oldest active entry and disable it
126 i = loopcounter;
127 l = m = 0; // to avoid compiler complaint
128 for (j = 0; j < MAGBUFFSIZEX; j++)
129 {
130 for (k = 0; k < MAGBUFFSIZEY; k++)
131 {
132 // check if the time stamp is older than the oldest found so far (normally fails this test)
133 if (pthisMagBuffer->index[j][k] < i)
134 {
135 // check if this bin is active (normally passes this test)
136 if (pthisMagBuffer->index[j][k] != -1)
137 {
138 // set l and m to the indices of the oldest entry found so far
139 l = j;
140 m = k;
141 // set i to the time stamp of the oldest entry found so far
142 i = pthisMagBuffer->index[l][m];
143 } // end of test for active
144 } // end of test for older
145 } // end of loop over k
146 } // end of loop over j
147
148 // deactivate the oldest measurement (no need to zero the measurement data)
149 pthisMagBuffer->index[l][m] = -1;
150 return;
151 } // end case 2
152
153 // case 3: buffer is not full and this bin is empty: store and increment number of measurements
154 if ((pthisMagBuffer->iMagBufferCount < MAXMEASUREMENTS) && (pthisMagBuffer->index[j][k] == -1))
155 {
156 // store the fast (unaveraged at typically 200Hz) integer magnetometer reading into the buffer bin j, k
157 for (i = X; i <= Z; i++)
158 {
159 pthisMagBuffer->iBp[i][j][k] = pthisMag->iBp[i];
160 }
161 pthisMagBuffer->index[j][k] = loopcounter;
162 (pthisMagBuffer->iMagBufferCount)++;
163 return;
164 } // end case 3
165
166 // case 4: buffer is not full and this bin has a measurement: over-write if close or try to slot in
167 // elsewhere if not close to the other measurements so as to create a mesh at power up
168 if ((pthisMagBuffer->iMagBufferCount < MAXMEASUREMENTS) && (pthisMagBuffer->index[j][k] != -1))
169 {
170 // calculate the vector difference between current measurement and the buffer entry
171 idelta = 0;
172 for (i = X; i <= Z; i++)
173 {
174 idelta += abs((int32)pthisMag->iBp[i] - (int32)pthisMagBuffer->iBp[i][j][k]);
175 }
176 // check to see if the current reading is close to this existing magnetic buffer entry
177 if (idelta < MESHDELTACOUNTS)
178 {
179 // simply over-write the measurement and return
180 for (i = X; i <= Z; i++)
181 {
182 pthisMagBuffer->iBp[i][j][k] = pthisMag->iBp[i];
183 }
184 pthisMagBuffer->index[j][k] = loopcounter;
185 }
186 else
187 {
188 // reset the flag denoting that the current measurement is close to any measurement in the buffer
189 itooclose = 0;
190 // to avoid compiler warning
191 l = m = 0;
192 // loop over the buffer j from 0 potentially up to MAGBUFFSIZEX - 1
193 j = 0;
194 while (!itooclose && (j < MAGBUFFSIZEX))
195 {
196 // loop over the buffer k from 0 potentially up to MAGBUFFSIZEY - 1
197 k = 0;
198 while (!itooclose && (k < MAGBUFFSIZEY))
199 {
200 // check whether this buffer entry already has a measurement or not
201 if (pthisMagBuffer->index[j][k] != -1)
202 {
203 // calculate the vector difference between current measurement and the buffer entry
204 idelta = 0;
205 for (i = X; i <= Z; i++)
206 {
207 idelta += abs((int32)pthisMag->iBp[i] - (int32)pthisMagBuffer->iBp[i][j][k]);
208 }
209 // check to see if the current reading is close to this existing magnetic buffer entry
210 if (idelta < MESHDELTACOUNTS)
211 {
212 // set the flag to abort the search
213 itooclose = 1;
214 }
215 }
216 else
217 {
218 // store the location of this empty bin for future use
219 l = j;
220 m = k;
221 } // end of test for valid measurement in this bin
222 k++;
223 } // end of k loop
224 j++;
225 } // end of j loop
226
227 // if none too close, store the measurement in the last empty bin found and return
228 // l and m are guaranteed to be set if no entries too close are detected
229 if (!itooclose)
230 {
231 for (i = X; i <= Z; i++)
232 {
233 pthisMagBuffer->iBp[i][l][m] = pthisMag->iBp[i];
234 }
235 pthisMagBuffer->index[l][m] = loopcounter;
236 (pthisMagBuffer->iMagBufferCount)++;
237 }
238 } // end of test for closeness to current buffer entry
239 return;
240 } // end case 4
241
242 // this line should be unreachable
243 return;
244 }
245
246 // function maps the uncalibrated magnetometer data Bp (uT) onto calibrated data Bc (uT)
247 void fInvertMagCal(struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal)
248 {
249 // local variables
250 float ftmp[3]; // temporary array
251 int8 i; // loop counter
252
253 // calculate fBc and iBc
254 // remove the computed hard iron offsets (uT): ftmp[]=fBp[]-V[]
255 for (i = X; i <= Z; i++)
256 {
257 ftmp[i] = pthisMag->fBp[i] - pthisMagCal->fV[i];
258 }
259 // remove the computed soft iron offsets (uT and counts): fBc=inv(W)*(fBp[]-V[])
260 for (i = X; i <= Z; i++)
261 {
262 pthisMag->fBc[i] = pthisMagCal->finvW[i][X] * ftmp[X] + pthisMagCal->finvW[i][Y] * ftmp[Y] + pthisMagCal->finvW[i][Z] * ftmp[Z];
263 //pthisMag->iBc[i] = (int16) (pthisMag->fBc[i] * pthisMag->fCountsPeruT); // [JNM 2-9-15] This quantity appears to be unnused.
264 }
265
266 return;
267 }
268
269 // function runs the magnetic calibration
270 void fRunMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag )
271 {
272 int8 i, j; // loop counters
273 int8 isolver; // magnetic solver used
274
275 // 4 element calibration case
276 if (pthisMagBuffer->iMagBufferCount < MINMEASUREMENTS7CAL)
277 {
278 // age the existing fit error to avoid one good calibration locking out future updates
279 if (pthisMagCal->iValidMagCal)
280 {
281 pthisMagCal->fFitErrorpc *= (1.0F + (float) INTERVAL4CAL * (float) MAG_OVERSAMPLE_RATIO / ((float) SENSORFS * FITERRORAGINGSECS));
282 }
283 // call the 4 element matrix inversion calibration
284 isolver = 4;
285 fUpdateCalibration4INV(pthisMagCal, pthisMagBuffer, pthisMag);
286 }
287 // 7 element calibration case
288 else if (pthisMagBuffer->iMagBufferCount < MINMEASUREMENTS10CAL)
289 {
290 // age the existing fit error to avoid one good calibration locking out future updates
291 if (pthisMagCal->iValidMagCal)
292 {
293 pthisMagCal->fFitErrorpc *= (1.0F + (float) INTERVAL7CAL * (float) MAG_OVERSAMPLE_RATIO / ((float) SENSORFS * FITERRORAGINGSECS));
294 }
295 // call the 7 element eigenpair calibration
296 isolver = 7;
297 fUpdateCalibration7EIG(pthisMagCal, pthisMagBuffer, pthisMag);
298 }
299 // 10 element calibration case
300 else
301 {
302 // age the existing fit error to avoid one good calibration locking out future updates
303 if (pthisMagCal->iValidMagCal)
304 {
305 pthisMagCal->fFitErrorpc *= (1.0F + (float) INTERVAL10CAL * (float) MAG_OVERSAMPLE_RATIO / ((float) SENSORFS * FITERRORAGINGSECS));
306 }
307 // call the 10 element eigenpair calibration
308 isolver = 10;
309 fUpdateCalibration10EIG(pthisMagCal, pthisMagBuffer, pthisMag);
310 }
311
312 // the trial geomagnetic field must be in range (earth is 22uT to 67uT)
313 if ((pthisMagCal->ftrB >= MINBFITUT) && (pthisMagCal->ftrB <= MAXBFITUT))
314 {
315 // always accept the calibration if i) no previous calibration exists ii) the calibration fit is reduced or
316 // an improved solver was used giving a good trial calibration (4% or under)
317 if ((pthisMagCal->iValidMagCal == 0) ||
318 (pthisMagCal->ftrFitErrorpc <= pthisMagCal->fFitErrorpc) ||
319 ((isolver > pthisMagCal->iValidMagCal) && (pthisMagCal->ftrFitErrorpc <= 4.0F)))
320 {
321 // accept the new calibration solution
322 pthisMagCal->iValidMagCal = isolver;
323 pthisMagCal->fFitErrorpc = pthisMagCal->ftrFitErrorpc;
324 pthisMagCal->fB = pthisMagCal->ftrB;
325 pthisMagCal->fFourBsq = 4.0F * pthisMagCal->ftrB * pthisMagCal->ftrB;
326 for (i = X; i <= Z; i++)
327 {
328 pthisMagCal->fV[i] = pthisMagCal->ftrV[i];
329 for (j = X; j <= Z; j++)
330 {
331 pthisMagCal->finvW[i][j] = pthisMagCal->ftrinvW[i][j];
332 }
333 }
334 } // end of test to accept the new calibration
335 } // end of test for geomagenetic field strength in range
336
337 // reset the calibration in progress flag to allow writing to the magnetic buffer
338 pthisMagCal->iCalInProgress = 0;
339
340 return;
341 }
342
343 // 4 element calibration using 4x4 matrix inverse
344 void fUpdateCalibration4INV(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
345 {
346 // local variables
347 float fBp2; // fBp[X]^2+fBp[Y]^2+fBp[Z]^2
348 float fSumBp4; // sum of fBp2
349 float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
350 float fE; // error function = r^T.r
351 int16 iOffset[3]; // offset to remove large DC hard iron bias in matrix
352 int16 iCount; // number of measurements counted
353 int8 i, j, k, l; // loop counters
354
355 // working arrays for 4x4 matrix inversion
356 float *pfRows[4];
357 int8 iColInd[4];
358 int8 iRowInd[4];
359 int8 iPivot[4];
360
361 // compute fscaling to reduce multiplications later
362 fscaling = FUTPERCOUNT * INV_DEFAULTB; // was pthisMag->fuTPerCount / DEFAULTB;
363
364 // the trial inverse soft iron matrix invW always equals the identity matrix for 4 element calibration
365 f3x3matrixAeqI(pthisMagCal->ftrinvW);
366
367 // zero fSumBp4=Y^T.Y, fvecB=X^T.Y (4x1) and on and above diagonal elements of fmatA=X^T*X (4x4)
368 fSumBp4 = 0.0F;
369 for (i = 0; i < 4; i++)
370 {
371 pthisMagCal->fvecB[i] = 0.0F;
372 for (j = i; j < 4; j++)
373 {
374 pthisMagCal->fmatA[i][j] = 0.0F;
375 }
376 }
377
378 // the offsets are guaranteed to be set from the first element but to avoid compiler error
379 iOffset[X] = iOffset[Y] = iOffset[Z] = 0;
380
381 // use from MINEQUATIONS up to MAXEQUATIONS entries from magnetic buffer to compute matrices
382 iCount = 0;
383 for (j = 0; j < MAGBUFFSIZEX; j++)
384 {
385 for (k = 0; k < MAGBUFFSIZEY; k++)
386 {
387 if (pthisMagBuffer->index[j][k] != -1)
388 {
389 // use first valid magnetic buffer entry as estimate (in counts) for offset
390 if (iCount == 0)
391 {
392 for (l = X; l <= Z; l++)
393 {
394 iOffset[l] = pthisMagBuffer->iBp[l][j][k];
395 }
396 }
397
398 // store scaled and offset fBp[XYZ] in fvecA[0-2] and fBp[XYZ]^2 in fvecA[3-5]
399 for (l = X; l <= Z; l++)
400 {
401 pthisMagCal->fvecA[l] = (float)((int32)pthisMagBuffer->iBp[l][j][k] - (int32)iOffset[l]) * fscaling;
402 pthisMagCal->fvecA[l + 3] = pthisMagCal->fvecA[l] * pthisMagCal->fvecA[l];
403 }
404
405 // calculate fBp2 = fBp[X]^2 + fBp[Y]^2 + fBp[Z]^2 (scaled uT^2)
406 fBp2 = pthisMagCal->fvecA[3] + pthisMagCal->fvecA[4] + pthisMagCal->fvecA[5];
407
408 // accumulate fBp^4 over all measurements into fSumBp4=Y^T.Y
409 fSumBp4 += fBp2 * fBp2;
410
411 // now we have fBp2, accumulate fvecB[0-2] = X^T.Y =sum(fBp2.fBp[XYZ])
412 for (l = X; l <= Z; l++)
413 {
414 pthisMagCal->fvecB[l] += pthisMagCal->fvecA[l] * fBp2;
415 }
416
417 //accumulate fvecB[3] = X^T.Y =sum(fBp2)
418 pthisMagCal->fvecB[3] += fBp2;
419
420 // accumulate on and above-diagonal terms of fmatA = X^T.X ignoring fmatA[3][3]
421 pthisMagCal->fmatA[0][0] += pthisMagCal->fvecA[X + 3];
422 pthisMagCal->fmatA[0][1] += pthisMagCal->fvecA[X] * pthisMagCal->fvecA[Y];
423 pthisMagCal->fmatA[0][2] += pthisMagCal->fvecA[X] * pthisMagCal->fvecA[Z];
424 pthisMagCal->fmatA[0][3] += pthisMagCal->fvecA[X];
425 pthisMagCal->fmatA[1][1] += pthisMagCal->fvecA[Y + 3];
426 pthisMagCal->fmatA[1][2] += pthisMagCal->fvecA[Y] * pthisMagCal->fvecA[Z];
427 pthisMagCal->fmatA[1][3] += pthisMagCal->fvecA[Y];
428 pthisMagCal->fmatA[2][2] += pthisMagCal->fvecA[Z + 3];
429 pthisMagCal->fmatA[2][3] += pthisMagCal->fvecA[Z];
430
431 // increment the counter for next iteration
432 iCount++;
433 }
434 }
435 }
436
437 // set the last element of the measurement matrix to the number of buffer elements used
438 pthisMagCal->fmatA[3][3] = (float) iCount;
439
440 // store the number of measurements accumulated (defensive programming, should never be needed)
441 pthisMagBuffer->iMagBufferCount = iCount;
442
443 // use above diagonal elements of symmetric fmatA to set both fmatB and fmatA to X^T.X
444 for (i = 0; i < 4; i++)
445 {
446 for (j = i; j < 4; j++)
447 {
448 pthisMagCal->fmatB[i][j] = pthisMagCal->fmatB[j][i] = pthisMagCal->fmatA[j][i] = pthisMagCal->fmatA[i][j];
449 }
450 }
451
452 // calculate in situ inverse of fmatB = inv(X^T.X) (4x4) while fmatA still holds X^T.X
453 for (i = 0; i < 4; i++)
454 {
455 pfRows[i] = pthisMagCal->fmatB[i];
456 }
457 fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 4);
458
459 // calculate fvecA = solution beta (4x1) = inv(X^T.X).X^T.Y = fmatB * fvecB
460 for (i = 0; i < 4; i++)
461 {
462 pthisMagCal->fvecA[i] = 0.0F;
463 for (k = 0; k < 4; k++)
464 {
465 pthisMagCal->fvecA[i] += pthisMagCal->fmatB[i][k] * pthisMagCal->fvecB[k];
466 }
467 }
468
469 // calculate P = r^T.r = Y^T.Y - 2 * beta^T.(X^T.Y) + beta^T.(X^T.X).beta
470 // = fSumBp4 - 2 * fvecA^T.fvecB + fvecA^T.fmatA.fvecA
471 // first set P = Y^T.Y - 2 * beta^T.(X^T.Y) = fSumBp4 - 2 * fvecA^T.fvecB
472 fE = 0.0F;
473 for (i = 0; i < 4; i++)
474 {
475 fE += pthisMagCal->fvecA[i] * pthisMagCal->fvecB[i];
476 }
477 fE = fSumBp4 - 2.0F * fE;
478
479 // set fvecB = (X^T.X).beta = fmatA.fvecA
480 for (i = 0; i < 4; i++)
481 {
482 pthisMagCal->fvecB[i] = 0.0F;
483 for (k = 0; k < 4; k++)
484 {
485 pthisMagCal->fvecB[i] += pthisMagCal->fmatA[i][k] * pthisMagCal->fvecA[k];
486 }
487 }
488
489 // complete calculation of P by adding beta^T.(X^T.X).beta = fvecA^T * fvecB
490 for (i = 0; i < 4; i++)
491 {
492 fE += pthisMagCal->fvecB[i] * pthisMagCal->fvecA[i];
493 }
494
495 // compute the hard iron vector (in uT but offset and scaled by FMATRIXSCALING)
496 for (l = X; l <= Z; l++)
497 {
498 pthisMagCal->ftrV[l] = 0.5F * pthisMagCal->fvecA[l];
499 }
500
501 // compute the scaled geomagnetic field strength B (in uT but scaled by FMATRIXSCALING)
502 pthisMagCal->ftrB = sqrtf(pthisMagCal->fvecA[3] + pthisMagCal->ftrV[X] * pthisMagCal->ftrV[X] +
503 pthisMagCal->ftrV[Y] * pthisMagCal->ftrV[Y] + pthisMagCal->ftrV[Z] * pthisMagCal->ftrV[Z]);
504
505 // calculate the trial fit error (percent) normalized to number of measurements and scaled geomagnetic field strength
506 pthisMagCal->ftrFitErrorpc = sqrtf(fE / (float) pthisMagBuffer->iMagBufferCount) * 100.0F /
507 (2.0F * pthisMagCal->ftrB * pthisMagCal->ftrB);
508
509 // correct the hard iron estimate for FMATRIXSCALING and the offsets applied (result in uT)
510 for (l = X; l <= Z; l++)
511 {
512 pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] * DEFAULTB + (float)iOffset[l]/256.0*FUTPERCOUNT;
513 }
514
515 // correct the geomagnetic field strength B to correct scaling (result in uT)
516 pthisMagCal->ftrB *= DEFAULTB;
517
518 return;
519 }
520
521 // 7 element calibration using direct eigen-decomposition
522 void fUpdateCalibration7EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
523 {
524 // local variables
525 float det; // matrix determinant
526 float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
527 float ftmp; // scratch variable
528 int16 iOffset[3]; // offset to remove large DC hard iron bias
529 int16 iCount; // number of measurements counted
530 int8 i, j, k, l, m, n; // loop counters
531
532 // compute fscaling to reduce multiplications later
533 fscaling = FUTPERCOUNT * INV_DEFAULTB; // was pthisMag->fuTPerCount / DEFAULTB;
534
535 // the offsets are guaranteed to be set from the first element but to avoid compiler error
536 iOffset[X] = iOffset[Y] = iOffset[Z] = 0;
537
538 // zero the on and above diagonal elements of the 7x7 symmetric measurement matrix fmatA
539 for (m = 0; m < 7; m++)
540 {
541 for (n = m; n < 7; n++)
542 {
543 pthisMagCal->fmatA[m][n] = 0.0F;
544 }
545 }
546
547 // place from MINEQUATIONS to MAXEQUATIONS entries into product matrix fmatA
548 iCount = 0;
549 for (j = 0; j < MAGBUFFSIZEX; j++)
550 {
551 for (k = 0; k < MAGBUFFSIZEY; k++)
552 {
553 if (pthisMagBuffer->index[j][k] != -1)
554 {
555 // use first valid magnetic buffer entry as offset estimate (bit counts)
556 if (iCount == 0)
557 {
558 for (l = X; l <= Z; l++)
559 {
560 iOffset[l] = pthisMagBuffer->iBp[l][j][k];
561 }
562 }
563
564 // apply the offset and scaling and store in fvecA
565 for (l = X; l <= Z; l++)
566 {
567 pthisMagCal->fvecA[l + 3] = (float)((int32)pthisMagBuffer->iBp[l][j][k] - (int32)iOffset[l]) * fscaling;
568 pthisMagCal->fvecA[l] = pthisMagCal->fvecA[l + 3] * pthisMagCal->fvecA[l + 3];
569 }
570
571 // accumulate the on-and above-diagonal terms of pthisMagCal->fmatA=Sigma{fvecA^T * fvecA}
572 // with the exception of fmatA[6][6] which will sum to the number of measurements
573 // and remembering that fvecA[6] equals 1.0F
574 // update the right hand column [6] of fmatA except for fmatA[6][6]
575 for (m = 0; m < 6; m++)
576 {
577 pthisMagCal->fmatA[m][6] += pthisMagCal->fvecA[m];
578 }
579 // update the on and above diagonal terms except for right hand column 6
580 for (m = 0; m < 6; m++)
581 {
582 for (n = m; n < 6; n++)
583 {
584 pthisMagCal->fmatA[m][n] += pthisMagCal->fvecA[m] * pthisMagCal->fvecA[n];
585 }
586 }
587
588 // increment the measurement counter for the next iteration
589 iCount++;
590 }
591 }
592 }
593
594 // finally set the last element fmatA[6][6] to the number of measurements
595 pthisMagCal->fmatA[6][6] = (float) iCount;
596
597 // store the number of measurements accumulated (defensive programming, should never be needed)
598 pthisMagBuffer->iMagBufferCount = iCount;
599
600 // copy the above diagonal elements of fmatA to below the diagonal
601 for (m = 1; m < 7; m++)
602 {
603 for (n = 0; n < m; n++)
604 {
605 pthisMagCal->fmatA[m][n] = pthisMagCal->fmatA[n][m];
606 }
607 }
608
609 // set tmpA7x1 to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA
610 eigencompute(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB, 7);
611
612 // find the smallest eigenvalue
613 j = 0;
614 for (i = 1; i < 7; i++)
615 {
616 if (pthisMagCal->fvecA[i] < pthisMagCal->fvecA[j])
617 {
618 j = i;
619 }
620 }
621
622 // set ellipsoid matrix A to the solution vector with smallest eigenvalue, compute its determinant
623 // and the hard iron offset (scaled and offset)
624 f3x3matrixAeqScalar(pthisMagCal->fA, 0.0F);
625 det = 1.0F;
626 for (l = X; l <= Z; l++)
627 {
628 pthisMagCal->fA[l][l] = pthisMagCal->fmatB[l][j];
629 det *= pthisMagCal->fA[l][l];
630 pthisMagCal->ftrV[l] = -0.5F * pthisMagCal->fmatB[l + 3][j] / pthisMagCal->fA[l][l];
631 }
632
633 // negate A if it has negative determinant
634 if (det < 0.0F)
635 {
636 f3x3matrixAeqMinusA(pthisMagCal->fA);
637 pthisMagCal->fmatB[6][j] = -pthisMagCal->fmatB[6][j];
638 det = -det;
639 }
640
641 // set ftmp to the square of the trial geomagnetic field strength B (counts times FMATRIXSCALING)
642 ftmp = -pthisMagCal->fmatB[6][j];
643 for (l = X; l <= Z; l++)
644 {
645 ftmp += pthisMagCal->fA[l][l] * pthisMagCal->ftrV[l] * pthisMagCal->ftrV[l];
646 }
647
648 // calculate the trial normalized fit error as a percentage
649 pthisMagCal->ftrFitErrorpc = 50.0F * sqrtf(fabs(pthisMagCal->fvecA[j]) / (float) pthisMagBuffer->iMagBufferCount) / fabs(ftmp);
650
651 // normalize the ellipsoid matrix A to unit determinant
652 f3x3matrixAeqAxScalar(pthisMagCal->fA, powf(det, -(ONETHIRD)));
653
654 // convert the geomagnetic field strength B into uT for normalized soft iron matrix A and normalize
655 pthisMagCal->ftrB = sqrtf(fabs(ftmp)) * DEFAULTB * powf(det, -(ONESIXTH));
656
657 // compute trial invW from the square root of A also with normalized determinant and hard iron offset in uT
658 f3x3matrixAeqI(pthisMagCal->ftrinvW);
659 for (l = X; l <= Z; l++)
660 {
661 pthisMagCal->ftrinvW[l][l] = sqrtf(fabs(pthisMagCal->fA[l][l]));
662 pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] * DEFAULTB + (float)iOffset[l]*FUTPERCOUNT;
663 }
664
665 return;
666 }
667
668 // 10 element calibration using direct eigen-decomposition
669 void fUpdateCalibration10EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
670 {
671 // local variables
672 float det; // matrix determinant
673 float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
674 float ftmp; // scratch variable
675 int16 iOffset[3]; // offset to remove large DC hard iron bias in matrix
676 int16 iCount; // number of measurements counted
677 int8 i, j, k, l, m, n; // loop counters
678
679 // compute fscaling to reduce multiplications later
680 fscaling = FUTPERCOUNT * INV_DEFAULTB; // was pthisMag->fuTPerCount / DEFAULTB;
681
682 // the offsets are guaranteed to be set from the first element but to avoid compiler error
683 iOffset[X] = iOffset[Y] = iOffset[Z] = 0;
684
685 // zero the on and above diagonal elements of the 10x10 symmetric measurement matrix fmatA
686 for (m = 0; m < 10; m++)
687 {
688 for (n = m; n < 10; n++)
689 {
690 pthisMagCal->fmatA[m][n] = 0.0F;
691 }
692 }
693
694 // sum between MINEQUATIONS to MAXEQUATIONS entries into the 10x10 product matrix fmatA
695 iCount = 0;
696 for (j = 0; j < MAGBUFFSIZEX; j++)
697 {
698 for (k = 0; k < MAGBUFFSIZEY; k++)
699 {
700 if (pthisMagBuffer->index[j][k] != -1)
701 {
702 // use first valid magnetic buffer entry as estimate for offset to help solution (bit counts)
703 if (iCount == 0)
704 {
705 for (l = X; l <= Z; l++)
706 {
707 iOffset[l] = pthisMagBuffer->iBp[l][j][k];
708 }
709 }
710
711 // apply the fixed offset and scaling and enter into fvecA[6-8]
712 for (l = X; l <= Z; l++)
713 {
714 pthisMagCal->fvecA[l + 6] = (float)((int32)pthisMagBuffer->iBp[l][j][k] - (int32)iOffset[l]) * fscaling;
715 }
716
717 // compute measurement vector elements fvecA[0-5] from fvecA[6-8]
718 pthisMagCal->fvecA[0] = pthisMagCal->fvecA[6] * pthisMagCal->fvecA[6];
719 pthisMagCal->fvecA[1] = 2.0F * pthisMagCal->fvecA[6] * pthisMagCal->fvecA[7];
720 pthisMagCal->fvecA[2] = 2.0F * pthisMagCal->fvecA[6] * pthisMagCal->fvecA[8];
721 pthisMagCal->fvecA[3] = pthisMagCal->fvecA[7] * pthisMagCal->fvecA[7];
722 pthisMagCal->fvecA[4] = 2.0F * pthisMagCal->fvecA[7] * pthisMagCal->fvecA[8];
723 pthisMagCal->fvecA[5] = pthisMagCal->fvecA[8] * pthisMagCal->fvecA[8];
724
725 // accumulate the on-and above-diagonal terms of fmatA=Sigma{fvecA^T * fvecA}
726 // with the exception of fmatA[9][9] which equals the number of measurements
727 // update the right hand column [9] of fmatA[0-8][9] ignoring fmatA[9][9]
728 for (m = 0; m < 9; m++)
729 {
730 pthisMagCal->fmatA[m][9] += pthisMagCal->fvecA[m];
731 }
732 // update the on and above diagonal terms of fmatA ignoring right hand column 9
733 for (m = 0; m < 9; m++)
734 {
735 for (n = m; n < 9; n++)
736 {
737 pthisMagCal->fmatA[m][n] += pthisMagCal->fvecA[m] * pthisMagCal->fvecA[n];
738 }
739 }
740
741 // increment the measurement counter for the next iteration
742 iCount++;
743 }
744 }
745 }
746
747 // set the last element fmatA[9][9] to the number of measurements
748 pthisMagCal->fmatA[9][9] = (float) iCount;
749
750 // store the number of measurements accumulated (defensive programming, should never be needed)
751 pthisMagBuffer->iMagBufferCount = iCount;
752
753 // copy the above diagonal elements of symmetric product matrix fmatA to below the diagonal
754 for (m = 1; m < 10; m++)
755 {
756 for (n = 0; n < m; n++)
757 {
758 pthisMagCal->fmatA[m][n] = pthisMagCal->fmatA[n][m];
759 }
760 }
761
762 // set pthisMagCal->fvecA to the unsorted eigenvalues and fmatB to the unsorted normalized eigenvectors of fmatA
763 eigencompute(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB, 10);
764
765 // set ellipsoid matrix A from elements of the solution vector column j with smallest eigenvalue
766 j = 0;
767 for (i = 1; i < 10; i++)
768 {
769 if (pthisMagCal->fvecA[i] < pthisMagCal->fvecA[j])
770 {
771 j = i;
772 }
773 }
774 pthisMagCal->fA[0][0] = pthisMagCal->fmatB[0][j];
775 pthisMagCal->fA[0][1] = pthisMagCal->fA[1][0] = pthisMagCal->fmatB[1][j];
776 pthisMagCal->fA[0][2] = pthisMagCal->fA[2][0] = pthisMagCal->fmatB[2][j];
777 pthisMagCal->fA[1][1] = pthisMagCal->fmatB[3][j];
778 pthisMagCal->fA[1][2] = pthisMagCal->fA[2][1] = pthisMagCal->fmatB[4][j];
779 pthisMagCal->fA[2][2] = pthisMagCal->fmatB[5][j];
780
781 // negate entire solution if A has negative determinant
782 det = f3x3matrixDetA(pthisMagCal->fA);
783 if (det < 0.0F)
784 {
785 f3x3matrixAeqMinusA(pthisMagCal->fA);
786 pthisMagCal->fmatB[6][j] = -pthisMagCal->fmatB[6][j];
787 pthisMagCal->fmatB[7][j] = -pthisMagCal->fmatB[7][j];
788 pthisMagCal->fmatB[8][j] = -pthisMagCal->fmatB[8][j];
789 pthisMagCal->fmatB[9][j] = -pthisMagCal->fmatB[9][j];
790 det = -det;
791 }
792
793 // compute the inverse of the ellipsoid matrix
794 f3x3matrixAeqInvSymB(pthisMagCal->finvA, pthisMagCal->fA);
795
796 // compute the trial hard iron vector in offset bit counts times FMATRIXSCALING
797 for (l = X; l <= Z; l++)
798 {
799 pthisMagCal->ftrV[l] = 0.0F;
800 for (m = X; m <= Z; m++)
801 {
802 pthisMagCal->ftrV[l] += pthisMagCal->finvA[l][m] * pthisMagCal->fmatB[m + 6][j];
803 }
804 pthisMagCal->ftrV[l] *= -0.5F;
805 }
806
807 // compute the trial geomagnetic field strength B in bit counts times FMATRIXSCALING
808 pthisMagCal->ftrB = sqrtf(fabs(pthisMagCal->fA[0][0] * pthisMagCal->ftrV[X] * pthisMagCal->ftrV[X] +
809 2.0F * pthisMagCal->fA[0][1] * pthisMagCal->ftrV[X] * pthisMagCal->ftrV[Y] +
810 2.0F * pthisMagCal->fA[0][2] * pthisMagCal->ftrV[X] * pthisMagCal->ftrV[Z] +
811 pthisMagCal->fA[1][1] * pthisMagCal->ftrV[Y] * pthisMagCal->ftrV[Y] +
812 2.0F * pthisMagCal->fA[1][2] * pthisMagCal->ftrV[Y] * pthisMagCal->ftrV[Z] +
813 pthisMagCal->fA[2][2] * pthisMagCal->ftrV[Z] * pthisMagCal->ftrV[Z] - pthisMagCal->fmatB[9][j]));
814
815 // calculate the trial normalized fit error as a percentage
816 pthisMagCal->ftrFitErrorpc = 50.0F * sqrtf(fabs(pthisMagCal->fvecA[j]) / (float) pthisMagBuffer->iMagBufferCount) /
817 (pthisMagCal->ftrB * pthisMagCal->ftrB);
818
819 // correct for the measurement matrix offset and scaling and get the computed hard iron offset in uT
820 for (l = X; l <= Z; l++)
821 {
822 pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] * DEFAULTB + (float)iOffset[l]*FUTPERCOUNT;
823 }
824
825 // convert the trial geomagnetic field strength B into uT for un-normalized soft iron matrix A
826 pthisMagCal->ftrB *= DEFAULTB;
827
828 // normalize the ellipsoid matrix A to unit determinant and correct B by root of this multiplicative factor
829 f3x3matrixAeqAxScalar(pthisMagCal->fA, powf(det, -(ONETHIRD)));
830 pthisMagCal->ftrB *= powf(det, -(ONESIXTH));
831
832 // compute trial invW from the square root of fA (both with normalized determinant)
833 // set fvecA to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA
834 // where fmatA holds the 3x3 matrix fA in its top left elements
835 for (i = 0; i < 3; i++)
836 {
837 for (j = 0; j < 3; j++)
838 {
839 pthisMagCal->fmatA[i][j] = pthisMagCal->fA[i][j];
840 }
841 }
842 eigencompute(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB, 3);
843
844 // set pthisMagCal->fmatB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) = fmatB . diag(sqrt(sqrt(fvecA))
845 for (j = 0; j < 3; j++) // loop over columns j
846 {
847 ftmp = sqrtf(sqrtf(fabs(pthisMagCal->fvecA[j])));
848 for (i = 0; i < 3; i++) // loop over rows i
849 {
850 pthisMagCal->fmatB[i][j] *= ftmp;
851 }
852 }
853
854 // set ftrinvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T
855 // = fmatB * fmatB^T = sqrt(fA) (guaranteed symmetric)
856 // loop over rows
857 for (i = 0; i < 3; i++)
858 {
859 // loop over on and above diagonal columns
860 for (j = i; j < 3; j++)
861 {
862 pthisMagCal->ftrinvW[i][j] = 0.0F;
863 // accumulate the matrix product
864 for (k = 0; k < 3; k++)
865 {
866 pthisMagCal->ftrinvW[i][j] += pthisMagCal->fmatB[i][k] * pthisMagCal->fmatB[j][k];
867 }
868 // copy to below diagonal element
869 pthisMagCal->ftrinvW[j][i] = pthisMagCal->ftrinvW[i][j];
870 }
871 }
872
873 return;
874 }
875
876
MagneticBuffer
Definition: magnetic.h:33
MagneticBuffer::iBp
int16 iBp[3][MAGBUFFSIZEX][MAGBUFFSIZEY]
Definition: magnetic.h:35
fRunMagCalibration
void fRunMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
Definition: magnetic.c:270
AccelSensor::iGp
int16 iGp[3]
Definition: sensor_data_types.h:35
MagCalibration::fvecA
float fvecA[10]
Definition: magnetic.h:57
eigencompute
void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8 n)
Definition: matrix.c:182
fUpdateCalibration4INV
void fUpdateCalibration4INV(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
Definition: magnetic.c:344
MagSensor::fBp
float fBp[3]
Definition: sensor_data_types.h:43
MagCalibration::fV
float fV[3]
Definition: magnetic.h:44
MagneticBuffer::tanarray
int16 tanarray[MAGBUFFSIZEX-1]
Definition: magnetic.h:37
SENSORFS
#define SENSORFS
Definition: fusion_config.h:21
fUpdateCalibration7EIG
void fUpdateCalibration7EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
Definition: magnetic.c:522
ONETHIRD
#define ONETHIRD
Definition: magnetic.c:38
MagCalibration::ftrinvW
float ftrinvW[3][3]
Definition: magnetic.h:50
MAXMEASUREMENTS
#define MAXMEASUREMENTS
Definition: magnetic_config.h:31
math_constants.h
f3x3matrixAeqMinusA
void f3x3matrixAeqMinusA(float A[][3])
Definition: matrix.c:113
f3x3matrixAeqAxScalar
void f3x3matrixAeqAxScalar(float A[][3], float Scalar)
Definition: matrix.c:94
INTERVAL7CAL
#define INTERVAL7CAL
Definition: magnetic_config.h:36
MagCalibration::fmatA
float fmatA[10][10]
Definition: magnetic.h:55
INV_DEFAULTB
#define INV_DEFAULTB
Definition: magnetic_config.h:13
INTERVAL4CAL
#define INTERVAL4CAL
Definition: magnetic_config.h:35
f3x3matrixDetA
float f3x3matrixDetA(float A[][3])
Definition: matrix.c:169
X
Definition: matrix.h:33
int8
signed char int8
Definition: basic_types.h:12
fInitMagCalibration
void fInitMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer)
Definition: magnetic.c:42
AccelSensor
Definition: sensor_data_types.h:28
MagCalibration::iValidMagCal
int8 iValidMagCal
Definition: magnetic.h:61
MagSensor
Definition: sensor_data_types.h:39
MagCalibration::fA
float fA[3][3]
Definition: magnetic.h:53
MagneticBuffer::index
int32 index[MAGBUFFSIZEX][MAGBUFFSIZEY]
Definition: magnetic.h:36
MESHDELTACOUNTS
#define MESHDELTACOUNTS
Definition: magnetic_config.h:46
FITERRORAGINGSECS
#define FITERRORAGINGSECS
Definition: magnetic_config.h:40
MagCalibration::iCalInProgress
int8 iCalInProgress
Definition: magnetic.h:59
MagCalibration::ftrV
float ftrV[3]
Definition: magnetic.h:49
MAG_OVERSAMPLE_RATIO
#define MAG_OVERSAMPLE_RATIO
Definition: fusion_config.h:23
MagCalibration::ftrFitErrorpc
float ftrFitErrorpc
Definition: magnetic.h:52
MAGBUFFSIZEX
#define MAGBUFFSIZEX
Definition: magnetic_config.h:26
MAGBUFFSIZEY
#define MAGBUFFSIZEY
Definition: magnetic_config.h:27
MagCalibration::fFitErrorpc
float fFitErrorpc
Definition: magnetic.h:48
MagCalibration::iMagCalHasRun
int8 iMagCalHasRun
Definition: magnetic.h:60
iUpdateMagnetometerBuffer
void iUpdateMagnetometerBuffer(struct MagneticBuffer *pthisMagBuffer, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, int32 loopcounter)
Definition: magnetic.c:79
PI
#define PI
Definition: math_constants.h:12
f3x3matrixAeqInvSymB
void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
Definition: matrix.c:134
matrix.h
MagCalibration::fvecB
float fvecB[4]
Definition: magnetic.h:58
magnetic.h
MagCalibration::finvA
float finvA[3][3]
Definition: magnetic.h:54
MagCalibration::fFourBsq
float fFourBsq
Definition: magnetic.h:47
MagCalibration::fB
float fB
Definition: magnetic.h:46
FUTPERCOUNT
#define FUTPERCOUNT
Definition: magnetic_config.h:43
fUpdateCalibration10EIG
void fUpdateCalibration10EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
Definition: magnetic.c:669
int32
long int32
This defines int32 as long.
Definition: isf_types.h:32
Y
Definition: matrix.h:34
int16
short int16
This defines int16 as short.
Definition: isf_types.h:23
f3x3matrixAeqI
void f3x3matrixAeqI(float A[][3])
Definition: matrix.c:36
MagSensor::fBc
float fBc[3]
Definition: sensor_data_types.h:45
MagCalibration
Definition: magnetic.h:42
fInvertMagCal
void fInvertMagCal(struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal)
Definition: magnetic.c:247
INTERVAL10CAL
#define INTERVAL10CAL
Definition: magnetic_config.h:37
Z
Definition: matrix.h:35
MagCalibration::fmatB
float fmatB[10][10]
Definition: magnetic.h:56
MagCalibration::ftrB
float ftrB
Definition: magnetic.h:51
ONESIXTH
#define ONESIXTH
Definition: magnetic.c:39
f3x3matrixAeqScalar
void f3x3matrixAeqScalar(float A[][3], float Scalar)
Definition: matrix.c:76
MINBFITUT
#define MINBFITUT
Definition: magnetic_config.h:38
DEFAULTB
#define DEFAULTB
Definition: magnetic_config.h:12
MagneticBuffer::iMagBufferCount
int16 iMagBufferCount
Definition: magnetic.h:38
fmatrixAeqInvA
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize)
Definition: matrix.c:325
MAXBFITUT
#define MAXBFITUT
Definition: magnetic_config.h:39
MINMEASUREMENTS10CAL
#define MINMEASUREMENTS10CAL
Definition: magnetic_config.h:30
MagCalibration::finvW
float finvW[3][3]
Definition: magnetic.h:45
MINMEASUREMENTS7CAL
#define MINMEASUREMENTS7CAL
Definition: magnetic_config.h:29
MagSensor::iBp
int16 iBp[3]
Definition: sensor_data_types.h:49
- © Freescale Semiconductor, Inc. 2015. All Rights Reserved.