FusionBias.c

00001 /**
00002  * @file FusionBias.c
00003  * @author Seb Madgwick
00004  * @brief The gyroscope bias correction algorithm achieves run-time calibration
00005  * of the gyroscope bias.  The algorithm will detect when the gyroscope is
00006  * stationary for a set period of time and then begin to sample gyroscope
00007  * measurements to calculate the bias as an average.
00008  */
00009 
00010 //------------------------------------------------------------------------------
00011 // Includes
00012 
00013 #include "FusionBias.h"
00014 #include "math.h" // fabs
00015 
00016 //------------------------------------------------------------------------------
00017 // Definitions
00018 
00019 /**
00020  * @brief Minimum stationary period (in seconds) after which the the algorithm
00021  * becomes active and begins sampling gyroscope measurements.
00022  */
00023 #define STATIONARY_PERIOD (5.0f)   // 5.0
00024 
00025 /**
00026  * @brief Corner frequency (in Hz) of the high-pass filter used to sample the
00027  * gyroscope bias.
00028  */
00029 #define CORNER_FREQUENCY (0.02f)  //0.02
00030 
00031 //------------------------------------------------------------------------------
00032 // Functions
00033 
00034 /**
00035  * @brief Initialises the gyroscope bias correction algorithm.
00036  * @param fusionBias FusionBias structure.
00037  * @param threshold Gyroscope threshold (in degrees per second) below which the
00038  * gyroscope is detected stationary.
00039  * @param samplePeriod Nominal sample period (in seconds) corresponding the rate
00040  * at which the application will update the algorithm.
00041  */
00042 void FusionBiasInitialise(FusionBias * const fusionBias, const float threshold, const float samplePeriod) {
00043     fusionBias->threshold = threshold;
00044     fusionBias->samplePeriod = samplePeriod;
00045     fusionBias->filterCoefficient = (2.0f * M_PI * CORNER_FREQUENCY) * fusionBias->samplePeriod;
00046     fusionBias->stationaryTimer = 0.0f;
00047     fusionBias->gyroscopeBias = FUSION_VECTOR3_ZERO;
00048 }
00049 
00050 /**
00051  * @brief Updates the gyroscope bias correction algorithm and returns the
00052  * corrected gyroscope measurement.
00053  * @param fusionBias FusionBias structure.
00054  * @param gyroscope Gyroscope measurement in degrees per second.
00055  * @return Corrected gyroscope measurement in degrees per second.
00056  */
00057 FusionVector3 FusionBiasUpdate(FusionBias * const fusionBias, FusionVector3 gyroscope) {
00058 
00059     // Subtract bias from gyroscope measurement
00060     gyroscope = FusionVectorSubtract(gyroscope, fusionBias->gyroscopeBias);
00061 
00062     // Reset stationary timer if gyroscope not stationary
00063     if ((fabs(gyroscope.axis.x) > fusionBias->threshold) || (fabs(gyroscope.axis.y) > fusionBias->threshold) || (fabs(gyroscope.axis.z) > fusionBias->threshold)) {
00064         fusionBias->stationaryTimer = 0.0f;
00065         return gyroscope;
00066     }
00067 
00068     // Increment stationary timer while gyroscope stationary
00069     if (fusionBias->stationaryTimer < STATIONARY_PERIOD) {
00070         fusionBias->stationaryTimer += fusionBias->samplePeriod;
00071         return gyroscope;
00072     }
00073 
00074     // Adjust bias if stationary timer has elapsed
00075     fusionBias->gyroscopeBias = FusionVectorAdd(fusionBias->gyroscopeBias, FusionVectorMultiplyScalar(gyroscope, fusionBias->filterCoefficient));
00076     return gyroscope;
00077 }
00078 
00079 /**
00080  * @brief Returns true if the gyroscope bias correction algorithm is active.
00081  * @param fusionBias FusionBias structure.
00082  * @return True if the gyroscope bias correction algorithm is active.
00083  */
00084 bool FusionBiasIsActive(FusionBias * const fusionBias) {
00085     return fusionBias->stationaryTimer >= STATIONARY_PERIOD;
00086 }
00087 
00088 //------------------------------------------------------------------------------
00089 // End of file