Comandos prioridade UDP
This commit is contained in:
Diego Freitas
parent
5b387de761
commit
270a7ed3d8
|
|
@ -563,6 +563,7 @@
|
|||
<Compile Include="Models\SyncDataModel.cs" />
|
||||
<Compile Include="Models\TrajetoriaMapaOperacaoModel.cs" />
|
||||
<Compile Include="Models\TreinamentoIAModel.cs" />
|
||||
<Compile Include="Models\UdpCtrlMessage.cs" />
|
||||
<Compile Include="Models\UltrasonicA05Model.cs" />
|
||||
<Compile Include="Models\Variaveis.cs" />
|
||||
<Compile Include="Models\VariaveisMonitoramento.cs" />
|
||||
|
|
@ -620,6 +621,7 @@
|
|||
<Compile Include="Services\SerialService.cs" />
|
||||
<Compile Include="Services\SonarService.cs" />
|
||||
<Compile Include="Services\SyncDataService.cs" />
|
||||
<Compile Include="Services\UdpReliableChannel.cs" />
|
||||
<Compile Include="Services\UltrasonicA05Service.cs" />
|
||||
<Compile Include="Services\UsrDR302Service.cs" />
|
||||
<Compile Include="Services\VersionamentoService.cs" />
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@ namespace AgroBase.Forms
|
|||
DeviceID = "",
|
||||
combo = cmbCameras,
|
||||
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",
|
||||
MqttTopico = "green_objects_T",
|
||||
panel = pnlCamera,
|
||||
|
|
@ -98,7 +98,7 @@ namespace AgroBase.Forms
|
|||
DeviceID = "123",
|
||||
combo = new ComboBox(),
|
||||
MaxLeituras = 1,
|
||||
VideoPorta = (VariaveisPortas.CameraSolo + 0).ToString(),
|
||||
VideoPorta = "", // (VariaveisPortas.CameraSolo + 0).ToString(),
|
||||
VideoUrl = "green_objects_" + CamNome,
|
||||
MqttTopico = "green_objects_" + CamNome,
|
||||
};
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@ namespace AgroBase.Forms.Movimentacao
|
|||
private Timer tmrLeitura;
|
||||
|
||||
private int MaxLeituras = 1;
|
||||
private string VideoPorta = VariaveisPortas.CameraCaminho.ToString();
|
||||
private string VideoPorta = ""; // VariaveisPortas.CameraCaminho.ToString();
|
||||
private string VideoUrl = "street_detector";
|
||||
private bool MostrarDebug = true;
|
||||
private string ArquivoLeitura = "leitura_solo.json";
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ namespace AgroBase.Forms.Sensoriamento
|
|||
private Timer tmrLeitura;
|
||||
|
||||
private int MaxLeituras = 10;
|
||||
private string VideoPorta = VariaveisPortas.CameraCaminho.ToString();
|
||||
private string VideoPorta = ""; // VariaveisPortas.CameraCaminho.ToString();
|
||||
private string VideoUrl = "line_angle";
|
||||
private bool MostrarDebug = true;
|
||||
private string ArquivoLeitura = "leitura_angulo";
|
||||
|
|
|
|||
|
|
@ -29,6 +29,8 @@ namespace AgroBase.Forms
|
|||
|
||||
Variaveis.IniciarMQTT();
|
||||
|
||||
Variaveis.IniciarUDP();
|
||||
|
||||
APIService.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 int CameraSolo { get; } = 6522;
|
||||
public static int CameraCaminho { get; } = 6544;
|
||||
public static int Ethernet_AT { 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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
Variaveis.IniciarMQTT();
|
||||
Variaveis.IniciarUDP();
|
||||
|
||||
tmrComunicacao.Elapsed += (_, __) => tmrComunicacao_Elapsed();
|
||||
tmrComunicacao.Start();
|
||||
|
|
|
|||
|
|
@ -12,6 +12,7 @@ namespace OperationControl.Models
|
|||
{
|
||||
public static MqttService MqttService;
|
||||
public static GpsService GpsService;
|
||||
public static UdpReliableChannel UdpChannel;
|
||||
|
||||
public static void MostrarLog(string message)
|
||||
{
|
||||
|
|
@ -49,6 +50,12 @@ namespace OperationControl.Models
|
|||
GpsService = new GpsService();
|
||||
}
|
||||
|
||||
public static void IniciarUDP()
|
||||
{
|
||||
UdpChannel = new UdpReliableChannel();
|
||||
UdpChannel.Start(VariaveisPortas.Ethernet_UDP_TX);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public class VariaveisControleOperacao
|
||||
|
|
@ -131,6 +138,8 @@ namespace OperationControl.Models
|
|||
if (rover != null)
|
||||
{
|
||||
obj.Momento = DateTime.Now;
|
||||
var logs = rover.DadosLeitura?.Logs ?? new List<OperacaoSensoriamentoLogErrosModel>();
|
||||
obj.Logs?.InsertRange(0, logs);
|
||||
rover.DadosLeitura = obj;
|
||||
rover.UltimoContato = DateTime.Now;
|
||||
}
|
||||
|
|
@ -294,6 +303,19 @@ namespace OperationControl.Models
|
|||
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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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