示例#1
0
        /// <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);
            }
        }
示例#2
0
        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);
             */
        }