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;
}
