diff --git a/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs b/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs index 4fd5c6b3d..9be4a4842 100644 --- a/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs +++ b/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs @@ -416,7 +416,7 @@ namespace AgroBase.Forms.Operacoes $"{id} Fator Curva", LogsMovimentacao, id, - x => x.FatorCompensacaoCurva, + x => x.FatorCompensacao, visivel: false )); } diff --git a/AgroBase/AgroBase/Models/GeneralJoystick.cs b/AgroBase/AgroBase/Models/GeneralJoystick.cs index 5710f8f14..84f6eddef 100644 --- a/AgroBase/AgroBase/Models/GeneralJoystick.cs +++ b/AgroBase/AgroBase/Models/GeneralJoystick.cs @@ -696,7 +696,8 @@ namespace AgroBase if (cmd == null) return; cmd.CalcularValor(direcao == Direcao.Parado, valorJoy: valor); bool mudou = direcao != _ultimaDirecaoMovEnviada || double.IsNaN(_ultimoValorMovEnviado) || Math.Abs(cmd.ValorAplicar - _ultimoValorMovEnviado) >= VariacaoMinimaMov; - bool heartbeat = (DateTime.Now - _ultimoEnvioMov).TotalMilliseconds >= HeartbeatComandoMs; + bool combinado = MovimentosCombinados.Contains(Variaveis.OperacaoEmAndamento.Controle.TipoMovimento); + bool heartbeat = !combinado && (DateTime.Now - _ultimoEnvioDir).TotalMilliseconds >= HeartbeatComandoMs; if (!mudou && !heartbeat) return; EnviarComandoMotorAtualizado(cmd, ForcarComando: false); _ultimaDirecaoMovEnviada = direcao; _ultimoValorMovEnviado = cmd.ValorAplicar; _ultimoEnvioMov = DateTime.Now; diff --git a/AgroBase/AgroBase/Models/MKS057DModel.cs b/AgroBase/AgroBase/Models/MKS057DModel.cs index b522c9cf2..904b48963 100644 --- a/AgroBase/AgroBase/Models/MKS057DModel.cs +++ b/AgroBase/AgroBase/Models/MKS057DModel.cs @@ -101,23 +101,44 @@ namespace AgroBase.Models MomentoUltimaLeitura = agora; } public double AnguloEstimado() + { + return AnguloEstimadoEm(DateTime.Now); + } + + public double AnguloEstimadoEm(DateTime momento, double maxExtrapolacaoMs = 500) { if (MomentoUltimaLeitura == DateTime.MinValue) return AnguloAtual; - double dtMs = (DateTime.Now - MomentoUltimaLeitura).TotalMilliseconds; - /* - * Durante referenciamento PulsosRecebidos trabalha rápido. - * Não deixamos uma leitura velha continuar "viajando" indefinidamente. + * Positivo: + * o evento aconteceu depois da última leitura de pulsos. + * + * Negativo: + * já recebemos uma leitura de pulsos posterior ao evento + * e estamos estimando para trás. */ - dtMs = Math.Max(0, Math.Min(dtMs, 150)); + double dtMs = (momento - MomentoUltimaLeitura).TotalMilliseconds; + + if (Math.Abs(dtMs) > maxExtrapolacaoMs) + { + Variaveis.MostrarLog( + $"[MKS057DModel] [AnguloEstimadoEm] [ADDR {EnderecoTx}] " + + $"Estimativa recusada | " + + $"delta={dtMs:F0}ms > limite={maxExtrapolacaoMs:F0}ms" + ); + + return double.NaN; + } double estimado = AnguloAtual + (VelocidadeEstimada * dtMs); Variaveis.MostrarLog( - $"[MKS057DModel] [AnguloEstimado] [ADDR {EnderecoTx}] " + - $"{AnguloAtual:F2}° + ({VelocidadeEstimada:F4} * {dtMs:F0}ms) = {estimado:F2}°" + $"[MKS057DModel] [AnguloEstimadoEm] [ADDR {EnderecoTx}] " + + $"base={AnguloAtual:F3}° | " + + $"vel={VelocidadeEstimada:F5}°/ms | " + + $"delta={dtMs:F0}ms | " + + $"estimado={estimado:F3}°" ); return estimado; diff --git a/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs b/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs index be09673a8..0c726acb7 100644 --- a/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs +++ b/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs @@ -496,73 +496,282 @@ namespace AgroBase.Models.Modules * dos quatro conjuntos. */ private const double FolgaMinimaReferencia = 0.05; + private const double FolgaAlertaReferencia = 6.0; private const double FolgaMaximaReferencia = 8.0; private static readonly TimeSpan TimeoutPrimeiraBorda = TimeSpan.FromSeconds(120); private static readonly TimeSpan TimeoutSegundaBorda = TimeSpan.FromSeconds(30); private static readonly TimeSpan TimeoutCentro = TimeSpan.FromSeconds(30); + private const int TentativasLeituraReferencia = 3; + private const int TimeoutLeituraReferenciaMs = 900; + private const int EsperaEntreTentativasLeituraMs = 80; + private const double DeltaIdealIoPulsoMs = 350; + private const double DeltaMaximoIoPulsoMs = 500; + private const int TentativasReferenciamento = 3; + private const int EsperaEntreTentativasReferenciaMs = 500; + private bool _falhaCriticaReferenciamento = false; + private const double MovimentoMaximoConsideradoParado = 0.12; + private const int TentativasParadaReferencia = 3; + private static readonly TimeSpan IntervaloConfirmacaoParada = TimeSpan.FromMilliseconds(250); - private async Task AtualizarPulsosAgoraAsync(int timeoutMs = 800) + private async Task AtualizarPulsosAgoraAsync(int timeoutMs = TimeoutLeituraReferenciaMs, int tentativas = TentativasLeituraReferencia) + { + for (int tentativa = 1; + tentativa <= tentativas; + tentativa++) + { + if (!Inicializado || Leitura == null) + return false; + + DateTime leituraAnterior = + Leitura.NumeroPulsos.lidoEm; + + MKS057DCanService.RequisitarDadoForcado( + _EnderecoCAN_Tx, + _EnderecoCAN_Rx, + MKS057DCanService.MksHandler.MksFuncCode.PulsosRecebidos + ); + + bool NovaLeitura() + { + var l = Leitura; + + return + l != null && + l.NumeroPulsos.lidoEm > leituraAnterior; + } + + await FuncoesGlobais.AguardarCondicaoAsync( + NovaLeitura, + timeoutMs, + 20 + ); + + if (NovaLeitura()) + { + if (tentativa > 1) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"PULSOS recuperados na tentativa " + + $"{tentativa}/{tentativas} | " + + $"dt={Leitura.NumeroPulsos.dt * 1000.0:F0}ms | " + + $"freq={Leitura.NumeroPulsos.frequencia:F1}Hz" + ); + } + + return true; + } + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"PULSOS sem resposta nova | " + + $"tentativa={tentativa}/{tentativas} | " + + $"idade={ObterIdadePulsosMs():F0}ms" + ); + + if (tentativa < tentativas) + await Task.Delay(EsperaEntreTentativasLeituraMs); + } + + return false; + } + + private async Task AtualizarIOAgoraAsync(int timeoutMs = TimeoutLeituraReferenciaMs, int tentativas = TentativasLeituraReferencia) + { + for (int tentativa = 1; + tentativa <= tentativas; + tentativa++) + { + if (!Inicializado || Leitura == null) + return false; + + DateTime leituraAnterior = + Leitura.EntradaIN1.lidoEm; + + MKS057DCanService.RequisitarDadoForcado( + _EnderecoCAN_Tx, + _EnderecoCAN_Rx, + MKS057DCanService.MksHandler.MksFuncCode.IO + ); + + bool NovaLeitura() + { + var l = Leitura; + + return + l != null && + l.EntradaIN1.lidoEm > leituraAnterior; + } + + await FuncoesGlobais.AguardarCondicaoAsync( + NovaLeitura, + timeoutMs, + 20 + ); + + if (NovaLeitura()) + { + if (tentativa > 1) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"IO recuperado na tentativa " + + $"{tentativa}/{tentativas} | " + + $"IN1={IN1_Atuado} | " + + $"dt={Leitura.EntradaIN1.dt * 1000.0:F0}ms | " + + $"freq={Leitura.EntradaIN1.frequencia:F1}Hz" + ); + } + + return true; + } + + double idade = + Leitura.EntradaIN1.lidoEm == DateTime.MinValue + ? double.PositiveInfinity + : (DateTime.Now - + Leitura.EntradaIN1.lidoEm) + .TotalMilliseconds; + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"IO sem resposta nova | " + + $"tentativa={tentativa}/{tentativas} | " + + $"idade={idade:F0}ms" + ); + + if (tentativa < tentativas) + await Task.Delay(EsperaEntreTentativasLeituraMs); + } + + return false; + } + + private double ObterIdadePulsosMs() { var leitura = Leitura; - if (leitura == null || !Inicializado) - return false; - - DateTime leituraAnterior = leitura.NumeroPulsos.lidoEm; - - MKS057DCanService.RequisitarDadoForcado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, MKS057DCanService.MksHandler.MksFuncCode.PulsosRecebidos); - - bool NovaLeitura() + if (leitura == null || + leitura.NumeroPulsos.lidoEm == DateTime.MinValue) { - var l = Leitura; - - return - l != null && - l.NumeroPulsos.lidoEm > leituraAnterior; + return double.PositiveInfinity; } - await FuncoesGlobais.AguardarCondicaoAsync(NovaLeitura, timeoutMs, 20); - - return NovaLeitura(); + return + (DateTime.Now - + leitura.NumeroPulsos.lidoEm) + .TotalMilliseconds; } - private async Task AtualizarIOAgoraAsync(int timeoutMs = 800) + private async Task ObterAnguloNaBordaAsync(DateTime momentoIO, string etapa) { - var leitura = Leitura; + if (!Inicializado || Leitura == null) + return null; - if (leitura == null || !Inicializado) - return false; + DateTime momentoPulsos = + Leitura.NumeroPulsos.lidoEm; - DateTime leituraAnterior = leitura.EntradaIN1.lidoEm; + double deltaIoPulsoMs = + momentoPulsos == DateTime.MinValue + ? double.PositiveInfinity + : (momentoIO - momentoPulsos) + .TotalMilliseconds; - MKS057DCanService.RequisitarDadoForcado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, MKS057DCanService.MksHandler.MksFuncCode.IO); + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: sincronismo inicial | " + + $"IO={momentoIO:HH:mm:ss.fff} | " + + $"PULSOS={momentoPulsos:HH:mm:ss.fff} | " + + $"delta_io_pulso={deltaIoPulsoMs:F0}ms | " + + $"idade_pulsos_agora={ObterIdadePulsosMs():F0}ms | " + + $"dt_pulsos={Leitura.NumeroPulsos.dt * 1000.0:F0}ms | " + + $"freq_pulsos={Leitura.NumeroPulsos.frequencia:F1}Hz" + ); - bool NovaLeitura() + /* + * A posição está relativamente próxima do evento. + * Podemos estimar diretamente. + */ + if (Math.Abs(deltaIoPulsoMs) <= DeltaIdealIoPulsoMs) { - var l = Leitura; + double angulo = + Leitura.AnguloEstimadoEm( + momentoIO, + DeltaMaximoIoPulsoMs + ); - return - l != null && - l.EntradaIN1.lidoEm > leituraAnterior; + if (!double.IsNaN(angulo)) + return angulo; } - await FuncoesGlobais.AguardarCondicaoAsync(NovaLeitura, timeoutMs, 20); + /* + * A última posição ficou velha demais. + * + * Não descartamos imediatamente a borda. + * Buscamos uma leitura nova de pulsos e então estimamos + * para trás até o timestamp exato do IO. + */ + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: posição distante do evento; " + + $"solicitando PULSOS para reconstruir a borda." + ); - return NovaLeitura(); - } + bool atualizou = + await AtualizarPulsosAgoraAsync( + timeoutMs: 600, + tentativas: 2 + ); - private bool PulsosEstaoRecentes(double maxIdadeMs = 200) - { - var leitura = Leitura; - - if (leitura == null || leitura.NumeroPulsos.lidoEm == DateTime.MinValue) + if (!atualizou || Leitura == null) { - return false; + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: não foi possível obter pulsos " + + $"para reconstruir a borda." + ); + + return null; } - return (DateTime.Now - leitura.NumeroPulsos.lidoEm).TotalMilliseconds <= maxIdadeMs; - } + momentoPulsos = + Leitura.NumeroPulsos.lidoEm; + deltaIoPulsoMs = + (momentoIO - momentoPulsos) + .TotalMilliseconds; + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: sincronismo após atualização | " + + $"delta_io_pulso={deltaIoPulsoMs:F0}ms | " + + $"angulo_pulsos={AnguloDriver:F3}°" + ); + + if (Math.Abs(deltaIoPulsoMs) > + DeltaMaximoIoPulsoMs) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: borda recusada | " + + $"|delta_io_pulso|={Math.Abs(deltaIoPulsoMs):F0}ms > " + + $"{DeltaMaximoIoPulsoMs:F0}ms" + ); + + return null; + } + + double estimado = + Leitura.AnguloEstimadoEm( + momentoIO, + DeltaMaximoIoPulsoMs + ); + + if (double.IsNaN(estimado)) + return null; + + return estimado; + } public async Task ReferenciaMotor() @@ -579,71 +788,201 @@ namespace AgroBase.Models.Modules Referenciando = true; Referenciado = false; - DateTime inicio = + DateTime inicioGeral = DateTime.Now; try { - /* - * Atualmente o produto usa somente referência relativa MKS. - * - * Quando o sensor absoluto voltar para o produto, - * tratamos o outro fluxo com o mesmo rigor. - */ - bool sucesso = - await ReferenciaMotorRelativo(); - - Referenciado = - sucesso; - - if (!sucesso) + for (int tentativa = 1; + tentativa <= TentativasReferenciamento; + tentativa++) { + _falhaCriticaReferenciamento = false; + Variaveis.MostrarLog( $"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " + - $"Referenciamento FALHOU." + $"===== TENTATIVA {tentativa}/{TentativasReferenciamento} =====" ); - op.Sensoriamento.InserirLog( - T_Code.Dir, - StatusModulo.Falha, - 0, - "Referenciamento Falhou!", - id: Mod_ID - ); + DateTime inicioTentativa = + DateTime.Now; - return false; + bool sucesso = + await ReferenciaMotorRelativo(); + + if (sucesso) + { + Referenciado = true; + + UltimoReferenciamento = + DateTime.Now; + + MKS057DCanService.UltimoReferenciamento = + UltimoReferenciamento; + + double tempoTentativa = + (DateTime.Now - inicioTentativa) + .TotalSeconds; + + double tempoTotal = + (DateTime.Now - inicioGeral) + .TotalSeconds; + + Variaveis.MostrarLog( + $"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " + + $"REFERENCIAMENTO CONCLUÍDO E CONFIRMADO | " + + $"tentativa={tentativa}/{TentativasReferenciamento} | " + + $"tempo_tentativa={tempoTentativa:F1}s | " + + $"tempo_total={tempoTotal:F1}s | " + + $"folga={FolgaMecanica:F3}°" + ); + + op.Sensoriamento.InserirLog( + T_Code.Dir, + StatusModulo.Operante, + 100, + $"Referenciamento concluído na tentativa " + + $"{tentativa}/{TentativasReferenciamento}.", + id: Mod_ID + ); + + return true; + } + + Referenciado = false; + + /* + * Driver desapareceu durante o processo. + * + * Não faz sentido ficar tentando referência + * sem comunicação. + */ + if (!Inicializado) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " + + $"Retry CANCELADO: driver não está inicializado." + ); + + break; + } + + /* + * Falha física/lógica grave. + * + * Exemplo: eixo movimentou no sentido contrário + * ao comando. + */ + if (_falhaCriticaReferenciamento) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " + + $"Retry CANCELADO por falha crítica." + ); + + break; + } + + if (tentativa < + TentativasReferenciamento) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " + + $"Tentativa {tentativa} falhou. " + + $"Preparando retry automático..." + ); + + bool parou = + await PararEConfirmarAsync( + $"RETRY_{tentativa}_PARADA" + ); + + if (!parou) + { + _falhaCriticaReferenciamento = true; + + Variaveis.MostrarLog( + $"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " + + $"Retry CANCELADO: não foi possível " + + $"comprovar parada do eixo." + ); + + break; + } + + await Task.Delay( + EsperaEntreTentativasReferenciaMs + ); + + /* + * Pequeno precheck antes de começar outra tentativa. + */ + bool ioOk = + await AtualizarIOAgoraAsync( + timeoutMs: 700, + tentativas: 2 + ); + + bool pulsosOk = + await AtualizarPulsosAgoraAsync( + timeoutMs: 700, + tentativas: 2 + ); + + Variaveis.MostrarLog( + $"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " + + $"PRECHECK RETRY | " + + $"IO={ioOk} | " + + $"PULSOS={pulsosOk} | " + + $"IN1={IN1_Atuado} | " + + $"angulo={AnguloDriver:F3}°" + ); + + if (!ioOk || + !pulsosOk) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " + + $"Precheck do retry incompleto. " + + $"A próxima tentativa ainda fará seu próprio precheck." + ); + } + } } - UltimoReferenciamento = - DateTime.Now; - - /* - * Se essa propriedade global ainda for usada em algum lugar, - * agora ela representa um referenciamento REALMENTE concluído. - */ - MKS057DCanService.UltimoReferenciamento = - UltimoReferenciamento; - - double tempo = - (DateTime.Now - inicio) + double tempoTotalFalha = + (DateTime.Now - inicioGeral) .TotalSeconds; + bool paradaFinal = + await PararEConfirmarAsync( + "FALHA_FINAL_PARADA" + ); + Variaveis.MostrarLog( $"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " + - $"Referenciamento CONCLUÍDO E CONFIRMADO | " + - $"tempo={tempo:F1}s | " + - $"folga={FolgaMecanica:F3}°" + $"Estado seguro após falha | " + + $"parada_confirmada={paradaFinal} | " + + $"angulo={AnguloDriver:F3}°" + ); + + Variaveis.MostrarLog( + $"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " + + $"REFERENCIAMENTO FALHOU DEFINITIVAMENTE | " + + $"tentativas={TentativasReferenciamento} | " + + $"tempo={tempoTotalFalha:F1}s" ); op.Sensoriamento.InserirLog( T_Code.Dir, - StatusModulo.Operante, - 100, - "Referenciamento Concluído!", + StatusModulo.Falha, + 0, + $"Referenciamento falhou após " + + $"{TentativasReferenciamento} tentativa(s).", id: Mod_ID ); - return true; + return false; } catch (Exception ex) { @@ -837,21 +1176,27 @@ namespace AgroBase.Models.Modules * ============================================================ */ - double folga = - Math.Abs( - borda1.Value - - borda2.Value - ); + double folga = Math.Abs(borda1.Value - borda2.Value); - if (folga < FolgaMinimaReferencia || - folga > FolgaMaximaReferencia) + if (folga < FolgaMinimaReferencia || folga > FolgaMaximaReferencia) { return FalharReferencia( - $"Folga medida fora da faixa esperada: {folga:F3}° " + + $"Folga medida fora da faixa física admissível: {folga:F3}° " + $"(esperado {FolgaMinimaReferencia:F2}..{FolgaMaximaReferencia:F2}°)." ); } + if (folga > FolgaAlertaReferencia) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"ALERTA DE FOLGA ELEVADA | " + + $"folga={folga:F3}° | " + + $"limite_alerta={FolgaAlertaReferencia:F2}° | " + + $"limite_max={FolgaMaximaReferencia:F2}°" + ); + } + double centro = ((borda1.Value + borda2.Value) / 2.0) + OffsetReferenciamento; @@ -951,43 +1296,65 @@ namespace AgroBase.Models.Modules private async Task SetZeroConfirmadoAsync(string etapa, int tentativas = 3) { - for (int tentativa = 1; tentativa <= tentativas; tentativa++) + /* + * ZERO somente com motor comprovadamente imóvel. + */ + if (!await PararEConfirmarAsync( + etapa + "_PRE_ZERO")) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: SetZero ABORTADO porque " + + $"a parada não foi confirmada." + ); + + return false; + } + + for (int tentativa = 1; + tentativa <= tentativas; + tentativa++) { if (!Inicializado) return false; Variaveis.MostrarLog( $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + - $"{etapa}: SetZero tentativa {tentativa}/{tentativas}" + $"{etapa}: SetZero tentativa " + + $"{tentativa}/{tentativas}" ); - MKS057DCanService.SetZero(_EnderecoCAN_Tx, _EnderecoCAN_Rx); + MKS057DCanService.SetZero( + _EnderecoCAN_Tx, + _EnderecoCAN_Rx + ); - /* - * Deixa o comando entrar na fila/processar. - * Depois solicitamos explicitamente uma posição nova. - */ await Task.Delay(100); - bool recebeuPulsos = await AtualizarPulsosAgoraAsync(); + bool recebeuPulsos = + await AtualizarPulsosAgoraAsync(); if (!recebeuPulsos) { Variaveis.MostrarLog( $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + - $"{etapa}: nenhuma leitura nova de pulsos após SetZero." + $"{etapa}: nenhuma leitura nova " + + $"de pulsos após SetZero." ); continue; } - double angulo = AnguloDriver; + double angulo = + AnguloDriver; - if (Math.Abs(angulo) <= ToleranciaZeroReferenciamento) + if (Math.Abs(angulo) <= + ToleranciaZeroReferenciamento) { Variaveis.MostrarLog( $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + - $"{etapa}: SetZero CONFIRMADO | angulo={angulo:F3}°" + $"{etapa}: SetZero CONFIRMADO | " + + $"angulo={angulo:F3}°" ); return true; @@ -995,7 +1362,8 @@ namespace AgroBase.Models.Modules Variaveis.MostrarLog( $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + - $"{etapa}: SetZero NÃO confirmado | angulo={angulo:F3}°" + $"{etapa}: SetZero NÃO confirmado | " + + $"angulo={angulo:F3}°" ); } @@ -1068,10 +1436,14 @@ namespace AgroBase.Models.Modules if (Math.Sign(delta) != Math.Sign(deltaEsperado)) { + _falhaCriticaReferenciamento = true; + Variaveis.MostrarLog( $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + $"{etapa}: movimento no sentido INESPERADO | " + - $"delta={delta:F3}° esperado={Math.Sign(deltaEsperado)}" + $"delta={delta:F3}° | " + + $"delta_esperado={deltaEsperado:F3}° | " + + $"FALHA CRÍTICA, retry bloqueado" ); return false; @@ -1093,6 +1465,174 @@ namespace AgroBase.Models.Modules return false; } + private async Task PararEConfirmarAsync(string etapa, int tentativas = TentativasParadaReferencia) + { + if (!Inicializado || Leitura == null) + return false; + + for (int tentativa = 1; + tentativa <= tentativas; + tentativa++) + { + /* + * Primeiro tentamos o comando de parada normal do protocolo. + */ + MKS057DCanService.Parar( + _EnderecoCAN_Tx, + _EnderecoCAN_Rx + ); + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: PARAR enviado | " + + $"tentativa={tentativa}/{tentativas}" + ); + + await Task.Delay(150); + + if (!await AtualizarPulsosAgoraAsync( + timeoutMs: 600, + tentativas: 2)) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: sem posição após PARAR." + ); + + continue; + } + + double angulo1 = + AnguloDriver; + + await Task.Delay( + IntervaloConfirmacaoParada + ); + + if (!await AtualizarPulsosAgoraAsync( + timeoutMs: 600, + tentativas: 2)) + { + continue; + } + + double angulo2 = + AnguloDriver; + + double deslocamento = + Math.Abs( + angulo2 - + angulo1 + ); + + if (deslocamento <= + MovimentoMaximoConsideradoParado) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: PARADA CONFIRMADA | " + + $"a1={angulo1:F3}° | " + + $"a2={angulo2:F3}° | " + + $"delta={deslocamento:F3}°" + ); + + return true; + } + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: motor ainda em movimento após PARAR | " + + $"a1={angulo1:F3}° | " + + $"a2={angulo2:F3}° | " + + $"delta={deslocamento:F3}°" + ); + + /* + * Fallback robusto: + * + * se o "Parar" do protocolo não matou a trajetória, + * substituímos o alvo antigo pela posição atual. + * + * Sabemos que o modo de posição absoluta funciona. + */ + double anguloHold = + angulo2; + + MKS057DCanService.Movimento( + _EnderecoCAN_Tx, + _EnderecoCAN_Rx, + anguloHold, + VelocidadeReferenciamento, + Sentido.Parado + ); + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: HOLD POSITION enviado | " + + $"alvo={anguloHold:F3}°" + ); + + await Task.Delay(250); + + if (!await AtualizarPulsosAgoraAsync( + timeoutMs: 600, + tentativas: 2)) + { + continue; + } + + double hold1 = + AnguloDriver; + + await Task.Delay( + IntervaloConfirmacaoParada + ); + + if (!await AtualizarPulsosAgoraAsync( + timeoutMs: 600, + tentativas: 2)) + { + continue; + } + + double hold2 = + AnguloDriver; + + double deslocamentoHold = + Math.Abs( + hold2 - + hold1 + ); + + if (deslocamentoHold <= + MovimentoMaximoConsideradoParado) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: PARADA CONFIRMADA VIA HOLD | " + + $"a1={hold1:F3}° | " + + $"a2={hold2:F3}° | " + + $"delta={deslocamentoHold:F3}°" + ); + + return true; + } + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: HOLD não estabilizou o eixo | " + + $"delta={deslocamentoHold:F3}°" + ); + } + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: NÃO foi possível confirmar parada." + ); + + return false; + } + private async Task AguardarTransicaoIN1Async(bool estadoOrigem, double anguloLimite, TimeSpan timeout, string etapa) { DateTime limite = DateTime.Now.Add(timeout); @@ -1132,7 +1672,7 @@ namespace AgroBase.Models.Modules * Se chegou ao limite de busca sem mudar IN1, * não existe motivo para continuar esperando. */ - if (PulsosEstaoRecentes(300) && Math.Abs(AnguloDriver - anguloLimite) <= 1.0) + if (ObterIdadePulsosMs() <= DeltaMaximoIoPulsoMs && Math.Abs(AnguloDriver - anguloLimite) <= 1.0) { Variaveis.MostrarLog( $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + @@ -1152,21 +1692,37 @@ namespace AgroBase.Models.Modules * Só aceitamos a borda se a posição estiver recente. * Nada de calcular referência com dado velho. */ - if (!PulsosEstaoRecentes()) + /* + * IMPORTANTE: + * + * Capturamos primeiro o timestamp EXATO da leitura + * em que detectamos a mudança do IN1. + */ + DateTime ioCandidato = + Leitura.EntradaIN1.lidoEm; + + bool estadoCandidato = + estadoAtual; + + double? anguloBorda = + await ObterAnguloNaBordaAsync( + ioCandidato, + etapa + ); + + if (!anguloBorda.HasValue) { Variaveis.MostrarLog( $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + - $"{etapa}: IN1 mudou, mas posição está desatualizada." + $"{etapa}: transição válida de IN1, " + + $"mas não foi possível reconstruir posição confiável." ); return null; } - double anguloCandidato = Leitura.AnguloEstimado(); - - bool estadoCandidato = estadoAtual; - - DateTime ioCandidato = Leitura.EntradaIN1.lidoEm; + double anguloCandidato = + anguloBorda.Value; /* * Debounce lógico: diff --git a/AgroBase/AgroBase/Models/Modules/MovimentacaoModel.cs b/AgroBase/AgroBase/Models/Modules/MovimentacaoModel.cs index f0640fa92..08d2a70c4 100644 --- a/AgroBase/AgroBase/Models/Modules/MovimentacaoModel.cs +++ b/AgroBase/AgroBase/Models/Modules/MovimentacaoModel.cs @@ -22,7 +22,7 @@ namespace AgroBase.Models.Modules return Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.MovMotor._EnderecoCAN_Tx == _EnderecoCAN_Tx)?.Modulo_ID ?? ""; } } - public double LeituraRPM { get; set; } = 0; + public double UltimoRPMComandado { get; set; } = 0; public bool ComandoLigado { get @@ -304,65 +304,7 @@ namespace AgroBase.Models.Modules } } - public bool CompensacaoNecessaria - { - get - { - if (ComandoLigado) - { - var op = Variaveis.OperacaoEmAndamento; - - var _Controle = op.Controle; - var _ControleDir = _Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir); - - bool movimentoCompensado = - (_Controle.TipoMovimento == TipoMovimentoDirecional.MovimentoArco) || - (_Controle.TipoMovimento == TipoMovimentoDirecional.RodasDianteiras && Mod_ID.Contains("F")) || - (_Controle.TipoMovimento == TipoMovimentoDirecional.RodasTraseiras && Mod_ID.Contains("T")); - bool emCurva = _ControleDir.UltimaDirecao != Direcao.Parado; - double anguloDir = Math.Abs(op.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.Modulo_ID == Mod_ID)?.DirMotor?.Angulo_SP ?? 0); - bool direcionalAtuado = anguloDir > 0.0; - - if (movimentoCompensado && emCurva && direcionalAtuado) - { - if ( - (_ControleDir.UltimaDirecao == Direcao.Esquerda && Mod_ID.Contains("D")) || - (_ControleDir.UltimaDirecao == Direcao.Direita && Mod_ID.Contains("E")) - ) - { - return true; - } - } - } - - return false; - } - } - - private float FatorCompensacaoCurvas - { - get - { - if (CompensacaoNecessaria) - { - //double compensacao = 1 + FuncoesMatematicas.Map(Math.Abs(Variaveis.OperacaoEmAndamento.Controle.Angulo), 0.0, Variaveis.OperacaoEmAndamento.Controle.Angulo_Max, 0.0, 0.3); - double compensacao = MovimentacaoMovimentoCompensadoModel.compensacoes[Mod_ID]; - return (float)compensacao; - } - else - { - return 1; - } - } - } - private float FatorCompensacaoCurvasAnterior { get; set; } = 1.0f; - public bool NovoRPMNecessario - { - get - { - return FatorCompensacaoCurvas != FatorCompensacaoCurvasAnterior; - } - } + public Dictionary ConfigSentidos { get; set; } public List Funcoes { get; set; } @@ -377,6 +319,8 @@ namespace AgroBase.Models.Modules public double UltimoSetPointEnviado { get; set; } = 0; public Direcao UltimaDirecao { get; set; } = Direcao.Parado; public bool UltimoFreioSP { get; set; } = false; + private static readonly TimeSpan IntervaloReafirmacaoControle = TimeSpan.FromSeconds(1.5); + private const int DeltaMinimoRpmNovoComando = 3; public static MovimentacaoModel CarregarParametrosIniciais(string Mod_ID, byte _EnderecoTx, byte _EnderecoRx) @@ -504,14 +448,17 @@ namespace AgroBase.Models.Modules } RPM_SP_Controle = _Controle.RPM_Motor; - RPM_SP = - DirecaoAtual == Direcao.Parado || Freado ? 0 : - RPM_SP_Controle; - int RpmCorrigido = Math.Min((int)(VariaveisEquipamento.RPM_Max_Roda * VariaveisEquipamento.RelacaoRPM), Math.Max(RPM_SP != 0 ? (int)(VariaveisEquipamento.RPM_Min_Roda * VariaveisEquipamento.RelacaoRPM) : 0, Convert.ToInt32(RPM_SP * FatorCompensacaoCurvas))); - //Console.WriteLine($"[{Mod_ID}] Direcao: {DirecaoAtual}, RPM_SP: {RPM_SP / VariaveisEquipamento.RelacaoRPM}, FC: {FatorCompensacaoCurvas.ToString("0.00")}, Novo RPM: {RpmCorrigido / VariaveisEquipamento.RelacaoRPM}"); - RPM_SP = Math.Abs(RpmCorrigido); - FatorCompensacaoCurvasAnterior = FatorCompensacaoCurvas; + if (DirecaoAtual == Direcao.Parado || Freado) + { + RPM_SP = 0; + } + else + { + int rpmMinMotor = Convert.ToInt32(VariaveisEquipamento.RPM_Min_Roda * VariaveisEquipamento.RelacaoRPM); + int rpmMaxMotor = Convert.ToInt32(VariaveisEquipamento.RPM_Max_Roda * VariaveisEquipamento.RelacaoRPM); + RPM_SP = MovimentacaoMovimentoCompensadoModel.CalcularRpmMotor(Mod_ID, RPM_SP_Controle, rpmMinMotor, rpmMaxMotor); + } op.SimulacaoRpmControle = _Controle.PercentualVelocidadeSP; //Console.WriteLine($"RPM Atualizado para {Variaveis.OperacaoEmAndamento.SimulacaoRpmControle}"); @@ -536,11 +483,20 @@ namespace AgroBase.Models.Modules bool PrimeiraLeitura = Leitura == null; bool SentidoAlterado = (Leitura?.Direcao ?? Sentido.Parado) != Sentido_SP; - bool RPMAlterado = LeituraRPM != RPM_SP; + bool RPMAlterado = Math.Abs(UltimoRPMComandado - RPM_SP) >= DeltaMinimoRpmNovoComando; bool LigadoAlterado = LeituraLigado != ComandoLigado; bool FreioAlterado = (Leitura?.Freado ?? false) != Freado; - bool EnviaComando = PrimeiraLeitura || SentidoAlterado || RPMAlterado || LigadoAlterado || FreioAlterado; + bool ReafirmacaoNecessaria = UltimoComandoEnviado == DateTime.MinValue || DateTime.Now - UltimoComandoEnviado >= IntervaloReafirmacaoControle; + + bool EnviaComando = + PrimeiraLeitura || + SentidoAlterado || + RPMAlterado || + LigadoAlterado || + FreioAlterado || + ReafirmacaoNecessaria; + OidFuncCode modoControle = Freado ? OidFuncCode.ComandoCorrenteFreio : ComandoLigado ? OidFuncCode.ComandoVelocidade : @@ -565,7 +521,7 @@ namespace AgroBase.Models.Modules OIDCanService.ComandoControle(_EnderecoCAN_Tx, _EnderecoCAN_Rx, modoControle, setPoint); - LeituraRPM = RPM_SP; + UltimoRPMComandado = RPM_SP; UltimoComandoEnviado = DateTime.Now; UltimoModoControleEnviado = modoControle; UltimoSetPointEnviado = setPoint; @@ -771,192 +727,558 @@ namespace AgroBase.Models.Modules public double CicloTrabalho { get; set; } } - public class MovimentacaoMovimentoCompensadoModel + public static class MovimentacaoMovimentoCompensadoModel { - static List MovimentosCompensar = new List() { TipoMovimentoDirecional.RodasDianteiras, TipoMovimentoDirecional.RodasTraseiras, TipoMovimentoDirecional.MovimentoArco }; - static GeometriaRobo g = new GeometriaRobo { L = VariaveisEquipamento.DistanciaEntreEixosCm / 100.0, W = VariaveisEquipamento.LarguraEquipamentoMm / 1000.0 }; - public static Dictionary compensacoes { get; set; } = new Dictionary(); - public static Dictionary _compAnt { get; set; } + private static readonly object _sync = new object(); + + private static readonly HashSet ModosCompensados = new HashSet + { + TipoMovimentoDirecional.RodasDianteiras, + TipoMovimentoDirecional.RodasTraseiras, + TipoMovimentoDirecional.MovimentoArco + }; + + /* + * Proteção contra geometria patológica. + * Depois podemos derivar esse teto exatamente do ângulo máximo + * permitido pelo rover. + */ + private const double FatorMaximo = 2.50; + + /* + * Tempo máximo para considerar posição do MKS fresca. + */ + private const double IdadeMaximaAnguloMs = 1000.0; + + private static DateTime _ultimoCalculo = DateTime.MinValue; + + /* + * Filtro no ÂNGULO, não no fator. + * + * Isso mantém todos os fatores pertencendo à mesma geometria. + */ + private const double TauFiltroAnguloSeg = 0.15; + + private static double _angFrenteFiltrado; + private static double _angTrasFiltrado; + + private static Dictionary _estado = new Dictionary(); + + private const double AnguloEntradaCurvaGraus = 1.0; + private const double AnguloSaidaCurvaGraus = 0.5; + private static bool _curvaAtiva; + + + public class EstadoCompensacaoRoda + { + public string ModuloId { get; set; } + public double RaioM { get; set; } + public double Fator { get; set; } = 1.0; + public bool Saturado { get; set; } + public double AnguloFrenteGraus { get; set; } + public double AnguloTrasGraus { get; set; } + public double IccX { get; set; } + public double IccY { get; set; } + } + + + private struct GeometriaRobo + { + public double L; + public double W; + } + public static void AtualizarDados() { var op = Variaveis.OperacaoEmAndamento; + var modulos = op?.DispMvd?.Dados?.Modulos; - var ctrl = op?.Controle; - var disp = op?.DispMvd?.Dados?.Modulos; - if (op?.Sensoriamento?.Operacao?.StatusOperacaoAtual != StatusOperacao.EmAndamento || ctrl == null || disp == null || !MovimentosCompensar.Contains(ctrl.TipoMovimento)) + if (modulos == null || modulos.Count == 0) { - compensacoes = (disp ?? Enumerable.Empty()).ToDictionary(x => x.Modulo_ID, x => 1.0); - return; + ResetarTudo(); + return; } - // Coleta ângulos dos módulos - var frente = disp.Where(m => m.Modulo_ID?.Contains("F") == true).Select(m => (double)(m.DirMotor?.Angulo_SP ?? 0) * (m.DirMotor.Sentido_SP == Sentido.Antihorario ? -1 : 1)); - var tras = disp.Where(m => m.Modulo_ID?.Contains("T") == true).Select(m => (double)(m.DirMotor?.Angulo_SP ?? 0) * (m.DirMotor.Sentido_SP == Sentido.Antihorario ? 1 : -1)); + var controle = op?.Controle; - // Ângulo equivalente do eixo (média em tan-space) - double angFrenteDeg = EqAngleEixo(frente); - double angTrasDeg = EqAngleEixo(tras); - - compensacoes = FatoresVelocidade( - ctrl.TipoMovimento, - angFrenteDeg, - angTrasDeg, - g, - PoliticaFator.ReferenciaNoCentro, - fatorMaximo: 1.6 - ); - - const double alpha = 0.2; - if (_compAnt != null && _compAnt.Count == compensacoes.Count) - compensacoes = compensacoes.ToDictionary(kv => kv.Key, - kv => _compAnt.TryGetValue(kv.Key, out var prev) ? prev * (1 - alpha) + kv.Value * alpha : kv.Value); - _compAnt = new Dictionary(compensacoes); - } - - public struct GeometriaRobo - { - public double L; // entre-eixos (m) - public double W; // bitola (m) - } - - // assuma ângulos em graus no seu sistema - static double Deg2Rad(double deg) => deg * Math.PI / 180.0; - - static double EqAngleEixo(IEnumerable angsDeg) - { - // Trata sequência vazia - if (angsDeg == null) return 0; - var list = angsDeg.ToList(); - if (list.Count == 0) return 0; - - // média de tan e volta pra graus - double mediaTan = list.Select(a => Math.Tan(a * Math.PI / 180.0)).Average(); - return Math.Atan(mediaTan) * 180.0 / Math.PI; - } - - static bool TryCalcularRaioCentro(TipoMovimentoDirecional modo, double deltaFrontDeg, double deltaRearDeg, GeometriaRobo g, out double Rc) - { - double tf = Math.Tan(Deg2Rad(deltaFrontDeg)); - double tr = Math.Tan(Deg2Rad(deltaRearDeg)); - - Rc = double.PositiveInfinity; - - switch (modo) + if (controle == null || !ModosCompensados.Contains(controle.TipoMovimento)) { - case TipoMovimentoDirecional.RodasDianteiras: - if (Math.Abs(tf) < 1e-6) return false; // reta - Rc = g.L / tf; - return true; + ResetarTudo(modulos); + return; + } - case TipoMovimentoDirecional.RodasTraseiras: - if (Math.Abs(tr) < 1e-6) return false; - Rc = g.L / tr; - return true; + GeometriaRobo g = ObterGeometriaAtual(); - case TipoMovimentoDirecional.MovimentoArco: - // uso geral: Rc = L / (tan(df) - tan(dr)) - double denom = (tf - tr); - if (Math.Abs(denom) < 1e-6) return false; - Rc = g.L / denom; - return true; + if (g.L <= 0 || g.W <= 0) + { + ResetarTudo(modulos); + return; + } + + /* + * Pega posição física REAL das rodas. + */ + double ef = 0; + double df = 0; + double et = 0; + double dt = 0; + + bool angulosValidos = + TryObterAnguloCanonico(modulos, "EF", out ef) && + TryObterAnguloCanonico(modulos, "DF", out df) && + TryObterAnguloCanonico(modulos, "ET", out et) && + TryObterAnguloCanonico(modulos, "DT", out dt); + + if (!angulosValidos) + { + /* + * Não inventa geometria quando não sabemos + * fisicamente onde as rodas estão. + * + * A saúde DIR decide em outro nível se a + * movimentação pode continuar. + */ + ResetarTudo(modulos); + return; + } + + /* + * Transforma as duas rodas em um ângulo equivalente + * de cada eixo. + * + * Média em tan-space é melhor do que média simples + * para geometria de esterçamento. + */ + double angFrente = AnguloEquivalenteEixo(ef, df); + double angTras = AnguloEquivalenteEixo(et, dt); + + /* + * Filtro temporal independente da frequência do timer. + */ + DateTime agora = DateTime.Now; + + double dtSeg = + _ultimoCalculo == DateTime.MinValue + ? 0 + : (agora - _ultimoCalculo).TotalSeconds; + + _ultimoCalculo = agora; + + if (dtSeg <= 0 || dtSeg > 1.0) + { + _angFrenteFiltrado = angFrente; + _angTrasFiltrado = angTras; + } + else + { + double alpha = 1.0 - Math.Exp(-dtSeg / TauFiltroAnguloSeg); + _angFrenteFiltrado += alpha * (angFrente - _angFrenteFiltrado); + _angTrasFiltrado += alpha * (angTras - _angTrasFiltrado); + } + + double deltaCurvatura = Math.Abs(_angFrenteFiltrado - _angTrasFiltrado); + + if (_curvaAtiva) + { + if (deltaCurvatura <= AnguloSaidaCurvaGraus) + { + _curvaAtiva = false; + ResetarCompensacao(modulos); + return; + } + } + else + { + if (deltaCurvatura < AnguloEntradaCurvaGraus) + { + ResetarCompensacao(modulos); + return; + } + + _curvaAtiva = true; + } + + double iccX; + double iccY; + + if (!TryCalcularICC(_angFrenteFiltrado, _angTrasFiltrado, g, out iccX, out iccY)) + { + ResetarTudo(modulos); + return; + } + + var raios = CalcularRaios(g, iccX, iccY); + + if (raios.Count == 0) + { + ResetarTudo(modulos); + return; + } + + double raioMin = raios.Values.Min(); + + /* + * ICC praticamente sobre uma roda. + * Geometria extrema demais para nossa política. + */ + if (raioMin < 0.05) + { + ResetarTudo(modulos); + return; + } + + var novoEstado = new Dictionary(); + + foreach (var kv in raios) + { + double fatorBruto = kv.Value / raioMin; + + bool saturado = fatorBruto > FatorMaximo; + + double fator = Math.Max(1.0, Math.Min(fatorBruto, FatorMaximo)); + + novoEstado[kv.Key] = new EstadoCompensacaoRoda + { + ModuloId = kv.Key, + RaioM = kv.Value, + Fator = fator, + Saturado = saturado, + + AnguloFrenteGraus = _angFrenteFiltrado, + AnguloTrasGraus = _angTrasFiltrado, + + IccX = iccX, + IccY = iccY + }; + } + + lock (_sync) + { + _estado = novoEstado; + } + } + + + private static GeometriaRobo ObterGeometriaAtual() + { + return new GeometriaRobo + { + L = VariaveisEquipamento.DistanciaEntreEixosCm / 100.0, + + /* + * CRIAR esta propriedade. + * Centro da roda esquerda até centro + * da roda direita. + */ + W = VariaveisEquipamento.BitolaRodasCm / 100.0 + }; + } + + + private static bool TryObterAnguloCanonico(List modulos, string modId, out double angulo) + { + angulo = 0.0; + + var modulo = modulos.FirstOrDefault(x => x.Modulo_ID == modId); + + var dir = modulo?.DirMotor; + + if (dir == null || + !dir.Inicializado || + dir.Leitura == null || + dir.Leitura.NumeroPulsos == null || + dir.Leitura.NumeroPulsos.lidoEm == DateTime.MinValue) + { + return false; + } + + DateTime agora = DateTime.Now; + + double idadeMs = (agora - dir.Leitura.NumeroPulsos.lidoEm).TotalMilliseconds; + + if (idadeMs < 0 || idadeMs > IdadeMaximaAnguloMs) + { + return false; + } + + angulo = ConverterAnguloMotorParaVeiculo(modId, dir.AnguloLeitura); + + return true; + } + + + private static double ConverterAnguloMotorParaVeiculo(string modId, double anguloMotor) + { + switch (modId) + { + case "EF": + return -anguloMotor; + + case "DF": + return anguloMotor; + + case "ET": + return anguloMotor; + + case "DT": + return -anguloMotor; default: - return false; + return 0.0; } } - // Raio de trajetória por roda para curva à esquerda (Rc > 0) ou direita (Rc < 0) - // Posições das rodas em relação ao centro do chassi: x = ±L/2, y = ±W/2 - static void CalcularRaiosPorRoda(TipoMovimentoDirecional modo, double Rc, GeometriaRobo g, out double rFL, out double rFR, out double rRL, out double rRR) + + private static double AnguloEquivalenteEixo(double esquerdo, double direito) { - // Por simplicidade, tratamos três casos: - // 1) Dianteiras: ICC está sobre a linha do eixo traseiro - // 2) Traseiras: ICC está sobre a linha do eixo dianteiro - // 3) ArcoOposto (simétrico): ICC está próximo ao centro; distância axial = L/2 para ambos - double ay = Math.Abs(Rc); // magnitude do Rc; sinal define esquerda/direita + double tanE = Math.Tan(Deg2Rad(esquerdo)); + double tanD = Math.Tan(Deg2Rad(direito)); - if (modo == TipoMovimentoDirecional.RodasDianteiras) - { - // Traseiro: raio = Rc ± W/2 - rRL = Math.Max(1e-6, ay - g.W / 2.0); - rRR = Math.Max(1e-6, ay + g.W / 2.0); - // Dianteiro: distância diagonal até ICC (pitágoras) - rFL = Math.Sqrt(Math.Pow(ay - g.W / 2.0, 2) + Math.Pow(g.L, 2)); - rFR = Math.Sqrt(Math.Pow(ay + g.W / 2.0, 2) + Math.Pow(g.L, 2)); - } - else if (modo == TipoMovimentoDirecional.RodasTraseiras) - { - // Dianteiro: raio = Rc ± W/2 - rFL = Math.Max(1e-6, ay - g.W / 2.0); - rFR = Math.Max(1e-6, ay + g.W / 2.0); - // Traseiro: distância diagonal - rRL = Math.Sqrt(Math.Pow(ay - g.W / 2.0, 2) + Math.Pow(g.L, 2)); - rRR = Math.Sqrt(Math.Pow(ay + g.W / 2.0, 2) + Math.Pow(g.L, 2)); - } - else // Arco (simétrico) - { - // Ambos eixos a L/2 do centro - double a = g.L / 2.0; - rFL = Math.Sqrt(Math.Pow(ay - g.W / 2.0, 2) + a * a); - rFR = Math.Sqrt(Math.Pow(ay + g.W / 2.0, 2) + a * a); - rRL = Math.Sqrt(Math.Pow(ay - g.W / 2.0, 2) + a * a); - rRR = Math.Sqrt(Math.Pow(ay + g.W / 2.0, 2) + a * a); - } - - // Observação: sinal de Rc indica sentido da curva: - // Rc > 0 -> curva à esquerda (rodas esquerdas são internas) - // Rc < 0 -> curva à direita (troque "interna"↔"externa") - // Aqui usamos |Rc|; para decidir quem é interna/externa, use o sinal fora daqui. + return Rad2Deg(Math.Atan((tanE + tanD) / 2.0)); } - // Retorna fatores por roda. Modo "SomenteAumentarExterna" mantém a interna em 1.0 - public enum PoliticaFator { ReferenciaNoCentro, SomenteAumentarExterna } - public static Dictionary FatoresVelocidade(TipoMovimentoDirecional modo, double deltaFrontDeg, double deltaRearDeg, GeometriaRobo g, PoliticaFator politica, double fatorMaximo = 1.5) + private static bool TryCalcularICC(double deltaFrenteGraus, double deltaTrasGraus, GeometriaRobo g, out double iccX, out double iccY) { - if (!TryCalcularRaioCentro(modo, deltaFrontDeg, deltaRearDeg, g, out double Rc)) - return Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.ToDictionary(x => x.Modulo_ID, x => 1.0); + iccX = 0; + iccY = 0; - CalcularRaiosPorRoda(modo, Rc, g, out var rFL, out var rFR, out var rRL, out var rRR); + double df = Deg2Rad(deltaFrenteGraus); + double dr = Deg2Rad(deltaTrasGraus); - // Centro do robô percorre raio |Rc| - double RcAbs = Math.Abs(Rc); + /* + * Vetor normal à orientação de cada eixo. + */ + double nfx = -Math.Sin(df); + double nfy = Math.Cos(df); + double nrx = -Math.Sin(dr); + double nry = Math.Cos(dr); - // Descobrir lado interno pela curva (sinal de Rc) - bool esquerda = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir).UltimaDirecao == Direcao.Esquerda; // Rc > 0; + /* + * cross(nf, nr) + */ + double denominador = nfx * nry - nfy * nrx; - double rIntFront = esquerda ? rFL : rFR; - double rExtFront = esquerda ? rFR : rFL; - double rIntRear = esquerda ? rRL : rRR; - double rExtRear = esquerda ? rRR : rRL; - - double fFL, fFR, fRL, fRR; - - if (politica == PoliticaFator.ReferenciaNoCentro) + /* + * Normais quase paralelas: + * raio tende ao infinito. + */ + if (Math.Abs(denominador) < 1e-6) { - fFL = rIntFront / RcAbs; - fFR = rExtFront / RcAbs; - fRL = rIntRear / RcAbs; - fRR = rExtRear / RcAbs; - } - else // SomenteAumentarExterna - { - fFL = 1.0; - fFR = rExtFront / rIntFront; - fRL = 1.0; - fRR = rExtRear / rIntRear; + return false; } - // Saturação e leve suavização (ex: EMA) podem ser aplicadas aqui - fFL = Math.Min(fatorMaximo, fFL); - fFR = Math.Min(fatorMaximo, fFR); - fRL = Math.Min(fatorMaximo, fRL); - fRR = Math.Min(fatorMaximo, fRR); + /* + * Centro do eixo dianteiro: + * F = (+L/2, 0) + * + * Centro do eixo traseiro: + * R = (-L/2, 0) + * + * vetor R-F = (-L,0) + */ + double vx = -g.L; - return Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.ToDictionary(x => x.Modulo_ID, x => (double)(x.Modulo_ID == "ET" ? fRL : x.Modulo_ID == "DT" ? fRR : x.Modulo_ID == "EF" ? fFL : x.Modulo_ID == "DF" ? fFR : 1.0)); + double vy = 0.0; + + /* + * t = + * cross(R-F, nr) / + * cross(nf, nr) + */ + double t = (vx * nry - vy * nrx) / denominador; + + iccX = g.L / 2.0 + t * nfx; + + iccY = t * nfy; + + return + !double.IsNaN(iccX) && + !double.IsNaN(iccY) && + !double.IsInfinity(iccX) && + !double.IsInfinity(iccY); } + + private static Dictionary CalcularRaios(GeometriaRobo g, double iccX, double iccY) + { + double xF = g.L / 2.0; + double xT = -g.L / 2.0; + double yE = g.W / 2.0; + double yD = -g.W / 2.0; + + return new Dictionary + { + { + "EF", + Distancia(xF, yE, iccX, iccY) + }, + { + "DF", + Distancia(xF, yD, iccX, iccY) + }, + { + "ET", + Distancia(xT, yE, iccX, iccY) + }, + { + "DT", + Distancia(xT, yD, iccX, iccY) + } + }; + } + + + public static double ObterFator(string modId) + { + lock (_sync) + { + EstadoCompensacaoRoda e; + + if (_estado.TryGetValue(modId, out e)) + { + return Math.Max(1.0, e.Fator); + } + } + + return 1.0; + } + + + public static int CalcularRpmMotor(string modId, int rpmBase, int rpmMin, int rpmMax) + { + if (rpmBase <= 0) + return 0; + + double fatorRoda = ObterFator(modId); + + double maiorFator; + + lock (_sync) + { + maiorFator = + _estado.Count == 0 + ? 1.0 + : _estado.Values.Max(x => x.Fator); + } + + /* + * Primeiro limita o RPM base válido. + */ + double baseSolicitada = Math.Max(rpmMin, Math.Min(rpmBase, rpmMax)); + + /* + * Se a roda externa estouraria RPM máximo, + * reduzimos TODAS proporcionalmente. + * + * Assim preservamos a geometria. + */ + double baseEfetiva = baseSolicitada; + + if (maiorFator > 1.0) + { + double baseMaximaPermitida = rpmMax / maiorFator; + + baseEfetiva = Math.Min(baseEfetiva, baseMaximaPermitida); + } + + /* + * Normalmente respeitamos rpmMin. + * + * Porém, se a própria geometria tornar impossível manter + * simultaneamente rpmMin e rpmMax, priorizamos a proporção + * cinemática e o limite máximo. + */ + bool geometriaIncompativelComRpmMin = maiorFator > 1.0 && (rpmMax / maiorFator) < rpmMin; + + if (!geometriaIncompativelComRpmMin) + { + baseEfetiva = Math.Max(baseEfetiva, rpmMin); + } + + double rpm = baseEfetiva * fatorRoda; + + return Convert.ToInt32(Math.Max(0, Math.Min(rpm, rpmMax))); + } + + + public static EstadoCompensacaoRoda ObterEstado(string modId) + { + lock (_sync) + { + EstadoCompensacaoRoda e; + + if (!_estado.TryGetValue(modId, out e)) + { + return new EstadoCompensacaoRoda + { + ModuloId = modId, + Fator = 1.0 + }; + } + + return e; + } + } + + + private static void ResetarTudo(List modulos = null) + { + ResetarCompensacao(modulos); + + _angFrenteFiltrado = 0.0; + _angTrasFiltrado = 0.0; + _ultimoCalculo = DateTime.MinValue; + _curvaAtiva = false; + } + + + private static void ResetarCompensacao(List modulos) + { + lock (_sync) + { + _estado = new Dictionary(); + + if (modulos != null) + { + foreach (var m in modulos) + { + if (m == null || string.IsNullOrWhiteSpace(m.Modulo_ID)) + { + continue; + } + + _estado[m.Modulo_ID] = new EstadoCompensacaoRoda + { + ModuloId = m.Modulo_ID, + Fator = 1.0 + }; + } + } + } + } + + + private static double Distancia(double x1, double y1, double x2, double y2) + { + double dx = x1 - x2; + double dy = y1 - y2; + + return Math.Sqrt(dx * dx + dy * dy); + } + + + private static double Deg2Rad(double graus) + { + return graus * Math.PI / 180.0; + } + + + private static double Rad2Deg(double rad) + { + return rad * 180.0 / Math.PI; + } } } diff --git a/AgroBase/AgroBase/Models/Modules/MovimentacaoUnificadoModel.cs b/AgroBase/AgroBase/Models/Modules/MovimentacaoUnificadoModel.cs index 2403bd416..e601e4e48 100644 --- a/AgroBase/AgroBase/Models/Modules/MovimentacaoUnificadoModel.cs +++ b/AgroBase/AgroBase/Models/Modules/MovimentacaoUnificadoModel.cs @@ -4,6 +4,7 @@ using System; using System.Collections.Generic; using System.Data; using System.Linq; +using System.Threading; using System.Threading.Tasks; using static AgroBase.Models.Enums; @@ -46,6 +47,10 @@ namespace AgroBase.Models.Modules [JsonIgnore] public AsyncTaskTimerModel tmrRegistradorDIR; + public bool MovimentoDirecionalCombinadoEmExecucao { get; private set; } + private readonly SemaphoreSlim _gateMovimentoCombinado = new SemaphoreSlim(1, 1); + private CancellationTokenSource _ctsMovimentoCombinado; + public List Modulos { get; set; } = new List(); @@ -150,6 +155,10 @@ namespace AgroBase.Models.Modules x.MovMotor.Temperatura = 0; x.MovMotor.RPM_SP = 0; x.MovMotor.RPM_SP_Controle = 0; + x.MovMotor.UltimoRPMComandado = 0; + x.MovMotor.UltimoComandoEnviado = DateTime.MinValue; + x.MovMotor.UltimoSetPointEnviado = 0; + x.MovMotor.UltimaDirecao = Direcao.Parado; x.DirMotor.Angulo_SP = 0; x.DirMotor.Sentido_SP = Sentido.Parado; @@ -163,6 +172,8 @@ namespace AgroBase.Models.Modules public void RegistrarLogMov(ModuloMvdModel Modulo) { + var comp = MovimentacaoMovimentoCompensadoModel.ObterEstado(Modulo.Modulo_ID); + Modulo.MovMotor.LogGrafico.Add(new MvdMotorMOVSensorimanetoGrafico() { Momento = DateTime.Now, @@ -177,7 +188,7 @@ namespace AgroBase.Models.Modules Tensao = Modulo.MovMotor.Tensao, Velocidade = Modulo.MovMotor.VelocidadeInstantanea, Potencia = Modulo.MovMotor.Potencia, - CicloTrabalho = Modulo.MovMotor.CicloTrabalho + CicloTrabalho = Modulo.MovMotor.CicloTrabalho, }); if (Modulo.MovMotor.LogGrafico.Count > 60) @@ -332,8 +343,11 @@ namespace AgroBase.Models.Modules //} private DateTime _ultimoEnvioControleMov = DateTime.MinValue; private DateTime _ultimoRegistroLogMov = DateTime.MinValue; - private static readonly TimeSpan IntervaloControleMov = TimeSpan.FromSeconds(1); + private static readonly TimeSpan IntervaloControleMov = TimeSpan.FromMilliseconds(100); private static readonly TimeSpan IntervaloLogMov = TimeSpan.FromSeconds(1); + private DateTime _ultimaAtualizacaoCompensacaoMov = DateTime.MinValue; + private static readonly TimeSpan IntervaloCompensacaoMov = TimeSpan.FromMilliseconds(50); + private async Task tmrRegistradorMOV_Tick() { var op = Variaveis.OperacaoEmAndamento; @@ -343,40 +357,27 @@ namespace AgroBase.Models.Modules DateTime agora = DateTime.Now; - if (!OIDCanService.Iniciado && SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Mov)) - { - foreach (var leitura in OIDCanService.DadosLeitura.Where(x => x.UltimoComandoRecebido.AddSeconds(10) < agora)) - { - OIDCanService.RequisitarDado( - leitura.EnderecoTx, - leitura.EnderecoRx, - OIDCanService.OidHandler.OidParametros.CodErro - ); - } - - tmrRegistradorMOV.SetInterval(3000); - return; - } - - bool deveEnviarControle = - _ultimoEnvioControleMov == DateTime.MinValue || - agora - _ultimoEnvioControleMov >= IntervaloControleMov; - - bool deveRegistrarLog = - _ultimoRegistroLogMov == DateTime.MinValue || - agora - _ultimoRegistroLogMov >= IntervaloLogMov; - - bool enviouControle = false; - bool registrouLog = false; - - var controleMov = deveEnviarControle ? op.Controle.TiposControle?.FirstOrDefault(x => x.Tipo == T_Code.Mov) : null; - /* * Uma única tentativa global por tick. * O OIDCanService escolhe o driver e o parâmetro mais adequado. */ OIDCanService.AtualizarDados(); + bool deveEnviarControle = _ultimoEnvioControleMov == DateTime.MinValue || agora - _ultimoEnvioControleMov >= IntervaloControleMov; + bool deveRegistrarLog = _ultimoRegistroLogMov == DateTime.MinValue || agora - _ultimoRegistroLogMov >= IntervaloLogMov; + bool deveAtualizarCompensacao = _ultimaAtualizacaoCompensacaoMov == DateTime.MinValue || agora - _ultimaAtualizacaoCompensacaoMov >= IntervaloCompensacaoMov; + + if (deveAtualizarCompensacao) + { + MovimentacaoMovimentoCompensadoModel.AtualizarDados(); + _ultimaAtualizacaoCompensacaoMov = agora; + } + + bool enviouControle = false; + bool registrouLog = false; + + var controleMov = deveEnviarControle ? op.Controle.TiposControle?.FirstOrDefault(x => x.Tipo == T_Code.Mov) : null; + foreach (var modulo in Modulos) { if (!modulo.MovMotor.Inicializado) @@ -387,7 +388,7 @@ namespace AgroBase.Models.Modules * O polling agora é centralizado em AtualizarDados(). */ - if (deveEnviarControle && controleMov != null) + if (!MovimentoDirecionalCombinadoEmExecucao && deveEnviarControle && controleMov != null) { modulo.MovMotor.EnviarComandoControle(controleMov.UltimaDirecao); @@ -477,7 +478,7 @@ namespace AgroBase.Models.Modules * Durante referenciamento não interferimos no comando, * pois o próprio fluxo de referência assume o controle do MKS. */ - if (!MKS057DCanService.Referenciando && deveEnviarControle && controleDir != null && !driver.AtingiuAnguloSP) + if (!MovimentoDirecionalCombinadoEmExecucao && !MKS057DCanService.Referenciando && deveEnviarControle && controleDir != null && !driver.AtingiuAnguloSP) { driver.EnviarComandoControle(controleDir.UltimaDirecao); enviouControle = true; @@ -532,51 +533,100 @@ namespace AgroBase.Models.Modules public async Task ExecutarMovimentoDirecionalCombinado(Direcao DirecaoAtual) { - var op = Variaveis.OperacaoEmAndamento; + _ctsMovimentoCombinado?.Cancel(); - if (DirecaoAtual == Direcao.Parado) + var cts = new CancellationTokenSource(); + _ctsMovimentoCombinado = cts; + + await _gateMovimentoCombinado.WaitAsync(); + + try { - foreach (var Modulo in Modulos) + if (cts.IsCancellationRequested) + return; + + MovimentoDirecionalCombinadoEmExecucao = true; + + var op = Variaveis.OperacaoEmAndamento; + + var ctrlMov = op.Controle.TiposControle.First(x => x.Tipo == T_Code.Mov); + var ctrlDir = op.Controle.TiposControle.First(x => x.Tipo == T_Code.Dir); + + ctrlDir.DirecaoAtual = DirecaoAtual; + ctrlDir.UltimaDirecao = DirecaoAtual; + + // Sempre parar a tração antes de alterar a geometria das rodas + ctrlMov.DirecaoAtual = Direcao.Parado; + ctrlMov.UltimaDirecao = Direcao.Parado; + + foreach (var modulo in Modulos) { - Modulo.MovMotor.EnviarComandoControle(Direcao.Parado); + modulo.MovMotor.EnviarComandoControle(Direcao.Parado); } - await Task.Delay(1000); - } + if (DirecaoAtual == Direcao.Parado) + { + await Task.Delay(1000, cts.Token); + } - switch (op.Controle.TipoMovimento) - { - case TipoMovimentoDirecional.RotacionarNoEixo: - case TipoMovimentoDirecional.MovimentoLateral: + switch (op.Controle.TipoMovimento) + { + case TipoMovimentoDirecional.RotacionarNoEixo: + case TipoMovimentoDirecional.MovimentoLateral: - foreach (var Modulo in Modulos.Where(x => x.DirMotor.Inicializado && !x.DirMotor.PosicaoTipoMovimentoConcluido(DirecaoAtual))) - { - Modulo.DirMotor.EnviarComandoControle(DirecaoAtual); - } - - DateTime Agora = DateTime.Now; - bool Sucesso = Modulos.All(x => x.DirMotor.PosicaoTipoMovimentoConcluido(DirecaoAtual)); - while (Agora.AddMilliseconds(5000) > DateTime.Now && !Sucesso) - { - await Task.Delay(100); - Sucesso = Modulos.All(x => x.DirMotor.PosicaoTipoMovimentoConcluido(DirecaoAtual)); - } - - if (Sucesso && DirecaoAtual != Direcao.Parado) - { - op.Controle.PercentualVelocidadeSP = 20; - foreach (var Modulo in Modulos) + foreach (var Modulo in Modulos.Where(x => x.DirMotor.Inicializado && !x.DirMotor.PosicaoTipoMovimentoConcluido(DirecaoAtual))) { - Modulo.MovMotor.EnviarComandoControle( - DirecaoAtual == Direcao.Direita ? Direcao.Frente : - DirecaoAtual == Direcao.Esquerda ? Direcao.Tras : - Direcao.Parado - ); + Modulo.DirMotor.EnviarComandoControle(DirecaoAtual); } - } - break; + DateTime Agora = DateTime.Now; + bool Sucesso = Modulos.All(x => x.DirMotor.PosicaoTipoMovimentoConcluido(DirecaoAtual)); + while (Agora.AddMilliseconds(5000) > DateTime.Now && !Sucesso) + { + await Task.Delay(100, cts.Token); + Sucesso = Modulos.All(x => x.DirMotor.PosicaoTipoMovimentoConcluido(DirecaoAtual)); + } + if (Sucesso && DirecaoAtual != Direcao.Parado) + { + op.Controle.PercentualVelocidadeSP = 20; + + Direcao direcaoMov = DirecaoAtual == Direcao.Direita ? Direcao.Frente : DirecaoAtual == Direcao.Esquerda ? Direcao.Tras : Direcao.Parado; + ctrlMov.DirecaoAtual = direcaoMov; + ctrlMov.UltimaDirecao = direcaoMov; + + foreach (var Modulo in Modulos) + { + Modulo.MovMotor.EnviarComandoControle( + DirecaoAtual == Direcao.Direita ? Direcao.Frente : + DirecaoAtual == Direcao.Esquerda ? Direcao.Tras : + Direcao.Parado + ); + } + } + + if (!Sucesso) + { + ctrlMov.DirecaoAtual = Direcao.Parado; + ctrlMov.UltimaDirecao = Direcao.Parado; + + Variaveis.MostrarLog( + $"[MVD] Movimento combinado não atingiu posição | " + + $"Tipo={op.Controle.TipoMovimento} | " + + $"Direcao={DirecaoAtual}" + ); + + return; + } + + break; + + } + } + finally + { + MovimentoDirecionalCombinadoEmExecucao = false; + _gateMovimentoCombinado.Release(); } } diff --git a/AgroBase/AgroBase/Models/Modules/SensoriamentoModel.cs b/AgroBase/AgroBase/Models/Modules/SensoriamentoModel.cs index 2731aa3c0..b2beb47b9 100644 --- a/AgroBase/AgroBase/Models/Modules/SensoriamentoModel.cs +++ b/AgroBase/AgroBase/Models/Modules/SensoriamentoModel.cs @@ -2115,6 +2115,8 @@ namespace AgroBase.Models.Modules Mod_ID = Modulo_ID, }); + if (op.Sensoriamento?.Operacao?.ModsCalibragem == null) + op.Sensoriamento.Operacao.ModsCalibragem = new Dictionary>(); op.Sensoriamento.Operacao.ModsCalibragem[Dispositivo] = new Dictionary(); op.Sensoriamento.InserirLog(Dispositivo, StatusModulo.Conectado, 0, "Dispositivo conectado"); } @@ -3094,7 +3096,7 @@ namespace AgroBase.Models.Modules double temperatura = ConverterTemperaturaNtc(data, 4); double tensao = ConverterTensaoNtc(data, 6); AtualizarDados(ID_Num, posicao, componente, new List() { raw, temperatura, tensao }); - Variaveis.MostrarLog($"[SEN] sNTC | ID_Num: {ID_Num} | Posicao: {posicao} | Leitura: {raw} | Temperatura: {temperatura} | Tensao: {tensao}"); + //Variaveis.MostrarLog($"[SEN] sNTC | ID_Num: {ID_Num} | Posicao: {posicao} | Leitura: {raw} | Temperatura: {temperatura} | Tensao: {tensao}"); break; } } diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs index fac9045db..98e6e2d9d 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs @@ -2045,11 +2045,10 @@ namespace AgroBase.Models { case T_Code.Mov: { - var modsAtualizar = op.DispMvd.Dados.Modulos.Where(Mod => Mod.MovMotor.NovoRPMNecessario).Select(x => x.Modulo_ID).ToList(); bool envioNecessario = ForcarEnvio || _Controle.RPM_SP != _ControleAnterior.RPM_SP || _Controle.EmFreio != _ControleAnterior.EmFreio; - if (envioNecessario || modsAtualizar.Any()) + if (envioNecessario) { - foreach (var Mod in op.DispMvd.Dados.Modulos.Where(x => envioNecessario ? true : modsAtualizar.Contains(x.Modulo_ID))) + foreach (var Mod in op.DispMvd.Dados.Modulos) { Mod.MovMotor.EnviarComandoControle(DirecaoAtual); } @@ -4740,6 +4739,8 @@ namespace AgroBase.Models string moduloId = modulo?.Modulo_ID ?? mov?.Mod_ID ?? ""; + var comp = MovimentacaoMovimentoCompensadoModel.ObterEstado(moduloId); + return new MovimentacaoModuloLogModel { Modulo_ID = moduloId, @@ -4790,7 +4791,12 @@ namespace AgroBase.Models UltimoSetPointEnviado = mov?.UltimoSetPointEnviado ?? 0, UltimaDirecao = mov?.UltimaDirecao ?? Direcao.Parado, - FatorCompensacaoCurva = BuscarFatorCompensacao(moduloId) + RPM_SP_Base = mov.RPM_SP_Controle, + FatorCompensacao = comp.Fator, + CompensacaoSaturada = comp.Saturado, + IccX = comp.IccX, + IccY = comp.IccY, + RaioTrajetoriaM = comp.RaioM, }; } @@ -4827,19 +4833,6 @@ namespace AgroBase.Models return valores.Count == 0 ? 0 : valores.Average(); } - double BuscarFatorCompensacao(string moduloId) - { - if (string.IsNullOrWhiteSpace(moduloId)) - return 1.0; - - if (MovimentacaoMovimentoCompensadoModel.compensacoes != null && MovimentacaoMovimentoCompensadoModel.compensacoes.TryGetValue(moduloId, out var fator)) - { - return fator; - } - - return 1.0; - } - var dados = op.DispMvd?.Dados; var modulos = dados?.Modulos? @@ -5245,7 +5238,6 @@ namespace AgroBase.Models CameraWorkerService.AtualizarDadosCameras(); GPSService.AtualizaDadosRedis(); LivoxManagerProcess.AtualizarLeitura(); - MovimentacaoMovimentoCompensadoModel.AtualizarDados(); DalyBMSService.AtualizarDados(); SerialService.DispositivosMapeados.ForEach(d => d.AtualizarDados()); @@ -5974,7 +5966,12 @@ namespace AgroBase.Models public Direcao UltimaDirecao { get; set; } // Compensação em curva - public double FatorCompensacaoCurva { get; set; } = 1.0; + public int RPM_SP_Base { get; set; } + public double FatorCompensacao { get; set; } + public double RaioTrajetoriaM { get; set; } + public double IccX { get; set; } + public double IccY { get; set; } + public bool CompensacaoSaturada { get; set; } public MovimentacaoModuloLogModel Clone() { diff --git a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs index 12d42d786..c8d4bb0f4 100644 --- a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs +++ b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs @@ -2437,6 +2437,9 @@ namespace AgroBase.Models public void ProjetarTrajetoriaFixa(GPSModel posicaoRobo = null) { + if (Variaveis.OperacaoEmAndamento.Sensoriamento?.Operacao?.Modo != ModoOperacao.MapaGPS) + return; + List _trajetoriaFixa = new List(); List> corredores; List CorredoresLarguras; diff --git a/AgroBase/AgroBase/Models/Variaveis.cs b/AgroBase/AgroBase/Models/Variaveis.cs index 92f7ac3e1..6bf4f4ca0 100644 --- a/AgroBase/AgroBase/Models/Variaveis.cs +++ b/AgroBase/AgroBase/Models/Variaveis.cs @@ -826,6 +826,7 @@ namespace AgroBase.Models public static double ComprimentoFrente { get; } = 90.0; // 7 public static double ComprimentoTras { get; } = 40.0; // 107 public static double DistanciaEntreEixosCm { get; set; } = 92.0; + public static double BitolaRodasCm { get; set; } = 75.0; public static double LeverArmFrontalCm { get; set; } = 37.0; // cm public static double LeverArmLateralCm { get; set; } = 0.0; // cm public static double LarguraEquipamentoMm diff --git a/AgroBase/AgroBase/Services/CANService.cs b/AgroBase/AgroBase/Services/CANService.cs index 9480b1a9b..718a3a596 100644 --- a/AgroBase/AgroBase/Services/CANService.cs +++ b/AgroBase/AgroBase/Services/CANService.cs @@ -322,7 +322,7 @@ namespace AgroBase.Services bool PortaIsCan(SerialPort Porta = null); void RegistrarHandler(uint endereco_rx, ICanMessageHandler handler); bool Inicializar(int BitRate = 250); - void AdicionarMensagemNaFila(T_Code Dispositivo, byte[] idTx, byte[] idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false); + bool AdicionarMensagemNaFila(T_Code Dispositivo, byte[] idTx, byte[] idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false); void Fechar(); bool PossuiMensagemPendente(T_Code dispositivo); @@ -373,6 +373,10 @@ namespace AgroBase.Services int PendentesNaoEnviadas { get; } int PendentesAguardandoResposta { get; } int FramesRxNaFila { get; } + + double IdadeMaxFilaNaoEnviadaMs { get; } + int PendentesNaoEnviadasAtrasadas { get; } + double IdadeMediaFilaNaoEnviadaMs { get; } } } \ No newline at end of file diff --git a/AgroBase/AgroBase/Services/CANServiceSerial.cs b/AgroBase/AgroBase/Services/CANServiceSerial.cs index 81bc9d4c4..9ec62364c 100644 --- a/AgroBase/AgroBase/Services/CANServiceSerial.cs +++ b/AgroBase/AgroBase/Services/CANServiceSerial.cs @@ -158,6 +158,54 @@ namespace AgroBase.Services public int FramesRxNaFila => FramesRecebidos?.Count ?? 0; + public double IdadeMaxFilaNaoEnviadaMs + { + get + { + lock (_LockMensagens) + { + var pendentes = MensagensPendentes.Where(x => !x.Enviado).ToList(); + + if (!pendentes.Any()) + return 0; + + DateTime agora = DateTime.Now; + + return pendentes.Max(x => (agora - x.Momento).TotalMilliseconds); + } + } + } + public int PendentesNaoEnviadasAtrasadas + { + get + { + lock (_LockMensagens) + { + DateTime limite = DateTime.Now.AddMilliseconds(-500); + + return MensagensPendentes.Count(x => !x.Enviado && x.Momento < limite); + } + } + } + public double IdadeMediaFilaNaoEnviadaMs + { + get + { + lock (_LockMensagens) + { + var pendentes = MensagensPendentes.Where(x => !x.Enviado).ToList(); + + if (!pendentes.Any()) + return 0; + + DateTime agora = DateTime.Now; + + return pendentes.Average(x => (agora - x.Momento).TotalMilliseconds); + } + } + } + + private bool DebugMode = false; private void MostrarLog(string message, uint id = 0) { @@ -607,9 +655,9 @@ namespace AgroBase.Services } } - public void AdicionarMensagemNaFila(T_Code Dispositivo, byte[] _idTx, byte[] _idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false) + public bool AdicionarMensagemNaFila(T_Code Dispositivo, byte[] _idTx, byte[] _idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false) { - if (_PortaCAN?.IsOpen != true) return; + if (_PortaCAN?.IsOpen != true) return false; uint idTx = CanManager.ConverterIdCan(_idTx); uint idRx = CanManager.ConverterIdCan(_idRx); @@ -653,12 +701,16 @@ namespace AgroBase.Services //{ // Console.WriteLine($"{DateTime.Now.ToString("dd/MM/yyyy HH:mm:ss.fff")} - MENSAGEM LORA ADICIONADO NA FILA CAN, FUNC_CODE: {((CanMessagePosicaoDados)dados[0]).ToString()} ({((DadosLoRaParse)dados[0]).ToString()}) " + string.Join(" ", dados)); //} + + return true; } else { MostrarLog($"Mensagem já pendente e idêntica. ID: {novaMensagem.IdTx}, Data: {FuncoesGlobais.ConverterComandoBytesParaTexto(novaMensagem.DataTx)}", idTx); } } + + return false; } private CanMessage ObterProximaMensagem() diff --git a/AgroBase/AgroBase/Services/CANServiceWaveshare.cs b/AgroBase/AgroBase/Services/CANServiceWaveshare.cs index 8722a0f88..accf61734 100644 --- a/AgroBase/AgroBase/Services/CANServiceWaveshare.cs +++ b/AgroBase/AgroBase/Services/CANServiceWaveshare.cs @@ -120,6 +120,54 @@ namespace AgroBase.Services private long _rxTotalUltimaJanela = 0; private DateTime _ultimaJanelaFreq = DateTime.UtcNow; + public double IdadeMaxFilaNaoEnviadaMs + { + get + { + lock (_LockMensagens) + { + var pendentes = MensagensPendentes.Where(x => !x.Enviado).ToList(); + + if (!pendentes.Any()) + return 0; + + DateTime agora = DateTime.Now; + + return pendentes.Max(x => (agora - x.Momento).TotalMilliseconds); + } + } + } + public int PendentesNaoEnviadasAtrasadas + { + get + { + lock (_LockMensagens) + { + DateTime limite = DateTime.Now.AddMilliseconds(-500); + + return MensagensPendentes.Count(x => !x.Enviado && x.Momento < limite); + } + } + } + public double IdadeMediaFilaNaoEnviadaMs + { + get + { + lock (_LockMensagens) + { + var pendentes = MensagensPendentes.Where(x => !x.Enviado).ToList(); + + if (!pendentes.Any()) + return 0; + + DateTime agora = DateTime.Now; + + return pendentes.Average(x => (agora - x.Momento).TotalMilliseconds); + } + } + } + + private static long NowUnixMs() { return DateTimeOffset.UtcNow.ToUnixTimeMilliseconds(); @@ -418,9 +466,9 @@ namespace AgroBase.Services } - public void AdicionarMensagemNaFila(T_Code Dispositivo, byte[] _idTx, byte[] _idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false) + public bool AdicionarMensagemNaFila(T_Code Dispositivo, byte[] _idTx, byte[] _idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false) { - if (!Connector.IsOpen) return; + if (!Connector.IsOpen) return false; uint idTx = CanManager.ConverterIdCan(_idTx); uint idRx = CanManager.ConverterIdCan(_idRx); @@ -467,12 +515,14 @@ namespace AgroBase.Services //{ // Console.WriteLine($"{DateTime.Now.ToString("dd/MM/yyyy HH:mm:ss.fff")} - MENSAGEM LORA ADICIONADO NA FILA CAN, FUNC_CODE: {((CanMessagePosicaoDados)payload[1]).ToString()} ({((DadosLoRaParse)payload[1]).ToString()})" + string.Join(" ", payload)); //} + return true; } else { MostrarLog($"Mensagem já pendente e idêntica. ID: {novaMensagem.IdTx}, Data: {FuncoesGlobais.ConverterComandoBytesParaTexto(novaMensagem.DataTx)}", idTx); } } + return false; } private CanMessage ObterProximaMensagem() diff --git a/AgroBase/AgroBase/Services/CanServiceEthernet.cs b/AgroBase/AgroBase/Services/CanServiceEthernet.cs index f1506d973..146e5175f 100644 --- a/AgroBase/AgroBase/Services/CanServiceEthernet.cs +++ b/AgroBase/AgroBase/Services/CanServiceEthernet.cs @@ -65,9 +65,7 @@ namespace AgroBase.Services { lock (_LockMensagens) { - return MensagensPendentes == null || - MensagensPendentes.Count == 0 || - !MensagensPendentes.Any(x => !x.Enviado && x.Momento >= DateTime.Now.AddSeconds(-10)); + return !MensagensPendentes.Any(x => !x.Enviado); } } } @@ -196,6 +194,53 @@ namespace AgroBase.Services private const int USR_FRAME_SIZE = 13; + public double IdadeMaxFilaNaoEnviadaMs + { + get + { + lock (_LockMensagens) + { + var pendentes = MensagensPendentes.Where(x => !x.Enviado).ToList(); + + if (!pendentes.Any()) + return 0; + + DateTime agora = DateTime.Now; + + return pendentes.Max(x => (agora - x.Momento).TotalMilliseconds); + } + } + } + public int PendentesNaoEnviadasAtrasadas + { + get + { + lock (_LockMensagens) + { + DateTime limite = DateTime.Now.AddMilliseconds(-500); + + return MensagensPendentes.Count(x => !x.Enviado && x.Momento < limite); + } + } + } + public double IdadeMediaFilaNaoEnviadaMs + { + get + { + lock (_LockMensagens) + { + var pendentes = MensagensPendentes.Where(x => !x.Enviado).ToList(); + + if (!pendentes.Any()) + return 0; + + DateTime agora = DateTime.Now; + + return pendentes.Average(x => (agora - x.Momento).TotalMilliseconds); + } + } + } + private void MostrarLog(string message, uint id = 0) { if (DebugMode) @@ -337,9 +382,10 @@ namespace AgroBase.Services } } - public void AdicionarMensagemNaFila(T_Code Dispositivo, byte[] _idTx, byte[] _idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false) + public bool AdicionarMensagemNaFila(T_Code Dispositivo, byte[] _idTx, byte[] _idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false) { - if (!IsConnected) return; + if (!IsConnected) + return false; uint idTx = CanManager.ConverterIdCan(_idTx); uint idRx = CanManager.ConverterIdCan(_idRx); @@ -355,21 +401,19 @@ namespace AgroBase.Services if (Dispositivo == T_Code.Mks) { byte crc = CanManager.CalcularChecksum(idTx, dados.ToArray()); + dados.Add(crc); } if (dados.Count > 8) - throw new InvalidOperationException($"Payload CAN maior que 8 bytes. ID=0x{idTx:X}, Len={dados.Count}"); - - string chaveCorrelacao = - CanManager.GerarChaveCorrelacao( - Dispositivo, - idTx, - idRx, - funcCodeTx, - funcCodeRx, - dados.ToArray() + { + throw new InvalidOperationException( + $"Payload CAN maior que 8 bytes. " + + $"ID=0x{idTx:X}, Len={dados.Count}" ); + } + + string chaveCorrelacao = CanManager.GerarChaveCorrelacao(Dispositivo, idTx, idRx, funcCodeTx, funcCodeRx, dados.ToArray()); var novaMensagem = new CanMessage { @@ -390,13 +434,6 @@ namespace AgroBase.Services { DateTime agora = DateTime.Now; - /* - * O OID não deve possuir mais de uma consulta aguardando - * resposta para o mesmo driver. - * - * Diferente do dedupe por ChaveInterna, esta proteção vale - * mesmo quando o parâmetro consultado é diferente. - */ if (get && Dispositivo == T_Code.Oid) { bool consultaOidAberta = @@ -407,14 +444,9 @@ namespace AgroBase.Services x.IdTx == idTx); if (consultaOidAberta) - return; + return false; } - /* - * Para request com resposta, não deixa duas pendências iguais - * ficarem esperando a mesma resposta. Isso é vital para OID, porque - * todas as consultas respondem com funcCode 0x0F e diferem pelo param. - */ bool mesmaPendenciaAberta = MensagensPendentes.Any(x => x.ComResposta && @@ -422,42 +454,37 @@ namespace AgroBase.Services x.ChaveInterna == chaveCorrelacao); if (get && mesmaPendenciaAberta) - return; + { + return false; + } - /* - * Para comando sem resposta, evita empilhar comandos idênticos - * ainda não enviados. Depois de enviado, ele será limpo normalmente. - */ bool comandoIdenticoNaoEnviado = MensagensPendentes.Any(x => !x.ComResposta && !x.Enviado && x.IdTx == idTx && x.DataTx != null && - x.DataTx.SequenceEqual(dados.ToArray())); + x.DataTx.SequenceEqual( + dados.ToArray() + )); if (!get && comandoIdenticoNaoEnviado) - return; + { + return false; + } - /* - * Dedupe temporal extra, útil para comandos de controle que chegam - * repetidos por loop rápido. - */ TimeSpan janela = get ? DEDUPE_JANELA_GET : DEDUPE_JANELA_SET; if (_ultimaFilaPorChave.TryGetValue(chaveCorrelacao, out DateTime ultima) && agora - ultima < janela) { - return; + return false; } _ultimaFilaPorChave[chaveCorrelacao] = agora; MensagensPendentes.Add(novaMensagem); - MostrarLog( - $"Mensagem adicionada. ID=0x{idTx:X}, Data={BitConverter.ToString(novaMensagem.DataTx)}", - idTx - ); + return true; } } diff --git a/AgroBase/AgroBase/Services/MKS057DCanService.cs b/AgroBase/AgroBase/Services/MKS057DCanService.cs index 16c9d471c..985c02b8b 100644 --- a/AgroBase/AgroBase/Services/MKS057DCanService.cs +++ b/AgroBase/AgroBase/Services/MKS057DCanService.cs @@ -696,6 +696,9 @@ namespace AgroBase.Services { try { + if (!CanManager.CanService.IsConnected) + return; + var modulos = Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos; if (modulos == null) return; @@ -862,7 +865,8 @@ namespace AgroBase.Services double angulo, int velocidade, Sentido sentido = Sentido.Horario, - ModosControle modo = ModosControle.AnguloAbsoluto) + ModosControle modo = ModosControle.AnguloAbsoluto, + int aceleracao = 0) { byte funcCode; double reducao = Variaveis.OperacaoEmAndamento?.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.DirMotor._EnderecoCAN_Tx == idTx)?.DirMotor?.Reducao ?? VariaveisEquipamento.ReducaoDirecional; @@ -873,6 +877,7 @@ namespace AgroBase.Services sentido, modo, reducao, + aceleracao, out funcCode ); @@ -956,6 +961,7 @@ namespace AgroBase.Services Sentido sentido, ModosControle modo, double reducao, + int aceleracao, out byte funcCode) { List payload = new List(); @@ -963,7 +969,7 @@ namespace AgroBase.Services ushort vel = (ushort)Math.Max(0, Math.Min(velocidade, ushort.MaxValue)); byte[] velocidadeBytes = BitConverter.GetBytes(vel); - byte acc = 0; + byte acc = (byte)aceleracao; if (modo == ModosControle.Pulsos) { diff --git a/AgroBase/AgroBase/Services/OIDCanService.cs b/AgroBase/AgroBase/Services/OIDCanService.cs index 72c97f9c4..dba6f4781 100644 --- a/AgroBase/AgroBase/Services/OIDCanService.cs +++ b/AgroBase/AgroBase/Services/OIDCanService.cs @@ -22,8 +22,7 @@ namespace AgroBase.Services private static readonly object _sync = new object(); private static readonly HashSet _handlersRegistrados = new HashSet(); private static readonly Dictionary _ultimoEnvioConsultaPorDriver = new Dictionary(); - private static readonly Dictionary _ultimoEnvioParametro = new Dictionary(); - private static readonly Dictionary _ultimaConsultaPendente = new Dictionary(); + private static readonly Dictionary _ultimaConsultaParametro = new Dictionary(); private static int _cursorDriverPolling = 0; private static readonly Dictionary _ultimoComandoControleAssinatura = new Dictionary(); private static readonly Dictionary _ultimoComandoControleQuando = new Dictionary(); @@ -35,11 +34,12 @@ namespace AgroBase.Services */ private static readonly TimeSpan IntervaloMinimoConsultaPorDriver = TimeSpan.FromMilliseconds(40); private static readonly TimeSpan IntervaloMinimoMesmoParametro = TimeSpan.FromMilliseconds(300); - private static readonly TimeSpan JanelaConsultaPendente = TimeSpan.FromMilliseconds(1200); private static readonly TimeSpan JanelaDedupeComandoControle = TimeSpan.FromMilliseconds(80); private static readonly TimeSpan TempoRespostaRecente = TimeSpan.FromSeconds(3); - private static readonly TimeSpan TempoRemoverDriver = TimeSpan.FromSeconds(15); + private static readonly TimeSpan TempoRemoverDriver = TimeSpan.FromSeconds(5); private static readonly TimeSpan PeriodoHeartbeat = TimeSpan.FromMilliseconds(500); + private static readonly TimeSpan IntervaloTentativaReconexao = TimeSpan.FromSeconds(1); + private static readonly Dictionary _ultimaTentativaReconexaoPorDriver = new Dictionary(); /* * Polling recomendado. Se outro ponto do sistema ainda chamar @@ -314,8 +314,8 @@ namespace AgroBase.Services )); campos.Add(( - $"{enderecoMov}.polling.timeout_pendente_ms", - JanelaConsultaPendente.TotalMilliseconds + $"{enderecoMov}.polling.timeout_resposta_gerenciado_por", + "CanService" )); campos.Add(( @@ -499,6 +499,65 @@ namespace AgroBase.Services tmrHeartbeat.Start(); } + private static void GarantirMapeamentoDriver(MovimentacaoModel motor) + { + if (motor == null) + return; + + string modId = motor.Mod_ID; + + if (string.IsNullOrWhiteSpace(modId)) + return; + + if (SerialService.DispositivosMapeados.Any( + x => + x.Dispositivo == T_Code.Mov && + x.Mod_ID == modId)) + { + return; + } + + SerialService.DispositivosMapeados.Add( + new DispositivoDetalhesModel + { + Dispositivo = T_Code.Mov, + Endereco = CanManager.CanService._portName, + Versao = "1.0", + Mod_ID = modId, + } + ); + + var op = Variaveis.OperacaoEmAndamento; + + if (op.Sensoriamento?.Operacao?.ModsCalibragem == null) + op.Sensoriamento.Operacao.ModsCalibragem = new Dictionary>(); + + if (!op.Sensoriamento.Operacao.ModsCalibragem.ContainsKey(T_Code.Mov)) + { + op.Sensoriamento.Operacao.ModsCalibragem.Add( + T_Code.Mov, + new Dictionary() + ); + } + + op.Sensoriamento.Operacao + .ModsCalibragem[T_Code.Mov][modId] = false; + + op.Sensoriamento.InserirLog( + T_Code.Mov, + StatusModulo.Conectado, + 0, + "Dispositivo conectado", + modId + ); + + Variaveis.MostrarLog( + $"[OIDCanService] [CONEXAO] " + + $"MOV {modId} mapeado | " + + $"CAN=0x{motor._EnderecoCAN_Tx:X2}" + ); + } + private static void RegistrarHandler(byte idRx) { lock (_sync) @@ -542,61 +601,44 @@ namespace AgroBase.Services ); } - private static void EnviarComando( - T_Code dispositivo, - byte idTx, - byte idRx, - byte funcCode, - byte[] payload = null, - bool get = true, - bool contabilizarTimeoutHealth = true) + private static bool EnviarComando(T_Code dispositivo, byte idTx, byte idRx, byte funcCode, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true) { RegistrarHandlersMotor(idTx, idRx); - CanManager.CanService.AdicionarMensagemNaFila( - dispositivo, - new byte[] { idTx }, - new byte[] { idRx }, - funcCode, - funcCode, - payload ?? new byte[0], - get, - contabilizarTimeoutHealth - ); + return CanManager.CanService.AdicionarMensagemNaFila( + dispositivo, + new byte[] { idTx }, + new byte[] { idRx }, + funcCode, + funcCode, + payload ?? new byte[0], + get, + contabilizarTimeoutHealth + ); } - public static void RequisitarDado( - byte idTx, - byte idRx, - OidParametros dado) + public static void RequisitarDado(byte idTx, byte idRx, OidParametros dado) { RequisitarDadoInterno( idTx, idRx, dado, forcar: false, - contabilizarTimeoutHealth: - DeveContabilizarTimeoutHealth(dado) + contabilizarTimeoutHealth: DeveContabilizarTimeoutHealth(dado) ); } - private static bool RequisitarDadoInterno( - byte idTx, - byte idRx, - OidParametros dado, - bool forcar, - bool contabilizarTimeoutHealth) + private static bool RequisitarDadoInterno(byte idTx, byte idRx, OidParametros dado, bool forcar, bool contabilizarTimeoutHealth) { - DateTime agora = - DateTime.Now; + DateTime agora = DateTime.Now; + + string chaveParametro = + idTx.ToString("X2") + + ":" + + ((byte)dado).ToString("X2"); lock (_sync) { - string chaveParametro = - idTx.ToString("X2") + - ":" + - ((byte)dado).ToString("X2"); - if (!forcar) { if (_ultimoEnvioConsultaPorDriver.TryGetValue(idTx, out DateTime ultimoDriver) && agora - ultimoDriver < IntervaloMinimoConsultaPorDriver) @@ -604,80 +646,59 @@ namespace AgroBase.Services return false; } - if (_ultimaConsultaPendente.TryGetValue(idTx, out OidParametros pendente)) - { - bool possuiMomentoEnvio = _ultimoEnvioConsultaPorDriver.TryGetValue(idTx, out DateTime ultimoPendente); - - bool aindaDentroDaJanela = - possuiMomentoEnvio && - agora - ultimoPendente < JanelaConsultaPendente; - - if (aindaDentroDaJanela) - { - return false; - } - - /* - * A consulta anterior venceu. Libera o driver para uma nova - * tentativa sem manter pendência órfã. - */ - _ultimaConsultaPendente.Remove(idTx); - - Variaveis.MostrarLog( - "[OID] Consulta pendente expirada: " + - "driver=" + idTx.ToString("X2") + - ", parametro=" + pendente - ); - } - TimeSpan intervalo = IntervaloParametro.TryGetValue( dado, - out TimeSpan valor - ) + out TimeSpan valor) ? valor : IntervaloMinimoMesmoParametro; - if (_ultimoEnvioParametro - .TryGetValue(chaveParametro, out DateTime ultimoParam) && - agora - ultimoParam < intervalo) + if (_ultimaConsultaParametro.TryGetValue(chaveParametro, out DateTime ultimoParam) && agora - ultimoParam < intervalo) { return false; } } - - _ultimoEnvioConsultaPorDriver[idTx] = agora; - _ultimoEnvioParametro[chaveParametro] = agora; - _ultimaConsultaPendente[idTx] = dado; } - //Console.WriteLine( - // "[OID/TX] idTx=" + - // idTx.ToString("X2") + - // " idRx=" + - // idRx.ToString("X2") + - // " param=" + - // dado + - // " data=0F-" + - // ((byte)dado).ToString("X2") - //); - - EnviarComando( + /* + * A partir daqui quem decide se existe espaço + * para esta transação é o CanService. + */ + bool aceitoNaFila = EnviarComando( Dispositivo, idTx, idRx, (byte)OidFuncCode.Consulta, - new byte[] { (byte)dado }, + new byte[] + { + (byte)dado + }, get: true, - contabilizarTimeoutHealth: - contabilizarTimeoutHealth + contabilizarTimeoutHealth: contabilizarTimeoutHealth ); + if (!aceitoNaFila) + return false; + + /* + * IMPORTANTE: + * isso significa "aceito pela fila", + * não "fisicamente transmitido". + * + * Timeout de resposta não é contado aqui. + */ + DateTime momentoAceite = DateTime.Now; + + lock (_sync) + { + _ultimoEnvioConsultaPorDriver[idTx] = momentoAceite; + _ultimaConsultaParametro[chaveParametro] = momentoAceite; + } + return true; } - private static bool DeveContabilizarTimeoutHealth( - OidParametros dado) + private static bool DeveContabilizarTimeoutHealth(OidParametros dado) { /* * Saúde CAN deve refletir comandos/leituras críticas. @@ -709,63 +730,132 @@ namespace AgroBase.Services { try { - var modulos = - Variaveis - .OperacaoEmAndamento - .DispMvd? - .Dados? - .Modulos? - .Where(x => - x?.MovMotor != null && - x.MovMotor.Inicializado - ) - .ToList(); + if (!CanManager.CanService.IsConnected) + return; + var modulos = Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.Where(x => x?.MovMotor != null).ToList(); if (modulos == null || modulos.Count == 0) { - RemoverDriversMuitoAntigos(); return; } + /* + * Existem OIDs configurados nesta operação e o CAN está ativo. + * O heartbeat deve existir independentemente de termos encontrado + * algum driver no discovery inicial. + */ + GarantirTimerHeartbeat(); + + /* + * 1. Primeiro atualiza lifecycle. + */ + MarcarDriversInativosPorTimeout(); + + /* + * 2. Offline recebe tentativa de reconexão. + */ + foreach (var modulo in modulos) + { + var motor = + modulo.MovMotor; + + if (motor.Inicializado) + continue; + + TentarReconectarDriver( + motor._EnderecoCAN_Tx, + motor._EnderecoCAN_Rx + ); + } + + /* + * 3. Drivers online que acabaram de voltar + * precisam recuperar configuração. + */ + foreach (var modulo in modulos) + { + var motor = + modulo.MovMotor; + + if (!motor.Inicializado) + continue; + + GarantirMapeamentoDriver(motor); + + var leitura = + DadosLeitura.FirstOrDefault( + x => x.EnderecoTx == + motor._EnderecoCAN_Tx + ); + + if (leitura != null && + !leitura.Configurado) + { + ConfigurarDriver(motor); + + Variaveis.MostrarLog( + $"[OIDCanService] [CONFIG] " + + $"Driver CAN 0x{motor._EnderecoCAN_Tx:X2} " + + $"reconfigurado." + ); + } + } + + /* + * 4. Polling global dos online. + */ + var online = + modulos + .Where(x => + x.MovMotor.Inicializado) + .ToList(); + + if (online.Count == 0) + return; + int inicio; lock (_sync) { if (_cursorDriverPolling < 0 || - _cursorDriverPolling >= modulos.Count) + _cursorDriverPolling >= online.Count) { _cursorDriverPolling = 0; } - inicio = _cursorDriverPolling; + inicio = + _cursorDriverPolling; } for (int deslocamento = 0; - deslocamento < modulos.Count; + deslocamento < online.Count; deslocamento++) { int index = - (inicio + deslocamento) % modulos.Count; + (inicio + deslocamento) % + online.Count; - var motor = modulos[index].MovMotor; - - byte idTx = motor._EnderecoCAN_Tx; - byte idRx = motor._EnderecoCAN_Rx; + var motor = + online[index].MovMotor; OidParametros? proximo = - EscolherProximoParametro(idTx); + EscolherProximoParametro( + motor._EnderecoCAN_Tx + ); if (!proximo.HasValue) continue; - bool enviado = RequisitarDadoInterno( - idTx, - idRx, - proximo.Value, - forcar: false, - contabilizarTimeoutHealth: - DeveContabilizarTimeoutHealth(proximo.Value) - ); + bool enviado = + RequisitarDadoInterno( + motor._EnderecoCAN_Tx, + motor._EnderecoCAN_Rx, + proximo.Value, + forcar: false, + contabilizarTimeoutHealth: + DeveContabilizarTimeoutHealth( + proximo.Value) + ); if (!enviado) continue; @@ -773,22 +863,20 @@ namespace AgroBase.Services lock (_sync) { _cursorDriverPolling = - (index + 1) % modulos.Count; + (index + 1) % + online.Count; } - // Uma consulta nova por tick. + /* + * Uma consulta nova global por tick. + */ break; } - - /* - * Mesmo quando ninguém estava apto a enviar, fazemos a limpeza. - */ - RemoverDriversMuitoAntigos(); } catch (Exception ex) { - Console.WriteLine( - "[OID] Erro em AtualizarDados: " + + Variaveis.MostrarLog( + "[OIDCanService] Erro em AtualizarDados: " + ex.Message ); } @@ -811,22 +899,17 @@ namespace AgroBase.Services ((byte)parametro).ToString("X2"); TimeSpan intervalo = - IntervaloParametro.TryGetValue( - parametro, - out TimeSpan configurado - ) + IntervaloParametro.TryGetValue(parametro, out TimeSpan configurado) ? configurado : IntervaloMinimoMesmoParametro; - if (!_ultimoEnvioParametro.TryGetValue( - chave, - out DateTime ultimoEnvio)) + if (!_ultimaConsultaParametro.TryGetValue(chave, out DateTime ultimaConsulta)) { // Parâmetro nunca consultado tem prioridade máxima. return parametro; } - TimeSpan decorrido = agora - ultimoEnvio; + TimeSpan decorrido = agora - ultimaConsulta; if (decorrido < intervalo) continue; @@ -853,10 +936,9 @@ namespace AgroBase.Services } } - private static void RemoverDriversMuitoAntigos() + private static void MarcarDriversInativosPorTimeout() { - DateTime agora = - DateTime.Now; + DateTime agora = DateTime.Now; List antigos; @@ -868,19 +950,69 @@ namespace AgroBase.Services x.Iniciado && x.UltimoComandoRecebido != DateTime.MinValue && agora - x.UltimoComandoRecebido > - TempoRemoverDriver) + TempoRemoverDriver) .ToList(); } foreach (var antigo in antigos) + { antigo.Iniciado = false; + + /* + * Importantíssimo: + * não sabemos se houve apenas perda CAN + * ou reboot/power-cycle do OID. + */ + antigo.Configurado = false; + + lock (_sync) + { + _ultimoEnvioConsultaPorDriver.Remove( + antigo.EnderecoTx + ); + + string prefixo = + antigo.EnderecoTx.ToString("X2") + + ":"; + + var chaves = + _ultimaConsultaParametro.Keys + .Where(x => + x.StartsWith(prefixo)) + .ToList(); + + foreach (var chave in chaves) + _ultimaConsultaParametro.Remove(chave); + } + + Variaveis.MostrarLog( + $"[OIDCanService] [CONEXAO] " + + $"Driver CAN 0x{antigo.EnderecoTx:X2} OFFLINE | " + + $"sem resposta há " + + $"{(agora - antigo.UltimoComandoRecebido).TotalSeconds:F1}s" + ); + } } - public static void ComandoControle( - byte idTx, - byte idRx, - OidFuncCode funcCode, - double setPoint) + private static void TentarReconectarDriver(byte idTx, byte idRx) + { + DateTime agora = DateTime.Now; + + lock (_sync) + { + if (_ultimaTentativaReconexaoPorDriver.TryGetValue(idTx, out DateTime ultima) && agora - ultima < IntervaloTentativaReconexao) + { + return; + } + + _ultimaTentativaReconexaoPorDriver[idTx] = agora; + } + + RegistrarHandlersMotor(idTx, idRx); + EnviarComandoDescoberta(idTx, idRx); + } + + public static void ComandoControle(byte idTx, byte idRx, OidFuncCode funcCode, double setPoint) { byte[] setPointBytes = MontarPayloadControle( @@ -914,10 +1046,7 @@ namespace AgroBase.Services ); } - private static bool DeveDroparComandoDuplicado( - byte idTx, - OidFuncCode funcCode, - byte[] payload) + private static bool DeveDroparComandoDuplicado(byte idTx, OidFuncCode funcCode, byte[] payload) { if (funcCode == OidFuncCode.Heartbeat) return false; @@ -955,10 +1084,7 @@ namespace AgroBase.Services } } - private static byte[] MontarPayloadControle( - byte idTx, - OidFuncCode funcCode, - double setPoint) + private static byte[] MontarPayloadControle(byte idTx, OidFuncCode funcCode, double setPoint) { OIDModel item = DadosLeitura.FirstOrDefault( @@ -1057,8 +1183,8 @@ namespace AgroBase.Services } default: - Console.WriteLine( - "[OID] Comando não mapeado: " + + Variaveis.MostrarLog( + "[OIDCanService] Comando não mapeado: " + funcCode ); return null; @@ -1400,18 +1526,14 @@ namespace AgroBase.Services { try { - if (mensagem == null || - mensagem.DataRx == null || - mensagem.DataRx.Length < 2) + if (mensagem == null || mensagem.DataRx == null || mensagem.DataRx.Length < 2) { return; } - OidFuncCode func = - (OidFuncCode)mensagem.DataRx[0]; + OidFuncCode func = (OidFuncCode)mensagem.DataRx[0]; - if (func != OidFuncCode.Consulta && - func != OidFuncCode.ConsultaStatusRetornoZero) + if (func != OidFuncCode.Consulta && func != OidFuncCode.ConsultaStatusRetornoZero) { /* * Controle não deveria responder. Se responder algo @@ -1434,7 +1556,7 @@ namespace AgroBase.Services return; } - //Console.WriteLine( + //Variaveis.MostrarLog( // "[OID/RX] idTx=" + // mensagem.IdTx.ToString("X") + // " idRx=" + @@ -1443,120 +1565,135 @@ namespace AgroBase.Services // BitConverter.ToString(mensagem.DataRx ?? new byte[0]) //); - OidParametros parametro = - (OidParametros)mensagem.DataRx[1]; + OidParametros parametro = (OidParametros)mensagem.DataRx[1]; - ProcessarParametro( - item, - parametro, - mensagem.DataRx - ); + bool processado = ProcessarParametro(item, parametro, mensagem.DataRx); + + if (!processado) + { + Variaveis.MostrarLog( + $"[OIDCanService] [RX] " + + $"Resposta inválida ignorada | " + + $"driver=0x{item.EnderecoTx:X2} | " + + $"parametro={parametro}" + ); + + return; + } + + bool jaFoiVisto = item.UltimoComandoRecebido != DateTime.MinValue; + bool estavaOffline = jaFoiVisto && !item.Iniciado; item.UltimoComandoRecebido = DateTime.Now; item.Iniciado = true; + DateTime agoraRx = DateTime.Now; + string chaveParametro = item.EnderecoTx.ToString("X2") + ":" + ((byte)parametro).ToString("X2"); lock (_sync) { - RemoverPendenteSeCorresponder( - item.EnderecoTx, - parametro - ); + _ultimaConsultaParametro[chaveParametro] = agoraRx; + } - if (item.EnderecoRx != item.EnderecoTx) - { - RemoverPendenteSeCorresponder( - item.EnderecoRx, - parametro - ); - } + if (estavaOffline) + { + Variaveis.MostrarLog( + $"[OIDCanService] [CONEXAO] " + + $"Driver CAN 0x{item.EnderecoTx:X2} ONLINE novamente" + ); } } catch (Exception ex) { - Console.WriteLine( - "[OID] Erro ao interpretar resposta CAN: " + + Variaveis.MostrarLog( + "[OIDCanService] Erro ao interpretar resposta CAN: " + ex.Message ); } } - private static void RemoverPendenteSeCorresponder(byte endereco, OidParametros parametroRecebido) - { - if (_ultimaConsultaPendente.TryGetValue(endereco, out OidParametros pendente) && pendente == parametroRecebido) - { - _ultimaConsultaPendente.Remove(endereco); - } - } - - private void ProcessarParametro( - OIDModel item, - OidParametros parametro, - byte[] data) + private bool ProcessarParametro(OIDModel item, OidParametros parametro, byte[] data) { switch (parametro) { case OidParametros.CodErro: if (data.Length >= 4) { - short codigo = - ReadInt16BE(data, 2); - - item.CodigoAlarme.valor = - (OidCodigoErro)codigo; + short codigo = ReadInt16BE(data, 2); + item.CodigoAlarme.valor = (OidCodigoErro)codigo; + return true; } break; case OidParametros.VelocidadeAtual: if (data.Length >= 6) + { item.ERPM.valor = ReadInt32BE(data, 2); + return true; + } break; case OidParametros.CicloTrabalho: if (data.Length >= 4) - item.CicloTrabalho.valor = - ReadInt16BE(data, 2) / 10.0; + { + item.CicloTrabalho.valor = ReadInt16BE(data, 2) / 10.0; + return true; + } break; case OidParametros.Potencia: if (data.Length >= 4) - item.Potencia.valor = - ReadInt16BE(data, 2); + { + item.Potencia.valor = ReadInt16BE(data, 2); + return true; + } break; case OidParametros.Tensao: if (data.Length >= 4) - item.Tensao.valor = - ReadInt16BE(data, 2); + { + item.Tensao.valor = ReadInt16BE(data, 2); + return true; + } break; case OidParametros.CorrenteMotor: if (data.Length >= 4) - item.CorrenteMotor.valor = - ReadInt16BE(data, 2) / 100.0; + { + item.CorrenteMotor.valor = ReadInt16BE(data, 2) / 100.0; + return true; + } break; case OidParametros.CorrenteBarramento: if (data.Length >= 4) - item.CorrenteBarramento.valor = - ReadInt16BE(data, 2) / 100.0; + { + item.CorrenteBarramento.valor = ReadInt16BE(data, 2) / 100.0; + return true; + } break; case OidParametros.Temperatura: if (data.Length >= 4) - item.Temperatura.valor = - ReadInt16BE(data, 2); + { + item.Temperatura.valor = ReadInt16BE(data, 2); + return true; + } break; case OidParametros.PosicaoAbsoluta: if (data.Length >= 6) - item.PosicaoAbsoluta = - ReadInt32BE(data, 2) / 100.0; + { + item.PosicaoAbsoluta = ReadInt32BE(data, 2) / 100.0; + return true; + } break; case OidParametros.PosicaoRelativa: if (data.Length >= 6) - item.PosicaoRelativa = - ReadInt32BE(data, 2) / 100.0; + { + item.PosicaoRelativa = ReadInt32BE(data, 2) / 100.0; + return true; + } break; case OidParametros.ModoCodificador: @@ -1564,7 +1701,10 @@ namespace AgroBase.Services * Manual: resposta DLC 3, DATA[2]=0/1. */ if (data.Length >= 3) + { item.ModoCodificador = data[2]; + return true; + } break; case OidParametros.ErroLerCache: @@ -1573,20 +1713,25 @@ namespace AgroBase.Services * Mantém o primeiro para compatibilidade. */ if (data.Length >= 3) + { item.ErroHistorico = data[2]; + return true; + } break; case OidParametros.IO_Status: if (data.Length >= 4) - item.IO_Status = - ReadUInt16BE(data, 2); + { + item.IO_Status = ReadUInt16BE(data, 2); + return true; + } break; } + + return false; } - private void ProcessarStatusRetornoZero( - OIDModel item, - byte[] data) + private void ProcessarStatusRetornoZero(OIDModel item, byte[] data) { /* * Manual: @@ -1603,9 +1748,7 @@ namespace AgroBase.Services internal static class OidModelExtensions { - public static void SetModoControleSeguro( - this OIDModel model, - OIDModoControle modo) + public static void SetModoControleSeguro(this OIDModel model, OIDModoControle modo) { if (model == null) return; diff --git a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs index 47dc5e5db..3c05895c6 100644 --- a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs +++ b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs @@ -1430,6 +1430,9 @@ namespace AgroBase.Services.Operadores ("pendentes_nao_enviadas", DadosCan.PendentesNaoEnviadas), ("pendentes_aguardando_resposta", DadosCan.PendentesAguardandoResposta), ("frames_rx_fila", DadosCan.FramesRxNaFila), + ("idade_max_fila_nao_enviada_ms", DadosCan.IdadeMaxFilaNaoEnviadaMs), + ("pendentes_nao_enviadas_atrasadas", DadosCan.PendentesNaoEnviadasAtrasadas), + ("idade_media_fila_nao_enviada_ms", DadosCan.IdadeMediaFilaNaoEnviadaMs), // Latência ("latencia_media_ms", DadosCan.LatenciaMediaMs), diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/can.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/can.py index 3cdfd47e5..2badfdd70 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/can.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/can.py @@ -77,6 +77,9 @@ class ModuloCAN(ModuloDiagnosticoBase): pend_nao_env = int(m.get("pendentes_nao_enviadas", 0) or 0) pend_resp = int(m.get("pendentes_aguardando_resposta", 0) or 0) frames_rx_fila = int(m.get("frames_rx_fila", 0) or 0) + idade_max_fila_nao_enviada_ms = float(m.get("idade_max_fila_nao_enviada_ms", 0.0) or 0.0) + pendentes_nao_enviadas_atrasadas = int(m.get("pendentes_nao_enviadas_atrasadas", 0) or 0) + idade_media_fila_nao_enviada_ms = float(m.get("idade_media_fila_nao_enviada_ms", 0.0) or 0.0) timeouts_total = int(m.get("timeouts_resposta_total", 0) or 0) timeouts_por_id_total = m.get("timeouts_por_id_total", {}) or {} @@ -276,6 +279,9 @@ class ModuloCAN(ModuloDiagnosticoBase): "pendentes_total": pend_total, "pendentes_nao_enviadas": pend_nao_env, "pendentes_aguardando_resposta": pend_resp, + "idade_max_fila_nao_enviada_ms": idade_max_fila_nao_enviada_ms, + "idade_media_fila_nao_enviada_ms": idade_media_fila_nao_enviada_ms, + "pendentes_nao_enviadas_atrasadas": pendentes_nao_enviadas_atrasadas, "frames_rx_fila": frames_rx_fila, "timeouts_resposta_total": timeouts_total, "timeouts_por_id_total": timeouts_por_id_total,