public virtual void OnCurrentValueChanged(DependencyPropertyChangedEventArgs e) { RangeReading rrNewValue = (RangeReading)e.NewValue; RangeReading rrOldValue = (RangeReading)e.OldValue; //Validate and set the new value if (rrNewValue.angleDegrees > this.angleMaxValue) { rrNewValue.angleDegrees = (int)this.angleMaxValue; } else if (rrNewValue.angleDegrees < this.angleMinValue) { rrNewValue.angleDegrees = (int)this.angleMinValue; } if (rrOldValue != null) { if (rrOldValue.angleDegrees > this.angleMaxValue) { rrOldValue.angleDegrees = (int)this.angleMaxValue; } else if (rrOldValue.angleDegrees < this.angleMinValue) { rrOldValue.angleDegrees = (int)this.angleMinValue; } } //Tracer.Trace("sonar: " + rrNewValue.angleDegrees + " " + rrNewValue.rangeMeters); angleDegreesCurr = rrNewValue.angleDegrees; timestampLastReading = rrNewValue.timestamp; _sonarData.addRangeReading(rrNewValue); //Tracer.Trace("angles: " + _sonarData.angles.Count); numRays = _sonarData.angles.Count; if (pointerVisible) { double db1 = 0; Double oldcurr_realworldunit = 0; Double newcurr_realworldunit = 0; Double realworldunit = (ScaleSweepAngle / (angleMaxValue - angleMinValue)); if (rrOldValue != null) { //Resetting the old value to min value the very first time. if (rrOldValue.angleDegrees == 0 && !isInitialValueSet) { rrOldValue.angleDegrees = (int)angleMinValue; isInitialValueSet = true; } if (rrOldValue.angleDegrees < 0) { db1 = angleMinValue + Math.Abs(rrOldValue.angleDegrees); oldcurr_realworldunit = ((double)(Math.Abs(db1 * realworldunit))); } else { db1 = Math.Abs(angleMinValue) + rrOldValue.angleDegrees; oldcurr_realworldunit = ((double)(db1 * realworldunit)); } } if (rrNewValue.angleDegrees < 0) { db1 = angleMinValue + Math.Abs(rrNewValue.angleDegrees); newcurr_realworldunit = ((double)(Math.Abs(db1 * realworldunit))); } else { db1 = Math.Abs(angleMinValue) + rrNewValue.angleDegrees; newcurr_realworldunit = ((double)(db1 * realworldunit)); } double oldcurrentvalueAngle = (ScaleStartAngle + oldcurr_realworldunit); double newcurrentvalueAngle = (ScaleStartAngle + newcurr_realworldunit); double newcurrentvalueScale = rrNewValue.rangeMeters * 2.0d / rangeMaxValueM; //Animate the pointer from the old value to the new value //MovePointer(newcurrentvalueAngle); AnimatePointer(oldcurrentvalueAngle, newcurrentvalueAngle, newcurrentvalueScale); } else { drawScannedArea(); } }
public bool setSonarRayReading(int angleRaw1, double rangeMeters1, int angleRaw2, double rangeMeters2, long timestamp) { bool haveData = false; //Tracer.Trace("setReading() sonar: " + angleRaw1 + " " + rangeMeters1 + " " + angleRaw2 + " " + rangeMeters2); if (inTestSamples > 0) { inTestSamples--; // for a while just try figuring out what comes in - sweep angle ranges for both sides if (inTestSamples > TEST_SAMPLES_COUNT - 10) // first few frames are garbled { return(haveData); } board.registerAnglesRaw(angleRaw1, angleRaw2); if (inTestSamples == 0) // last count { board.finishPrerun(); Tracer.Trace(board.ToString()); } return(haveData); } if (angleRaw1 == board.rawAngle1_Min || angleRaw1 == board.rawAngle1_Max) { RangeReading rr1 = board.angleRawToSonarAngleCombo(1, angleRaw1, rangeMeters1, timestamp); RangeReading rr2 = board.angleRawToSonarAngleCombo(2, angleRaw2, rangeMeters2, timestamp); sonarData.addRangeReading(rr1, rr2, timestamp); if (sonarData.angles.Count == 26) { lock (_sonarDatas) { _sonarDatas.Enqueue(sonarData); } haveData = true; // allow DataPort.Post(new Recv()) to happen, with data already enqueued in the ...Datas _badCount = 0; TimeSpan sinceLastReading = new TimeSpan(timestamp - timestampLastReading); TimeSpan sinceLastSweepReading = new TimeSpan(timestamp - timestampLastSweepReading); //Tracer.Trace("SONAR PACKET READY -- angles: " + sonarData.angles.Count + " packets: " + _sonarDatas.Count // + (new DateTime(timestamp)).ToString() // + String.Format(" {0:0.000} s/ray {1:0.000} s/sweep", sinceLastReading.TotalSeconds, sinceLastSweepReading.TotalSeconds)); timestampLastSweepReading = timestamp; } else { _badCount++; Tracer.Trace(string.Format("BAD SONAR PACKET -- angles: {0} angleRaw1: {1} - #{2} since last good packet", sonarData.angles.Count, angleRaw1, _badCount)); } sonarData = new SonarData(); // prepare for the next one } timestampLastReading = timestamp; RangeReading rrr1 = board.angleRawToSonarAngleCombo(1, angleRaw1, rangeMeters1, timestamp); RangeReading rrr2 = board.angleRawToSonarAngleCombo(2, angleRaw2, rangeMeters2, timestamp); sonarData.addRangeReading(rrr1, rrr2, timestamp); return(haveData); }