/// <summary>
/// Cloning constructor
/// </summary>
/// <param name = "v">Vector to clone</param>
public Simple3DVector(Simple3DVector v)
{
if (v != null)
{
this.X = v.X;
this.Y = v.Y;
this.Z = v.Z;
}
}
/// <summary>
/// Called on accelerometer sensor sample available.
/// Main accelerometer data filtering routine
/// </summary>
/// <param name = "sender">Sender of the event.</param>
/// <param name = "e">AccelerometerReadingAsyncEventArgs</param>
private void SensorReadingChanged(object sender, AccelerometerReadingEventArgs e)
{
Simple3DVector lowPassFilteredAcceleration;
Simple3DVector optimalFilteredAcceleration;
Simple3DVector averagedAcceleration;
var rawAcceleration = new Simple3DVector(e.X, e.Y, e.Z);
lock (this.sampleBuffer)
{
if (!this.initialized)
{
// Initialize file with 1st value
this.sampleSum = rawAcceleration * SamplesCount;
this.averageAcceleration = rawAcceleration;
// Initialize file with 1st value
for (int i = 0; i < SamplesCount; i++)
{
this.sampleBuffer[i] = this.averageAcceleration;
}
this.previousLowPassOutput = this.averageAcceleration;
this.previousOptimalFilterOutput = this.averageAcceleration;
this.initialized = true;
}
// low-pass filter
lowPassFilteredAcceleration =
new Simple3DVector(
LowPassFilter(rawAcceleration.X, this.previousLowPassOutput.X),
LowPassFilter(rawAcceleration.Y, this.previousLowPassOutput.Y),
LowPassFilter(rawAcceleration.Z, this.previousLowPassOutput.Z));
this.previousLowPassOutput = lowPassFilteredAcceleration;
// optimal filter
optimalFilteredAcceleration =
new Simple3DVector(
FastLowAmplitudeNoiseFilter(rawAcceleration.X, this.previousOptimalFilterOutput.X),
FastLowAmplitudeNoiseFilter(rawAcceleration.Y, this.previousOptimalFilterOutput.Y),
FastLowAmplitudeNoiseFilter(rawAcceleration.Z, this.previousOptimalFilterOutput.Z));
this.previousOptimalFilterOutput = optimalFilteredAcceleration;
// Increment circular buffer insertion index
this.sampleIndex++;
if (this.sampleIndex >= SamplesCount)
{
this.sampleIndex = 0;
// if at max SampleCount then wrap samples back to the beginning position in the list
}
// Add new and remove old at _sampleIndex
Simple3DVector newVect = optimalFilteredAcceleration;
this.sampleSum += newVect;
this.sampleSum -= this.sampleBuffer[this.sampleIndex];
this.sampleBuffer[this.sampleIndex] = newVect;
averagedAcceleration = this.sampleSum / SamplesCount;
this.averageAcceleration = averagedAcceleration;
// Stablity check
// If current low-pass filtered sample is deviating for more than 1/100 g from average (max of 0.5 deg inclination noise if device steady)
// then reset the stability counter.
// The calibration will be prevented until the counter is reaching the sample count size (calibration enabled only if entire
// sampling buffer is "stable"
Simple3DVector deltaAcceleration = averagedAcceleration - optimalFilteredAcceleration;
if ((Math.Abs(deltaAcceleration.X) > maximumStabilityDeltaOffset) ||
(Math.Abs(deltaAcceleration.Y) > maximumStabilityDeltaOffset) ||
(Math.Abs(deltaAcceleration.Z) > maximumStabilityDeltaOffset))
{
// Unstable
this.deviceStableCount = 0;
}
else
{
if (this.deviceStableCount < SamplesCount)
{
++this.deviceStableCount;
}
}
}
if (this.ReadingChanged != null)
{
var readingEventArgs = new AccelerometerHelperReadingEventArgs
{
RawAcceleration = rawAcceleration,
LowPassFilteredAcceleration = lowPassFilteredAcceleration,
OptimalyFilteredAcceleration = optimalFilteredAcceleration,
AverageAcceleration = averagedAcceleration
};
this.ReadingChanged(this, readingEventArgs);
}
}
