public ParkingSensorDataDssSerializable(ParkingSensorData parkingSensorData)
{
TimeStamp = new DateTime(parkingSensorData.TimeStamp);
parkingSensorMetersLF = parkingSensorData.parkingSensorMetersLF;
parkingSensorMetersRF = parkingSensorData.parkingSensorMetersRF;
parkingSensorMetersLB = parkingSensorData.parkingSensorMetersLB;
parkingSensorMetersRB = parkingSensorData.parkingSensorMetersRB;
arrangedForDrawing[0] = parkingSensorMetersRB;
arrangedForDrawing[1] = parkingSensorMetersLB;
arrangedForDrawing[2] = parkingSensorMetersLF;
arrangedForDrawing[3] = parkingSensorMetersRF;
}
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);
}
void ParkingSensorMeasurementHandler(ParkingSensorData measurement)
{
//Tracer.Trace("TrackRoamerBrickProximityBoardService::ParkingSensorMeasurementHandler()");
try
{
_state.LastSampleTimestamp = new DateTime(measurement.TimeStamp);
_state.MostRecentParkingSensor = new ParkingSensorDataDssSerializable(measurement);
_state.LinkState = "receiving Parking Sensor Data";
//
// Inform subscribed services that the state has changed.
//
_submgrPort.Post(new submgr.Submit(_state, DsspActions.ReplaceRequest));
UpdateParkingSensorData usd = new UpdateParkingSensorData();
usd.Body = _state.MostRecentParkingSensor;
base.SendNotification<UpdateParkingSensorData>(_submgrPort, usd);
}
catch (Exception e)
{
_state.LinkState = "Error while receiving Parking Sensor Data";
LogError(e);
}
}
