Comandos prioridade UDP
This commit is contained in:
parent
5b387de761
commit
270a7ed3d8
|
|
@ -563,6 +563,7 @@
|
||||||
<Compile Include="Models\SyncDataModel.cs" />
|
<Compile Include="Models\SyncDataModel.cs" />
|
||||||
<Compile Include="Models\TrajetoriaMapaOperacaoModel.cs" />
|
<Compile Include="Models\TrajetoriaMapaOperacaoModel.cs" />
|
||||||
<Compile Include="Models\TreinamentoIAModel.cs" />
|
<Compile Include="Models\TreinamentoIAModel.cs" />
|
||||||
|
<Compile Include="Models\UdpCtrlMessage.cs" />
|
||||||
<Compile Include="Models\UltrasonicA05Model.cs" />
|
<Compile Include="Models\UltrasonicA05Model.cs" />
|
||||||
<Compile Include="Models\Variaveis.cs" />
|
<Compile Include="Models\Variaveis.cs" />
|
||||||
<Compile Include="Models\VariaveisMonitoramento.cs" />
|
<Compile Include="Models\VariaveisMonitoramento.cs" />
|
||||||
|
|
@ -620,6 +621,7 @@
|
||||||
<Compile Include="Services\SerialService.cs" />
|
<Compile Include="Services\SerialService.cs" />
|
||||||
<Compile Include="Services\SonarService.cs" />
|
<Compile Include="Services\SonarService.cs" />
|
||||||
<Compile Include="Services\SyncDataService.cs" />
|
<Compile Include="Services\SyncDataService.cs" />
|
||||||
|
<Compile Include="Services\UdpReliableChannel.cs" />
|
||||||
<Compile Include="Services\UltrasonicA05Service.cs" />
|
<Compile Include="Services\UltrasonicA05Service.cs" />
|
||||||
<Compile Include="Services\UsrDR302Service.cs" />
|
<Compile Include="Services\UsrDR302Service.cs" />
|
||||||
<Compile Include="Services\VersionamentoService.cs" />
|
<Compile Include="Services\VersionamentoService.cs" />
|
||||||
|
|
|
||||||
|
|
@ -34,7 +34,7 @@ namespace AgroBase.Forms
|
||||||
DeviceID = "",
|
DeviceID = "",
|
||||||
combo = cmbCameras,
|
combo = cmbCameras,
|
||||||
MaxLeituras = 10,
|
MaxLeituras = 10,
|
||||||
VideoPorta = (VariaveisPortas.CameraSolo + Variaveis.OperacaoEmAndamento.DispSen.Dados.CamerasSolo.Count()).ToString(),
|
VideoPorta = "", //(VariaveisPortas.CameraSolo + Variaveis.OperacaoEmAndamento.DispSen.Dados.CamerasSolo.Count()).ToString(),
|
||||||
VideoUrl = "green_objects_T",
|
VideoUrl = "green_objects_T",
|
||||||
MqttTopico = "green_objects_T",
|
MqttTopico = "green_objects_T",
|
||||||
panel = pnlCamera,
|
panel = pnlCamera,
|
||||||
|
|
@ -98,7 +98,7 @@ namespace AgroBase.Forms
|
||||||
DeviceID = "123",
|
DeviceID = "123",
|
||||||
combo = new ComboBox(),
|
combo = new ComboBox(),
|
||||||
MaxLeituras = 1,
|
MaxLeituras = 1,
|
||||||
VideoPorta = (VariaveisPortas.CameraSolo + 0).ToString(),
|
VideoPorta = "", // (VariaveisPortas.CameraSolo + 0).ToString(),
|
||||||
VideoUrl = "green_objects_" + CamNome,
|
VideoUrl = "green_objects_" + CamNome,
|
||||||
MqttTopico = "green_objects_" + CamNome,
|
MqttTopico = "green_objects_" + CamNome,
|
||||||
};
|
};
|
||||||
|
|
|
||||||
|
|
@ -32,7 +32,7 @@ namespace AgroBase.Forms.Movimentacao
|
||||||
private Timer tmrLeitura;
|
private Timer tmrLeitura;
|
||||||
|
|
||||||
private int MaxLeituras = 1;
|
private int MaxLeituras = 1;
|
||||||
private string VideoPorta = VariaveisPortas.CameraCaminho.ToString();
|
private string VideoPorta = ""; // VariaveisPortas.CameraCaminho.ToString();
|
||||||
private string VideoUrl = "street_detector";
|
private string VideoUrl = "street_detector";
|
||||||
private bool MostrarDebug = true;
|
private bool MostrarDebug = true;
|
||||||
private string ArquivoLeitura = "leitura_solo.json";
|
private string ArquivoLeitura = "leitura_solo.json";
|
||||||
|
|
|
||||||
|
|
@ -15,7 +15,7 @@ namespace AgroBase.Forms.Sensoriamento
|
||||||
private Timer tmrLeitura;
|
private Timer tmrLeitura;
|
||||||
|
|
||||||
private int MaxLeituras = 10;
|
private int MaxLeituras = 10;
|
||||||
private string VideoPorta = VariaveisPortas.CameraCaminho.ToString();
|
private string VideoPorta = ""; // VariaveisPortas.CameraCaminho.ToString();
|
||||||
private string VideoUrl = "line_angle";
|
private string VideoUrl = "line_angle";
|
||||||
private bool MostrarDebug = true;
|
private bool MostrarDebug = true;
|
||||||
private string ArquivoLeitura = "leitura_angulo";
|
private string ArquivoLeitura = "leitura_angulo";
|
||||||
|
|
|
||||||
|
|
@ -29,6 +29,8 @@ namespace AgroBase.Forms
|
||||||
|
|
||||||
Variaveis.IniciarMQTT();
|
Variaveis.IniciarMQTT();
|
||||||
|
|
||||||
|
Variaveis.IniciarUDP();
|
||||||
|
|
||||||
APIService.IniciarRotinas();
|
APIService.IniciarRotinas();
|
||||||
|
|
||||||
GeneralJoystick.IniciarRotinas();
|
GeneralJoystick.IniciarRotinas();
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,97 @@
|
||||||
|
using System;
|
||||||
|
using System.Buffers.Binary; // se seu .NET não tiver, eu te passo versão sem isso
|
||||||
|
using System.Windows.Forms;
|
||||||
|
|
||||||
|
[Flags]
|
||||||
|
public enum UdpCtrlFlags : byte
|
||||||
|
{
|
||||||
|
None = 0,
|
||||||
|
Emergencia = 1 << 0,
|
||||||
|
Pausa = 1 << 1,
|
||||||
|
HasKey = 1 << 2,
|
||||||
|
HasPayload = 1 << 3,
|
||||||
|
}
|
||||||
|
|
||||||
|
public sealed class UdpCtrlMessage
|
||||||
|
{
|
||||||
|
public byte Version = 1;
|
||||||
|
public byte Device;
|
||||||
|
public UdpCtrlFlags Flags;
|
||||||
|
public Keys Key;
|
||||||
|
public short P1;
|
||||||
|
public short P2;
|
||||||
|
|
||||||
|
public byte[] ToBytes()
|
||||||
|
{
|
||||||
|
// 10 bytes fixos
|
||||||
|
var buf = new byte[10];
|
||||||
|
buf[0] = Version;
|
||||||
|
buf[1] = Device;
|
||||||
|
buf[2] = (byte)Flags;
|
||||||
|
|
||||||
|
// key UInt16 BE
|
||||||
|
ushort k = (ushort)Key;
|
||||||
|
buf[3] = (byte)(k >> 8);
|
||||||
|
buf[4] = (byte)(k & 0xFF);
|
||||||
|
|
||||||
|
// p1 int16 BE
|
||||||
|
buf[5] = (byte)((ushort)P1 >> 8);
|
||||||
|
buf[6] = (byte)((ushort)P1 & 0xFF);
|
||||||
|
|
||||||
|
// p2 int16 BE
|
||||||
|
buf[7] = (byte)((ushort)P2 >> 8);
|
||||||
|
buf[8] = (byte)((ushort)P2 & 0xFF);
|
||||||
|
|
||||||
|
// reservado (checksum simples futuro / seq local etc)
|
||||||
|
buf[9] = 0;
|
||||||
|
|
||||||
|
return buf;
|
||||||
|
}
|
||||||
|
|
||||||
|
public static bool TryParse(byte[] payload, out UdpCtrlMessage msg)
|
||||||
|
{
|
||||||
|
msg = null;
|
||||||
|
if (payload == null || payload.Length < 10) return false;
|
||||||
|
|
||||||
|
var m = new UdpCtrlMessage();
|
||||||
|
m.Version = payload[0];
|
||||||
|
if (m.Version != 1) return false;
|
||||||
|
|
||||||
|
m.Device = payload[1];
|
||||||
|
m.Flags = (UdpCtrlFlags)payload[2];
|
||||||
|
|
||||||
|
ushort k = (ushort)((payload[3] << 8) | payload[4]);
|
||||||
|
m.Key = (Keys)k;
|
||||||
|
|
||||||
|
m.P1 = (short)((payload[5] << 8) | payload[6]);
|
||||||
|
m.P2 = (short)((payload[7] << 8) | payload[8]);
|
||||||
|
|
||||||
|
msg = m;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public static class UdpKeyPayload
|
||||||
|
{
|
||||||
|
// [0] device
|
||||||
|
// [1..2] key (UInt16 BE)
|
||||||
|
public static byte[] Build(byte device, Keys key)
|
||||||
|
{
|
||||||
|
byte[] buf = new byte[3];
|
||||||
|
buf[0] = device;
|
||||||
|
BinaryPrimitives.WriteUInt16BigEndian(buf.AsSpan(1, 2), (ushort)key);
|
||||||
|
return buf;
|
||||||
|
}
|
||||||
|
|
||||||
|
public static bool TryParse(byte[] payload, out byte device, out Keys key)
|
||||||
|
{
|
||||||
|
device = 0;
|
||||||
|
key = Keys.None;
|
||||||
|
if (payload == null || payload.Length < 3) return false;
|
||||||
|
|
||||||
|
device = payload[0];
|
||||||
|
ushort raw = BinaryPrimitives.ReadUInt16BigEndian(payload.AsSpan(1, 2));
|
||||||
|
key = (Keys)raw;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -174,14 +174,123 @@ namespace AgroBase.Models
|
||||||
}
|
}
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
public static UdpReliableChannel UdpChannel;
|
||||||
|
|
||||||
|
// Thread-safe: guarda o último comando UDP recebido (UTC ticks). 0 = nunca/armado off
|
||||||
|
private static long _lastUdpTicksUtc = 0;
|
||||||
|
|
||||||
|
// Evita reentrância do tick (caso seu timer possa chamar antes do anterior terminar)
|
||||||
|
private static int _failSafeTickRunning = 0;
|
||||||
|
|
||||||
|
private static AsyncTaskTimerModel _tmrFailSafe;
|
||||||
|
|
||||||
|
private const int FAILSAFE_MS = 400;
|
||||||
|
|
||||||
|
public static void IniciarUDP()
|
||||||
|
{
|
||||||
|
UdpChannel = new UdpReliableChannel();
|
||||||
|
UdpChannel.Start(VariaveisPortas.Ethernet_UDP_RX);
|
||||||
|
|
||||||
|
UdpChannel.OnPacket += (type, payload, from) =>
|
||||||
|
{
|
||||||
|
if (type != 0x01) return;
|
||||||
|
if (!UdpCtrlMessage.TryParse(payload, out var msg)) return;
|
||||||
|
|
||||||
|
Interlocked.Exchange(ref _lastUdpTicksUtc, DateTime.UtcNow.Ticks);
|
||||||
|
|
||||||
|
// flags globais
|
||||||
|
bool emergencia = (msg.Flags & UdpCtrlFlags.Emergencia) != 0;
|
||||||
|
bool pausa = (msg.Flags & UdpCtrlFlags.Pausa) != 0;
|
||||||
|
|
||||||
|
if (emergencia)
|
||||||
|
{
|
||||||
|
Variaveis.OperacaoEmAndamento.Emergencia = true;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (pausa)
|
||||||
|
{
|
||||||
|
Variaveis.OperacaoEmAndamento.Pausa = true;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
var disp = (T_Code)msg.Device;
|
||||||
|
|
||||||
|
// aplica parâmetros por dispositivo (alinhado ao MQTT)
|
||||||
|
if ((msg.Flags & UdpCtrlFlags.HasPayload) != 0)
|
||||||
|
{
|
||||||
|
if (disp == T_Code.Dir)
|
||||||
|
{
|
||||||
|
double angulo = msg.P1 / 100.0;
|
||||||
|
var tipoMov = (TipoMovimentoDirecional)(byte)msg.P2;
|
||||||
|
|
||||||
|
// atualiza seu contexto de controle (igual mqtt faz)
|
||||||
|
Variaveis.OperacaoEmAndamento.Controle.Angulo = angulo;
|
||||||
|
Variaveis.OperacaoEmAndamento.Controle.TipoMovimento = tipoMov;
|
||||||
|
}
|
||||||
|
else if (disp == T_Code.Mov)
|
||||||
|
{
|
||||||
|
double velPct = msg.P1 / 100.0;
|
||||||
|
Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP = velPct;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// aplica tecla (se tiver)
|
||||||
|
if ((msg.Flags & UdpCtrlFlags.HasKey) != 0)
|
||||||
|
{
|
||||||
|
GeneralJoystick.EnviaComandoMotor(msg.Key, disp, ForcarComando: true, ID: null);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
// Tick 100ms (ok). Se quiser mais responsivo, use 50ms.
|
||||||
|
_tmrFailSafe = new AsyncTaskTimerModel("tmrFailSafe", tmrFailSafe_Tick, 100);
|
||||||
|
_tmrFailSafe.Start();
|
||||||
|
}
|
||||||
|
|
||||||
|
public static async Task tmrFailSafe_Tick()
|
||||||
|
{
|
||||||
|
// Guard anti-reentrância
|
||||||
|
if (Interlocked.Exchange(ref _failSafeTickRunning, 1) == 1)
|
||||||
|
return;
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
long ticks = Interlocked.Read(ref _lastUdpTicksUtc);
|
||||||
|
if (ticks == 0) return;
|
||||||
|
|
||||||
|
var last = new DateTime(ticks, DateTimeKind.Utc);
|
||||||
|
double ms = (DateTime.UtcNow - last).TotalMilliseconds;
|
||||||
|
|
||||||
|
if (ms > FAILSAFE_MS)
|
||||||
|
{
|
||||||
|
// Para movimento e direção por segurança
|
||||||
|
GeneralJoystick.EnviaComandoMotor(Keys.Escape, T_Code.Mov, ForcarComando: true, ID: null);
|
||||||
|
GeneralJoystick.EnviaComandoMotor(Keys.Escape, T_Code.Dir, ForcarComando: true, ID: null);
|
||||||
|
|
||||||
|
// desarma para não ficar repetindo ESC pra sempre
|
||||||
|
Interlocked.Exchange(ref _lastUdpTicksUtc, 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
finally
|
||||||
|
{
|
||||||
|
Interlocked.Exchange(ref _failSafeTickRunning, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
await Task.CompletedTask;
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public static class VariaveisPortas
|
public static class VariaveisPortas
|
||||||
{
|
{
|
||||||
public static int CameraSolo { get; } = 6522;
|
|
||||||
public static int CameraCaminho { get; } = 6544;
|
|
||||||
public static int Ethernet_AT { get; } = 8899;
|
public static int Ethernet_AT { get; } = 8899;
|
||||||
public static int Ethernet_TCP { get; } = 8899;
|
public static int Ethernet_TCP { get; } = 8899;
|
||||||
|
public static int Ethernet_UDP_TX { get; } = 5005;
|
||||||
|
public static int Ethernet_UDP_RX { get; } = 5006;
|
||||||
}
|
}
|
||||||
|
|
||||||
public static class VariaveisEquipamento
|
public static class VariaveisEquipamento
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,233 @@
|
||||||
|
using AgroBase.Models;
|
||||||
|
using System;
|
||||||
|
using System.Collections.Concurrent;
|
||||||
|
using System.Net;
|
||||||
|
using System.Net.Sockets;
|
||||||
|
using System.Threading;
|
||||||
|
using System.Threading.Tasks;
|
||||||
|
|
||||||
|
public sealed class UdpReliableChannel : IDisposable
|
||||||
|
{
|
||||||
|
public const ushort MAGIC = 0xA711;
|
||||||
|
|
||||||
|
public int AckTimeoutMs { get; set; } = 120;
|
||||||
|
public int MaxPayloadBytes { get; set; } = 1200; // evita fragmentação
|
||||||
|
|
||||||
|
public IPEndPoint Remote { get; private set; }
|
||||||
|
|
||||||
|
// type, payload, from
|
||||||
|
public event Action<byte, byte[], IPEndPoint> OnPacket;
|
||||||
|
public event Action<uint> OnAck;
|
||||||
|
|
||||||
|
private UdpClient _udp;
|
||||||
|
private Thread _rxThread;
|
||||||
|
private volatile bool _running;
|
||||||
|
|
||||||
|
private uint _seqTx = 0;
|
||||||
|
private uint _lastSeqRx = 0;
|
||||||
|
|
||||||
|
private readonly ConcurrentDictionary<uint, TaskCompletionSource<bool>> _pendingAck =
|
||||||
|
new ConcurrentDictionary<uint, TaskCompletionSource<bool>>();
|
||||||
|
|
||||||
|
public void SetRemote(string ip, int port)
|
||||||
|
{
|
||||||
|
var addr = IPAddress.Parse(ip);
|
||||||
|
|
||||||
|
if (Remote == null || !Remote.Address.Equals(addr) || Remote.Port != port)
|
||||||
|
{
|
||||||
|
Remote = new IPEndPoint(addr, port);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Start(int localPort)
|
||||||
|
{
|
||||||
|
if (_udp != null) return;
|
||||||
|
|
||||||
|
_udp = new UdpClient(localPort);
|
||||||
|
_udp.Client.ReceiveBufferSize = 1 << 20;
|
||||||
|
_udp.Client.SendBufferSize = 1 << 20;
|
||||||
|
|
||||||
|
_running = true;
|
||||||
|
_rxThread = new Thread(RxLoop) { IsBackground = true, Name = "UdpReliableChannel.Rx" };
|
||||||
|
_rxThread.Start();
|
||||||
|
}
|
||||||
|
|
||||||
|
public uint NextSeq()
|
||||||
|
{
|
||||||
|
unchecked { return ++_seqTx; }
|
||||||
|
}
|
||||||
|
|
||||||
|
public Task SendAsync(byte type, byte[] payload, bool requestAck = false)
|
||||||
|
{
|
||||||
|
uint seq = NextSeq();
|
||||||
|
uint tsMs = (uint)Environment.TickCount;
|
||||||
|
return SendInternalAsync(type, payload ?? new byte[0], seq, tsMs, requestAck);
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task SendBurstAsync(byte type, byte[] payload, int count, int intervalMs, bool requestAck = false)
|
||||||
|
{
|
||||||
|
if (count < 1) count = 1;
|
||||||
|
if (intervalMs < 0) intervalMs = 0;
|
||||||
|
|
||||||
|
for (int i = 0; i < count; i++)
|
||||||
|
{
|
||||||
|
await SendAsync(type, payload, requestAck).ConfigureAwait(false);
|
||||||
|
if (intervalMs > 0 && i < count - 1)
|
||||||
|
await Task.Delay(intervalMs).ConfigureAwait(false);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task<bool> SendAndWaitAckAsync(byte type, byte[] payload)
|
||||||
|
{
|
||||||
|
uint seq = NextSeq();
|
||||||
|
uint tsMs = (uint)Environment.TickCount;
|
||||||
|
|
||||||
|
var tcs = new TaskCompletionSource<bool>();
|
||||||
|
_pendingAck[seq] = tcs;
|
||||||
|
|
||||||
|
await SendInternalAsync(type, payload ?? new byte[0], seq, tsMs, requestAck: true).ConfigureAwait(false);
|
||||||
|
|
||||||
|
// timeout manual (framework-friendly)
|
||||||
|
var delay = Task.Delay(AckTimeoutMs);
|
||||||
|
var done = await Task.WhenAny(tcs.Task, delay).ConfigureAwait(false);
|
||||||
|
|
||||||
|
bool ok = (done == tcs.Task) && tcs.Task.Result;
|
||||||
|
TaskCompletionSource<bool> _;
|
||||||
|
_pendingAck.TryRemove(seq, out _);
|
||||||
|
return ok;
|
||||||
|
}
|
||||||
|
|
||||||
|
private Task SendInternalAsync(byte type, byte[] payload, uint seq, uint tsMs, bool requestAck)
|
||||||
|
{
|
||||||
|
if (_udp == null) throw new InvalidOperationException("Channel not started.");
|
||||||
|
if (Remote == null) throw new InvalidOperationException("Remote not set.");
|
||||||
|
|
||||||
|
if (payload.Length > MaxPayloadBytes)
|
||||||
|
throw new ArgumentException("Payload too big (" + payload.Length + " > " + MaxPayloadBytes + ").");
|
||||||
|
|
||||||
|
byte flags = (byte)(requestAck ? 1 : 0);
|
||||||
|
|
||||||
|
byte[] buf = new byte[12 + payload.Length];
|
||||||
|
// magic BE
|
||||||
|
buf[0] = (byte)(MAGIC >> 8);
|
||||||
|
buf[1] = (byte)(MAGIC & 0xFF);
|
||||||
|
buf[2] = type;
|
||||||
|
buf[3] = flags;
|
||||||
|
|
||||||
|
WriteU32BE(buf, 4, seq);
|
||||||
|
WriteU32BE(buf, 8, tsMs);
|
||||||
|
|
||||||
|
if (payload.Length > 0)
|
||||||
|
Buffer.BlockCopy(payload, 0, buf, 12, payload.Length);
|
||||||
|
|
||||||
|
// UdpClient.SendAsync existe no .NET Framework 4.6+, ok.
|
||||||
|
return _udp.SendAsync(buf, buf.Length, Remote);
|
||||||
|
}
|
||||||
|
|
||||||
|
private void RxLoop()
|
||||||
|
{
|
||||||
|
var from = new IPEndPoint(IPAddress.Any, 0);
|
||||||
|
|
||||||
|
while (_running)
|
||||||
|
{
|
||||||
|
byte[] data = null;
|
||||||
|
try
|
||||||
|
{
|
||||||
|
data = _udp.Receive(ref from); // bloqueia
|
||||||
|
}
|
||||||
|
catch
|
||||||
|
{
|
||||||
|
if (!_running) break;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (data == null || data.Length < 12) continue;
|
||||||
|
|
||||||
|
ushort magic = (ushort)((data[0] << 8) | data[1]);
|
||||||
|
if (magic != MAGIC) continue;
|
||||||
|
|
||||||
|
byte type = data[2];
|
||||||
|
byte flags = data[3];
|
||||||
|
uint seq = ReadU32BE(data, 4);
|
||||||
|
|
||||||
|
int payloadLen = data.Length - 12;
|
||||||
|
byte[] payload = new byte[payloadLen];
|
||||||
|
if (payloadLen > 0)
|
||||||
|
Buffer.BlockCopy(data, 12, payload, 0, payloadLen);
|
||||||
|
|
||||||
|
// ACK
|
||||||
|
if (type == 0xFF)
|
||||||
|
{
|
||||||
|
if (payloadLen >= 4)
|
||||||
|
{
|
||||||
|
uint ackSeq = ReadU32BE(payload, 0);
|
||||||
|
var h = OnAck;
|
||||||
|
if (h != null) h(ackSeq);
|
||||||
|
|
||||||
|
TaskCompletionSource<bool> tcs;
|
||||||
|
if (_pendingAck.TryGetValue(ackSeq, out tcs))
|
||||||
|
tcs.TrySetResult(true);
|
||||||
|
}
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// newest wins
|
||||||
|
if (seq <= _lastSeqRx) continue;
|
||||||
|
_lastSeqRx = seq;
|
||||||
|
|
||||||
|
var handler = OnPacket;
|
||||||
|
if (handler != null)
|
||||||
|
handler(type, payload, from);
|
||||||
|
|
||||||
|
// responde ACK se pedido
|
||||||
|
if ((flags & 1) != 0)
|
||||||
|
{
|
||||||
|
try { SendAckAsync(from, seq); }
|
||||||
|
catch { }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private void SendAckAsync(IPEndPoint to, uint seqAck)
|
||||||
|
{
|
||||||
|
if (_udp == null) return;
|
||||||
|
|
||||||
|
byte[] buf = new byte[12 + 4];
|
||||||
|
buf[0] = (byte)(MAGIC >> 8);
|
||||||
|
buf[1] = (byte)(MAGIC & 0xFF);
|
||||||
|
buf[2] = 0xFF; // ACK
|
||||||
|
buf[3] = 0;
|
||||||
|
|
||||||
|
WriteU32BE(buf, 4, 0); // seq do ack msg (opcional)
|
||||||
|
WriteU32BE(buf, 8, (uint)Environment.TickCount);
|
||||||
|
WriteU32BE(buf, 12, seqAck);
|
||||||
|
|
||||||
|
_udp.Send(buf, buf.Length, to);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void WriteU32BE(byte[] buf, int offset, uint v)
|
||||||
|
{
|
||||||
|
buf[offset + 0] = (byte)(v >> 24);
|
||||||
|
buf[offset + 1] = (byte)(v >> 16);
|
||||||
|
buf[offset + 2] = (byte)(v >> 8);
|
||||||
|
buf[offset + 3] = (byte)(v & 0xFF);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static uint ReadU32BE(byte[] buf, int offset)
|
||||||
|
{
|
||||||
|
return (uint)(
|
||||||
|
(buf[offset + 0] << 24) |
|
||||||
|
(buf[offset + 1] << 16) |
|
||||||
|
(buf[offset + 2] << 8) |
|
||||||
|
(buf[offset + 3])
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Dispose()
|
||||||
|
{
|
||||||
|
_running = false;
|
||||||
|
|
||||||
|
try { _udp?.Close(); } catch { }
|
||||||
|
_udp = null;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -21,6 +21,7 @@ namespace OperationControl.Models
|
||||||
|
|
||||||
APIService.IniciarRotinas();
|
APIService.IniciarRotinas();
|
||||||
Variaveis.IniciarMQTT();
|
Variaveis.IniciarMQTT();
|
||||||
|
Variaveis.IniciarUDP();
|
||||||
|
|
||||||
tmrComunicacao.Elapsed += (_, __) => tmrComunicacao_Elapsed();
|
tmrComunicacao.Elapsed += (_, __) => tmrComunicacao_Elapsed();
|
||||||
tmrComunicacao.Start();
|
tmrComunicacao.Start();
|
||||||
|
|
|
||||||
|
|
@ -12,6 +12,7 @@ namespace OperationControl.Models
|
||||||
{
|
{
|
||||||
public static MqttService MqttService;
|
public static MqttService MqttService;
|
||||||
public static GpsService GpsService;
|
public static GpsService GpsService;
|
||||||
|
public static UdpReliableChannel UdpChannel;
|
||||||
|
|
||||||
public static void MostrarLog(string message)
|
public static void MostrarLog(string message)
|
||||||
{
|
{
|
||||||
|
|
@ -49,6 +50,12 @@ namespace OperationControl.Models
|
||||||
GpsService = new GpsService();
|
GpsService = new GpsService();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public static void IniciarUDP()
|
||||||
|
{
|
||||||
|
UdpChannel = new UdpReliableChannel();
|
||||||
|
UdpChannel.Start(VariaveisPortas.Ethernet_UDP_TX);
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public class VariaveisControleOperacao
|
public class VariaveisControleOperacao
|
||||||
|
|
@ -131,6 +138,8 @@ namespace OperationControl.Models
|
||||||
if (rover != null)
|
if (rover != null)
|
||||||
{
|
{
|
||||||
obj.Momento = DateTime.Now;
|
obj.Momento = DateTime.Now;
|
||||||
|
var logs = rover.DadosLeitura?.Logs ?? new List<OperacaoSensoriamentoLogErrosModel>();
|
||||||
|
obj.Logs?.InsertRange(0, logs);
|
||||||
rover.DadosLeitura = obj;
|
rover.DadosLeitura = obj;
|
||||||
rover.UltimoContato = DateTime.Now;
|
rover.UltimoContato = DateTime.Now;
|
||||||
}
|
}
|
||||||
|
|
@ -294,6 +303,19 @@ namespace OperationControl.Models
|
||||||
EnviarDadosControle(SelectedRoverId, cmd);
|
EnviarDadosControle(SelectedRoverId, cmd);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public static Task EnviarComandoParadaUDP(string ipRover, bool emergencia = false, bool pausa = false)
|
||||||
|
{
|
||||||
|
Variaveis.UdpChannel.SetRemote(ipRover, VariaveisPortas.Ethernet_UDP_RX);
|
||||||
|
|
||||||
|
var msg = new UdpCtrlMessage
|
||||||
|
{
|
||||||
|
Device = (byte)AgroBase.Models.Enums.T_Code.Mod,
|
||||||
|
Flags = (emergencia ? UdpCtrlFlags.Emergencia : 0) | (pausa ? UdpCtrlFlags.Pausa : 0)
|
||||||
|
};
|
||||||
|
|
||||||
|
return Variaveis.UdpChannel.SendBurstAsync(0x01, msg.ToBytes(), count: 6, intervalMs: 25, requestAck: false);
|
||||||
|
}
|
||||||
|
|
||||||
public static void EnviarComandoReferenciamento(string mod_id = null)
|
public static void EnviarComandoReferenciamento(string mod_id = null)
|
||||||
{
|
{
|
||||||
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
||||||
|
|
@ -394,6 +416,22 @@ namespace OperationControl.Models
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public static Task EnviarComandoDirecionalUDP(string ipRover, Keys key, double angulo, AgroBase.Models.Enums.TipoMovimentoDirecional tipoMov)
|
||||||
|
{
|
||||||
|
Variaveis.UdpChannel.SetRemote(ipRover, VariaveisPortas.Ethernet_UDP_RX);
|
||||||
|
|
||||||
|
var msg = new UdpCtrlMessage
|
||||||
|
{
|
||||||
|
Device = (byte)AgroBase.Models.Enums.T_Code.Dir,
|
||||||
|
Flags = UdpCtrlFlags.HasKey | UdpCtrlFlags.HasPayload,
|
||||||
|
Key = key,
|
||||||
|
P1 = (short)Math.Max(short.MinValue, Math.Min(short.MaxValue, (int)Math.Round(angulo * 100.0))),
|
||||||
|
P2 = (short)tipoMov
|
||||||
|
};
|
||||||
|
|
||||||
|
return Variaveis.UdpChannel.SendBurstAsync(0x01, msg.ToBytes(), count: 6, intervalMs: 25, requestAck: false);
|
||||||
|
}
|
||||||
|
|
||||||
public static void EnviarComandoMovimentacao(AgroBase.Models.Enums.Direcao direcao, double velocidade)
|
public static void EnviarComandoMovimentacao(AgroBase.Models.Enums.Direcao direcao, double velocidade)
|
||||||
{
|
{
|
||||||
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
||||||
|
|
@ -408,6 +446,22 @@ namespace OperationControl.Models
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public static Task EnviarComandoMovimentacaoUDP(string ipRover, Keys key, double velPct)
|
||||||
|
{
|
||||||
|
Variaveis.UdpChannel.SetRemote(ipRover, VariaveisPortas.Ethernet_UDP_RX);
|
||||||
|
|
||||||
|
var msg = new UdpCtrlMessage
|
||||||
|
{
|
||||||
|
Device = (byte)AgroBase.Models.Enums.T_Code.Mov,
|
||||||
|
Flags = UdpCtrlFlags.HasKey | UdpCtrlFlags.HasPayload,
|
||||||
|
Key = key,
|
||||||
|
P1 = (short)Math.Max(short.MinValue, Math.Min(short.MaxValue, (int)Math.Round(velPct * 100.0))),
|
||||||
|
P2 = 0
|
||||||
|
};
|
||||||
|
|
||||||
|
return Variaveis.UdpChannel.SendBurstAsync(0x01, msg.ToBytes(), count: 6, intervalMs: 25, requestAck: false);
|
||||||
|
}
|
||||||
|
|
||||||
public static void EnviarComandoAtuador(string componente_id, bool? status = null, int? angulo_controle = null, double? angulo_abertura = null, double? altura = null)
|
public static void EnviarComandoAtuador(string componente_id, bool? status = null, int? angulo_controle = null, double? angulo_abertura = null, double? altura = null)
|
||||||
{
|
{
|
||||||
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,114 @@
|
||||||
|
using System;
|
||||||
|
using System.Collections.Generic;
|
||||||
|
using System.Linq;
|
||||||
|
using System.Threading;
|
||||||
|
using System.Threading.Tasks;
|
||||||
|
using System.Windows.Forms;
|
||||||
|
|
||||||
|
public sealed class ManualControlSender : IDisposable
|
||||||
|
{
|
||||||
|
private readonly UdpReliableChannel _udp;
|
||||||
|
private readonly byte _device; // ex: (byte)T_Code.Mov ou T_Code.Dir
|
||||||
|
private readonly HashSet<Keys> _pressed = new();
|
||||||
|
private readonly object _lock = new();
|
||||||
|
|
||||||
|
private CancellationTokenSource? _cts;
|
||||||
|
private Task? _loop;
|
||||||
|
|
||||||
|
// Ajuste fino
|
||||||
|
public int RepeatIntervalMs { get; set; } = 250; // 4 Hz
|
||||||
|
public int BurstDownCount { get; set; } = 6;
|
||||||
|
public int BurstDownIntervalMs { get; set; } = 25;
|
||||||
|
public int BurstStopCount { get; set; } = 10;
|
||||||
|
public int BurstStopIntervalMs { get; set; } = 25;
|
||||||
|
|
||||||
|
// protocolo: type 0x01 = KeyCmd
|
||||||
|
private const byte TYPE_KEY_CMD = 0x01;
|
||||||
|
|
||||||
|
public ManualControlSender(UdpReliableChannel udp, byte device)
|
||||||
|
{
|
||||||
|
_udp = udp;
|
||||||
|
_device = device;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Start()
|
||||||
|
{
|
||||||
|
_cts = new CancellationTokenSource();
|
||||||
|
_loop = Task.Run(() => Loop(_cts.Token));
|
||||||
|
}
|
||||||
|
|
||||||
|
public void KeyDown(Keys key)
|
||||||
|
{
|
||||||
|
if (!IsManualKey(key)) return;
|
||||||
|
|
||||||
|
lock (_lock) _pressed.Add(key);
|
||||||
|
|
||||||
|
// burst imediato pra resposta rápida
|
||||||
|
_ = SendKeyBurstAsync(key, BurstDownCount, BurstDownIntervalMs);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void KeyUp(Keys key)
|
||||||
|
{
|
||||||
|
if (!IsManualKey(key)) return;
|
||||||
|
|
||||||
|
lock (_lock) _pressed.Remove(key);
|
||||||
|
|
||||||
|
// sempre que soltar algo, manda STOP (Esc) burst
|
||||||
|
_ = SendKeyBurstAsync(Keys.Escape, BurstStopCount, BurstStopIntervalMs);
|
||||||
|
}
|
||||||
|
|
||||||
|
private async Task Loop(CancellationToken ct)
|
||||||
|
{
|
||||||
|
while (!ct.IsCancellationRequested)
|
||||||
|
{
|
||||||
|
Keys? active = null;
|
||||||
|
lock (_lock)
|
||||||
|
{
|
||||||
|
if (_pressed.Count > 0)
|
||||||
|
active = ChooseActiveKey(_pressed);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (active.HasValue)
|
||||||
|
{
|
||||||
|
// reenviar leve enquanto segurando
|
||||||
|
await SendKeyOnceAsync(active.Value);
|
||||||
|
}
|
||||||
|
|
||||||
|
try { await Task.Delay(RepeatIntervalMs, ct); }
|
||||||
|
catch (TaskCanceledException) { }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private static bool IsManualKey(Keys k) =>
|
||||||
|
k == Keys.Up || k == Keys.Down || k == Keys.Left || k == Keys.Right ||
|
||||||
|
k == Keys.Escape;
|
||||||
|
|
||||||
|
// Prioridade: se tiver Up/Down e Left/Right junto, você pode escolher uma regra
|
||||||
|
private static Keys ChooseActiveKey(IEnumerable<Keys> keys)
|
||||||
|
{
|
||||||
|
// Exemplo: prioriza ESC, depois Up/Down, depois Left/Right
|
||||||
|
if (keys.Contains(Keys.Escape)) return Keys.Escape;
|
||||||
|
if (keys.Contains(Keys.Up)) return Keys.Up;
|
||||||
|
if (keys.Contains(Keys.Down)) return Keys.Down;
|
||||||
|
if (keys.Contains(Keys.Left)) return Keys.Left;
|
||||||
|
if (keys.Contains(Keys.Right)) return Keys.Right;
|
||||||
|
return Keys.Escape;
|
||||||
|
}
|
||||||
|
|
||||||
|
private Task SendKeyBurstAsync(Keys key, int count, int intervalMs)
|
||||||
|
{
|
||||||
|
var payload = UdpKeyPayload.Build(_device, key);
|
||||||
|
return _udp.SendBurstAsync(TYPE_KEY_CMD, payload, count, intervalMs, requestAck: false);
|
||||||
|
}
|
||||||
|
|
||||||
|
private Task SendKeyOnceAsync(Keys key)
|
||||||
|
{
|
||||||
|
var payload = UdpKeyPayload.Build(_device, key);
|
||||||
|
return _udp.SendAsync(TYPE_KEY_CMD, payload, requestAck: false);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Dispose()
|
||||||
|
{
|
||||||
|
try { _cts?.Cancel(); } catch { }
|
||||||
|
}
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue