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