src/main/sensors/gyro_filter_impl.h

   1 static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor)
   2 {
   3     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
   4         GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_RAW, axis, gyroSensor->gyroDev.gyroADCRaw[axis]);
   5         // scale gyro output to degrees per second
   6         float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
   7         // DEBUG_GYRO_SCALED records the unfiltered, scaled gyro output
   8         GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroADCf));
   9
  10 #ifdef USE_GYRO_DATA_ANALYSE
  11         if (isDynamicFilterActive()) {
  12             if (axis == X) {
  13                 GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data
  14                 GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); // store raw data
  15             }
  16         }
  17 #endif
  18
  19         // apply static notch filters and software lowpass filters
  20         gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
  21         gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
  22         gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf);
  23         gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
  24
  25 #ifdef USE_GYRO_DATA_ANALYSE
  26         if (isDynamicFilterActive()) {
  27             gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
  28             gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
  29             if (axis == X) {
  30                 GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
  31             }
  32         }
  33 #endif
  34
  35         // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
  36         GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));
  37
  38         gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf;
  39     }
  40 }