Ejemplo n.º 1
0
        public void CalcMeasure()
        {
            if (measureRange.FromLocation != null && measureRange.ToLocation != null)
            {
                double lat1 = measureRange.FromLocation.Latitude;
                double lon1 = measureRange.FromLocation.Longitude;
                double lat2 = measureRange.ToLocation.Latitude;
                double lon2 = measureRange.ToLocation.Longitude;

                measureResult.DST = CalcDistance(lat1, lon1, lat2, lon2) / 1852;
                double brg = CalcBearing(lat1, lon1, lat2, lon2);
                measureResult.BRG = (brg + 360) % 360;

                if (TWS.IsValid())
                {
                    measureResult.TWA = ConvertTo180((TWD.Val - brg + 360) % 360);
                }

                double     vmc = 0;
                PolarPoint p   = new PolarPoint();

                if (SOG.IsValid())
                {
                    vmc = SOG.Val;
                }

                if (TWD.IsValid() && SPD.IsValid() && NavPolar.IsLoaded)
                {
                    double Angle = Math.Abs(measureResult.TWA % 360);
                    if (Angle > 180)
                    {
                        Angle = 360 - Angle;
                    }
                    if (Angle < 50)
                    {
                        p   = NavPolar.GetBeatTargeInterpolated(TWS.Val);
                        vmc = p.SPD * Math.Cos(p.TWA * Math.PI / 180);
                    }
                    else
                    if (Angle > 140)
                    {
                        p   = NavPolar.GetRunTargetInterpolated(TWS.Val);
                        vmc = -p.SPD * Math.Cos(p.TWA * Math.PI / 180);
                    }
                    else
                    {
                        vmc = NavPolar.GetTargeInterpolated(Angle, TWS.Val);
                    }
                }

                if (vmc != 0)
                {
                    double xx = measureResult.DST / vmc;
                    if (xx > TimeSpan.MaxValue.TotalHours)
                    {
                        xx = TimeSpan.MaxValue.TotalHours - 1;
                    }
                    if (xx < TimeSpan.MinValue.TotalHours)
                    {
                        xx = TimeSpan.MinValue.TotalHours + 1;
                    }
                    measureResult.TTG = TimeSpan.FromHours(xx);
                }
                else
                {
                    measureResult.TTG = TimeSpan.FromHours(0);
                }
            }
        }
Ejemplo n.º 2
0
        public void CalcNav(DateTime now, bool bypassComm = false)
        {
            sailingMode = SailingMode.None;

            #region Primitives
            if (rmc_received || bypassComm)
            {
                LAT.Val = lat;
                LON.Val = lon;
                SOG.Val = sog;
                COG.Val = cog;
                LAT.SetValid(now);
                LON.SetValid(now);
                SOG.SetValid(now);
                COG.SetValid(now);
                RMC_received_Timer.Start();
            }

            if (vhw_received || bypassComm)
            {
                SPD.Val = spd;
                SPD.SetValid(now);
            }

            if (dpt_received || bypassComm)
            {
                DPT.Val = dpt;
                DPT.SetValid(now);
            }

            if (mwv_received || bypassComm)
            {
                AWA.Val = awa;
                AWS.Val = aws;
                AWA.SetValid(now);
                AWS.SetValid(now);
            }

            if (mtw_received || bypassComm)
            {
                TEMP.Val = temp;
                TEMP.SetValid(now);
            }

            if (hdg_received || bypassComm)
            {
                double mv = Properties.Settings.Default.MagVar; //default
                if (mvar2 != 0)
                {
                    mv = mvar2;                                 //From HDG
                }
                if (mvar1 != 0)
                {
                    mv = mvar1;                                 //From RMC
                }
                MVAR.Val = mv;
                MVAR.SetValid(now);

                if (bypassComm)
                {
                    mv = 0;         // heading from log file is "true heading" no need for correction
                }
                HDT.Val = hdg + mv;
                HDT.SetValid(now);
            }

            #endregion

            #region Position, Leg bearing, distance, XTE and VMG

            if (LAT.IsValid() && LON.IsValid())
            {
                POS.Val.Latitude  = LAT.Val;
                POS.Val.Longitude = LON.Val;
                POS.SetValid(now);
            }
            else
            {
                POS.Invalidate();
            }

            if (ActiveLeg != null)
            {
                LWLAT.Val = ActiveLeg.FromLocation.Latitude;
                LWLAT.SetValid(now);
                LWLON.Val = ActiveLeg.FromLocation.Longitude;
                LWLON.SetValid(now);
                LWPT.Val.str = ActiveLeg.FromMark.Name;
                LWPT.SetValid(now);
            }
            else
            {
                LWLAT.Invalidate();
                LWLON.Invalidate();
                LWPT.Invalidate();
            }

            if (!bypassComm || replayLog)
            {
                if (ActiveMark != null && POS.IsValid())
                {
                    WLAT.Val = ActiveMark.Location.Latitude;
                    WLAT.SetValid(now);
                    WLON.Val = ActiveMark.Location.Longitude;
                    WLON.SetValid(now);
                    WPT.Val.str = ActiveMark.Name;
                    WPT.SetValid(now);
                    BRG.Val = CalcBearing(LAT.Val, LON.Val, WLAT.Val, WLON.Val);
                    BRG.SetValid(now);
                    DST.Val = CalcDistance(LAT.Val, LON.Val, WLAT.Val, WLON.Val) / 1852;
                    DST.SetValid(now);
                }
                else
                {
                    WLAT.Invalidate();
                    WLON.Invalidate();
                    WPT.Invalidate();
                    BRG.Invalidate();
                    DST.Invalidate();
                }
            }

            if (WPT.IsValid() && LWPT.IsValid())
            {
                LEGBRG.Val = CalcBearing(LWLAT.Val, LWLON.Val, WLAT.Val, WLON.Val);
                LEGBRG.SetValid(now);
            }
            else
            {
                if (LEGBRG.IsValid())
                {
                    LEGBRG.Invalidate();
                }
            }

            if (LWPT.IsValid())
            {
                XTE.Val = Math.Asin(Math.Sin(DST.Val * 1.852 / 6371) * Math.Sin((BRG.Val - LEGBRG.Val) * Math.PI / 180)) * 6371 / 1.852;
                XTE.SetValid(now);
            }
            else
            if (XTE.IsValid())
            {
                XTE.Invalidate();
            }

            if (SOG.IsValid() && BRG.IsValid() && WPT.IsValid())
            {
                VMGWPT.Val = SOG.Val * Math.Cos((COG.Val - BRG.Val) * Math.PI / 180);
                VMGWPT.SetValid(now);
            }
            else
            {
                if (VMGWPT.IsValid())
                {
                    VMGWPT.Invalidate();
                }
            }
            #endregion

            #region True Wind
            if (AWA.IsValid() && SPD.IsValid())
            {
                double Dx = AWS.Val * Math.Cos(AWA.Val * Math.PI / 180) - SPD.Val;
                double Dy = AWS.Val * Math.Sin(AWA.Val * Math.PI / 180);
                TWS.Val = Math.Sqrt(Dx * Dx + Dy * Dy);
                TWS.SetValid(now);
                TWA.Val = Math.Atan2(Dy, Dx) * 180 / Math.PI;
                TWA.SetValid(now);
                VMG.Val = SPD.Val * Math.Cos(TWA.Val * Math.PI / 180);
                VMG.SetValid(now);

                //Set estimated saling mode in case route and/or performance data is not available
                if (Math.Abs(TWA.Val) < 55)
                {
                    sailingMode = SailingMode.Beating;
                }
                else
                if (Math.Abs(TWA.Val) > 130)
                {
                    sailingMode = SailingMode.Running;
                }
                else
                {
                    sailingMode = SailingMode.Reaching;
                }
            }
            else
            {
                if (TWS.IsValid())
                {
                    TWS.Invalidate();
                }
                if (TWA.IsValid())
                {
                    TWA.Invalidate();
                }
                if (VMG.IsValid())
                {
                    VMG.Invalidate();
                }
            }

            if (TWS.IsValid() && HDT.IsValid())
            {
                TWD.Val = HDT.Val + TWA.Val;
                TWD.SetValid(now);
            }
            else
            {
                if (TWD.IsValid())
                {
                    TWD.Invalidate();
                }
            }
            #endregion

            #region Leeway
            if (AWA.IsValid() && SPD.IsValid() && LWay.IsAvailable() && Properties.Settings.Default.EstimateLeeway)
            {
                LWY.Val = LWay.Get(AWA.Val, AWS.Val, SPD.Val);
                LWY.SetValid(now);
            }

            #endregion

            #region Heel
            //if (AWA.IsValid() && SPD.IsValid())
            //{
            //    double k = 7,
            //            a = 2,
            //            b = 200,
            //            c = 1.5;

            //    var awa = Math.Abs(AWA.Val);
            //    var aws = AWS.Val;

            //    HEEL.Val = k * awa * Math.Pow(aws, c) / (Math.Pow(awa, a) + b);
            //    if (HEEL.Val > 45) HEEL.Val = 45;
            //    HEEL.SetValid(now);
            //}
            //else
            //{
            //    if (HEEL.IsValid())
            //        HEEL.Invalidate();
            //}

            #endregion

            #region Drift
            if (SOG.IsValid() && COG.IsValid() && HDT.IsValid() && SPD.IsValid())
            {
                double Dx = SOG.Val * Math.Cos(COG.Val * Math.PI / 180) - SPD.Val * Math.Cos(HDT.Val * Math.PI / 180);
                double Dy = SOG.Val * Math.Sin(COG.Val * Math.PI / 180) - SPD.Val * Math.Sin(HDT.Val * Math.PI / 180);

                if (LWY.IsValid())
                {
                    double lwy;
                    if (AWA.Val < 0)
                    {
                        lwy = -LWY.Val;
                    }
                    else
                    {
                        lwy = LWY.Val;
                    }

                    double lm = SPD.Val * Math.Tan(lwy * Math.PI / 180);
                    double la = HDT.Val - 90;
                    double lx = lm * Math.Cos(la * Math.PI / 180);
                    double ly = lm * Math.Sin(la * Math.PI / 180);

                    double ang = Math.Atan2(ly, lx) * 180 / Math.PI;

                    Dx -= lx;
                    Dy -= ly;
                }

                DRIFT.Val = Math.Sqrt(Dx * Dx + Dy * Dy);
                DRIFT.SetValid(now);
                SET.Val = Math.Atan2(Dy, Dx) * 180 / Math.PI;
                SET.SetValid(now);
            }
            else
            {
                if (DRIFT.IsValid())
                {
                    DRIFT.Invalidate();
                }
                if (SET.IsValid())
                {
                    SET.Invalidate();
                }
            }
            #endregion

            #region Performance
            if (TWA.IsValid() && SPD.IsValid() && NavPolar.IsLoaded && BRG.IsValid())
            {
                double Angle = Math.Abs(TWD.Val - BRG.Val + 360) % 360;
                if (Angle > 180)
                {
                    Angle = 360 - Angle;
                }

                PolarPoint pb = NavPolar.GetBeatTargeInterpolated(TWS.Val);
                PolarPoint pr = NavPolar.GetRunTargetInterpolated(TWS.Val);

                if (Angle <= (pb.TWA + 20)) // Beating
                {
                    TGTSPD.Val = pb.SPD;
                    TGTSPD.SetValid(now);
                    TGTTWA.Val = pb.TWA;
                    TGTTWA.SetValid(now);
                    PERF.Val = VMG.Val / (pb.SPD * Math.Cos(pb.TWA * Math.PI / 180));
                    PERF.SetValid(now);

                    sailingMode = SailingMode.Beating;
                }

                if (Angle < (pr.TWA - 20) && Angle > (pb.TWA + 20)) // Reaching
                {
                    TGTSPD.Val = NavPolar.GetTargeInterpolated(Math.Abs(TWA.Val), TWS.Val);
                    TGTSPD.SetValid(now);
                    TGTTWA.Val = Math.Abs(TWA.Val);
                    TGTTWA.SetValid(now);
                    PERF.Val = Math.Abs(SPD.Val * Math.Cos((COG.Val - BRG.Val) * Math.PI / 180) / TGTSPD.Val);
                    PERF.SetValid(now);

                    sailingMode = SailingMode.Reaching;
                }

                if (Angle >= (pr.TWA - 20)) // Running
                {
                    TGTSPD.Val = pr.SPD;
                    TGTSPD.SetValid(now);
                    TGTTWA.Val = pr.TWA;
                    TGTTWA.SetValid(now);
                    PERF.Val = VMG.Val / (pr.SPD * Math.Cos(pr.TWA * Math.PI / 180));
                    PERF.SetValid(now);

                    sailingMode = SailingMode.Running;
                }
            }
            else
            {
                if (TGTSPD.IsValid())
                {
                    TGTSPD.Invalidate();
                }
                if (TGTTWA.IsValid())
                {
                    TGTTWA.Invalidate();
                }
                if (PERF.IsValid())
                {
                    PERF.Invalidate();
                }
            }
            #endregion

            #region Line
            if (p1_set && p2_set && LAT.IsValid() && HDT.IsValid())
            {
                double p3_lat = LAT.Val, p3_lon = LON.Val;

                if (Properties.Settings.Default.GPSoffsetToBow != 0)
                {
                    CalcPosition(LAT.Val, LON.Val, Properties.Settings.Default.GPSoffsetToBow, HDT.Val, ref p3_lat, ref p3_lon);
                }
                double brg32 = CalcBearing(p3_lat, p3_lon, p2_lat, p2_lon);
                double dst32 = CalcDistance(p3_lat, p3_lon, p2_lat, p2_lon);

                LINEDST.Val = dst32 * Math.Sin((linebrg - brg32) * Math.PI / 180);
                LINEDST.SetValid(now);
            }
            else
            {
                if (LINEDST.IsValid())
                {
                    LINEDST.Invalidate();
                }
            }
            #endregion

            #region Route nav

            if (ActiveMark != null && DST.IsValid() && !ManOverBoard)
            {
                if (DST.Val <= Properties.Settings.Default.WptProximity && ActiveMark != MOB)
                {
                    (new SoundPlayer(@".\Sounds\BELL7.WAV")).PlaySync();
                    if (ActiveLeg != null)
                    {
                        if (ActiveLeg.NextLeg != null)
                        {
                            ActiveLeg  = ActiveLeg.NextLeg;
                            ActiveMark = ActiveLeg.ToMark;
                        }
                        else
                        {
                            ActiveMark  = null;
                            ActiveLeg   = null;
                            ActiveRoute = null;
                        }
                    }
                    else
                    {
                        ActiveMark = null;
                    }
                }
            }


            if (ActiveRoute != null)
            {
                if (ActiveLeg.NextLeg != null && TWD.IsValid())
                {
                    NTWA.Val = TWD.Val - ActiveLeg.NextLeg.Bearing;
                    NTWA.SetValid(now);
                }
                else
                {
                    NTWA.Invalidate();
                }
            }

            #endregion

            #region Laylines

            insideCourse = false; // Need to determine later

            if (DRIFT.IsValid() && PERF.IsValid() && TWD.IsValid())
            {
                double ttwa   = TGTTWA.Val;
                double tgtlwy = 0;

                if (LWY.IsValid())
                {
                    double awx    = TWS.Val * Math.Cos(ttwa * Math.PI / 180) + TGTSPD.Val;
                    double awy    = TWS.Val * Math.Sin(ttwa * Math.PI / 180);
                    double tgtawa = Math.Atan2(awy, awx) * 180 / Math.PI;
                    double tgtaws = Math.Sqrt(awx * awx + awy * awy);
                    tgtlwy = LWay.Get(tgtawa, tgtaws, TGTSPD.Val);
                }

                ttwa += tgtlwy;
                if (ttwa > 180)
                {
                    ttwa = 180;
                }

                double relset = SET.Val - TWD.Val;
                double dxs    = TGTSPD.Val * Math.Cos(ttwa * Math.PI / 180) + DRIFT.Val * Math.Cos(relset * Math.PI / 180);
                double dys    = TGTSPD.Val * Math.Sin(ttwa * Math.PI / 180) + DRIFT.Val * Math.Sin(relset * Math.PI / 180);

                TGTCOGp.Val = Math.Atan2(dys, dxs) * 180 / Math.PI + TWD.Val;
                TGTCOGp.SetValid(now);
                TGTSOGp.Val = Math.Sqrt(dxs * dxs + dys * dys);
                TGTSOGp.SetValid(now);

                double dxp = TGTSPD.Val * Math.Cos(-ttwa * Math.PI / 180) + DRIFT.Val * Math.Cos(relset * Math.PI / 180);
                double dyp = TGTSPD.Val * Math.Sin(-ttwa * Math.PI / 180) + DRIFT.Val * Math.Sin(relset * Math.PI / 180);

                TGTCOGs.Val = Math.Atan2(dyp, dxp) * 180 / Math.PI + TWD.Val;
                TGTCOGs.SetValid(now);
                TGTSOGs.Val = Math.Sqrt(dxp * dxp + dyp * dyp);
                TGTSOGs.SetValid(now);

                // Determine if sailing inside course +/- 0 degrees
                double a1 = (BRG.Val - TGTCOGs.Val - 0 + 360) % 360;
                double a2 = (TGTCOGp.Val - 0 - TGTCOGs.Val + 360) % 360;

                switch (sailingMode)
                {
                case SailingMode.Beating:

                    if (a1 < a2)
                    {
                        insideCourse = true;
                    }
                    break;

                case SailingMode.Running:

                    if (a2 < a1)
                    {
                        insideCourse = true;
                    }
                    break;
                }

                // Calculate Layline hit points

                if (ActiveMark != null)
                {
                    double alpha = (TGTCOGp.Val - BRG.Val + 360) % 360;
                    double beta  = (BRG.Val - TGTCOGs.Val + 360) % 360;

                    double dist_s, dist_p;

                    if (alpha == 0)
                    {
                        dist_p = DST.Val;
                        dist_s = 0;
                    }
                    else
                    {
                        if (beta == 0)
                        {
                            dist_s = DST.Val;
                            dist_p = 0;
                        }
                        else
                        {
                            dist_p = DST.Val * Math.Sin(beta * Math.PI / 180) /
                                     (Math.Sin(alpha * Math.PI / 180) * Math.Cos(beta * Math.PI / 180) +
                                      Math.Cos(alpha * Math.PI / 180) * Math.Sin(beta * Math.PI / 180));
                            dist_s = DST.Val * Math.Sin(alpha * Math.PI / 180) /
                                     (Math.Sin(alpha * Math.PI / 180) * Math.Cos(beta * Math.PI / 180) +
                                      Math.Cos(alpha * Math.PI / 180) * Math.Sin(beta * Math.PI / 180));
                        }
                    }
                    DSTLYLp.Val = dist_p * 1852;
                    DSTLYLp.SetValid(now);
                    DSTLYLs.Val = dist_s * 1852;
                    DSTLYLs.SetValid(now);

                    double xx = DSTLYLp.Val / TGTSOGp.Val * 3600 / 1852;
                    if (xx > TimeSpan.MaxValue.TotalHours)
                    {
                        xx = TimeSpan.MaxValue.TotalHours - 1;
                    }
                    if (xx < TimeSpan.MinValue.TotalHours)
                    {
                        xx = TimeSpan.MinValue.TotalHours + 1;
                    }
                    TTGLYLp.Val = TimeSpan.FromSeconds(xx);
                    TTGLYLp.SetValid(now);

                    xx = DSTLYLs.Val / TGTSOGs.Val * 3600 / 1852;
                    if (xx > TimeSpan.MaxValue.TotalHours)
                    {
                        xx = TimeSpan.MaxValue.TotalHours - 1;
                    }
                    if (xx < TimeSpan.MinValue.TotalHours)
                    {
                        xx = TimeSpan.MinValue.TotalHours + 1;
                    }
                    TTGLYLs.Val = TimeSpan.FromSeconds(xx);
                    TTGLYLs.SetValid(now);
                }
            }
            else
            {
                if (TGTCOGp.IsValid())
                {
                    TGTCOGp.Invalidate();
                }
                if (TGTSOGp.IsValid())
                {
                    TGTSOGp.Invalidate();
                }
                if (TGTCOGs.IsValid())
                {
                    TGTCOGs.Invalidate();
                }
                if (TGTSOGs.IsValid())
                {
                    TGTSOGs.Invalidate();
                }
                if (DSTLYLp.IsValid())
                {
                    DSTLYLp.Invalidate();
                }
                if (DSTLYLs.IsValid())
                {
                    DSTLYLs.Invalidate();
                }
            }

            #endregion

            if (replayLog == true)
            {
                if (deltaLog == TimeSpan.Zero)
                {
                    //deltaLog = now - new DateTime(2020, 03, 15);
                    deltaLog = now - DateTime.Now;
                }


                now = now - deltaLog;
            }

            PushToLogDB(now);
        }