src/main/common/filter.c

   1 /*
   2  * filter.c
   3  *
   4  *  Created on: 24 jun. 2015
   5  *      Author: borisb
   6  */
   7
   8 #include <stdbool.h>
   9 #include <stdint.h>
  10 #include <stdlib.h>
  11 #include <math.h>
  12
  13 #include "common/filter.h"
  14 #include "common/axis.h"
  15
  16
  17 // PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
  18 float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dT) {
  19
  20         // Pre calculate and store RC
  21         if (!filter->RC) {
  22                 filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
  23         }
  24
  25     filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
  26
  27     return filter->state;
  28 }
  29
  30 // 7 Tap FIR filter as described here:
  31 // Thanks to Qcopter
  32 void filterApply7TapFIR(int16_t data[]) {
  33     int16_t FIRcoeff[7] = { 12, 23, 40, 51, 52, 40, 38 }; // TODO - More coefficients needed. Now fixed to 1khz
  34     static int16_t gyro_delay[3][7] = { {0}, {0}, {0} };
  35     int32_t FIRsum;
  36     int axis, i;
  37
  38     // 7 tap FIR, <-20dB at >170Hz with looptime 1ms, groupdelay = 2.5ms
  39     for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
  40         FIRsum = 0;
  41         for (i = 0; i <= 5; i++) {
  42             gyro_delay[axis][i] = gyro_delay[axis][i + 1];
  43             FIRsum += gyro_delay[axis][i] * FIRcoeff[i];
  44         }
  45         gyro_delay[axis][6] = data[axis];
  46         FIRsum += gyro_delay[axis][6] * FIRcoeff[6];
  47         data[axis] = FIRsum / 256;
  48     }
  49 }