Ejemplo n.º 1
0
        public void CalcNav(DateTime now, bool bypassComm = false)
        {
            #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.IsValid();
                    DST.IsValid();
                }
            }

            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())
            {
                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);
            }
            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 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())
            {
                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);
                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 (BRG.IsValid() && TWD.IsValid() && SPD.IsValid() && NavPolar.IsLoaded)
            {
                double Angle = Math.Abs((TWD.Val - BRG.Val) % 360);
                if (Angle > 180)
                {
                    Angle = 360 - Angle;
                }

                PolarPoint pb = NavPolar.GetBeatTarget(TWS.Average(Inst.BufHalfMin));
                PolarPoint pr = NavPolar.GetRunTarget(TWS.Average(Inst.BufHalfMin));

                if (Math.Abs(Angle) <= pb.TWA) // 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 (Math.Abs(Angle) < pr.TWA && Math.Abs(Angle) > pb.TWA) // Reaching
                {
                    TGTSPD.Val = NavPolar.GetTarget(Math.Abs(TWA.Average(Inst.BufHalfMin)), TWS.Average(Inst.BufHalfMin));
                    TGTSPD.SetValid(now);
                    TGTTWA.Val = Math.Abs(TWA.Val);
                    TGTTWA.SetValid(now);
                    PERF.Val = SPD.Val / TGTSPD.Val;
                    if (VMGWPT.Val < 0)
                    {
                        PERF.Val = -PERF.Val;
                    }
                    PERF.SetValid(now);

                    sailingMode = SailingMode.Reaching;
                }

                if (Math.Abs(Angle) >= pr.TWA) // 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();
                }

                sailingMode = SailingMode.None;
            }
            #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 (!bypassComm)
            {
                if (ActiveMark != null && DST.IsValid() && !ManOverBoard)
                {
                    if (DST.Val <= Properties.Settings.Default.WptProximity)
                    {
                        (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.Average(Inst.BufTwoMin) - ActiveLeg.NextLeg.Bearing;
                    NTWA.SetValid();
                }
                else
                {
                    NTWA.Invalidate();
                }
            }

            #endregion

            #region Laylines

            //if (DRIFT.IsValid() && PERF.IsValid() && TWD.IsValid())
            //{
            //    double relset = SET.Average(Inst.BufTenMin) - TWD.Average(Inst.BufHalfMin);
            //    double dxs = TGTSPD.Average(Inst.BufHalfMin) * Math.Cos(TGTTWA.Average(Inst.BufHalfMin) * Math.PI / 180) + DRIFT.Average(Inst.BufTenMin) * Math.Cos(relset * Math.PI / 180);
            //    double dys = TGTSPD.Average(Inst.BufHalfMin) * Math.Sin(TGTTWA.Average(Inst.BufHalfMin) * Math.PI / 180) + DRIFT.Average(Inst.BufTenMin) * Math.Sin(relset * Math.PI / 180);

            //    TGTCOGs.Val = Math.Atan2(dys, dxs) * 180 / Math.PI + TWD.Average(Inst.BufHalfMin);
            //    TGTCOGs.SetValid(now);
            //    TGTSOGs.Val = Math.Sqrt(dxs * dxs + dys * dys);
            //    TGTSOGs.SetValid(now);

            //    double dxp = TGTSPD.Average(Inst.BufHalfMin) * Math.Cos(-TGTTWA.Average(Inst.BufHalfMin) * Math.PI / 180) + DRIFT.Average(Inst.BufTenMin) * Math.Cos(relset * Math.PI / 180);
            //    double dyp = TGTSPD.Average(Inst.BufHalfMin) * Math.Sin(-TGTTWA.Average(Inst.BufHalfMin) * Math.PI / 180) + DRIFT.Average(Inst.BufTenMin) * Math.Sin(relset * Math.PI / 180);

            //    TGTCOGp.Val = Math.Atan2(dyp, dxp) * 180 / Math.PI + TWD.Average(Inst.BufHalfMin);
            //    TGTCOGp.SetValid(now);
            //    TGTSOGp.Val = Math.Sqrt(dxp * dxp + dyp * dyp);
            //    TGTSOGp.SetValid(now);
            //}
            //else
            //{
            //    if (TGTCOGs.IsValid())
            //        TGTCOGs.Invalidate();
            //    if (TGTSOGs.IsValid())
            //        TGTSOGs.Invalidate();
            //    if (TGTCOGp.IsValid())
            //        TGTCOGp.Invalidate();
            //    if (TGTSOGp.IsValid())
            //        TGTSOGp.Invalidate();
            //}

            #endregion
        }
Ejemplo n.º 2
0
        public void SendNMEA()
        {
            string message;

            // Build HDG Sentence ****************************************************************************************

            if (HDT.IsValid())  // Implies MVAR is valid too
            {
                string mv;

                if (MVAR.Val > 0)
                {
                    mv = "E";
                }
                else
                {
                    mv = "W";
                }

                double hdg = (HDT.Val - MVAR.Val + 360) % 360;

                message = "IIHDG," + hdg.ToString("0.#") + ",,," + Math.Abs(MVAR.Val).ToString("0.#") + "," + mv;

                int checksum = 0;

                foreach (char c in message)
                {
                    checksum ^= Convert.ToByte(c);
                }

                message = "$" + message + "*" + checksum.ToString("X2") + "\r\n";

                if (Properties.Settings.Default.HeadingSentence.OutPort1)
                {
                    if (SerialPort1.IsOpen)
                    {
                        SerialPort1.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.HeadingSentence.OutPort2)
                {
                    if (SerialPort2.IsOpen)
                    {
                        SerialPort2.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.HeadingSentence.OutPort3)
                {
                    if (SerialPort3.IsOpen)
                    {
                        SerialPort3.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.HeadingSentence.OutPort4)
                {
                    if (SerialPort4.IsOpen)
                    {
                        SerialPort4.WriteLine(message);
                    }
                }
            }

            // Build MWV Sentence ****************************************************************************************

            if (AWA.IsValid())
            {
                message = "IIMWV," + ((AWA.Val + 360) % 360).ToString("0") + ",R," + AWS.Val.ToString("0.#") + ",N,A";

                int checksum = 0;

                foreach (char c in message)
                {
                    checksum ^= Convert.ToByte(c);
                }

                message = "$" + message + "*" + checksum.ToString("X2") + "\r\n";

                if (Properties.Settings.Default.AppWindSentence.OutPort1)
                {
                    if (SerialPort1.IsOpen)
                    {
                        SerialPort1.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.AppWindSentence.OutPort2)
                {
                    if (SerialPort2.IsOpen)
                    {
                        SerialPort2.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.AppWindSentence.OutPort3)
                {
                    if (SerialPort3.IsOpen)
                    {
                        SerialPort3.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.AppWindSentence.OutPort4)
                {
                    if (SerialPort4.IsOpen)
                    {
                        SerialPort4.WriteLine(message);
                    }
                }
            }

            // Build VHW Sentence ****************************************************************************************

            if (SPD.IsValid())
            {
                string hdg;
                if (HDT.IsValid())
                {
                    hdg = HDT.Val.ToString("0") + ",T,,M,";
                }
                else
                {
                    hdg = ",T,,M,";
                }

                message = "IIVHW," + hdg + SPD.Val.ToString("0.##") + ",N,,K";

                int checksum = 0;

                foreach (char c in message)
                {
                    checksum ^= Convert.ToByte(c);
                }

                message = "$" + message + "*" + checksum.ToString("X2") + "\r\n";

                if (Properties.Settings.Default.HullSpeedSentence.OutPort1)
                {
                    if (SerialPort1.IsOpen)
                    {
                        SerialPort1.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.HullSpeedSentence.OutPort2)
                {
                    if (SerialPort2.IsOpen)
                    {
                        SerialPort2.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.HullSpeedSentence.OutPort3)
                {
                    if (SerialPort3.IsOpen)
                    {
                        SerialPort3.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.HullSpeedSentence.OutPort4)
                {
                    if (SerialPort4.IsOpen)
                    {
                        SerialPort4.WriteLine(message);
                    }
                }
            }

            // Build DPT Sentence ****************************************************************************************

            if (DPT.IsValid())
            {
                message = "IIDPT," + DPT.Val.ToString("0.#") + ",0";

                int checksum = 0;

                foreach (char c in message)
                {
                    checksum ^= Convert.ToByte(c);
                }

                message = "$" + message + "*" + checksum.ToString("X2") + "\r\n";

                if (Properties.Settings.Default.DepthSentence.OutPort1)
                {
                    if (SerialPort1.IsOpen)
                    {
                        SerialPort1.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.DepthSentence.OutPort2)
                {
                    if (SerialPort2.IsOpen)
                    {
                        SerialPort2.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.DepthSentence.OutPort3)
                {
                    if (SerialPort3.IsOpen)
                    {
                        SerialPort3.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.DepthSentence.OutPort4)
                {
                    if (SerialPort4.IsOpen)
                    {
                        SerialPort4.WriteLine(message);
                    }
                }
            }

            // Build RMC Sentence ****************************************************************************************

            if (COG.IsValid())   // Implies SOG, LAT and LON are also valid
            {
                DateTime UTC = DateTime.UtcNow;

                string hms  = UTC.Hour.ToString("00") + UTC.Minute.ToString("00") + UTC.Second.ToString("00");
                string date = UTC.Date.Day.ToString("00") + UTC.Date.Month.ToString("00") + UTC.Date.Year.ToString().Substring(2, 2);

                double deg, min;
                string cd;

                deg = Math.Abs(Math.Truncate(LAT.Val));
                min = (Math.Abs(LAT.Val) - deg) * 60;

                if (LAT.Val > 0)
                {
                    cd = "N";
                }
                else
                {
                    cd = "S";
                }

                string lat = deg.ToString("000") + min.ToString("00.###") + "," + cd;

                deg = Math.Abs(Math.Truncate(LON.Val));
                min = (Math.Abs(LON.Val) - deg) * 60;

                if (LON.Val > 0)
                {
                    cd = "E";
                }
                else
                {
                    cd = "W";
                }

                string lon = deg.ToString("000") + min.ToString("00.###") + "," + cd;

                if (MVAR.Val > 0)
                {
                    cd = "E";
                }
                else
                {
                    cd = "W";
                }

                double cog = (COG.Val + 360) % 360;

                message = "IIRMC," + hms + ",A," + lat + "," + lon + "," + SOG.Val.ToString("#.##") + "," + cog.ToString("0.#") + ","
                          + date + "," + Math.Abs(MVAR.Val).ToString("0.#") + "," + cd + ",A";

                int checksum = 0;

                foreach (char c in message)
                {
                    checksum ^= Convert.ToByte(c);
                }

                message = "$" + message + "*" + checksum.ToString("X2") + "\r\n";

                if (Properties.Settings.Default.NavSentence.OutPort1)
                {
                    if (SerialPort1.IsOpen)
                    {
                        SerialPort1.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.NavSentence.OutPort2)
                {
                    if (SerialPort2.IsOpen)
                    {
                        SerialPort2.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.NavSentence.OutPort3)
                {
                    if (SerialPort3.IsOpen)
                    {
                        SerialPort3.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.NavSentence.OutPort4)
                {
                    if (SerialPort4.IsOpen)
                    {
                        SerialPort4.WriteLine(message);
                    }
                }
            }

            // Build RMB Sentence ****************************************************************************************

            if (WPT.IsValid())   // Implies BRG and DST are also valid
            {
                string xte  = ",,";
                string owpt = ",";

                if (XTE.IsValid())
                {
                    if (XTE.Val > 0)
                    {
                        xte = XTE.Val.ToString("0.##") + ",R,";
                    }
                    else
                    {
                        xte = Math.Abs(XTE.Val).ToString("0.##") + ",L,";
                    }
                    owpt = LWPT.FormattedValue + ",";
                }

                double brg = (BRG.Val + 360) % 360;

                message = "IIRMB,A," + xte + owpt + WPT.FormattedValue + ",,,,," + DST.Val.ToString("0.##") + "," + brg.ToString("0.#")
                          + "," + VMGWPT.Val.ToString("0.##") + ",,A";

                int checksum = 0;

                foreach (char c in message)
                {
                    checksum ^= Convert.ToByte(c);
                }

                message = "$" + message + "*" + checksum.ToString("X2") + "\r\n";

                if (Properties.Settings.Default.RouteSentence.OutPort1)
                {
                    if (SerialPort1.IsOpen)
                    {
                        SerialPort1.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.RouteSentence.OutPort2)
                {
                    if (SerialPort2.IsOpen)
                    {
                        SerialPort2.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.RouteSentence.OutPort3)
                {
                    if (SerialPort3.IsOpen)
                    {
                        SerialPort3.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.RouteSentence.OutPort4)
                {
                    if (SerialPort4.IsOpen)
                    {
                        SerialPort4.WriteLine(message);
                    }
                }
            }

            // Build MTW Sentence ****************************************************************************************

            if (TEMP.IsValid())
            {
                message = "IIMTW," + TEMP.Val.ToString("0.#") + ",C";

                int checksum = 0;

                foreach (char c in message)
                {
                    checksum ^= Convert.ToByte(c);
                }

                message = "$" + message + "*" + checksum.ToString("X2") + "\r\n";

                if (Properties.Settings.Default.WaterTempSentence.OutPort1)
                {
                    if (SerialPort1.IsOpen)
                    {
                        SerialPort1.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.WaterTempSentence.OutPort2)
                {
                    if (SerialPort2.IsOpen)
                    {
                        SerialPort2.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.WaterTempSentence.OutPort3)
                {
                    if (SerialPort3.IsOpen)
                    {
                        SerialPort3.WriteLine(message);
                    }
                }
                if (Properties.Settings.Default.WaterTempSentence.OutPort4)
                {
                    if (SerialPort4.IsOpen)
                    {
                        SerialPort4.WriteLine(message);
                    }
                }
            }

            // Build PTAK4 Sentence ****************************************************************************************

            if (LINEDST.IsValid())
            {
                string message4;

                message4 = "PTAK,FFD4," + LINEDST.Val.ToString("0");

                int checksum = 0;

                foreach (char c in message4)
                {
                    checksum ^= Convert.ToByte(c);
                }
                message4 = "$" + message4 + "*" + checksum.ToString("X2") + "\r\n";

                if (Properties.Settings.Default.TacktickPerformanceSentence.OutPort1)
                {
                    if (SerialPort1.IsOpen)
                    {
                        SerialPort1.WriteLine(message4);
                    }
                }
                if (Properties.Settings.Default.TacktickPerformanceSentence.OutPort2)
                {
                    if (SerialPort2.IsOpen)
                    {
                        SerialPort2.WriteLine(message4);
                    }
                }
                if (Properties.Settings.Default.TacktickPerformanceSentence.OutPort3)
                {
                    if (SerialPort3.IsOpen)
                    {
                        SerialPort3.WriteLine(message4);
                    }
                }
                if (Properties.Settings.Default.TacktickPerformanceSentence.OutPort4)
                {
                    if (SerialPort4.IsOpen)
                    {
                        SerialPort4.WriteLine(message4);
                    }
                }
            }
        }
Ejemplo n.º 3
0
        public void BuildNMEASentences()
        {
            string message;

            // #CommRework - All primitive NMEA sentences are relayed inmeditely

            //// Build HDG Sentence ****************************************************************************************

            //if (HDT.IsValid())  // Implies MVAR is valid too
            //{

            //    string mv;

            //    if (MVAR.Val > 0)
            //        mv = "E";
            //    else
            //        mv = "W";

            //    double hdg = (HDT.Val - MVAR.Val + 360) % 360;

            //    message = "IIHDG," + hdg.ToString("0.#") + ",,," + Math.Abs(MVAR.Val).ToString("0.#") + "," + mv;

            //    int checksum = 0;

            //    foreach (char c in message)
            //        checksum ^= Convert.ToByte(c);

            //    message = "$" + message + "*" + checksum.ToString("X2") + "\r\n";

            //    if (Properties.Settings.Default.HeadingSentence.OutPort1)
            //        if (SerialPort1.IsOpen)
            //            WriteSerial(1,message);
            //    if (Properties.Settings.Default.HeadingSentence.OutPort2)
            //        if (SerialPort2.IsOpen)
            //            WriteSerial(2,message);
            //    if (Properties.Settings.Default.HeadingSentence.OutPort3)
            //        if (SerialPort3.IsOpen)
            //            WriteSerial(3,message);
            //    if (Properties.Settings.Default.HeadingSentence.OutPort4)
            //        if (SerialPort4.IsOpen)
            //            WriteSerial(4,message);
            //}

            //// Build MWV Sentence ****************************************************************************************

            //if (AWA.IsValid())
            //{

            //    message = "IIMWV," + ((AWA.Val+360)%360).ToString("0") + ",R," + AWS.Val.ToString("0.#") + ",N,A";

            //    int checksum = 0;

            //    foreach (char c in message)
            //        checksum ^= Convert.ToByte(c);

            //    message = "$" + message + "*" + checksum.ToString("X2") + "\r\n";

            //    if (Properties.Settings.Default.AppWindSentence.OutPort1)
            //        if(SerialPort1.IsOpen)
            //            WriteSerial(1,message);
            //    if (Properties.Settings.Default.AppWindSentence.OutPort2)
            //        if (SerialPort2.IsOpen)
            //            WriteSerial(2,message);
            //    if (Properties.Settings.Default.AppWindSentence.OutPort3)
            //        if (SerialPort3.IsOpen)
            //            WriteSerial(3,message);
            //    if (Properties.Settings.Default.AppWindSentence.OutPort4)
            //        if (SerialPort4.IsOpen)
            //            WriteSerial(4,message);
            //}

            //// Build VHW Sentence ****************************************************************************************

            //if (SPD.IsValid())
            //{
            //    string hdg;
            //    if (HDT.IsValid())
            //        hdg = HDT.Val.ToString("0") + ",T,,M,";
            //    else
            //        hdg = ",T,,M,";

            //    message = "IIVHW," + hdg + SPD.Val.ToString("0.##") + ",N,,K";

            //    int checksum = 0;

            //    foreach (char c in message)
            //        checksum ^= Convert.ToByte(c);

            //    message = "$" + message + "*" + checksum.ToString("X2") + "\r\n";

            //    if (Properties.Settings.Default.HullSpeedSentence.OutPort1)
            //        if (SerialPort1.IsOpen)
            //            WriteSerial(1,message);
            //    if (Properties.Settings.Default.HullSpeedSentence.OutPort2)
            //        if (SerialPort2.IsOpen)
            //            WriteSerial(2,message);
            //    if (Properties.Settings.Default.HullSpeedSentence.OutPort3)
            //        if (SerialPort3.IsOpen)
            //            WriteSerial(3,message);
            //    if (Properties.Settings.Default.HullSpeedSentence.OutPort4)
            //        if (SerialPort4.IsOpen)
            //            WriteSerial(4,message);
            //}

            //// Build DPT Sentence ****************************************************************************************

            //if (DPT.IsValid())
            //{

            //    message = "IIDPT,"+DPT.Val.ToString("0.#")+",0";
            //    //message = "IIDPT,25.4,0";

            //    int checksum = 0;

            //    foreach (char c in message)
            //        checksum ^= Convert.ToByte(c);

            //    message = "$" + message + "*" + checksum.ToString("X2") + "\r\n";

            //    if (Properties.Settings.Default.DepthSentence.OutPort1)
            //        if (SerialPort1.IsOpen)
            //            WriteSerial(1,message);
            //    if (Properties.Settings.Default.DepthSentence.OutPort2)
            //        if (SerialPort2.IsOpen)
            //            WriteSerial(2,message);
            //    if (Properties.Settings.Default.DepthSentence.OutPort3)
            //        if (SerialPort3.IsOpen)
            //            WriteSerial(3,message);
            //    if (Properties.Settings.Default.DepthSentence.OutPort4)
            //        if (SerialPort4.IsOpen)
            //            WriteSerial(4,message);
            //}

            //// Build RMC Sentence ****************************************************************************************

            //if (COG.IsValid())   // Implies SOG, LAT and LON are also valid
            //{

            //    DateTime UTC = DateTime.UtcNow;

            //    string hms = UTC.Hour.ToString("00") + UTC.Minute.ToString("00") + UTC.Second.ToString("00");
            //    string date = UTC.Date.Day.ToString("00") + UTC.Date.Month.ToString("00") + UTC.Date.Year.ToString().Substring(2, 2);

            //    double deg, min;
            //    string cd;

            //    deg = Math.Abs(Math.Truncate(LAT.Val));
            //    min = (Math.Abs(LAT.Val) - deg) * 60;

            //    if (LAT.Val > 0)
            //        cd = "N";
            //    else
            //        cd = "S";

            //    string lat = deg.ToString("#")+min.ToString("00.####")+","+cd;

            //    deg = Math.Abs(Math.Truncate(LON.Val));
            //    min = (Math.Abs(LON.Val) - deg) * 60;

            //    if (LON.Val > 0)
            //        cd = "E";
            //    else
            //        cd = "W";

            //    string lon = deg.ToString("#")+min.ToString("00.####")+","+cd;

            //    if (MVAR.Val > 0)
            //        cd = "E";
            //    else
            //        cd = "W";

            //    double cog = (COG.Val + 360) % 360;

            //    message = "IIRMC," + hms + ",A," + lat + "," + lon + "," + SOG.Val.ToString("#.##") + "," + cog.ToString("0.#") + ","
            //        + date + "," + Math.Abs(MVAR.Val).ToString("0.#") + "," + cd + ",A";

            //    int checksum = 0;

            //    foreach (char c in message)
            //        checksum ^= Convert.ToByte(c);

            //    message = "$" + message + "*" + checksum.ToString("X2") + "\r\n";

            //    //message = "$GPRMC,173933,A,3430.6759,S,05828.3633,W,000.1,173.3,291220,008.1,W*68\r\n";

            //    if (Properties.Settings.Default.NavSentence.OutPort1)
            //        if (SerialPort1.IsOpen)
            //            WriteSerial(1,message);
            //    if (Properties.Settings.Default.NavSentence.OutPort2)
            //        if (SerialPort2.IsOpen)
            //            WriteSerial(2,message);
            //    if (Properties.Settings.Default.NavSentence.OutPort3)
            //        if (SerialPort3.IsOpen)
            //            WriteSerial(3,message);
            //    if (Properties.Settings.Default.NavSentence.OutPort4)
            //        if (SerialPort4.IsOpen)
            //            WriteSerial(4,message);
            //}

            //// Build MTW Sentence ****************************************************************************************

            //if (TEMP.IsValid())
            //{

            //    message = "IIMTW," + TEMP.Val.ToString("0.#") + ",C";

            //    int checksum = 0;

            //    foreach (char c in message)
            //        checksum ^= Convert.ToByte(c);

            //    message = "$" + message + "*" + checksum.ToString("X2") + "\r\n";

            //    if (Properties.Settings.Default.WaterTempSentence.OutPort1)
            //        if (SerialPort1.IsOpen)
            //            WriteSerial(1,message);
            //    if (Properties.Settings.Default.WaterTempSentence.OutPort2)
            //        if (SerialPort2.IsOpen)
            //            WriteSerial(2,message);
            //    if (Properties.Settings.Default.WaterTempSentence.OutPort3)
            //        if (SerialPort3.IsOpen)
            //            WriteSerial(3,message);
            //    if (Properties.Settings.Default.WaterTempSentence.OutPort4)
            //        if (SerialPort4.IsOpen)
            //            WriteSerial(4,message);
            //}

            // Build RMB Sentence ****************************************************************************************

            if (WPT.IsValid())   // Implies BRG and DST are also valid
            {
                string xte  = ",,";
                string owpt = ",";

                if (XTE.IsValid())
                {
                    if (XTE.Val > 0)
                    {
                        xte = XTE.Val.ToString("0.##") + ",R,";
                    }
                    else
                    {
                        xte = Math.Abs(XTE.Val).ToString("0.##") + ",L,";
                    }
                    owpt = LWPT.FormattedValue + ",";
                }

                double brg = (BRG.Val + 360) % 360;

                message = "IIRMB,A," + xte + owpt + WPT.FormattedValue + ",,,,," + DST.Val.ToString("0.##") + "," + brg.ToString("0.#")
                          + "," + VMGWPT.Val.ToString("0.##") + ",,A";

                int checksum = 0;

                foreach (char c in message)
                {
                    checksum ^= Convert.ToByte(c);
                }

                message = "$" + message + "*" + checksum.ToString("X2") + "\r\n";

                rmb_sentence           = message;
                rmb_sentence_available = true;
            }

            // Build PTAK4 Sentence ****************************************************************************************

            if (LINEDST.IsValid())
            {
                string message4;

                message4 = "PTAK,FFD4," + LINEDST.Val.ToString("0");

                int checksum = 0;

                foreach (char c in message4)
                {
                    checksum ^= Convert.ToByte(c);
                }
                message4 = "$" + message4 + "*" + checksum.ToString("X2") + "\r\n";

                if (Properties.Settings.Default.TacktickPerformanceSentence.OutPort1)
                {
                    if (SerialPort1.IsOpen)
                    {
                        SerialPort1.WriteLine(message4);
                    }
                }
                if (Properties.Settings.Default.TacktickPerformanceSentence.OutPort2)
                {
                    if (SerialPort2.IsOpen)
                    {
                        SerialPort2.WriteLine(message4);
                    }
                }
                if (Properties.Settings.Default.TacktickPerformanceSentence.OutPort3)
                {
                    if (SerialPort3.IsOpen)
                    {
                        SerialPort3.WriteLine(message4);
                    }
                }
                if (Properties.Settings.Default.TacktickPerformanceSentence.OutPort4)
                {
                    if (SerialPort4.IsOpen)
                    {
                        SerialPort4.WriteLine(message4);
                    }
                }
            }
        }
Ejemplo n.º 4
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);
        }