src/main/common/filter.h

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #pragma once
  22 #include <stdbool.h>
  23
  24 struct filter_s;
  25 typedef struct filter_s filter_t;
  26
  27 typedef struct pt1Filter_s {
  28     float state;
  29     float k;
  30 } pt1Filter_t;
  31
  32 typedef struct pt2Filter_s {
  33     float state;
  34     float state1;
  35     float k;
  36 } pt2Filter_t;
  37
  38 typedef struct pt3Filter_s {
  39     float state;
  40     float state1;
  41     float state2;
  42     float k;
  43 } pt3Filter_t;
  44
  45 typedef struct slewFilter_s {
  46     float state;
  47     float slewLimit;
  48     float threshold;
  49 } slewFilter_t;
  50
  51 /* this holds the data required to update samples thru a filter */
  52 typedef struct biquadFilter_s {
  53     float b0, b1, b2, a1, a2;
  54     float x1, x2, y1, y2;
  55     float weight;
  56 } biquadFilter_t;
  57
  58 typedef struct laggedMovingAverage_s {
  59     uint16_t movingWindowIndex;
  60     uint16_t windowSize;
  61     float movingSum;
  62     float *buf;
  63     bool primed;
  64 } laggedMovingAverage_t;
  65
  66 typedef enum {
  67     FILTER_PT1 = 0,
  68     FILTER_BIQUAD,
  69     FILTER_PT2,
  70     FILTER_PT3,
  71 } lowpassFilterType_e;
  72
  73 typedef enum {
  74     FILTER_LPF,    // 2nd order Butterworth section
  75     FILTER_NOTCH,
  76     FILTER_BPF,
  77 } biquadFilterType_e;
  78
  79 typedef float (*filterApplyFnPtr)(filter_t *filter, float input);
  80
  81 float nullFilterApply(filter_t *filter, float input);
  82
  83 void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
  84 void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
  85 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
  86 void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
  87
  88 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
  89 float biquadFilterApplyDF1Weighted(biquadFilter_t *filter, float input);
  90 float biquadFilterApply(biquadFilter_t *filter, float input);
  91 float filterGetNotchQ(float centerFreq, float cutoffFreq);
  92
  93 void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf);
  94 float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input);
  95
  96 float pt1FilterGain(float f_cut, float dT);
  97 void pt1FilterInit(pt1Filter_t *filter, float k);
  98 void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k);
  99 float pt1FilterApply(pt1Filter_t *filter, float input);
 100
 101 float pt2FilterGain(float f_cut, float dT);
 102 void pt2FilterInit(pt2Filter_t *filter, float k);
 103 void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k);
 104 float pt2FilterApply(pt2Filter_t *filter, float input);
 105
 106 float pt3FilterGain(float f_cut, float dT);
 107 void pt3FilterInit(pt3Filter_t *filter, float k);
 108 void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k);
 109 float pt3FilterApply(pt3Filter_t *filter, float input);
 110
 111 void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
 112 float slewFilterApply(slewFilter_t *filter, float input);
 113
 114 typedef struct simpleLowpassFilter_s {
 115     int32_t fp;
 116     int32_t beta;
 117     int32_t fpShift;
 118 } simpleLowpassFilter_t;
 119
 120 int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal);
 121 void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpShift);