private void gtr_QueryRemote(int targetAddr, CDS_CMD cmd, int timeoutMs)
{
// $PTNTE,targetAddr,requestedCmd,timeoutMs
var msg = NMEAParser.BuildProprietarySentence(ManufacturerCodes.TNT, "E", new object[] { targetAddr, (int)cmd, timeoutMs });
gtr_requestID = cmd;
gtr_TrySend(msg, string.Format("RedGTR << ? SUB #{0}, CMD {1}, TMO {2} ms", targetAddr, cmd, timeoutMs), true);
}
private void Parse_REM_PONGEX(object[] parameters)
{
gtr_isWaitingRemote = false;
//#define IC_D2H_REM_PONGEX 'D' // $PTNTD,requestedAddr,requestedCmd,receivedValue_decoded,snrd,dpl,pTime,[dst],[dpt],[tmp]
try
{
int requestedAddr = (int)parameters[0];
CDS_CMD cmdID = (CDS_CMD)Enum.ToObject(typeof(CDS_CMD), (int)parameters[1]);
double value = (double)parameters[2];
double snrd = 10 * Math.Log10(Math.Abs((double)parameters[3]));
double dpl = (double)parameters[4];
double pTime = (double)parameters[5];
double dst = (double)parameters[6];
double dpt = (double)parameters[7];
double tmp = (double)parameters[8];
bTemperature.Value = tmp;
bDepth.Value = dpt;
if (cmdID == CDS_CMD.CDS_CMD_TMP)
{
tTemperature.Value = value;
}
else if (cmdID == CDS_CMD.CDS_CMD_DPT)
{
tDepth.Value = value;
}
StringBuilder sb = new StringBuilder();
sb.AppendFormat(CultureInfo.InvariantCulture, "BOAT\r\nLAT: {0}\r\nLON: {1}\r\nDPT: {2}\r\nTMP: {3}\r\nPTM: {4:F04} s\r\n",
bLatitude.ToString(), bLongitude.ToString(), bDepth.ToString(), bTemperature.ToString(), pTime);
sb.AppendFormat(CultureInfo.InvariantCulture, "\r\nTARGET\r\nDST: {0:F02} m\r\nSNR: {1:F01} dB\r\nDPL: {2:F02} Hz\r\n", dst, snrd, dpl);
if (tDepth.IsInitialized && !tDepth.IsObsolete)
{
sb.AppendFormat(CultureInfo.InvariantCulture, "DPT: {0}\r\n", tDepth.ToString());
}
if (tTemperature.IsInitialized && !tTemperature.IsObsolete)
{
sb.AppendFormat(CultureInfo.InvariantCulture, "TMP: {0}\r\n", tTemperature.ToString());
}
if (bLatitude.IsInitialized && !bLatitude.IsObsolete &&
bLongitude.IsInitialized && !bLongitude.IsObsolete)
{
measurements.Add(new Measurement(bLatitude.Value, bLongitude.Value, dst, snrd, dpt));
InvokeUpdateTrack("MEASUREMENTS", bLatitude.Value, bLongitude.Value);
if (measurements.IsBaseExists && tDepth.IsInitialized && (measurements.AngleRange > 270))
{
GeoPoint3DWE prevLocation = new GeoPoint3DWE();
prevLocation.Latitude = double.NaN;
prevLocation.Longitude = double.NaN;
prevLocation.Depth = tDepth.Value;
prevLocation.RadialError = double.NaN;
double stStageRErr = 0.0;
int itCnt = 0;
var basePoints = measurements.GetBase();
List <PointF> basePnts = new List <PointF>();
foreach (var bPoint in basePoints)
{
basePnts.Add(new PointF(Convert.ToSingle(bPoint.Latitude), Convert.ToSingle(bPoint.Longitude)));
}
InvokeUpdateTrack("BASE", basePnts.ToArray());
var locResult = Navigation.LocateLBL_NLM(basePoints, prevLocation, settingsProvider.Data.RadialErrorThreshold, out stStageRErr, out itCnt);
tLatitude.Value = locResult.Latitude;
tLongitude.Value = locResult.Longitude;
tRadialError.Value = locResult.RadialError;
tLocation.Add(locResult);
InvokeUpdateTrack("TARGET", locResult.Latitude, locResult.Longitude);
if (settingsProvider.Data.IsGNSSEmulator)
{
SendEMU(locResult.Latitude, locResult.Longitude, tDepth.Value, locResult.RadialError);
}
if ((double.IsNaN(tBestLocation.Latitude) || (tBestLocation.RadialError > locResult.RadialError)))
{
tBestLocation.Latitude = locResult.Latitude;
tBestLocation.Longitude = locResult.Longitude;
tBestLocation.RadialError = locResult.RadialError;
measurements.UpdateReferencePoint(tBestLocation.Latitude, tBestLocation.Longitude);
InvokeUpdateTrack("BEST", tBestLocation.Latitude, tBestLocation.Longitude);
}
InvokeSetEnabled(mainToolStrip, tracksBtn, true);
}
else
{
double cLat = 0;
double cLon = 0;
measurements.CenterOfMass(out cLat, out cLon);
measurements.UpdateReferencePoint(cLat, cLon);
}
}
if (tLatitude.IsInitialized && !tLatitude.IsObsolete &&
tLongitude.IsInitialized && !tLongitude.IsObsolete)
{
sb.AppendFormat(CultureInfo.InvariantCulture, "LAT: {0}\r\nLON: {1}\r\n", tLatitude.ToString(), tLongitude.ToString());
}
if (tRadialError.IsInitialized && !tRadialError.IsObsolete)
{
sb.AppendFormat(CultureInfo.InvariantCulture, "RER: {0}\r\n", tRadialError.ToString());
}
if (!double.IsNaN(tBestLocation.RadialError))
{
sb.AppendFormat(CultureInfo.InvariantCulture, "BRE: {0:F03}\r\n", tBestLocation.RadialError);
}
InvokeSetLeftUpperCornerText(sb.ToString());
InvokeInvalidatePlot();
if (isAutosnapshot)
{
InvokeSaveSnapShot();
}
}
catch (Exception ex)
{
ProcessException(ex, false);
}
}
private void timer_Tick(object sender, EventArgs e)
{
if (gtr_isWaiting)
{
if (++gtr_timeoutCounter > gtr_Timeout)
{
gtr_OnTimeout();
}
}
else if (gtr_isWaitingRemote)
{
if (++gtr_remoteTimeoutCounter > gtr_RemoteTimeout_S)
{
gtr_OnRemotTimeout();
}
}
else
{
if (gtrPort.IsOpen)
{
if (!gtr_deviceInfoUpdated)
{
// query device info
gtr_DeviceInfoQuery();
}
else if (!gtr_salinityUpdated)
{
// query salinity set
gtr_QuerySalinitySet(settingsProvider.Data.Salinity);
}
else if (isAutoquery)
{
// query remote
CDS_CMD cmdID = CDS_CMD.CDS_CMD_DPT;
if (!bTemperature.IsInitialized || bTemperature.IsObsolete)
{
cmdID = CDS_CMD.CDS_CMD_TMP;
}
gtr_QueryRemote(settingsProvider.Data.TargetAddr, cmdID, gtr_RemoteTimeout_S * 1000);
}
}
}
/// DEBUG
///
//double ranLat = 48 + rnd.NextDouble();
//double ranLon = 44 + rnd.NextDouble();
//measurements.Add(new Measurement(ranLat, ranLon, rnd.NextDouble(), rnd.NextDouble(), rnd.NextDouble()));
//double refLat = 0;
//double refLon = 0;
//measurements.CenterOfMass(out refLat, out refLon);
//measurements.UpdateReferencePoint(refLat, refLon);
//if (measurements.IsBaseExists)
//{
// var bs = measurements.GetBase();
// List<PointF> bsp = new List<PointF>();
// foreach (var bsItem in bs)
// {
// bsp.Add(new PointF(Convert.ToSingle(bsItem.Latitude), Convert.ToSingle(bsItem.Longitude)));
// }
// marinePlot.Invoke((MethodInvoker)delegate
// {
// marinePlot.UpdateTrack("MEASUREMENTS", ranLat, ranLon);
// marinePlot.UpdateTrack("BASE", bsp.ToArray());
// marinePlot.Invalidate();
// });
//}
}
