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