Skip to content

Commit 5717f79

Browse files
AlekUnderwaterAlekUnderwater
authored
Add files via upload
1 parent b678279 commit 5717f79

File tree

11 files changed

+1791
-0
lines changed

11 files changed

+1791
-0
lines changed

‎uTrackLib/DiversTOAProcessor.cs‎

Lines changed: 296 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,296 @@
1+
using System;
2+
using System.Collections.Generic;
3+
using UCNLDrivers;
4+
using UCNLNav;
5+
using UCNLPhysics;
6+
7+
namespace uTrackLib
8+
{
9+
public class RemoteDescriptor
10+
{
11+
#region Properties
12+
13+
public int ID;
14+
public AgingValue<double> Latitude;
15+
public AgingValue<double> Longitude;
16+
public AgingValue<double> RadialError;
17+
public AgingValue<double> Distance;
18+
public AgingValue<double> ForwardAzimuth;
19+
public AgingValue<double> ReverseAzimuth;
20+
public AgingValue<bool> IsRERRExceed;
21+
public AgingValue<DOPState> DOP;
22+
public AgingValue<TBAQuality> TBA;
23+
24+
#endregion
25+
26+
#region Constructor
27+
28+
public RemoteDescriptor(int id)
29+
{
30+
ID = id;
31+
Latitude = new AgingValue<double>(10, 600, RWLT.LatLonFormatter);
32+
Longitude = new AgingValue<double>(10, 600, RWLT.LatLonFormatter);
33+
RadialError = new AgingValue<double>(10, 600, RWLT.DptDstFormatter);
34+
Distance = new AgingValue<double>(10, 600, RWLT.DptDstFormatter);
35+
ForwardAzimuth = new AgingValue<double>(10, 600, RWLT.CourseFormatter);
36+
ReverseAzimuth = new AgingValue<double>(10, 600, RWLT.CourseFormatter);
37+
DOP = new AgingValue<DOPState>(10, 600, x => x.ToString());
38+
TBA = new AgingValue<TBAQuality>(10, 600, x => x.ToString().Replace('_', ' '));
39+
IsRERRExceed = new AgingValue<bool>(10, 600, x => x.ToString());
40+
}
41+
42+
#endregion
43+
44+
#region Methods
45+
46+
public Dictionary<string, string> GetDescription()
47+
{
48+
return GetDescription(true, true, true, true, true, true);
49+
}
50+
51+
public Dictionary<string, string> GetDescription(bool isDst, bool isAzm, bool isRaz, bool isLatLon, bool isRer, bool isDopTba)
52+
{
53+
Dictionary<string, string> result = new Dictionary<string, string>();
54+
55+
if (Distance.IsInitialized && isDst)
56+
result.Add("DST", Distance.ToString());
57+
if (ForwardAzimuth.IsInitialized && isAzm)
58+
result.Add("AZM", ForwardAzimuth.ToString());
59+
if (ReverseAzimuth.IsInitialized && isRaz)
60+
result.Add("RAZ", ReverseAzimuth.ToString());
61+
62+
if (isLatLon)
63+
{
64+
if (Latitude.IsInitialized)
65+
result.Add("LAT", Latitude.ToString());
66+
if (Longitude.IsInitialized)
67+
result.Add("LON", Longitude.ToString());
68+
}
69+
70+
if (RadialError.IsInitialized && isRer)
71+
result.Add("RER", RadialError.ToString());
72+
73+
if (IsRERRExceed.IsInitialized && IsRERRExceed.Value)
74+
result.Add("RERE", IsRERRExceed.Value.ToString());
75+
76+
if (isDopTba)
77+
{
78+
if (DOP.IsInitialized)
79+
result.Add("DOP", DOP.ToString());
80+
if (TBA.IsInitialized)
81+
result.Add("TBA", TBA.ToString());
82+
}
83+
84+
return result;
85+
}
86+
87+
#endregion
88+
}
89+
90+
public class DiversTOAProcessor : ITOAProcessor
91+
{
92+
#region Properties
93+
94+
Dictionary<int, RWLT_BProc> bProcs;
95+
Dictionary<int, PCore2D<GeoPoint3DT>> estimators;
96+
Dictionary<int, RemoteDescriptor> remotes;
97+
98+
double soundSpeed = PHX.PHX_FWTR_SOUND_SPEED_MPS;
99+
public double SoundSpeed_mps
100+
{
101+
get { return soundSpeed; }
102+
set
103+
{
104+
if ((value >= PHX.PHX_FWTR_SOUND_SPEED_MPS_MIN) && (value <= PHX.PHX_FWTR_SOUND_SPEED_MPS_MAX))
105+
{
106+
soundSpeed = value;
107+
foreach (var item in estimators)
108+
{
109+
item.Value.SoundSpeed = soundSpeed;
110+
}
111+
}
112+
else
113+
throw new ArgumentOutOfRangeException("value",
114+
string.Format("must be in the a range from {0} to {1} m/s",
115+
PHX.PHX_FWTR_SOUND_SPEED_MPS_MIN,
116+
PHX.PHX_FWTR_SOUND_SPEED_MPS_MAX));
117+
}
118+
119+
}
120+
121+
double gravityAcc = PHX.PHX_GRAVITY_ACC_MPS2;
122+
public double GravityAcceleration
123+
{
124+
get { return gravityAcc; }
125+
set
126+
{
127+
if ((value >= PHX.PHX_GRAVITY_ACC_MPS2_MIN) && (value <= PHX.PHX_GRAVITY_ACC_MPS2_MAX))
128+
gravityAcc = value;
129+
else
130+
throw new ArgumentOutOfRangeException("value",
131+
string.Format("must be in the a range from {0} to {1} m/s²",
132+
PHX.PHX_GRAVITY_ACC_MPS2_MIN,
133+
PHX.PHX_GRAVITY_ACC_MPS2_MAX));
134+
}
135+
}
136+
137+
public string Moniker { get; set; }
138+
139+
public double RadialErrorThreshold { get; private set; }
140+
public double SimplexSize { get; private set; }
141+
142+
#endregion
143+
144+
#region Constructor
145+
146+
public DiversTOAProcessor(double rErrThreshold, double smplxSize)
147+
{
148+
if (rErrThreshold <= 0)
149+
throw new ArgumentOutOfRangeException("rErrThreshold should be greater than zero");
150+
151+
RadialErrorThreshold = rErrThreshold;
152+
153+
if (smplxSize <= 0)
154+
throw new ArgumentOutOfRangeException("smplxSize should be greater than zero");
155+
156+
Moniker = "Diver";
157+
158+
SimplexSize = smplxSize;
159+
160+
bProcs = new Dictionary<int, RWLT_BProc>();
161+
estimators = new Dictionary<int, PCore2D<GeoPoint3DT>>();
162+
remotes = new Dictionary<int, RemoteDescriptor>();
163+
}
164+
165+
#endregion
166+
167+
#region Methods
168+
169+
public Dictionary<string, Dictionary<string, string>> GetRemotesDescription()
170+
{
171+
Dictionary<string, Dictionary<string, string>> result = new Dictionary<string, Dictionary<string, string>>();
172+
173+
foreach (var remote in remotes)
174+
result.Add(string.Format("Diver #{0}", remote.Key.ToString()), remote.Value.GetDescription());
175+
176+
return result;
177+
}
178+
179+
public Dictionary<string, Dictionary<string, string>> GetRemotesDescription(bool isDst, bool isAzm, bool isRaz, bool isLatLon, bool isRer, bool isDopTba)
180+
{
181+
Dictionary<string, Dictionary<string, string>> result = new Dictionary<string, Dictionary<string, string>>();
182+
183+
foreach (var remote in remotes)
184+
result.Add(string.Format("Diver #{0}", remote.Key.ToString()),
185+
remote.Value.GetDescription(isDst, isAzm, isRaz, isLatLon, isRer, isDopTba));
186+
187+
return result;
188+
}
189+
190+
#endregion
191+
192+
#region ITOAProcessor
193+
194+
public void TOAProcess(BaseIDs baseID, double baseLat_deg, double baseLon_deg, double baseDpt_m, double baseBat_V, double baseMSR_dB, int pcode, double TOAs)
195+
{
196+
if (pcode < 0)
197+
return;
198+
199+
if (!remotes.ContainsKey(pcode))
200+
remotes.Add(pcode, new RemoteDescriptor(pcode));
201+
202+
if (!bProcs.ContainsKey(pcode))
203+
bProcs.Add(pcode, new RWLT_BProc(4, RWLT.PNG_INTERVAL_S));
204+
205+
var bases = bProcs[pcode].ProcessBase(baseID, baseLat_deg, baseLon_deg, baseDpt_m, TOAs);
206+
if ((bases != null) && (bases.Length >= 3))
207+
{
208+
if (!estimators.ContainsKey(pcode))
209+
{
210+
estimators.Add(pcode, new PCore2D<GeoPoint3DT>(RadialErrorThreshold, SimplexSize, Algorithms.WGS84Ellipsoid, 2));
211+
212+
estimators[pcode].SoundSpeed = soundSpeed;
213+
estimators[pcode].TargetDepth = baseDpt_m;
214+
215+
estimators[pcode].BaseQualityUpdatedHandler += (o, e) =>
216+
{
217+
remotes[pcode].TBA.Value = e.TBAState;
218+
remotes[pcode].DOP.Value = e.DopState;
219+
RemotesUpdatedHandler.Rise(this, new EventArgs());
220+
};
221+
222+
estimators[pcode].RadialErrorExeedsThrehsoldEventHandler += (o, e) =>
223+
{
224+
remotes[pcode].IsRERRExceed.Value = true;
225+
RemotesUpdatedHandler.Rise(this, new EventArgs());
226+
};
227+
estimators[pcode].TargetLocationUpdatedHandler += (o, e) =>
228+
{
229+
remotes[pcode].IsRERRExceed.Value = false;
230+
remotes[pcode].Latitude.Value = e.Location.Latitude;
231+
remotes[pcode].Longitude.Value = e.Location.Longitude;
232+
remotes[pcode].RadialError.Value = e.Location.RadialError;
233+
234+
LocationUpdatedHandler.Rise(this,
235+
new LocationUpdatedEventArgs(string.Format("{0} #{1}", Moniker, pcode),
236+
e.Location.Latitude,
237+
e.Location.Longitude,
238+
e.Location.RadialError));
239+
240+
RemotesUpdatedHandler.Rise(this, new EventArgs());
241+
};
242+
}
243+
244+
estimators[pcode].ProcessBasePoints(bases, DateTime.Now);
245+
}
246+
}
247+
248+
public void UpdateRelativePosition(double slat_deg, double slon_deg)
249+
{
250+
double slat_rad = Algorithms.Deg2Rad(slat_deg);
251+
double slon_rad = Algorithms.Deg2Rad(slon_deg);
252+
253+
foreach (var remote in remotes)
254+
{
255+
if (remote.Value.Latitude.IsInitialized &&
256+
remote.Value.Longitude.IsInitialized)
257+
{
258+
double rlat_rad = Algorithms.Deg2Rad(remote.Value.Latitude.Value);
259+
double rlon_rad = Algorithms.Deg2Rad(remote.Value.Longitude.Value);
260+
261+
int its = 0;
262+
double dst_m = 0;
263+
double fwd_az = 0;
264+
double rev_az = 0;
265+
266+
if (!Algorithms.VincentyInverse(slat_rad, slon_rad, rlat_rad, rlon_rad,
267+
Algorithms.WGS84Ellipsoid, Algorithms.VNC_DEF_EPSILON, Algorithms.VNC_DEF_IT_LIMIT,
268+
out dst_m, out fwd_az, out rev_az, out its))
269+
{
270+
dst_m = Algorithms.HaversineInverse(slat_rad, slon_rad, rlat_rad, rlon_rad, Algorithms.WGS84Ellipsoid.MajorSemiAxis_m);
271+
fwd_az = Algorithms.HaversineInitialBearing(slat_rad, slon_rad, rlat_rad, rlon_rad);
272+
rev_az = Algorithms.HaversineInitialBearing(rlat_rad, rlon_rad, slat_rad, slon_rad);
273+
}
274+
275+
rev_az = Algorithms.Wrap2PI(rev_az + Math.PI);
276+
fwd_az = Algorithms.Rad2Deg(fwd_az);
277+
rev_az = Algorithms.Rad2Deg(rev_az);
278+
remote.Value.Distance.Value = dst_m;
279+
remote.Value.ForwardAzimuth.Value = fwd_az;
280+
remote.Value.ReverseAzimuth.Value = rev_az;
281+
}
282+
}
283+
284+
RemotesUpdatedHandler.Rise(this, new EventArgs());
285+
}
286+
287+
#endregion
288+
289+
#region Events
290+
291+
public EventHandler<LocationUpdatedEventArgs> LocationUpdatedHandler;
292+
public EventHandler RemotesUpdatedHandler;
293+
294+
#endregion
295+
}
296+
}

‎uTrackLib/ITOAProcessor.cs‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
namespace uTrackLib
2+
{
3+
public interface ITOAProcessor
4+
{
5+
void TOAProcess(BaseIDs baseID, double baseLat_deg, double baseLon_deg, double baseDpt_m, double baseBat_V, double baseMSR_dB, int pcode, double TOAs);
6+
7+
void UpdateRelativePosition(double slat_deg, double slon_deg);
8+
}
9+
}

0 commit comments

Comments
 (0)