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];
}
}
