示例#1
0
 /// <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;
     }
 }
示例#2
0
        /// <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);
            }
        }