KL46 Accelerometer + Magnetometer, with 3-axis calibration. Readout through OpenSDA CDC.
Dependencies: MAG3110 MMA8451Q USBDevice mbed
magnetic.h@0:c569d820861b, 2014-04-10 (annotated)
- Committer:
- wue
- Date:
- Thu Apr 10 07:48:46 2014 +0000
- Revision:
- 0:c569d820861b
KL46Z Accelerometer & Magnetometer readout over OpenSDA CDC.; 3-axis 4-value calibration taken from eCompass.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
wue | 0:c569d820861b | 1 | // Source: eCompass v3 |
wue | 0:c569d820861b | 2 | |
wue | 0:c569d820861b | 3 | #include "mbed.h" |
wue | 0:c569d820861b | 4 | |
wue | 0:c569d820861b | 5 | #ifndef _MAGNETIC_H_ |
wue | 0:c569d820861b | 6 | #define _MAGNETIC_H_ |
wue | 0:c569d820861b | 7 | |
wue | 0:c569d820861b | 8 | typedef int16_t int16; |
wue | 0:c569d820861b | 9 | typedef uint16_t uint16; |
wue | 0:c569d820861b | 10 | typedef int32_t int32; |
wue | 0:c569d820861b | 11 | typedef uint32_t uint32; |
wue | 0:c569d820861b | 12 | |
wue | 0:c569d820861b | 13 | #define MAGBUFFSIZE (216) // magnetic buffer size: 6 implies 6^3 = 216 entries |
wue | 0:c569d820861b | 14 | #define FINVMATRIXSCALING (50.0F) // inverse of FMATRIXSCALING |
wue | 0:c569d820861b | 15 | #define FUTPERCOUNT (0.1F) // MAG3110 and FXOS8700 provide 0.1uT per count resolution |
wue | 0:c569d820861b | 16 | #define FMATRIXSCALING (0.02F) // approx normalizes geomagnetic field 50uT |
wue | 0:c569d820861b | 17 | #define MAXMATINV 6 // maximum size supported in matrix inverse function |
wue | 0:c569d820861b | 18 | |
wue | 0:c569d820861b | 19 | // 3d arrays, dude... |
wue | 0:c569d820861b | 20 | |
wue | 0:c569d820861b | 21 | struct MagneticBuffer { |
wue | 0:c569d820861b | 22 | int16 iBx[MAGBUFFSIZE]/*[MAGBUFFSIZE][MAGBUFFSIZE]*/; // array of x magnetic fields |
wue | 0:c569d820861b | 23 | int16 iBy[MAGBUFFSIZE]/*[MAGBUFFSIZE][MAGBUFFSIZE]*/; // array of y magnetic fields |
wue | 0:c569d820861b | 24 | int16 iBz[MAGBUFFSIZE]/*[MAGBUFFSIZE][MAGBUFFSIZE]*/; // array of z magnetic fields |
wue | 0:c569d820861b | 25 | //int32 index[MAGBUFFSIZE][MAGBUFFSIZE][MAGBUFFSIZE]; // array of time indices |
wue | 0:c569d820861b | 26 | //int32 iMagBufferCount; // number of magnetometer readings |
wue | 0:c569d820861b | 27 | }; |
wue | 0:c569d820861b | 28 | |
wue | 0:c569d820861b | 29 | // magnetic calibration structure |
wue | 0:c569d820861b | 30 | struct MagCalibration |
wue | 0:c569d820861b | 31 | { |
wue | 0:c569d820861b | 32 | float fVx; // x component of computed hard iron offset |
wue | 0:c569d820861b | 33 | float fVy; // y component of computed hard iron offset |
wue | 0:c569d820861b | 34 | float fVz; // z component of computed hard iron offset |
wue | 0:c569d820861b | 35 | float fB; // computed geomagnetic field magnitude in uT |
wue | 0:c569d820861b | 36 | float fFitErrorpc; // computed fit error % |
wue | 0:c569d820861b | 37 | float ftrVx; // trial value of x component of hard iron offset |
wue | 0:c569d820861b | 38 | float ftrVy; // trial value of y component of hard iron offset |
wue | 0:c569d820861b | 39 | float ftrVz; // trial value of z component of hard iron offset |
wue | 0:c569d820861b | 40 | float ftrB; // trial value of geomagnetic field magnitude in uT |
wue | 0:c569d820861b | 41 | float ftrFitErrorpc; // trial value of fit error % |
wue | 0:c569d820861b | 42 | int32 iValidMagCal; // valid magnetic calibration flag |
wue | 0:c569d820861b | 43 | float xfinvW[3][3]; // estimated inverse soft iron matrix size |
wue | 0:c569d820861b | 44 | float *finvW[3]; |
wue | 0:c569d820861b | 45 | float xfA[3][3]; // estimated ellipsoid matrix A |
wue | 0:c569d820861b | 46 | float *fA[3]; |
wue | 0:c569d820861b | 47 | float xinvA[3][3]; // inverse of ellipsoid matrix A |
wue | 0:c569d820861b | 48 | float *finvA[3]; |
wue | 0:c569d820861b | 49 | float xftrinvW[3][3]; // trial computed inverse soft iron matrix size |
wue | 0:c569d820861b | 50 | float *ftrinvW[3]; |
wue | 0:c569d820861b | 51 | }; |
wue | 0:c569d820861b | 52 | |
wue | 0:c569d820861b | 53 | void fUpdateCalibration4INV(struct MagCalibration *pthisMagCal, |
wue | 0:c569d820861b | 54 | struct MagneticBuffer *pthisMagneticBuffer, |
wue | 0:c569d820861b | 55 | float **ftmpA4x4, float **ftmpB4x4, float **ftmpA4x1, |
wue | 0:c569d820861b | 56 | float **ftmpB4x1, int32 **icolind, int32 **irowind, int32 **ipivot); |
wue | 0:c569d820861b | 57 | |
wue | 0:c569d820861b | 58 | void ResetMagCalibration(struct MagCalibration *pthisMagCal/*, struct MagneticBuffer *pthisMagneticBuffer*/); |
wue | 0:c569d820861b | 59 | |
wue | 0:c569d820861b | 60 | void magUpdateCalibration(struct MagCalibration *pthisMagCal, |
wue | 0:c569d820861b | 61 | struct MagneticBuffer *pthisMagneticBuffer); |
wue | 0:c569d820861b | 62 | |
wue | 0:c569d820861b | 63 | #endif |