private void OnMeasurement(Packet p, DateTime TimeStamp)
{
byte[] data = p.Data;
LinkMeasurement lsd = new LinkMeasurement();
lsd.TimeStamp = TimeStamp;
ushort lengthAndFlags = Packet.MakeUshort(data[1], data[2]);
int length = lengthAndFlags & 0x3FF;
switch (lengthAndFlags >> 14)
{
case 0:
lsd.Units = Units.Centimeters;
break;
case 1:
lsd.Units = Units.Millimeters;
break;
default:
return;
}
lsd.Ranges = new int[length];
int offset = 3;
for (int i = 0; i < length; i++, offset += 2)
{
ushort range = Packet.MakeUshort(data[offset], data[offset + 1]);
if (range > 0x1FF7)
{
range = 0x2000;
}
lsd.Ranges[i] = range;
}
if (offset < p.Length - 1)
{
lsd.ScanIndex = data[offset++];
}
else
{
lsd.ScanIndex = -1;
}
if (offset < p.Length - 1)
{
lsd.TelegramIndex = data[offset++];
}
else
{
lsd.TelegramIndex = -1;
}
_internalPort.Post(lsd);
}
/// <summary>
/// Handle new measurement data from the LRF.
/// </summary>
/// <param name="measurement">Measurement Data</param>
void MeasurementHandler(LinkMeasurement measurement)
{
try
{
//
// The SickLRF typically reports on either a 180 degrees or 100 degrees
// field of vision. From the number of readings we can calculate the
// Angular Range and Resolution.
//
switch (measurement.Ranges.Length)
{
case 181:
_state.AngularRange = 180;
_state.AngularResolution = 1;
break;
case 361:
_state.AngularRange = 180;
_state.AngularResolution = 0.5;
break;
case 101:
_state.AngularRange = 100;
_state.AngularResolution = 1;
break;
case 201:
_state.AngularRange = 100;
_state.AngularResolution = 0.5;
break;
case 401:
_state.AngularRange = 100;
_state.AngularResolution = 0.25;
break;
default:
break;
}
_state.DistanceMeasurements = measurement.Ranges;
_state.Units = measurement.Units;
_state.TimeStamp = measurement.TimeStamp;
_state.LinkState = "Measurement received";
//
// Inform subscribed services that the state has changed.
//
_subMgrPort.Post(new submgr.Submit(_state, DsspActions.ReplaceRequest));
}
catch (Exception e)
{
LogError(e);
}
}
