/* VectorNav HSI Calibration Library v1.0.0.2 * * Copyright (c) 2018 VectorNav Technologies, LLC * * This source code is proprietary to and copyrighted by VectorNav Technologies, LLC and is * available solely under license from VectorNav. All rights reserved. If you do not have a * license to use this source code from VectorNav, any use hereof is strictly prohibited by * U.S. law and other applicable law. This source code is restricted for use solely with the * products of VectorNav and as otherwise set forth in any agreement with VectorNav. Any * disclosure of this source code to the public or to any third party is also prohibited. */ /** \file * * \section Description * This header file contains declarations for using VectorNav HSI tool. */ #ifndef VNHSI_H_INCLUDED #define VNHSI_H_INCLUDED #include "vnbool.h" #include "vnvector.h" #include "vnmatrix.h" #include /* max number of patches - Leave undefined to eliminate patches from build - significantly reduces RAM requirements*/ #define MAXPATCHES 212 #if !VN_HAVE_EXTRA_MATH_FUNCS #define sinf(x) (sin(x)) #define cosf(x) (cos(x)) #define acosf(x) (acos(x)) #define asinf(x) (asin(x)) #define atan2f(x,y) (atan2(x,y)) #define fabsf(x) (fabs(x)) #define sqrtf(x) (sqrt(x)) #endif #ifdef __cplusplus extern "C" { #endif /** \brief Enumerator representing the HSI different modes. * * The VnHsiLibMode Enumerator contains all possible HSI modes which could be used within the HSI tool. * These available modes are; the 3D mode, the 2D mode, the 3D with no location, the 2D with no location, * and the 2D with scaled S33 (last element of the soft iron matrix). */ typedef enum VnHsiLibMode { HSI_MODE_3D = 1, /**< The HSI_MODE_3D field. The 3D mode with correct location. */ HSI_MODE_2D = 2, /**< The HSI_MODE_2D field. The 2D mode with correct location. */ HSI_MODE_3D_NOLOCATION = 3, /**< The HSI_MODE_3D_NOLOCATION field. The 3D mode with no location. */ HSI_MODE_2D_NOLOCATION = 4, /**< The HSI_MODE_2D_NOLOCATION field. The 2D mode with no location. */ HSI_MODE_2D_SCALE_S33 = 5, /**< The HSI_MODE_2D_SCALE_S33 field. Same as mode 2 with scaled S33. */ HSI_MODE_MAX = 6 /**< The HSI_MODE_MAX field. The maximum number of modes. */ } VnHsiLibMode; /** \brief Structure representing the HSI Configurations. * * The VnHsiConfig structure contains all the required fields to define the * configurations used in the HSI tool. */ typedef struct { VnHsiLibMode mode; /**< The mode field. Calibration mode - 1=3D, 2=2D. */ union vn_mat3f oldSI; /**< The oldSI field. Soft-Iron matrix (Register 23) that was in effect when data was collected. */ union vn_vec3f oldHI; /**< The oldHI field. Hard-Iron vector HI (Register 23) that was in effect when data was collected.*/ union vn_mat3f RFR; /**< The RFR field. Reference Frame Rotation RFR (Register 26) that was in effect when data was collected.*/ float latitude; /**< The latitude field. Calibration location latitude (decimal degrees). */ float longitude; /**< The longitude field. Calibration location longitude (decimal degrees). */ unsigned int dataRate; /**< The dataRate field. 2D Mode Only: (not required for 3D calibration), Rate at which data is being logged from the sensor (Hz).*/ unsigned int nPatches; /**< The nPatches field. 3D mode: see flagKM for definition. 2D mode: data selection algorithm keeps a measurement whenever the heading has changed by 360/nPatches degrees. */ unsigned int nDataPerPatch; /**< The nDataPerPatch field. The number of data points per each patch - 3D Mode Only.*/ unsigned int flagKM; /**< The flagKM field. 3D Mode Only: (not required for 2D calibration) Sets the keep measurement mode as follows: 1 - Deprecated. Defines nPatches equal circular patches on the surface of a sphere. Only keeps nDataPerPatch points per patch according to when the magnetic vector passes through the patch. 2 - Similar to (1) but also defines a second set of patches to improve observability of the alignment data fit. 3 - Keeps a measurement whenever the attitude has changed by 360/nPatches degrees (does not rely on the spherical patch algorithms). */ } VnHsiConfig; /** \brief Structure representing the YMR measurement. * * The VnHsiYmr structure has the required measurement fields needed to operate the HSI functions. */ typedef struct VnHsiYmr { union vn_vec3f ypr; /**< The ypr field. Yaw, Pitch and Roll (in degrees). */ union vn_vec3f mag; /**< The mag field. The 3-axes magnetic reading. */ union vn_vec3f acc; /**< The acc field. The 3-axes acceleration reading. */ union vn_vec3f gyro; /**< The gyro field. The 3-axes gyro reading. */ size_t index; /**< The index field. The index of the current YMR reading. */ } VnHsiYmr; /** \brief Structure representing the figures of merit (FOM). * * The VnHsiFom structure has the figures of merit (FOM) computed by the HSI algorithms that provide useful feedback on the accuracy of the final solution. */ typedef struct VnHsiFom { float ellipsoidFitResidualsMean; /**< Mean of residuals from ellipsoid data fit (as percent sphere radius). */ float ellipsoidFitResidualsMax; /**< Max of residuals from ellipsoid data fit (as percent sphere radius). */ float ellipsoidFitResidualsStd; /**< Standard deviation of residuals from ellipsoid data fit (as percent sphere radius). */ float alignmentFitResidualsMean; /**< Mean of residuals from alignment data fit (as percent sphere radius). */ float alignmentFitResidualsMax; /**< Max of residuals from alignment data fit (as percent sphere radius). */ float alignmentFitResidualsStd; /**< Standard deviation of residuals from alignment data fit (as percent sphere radius). */ float ellipsoidCoverage; /**< Error multiplier based on distribution of data for ellipsoid data fit. */ float alignmentCoverage; /**< Error multiplier based on distribution of data for alignment data fit. */ } VnHsiFom; /** \brief Structure representing the magnetic field dipole model. * * The VnHsiDipole structure defines the local Earth's magnetic field dipole model. * The magnetic field model vector has 2 elements the Bnorth and the Bdown. * Bnorth is north component of Earth's magnetic field; * Bdown (positive down) component of Earth's magnetic field. */ typedef struct VnHsiDipole { float Bdown; /**< The Bdown field. The vertical (+ down) component of Earth's magnetic field.*/ float Bnorth; /**< The Bnorth field. The north component of Earth's magnetic field.*/ float sphereRad; /**< The sphereRad field. The norm of the magnetic field model vector. */ } VnHsiDipole; /** \brief Structure representing the HSI solution. * * The VnHsiSol structure has the calculated hard and soft iron parameters. */ typedef struct VnHsiSol { union vn_mat3f SI; /**< The SI field. 9 floats define the soft iron matrix.*/ union vn_vec3f HI; /**< The HI field. 3 floats define the hard iron vector.*/ float magInc; /**< The magInc field. The calculated magnetic inclination.*/ } VnHsiSol; /** \brief Initialize the HSI with the configuration parameters. * * \param[in] c The configuration structure. * \return None. */ void vnhsi_init(VnHsiConfig* c); /** \brief Determine if the current measurement should be kept or no. * * \param[in] c The configuration structure. * \param[in] meas The current YMR measurement. * \return true to keep measurement, otherwise false. */ bool vnhsi_keep(VnHsiConfig* c, VnHsiYmr meas); /** \brief Solve for the HSI 12 calibration parameters. * * \param[in] c The configuration structure. * \param[in] meas The kept YMR measurement. * \param[in] numMeas The number of kept measurements. * \param[out] hsisoln The HSI solution. * \param[out] FOM The figures of merit. * \return Error code. */ enum VnError vnhsi_solve(VnHsiConfig* c, VnHsiYmr* meas, size_t numMeas, VnHsiSol* hsisoln); /** \brief Calculate the figures of merit. * * \param[in] c The configuration structure. * \param[in] meas The kept YMR measurement. * \param[in] numMeas The number of kept measurements. * \param[in] hsisoln The HSI solution. * \param[out] FOM The figures of merit. * \param[out] fomScore Score 0-3 for each FOM based on predefined thresholds (0-Great,3-Poor). * \param[out] sphereResiduals The calculated sphere residuals. * \param[out] vertResiduals The calculated vertical (alignment) residuals. * \return None. */ void vnhsi_figure_of_merit(VnHsiConfig* c, VnHsiYmr* meas, size_t numMeas, VnHsiSol hsisoln, VnHsiFom* FOM, unsigned int fomScore[8], float* sphereResiduals, float* vertResiduals, float(*correctedMag)[3]); /** \brief Returns worst fomScore possible. * * \param[in] None. * \return fomScoreMax Worst fomScore possible. */ unsigned int vnhsi_get_max_fomScore(void); /** \brief Finalize the HSI results. * * \param[in] c The configuration structure. * \param[in] hsisoln The HSI solution. * \param[out] si_m3 The final soft iron matrix. * \param[out] hi_v3 The final hard iron vector. * \return None. */ void vnhsi_results(VnHsiConfig* c, union vn_mat3f* si, union vn_vec3f* hi, VnHsiSol hsisoln); #ifdef MAXPATCHES /** \brief Structure defines the HSI patches. * * The VnHsiPatch structure has the index of each patch used in the keep measurement function and the number of data points per each patch. */ typedef struct VnHsiPatch { int patchInd; /**< The patchInd field. The patch index.*/ int numData; /**< The numData field. The number of data points for the current patch.*/ } VnHsiPatch; /** \brief Structure representing the filled (selected) patches. * * The VnHsiPatchFill structure has the selected (kept) data point per each patch. */ typedef struct VnHsiPatchFill { int numData[MAXPATCHES]; /**< The numData field. The selected data points for the current patch.*/ unsigned int numFilled; /**< The numFilled field. The number of selected data points for the current patch.*/ } VnHsiPatchFill; /** \brief Structure representing the patch type. * * The VnHsiPatchType structure defines the selected patches either north or east patch. */ typedef enum VnHsiPatchType { NORTHPATCHTYPE, /**< The NORTHPATCHTYPE type.*/ EASTPATCHTYPE, /**< The EASTPATCHTYPE type.*/ TWODPATCHTYPE /**< The TWODPATCHTYPE type.*/ } VnHsiPatchType; /** \brief Structure representing the patch definition. * * The VnHsiPatchType structure defines the patch vector and cosine error angle. */ typedef struct VnHsiPatchDef { const float (*Patches)[3]; /**< The Patches field. A vector of 3 float representing the Patch vector.*/ float cosErr; /**< The cosErr field. The cosine error of the dot product between 2 vectors.*/ union vn_vec3f refYPR; /**< The refYPR field. The reference YPR (in deg).*/ } VnHsiPatchDef; /** \brief Get the patch information. * * \param[in] patchType The patch type. * \return VnHsiPatchFill structure. */ VnHsiPatchFill vnhsi_get_patch_fill(VnHsiPatchType patchType); /** \brief Return the patch definition. * * \param[in] None. * \return VnHsiPatchDef structure. */ VnHsiPatchDef vnhsi_get_patch_def(); /** \brief Gets the last patch index. * * \param[in] None. * \return Integer patch index. */ int vnhsi_get_last_patch(); #endif /* MAXPATCHES */ /** \brief Calculate the unit magnetic vector. * * \param[in] Dipole struct. * \return vec3f with the magnetic unit vector. */ union vn_vec3f vnhsi_unit_mag_ref(VnHsiDipole dp); /** \brief Get the Earth's magnetic dipole model. * * \param[in] updatedFlag Boolean flag: false returns original calculated values, true returns updated values calculated during solve. * \return VnHsiDipole The magnetic dipole. */ VnHsiDipole vnhsi_get_dipole(bool updatedFlag); #ifdef __cplusplus } #endif #endif /*VNHSI_H_INCLUDED*/