1 /* 2 * Copyright (c) 2015, Freescale Semiconductor, Inc. 3 * Copyright 2016-2017 NXP 4 * All rights reserved. 5 * 6 * SPDX-License-Identifier: BSD-3-Clause 7 */ 8 9 /*! \file magnetic.h 10 \brief Lower level magnetic calibration interface 11 12 Many developers can utilize the NXP Sensor Fusion Library without ever 13 making any adjustment to the lower level magnetic calibration functions 14 defined in this file. 15 */ 16 17 #ifndef MAGNETIC_H 18 #define MAGNETIC_H 19 20 #ifndef F_USING_MAG 21 #define F_USING_MAG 0x0002 // normally should be defined in build.h 22 #endif 23 24 #if F_USING_MAG 25 /// @name Magnetic Calibration Constants 26 ///@{ 27 #define MAGBUFFSIZEX 14 ///< x dimension in magnetometer buffer (12x24 equals 288 elements) 28 #define MAGBUFFSIZEY (2 * MAGBUFFSIZEX) ///< y dimension in magnetometer buffer (12x24 equals 288 elements) 29 #define MINMEASUREMENTS4CAL 110 ///< minimum number of measurements for 4 element calibration 30 #define MINMEASUREMENTS7CAL 220 ///< minimum number of measurements for 7 element calibration 31 #define MINMEASUREMENTS10CAL 330 ///< minimum number of measurements for 10 element calibration 32 #define MAXMEASUREMENTS 360 ///< maximum number of measurements used for calibration 33 #define CAL_INTERVAL_SECS 300 ///< 300s or 5min interval for regular calibration checks 34 #define MINBFITUT 10.0F ///< minimum acceptable geomagnetic field B (uT) for valid calibration 35 #define MAXBFITUT 90.0F ///< maximum acceptable geomagnetic field B (uT) for valid calibration 36 #define FITERRORAGINGSECS 86400.0F ///< 24 hours: time (s) for fit error to increase (age) by e=2.718 37 #define MESHDELTACOUNTS 50 ///< magnetic buffer mesh spacing in counts (here 5uT) 38 #define DEFAULTB 50.0F ///< default geomagnetic field (uT) 39 ///@} 40 41 /// The Magnetometer Measurement Buffer holds a 3-dimensional "constellation" 42 /// of data points. 43 /// 44 /// The constellation of points are used to compute magnetic hard/soft iron compensation terms. 45 /// The contents of this buffer are updated on a continuing basis. 46 struct MagBuffer 47 { 48 int16_t iBs[3][MAGBUFFSIZEX][MAGBUFFSIZEY]; ///< uncalibrated magnetometer readings 49 int32_t index[MAGBUFFSIZEX][MAGBUFFSIZEY]; ///< array of time indices 50 int16_t tanarray[MAGBUFFSIZEX - 1]; ///< array of tangents of (100 * angle) 51 int16_t iMagBufferCount; ///< number of magnetometer readings 52 }; 53 54 /// Magnetic Calibration Structure 55 struct MagCalibration 56 { 57 // start of elements stored to flash memory on Save (16 * 4 = 64 bytes) 58 float fV[3]; ///< current hard iron offset x, y, z, (uT) 59 float finvW[3][3]; ///< current inverse soft iron matrix 60 float fB; ///< current geomagnetic field magnitude (uT) 61 float fBSq; ///< square of fB (uT^2) 62 float fFitErrorpc; ///< current fit error % 63 int32_t iValidMagCal; ///< solver used: 0 (no calibration) or 4, 7, 10 element 64 // end of elements stored to flash memory 65 // start of working elements not stored to flash memory 66 float ftrV[3]; ///< trial value of hard iron offset z, y, z (uT) 67 float ftrinvW[3][3]; ///< trial inverse soft iron matrix size 68 float ftrB; ///< trial value of geomagnetic field magnitude in uT 69 float ftrFitErrorpc; ///< trial value of fit error % 70 float fA[3][3]; ///< ellipsoid matrix A 71 float finvA[3][3]; ///< inverse of ellipsoid matrix A 72 float fmatA[10][10]; ///< scratch 10x10 float matrix used by calibration algorithms 73 float fmatB[10][10]; ///< scratch 10x10 float matrix used by calibration algorithms 74 float fvecA[10]; ///< scratch 10x1 vector used by calibration algorithms 75 float fvecB[4]; ///< scratch 4x1 vector used by calibration algorithms 76 float fYTY; ///< Y^T.Y for 4 element calibration = (iB^2)^2 77 int32_t iSumBs[3]; ///< sum of measurements in buffer (counts) 78 int32_t iMeanBs[3]; ///< average magnetic measurement (counts) 79 int32_t itimeslice; ///< counter for tine slicing magnetic calibration calculations 80 int8_t iCalInProgress; ///< flag denoting that a calibration is in progress 81 int8_t iNewCalibrationAvailable; ///< flag denoting that a new calibration has been computed 82 int8_t iInitiateMagCal; ///< flag to start a new magnetic calibration 83 int8_t iMagBufferReadOnly; ///< flag to denote that the magnetic measurement buffer is temporarily read only 84 int8_t i4ElementSolverTried; ///< flag to denote at least one attempt made with 4 element calibration 85 int8_t i7ElementSolverTried; ///< flag to denote at least one attempt made with 4 element calibration 86 int8_t i10ElementSolverTried; ///< flag to denote at least one attempt made with 4 element calibration 87 }; 88 89 90 struct MagSensor; // actual typedef is located in sensor_fusion_types.h 91 92 /// @name Function prototypes for functions in magnetic.c 93 /// These functions comprise the core of the magnetic calibration features of 94 /// the library. Parameter descriptions are not included here, 95 /// as details are provided in sensor_fusion.h. 96 ///@{ 97 void fInitializeMagCalibration(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer); 98 void iUpdateMagBuffer(struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag, int32_t loopcounter); 99 void fInvertMagCal(struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal); 100 void fRunMagCalibration(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer, struct MagSensor* pthisMag, int32_t loopcounter); 101 void fUpdateMagCalibration4(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag); 102 void fUpdateMagCalibration7(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag); 103 void fUpdateMagCalibration10(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag); 104 void fUpdateMagCalibration4Slice(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag); 105 void fUpdateMagCalibration7Slice(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag); 106 void fUpdateMagCalibration10Slice(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag); 107 ///@} 108 #else // if F_USING_MAG 109 struct MagBuffer 110 { 111 void *placeholder; 112 }; 113 114 /// Magnetic Calibration Structure 115 struct MagCalibration 116 { 117 void *placeholder; 118 }; 119 #endif // if F_USING_MAG 120 #endif // #ifndef MAGNETIC_H 121
