Exemplo n.º 1
0
        public RWLT_Core(SerialPortSettings rwltPortSettings, double radialErrorThreshold, double simplexSize,
                         int courseEstimatorFIFOSize, int trkFilterFIFOSize)
        {
            #region parameters

            var basesIDs = Enum.GetValues(typeof(BaseIDs));
            foreach (BaseIDs baseID in basesIDs)
            {
                if (baseID != BaseIDs.BASE_INVALID)
                {
                    BaseBatVoltages.Add(baseID, new AgingValue <double>(4, 10, svoltageFormatter));
                    BaseMSRs.Add(baseID, new AgingValue <double>(4, 10, msrFormatter));
                }
            }

            HDOPState = new AgingValue <DOPState>(4, 10, x => x.ToString().ToUpperInvariant());
            TBAState  = new AgingValue <TBAQuality>(4, 10, x => x.ToString().Replace('_', ' ').ToUpperInvariant());

            TargetLatitude            = new AgingValue <double>(4, 10, latlonFormatter);
            TargetLongitude           = new AgingValue <double>(4, 10, latlonFormatter);
            TargetLocationRadialError = new AgingValue <double>(4, 10, rerrFormatter);
            TargetPressure            = new AgingValue <double>(6, 10, prsFormatter);
            TargetTemperature         = new AgingValue <double>(60, 120, tempFormatter);
            TargetBatVoltage          = new AgingValue <double>(30, 120, svoltageFormatter);
            TargetDepth = new AgingValue <double>(6, 10, dptdstFormatter);
            TargetAlarm = new AgingValue <PingerCodeIDs>(600, 6000, pAlmFormatter);

            DistanceToTarget       = new AgingValue <double>(4, 10, dptdstFormatter);
            ForwardAzimuthToTarget = new AgingValue <double>(4, 10, courseFormatter);
            ReverseAzimuthToTarget = new AgingValue <double>(4, 10, courseFormatter);

            TargetCourse = new AgingValue <double>(4, 10, courseFormatter);

            AUXLatitude  = new AgingValue <double>(4, 10, latlonFormatter);
            AUXLongitude = new AgingValue <double>(4, 10, latlonFormatter);
            AUXTrack     = new AgingValue <double>(4, 10, courseFormatter);
            AUXSpeed     = new AgingValue <double>(4, 10, speedFormatter);

            CEP   = new AgingValue <double>(4, 300, x => string.Format(CultureInfo.InvariantCulture, "{0:F03} m", x));
            DRMS  = new AgingValue <double>(4, 300, x => string.Format(CultureInfo.InvariantCulture, "{0:F03} m", x));
            DRMS2 = new AgingValue <double>(4, 300, x => string.Format(CultureInfo.InvariantCulture, "{0:F03} m", x));
            DRMS3 = new AgingValue <double>(4, 300, x => string.Format(CultureInfo.InvariantCulture, "{0:F03} m", x));

            #endregion

            #region trkFilter

            trkFilter = new TrackFilter(trkFilterFIFOSize);

            #endregion

            #region pCore

            pCore = new PCore2D <GeoPoint3DT>(radialErrorThreshold, simplexSize, Algorithms.WGS84Ellipsoid, courseEstimatorFIFOSize);
            pCore.RadialErrorExeedsThrehsoldEventHandler += new EventHandler(pCore_RadialErrorExeedsThresholdEventHandler);
            pCore.TargetLocationUpdatedExHandler         += new EventHandler <TargetLocationUpdatedExEventArgs>(pCore_TargetLocationUpdatedExEventHandler);
            pCore.BaseQualityUpdatedHandler += new EventHandler <BaseQualityUpdatedEventArgs>(pCore_BaseQualityUpdatedEventHandler);

            #endregion

            #region basesProcessor

            baseProcessor = new RWLT_BaseProcessor(4, 2.0);

            #endregion

            #region NMEA

            if (!nmeaSingleton)
            {
                NMEAParser.AddManufacturerToProprietarySentencesBase(ManufacturerCodes.RWL);
                NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.RWL, "A", "x,x.x,x.x,x.x,x.x,x,x.x,x.x,x.x");
            }

            #endregion

            #region inPort

            inPort = new NMEASerialPort(rwltPortSettings);
            inPort.NewNMEAMessage += (o, e) =>
            {
                LogEvent.Rise(o, new LogEventArgs(LogLineType.INFO, string.Format("{0} (IN) >> {1}", inPort.PortName, e.Message)));
                NMEAListener.ProcessIncoming(0, e.Message);
            };

            inPort.PortError += (o, e) => LogEvent.Rise(o, new LogEventArgs(LogLineType.ERROR, string.Format("{0} (IN) >> {1}", inPort.PortName, e.EventType.ToString())));

            #endregion

            #region NMEAListener

            NMEAListener = new NMEAMultipleListener();
            NMEAListener.NMEAProprietaryUnsupportedSentenceParsed += new EventHandler <NMEAUnsupportedProprietaryEventArgs>(NMEAPSentenceReceived);
            NMEAListener.RMCSentenceReceived += new EventHandler <RMCMessageEventArgs>(GNSS_RMCSentenceReceived);

            #endregion

            #region timer

            timer        = new PrecisionTimer();
            timer.Period = 100;
            timer.Tick  += (o, e) =>
            {
                if (++systemUpdateTS > systemUpdateLimit)
                {
                    systemUpdateTS = 0;
                    SystemUpdateEvent.Rise(this, new EventArgs());
                }

                if (inPort.IsOpen && (++inPortTimeoutTS > inPortTimeoutLimit))
                {
                    inPortTimeoutTS = 0;
                    InPortTimeout   = true;
                    LogEvent.Rise(this, new LogEventArgs(LogLineType.ERROR, string.Format("{0} (IN) >> TIMEOUT", inPort.PortName)));
                }

                if (AUXGNSSUsed && auxGNSSPort.IsOpen && (++AUXGNSSTimeoutTS > AUXGNSSTimeoutLimit))
                {
                    AUXGNSSTimeoutTS = 0;
                    AUXGNSSTimeout   = true;
                    LogEvent.Rise(this, new LogEventArgs(LogLineType.ERROR, string.Format("{0} (AUX) >> TIMEOUT", auxGNSSPort.PortName)));
                }
            };

            timer.Start();

            #endregion

            statHelper = new List <GeoPoint>();
        }
Exemplo n.º 2
0
        public APLLBLCore(Dictionary <string, SerialPortSettings> inPortSettings,
                          double radialErrorThreshold, double simplexSize, int courseEstimatorFifoSize, int trackFilterSize)
        {
            #region misc. initialization

            primaryGNSSFixRMC = new AgingValue <RMCMessageEventArgs>(2, 10,
                                                                     x => string.Format(CultureInfo.InvariantCulture, "LAT: {0:F06}°\r\nLON: {1:F06}°\r\nSPD: {2:F01} m/s (3:F01 km/h)\r\nTRK: {4:F01}°",
                                                                                        x.Latitude, x.Longitude, x.SpeedKmh / 3.6, x.SpeedKmh, x.TrackTrue));

            meanWaterTemperatureC = new AgingValue <double>(10, 30, x => string.Format(CultureInfo.InvariantCulture, "MTW: {0:F01} °C", x));

            targetFix = new AgingValue <GeoPoint3DETm>(3, 60,
                                                       x => string.Format(CultureInfo.InvariantCulture, "LAT: {0:F06}°\r\nLON: {1:F06}°\r\nRER: {2:F03} m",
                                                                          x.Latitude, x.Longitude, x.RadialError));

            targetFixFlt = new AgingValue <GeoPoint3DETm>(3, 60,
                                                          x => string.Format(CultureInfo.InvariantCulture, "LAT: {0:F06}°\r\nLON: {1:F06}°",
                                                                             x.Latitude, x.Longitude));

            targetCourse           = new AgingValue <double>(4, 8, x => string.Format(CultureInfo.InvariantCulture, "CRS: {0:F01}°", x));
            distanceToTarget       = new AgingValue <double>(4, 8, x => string.Format(CultureInfo.InvariantCulture, "D2T: {0:F01} m", x));
            forwardAzimuthToTarget = new AgingValue <double>(4, 8, x => string.Format(CultureInfo.InvariantCulture, "FAZ: {0:F01}°", x));
            reverseAzimuthToTarget = new AgingValue <double>(4, 8, x => string.Format(CultureInfo.InvariantCulture, "RAZ: {0:F01}°", x));

            BaseBatStates = new Dictionary <BaseIDs, AgingValue <bool> >();
            BaseBatStates.Add(BaseIDs.BASE_1, new AgingValue <bool>(30, 60, x => !x ? string.Format("{0} BAT LOW!", BaseIDs.BASE_1) : string.Empty));
            BaseBatStates.Add(BaseIDs.BASE_2, new AgingValue <bool>(30, 60, x => !x ? string.Format("{0} BAT LOW!", BaseIDs.BASE_1) : string.Empty));
            BaseBatStates.Add(BaseIDs.BASE_3, new AgingValue <bool>(30, 60, x => !x ? string.Format("{0} BAT LOW!", BaseIDs.BASE_1) : string.Empty));
            BaseBatStates.Add(BaseIDs.BASE_4, new AgingValue <bool>(30, 60, x => !x ? string.Format("{0} BAT LOW!", BaseIDs.BASE_1) : string.Empty));

            dopState   = new AgingValue <DOPState>(4, 10, x => x.ToString().Replace('_', ' ').ToUpperInvariant());
            tbaQuality = new AgingValue <TBAQuality>(4, 10, x => x.ToString().Replace('_', ' ').ToUpperInvariant());

            #endregion

            #region pCore

            trkFilter = new TrackFilter(trackFilterSize);

            pCore = new PCore2D <GeoPoint3DT>(radialErrorThreshold, simplexSize, Algorithms.WGS84Ellipsoid, courseEstimatorFifoSize);
            pCore.RadialErrorExeedsThrehsoldEventHandler += (o, e) => InfoEventHandler.Rise(this, new LogEventArgs(LogLineType.INFO, "WAYU Pinger location failed: radial error exeeds threshold"));
            pCore.TargetLocationUpdatedExHandler         += (o, e) =>
            {
                targetFix.Value    = new GeoPoint3DETm(e.Location.Latitude, e.Location.Longitude, e.Location.Depth, e.Location.RadialError, e.TimeStamp);
                targetCourse.Value = e.Course;

                var fltTrk = trkFilter.Filter(e.Location.Latitude, e.Location.Longitude);
                targetFixFlt.Value = new GeoPoint3DETm(fltTrk.Latitude, fltTrk.Longitude, e.Location.Depth, e.Location.RadialError, e.TimeStamp);

                if (primaryGNSSFixRMC.IsInitializedAndNotObsolete)
                {
                    UpdateTargetRelativeData();
                }

                InfoEventHandler.Rise(this, new LogEventArgs(LogLineType.INFO,
                                                             string.Format(CultureInfo.InvariantCulture, "WAYU Pinger located: {0}, Course: {1:F01}°",
                                                                           targetFixFlt, e.Course)));

                TrackUpdateHandler.Rise(this,
                                        new TrackUpdateEventArgs("WAYU (RAW)", targetFix.Value, e.Course));

                TrackUpdateHandler.Rise(this,
                                        new TrackUpdateEventArgs("WAYU (FLT)", targetFixFlt.Value, e.Course));

                if ((outPort != null) &&
                    (outPort.IsOpen))
                {
                    WriteOutData(targetFixFlt.Value.Latitude, targetFixFlt.Value.Longitude, 0.0, targetFix.Value.RadialError,
                                 targetCourse.Value, targetFix.Value.RadialError <= pCore.RadialErrorThreshold);
                }

                SystemUpdate();
            };

            pCore.BaseQualityUpdatedHandler += (o, e) =>
            {
                dopState.Value   = e.DopState;
                tbaQuality.Value = e.TBAState;
            };

            #endregion

            #region basesProcessor

            basesProcessor = new APLBaseProcessor(4, 2.0);

            #endregion

            #region NMEA

            if (!nmeaSingleton)
            {
                NMEAParser.AddManufacturerToProprietarySentencesBase(ManufacturerCodes.APL);
                NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.APL, "A", "x,x.x,x.x,x.x,x,x.x");
                nmeaSingleton = true;
            }

            #endregion

            #region Port dictionaries

            portDescrByHash = new Dictionary <int, string>();
            portNameByHash  = new Dictionary <int, string>();
            portHashByDescr = new Dictionary <string, int>();
            portTimoutFlags = new Dictionary <int, bool>();
            portTSByHash    = new Dictionary <int, int>();

            portDescrByHash.Add(emuID, emuIDstr);
            portNameByHash.Add(emuID, emuIDstr);
            portHashByDescr.Add(emuIDstr, emuID);

            #endregion

            #region nmeaListener

            nmeaListener = new NMEAMultipleListener();
            nmeaListener.LogEventHandler             += (o, e) => InfoEventHandler.Rise(o, e);
            nmeaListener.NMEAIncomingMessageReceived += (o, e) => InfoEventHandler.Rise(o,
                                                                                        new LogEventArgs(LogLineType.INFO,
                                                                                                         string.Format("{0} ({1}) >> {2}", portNameByHash[e.SourceID], portDescrByHash[e.SourceID], e.Message)));

            nmeaListener.RMCSentenceReceived += new EventHandler <RMCMessageEventArgs>(nmeaListener_RMCSentenceReceivedHandler);
            nmeaListener.MTWSentenceReceived += new EventHandler <MTWMessageEventArgs>(nmeaListener_MTWSentenceReceivedHandler);
            nmeaListener.NMEAProprietaryUnsupportedSentenceParsed += new EventHandler <NMEAUnsupportedProprietaryEventArgs>(nmeaListener_NMEAUnsupportedProprietaryReceivedHander);

            #endregion

            #region inPorts

            List <SerialPortSettings> portsList = new List <SerialPortSettings>();

            foreach (var inPort in inPortSettings)
            {
                int portHash = inPort.Value.PortName.GetHashCode();
                portDescrByHash.Add(portHash, inPort.Key);
                portNameByHash.Add(portHash, inPort.Value.PortName);
                portHashByDescr.Add(inPort.Key, portHash);

                portTimoutFlags.Add(portHash, false);
                portTSByHash.Add(portHash, 0);
                portsList.Add(inPort.Value);
            }

            inPorts = new SerialPortsPool(portsList.ToArray());
            inPorts.DataReceived += (o, e) =>
            {
                int portKey = e.PortName.GetHashCode();
                portTSByHash[portKey] = 0;
                if (portTimoutFlags[portKey])
                {
                    portTimoutFlags[portKey] = false;
                    PortStateChangedHandler.Rise(this, new EventArgs());
                }

                nmeaListener.ProcessIncoming(portKey, e.Data);
            };
            inPorts.ErrorReceived += (o, e) => InfoEventHandler.Rise(o,
                                                                     new LogEventArgs(LogLineType.ERROR,
                                                                                      string.Format("{0} ({1}) >> {2}", e.PortName, portDescrByHash[e.PortName.GetHashCode()], e.EventType.ToString())));

            #endregion

            #region timer

            timer        = new PrecisionTimer();
            timer.Mode   = Mode.Periodic;
            timer.Period = 100;

            timer.Tick += (o, e) =>
            {
                if (IsOpen)
                {
                    int[] keys = new int[portTSByHash.Count];
                    portTSByHash.Keys.CopyTo(keys, 0);
                    for (int i = 0; i < keys.Length; i++)
                    {
                        portTSByHash[keys[i]] += timerPeriodMS;
                        if (portTSByHash[keys[i]] > portsTimeoutMS)
                        {
                            portTSByHash[keys[i]] = 0;
                            if (!portTimoutFlags[keys[i]])
                            {
                                portTimoutFlags[keys[i]] = true;
                                PortStateChangedHandler.Rise(this, new EventArgs());
                            }
                        }
                    }
                }

                systemUpdateTS += timerPeriodMS;
                if (systemUpdateTS > systemUpdateTimeoutMS)
                {
                    SystemUpdate();
                }
            };

            timer.Start();

            #endregion
        }