src/main/common/filter.h

   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 #pragma once
  19
  20 typedef struct rateLimitFilter_s {
  21     float state;
  22 } rateLimitFilter_t;
  23
  24 typedef struct pt1Filter_s {
  25     float state;
  26     float RC;
  27     float dT;
  28     float alpha;
  29 } pt1Filter_t;
  30 typedef struct pt2Filter_s {
  31     float state;
  32     float state1;
  33     float k;
  34 } pt2Filter_t;
  35 typedef struct pt3Filter_s {
  36     float state;
  37     float state1;
  38     float state2;
  39     float k;
  40 } pt3Filter_t;
  41
  42 /* this holds the data required to update samples thru a filter */
  43 typedef struct biquadFilter_s {
  44     float b0, b1, b2, a1, a2;
  45     float x1, x2, y1, y2;
  46 } biquadFilter_t;
  47
  48 typedef union {
  49     biquadFilter_t biquad;
  50     pt1Filter_t pt1;
  51     pt2Filter_t pt2;
  52     pt3Filter_t pt3;
  53 } filter_t;
  54
  55 typedef enum {
  56     FILTER_PT1 = 0,
  57     FILTER_BIQUAD,
  58     FILTER_PT2,
  59     FILTER_PT3
  60 } filterType_e;
  61
  62 typedef enum {
  63     FILTER_LPF,
  64     FILTER_NOTCH
  65 } biquadFilterType_e;
  66
  67 typedef struct firFilter_s {
  68     float *buf;
  69     const float *coeffs;
  70     uint8_t bufLength;
  71     uint8_t coeffsLength;
  72 } firFilter_t;
  73
  74 typedef struct alphaBetaGammaFilter_s {
  75     float a, b, g, e;
  76     float ak; // derivative of system velociy (ie: acceleration)
  77     float vk; // derivative of system state (ie: velocity)
  78     float xk; // current system state (ie: position)
  79     float jk; // derivative of system acceleration (ie: jerk)
  80     float rk; // residual error
  81     float dT, dT2, dT3;
  82     float halfLife, boost;
  83     pt1Filter_t boostFilter;
  84 } alphaBetaGammaFilter_t;
  85
  86 typedef float (*filterApplyFnPtr)(void *filter, float input);
  87 typedef float (*filterApply4FnPtr)(void *filter, float input, float f_cut, float dt);
  88
  89 #define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
  90 #define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
  91
  92 float nullFilterApply(void *filter, float input);
  93 float nullFilterApply4(void *filter, float input, float f_cut, float dt);
  94
  95 void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT);
  96 void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT);
  97 void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau);
  98 void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut);
  99 float pt1FilterGetLastOutput(pt1Filter_t *filter);
 100 float pt1FilterApply(pt1Filter_t *filter, float input);
 101 float pt1FilterApply3(pt1Filter_t *filter, float input, float dT);
 102 float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt);
 103 void pt1FilterReset(pt1Filter_t *filter, float input);
 104
 105 /*
 106  * PT2 LowPassFilter
 107  */
 108 float pt2FilterGain(float f_cut, float dT);
 109 void pt2FilterInit(pt2Filter_t *filter, float k);
 110 void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k);
 111 float pt2FilterApply(pt2Filter_t *filter, float input);
 112
 113 /*
 114  * PT3 LowPassFilter
 115  */
 116 float pt3FilterGain(float f_cut, float dT);
 117 void pt3FilterInit(pt3Filter_t *filter, float k);
 118 void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k);
 119 float pt3FilterApply(pt3Filter_t *filter, float input);
 120
 121 void rateLimitFilterInit(rateLimitFilter_t *filter);
 122 float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT);
 123
 124 void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t samplingIntervalUs, uint16_t filterFreq, uint16_t cutoffHz);
 125 void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs);
 126 void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType);
 127 float biquadFilterApply(biquadFilter_t *filter, float sample);
 128 float biquadFilterReset(biquadFilter_t *filter, float value);
 129 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
 130 float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz);
 131 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
 132
 133 void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT);
 134 float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input);
 135
 136 void initFilter(uint8_t filterType, filter_t *filter, float cutoffFrequency, uint32_t refreshRate);
 137 void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn);