/// <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>
/// 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, SensorReadingEventArgs<AccelerometerReading> e)
{
_rawAcceleration.Set(e.SensorReading.Acceleration.X, e.SensorReading.Acceleration.Y, e.SensorReading.Acceleration.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.Set(
LowPassFilter(_rawAcceleration.X, _previousLowPassOutput.X),
LowPassFilter(_rawAcceleration.Y, _previousLowPassOutput.Y),
LowPassFilter(_rawAcceleration.Z, _previousLowPassOutput.Z));
_previousLowPassOutput = _lowPassFilteredAcceleration;
// optimal filter
_optimalFilteredAcceleration.Set(
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;
_averageAcceleration = _sampleSum / SamplesCount;
// Stability 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 = _averageAcceleration - _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;
_averageAcceleration += ZeroAccelerationCalibrationOffset;
}
if (ReadingChanged != null)
{
AccelerometerHelperReadingEventArgs readingEventArgs = new AccelerometerHelperReadingEventArgs();
readingEventArgs.RawAcceleration = _rawAcceleration;
readingEventArgs.LowPassFilteredAcceleration = _lowPassFilteredAcceleration;
readingEventArgs.OptimallyFilteredAcceleration = _optimalFilteredAcceleration;
readingEventArgs.AverageAcceleration = _averageAcceleration;
ReadingChanged(this, readingEventArgs);
}
}
/// <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);
retval = true;
}
}
return retval;
}
/// <summary>
/// Private constructor,
/// Use Instance property to get singleton instance
/// </summary>
private AccelerometerHelper()
{
// Determine if accelerometer is present
_sensor = new Accelerometer();
if (_sensor == null)
{
NoAccelerometer = true;
}
else
{
NoAccelerometer = (_sensor.State == SensorState.NotSupported);
}
_sensor = null;
//Set up buckets for calculating rolling average of the accelerations
_sampleIndex = 0;
ZeroAccelerationCalibrationOffset = new Simple3DVector();
_lowPassFilteredAcceleration = new Simple3DVector();
_optimalFilteredAcceleration = new Simple3DVector();
_averageAcceleration = new Simple3DVector();
_rawAcceleration = new Simple3DVector();
}
/// <summary>
/// Process still signal: calculate average still vector
/// </summary>
private void ProcessStillSignal()
{
Simple3DVector sumVector = new Simple3DVector(0, 0, 0);
int count = 0;
// going over vectors in still signal
// still signal was saved backwards, i.e. newest vectors are first
foreach (Simple3DVector currentStillVector in _stillSignal)
{
// make sure current vector is very still
bool isStillMagnitude = (Math.Abs(_lastStillVector.Magnitude - currentStillVector.Magnitude) < StillMagnitudeWithoutGravitationThreshold);
if (isStillMagnitude)
{
// sum x,y,z values
sumVector += currentStillVector;
++count;
// 20 samples are sufficent
if (count >= MaximumStillVectorsNeededForAverage)
{
break;
}
}
}
// need at least a few vectors to get a good average
if (count >= MinimumStillVectorsNeededForAverage)
{
// calculate average of still vectors
_lastStillVector = sumVector / count;
}
}
/// <summary>
/// Classify vector shake type
/// </summary>
private ShakeType ClassifyVectorShakeType(Simple3DVector v)
{
double absX = Math.Abs(v.X);
double absY = Math.Abs(v.Y);
double absZ = Math.Abs(v.Z);
// check if X is the most significant component
if ((absX >= absY) & (absX >= absZ))
{
return ShakeType.X;
}
// check if Y is the most significant component
if ((absY >= absX) & (absY >= absZ))
{
return ShakeType.Y;
}
// Z is the most significant component
return ShakeType.Z;
}
private void AddVectorToStillSignal(Simple3DVector currentVector)
{
// add current vector to still signal, newest vectors are first
_stillSignal.AddFirst(currentVector);
// if still signal is getting too big, remove old items
if (_stillSignal.Count > 2 * MaximumStillVectorsNeededForAverage)
{
_stillSignal.RemoveLast();
}
}
/// <summary>
/// Add a vector the shake signal and does some preprocessing
/// </summary>
private void AddVectorToShakeSignal(Simple3DVector currentVector)
{
// remove still vector from current vector
Simple3DVector currentVectorWithoutGravitation = currentVector - _lastStillVector;
// add current vector to shake signal
_shakeSignal.Add(currentVectorWithoutGravitation);
// skip weak vectors
if (currentVectorWithoutGravitation.Magnitude < WeakMagnitudeWithoutGravitationThreshold)
{
return;
}
// classify vector
ShakeType vectorShakeType = ClassifyVectorShakeType(currentVectorWithoutGravitation);
// count vector to histogram
_shakeHistogram[(int)vectorShakeType]++;
}
/// <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.OptimallyFilteredAcceleration = optimalFilteredAcceleration;
readingEventArgs.AverageAcceleration = averagedAcceleration;
ReadingChanged(this, readingEventArgs);
}
}
/// <summary>
/// Main orientation change detection logic
/// </summary>
/// <param name="filteredAcceleration">current filtered acceleration</param>
private void CheckOrientation(Simple3DVector filteredAcceleration)
{
DeviceOrientation currentOrientation = DeviceOrientation.Unknown;
double xAcceleration = filteredAcceleration.X;
double yAcceleration = filteredAcceleration.Y;
double zAcceleration = filteredAcceleration.Z;
// Normalize acceleration to 1g
double magnitudeXYZ = Math.Sqrt(xAcceleration * xAcceleration + yAcceleration * yAcceleration + zAcceleration * zAcceleration);
xAcceleration = xAcceleration / magnitudeXYZ;
yAcceleration = yAcceleration / magnitudeXYZ;
zAcceleration = zAcceleration / magnitudeXYZ;
if (_currentOrientation == DeviceOrientation.Unknown)
{ // No pre-existing orientation: default is flat
if (zAcceleration < 0)
{
currentOrientation = DeviceOrientation.ScreenSideUp;
}
else
{
currentOrientation = DeviceOrientation.ScreenSideDown;
}
}
if (yAcceleration < -tiltAccelerationThreshold)
{
currentOrientation = DeviceOrientation.PortraitRightSideUp;
}
else if (yAcceleration > tiltAccelerationThreshold)
{
currentOrientation = DeviceOrientation.PortraitUpSideDown;
}
else if (xAcceleration < -tiltAccelerationThreshold)
{
currentOrientation = DeviceOrientation.LandscapeLeft;
}
else if (xAcceleration > tiltAccelerationThreshold)
{
currentOrientation = DeviceOrientation.LandscapeRight;
}
else if (zAcceleration < -tiltAccelerationThreshold)
{
currentOrientation = DeviceOrientation.ScreenSideUp;
}
else if (zAcceleration > tiltAccelerationThreshold)
{
currentOrientation = DeviceOrientation.ScreenSideDown;
}
DeviceOrientation previousOrientation = DeviceOrientation.Unknown;
bool fireEvent = false;
if (currentOrientation != DeviceOrientation.Unknown)
{
lock (this) // Keep the lock as brief as posible
{
_currentOrientation = currentOrientation;
if (_previousOrientation != _currentOrientation)
{
previousOrientation = _previousOrientation;
_previousOrientation = _currentOrientation;
fireEvent = true;
}
}
}
if (fireEvent)
{
DeviceOrientationChangedEventArgs orientationEventArgs = new DeviceOrientationChangedEventArgs();
orientationEventArgs.CurrentOrientation = currentOrientation;
orientationEventArgs.PreviousOrientation = previousOrientation;
if (OrientationChanged != null)
{
OrientationChanged(this, orientationEventArgs);
}
}
}
private void sensor_ReadingChanged(object sender, AccelerometerReadingEventArgs e)
{
Simple3DVector simple3Dvector1 = new Simple3DVector(e.X, e.Y, e.Z);
Simple3DVector simple3Dvector2;
Simple3DVector simple3Dvector3;
Simple3DVector simple3Dvector4;
lock (this._sampleBuffer)
{
if (!this._initialized)
{
this._sampleSum = simple3Dvector1 * 25.0;
this._averageAcceleration = simple3Dvector1;
for (int index = 0; index < 25; ++index)
{
this._sampleBuffer[index] = this._averageAcceleration;
}
this._previousLowPassOutput = this._averageAcceleration;
this._previousOptimalFilterOutput = this._averageAcceleration;
this._initialized = true;
}
simple3Dvector2 = new Simple3DVector(AccelerometerHelper.LowPassFilter(simple3Dvector1.X, this._previousLowPassOutput.X), AccelerometerHelper.LowPassFilter(simple3Dvector1.Y, this._previousLowPassOutput.Y), AccelerometerHelper.LowPassFilter(simple3Dvector1.Z, this._previousLowPassOutput.Z));
this._previousLowPassOutput = simple3Dvector2;
simple3Dvector3 = new Simple3DVector(AccelerometerHelper.FastLowAmplitudeNoiseFilter(simple3Dvector1.X, this._previousOptimalFilterOutput.X), AccelerometerHelper.FastLowAmplitudeNoiseFilter(simple3Dvector1.Y, this._previousOptimalFilterOutput.Y), AccelerometerHelper.FastLowAmplitudeNoiseFilter(simple3Dvector1.Z, this._previousOptimalFilterOutput.Z));
this._previousOptimalFilterOutput = simple3Dvector3;
this._sampleIndex = this._sampleIndex + 1;
if (this._sampleIndex >= 25)
{
this._sampleIndex = 0;
}
Simple3DVector simple3Dvector5 = simple3Dvector3;
this._sampleSum = this._sampleSum + simple3Dvector5;
this._sampleSum = this._sampleSum - this._sampleBuffer[this._sampleIndex];
this._sampleBuffer[this._sampleIndex] = simple3Dvector5;
simple3Dvector4 = this._sampleSum / 25.0;
this._averageAcceleration = simple3Dvector4;
Simple3DVector simple3Dvector6 = simple3Dvector4 - simple3Dvector3;
if (Math.Abs(simple3Dvector6.X) > AccelerometerHelper._maximumStabilityDeltaOffset || Math.Abs(simple3Dvector6.Y) > AccelerometerHelper._maximumStabilityDeltaOffset || Math.Abs(simple3Dvector6.Z) > AccelerometerHelper._maximumStabilityDeltaOffset)
{
this._deviceStableCount = 0;
}
else if (this._deviceStableCount < 25)
{
this._deviceStableCount = this._deviceStableCount + 1;
}
simple3Dvector1 += this.ZeroAccelerationCalibrationOffset;
simple3Dvector2 += this.ZeroAccelerationCalibrationOffset;
simple3Dvector3 += this.ZeroAccelerationCalibrationOffset;
simple3Dvector4 += this.ZeroAccelerationCalibrationOffset;
}
// ISSUE: reference to a compiler-generated field
if (this.ReadingChanged == null)
{
return;
}
// ISSUE: reference to a compiler-generated field
this.ReadingChanged(this, new AccelerometerHelperReadingEventArgs()
{
RawAcceleration = simple3Dvector1,
LowPassFilteredAcceleration = simple3Dvector2,
OptimallyFilteredAcceleration = simple3Dvector3,
AverageAcceleration = simple3Dvector4
});
}
private void CheckOrientation(Simple3DVector filteredAcceleration)
{
DeviceOrientation deviceOrientation1 = DeviceOrientation.Unknown;
double x = filteredAcceleration.X;
double y = filteredAcceleration.Y;
double z = filteredAcceleration.Z;
double num1 = Math.Sqrt(x * x + y * y + z * z);
double num2 = x / num1;
double num3 = y / num1;
double num4 = z / num1;
if (this._currentOrientation == DeviceOrientation.Unknown)
{
deviceOrientation1 = num4 >= 0.0 ? DeviceOrientation.ScreenSideDown : DeviceOrientation.ScreenSideUp;
}
if (num3 < -0.8)
{
deviceOrientation1 = DeviceOrientation.PortraitRightSideUp;
}
else if (num3 > 0.8)
{
deviceOrientation1 = DeviceOrientation.PortraitUpSideDown;
}
else if (num2 < -0.8)
{
deviceOrientation1 = DeviceOrientation.LandscapeLeft;
}
else if (num2 > 0.8)
{
deviceOrientation1 = DeviceOrientation.LandscapeRight;
}
else if (num4 < -0.8)
{
deviceOrientation1 = DeviceOrientation.ScreenSideUp;
}
else if (num4 > 0.8)
{
deviceOrientation1 = DeviceOrientation.ScreenSideDown;
}
DeviceOrientation deviceOrientation2 = DeviceOrientation.Unknown;
bool flag = false;
if (deviceOrientation1 != DeviceOrientation.Unknown)
{
lock (this)
{
this._currentOrientation = deviceOrientation1;
if (this._previousOrientation != this._currentOrientation)
{
deviceOrientation2 = this._previousOrientation;
this._previousOrientation = this._currentOrientation;
flag = true;
}
}
}
if (!flag)
{
return;
}
DeviceOrientationChangedEventArgs e = new DeviceOrientationChangedEventArgs();
e.CurrentOrientation = deviceOrientation1;
e.PreviousOrientation = deviceOrientation2;
// ISSUE: reference to a compiler-generated field
if (this.OrientationChanged == null)
{
return;
}
// ISSUE: reference to a compiler-generated field
this.OrientationChanged(this, e);
}
/// <summary>
/// Main orientation change detection logic
/// </summary>
/// <param name="filteredAcceleration">current filtered acceleration</param>
private void CheckOrientation(Simple3DVector filteredAcceleration)
{
DeviceOrientation currentOrientation = DeviceOrientation.Unknown;
double xAcceleration = filteredAcceleration.X;
double yAcceleration = filteredAcceleration.Y;
double zAcceleration = filteredAcceleration.Z;
// Normalize acceleration to 1g
double magnitudeXYZ = Math.Sqrt(xAcceleration * xAcceleration + yAcceleration * yAcceleration + zAcceleration * zAcceleration);
xAcceleration = xAcceleration / magnitudeXYZ;
yAcceleration = yAcceleration / magnitudeXYZ;
zAcceleration = zAcceleration / magnitudeXYZ;
if (_currentOrientation == DeviceOrientation.Unknown)
{ // No pre-existing orientation: default is flat
if (zAcceleration < 0)
{
currentOrientation = DeviceOrientation.ScreenSideUp;
}
else
{
currentOrientation = DeviceOrientation.ScreenSideDown;
}
}
if (yAcceleration < -tiltAccelerationThreshold)
{
currentOrientation = DeviceOrientation.PortraitRightSideUp;
}
else if (yAcceleration > tiltAccelerationThreshold)
{
currentOrientation = DeviceOrientation.PortraitUpSideDown;
}
else if (xAcceleration < -tiltAccelerationThreshold)
{
currentOrientation = DeviceOrientation.LandscapeLeft;
}
else if (xAcceleration > tiltAccelerationThreshold)
{
currentOrientation = DeviceOrientation.LandscapeRight;
}
else if (zAcceleration < -tiltAccelerationThreshold)
{
currentOrientation = DeviceOrientation.ScreenSideUp;
}
else if (zAcceleration > tiltAccelerationThreshold)
{
currentOrientation = DeviceOrientation.ScreenSideDown;
}
DeviceOrientation previousOrientation = DeviceOrientation.Unknown;
bool fireEvent = false;
if (currentOrientation != DeviceOrientation.Unknown)
{
lock (this) // Keep the lock as brief as posible
{
_currentOrientation = currentOrientation;
if (_previousOrientation != _currentOrientation)
{
previousOrientation = _previousOrientation;
_previousOrientation = _currentOrientation;
fireEvent = true;
}
}
}
if (fireEvent)
{
DeviceOrientationChangedEventArgs orientationEventArgs = new DeviceOrientationChangedEventArgs();
orientationEventArgs.CurrentOrientation = currentOrientation;
orientationEventArgs.PreviousOrientation = previousOrientation;
if (OrientationChanged != null)
{
OrientationChanged(this, orientationEventArgs);
}
}
}
