// 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;
}
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);
}
// =================================================================================================================
#region Processing Data Frame from LibPicSensors.ProximityModule - pmFrameCompleteHandler()
// This will be called whenever the data is read from the board:
private void pmFrameCompleteHandler(object sender, AsyncInputFrameArgs aira)
{
/*
* Tracer.Trace("Async frame arrived. " + now);
*
* StringBuilder frameValue = new StringBuilder();
*
* frameValue.AppendFormat("ProximityBoardManager: frame arrived: {0} ", aira.dPos1Mks);
* frameValue.AppendFormat("{0} ", aira.dPos2Mks);
* frameValue.AppendFormat("{0} ", aira.dPing1DistanceMm);
* frameValue.AppendFormat("{0}", aira.dPing2DistanceMm);
*
* Tracer.Trace(frameValue.ToString());
*/
bool haveData = false;
// IR Proximity sensors data:
ProximityData proxData = new ProximityData()
{
TimeStamp = aira.timestamp
};
proxData.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);
lock (_proxiDatas)
{
_proxiDatas.Enqueue(proxData);
}
haveData = true; // allow DataPort.Post(new Recv()) to happen, with data already enqueued in the ...Datas
// accelerometer data:
AccelerometerData accData = new AccelerometerData()
{
TimeStamp = aira.timestamp
};
accData.setAccelerometerData(aira.sensorsState.accelX, aira.sensorsState.accelY, aira.sensorsState.accelZ);
lock (_accelDatas)
{
_accelDatas.Enqueue(accData);
}
haveData = true; // allow DataPort.Post(new Recv()) to happen, with data already enqueued in the ...Datas
// compass data:
DirectionData dirData = null;
if (aira.sensorsState.compassHeading >= 0.0d && aira.sensorsState.compassHeading <= 359.999999d)
{
dirData = new DirectionData()
{
heading = aira.sensorsState.compassHeading, TimeStamp = aira.timestamp
};
lock (_directionDatas)
{
_directionDatas.Enqueue(dirData);
}
haveData = true; // allow DataPort.Post(new Recv()) to happen, with data already enqueued in the ...Datas
}
ParkingSensorData psiData = null;
if (aira.sensorsState.parkingSensorsValid)
{
psiData = new ParkingSensorData()
{
TimeStamp = aira.timestamp
};
psiData.setParkingSensorData(aira.sensorsState);
lock (_parkingSensorsDatas)
{
_parkingSensorsDatas.Enqueue(psiData);
}
haveData = true; // allow DataPort.Post(new Recv()) to happen, with data already enqueued in the ...Datas
}
// frames that are marked "fromPingScanStop" feed the sonar sweep view controls:
if (aira.fromPingScanStop)
{
int angleRaw1 = (int)aira.dPos1Mks;
double distM1 = aira.dPing1DistanceM;
//Tracer.Trace(String.Format("angleRaw1 = {0} us", angleRaw1));
//Tracer.Trace(String.Format("distMm1 = {0} mm", distMm1));
int angleRaw2 = (int)aira.dPos2Mks;
double distM2 = aira.dPing2DistanceM;
//Tracer.Trace(String.Format("angleRaw2 = {0} us", angleRaw2));
//Tracer.Trace(String.Format("distMm2 = {0} mm", distMm2));
haveData = haveData && setSonarRayReading(angleRaw1, distM1, angleRaw2, distM2, aira.timestamp);
}
if (haveData)
{
AnalogValue1 = aira.sensorsState.analogValue1;
// just post internally an empty signal message, the receiver - DataHandler(Recv recv) should examine the ...Datas and forward data to Responses port
DataReadyPort.Post(new Recv());
}
}
