1344 lines
45 KiB
C#
1344 lines
45 KiB
C#
using AgroBase.Models;
|
|
using AgroBase.Models.Modules;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Threading.Tasks;
|
|
using static AgroBase.Models.Enums;
|
|
using static AgroBase.Services.MKS057DCanService.MksHandler;
|
|
|
|
namespace AgroBase.Services
|
|
{
|
|
public class MKS057DCanService
|
|
{
|
|
public static List<MKS057DModel> DadosLeitura = new List<MKS057DModel>();
|
|
|
|
private static int _MStep { get; set; } = 16;
|
|
private static T_Code Dispositivo { get; set; } = T_Code.Mks;
|
|
|
|
public static MksHandler _MksHandler { get; set; } = new MksHandler();
|
|
|
|
private static readonly object _sync = new object();
|
|
|
|
private static readonly HashSet<byte> _handlersRegistrados = new HashSet<byte>();
|
|
private static readonly Dictionary<byte, DateTime> _ultimoEnvioConsultaPorDriver = new Dictionary<byte, DateTime>();
|
|
private static readonly Dictionary<string, DateTime> _ultimoEnvioParametro = new Dictionary<string, DateTime>();
|
|
private static readonly Dictionary<byte, int> _cursorPollingPorDriver = new Dictionary<byte, int>();
|
|
private static readonly Dictionary<string, string> _ultimoComandoControleAssinatura = new Dictionary<string, string>();
|
|
private static readonly Dictionary<string, DateTime> _ultimoComandoControleQuando = new Dictionary<string, DateTime>();
|
|
|
|
/*
|
|
* MKS V3
|
|
*
|
|
* Classe completa:
|
|
* - mantém parsers de todos os dados úteis do MKS;
|
|
* - mantém intervalos configurados para todos;
|
|
* - permite RequisitarDado(...) manual para qualquer parâmetro suportado.
|
|
*
|
|
* Polling normal:
|
|
* - só pede PulsosRecebidos e IO.
|
|
*
|
|
* Motivo:
|
|
* - PulsosRecebidos calcula ângulo.
|
|
* - IO identifica o lado real pelo sensor do rasgo/rebaixo no eixo tubular.
|
|
* - demais leituras ficam disponíveis para diagnóstico, mas não entram no loop normal.
|
|
*
|
|
* Controle:
|
|
* - Movimento, Parar e SetZero são prioridade e não usam dedupe.
|
|
*/
|
|
|
|
private static readonly TimeSpan TempoRespostaRecente = TimeSpan.FromSeconds(3);
|
|
private static readonly TimeSpan TempoRemoverDriver = TimeSpan.FromSeconds(5);
|
|
private static readonly TimeSpan TempoRespostaDescoberta = TimeSpan.FromMilliseconds(700);
|
|
private static readonly TimeSpan JanelaDedupeComandoControle = TimeSpan.FromMilliseconds(20);
|
|
|
|
private static readonly Dictionary<byte, DateTime> _ultimaTentativaReconexaoPorDriver = new Dictionary<byte, DateTime>();
|
|
private static readonly TimeSpan IntervaloTentativaReconexao = TimeSpan.FromSeconds(1);
|
|
|
|
/*
|
|
* Pooling de produção:
|
|
* operação normal = ângulo rápido + IO lento.
|
|
*/
|
|
public static readonly MksFuncCode[] OrdemPolling =
|
|
{
|
|
MksFuncCode.PulsosRecebidos,
|
|
MksFuncCode.IO,
|
|
MksFuncCode.RPM,
|
|
};
|
|
|
|
/*
|
|
* Pooling de referenciamento:
|
|
* IO e ângulo precisam ser rápidos para pegar a transição do sensor.
|
|
*/
|
|
public static readonly MksFuncCode[] OrdemPollingReferenciando =
|
|
{
|
|
MksFuncCode.IO,
|
|
MksFuncCode.PulsosRecebidos,
|
|
};
|
|
|
|
/*
|
|
* Todos os parâmetros suportados pela classe.
|
|
* Não significa que todos entram no polling.
|
|
*/
|
|
public static readonly MksFuncCode[] ParametrosSuportados =
|
|
{
|
|
MksFuncCode.EncoderCarregado,
|
|
MksFuncCode.EncoderAcumulado,
|
|
MksFuncCode.RPM,
|
|
MksFuncCode.PulsosRecebidos,
|
|
MksFuncCode.IO,
|
|
MksFuncCode.ErroAcumulado,
|
|
MksFuncCode.Versao,
|
|
};
|
|
|
|
private static readonly Dictionary<MksFuncCode, TimeSpan> IntervaloParametro =
|
|
new Dictionary<MksFuncCode, TimeSpan>
|
|
{
|
|
/*
|
|
* Produção.
|
|
*/
|
|
{ MksFuncCode.PulsosRecebidos, TimeSpan.FromMilliseconds(70) },
|
|
{ MksFuncCode.IO, TimeSpan.FromMilliseconds(700) },
|
|
{ MksFuncCode.RPM, TimeSpan.FromMilliseconds(500) },
|
|
|
|
/*
|
|
* Diagnóstico opcional.
|
|
* Não entram em OrdemPolling por padrão.
|
|
*/
|
|
{ MksFuncCode.ErroAcumulado, TimeSpan.FromSeconds(1) },
|
|
{ MksFuncCode.EncoderAcumulado, TimeSpan.FromSeconds(1) },
|
|
{ MksFuncCode.EncoderCarregado, TimeSpan.FromSeconds(2) },
|
|
{ MksFuncCode.Versao, TimeSpan.FromSeconds(5) },
|
|
};
|
|
|
|
private static readonly Dictionary<MksFuncCode, TimeSpan> IntervaloParametroReferenciando =
|
|
new Dictionary<MksFuncCode, TimeSpan>
|
|
{
|
|
/*
|
|
* Referenciamento.
|
|
*/
|
|
{ MksFuncCode.IO, TimeSpan.FromMilliseconds(35) },
|
|
{ MksFuncCode.PulsosRecebidos, TimeSpan.FromMilliseconds(35) },
|
|
|
|
/*
|
|
* Diagnóstico opcional durante referenciamento.
|
|
*/
|
|
{ MksFuncCode.RPM, TimeSpan.FromMilliseconds(500) },
|
|
{ MksFuncCode.ErroAcumulado, TimeSpan.FromSeconds(1) },
|
|
{ MksFuncCode.EncoderAcumulado, TimeSpan.FromMilliseconds(700) },
|
|
{ MksFuncCode.EncoderCarregado, TimeSpan.FromSeconds(2) },
|
|
{ MksFuncCode.Versao, TimeSpan.FromSeconds(5) },
|
|
};
|
|
|
|
public class MksParametroHealthConfig
|
|
{
|
|
public MksFuncCode Parametro { get; set; }
|
|
public string RedisKey { get; set; }
|
|
|
|
/*
|
|
* Frequência saudável esperada.
|
|
* Não é a frequência máxima possível, nem 1000 / polling_ms.
|
|
*/
|
|
public double FreqBaseHz { get; set; }
|
|
|
|
/*
|
|
* Abaixo disso começa alerta.
|
|
*/
|
|
public double FreqMinHz { get; set; }
|
|
|
|
/*
|
|
* Se false, publica config e frequência, mas não cobra no health.
|
|
*/
|
|
public bool EmUsoHealth { get; set; } = true;
|
|
}
|
|
|
|
public static readonly Dictionary<MksFuncCode, MksParametroHealthConfig> HealthParametroConfig = new Dictionary<MksFuncCode, MksParametroHealthConfig>
|
|
{
|
|
{
|
|
MksFuncCode.PulsosRecebidos,
|
|
new MksParametroHealthConfig
|
|
{
|
|
Parametro = MksFuncCode.PulsosRecebidos,
|
|
RedisKey = "pulsosrecebidos",
|
|
FreqBaseHz = 5.0,
|
|
FreqMinHz = 2.0,
|
|
EmUsoHealth = true
|
|
}
|
|
},
|
|
{
|
|
MksFuncCode.IO,
|
|
new MksParametroHealthConfig
|
|
{
|
|
Parametro = MksFuncCode.IO,
|
|
RedisKey = "io",
|
|
FreqBaseHz = 0.7,
|
|
FreqMinHz = 0.25,
|
|
EmUsoHealth = true
|
|
}
|
|
},
|
|
{
|
|
MksFuncCode.RPM,
|
|
new MksParametroHealthConfig
|
|
{
|
|
Parametro = MksFuncCode.RPM,
|
|
RedisKey = "rpm",
|
|
FreqBaseHz = 0.7,
|
|
FreqMinHz = 0.25,
|
|
EmUsoHealth = true
|
|
}
|
|
},
|
|
|
|
/*
|
|
* Diagnóstico disponível, mas não cobrado no health.
|
|
*/
|
|
{
|
|
MksFuncCode.ErroAcumulado,
|
|
new MksParametroHealthConfig
|
|
{
|
|
Parametro = MksFuncCode.ErroAcumulado,
|
|
RedisKey = "erroacumulado",
|
|
FreqBaseHz = 0.2,
|
|
FreqMinHz = 0.08,
|
|
EmUsoHealth = false
|
|
}
|
|
},
|
|
{
|
|
MksFuncCode.EncoderAcumulado,
|
|
new MksParametroHealthConfig
|
|
{
|
|
Parametro = MksFuncCode.EncoderAcumulado,
|
|
RedisKey = "encoderacumulado",
|
|
FreqBaseHz = 0.2,
|
|
FreqMinHz = 0.08,
|
|
EmUsoHealth = false
|
|
}
|
|
},
|
|
{
|
|
MksFuncCode.EncoderCarregado,
|
|
new MksParametroHealthConfig
|
|
{
|
|
Parametro = MksFuncCode.EncoderCarregado,
|
|
RedisKey = "encodercarregado",
|
|
FreqBaseHz = 0.1,
|
|
FreqMinHz = 0.04,
|
|
EmUsoHealth = false
|
|
}
|
|
},
|
|
{
|
|
MksFuncCode.Versao,
|
|
new MksParametroHealthConfig
|
|
{
|
|
Parametro = MksFuncCode.Versao,
|
|
RedisKey = "versao",
|
|
FreqBaseHz = 0.1,
|
|
FreqMinHz = 0.04,
|
|
EmUsoHealth = false
|
|
}
|
|
},
|
|
};
|
|
|
|
public static MksParametroHealthConfig ObterConfigHealth(MksFuncCode parametro)
|
|
{
|
|
if (HealthParametroConfig.TryGetValue(parametro, out var cfg))
|
|
return cfg;
|
|
|
|
return new MksParametroHealthConfig
|
|
{
|
|
Parametro = parametro,
|
|
RedisKey = parametro.ToString().ToLowerInvariant(),
|
|
FreqBaseHz = 0.5,
|
|
FreqMinHz = 0.2,
|
|
EmUsoHealth = false
|
|
};
|
|
}
|
|
|
|
public static double ObterFreqBaseHz(MksFuncCode parametro)
|
|
{
|
|
return ObterConfigHealth(parametro).FreqBaseHz;
|
|
}
|
|
|
|
public static double ObterFreqMinHz(MksFuncCode parametro)
|
|
{
|
|
return ObterConfigHealth(parametro).FreqMinHz;
|
|
}
|
|
|
|
public static bool ParametroEmUsoHealth(MksFuncCode parametro)
|
|
{
|
|
return ObterConfigHealth(parametro).EmUsoHealth;
|
|
}
|
|
|
|
public static bool Iniciado
|
|
{
|
|
get
|
|
{
|
|
return
|
|
CanManager.CanService.IsConnected &&
|
|
SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Dir) &&
|
|
DadosLeitura.Any(x =>
|
|
x.Iniciado &&
|
|
x.UltimoComandoRecebido != DateTime.MinValue &&
|
|
DateTime.Now - x.UltimoComandoRecebido <= TempoRespostaRecente
|
|
);
|
|
}
|
|
}
|
|
|
|
public static bool Referenciando
|
|
{
|
|
get
|
|
{
|
|
return
|
|
Variaveis
|
|
.OperacaoEmAndamento
|
|
.DispMvd?
|
|
.Dados?
|
|
.Modulos?
|
|
.Any(x => x?.DirMotor?.Referenciando ?? false) ??
|
|
false;
|
|
}
|
|
}
|
|
|
|
public static DateTime UltimoReferenciamento = DateTime.MinValue;
|
|
|
|
public static int TaxaAmostragem
|
|
{
|
|
get
|
|
{
|
|
return Referenciando ? 40 : 80;
|
|
}
|
|
}
|
|
|
|
public static double ObterIntervaloPollingParametroMs(MksFuncCode parametro)
|
|
{
|
|
return ObterIntervaloParametro(parametro).TotalMilliseconds;
|
|
}
|
|
|
|
public static (string campo, object valor)[] GerarCamposRedisHealthBase(int enderecoDir)
|
|
{
|
|
var campos = new List<(string campo, object valor)>();
|
|
|
|
foreach (var parametro in ParametrosSuportados)
|
|
{
|
|
var cfg = ObterConfigHealth(parametro);
|
|
string key = cfg.RedisKey;
|
|
|
|
campos.Add(($"{enderecoDir}.freq_base.{key}", cfg.FreqBaseHz));
|
|
campos.Add(($"{enderecoDir}.freq_min.{key}", cfg.FreqMinHz));
|
|
campos.Add(($"{enderecoDir}.freq_uso.{key}", cfg.EmUsoHealth));
|
|
|
|
campos.Add((
|
|
$"{enderecoDir}.polling_ms.{key}",
|
|
ObterIntervaloPollingParametroMs(parametro)
|
|
));
|
|
|
|
campos.Add((
|
|
$"{enderecoDir}.polling_em_uso.{key}",
|
|
OrdemPolling.Contains(parametro) ||
|
|
OrdemPollingReferenciando.Contains(parametro)
|
|
));
|
|
}
|
|
|
|
campos.Add(($"{enderecoDir}.polling.intervalo_timer_ms", TaxaAmostragem));
|
|
campos.Add(($"{enderecoDir}.polling.qtd_parametros_pool", OrdemPolling.Length));
|
|
campos.Add(($"{enderecoDir}.polling.qtd_parametros_suportados", ParametrosSuportados.Length));
|
|
|
|
campos.Add((
|
|
$"{enderecoDir}.polling.ordem",
|
|
OrdemPolling.Select(x => ObterConfigHealth(x).RedisKey).ToList()
|
|
));
|
|
|
|
campos.Add((
|
|
$"{enderecoDir}.polling.ordem_referenciando",
|
|
OrdemPollingReferenciando.Select(x => ObterConfigHealth(x).RedisKey).ToList()
|
|
));
|
|
|
|
campos.Add((
|
|
$"{enderecoDir}.polling.suportados",
|
|
ParametrosSuportados.Select(x => ObterConfigHealth(x).RedisKey).ToList()
|
|
));
|
|
|
|
campos.Add((
|
|
$"{enderecoDir}.polling.filosofia",
|
|
"MKS completo nos parsers, health por parâmetro e polling enxuto"
|
|
));
|
|
|
|
return campos.ToArray();
|
|
}
|
|
|
|
// ============================================================
|
|
// Descoberta / definição do dispositivo
|
|
// ============================================================
|
|
|
|
public static async Task<bool> VerificaDispositivoConectado()
|
|
{
|
|
if (!CanManager.CanService.IsConnected)
|
|
return false;
|
|
|
|
if (Variaveis.OperacaoEmAndamento.DispMvd == null)
|
|
return false;
|
|
|
|
List<string> modIds = new List<string>();
|
|
|
|
var modulos =
|
|
Variaveis
|
|
.OperacaoEmAndamento
|
|
.DispMvd
|
|
.Dados
|
|
.Modulos
|
|
.Where(x => x?.DirMotor != null && !x.DirMotor.Inicializado)
|
|
.ToList();
|
|
|
|
foreach (var modulo in modulos)
|
|
{
|
|
byte idTx = modulo.DirMotor._EnderecoCAN_Tx;
|
|
byte idRx = modulo.DirMotor._EnderecoCAN_Rx;
|
|
|
|
RegistrarHandlersMotor(idTx, idRx);
|
|
|
|
EnviarComandoDescoberta(idTx, idRx);
|
|
|
|
DateTime inicio = DateTime.Now;
|
|
|
|
bool Respondido() =>
|
|
DadosLeitura.Any(x =>
|
|
x.EnderecoRx == idRx &&
|
|
x.UltimoComandoRecebido > inicio
|
|
);
|
|
|
|
await FuncoesGlobais.AguardarCondicaoAsync(
|
|
Respondido,
|
|
(int)TempoRespostaDescoberta.TotalMilliseconds,
|
|
50
|
|
);
|
|
|
|
if (Respondido())
|
|
modIds.Add(modulo.Modulo_ID);
|
|
}
|
|
|
|
if (modIds.Any())
|
|
DefinirDispositivo(modIds);
|
|
|
|
return modIds.Any();
|
|
}
|
|
|
|
private static void DefinirDispositivo(List<string> modIds)
|
|
{
|
|
var op = Variaveis.OperacaoEmAndamento;
|
|
|
|
foreach (var modId in modIds)
|
|
{
|
|
var modulo = op.DispMvd.Dados.Modulos.FirstOrDefault(x => x.Modulo_ID == modId);
|
|
|
|
if (modulo?.DirMotor == null)
|
|
continue;
|
|
|
|
RegistrarHandlersMotor(
|
|
modulo.DirMotor._EnderecoCAN_Tx,
|
|
modulo.DirMotor._EnderecoCAN_Rx
|
|
);
|
|
|
|
if (SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Dir && x.Mod_ID == modId))
|
|
continue;
|
|
|
|
var leitura =
|
|
DadosLeitura.FirstOrDefault(x => x.EnderecoTx == modulo.DirMotor._EnderecoCAN_Tx);
|
|
|
|
if (leitura == null)
|
|
continue;
|
|
|
|
SerialService.DispositivosMapeados.Add(new DispositivoDetalhesModel()
|
|
{
|
|
Dispositivo = T_Code.Dir,
|
|
Endereco = CanManager.CanService._portName,
|
|
Versao = (leitura.FV ?? "").ToString(),
|
|
Mod_ID = modId,
|
|
});
|
|
|
|
op.Sensoriamento.Operacao.ModsCalibragem.TryGetValue(T_Code.Dir, out var m);
|
|
|
|
if (m == null)
|
|
{
|
|
op.Sensoriamento.Operacao.ModsCalibragem.Add(T_Code.Dir, new Dictionary<string, bool>());
|
|
}
|
|
|
|
op.Sensoriamento.Operacao.ModsCalibragem[T_Code.Dir][modId] = false;
|
|
|
|
op.Sensoriamento.InserirLog(
|
|
T_Code.Dir,
|
|
StatusModulo.Conectado,
|
|
0,
|
|
"Dispositivo conectado",
|
|
modId
|
|
);
|
|
}
|
|
}
|
|
|
|
// ============================================================
|
|
// Registro de handlers
|
|
// ============================================================
|
|
|
|
private static void RegistrarHandler(byte idRx)
|
|
{
|
|
lock (_sync)
|
|
{
|
|
if (_handlersRegistrados.Contains(idRx))
|
|
return;
|
|
|
|
CanManager.CanService.RegistrarHandler(idRx, _MksHandler);
|
|
_handlersRegistrados.Add(idRx);
|
|
}
|
|
}
|
|
|
|
private static void RegistrarHandlersMotor(byte idTx, byte idRx)
|
|
{
|
|
RegistrarHandler(idRx);
|
|
|
|
if (idTx != idRx)
|
|
RegistrarHandler(idTx);
|
|
}
|
|
|
|
// ============================================================
|
|
// Utils
|
|
// ============================================================
|
|
|
|
private static int ConverterAnguloParaValor(double angulo, ModosControle modo, double reducao)
|
|
{
|
|
int val = 0;
|
|
|
|
if (modo == ModosControle.Pulsos)
|
|
{
|
|
double pulsos = _MStep * 200.0;
|
|
double relacao = (pulsos * reducao) / 360.0;
|
|
val = Convert.ToInt32(angulo * relacao);
|
|
}
|
|
else
|
|
{
|
|
val = Convert.ToInt32(angulo * reducao);
|
|
}
|
|
|
|
return val;
|
|
}
|
|
|
|
private static TimeSpan ObterIntervaloParametro(MksFuncCode parametro)
|
|
{
|
|
var tabela = Referenciando ? IntervaloParametroReferenciando : IntervaloParametro;
|
|
|
|
if (tabela.TryGetValue(parametro, out TimeSpan intervalo))
|
|
return intervalo;
|
|
|
|
return TimeSpan.FromMilliseconds(500);
|
|
}
|
|
|
|
private static TimeSpan ObterIntervaloMinimoConsultaPorDriver()
|
|
{
|
|
return Referenciando
|
|
? TimeSpan.FromMilliseconds(18)
|
|
: TimeSpan.FromMilliseconds(45);
|
|
}
|
|
|
|
private static bool DeveContabilizarTimeoutHealth(MksFuncCode dado)
|
|
{
|
|
return dado == MksFuncCode.PulsosRecebidos;
|
|
}
|
|
|
|
private static bool EhFuncaoConsulta(MksFuncCode dado)
|
|
{
|
|
return ParametrosSuportados.Contains(dado);
|
|
}
|
|
|
|
// ============================================================
|
|
// Comandos CAN
|
|
// ============================================================
|
|
|
|
private static void EnviarComandoDescoberta(byte idTx, byte idRx)
|
|
{
|
|
RegistrarHandlersMotor(idTx, idRx);
|
|
|
|
EnviarComando(
|
|
Dispositivo,
|
|
idTx,
|
|
idRx,
|
|
(byte)MksFuncCode.Versao,
|
|
null,
|
|
get: true,
|
|
contabilizarTimeoutHealth: false,
|
|
mensagemDescoberta: true
|
|
);
|
|
}
|
|
|
|
private static void EnviarComando(
|
|
T_Code dispositivo,
|
|
byte idTx,
|
|
byte idRx,
|
|
byte funcCode,
|
|
byte[] payload = null,
|
|
bool get = true,
|
|
bool contabilizarTimeoutHealth = true,
|
|
bool mensagemDescoberta = false)
|
|
{
|
|
RegistrarHandlersMotor(idTx, idRx);
|
|
|
|
CanManager.CanService.AdicionarMensagemNaFila(
|
|
dispositivo,
|
|
new byte[] { idTx },
|
|
new byte[] { idRx },
|
|
funcCode,
|
|
funcCode,
|
|
payload,
|
|
get,
|
|
contabilizarTimeoutHealth,
|
|
mensagemDescoberta
|
|
);
|
|
}
|
|
|
|
public static void SetModoOperacao(byte idTx, byte idRx, byte modo)
|
|
{
|
|
EnviarComandoControle(
|
|
idTx,
|
|
idRx,
|
|
0x82,
|
|
new byte[] { modo },
|
|
dedupe: true
|
|
);
|
|
}
|
|
|
|
public static void RequisitarDado(byte idTx, byte idRx, MksFuncCode dado)
|
|
{
|
|
RequisitarDadoInterno(
|
|
idTx,
|
|
idRx,
|
|
dado,
|
|
forcar: false,
|
|
contabilizarTimeoutHealth: DeveContabilizarTimeoutHealth(dado)
|
|
);
|
|
}
|
|
|
|
public static void RequisitarDadoForcado(byte idTx, byte idRx, MksFuncCode dado)
|
|
{
|
|
RequisitarDadoInterno(
|
|
idTx,
|
|
idRx,
|
|
dado,
|
|
forcar: true,
|
|
contabilizarTimeoutHealth: DeveContabilizarTimeoutHealth(dado)
|
|
);
|
|
}
|
|
|
|
private static bool RequisitarDadoInterno(
|
|
byte idTx,
|
|
byte idRx,
|
|
MksFuncCode dado,
|
|
bool forcar,
|
|
bool contabilizarTimeoutHealth)
|
|
{
|
|
if (!EhFuncaoConsulta(dado))
|
|
return false;
|
|
|
|
DateTime agora = DateTime.Now;
|
|
|
|
lock (_sync)
|
|
{
|
|
string chaveParametro =
|
|
idTx.ToString("X2") + ":" + ((byte)dado).ToString("X2");
|
|
|
|
if (!forcar)
|
|
{
|
|
TimeSpan intervaloMinDriver = ObterIntervaloMinimoConsultaPorDriver();
|
|
|
|
if (_ultimoEnvioConsultaPorDriver.TryGetValue(idTx, out DateTime ultimoDriver) &&
|
|
agora - ultimoDriver < intervaloMinDriver)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
TimeSpan intervalo = ObterIntervaloParametro(dado);
|
|
|
|
if (_ultimoEnvioParametro.TryGetValue(chaveParametro, out DateTime ultimoParam) &&
|
|
agora - ultimoParam < intervalo)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
_ultimoEnvioConsultaPorDriver[idTx] = agora;
|
|
_ultimoEnvioParametro[chaveParametro] = agora;
|
|
}
|
|
|
|
EnviarComando(
|
|
Dispositivo,
|
|
idTx,
|
|
idRx,
|
|
(byte)dado,
|
|
payload: null,
|
|
get: true,
|
|
contabilizarTimeoutHealth: contabilizarTimeoutHealth
|
|
);
|
|
|
|
return true;
|
|
}
|
|
|
|
public static void RequisitarProximoDado(byte idTx, byte idRx)
|
|
{
|
|
MksFuncCode? proximo = EscolherProximoParametro(idTx);
|
|
|
|
if (proximo.HasValue)
|
|
{
|
|
RequisitarDadoInterno(
|
|
idTx,
|
|
idRx,
|
|
proximo.Value,
|
|
forcar: false,
|
|
contabilizarTimeoutHealth: DeveContabilizarTimeoutHealth(proximo.Value)
|
|
);
|
|
}
|
|
}
|
|
|
|
public static void AtualizarDados()
|
|
{
|
|
try
|
|
{
|
|
var modulos = Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos;
|
|
if (modulos == null)
|
|
return;
|
|
|
|
MarcarDriversInativosPorTimeout();
|
|
|
|
foreach (var mod in modulos)
|
|
{
|
|
var driver = mod?.DirMotor;
|
|
|
|
if (driver == null)
|
|
continue;
|
|
|
|
if (driver.Inicializado)
|
|
{
|
|
RequisitarProximoDado(driver._EnderecoCAN_Tx, driver._EnderecoCAN_Rx);
|
|
continue;
|
|
}
|
|
|
|
TentarReconectarDriver(driver._EnderecoCAN_Tx, driver._EnderecoCAN_Rx);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Variaveis.MostrarLog("[MKS057DCanService] Erro em AtualizarDados: " + ex.Message);
|
|
}
|
|
}
|
|
|
|
private static void TentarReconectarDriver(byte idTx, byte idRx)
|
|
{
|
|
/*
|
|
* Só tentamos recuperar um driver que já existiu anteriormente.
|
|
*
|
|
* Descoberta inicial continua sendo responsabilidade de
|
|
* VerificaDispositivoConectado().
|
|
*/
|
|
bool driverConhecido;
|
|
|
|
lock (DadosLeitura)
|
|
{
|
|
driverConhecido = DadosLeitura.Any(x => x.EnderecoTx == idTx);
|
|
}
|
|
|
|
if (!driverConhecido)
|
|
return;
|
|
|
|
DateTime agora = DateTime.Now;
|
|
|
|
lock (_sync)
|
|
{
|
|
if (_ultimaTentativaReconexaoPorDriver.TryGetValue(idTx, out DateTime ultima) && agora - ultima < IntervaloTentativaReconexao)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_ultimaTentativaReconexaoPorDriver[idTx] = agora;
|
|
}
|
|
|
|
/*
|
|
* Versão é uma consulta pequena e já é utilizada
|
|
* pela descoberta.
|
|
*
|
|
* Não contamina timeout do health.
|
|
*/
|
|
EnviarComandoDescoberta(idTx, idRx);
|
|
}
|
|
|
|
private static MksFuncCode? EscolherProximoParametro(byte idTx)
|
|
{
|
|
DateTime agora = DateTime.Now;
|
|
|
|
MksFuncCode[] ordem = Referenciando ? OrdemPollingReferenciando : OrdemPolling;
|
|
|
|
if (ordem == null || ordem.Length == 0)
|
|
return null;
|
|
|
|
int cursor = _cursorPollingPorDriver.TryGetValue(idTx, out int c) ? c : 0;
|
|
|
|
for (int tentativa = 0; tentativa < ordem.Length; tentativa++)
|
|
{
|
|
int index = (cursor + tentativa) % ordem.Length;
|
|
MksFuncCode parametro = ordem[index];
|
|
|
|
string chave =
|
|
idTx.ToString("X2") + ":" + ((byte)parametro).ToString("X2");
|
|
|
|
TimeSpan intervalo = ObterIntervaloParametro(parametro);
|
|
|
|
if (!_ultimoEnvioParametro.TryGetValue(chave, out DateTime ultimo) ||
|
|
agora - ultimo >= intervalo)
|
|
{
|
|
_cursorPollingPorDriver[idTx] = (index + 1) % ordem.Length;
|
|
return parametro;
|
|
}
|
|
}
|
|
|
|
return null;
|
|
}
|
|
|
|
private static void MarcarDriversInativosPorTimeout()
|
|
{
|
|
DateTime agora = DateTime.Now;
|
|
|
|
List<MKS057DModel> antigos;
|
|
|
|
lock (DadosLeitura)
|
|
{
|
|
antigos =
|
|
DadosLeitura
|
|
.Where(x =>
|
|
x.Iniciado &&
|
|
x.UltimoComandoRecebido != DateTime.MinValue &&
|
|
agora - x.UltimoComandoRecebido > TempoRemoverDriver)
|
|
.ToList();
|
|
}
|
|
|
|
foreach (var antigo in antigos)
|
|
{
|
|
antigo.Iniciado = false;
|
|
|
|
double tempoSemResposta = (agora - antigo.UltimoComandoRecebido).TotalSeconds;
|
|
|
|
Variaveis.MostrarLog(
|
|
$"[MKS057DCanService] [CONEXAO] " +
|
|
$"Driver CAN 0x{antigo.EnderecoTx:X2} OFFLINE | " +
|
|
$"sem resposta há {tempoSemResposta:F1}s"
|
|
);
|
|
}
|
|
}
|
|
|
|
public static void SetZero(byte idTx, byte idRx)
|
|
{
|
|
EnviarComandoControle(
|
|
idTx,
|
|
idRx,
|
|
(byte)MksFuncCode.SetZero,
|
|
null,
|
|
dedupe: false
|
|
);
|
|
|
|
UltimoReferenciamento = DateTime.Now;
|
|
}
|
|
|
|
public static void Parar(
|
|
byte idTx,
|
|
byte idRx,
|
|
ModosControle modo = ModosControle.AnguloAbsoluto)
|
|
{
|
|
byte funcCode =
|
|
modo == ModosControle.Pulsos ? (byte)MksFuncCode.ModoControlePulsos :
|
|
modo == ModosControle.AnguloRelativo ? (byte)MksFuncCode.ModoControleAnguloRelativo :
|
|
(byte)MksFuncCode.ModoControleAnguloAbsoluto;
|
|
|
|
EnviarComandoControle(
|
|
idTx,
|
|
idRx,
|
|
funcCode,
|
|
new byte[] { 0x00 },
|
|
dedupe: false
|
|
);
|
|
}
|
|
|
|
public static void Movimento(
|
|
byte idTx,
|
|
byte idRx,
|
|
double angulo,
|
|
int velocidade,
|
|
Sentido sentido = Sentido.Horario,
|
|
ModosControle modo = ModosControle.AnguloAbsoluto)
|
|
{
|
|
byte funcCode;
|
|
double reducao = Variaveis.OperacaoEmAndamento?.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.DirMotor._EnderecoCAN_Tx == idTx)?.DirMotor?.Reducao ?? VariaveisEquipamento.ReducaoDirecional;
|
|
byte[] payload =
|
|
MontarPayloadMovimento(
|
|
angulo,
|
|
velocidade,
|
|
sentido,
|
|
modo,
|
|
reducao,
|
|
out funcCode
|
|
);
|
|
|
|
if (payload == null)
|
|
return;
|
|
|
|
EnviarComandoControle(
|
|
idTx,
|
|
idRx,
|
|
funcCode,
|
|
payload,
|
|
dedupe: false
|
|
);
|
|
}
|
|
|
|
public static void AlterarStatusMotor(byte idTx, byte idRx, bool habilitado)
|
|
{
|
|
byte comando = habilitado ? (byte)0x01 : (byte)0x00;
|
|
|
|
EnviarComandoControle(
|
|
idTx,
|
|
idRx,
|
|
(byte)MksFuncCode.HabilitarMotor,
|
|
new byte[] { comando },
|
|
dedupe: true
|
|
);
|
|
}
|
|
|
|
private static void EnviarComandoControle(
|
|
byte idTx,
|
|
byte idRx,
|
|
byte funcCode,
|
|
byte[] payload,
|
|
bool dedupe)
|
|
{
|
|
if (dedupe && DeveDroparComandoDuplicado(idTx, funcCode, payload))
|
|
return;
|
|
|
|
EnviarComando(
|
|
Dispositivo,
|
|
idTx,
|
|
idRx,
|
|
funcCode,
|
|
payload,
|
|
get: false,
|
|
contabilizarTimeoutHealth: false
|
|
);
|
|
}
|
|
|
|
private static bool DeveDroparComandoDuplicado(byte idTx, byte funcCode, byte[] payload)
|
|
{
|
|
string chave = idTx.ToString("X2") + ":" + funcCode.ToString("X2");
|
|
|
|
string assinatura =
|
|
payload == null
|
|
? ""
|
|
: string.Join("", payload.Select(x => x.ToString("X2")));
|
|
|
|
DateTime agora = DateTime.Now;
|
|
|
|
lock (_sync)
|
|
{
|
|
if (_ultimoComandoControleAssinatura.TryGetValue(chave, out string anterior) &&
|
|
anterior == assinatura &&
|
|
_ultimoComandoControleQuando.TryGetValue(chave, out DateTime quando) &&
|
|
agora - quando < JanelaDedupeComandoControle)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
_ultimoComandoControleAssinatura[chave] = assinatura;
|
|
_ultimoComandoControleQuando[chave] = agora;
|
|
|
|
return false;
|
|
}
|
|
}
|
|
|
|
private static byte[] MontarPayloadMovimento(
|
|
double angulo,
|
|
int velocidade,
|
|
Sentido sentido,
|
|
ModosControle modo,
|
|
double reducao,
|
|
out byte funcCode)
|
|
{
|
|
List<byte> payload = new List<byte>();
|
|
|
|
ushort vel = (ushort)Math.Max(0, Math.Min(velocidade, ushort.MaxValue));
|
|
byte[] velocidadeBytes = BitConverter.GetBytes(vel);
|
|
|
|
byte acc = 0;
|
|
|
|
if (modo == ModosControle.Pulsos)
|
|
{
|
|
byte sentidoByte = sentido == Sentido.Horario ? (byte)0x80 : (byte)0x00;
|
|
|
|
uint pulsos = (uint)ConverterAnguloParaValor(angulo, modo, reducao);
|
|
byte[] pulsosBytes = BitConverter.GetBytes(pulsos);
|
|
|
|
velocidadeBytes[1] |= sentidoByte;
|
|
|
|
payload.Add(velocidadeBytes[1]);
|
|
payload.Add(velocidadeBytes[0]);
|
|
payload.Add(acc);
|
|
|
|
payload.Add(pulsosBytes[2]);
|
|
payload.Add(pulsosBytes[1]);
|
|
payload.Add(pulsosBytes[0]);
|
|
|
|
funcCode = (byte)MksFuncCode.ModoControlePulsos;
|
|
return payload.ToArray();
|
|
}
|
|
|
|
int pos = ConverterAnguloParaValor(angulo, modo, reducao);
|
|
|
|
long absAxisText = Convert.ToInt32(pos.ToString(), 10);
|
|
byte[] absAxis = BitConverter.GetBytes(absAxisText * 16384 / 360);
|
|
byte[] posInt24 = new byte[] { absAxis[2], absAxis[1], absAxis[0] };
|
|
|
|
payload.AddRange(new byte[] { velocidadeBytes[1], velocidadeBytes[0] });
|
|
payload.Add(acc);
|
|
payload.AddRange(posInt24);
|
|
|
|
funcCode =
|
|
modo == ModosControle.AnguloRelativo
|
|
? (byte)MksFuncCode.ModoControleAnguloRelativo
|
|
: (byte)MksFuncCode.ModoControleAnguloAbsoluto;
|
|
|
|
return payload.ToArray();
|
|
}
|
|
|
|
// ============================================================
|
|
// Decodificação
|
|
// ============================================================
|
|
|
|
private static ushort DecodeUInt16BE(byte[] data, int startIndex)
|
|
{
|
|
if (data == null || data.Length < startIndex + 2)
|
|
return 0;
|
|
|
|
return (ushort)((data[startIndex] << 8) | data[startIndex + 1]);
|
|
}
|
|
|
|
private static short DecodeInt16BE(byte[] data, int startIndex)
|
|
{
|
|
if (data == null || data.Length < startIndex + 2)
|
|
return 0;
|
|
|
|
ushort raw = (ushort)((data[startIndex] << 8) | data[startIndex + 1]);
|
|
return unchecked((short)raw);
|
|
}
|
|
|
|
private static int DecodeInt32BE(byte[] data, int startIndex)
|
|
{
|
|
if (data == null || data.Length < startIndex + 4)
|
|
return 0;
|
|
|
|
return
|
|
(data[startIndex] << 24) |
|
|
(data[startIndex + 1] << 16) |
|
|
(data[startIndex + 2] << 8) |
|
|
data[startIndex + 3];
|
|
}
|
|
|
|
private static long DecodeInt48BE(byte[] data, int startIndex)
|
|
{
|
|
if (data == null || data.Length < startIndex + 6)
|
|
return 0;
|
|
|
|
byte[] buffer = new byte[8];
|
|
|
|
for (int i = 0; i < 6; i++)
|
|
buffer[i] = data[startIndex + 5 - i];
|
|
|
|
buffer[6] = (buffer[5] & 0x80) != 0 ? (byte)0xFF : (byte)0x00;
|
|
buffer[7] = buffer[6];
|
|
|
|
return BitConverter.ToInt64(buffer, 0);
|
|
}
|
|
|
|
// ============================================================
|
|
// Handler CAN
|
|
// ============================================================
|
|
|
|
public class MksHandler : ICanMessageHandler
|
|
{
|
|
public T_Code Dispositivo { get; set; } =
|
|
MKS057DCanService.Dispositivo;
|
|
|
|
public enum MksFuncCode : byte
|
|
{
|
|
EncoderCarregado = 0x30,
|
|
EncoderAcumulado = 0x31,
|
|
RPM = 0x32,
|
|
PulsosRecebidos = 0x33,
|
|
IO = 0x34,
|
|
Versao = 0x40,
|
|
ErroAcumulado = 0x36,
|
|
SetZero = 0x92,
|
|
HabilitarMotor = 0xF3,
|
|
ModoControleAnguloRelativo = 0xF4,
|
|
ModoControleAnguloAbsoluto = 0xF5,
|
|
ModoControlePulsos = 0xFD
|
|
}
|
|
|
|
private double PulsosRecebidosParaAngulo(int valor, double reducao)
|
|
{
|
|
double pulsos = _MStep * 200.0;
|
|
double relacao = (pulsos * reducao) / 360.0;
|
|
|
|
return valor / relacao;
|
|
}
|
|
|
|
private double PulsosEncoderParaAngulo(int valor)
|
|
{
|
|
return valor * 360.0 / 16384.0;
|
|
}
|
|
|
|
private MKS057DModel ObterOuCriar(CanMessage mensagem)
|
|
{
|
|
lock (DadosLeitura)
|
|
{
|
|
byte idTxMsg = CanManager.ConverterIdCan(mensagem.IdTx)[0];
|
|
byte idRxMsg = CanManager.ConverterIdCan(mensagem.IdRx)[0];
|
|
|
|
var item =
|
|
DadosLeitura.FirstOrDefault(
|
|
x => x.EnderecoTx == idTxMsg &&
|
|
x.EnderecoRx == idRxMsg
|
|
);
|
|
|
|
if (item != null)
|
|
return item;
|
|
|
|
var motor =
|
|
Variaveis
|
|
.OperacaoEmAndamento
|
|
.DispMvd?
|
|
.Dados?
|
|
.Modulos?
|
|
.Select(x => x?.DirMotor)
|
|
.FirstOrDefault(m =>
|
|
m != null &&
|
|
(
|
|
m._EnderecoCAN_Tx == idTxMsg ||
|
|
m._EnderecoCAN_Rx == idTxMsg ||
|
|
m._EnderecoCAN_Tx == idRxMsg ||
|
|
m._EnderecoCAN_Rx == idRxMsg
|
|
)
|
|
);
|
|
|
|
if (motor != null)
|
|
{
|
|
item =
|
|
DadosLeitura.FirstOrDefault(
|
|
x => x.EnderecoTx == motor._EnderecoCAN_Tx
|
|
);
|
|
|
|
if (item == null)
|
|
{
|
|
item = new MKS057DModel
|
|
{
|
|
EnderecoTx = motor._EnderecoCAN_Tx,
|
|
EnderecoRx = motor._EnderecoCAN_Rx
|
|
};
|
|
|
|
DadosLeitura.Add(item);
|
|
}
|
|
else
|
|
{
|
|
item.EnderecoRx = motor._EnderecoCAN_Rx;
|
|
}
|
|
|
|
return item;
|
|
}
|
|
|
|
item =
|
|
DadosLeitura.FirstOrDefault(
|
|
x =>
|
|
x.EnderecoTx == idTxMsg ||
|
|
x.EnderecoRx == idTxMsg ||
|
|
x.EnderecoTx == idRxMsg ||
|
|
x.EnderecoRx == idRxMsg
|
|
);
|
|
|
|
if (item == null)
|
|
{
|
|
item = new MKS057DModel
|
|
{
|
|
EnderecoTx = idTxMsg,
|
|
EnderecoRx = idRxMsg
|
|
};
|
|
|
|
DadosLeitura.Add(item);
|
|
}
|
|
|
|
return item;
|
|
}
|
|
}
|
|
|
|
public void ProcessarMensagem(CanMessage mensagem)
|
|
{
|
|
try
|
|
{
|
|
if (mensagem == null || mensagem.DataRx == null || mensagem.DataRx.Length < 1)
|
|
{
|
|
return;
|
|
}
|
|
|
|
MksFuncCode func = (MksFuncCode)mensagem.DataRx[0];
|
|
byte[] data = mensagem.DataRx;
|
|
|
|
var item = ObterOuCriar(mensagem);
|
|
|
|
bool estavaOffline = !item.Iniciado && item.UltimoComandoRecebido != DateTime.MinValue;
|
|
|
|
switch (func)
|
|
{
|
|
case MksFuncCode.EncoderCarregado:
|
|
ProcessarEncoderCarregado(item, data);
|
|
break;
|
|
|
|
case MksFuncCode.EncoderAcumulado:
|
|
ProcessarEncoderAcumulado(item, data);
|
|
break;
|
|
|
|
case MksFuncCode.RPM:
|
|
ProcessarRpm(item, data);
|
|
break;
|
|
|
|
case MksFuncCode.PulsosRecebidos:
|
|
ProcessarPulsosRecebidos(item, data);
|
|
break;
|
|
|
|
case MksFuncCode.IO:
|
|
ProcessarIo(item, data);
|
|
break;
|
|
|
|
case MksFuncCode.ErroAcumulado:
|
|
ProcessarErroAcumulado(item, data);
|
|
break;
|
|
|
|
case MksFuncCode.Versao:
|
|
ProcessarVersao(item, data);
|
|
break;
|
|
|
|
case MksFuncCode.ModoControleAnguloRelativo:
|
|
case MksFuncCode.ModoControleAnguloAbsoluto:
|
|
case MksFuncCode.ModoControlePulsos:
|
|
ProcessarStatusMovimento(item, data);
|
|
break;
|
|
|
|
case MksFuncCode.SetZero:
|
|
case MksFuncCode.HabilitarMotor:
|
|
/*
|
|
* ACK simples, quando existir.
|
|
*/
|
|
break;
|
|
}
|
|
|
|
item.UltimoComandoRecebido = DateTime.Now;
|
|
item.Iniciado = true;
|
|
|
|
if (estavaOffline)
|
|
{
|
|
Variaveis.MostrarLog($"[MKS057DCanService] [CONEXAO] Driver CAN 0x{item.EnderecoTx:X2} ONLINE novamente");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Variaveis.MostrarLog("[MKS057DCanService] Erro ao interpretar resposta CAN: " + ex.Message);
|
|
}
|
|
}
|
|
|
|
private void ProcessarEncoderCarregado(MKS057DModel item, byte[] data)
|
|
{
|
|
if (data == null || data.Length < 7)
|
|
return;
|
|
|
|
int carry = DecodeInt32BE(data, 1);
|
|
ushort value = DecodeUInt16BE(data, 5);
|
|
|
|
int pulsos = (int)(carry * 16384.0) + value;
|
|
|
|
item.NumeroPulsosRelativo = pulsos;
|
|
item.AnguloPulsosRelativo = PulsosEncoderParaAngulo(pulsos);
|
|
}
|
|
|
|
private void ProcessarEncoderAcumulado(MKS057DModel item, byte[] data)
|
|
{
|
|
if (data == null || data.Length < 7)
|
|
return;
|
|
|
|
long pulsos = DecodeInt48BE(data, 1);
|
|
|
|
item.NumeroPulsosAbsoluto = (int)pulsos;
|
|
item.AnguloPulsosAbsoluto = PulsosEncoderParaAngulo((int)pulsos);
|
|
}
|
|
|
|
private void ProcessarRpm(MKS057DModel item, byte[] data)
|
|
{
|
|
if (data == null || data.Length < 3)
|
|
return;
|
|
|
|
short rpm = DecodeInt16BE(data, 1);
|
|
|
|
item.RPM.valor = rpm;
|
|
|
|
Variaveis.OperacaoEmAndamento?.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.DirMotor._EnderecoCAN_Tx == item.EnderecoTx)?.DirMotor?.AtualizarEmMovimento(Math.Abs(rpm) > 1);
|
|
}
|
|
|
|
private void ProcessarPulsosRecebidos(MKS057DModel item, byte[] data)
|
|
{
|
|
if (data == null || data.Length < 5)
|
|
return;
|
|
|
|
int pulsos = DecodeInt32BE(data, 1);
|
|
|
|
item.NumeroPulsos.valor = pulsos;
|
|
item.AnguloPulsos = PulsosRecebidosParaAngulo(pulsos, item.Reducao);
|
|
}
|
|
|
|
private void ProcessarIo(MKS057DModel item, byte[] data)
|
|
{
|
|
if (data == null || data.Length < 3)
|
|
return;
|
|
|
|
byte status = data[1];
|
|
|
|
item.Entradas = new List<bool>() { false, false };
|
|
item.Entradas[0] = (status & 0b00000001) != 0;
|
|
item.Entradas[1] = (status & 0b00000010) != 0;
|
|
|
|
item.Saidas = new List<bool>() { false, false };
|
|
item.Saidas[0] = (status & 0b00000100) != 0;
|
|
item.Saidas[1] = (status & 0b00001000) != 0;
|
|
|
|
item.EntradaIN1.valor = item.Entradas[0];
|
|
}
|
|
|
|
private void ProcessarErroAcumulado(MKS057DModel item, byte[] data)
|
|
{
|
|
if (data == null || data.Length < 3)
|
|
return;
|
|
|
|
item.ErroAcumulado = DecodeUInt16BE(data, 1);
|
|
}
|
|
|
|
private void ProcessarVersao(MKS057DModel item, byte[] data)
|
|
{
|
|
if (data == null || data.Length < 5)
|
|
return;
|
|
|
|
item.FV = $"{data[1]}.{data[2]}.{data[3]}.{data[4]}";
|
|
}
|
|
|
|
private void ProcessarStatusMovimento(MKS057DModel item, byte[] data)
|
|
{
|
|
if (data == null || data.Length < 2)
|
|
return;
|
|
|
|
item.StatusMovimento = (MksMovimentStatus)data[1];
|
|
}
|
|
}
|
|
}
|
|
}
|