static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { return; } gyroSensor->gyroDev.dataReady = false; if (isGyroSensorCalibrationComplete(gyroSensor)) { // move 16-bit gyro data into 32-bit variables to avoid overflows in calculations #if defined(USE_GYRO_SLEW_LIMITER) gyroSensor->gyroDev.gyroADC[X] = gyroSlewLimiter(gyroSensor, X) - gyroSensor->gyroDev.gyroZero[X]; gyroSensor->gyroDev.gyroADC[Y] = gyroSlewLimiter(gyroSensor, Y) - gyroSensor->gyroDev.gyroZero[Y]; gyroSensor->gyroDev.gyroADC[Z] = gyroSlewLimiter(gyroSensor, Z) - gyroSensor->gyroDev.gyroZero[Z]; #else gyroSensor->gyroDev.gyroADC[X] = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X]; gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y]; gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z]; #endif alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign); } else { performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold); // still calibrating, so no need to further process gyro data return; } if (gyroDebugMode == DEBUG_NONE) { filterGyro(gyroSensor); } else { filterGyroDebug(gyroSensor); } #ifdef USE_GYRO_OVERFLOW_CHECK if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) { checkForOverflow(gyroSensor, currentTimeUs); } #endif #ifdef USE_YAW_SPIN_RECOVERY if (gyroConfig()->yaw_spin_recovery) { checkForYawSpin(gyroSensor, currentTimeUs); } #endif #ifdef USE_GYRO_DATA_ANALYSE if (isDynamicFilterActive()) { gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn, gyroSensor->notchFilterDyn2); } #endif #if (!defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY)) UNUSED(currentTimeUs); #endif }
void gyroUpdate(void) { int16_t gyroADCRaw[XYZ_AXIS_COUNT]; // range: +/- 8192; +/- 2000 deg/sec if (!gyro.read(gyroADCRaw)) { return; } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyroADC[axis] = gyroADCRaw[axis]; } alignSensors(gyroADC, gyroADC, gyroAlign); if (!isGyroCalibrationComplete()) { performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold); } applyGyroZero(); if (gyroSoftLpfHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis]; if (gyroSoftLpfType == FILTER_BIQUAD) gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]); else if (gyroSoftLpfType == FILTER_PT1) gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt); else gyroADCf[axis] = firFilterDenoiseUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]); if (debugMode == DEBUG_NOTCH) debug[axis] = lrintf(gyroADCf[axis]); if (gyroSoftNotchHz1) gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_1[axis], gyroADCf[axis]); if (gyroSoftNotchHz2) gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_2[axis], gyroADCf[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); } } else { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADCf[axis] = gyroADC[axis]; } }