/// <summary> /// Handle new measurement data from the LRF. /// </summary> /// <param name="measurement">Measurement Data</param> void MeasurementHandler(LinkMeasurement measurement) { //Tracer.Trace("SickLRF::MeasurementHandler()"); 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: // we always get here: _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); } }
private void OnMeasurement(Packet p, DateTime TimeStamp) { //Tracer.Trace("CommLink::OnMeasurement()"); int packetLength = p.angles.Count; // typically 26 packets, each covering 7 degrees if (packetLength != 26) { Tracer.Error("CommLink::OnMeasurement() not a standard measurement, angles.Count=" + packetLength + " (expected 26) -- ignored"); return; } LinkMeasurement lsd = new LinkMeasurement(); lsd.TimeStamp = TimeStamp; lsd.Units = Units.Millimeters; int angularRange = 180; int mesLength = angularRange + 1; // 181 lsd.Ranges = new int[mesLength]; // millimeters, with 1 degree resolution List <double> ranges = new List <double>(); foreach (int angle in p.angles.Keys) { RangeReading rr = p.getLatestReadingAt(angle); double range = rr.rangeMeters * 1000.0d; // millimeters ranges.Add(range); //Tracer.Trace("&&&&&&&&&&&&&&&&&&&&&&&&&&&& angle=" + angle + " range=" + range); /* * typical measurement: * PACKET READY -- angles: 26 packets: 1 * angle=150 range=1440 * angle=190 range=1450 * angle=230 range=1450 * angle=270 range=1450 * angle=310 range=1460 * angle=350 range=1540 * angle=390 range=1540 * angle=430 range=1700 * angle=470 range=1700 * angle=510 range=1740 * angle=550 range=2260 * angle=590 range=1100 * angle=630 range=1100 * angle=670 range=1090 * angle=710 range=1100 * angle=750 range=1090 * angle=790 range=1090 * angle=830 range=1090 * angle=870 range=1090 * angle=910 range=1700 * angle=950 range=1710 * angle=990 range=1730 * angle=1030 range=1720 * angle=1070 range=1710 * angle=1110 range=3500 * angle=1150 range=3500 */ } 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; for (int i = 0; i < mesLength; i++) { int angleIndex = i / step; //ushort range = (ushort)(i * 40); ushort range = (ushort)Math.Round(ranges[angleIndex]); if (range > 0x1FF7) { range = 0x2000; } // calculate exponential moving average - smooth the curve a bit: double?ema = !emaPrev.HasGoodValue() ? range : ((range - emaPrev) * emaMultiplier + emaPrev); emaPrev = ema; int iRange = (int)Math.Round((double)ema); //Tracer.Trace("&&&&&&&&&&&&&&&&&&&&&&&&&&&& i=" + i + " range=" + range + " ema=" + iRange); //lsd.Ranges[i] = range; // 5000; // milimeters lsd.Ranges[i] = iRange; // 5000; // milimeters } lsd.ScanIndex = -1; lsd.TelegramIndex = -1; _internalPort.Post(lsd); /* * 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); */ }