1修复开网络RTK后,界面卡顿bug,rtkloop增加关键延迟

2将now改为utcnow,减少系统占用
This commit is contained in:
pxzleo 2024-01-10 16:15:33 +08:00
parent 9cfd7db051
commit 17e4c8bd97
2 changed files with 1133 additions and 1131 deletions

File diff suppressed because it is too large Load Diff

View File

@ -1,431 +1,430 @@
using System; using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.ComponentModel; using System.ComponentModel;
using System.Reflection; using System.Reflection;
using System.Text; using System.Text;
using System.IO.Ports; using System.IO.Ports;
using System.Threading; using System.Threading;
using System.Net; // dns, ip address using System.Net; // dns, ip address
using System.Net.Sockets; // tcplistner using System.Net.Sockets; // tcplistner
using System.IO; using System.IO;
using System.Runtime.InteropServices; using System.Runtime.InteropServices;
using System.Text.RegularExpressions; using System.Text.RegularExpressions;
namespace Plane.Util namespace Plane.Util
{ {
public class CommsNTRIP : ICommsSerial, IDisposable public class CommsNTRIP : ICommsSerial, IDisposable
{ {
public TcpClient client = new TcpClient(); public TcpClient client = new TcpClient();
IPEndPoint RemoteIpEndPoint = new IPEndPoint(IPAddress.Any, 0); IPEndPoint RemoteIpEndPoint = new IPEndPoint(IPAddress.Any, 0);
private Uri remoteUri; private Uri remoteUri;
public double lat = 0; public double lat = 0;
public double lng = 0; public double lng = 0;
public double alt = 0; public double alt = 0;
int retrys = 3; int retrys = 3;
private string host; private string host;
public int WriteBufferSize { get; set; } public int WriteBufferSize { get; set; }
public int WriteTimeout { get; set; } public int WriteTimeout { get; set; }
public bool RtsEnable { get; set; } public bool RtsEnable { get; set; }
private string url; private string url;
public Stream BaseStream public Stream BaseStream
{ {
get { return client.GetStream(); } get { return client.GetStream(); }
} }
public CommsNTRIP(string url, double lat, double lng) public CommsNTRIP(string url, double lat, double lng)
{ {
this.url = url; this.url = url;
this.lat = lat; this.lat = lat;
this.lng = lng; this.lng = lng;
ReadTimeout = 500; ReadTimeout = 500;
} }
public void toggleDTR() public void toggleDTR()
{ {
} }
public string Port { get; set; } public string Port { get; set; }
public int ReadTimeout public int ReadTimeout
{ {
get; // { return client.ReceiveTimeout; } get; // { return client.ReceiveTimeout; }
set; // { client.ReceiveTimeout = value; } set; // { client.ReceiveTimeout = value; }
} }
public int ReadBufferSize { get; set; } public int ReadBufferSize { get; set; }
public int BaudRate { get; set; } public int BaudRate { get; set; }
public StopBits StopBits { get; set; } public StopBits StopBits { get; set; }
public Parity Parity { get; set; } public Parity Parity { get; set; }
public int DataBits { get; set; } public int DataBits { get; set; }
public string PortName { get; set; } public string PortName { get; set; }
public int BytesToRead public int BytesToRead
{ {
get get
{ {
/*Console.WriteLine(DateTime.Now.Millisecond + " tcp btr " + (client.Available + rbuffer.Length - rbufferread));*/ /*Console.WriteLine(DateTime.Now.Millisecond + " tcp btr " + (client.Available + rbuffer.Length - rbufferread));*/
SendNMEA(); SendNMEA();
return (int)client.Available; return (int)client.Available;
} }
} }
public int BytesToWrite public int BytesToWrite
{ {
get { return 0; } get { return 0; }
} }
public bool IsOpen public bool IsOpen
{ {
get get
{ {
try try
{ {
return client.Client.Connected; return client.Client.Connected;
} }
catch catch
{ {
return false; return false;
} }
} }
} }
public bool DtrEnable { get; set; } public bool DtrEnable { get; set; }
public void Open() public void Open()
{ {
if (client.Client.Connected) if (client.Client.Connected)
{ {
return; return;
} }
/*string url = "";*/ /*string url = "";*/
int count = url.Split('@').Length - 1; int count = url.Split('@').Length - 1;
if (count > 1) if (count > 1)
{ {
var regex = new Regex("@"); var regex = new Regex("@");
url = regex.Replace(url, "%40", 1); url = regex.Replace(url, "%40", 1);
} }
url = url.Replace("ntrip://", "http://"); url = url.Replace("ntrip://", "http://");
remoteUri = new Uri(url); remoteUri = new Uri(url);
doConnect(); doConnect();
} }
private byte[] TcpKeepAlive(bool On_Off, uint KeepaLiveTime, uint KeepaLiveInterval) private byte[] TcpKeepAlive(bool On_Off, uint KeepaLiveTime, uint KeepaLiveInterval)
{ {
byte[] InValue = new byte[12]; byte[] InValue = new byte[12];
Array.ConstrainedCopy(BitConverter.GetBytes(Convert.ToUInt32(On_Off)), 0, InValue, 0, 4); Array.ConstrainedCopy(BitConverter.GetBytes(Convert.ToUInt32(On_Off)), 0, InValue, 0, 4);
Array.ConstrainedCopy(BitConverter.GetBytes(KeepaLiveTime), 0, InValue, 4, 4); Array.ConstrainedCopy(BitConverter.GetBytes(KeepaLiveTime), 0, InValue, 4, 4);
Array.ConstrainedCopy(BitConverter.GetBytes(KeepaLiveInterval), 0, InValue, 8, 4); Array.ConstrainedCopy(BitConverter.GetBytes(KeepaLiveInterval), 0, InValue, 8, 4);
return InValue; return InValue;
} }
private void doConnect() private void doConnect()
{ {
string usernamePassword = remoteUri.UserInfo; string usernamePassword = remoteUri.UserInfo;
string userpass2 = Uri.UnescapeDataString(usernamePassword); string userpass2 = Uri.UnescapeDataString(usernamePassword);
string auth = "Authorization: Basic " + string auth = "Authorization: Basic " +
Convert.ToBase64String(new ASCIIEncoding().GetBytes(userpass2)) + "\r\n"; Convert.ToBase64String(new ASCIIEncoding().GetBytes(userpass2)) + "\r\n";
if (usernamePassword == "") if (usernamePassword == "")
auth = ""; auth = "";
host = remoteUri.Host; host = remoteUri.Host;
Port = remoteUri.Port.ToString(); Port = remoteUri.Port.ToString();
client = new TcpClient(host, int.Parse(Port)); client = new TcpClient(host, int.Parse(Port));
client.Client.IOControl(IOControlCode.KeepAliveValues, TcpKeepAlive(true, 36000000, 3000), null); client.Client.IOControl(IOControlCode.KeepAliveValues, TcpKeepAlive(true, 36000000, 3000), null);
NetworkStream ns = client.GetStream(); NetworkStream ns = client.GetStream();
StreamWriter sw = new StreamWriter(ns); StreamWriter sw = new StreamWriter(ns);
StreamReader sr = new StreamReader(ns); StreamReader sr = new StreamReader(ns);
string line = "GET " + remoteUri.PathAndQuery + " HTTP/1.0\r\n" string line = "GET " + remoteUri.PathAndQuery + " HTTP/1.0\r\n"
+ "User-Agent: NTRIP MissionPlanner/1.0\r\n" + "User-Agent: NTRIP MissionPlanner/1.0\r\n"
+ auth + auth
+ "Connection: close\r\n\r\n"; + "Connection: close\r\n\r\n";
sw.Write(line); sw.Write(line);
sw.Flush(); sw.Flush();
line = sr.ReadLine(); line = sr.ReadLine();
if (!line.Contains("200")) if (!line.Contains("200"))
{ {
client.Dispose(); client.Dispose();
client = new TcpClient(); client = new TcpClient();
throw new Exception("Bad ntrip Responce\n\n" + line); throw new Exception("Bad ntrip Responce\n\n" + line);
} }
// vrs may take up to 60+ seconds to respond // vrs may take up to 60+ seconds to respond
SendNMEA(); SendNMEA();
VerifyConnected(); VerifyConnected();
} }
DateTime _lastnmea = DateTime.MinValue; DateTime _lastnmea = DateTime.MinValue;
private void SendNMEA() private void SendNMEA()
{ {
if (lat != 0 || lng != 0) if (lat != 0 || lng != 0)
{ {
if (_lastnmea.AddSeconds(30) < DateTime.Now) DateTime currentUtcTime = DateTime.UtcNow;
{ if (_lastnmea.AddSeconds(30) < currentUtcTime)
double latdms = (int)lat + ((lat - (int)lat) * .6f); {
double lngdms = (int)lng + ((lng - (int)lng) * .6f); double latdms = (int)lat + ((lat - (int)lat) * .6f);
double lngdms = (int)lng + ((lng - (int)lng) * .6f);
var line = string.Format(System.Globalization.CultureInfo.InvariantCulture,
"$GP{0},{1:HHmmss.ff},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14}", "GGA", var line = string.Format(System.Globalization.CultureInfo.InvariantCulture,
DateTime.Now.ToUniversalTime(), Math.Abs(latdms * 100).ToString("0000.00"), lat < 0 ? "S" : "N", "$GP{0},{1:HHmmss.ff},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14}", "GGA",
Math.Abs(lngdms * 100).ToString("00000.00"), lng < 0 ? "W" : "E", 1, 10, currentUtcTime, Math.Abs(latdms * 100).ToString("0000.00"), lat < 0 ? "S" : "N",
1, alt.ToString("0.00"), "M", 0, "M", "0.0", "0"); Math.Abs(lngdms * 100).ToString("00000.00"), lng < 0 ? "W" : "E", 1, 10,
1, alt.ToString("0.00"), "M", 0, "M", "0.0", "0");
string checksum = GetChecksum(line);
WriteLine(line + "*" + checksum); string checksum = GetChecksum(line);
WriteLine(line + "*" + checksum);
_lastnmea = currentUtcTime;
_lastnmea = DateTime.Now; }
} }
}
}
}
// Calculates the checksum for a sentence
// Calculates the checksum for a sentence string GetChecksum(string sentence)
string GetChecksum(string sentence) {
{ // Loop through all chars to get a checksum
// Loop through all chars to get a checksum int Checksum = 0;
int Checksum = 0; foreach (char Character in sentence.ToCharArray())
foreach (char Character in sentence.ToCharArray()) {
{ switch (Character)
switch (Character) {
{ case '$':
case '$': // Ignore the dollar sign
// Ignore the dollar sign break;
break; case '*':
case '*': // Stop processing before the asterisk
// Stop processing before the asterisk continue;
continue; default:
default: // Is this the first value for the checksum?
// Is this the first value for the checksum? if (Checksum == 0)
if (Checksum == 0) {
{ // Yes. Set the checksum to the value
// Yes. Set the checksum to the value Checksum = Convert.ToByte(Character);
Checksum = Convert.ToByte(Character); }
} else
else {
{ // No. XOR the checksum with this character's value
// No. XOR the checksum with this character's value Checksum = Checksum ^ Convert.ToByte(Character);
Checksum = Checksum ^ Convert.ToByte(Character); }
} break;
break; }
} }
} // Return the checksum formatted as a two-character hexadecimal
// Return the checksum formatted as a two-character hexadecimal return Checksum.ToString("X2");
return Checksum.ToString("X2"); }
}
void VerifyConnected()
void VerifyConnected() {
{ if (!IsOpen)
if (!IsOpen) {
{ try
try {
{ client.Dispose();
client.Dispose(); client = new TcpClient();
client = new TcpClient(); }
} catch { }
catch { }
// this should only happen if we have established a connection in the first place
// this should only happen if we have established a connection in the first place if (client != null && retrys > 0)
if (client != null && retrys > 0) {
{ doConnect();
doConnect(); retrys--;
retrys--; }
}
throw new Exception("网络RTK基站通讯已关闭!");
throw new Exception("网络RTK基站通讯已关闭!"); }
} }
}
public int Read(byte[] readto, int offset, int length)
public int Read(byte[] readto, int offset, int length) {
{ VerifyConnected();
VerifyConnected();
SendNMEA();
SendNMEA();
try
try {
{ if (length < 1) { return 0; }
if (length < 1) { return 0; }
return client.Client.Receive(readto, offset, length, SocketFlags.Partial);
return client.Client.Receive(readto, offset, length, SocketFlags.Partial); /*
/* byte[] temp = new byte[length];
byte[] temp = new byte[length]; clientbuf.Read(temp, 0, length);
clientbuf.Read(temp, 0, length);
temp.CopyTo(readto, offset);
temp.CopyTo(readto, offset);
return length;*/
return length;*/ }
} catch { throw new Exception("ntrip Socket Closed"); }
catch { throw new Exception("ntrip Socket Closed"); } }
}
public int ReadByte()
public int ReadByte() {
{ VerifyConnected();
VerifyConnected(); int count = 0;
int count = 0; while (this.BytesToRead == 0)
while (this.BytesToRead == 0) {
{ System.Threading.Thread.Sleep(1);
System.Threading.Thread.Sleep(1); if (count > ReadTimeout)
if (count > ReadTimeout) throw new Exception("ntrip Timeout on read");
throw new Exception("ntrip Timeout on read"); count++;
count++; }
} byte[] buffer = new byte[1];
byte[] buffer = new byte[1]; Read(buffer, 0, 1);
Read(buffer, 0, 1); return buffer[0];
return buffer[0]; }
}
public int ReadChar()
public int ReadChar() {
{ return ReadByte();
return ReadByte(); }
}
public string ReadExisting()
public string ReadExisting() {
{ VerifyConnected();
VerifyConnected(); byte[] data = new byte[client.Available];
byte[] data = new byte[client.Available]; if (data.Length > 0)
if (data.Length > 0) Read(data, 0, data.Length);
Read(data, 0, data.Length);
string line = Encoding.ASCII.GetString(data, 0, data.Length);
string line = Encoding.ASCII.GetString(data, 0, data.Length);
return line;
return line; }
}
public void WriteLine(string line)
public void WriteLine(string line) {
{ VerifyConnected();
VerifyConnected(); line = line + "\r\n";
line = line + "\r\n"; Write(line);
Write(line); }
}
public void Write(string line)
public void Write(string line) {
{ VerifyConnected();
VerifyConnected(); byte[] data = new System.Text.ASCIIEncoding().GetBytes(line);
byte[] data = new System.Text.ASCIIEncoding().GetBytes(line); Write(data, 0, data.Length);
Write(data, 0, data.Length); }
}
public void Write(byte[] write, int offset, int length)
public void Write(byte[] write, int offset, int length) {
{ VerifyConnected();
VerifyConnected(); try
try {
{ client.Client.Send(write, length, SocketFlags.None);
client.Client.Send(write, length, SocketFlags.None); }
} catch { }//throw new Exception("Comport / Socket Closed"); }
catch { }//throw new Exception("Comport / Socket Closed"); } }
}
public void DiscardInBuffer()
public void DiscardInBuffer() {
{ VerifyConnected();
VerifyConnected(); int size = (int)client.Available;
int size = (int)client.Available; byte[] crap = new byte[size];
byte[] crap = new byte[size]; Read(crap, 0, size);
Read(crap, 0, size); }
}
public string ReadLine()
public string ReadLine() {
{ byte[] temp = new byte[4000];
byte[] temp = new byte[4000]; int count = 0;
int count = 0; int timeout = 0;
int timeout = 0;
while (timeout <= 100)
while (timeout <= 100) {
{ if (!this.IsOpen) { break; }
if (!this.IsOpen) { break; } if (this.BytesToRead > 0)
if (this.BytesToRead > 0) {
{ byte letter = (byte)this.ReadByte();
byte letter = (byte)this.ReadByte();
temp[count] = letter;
temp[count] = letter;
if (letter == '\n') // normal line
if (letter == '\n') // normal line {
{ break;
break; }
}
count++;
count++; if (count == temp.Length)
if (count == temp.Length) break;
break; timeout = 0;
timeout = 0; }
} else
else {
{ timeout++;
timeout++; System.Threading.Thread.Sleep(5);
System.Threading.Thread.Sleep(5); }
} }
}
Array.Resize<byte>(ref temp, count + 1);
Array.Resize<byte>(ref temp, count + 1);
return Encoding.ASCII.GetString(temp, 0, temp.Length);
return Encoding.ASCII.GetString(temp, 0, temp.Length); }
}
public void Close()
public void Close() {
{ try
try {
{ if (client.Client != null && client.Client.Connected)
if (client.Client != null && client.Client.Connected) {
{ client.Client.Dispose();
client.Client.Dispose(); client.Dispose();
client.Dispose(); }
} }
} catch { }
catch { }
try
try {
{ client.Dispose();
client.Dispose(); }
} catch { }
catch { }
client = new TcpClient();
client = new TcpClient(); }
}
protected virtual void Dispose(bool disposing)
protected virtual void Dispose(bool disposing) {
{ if (disposing)
if (disposing) {
{ // dispose managed resources
// dispose managed resources this.Close();
this.Close(); client = null;
client = null; }
} // free native resources
// free native resources }
}
public void Dispose()
public void Dispose() {
{ Dispose(true);
Dispose(true); GC.SuppressFinalize(this);
GC.SuppressFinalize(this); }
} }
} }
}