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);
}
}
public MainForm()
{
InitializeComponent();
this.Text = string.Format("{0}", Application.ProductName);
#region file names & paths
DateTime startTime = DateTime.Now;
settingsFileName = Path.ChangeExtension(Application.ExecutablePath, "settings");
logPath = Path.Combine(Path.GetDirectoryName(Application.ExecutablePath), "LOG");
logFileName = StrUtils.GetTimeDirTreeFileName(startTime, Application.ExecutablePath, "LOG", "log", true);
snapshotsPath = StrUtils.GetTimeDirTree(startTime, Application.ExecutablePath, "SNAPSHOTS", false);
#endregion
#region logger
loggerTextAddedEventHandler = new EventHandler <TextAddedEventArgs>(logger_TextAdded);
logger = new TSLogProvider(logFileName);
logger.TextAddedEvent += loggerTextAddedEventHandler;
logger.WriteStart();
#endregion
#region settings
logger.Write("Loading settings...");
settingsProvider = new SettingsProviderXML <SettingsContainer>();
settingsProvider.isSwallowExceptions = false;
try
{
settingsProvider.Load(settingsFileName);
}
catch (Exception ex)
{
ProcessException(ex, true);
}
logger.Write(settingsProvider.Data.ToString());
gtr_RemoteTimeout_S = Convert.ToInt32((GTR.MIN_REM_TOUT_MS / 1000) + 2 * (double)settingsProvider.Data.MaxDistance / 1500) + 1;
#endregion
#region NMEA
NMEAParser.AddManufacturerToProprietarySentencesBase(ManufacturerCodes.TNT);
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "0", "x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "1", "x,x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "2", "x,x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "3", "x,x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "4", "x,x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "5", "x,x.x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "6", "x,x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "7", "x,x.x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "8", "x,x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "9", "x,x.x,x.x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "A", "x,x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "B", "x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "C", "x,x.x,x.x,x.x,x.x,x.x,x.x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "D", "x,x,x.x,x.x,x.x,x.x,x.x,x.x,x.x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "E", "x,x,x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "!", "c--c,x,c--c,x,x,c--c");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "O", "x.x,x.x");
NMEAParser.AddProprietarySentenceDescription(ManufacturerCodes.TNT, "P", "x,x.x");
/*
#define IC_D2H_ACK '0' // $PTNT0,errCode
#define IC_H2D_FLD_GET '1' // $PTNT1,fldID,reserved
#define IC_H2D_FLD_SET '2' // $PTNT2,fldID,fldNewValue
#define IC_D2H_FLD_VAL '3' // $PTNT3,fldID,fldValue
#define IC_H2D_LOC_DATA_GET '4' // $PTNT4,locDataID,reserved
#define IC_D2H_LOC_DATA_VAL '5' // $PTNT5,locDataID,locDataValue
#define IC_H2D_ACT_INVOKE '6' // $PTNT6,actionID,reserved
#define IC_H2D_LOC_DATA_SET '7' // $PTNT7,locDataID,locDataValue
#define IC_H2D_REM_SEND '8' // $PTNT8,targetAddr,codeMsg
#define IC_D2H_REM_RECEIVED '9' // $PTNT9,codeMsg,snrd,dpl
#define IC_H2D_REM_PING 'A' // $PTNTA,targetAddr,timeoutMs
#define IC_D2H_REM_TOUT 'B' // $PTNTB,targetAddr
#define IC_D2H_REM_PONG 'C' // $PTNTC,requestedAddr,snrd,dpl,pTime,[dst],[dpt],[tmp]
#define IC_D2H_REM_PONGEX 'D' // $PTNTD,requestedAddr,requestedCmd,receivedValue_decoded,snrd,dpl,pTime,[dst],[dpt],[tmp]
#define IC_H2D_REM_PINGEX 'E' // $PTNTE,targetAddr,requestedCmd,timeoutMs
#define IC_D2H_DEV_INFO_VAL '!' // $PTNT!,sys_moniker,sys_ver,dev_type,core_moniker,core_ver,dev_serial_num
#define IC_D2H_PRETMP_VAL 'O' // $PTNTO,pressure_mbar,temp_deg_c
#define IC_H2D_SETVAL 'P' // $PTNTP,valueID,value
*/
#endregion
#region other
gtrPortErrorEventHandler = new EventHandler <SerialErrorReceivedEventArgs>(gtrPort_Error);
gtrPortNewNMEAMessageEventHandler = new EventHandler <NewNMEAMessageEventArgs>(gtrPort_NewNMEAMessage);
gtrPort = new NMEASerialPort(new SerialPortSettings(settingsProvider.Data.GTRPortName, BaudRate.baudRate9600,
System.IO.Ports.Parity.None, DataBits.dataBits8, System.IO.Ports.StopBits.One, System.IO.Ports.Handshake.None));
gtrPort.IsRawModeOnly = false;
timerTickHandler = new EventHandler(timer_Tick);
timer = new PrecisionTimer();
timer.Mode = Mode.Periodic;
timer.Period = 1000;
timer.Tick += timerTickHandler;
timer.Start();
if (settingsProvider.Data.IsGNSSEmulator)
{
gnssEmulatorPort = new NMEASerialPort(new SerialPortSettings(settingsProvider.Data.GNSSEmulatorPortName, BaudRate.baudRate9600,
Parity.None, DataBits.dataBits8, StopBits.One, Handshake.None));
}
measurements = new Measurements(settingsProvider.Data.MeasurementsFIFOSize, settingsProvider.Data.BaseSize);
bLatitude = new AgingDouble("F06", "°");
bLongitude = new AgingDouble("F06", "°");
bTemperature = new AgingDouble("F01", "°C");
bDepth = new AgingDouble("F03", " m");
tTemperature = new AgingDouble("F01", "°C");
tDepth = new AgingDouble("F02", " m");
tLatitude = new AgingDouble("F06", "°");
tLongitude = new AgingDouble("F06", "°");
tRadialError = new AgingDouble("F03", " m");
tLocation = new List <GeoPoint3DWE>();
bLocation = new List <GeoPoint>();
tBestLocation = new GeoPoint3DWE();
tBestLocation.Latitude = double.NaN;
tBestLocation.Longitude = double.NaN;
tBestLocation.RadialError = double.NaN;
marinePlot.InitTracks(settingsProvider.Data.MeasurementsFIFOSize);
marinePlot.AddTrack("BOAT GNSS", Color.Blue, 2.0f, 2, settingsProvider.Data.MeasurementsFIFOSize, true);
marinePlot.AddTrack("BASE", Color.Salmon, 2.0f, 8, settingsProvider.Data.BaseSize, false);
marinePlot.AddTrack("MEASUREMENTS", Color.Green, 2.0f, 4, settingsProvider.Data.MeasurementsFIFOSize, false);
marinePlot.AddTrack("TARGET", Color.Black, 2.0f, 4, settingsProvider.Data.MeasurementsFIFOSize, false);
marinePlot.AddTrack("BEST", Color.Red, 2.0f, 8, 1, false);
#endregion
}
public static GeoPoint3DWE LocateLBL_NLM(List <Measurement> bases, GeoPoint3DWE previousLocation, double rErrThreshold, out double stStageRErr, out int nlm_itCount)
{
// bases sorted from nearest to farest
#region variables
double[] bx = new double[bases.Count]; // base x-coordinate
double[] by = new double[bases.Count]; // base y-coordinate
double[] bz = new double[bases.Count]; // base z-coordinate
double[] bd = new double[bases.Count]; // distance to a base
double meanLat = 0;
double meanLon = 0;
double z = previousLocation.Depth;
double radialError = double.NaN;
double simplexSize = 25;
#endregion
#region bases mid-point
foreach (var item in bases)
{
meanLat += item.Latitude;
meanLon += item.Longitude;
}
meanLat /= bases.Count;
meanLon /= bases.Count;
#endregion
#region translation of coordinates from degrees to meters
for (int i = 0; i < bases.Count; i++)
{
Navigation.GetDistance2DDeg(meanLat, meanLon, bases[i].Latitude, bases[i].Longitude, out by[i], out bx[i]);
bd[i] = bases[i].Distance_m;
bz[i] = bases[i].OwnDepth;
}
#endregion
double xBest = double.NaN;
double yBest = double.NaN;
stStageRErr = double.NaN;
if ((double.IsNaN(previousLocation.Latitude)) ||
(double.IsNaN(previousLocation.Longitude)) ||
(previousLocation.RadialError > rErrThreshold))
{
#region 1st stage 1D optimization
double alpha = LocateLBL_1D(z, bx, by, bz, bd, 0, 360, 10, out xBest, out yBest);
alpha = LocateLBL_1D(z, bx, by, bz, bd, alpha - 10, alpha + 10, 1, out xBest, out yBest);
stStageRErr = Math.Sqrt(EpsTOA(xBest, yBest, z, bx, by, bz, bd));
simplexSize = stStageRErr;// *stStageRErr;
#endregion
}
else
{
Navigation.GetDistance2DDeg(meanLat, meanLon, previousLocation.Latitude, previousLocation.Longitude, out yBest, out xBest);
}
#region Nelder-Mead 2D optimization
bool isFinished = false;
int itCnt = 0;
double tmp, tmp1;
double[] xix = new double[3];
double[] xiy = new double[3];
double[] fxi = new double[3];
double fl, fg, fh, fr, fe, fs;
double xcx, xcy, xrx, xry, xex, xey, xsx, xsy;
xix[0] = xBest;
xiy[0] = yBest;
xix[1] = xix[0] + simplexSize; //
xiy[1] = xiy[0] + simplexSize; //
xix[2] = xix[0] - simplexSize / 2; //
xiy[2] = xiy[0] + simplexSize / 2; //
while ((!isFinished) && (itCnt < NAV_NLM_MIT))
{
fxi[0] = EpsTOA(xix[0], xiy[0], z, bx, by, bz, bd);
fxi[1] = EpsTOA(xix[1], xiy[1], z, bx, by, bz, bd);
fxi[2] = EpsTOA(xix[2], xiy[2], z, bx, by, bz, bd);
if (fxi[0] > fxi[1])
{
tmp = fxi[0]; fxi[0] = fxi[1]; fxi[1] = tmp;
tmp = xix[0]; xix[0] = xix[1]; xix[1] = tmp;
tmp = xiy[0]; xiy[0] = xiy[1]; xiy[1] = tmp;
}
if (fxi[0] > fxi[2])
{
tmp = fxi[0]; fxi[0] = fxi[2]; fxi[2] = tmp;
tmp = xix[0]; xix[0] = xix[2]; xix[2] = tmp;
tmp = xiy[0]; xiy[0] = xiy[2]; xiy[2] = tmp;
}
if (fxi[1] > fxi[2])
{
tmp = fxi[1]; fxi[1] = fxi[2]; fxi[2] = tmp;
tmp = xix[1]; xix[1] = xix[2]; xix[2] = tmp;
tmp = xiy[1]; xiy[1] = xiy[2]; xiy[2] = tmp;
}
fl = fxi[0];
fg = fxi[1];
fh = fxi[2];
xcx = (xix[0] + xix[1]) / 2.0f;
xcy = (xiy[0] + xiy[1]) / 2.0f;
xrx = (1.0f + NAV_NLM_A) * xcx - NAV_NLM_A * xix[2];
xry = (1.0f + NAV_NLM_A) * xcy - NAV_NLM_A * xiy[2];
fr = EpsTOA(xrx, xry, z, bx, by, bz, bd);
if (fr < fl)
{
xex = (1.0f - NAV_NLM_G) * xcx + NAV_NLM_G * xrx;
xey = (1.0f - NAV_NLM_G) * xcy + NAV_NLM_G * xry;
fe = EpsTOA(xex, xey, z, bx, by, bz, bd);
if (fe < fr)
{
xix[2] = xex;
xiy[2] = xey;
}
else
{
xix[2] = xrx;
xiy[2] = xry;
}
}
else
{
if ((fr > fl) && (fr < fg))
{
xix[2] = xrx;
xiy[2] = xry;
}
else
{
if ((fr > fg) && (fr < fh))
{
tmp = xix[2]; xix[2] = xrx; xrx = tmp;
tmp = xiy[2]; xiy[2] = xry; xry = tmp;
tmp = fxi[2]; fxi[2] = fr; fr = tmp;
}
else
{
if (fh < fr)
{
//
}
}
xsx = NAV_NLM_B * xix[2] + (1.0f - NAV_NLM_B) * xcx;
xsy = NAV_NLM_B * xiy[2] + (1.0f - NAV_NLM_B) * xcy;
fs = EpsTOA(xsx, xsy, z, bx, by, bz, bd);
if (fs < fh)
{
xix[2] = xsx;
xiy[2] = xsy;
}
else
{
xix[1] = (xix[1] - xix[0]) / 2.0f;
xiy[1] = (xiy[1] - xiy[0]) / 2.0f;
xix[2] = (xix[2] - xix[0]) / 2.0f;
xiy[2] = (xiy[2] - xiy[0]) / 2.0f;
}
}
}
tmp = (fxi[0] + fxi[1] + fxi[2]) / 3.0f;
tmp1 = ((fxi[0] - tmp) * (fxi[0] - tmp) +
(fxi[1] - tmp) * (fxi[1] - tmp) +
(fxi[2] - tmp) * (fxi[2] - tmp)) / 3.0f;
isFinished = (Math.Sqrt(tmp1) <= NAV_NLM_EPSILON);
itCnt++;
}
xBest = xix[0];
yBest = xiy[0];
radialError = Math.Sqrt(EpsTOA(xix[0], xiy[0], z, bx, by, bz, bd));
nlm_itCount = itCnt;
#endregion
#region convert coordinates to degrees
double targetLongitude = double.NaN;
double targetLatitude = double.NaN;
Meters2Deg(meanLon, meanLat, yBest, xBest, out targetLongitude, out targetLatitude);
#endregion
GeoPoint3DWE result = new GeoPoint3DWE();
result.Depth = previousLocation.Depth;
result.Latitude = targetLatitude;
result.Longitude = targetLongitude;
result.RadialError = radialError;
return(result);
}
