private void OnLBLA(object[] parameters)
{
// IC_D2H_LBLA $PRWLA,bID,baseLat,baseLon,[baseDpt],baseBat,pingerDataID,pingerData,TOAsecond,MSR_dB
BaseIDs baseID = (parameters[0] == null) ? BaseIDs.BASE_INVALID : (BaseIDs)(int)parameters[0];
double baseLat = doubleNullChecker(parameters[1]);
double baseLon = doubleNullChecker(parameters[2]);
double baseDepth = (parameters[3] == null) ? RWLT.DEFAULT_BASE_DPT_M : (double)parameters[3];
double baseBat = doubleNullChecker(parameters[4]);
PingerDataIDs pDataID = (parameters[5] == null) ? PingerDataIDs.DID_INVALID : (PingerDataIDs)(int)parameters[5];
double pData = doubleNullChecker(parameters[6]);
double TOAs = doubleNullChecker(parameters[7]);
double MSR = doubleNullChecker(parameters[8]);
if ((baseID != BaseIDs.BASE_INVALID) &&
(!double.IsNaN(baseLat)) &&
(!double.IsNaN(baseLon)) &&
(!double.IsNaN(baseDepth)))
{
if ((baseID == auxGNSSBuoyID) && (!AUXGNSSUsed))
{
GNSS_RMCSentenceReceived(this,
new RMCMessageEventArgs(0xFF,
TalkerIdentifiers.GN,
GetTimeStamp(),
baseLat,
baseLon,
double.NaN,
double.NaN,
double.NaN,
true));
}
LocationUpdatedEvent.Rise(this, new LocationUpdatedEventArgs(baseID.ToString().Replace('_', ' '),
baseLat, baseLon, baseDepth,
true,
GetTimeStamp()));
if (!double.IsNaN(MSR))
{
BaseMSRs[baseID].Value = MSR;
}
if (!double.IsNaN(baseBat))
{
BaseBatVoltages[baseID].Value = baseBat;
}
if (gravityAccNeedsUpdate)
{
gravityAcc = PHX.Gravity_constant_wgs84_calc(baseLat);
gravityAccNeedsUpdate = false;
}
if (pDataID == PingerDataIDs.DID_BAT)
{
TargetBatVoltage.Value = pData;
}
else if (pDataID == PingerDataIDs.DID_TMP)
{
TargetTemperature.Value = pData;
if (IsAutoSoundSpeed && TargetPressure.IsInitialized)
{
Soundspeed = PHX.Speed_of_sound_UNESCO_calc(TargetTemperature.Value, TargetPressure.Value / 2.0, salinity);
}
}
else if (pDataID == PingerDataIDs.DID_PRS)
{
TargetPressure.Value = pData;
if (TargetTemperature.IsInitialized)
{
if (IsAutoSoundSpeed)
{
Soundspeed = PHX.Speed_of_sound_UNESCO_calc(TargetTemperature.Value, TargetPressure.Value / 2.0, salinity);
}
double rho = PHX.Water_density_calc(TargetTemperature.Value, TargetPressure.Value / 2.0, salinity);
// p0 is zero, because pinger calibrates surface pressure on start and transmits pressure relative to the surface
TargetDepth.Value = PHX.Depth_by_pressure_calc(TargetPressure.Value, 0, rho, gravityAcc);
}
else if (TargetPressure.IsInitialized)
{
TargetDepth.Value = PHX.Depth_by_pressure_calc(TargetPressure.Value, 0, PHX.PHX_FWTR_DENSITY_KGM3, gravityAcc);
}
}
else if (pDataID == PingerDataIDs.DID_CODE)
{
TargetAlarm.Value = (PingerCodeIDs)Enum.ToObject(typeof(PingerCodeIDs), Convert.ToInt32(pData));
}
var bases = baseProcessor.ProcessBase(baseID, baseLat, baseLon, baseDepth, TOAs);
if (bases != null)
{
TryLocate(bases);
}
}
OnSystemUpdate();
}
private void timer_Tick(object sender, EventArgs e)
{
#region randomize basepoints
double dst = timeSlice * targetSpeed;
targetLocation = MoveGeoPoint(targetLocation, dst, targetCourse);
targetCourse = targetCourse * 0.99 + 0.01 * rnd.NextDouble() * Math.PI * 2.0;
targetSpeed = Math.Abs(targetSpeed * 0.9 + 0.1 * rnd.NextDouble() * 2 - 1);
targetDpt = Math.Abs(targetDpt + rnd.NextDouble() * 0.5 - 0.25);
targetTemp = Math.Abs(targetTemp + rnd.NextDouble() * 0.1 - 0.05);
targetBat = Math.Abs(targetBat + rnd.NextDouble() * 0.1 - 0.05);
dst = timeSlice * auxSpeed;
auxLocation = MoveGeoPoint(auxLocation, dst, auxCourse);
auxCourse = auxCourse * 0.99 + 0.01 * rnd.NextDouble() * Math.PI * 2.0;
auxSpeed = Math.Abs(auxSpeed * 0.9 + 0.1 * rnd.NextDouble() * 2 - 1);
foreach (BaseIDs baseID in baseIDs)
{
if (baseID != BaseIDs.BASE_INVALID)
{
dst = timeSlice * baseSpeeds[baseID];
baseLocations[baseID] = MoveGeoPoint(baseLocations[baseID], dst, baseCourses[baseID]);
baseCourses[baseID] = baseCourses[baseID] * 0.9 + 0.1 * rnd.NextDouble() * Math.PI * 2.0;
baseSpeeds[baseID] = Math.Abs(baseSpeeds[baseID] * 0.9 + 0.1 * rnd.NextDouble() * 0.5 - 0.25);
baseBats[baseID] = baseBats[baseID] + rnd.NextDouble() * 0.1;
}
}
#endregion
if (++targetPeriod == 2)
{
targetPeriod = 0;
double dst_p = 0;
double ddpt = 0;
double fwd_az_rad = 0;
double rev_az_rad = 0;
int its = 0;
foreach (BaseIDs baseID in baseIDs)
{
if (baseID != BaseIDs.BASE_INVALID)
{
Algorithms.VincentyInverse(targetLocation.Latitude, targetLocation.Longitude,
baseLocations[baseID].Latitude, baseLocations[baseID].Longitude,
Algorithms.WGS84Ellipsoid,
Algorithms.VNC_DEF_EPSILON,
Algorithms.VNC_DEF_IT_LIMIT,
out dst_p,
out fwd_az_rad,
out rev_az_rad,
out its);
ddpt = RWLT.DEFAULT_BASE_DPT_M - targetDpt;
TOAs[baseID] = Math.Sqrt(ddpt * ddpt + dst_p * dst_p) / SoundSpeed;
}
}
#region emulate LBLAs
PingerDataIDs dataID = dataIDs[dataIDIdx];
dataIDIdx = (dataIDIdx + 1) % dataIDs.Length;
double data = 0.0;
if (dataID == PingerDataIDs.DID_BAT)
{
data = targetBat;
}
else if (dataID == PingerDataIDs.DID_TMP)
{
data = targetTemp;
}
else if (dataID == PingerDataIDs.DID_PRS)
{
data = PHX.Pressure_by_depth_calc(targetDpt, PHX.PHX_ATM_PRESSURE_MBAR, PHX.PHX_FWTR_DENSITY_KGM3, PHX.PHX_GRAVITY_ACC_MPS2);
}
else
{
data = (int)dataID;
}
// IC_D2H_LBLA $PRWLA,bID,baseLat,baseLon,[baseDpt],baseBat,pingerDataID,pingerData,TOAsecond,MSR_dB
foreach (BaseIDs baseID in baseIDs)
{
if ((baseID != BaseIDs.BASE_INVALID)) //&& (baseID != BaseIDs.BASE_2))
{
NewEmuStringEvent.Rise(this, new EmuStringEventArgs(
NMEAParser.BuildProprietarySentence(ManufacturerCodes.RWL, "A",
new object[]
{
(int)baseID,
Algorithms.Rad2Deg(baseLocations[baseID].Latitude),
Algorithms.Rad2Deg(baseLocations[baseID].Longitude),
RWLT.DEFAULT_BASE_DPT_M,
baseBats[baseID],
dataID,
data,
TOAs[baseID],
77.7777 // it's an emulation
})));
}
}
#endregion
}
#region emulate AUX GNSS
string latCardinal, lonCardinal;
double auxLat = Algorithms.Rad2Deg(auxLocation.Latitude);
double auxLon = Algorithms.Rad2Deg(auxLocation.Longitude);
if (auxLat > 0)
{
latCardinal = "North";
}
else
{
latCardinal = "South";
}
if (auxLon > 0)
{
lonCardinal = "East";
}
else
{
lonCardinal = "West";
}
/*
* NewEmuStringEvent.Rise(this,
* new EmuStringEventArgs(
* NMEAParser.BuildSentence(TalkerIdentifiers.GN, SentenceIdentifiers.RMC, new object[]
* {
* DateTime.Now,
* "Valid",
* Math.Abs(auxLat), latCardinal,
* Math.Abs(auxLon), lonCardinal,
* auxSpeed * 3.6, // speed
* Algorithms.Rad2Deg(auxCourse), // track true
* DateTime.Now,
* null, // magnetic variation
* null, // magnetic variation direction
* "A",
* })));
*/
#endregion
}
