// Invoke the HasReadFrame event; called whenever the Read completes:
protected virtual void OnDataFrameComplete(AsyncInputFrameArgs e)
{
if (HasReadFrameEvent != null)
{
HasReadFrameEvent(this, e);
}
}
// This will be called whenever the data is read from the board:
private void DataReadCompleteHandler(object sender, AsyncInputReportArgs aira)
{
// do some quick sanity checking here to avoid exceptions:
uint parkingSensorsCount = (uint)aira.InputBuffer[23];
switch (parkingSensorsCount)
{
case 0:
case 32:
case 64:
break;
default:
return;
}
switch (aira.InputBuffer[9])
{
case 0:
case 1:
break;
default:
return;
}
/*
* Tracer.Trace("PM Command: 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);
SensorsState sensState = new SensorsState();
bool fromPingScanStop = aira.InputBuffer[9] > 0;
// infrared distance sensors:
sensState.irbE1 = aira.InputBuffer[10];
sensState.irbE2 = aira.InputBuffer[11];
sensState.irbE3 = aira.InputBuffer[12];
sensState.irbE4 = aira.InputBuffer[13];
sensState.irbO1 = aira.InputBuffer[14];
sensState.irbO2 = aira.InputBuffer[15];
sensState.irbO3 = aira.InputBuffer[16];
sensState.irbO4 = aira.InputBuffer[17];
sensState.compassHeading = (((uint)aira.InputBuffer[18] << 8) + (uint)aira.InputBuffer[19]) / 10.0d;
sensState.accelX = ProximityBoard.toAccel(aira.InputBuffer[20]);
sensState.accelY = ProximityBoard.toAccel(aira.InputBuffer[21]);
sensState.accelZ = ProximityBoard.toAccel(aira.InputBuffer[22]);
// ultrasound car parking sensors - bytes 23 to 31 (only first 4 bytes used, next 4 are reserved for 8-sensor device):
sensState.parkingSensorsCount = parkingSensorsCount; // 32 or 0 for invalid
for (int i = 0; i < parkingSensorsCount / 8; i++)
{
sensState.parkingSensors[i] = aira.InputBuffer[24 + i];
}
sensState.mapParkingSensorsData();
sensState.mapAnalogData(0, aira.InputBuffer[28], aira.InputBuffer[29]); // LSB, MSB for AN0 (pin 2 on PIC 4550)
// calibration for POT data (pin 2 RA0/AN0 on PIC4550):
// 0v = 0
// 1v = 220
// 2v = 415
// 3v = 630
// 4v = 835
// 4.88v = 1023
AsyncInputFrameArgs args = new AsyncInputFrameArgs(servo1target, servo2target, ping1value, ping2value, fromPingScanStop, sensState);
OnDataFrameComplete(args);
if (inDataContinuousMode)
{
// initiate next read:
ReadFromDevice(new EventHandler <AsyncInputReportArgs>(DataReadCompleteHandler), INPUT_CONT_REPORT_ID);
}
}
// This will be called whenever the data is read from the board:
private void DataReadCompleteHandler(object sender, AsyncInputReportArgs aira)
{
// do some quick sanity checking here to avoid exceptions:
uint parkingSensorsCount = (uint)aira.InputBuffer[23];
switch (parkingSensorsCount)
{
case 0:
case 32:
case 64:
break;
default:
return;
}
switch (aira.InputBuffer[9])
{
case 0:
case 1:
break;
default:
return;
}
/*
Tracer.Trace("PM Command: 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);
SensorsState sensState = new SensorsState();
bool fromPingScanStop = aira.InputBuffer[9] > 0;
// infrared distance sensors:
sensState.irbE1 = aira.InputBuffer[10];
sensState.irbE2 = aira.InputBuffer[11];
sensState.irbE3 = aira.InputBuffer[12];
sensState.irbE4 = aira.InputBuffer[13];
sensState.irbO1 = aira.InputBuffer[14];
sensState.irbO2 = aira.InputBuffer[15];
sensState.irbO3 = aira.InputBuffer[16];
sensState.irbO4 = aira.InputBuffer[17];
sensState.compassHeading = (((uint)aira.InputBuffer[18] << 8) + (uint)aira.InputBuffer[19]) / 10.0d;
sensState.accelX = ProximityBoard.toAccel(aira.InputBuffer[20]);
sensState.accelY = ProximityBoard.toAccel(aira.InputBuffer[21]);
sensState.accelZ = ProximityBoard.toAccel(aira.InputBuffer[22]);
// ultrasound car parking sensors - bytes 23 to 31 (only first 4 bytes used, next 4 are reserved for 8-sensor device):
sensState.parkingSensorsCount = parkingSensorsCount; // 32 or 0 for invalid
for (int i = 0; i < parkingSensorsCount / 8; i++)
{
sensState.parkingSensors[i] = aira.InputBuffer[24 + i];
}
sensState.mapParkingSensorsData();
sensState.mapPotValueData(aira.InputBuffer[28], aira.InputBuffer[29]); // LSB, MSB
// calibration for POT data (pin 2 RA0/AN0 on PIC4550):
// 0v = 0
// 1v = 220
// 2v = 415
// 3v = 630
// 4v = 835
// 4.88v = 1023
AsyncInputFrameArgs args = new AsyncInputFrameArgs(servo1target, servo2target, ping1value, ping2value, fromPingScanStop, sensState);
OnDataFrameComplete(args);
if (inDataContinuousMode)
{
// initiate next read:
ReadFromDevice(new EventHandler<AsyncInputReportArgs>(DataReadCompleteHandler), INPUT_CONT_REPORT_ID);
}
}
// Invoke the HasReadFrame event; called whenever the Read completes:
protected virtual void OnDataFrameComplete(AsyncInputFrameArgs e)
{
if (HasReadFrameEvent != null)
{
HasReadFrameEvent(this, e);
}
}
private void pmFrameCompleteHandler_UI(object sender, AsyncInputFrameArgs aira)
{
long timestamp = aira.timestamp;
// have raw data displayed in a text box:
StringBuilder frameValue = new StringBuilder();
frameValue.AppendFormat("{0} POT: {1}\r\n", aira.dPos1Mks, aira.sensorsState.potValue);
frameValue.AppendFormat("{0}\r\n", aira.dPos2Mks);
frameValue.AppendFormat("{0} ", aira.dPing1DistanceM * 1000.0d); // print distance in milimeters
frameValue.AppendFormat("{0}\r\n", aira.fromPingScanStop);
frameValue.AppendFormat("{0}\r\n", aira.dPing2DistanceM * 1000.0d);
// expect 7 on "not in sight", 3 on "far", 1 on "medium" and 0 on "close":
frameValue.AppendFormat("{0} ", aira.sensorsState.irbE1);
frameValue.AppendFormat("{0} ", aira.sensorsState.irbE2);
frameValue.AppendFormat("{0} ", aira.sensorsState.irbE3);
frameValue.AppendFormat("{0} ", aira.sensorsState.irbE4);
frameValue.AppendFormat("{0} ", aira.sensorsState.irbO1);
frameValue.AppendFormat("{0} ", aira.sensorsState.irbO2);
frameValue.AppendFormat("{0} ", aira.sensorsState.irbO3);
frameValue.AppendFormat("{0}\r\n", aira.sensorsState.irbO4);
frameValue.AppendFormat("{0} : {1:0.0} * {2:0.0} * {3:0.0} * {4:0.0} \r\n", aira.sensorsState.parkingSensorsCount,
aira.sensorsState.parkingSensorMetersLB, aira.sensorsState.parkingSensorMetersLF, aira.sensorsState.parkingSensorMetersRF, aira.sensorsState.parkingSensorMetersRB);
/*
Tracer.Trace(string.Format("=== xpsi: {0} : {1} {2} {3} {4} : {5} {6} {7} {8}", aira.sensorsState.parkingSensorsCount,
aira.sensorsState.parkingSensors[0], aira.sensorsState.parkingSensors[1], aira.sensorsState.parkingSensors[2], aira.sensorsState.parkingSensors[3],
aira.sensorsState.parkingSensors[4], aira.sensorsState.parkingSensors[5], aira.sensorsState.parkingSensors[6], aira.sensorsState.parkingSensors[7]));
Tracer.Trace(string.Format("=== bpsi: {0} : {1} {2} {3} {4} : {5} {6} {7} {8}", aira.sensorsState.parkingSensorsCount,
Convert.ToString(aira.sensorsState.parkingSensors[0],2), Convert.ToString(aira.sensorsState.parkingSensors[1],2), Convert.ToString(aira.sensorsState.parkingSensors[2],2), Convert.ToString(aira.sensorsState.parkingSensors[3],2),
Convert.ToString(aira.sensorsState.parkingSensors[4],2), Convert.ToString(aira.sensorsState.parkingSensors[5],2), Convert.ToString(aira.sensorsState.parkingSensors[6],2), Convert.ToString(aira.sensorsState.parkingSensors[7],2)));
*/
//frameValue.AppendFormat("{0} {1} {2} {3}\r\n", aira.sensorsState.parkingSensors[4], aira.sensorsState.parkingSensors[5], aira.sensorsState.parkingSensors[6], aira.sensorsState.parkingSensors[7]);
pmValuesLabel.Content = frameValue.ToString();
// Proximity sensors display their data in dedicated control:
lastProxData = new ProximityData() { TimeStamp = timestamp };
lastProxData.setProximityData(aira.sensorsState.irbE1, aira.sensorsState.irbE2, aira.sensorsState.irbE3, aira.sensorsState.irbE4,
aira.sensorsState.irbO1, aira.sensorsState.irbO2, aira.sensorsState.irbO3, aira.sensorsState.irbO4);
lastProxDataChanged = true;
// accelerometer has its own control for displaying data:
lastAccData = new AccelerometerDataWpf() { TimeStamp = timestamp };
lastAccData.setAccelerometerData(aira.sensorsState.accelX, aira.sensorsState.accelY, aira.sensorsState.accelZ);
lastAccDataChanged = true;
// compass has its own control for displaying data:
if (aira.sensorsState.compassHeading >= 0.0d && aira.sensorsState.compassHeading <= 359.999999d && (lastDirData == null || Math.Abs(lastDirData.heading - aira.sensorsState.compassHeading) > 2.0d))
{
lastDirData = new DirectionData() { heading = aira.sensorsState.compassHeading, TimeStamp = timestamp };
lastDirDataChanged = true;
}
else if ((bool)compassCheckBox.IsChecked && (lastCompassSlider != compassSlider.Value || timestamp > lastCompassSliderTimestamp + 20000000L))
{
// simulated direction, controlled by a slider:
lastCompassSliderTimestamp = timestamp;
lastCompassSlider = compassSlider.Value;
double hdg = compassSlider.Value; // -180 ... 180
if (hdg < 0)
{
hdg += 360.0d; // 0...360
}
lastDirData = new DirectionData() { heading = hdg, TimeStamp = timestamp };
lastDirDataChanged = true;
}
if (aira.sensorsState.parkingSensorsValid)
{
lastPsiData = new ParkingSensorData()
{
parkingSensorMetersLB = aira.sensorsState.parkingSensorMetersLB,
parkingSensorMetersLF = aira.sensorsState.parkingSensorMetersLF,
parkingSensorMetersRB = aira.sensorsState.parkingSensorMetersRB,
parkingSensorMetersRF = aira.sensorsState.parkingSensorMetersRF,
TimeStamp = timestamp
};
lastPsiDataChanged = true;
}
// frames that are marked "" feed the sonar sweep view controls. They are updated in real time:
if (aira.fromPingScanStop)
{
int angleRaw1 = (int)aira.dPos1Mks;
double distM1 = aira.dPing1DistanceM;
int angleRaw2 = (int)aira.dPos2Mks;
double distM2 = aira.dPing2DistanceM;
if (inTestSamples > 0)
{
// prerun - for a while just try figuring out what comes in - sweep angle ranges for both sides
inTestSamples--;
if (inTestSamples < TEST_SAMPLES_COUNT - 10) // first few frames are garbled
{
board.registerAnglesRaw(angleRaw1, angleRaw2);
if (inTestSamples == 0) // last count
{
board.finishPrerun();
Tracer.Trace(board.ToString());
}
}
}
else
{
// real sonar data is coming, and we finished prerun and know how many beams per sweep we are dealing with
lastRangeReadingSet = new RangeReadingSet() { left = board.angleRawToSonarAngle(new RangeReading(angleRaw1, distM1, timestamp)), right = board.angleRawToSonarAngle(new RangeReading(angleRaw2, distM2, timestamp)) };
lastRangeReadingSetChanged = true;
lastRangeReadingSet.combo1 = board.angleRawToSonarAngleCombo(1, angleRaw1, distM1, timestamp);
lastRangeReadingSet.combo2 = board.angleRawToSonarAngleCombo(2, angleRaw2, distM2, timestamp);
lastRangeReadingSet.hasCombo = true;
if (angleRaw1 == board.rawAngle1_Min || angleRaw1 == board.rawAngle1_Max)
{
lastRangeReadingSet.sweepFrameReady = true;
}
}
}
// sanity check:
if (lastRangeReadingSetChanged && lastRangeReadingSet.sweepFrameReady && lastDirData == null)
{
// we probably don't have compass connected - have a fake dir data for now.
lastDirData = new DirectionData() { heading = 0.0d, TimeStamp = timestamp };
lastDirDataChanged = true;
}
// update all controls with collected data:
setSonarViewControls(timestamp);
updateMapperVicinity(timestamp, sweepViewControlCombo.sonarData);
lastRangeReadingSetChanged = false;
setViewControls(timestamp);
}
// This will be called whenever the data is read from the board; it will work in a different thread:
private void pmFrameCompleteHandler(object sender, AsyncInputFrameArgs aira)
{
Tracer.Trace("Async sonar frame arrived. " + DateTime.Now);
if (this.Dispatcher.CheckAccess())
{
// do work on UI thread
pmFrameCompleteHandler_UI(sender, aira);
}
else
{
// we normally get here - the async read from HID is happening on a different thread:
this.Dispatcher.Invoke(new Action<object, AsyncInputFrameArgs>(pmFrameCompleteHandler_UI), sender, aira); // or BeginInvoke()
}
}
private void pmFrameCompleteHandler_UI(object sender, AsyncInputFrameArgs aira)
{
long timestamp = aira.timestamp;
}
