src/main/common/filter.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdlib.h>
  21 #include <math.h>
  22
  23 #include "common/axis.h"
  24 #include "common/filter.h"
  25 #include "common/maths.h"
  26
  27 #include "drivers/gyro_sync.h"
  28
  29 #define BIQUAD_Q    (1.0f / 1.41421356f)     /* quality factor - butterworth (1 / sqrt(2)) */
  30
  31 /* sets up a biquad Filter */
  32 void filterInitBiQuad(uint8_t filterCutFreq, biquad_t *newState, int16_t samplingRate)
  33 {
  34     float omega, sn, cs, alpha;
  35     float a0, a1, a2, b0, b1, b2;
  36
  37     /* If sampling rate == 0 - use main loop target rate */
  38     if (!samplingRate) {
  39         samplingRate = 1000000 / targetLooptime;
  40     }
  41
  42     /* setup variables */
  43     omega = 2 * M_PIf * (float)filterCutFreq / (float)samplingRate;
  44     sn = sin_approx(omega);
  45     cs = cos_approx(omega);
  46     alpha = sn / (2 * BIQUAD_Q);
  47
  48     b0 = (1 - cs) / 2;
  49     b1 = 1 - cs;
  50     b2 = (1 - cs) / 2;
  51     a0 = 1 + alpha;
  52     a1 = -2 * cs;
  53     a2 = 1 - alpha;
  54
  55     /* precompute the coefficients */
  56     newState->b0 = b0 / a0;
  57     newState->b1 = b1 / a0;
  58     newState->b2 = b2 / a0;
  59     newState->a1 = a1 / a0;
  60     newState->a2 = a2 / a0;
  61
  62     /* zero initial samples */
  63     newState->d1 = newState->d2 = 1;
  64 }
  65
  66 /* Computes a biquad_t filter on a sample */
  67 float filterApplyBiQuad(float sample, biquad_t *state)
  68 {
  69     float result;
  70
  71     result = state->b0 * sample + state->d1;
  72     state->d1 = state->b1 * sample - state->a1 * result + state->d2;
  73     state->d2 = state->b2 * sample - state->a2 * result;
  74
  75     return result;
  76 }
  77
  78 // PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
  79 float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dT)
  80 {
  81         // Pre calculate and store RC
  82         if (!filter->RC) {
  83                 filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
  84         }
  85
  86     filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
  87     return filter->state;
  88 }
  89
  90 // PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
  91 // f_cut = cutoff frequency
  92 // rate_limit = maximum rate of change of the output value in units per second
  93 float filterApplyPt1WithRateLimit(float input, filterStatePt1_t *filter, float f_cut, float rate_limit, float dT)
  94 {
  95         // Pre calculate and store RC
  96         if (!filter->RC) {
  97                 filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
  98         }
  99
 100     const float newState = filter->state + dT / (filter->RC + dT) * (input - filter->state);
 101     const float rateLimitPerSample = rate_limit * dT;
 102     filter->state = constrainf(newState, filter->state - rateLimitPerSample, filter->state + rateLimitPerSample);
 103
 104     return filter->state;
 105 }
 106
 107 void filterResetPt1(filterStatePt1_t *filter, float input)
 108 {
 109     filter->state = input;
 110 }
 111
 112 void filterUpdateFIR(int filterLength, float *shiftBuf, float newSample)
 113 {
 114     // Shift history buffer and push new sample
 115     for (int i = filterLength - 1; i > 0; i--)
 116         shiftBuf[i] = shiftBuf[i - 1];
 117
 118     shiftBuf[0] = newSample;
 119 }
 120
 121 float filterApplyFIR(int filterLength, const float *shiftBuf, const float *coeffBuf, float commonMultiplier)
 122 {
 123     float accum = 0;
 124
 125     for (int i = 0; i < filterLength; i++)
 126         accum += shiftBuf[i] * coeffBuf[i];
 127
 128     return accum * commonMultiplier;
 129 }