| ISF 2.2 rev 5 Intelligent Sensing Framework for Kinetis with Processor Expert

- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

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