Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 17:25:20 +03:00
Code Issues Wiki Activity

Minor gyro optimisations

Browse source
This commit is contained in:
Martin Budden 2017-01-08 21:36:16 +00:00
parent 964a402053
commit 8edb9ef810
3 changed files with 14 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

24
src/main/sensors/gyro.c
View file

@ -409,6 +409,12 @@ void gyroUpdate(void)
return;
}
gyro.dev.dataReady = false;
// move gyro data into 32-bit variables to avoid overflows in calculations
gyroADC[X] = gyro.dev.gyroADCRaw[X];
gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
alignSensors(gyroADC, gyro.dev.gyroAlign);
const bool calibrationComplete = isGyroCalibrationComplete();
if (calibrationComplete) {
@ -422,15 +428,7 @@ void gyroUpdate(void)
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
debug[3] = (uint16_t)(micros() & 0xffff);
#endif
}
// move gyro data into 32-bit variables to avoid overflows in calculations
gyroADC[X] = gyro.dev.gyroADCRaw[X];
gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
alignSensors(gyroADC, gyro.dev.gyroAlign);
if (!calibrationComplete) {
} else {
performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
@ -448,9 +446,11 @@ void gyroUpdate(void)
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
gyro.gyroADCf[axis] = gyroADCf;
}
if (!calibrationComplete) {
gyroADC[axis] = lrintf(gyro.gyroADCf[axis] / gyro.dev.scale);
}
if (!calibrationComplete) {
gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyro.dev.scale);
gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyro.dev.scale);
gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyro.dev.scale);
}
}