Exemple #1
0
        public MainForm()
        {
            InitializeComponent();

            #region filenames

            DateTime startTime = DateTime.Now;
            settingsFileName = Path.ChangeExtension(Application.ExecutablePath, "settings");
            loggerFileName   = StrUtils.GetTimeDirTreeFileName(startTime, Application.ExecutablePath, "LOG", "log", true);
            snapshotsPath    = StrUtils.GetTimeDirTree(startTime, Application.ExecutablePath, "SNAPSHOTS", false);

            #endregion

            #region logger

            logger = new TSLogProvider(loggerFileName);
            logger.WriteStart();
            logger.TextAddedEvent += (o, e) =>
            {
                if (geoPlotCartesian.InvokeRequired)
                {
                    geoPlotCartesian.Invoke((MethodInvoker) delegate
                    {
                        geoPlotCartesian.AppendHistoryLine(e.Text);
                        geoPlotCartesian.Invalidate();
                    });
                }
                else
                {
                    geoPlotCartesian.AppendHistoryLine(e.Text);
                    geoPlotCartesian.Invalidate();
                }
            };

            #endregion

            #region settingsProvider

            settingsProvider = new SimpeSettingsProviderXML <SettingsContainer>();
            settingsProvider.isSwallowExceptions = false;

            try
            {
                settingsProvider.Load(settingsFileName);
            }
            catch (Exception ex)
            {
                ProcessException(ex, true);
            }

            settingsProvider.isSwallowExceptions = true;

            logger.Write(settingsProvider.Data.ToString());

            #endregion

            #region NMEA

            NMEAParser.AddManufacturerToProprietarySentencesBase(ManufacturerCodes.VLB);
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.VLB, "L", "x.x,x.x,x.x,x.x,x,x.x,x.x,x.x");

            #endregion

            #region agingValues

            targetLatitude    = new AgingValue <double>(3, 20, (x) => string.Format(CultureInfo.InvariantCulture, "LAT: {0:F06}°", x));
            targetLongitude   = new AgingValue <double>(3, 20, (x) => string.Format(CultureInfo.InvariantCulture, "LON: {0:F06}°", x));
            targetRadialError = new AgingValue <double>(3, 300, (x) => string.Format(CultureInfo.InvariantCulture, "RER: {0:F03} m", x));
            targetTemperature = new AgingValue <double>(3, 600, (x) => string.Format(CultureInfo.InvariantCulture, "TMP: {0:F01} °C", x));
            targetDepth       = new AgingValue <double>(3, 60, (x) => string.Format(CultureInfo.InvariantCulture, "DPT: {0:F03} m", x));
            targetBatVoltage  = new AgingValue <double>(3, 300, (x) => string.Format(CultureInfo.InvariantCulture, "BAT: {0:F01} V", x));
            targetMSR         = new AgingValue <double>(3, 20, (x) => string.Format(CultureInfo.InvariantCulture, "MSR: {0:F01} dB", x));
            targetPTime       = new AgingValue <double>(3, 20, (x) => string.Format(CultureInfo.InvariantCulture, "TIM: {0:F04} sec", x));

            ownLatitude       = new AgingValue <double>(3, 20, (x) => string.Format(CultureInfo.InvariantCulture, "LAT: {0:F06}°", x));
            ownLongitude      = new AgingValue <double>(3, 20, (x) => string.Format(CultureInfo.InvariantCulture, "LON: {0:F06}°", x));
            ownDepth          = new AgingValue <double>(3, 20, (x) => string.Format(CultureInfo.InvariantCulture, "DPT: {0:F03} m", x));
            ownBatteryVoltage = new AgingValue <double>(3, 20, (x) => string.Format(CultureInfo.InvariantCulture, "BAT: {0:F01} V", x));

            #endregion

            #region geoPlotCartesian

            geoPlotCartesian.AddTrack("Rover", Color.Gray, 2, 2, settingsProvider.Data.MeasurementsFIFOSize, true);
            geoPlotCartesian.AddTrack("Measurements", Color.Blue, 2, 2, settingsProvider.Data.MeasurementsFIFOSize, true);
            geoPlotCartesian.AddTrack("Reference point", Color.Magenta, 4, 4, 1, false);
            geoPlotCartesian.AddTrack("Base", Color.Green, 4, 4, settingsProvider.Data.BaseSize, false);
            geoPlotCartesian.AddTrack("Target", Color.Red, 6, 6, settingsProvider.Data.TargetLocationFIFOSize, true);

            #endregion

            #region vlblCore

            vlblCore = new VLBLCore <VLBLTOAMeasurement>(settingsProvider.Data.MeasurementsFIFOSize,
                                                         settingsProvider.Data.BaseSize,
                                                         settingsProvider.Data.RadialErrorThreshold,
                                                         settingsProvider.Data.SimplexSize,
                                                         UCNLNav.Algorithms.WGS84Ellipsoid);

            vlblCore.BaseUpdatedEventHandler           += (o, e) => { UpdateTrack("Base", e.BasePoints); };
            vlblCore.ReferencePointUpdatedEventHandler += (o, e) => { UpdateTrack("Reference point", vlblCore.ReferencePoint); };

            vlblCore.TargetPositionUpdatedEventHandler += (o, e) =>
            {
                UpdateTrack("Target", e.TargetPosition);

                if (InvokeRequired)
                {
                    Invoke((MethodInvoker) delegate
                    {
                        targetLatitude.Value    = e.TargetPosition.Latitude;
                        targetLongitude.Value   = e.TargetPosition.Longitude;
                        targetRadialError.Value = e.RadialError;
                    });
                }
                else
                {
                    targetLatitude.Value    = e.TargetPosition.Latitude;
                    targetLongitude.Value   = e.TargetPosition.Longitude;
                    targetRadialError.Value = e.RadialError;
                }
            };

            #endregion

            #region tracks

            tracks = new Dictionary <string, List <GeoPoint3D> >();

            #endregion

            #region InPort

            inPort = new NMEASerialPort(new SerialPortSettings(settingsProvider.Data.InPortName, settingsProvider.Data.InPortBaudrate,
                                                               System.IO.Ports.Parity.None,
                                                               DataBits.dataBits8,
                                                               System.IO.Ports.StopBits.One,
                                                               System.IO.Ports.Handshake.None));

            inPort.NewNMEAMessage += new EventHandler <NewNMEAMessageEventArgs>(inPort_NewNMEAMessage);

            #endregion

            #region OutPort

            if (settingsProvider.Data.IsUseOutPort)
            {
                outPort = new SerialPort(settingsProvider.Data.OutPortName, (int)settingsProvider.Data.OutPortBaudrate);
            }

            #endregion

            #region timer

            timer        = new PrecisionTimer();
            timer.Period = 1000;
            timer.Mode   = Mode.Periodic;
            timer.Tick  += new EventHandler(timer_Tick);
            timer.Start();

            #endregion
        }
Exemple #2
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>();
        }
Exemple #3
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
        }