src/main/sensors/gyro_filter_impl.h

   1 static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDelta_t sampleDeltaUs)
   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         // apply static notch filters and software lowpass filters
  11         gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
  12         gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
  13         gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf);
  14         gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
  15
  16 #ifdef USE_GYRO_DATA_ANALYSE
  17         if (isDynamicFilterActive()) {
  18             gyroDataAnalysePush(axis, gyroADCf);
  19             if (axis == X) {
  20                 GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data
  21                 GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); // store raw data
  22             }
  23             gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
  24             if (axis == X) {
  25                 GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
  26             }
  27         }
  28 #endif
  29
  30         // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
  31         GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));
  32
  33         gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf;
  34         if (!gyroSensor->overflowDetected) {
  35             // integrate using trapezium rule to avoid bias
  36             accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
  37             gyroPrevious[axis] = gyroADCf;
  38         }
  39     }
  40 }