/// <summary> /// Change orientation of bubble surface or tube /// </summary> /// <param name="e">Event arguments</param> private void ChangeLevelOrientation(DeviceOrientationChangedEventArgs e) { _bubbleSpeed = new Simple3DVector(); if (DeviceOrientationHelper.IsFlat(e.CurrentOrientation)) { SurfaceShown = true; DeviceOrientationInfo priorDoi = DeviceOrientationHelper.GetDeviceOrientationInfo(e.PreviousOrientation); _bubblePosition = new Simple3DVector(priorDoi.NormalGravityVector.X, priorDoi.NormalGravityVector.Y, 0); } else { DeviceOrientationInfo doi = DeviceOrientationHelper.GetDeviceOrientationInfo(e.CurrentOrientation); if (DeviceOrientationHelper.IsFlat(e.PreviousOrientation) || (e.PreviousOrientation == DeviceOrientation.Unknown)) { MoveLevel.Angle = doi.AngleOnXYPlan; _bubblePosition = new Simple3DVector(); SurfaceShown = false; } double accumulatedLoops = MoveLevel.Angle / 360; accumulatedLoops = Math.Floor(accumulatedLoops + 0.5); _tubeRotationAnimation.To = 360 * accumulatedLoops + doi.AngleOnXYPlan; if ((_tubeRotationAnimation.To - MoveLevel.Angle) > 180) { _tubeRotationAnimation.To -= 360; } if ((_tubeRotationAnimation.To - MoveLevel.Angle) < -180) { _tubeRotationAnimation.To += 360; } _tubeRotationStoryboard.Begin(); _bubbleDirection = doi.HorizontalAxisPolarity; } _levelOrientation = e.CurrentOrientation; }
/// <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; } }
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> /// 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); }
/// <summary> /// Called when the accelerometer provides a new value /// </summary> private void OnAccelerometerHelperReadingChanged(object sender, AccelerometerHelperReadingEventArgs e) { // work with optimal vector (without noise) Simple3DVector currentVector = e.OptimalyFilteredAcceleration; // check if this vector is considered a shake vector bool isShakeMagnitude = (Math.Abs(_lastStillVector.Magnitude - currentVector.Magnitude) > ShakeMagnitudeWithoutGravitationThreshold); // following is a state machine for detection of shake signal start and end // if still --> shake if ((!_isInShakeState) && (isShakeMagnitude)) { // set shake state flag _isInShakeState = true; // clear old shake signal ClearShakeSignal(); // process still signal ProcessStillSignal(); // add vector to shake signal AddVectorToShakeSignal(currentVector); } // if still --> still else if ((!_isInShakeState) && (!isShakeMagnitude)) { // add vector to still signal AddVectorToStillSignal(currentVector); } // if shake --> shake else if ((_isInShakeState) && (isShakeMagnitude)) { // add vector to shake signal AddVectorToShakeSignal(currentVector); // reset still counter _stillCounter = 0; // try to process shake signal if (ProcessShakeSignal()) { // shake signal generated, clear used data ClearShakeSignal(); } } // if shake --> still else if ((_isInShakeState) && (!isShakeMagnitude)) { // add vector to shake signal AddVectorToShakeSignal(currentVector); // count still vectors _stillCounter++; // if too much still samples if (_stillCounter > StillCounterThreshold) { // clear old still signal _stillSignal.Clear(); // add still vectors from shake signal to still signal for (int i = 0; i < StillCounterThreshold; ++i) { // calculate current index element int currentSampleIndex = _shakeSignal.Count - StillCounterThreshold + i; // add vector to still signal AddVectorToStillSignal(currentVector); } // remove last samples from shake signal _shakeSignal.RemoveRange(_shakeSignal.Count - StillCounterThreshold, StillCounterThreshold); // reset shake state flag _isInShakeState = false; _stillCounter = 0; // try to process shake signal if (ProcessShakeSignal()) { ClearShakeSignal(); } } } }
/// <summary> /// Update the tube level visuals /// </summary> private void UpdateTubeBubble(AccelerometerHelperReadingEventArgs e) { // ANGLE TEXT // ---------- // Use filtered accelemeter data (st5ady) double x = e.OptimallyFilteredAcceleration.X; double y = e.OptimallyFilteredAcceleration.Y; double horizontalAcceleration; double verticalAcceleration; // Choose appropriate axis based on orientation of level horizontalAcceleration = _bubbleDirection * (DeviceOrientationHelper.IsPortrait(_levelOrientation) ? x : y); verticalAcceleration = _bubbleDirection * (DeviceOrientationHelper.IsPortrait(_levelOrientation) ? y : -x); // rotate XY plan by -90 // Convert acceleration vector coordinates to Angles and Magnitude // Update reading on screen of instant inclination assuming steady device (gravity = measured acceleration) double magnitudeXYZ = e.OptimallyFilteredAcceleration.Magnitude; double angle = 0.0; if (magnitudeXYZ != 0.0) { angle = Math.Asin(horizontalAcceleration / magnitudeXYZ) * 180.0 / Math.PI; } _angle = angle; // Display angles as if they were buoyancy force instead of gravity (opposite) since it is // more natural to match targeted bubble location _angleText = String.Format("{0:0.0}°", _angle); // BUBBLE POSITION // --------------- // ---------------------------------------------------------- // For simplicity we are approximating that the bubble experiences a lateral attraction force // proportional to the distance to its target location (top of the glass based on inclination). // We will neglect the vertical speed of the bubble since the radius of the glass curve is much greater // than the radius of radius of usable glass surface // Assume tube curve has a 1m radius // Destination position is x and y // Current position is _bubblePosition.X (use only one dimension) // Update Buoyancy double lateralAcceleration = (horizontalAcceleration - _bubblePosition.X) * BuoyancyCoef * StandardGravityInMetric; // Update drag: double drag = _bubbleSpeed.X * (-ViscosityCoef); // Update speed: lateralAcceleration += drag; lateralAcceleration /= AccelerometerRefreshRate; // impulse _bubbleSpeed += new Simple3DVector(lateralAcceleration, 0, 0); // Update position _bubblePosition += (_bubbleSpeed / AccelerometerRefreshRate); double edgeRadius = Math.Sin(EdgeGlassAngle); // Get resulting direction and magnitude of bubble position given X double magnitudeFlat = Math.Abs(_bubblePosition.X); double direction = Math.Sign(_bubblePosition.X); bool atEdge = false; if (magnitudeFlat > edgeRadius) { // Bubble reaches the edge magnitudeFlat = edgeRadius; // lossy bouncing when reaching edges // change direction _bubbleSpeed *= -EdgeBouncingLossCoef; // limit bubble position to edge _bubblePosition = new Simple3DVector(magnitudeFlat * direction, 0, 0); atEdge = true; } // Calculate position of bubble MoveBubble.X = (_bubblePosition.X / edgeRadius) * ((UsableLateralAmplitude / 2) - (BubbleCanvas.Width / 4)); // Change bubble shape double stretchRatio; if (atEdge) { TubeBubbleSkew.AngleX = 0; stretchRatio = 1.0 + (Math.Abs(Math.Sin(angle / 180.0 * Math.PI)) - Math.Sin(EdgeGlassAngle)) * AngleBasedTubeBubbleStrechingAtEdgeCoef; TubeBubbleScale.ScaleX = 1.0 / stretchRatio; TubeBubbleScale.ScaleY = SideWayBubbleAtEdgeBoostOnYAxis * stretchRatio; } else { double horizontalSpeed = Math.Abs(_bubbleSpeed.X); double horizontalDirection = Math.Sign(_bubbleSpeed.X); // Stretch is proportional to horizontal speed stretchRatio = Math.Min(horizontalSpeed * SpeedBubbleStrechingCoef, MaximumBubbleXYStretchRatio - 1.0) + 1.0; if (stretchRatio < MinimumStretchRatio) { stretchRatio = MinimumStretchRatio; } TubeBubbleSkew.AngleX = MaximumTubeBubbleSkew * (stretchRatio - 1.0) / (MaximumBubbleXYStretchRatio - 1.0) * horizontalDirection; // Stretch is also proportional to buoyancy stretchRatio -= TubeBubbleStretchBuoyancyCoef * ((verticalAcceleration / magnitudeXYZ) - MedianVerticalAcceleration); if (stretchRatio < MinimumStretchRatio) { stretchRatio = MinimumStretchRatio; } TubeBubbleScale.ScaleX = 1.0 / stretchRatio; TubeBubbleScale.ScaleY = MaximumBubbleXYStretchRatio * stretchRatio; } }
/// <summary> /// Update the surface level visuals /// </summary> private void UpdateSurfaceBubble(AccelerometerHelperReadingEventArgs e) { // ANGLE TEXT // ---------- // Use filtered accelemeter data (steady) double x = -e.OptimallyFilteredAcceleration.X; // Orientation compensation double y = e.OptimallyFilteredAcceleration.Y; // Convert acceleration vector coordinates to Angles and Magnitude // Update reading on screen of instant inclination assuming steady device (gravity = measured acceleration) double magnitudeXYZ = e.OptimallyFilteredAcceleration.Magnitude; double xAngle = 0.0; double yAngle = 0.0; if (magnitudeXYZ != 0.0) { xAngle = Math.Asin(x / magnitudeXYZ) * 180.0 / Math.PI; yAngle = Math.Asin(y / magnitudeXYZ) * 180.0 / Math.PI; } _angle = Math.Abs(xAngle) + Math.Abs(yAngle); // Display angles as if they were buoyancy force instead of gravity (opposite) since it is // more natural to match targeted bubble location // Also orientation of Y-axis is opposite of screen for natural upward orientation _angleText = String.Format("{0:0.0}° {1:0.0}°", xAngle, -yAngle); // BUBBLE POSITION // --------------- // ---------------------------------------------------------- // For simplicity we are approximating that the bubble experiences a lateral attraction force // proportional to the distance to its target location (top of the glass based on inclination). // We will neglect the vertical speed of the bubble since the radius of the glass curve is much greater // than the radius of radius of usable glass surface // Assume sphere has a 1m radius // Destination position is x and y // Current position is _bubblePosition // Update Buoyancy Simple3DVector lateralAcceleration = (new Simple3DVector(x, y, 0) - _bubblePosition) * BuoyancyCoef * StandardGravityInMetric; // Update drag: Simple3DVector drag = _bubbleSpeed * (-ViscosityCoef); // Update speed: lateralAcceleration += drag; lateralAcceleration /= AccelerometerRefreshRate; // impulse _bubbleSpeed += lateralAcceleration; // Update position _bubblePosition += _bubbleSpeed / AccelerometerRefreshRate; double edgeRadius = Math.Sin(EdgeGlassAngle); x = _bubblePosition.X; y = _bubblePosition.Y; // Get resulting angle and magnitude of bubble position given X and Y double angleFlat = Math.Atan2(y, x); double magnitudeFlat = Math.Sqrt(x * x + y * y); bool atEdge = false; if (magnitudeFlat > edgeRadius) { // Bubble reaches the edge magnitudeFlat = edgeRadius; // lossy bouncing when reaching edges _bubbleSpeed *= EdgeBouncingLossCoef; // Mirror movement along center to edge line _bubbleSpeed = new Simple3DVector(_bubbleSpeed.X * Math.Cos(2 * angleFlat) + _bubbleSpeed.Y * Math.Sin(2 * angleFlat), _bubbleSpeed.X * Math.Sin(2 * angleFlat) - _bubbleSpeed.Y * Math.Cos(2 * angleFlat), 0); // change direction on x and y _bubbleSpeed *= new Simple3DVector(-1, -1, 1); // limit bubble position to edge _bubblePosition = new Simple3DVector(magnitudeFlat * Math.Cos(angleFlat), magnitudeFlat * Math.Sin(angleFlat), 0); atEdge = true; } // Set x and y location of the surface level bubble based on angle and magnitude double xPixelLocation = Math.Cos(angleFlat) * (magnitudeFlat / edgeRadius) * (UsableLateralAmplitude - SurfaceBubble.Width) / 2; double yPixelLocation = Math.Sin(angleFlat) * (magnitudeFlat / edgeRadius) * (UsableLateralAmplitude - SurfaceBubble.Width) / 2; SurfaceBubble.SetValue(Canvas.LeftProperty, xPixelLocation + (SurfaceLevelOuter.Width - SurfaceBubble.Width) / 2); SurfaceBubble.SetValue(Canvas.TopProperty, yPixelLocation + (SurfaceLevelOuter.Height - SurfaceBubble.Width) / 2); // Change bubble shape double stretchRatio; double horizontalDirection; if (atEdge) { stretchRatio = MaximumBubbleXYStretchRatio; horizontalDirection = angleFlat - Math.PI / 2; } else { x = _bubbleSpeed.X; y = _bubbleSpeed.Y; double horizontalSpeed = Math.Sqrt(x * x + y * y); horizontalDirection = Math.Atan2(y, x) - Math.PI / 2; stretchRatio = Math.Min(horizontalSpeed * SpeedBubbleStrechingCoef, MaximumBubbleXYStretchRatio - 1.0) + 1.0; } SurfaceBubbleDirection.Angle = horizontalDirection * 180.0 / Math.PI; SurfaceBubbleScale.ScaleX = stretchRatio; SurfaceBubbleScale.ScaleY = 1.0 / stretchRatio; }
/// <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); } }
void _accelerometer_ReadingChanged(object sender, AccelerometerHelperReadingEventArgs e) { if (_last == null) { _last = e.RawAcceleration; } else { Simple3DVector _current = e.RawAcceleration; double _cosa = (_current.X * _last.X + _current.Y * _last.Y + _current.Z * _last.Z) / (Math.Sqrt(_current.X * _current.X + _current.Y * _current.Y + _current.Z * _current.Z) * Math.Sqrt(_last.X * _last.X + _last.Y * _last.Y + _last.Z * _last.Z)); if (_samplesCount < 10) { _dsamples[_samplesCount] += _cosa; _samplesCount += 1; } else { Array.Copy(_dsamples, 1, _dsamples, 0, _dsamples.Length - 1); _dsamples[9] = _cosa; if (_skip >= 10) { _skip = 0; _isSkipping = false; } else { if (_isSkipping) { _skip += 1; } } if (!_isSkipping) { //double _weightedAverage = (double)Enumerable.Range(0, 9).Select(x => (x + 2) * _dsamples[x]).Sum() / 55; ////don't know why +2 works but as long as it does...or not //non-LINQ alternative double _weightedAverage = 0; for (int i = 0; i < 10; i++) { _weightedAverage += (i + 1) * _dsamples[i]; } _weightedAverage = _weightedAverage / 55; if (_weightedAverage > _lastAverage && _weightedAverage < 0.95) //hardcoded threshold { OnStepDetected(); _isSkipping = true; _skip = 0; } _lastAverage = _weightedAverage; } } _last = _current; } }
public DeviceOrientationInfo(double angle, int horizontalScreenAxisPolarity, Simple3DVector typicalGravityVector) { this.AngleOnXYPlan = angle; this.HorizontalAxisPolarity = horizontalScreenAxisPolarity; this.NormalGravityVector = typicalGravityVector; }
public DeviceOrientationInfo(double angle, int horizontalScreenAxisPolarity, Simple3DVector typicalGravityVector) { AngleOnXYPlan = angle; HorizontalAxisPolarity = horizontalScreenAxisPolarity; NormalGravityVector = typicalGravityVector; }