void trpbUpdateSonarNotification(proxibrick.UpdateSonarData update)
{
try
{
proxibrick.SonarDataDssSerializable sweep = update.Body;
state.MostRecentSonar = sweep;
state.SonarTimeStamp = sweep.TimeStamp;
state.SonarLinkState = "Sonar Measurement received";
//
// Inform subscribed services that the state has changed.
//
//submgrPort.Post(new submgr.Submit(state, DsspActions.ReplaceRequest));
}
catch (Exception exc)
{
Tracer.Trace("trpbUpdateSonarNotification() - " + exc);
}
}
/// <summary>
/// convert sonar sweep into laser-like 180 degrees data
/// </summary>
/// <param name="update"></param>
void trpbUpdateSonarNotification(proxibrick.UpdateSonarData update)
{
//LogInfo("TrackRoamerUsrfService: trpbUpdateNotification()");
//Tracer.Trace("TrackRoamerUsrfService: trpbUpdateNotification()");
try
{
proxibrick.SonarDataDssSerializable p = update.Body;
int packetLength = p.RangeMeters.Length; // must be 26 packets, each covering 7 degrees;
if (packetLength != 26)
{
Tracer.Error("TrackRoamerUsrfService::trpbUpdateNotification() not a standard measurement, angles.Count=" + packetLength + " (expected 26) -- ignored");
return;
}
int[] intRanges = new int[packetLength];
for (int i = 0; i < packetLength; i++)
{
// range = (ushort)(i * 40);
ushort range = (ushort)Math.Round(p.RangeMeters[i] * 1000.0d);
if (range > 0x1FF7)
{
range = 0x2000; // limit to 8192 mm; actual range about 4 meters
}
intRanges[i] = (int)range;
}
if (doWeeding)
{
if (intRanges[0] < (intRanges[1] + intRanges[2]) / 4)
{
intRanges[0] = (intRanges[1] + intRanges[2]) / 2;
}
if (intRanges[packetLength - 1] < (intRanges[packetLength - 2] + intRanges[packetLength - 3]) / 4)
{
intRanges[packetLength - 1] = (intRanges[packetLength - 2] + intRanges[packetLength - 3]) / 2;
}
for (int i = 1; i < packetLength - 1; i++)
{
if (intRanges[i] < (intRanges[i - 1] + intRanges[i + 1]) * 3 / 8)
{
intRanges[i] = (intRanges[i - 1] + intRanges[i + 1]) / 2;
}
}
}
int angularRange = 180;
int angularResolution = 1;
int mesLength = angularRange + 1; // 181
int[] lsdRanges = new int[mesLength]; // millimeters, with 1 degree resolution
int step = (int)Math.Round((double)mesLength / (double)packetLength); // typically round(6.96) = 7
// if we smooth the measurements, Decide() has better chance of sorting the values right. It does not like 7 degrees steps.
// we need these for exponential moving average:
double emaPeriod = 4.0d;
double emaMultiplier = 2.0d / (1.0d + emaPeriod);
double?emaPrev = null;
int iRange = 0;
for (int i = 0; i < mesLength; i++) // 0...181
{
int angleIndex = Math.Min(i / step, packetLength - 1);
iRange = intRanges[angleIndex];
if (doAveraging)
{
// calculate exponential moving average - smooth the curve a bit:
double?ema = !emaPrev.HasGoodValue() ? iRange : ((iRange - emaPrev) * emaMultiplier + emaPrev);
emaPrev = ema;
iRange = (int)Math.Round((double)ema);
}
//Tracer.Trace("&&&&&&&&&&&&&&&&&&&&&&&&&&&& i=" + i + " range=" + range + " ema=" + iRange);
lsdRanges[i] = iRange; // 5000; // milimeters
}
_state.AngularRange = angularRange;
_state.AngularResolution = angularResolution;
_state.DistanceMeasurements = lsdRanges;
_state.Units = sicklrf.Units.Millimeters;
_state.TimeStamp = p.TimeStamp;
_state.LinkState = "Measurement received";
//
// Inform subscribed services that the state has changed.
//
_submgrPort.Post(new submgr.Submit(_state, DsspActions.ReplaceRequest));
}
catch (Exception exc)
{
LogError(exc);
}
}
