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 <stdint.h>
  23 #include <string.h>
  24 #include <math.h>
  25
  26 #include "platform.h"
  27
  28 #include "common/filter.h"
  29 #include "common/maths.h"
  30 #include "common/utils.h"
  31
  32 #define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - 2nd order butterworth*/
  33
  34 // NULL filter
  35
  36 FAST_CODE float nullFilterApply(filter_t *filter, float input)
  37 {
  38     UNUSED(filter);
  39     return input;
  40 }
  41
  42
  43 // PT1 Low Pass filter
  44
  45 float pt1FilterGain(float f_cut, float dT)
  46 {
  47     float RC = 1 / (2 * M_PIf * f_cut);
  48     return dT / (RC + dT);
  49 }
  50
  51 void pt1FilterInit(pt1Filter_t *filter, float k)
  52 {
  53     filter->state = 0.0f;
  54     filter->k = k;
  55 }
  56
  57 void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k)
  58 {
  59     filter->k = k;
  60 }
  61
  62 FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input)
  63 {
  64     filter->state = filter->state + filter->k * (input - filter->state);
  65     return filter->state;
  66 }
  67
  68 // PT2 Low Pass filter
  69
  70 float pt2FilterGain(float f_cut, float dT)
  71 {
  72     const float order = 2.0f;
  73     const float orderCutoffCorrection = 1 / sqrtf(powf(2, 1.0f / order) - 1);
  74     float RC = 1 / (2 * orderCutoffCorrection * M_PIf * f_cut);
  75     // float RC = 1 / (2 * 1.553773974f * M_PIf * f_cut);
  76     // where 1.553773974 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 2
  77     return dT / (RC + dT);
  78 }
  79
  80 void pt2FilterInit(pt2Filter_t *filter, float k)
  81 {
  82     filter->state = 0.0f;
  83     filter->state1 = 0.0f;
  84     filter->k = k;
  85 }
  86
  87 void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k)
  88 {
  89     filter->k = k;
  90 }
  91
  92 FAST_CODE float pt2FilterApply(pt2Filter_t *filter, float input)
  93 {
  94     filter->state1 = filter->state1 + filter->k * (input - filter->state1);
  95     filter->state = filter->state + filter->k * (filter->state1 - filter->state);
  96     return filter->state;
  97 }
  98
  99 // PT3 Low Pass filter
 100
 101 float pt3FilterGain(float f_cut, float dT)
 102 {
 103     const float order = 3.0f;
 104     const float orderCutoffCorrection = 1 / sqrtf(powf(2, 1.0f / order) - 1);
 105     float RC = 1 / (2 * orderCutoffCorrection * M_PIf * f_cut);
 106     // float RC = 1 / (2 * 1.961459177f * M_PIf * f_cut);
 107     // where 1.961459177 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 3
 108     return dT / (RC + dT);
 109 }
 110
 111 void pt3FilterInit(pt3Filter_t *filter, float k)
 112 {
 113     filter->state = 0.0f;
 114     filter->state1 = 0.0f;
 115     filter->state2 = 0.0f;
 116     filter->k = k;
 117 }
 118
 119 void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k)
 120 {
 121     filter->k = k;
 122 }
 123
 124 FAST_CODE float pt3FilterApply(pt3Filter_t *filter, float input)
 125 {
 126     filter->state1 = filter->state1 + filter->k * (input - filter->state1);
 127     filter->state2 = filter->state2 + filter->k * (filter->state1 - filter->state2);
 128     filter->state = filter->state + filter->k * (filter->state2 - filter->state);
 129     return filter->state;
 130 }
 131
 132
 133 // Slew filter with limit
 134
 135 void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold)
 136 {
 137     filter->state = 0.0f;
 138     filter->slewLimit = slewLimit;
 139     filter->threshold = threshold;
 140 }
 141
 142 FAST_CODE float slewFilterApply(slewFilter_t *filter, float input)
 143 {
 144     if (filter->state >= filter->threshold) {
 145         if (input >= filter->state - filter->slewLimit) {
 146             filter->state = input;
 147         }
 148     } else if (filter->state <= -filter->threshold) {
 149         if (input <= filter->state + filter->slewLimit) {
 150             filter->state = input;
 151         }
 152     } else {
 153         filter->state = input;
 154     }
 155     return filter->state;
 156 }
 157
 158 // get notch filter Q given center frequency (f0) and lower cutoff frequency (f1)
 159 // Q = f0 / (f2 - f1) ; f2 = f0^2 / f1
 160 float filterGetNotchQ(float centerFreq, float cutoffFreq)
 161 {
 162     return centerFreq * cutoffFreq / (centerFreq * centerFreq - cutoffFreq * cutoffFreq);
 163 }
 164
 165 /* sets up a biquad filter as a 2nd order butterworth LPF */
 166 void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
 167 {
 168     biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF, 1.0f);
 169 }
 170
 171 void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight)
 172 {
 173     biquadFilterUpdate(filter, filterFreq, refreshRate, Q, filterType, weight);
 174
 175     // zero initial samples
 176     filter->x1 = filter->x2 = 0;
 177     filter->y1 = filter->y2 = 0;
 178 }
 179
 180 FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight)
 181 {
 182     // setup variables
 183     const float omega = 2.0f * M_PIf * filterFreq * refreshRate * 0.000001f;
 184     const float sn = sin_approx(omega);
 185     const float cs = cos_approx(omega);
 186     const float alpha = sn / (2.0f * Q);
 187
 188     switch (filterType) {
 189     case FILTER_LPF:
 190         // 2nd order Butterworth (with Q=1/sqrt(2)) / Butterworth biquad section with Q
 191         // described in http://www.ti.com/lit/an/slaa447/slaa447.pdf
 192         filter->b1 = 1 - cs;
 193         filter->b0 = filter->b1 * 0.5f;
 194         filter->b2 = filter->b0;
 195         filter->a1 = -2 * cs;
 196         filter->a2 = 1 - alpha;
 197         break;
 198     case FILTER_NOTCH:
 199         filter->b0 = 1;
 200         filter->b1 = -2 * cs;
 201         filter->b2 = 1;
 202         filter->a1 = filter->b1;
 203         filter->a2 = 1 - alpha;
 204         break;
 205     case FILTER_BPF:
 206         filter->b0 = alpha;
 207         filter->b1 = 0;
 208         filter->b2 = -alpha;
 209         filter->a1 = -2 * cs;
 210         filter->a2 = 1 - alpha;
 211         break;
 212     }
 213
 214     const float a0 = 1 + alpha;
 215
 216     // precompute the coefficients
 217     filter->b0 /= a0;
 218     filter->b1 /= a0;
 219     filter->b2 /= a0;
 220     filter->a1 /= a0;
 221     filter->a2 /= a0;
 222
 223     // update weight
 224     filter->weight = weight;
 225 }
 226
 227 FAST_CODE void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
 228 {
 229     biquadFilterUpdate(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF, 1.0f);
 230 }
 231
 232 /* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */
 233 FAST_CODE float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
 234 {
 235     /* compute result */
 236     const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2;
 237
 238     /* shift x1 to x2, input to x1 */
 239     filter->x2 = filter->x1;
 240     filter->x1 = input;
 241
 242     /* shift y1 to y2, result to y1 */
 243     filter->y2 = filter->y1;
 244     filter->y1 = result;
 245
 246     return result;
 247 }
 248
 249 /* Computes a biquadFilter_t filter in df1 and crossfades input with output */
 250 FAST_CODE float biquadFilterApplyDF1Weighted(biquadFilter_t* filter, float input)
 251 {
 252     // compute result
 253     const float result = biquadFilterApplyDF1(filter, input);
 254
 255     // crossfading of input and output to turn filter on/off gradually
 256     return filter->weight * result + (1 - filter->weight) * input;
 257 }
 258
 259 /* Computes a biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */
 260 FAST_CODE float biquadFilterApply(biquadFilter_t *filter, float input)
 261 {
 262     const float result = filter->b0 * input + filter->x1;
 263
 264     filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2;
 265     filter->x2 = filter->b2 * input - filter->a2 * result;
 266
 267     return result;
 268 }
 269
 270 void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf)
 271 {
 272     filter->movingWindowIndex = 0;
 273     filter->windowSize = windowSize;
 274     filter->buf = buf;
 275     filter->movingSum = 0;
 276     memset(filter->buf, 0, windowSize * sizeof(float));
 277     filter->primed = false;
 278 }
 279
 280 FAST_CODE float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input)
 281 {
 282     filter->movingSum -= filter->buf[filter->movingWindowIndex];
 283     filter->buf[filter->movingWindowIndex] = input;
 284     filter->movingSum += input;
 285
 286     if (++filter->movingWindowIndex == filter->windowSize) {
 287         filter->movingWindowIndex = 0;
 288         filter->primed = true;
 289     }
 290
 291     const uint16_t denom = filter->primed ? filter->windowSize : filter->movingWindowIndex;
 292     return filter->movingSum  / denom;
 293 }
 294
 295 // Simple fixed-point lowpass filter based on integer math
 296
 297 int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal)
 298 {
 299     filter->fp = (filter->fp << filter->beta) - filter->fp;
 300     filter->fp += newVal << filter->fpShift;
 301     filter->fp >>= filter->beta;
 302     int32_t result = filter->fp >> filter->fpShift;
 303     return result;
 304 }
 305
 306 void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpShift)
 307 {
 308     filter->fp = 0;
 309     filter->beta = beta;
 310     filter->fpShift = fpShift;
 311 }