FusionCalibration.h

00001 /**
00002  * @file FusionCalibration.h
00003  * @author Seb Madgwick
00004  * @brief Gyroscope, accelerometer, and magnetometer calibration model.
00005  *
00006  * Static inline implementations are used to optimise for increased execution
00007  * speed.
00008  */
00009 
00010 #ifndef FUSION_CALIBRATION_H
00011 #define FUSION_CALIBRATION_H
00012 
00013 //------------------------------------------------------------------------------
00014 // Includes
00015 
00016 #include "FusionTypes.h"
00017 
00018 //------------------------------------------------------------------------------
00019 // Inline functions
00020 
00021 /**
00022  * @brief Gyroscope and accelerometer calibration model.
00023  * @param uncalibrated Uncalibrated gyroscope or accelerometer measurement in
00024  * lsb.
00025  * @param misalignment Misalignment matrix (may not be a true rotation matrix).
00026  * @param sensitivity Sensitivity in g per lsb for an accelerometer and degrees
00027  * per second per lsb for a gyroscope.
00028  * @param bias Bias in lsb.
00029  * @return Calibrated gyroscope or accelerometer measurement.
00030  */
00031 static inline __attribute__((always_inline)) FusionVector3 FusionCalibrationInertial(const FusionVector3 uncalibrated, const FusionRotationMatrix misalignment, const FusionVector3 sensitivity, const FusionVector3 bias) {
00032     return FusionRotationMatrixMultiplyVector(misalignment, FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, bias), sensitivity));
00033 }
00034 
00035 /**
00036  * @brief Magnetometer calibration model.
00037  * @param magnetometer Uncalibrated magnetometer measurement in uT.
00038  * @param softIronMatrix Soft-iron matrix (may not be a true rotation matrix).
00039  * @param hardIronBias Hard-iron bias in uT.
00040  * @return Calibrated magnetometer measurement.
00041  */
00042 static inline __attribute__((always_inline)) FusionVector3 FusionCalibrationMagnetic(const FusionVector3 uncalibrated, const FusionRotationMatrix softIronMatrix, const FusionVector3 hardIronBias) {
00043     return FusionVectorSubtract(FusionRotationMatrixMultiplyVector(softIronMatrix, uncalibrated), hardIronBias);
00044 }
00045 
00046 #endif
00047 
00048 //------------------------------------------------------------------------------
00049 // End of file