Closed JerrySOET closed 1 year ago
void MPU6050::calibrateGyro(uint8_t samples) { // Set calibrate useCalibrate = true; // Reset values float sumX = 0; float sumY = 0; float sumZ = 0; float sigmaX = 0; float sigmaY = 0; float sigmaZ = 0; // Read n-samples for (uint8_t i = 0; i < samples; ++i) { readRawGyro(); sumX += rg.XAxis; sumY += rg.YAxis; sumZ += rg.ZAxis; sigmaX += rg.XAxis * rg.XAxis; sigmaY += rg.YAxis * rg.YAxis; sigmaZ += rg.ZAxis * rg.ZAxis; delay(5); } // Calculate delta vectors dg.XAxis = sumX / samples; dg.YAxis = sumY / samples; dg.ZAxis = sumZ / samples; // Calculate threshold vectors th.XAxis = sqrt((sigmaX / 50) - (dg.XAxis * dg.XAxis)); th.YAxis = sqrt((sigmaY / 50) - (dg.YAxis * dg.YAxis)); th.ZAxis = sqrt((sigmaZ / 50) - (dg.ZAxis * dg.ZAxis)); // If already set threshold, recalculate threshold vectors if (actualThreshold > 0) { setThreshold(actualThreshold); } }
the number 50 should be samples number?
yes - is fixed in master
the number 50 should be samples number?