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 == gyroSensor->gyroDebugAxis) {
  13                 GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
  14                 GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
  15                 GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf));
  16             }
  17         }
  18 #endif
  19
  20 #ifdef USE_RPM_FILTER
  21         gyroADCf = rpmFilterGyro(axis, gyroADCf);
  22 #endif
  23
  24
  25         // apply static notch filters and software lowpass filters
  26         gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
  27         gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
  28         gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf);
  29         gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
  30
  31 #ifdef USE_GYRO_DATA_ANALYSE
  32         if (isDynamicFilterActive()) {
  33             if (axis == gyroSensor->gyroDebugAxis) {
  34                 GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf));
  35                 GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf));
  36                 GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 3, lrintf(gyroADCf));
  37             }
  38             gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
  39             gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
  40             gyroADCf = gyroSensor->notchFilterDynApplyFn2((filter_t *)&gyroSensor->notchFilterDyn2[axis], gyroADCf);
  41         }
  42 #endif
  43
  44         // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
  45         GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));
  46
  47         gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf;
  48     }
  49 }