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);
}