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