コード例 #1
0
        public void ProcessBasePoints(IEnumerable <T> basePoints, double depth, DateTime timeStamp)
        {
            double lat_deg, lon_deg, rErr;
            int    it_Cnt;

            if (typeof(GeoPoint3DD).IsAssignableFrom(typeof(T)))
            {
                UCNLNav.Navigation.TOA_Locate2D(basePoints.Cast <GeoPoint3DD>().ToArray <GeoPoint3DD>(),
                                                targetLocation.Latitude, targetLocation.Longitude, depth,
                                                Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize,
                                                referenceEllipsoid,
                                                out lat_deg, out lon_deg, out rErr, out it_Cnt);
            }
            else if (typeof(GeoPoint3DT).IsAssignableFrom(typeof(T)))
            {
                UCNLNav.Navigation.TDOA_Locate2D(basePoints.Cast <GeoPoint3DT>().ToArray <GeoPoint3DT>(),
                                                 targetLocation.Latitude, targetLocation.Longitude, depth,
                                                 Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize,
                                                 referenceEllipsoid, soundSpeed,
                                                 out lat_deg, out lon_deg, out rErr, out it_Cnt);
            }
            else
            {
                if (ExternalSolver != null)
                {
                    ExternalSolver(basePoints, targetLocation, out lat_deg, out lon_deg, out rErr, out it_Cnt);
                }
                else
                {
                    throw new NullReferenceException("ExternalSolver not defined");
                }
            }

            if (rErr < radialErrorThreshold)
            {
                crsEstimator.AddPoint(new GeoPoint(lat_deg, lon_deg));

                if (crsEstimator.IsCourse)
                {
                    TargetCourseUpdatedHandler.Rise(this,
                                                    new TargetCourseUpdatedEventArgs(crsEstimator.Course_deg, timeStamp));
                }

                targetLocation.Latitude  = lat_deg;
                targetLocation.Longitude = lon_deg;
                targetLocationTS         = timeStamp;

                TargetLocationUpdatedHandler.Rise(this,
                                                  new TargetLocationUpdatedEventArgs(targetLocation, rErr, timeStamp));

                TargetLocationUpdatedExHandler.Rise(this,
                                                    new TargetLocationUpdatedExEventArgs(targetLocation, rErr, crsEstimator.Course_deg, timeStamp));
            }
            else
            {
                RadialErrorExeedsThrehsoldEventHandler.Rise(this, new EventArgs());
            }
        }
コード例 #2
0
ファイル: PCore2D.cs プロジェクト: ucnl/UCNLNav
        public void ProcessBasePoints(IEnumerable <T> basePoints, double depth, DateTime timeStamp)
        {
            double lat_deg, lon_deg, rErr;
            int    it_Cnt;

            if (typeof(GeoPoint3DD).IsAssignableFrom(typeof(T)))
            {
                UCNLNav.Navigation.TOA_Locate2D(basePoints.Cast <GeoPoint3DD>().ToArray <GeoPoint3DD>(),
                                                targetLocation.Latitude, targetLocation.Longitude, depth,
                                                Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize,
                                                referenceEllipsoid,
                                                out lat_deg, out lon_deg, out rErr, out it_Cnt);
            }
            else if (typeof(GeoPoint3DT).IsAssignableFrom(typeof(T)))
            {
                UCNLNav.Navigation.TDOA_Locate2D(basePoints.Cast <GeoPoint3DT>().ToArray <GeoPoint3DT>(),
                                                 targetLocation.Latitude, targetLocation.Longitude, depth,
                                                 Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize,
                                                 referenceEllipsoid, soundSpeed,
                                                 out lat_deg, out lon_deg, out rErr, out it_Cnt);
            }
            else
            {
                if (ExternalSolver != null)
                {
                    ExternalSolver(basePoints, targetLocation, out lat_deg, out lon_deg, out rErr, out it_Cnt);
                }
                else
                {
                    throw new NullReferenceException("ExternalSolver not defined");
                }
            }

            if (rErr < radialErrorThreshold)
            {
                crsEstimator.AddPoint(new GeoPoint(lat_deg, lon_deg));

                if (crsEstimator.IsCourse)
                {
                    TargetCourseUpdatedHandler.Rise(this,
                                                    new TargetCourseUpdatedEventArgs(crsEstimator.Course_deg, timeStamp));
                }

                targetLocation.Latitude  = lat_deg;
                targetLocation.Longitude = lon_deg;
                targetLocationTS         = timeStamp;

                TargetLocationUpdatedHandler.Rise(this,
                                                  new TargetLocationUpdatedEventArgs(targetLocation, rErr, timeStamp));

                TargetLocationUpdatedExHandler.Rise(this,
                                                    new TargetLocationUpdatedExEventArgs(targetLocation, rErr, crsEstimator.Course_deg, timeStamp));


                TBAQuality tbaState = Navigation.GetTBAState(Navigation.GetBasesMaxAngularGapDeg(basePoints, lat_deg, lon_deg));

                DOPState dopState = DOPState.Invalid;
                double   gdop = double.NaN, pdop = double.NaN, hdop = double.NaN, vdop = double.NaN, tdop = double.NaN;

                GeoPoint3D tL = new GeoPoint3D(targetLocation.Latitude, targetLocation.Longitude, depth);
                if (Navigation.GetDOPs(basePoints, tL, Algorithms.WGS84Ellipsoid, out gdop, out pdop, out hdop, out vdop, out tdop))
                {
                    dopState = Navigation.GetDOPState(hdop);
                }

                BaseQualityUpdatedHandler.Rise(this,
                                               new BaseQualityUpdatedEventArgs(tbaState, gdop, pdop, hdop, vdop, tdop, dopState));
            }
            else
            {
                RadialErrorExeedsThrehsoldEventHandler.Rise(this, new EventArgs());
            }
        }