using Newtonsoft.Json;
using Serilog;
using System;
using System.IO;
using System.Net;
using System.Net.Sockets;
using System.Reflection;
using System.Text;
using System.Threading.Tasks.Dataflow;
namespace BeiDou
{
internal class Program
{
private static TcpListener tcpListener;
private static byte[] tcp_buffer = new byte[8192];
private static Queue<char> char_buffer = new Queue<char>();
private static double fixed_location = 0;
private static double fixed_alpha = 0;
private static double fixed_beta = 0;
private static int gps_status = 0;
private static DateTime gps_time = DateTime.MinValue;
private static string configfile = "config.json";
private static ConfigData configData = new ConfigData();
private static MqttClient _client = null;
private static TcpClient _tcpClient = null;
private static PlcConnector plcConnector;
private static ApiService apiService;
public static ConfigData LoadConfig(string filePath)
{
try
{
if (File.Exists(filePath))
{
string jsonString = File.ReadAllText(filePath);
ConfigData data = JsonConvert.DeserializeObject<ConfigData>(jsonString);
return data;
}
else
{
return null;
}
}
catch (Exception ex)
{
return null;
}
}
private static void StartListener()
{
try
{
int port = configData.port;
tcpListener = new TcpListener(IPAddress.Any, port);
tcpListener.Start();
tcpListener.BeginAcceptTcpClient(AcceptCallback, tcpListener);
Log.Information($"tcp服务启动@Port {port}");
}
catch (Exception ex)
{
Log.Information($"服务器启动失败: {ex.Message}");
}
}
private static void AcceptCallback(IAsyncResult ar)
{
var r = tcpListener.EndAcceptTcpClient(ar);
Log.Information($"tcp 客户端连接来自:{r.Client?.RemoteEndPoint?.ToString()}");
if (_tcpClient != null)
{
try
{
_tcpClient.Close();
lock (char_buffer)
{
char_buffer.Clear();
}
}
catch (Exception e)
{
}
Log.Information($"断开前一个连接:{_tcpClient.Client?.RemoteEndPoint?.ToString()}");
}
_tcpClient = r;
_tcpClient.GetStream().BeginRead(tcp_buffer, 0, tcp_buffer.Length, ReadCallback, _tcpClient);
Log.Information($"开始读入数据");
tcpListener.BeginAcceptTcpClient(AcceptCallback, tcpListener);
}
private static void ReadCallback(IAsyncResult ar)
{
try
{
int bytesRead = _tcpClient.GetStream().EndRead(ar);
if (bytesRead == 0)
{
Log.Information($"客户端断开");
_tcpClient.Close();
return;
}
string data = Encoding.ASCII.GetString(tcp_buffer, 0, bytesRead);
lock (char_buffer)
{
foreach (char c in data)
{
char_buffer.Enqueue(c);
}
}
_tcpClient.GetStream().BeginRead(tcp_buffer, 0, tcp_buffer.Length, ReadCallback, _tcpClient);
gps_time = DateTime.Now;
}
catch (Exception ex)
{
Log.Information($"客户端连接出错: {ex.Message}");
//_tcpClient.GetStream().Close(); // 明确关闭流
//_tcpClient.Close(); // 明确关闭客户端
}
}
private static void ProcessBufferAsync()
{
StringBuilder sb = new StringBuilder();
string psatMessage = null;
string gpggaMessage = null;
while (true)
{
lock (char_buffer)
{
while (char_buffer.Count > 0 && char_buffer.Peek() != '$')
{
char_buffer.Dequeue();
}
while (char_buffer.Count > 0 && char_buffer.Peek() != '\n')
{
sb.Append(char_buffer.Dequeue());
}
if (char_buffer.Count > 0 && char_buffer.Peek() == '\n')
{
char_buffer.Dequeue();
string message = sb.ToString();
sb.Clear();
if (message.StartsWith("$PSAT"))
{
psatMessage = message;
}
else if (message.StartsWith("$GPGGA"))
{
gpggaMessage = message;
}
if (psatMessage != null && gpggaMessage != null)
{
Process(psatMessage, gpggaMessage);
psatMessage = null;
gpggaMessage = null;
}
}
}
Thread.Sleep(50);
}
}
private static void Process(string PSAT, string GPGGA)
{
string[] parts1 = PSAT.Split(',');
var hpr_heading = MakeContinuous(float.Parse(parts1[3]));
var hpr_pitch = float.Parse(parts1[4]);
var hpr_roll = float.Parse(parts1[5].Split('*')[0]);
string[] parts2 = GPGGA.Split(',');
double lat = double.Parse(parts2[2].Substring(0, 2)) + double.Parse(parts2[2].Substring(2)) / 60;
double lon = double.Parse(parts2[4].Substring(0, 3)) + double.Parse(parts2[4].Substring(3)) / 60;
double alt = double.Parse(parts2[9]);
if (parts2[3] == "S")
lat = -lat;
if (parts2[5] == "W")
lon = -lon;
double east, north;
WGS84ToUTM(lat, lon, out east, out north);
var latitude = lat;
var longitude = lon;
var altitude = double.Parse(parts2[9]);
var easting = east;
var northing = north;
var solution_status = parts2[6];
gps_status = Convert.ToInt32(solution_status);
var result = GetLocation(hpr_heading, hpr_pitch, easting, northing, altitude);
var locate = result.Item1;
var alpha = 270 - (hpr_heading + 180) % 360 + configData.alpha_fix;
var beta = -hpr_pitch + configData.beta_fix;
var location = locate + configData.dx + configData.location_fix;
//临时
var location_fix = location + alpha * 0.017 + 2.2;
var alpha2 = alpha * -1 + 90;
fixed_location = location_fix;
fixed_alpha = alpha2;
fixed_beta = beta;
}
private static double MakeContinuous(double angle, double reference = 180)
{
if (angle < reference)
angle += 360;
return angle - reference;
}
private static void WGS84ToUTM(double lat, double lon, out double easting, out double northing)
{
const double a = 6378137.0;
const double e = 0.081819190842;
const double k0 = 0.9996;
const int zone = 51;
double latRad = lat * Math.PI / 180.0;
double lonRad = lon * Math.PI / 180.0;
double lonOrigin = (zone - 1) * 6 - 180 + 3;
double lonOriginRad = lonOrigin * Math.PI / 180.0;
double N = a / Math.Sqrt(1 - e * e * Math.Sin(latRad) * Math.Sin(latRad));
double T = Math.Tan(latRad) * Math.Tan(latRad);
double C = e * e * Math.Cos(latRad) * Math.Cos(latRad) / (1 - e * e);
double A = (lonRad - lonOriginRad) * Math.Cos(latRad);
double M = a * ((1 - e * e / 4 - 3 * e * e * e * e / 64 - 5 * e * e * e * e * e * e / 256) * latRad
- (3 * e * e / 8 + 3 * e * e * e * e / 32 + 45 * e * e * e * e * e * e / 1024) * Math.Sin(2 * latRad)
+ (15 * e * e * e * e / 256 + 45 * e * e * e * e * e * e / 1024) * Math.Sin(4 * latRad)
- (35 * e * e * e * e * e * e / 3072) * Math.Sin(6 * latRad));
easting = k0 * N * (A + (1 - T + C) * A * A * A / 6
+ (5 - 18 * T + T * T + 72 * C - 58 * e * e) * A * A * A * A * A / 120)
+ 500000.0;
northing = k0 * (M + N * Math.Tan(latRad) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24
+ (61 - 58 * T + T * T + 600 * C - 330 * e * e) * A * A * A * A * A * A / 720));
if (lat < 0)
northing += 10000000.0; // ϰ
}
private static (double, double, double) GetLocation(double heading, double pitch, double e, double n, double alt)
{
//var dx = -0.019701;
//var dl = 0.070986;
//var dp = -0.065288;
//var dh = -1.826911;
//var dn = 1037737.1908643508;
//var w_z = -1.9326628753411224;
//var dx = -0.158508;
//var dl = 0.978683;
//var dp = -0.059042;
//var dh = -1.824058;
//var dn = 1033585.1515670037;
//var w_z = -1.931425470920315;
double heading_r = MakeContinuous(heading) * Math.PI / 180.0;
double pitch_r = pitch * Math.PI / 180.0;
//var dx = configData.dx;
//var dl = configData.dl;
//var dp = configData.dp;
//var dh = configData.dh;
var dn = configData.dn;
var de = configData.de;
var w_z = configData.w_z;
var X = dn + n * Math.Cos(w_z) - e * Math.Sin(w_z);
//var location = X + dx - (4.3 + (12.7 + dl) * Math.Cos(pitch_r + dp) * Math.Cos(heading_r + dh));
var dx = -1.73158823;
var dbeta = -0.23008302;
var dalpha = -2.0795134;
var dl2 = 0.39443275;
var beta = pitch_r + dbeta;
var alpha = heading_r + dalpha;
var l1 = 4.300;
var l2 = 20.00 + dl2;
var lt = l1 + l2 * Math.Cos(beta);
var d = 3.7;
var distance = lt * Math.Cos(alpha) - d * Math.Sin(alpha);
var location = X + dx - distance;
return (location, 0, 0);
}
static void Main(string[] args)
{
Serilog.Log.Logger = new Serilog.LoggerConfiguration()
.MinimumLevel.Information()
.WriteTo.File("log-.txt",
rollingInterval: RollingInterval.Day,
outputTemplate: "{Timestamp:yyyy-MM-dd HH:mm:ss.fff} [{Level}] [{SourceContext}] {Message}{NewLine}{Exception}"
)
.WriteTo.Console()
.CreateLogger();
Log.Information("==========程序启动==========");
configData = LoadConfig(configfile);
if (configData == null)
{
configData = new ConfigData();
}
StartListener();
Task.Run(() =>
{
while (true)
{
ProcessBufferAsync();
}
});
_client = new MqttClient(DateTime.Now.Ticks.ToString(), "127.0.0.1", 1883);
_client.Init();
apiService = new ApiService();
plcConnector = new PlcConnector();
Task.Run(() =>
{
while (true)
{
var _sec = (DateTime.Now - gps_time).TotalSeconds;
if (_sec < 2)
{
if (gps_status == 4)
{
try
{
SLState state = apiService.GetEquipmentStateAsync().Result;
var str = JsonConvert.SerializeObject(new
{
WalkPosition = fixed_location,
RotatePosition = fixed_alpha,
PitchPosition = fixed_beta,
ShenSuo = Math.Round(state.boomExtensionPosition, 2),
FuYang = state.pitchPosition,
ZouXing = state.walkPosition,
time = DateTime.Now
});
_client.Publish($"{configData.mqtt_topic}/BD", str);
plcConnector.WriteThreeFloats((float)fixed_location, (float)fixed_alpha, (float)fixed_beta);
Log.Information($"{fixed_location:0.00},{fixed_alpha:0.00},{fixed_beta:0.00},{gps_time.Millisecond},{gps_status}");
}
catch (Exception ex)
{
Log.Warning($"错误: {ex.Message}");
}
}
else
{
Log.Warning($"差分失败:{gps_status}, {_sec}");
}
}
else
{
Log.Warning($"离线:{gps_status}, {_sec}");
if (_sec > 5 && gps_time != DateTime.MinValue)
{
try
{
_tcpClient.Close();
}
catch (Exception e)
{
Log.Information($"断开失败3:{gps_status}, {_sec}");
}
}
}
Thread.Sleep(500);
}
}
);
while (true)
{
Console.ReadLine();
}
}
}
}
这是一篇关于动态规划的博客，探讨如何使用动态规划求解最大正方形问题。文章通过举例介绍了如何在给定的二进制矩阵中找到只包含1的最大正方形并返回其面积。解决方案的时间复杂度为O(m*n)。
最低0.47元/天 解锁文章
扫一扫
2883
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
