void gyroUpdate(void) { int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int axis; // range: +/- 8192; +/- 2000 deg/sec if (!gyro.read(gyroADCRaw)) { return; } for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis]; gyroADC[axis] = gyroADCRaw[axis]; } alignSensors(gyroADC, gyroADC, gyroAlign); if (gyroLpfCutFreq) { if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */ if (gyroFilterStateIsSet) { for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis])); } } if (!isGyroCalibrationComplete()) { performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold); } applyGyroZero(); }
void gyroUpdate(void) { // range: +/- 8192; +/- 2000 deg/sec if (!gyro.read(gyroADC)) { return; } alignSensors(gyroADC, gyroADC, gyroAlign); if (gyroLpfCutFreq) { if (!gyroFilterStateIsSet) { initGyroFilterCoefficients(); } else { for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroBiQuadState[axis])); } } if (!isGyroCalibrationComplete()) { performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold); } applyGyroZero(); }