/// <summary>
/// Cloning constructor
/// </summary>
/// <param name="v">Vector to clone</param>
public Simple3DVector(Simple3DVector v)
{
if (v != null)
{
X = v.X;
Y = v.Y;
Z = v.Z;
}
}
/// <summary>
/// Calibrates the accelerometer on X and / or Y axis and save data to isolated storage.
/// </summary>
/// <param name="xAxis">calibrates X axis if true</param>
/// <param name="yAxis">calibrates Y axis if true</param>
/// <returns>true if succeeds</returns>
public bool Calibrate(bool xAxis, bool yAxis)
{
bool retval = false;
lock (_sampleBuffer)
{
if (CanCalibrate(xAxis, yAxis))
{
ZeroAccelerationCalibrationOffset =
new Simple3DVector(
xAxis ? -_averageAcceleration.X : ZeroAccelerationCalibrationOffset.X,
yAxis ? -_averageAcceleration.Y : ZeroAccelerationCalibrationOffset.Y,
0);
// Persist data
AccelerometerCalibrationPersisted = ZeroAccelerationCalibrationOffset;
retval = true;
}
}
return(retval);
}
/// <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 sensor_ReadingChanged(object sender, AccelerometerReadingEventArgs e)
{
Simple3DVector lowPassFilteredAcceleration;
Simple3DVector optimalFilteredAcceleration;
Simple3DVector averagedAcceleration;
Simple3DVector rawAcceleration = new Simple3DVector(e.X, e.Y, e.Z);
lock (_sampleBuffer)
{
if (!_initialized)
{ // Initialize file with 1st value
_sampleSum = rawAcceleration * SamplesCount;
_averageAcceleration = rawAcceleration;
// Initialize file with 1st value
for (int i = 0; i < SamplesCount; i++)
{
_sampleBuffer[i] = _averageAcceleration;
}
_previousLowPassOutput = _averageAcceleration;
_previousOptimalFilterOutput = _averageAcceleration;
_initialized = true;
}
// low-pass filter
lowPassFilteredAcceleration = new Simple3DVector(
LowPassFilter(rawAcceleration.X, _previousLowPassOutput.X),
LowPassFilter(rawAcceleration.Y, _previousLowPassOutput.Y),
LowPassFilter(rawAcceleration.Z, _previousLowPassOutput.Z));
_previousLowPassOutput = lowPassFilteredAcceleration;
// optimal filter
optimalFilteredAcceleration = new Simple3DVector(
FastLowAmplitudeNoiseFilter(rawAcceleration.X, _previousOptimalFilterOutput.X),
FastLowAmplitudeNoiseFilter(rawAcceleration.Y, _previousOptimalFilterOutput.Y),
FastLowAmplitudeNoiseFilter(rawAcceleration.Z, _previousOptimalFilterOutput.Z));
_previousOptimalFilterOutput = optimalFilteredAcceleration;
// Increment circular buffer insertion index
_sampleIndex++;
if (_sampleIndex >= SamplesCount)
{
_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;
_sampleSum += newVect;
_sampleSum -= _sampleBuffer[_sampleIndex];
_sampleBuffer[_sampleIndex] = newVect;
averagedAcceleration = _sampleSum / SamplesCount;
_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
_deviceStableCount = 0;
}
else
{
if (_deviceStableCount < SamplesCount)
{
++_deviceStableCount;
}
}
// Add calibrations
rawAcceleration += ZeroAccelerationCalibrationOffset;
lowPassFilteredAcceleration += ZeroAccelerationCalibrationOffset;
optimalFilteredAcceleration += ZeroAccelerationCalibrationOffset;
averagedAcceleration += ZeroAccelerationCalibrationOffset;
}
if (ReadingChanged != null)
{
AccelerometerHelperReadingEventArgs readingEventArgs = new AccelerometerHelperReadingEventArgs();
readingEventArgs.RawAcceleration = rawAcceleration;
readingEventArgs.LowPassFilteredAcceleration = lowPassFilteredAcceleration;
readingEventArgs.OptimalyFilteredAcceleration = optimalFilteredAcceleration;
readingEventArgs.AverageAcceleration = averagedAcceleration;
ReadingChanged(this, readingEventArgs);
}
}
