2589 lines
83 KiB
C#
2589 lines
83 KiB
C#
using AgroBase.Models;
|
|
using AgroBase.Models.Operacoes;
|
|
using AgroMonitor;
|
|
using Newtonsoft.Json;
|
|
using OperationControl.Services;
|
|
using OperationControl.Windows;
|
|
using System;
|
|
using System.Collections.Concurrent;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using System.Linq;
|
|
using System.Threading;
|
|
using System.Threading.Tasks;
|
|
using Application = System.Windows.Application;
|
|
|
|
namespace OperationControl.Models
|
|
{
|
|
/// <summary>
|
|
/// Recursos globais de comunicação da base.
|
|
///
|
|
/// O MQTT foi dividido em dois clientes independentes:
|
|
/// - Critical: RTCM, heartbeat, comandos, posição e discovery ACK;
|
|
/// - Monitoring: discovery, telemetria e parâmetros.
|
|
///
|
|
/// Essa divisão evita que JSON pesado, callbacks de interface ou
|
|
/// reconexões de monitoramento disputem a fila interna do RTCM.
|
|
/// </summary>
|
|
public class Variaveis
|
|
{
|
|
private static readonly SemaphoreSlim _mqttLifecycleLock =
|
|
new SemaphoreSlim(1, 1);
|
|
private static readonly object _udpLifecycleLock =
|
|
new object();
|
|
|
|
public static MqttService MqttServiceCritical { get; private set; }
|
|
public static MqttService MqttServiceMonitoring { get; private set; }
|
|
|
|
/// <summary>
|
|
/// Ponte temporária de compatibilidade.
|
|
/// Código novo deve escolher explicitamente Critical ou Monitoring.
|
|
/// </summary>
|
|
public static MqttService MqttService
|
|
{
|
|
get { return MqttServiceMonitoring; }
|
|
set { MqttServiceMonitoring = value; }
|
|
}
|
|
|
|
public static MqttService.MqttTopicosModel TopicoRtcm { get; private set; }
|
|
public static MqttService.MqttTopicosModel TopicoPosicaoBase { get; private set; }
|
|
public static MqttService.MqttTopicosModel TopicoDiscovery { get; private set; }
|
|
|
|
public static RtcmPublisherService RtcmPublisher { get; private set; }
|
|
|
|
public static GpsService GpsService;
|
|
public static UdpReliableChannel UdpChannel;
|
|
public static ManualControlSender ControlSenderDir;
|
|
public static ManualControlSender ControlSenderMov;
|
|
|
|
public static AppShell? Shell => ((App)Application.Current)?.Shell;
|
|
public static DockWindow? Dock => Shell?.Dock;
|
|
|
|
public static void MostrarLog(string message)
|
|
{
|
|
Debug.WriteLine(message);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Compatibilidade com chamadas antigas. O startup deve preferir:
|
|
/// await Variaveis.IniciarMqttAsync();
|
|
/// </summary>
|
|
public static Task IniciarMQTT()
|
|
{
|
|
return IniciarMqttAsync();
|
|
}
|
|
|
|
public static async Task IniciarMqttAsync(
|
|
CancellationToken cancellationToken = default(CancellationToken))
|
|
{
|
|
await _mqttLifecycleLock
|
|
.WaitAsync(cancellationToken)
|
|
.ConfigureAwait(false);
|
|
|
|
try
|
|
{
|
|
await EncerrarMqttInternoAsync(
|
|
"Reinicialização MQTT"
|
|
).ConfigureAwait(false);
|
|
|
|
VariaveisControleOperacao.ResetarTopicosMqtt();
|
|
|
|
MqttServiceCritical = new MqttService(
|
|
"localhost",
|
|
1883,
|
|
"base-critical",
|
|
true,
|
|
msg => MostrarLog(
|
|
"[MQTT CRITICAL localhost:1883] - " + msg
|
|
)
|
|
);
|
|
|
|
MqttServiceMonitoring = new MqttService(
|
|
"localhost",
|
|
1883,
|
|
"base-monitoring",
|
|
true,
|
|
msg => MostrarLog(
|
|
"[MQTT MONITORING localhost:1883] - " + msg
|
|
)
|
|
);
|
|
|
|
await ConfigurarTopicosBaseAsync()
|
|
.ConfigureAwait(false);
|
|
|
|
RtcmPublisher = new RtcmPublisherService(
|
|
() => MqttServiceCritical,
|
|
() => TopicoRtcm,
|
|
MostrarLog
|
|
);
|
|
|
|
RtcmPublisher.Start();
|
|
|
|
await Task.WhenAll(
|
|
MqttServiceCritical.StartAsync(),
|
|
MqttServiceMonitoring.StartAsync()
|
|
).ConfigureAwait(false);
|
|
|
|
if (GpsService == null)
|
|
GpsService = new GpsService();
|
|
|
|
await VariaveisControleOperacao
|
|
.ReconfigurarRoversConhecidosAsync()
|
|
.ConfigureAwait(false);
|
|
}
|
|
catch
|
|
{
|
|
await EncerrarMqttInternoAsync(
|
|
"Falha durante inicialização MQTT"
|
|
).ConfigureAwait(false);
|
|
|
|
throw;
|
|
}
|
|
finally
|
|
{
|
|
_mqttLifecycleLock.Release();
|
|
}
|
|
}
|
|
|
|
private static async Task ConfigurarTopicosBaseAsync()
|
|
{
|
|
TopicoPosicaoBase =
|
|
await MqttServiceCritical.AdicionarNovoTopico(
|
|
VariaveisMonitoramento.TopicoMqttPosicao,
|
|
inscrever: false,
|
|
mensagensManter: 0,
|
|
callback: null,
|
|
payloadType: MqttPayloadType.Text,
|
|
dispatchMode: MqttDispatchMode.LatestOnly,
|
|
queueCapacity: 1,
|
|
overflowPolicy:
|
|
MqttQueueOverflowPolicy.DropOldest,
|
|
qos:
|
|
MqttQosLevel.AtMostOnce,
|
|
retain: false
|
|
).ConfigureAwait(false);
|
|
|
|
TopicoRtcm =
|
|
await MqttServiceCritical.AdicionarNovoTopico(
|
|
VariaveisMonitoramento.TopicoMqttRTCM,
|
|
inscrever: false,
|
|
mensagensManter: 0,
|
|
callback: null,
|
|
payloadType: MqttPayloadType.Binary,
|
|
dispatchMode: MqttDispatchMode.Sequential,
|
|
queueCapacity: 32,
|
|
overflowPolicy:
|
|
MqttQueueOverflowPolicy.DropOldest,
|
|
qos:
|
|
MqttQosLevel.AtMostOnce,
|
|
retain: false
|
|
).ConfigureAwait(false);
|
|
|
|
TopicoDiscovery =
|
|
await MqttServiceMonitoring.AdicionarNovoTopico(
|
|
VariaveisMonitoramento
|
|
.TopicoMqttDispositivos,
|
|
inscrever: true,
|
|
mensagensManter: 2,
|
|
callback: async message =>
|
|
{
|
|
try
|
|
{
|
|
RoverDiscoveryInfo discovery =
|
|
RoverDiscoveryInfo.Parse(
|
|
message.Mensagem
|
|
);
|
|
|
|
if (discovery == null)
|
|
return;
|
|
|
|
await VariaveisControleOperacao
|
|
.AdicionarNovoRoverNaRede(
|
|
discovery.RoverId,
|
|
discovery.RoverIp,
|
|
discovery.SessionId
|
|
).ConfigureAwait(false);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
MostrarLog(
|
|
"Erro ao processar discovery do rover: " +
|
|
ex.Message
|
|
);
|
|
}
|
|
},
|
|
payloadType: MqttPayloadType.Text,
|
|
dispatchMode:
|
|
MqttDispatchMode.Sequential,
|
|
queueCapacity: 128,
|
|
overflowPolicy:
|
|
MqttQueueOverflowPolicy.DropOldest,
|
|
qos:
|
|
MqttQosLevel.AtMostOnce,
|
|
retain: false
|
|
).ConfigureAwait(false);
|
|
}
|
|
|
|
public static async Task EncerrarMqttAsync()
|
|
{
|
|
await _mqttLifecycleLock
|
|
.WaitAsync()
|
|
.ConfigureAwait(false);
|
|
|
|
try
|
|
{
|
|
await EncerrarMqttInternoAsync(
|
|
"Encerramento solicitado"
|
|
).ConfigureAwait(false);
|
|
}
|
|
finally
|
|
{
|
|
_mqttLifecycleLock.Release();
|
|
}
|
|
}
|
|
|
|
private static async Task EncerrarMqttInternoAsync(
|
|
string reason)
|
|
{
|
|
RtcmPublisherService rtcm = RtcmPublisher;
|
|
RtcmPublisher = null;
|
|
|
|
if (rtcm != null)
|
|
{
|
|
try
|
|
{
|
|
await rtcm.StopAsync()
|
|
.ConfigureAwait(false);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
MostrarLog(
|
|
"Erro ao encerrar publisher RTCM: " +
|
|
ex.Message
|
|
);
|
|
}
|
|
|
|
rtcm.Dispose();
|
|
}
|
|
|
|
MqttService critical = MqttServiceCritical;
|
|
MqttService monitoring = MqttServiceMonitoring;
|
|
|
|
MqttServiceCritical = null;
|
|
MqttServiceMonitoring = null;
|
|
|
|
TopicoRtcm = null;
|
|
TopicoPosicaoBase = null;
|
|
TopicoDiscovery = null;
|
|
|
|
VariaveisControleOperacao.ResetarTopicosMqtt();
|
|
|
|
if (critical != null)
|
|
{
|
|
try
|
|
{
|
|
await critical.DisposeAsync()
|
|
.ConfigureAwait(false);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
MostrarLog(
|
|
"Erro ao encerrar MQTT crítico: " +
|
|
ex.Message
|
|
);
|
|
}
|
|
}
|
|
|
|
if (monitoring != null)
|
|
{
|
|
try
|
|
{
|
|
await monitoring.DisposeAsync()
|
|
.ConfigureAwait(false);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
MostrarLog(
|
|
"Erro ao encerrar MQTT de monitoramento: " +
|
|
ex.Message
|
|
);
|
|
}
|
|
}
|
|
|
|
if (!string.IsNullOrWhiteSpace(reason))
|
|
MostrarLog("[MQTT] " + reason);
|
|
}
|
|
|
|
public static OperationControlCommunicationMetrics
|
|
GetCommunicationMetrics()
|
|
{
|
|
return new OperationControlCommunicationMetrics
|
|
{
|
|
Critical =
|
|
MqttServiceCritical?.GetMetrics(),
|
|
Monitoring =
|
|
MqttServiceMonitoring?.GetMetrics(),
|
|
Rtcm =
|
|
RtcmPublisher?.GetMetrics(),
|
|
RoverCount =
|
|
VariaveisControleOperacao
|
|
.GetRoversSnapshot()
|
|
.Count
|
|
};
|
|
}
|
|
|
|
public static void IniciarUDP()
|
|
{
|
|
lock (_udpLifecycleLock)
|
|
{
|
|
/*
|
|
* Garante idempotência.
|
|
* Se a base reiniciar a comunicação, não deixa canal UDP
|
|
* e senders antigos rodando no fundo.
|
|
*/
|
|
StopUdpChannelInterno();
|
|
|
|
UdpReliableChannel channel = new UdpReliableChannel
|
|
{
|
|
EnableHeartbeat = true,
|
|
HeartbeatIntervalMs = 1000,
|
|
HeartbeatPayload = new byte[] { 0 }
|
|
};
|
|
|
|
channel.Start(VariaveisPortas.Ethernet_UDP_TX);
|
|
|
|
UdpChannel = channel;
|
|
|
|
ControlSenderDir = new ManualControlSender(UdpChannel, (byte)AgroBase.Models.Enums.T_Code.Dir);
|
|
|
|
ControlSenderDir.Start();
|
|
|
|
ControlSenderMov = new ManualControlSender(UdpChannel, (byte)AgroBase.Models.Enums.T_Code.Mov);
|
|
|
|
ControlSenderMov.Start();
|
|
|
|
MostrarLog("[UDP] Canal UDP iniciado.");
|
|
}
|
|
}
|
|
|
|
public static void StopUdpChannel()
|
|
{
|
|
lock (_udpLifecycleLock)
|
|
{
|
|
StopUdpChannelInterno();
|
|
}
|
|
}
|
|
|
|
public static void EncerrarUDP()
|
|
{
|
|
StopUdpChannel();
|
|
}
|
|
|
|
private static void StopUdpChannelInterno()
|
|
{
|
|
ManualControlSender senderDir = ControlSenderDir;
|
|
ManualControlSender senderMov = ControlSenderMov;
|
|
UdpReliableChannel channel = UdpChannel;
|
|
|
|
ControlSenderDir = null;
|
|
ControlSenderMov = null;
|
|
UdpChannel = null;
|
|
|
|
PararObjetoSePossivel(
|
|
senderDir,
|
|
"ManualControlSender DIR"
|
|
);
|
|
|
|
PararObjetoSePossivel(
|
|
senderMov,
|
|
"ManualControlSender MOV"
|
|
);
|
|
|
|
if (channel != null)
|
|
{
|
|
try
|
|
{
|
|
channel.SetRemote(
|
|
null,
|
|
VariaveisPortas.Ethernet_UDP_RX
|
|
);
|
|
}
|
|
catch { }
|
|
|
|
PararObjetoSePossivel(
|
|
channel,
|
|
"UdpReliableChannel"
|
|
);
|
|
}
|
|
|
|
MostrarLog("[UDP] Canal UDP encerrado.");
|
|
}
|
|
|
|
private static void PararObjetoSePossivel(
|
|
object obj,
|
|
string nome)
|
|
{
|
|
if (obj == null)
|
|
return;
|
|
|
|
try
|
|
{
|
|
InvocarMetodoSemParametroSeExistir(
|
|
obj,
|
|
"Stop"
|
|
);
|
|
|
|
InvocarMetodoSemParametroSeExistir(
|
|
obj,
|
|
"Close"
|
|
);
|
|
|
|
if (obj is IDisposable disposable)
|
|
{
|
|
disposable.Dispose();
|
|
}
|
|
else
|
|
{
|
|
InvocarMetodoSemParametroSeExistir(
|
|
obj,
|
|
"Dispose"
|
|
);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
MostrarLog(
|
|
"[UDP] Erro ao encerrar " +
|
|
nome + ": " + ex.Message
|
|
);
|
|
}
|
|
}
|
|
|
|
private static void InvocarMetodoSemParametroSeExistir(
|
|
object obj,
|
|
string metodo)
|
|
{
|
|
if (obj == null ||
|
|
string.IsNullOrWhiteSpace(metodo))
|
|
{
|
|
return;
|
|
}
|
|
|
|
var method = obj
|
|
.GetType()
|
|
.GetMethod(
|
|
metodo,
|
|
Type.EmptyTypes
|
|
);
|
|
|
|
if (method == null)
|
|
return;
|
|
|
|
method.Invoke(obj, null);
|
|
}
|
|
|
|
}
|
|
|
|
public class VariaveisControleOperacao
|
|
{
|
|
public static double LeverArmFrontal { get; } = 0.0;
|
|
public static double LeverArmLateral { get; } = 0.0;
|
|
|
|
public static readonly string BaseMarkerID = "BASE";
|
|
|
|
private static readonly object _RoversLock =
|
|
new object();
|
|
|
|
private static readonly ConcurrentDictionary<
|
|
string,
|
|
SemaphoreSlim> _roverSetupLocks =
|
|
new ConcurrentDictionary<
|
|
string,
|
|
SemaphoreSlim>(
|
|
StringComparer.OrdinalIgnoreCase
|
|
);
|
|
|
|
private static readonly ConcurrentDictionary<
|
|
string,
|
|
long> _roverLastContactMono =
|
|
new ConcurrentDictionary<
|
|
string,
|
|
long>(
|
|
StringComparer.OrdinalIgnoreCase
|
|
);
|
|
|
|
private static readonly ConcurrentDictionary<
|
|
string,
|
|
MqttService.MqttTopicosModel>
|
|
_heartbeatTopics =
|
|
new ConcurrentDictionary<
|
|
string,
|
|
MqttService.MqttTopicosModel>(
|
|
StringComparer.OrdinalIgnoreCase
|
|
);
|
|
|
|
private static readonly ConcurrentDictionary<
|
|
string,
|
|
MqttService.MqttTopicosModel>
|
|
_commandTopics =
|
|
new ConcurrentDictionary<
|
|
string,
|
|
MqttService.MqttTopicosModel>(
|
|
StringComparer.OrdinalIgnoreCase
|
|
);
|
|
|
|
private static readonly ConcurrentDictionary<
|
|
string,
|
|
MqttService.MqttTopicosModel>
|
|
_telemetryTopics =
|
|
new ConcurrentDictionary<
|
|
string,
|
|
MqttService.MqttTopicosModel>(
|
|
StringComparer.OrdinalIgnoreCase
|
|
);
|
|
|
|
private static readonly ConcurrentDictionary<
|
|
string,
|
|
MqttService.MqttTopicosModel>
|
|
_parameterTopics =
|
|
new ConcurrentDictionary<
|
|
string,
|
|
MqttService.MqttTopicosModel>(
|
|
StringComparer.OrdinalIgnoreCase
|
|
);
|
|
|
|
private static readonly ConcurrentDictionary<
|
|
string,
|
|
MqttService.MqttTopicosModel>
|
|
_discoveryAckTopics =
|
|
new ConcurrentDictionary<
|
|
string,
|
|
MqttService.MqttTopicosModel>(
|
|
StringComparer.OrdinalIgnoreCase
|
|
);
|
|
|
|
private static readonly ConcurrentDictionary<
|
|
string,
|
|
byte> _telemetryUiScheduled =
|
|
new ConcurrentDictionary<
|
|
string,
|
|
byte>(
|
|
StringComparer.OrdinalIgnoreCase
|
|
);
|
|
|
|
private static readonly ConcurrentDictionary<
|
|
string,
|
|
byte> _parameterUiScheduled =
|
|
new ConcurrentDictionary<
|
|
string,
|
|
byte>(
|
|
StringComparer.OrdinalIgnoreCase
|
|
);
|
|
|
|
private static int _updatingRovers;
|
|
|
|
public static double TempoRoverVivo = 5.0;
|
|
|
|
public static AgroBase.Models.Enums.StatusModulo
|
|
StatusBase
|
|
{
|
|
get
|
|
{
|
|
if ((Variaveis.GpsService?.IsConnected ?? false) &&
|
|
(Variaveis.GpsService?
|
|
.BaseFix?
|
|
.CorrecaoAbsoluta ?? false))
|
|
{
|
|
return AgroBase.Models.Enums
|
|
.StatusModulo.Operante;
|
|
}
|
|
|
|
if (Variaveis.GpsService?.IsConnected ?? false)
|
|
{
|
|
return AgroBase.Models.Enums
|
|
.StatusModulo.Alerta;
|
|
}
|
|
|
|
return AgroBase.Models.Enums
|
|
.StatusModulo.Desconectado;
|
|
}
|
|
}
|
|
|
|
public static List<OperacaoParametrosModel>
|
|
RoversNaRede { get; set; } =
|
|
new List<OperacaoParametrosModel>();
|
|
|
|
public static string SelectedRoverId { get; set; } =
|
|
BaseMarkerID;
|
|
|
|
public static OperacaoParametrosModel? RoverEmFoco
|
|
{
|
|
get
|
|
{
|
|
lock (_RoversLock)
|
|
{
|
|
return RoversNaRede.FirstOrDefault(
|
|
x => x.RoverId == SelectedRoverId
|
|
);
|
|
}
|
|
}
|
|
}
|
|
|
|
public static bool BaseEmFoco =>
|
|
SelectedRoverId == BaseMarkerID;
|
|
|
|
public static List<OperacaoParametrosModel>
|
|
GetRoversSnapshot()
|
|
{
|
|
lock (_RoversLock)
|
|
{
|
|
return new List<OperacaoParametrosModel>(
|
|
RoversNaRede
|
|
);
|
|
}
|
|
}
|
|
|
|
internal static void ResetarTopicosMqtt()
|
|
{
|
|
_heartbeatTopics.Clear();
|
|
_commandTopics.Clear();
|
|
_telemetryTopics.Clear();
|
|
_parameterTopics.Clear();
|
|
_discoveryAckTopics.Clear();
|
|
}
|
|
|
|
internal static async Task
|
|
ReconfigurarRoversConhecidosAsync()
|
|
{
|
|
List<OperacaoParametrosModel> snapshot =
|
|
GetRoversSnapshot();
|
|
|
|
foreach (OperacaoParametrosModel rover in snapshot)
|
|
{
|
|
if (rover == null ||
|
|
string.IsNullOrWhiteSpace(rover.RoverId))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
await GarantirTopicosRoverAsync(
|
|
rover.RoverId
|
|
).ConfigureAwait(false);
|
|
}
|
|
}
|
|
|
|
public static async Task AdicionarNovoRoverNaRede(
|
|
string device_id,
|
|
string device_ip,
|
|
string sessionId = null)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(device_id))
|
|
return;
|
|
|
|
device_id = device_id.Trim();
|
|
device_ip = (device_ip ?? string.Empty).Trim();
|
|
|
|
SemaphoreSlim gate =
|
|
_roverSetupLocks.GetOrAdd(
|
|
device_id,
|
|
_ => new SemaphoreSlim(1, 1)
|
|
);
|
|
|
|
await gate.WaitAsync().ConfigureAwait(false);
|
|
|
|
try
|
|
{
|
|
bool novoRover = false;
|
|
OperacaoParametrosModel rover;
|
|
|
|
lock (_RoversLock)
|
|
{
|
|
rover = RoversNaRede.FirstOrDefault(
|
|
x => string.Equals(
|
|
x.RoverId,
|
|
device_id,
|
|
StringComparison.OrdinalIgnoreCase
|
|
)
|
|
);
|
|
|
|
if (rover == null)
|
|
{
|
|
rover = new OperacaoParametrosModel
|
|
{
|
|
RoverId = device_id,
|
|
IP = device_ip,
|
|
Configurado = false,
|
|
Alive = true,
|
|
UltimoContato = DateTime.Now,
|
|
DadosLeitura =
|
|
new OperacaoParametrosDadosModel
|
|
{
|
|
Momento = DateTime.Now
|
|
}
|
|
};
|
|
|
|
RoversNaRede.Add(rover);
|
|
novoRover = true;
|
|
}
|
|
else
|
|
{
|
|
if (!string.IsNullOrWhiteSpace(device_ip))
|
|
rover.IP = device_ip;
|
|
|
|
rover.Alive = true;
|
|
rover.UltimoContato = DateTime.Now;
|
|
}
|
|
}
|
|
|
|
MarcarContatoRover(device_id);
|
|
|
|
await GarantirTopicosRoverAsync(device_id)
|
|
.ConfigureAwait(false);
|
|
|
|
if (!string.IsNullOrWhiteSpace(sessionId))
|
|
{
|
|
await EnviarDiscoveryAckAsync(
|
|
device_id,
|
|
sessionId
|
|
).ConfigureAwait(false);
|
|
}
|
|
|
|
if (novoRover)
|
|
{
|
|
await RequisitarParametrosOperacaoAsync(
|
|
device_id
|
|
).ConfigureAwait(false);
|
|
}
|
|
|
|
AgendarAtualizacaoListaRovers();
|
|
}
|
|
finally
|
|
{
|
|
gate.Release();
|
|
}
|
|
}
|
|
|
|
private static async Task GarantirTopicosRoverAsync(
|
|
string roverId)
|
|
{
|
|
MqttService critical =
|
|
Variaveis.MqttServiceCritical;
|
|
|
|
MqttService monitoring =
|
|
Variaveis.MqttServiceMonitoring;
|
|
|
|
if (critical == null || monitoring == null)
|
|
return;
|
|
|
|
string heartbeatName =
|
|
VariaveisEquipamento
|
|
.TopicoMqttHeartbeat
|
|
.Replace("<id>", roverId);
|
|
|
|
string commandName =
|
|
VariaveisEquipamento
|
|
.TopicoMqttComandos
|
|
.Replace("<id>", roverId);
|
|
|
|
string telemetryName =
|
|
VariaveisEquipamento
|
|
.TopicoMqttTelemetria
|
|
.Replace("<id>", roverId);
|
|
|
|
string parameterName =
|
|
VariaveisEquipamento
|
|
.TopicoMqttParametros
|
|
.Replace("<id>", roverId);
|
|
|
|
if (!_heartbeatTopics.ContainsKey(roverId))
|
|
{
|
|
MqttService.MqttTopicosModel topic =
|
|
await critical.AdicionarNovoTopico(
|
|
heartbeatName,
|
|
inscrever: false,
|
|
mensagensManter: 0,
|
|
callback: null,
|
|
payloadType: MqttPayloadType.Text,
|
|
dispatchMode:
|
|
MqttDispatchMode.LatestOnly,
|
|
queueCapacity: 1,
|
|
overflowPolicy:
|
|
MqttQueueOverflowPolicy
|
|
.DropOldest,
|
|
qos:
|
|
MqttQosLevel
|
|
.AtMostOnce,
|
|
retain: false
|
|
).ConfigureAwait(false);
|
|
|
|
_heartbeatTopics[roverId] = topic;
|
|
}
|
|
|
|
if (!_commandTopics.ContainsKey(roverId))
|
|
{
|
|
MqttService.MqttTopicosModel topic =
|
|
await critical.AdicionarNovoTopico(
|
|
commandName,
|
|
inscrever: false,
|
|
mensagensManter: 0,
|
|
callback: null,
|
|
payloadType: MqttPayloadType.Text,
|
|
dispatchMode:
|
|
MqttDispatchMode.Sequential,
|
|
queueCapacity: 64,
|
|
overflowPolicy:
|
|
MqttQueueOverflowPolicy
|
|
.DropNewest,
|
|
qos:
|
|
MqttQosLevel
|
|
.AtLeastOnce,
|
|
retain: false
|
|
).ConfigureAwait(false);
|
|
|
|
_commandTopics[roverId] = topic;
|
|
}
|
|
|
|
if (!_telemetryTopics.ContainsKey(roverId))
|
|
{
|
|
MqttService.MqttTopicosModel topic =
|
|
await monitoring.AdicionarNovoTopico(
|
|
telemetryName,
|
|
inscrever: true,
|
|
mensagensManter: 1,
|
|
callback: message =>
|
|
ProcessarTelemetriaAsync(
|
|
roverId,
|
|
message
|
|
),
|
|
payloadType: MqttPayloadType.Text,
|
|
dispatchMode:
|
|
MqttDispatchMode.LatestOnly,
|
|
queueCapacity: 1,
|
|
overflowPolicy:
|
|
MqttQueueOverflowPolicy
|
|
.DropOldest,
|
|
qos:
|
|
MqttQosLevel
|
|
.AtMostOnce,
|
|
retain: false
|
|
).ConfigureAwait(false);
|
|
|
|
_telemetryTopics[roverId] = topic;
|
|
}
|
|
|
|
if (!_parameterTopics.ContainsKey(roverId))
|
|
{
|
|
MqttService.MqttTopicosModel topic =
|
|
await monitoring.AdicionarNovoTopico(
|
|
parameterName,
|
|
inscrever: true,
|
|
mensagensManter: 1,
|
|
callback: message =>
|
|
ProcessarParametrosAsync(
|
|
roverId,
|
|
message
|
|
),
|
|
payloadType: MqttPayloadType.Text,
|
|
dispatchMode:
|
|
MqttDispatchMode.LatestOnly,
|
|
queueCapacity: 1,
|
|
overflowPolicy:
|
|
MqttQueueOverflowPolicy
|
|
.DropOldest,
|
|
qos:
|
|
MqttQosLevel
|
|
.AtLeastOnce,
|
|
retain: false
|
|
).ConfigureAwait(false);
|
|
|
|
_parameterTopics[roverId] = topic;
|
|
}
|
|
}
|
|
|
|
private static Task ProcessarTelemetriaAsync(
|
|
string roverId,
|
|
MqttService.MqttTopicosMensagensModel message)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(message.Mensagem))
|
|
return Task.CompletedTask;
|
|
|
|
try
|
|
{
|
|
OperacaoParametrosDadosModel obj =
|
|
JsonConvert.DeserializeObject<
|
|
OperacaoParametrosDadosModel>(
|
|
message.Mensagem
|
|
);
|
|
|
|
if (obj == null)
|
|
return Task.CompletedTask;
|
|
|
|
OperacaoParametrosModel rover = null;
|
|
|
|
lock (_RoversLock)
|
|
{
|
|
rover = RoversNaRede.FirstOrDefault(
|
|
x => string.Equals(
|
|
x.RoverId,
|
|
roverId,
|
|
StringComparison.OrdinalIgnoreCase
|
|
)
|
|
);
|
|
|
|
if (rover != null)
|
|
{
|
|
obj.Momento = DateTime.Now;
|
|
|
|
List<
|
|
OperacaoSensoriamentoLogErrosModel>
|
|
logsAnteriores =
|
|
rover.DadosLeitura?.Logs ??
|
|
new List<
|
|
OperacaoSensoriamentoLogErrosModel>();
|
|
|
|
if (obj.Logs == null)
|
|
{
|
|
obj.Logs =
|
|
new List<
|
|
OperacaoSensoriamentoLogErrosModel>();
|
|
}
|
|
|
|
if (logsAnteriores.Count > 0)
|
|
{
|
|
obj.Logs.InsertRange(
|
|
0,
|
|
logsAnteriores
|
|
);
|
|
}
|
|
|
|
rover.DadosLeitura = obj;
|
|
rover.UltimoContato = DateTime.Now;
|
|
rover.Alive = true;
|
|
}
|
|
}
|
|
|
|
MarcarContatoRover(roverId);
|
|
|
|
if (rover != null)
|
|
AgendarAtualizacaoTelemetriaUi(rover);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"Erro ao deserializar telemetria do rover " +
|
|
roverId + ": " + ex.Message
|
|
);
|
|
}
|
|
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
private static Task ProcessarParametrosAsync(
|
|
string roverId,
|
|
MqttService.MqttTopicosMensagensModel message)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(message.Mensagem))
|
|
return Task.CompletedTask;
|
|
|
|
try
|
|
{
|
|
OperacaoParametrosModel obj =
|
|
JsonConvert.DeserializeObject<
|
|
OperacaoParametrosModel>(
|
|
message.Mensagem
|
|
);
|
|
|
|
if (obj == null)
|
|
return Task.CompletedTask;
|
|
|
|
lock (_RoversLock)
|
|
{
|
|
int index = RoversNaRede.FindIndex(
|
|
x => string.Equals(
|
|
x.RoverId,
|
|
roverId,
|
|
StringComparison.OrdinalIgnoreCase
|
|
)
|
|
);
|
|
|
|
if (index >= 0)
|
|
{
|
|
List<
|
|
OperacaoSensoriamentoLogErrosModel>
|
|
logs =
|
|
RoversNaRede[index]
|
|
.DadosLeitura?
|
|
.Logs ??
|
|
new List<
|
|
OperacaoSensoriamentoLogErrosModel>();
|
|
|
|
obj.RoverId = roverId;
|
|
obj.UltimoContato = DateTime.Now;
|
|
obj.Alive = true;
|
|
|
|
if (obj.DadosLeitura == null)
|
|
{
|
|
obj.DadosLeitura =
|
|
new OperacaoParametrosDadosModel();
|
|
}
|
|
|
|
if (obj.DadosLeitura.Logs == null)
|
|
{
|
|
obj.DadosLeitura.Logs =
|
|
new List<
|
|
OperacaoSensoriamentoLogErrosModel>();
|
|
}
|
|
|
|
obj.DadosLeitura.Logs.AddRange(logs);
|
|
RoversNaRede[index] = obj;
|
|
}
|
|
}
|
|
|
|
MarcarContatoRover(roverId);
|
|
AgendarAtualizacaoParametrosUi(obj);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"Erro ao deserializar parâmetros do rover " +
|
|
roverId + ": " + ex.Message
|
|
);
|
|
}
|
|
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
public static Task AtualizarListaRoversNaRede()
|
|
{
|
|
if (Interlocked.Exchange(
|
|
ref _updatingRovers,
|
|
1) == 1)
|
|
{
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
try
|
|
{
|
|
List<OperacaoParametrosModel> snapshot =
|
|
GetRoversSnapshot();
|
|
|
|
foreach (OperacaoParametrosModel rover
|
|
in snapshot)
|
|
{
|
|
bool alive =
|
|
AppShell.Mock ||
|
|
GetRoverContactAgeSeconds(
|
|
rover.RoverId,
|
|
rover.UltimoContato
|
|
) < TempoRoverVivo;
|
|
|
|
lock (_RoversLock)
|
|
{
|
|
OperacaoParametrosModel current =
|
|
RoversNaRede.FirstOrDefault(
|
|
x => string.Equals(
|
|
x.RoverId,
|
|
rover.RoverId,
|
|
StringComparison
|
|
.OrdinalIgnoreCase
|
|
)
|
|
);
|
|
|
|
if (current == null)
|
|
continue;
|
|
|
|
current.Alive = alive;
|
|
|
|
if (!alive)
|
|
{
|
|
current.DadosLeitura =
|
|
new OperacaoParametrosDadosModel
|
|
{
|
|
Momento = DateTime.Now,
|
|
Operacao =
|
|
new OperacaoParametrosDadosOperacaoModel
|
|
{
|
|
Status =
|
|
AgroBase.Models.Enums
|
|
.StatusOperacao
|
|
.Erro
|
|
}
|
|
};
|
|
}
|
|
}
|
|
|
|
if (!alive)
|
|
{
|
|
Variaveis.Shell?
|
|
.AdicionarAlertaDock(
|
|
rover.RoverId,
|
|
AgroBase.Models.Enums.T_Code.Ipb,
|
|
SeveridadeAlerta.Critical,
|
|
"Perda de comunicação com o equipamento!"
|
|
);
|
|
|
|
Variaveis.Dock?
|
|
._vm?
|
|
._viewOperacaoCenter?
|
|
.Mapa?
|
|
.markers?
|
|
.UpdateMarkerInfo(
|
|
rover.RoverId,
|
|
status:
|
|
AgroBase.Models.Enums
|
|
.StatusOperacao.Erro
|
|
);
|
|
}
|
|
else
|
|
{
|
|
Variaveis.Shell?
|
|
.RemoverAlertaDock(
|
|
rover.RoverId,
|
|
AgroBase.Models.Enums.T_Code.Ipb
|
|
);
|
|
}
|
|
|
|
Variaveis.Shell?
|
|
.Dock?
|
|
._vm?
|
|
.AtualizarBarraSuperior(rover);
|
|
}
|
|
|
|
Application.Current?
|
|
.Dispatcher?
|
|
.BeginInvoke(
|
|
new Action(() =>
|
|
{
|
|
Variaveis.Shell?
|
|
.Dock?
|
|
._vm?
|
|
.AtualizarListaRovers(
|
|
snapshot
|
|
);
|
|
})
|
|
);
|
|
}
|
|
finally
|
|
{
|
|
Interlocked.Exchange(
|
|
ref _updatingRovers,
|
|
0
|
|
);
|
|
}
|
|
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
public static async Task EnviarDadosHeartbeat()
|
|
{
|
|
MqttService service =
|
|
Variaveis.MqttServiceCritical;
|
|
|
|
if (service == null)
|
|
return;
|
|
|
|
KeyValuePair<
|
|
string,
|
|
MqttService.MqttTopicosModel>[] topics =
|
|
_heartbeatTopics.ToArray();
|
|
|
|
foreach (var item in topics)
|
|
{
|
|
MqttPublishResult result =
|
|
await service.PublishWithResultAsync(
|
|
item.Value,
|
|
"0"
|
|
).ConfigureAwait(false);
|
|
|
|
if (!result.Succeeded &&
|
|
result.Status !=
|
|
MqttPublishStatus.Disconnected)
|
|
{
|
|
Variaveis.MostrarLog(
|
|
"Falha no heartbeat para " +
|
|
item.Key + ": " + result.Error
|
|
);
|
|
}
|
|
}
|
|
}
|
|
|
|
private static void EnviarDadosControle(
|
|
string rover_id,
|
|
OperacaoComandoBaseModel controle)
|
|
{
|
|
Forget(
|
|
EnviarDadosControleAsync(
|
|
rover_id,
|
|
controle
|
|
),
|
|
"Enviar comando para " + rover_id
|
|
);
|
|
}
|
|
|
|
private static async Task<MqttPublishResult>
|
|
EnviarDadosControleAsync(
|
|
string roverId,
|
|
OperacaoComandoBaseModel controle,
|
|
CancellationToken cancellationToken =
|
|
default(CancellationToken))
|
|
{
|
|
if (string.IsNullOrWhiteSpace(roverId) ||
|
|
roverId == BaseMarkerID ||
|
|
controle == null)
|
|
{
|
|
return MqttPublishResult.Failure(
|
|
MqttPublishStatus.InvalidTopic,
|
|
"Rover ou comando inválido."
|
|
);
|
|
}
|
|
|
|
await GarantirTopicosRoverAsync(roverId)
|
|
.ConfigureAwait(false);
|
|
|
|
MqttService.MqttTopicosModel topic;
|
|
|
|
if (!_commandTopics.TryGetValue(
|
|
roverId,
|
|
out topic))
|
|
{
|
|
return MqttPublishResult.Failure(
|
|
MqttPublishStatus.InvalidTopic,
|
|
"Tópico de comandos não configurado."
|
|
);
|
|
}
|
|
|
|
string json =
|
|
JsonConvert.SerializeObject(controle);
|
|
|
|
return await Variaveis
|
|
.MqttServiceCritical
|
|
.PublishWithResultAsync(
|
|
topic,
|
|
json,
|
|
cancellationToken:
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
}
|
|
|
|
public static void EnviarDadosPosicao(
|
|
GPSModel posicao)
|
|
{
|
|
Forget(
|
|
EnviarDadosPosicaoAsync(posicao),
|
|
"Enviar posição da base"
|
|
);
|
|
}
|
|
|
|
public static async Task<MqttPublishResult>
|
|
EnviarDadosPosicaoAsync(
|
|
GPSModel posicao,
|
|
CancellationToken cancellationToken =
|
|
default(CancellationToken))
|
|
{
|
|
if (posicao == null ||
|
|
Variaveis.MqttServiceCritical == null ||
|
|
Variaveis.TopicoPosicaoBase == null)
|
|
{
|
|
return MqttPublishResult.Failure(
|
|
MqttPublishStatus.InvalidTopic,
|
|
"Posição, serviço ou tópico inválido."
|
|
);
|
|
}
|
|
|
|
string json =
|
|
JsonConvert.SerializeObject(posicao);
|
|
|
|
return await Variaveis
|
|
.MqttServiceCritical
|
|
.PublishWithResultAsync(
|
|
Variaveis.TopicoPosicaoBase,
|
|
json,
|
|
cancellationToken:
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Assinatura usada pelo GpsService da base.
|
|
/// A chamada apenas copia/enfileira e retorna imediatamente.
|
|
/// </summary>
|
|
public static void EnviarDadosCorrecaoRTCM(
|
|
byte[] correcao)
|
|
{
|
|
RtcmPublisherService publisher =
|
|
Variaveis.RtcmPublisher;
|
|
|
|
if (publisher == null)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"O publicador RTCM da base não está iniciado."
|
|
);
|
|
}
|
|
|
|
if (!publisher.TryEnqueue(correcao))
|
|
{
|
|
throw new ArgumentException(
|
|
"O frame RTCM é inválido ou o publicador está encerrado.",
|
|
nameof(correcao)
|
|
);
|
|
}
|
|
}
|
|
|
|
public static bool TentarEnfileirarCorrecaoRTCM(
|
|
byte[] correcao)
|
|
{
|
|
return Variaveis.RtcmPublisher?
|
|
.TryEnqueue(correcao) == true;
|
|
}
|
|
|
|
public static RtcmPublisherMetrics
|
|
GetRtcmPublisherMetrics()
|
|
{
|
|
return Variaveis.RtcmPublisher?
|
|
.GetMetrics();
|
|
}
|
|
|
|
private static async Task
|
|
RequisitarParametrosOperacaoAsync(
|
|
string roverId)
|
|
{
|
|
await EnviarDadosControleAsync(
|
|
roverId,
|
|
new OperacaoComandoBaseModel
|
|
{
|
|
Momento = DateTime.Now,
|
|
Dispositivo =
|
|
AgroBase.Models.Enums.T_Code.Mod,
|
|
Tecla =
|
|
AgroBase.Models.Enums
|
|
.BotoesJoystick.Share
|
|
}
|
|
).ConfigureAwait(false);
|
|
}
|
|
|
|
private static async Task EnviarDiscoveryAckAsync(
|
|
string roverId,
|
|
string sessionId)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(roverId) ||
|
|
string.IsNullOrWhiteSpace(sessionId))
|
|
{
|
|
return;
|
|
}
|
|
|
|
MqttService critical =
|
|
Variaveis.MqttServiceCritical;
|
|
|
|
if (critical == null)
|
|
return;
|
|
|
|
MqttService.MqttTopicosModel topic;
|
|
|
|
if (!_discoveryAckTopics.TryGetValue(
|
|
roverId,
|
|
out topic))
|
|
{
|
|
topic =
|
|
await critical.AdicionarNovoTopico(
|
|
BuildDiscoveryAckTopic(roverId),
|
|
inscrever: false,
|
|
mensagensManter: 0,
|
|
callback: null,
|
|
payloadType: MqttPayloadType.Text,
|
|
dispatchMode:
|
|
MqttDispatchMode.LatestOnly,
|
|
queueCapacity: 1,
|
|
overflowPolicy:
|
|
MqttQueueOverflowPolicy
|
|
.DropOldest,
|
|
qos:
|
|
MqttQosLevel
|
|
.AtMostOnce,
|
|
retain: false
|
|
).ConfigureAwait(false);
|
|
|
|
_discoveryAckTopics[roverId] = topic;
|
|
}
|
|
|
|
var ack = new RoverDiscoveryAck
|
|
{
|
|
RoverId = roverId,
|
|
SessionId = sessionId,
|
|
BaseId = BaseMarkerID,
|
|
Accepted = true,
|
|
SentAtUtc = DateTime.UtcNow
|
|
};
|
|
|
|
await critical.PublishWithResultAsync(
|
|
topic,
|
|
JsonConvert.SerializeObject(ack)
|
|
).ConfigureAwait(false);
|
|
}
|
|
|
|
private static string BuildDiscoveryAckTopic(
|
|
string roverId)
|
|
{
|
|
return "agrobot/v1/rover/" +
|
|
roverId +
|
|
"/discovery_ack";
|
|
}
|
|
|
|
private static void MarcarContatoRover(
|
|
string roverId)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(roverId))
|
|
return;
|
|
|
|
_roverLastContactMono[roverId] =
|
|
Stopwatch.GetTimestamp();
|
|
}
|
|
|
|
private static double GetRoverContactAgeSeconds(
|
|
string roverId,
|
|
DateTime fallback)
|
|
{
|
|
long timestamp;
|
|
|
|
if (_roverLastContactMono.TryGetValue(
|
|
roverId,
|
|
out timestamp) &&
|
|
timestamp > 0)
|
|
{
|
|
long delta =
|
|
Stopwatch.GetTimestamp() -
|
|
timestamp;
|
|
|
|
if (delta <= 0)
|
|
return 0;
|
|
|
|
return delta /
|
|
(double)Stopwatch.Frequency;
|
|
}
|
|
|
|
if (fallback == DateTime.MinValue)
|
|
return double.PositiveInfinity;
|
|
|
|
return Math.Max(
|
|
0,
|
|
(DateTime.Now - fallback)
|
|
.TotalSeconds
|
|
);
|
|
}
|
|
|
|
private static void
|
|
AgendarAtualizacaoTelemetriaUi(
|
|
OperacaoParametrosModel rover)
|
|
{
|
|
if (rover == null ||
|
|
string.IsNullOrWhiteSpace(rover.RoverId))
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (!_telemetryUiScheduled.TryAdd(
|
|
rover.RoverId,
|
|
0))
|
|
{
|
|
return;
|
|
}
|
|
|
|
Application.Current?
|
|
.Dispatcher?
|
|
.BeginInvoke(
|
|
new Action(() =>
|
|
{
|
|
try
|
|
{
|
|
Variaveis.Dock?
|
|
._vm?
|
|
.AtualizarDadosTela(rover);
|
|
}
|
|
finally
|
|
{
|
|
byte ignored;
|
|
|
|
_telemetryUiScheduled
|
|
.TryRemove(
|
|
rover.RoverId,
|
|
out ignored
|
|
);
|
|
}
|
|
})
|
|
);
|
|
}
|
|
|
|
private static void
|
|
AgendarAtualizacaoParametrosUi(
|
|
OperacaoParametrosModel parametros)
|
|
{
|
|
if (parametros == null ||
|
|
string.IsNullOrWhiteSpace(
|
|
parametros.RoverId))
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (!_parameterUiScheduled.TryAdd(
|
|
parametros.RoverId,
|
|
0))
|
|
{
|
|
return;
|
|
}
|
|
|
|
Application.Current?
|
|
.Dispatcher?
|
|
.BeginInvoke(
|
|
new Action(() =>
|
|
{
|
|
try
|
|
{
|
|
Variaveis.Dock?
|
|
._vm?
|
|
.AtualizarParametrosRover(
|
|
parametros
|
|
);
|
|
}
|
|
finally
|
|
{
|
|
byte ignored;
|
|
|
|
_parameterUiScheduled
|
|
.TryRemove(
|
|
parametros.RoverId,
|
|
out ignored
|
|
);
|
|
}
|
|
})
|
|
);
|
|
}
|
|
|
|
private static void
|
|
AgendarAtualizacaoListaRovers()
|
|
{
|
|
Application.Current?
|
|
.Dispatcher?
|
|
.BeginInvoke(
|
|
new Action(() =>
|
|
{
|
|
Variaveis.Shell?
|
|
.Dock?
|
|
._vm?
|
|
.AtualizarListaRovers(
|
|
GetRoversSnapshot()
|
|
);
|
|
})
|
|
);
|
|
}
|
|
|
|
private static void Forget(
|
|
Task task,
|
|
string context)
|
|
{
|
|
if (task == null)
|
|
return;
|
|
|
|
_ = task.ContinueWith(
|
|
completed =>
|
|
{
|
|
Exception ex =
|
|
completed.Exception?
|
|
.GetBaseException();
|
|
|
|
if (ex != null)
|
|
{
|
|
Variaveis.MostrarLog(
|
|
context + ": " + ex.Message
|
|
);
|
|
}
|
|
},
|
|
CancellationToken.None,
|
|
TaskContinuationOptions
|
|
.OnlyOnFaulted |
|
|
TaskContinuationOptions
|
|
.ExecuteSynchronously,
|
|
TaskScheduler.Default
|
|
);
|
|
}
|
|
|
|
#region Controle
|
|
|
|
public static void EnviarComandoIniciarUDP(string ipRover, bool iniciar)
|
|
{
|
|
if (iniciar)
|
|
Variaveis.UdpChannel.SetRemote(ipRover, VariaveisPortas.Ethernet_UDP_RX);
|
|
else
|
|
Variaveis.UdpChannel.SetRemote(null, VariaveisPortas.Ethernet_UDP_RX);
|
|
|
|
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
|
{
|
|
Momento = DateTime.Now,
|
|
Dispositivo = AgroBase.Models.Enums.T_Code.Ipb,
|
|
_comp_value = iniciar
|
|
});
|
|
}
|
|
|
|
public static void EnviarComandoIniciarOperacao(bool iniciar, bool simulador = false)
|
|
{
|
|
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
|
{
|
|
Momento = DateTime.Now,
|
|
Dispositivo = AgroBase.Models.Enums.T_Code.Mod,
|
|
Tecla = simulador ? AgroBase.Models.Enums.BotoesJoystick.L1 : iniciar ? AgroBase.Models.Enums.BotoesJoystick.L2 : AgroBase.Models.Enums.BotoesJoystick.L3
|
|
});
|
|
}
|
|
|
|
public static void EnviarComandoTransmissaoVideo(AgroBase.Models.Enums.T_Code disp, bool ligar, AgroBase.Models.Enums.TipoFrameCamera tipoFrame)
|
|
{
|
|
if (string.IsNullOrEmpty(SelectedRoverId) || SelectedRoverId == BaseMarkerID) return;
|
|
|
|
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
|
{
|
|
Momento = DateTime.Now,
|
|
Dispositivo = disp,
|
|
Tecla = ligar ? AgroBase.Models.Enums.BotoesJoystick.L1 : AgroBase.Models.Enums.BotoesJoystick.Vazio,
|
|
_comp_value = tipoFrame
|
|
});
|
|
}
|
|
|
|
public static void EnviarComandoParada(bool? emergencia = null, bool? pausa = null)
|
|
{
|
|
var cmd = new OperacaoComandoBaseModel() { Momento = DateTime.Now };
|
|
if (emergencia != null) cmd.Emergencia = (bool)emergencia;
|
|
if (pausa != null) cmd.Pausa = (bool)pausa;
|
|
if (emergencia != null || pausa != null)
|
|
EnviarDadosControle(SelectedRoverId, cmd);
|
|
}
|
|
|
|
public static Task EnviarComandoParadaUDP(bool emergencia = false, bool pausa = false)
|
|
{
|
|
var msg = new UdpCtrlMessage
|
|
{
|
|
Device = (byte)AgroBase.Models.Enums.T_Code.Vzo,
|
|
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()
|
|
{
|
|
Momento = DateTime.Now,
|
|
Dispositivo = AgroBase.Models.Enums.T_Code.Dir,
|
|
Tecla = AgroBase.Models.Enums.BotoesJoystick.R3,
|
|
_comp_id = mod_id
|
|
});
|
|
}
|
|
|
|
public static void EnviarComandoLeverArm(double? lateral = null, double? frontal = null)
|
|
{
|
|
var cmd = new OperacaoComandoBaseModel()
|
|
{
|
|
Momento = DateTime.Now,
|
|
Dispositivo = AgroBase.Models.Enums.T_Code.Gps,
|
|
_comp_value = (lateral, frontal)
|
|
};
|
|
EnviarDadosControle(SelectedRoverId, cmd);
|
|
}
|
|
|
|
public static void EnviarParametrosOperacao(OperacaoParametrosModel parametros)
|
|
{
|
|
bool operacao_iniciada = (RoverEmFoco?.DadosLeitura?.Operacao?.Iniciada ?? false);
|
|
bool parametros_parciais = parametros.Modo == null;
|
|
|
|
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
|
{
|
|
Momento = DateTime.Now,
|
|
Dispositivo = AgroBase.Models.Enums.T_Code.Mod,
|
|
Parametros = parametros,
|
|
Tecla = (operacao_iniciada || parametros_parciais ? AgroBase.Models.Enums.BotoesJoystick.Touchpad : AgroBase.Models.Enums.BotoesJoystick.Options)
|
|
});
|
|
}
|
|
|
|
public static void EnviarConfirmacaoHumanaTrajetoria(int idx_corredor, bool bat_liberada, bool herb_liberado)
|
|
{
|
|
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
|
{
|
|
Momento = DateTime.Now,
|
|
Dispositivo = AgroBase.Models.Enums.T_Code.Mod,
|
|
Tecla = AgroBase.Models.Enums.BotoesJoystick.R1,
|
|
_comp_value = (idx_corredor, bat_liberada, herb_liberado)
|
|
});
|
|
}
|
|
|
|
public static void EnviarComandoCoolerControl(AgroBase.Services.CoolerControlService.CoolerMode modo, AgroBase.Models.Enums.Estado? entrada = null, AgroBase.Models.Enums.Estado? saida = null, double? temp_on = null, double? temp_turbo = null)
|
|
{
|
|
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
|
{
|
|
Momento = DateTime.Now,
|
|
Dispositivo = AgroBase.Models.Enums.T_Code.Npc,
|
|
_comp_value = (modo, entrada, saida, temp_on, temp_turbo)
|
|
});
|
|
}
|
|
|
|
public static void RequisitarParametrosOperacao()
|
|
{
|
|
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
|
{
|
|
Momento = DateTime.Now,
|
|
Dispositivo = AgroBase.Models.Enums.T_Code.Mod,
|
|
Tecla = AgroBase.Models.Enums.BotoesJoystick.Share
|
|
});
|
|
}
|
|
|
|
private static AgroBase.Models.Enums.BotoesJoystick DeParaDirecaoTecla(AgroBase.Models.Enums.Direcao direcao)
|
|
{
|
|
switch (direcao)
|
|
{
|
|
case AgroBase.Models.Enums.Direcao.Esquerda:
|
|
return AgroBase.GeneralJoystick.DeParaComandos.FirstOrDefault(x => x.key == Keys.Left)?.botaoJoy ?? AgroBase.Models.Enums.BotoesJoystick.Vazio;
|
|
case AgroBase.Models.Enums.Direcao.Direita:
|
|
return AgroBase.GeneralJoystick.DeParaComandos.FirstOrDefault(x => x.key == Keys.Right)?.botaoJoy ?? AgroBase.Models.Enums.BotoesJoystick.Vazio;
|
|
case AgroBase.Models.Enums.Direcao.Cima:
|
|
return AgroBase.GeneralJoystick.DeParaComandos.FirstOrDefault(x => x.key == Keys.Up)?.botaoJoy ?? AgroBase.Models.Enums.BotoesJoystick.Vazio;
|
|
case AgroBase.Models.Enums.Direcao.Baixo:
|
|
return AgroBase.GeneralJoystick.DeParaComandos.FirstOrDefault(x => x.key == Keys.Down)?.botaoJoy ?? AgroBase.Models.Enums.BotoesJoystick.Vazio;
|
|
default:
|
|
return AgroBase.Models.Enums.BotoesJoystick.Vazio;
|
|
}
|
|
}
|
|
|
|
public static void EnviarComandoDirecional(AgroBase.Models.Enums.BotoesJoystick botao, bool solto, double angulo, AgroBase.Models.Enums.TipoMovimentoDirecional movimento, string mod_id = null)
|
|
{
|
|
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
|
{
|
|
Momento = DateTime.Now,
|
|
Dispositivo = AgroBase.Models.Enums.T_Code.Dir,
|
|
Tecla = botao,
|
|
Solto = solto,
|
|
_comp_id = mod_id,
|
|
Controle = new OperacaoComandoBaseControleModel()
|
|
{
|
|
AnguloSP = angulo,
|
|
TipoMovimentoDirecional = movimento
|
|
},
|
|
});
|
|
}
|
|
|
|
public static void EnviarComandoDirecionalUDP(AgroBase.Models.Enums.BotoesJoystick botao, bool solto, double angulo, AgroBase.Models.Enums.TipoMovimentoDirecional tipoMov)
|
|
{
|
|
var msg = new UdpCtrlMessage
|
|
{
|
|
Device = (byte)AgroBase.Models.Enums.T_Code.Dir,
|
|
Flags = UdpCtrlFlags.HasKey | UdpCtrlFlags.HasPayload,
|
|
Key = botao,
|
|
released = solto,
|
|
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);
|
|
Variaveis.ControlSenderDir.Update(msg);
|
|
}
|
|
|
|
public static void EnviarComandoMovimentacao(AgroBase.Models.Enums.BotoesJoystick botao, bool solto, double velocidade)
|
|
{
|
|
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
|
{
|
|
Momento = DateTime.Now,
|
|
Dispositivo = AgroBase.Models.Enums.T_Code.Mov,
|
|
Tecla = botao,
|
|
Solto = solto,
|
|
Controle = new OperacaoComandoBaseControleModel()
|
|
{
|
|
PercentualVelocidadeSP = velocidade
|
|
}
|
|
});
|
|
}
|
|
|
|
public static void EnviarComandoMovimentacaoUDP(AgroBase.Models.Enums.BotoesJoystick botao, bool solto, double velPct, bool emfreio)
|
|
{
|
|
var msg = new UdpCtrlMessage
|
|
{
|
|
Device = (byte)AgroBase.Models.Enums.T_Code.Mov,
|
|
Flags = UdpCtrlFlags.HasKey | UdpCtrlFlags.HasPayload,
|
|
Key = botao,
|
|
released = solto,
|
|
P1 = (short)Math.Max(short.MinValue, Math.Min(short.MaxValue, (int)Math.Round(velPct * 100.0))),
|
|
P2 = (short)(emfreio ? 1 : 0)
|
|
};
|
|
|
|
//return Variaveis.UdpChannel.SendBurstAsync(0x01, msg.ToBytes(), count: 6, intervalMs: 25, requestAck: false);
|
|
Variaveis.ControlSenderMov.Update(msg);
|
|
}
|
|
|
|
public static void EnviarComandoAtuador(string componente_id, bool? status = null, int? angulo_controle = null, double? angulo_abertura = null, double? altura = null, double? potencia = null, double? pressao = null)
|
|
{
|
|
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
|
{
|
|
Momento = DateTime.Now,
|
|
Dispositivo = AgroBase.Models.Enums.T_Code.Atu,
|
|
_comp_id = componente_id,
|
|
_comp_value = angulo_abertura,
|
|
Controle = new OperacaoComandoBaseControleModel()
|
|
{
|
|
AnguloSP = angulo_controle,
|
|
Estado = status,
|
|
Altura = altura,
|
|
Potencia = potencia,
|
|
Pressao = pressao
|
|
}
|
|
});
|
|
}
|
|
|
|
public static void EnviarComandoSensoriamento(string componente_id, dynamic status)
|
|
{
|
|
EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel()
|
|
{
|
|
Momento = DateTime.Now,
|
|
Dispositivo = AgroBase.Models.Enums.T_Code.Sen,
|
|
_comp_id = componente_id,
|
|
_comp_value = status
|
|
});
|
|
}
|
|
|
|
public static void EnviarComandoRetornoBase(double? lat = null, double? lon = null, List<double[]> pontosRetorno = null)
|
|
{
|
|
var cmd = new OperacaoComandoBaseModel()
|
|
{
|
|
Momento = DateTime.Now,
|
|
Dispositivo = AgroBase.Models.Enums.T_Code.Trj,
|
|
_comp_value = (lat, lon, pontosRetorno)
|
|
};
|
|
EnviarDadosControle(SelectedRoverId, cmd);
|
|
}
|
|
|
|
#endregion
|
|
|
|
}
|
|
|
|
/// <summary>
|
|
/// Publicador exclusivo de RTCM.
|
|
///
|
|
/// Mantém somente a mensagem pendente mais recente por tipo RTCM,
|
|
/// publica sequencialmente com QoS 0 e nunca retransmite correção velha.
|
|
/// </summary>
|
|
public sealed class RtcmPublisherService : IDisposable
|
|
{
|
|
private readonly Func<MqttService> _serviceProvider;
|
|
private readonly Func<MqttService.MqttTopicosModel> _topicProvider;
|
|
private readonly Action<string> _log;
|
|
|
|
private readonly object _queueLock = new object();
|
|
private readonly object _metricsLock = new object();
|
|
private readonly Dictionary<int, RtcmPublishEnvelope>
|
|
_latestByType =
|
|
new Dictionary<int, RtcmPublishEnvelope>();
|
|
|
|
private readonly SemaphoreSlim _signal =
|
|
new SemaphoreSlim(0, int.MaxValue);
|
|
|
|
private CancellationTokenSource _cts;
|
|
private Task _worker;
|
|
private int _started;
|
|
private int _disposed;
|
|
|
|
private long _sequence;
|
|
private long _received;
|
|
private long _queued;
|
|
private long _replaced;
|
|
private long _published;
|
|
private long _publishedBytes;
|
|
private long _droppedInvalid;
|
|
private long _droppedStale;
|
|
private long _droppedDisconnected;
|
|
private long _publishTimeouts;
|
|
private long _publishErrors;
|
|
private long _consecutiveFailures;
|
|
|
|
private long _lastReceivedMono;
|
|
private long _lastPublishedMono;
|
|
private double _lastPublishDurationMs;
|
|
private double _averagePublishDurationMs;
|
|
private int _lastMessageType = -1;
|
|
private string _lastError;
|
|
|
|
private const int MaxMessageBytes = 64 * 1024;
|
|
private const int MaxPendingTypes = 32;
|
|
private const int MaxAgeMs = 2500;
|
|
|
|
public RtcmPublisherService(
|
|
Func<MqttService> serviceProvider,
|
|
Func<MqttService.MqttTopicosModel>
|
|
topicProvider,
|
|
Action<string> log)
|
|
{
|
|
_serviceProvider =
|
|
serviceProvider ??
|
|
throw new ArgumentNullException(
|
|
nameof(serviceProvider)
|
|
);
|
|
|
|
_topicProvider =
|
|
topicProvider ??
|
|
throw new ArgumentNullException(
|
|
nameof(topicProvider)
|
|
);
|
|
|
|
_log = log ?? (_ => { });
|
|
}
|
|
|
|
public void Start()
|
|
{
|
|
ThrowIfDisposed();
|
|
|
|
if (Interlocked.Exchange(
|
|
ref _started,
|
|
1) == 1)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_cts = new CancellationTokenSource();
|
|
|
|
_worker = Task.Run(
|
|
() => WorkerLoopAsync(_cts.Token),
|
|
_cts.Token
|
|
);
|
|
}
|
|
|
|
public bool TryEnqueue(byte[] frame)
|
|
{
|
|
if (Volatile.Read(ref _started) != 1 ||
|
|
Volatile.Read(ref _disposed) == 1 ||
|
|
frame == null ||
|
|
frame.Length < 6 ||
|
|
frame.Length > MaxMessageBytes ||
|
|
frame[0] != 0xD3)
|
|
{
|
|
Interlocked.Increment(
|
|
ref _droppedInvalid
|
|
);
|
|
|
|
return false;
|
|
}
|
|
|
|
int type = GetMessageType(frame);
|
|
|
|
if (type < 0)
|
|
{
|
|
Interlocked.Increment(
|
|
ref _droppedInvalid
|
|
);
|
|
|
|
return false;
|
|
}
|
|
|
|
byte[] copy = new byte[frame.Length];
|
|
|
|
Buffer.BlockCopy(
|
|
frame,
|
|
0,
|
|
copy,
|
|
0,
|
|
frame.Length
|
|
);
|
|
|
|
var envelope = new RtcmPublishEnvelope
|
|
{
|
|
Data = copy,
|
|
Type = type,
|
|
Sequence = Interlocked.Increment(
|
|
ref _sequence
|
|
),
|
|
ReceivedMono =
|
|
Stopwatch.GetTimestamp(),
|
|
ReceivedUtc = DateTime.UtcNow
|
|
};
|
|
|
|
Interlocked.Increment(ref _received);
|
|
Interlocked.Exchange(
|
|
ref _lastReceivedMono,
|
|
envelope.ReceivedMono
|
|
);
|
|
|
|
bool signal;
|
|
|
|
lock (_queueLock)
|
|
{
|
|
signal = _latestByType.Count == 0;
|
|
|
|
if (_latestByType.ContainsKey(type))
|
|
{
|
|
Interlocked.Increment(
|
|
ref _replaced
|
|
);
|
|
}
|
|
else if (_latestByType.Count >=
|
|
MaxPendingTypes)
|
|
{
|
|
RtcmPublishEnvelope oldest =
|
|
_latestByType.Values
|
|
.OrderBy(x => x.Sequence)
|
|
.First();
|
|
|
|
_latestByType.Remove(oldest.Type);
|
|
|
|
Interlocked.Increment(
|
|
ref _droppedStale
|
|
);
|
|
}
|
|
|
|
_latestByType[type] = envelope;
|
|
Interlocked.Increment(ref _queued);
|
|
}
|
|
|
|
if (signal)
|
|
{
|
|
try { _signal.Release(); }
|
|
catch (SemaphoreFullException) { }
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
private async Task WorkerLoopAsync(
|
|
CancellationToken cancellationToken)
|
|
{
|
|
while (!cancellationToken
|
|
.IsCancellationRequested)
|
|
{
|
|
try
|
|
{
|
|
await _signal.WaitAsync(
|
|
cancellationToken
|
|
).ConfigureAwait(false);
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
break;
|
|
}
|
|
|
|
while (!cancellationToken
|
|
.IsCancellationRequested)
|
|
{
|
|
List<RtcmPublishEnvelope> batch;
|
|
|
|
lock (_queueLock)
|
|
{
|
|
if (_latestByType.Count == 0)
|
|
break;
|
|
|
|
batch = _latestByType.Values
|
|
.OrderBy(x => x.Sequence)
|
|
.ToList();
|
|
|
|
_latestByType.Clear();
|
|
}
|
|
|
|
foreach (RtcmPublishEnvelope envelope
|
|
in batch)
|
|
{
|
|
if (cancellationToken
|
|
.IsCancellationRequested)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (AgeMs(envelope.ReceivedMono) >
|
|
MaxAgeMs)
|
|
{
|
|
Interlocked.Increment(
|
|
ref _droppedStale
|
|
);
|
|
|
|
continue;
|
|
}
|
|
|
|
MqttService service =
|
|
_serviceProvider();
|
|
|
|
MqttService.MqttTopicosModel topic =
|
|
_topicProvider();
|
|
|
|
if (service == null ||
|
|
topic == null ||
|
|
!service.StatusConexao())
|
|
{
|
|
Interlocked.Increment(
|
|
ref _droppedDisconnected
|
|
);
|
|
|
|
Interlocked.Increment(
|
|
ref _consecutiveFailures
|
|
);
|
|
|
|
SetLastError(
|
|
"MQTT crítico desconectado."
|
|
);
|
|
|
|
continue;
|
|
}
|
|
|
|
MqttPublishResult result;
|
|
|
|
try
|
|
{
|
|
result =
|
|
await service
|
|
.PublishWithResultAsync(
|
|
topic,
|
|
envelope.Data,
|
|
cancellationToken:
|
|
cancellationToken
|
|
)
|
|
.ConfigureAwait(false);
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
return;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Interlocked.Increment(
|
|
ref _publishErrors
|
|
);
|
|
|
|
Interlocked.Increment(
|
|
ref _consecutiveFailures
|
|
);
|
|
|
|
SetLastError(ex.Message);
|
|
continue;
|
|
}
|
|
|
|
if (result.Succeeded)
|
|
{
|
|
Interlocked.Increment(
|
|
ref _published
|
|
);
|
|
|
|
Interlocked.Add(
|
|
ref _publishedBytes,
|
|
envelope.Data.LongLength
|
|
);
|
|
|
|
Interlocked.Exchange(
|
|
ref _lastPublishedMono,
|
|
Stopwatch.GetTimestamp()
|
|
);
|
|
|
|
Interlocked.Exchange(
|
|
ref _consecutiveFailures,
|
|
0
|
|
);
|
|
|
|
lock (_metricsLock)
|
|
{
|
|
_lastPublishDurationMs =
|
|
result.DurationMs;
|
|
|
|
long count =
|
|
Interlocked.Read(
|
|
ref _published
|
|
);
|
|
|
|
if (count <= 1)
|
|
{
|
|
_averagePublishDurationMs =
|
|
result.DurationMs;
|
|
}
|
|
else
|
|
{
|
|
_averagePublishDurationMs +=
|
|
(
|
|
result.DurationMs -
|
|
_averagePublishDurationMs
|
|
) / count;
|
|
}
|
|
|
|
_lastMessageType =
|
|
envelope.Type;
|
|
|
|
_lastError = null;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Interlocked.Increment(
|
|
ref _consecutiveFailures
|
|
);
|
|
|
|
switch (result.Status)
|
|
{
|
|
case MqttPublishStatus
|
|
.Disconnected:
|
|
case MqttPublishStatus
|
|
.ServiceStopping:
|
|
Interlocked.Increment(
|
|
ref _droppedDisconnected
|
|
);
|
|
break;
|
|
|
|
case MqttPublishStatus
|
|
.Timeout:
|
|
Interlocked.Increment(
|
|
ref _publishTimeouts
|
|
);
|
|
break;
|
|
|
|
default:
|
|
Interlocked.Increment(
|
|
ref _publishErrors
|
|
);
|
|
break;
|
|
}
|
|
|
|
SetLastError(
|
|
result.Error ??
|
|
result.Status.ToString()
|
|
);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
public async Task StopAsync()
|
|
{
|
|
if (Interlocked.Exchange(
|
|
ref _started,
|
|
0) == 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
CancellationTokenSource cts = _cts;
|
|
Task worker = _worker;
|
|
|
|
_cts = null;
|
|
_worker = null;
|
|
|
|
if (cts != null)
|
|
{
|
|
try { cts.Cancel(); }
|
|
catch { }
|
|
}
|
|
|
|
try { _signal.Release(); }
|
|
catch (SemaphoreFullException) { }
|
|
|
|
if (worker != null)
|
|
{
|
|
try
|
|
{
|
|
await worker.ConfigureAwait(false);
|
|
}
|
|
catch (OperationCanceledException) { }
|
|
}
|
|
|
|
if (cts != null)
|
|
cts.Dispose();
|
|
|
|
lock (_queueLock)
|
|
_latestByType.Clear();
|
|
}
|
|
|
|
public RtcmPublisherMetrics GetMetrics()
|
|
{
|
|
int queueDepth;
|
|
|
|
lock (_queueLock)
|
|
queueDepth = _latestByType.Count;
|
|
|
|
double lastDuration;
|
|
double avgDuration;
|
|
int lastType;
|
|
string lastError;
|
|
|
|
lock (_metricsLock)
|
|
{
|
|
lastDuration =
|
|
_lastPublishDurationMs;
|
|
|
|
avgDuration =
|
|
_averagePublishDurationMs;
|
|
|
|
lastType = _lastMessageType;
|
|
lastError = _lastError;
|
|
}
|
|
|
|
return new RtcmPublisherMetrics
|
|
{
|
|
Running =
|
|
Volatile.Read(ref _started) == 1,
|
|
|
|
Received =
|
|
Interlocked.Read(ref _received),
|
|
|
|
Queued =
|
|
Interlocked.Read(ref _queued),
|
|
|
|
Replaced =
|
|
Interlocked.Read(ref _replaced),
|
|
|
|
Published =
|
|
Interlocked.Read(ref _published),
|
|
|
|
PublishedBytes =
|
|
Interlocked.Read(
|
|
ref _publishedBytes
|
|
),
|
|
|
|
DroppedInvalid =
|
|
Interlocked.Read(
|
|
ref _droppedInvalid
|
|
),
|
|
|
|
DroppedStale =
|
|
Interlocked.Read(
|
|
ref _droppedStale
|
|
),
|
|
|
|
DroppedDisconnected =
|
|
Interlocked.Read(
|
|
ref _droppedDisconnected
|
|
),
|
|
|
|
PublishTimeouts =
|
|
Interlocked.Read(
|
|
ref _publishTimeouts
|
|
),
|
|
|
|
PublishErrors =
|
|
Interlocked.Read(
|
|
ref _publishErrors
|
|
),
|
|
|
|
ConsecutiveFailures =
|
|
Interlocked.Read(
|
|
ref _consecutiveFailures
|
|
),
|
|
|
|
QueueDepth = queueDepth,
|
|
LastReceivedAgeMs =
|
|
AgeMs(
|
|
Interlocked.Read(
|
|
ref _lastReceivedMono
|
|
)
|
|
),
|
|
|
|
LastPublishedAgeMs =
|
|
AgeMs(
|
|
Interlocked.Read(
|
|
ref _lastPublishedMono
|
|
)
|
|
),
|
|
|
|
LastPublishDurationMs =
|
|
lastDuration,
|
|
|
|
AveragePublishDurationMs =
|
|
avgDuration,
|
|
|
|
LastMessageType = lastType,
|
|
LastError = lastError
|
|
};
|
|
}
|
|
|
|
private void SetLastError(string error)
|
|
{
|
|
lock (_metricsLock)
|
|
_lastError = error;
|
|
|
|
if (!string.IsNullOrWhiteSpace(error))
|
|
_log("[RTCM Publisher] " + error);
|
|
}
|
|
|
|
private static int GetMessageType(
|
|
byte[] message)
|
|
{
|
|
if (message == null ||
|
|
message.Length < 5 ||
|
|
message[0] != 0xD3)
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
return (
|
|
(message[3] << 4) |
|
|
(message[4] >> 4)
|
|
) & 0x0FFF;
|
|
}
|
|
|
|
private static double AgeMs(long timestamp)
|
|
{
|
|
if (timestamp <= 0)
|
|
return double.PositiveInfinity;
|
|
|
|
long delta =
|
|
Stopwatch.GetTimestamp() -
|
|
timestamp;
|
|
|
|
if (delta <= 0)
|
|
return 0;
|
|
|
|
return delta * 1000.0 /
|
|
Stopwatch.Frequency;
|
|
}
|
|
|
|
private void ThrowIfDisposed()
|
|
{
|
|
if (Volatile.Read(ref _disposed) == 1)
|
|
throw new ObjectDisposedException(
|
|
nameof(RtcmPublisherService)
|
|
);
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
if (Interlocked.Exchange(
|
|
ref _disposed,
|
|
1) == 1)
|
|
{
|
|
return;
|
|
}
|
|
|
|
try
|
|
{
|
|
StopAsync()
|
|
.GetAwaiter()
|
|
.GetResult();
|
|
}
|
|
catch { }
|
|
|
|
_signal.Dispose();
|
|
}
|
|
|
|
private sealed class RtcmPublishEnvelope
|
|
{
|
|
public byte[] Data { get; set; }
|
|
public int Type { get; set; }
|
|
public long Sequence { get; set; }
|
|
public long ReceivedMono { get; set; }
|
|
public DateTime ReceivedUtc { get; set; }
|
|
}
|
|
}
|
|
|
|
public sealed class RtcmPublisherMetrics
|
|
{
|
|
public bool Running { get; set; }
|
|
|
|
public long Received { get; set; }
|
|
public long Queued { get; set; }
|
|
public long Replaced { get; set; }
|
|
public long Published { get; set; }
|
|
public long PublishedBytes { get; set; }
|
|
|
|
public long DroppedInvalid { get; set; }
|
|
public long DroppedStale { get; set; }
|
|
public long DroppedDisconnected { get; set; }
|
|
public long PublishTimeouts { get; set; }
|
|
public long PublishErrors { get; set; }
|
|
public long ConsecutiveFailures { get; set; }
|
|
|
|
public int QueueDepth { get; set; }
|
|
public double LastReceivedAgeMs { get; set; }
|
|
public double LastPublishedAgeMs { get; set; }
|
|
public double LastPublishDurationMs { get; set; }
|
|
public double AveragePublishDurationMs { get; set; }
|
|
|
|
public int LastMessageType { get; set; }
|
|
public string LastError { get; set; }
|
|
}
|
|
|
|
public sealed class OperationControlCommunicationMetrics
|
|
{
|
|
public MqttServiceMetrics Critical { get; set; }
|
|
public MqttServiceMetrics Monitoring { get; set; }
|
|
public RtcmPublisherMetrics Rtcm { get; set; }
|
|
public int RoverCount { get; set; }
|
|
}
|
|
|
|
internal sealed class RoverDiscoveryInfo
|
|
{
|
|
[JsonProperty("rover_id")]
|
|
public string RoverId { get; set; }
|
|
|
|
[JsonProperty("rover_ip")]
|
|
public string RoverIp { get; set; }
|
|
|
|
[JsonProperty("session_id")]
|
|
public string SessionId { get; set; }
|
|
|
|
public static RoverDiscoveryInfo Parse(
|
|
string payload)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(payload))
|
|
return null;
|
|
|
|
string trimmed = payload.Trim();
|
|
|
|
if (trimmed.StartsWith(
|
|
"{",
|
|
StringComparison.Ordinal))
|
|
{
|
|
RoverDiscoveryInfo json =
|
|
JsonConvert.DeserializeObject<
|
|
RoverDiscoveryInfo>(
|
|
trimmed
|
|
);
|
|
|
|
if (json == null ||
|
|
string.IsNullOrWhiteSpace(
|
|
json.RoverId))
|
|
{
|
|
return null;
|
|
}
|
|
|
|
json.RoverId = json.RoverId.Trim();
|
|
json.RoverIp =
|
|
(json.RoverIp ??
|
|
string.Empty).Trim();
|
|
|
|
json.SessionId =
|
|
string.IsNullOrWhiteSpace(
|
|
json.SessionId)
|
|
? null
|
|
: json.SessionId.Trim();
|
|
|
|
return json;
|
|
}
|
|
|
|
string[] parts = trimmed.Split(',');
|
|
|
|
if (parts.Length < 2 ||
|
|
string.IsNullOrWhiteSpace(parts[0]))
|
|
{
|
|
return null;
|
|
}
|
|
|
|
return new RoverDiscoveryInfo
|
|
{
|
|
RoverId = parts[0].Trim(),
|
|
RoverIp = parts[1].Trim(),
|
|
SessionId = null
|
|
};
|
|
}
|
|
}
|
|
|
|
internal sealed class RoverDiscoveryAck
|
|
{
|
|
[JsonProperty("rover_id")]
|
|
public string RoverId { get; set; }
|
|
|
|
[JsonProperty("session_id")]
|
|
public string SessionId { get; set; }
|
|
|
|
[JsonProperty("base_id")]
|
|
public string BaseId { get; set; }
|
|
|
|
[JsonProperty("accepted")]
|
|
public bool Accepted { get; set; }
|
|
|
|
[JsonProperty("sent_at_utc")]
|
|
public DateTime SentAtUtc { get; set; }
|
|
}
|
|
}
|