/// <summary>
/// Aggiorna la posizione del cursore ridefinendone i margini
/// </summary>
void UpdateImagePos(AccelerometerHelperReadingEventArgs e)
{
/* Vado a fare data smoothing dei dati presi dall'accelerometro */
timerX = Math.Round(e.LowPassFilteredAcceleration.X, 3) - x_calib;
timerY = Math.Round(e.LowPassFilteredAcceleration.Y, 3) - y_calib;
Cursore.Margin = new Thickness(getX(), getY(), (width - (getX() + Cursore.Width)), (height - (getY() + Cursore.Height)));
}
void UpdateImagePos(AccelerometerHelperReadingEventArgs e)
{
/* Vado a fare data smoothing dei dati presi dall'accelerometro */
x_calib = Math.Round(e.LowPassFilteredAcceleration.X, 3);
y_calib = Math.Round(e.LowPassFilteredAcceleration.Y, 3);
}
private void OnAccelerometerHelperReadingChanged(object sender, AccelerometerHelperReadingEventArgs e)
{
Deployment.Current.Dispatcher.BeginInvoke(() => UpdateImagePos(e));
}
/// <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);
}
}