public void BuildPTAKSentences() { // Build PTAK Sentence **************************************************************************************** if (PERF.IsValid()) { string message = ""; switch (ptak_cntr) { case 0: message = "PTAK,FFD1," + TGTSPD.Val.ToString("0.0"); break; case 1: message = "PTAK,FFD2," + TGTTWA.Val.ToString("0") + "@"; break; case 2: double pf = PERF.Val * 100; message = "PTAK,FFD3," + pf.ToString("0"); break; case 3: message = "PTAK,FFD4," + TGTVMC.Val.ToString("0.0"); break; case 4: message = "PTAK,FFD5," + NTWA.FormattedValue + "@"; break; case 5: message = "PTAK,FFD6," + TGTCTS.Val.ToString("0") + "@"; break; } ptak_cntr++; if (ptak_cntr > 5) { ptak_cntr = 0; } int checksum = 0; foreach (char c in message) { checksum ^= Convert.ToByte(c); } message = "$" + message + "*" + checksum.ToString("X2") + "\r\n"; ptak_sentence = message; ptak_sentence_available = true; } }
/// <summary> 将Eclipse完井数据转换成SimON完井数据 </summary> void ConvertCompadat(WELLCTRL well, List <NAME> names, List <BaseKey> basekey, List <WPIMULT> wpimult, List <PERF> compTemps) { List <BaseKey> compdats = basekey.FindAll(l => l is COMPDAT).ToList(); //List<WELOPEN> welopen if (string.IsNullOrEmpty(well.WellName0) && compdats != null && compdats.Count > 0) { COMPDAT c = compdats.FirstOrDefault() as COMPDAT; well.WellName0 = c.Items.FirstOrDefault().jm0; } NAME name = names.Find(l => l.WellName == well.WellName0); // 添加完井数据 foreach (BaseKey c in basekey) { if (c is COMPDAT) { COMPDAT com = c as COMPDAT; foreach (COMPDAT.Item citem in com.Items) { if (citem.jm0 != well.WellName0) { continue; } #region - SCh数据 - PERF perf = new PERF("PERF"); perf.WellName = well.WellName0; perf.I0 = citem.i1; perf.J1 = citem.j2; perf.K12 = citem.swg3; perf.K23 = citem.xwg4; perf.Kgbs4 = citem.kgbz5; perf.Jzs6 = citem.ljyz7; perf.WjfxX7 = citem.skfx12 == "X" ? "DX" : "0"; perf.WjfxY8 = citem.skfx12 == "Y" ? "DY" : "0"; perf.WjfxZ9 = citem.skfx12 == "Z" ? "DZ" : "0"; perf.Bp10 = citem.bpxs10; // Todo :查找井指数乘子 foreach (WPIMULT wp in wpimult) { var v = wp.Items.Find(l => l.jm0 == well.WellName0); if (v != null) { perf.Jzscz5 = v.jzscz1; break; } } // Todo :增加前先删除存在的重复数据 well.DeleteAll <PERF>(l => l.I0 == perf.I0 && l.J1 == perf.J1 && l.K12 == perf.K12); well.Add(perf); #endregion #region - WELL数据 - NAME.Item nameItem = new NAME.Item(); nameItem.i0 = citem.i1; nameItem.j1 = citem.j2; nameItem.k12 = citem.swg3; nameItem.k23 = citem.xwg4; nameItem.kgbz4 = citem.kgbz5; //nameItem.wi5 = "NA";// v.Value.skin.Value.Value.ToString(); //nameItem.dx6 = v.Value.wellIndex.Value.GetValue(v.Value.wellIndex.GetUnitValue(_ecl)).ToString(); //nameItem.dy7 = v.Value.wellDirection.Value.Value == "X" ? "0" : v.Value.wellDirection.Value.Value == "Y" ? "1" : "2"; nameItem.bpxs9 = citem.bpxs10; nameItem.jj10 = (citem.jtnj8.ToDouble() / 2).ToString(); name.Items.Add(nameItem); #endregion // Todo :将当前时间点下 WELOPEN前增加到数据中 compTemps.Add(perf); } } else if (c is WELOPEN) { WELOPEN wp = c as WELOPEN; if (wp.Items == null || wp.Items.Count == 0) { continue; } var vs = wp.Items.FindAll(l => l.jm0 == well.WellName0 || l.jm0 == KeyConfiger.EclipseDefalt); if (vs == null || vs.Count == 0) { continue; } WELOPEN.Item v = vs.Last(); if (v == null) { continue; } // WELOPEN //'G13' 'SHUT' 0 0 0 2 * / // / // Todo :查找之前所有完井 var coms = compTemps.FindAll(l => l.WellName == well.WellName0); Predicate <PERF> match = l => true; // Todo :0 或 *表示默认值全都取 if (v.i2 != KeyConfiger.EclipseDefalt && v.i2 != "0") { match += l => l.I0 == v.i2; } if (v.j3 != KeyConfiger.EclipseDefalt && v.j3 != "0") { match += l => l.J1 == v.j3; } if (v.k4 != KeyConfiger.EclipseDefalt && v.k4 != "0") { match += l => l.K12 == v.k4; } var findComs = coms.FindAll(match); // Todo :增加WELOPEN控制的完井 foreach (var item in findComs) { PERF perf = item.Copy(); perf.Kgbs4 = v.jz1; // Todo :增加前先删除存在的重复数据 well.DeleteAll <PERF>(l => l.I0 == item.I0 && l.J1 == item.J1 && l.K12 == item.K12); well.Add(perf); } } } }
/// <summary> 将Eclipse生产数据转换成SimON生产数据 </summary> public SCHEDULE ConvertToSimON(SCHEDULE sch, WELL location, DateTime startTime, BaseFile history) { // Todo :保存SCH SCHEDULE schedule = new SCHEDULE("SCHEDULE"); List <string> wellNames = new List <string>(); List <WELSPECS> ws = sch.FindAll <WELSPECS>(); // Todo :查找所有井名 ws.ForEach(l => wellNames.AddRange(l.Items.Select(k => k.jm0))); List <NAME> histNames = new List <NAME>(); // Todo :初始化名称 生产_historyproduction.dat wellNames.ForEach(l => histNames.Add(new NAME("NAME") { WellName = l })); histNames.ForEach(l => history.Key.Add(l)); // Todo :初始化完井WELL数据 List <NAME> names = new List <NAME>(); wellNames.ForEach(l => names.Add(new NAME("NAME") { WellName = l })); names.ForEach(l => location.Add(l)); List <DATES> ds = sch.FindAll <DATES>(); string format = "井名:{0} ({1},{2})"; // Todo :添加起始信息到时间步 DATES start = new DATES("DATES", startTime); sch.DeleteAll <DATES>(); start.AddRange <BaseKey>(sch.Keys); ds.Insert(0, start); List <PERF> comAllTemp = new List <PERF>(); foreach (DATES d in ds) { // Todo :对缓存中完井井名去重复取最后一条 var distincts = comAllTemp.GroupBy(l => l.WellName + l.I0 + l.J1 + l.K12).ToList(); comAllTemp.Clear(); foreach (var item in distincts) { comAllTemp.Add(item.Last()); } // 创建SimON日期 TIME time = new TIME("TIME"); time.Date = d.DateTime; schedule.Add(time); var wconprod = d.FindAll <WCONPROD>(); var wconhist = d.FindAll <WCONHIST>(); var wconinje = d.FindAll <WCONINJE>(); var wconinjh = d.FindAll <WCONINJH>(); // 完井数据(考虑到排序) List <BaseKey> compdats = d.FindAll <BaseKey>(l => l is COMPDAT || l is WELOPEN); List <WPIMULT> wpimult = d.FindAll <WPIMULT>(); List <WELOPEN> welopen = d.FindAll <WELOPEN>(); #region - 添加没有生产信息的完井 - // 添加完井数据 foreach (BaseKey c in compdats) { if (c is COMPDAT) { COMPDAT com = c as COMPDAT; foreach (COMPDAT.Item citem in com.Items) { // Todo :过滤有生产数据的,用后面方法处理 if (wconprod.Exists(l => l.Items.Exists(k => k.jm0 == citem.jm0))) { continue; } if (wconhist.Exists(l => l.Items.Exists(k => k.wellName0 == citem.jm0))) { continue; } if (wconinje.Exists(l => l.Items.Exists(k => k.jm0 == citem.jm0))) { continue; } if (wconinjh.Exists(l => l.Items.Exists(k => k.jm0 == citem.jm0))) { continue; } WELLCTRL well = time.Find <WELLCTRL>(l => l.WellName0 == citem.jm0); if (well == null) { // Todo :创建一个空的生产信息 well = new WELLCTRL("WELLCTRL"); well.ProType = SimONProductType.NA; well.WellName0 = citem.jm0; time.Add(well); } NAME name = names.Find(l => l.WellName == well.WellName0); #region - SCh数据 - PERF perf = new PERF("PERF"); perf.WellName = well.WellName0; perf.I0 = citem.i1; perf.J1 = citem.j2; perf.K12 = citem.swg3; perf.K23 = citem.xwg4; perf.Kgbs4 = citem.kgbz5; perf.Jzs6 = citem.ljyz7; perf.WjfxX7 = citem.skfx12 == "X" ? "DX" : "0"; perf.WjfxY8 = citem.skfx12 == "Y" ? "DY" : "0"; perf.WjfxZ9 = citem.skfx12 == "Z" ? "DZ" : "0"; perf.Bp10 = citem.bpxs10; // Todo :查找井指数乘子 foreach (WPIMULT wp in wpimult) { var v = wp.Items.Find(l => l.jm0 == well.WellName0); if (v != null) { perf.Jzscz5 = v.jzscz1; break; } } // Todo :增加前先删除存在的重复数据 well.DeleteAll <PERF>(l => l.I0 == perf.I0 && l.J1 == perf.J1 && l.K12 == perf.K12); well.Add(perf); #endregion #region - WELL数据 - NAME.Item nameItem = new NAME.Item(); nameItem.i0 = citem.i1; nameItem.j1 = citem.j2; nameItem.k12 = citem.swg3; nameItem.k23 = citem.xwg4; nameItem.kgbz4 = citem.kgbz5; //nameItem.wi5 = "NA";// v.Value.skin.Value.Value.ToString(); //nameItem.dx6 = v.Value.wellIndex.Value.GetValue(v.Value.wellIndex.GetUnitValue(_ecl)).ToString(); //nameItem.dy7 = v.Value.wellDirection.Value.Value == "X" ? "0" : v.Value.wellDirection.Value.Value == "Y" ? "1" : "2"; nameItem.bpxs9 = citem.bpxs10; nameItem.jj10 = (citem.jtnj8.ToDouble() / 2).ToString(); name.Items.Add(nameItem); #endregion comAllTemp.Add(perf); } } else if (c is WELOPEN) { WELOPEN wp = c as WELOPEN; foreach (var v in wp.Items) { // Todo :过滤有生产数据的,用后面方法处理 if (wconprod.Exists(l => l.Items.Exists(k => k.jm0 == v.jm0))) { continue; } if (wconhist.Exists(l => l.Items.Exists(k => k.wellName0 == v.jm0))) { continue; } if (wconinje.Exists(l => l.Items.Exists(k => k.jm0 == v.jm0))) { continue; } if (wconinjh.Exists(l => l.Items.Exists(k => k.jm0 == v.jm0))) { continue; } // WELOPEN //'G13' 'SHUT' 0 0 0 2 * / // / // Todo :查找之前所有完井 var coms = comAllTemp.FindAll(l => l.WellName == v.jm0); Predicate <PERF> match = l => true; // Todo :0 或 *表示默认值全都取 if (v.i2 != KeyConfiger.EclipseDefalt && v.i2 != "0") { match += l => l.I0 == v.i2; } if (v.j3 != KeyConfiger.EclipseDefalt && v.j3 != "0") { match += l => l.J1 == v.j3; } if (v.k4 != KeyConfiger.EclipseDefalt && v.k4 != "0") { match += l => l.K12 == v.k4; } var findComs = coms.FindAll(match); WELLCTRL well = time.Find <WELLCTRL>(l => l.WellName0 == v.jm0); if (well == null) { // Todo :创建一个空的生产信息 well = new WELLCTRL("WELLCTRL"); well.ProType = SimONProductType.NA; well.WellName0 = v.jm0; time.Add(well); } // Todo :增加WELOPEN控制的完井 foreach (var fitem in findComs) { PERF perf = fitem.Copy(); perf.Kgbs4 = v.jz1; // Todo :增加前先删除存在的重复数据 well.DeleteAll <PERF>(l => l.I0 == fitem.I0 && l.J1 == fitem.J1 && l.K12 == fitem.K12); well.Add(perf); } } } //this.ConvertCompadat(well, names, compdats, wpimult, comAllTemp); } #endregion foreach (var item in wconprod) { foreach (WCONPROD.ItemHY it in item.Items) { // 生产数据 WELLCTRL well = new WELLCTRL("WELLCTRL"); well.WellName0 = it.jm0; well = this.ConvertToSimON(it, d, histNames); if (well != null) { this.ConvertCompadat(well, names, compdats, wpimult, comAllTemp); time.Add(well); } } } foreach (var item in wconhist) { foreach (WCONHIST.Item it in item.Items) { // 生产数据 WELLCTRL well = new WELLCTRL("WELLCTRL"); well.WellName0 = it.wellName0; well = this.ConvertToSimON(it, d, histNames); this.ConvertCompadat(well, names, compdats, wpimult, comAllTemp); time.Add(well); } } foreach (var item in wconinje) { foreach (WCONINJE.ItemHY it in item.Items) { // 生产数据 WELLCTRL well = new WELLCTRL("WELLCTRL"); well.WellName0 = it.jm0; well = this.ConvertToSimON(it, d, histNames); this.ConvertCompadat(well, names, compdats, wpimult, comAllTemp); time.Add(well); } } foreach (var item in wconinjh) { foreach (WCONINJH.Item it in item.Items) { // 生产数据 WELLCTRL well = new WELLCTRL("WELLCTRL"); well.WellName0 = it.jm0; well = this.ConvertToSimON(it, d, histNames); this.ConvertCompadat(well, names, compdats, wpimult, comAllTemp); time.Add(well); } } //// Todo :将之前的完井信息都加入到缓存中 //foreach (var item in compdats) //{ // comAllTemp.AddRange(item.Items); //} } return(schedule); }
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 }
public void SendPerformanceNMEA() { // Build PTAK Sentence **************************************************************************************** if (PERF.IsValid()) { string message1, message2, message3, message5, message6; message1 = "PTAK,FFD1," + TGTSPD.Average(Inst.BufFiveSec).ToString("0.0"); message2 = "PTAK,FFD2," + TGTTWA.Average(Inst.BufFiveSec).ToString("0") + "@"; double pf = PERF.Average(Inst.BufFiveSec) * 100; message3 = "PTAK,FFD3," + pf.ToString("0"); //message5 = "PTAK,FFD5," + TGTVMC.Average(Inst.BufFiveSec).ToString("0.0"); message5 = "PTAK,FFD5," + NTWA.FormattedValue + "@"; message6 = "PTAK,FFD6," + TGTCTS.Average(Inst.BufFiveSec).ToString("0") + "@"; int checksum = 0; checksum = 0; foreach (char c in message1) { checksum ^= Convert.ToByte(c); } message1 = "$" + message1 + "*" + checksum.ToString("X2") + "\r\n"; checksum = 0; foreach (char c in message2) { checksum ^= Convert.ToByte(c); } message2 = "$" + message2 + "*" + checksum.ToString("X2") + "\r\n"; checksum = 0; foreach (char c in message3) { checksum ^= Convert.ToByte(c); } message3 = "$" + message3 + "*" + checksum.ToString("X2") + "\r\n"; checksum = 0; foreach (char c in message5) { checksum ^= Convert.ToByte(c); } message5 = "$" + message5 + "*" + checksum.ToString("X2") + "\r\n"; checksum = 0; foreach (char c in message6) { checksum ^= Convert.ToByte(c); } message6 = "$" + message6 + "*" + checksum.ToString("X2") + "\r\n"; if (Properties.Settings.Default.TacktickPerformanceSentence.OutPort1) { if (SerialPort1.IsOpen) { SerialPort1.WriteLine(message1); SerialPort1.WriteLine(message2); SerialPort1.WriteLine(message3); SerialPort1.WriteLine(message5); SerialPort1.WriteLine(message6); } } if (Properties.Settings.Default.TacktickPerformanceSentence.OutPort2) { if (SerialPort2.IsOpen) { SerialPort2.WriteLine(message1); SerialPort2.WriteLine(message2); SerialPort2.WriteLine(message3); SerialPort2.WriteLine(message5); SerialPort2.WriteLine(message6); } } if (Properties.Settings.Default.TacktickPerformanceSentence.OutPort3) { if (SerialPort3.IsOpen) { SerialPort3.WriteLine(message1); SerialPort3.WriteLine(message2); SerialPort3.WriteLine(message3); SerialPort3.WriteLine(message5); SerialPort3.WriteLine(message6); } } if (Properties.Settings.Default.TacktickPerformanceSentence.OutPort4) { if (SerialPort4.IsOpen) { SerialPort4.WriteLine(message1); SerialPort4.WriteLine(message2); SerialPort4.WriteLine(message3); SerialPort4.WriteLine(message5); SerialPort4.WriteLine(message6); } } } }
public void PushToLogDB(DateTime dt) { if (POS.IsValid()) { POS.PushToBuffer(POS.Val, dt, 0); COG.PushToBuffer(COG.Val, dt, 0); SOG.PushToBuffer(SOG.Val, dt, 0); HDT.PushToBuffer(HDT.Val, dt, 0); TWD.PushToBuffer(TWD.Val, dt, 0); PERF.PushToBuffer(PERF.Val, dt, 0); DPT.PushToBuffer(DPT.Val, dt, 0); TWS.PushToBuffer(TWS.Val, dt, 0); DRIFT.PushToBuffer(DRIFT.Val, dt, 0); SET.PushToBuffer(SET.Val, dt, 0); SPD.PushToBuffer(SPD.Val, dt, 0); using (var context = new LionRiverDBContext()) { for (int i = 0; i < Inst.MaxBuffers; i++) { if (POS.AvgBufferDataAvailable(i)) { if (POS.GetLastVal(i) != null) { var log = new Log() { timestamp = POS.GetLastVal(i).Time, level = i, LAT = POS.GetLastVal(i).Val.Latitude, LON = POS.GetLastVal(i).Val.Longitude, COG = COG.GetLastVal(i).Val, SOG = SOG.GetLastVal(i).Val, HDT = HDT.GetLastVal(i).Val, TWD = TWD.GetLastVal(i).Val, PERF = PERF.GetLastVal(i).Val, DPT = DPT.GetLastVal(i).Val, TWS = TWS.GetLastVal(i).Val, DRIFT = DRIFT.GetLastVal(i).Val, SET = SET.GetLastVal(i).Val, SPD = SPD.GetLastVal(i).Val }; context.Logs.Add(log); if (i == NavPlotModel.Resolution) { newTrackPositionAvailable = true; } POS.ClearAvgBufferDataAvailable(i); COG.ClearAvgBufferDataAvailable(i); SOG.ClearAvgBufferDataAvailable(i); HDT.ClearAvgBufferDataAvailable(i); TWD.ClearAvgBufferDataAvailable(i); PERF.ClearAvgBufferDataAvailable(i); DPT.ClearAvgBufferDataAvailable(i); TWS.ClearAvgBufferDataAvailable(i); DRIFT.ClearAvgBufferDataAvailable(i); SET.ClearAvgBufferDataAvailable(i); SPD.ClearAvgBufferDataAvailable(i); } } } context.SaveChanges(); } } }
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); }