public DirectionDataDssSerializable(DirectionData directionData)
{
TimeStamp = new DateTime(directionData.TimeStamp);
heading = directionData.heading;
bearing = directionData.bearing;
}
/// <summary>
/// Handle CH Robotics UM6 Orientation Sensor Notification - Euler
/// </summary>
/// <param name="notification">Euler notification</param>
private void ChrEulerHandler(chrum6orientationsensor.EulerNotification notification)
{
Tracer.Trace(string.Format("the UM6 Sensor reported Euler: {0} PHI={1} THETA={2} PSI={3}", notification.Body.LastUpdate, notification.Body.phi, notification.Body.theta, notification.Body.psi));
try
{
// mag heading is reported as a ahort within +-16200 range
double magHeading = Direction.to180(notification.Body.psi * CHR_EILER_YAW_FACTOR); // convert to degrees and ensure that it is within +- 180 degrees.
DirectionData newDir = new DirectionData() { TimeStamp = DateTime.Now.Ticks, heading = magHeading };
DirectionData curDir = MostRecentDirection;
if (curDir == null || Math.Abs(newDir.heading - curDir.heading) > 0.9d) // only react on significant changes in direction
{
MostRecentDirection = newDir; //.Clone()
if (_mapperVicinity.robotDirection.bearing.HasValue)
{
_currentGoalBearing = (double)_mapperVicinity.robotDirection.bearing;
}
if (_mapperVicinity.turnState != null && !_mapperVicinity.turnState.hasFinished)
{
_mapperVicinity.turnState.directionCurrent = new Direction() { heading = newDir.heading, TimeStamp = newDir.TimeStamp };
}
// update mapper with Direction data:
_mapperVicinity.robotDirection = new Direction() { TimeStamp = newDir.TimeStamp, heading = newDir.heading, bearing = _currentGoalBearing };
// update mapper with Odometry data:
//updateMapperWithOdometryData();
// update GUI (compass control):
setGuiCurrentDirection(newDir);
//if (!_doUnitTest)
//{
// Decide(SensorEventSource.Compass, null);
//}
}
}
catch (Exception exc)
{
Tracer.Trace("ChrEulerHandler() - " + exc);
}
}
protected void setGuiCurrentDirection(DirectionData dir)
{
if (_mainWindow != null)
{
ccrwpf.Invoke invoke = new ccrwpf.Invoke(delegate()
{
_mainWindow.CurrentDirection = new DirectionData() { TimeStamp = dir.TimeStamp, heading = dir.heading, bearing = _currentGoalBearing };
}
);
_wpfServicePort.Post(invoke);
Arbiter.Activate(TaskQueue,
invoke.ResponsePort.Choice(
s => { }, // delegate for success
ex => { } //Tracer.Error(ex) // delegate for failure
));
}
}
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 DirectionMeasurementHandler(DirectionData measurement)
{
//Tracer.Trace("TrackRoamerBrickProximityBoardService::DirectionMeasurementHandler()");
try
{
_state.LastSampleTimestamp = new DateTime(measurement.TimeStamp);
_state.MostRecentDirection = new DirectionDataDssSerializable(measurement);
_state.LinkState = "receiving Direction Data";
//
// Inform subscribed services that the state has changed.
//
_submgrPort.Post(new submgr.Submit(_state, DsspActions.ReplaceRequest));
UpdateDirectionData usd = new UpdateDirectionData();
usd.Body = _state.MostRecentDirection;
base.SendNotification<UpdateDirectionData>(_submgrPort, usd);
}
catch (Exception e)
{
_state.LinkState = "Error while receiving Direction Data";
LogError(e);
}
}