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
  23 #include <stdbool.h>
  24 #include <stdint.h>
  25
  26 struct filter_s;
  27 typedef struct filter_s filter_t;
  28 typedef float (*filterApplyFnPtr)(filter_t *filter, float input);
  29
  30 typedef enum {
  31     FILTER_PT1 = 0,
  32     FILTER_BIQUAD,
  33     FILTER_PT2,
  34     FILTER_PT3,
  35 } lowpassFilterType_e;
  36
  37 typedef enum {
  38     FILTER_LPF,    // 2nd order Butterworth section
  39     FILTER_NOTCH,
  40     FILTER_BPF,
  41 } biquadFilterType_e;
  42
  43 typedef struct pt1Filter_s {
  44     float state;
  45     float k;
  46 } pt1Filter_t;
  47
  48 typedef struct pt2Filter_s {
  49     float state;
  50     float state1;
  51     float k;
  52 } pt2Filter_t;
  53
  54 typedef struct pt3Filter_s {
  55     float state;
  56     float state1;
  57     float state2;
  58     float k;
  59 } pt3Filter_t;
  60
  61 /* this holds the data required to update samples thru a filter */
  62 typedef struct biquadFilter_s {
  63     float b0, b1, b2, a1, a2;
  64     float x1, x2, y1, y2;
  65     float weight;
  66 } biquadFilter_t;
  67
  68 typedef struct phaseComp_s {
  69     float b0, b1, a1;
  70     float x1, y1;
  71 } phaseComp_t;
  72
  73 typedef struct slewFilter_s {
  74     float state;
  75     float slewLimit;
  76     float threshold;
  77 } slewFilter_t;
  78
  79 typedef struct laggedMovingAverage_s {
  80     uint16_t movingWindowIndex;
  81     uint16_t windowSize;
  82     float movingSum;
  83     float *buf;
  84     bool primed;
  85 } laggedMovingAverage_t;
  86
  87 typedef struct simpleLowpassFilter_s {
  88     int32_t fp;
  89     int32_t beta;
  90     int32_t fpShift;
  91 } simpleLowpassFilter_t;
  92
  93 typedef struct meanAccumulator_s {
  94     int32_t accumulator;
  95     int32_t count;
  96 } meanAccumulator_t;
  97
  98 float nullFilterApply(filter_t *filter, float input);
  99
 100 float pt1FilterGain(float f_cut, float dT);
 101 void pt1FilterInit(pt1Filter_t *filter, float k);
 102 void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k);
 103 float pt1FilterApply(pt1Filter_t *filter, float input);
 104
 105 float pt2FilterGain(float f_cut, float dT);
 106 void pt2FilterInit(pt2Filter_t *filter, float k);
 107 void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k);
 108 float pt2FilterApply(pt2Filter_t *filter, float input);
 109
 110 float pt3FilterGain(float f_cut, float dT);
 111 void pt3FilterInit(pt3Filter_t *filter, float k);
 112 void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k);
 113 float pt3FilterApply(pt3Filter_t *filter, float input);
 114
 115 float filterGetNotchQ(float centerFreq, float cutoffFreq);
 116
 117 void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
 118 void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
 119 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
 120 void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
 121 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
 122 float biquadFilterApplyDF1Weighted(biquadFilter_t *filter, float input);
 123 float biquadFilterApply(biquadFilter_t *filter, float input);
 124
 125 void phaseCompInit(phaseComp_t *filter, const float centerFreq, const float centerPhase, const uint32_t looptimeUs);
 126 void phaseCompUpdate(phaseComp_t *filter, const float centerFreq, const float centerPhase, const uint32_t looptimeUs);
 127 float phaseCompApply(phaseComp_t *filter, const float input);
 128
 129 void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
 130 float slewFilterApply(slewFilter_t *filter, float input);
 131
 132 void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf);
 133 float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input);
 134
 135 void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpShift);
 136 int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal);
 137
 138 void meanAccumulatorInit(meanAccumulator_t *filter);
 139 void meanAccumulatorAdd(meanAccumulator_t *filter, const int8_t newVal);
 140 int8_t meanAccumulatorCalc(meanAccumulator_t *filter, const int8_t defaultValue);