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 <string.h>
  21 #include <math.h>
  22
  23 #include "platform.h"
  24
  25 #include "common/filter.h"
  26 #include "common/maths.h"
  27 #include "common/utils.h"
  28 #include "common/time.h"
  29
  30 // NULL filter
  31 float nullFilterApply(void *filter, float input)
  32 {
  33     UNUSED(filter);
  34     return input;
  35 }
  36
  37 float nullFilterApply4(void *filter, float input, float f_cut, float dt)
  38 {
  39     UNUSED(filter);
  40     UNUSED(f_cut);
  41     UNUSED(dt);
  42     return input;
  43 }
  44
  45 // PT1 Low Pass filter
  46
  47 static float pt1ComputeRC(const float f_cut)
  48 {
  49     return 1.0f / (2.0f * M_PIf * f_cut);
  50 }
  51
  52 // f_cut = cutoff frequency
  53 void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT)
  54 {
  55     filter->state = 0.0f;
  56     filter->RC = tau;
  57     filter->dT = dT;
  58     filter->alpha = filter->dT / (filter->RC + filter->dT);
  59 }
  60
  61 void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT)
  62 {
  63     pt1FilterInitRC(filter, pt1ComputeRC(f_cut), dT);
  64 }
  65
  66 void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau) {
  67     filter->RC = tau;
  68 }
  69
  70 float pt1FilterGetLastOutput(pt1Filter_t *filter) {
  71     return filter->state;
  72 }
  73
  74 void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut)
  75 {
  76     filter->RC = pt1ComputeRC(f_cut);
  77     filter->alpha = filter->dT / (filter->RC + filter->dT);
  78 }
  79
  80 float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input)
  81 {
  82     filter->state = filter->state + filter->alpha * (input - filter->state);
  83     return filter->state;
  84 }
  85
  86 float pt1FilterApply3(pt1Filter_t *filter, float input, float dT)
  87 {
  88     filter->dT = dT;
  89     filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
  90     return filter->state;
  91 }
  92
  93 float FAST_CODE NOINLINE pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT)
  94 {
  95     // Pre calculate and store RC
  96     if (!filter->RC) {
  97         filter->RC = pt1ComputeRC(f_cut);
  98     }
  99
 100     filter->dT = dT;    // cache latest dT for possible use in pt1FilterApply
 101     filter->alpha = filter->dT / (filter->RC + filter->dT);
 102     filter->state = filter->state + filter->alpha * (input - filter->state);
 103     return filter->state;
 104 }
 105
 106 void pt1FilterReset(pt1Filter_t *filter, float input)
 107 {
 108     filter->state = input;
 109 }
 110
 111 /*
 112  * PT2 LowPassFilter
 113  */
 114 float pt2FilterGain(float f_cut, float dT)
 115 {
 116     const float order = 2.0f;
 117     const float orderCutoffCorrection = 1 / sqrtf(powf(2, 1.0f / order) - 1);
 118     float RC = 1 / (2 * orderCutoffCorrection * M_PIf * f_cut);
 119     // float RC = 1 / (2 * 1.553773974f * M_PIf * f_cut);
 120     // where 1.553773974 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 2
 121     return dT / (RC + dT);
 122 }
 123
 124 void pt2FilterInit(pt2Filter_t *filter, float k)
 125 {
 126     filter->state = 0.0f;
 127     filter->state1 = 0.0f;
 128     filter->k = k;
 129 }
 130
 131 void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k)
 132 {
 133     filter->k = k;
 134 }
 135
 136 FAST_CODE float pt2FilterApply(pt2Filter_t *filter, float input)
 137 {
 138     filter->state1 = filter->state1 + filter->k * (input - filter->state1);
 139     filter->state = filter->state + filter->k * (filter->state1 - filter->state);
 140     return filter->state;
 141 }
 142
 143 /*
 144  * PT3 LowPassFilter
 145  */
 146 float pt3FilterGain(float f_cut, float dT)
 147 {
 148     const float order = 3.0f;
 149     const float orderCutoffCorrection = 1 / sqrtf(powf(2, 1.0f / order) - 1);
 150     float RC = 1 / (2 * orderCutoffCorrection * M_PIf * f_cut);
 151     // float RC = 1 / (2 * 1.961459177f * M_PIf * f_cut);
 152     // where 1.961459177 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 3
 153     return dT / (RC + dT);
 154 }
 155
 156 void pt3FilterInit(pt3Filter_t *filter, float k)
 157 {
 158     filter->state = 0.0f;
 159     filter->state1 = 0.0f;
 160     filter->state2 = 0.0f;
 161     filter->k = k;
 162 }
 163
 164 void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k)
 165 {
 166     filter->k = k;
 167 }
 168
 169 FAST_CODE float pt3FilterApply(pt3Filter_t *filter, float input)
 170 {
 171     filter->state1 = filter->state1 + filter->k * (input - filter->state1);
 172     filter->state2 = filter->state2 + filter->k * (filter->state1 - filter->state2);
 173     filter->state = filter->state + filter->k * (filter->state2 - filter->state);
 174     return filter->state;
 175 }
 176
 177 // rate_limit = maximum rate of change of the output value in units per second
 178 void rateLimitFilterInit(rateLimitFilter_t *filter)
 179 {
 180     filter->state = 0;
 181 }
 182
 183 float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT)
 184 {
 185     if (rate_limit > 0) {
 186         const float rateLimitPerSample = rate_limit * dT;
 187         filter->state = constrainf(input, filter->state - rateLimitPerSample, filter->state + rateLimitPerSample);
 188     }
 189     else {
 190         filter->state = input;
 191     }
 192
 193     return filter->state;
 194 }
 195
 196 float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz)
 197 {
 198     return centerFrequencyHz * cutoffFrequencyHz / (centerFrequencyHz * centerFrequencyHz - cutoffFrequencyHz * cutoffFrequencyHz);
 199 }
 200
 201 void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t samplingIntervalUs, uint16_t filterFreq, uint16_t cutoffHz)
 202 {
 203     float Q = filterGetNotchQ(filterFreq, cutoffHz);
 204     biquadFilterInit(filter, filterFreq, samplingIntervalUs, Q, FILTER_NOTCH);
 205 }
 206
 207 // sets up a biquad Filter
 208 void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs)
 209 {
 210     biquadFilterInit(filter, filterFreq, samplingIntervalUs, BIQUAD_Q, FILTER_LPF);
 211 }
 212
 213
 214 static void biquadFilterSetupPassthrough(biquadFilter_t *filter)
 215 {
 216     // By default set as passthrough
 217     filter->b0 = 1.0f;
 218     filter->b1 = 0.0f;
 219     filter->b2 = 0.0f;
 220     filter->a1 = 0.0f;
 221     filter->a2 = 0.0f;
 222 }
 223
 224 void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType)
 225 {
 226     // Check for Nyquist frequency and if it's not possible to initialize filter as requested - set to no filtering at all
 227     if (filterFreq < (1000000 / samplingIntervalUs / 2)) {
 228         // setup variables
 229         const float sampleRate = 1.0f / ((float)samplingIntervalUs * 0.000001f);
 230         const float omega = 2.0f * M_PIf * ((float)filterFreq) / sampleRate;
 231         const float sn = sin_approx(omega);
 232         const float cs = cos_approx(omega);
 233         const float alpha = sn / (2 * Q);
 234
 235         float b0, b1, b2;
 236         switch (filterType) {
 237             case FILTER_LPF:
 238                 b0 = (1 - cs) / 2;
 239                 b1 = 1 - cs;
 240                 b2 = (1 - cs) / 2;
 241                 break;
 242             case FILTER_NOTCH:
 243                 b0 = 1;
 244                 b1 = -2 * cs;
 245                 b2 = 1;
 246                 break;
 247             default:
 248                 biquadFilterSetupPassthrough(filter);
 249                 return;
 250         }
 251         const float a0 =  1 + alpha;
 252         const float a1 = -2 * cs;
 253         const float a2 =  1 - alpha;
 254
 255         // precompute the coefficients
 256         filter->b0 = b0 / a0;
 257         filter->b1 = b1 / a0;
 258         filter->b2 = b2 / a0;
 259         filter->a1 = a1 / a0;
 260         filter->a2 = a2 / a0;
 261     } else {
 262         biquadFilterSetupPassthrough(filter);
 263     }
 264
 265     // zero initial samples
 266     filter->x1 = filter->x2 = 0;
 267     filter->y1 = filter->y2 = 0;
 268 }
 269
 270 FAST_CODE float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
 271 {
 272     /* compute result */
 273     const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2;
 274
 275     /* shift x1 to x2, input to x1 */
 276     filter->x2 = filter->x1;
 277     filter->x1 = input;
 278
 279     /* shift y1 to y2, result to y1 */
 280     filter->y2 = filter->y1;
 281     filter->y1 = result;
 282
 283     return result;
 284 }
 285
 286 // Computes a biquad_t filter on a sample
 287 float FAST_CODE NOINLINE biquadFilterApply(biquadFilter_t *filter, float input)
 288 {
 289     const float result = filter->b0 * input + filter->x1;
 290     filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2;
 291     filter->x2 = filter->b2 * input - filter->a2 * result;
 292     return result;
 293 }
 294
 295 float biquadFilterReset(biquadFilter_t *filter, float value)
 296 {
 297     filter->x1 = value - (value * filter->b0);
 298     filter->x2 = (filter->b2 - filter->a2) * value;
 299     return value;
 300 }
 301
 302 FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
 303 {
 304     // backup state
 305     float x1 = filter->x1;
 306     float x2 = filter->x2;
 307     float y1 = filter->y1;
 308     float y2 = filter->y2;
 309
 310     biquadFilterInit(filter, filterFreq, refreshRate, Q, filterType);
 311
 312     // restore state
 313     filter->x1 = x1;
 314     filter->x2 = x2;
 315     filter->y1 = y1;
 316     filter->y2 = y2;
 317 }
 318
 319 void initFilter(const uint8_t filterType, filter_t *filter, const float cutoffFrequency, const uint32_t refreshRate) {
 320     const float dT = US2S(refreshRate);
 321
 322     if (cutoffFrequency) {
 323         if (filterType == FILTER_PT1) {
 324             pt1FilterInit(&filter->pt1, cutoffFrequency, dT);
 325         } if (filterType == FILTER_PT2) {
 326             pt2FilterInit(&filter->pt2, pt2FilterGain(cutoffFrequency, dT));
 327         } if (filterType == FILTER_PT3) {
 328             pt3FilterInit(&filter->pt3, pt3FilterGain(cutoffFrequency, dT));
 329         } else {
 330             biquadFilterInitLPF(&filter->biquad, cutoffFrequency, refreshRate);
 331         }
 332     }
 333 }
 334
 335 void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn) {
 336     *applyFn = nullFilterApply;
 337     if (cutoffFrequency) {
 338         if (filterType == FILTER_PT1) {
 339             *applyFn = (filterApplyFnPtr) pt1FilterApply;
 340         } if (filterType == FILTER_PT2) {
 341             *applyFn = (filterApplyFnPtr) pt2FilterApply;
 342         } if (filterType == FILTER_PT3) {
 343             *applyFn = (filterApplyFnPtr) pt3FilterApply;
 344         } else {
 345             *applyFn = (filterApplyFnPtr) biquadFilterApply;
 346         }
 347     }
 348 }