// Invoke the HasReadFrame event; called whenever the Read completes:
protected virtual void OnDataFrameComplete(AsyncInputFrameArgs e)
{
if (HasReadFrame != null)
{
HasReadFrame(this, e);
}
}
// This will be called whenever the data is read from the board:
private void DataReadCompleteHandler(object sender, AsyncInputReportArgs aira)
{
Tracer.Trace("Async data arrived. " + DateTime.Now);
StringBuilder byteValue = new StringBuilder();
byteValue.Append("DataReadCompleteHandler(): Input Report Data: ");
for (int count = 0; count <= aira.InputBuffer.Length - 1; count++)
{
// Display bytes as 2-character Hex strings.
byteValue.AppendFormat("{0:X02} ", aira.InputBuffer[count]);
}
Tracer.Trace(byteValue.ToString());
int servo1target = intFromBuffer(aira.InputBuffer, 1);
int servo2target = intFromBuffer(aira.InputBuffer, 3);
int ping1value = intFromBuffer(aira.InputBuffer, 5);
int ping2value = intFromBuffer(aira.InputBuffer, 7);
AsyncInputFrameArgs args = new AsyncInputFrameArgs(servo1target, servo2target, ping1value, ping2value);
OnDataFrameComplete(args);
if (inDataContinuousMode)
{
// initiate next read:
ReadFromDevice(new EventHandler <AsyncInputReportArgs>(DataReadCompleteHandler));
}
}
// This will be called whenever the data is read from the board:
private void pmFrameCompleteHandler(object sender, AsyncInputFrameArgs aira)
{
long timestamp = DateTime.Now.Ticks;
Tracer.Trace("Async sonar frame arrived. " + DateTime.Now);
StringBuilder frameValue = new StringBuilder();
frameValue.AppendFormat("{0}\r\n", aira.dPos1Mks);
frameValue.AppendFormat("{0}\r\n", aira.dPos2Mks);
frameValue.AppendFormat("{0}\r\n", aira.dPing1DistanceMm);
frameValue.AppendFormat("{0}\r\n", aira.dPing2DistanceMm);
pmValuesTextBox.Text = frameValue.ToString();
int angleRaw1 = (int)aira.dPos1Mks;
double distCm1 = aira.dPing1DistanceMm / 10.0d;
int angleRaw2 = (int)aira.dPos2Mks;
double distCm2 = aira.dPing2DistanceMm / 10.0d;
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;
}
//rawAngle1_Min = int.MaxValue;
//rawAngle1_Max = int.MinValue;
//rawAngle2_Min = int.MaxValue;
//rawAngle2_Max = int.MinValue;
//inTestSamples = 200;
rawAngle1_Min = Math.Min(rawAngle1_Min, angleRaw1);
rawAngle1_Max = Math.Max(rawAngle1_Max, angleRaw1);
rawAngle2_Min = Math.Min(rawAngle2_Min, angleRaw2);
rawAngle2_Max = Math.Max(rawAngle2_Max, angleRaw2);
if (!rawAngle1Values.ContainsKey(angleRaw1))
{
rawAngle1Values.Add(angleRaw1, angleRaw1);
}
if (!rawAngle2Values.ContainsKey(angleRaw2))
{
rawAngle2Values.Add(angleRaw2, angleRaw2);
}
if (inTestSamples == 0) // last count
{
numRays1 = rawAngle1Values.Count;
numRays2 = rawAngle2Values.Count;
angleRawStep1 = (int)Math.Round((double)(rawAngle1_Max - rawAngle1_Min) / (double)numRays1);
angleRawStep2 = (int)Math.Round((double)(rawAngle2_Max - rawAngle2_Min) / (double)numRays2);
angleDegreesStep1 = (angleDegrees1_Max - angleDegrees1_Min) / numRays1;
angleDegreesStep2 = (angleDegrees2_Max - angleDegrees2_Min) / numRays2;
StringBuilder sBuf = new StringBuilder();
sBuf.Append("setReading(): numRays1=" + numRays1 + " angleRawStep1=" + angleRawStep1 + " rawAngle1_Min=" + rawAngle1_Min + " rawAngle1_Max=" + rawAngle1_Max + " angleDegreesStep1=" + angleDegreesStep1 + "\r\nsweep angles1 (us) : ");
for (int count = 0; count < rawAngle1Values.Count; count++)
{
// Display bytes as 2-character Hex strings.
sBuf.AppendFormat("{0} ", rawAngle1Values.ElementAt(count).Key);
}
sBuf.Append("\r\nsetReading(): numRays2=" + numRays2 + " angleRawStep2=" + angleRawStep2 + " rawAngle2_Min=" + rawAngle2_Min + " rawAngle2_Max=" + rawAngle2_Max + " angleDegreesStep2=" + angleDegreesStep2 + "\r\nsweep angles2 (us) : ");
for (int count = 0; count < rawAngle2Values.Count; count++)
{
// Display bytes as 2-character Hex strings.
sBuf.AppendFormat("{0} ", rawAngle2Values.ElementAt(count).Key);
}
Tracer.Trace(sBuf.ToString());
}
}
this.pmBearingLabel1.Text = String.Format("{0} us", angleRaw1);
this.pmRangeLabel1.Text = String.Format("{0} cm", distCm1);
pmSonarViewControl1.setReading(angleRaw1, 0.01d * distCm1, timestamp);
this.pmNraysLabel1.Text = String.Format("{0} rays", pmSonarViewControl1.NumNumbers);
this.pmBearingLabel2.Text = String.Format("{0} us", angleRaw2);
this.pmRangeLabel2.Text = String.Format("{0} cm", distCm2);
pmSonarViewControl2.setReading(angleRaw2, 0.01d * distCm2, timestamp);
this.pmNraysLabel2.Text = String.Format("{0} rays", pmSonarViewControl2.NumNumbers);
if (inTestSamples == 0)
{
this.pmBearingLabelComb.Text = String.Format("{0} us", angleRaw2);
this.pmRangeLabelComb.Text = String.Format("{0} cm", distCm2);
pmSonarViewControlComb.setReading(angleRawToSonarAngle(1, angleRaw1), 0.01d * distCm1, angleRawToSonarAngle(2, angleRaw2), 0.01d * distCm2, timestamp);
this.pmNraysLabelComb.Text = String.Format("{0} rays", pmSonarViewControlComb.NumNumbers);
if (angleRaw1 == rawAngle1_Min || angleRaw1 == rawAngle1_Max)
{
Tracer.Trace("Sweep Frame Ready");
SonarData p = pmSonarViewControlComb.sonarData;
foreach (int angle in p.angles.Keys)
{
RangeReading rr = p.getLatestReadingAt(angle);
double range = rr.rangeMeters * 1000.0d; // millimeters
//ranges.Add(range);
Tracer.Trace("&&&&&&&&&&&&&&&&&&&&&&&&&&&& angle=" + angle + " range=" + range);
/*
* typical measurement:
* for dual radar:
angles: 26
Sweep Frame Ready
angle=883 range=2052
angle=982 range=2047
angle=1081 range=394
angle=1179 range=394
angle=1278 range=398
angle=1377 range=390
angle=1475 range=390
angle=1574 range=390
angle=1673 range=399
angle=1771 range=416
angle=1870 range=1972
angle=1969 range=2182
angle=2067 range=3802
angle=2166 range=245
angle=2265 range=241
angle=2364 range=224
angle=2462 range=211
angle=2561 range=202
angle=2660 range=202
angle=2758 range=135
angle=2857 range=135
angle=2956 range=135
angle=3054 range=228
angle=3153 range=254
angle=3252 range=248
angle=3350 range=244
* for single radar:
* PACKET READY -- angles: 26 packets: 1
angle=150 range=1440
angle=190 range=1450
angle=230 range=1450
angle=270 range=1450
angle=310 range=1460
angle=350 range=1540
angle=390 range=1540
angle=430 range=1700
angle=470 range=1700
angle=510 range=1740
angle=550 range=2260
angle=590 range=1100
angle=630 range=1100
angle=670 range=1090
angle=710 range=1100
angle=750 range=1090
angle=790 range=1090
angle=830 range=1090
angle=870 range=1090
angle=910 range=1700
angle=950 range=1710
angle=990 range=1730
angle=1030 range=1720
angle=1070 range=1710
angle=1110 range=3500
angle=1150 range=3500
*/
}
}
}
}
// This will be called whenever the data is read from the board:
private void DataReadCompleteHandler(object sender, AsyncInputReportArgs aira)
{
Tracer.Trace("Async data arrived. " + DateTime.Now);
StringBuilder byteValue = new StringBuilder();
byteValue.Append("DataReadCompleteHandler(): Input Report Data: ");
for (int count = 0; count <= aira.InputBuffer.Length - 1; count++)
{
// Display bytes as 2-character Hex strings.
byteValue.AppendFormat("{0:X02} ", aira.InputBuffer[count]);
}
Tracer.Trace(byteValue.ToString());
int servo1target = intFromBuffer(aira.InputBuffer, 1);
int servo2target = intFromBuffer(aira.InputBuffer, 3);
int ping1value = intFromBuffer(aira.InputBuffer, 5);
int ping2value = intFromBuffer(aira.InputBuffer, 7);
AsyncInputFrameArgs args = new AsyncInputFrameArgs(servo1target, servo2target, ping1value, ping2value);
OnDataFrameComplete(args);
if (inDataContinuousMode)
{
// initiate next read:
ReadFromDevice(new EventHandler<AsyncInputReportArgs>(DataReadCompleteHandler));
}
}
// Invoke the HasReadFrame event; called whenever the Read completes:
protected virtual void OnDataFrameComplete(AsyncInputFrameArgs e)
{
if (HasReadFrame != null)
{
HasReadFrame(this, e);
}
}
