Esempio n. 1
0
        private void Parse_REM_PONGEX(object[] parameters)
        {
            gtr_isWaitingRemote = false;
            //#define IC_D2H_REM_PONGEX       'D'        // $PTNTD,requestedAddr,requestedCmd,receivedValue_decoded,snrd,dpl,pTime,[dst],[dpt],[tmp]

            try
            {
                int     requestedAddr = (int)parameters[0];
                CDS_CMD cmdID         = (CDS_CMD)Enum.ToObject(typeof(CDS_CMD), (int)parameters[1]);
                double  value         = (double)parameters[2];
                double  snrd          = 10 * Math.Log10(Math.Abs((double)parameters[3]));
                double  dpl           = (double)parameters[4];
                double  pTime         = (double)parameters[5];
                double  dst           = (double)parameters[6];
                double  dpt           = (double)parameters[7];
                double  tmp           = (double)parameters[8];

                bTemperature.Value = tmp;
                bDepth.Value       = dpt;

                if (cmdID == CDS_CMD.CDS_CMD_TMP)
                {
                    tTemperature.Value = value;
                }
                else if (cmdID == CDS_CMD.CDS_CMD_DPT)
                {
                    tDepth.Value = value;
                }

                StringBuilder sb = new StringBuilder();
                sb.AppendFormat(CultureInfo.InvariantCulture, "BOAT\r\nLAT: {0}\r\nLON: {1}\r\nDPT: {2}\r\nTMP: {3}\r\nPTM: {4:F04} s\r\n",
                                bLatitude.ToString(), bLongitude.ToString(), bDepth.ToString(), bTemperature.ToString(), pTime);

                sb.AppendFormat(CultureInfo.InvariantCulture, "\r\nTARGET\r\nDST: {0:F02} m\r\nSNR: {1:F01} dB\r\nDPL: {2:F02} Hz\r\n", dst, snrd, dpl);

                if (tDepth.IsInitialized && !tDepth.IsObsolete)
                {
                    sb.AppendFormat(CultureInfo.InvariantCulture, "DPT: {0}\r\n", tDepth.ToString());
                }

                if (tTemperature.IsInitialized && !tTemperature.IsObsolete)
                {
                    sb.AppendFormat(CultureInfo.InvariantCulture, "TMP: {0}\r\n", tTemperature.ToString());
                }


                if (bLatitude.IsInitialized && !bLatitude.IsObsolete &&
                    bLongitude.IsInitialized && !bLongitude.IsObsolete)
                {
                    measurements.Add(new Measurement(bLatitude.Value, bLongitude.Value, dst, snrd, dpt));

                    InvokeUpdateTrack("MEASUREMENTS", bLatitude.Value, bLongitude.Value);

                    if (measurements.IsBaseExists && tDepth.IsInitialized && (measurements.AngleRange > 270))
                    {
                        GeoPoint3DWE prevLocation = new GeoPoint3DWE();
                        prevLocation.Latitude    = double.NaN;
                        prevLocation.Longitude   = double.NaN;
                        prevLocation.Depth       = tDepth.Value;
                        prevLocation.RadialError = double.NaN;

                        double stStageRErr = 0.0;
                        int    itCnt       = 0;

                        var           basePoints = measurements.GetBase();
                        List <PointF> basePnts   = new List <PointF>();
                        foreach (var bPoint in basePoints)
                        {
                            basePnts.Add(new PointF(Convert.ToSingle(bPoint.Latitude), Convert.ToSingle(bPoint.Longitude)));
                        }

                        InvokeUpdateTrack("BASE", basePnts.ToArray());

                        var locResult = Navigation.LocateLBL_NLM(basePoints, prevLocation, settingsProvider.Data.RadialErrorThreshold, out stStageRErr, out itCnt);

                        tLatitude.Value    = locResult.Latitude;
                        tLongitude.Value   = locResult.Longitude;
                        tRadialError.Value = locResult.RadialError;
                        tLocation.Add(locResult);

                        InvokeUpdateTrack("TARGET", locResult.Latitude, locResult.Longitude);

                        if (settingsProvider.Data.IsGNSSEmulator)
                        {
                            SendEMU(locResult.Latitude, locResult.Longitude, tDepth.Value, locResult.RadialError);
                        }

                        if ((double.IsNaN(tBestLocation.Latitude) || (tBestLocation.RadialError > locResult.RadialError)))
                        {
                            tBestLocation.Latitude    = locResult.Latitude;
                            tBestLocation.Longitude   = locResult.Longitude;
                            tBestLocation.RadialError = locResult.RadialError;
                            measurements.UpdateReferencePoint(tBestLocation.Latitude, tBestLocation.Longitude);

                            InvokeUpdateTrack("BEST", tBestLocation.Latitude, tBestLocation.Longitude);
                        }

                        InvokeSetEnabled(mainToolStrip, tracksBtn, true);
                    }
                    else
                    {
                        double cLat = 0;
                        double cLon = 0;
                        measurements.CenterOfMass(out cLat, out cLon);
                        measurements.UpdateReferencePoint(cLat, cLon);
                    }
                }



                if (tLatitude.IsInitialized && !tLatitude.IsObsolete &&
                    tLongitude.IsInitialized && !tLongitude.IsObsolete)
                {
                    sb.AppendFormat(CultureInfo.InvariantCulture, "LAT: {0}\r\nLON: {1}\r\n", tLatitude.ToString(), tLongitude.ToString());
                }

                if (tRadialError.IsInitialized && !tRadialError.IsObsolete)
                {
                    sb.AppendFormat(CultureInfo.InvariantCulture, "RER: {0}\r\n", tRadialError.ToString());
                }

                if (!double.IsNaN(tBestLocation.RadialError))
                {
                    sb.AppendFormat(CultureInfo.InvariantCulture, "BRE: {0:F03}\r\n", tBestLocation.RadialError);
                }

                InvokeSetLeftUpperCornerText(sb.ToString());
                InvokeInvalidatePlot();

                if (isAutosnapshot)
                {
                    InvokeSaveSnapShot();
                }
            }
            catch (Exception ex)
            {
                ProcessException(ex, false);
            }
        }
Esempio n. 2
0
        public MainForm()
        {
            InitializeComponent();

            this.Text = string.Format("{0}", Application.ProductName);

            #region file names & paths

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

            #endregion

            #region logger

            loggerTextAddedEventHandler = new EventHandler <TextAddedEventArgs>(logger_TextAdded);

            logger = new TSLogProvider(logFileName);
            logger.TextAddedEvent += loggerTextAddedEventHandler;
            logger.WriteStart();

            #endregion

            #region settings

            logger.Write("Loading settings...");
            settingsProvider = new SettingsProviderXML <SettingsContainer>();
            settingsProvider.isSwallowExceptions = false;

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

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

            gtr_RemoteTimeout_S = Convert.ToInt32((GTR.MIN_REM_TOUT_MS / 1000) + 2 * (double)settingsProvider.Data.MaxDistance / 1500) + 1;

            #endregion

            #region NMEA

            NMEAParser.AddManufacturerToProprietarySentencesBase(ManufacturerCodes.TNT);
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "0", "x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "1", "x,x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "2", "x,x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "3", "x,x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "4", "x,x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "5", "x,x.x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "6", "x,x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "7", "x,x.x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "8", "x,x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "9", "x,x.x,x.x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "A", "x,x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "B", "x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "C", "x,x.x,x.x,x.x,x.x,x.x,x.x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "D", "x,x,x.x,x.x,x.x,x.x,x.x,x.x,x.x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "E", "x,x,x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "!", "c--c,x,c--c,x,x,c--c");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "O", "x.x,x.x");
            NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "P", "x,x.x");

            /*
             #define IC_D2H_ACK              '0'        // $PTNT0,errCode
             #define IC_H2D_FLD_GET          '1'        // $PTNT1,fldID,reserved
             #define IC_H2D_FLD_SET          '2'        // $PTNT2,fldID,fldNewValue
             #define IC_D2H_FLD_VAL          '3'        // $PTNT3,fldID,fldValue
             #define IC_H2D_LOC_DATA_GET     '4'        // $PTNT4,locDataID,reserved
             #define IC_D2H_LOC_DATA_VAL     '5'        // $PTNT5,locDataID,locDataValue
             #define IC_H2D_ACT_INVOKE       '6'        // $PTNT6,actionID,reserved
             #define IC_H2D_LOC_DATA_SET     '7'        // $PTNT7,locDataID,locDataValue
             #define IC_H2D_REM_SEND         '8'        // $PTNT8,targetAddr,codeMsg
             #define IC_D2H_REM_RECEIVED     '9'        // $PTNT9,codeMsg,snrd,dpl
             #define IC_H2D_REM_PING         'A'        // $PTNTA,targetAddr,timeoutMs
             #define IC_D2H_REM_TOUT         'B'        // $PTNTB,targetAddr
             #define IC_D2H_REM_PONG         'C'        // $PTNTC,requestedAddr,snrd,dpl,pTime,[dst],[dpt],[tmp]
             #define IC_D2H_REM_PONGEX       'D'        // $PTNTD,requestedAddr,requestedCmd,receivedValue_decoded,snrd,dpl,pTime,[dst],[dpt],[tmp]
             #define IC_H2D_REM_PINGEX       'E'        // $PTNTE,targetAddr,requestedCmd,timeoutMs
             #define IC_D2H_DEV_INFO_VAL     '!'        // $PTNT!,sys_moniker,sys_ver,dev_type,core_moniker,core_ver,dev_serial_num
             #define IC_D2H_PRETMP_VAL       'O'        // $PTNTO,pressure_mbar,temp_deg_c
             #define IC_H2D_SETVAL           'P'        // $PTNTP,valueID,value
             */

            #endregion

            #region other

            gtrPortErrorEventHandler          = new EventHandler <SerialErrorReceivedEventArgs>(gtrPort_Error);
            gtrPortNewNMEAMessageEventHandler = new EventHandler <NewNMEAMessageEventArgs>(gtrPort_NewNMEAMessage);

            gtrPort = new NMEASerialPort(new SerialPortSettings(settingsProvider.Data.GTRPortName, BaudRate.baudRate9600,
                                                                System.IO.Ports.Parity.None, DataBits.dataBits8, System.IO.Ports.StopBits.One, System.IO.Ports.Handshake.None));

            gtrPort.IsRawModeOnly = false;

            timerTickHandler = new EventHandler(timer_Tick);

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

            timer.Tick += timerTickHandler;
            timer.Start();


            if (settingsProvider.Data.IsGNSSEmulator)
            {
                gnssEmulatorPort = new NMEASerialPort(new SerialPortSettings(settingsProvider.Data.GNSSEmulatorPortName, BaudRate.baudRate9600,
                                                                             Parity.None, DataBits.dataBits8, StopBits.One, Handshake.None));
            }

            measurements = new Measurements(settingsProvider.Data.MeasurementsFIFOSize, settingsProvider.Data.BaseSize);

            bLatitude    = new AgingDouble("F06", "°");
            bLongitude   = new AgingDouble("F06", "°");
            bTemperature = new AgingDouble("F01", "°C");
            bDepth       = new AgingDouble("F03", " m");
            tTemperature = new AgingDouble("F01", "°C");
            tDepth       = new AgingDouble("F02", " m");

            tLatitude    = new AgingDouble("F06", "°");
            tLongitude   = new AgingDouble("F06", "°");
            tRadialError = new AgingDouble("F03", " m");

            tLocation = new List <GeoPoint3DWE>();
            bLocation = new List <GeoPoint>();

            tBestLocation             = new GeoPoint3DWE();
            tBestLocation.Latitude    = double.NaN;
            tBestLocation.Longitude   = double.NaN;
            tBestLocation.RadialError = double.NaN;

            marinePlot.InitTracks(settingsProvider.Data.MeasurementsFIFOSize);
            marinePlot.AddTrack("BOAT GNSS", Color.Blue, 2.0f, 2, settingsProvider.Data.MeasurementsFIFOSize, true);
            marinePlot.AddTrack("BASE", Color.Salmon, 2.0f, 8, settingsProvider.Data.BaseSize, false);
            marinePlot.AddTrack("MEASUREMENTS", Color.Green, 2.0f, 4, settingsProvider.Data.MeasurementsFIFOSize, false);
            marinePlot.AddTrack("TARGET", Color.Black, 2.0f, 4, settingsProvider.Data.MeasurementsFIFOSize, false);
            marinePlot.AddTrack("BEST", Color.Red, 2.0f, 8, 1, false);

            #endregion
        }
Esempio n. 3
0
        public static GeoPoint3DWE LocateLBL_NLM(List <Measurement> bases, GeoPoint3DWE previousLocation, double rErrThreshold, out double stStageRErr, out int nlm_itCount)
        {
            // bases sorted from nearest to farest

            #region variables

            double[] bx = new double[bases.Count]; // base x-coordinate
            double[] by = new double[bases.Count]; // base y-coordinate
            double[] bz = new double[bases.Count]; // base z-coordinate
            double[] bd = new double[bases.Count]; // distance to a base

            double meanLat = 0;
            double meanLon = 0;

            double z = previousLocation.Depth;

            double radialError = double.NaN;

            double simplexSize = 25;

            #endregion

            #region bases mid-point

            foreach (var item in bases)
            {
                meanLat += item.Latitude;
                meanLon += item.Longitude;
            }

            meanLat /= bases.Count;
            meanLon /= bases.Count;

            #endregion

            #region translation of coordinates from degrees to meters

            for (int i = 0; i < bases.Count; i++)
            {
                Navigation.GetDistance2DDeg(meanLat, meanLon, bases[i].Latitude, bases[i].Longitude, out by[i], out bx[i]);
                bd[i] = bases[i].Distance_m;
                bz[i] = bases[i].OwnDepth;
            }

            #endregion

            double xBest = double.NaN;
            double yBest = double.NaN;
            stStageRErr = double.NaN;

            if ((double.IsNaN(previousLocation.Latitude)) ||
                (double.IsNaN(previousLocation.Longitude)) ||
                (previousLocation.RadialError > rErrThreshold))
            {
                #region 1st stage 1D optimization

                double alpha = LocateLBL_1D(z, bx, by, bz, bd, 0, 360, 10, out xBest, out yBest);
                alpha = LocateLBL_1D(z, bx, by, bz, bd, alpha - 10, alpha + 10, 1, out xBest, out yBest);

                stStageRErr = Math.Sqrt(EpsTOA(xBest, yBest, z, bx, by, bz, bd));
                simplexSize = stStageRErr;// *stStageRErr;

                #endregion
            }
            else
            {
                Navigation.GetDistance2DDeg(meanLat, meanLon, previousLocation.Latitude, previousLocation.Longitude, out yBest, out xBest);
            }

            #region Nelder-Mead 2D optimization

            bool     isFinished = false;
            int      itCnt = 0;
            double   tmp, tmp1;
            double[] xix = new double[3];
            double[] xiy = new double[3];
            double[] fxi = new double[3];
            double   fl, fg, fh, fr, fe, fs;
            double   xcx, xcy, xrx, xry, xex, xey, xsx, xsy;

            xix[0] = xBest;
            xiy[0] = yBest;
            xix[1] = xix[0] + simplexSize;     //
            xiy[1] = xiy[0] + simplexSize;     //
            xix[2] = xix[0] - simplexSize / 2; //
            xiy[2] = xiy[0] + simplexSize / 2; //

            while ((!isFinished) && (itCnt < NAV_NLM_MIT))
            {
                fxi[0] = EpsTOA(xix[0], xiy[0], z, bx, by, bz, bd);
                fxi[1] = EpsTOA(xix[1], xiy[1], z, bx, by, bz, bd);
                fxi[2] = EpsTOA(xix[2], xiy[2], z, bx, by, bz, bd);

                if (fxi[0] > fxi[1])
                {
                    tmp = fxi[0]; fxi[0] = fxi[1]; fxi[1] = tmp;
                    tmp = xix[0]; xix[0] = xix[1]; xix[1] = tmp;
                    tmp = xiy[0]; xiy[0] = xiy[1]; xiy[1] = tmp;
                }

                if (fxi[0] > fxi[2])
                {
                    tmp = fxi[0]; fxi[0] = fxi[2]; fxi[2] = tmp;
                    tmp = xix[0]; xix[0] = xix[2]; xix[2] = tmp;
                    tmp = xiy[0]; xiy[0] = xiy[2]; xiy[2] = tmp;
                }

                if (fxi[1] > fxi[2])
                {
                    tmp = fxi[1]; fxi[1] = fxi[2]; fxi[2] = tmp;
                    tmp = xix[1]; xix[1] = xix[2]; xix[2] = tmp;
                    tmp = xiy[1]; xiy[1] = xiy[2]; xiy[2] = tmp;
                }

                fl = fxi[0];
                fg = fxi[1];
                fh = fxi[2];

                xcx = (xix[0] + xix[1]) / 2.0f;
                xcy = (xiy[0] + xiy[1]) / 2.0f;

                xrx = (1.0f + NAV_NLM_A) * xcx - NAV_NLM_A * xix[2];
                xry = (1.0f + NAV_NLM_A) * xcy - NAV_NLM_A * xiy[2];

                fr = EpsTOA(xrx, xry, z, bx, by, bz, bd);

                if (fr < fl)
                {
                    xex = (1.0f - NAV_NLM_G) * xcx + NAV_NLM_G * xrx;
                    xey = (1.0f - NAV_NLM_G) * xcy + NAV_NLM_G * xry;

                    fe = EpsTOA(xex, xey, z, bx, by, bz, bd);

                    if (fe < fr)
                    {
                        xix[2] = xex;
                        xiy[2] = xey;
                    }
                    else
                    {
                        xix[2] = xrx;
                        xiy[2] = xry;
                    }
                }
                else
                {
                    if ((fr > fl) && (fr < fg))
                    {
                        xix[2] = xrx;
                        xiy[2] = xry;
                    }
                    else
                    {
                        if ((fr > fg) && (fr < fh))
                        {
                            tmp = xix[2]; xix[2] = xrx; xrx = tmp;
                            tmp = xiy[2]; xiy[2] = xry; xry = tmp;
                            tmp = fxi[2]; fxi[2] = fr; fr = tmp;
                        }
                        else
                        {
                            if (fh < fr)
                            {
                                //
                            }
                        }

                        xsx = NAV_NLM_B * xix[2] + (1.0f - NAV_NLM_B) * xcx;
                        xsy = NAV_NLM_B * xiy[2] + (1.0f - NAV_NLM_B) * xcy;
                        fs  = EpsTOA(xsx, xsy, z, bx, by, bz, bd);

                        if (fs < fh)
                        {
                            xix[2] = xsx;
                            xiy[2] = xsy;
                        }
                        else
                        {
                            xix[1] = (xix[1] - xix[0]) / 2.0f;
                            xiy[1] = (xiy[1] - xiy[0]) / 2.0f;
                            xix[2] = (xix[2] - xix[0]) / 2.0f;
                            xiy[2] = (xiy[2] - xiy[0]) / 2.0f;
                        }
                    }
                }

                tmp  = (fxi[0] + fxi[1] + fxi[2]) / 3.0f;
                tmp1 = ((fxi[0] - tmp) * (fxi[0] - tmp) +
                        (fxi[1] - tmp) * (fxi[1] - tmp) +
                        (fxi[2] - tmp) * (fxi[2] - tmp)) / 3.0f;

                isFinished = (Math.Sqrt(tmp1) <= NAV_NLM_EPSILON);
                itCnt++;
            }

            xBest = xix[0];
            yBest = xiy[0];


            radialError = Math.Sqrt(EpsTOA(xix[0], xiy[0], z, bx, by, bz, bd));
            nlm_itCount = itCnt;

            #endregion

            #region convert coordinates to degrees

            double targetLongitude = double.NaN;
            double targetLatitude  = double.NaN;

            Meters2Deg(meanLon, meanLat, yBest, xBest, out targetLongitude, out targetLatitude);

            #endregion

            GeoPoint3DWE result = new GeoPoint3DWE();

            result.Depth       = previousLocation.Depth;
            result.Latitude    = targetLatitude;
            result.Longitude   = targetLongitude;
            result.RadialError = radialError;

            return(result);
        }