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); } }