src/main/common/filter.c

   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 #include <stdbool.h>
  22 #include <string.h>
  23 #include <math.h>
  24
  25 #include "platform.h"
  26
  27 #include "common/filter.h"
  28 #include "common/maths.h"
  29 #include "common/utils.h"
  30
  31 #define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - 2nd order butterworth*/
  32
  33 // PTn cutoff correction = 1 / sqrt(2^(1/n) - 1)
  34 #define CUTOFF_CORRECTION_PT2 1.553773974f
  35 #define CUTOFF_CORRECTION_PT3 1.961459177f
  36
  37 // NULL filter
  38
  39 float nullFilterApply(filter_t *filter, float input)
  40 {
  41     UNUSED(filter);
  42     return input;
  43 }
  44
  45 // PT1 Low Pass filter
  46
  47 FAST_CODE_NOINLINE float pt1FilterGain(float f_cut, float dT)
  48 {
  49     float omega = 2.0f * M_PIf * f_cut * dT;
  50     return omega / (omega + 1.0f);
  51 }
  52
  53 // Calculates filter gain based on delay (time constant of filter) - time it takes for filter response to reach 63.2% of a step input.
  54 float pt1FilterGainFromDelay(float delay, float dT)
  55 {
  56     if (delay <= 0) {
  57         return 1.0f; // gain = 1 means no filtering
  58     }
  59
  60     const float cutoffHz = 1.0f / (2.0f * M_PIf * delay);
  61     return pt1FilterGain(cutoffHz, dT);
  62 }
  63
  64 void pt1FilterInit(pt1Filter_t *filter, float k)
  65 {
  66     filter->state = 0.0f;
  67     filter->k = k;
  68 }
  69
  70 void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k)
  71 {
  72     filter->k = k;
  73 }
  74
  75 FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input)
  76 {
  77     filter->state = filter->state + filter->k * (input - filter->state);
  78     return filter->state;
  79 }
  80
  81 // PT2 Low Pass filter
  82
  83 FAST_CODE float pt2FilterGain(float f_cut, float dT)
  84 {
  85     // shift f_cut to satisfy -3dB cutoff condition
  86     return pt1FilterGain(f_cut * CUTOFF_CORRECTION_PT2, dT);
  87 }
  88
  89 // Calculates filter gain based on delay (time constant of filter) - time it takes for filter response to reach 63.2% of a step input.
  90 float pt2FilterGainFromDelay(float delay, float dT)
  91 {
  92     if (delay <= 0) {
  93         return 1.0f; // gain = 1 means no filtering
  94     }
  95
  96     const float cutoffHz = 1.0f / (M_PIf * delay * CUTOFF_CORRECTION_PT2);
  97     return pt2FilterGain(cutoffHz, dT);
  98 }
  99
 100 void pt2FilterInit(pt2Filter_t *filter, float k)
 101 {
 102     filter->state = 0.0f;
 103     filter->state1 = 0.0f;
 104     filter->k = k;
 105 }
 106
 107 void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k)
 108 {
 109     filter->k = k;
 110 }
 111
 112 FAST_CODE float pt2FilterApply(pt2Filter_t *filter, float input)
 113 {
 114     filter->state1 = filter->state1 + filter->k * (input - filter->state1);
 115     filter->state = filter->state + filter->k * (filter->state1 - filter->state);
 116     return filter->state;
 117 }
 118
 119 // PT3 Low Pass filter
 120
 121 FAST_CODE float pt3FilterGain(float f_cut, float dT)
 122 {
 123     // shift f_cut to satisfy -3dB cutoff condition
 124     return pt1FilterGain(f_cut * CUTOFF_CORRECTION_PT3, dT);
 125 }
 126
 127 // Calculates filter gain based on delay (time constant of filter) - time it takes for filter response to reach 63.2% of a step input.
 128 float pt3FilterGainFromDelay(float delay, float dT)
 129 {
 130     if (delay <= 0) {
 131         return 1.0f; // gain = 1 means no filtering
 132     }
 133
 134     const float cutoffHz = 1.0f / (M_PIf * delay * CUTOFF_CORRECTION_PT3);
 135     return pt3FilterGain(cutoffHz, dT);
 136 }
 137
 138 void pt3FilterInit(pt3Filter_t *filter, float k)
 139 {
 140     filter->state = 0.0f;
 141     filter->state1 = 0.0f;
 142     filter->state2 = 0.0f;
 143     filter->k = k;
 144 }
 145
 146 void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k)
 147 {
 148     filter->k = k;
 149 }
 150
 151 FAST_CODE float pt3FilterApply(pt3Filter_t *filter, float input)
 152 {
 153     filter->state1 = filter->state1 + filter->k * (input - filter->state1);
 154     filter->state2 = filter->state2 + filter->k * (filter->state1 - filter->state2);
 155     filter->state = filter->state + filter->k * (filter->state2 - filter->state);
 156     return filter->state;
 157 }
 158
 159 // Biquad filter
 160
 161 // get notch filter Q given center frequency (f0) and lower cutoff frequency (f1)
 162 // Q = f0 / (f2 - f1) ; f2 = f0^2 / f1
 163 float filterGetNotchQ(float centerFreq, float cutoffFreq)
 164 {
 165     return centerFreq * cutoffFreq / (centerFreq * centerFreq - cutoffFreq * cutoffFreq);
 166 }
 167
 168 /* sets up a biquad filter as a 2nd order butterworth LPF */
 169 void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
 170 {
 171     biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF, 1.0f);
 172 }
 173
 174 void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight)
 175 {
 176     biquadFilterUpdate(filter, filterFreq, refreshRate, Q, filterType, weight);
 177
 178     // zero initial samples
 179     filter->x1 = filter->x2 = 0;
 180     filter->y1 = filter->y2 = 0;
 181 }
 182
 183 FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight)
 184 {
 185     // setup variables
 186     const float omega = 2.0f * M_PIf * filterFreq * refreshRate * 0.000001f;
 187     const float sn = sin_approx(omega);
 188     const float cs = cos_approx(omega);
 189     const float alpha = sn / (2.0f * Q);
 190
 191     switch (filterType) {
 192     case FILTER_LPF:
 193         // 2nd order Butterworth (with Q=1/sqrt(2)) / Butterworth biquad section with Q
 194         // described in http://www.ti.com/lit/an/slaa447/slaa447.pdf
 195         filter->b1 = 1 - cs;
 196         filter->b0 = filter->b1 * 0.5f;
 197         filter->b2 = filter->b0;
 198         filter->a1 = -2 * cs;
 199         filter->a2 = 1 - alpha;
 200         break;
 201     case FILTER_NOTCH:
 202         filter->b0 = 1;
 203         filter->b1 = -2 * cs;
 204         filter->b2 = 1;
 205         filter->a1 = filter->b1;
 206         filter->a2 = 1 - alpha;
 207         break;
 208     case FILTER_BPF:
 209         filter->b0 = alpha;
 210         filter->b1 = 0;
 211         filter->b2 = -alpha;
 212         filter->a1 = -2 * cs;
 213         filter->a2 = 1 - alpha;
 214         break;
 215     }
 216
 217     const float a0 = 1 + alpha;
 218
 219     // precompute the coefficients
 220     filter->b0 /= a0;
 221     filter->b1 /= a0;
 222     filter->b2 /= a0;
 223     filter->a1 /= a0;
 224     filter->a2 /= a0;
 225
 226     // update weight
 227     filter->weight = weight;
 228 }
 229
 230 FAST_CODE void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
 231 {
 232     biquadFilterUpdate(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF, 1.0f);
 233 }
 234
 235 /* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */
 236 FAST_CODE float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
 237 {
 238     /* compute result */
 239     const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2;
 240
 241     /* shift x1 to x2, input to x1 */
 242     filter->x2 = filter->x1;
 243     filter->x1 = input;
 244
 245     /* shift y1 to y2, result to y1 */
 246     filter->y2 = filter->y1;
 247     filter->y1 = result;
 248
 249     return result;
 250 }
 251
 252 /* Computes a biquadFilter_t filter in df1 and crossfades input with output */
 253 FAST_CODE float biquadFilterApplyDF1Weighted(biquadFilter_t* filter, float input)
 254 {
 255     // compute result
 256     const float result = biquadFilterApplyDF1(filter, input);
 257
 258     // crossfading of input and output to turn filter on/off gradually
 259     return filter->weight * result + (1 - filter->weight) * input;
 260 }
 261
 262 /* Computes a biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */
 263 FAST_CODE float biquadFilterApply(biquadFilter_t *filter, float input)
 264 {
 265     const float result = filter->b0 * input + filter->x1;
 266
 267     filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2;
 268     filter->x2 = filter->b2 * input - filter->a2 * result;
 269
 270     return result;
 271 }
 272
 273 // Phase Compensator (Lead-Lag-Compensator)
 274
 275 void phaseCompInit(phaseComp_t *filter, const float centerFreqHz, const float centerPhaseDeg, const uint32_t looptimeUs)
 276 {
 277     phaseCompUpdate(filter, centerFreqHz, centerPhaseDeg, looptimeUs);
 278
 279     filter->x1 = 0.0f;
 280     filter->y1 = 0.0f;
 281 }
 282
 283 FAST_CODE void phaseCompUpdate(phaseComp_t *filter, const float centerFreqHz, const float centerPhaseDeg, const uint32_t looptimeUs)
 284 {
 285     const float omega = 2.0f * M_PIf * centerFreqHz * looptimeUs * 1e-6f;
 286     const float sn = sin_approx(centerPhaseDeg * RAD);
 287     const float gain = (1 + sn) / (1 - sn);
 288     const float alpha = (12 - sq(omega)) / (6 * omega * sqrtf(gain));  // approximate prewarping (series expansion)
 289
 290     filter->b0 = 1 + alpha * gain;
 291     filter->b1 = 2 - filter->b0;
 292     filter->a1 = 1 - alpha;
 293
 294     const float a0 = 1 / (1 + alpha);
 295
 296     filter->b0 *= a0;
 297     filter->b1 *= a0;
 298     filter->a1 *= a0;
 299 }
 300
 301 FAST_CODE float phaseCompApply(phaseComp_t *filter, const float input)
 302 {
 303     // compute result
 304     const float result = filter->b0 * input + filter->b1 * filter->x1 - filter->a1 * filter->y1;
 305
 306     // shift input to x1 and result to y1
 307     filter->x1 = input;
 308     filter->y1 = result;
 309
 310     return result;
 311 }
 312
 313 // Slew filter with limit
 314
 315 void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold)
 316 {
 317     filter->state = 0.0f;
 318     filter->slewLimit = slewLimit;
 319     filter->threshold = threshold;
 320 }
 321
 322 FAST_CODE float slewFilterApply(slewFilter_t *filter, float input)
 323 {
 324     if (filter->state >= filter->threshold) {
 325         if (input >= filter->state - filter->slewLimit) {
 326             filter->state = input;
 327         }
 328     } else if (filter->state <= -filter->threshold) {
 329         if (input <= filter->state + filter->slewLimit) {
 330             filter->state = input;
 331         }
 332     } else {
 333         filter->state = input;
 334     }
 335     return filter->state;
 336 }
 337
 338 // Moving average
 339
 340 void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf)
 341 {
 342     filter->movingWindowIndex = 0;
 343     filter->windowSize = windowSize;
 344     filter->buf = buf;
 345     filter->movingSum = 0;
 346     memset(filter->buf, 0, windowSize * sizeof(float));
 347     filter->primed = false;
 348 }
 349
 350 FAST_CODE float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input)
 351 {
 352     filter->movingSum -= filter->buf[filter->movingWindowIndex];
 353     filter->buf[filter->movingWindowIndex] = input;
 354     filter->movingSum += input;
 355
 356     if (++filter->movingWindowIndex == filter->windowSize) {
 357         filter->movingWindowIndex = 0;
 358         filter->primed = true;
 359     }
 360
 361     const uint16_t denom = filter->primed ? filter->windowSize : filter->movingWindowIndex;
 362     return filter->movingSum / denom;
 363 }
 364
 365 // Simple fixed-point lowpass filter based on integer math
 366
 367 void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpShift)
 368 {
 369     filter->fp = 0;
 370     filter->beta = beta;
 371     filter->fpShift = fpShift;
 372 }
 373
 374 int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal)
 375 {
 376     filter->fp = (filter->fp << filter->beta) - filter->fp;
 377     filter->fp += newVal << filter->fpShift;
 378     filter->fp >>= filter->beta;
 379     int32_t result = filter->fp >> filter->fpShift;
 380     return result;
 381 }
 382
 383 // Mean accumulator
 384
 385 void meanAccumulatorInit(meanAccumulator_t *filter)
 386 {
 387     filter->accumulator = 0;
 388     filter->count = 0;
 389 }
 390
 391 void meanAccumulatorAdd(meanAccumulator_t *filter, const int8_t newVal)
 392 {
 393     filter->accumulator += newVal;
 394     filter->count++;
 395 }
 396
 397 int8_t meanAccumulatorCalc(meanAccumulator_t *filter, const int8_t defaultValue)
 398 {
 399     if (filter->count) {
 400         int8_t retVal = filter->accumulator / filter->count;
 401         meanAccumulatorInit(filter);
 402         return retVal;
 403     }
 404     return defaultValue;
 405 }