public void StartContinuousCalibration() { for (int i = 0; i < gyro_average_window.Length; i++) { gyro_average_window[i] = new GyroAverageWindow(); } gyroAverageTimer.Start(); }
public DS4SixAxis() { sPrev = new SixAxis(0, 0, 0, 0, 0, 0, 0.0); now = new SixAxis(0, 0, 0, 0, 0, 0, 0.0); for (int i = 0; i < gyro_average_window.Length; i++) { gyro_average_window[i] = new GyroAverageWindow(); } gyroAverageTimer.Start(); }
public void AverageGyro(ref int x, ref int y, ref int z, ref double accelMagnitude) { double weight = 0.0; double totalX = 0.0; double totalY = 0.0; double totalZ = 0.0; double totalAccelMagnitude = 0.0; int wantedMs = 5000; for (int i = 0; i < num_gyro_average_windows && wantedMs > 0; i++) { int cycledIndex = (i + gyro_average_window_front_index) % num_gyro_average_windows; GyroAverageWindow windowPointer = gyro_average_window[cycledIndex]; if (windowPointer.numSamples == 0 || windowPointer.DurationMs == 0) { continue; } double thisWeight; double fNumSamples = windowPointer.numSamples; if (wantedMs < windowPointer.DurationMs) { thisWeight = (float)wantedMs / windowPointer.DurationMs; wantedMs = 0; } else { thisWeight = windowPointer.GetWeight(gyro_average_window_ms); wantedMs -= windowPointer.DurationMs; } totalX += (windowPointer.x / fNumSamples) * thisWeight; totalY += (windowPointer.y / fNumSamples) * thisWeight; totalZ += (windowPointer.z / fNumSamples) * thisWeight; totalAccelMagnitude += (windowPointer.accelMagnitude / fNumSamples) * thisWeight; weight += thisWeight; } if (weight > 0.0) { x = (int)(totalX / weight); y = (int)(totalY / weight); z = (int)(totalZ / weight); accelMagnitude = totalAccelMagnitude / weight; } }
public unsafe void PushSensorSamples(int x, int y, int z, double accelMagnitude) { // push samples GyroAverageWindow windowPointer = gyro_average_window[gyro_average_window_front_index]; if (windowPointer.StopIfElapsed(gyro_average_window_ms)) { Console.WriteLine("GyroAvg[{0}], numSamples: {1}", gyro_average_window_front_index, windowPointer.numSamples); // next gyro_average_window_front_index = (gyro_average_window_front_index + num_gyro_average_windows - 1) % num_gyro_average_windows; windowPointer = gyro_average_window[gyro_average_window_front_index]; windowPointer.Reset(); } // accumulate windowPointer.numSamples++; windowPointer.x += x; windowPointer.y += y; windowPointer.z += z; windowPointer.accelMagnitude += accelMagnitude; }