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.dev.read(&gyro.dev)) { return; } // Prepare a copy of int32_t gyroADC for mangling to prevent overflow gyro.gyroADC[X] = gyro.dev.gyroADCRaw[X]; gyro.gyroADC[Y] = gyro.dev.gyroADCRaw[Y]; gyro.gyroADC[Z] = gyro.dev.gyroADCRaw[Z]; if (gyroConfig->gyro_soft_lpf_hz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyro.gyroADC[axis] = lrintf(biquadFilterApply(&gyroFilterLPF[axis], (float)gyro.gyroADC[axis])); } } if (!isGyroCalibrationComplete()) { performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold); } gyro.gyroADC[X] -= gyroZero[X]; gyro.gyroADC[Y] -= gyroZero[Y]; gyro.gyroADC[Z] -= gyroZero[Z]; alignSensors(gyro.gyroADC, gyro.dev.gyroAlign); }
void updateAccelerationReadings(void) { if (!acc.read(accADCRaw)) { return; } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) accADC[axis] = accADCRaw[axis]; if (accLpfCutHz) { if (!accFilterInitialised) { if (targetLooptime) { /* Initialisation needs to happen once sample rate is known */ for (int axis = 0; axis < 3; axis++) { biquadFilterInit(&accFilterState[axis], accLpfCutHz, 0); } accFilterInitialised = true; } } if (accFilterInitialised) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { accADC[axis] = lrintf(biquadFilterApply(&accFilterState[axis], (float) accADC[axis])); } } } if (!isAccelerationCalibrationComplete()) { performAcclerationCalibration(); } applyAccelerationZero(accZero, accGain); alignSensors(accADC, accADC, accAlign); }
void gyroGetADC(void) { // FIXME When gyro.read() fails due to i2c or other error gyroZero is continually re-applied to gyroADC resulting in a old reading that gets worse over time. // range: +/- 8192; +/- 2000 deg/sec gyro.read(gyroADC); alignSensors(gyroADC, gyroADC, gyroAlign); if (!isGyroCalibrationComplete()) { performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold); } applyGyroZero(); }
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()) { performAcclerationCalibration(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] = denoisingFilterUpdate(&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]; } }
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) { acc.read(accADC); alignSensors(accADC, accADC, accAlign); if (!isAccelerationCalibrationComplete()) { performAcclerationCalibration(rollAndPitchTrims); } if (feature(FEATURE_INFLIGHT_ACC_CAL)) { performInflightAccelerationCalibration(rollAndPitchTrims); } applyAccelerationTrims(accelerationTrims); }
void gyroUpdate(void) { // range: +/- 8192; +/- 2000 deg/sec if (!gyro.read(gyroADCRaw)) { return; } gyroADC[X] = gyroADCRaw[X]; gyroADC[Y] = gyroADCRaw[Y]; gyroADC[Z] = gyroADCRaw[Z]; alignSensors(gyroADC, gyroADC, gyroAlign); if (!isGyroCalibrationComplete()) { performAcclerationCalibration(gyroConfig()->gyroMovementCalibrationThreshold); } gyroADC[X] -= gyroZero[X]; gyroADC[Y] -= gyroZero[Y]; gyroADC[Z] -= gyroZero[Z]; if (gyroConfig()->gyro_soft_lpf_hz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis]; if (gyroConfig()->gyro_soft_type == FILTER_BIQUAD) gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]); else gyroADCf[axis] = pt1FilterApply(&gyroFilterPt1[axis], (float) gyroADC[axis]); if (debugMode == DEBUG_NOTCH) debug[axis] = lrintf(gyroADCf[axis]); if (gyroConfig()->gyro_soft_notch_hz) gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch[axis], gyroADCf[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); } } else { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyroADCf[axis] = gyroADC[axis]; } } }
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) { int16_t accADCRaw[XYZ_AXIS_COUNT]; if (!acc.read(accADCRaw)) { return; } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis]; accSmooth[axis] = accADCRaw[axis]; } if (accLpfCutHz) { if (!accFilterInitialised) { if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime); } accFilterInitialised = true; } } if (accFilterInitialised) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis])); } } } alignSensors(accSmooth, accSmooth, accAlign); if (!isAccelerationCalibrationComplete()) { performAcclerationCalibration(rollAndPitchTrims); } if (feature(FEATURE_INFLIGHT_ACC_CAL)) { performInflightAccelerationCalibration(rollAndPitchTrims); } applyAccelerationTrims(accelerationTrims); }
void gyroUpdate(void) { int8_t axis; // range: +/- 8192; +/- 2000 deg/sec if (!gyro.read(gyroADCRaw)) { return; } // Prepare a copy of int32_t gyroADC for mangling to prevent overflow for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = gyroADCRaw[axis]; if (gyroLpfCutHz) { if (!gyroFilterInitialised) { if (targetLooptime) { /* Initialisation needs to happen once sample rate is known */ for (axis = 0; axis < 3; axis++) { filterInitBiQuad(gyroLpfCutHz, &gyroFilterState[axis], 0); } gyroFilterInitialised = true; } } if (gyroFilterInitialised) { for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyroADC[axis] = lrintf(filterApplyBiQuad((float) gyroADC[axis], &gyroFilterState[axis])); } } } alignSensors(gyroADC, gyroADC, gyroAlign); 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(); }