56 lines
1.9 KiB
C#
56 lines
1.9 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.IO.Ports;
|
|
using System.Threading.Tasks;
|
|
|
|
namespace AgroBase.Services
|
|
{
|
|
public class RTCMSniffer
|
|
{
|
|
public async Task Main(string[] args)
|
|
{
|
|
string port = args.Length > 0 ? args[0] : "COM34"; // troque para a COM do E220
|
|
var sp = new SerialPort(port, 115200, Parity.None, 8, StopBits.One);
|
|
sp.ReadTimeout = 200;
|
|
sp.Open();
|
|
Console.WriteLine($"Sniffando RTCM em {port}... (Ctrl+C para sair)");
|
|
|
|
var buf = new List<byte>(8192);
|
|
while (true)
|
|
{
|
|
try
|
|
{
|
|
int n = sp.BytesToRead;
|
|
if (n > 0)
|
|
{
|
|
var tmp = new byte[n];
|
|
sp.Read(tmp, 0, n);
|
|
buf.AddRange(tmp);
|
|
|
|
int i = 0;
|
|
while (i + 6 < buf.Count)
|
|
{
|
|
if (buf[i] != 0xD3) { i++; continue; }
|
|
if (i + 2 >= buf.Count) break;
|
|
|
|
int len = ((buf[i + 1] & 0x03) << 8) | buf[i + 2]; // 10 bits
|
|
int frameEnd = i + 3 + len + 3; // header(3) + payload + CRC(3)
|
|
if (frameEnd > buf.Count) break;
|
|
|
|
if (len >= 2)
|
|
{
|
|
int msgType = (buf[i + 3] << 4) | (buf[i + 4] >> 4); // 12 bits iniciais
|
|
Console.WriteLine($"[{DateTime.Now:HH:mm:ss.fff}] RTCM{msgType} len={len}");
|
|
}
|
|
buf.RemoveRange(0, frameEnd);
|
|
i = 0;
|
|
}
|
|
}
|
|
await Task.Delay(5);
|
|
}
|
|
catch { /* timeout/ctrl-c */ }
|
|
}
|
|
}
|
|
}
|
|
}
|