From 22d57ec06af748021021a0435ff29df0a132662b Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Mon, 3 Aug 2026 17:43:02 -0300 Subject: [PATCH] ajustes no imu, resultados da operacao, controle manual, can service, requisicao sequencial bms, ordem dos arquivos versionamento --- .../Forms/Operacoes/frmResultadosOperacao.cs | 6 +- AgroBase/AgroBase/Models/GeneralJoystick.cs | 328 +++++++++++++++--- .../Models/Operadores/CameraWorkerModel.cs | 4 +- AgroBase/AgroBase/Models/Variaveis.cs | 14 +- AgroBase/AgroBase/Services/CANService.cs | 1 + .../AgroBase/Services/CANServiceSerial.cs | 10 + .../AgroBase/Services/CANServiceWaveshare.cs | 10 + .../AgroBase/Services/CanServiceEthernet.cs | 73 ++-- AgroBase/AgroBase/Services/DalyBMSService.cs | 85 ++--- .../Services/Operadores/OperadoresService.cs | 18 +- .../AgroBase/Services/VersionamentoService.cs | 8 +- .../workers/camera_worker/camera_imu.py | 219 +++++++++++- .../camera_worker/camera_multispectral.py | 4 +- .../workers/camera_worker/camera_oak.py | 129 ++++--- .../Scripts/workers/health_worker/main.py | 2 +- .../workers/health_worker/modulos/imu.py | 120 ++++++- .../ConfigConexao/ConfigConexaoViewModel.cs | 2 +- 17 files changed, 791 insertions(+), 242 deletions(-) diff --git a/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs b/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs index a9645f4c4..f4137f37b 100644 --- a/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs +++ b/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs @@ -238,7 +238,7 @@ namespace AgroBase.Forms.Operacoes pnlMapa = pnlMapa }; MapasService.CriarArquivoMapa(new List>() { LogsGPS.ToList() }, 1517, data.id); - Mapa.btnCarregar_Click(sender, e, Variaveis.CaminhoSistema + "/" + Variaveis.CaminhoMapasConvertidos + data.id + ".json"); + Mapa.btnCarregar_Click(sender, e, Path.Combine(Variaveis.CaminhoSistema, Variaveis.CaminhoMapasConvertidos + data.id + ".json")); trbMomento.Maximum = LogsOperacao.Count - 1; trbMomento.Minimum = 0; @@ -1300,8 +1300,8 @@ namespace AgroBase.Forms.Operacoes txtGPS_DistanciaBase.Text = ((LogBase?.Latitude != 0 && LogBase?.Longitude != 0) ? GPSUtils.DistanciaEntrePontos(LogBase ?? new GPSModel(), Log ?? new GPSModel()) : 0).ToString("0.00") + " m"; - GPSService.EnviarCoordenadasParaMapa(Log?.Latitude ?? 0, Log?.Longitude ?? 0, Log?.AnguloCarroDefinido ?? 0, true, rover: true); - GPSService.EnviarCoordenadasParaMapa(LogBase?.Latitude ?? 0, LogBase?.Longitude ?? 0, LogBase?.AnguloCarroDefinido ?? 0, false, rover: false); + Mapa?.AtualizarPosicaoMapaAsync(MapasVariaveisModel.IDMarcadorRover, Log?.Latitude ?? 0, Log?.Longitude ?? 0, Log?.AnguloCarroDefinido ?? 0, true); + Mapa?.AtualizarPosicaoMapaAsync(MapasVariaveisModel.IDMarcadorBase, LogBase?.Latitude ?? 0, LogBase?.Longitude ?? 0, LogBase?.AnguloCarroDefinido ?? 0, false); lblGPS.ForeColor = !Log.Inicializado ? Color.Red : Color.Black; diff --git a/AgroBase/AgroBase/Models/GeneralJoystick.cs b/AgroBase/AgroBase/Models/GeneralJoystick.cs index 6f96a355b..0b7c28893 100644 --- a/AgroBase/AgroBase/Models/GeneralJoystick.cs +++ b/AgroBase/AgroBase/Models/GeneralJoystick.cs @@ -7,12 +7,14 @@ using System; using System.Collections.Generic; using System.Data; using System.Linq; +using System.Threading; using System.Threading.Tasks; using System.Windows.Forms; using static AgroBase.Models.Enums; namespace AgroBase { + /// Leitura consolidada e tolerante a falhas do controle manual. public class GeneralJoystick { public static List JoysticksConectados = new List(); @@ -34,7 +36,7 @@ namespace AgroBase new DeParaComandos() { key = Keys.Space, botaoJoy = BotoesJoystick.Bolinha, direcao = Direcao.EmFreio, Dispositivo = T_Code.Mov, PararAoSoltar = true }, new DeParaComandos() { key = Keys.Subtract, botaoJoy = BotoesJoystick.L1, Dispositivo = T_Code.Mov, PararAoSoltar = false }, new DeParaComandos() { key = Keys.Add, botaoJoy = BotoesJoystick.R1, Dispositivo = T_Code.Mov, PararAoSoltar = false }, - + new DeParaComandos() { key = Keys.D1, botaoJoy = BotoesJoystick.Touchpad, Dispositivo = T_Code.Atu, PararAoSoltar = true, IDcontrolar = "B01" }, new DeParaComandos() { key = Keys.D2, botaoJoy = BotoesJoystick.Touchpad, Dispositivo = T_Code.Atu, PararAoSoltar = true, IDcontrolar = "B02" }, new DeParaComandos() { key = Keys.D3, botaoJoy = BotoesJoystick.Touchpad, Dispositivo = T_Code.Atu, PararAoSoltar = true, IDcontrolar = "B03" }, @@ -44,9 +46,22 @@ namespace AgroBase new DeParaComandos() { key = Keys.D7, botaoJoy = BotoesJoystick.Touchpad, Dispositivo = T_Code.Atu, PararAoSoltar = true, IDcontrolar = "B07" }, }; public static List BotoesJoyPressionados = new List() { }; + private static readonly object _syncJoystick = new object(); + private static readonly SemaphoreSlim _tickGate = new SemaphoreSlim(1, 1); + private static readonly HashSet _estadoBotoes = new HashSet(); + private static DateTime _ultimaTentativaReconexao = DateTime.MinValue; + private static DateTime _ultimoEnvioMov = DateTime.MinValue; + private static DateTime _ultimoEnvioDir = DateTime.MinValue; + private static Direcao _ultimaDirecaoMovEnviada = Direcao.Parado; + private static double _ultimoValorMovEnviado = double.NaN; + private static double _ultimoAnguloDirEnviado = double.NaN; + private const int IntervaloReconexaoMs = 2000; + private const int HeartbeatComandoMs = 300; + private const double VariacaoMinimaMov = 0.25; + private const double VariacaoMinimaDir = 0.20; public static List MovimentosCombinados = new List() { - TipoMovimentoDirecional.MovimentoLateral, + TipoMovimentoDirecional.MovimentoLateral, TipoMovimentoDirecional.RotacionarNoEixo }; @@ -59,6 +74,7 @@ namespace AgroBase public static void IniciarRotinas() { tmrJoystick = new AsyncTaskTimerModel("tmrJoystick", tmrJoystick_Tick, 100); + tmrJoystick.Start(); } public static void AtualizaDispositivo() @@ -68,7 +84,7 @@ namespace AgroBase DispCon = Variaveis.DispositivosConectados.Where(x => x.Dados.ConexaoAtiva).ToList(); frmInstancial.frmJoystick.lblDispositivo.Text = "Dispositivos: " + (DispCon.Count == 0 ? "N/A" : string.Join(", ", DispCon.Select(x => x._Descricao + " (" + Enum.GetName(typeof(T_Code), x.Dispositivo) + ")").ToArray())); - InicializarJoysticksConectados(); + AtualizarListaJoysticks(); return true; }); @@ -77,60 +93,90 @@ namespace AgroBase public static void InicializarJoysticksConectados() { - JoysticksConectados = new List(); - - var directInput = new DirectInput(); - - // Find a Joystick Guid - var joystickGuid = Guid.Empty; - - foreach (var deviceInstance in directInput.GetDevices(DeviceType.Gamepad, DeviceEnumerationFlags.AllDevices)) + lock (_syncJoystick) { - joystickGuid = deviceInstance.InstanceGuid; - JoysticksConectados.Add(deviceInstance.InstanceName); - } + var directInput = new DirectInput(); + var nomes = new List(); - // If Gamepad not found, look for a Joystick - if (joystickGuid == Guid.Empty) - { - foreach (var deviceInstance in directInput.GetDevices(DeviceType.Joystick, DeviceEnumerationFlags.AllDevices)) + // Find a Joystick Guid + var joystickGuid = Guid.Empty; + + foreach (var deviceInstance in directInput.GetDevices(DeviceType.Gamepad, DeviceEnumerationFlags.AllDevices)) { joystickGuid = deviceInstance.InstanceGuid; - JoysticksConectados.Add(deviceInstance.InstanceName); + nomes.Add(deviceInstance.InstanceName); + } + + // If Gamepad not found, look for a Joystick + if (joystickGuid == Guid.Empty) + { + foreach (var deviceInstance in directInput.GetDevices(DeviceType.Joystick, DeviceEnumerationFlags.AllDevices)) + { + joystickGuid = deviceInstance.InstanceGuid; + nomes.Add(deviceInstance.InstanceName); + } + } + + // If Joystick not found, throws an error + if (joystickGuid == Guid.Empty) + { + JoysticksConectados = nomes; + DesconectarJoystickInterno(); + return; + } + + // Instantiate the joystick + DesconectarJoystickInterno(); + JoystickConectado = new Joystick(directInput, joystickGuid); + JoysticksConectados = nomes; + + // Query all suported ForceFeedback effects + var allEffects = JoystickConectado.GetEffects(); + foreach (var effectInfo in allEffects) + { + Console.WriteLine("Effect available {0}", effectInfo.Name); + } + + // Set BufferSize in order to use buffered data. + JoystickConectado.Acquire(); + + + if (JoystickConectado != null) + { + tmrJoystick?.Start(); } } + } - // If Joystick not found, throws an error - if (joystickGuid == Guid.Empty) + private static void AtualizarListaJoysticks() + { + try { - JoystickConectado = null; - tmrJoystick?.Stop(); - - return; + using (var directInput = new DirectInput()) + { + var nomes = directInput.GetDevices(DeviceType.Gamepad, DeviceEnumerationFlags.AllDevices) + .Concat(directInput.GetDevices(DeviceType.Joystick, DeviceEnumerationFlags.AllDevices)) + .Select(x => x.InstanceName) + .Distinct() + .ToList(); + lock (_syncJoystick) JoysticksConectados = nomes; + } } - - // Instantiate the joystick - JoystickConectado = new Joystick(directInput, joystickGuid); - - // Query all suported ForceFeedback effects - var allEffects = JoystickConectado.GetEffects(); - foreach (var effectInfo in allEffects) + catch (Exception ex) { - Console.WriteLine("Effect available {0}", effectInfo.Name); - } - - // Set BufferSize in order to use buffered data. - JoystickConectado.Properties.BufferSize = 128; - - // Acquire the joystick - JoystickConectado.Acquire(); - - - if (JoystickConectado != null) - { - tmrJoystick.Start(); + Variaveis.MostrarLog($"[GeneralJoystick] Falha ao listar joysticks: {ex.GetType().Name}: {ex.Message}"); } + } + private static void DesconectarJoystickInterno() + { + var antigo = JoystickConectado; + JoystickConectado = null; + _estadoBotoes.Clear(); + BotoesJoyPressionados.Clear(); + if (antigo == null) return; + try { antigo.Unacquire(); } catch { } + try { antigo.Dispose(); } catch { } } private static bool ComparaPosicaoAnalogico(int Atual, ComparativoAnalogico Comparativo) @@ -173,6 +219,9 @@ namespace AgroBase private static async Task tmrJoystick_Tick() { + await ProcessarEstadoAtualJoystick(); + return; +#pragma warning disable CS0162 if (JoystickConectado == null) { return; @@ -192,7 +241,7 @@ namespace AgroBase AtualizaDispositivo(); return; } - + List _botoesPressionados = new List(); List _botoesSoltos = new List(); @@ -529,6 +578,155 @@ namespace AgroBase //Console.WriteLine($"Botao solto: {botao}"); BotoesJoyPressionados.Remove(botao); }); +#pragma warning restore CS0162 + } + + private static async Task ProcessarEstadoAtualJoystick() + { + if (!await _tickGate.WaitAsync(0)) return; + try + { + Joystick joystick; + lock (_syncJoystick) joystick = JoystickConectado; + + if (joystick == null) + { + TentarReconectar(); + return; + } + + JoystickState estado; + try + { + joystick.Poll(); + estado = joystick.GetCurrentState(); + } + catch (Exception ex) + { + Variaveis.MostrarLog($"[GeneralJoystick] Controle desconectado/falha de leitura: {ex.GetType().Name}: {ex.Message}"); + PararCarroControle("Perda de comunicação com o joystick"); + lock (_syncJoystick) DesconectarJoystickInterno(); + TentarReconectar(); + return; + } + + AtualizarEstadoFisico(estado); + ResolverMovimento(estado); + ResolverDirecional(estado); + ResolverComandosDeBorda(); + } + catch (Exception ex) + { + Variaveis.MostrarLog($"[GeneralJoystick] Erro inesperado no tick: {ex.GetType().Name}: {ex.Message}"); + PararCarroControle("Falha interna no processamento do joystick"); + } + finally + { + _tickGate.Release(); + } + } + + private static void TentarReconectar() + { + if ((DateTime.Now - _ultimaTentativaReconexao).TotalMilliseconds < IntervaloReconexaoMs) return; + _ultimaTentativaReconexao = DateTime.Now; + try { InicializarJoysticksConectados(); } + catch (Exception ex) + { + Variaveis.MostrarLog($"[GeneralJoystick] Reconexão falhou: {ex.GetType().Name}: {ex.Message}"); + } + } + + private static void AtualizarEstadoFisico(JoystickState s) + { + var anterior = new HashSet(_estadoBotoes); + _estadoBotoes.Clear(); + var buttons = s.Buttons ?? new bool[0]; + Action addButton = (i, b) => { if (i < buttons.Length && buttons[i]) _estadoBotoes.Add(b); }; + addButton(0, BotoesJoystick.Quadrado); addButton(1, BotoesJoystick.Xis); + addButton(2, BotoesJoystick.Bolinha); addButton(3, BotoesJoystick.Triangulo); + addButton(4, BotoesJoystick.L1); addButton(5, BotoesJoystick.R1); + addButton(6, BotoesJoystick.L2); addButton(7, BotoesJoystick.R2); + addButton(8, BotoesJoystick.Share); addButton(9, BotoesJoystick.Options); + addButton(10, BotoesJoystick.L3); addButton(11, BotoesJoystick.R3); + addButton(12, BotoesJoystick.PS); addButton(13, BotoesJoystick.Touchpad); + + if (s.RotationX > MargemAnalogico) _estadoBotoes.Add(BotoesJoystick.L2A); + if (s.RotationY > MargemAnalogico) _estadoBotoes.Add(BotoesJoystick.R2A); + if (s.X < MaximoAnalogico / 2 - 3000) _estadoBotoes.Add(BotoesJoystick.AEEsquerda); + if (s.X > MaximoAnalogico / 2 + 3000) _estadoBotoes.Add(BotoesJoystick.AEDireita); + + int pov = s.PointOfViewControllers != null && s.PointOfViewControllers.Length > 0 ? s.PointOfViewControllers[0] : -1; + if (pov == 27000) _estadoBotoes.Add(BotoesJoystick.SEsquerda); + if (pov == 9000) _estadoBotoes.Add(BotoesJoystick.SDireita); + if (pov == 0) _estadoBotoes.Add(BotoesJoystick.SCima); + if (pov == 18000) _estadoBotoes.Add(BotoesJoystick.SBaixo); + + _botoesPressionadosNoTick = new HashSet(_estadoBotoes.Except(anterior)); + _botoesSoltosNoTick = new HashSet(anterior.Except(_estadoBotoes)); + BotoesJoyPressionados = _estadoBotoes.ToList(); + } + + private static HashSet _botoesPressionadosNoTick = new HashSet(); + private static HashSet _botoesSoltosNoTick = new HashSet(); + + private static void ResolverMovimento(JoystickState s) + { + bool frenteDigital = _estadoBotoes.Contains(BotoesJoystick.Xis); + bool trasDigital = _estadoBotoes.Contains(BotoesJoystick.Quadrado); + double frenteAnalogico = s.RotationY > MargemAnalogico ? s.RotationY : 0; + double trasAnalogico = s.RotationX > MargemAnalogico ? s.RotationX : 0; + bool conflito = (frenteDigital || frenteAnalogico > 0) && (trasDigital || trasAnalogico > 0); + + BotoesJoystick botao; + double valor; + Direcao direcao; + if (conflito) + { + botao = BotoesJoystick.Xis; valor = 0; direcao = Direcao.Parado; + RegistrarMotivoManual("Comandos simultâneos de frente e ré; movimento neutralizado"); + } + else if (frenteAnalogico > 0) { botao = BotoesJoystick.R2A; valor = frenteAnalogico; direcao = Direcao.Frente; } + else if (trasAnalogico > 0) { botao = BotoesJoystick.L2A; valor = trasAnalogico; direcao = Direcao.Tras; } + else if (frenteDigital) { botao = BotoesJoystick.Xis; valor = MaximoAnalogico; direcao = Direcao.Frente; } + else if (trasDigital) { botao = BotoesJoystick.Quadrado; valor = MaximoAnalogico; direcao = Direcao.Tras; } + else { botao = BotoesJoystick.Xis; valor = 0; direcao = Direcao.Parado; } + + var cmd = DeParaComandos.First(x => x.botaoJoy == botao).Clone(); + 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; + if (!mudou && !heartbeat) return; + EnviarComandoMotorAtualizado(cmd, ForcarComando: false); + _ultimaDirecaoMovEnviada = direcao; _ultimoValorMovEnviado = cmd.ValorAplicar; _ultimoEnvioMov = DateTime.Now; + } + + private static void ResolverDirecional(JoystickState s) + { + bool esqDigital = _estadoBotoes.Contains(BotoesJoystick.SEsquerda); + bool dirDigital = _estadoBotoes.Contains(BotoesJoystick.SDireita); + BotoesJoystick botao = BotoesJoystick.SEsquerda; + double valor = 0; + bool solto = true; + if (esqDigital ^ dirDigital) { botao = esqDigital ? BotoesJoystick.SEsquerda : BotoesJoystick.SDireita; solto = false; } + else if (_estadoBotoes.Contains(BotoesJoystick.AEEsquerda)) { botao = BotoesJoystick.AEEsquerda; valor = s.X; solto = false; } + else if (_estadoBotoes.Contains(BotoesJoystick.AEDireita)) { botao = BotoesJoystick.AEDireita; valor = s.X; solto = false; } + + var cmd = DeParaComandos.First(x => x.botaoJoy == botao).Clone(); + cmd.CalcularValor(solto, valorJoy: valor); + bool mudou = double.IsNaN(_ultimoAnguloDirEnviado) || Math.Abs(cmd.ValorAplicar - _ultimoAnguloDirEnviado) >= VariacaoMinimaDir; + bool heartbeat = (DateTime.Now - _ultimoEnvioDir).TotalMilliseconds >= HeartbeatComandoMs; + if (!mudou && !heartbeat) return; + EnviarComandoMotorAtualizado(cmd, ForcarComando: false); + _ultimoAnguloDirEnviado = cmd.ValorAplicar; _ultimoEnvioDir = DateTime.Now; + } + + private static void ResolverComandosDeBorda() + { + foreach (var b in _botoesPressionadosNoTick.Where(x => x == BotoesJoystick.L1 || x == BotoesJoystick.R1 || x == BotoesJoystick.R3 || x == BotoesJoystick.Bolinha)) + ProcessarDadosControle(b, false, ForcarComando: false); + if (_botoesSoltosNoTick.Contains(BotoesJoystick.Bolinha)) + ProcessarDadosControle(BotoesJoystick.Bolinha, true, ForcarComando: false); } public static void ProcessarDadosControle(BotoesJoystick botao, bool solto, Keys? key = null, double? valorJoy = null, double? valorDesejado = null, string ID = "", bool ForcarComando = false) @@ -590,14 +788,18 @@ namespace AgroBase { var op = Variaveis.OperacaoEmAndamento; - if (op?.Controle == null) + if (op?.Controle == null || op.Parametros?.Controle == null || op.Sensoriamento?.Operacao == null || Comando == null) return; - bool bloqueioAutomatico = - (op.Parametros.Controle.MovimentoAutomatico && Comando.Dispositivo == T_Code.Mov) || - (op.Parametros.Controle.DirecionalAutomatico && Comando.Dispositivo == T_Code.Dir); + bool solicitacaoAtiva = Comando.ValorAplicar != 0 || Comando.EstadoAplicar || + Comando.direcao == Direcao.Referenciar; - bool bloqueioCalibragem = + bool bloqueioAutomatico = + !ForcarComando && + ((op.Parametros.Controle.MovimentoAutomatico && Comando.Dispositivo == T_Code.Mov) || + (op.Parametros.Controle.DirecionalAutomatico && Comando.Dispositivo == T_Code.Dir)); + + bool bloqueioCalibragem = op.CalibrandoRuntime; bool bloqueioOperacional = @@ -607,7 +809,7 @@ namespace AgroBase bool comandoParada = Comando.ValorAplicar == 0; - bool bloqueioSonar = !VerificaComandoValidoSonar(Comando.key); + bool bloqueioSonar = !comandoParada && !VerificaComandoValidoSonar(Comando.key); bool deveInterromper = bloqueioAutomatico || @@ -619,6 +821,10 @@ namespace AgroBase if (deveInterromper) { + // MotivosManual descreve somente uma tentativa ativa barrada. + // Solturas, neutralizações e comandos de parada não sobrescrevem o diagnóstico. + if (!solicitacaoAtiva) return; + _Controle.MotivosManual.Clear(); if (bloqueioAutomatico) @@ -642,6 +848,10 @@ namespace AgroBase return; } + // Uma solicitação ativa aceita invalida imediatamente qualquer bloqueio antigo. + if (solicitacaoAtiva) + _Controle.MotivosManual.Clear(); + if (Comando.direcao.HasValue && _Controle.TiposControle.Any(x => x.Tipo == Comando.Dispositivo)) _Controle.TiposControle.FirstOrDefault(x => x.Tipo == Comando.Dispositivo).DirecaoAtual = Comando.direcao.Value; @@ -677,6 +887,15 @@ namespace AgroBase AtualizaConsole(_Controle.TiposControle.FirstOrDefault(x => x.Tipo == Comando.Dispositivo).DirecaoAtual.ToString()); } + private static void RegistrarMotivoManual(string motivo) + { + var motivos = Variaveis.OperacaoEmAndamento?.Controle?.MotivosManual; + if (motivos == null) return; + motivos.Clear(); + motivos.Add(motivo); + Variaveis.MostrarLog($"[GeneralJoystick] Comando manual não aplicado. Motivo: {motivo}"); + } + public static void EnviarComandoAtuadorAtualizado(DeParaComandos Comando, bool ForcarComando = false) { var op = Variaveis.OperacaoEmAndamento; @@ -985,7 +1204,7 @@ namespace AgroBase case BotoesJoystick.Touchpad: estadoSaida = !solto; break; - + } if (solto && PararAoSoltar) @@ -993,7 +1212,7 @@ namespace AgroBase ValorAplicar = valorSaida; EstadoAplicar = estadoSaida; } - + public DeParaComandos Clone() { return new DeParaComandos() @@ -1003,6 +1222,7 @@ namespace AgroBase Dispositivo = Dispositivo, PararAoSoltar = PararAoSoltar, IDcontrolar = IDcontrolar, + Comando = Comando, direcao = direcao, ValorAplicar = ValorAplicar, EstadoAplicar = EstadoAplicar, diff --git a/AgroBase/AgroBase/Models/Operadores/CameraWorkerModel.cs b/AgroBase/AgroBase/Models/Operadores/CameraWorkerModel.cs index 67d534566..cf0013fff 100644 --- a/AgroBase/AgroBase/Models/Operadores/CameraWorkerModel.cs +++ b/AgroBase/AgroBase/Models/Operadores/CameraWorkerModel.cs @@ -56,8 +56,8 @@ namespace AgroBase.Models.Operadores public double fps { get; set; } public int stream_w { get; set; } public int stream_h { get; set; } - public double stream_kbps { get; set; } - public double stream_last_frame { get; set; } + public double? stream_kbps { get; set; } + public double? stream_last_frame { get; set; } public SaudeWorkerModel saude { get; set; } public CameraWorkerItemPerformanceModel performance { get; set; } public OAKImuDataModel imu { get; set; } diff --git a/AgroBase/AgroBase/Models/Variaveis.cs b/AgroBase/AgroBase/Models/Variaveis.cs index a54a7ee5e..dc27d26f8 100644 --- a/AgroBase/AgroBase/Models/Variaveis.cs +++ b/AgroBase/AgroBase/Models/Variaveis.cs @@ -54,11 +54,11 @@ namespace AgroBase.Models return version.ToString(); } } - public static string CaminhoLogsDispositivos { get; } = ResolverCaminhoLocal("Logs"); - public static string CaminhoOperacoes { get; } = ResolverCaminhoLocal("Operacoes"); - public static string CaminhoOperacoesSalvas { get; } = ResolverCaminhoLocal("OperacoesSalvas"); - public static string CaminhoMapasConvertidos { get; } = ResolverCaminhoLocal("Mapas"); - public static string CaminhoParametros { get; } = ResolverCaminhoLocal("Parametros"); + public static string CaminhoLogsDispositivos { get; } = ResolverCaminhoLocal("Logs/"); + public static string CaminhoOperacoes { get; } = ResolverCaminhoLocal("Operacoes/"); + public static string CaminhoOperacoesSalvas { get; } = ResolverCaminhoLocal("OperacoesSalvas/"); + public static string CaminhoMapasConvertidos { get; } = ResolverCaminhoLocal("Mapas/"); + public static string CaminhoParametros { get; } = ResolverCaminhoLocal("Parametros/"); public static string CaminhoModelos { get; } = ResolverCaminhoLocal(@"C:\AgroBaseModels"); private static string ResolverCaminhoLocal(string caminho) { @@ -821,8 +821,8 @@ namespace AgroBase.Models public static string TopicoMqttComandos { get; } = $"agrobot/v1/rover//cmd"; public static string TopicoMqttTelemetria { get; } = $"agrobot/v1/rover//telemetry"; public static string TopicoMqttParametros { get; } = $"agrobot/v1/rover//parameters"; - public static double LarguraEsquerda { get; } = 44.0; // 62 - public static double LarguraDireita { get; } = 44.0; // 22 + public static double LarguraEsquerda { get; } = 53.0; // 62 + public static double LarguraDireita { get; } = 53.0; // 22 public static double ComprimentoFrente { get; } = 90.0; // 7 public static double ComprimentoTras { get; } = 40.0; // 107 public static double DistanciaEntreEixosCm { get; set; } = 92.0; diff --git a/AgroBase/AgroBase/Services/CANService.cs b/AgroBase/AgroBase/Services/CANService.cs index 4ce75c3c6..0bda7889d 100644 --- a/AgroBase/AgroBase/Services/CANService.cs +++ b/AgroBase/AgroBase/Services/CANService.cs @@ -323,6 +323,7 @@ namespace AgroBase.Services 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); void Fechar(); + bool PossuiMensagemPendente(T_Code dispositivo); string _portName { get; set; } diff --git a/AgroBase/AgroBase/Services/CANServiceSerial.cs b/AgroBase/AgroBase/Services/CANServiceSerial.cs index 3c499cac0..9a7827d4f 100644 --- a/AgroBase/AgroBase/Services/CANServiceSerial.cs +++ b/AgroBase/AgroBase/Services/CANServiceSerial.cs @@ -90,6 +90,16 @@ namespace AgroBase.Services } } + public bool PossuiMensagemPendente(T_Code dispositivo) + { + lock (_LockMensagens) + { + return MensagensPendentes.Any(x => + x.Dispositivo == dispositivo && + !x.Respondido); + } + } + public long UltimoTxUnixMs { get; private set; } = 0; public long UltimoRxUnixMs { get; private set; } = 0; public long UltimoErroCriticoUnixMs { get; private set; } = 0; diff --git a/AgroBase/AgroBase/Services/CANServiceWaveshare.cs b/AgroBase/AgroBase/Services/CANServiceWaveshare.cs index cd45ef8f1..9269371cf 100644 --- a/AgroBase/AgroBase/Services/CANServiceWaveshare.cs +++ b/AgroBase/AgroBase/Services/CANServiceWaveshare.cs @@ -86,6 +86,16 @@ namespace AgroBase.Services } } + public bool PossuiMensagemPendente(T_Code dispositivo) + { + lock (_LockMensagens) + { + return MensagensPendentes.Any(x => + x.Dispositivo == dispositivo && + !x.Respondido); + } + } + public long UltimoTxUnixMs { get; private set; } = 0; public long UltimoRxUnixMs { get; private set; } = 0; public long UltimoErroCriticoUnixMs { get; private set; } = 0; diff --git a/AgroBase/AgroBase/Services/CanServiceEthernet.cs b/AgroBase/AgroBase/Services/CanServiceEthernet.cs index c8f0d6588..f097ca4bb 100644 --- a/AgroBase/AgroBase/Services/CanServiceEthernet.cs +++ b/AgroBase/AgroBase/Services/CanServiceEthernet.cs @@ -90,14 +90,9 @@ namespace AgroBase.Services private readonly ConcurrentDictionary _timeoutsRespostaPorDispositivo = new ConcurrentDictionary(); private readonly ConcurrentDictionary _timeoutsRespostaPorChave = new ConcurrentDictionary(); - public Dictionary TimeoutsRespostaPorId => - _timeoutsRespostaPorId.ToDictionary(x => x.Key, x => x.Value); - - public Dictionary TimeoutsRespostaPorDispositivo => - _timeoutsRespostaPorDispositivo.ToDictionary(x => x.Key, x => x.Value); - - public Dictionary TimeoutsRespostaPorChave => - _timeoutsRespostaPorChave.ToDictionary(x => x.Key, x => x.Value); + public Dictionary TimeoutsRespostaPorId => _timeoutsRespostaPorId.ToDictionary(x => x.Key, x => x.Value); + public Dictionary TimeoutsRespostaPorDispositivo => _timeoutsRespostaPorDispositivo.ToDictionary(x => x.Key, x => x.Value); + public Dictionary TimeoutsRespostaPorChave => _timeoutsRespostaPorChave.ToDictionary(x => x.Key, x => x.Value); public Dictionary PendentesAguardandoRespostaPorId { @@ -113,6 +108,16 @@ namespace AgroBase.Services } } + public bool PossuiMensagemPendente(T_Code dispositivo) + { + lock (_LockMensagens) + { + return MensagensPendentes.Any(x => + x.Dispositivo == dispositivo && + !x.Respondido); + } + } + public long UltimoTxUnixMs { get; private set; } = 0; public long UltimoRxUnixMs { get; private set; } = 0; public long UltimoErroCriticoUnixMs { get; private set; } = 0; @@ -139,32 +144,17 @@ namespace AgroBase.Services private DateTime _inicioJanelaLatencia = DateTime.UtcNow; private double _latenciaMaxJanelaMs = 0; - private readonly ConcurrentDictionary _cooldownPorCanal = - new ConcurrentDictionary(); + private readonly ConcurrentDictionary _cooldownPorCanal = new ConcurrentDictionary(); - private readonly ConcurrentDictionary _ultimaFilaPorChave = - new ConcurrentDictionary(); + private readonly ConcurrentDictionary _ultimaFilaPorChave = new ConcurrentDictionary(); - private static readonly TimeSpan COOLDOWN_DEFAULT = - TimeSpan.FromMilliseconds(0); - - private static readonly TimeSpan COOLDOWN_MKS = - TimeSpan.FromMilliseconds(20); - - private static readonly TimeSpan COOLDOWN_OID = - TimeSpan.FromMilliseconds(8); - - private static readonly TimeSpan COOLDOWN_BAT = - TimeSpan.FromMilliseconds(80); - - private static readonly TimeSpan COOLDOWN_ATU_SEN = - TimeSpan.FromMilliseconds(4); - - private static readonly TimeSpan DEDUPE_JANELA_GET = - TimeSpan.FromMilliseconds(250); - - private static readonly TimeSpan DEDUPE_JANELA_SET = - TimeSpan.FromMilliseconds(60); + private static readonly TimeSpan COOLDOWN_DEFAULT = TimeSpan.FromMilliseconds(0); + private static readonly TimeSpan COOLDOWN_MKS = TimeSpan.FromMilliseconds(20); + private static readonly TimeSpan COOLDOWN_OID = TimeSpan.FromMilliseconds(8); + private static readonly TimeSpan COOLDOWN_BAT = TimeSpan.FromMilliseconds(80); + private static readonly TimeSpan COOLDOWN_ATU_SEN = TimeSpan.FromMilliseconds(4); + private static readonly TimeSpan DEDUPE_JANELA_GET = TimeSpan.FromMilliseconds(250); + private static readonly TimeSpan DEDUPE_JANELA_SET = TimeSpan.FromMilliseconds(60); private static long NowUnixMs() { @@ -394,8 +384,7 @@ namespace AgroBase.Services lock (_LockMensagens) { - DateTime agora = - DateTime.Now; + DateTime agora = DateTime.Now; /* * Para request com resposta, não deixa duas pendências iguais @@ -430,15 +419,9 @@ namespace AgroBase.Services * Dedupe temporal extra, útil para comandos de controle que chegam * repetidos por loop rápido. */ - TimeSpan janela = - get - ? DEDUPE_JANELA_GET - : DEDUPE_JANELA_SET; + TimeSpan janela = get ? DEDUPE_JANELA_GET : DEDUPE_JANELA_SET; - if (_ultimaFilaPorChave.TryGetValue( - chaveCorrelacao, - out DateTime ultima) && - agora - ultima < janela) + if (_ultimaFilaPorChave.TryGetValue(chaveCorrelacao, out DateTime ultima) && agora - ultima < janela) { return; } @@ -915,11 +898,7 @@ namespace AgroBase.Services } } - private bool MensagemCorrespondeResposta( - CanMessage pendente, - T_Code dispositivoResposta, - uint idResposta, - byte[] dataResposta) + private bool MensagemCorrespondeResposta(CanMessage pendente, T_Code dispositivoResposta, uint idResposta, byte[] dataResposta) { if (pendente == null) return false; diff --git a/AgroBase/AgroBase/Services/DalyBMSService.cs b/AgroBase/AgroBase/Services/DalyBMSService.cs index 352184d6d..058b30b82 100644 --- a/AgroBase/AgroBase/Services/DalyBMSService.cs +++ b/AgroBase/AgroBase/Services/DalyBMSService.cs @@ -71,6 +71,9 @@ namespace AgroBase.Services private static DateTime _ultimoEnvioCan = DateTime.MinValue; + private static readonly TimeSpan RespiroAposResposta = TimeSpan.FromMilliseconds(100); + private static DateTime _ultimaRespostaCan = DateTime.MinValue; + /* * Mantém prioridade determinística. * O método EscolherProximoDadoParaRequisitar() varre essa lista e @@ -149,6 +152,21 @@ namespace AgroBase.Services } public static void AtualizarDados() + { + lock (_sync) + { + try + { + AtualizarDadosInterno(); + } + catch (Exception ex) + { + Variaveis.MostrarLog("[DalyBMSService.AtualizarDados] Erro: " + ex.Message); + } + } + } + + private static void AtualizarDadosInterno() { try { @@ -195,6 +213,12 @@ namespace AgroBase.Services return; } + if (CanManager.CanService.PossuiMensagemPendente(T_Code.Bat)) + return; + + if (DateTime.UtcNow - _ultimaRespostaCan < RespiroAposResposta) + return; + BatFuncCode? proximo = EscolherProximoDadoParaRequisitar(agora); if (!proximo.HasValue) @@ -310,27 +334,18 @@ namespace AgroBase.Services ); } - private static void EnviarComando( - T_Code dispositivo, - byte[] idTx, - byte[] idRx, - byte funcCode, - byte[] payload = null, - bool contabilizarTimeoutHealth = true) + private static void EnviarComando(T_Code dispositivo, byte[] idTx, byte[] idRx, byte funcCode, byte[] payload = null, bool contabilizarTimeoutHealth = true) { - CanManager - .CanService - .AdicionarMensagemNaFila( - dispositivo, - idTx, - idRx, - funcCode, - funcCode, - payload ?? new byte[8], - get: true, - contabilizarTimeoutHealth: - contabilizarTimeoutHealth - ); + CanManager.CanService.AdicionarMensagemNaFila( + dispositivo, + idTx, + idRx, + funcCode, + funcCode, + payload ?? new byte[8], + get: true, + contabilizarTimeoutHealth: contabilizarTimeoutHealth + ); } private static byte[] MakeRequestID(BatFuncCode dado) @@ -367,9 +382,7 @@ namespace AgroBase.Services }; } - private static void RequisitarDado( - BatFuncCode dado, - bool contabilizarTimeoutHealth) + private static void RequisitarDado(BatFuncCode dado, bool contabilizarTimeoutHealth) { EnviarComando( Dispositivo, @@ -377,8 +390,7 @@ namespace AgroBase.Services MakeResponseID(dado), (byte)dado, payload: new byte[8], - contabilizarTimeoutHealth: - contabilizarTimeoutHealth + contabilizarTimeoutHealth: contabilizarTimeoutHealth ); } @@ -419,33 +431,25 @@ namespace AgroBase.Services if (mensagem == null) return; - if (mensagem.DataRx == null || - mensagem.DataRx.Length < 8) + if (mensagem.DataRx == null || mensagem.DataRx.Length < 8) { return; } - uint canId = - mensagem.IdRx; + uint canId = mensagem.IdRx; - BatFuncCode dataId = - GetDataId(canId); + BatFuncCode dataId = GetDataId(canId); - byte[] data = - mensagem - .DataRx - .Take(8) - .ToArray(); + byte[] data = mensagem.DataRx.Take(8).ToArray(); lock (DadosLeitura) { DadosLeitura.Momento = DateTime.Now; DadosLeitura.Iniciado = true; - DadosLeitura.UltimoComandoRecebido = - DateTime.Now; + DadosLeitura.UltimoComandoRecebido = DateTime.Now; + _ultimaRespostaCan = DateTime.UtcNow; - DadosLeitura.LeiturasBrutas[(byte)dataId] = - data.ToArray(); + DadosLeitura.LeiturasBrutas[(byte)dataId] = data.ToArray(); switch (dataId) { @@ -881,5 +885,6 @@ namespace AgroBase.Services d.Take(7).ToArray(); } } + } } diff --git a/AgroBase/AgroBase/Services/Operadores/OperadoresService.cs b/AgroBase/AgroBase/Services/Operadores/OperadoresService.cs index aa710d88e..1b683eba3 100644 --- a/AgroBase/AgroBase/Services/Operadores/OperadoresService.cs +++ b/AgroBase/AgroBase/Services/Operadores/OperadoresService.cs @@ -415,15 +415,27 @@ namespace AgroBase.Services.Operadores Dictionary statusWorkers = AtualizarSaudeWorkers(); bool existemWorkers = statusWorkers.Count > 0; - bool todosWorkersOk = existemWorkers && statusWorkers.All(x => x.Value); + bool todosWorkersOk = + existemWorkers && + statusWorkers.All(x => x.Value); + + bool workersCriticosOk = statusWorkers + .Where(x => x.Key != nameof(WeedWorkerService) && x.Key != nameof(VisualWorkerService)) + .Any() + && + statusWorkers + .Where(x => x.Key != nameof(WeedWorkerService) && x.Key != nameof(VisualWorkerService)) + .All(x => x.Value); bool pythonOk = !iniciarScript || PythonRodando; - - bool nucleoCriticoOk = pythonOk && todosWorkersOk; + bool nucleoCriticoOk = pythonOk && workersCriticosOk; lock (_estadoLock) { + // Mantém a informação real: há um worker degradado. TodosWorkersConectados = todosWorkersOk; + + // Mas o núcleo continua vivo e não mata os demais. Conectado = nucleoCriticoOk; } diff --git a/AgroBase/AgroBase/Services/VersionamentoService.cs b/AgroBase/AgroBase/Services/VersionamentoService.cs index b2b188a49..477f8353b 100644 --- a/AgroBase/AgroBase/Services/VersionamentoService.cs +++ b/AgroBase/AgroBase/Services/VersionamentoService.cs @@ -49,7 +49,7 @@ namespace AgroBase.Services { return ArquivosPorTipo( TipoArquivoVersionado.ModeloIA_StreetDetector - ).FirstOrDefault(); + ).Skip(1).FirstOrDefault(); } } } @@ -62,7 +62,7 @@ namespace AgroBase.Services { return ArquivosPorTipo( TipoArquivoVersionado.ModeloIA_StreetDetector - ).Skip(1).FirstOrDefault(); + ).Skip(2).FirstOrDefault(); } } } @@ -75,7 +75,7 @@ namespace AgroBase.Services { return ArquivosPorTipo( TipoArquivoVersionado.ModeloIA_StreetDetector - ).Skip(2).FirstOrDefault(); + ).Skip(3).FirstOrDefault(); } } } @@ -88,7 +88,7 @@ namespace AgroBase.Services { return ArquivosPorTipo( TipoArquivoVersionado.ModeloIA_StreetDetector - ).Skip(3).FirstOrDefault(); + ).Skip(0).FirstOrDefault(); } } } diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_imu.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_imu.py index 820599cd3..92371b48b 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_imu.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_imu.py @@ -45,6 +45,7 @@ class IMUCamera(ModuloDiagnosticoBase): gyro_unidade="deg_s", raw_acc_alpha=0.04, raw_max_acc_erro_g=0.18, + referencial_robo="r,p,y", ): self.mx_id = mx_id self.imu_queue = queue @@ -132,6 +133,9 @@ class IMUCamera(ModuloDiagnosticoBase): self._calib_roll_hist = deque(maxlen=200) self._calib_pitch_hist = deque(maxlen=200) + self.referencial_robo = str(referencial_robo or "r,p,y").lower().replace(" ", "") + self.matriz_sensor_para_robo = (self._criar_matriz_sensor_para_robo(self.referencial_robo)) + if queue is None: self.mostrar_log("Inicializado sem queue. Classe ficará inativa.") return @@ -145,7 +149,9 @@ class IMUCamera(ModuloDiagnosticoBase): self.imu_thread.start() self.mostrar_log( - f"Task iniciada | modo={self.modo} | freq alvo={self.freq:.0f}Hz | " + f"Task iniciada | modo={self.modo} | " + f"referencial={self.referencial_robo} | " + f"freq alvo={self.freq:.0f}Hz | " f"publish={1.0/self.publish_period:.0f}Hz" ) @@ -166,6 +172,8 @@ class IMUCamera(ModuloDiagnosticoBase): pass def iniciar_calibracao(self): + self._resetar_estado_angular() + self.calibrado = False self.calibrando = True self.calib_inicio_ts = time.perf_counter() @@ -336,6 +344,10 @@ class IMUCamera(ModuloDiagnosticoBase): if self.gyro_unidade in ["rad_s", "rad/s", "rads"]: g = np.rad2deg(g) + # Normaliza acelerômetro e giroscópio para o referencial do rover. + a = self._vetor_sensor_para_robo(a) + g = self._vetor_sensor_para_robo(g) + sensor_ts = None try: sensor_ts = packet.timestamp.get() @@ -381,14 +393,19 @@ class IMUCamera(ModuloDiagnosticoBase): q_delta = q_ref_inv * q_atual """ - r_atual = R.from_quat(q_xyzw) + r_atual_sensor = R.from_quat(q_xyzw) if self.calibrado and self.q_ref_inv is not None: - r_delta = self.q_ref_inv * r_atual + r_delta_sensor = self.q_ref_inv * r_atual_sensor else: - r_delta = r_atual + r_delta_sensor = r_atual_sensor - roll, pitch, yaw = r_delta.as_euler("xyz", degrees=True) + r_delta_robo = self._rotacao_sensor_para_robo(r_delta_sensor) + + roll, pitch, yaw = r_delta_robo.as_euler( + "xyz", + degrees=True, + ) return float(roll), float(pitch), float(yaw) @@ -400,11 +417,22 @@ class IMUCamera(ModuloDiagnosticoBase): self._last_yaw_deg = yaw return - dt = max(float(ts - self._last_angle_ts), 1e-4) + dt = float(ts - self._last_angle_ts) - self.roll_rate_dps = self._wrap_angle_delta_deg(roll, self._last_roll_deg) / dt - self.pitch_rate_dps = self._wrap_angle_delta_deg(pitch, self._last_pitch_deg) / dt - self.yaw_rate_dps = self._wrap_angle_delta_deg(yaw, self._last_yaw_deg) / dt + if dt <= 0.0 or dt > 0.2: + self._last_angle_ts = ts + self._last_roll_deg = roll + self._last_pitch_deg = pitch + self._last_yaw_deg = yaw + return + + self.roll_rate_dps = (self._wrap_angle_delta_deg(roll, self._last_roll_deg) / dt) + self.pitch_rate_dps = (self._wrap_angle_delta_deg(pitch, self._last_pitch_deg) / dt) + self.yaw_rate_dps = (self._wrap_angle_delta_deg(yaw, self._last_yaw_deg) / dt) + + self.roll_rate_dps = float(np.clip(self.roll_rate_dps, -500.0, 500.0)) + self.pitch_rate_dps = float(np.clip(self.pitch_rate_dps, -500.0, 500.0)) + self.yaw_rate_dps = float(np.clip(self.yaw_rate_dps, -500.0, 500.0)) self._last_angle_ts = ts self._last_roll_deg = roll @@ -428,6 +456,118 @@ class IMUCamera(ModuloDiagnosticoBase): self.yaw_filtrado_deg = self.yaw_filtrado_deg + a * dyaw self.yaw_filtrado_deg = (self.yaw_filtrado_deg + 180.0) % 360.0 - 180.0 + def _sensor_ts_para_segundos(self, sensor_ts): + if sensor_ts is None: + return None + + if hasattr(sensor_ts, "total_seconds"): + return float(sensor_ts.total_seconds()) + + try: + return float(sensor_ts) + except (TypeError, ValueError): + return None + + def _criar_matriz_sensor_para_robo(self, configuracao): + """ + Define como os eixos do sensor correspondem aos eixos do rover. + + Convenção: + r = roll = eixo X + p = pitch = eixo Y + y = yaw = eixo Z + + Exemplos: + "r,p,y" -> mesma orientação + "-r,p,-y" -> inverte roll e yaw + "p,r,-y" -> troca roll/pitch e inverte yaw + + Cada posição representa a saída no referencial do rover: + + posição 0 -> roll/X do rover + posição 1 -> pitch/Y do rover + posição 2 -> yaw/Z do rover + """ + texto = str(configuracao or "r,p,y").lower().replace(" ", "") + tokens = texto.split(",") + + if len(tokens) != 3: + raise ValueError( + f"Referencial IMU inválido: '{configuracao}'. " + f"Use, por exemplo: 'r,p,y', '-r,p,-y' ou 'p,r,-y'." + ) + + indices = { + "r": 0, + "roll": 0, + "x": 0, + + "p": 1, + "pitch": 1, + + "y": 2, + "yaw": 2, + "z": 2, + } + + matriz = np.zeros((3, 3), dtype=np.float64) + eixos_usados = [] + + for eixo_robo, token in enumerate(tokens): + sinal = -1.0 if token.startswith("-") else 1.0 + nome = token.lstrip("+-") + + if nome not in indices: + raise ValueError( + f"Eixo IMU inválido: '{token}' em '{configuracao}'." + ) + + eixo_sensor = indices[nome] + + if eixo_sensor in eixos_usados: + raise ValueError( + f"Eixo repetido em referencial IMU: '{configuracao}'." + ) + + eixos_usados.append(eixo_sensor) + matriz[eixo_robo, eixo_sensor] = sinal + + # Uma orientação física deve manter um sistema destro. + determinante = float(np.linalg.det(matriz)) + + if not np.isclose(determinante, 1.0, atol=1e-6): + raise ValueError( + f"Referencial IMU '{configuracao}' resulta em sistema invertido " + f"(det={determinante:.0f}). Ao trocar dois eixos, normalmente é " + f"necessário inverter também um deles. Exemplo: 'p,r,-y'." + ) + + return matriz + + def _vetor_sensor_para_robo(self, vetor): + vetor = np.asarray(vetor, dtype=np.float64) + + if vetor.shape != (3,): + raise ValueError( + f"Vetor IMU deve possuir shape (3,), recebido {vetor.shape}." + ) + + return self.matriz_sensor_para_robo @ vetor + + def _rotacao_sensor_para_robo(self, rotacao_sensor): + """ + Expressa uma rotação medida no referencial do sensor + usando os eixos configurados do rover. + + R_robo = M * R_sensor * M^T + """ + matriz_sensor = rotacao_sensor.as_matrix() + m = self.matriz_sensor_para_robo + + matriz_robo = m @ matriz_sensor @ m.T + + return R.from_matrix(matriz_robo) + # ========================================================== # Calibração # ========================================================== @@ -475,6 +615,8 @@ class IMUCamera(ModuloDiagnosticoBase): f"std_roll={roll_std:.3f}° | std_pitch={pitch_std:.3f}°" ) + self._resetar_estado_angular() + def _processar_calibracao_raw(self, a, gyro_dps): if not self.calibrando: return @@ -564,6 +706,24 @@ class IMUCamera(ModuloDiagnosticoBase): f"gyro_std_medio={gyro_std:.4f}°/s" ) + def _resetar_estado_angular(self): + self.roll_deg = 0.0 + self.pitch_deg = 0.0 + self.yaw_deg = 0.0 + + self.roll_filtrado_deg = 0.0 + self.pitch_filtrado_deg = 0.0 + self.yaw_filtrado_deg = 0.0 + + self.roll_rate_dps = 0.0 + self.pitch_rate_dps = 0.0 + self.yaw_rate_dps = 0.0 + + self._last_roll_deg = None + self._last_pitch_deg = None + self._last_yaw_deg = None + self._last_angle_ts = None + # ========================================================== # Consumo da queue # ========================================================== @@ -616,7 +776,13 @@ class IMUCamera(ModuloDiagnosticoBase): self.pitch_deg = pitch self.yaw_deg = yaw - self._atualizar_rates(roll, pitch, yaw, agora) + sensor_ts_s = self._sensor_ts_para_segundos(sensor_ts) + if sensor_ts_s is not None: + ts_rate = sensor_ts_s + else: + ts_rate = time.perf_counter() + + self._atualizar_rates(roll, pitch, yaw, ts_rate) self._atualizar_filtro_leve(roll, pitch, yaw) self.last_imu_ts = sensor_ts @@ -624,6 +790,7 @@ class IMUCamera(ModuloDiagnosticoBase): self.last_data = { "valido": bool(self.calibrado and not self.calibrando), "tem_leitura": True, + "referencial_robo": self.referencial_robo, "mx_id": self.mx_id, "sensor": self.nome_sensor, @@ -692,11 +859,18 @@ class IMUCamera(ModuloDiagnosticoBase): roll_acc_abs, pitch_acc_abs = self._accel_para_roll_pitch_deg(a) + sensor_ts_s = self._sensor_ts_para_segundos(sensor_ts) + ts_integracao = sensor_ts_s if sensor_ts_s is not None else agora + if self._last_angle_ts is None: dt = 1.0 / max(self.freq, 1.0) else: - dt = max(agora - self._last_angle_ts, 1e-4) - dt = min(dt, 0.05) + dt = ts_integracao - self._last_angle_ts + + if dt <= 0.0 or dt > 0.2: + dt = 1.0 / max(self.freq, 1.0) + + dt = min(max(dt, 1e-4), 0.05) if not self.raw_inicializado: self.raw_roll_abs_deg = roll_acc_abs @@ -742,7 +916,7 @@ class IMUCamera(ModuloDiagnosticoBase): self.pitch_rate_dps = gy self.yaw_rate_dps = gz - self._last_angle_ts = agora + self._last_angle_ts = ts_integracao self._last_roll_deg = roll self._last_pitch_deg = pitch self._last_yaw_deg = yaw @@ -757,6 +931,7 @@ class IMUCamera(ModuloDiagnosticoBase): self.last_data = { "valido": bool(self.calibrado and not self.calibrando), "tem_leitura": True, + "referencial_robo": self.referencial_robo, "mx_id": self.mx_id, "sensor": self.nome_sensor, @@ -989,6 +1164,8 @@ class IMUCamera(ModuloDiagnosticoBase): return { "valido": False, "mx_id": self.mx_id, + "referencial_robo": self.referencial_robo, + "sensor": self.nome_sensor, "tipo": tipo, "modo": self.modo, @@ -1030,7 +1207,21 @@ class IMUCamera(ModuloDiagnosticoBase): "gyro_bias_dps": [0.0, 0.0, 0.0], } - return dict(self.last_data) + data = dict(self.last_data) + + agora = time.time() + last_packet_ts = float(data.get("last_packet_ts", 0.0) or 0.0) + packet_age_ms = ((agora - last_packet_ts) * 1000.0 if last_packet_ts > 0.0 else float("inf")) + + data["packet_age_ms"] = round(packet_age_ms, 1) + data["valido"] = bool( + self.ativo + and self.calibrado + and not self.calibrando + and packet_age_ms <= 500.0 + ) + + return data def mostrar_log(self, mensagem): print(f"{time.time()} - [IMUCamera][mx_id={self.mx_id}][id={id(self)}] {mensagem}") diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py index fdb37321e..776a15162 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py @@ -177,7 +177,9 @@ class CameraMultispectral: filtro_alpha=0.35, max_queue_drain=20, nome_sensor=f"{self.modelo}_imu", - modo=self.imu_modo + modo=self.imu_modo, + gyro_unidade="deg_s", + referencial_robo="p,-y,-r", ) else: self.tem_imu = False diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py index 7e98f5477..58be478c4 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py @@ -1,3 +1,6 @@ +import hashlib +from pathlib import Path + import numpy as np import depthai as dai import time @@ -133,7 +136,9 @@ class CameraOak: filtro_alpha=0.35, max_queue_drain=20, nome_sensor=f"{self.modelo}_imu", - modo=self.imu_modo + modo=self.imu_modo, + gyro_unidade="rad_s", + referencial_robo="r,p,y", ) if self.modelo_ia_seg is not None: @@ -887,60 +892,86 @@ class CameraOak: y2 = 1.0 - ROI_INICIO if blob_path: - manip_det = pipeline.createImageManip() - manip_det.initialConfig.setCropRect(0.0, y1, 1.0, y2) - manip_det.initialConfig.setResize(RESOLUCAO[0], RESOLUCAO[1]) - manip_det.initialConfig.setKeepAspectRatio(True) - manip_det.initialConfig.setFrameType(dai.RawImgFrame.Type.RGB888p) + try: + dai.OpenVINO.Blob(blob_path) - det = pipeline.createMobileNetDetectionNetwork() - #det = pipeline.createMobileNetSpatialDetectionNetwork() - det.setBlobPath(blob_path) - det.setConfidenceThreshold(CONF) - det.setNumInferenceThreads(2) - det.input.setBlocking(False) - det.input.setQueueSize(2) - manip_det.out.link(det.input) + manip_det = pipeline.createImageManip() + manip_det.initialConfig.setCropRect(0.0, y1, 1.0, y2) + manip_det.initialConfig.setResize(RESOLUCAO[0], RESOLUCAO[1]) + manip_det.initialConfig.setKeepAspectRatio(True) + manip_det.initialConfig.setFrameType(dai.RawImgFrame.Type.RGB888p) - xout_det = pipeline.createXLinkOut() - xout_det.setStreamName("det") - det.out.link(xout_det.input) - #stereo.depth.link(det.inputDepth) + det = pipeline.createMobileNetDetectionNetwork() + #det = pipeline.createMobileNetSpatialDetectionNetwork() + + blob_resolvido = Path(blob_path).resolve() - # Tracker oficial da OAK, sem alterar a saída "det" - if TRACK: - manip_track = pipeline.createImageManip() - manip_track.initialConfig.setCropRect(0.0, y1, 1.0, y2) - manip_track.initialConfig.setResize(RESOLUCAO[0], RESOLUCAO[1]) - manip_track.initialConfig.setKeepAspectRatio(True) - manip_track.initialConfig.setFrameType(dai.RawImgFrame.Type.BGR888p) + self.mostrar_log( + "[BLOB_DEBUG] " + f"original={blob_path!r}; " + f"resolvido={str(blob_resolvido)!r}; " + f"existe={blob_resolvido.is_file()}; " + f"tamanho={blob_resolvido.stat().st_size if blob_resolvido.is_file() else None}" + ) + + if blob_resolvido.is_file(): + sha256 = hashlib.sha256(blob_resolvido.read_bytes()).hexdigest() + self.mostrar_log(f"[BLOB_DEBUG] sha256={sha256}") + + det.setBlobPath(str(blob_resolvido)) - tracker = pipeline.create(dai.node.ObjectTracker) - tracker.setTrackerType(dai.TrackerType.ZERO_TERM_COLOR_HISTOGRAM) - tracker.setTrackerIdAssignmentPolicy(dai.TrackerIdAssignmentPolicy.SMALLEST_ID) + det.setConfidenceThreshold(CONF) + det.setNumInferenceThreads(2) + det.input.setBlocking(False) + det.input.setQueueSize(2) + manip_det.out.link(det.input) + + xout_det = pipeline.createXLinkOut() + xout_det.setStreamName("det") + det.out.link(xout_det.input) + #stereo.depth.link(det.inputDepth) + + # Tracker oficial da OAK, sem alterar a saída "det" + if TRACK: + manip_track = pipeline.createImageManip() + manip_track.initialConfig.setCropRect(0.0, y1, 1.0, y2) + manip_track.initialConfig.setResize(RESOLUCAO[0], RESOLUCAO[1]) + manip_track.initialConfig.setKeepAspectRatio(True) + manip_track.initialConfig.setFrameType(dai.RawImgFrame.Type.BGR888p) + + tracker = pipeline.create(dai.node.ObjectTracker) + tracker.setTrackerType(dai.TrackerType.ZERO_TERM_COLOR_HISTOGRAM) + tracker.setTrackerIdAssignmentPolicy(dai.TrackerIdAssignmentPolicy.SMALLEST_ID) + + # Mesmo frame usado pela detecção, mas convertido para BGR aceito pelo tracker + script.outputs['toDet'].link(manip_track.inputImage) + + manip_track.out.link(tracker.inputTrackerFrame) + manip_track.out.link(tracker.inputDetectionFrame) + + # As detecções reais continuam vindo do modelo + det.out.link(tracker.inputDetections) + + xout_track = pipeline.createXLinkOut() + xout_track.setStreamName("det_track") + tracker.out.link(xout_track.input) + + script.outputs['toDet'].link(manip_det.inputImage) + #cam.video.link(manip_det.inputImage) + + # (opcional) passthrough para sincronizar timestamp/frame com a detecção + # xout_det_img = pipeline.createXLinkOut() + # xout_det_img.setStreamName("det_img") + # det.passthrough.link(xout_det_img.input) + + self.mostrar_log("Pipeline de detecção leve (MobileNet-SSD) criado") - # Mesmo frame usado pela detecção, mas convertido para BGR aceito pelo tracker - script.outputs['toDet'].link(manip_track.inputImage) + except Exception as e: + raise RuntimeError( + f"Falha ao carregar detector MobileNet: " + f"path={blob_path!r}; erro={e}" + ) from e - manip_track.out.link(tracker.inputTrackerFrame) - manip_track.out.link(tracker.inputDetectionFrame) - - # As detecções reais continuam vindo do modelo - det.out.link(tracker.inputDetections) - - xout_track = pipeline.createXLinkOut() - xout_track.setStreamName("det_track") - tracker.out.link(xout_track.input) - - script.outputs['toDet'].link(manip_det.inputImage) - #cam.video.link(manip_det.inputImage) - - # (opcional) passthrough para sincronizar timestamp/frame com a detecção - # xout_det_img = pipeline.createXLinkOut() - # xout_det_img.setStreamName("det_img") - # det.passthrough.link(xout_det_img.input) - - self.mostrar_log("Pipeline de detecção leve (MobileNet-SSD) criado") else: self.mostrar_log("[INFO] Detector não configurado (detector_blob_path ausente). Pulando.") except Exception as e: diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/main.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/main.py index eed422331..e6613785c 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/main.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/main.py @@ -30,7 +30,7 @@ def main(): T_Code.Sen: ModuloSensoriamento(), T_Code.Atu: ModuloAtuador(), #T_Code.Lra: ModuloLoRa(), - T_Code.Imu: CameraIMU(modulos_imu=[T_Code.Snr]), + T_Code.Imu: CameraIMU(modulos_imu=[T_Code.Snr, T_Code.Cam]), T_Code.Npc: ModuloPC(), T_Code.Lvx: ModuloLivox(), T_Code.Ipb: ModuloIPBribge(), diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py index 350c2b2a8..9a1a1f0c9 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py @@ -146,6 +146,8 @@ class CameraIMU(ModuloDiagnosticoBase): self.pitch_rate_hist = deque(maxlen=historico_len) self.risk_hist = deque(maxlen=historico_len) + self._assinatura_fontes_anterior = None + self.thread = threading.Thread( target=self.imu_task_loop, name="CameraIMUFinalWorker", @@ -189,10 +191,18 @@ class CameraIMU(ModuloDiagnosticoBase): def _safe_float(self, data, key, default=0.0): try: - v = data.get(key, default) - if v is None: + valor = data.get(key, default) + + if valor is None: return float(default) - return float(v) + + valor = float(valor) + + if not math.isfinite(valor): + return float(default) + + return valor + except Exception: return float(default) @@ -503,6 +513,7 @@ class CameraIMU(ModuloDiagnosticoBase): "modelo": cam_data.get("modelo"), "dispositivo": cam_data.get("dispositivo"), "t_code": codigo_normalizado, + "referencial_robo": imu.get("referencial_robo", cam_data.get("referencial_robo")), "valido": valido, "calibrado": calibrado, @@ -806,13 +817,24 @@ class CameraIMU(ModuloDiagnosticoBase): "divergentes": divergentes, } - def _fundir_fontes(self, fontes): + def _valor_mais_conservador(self, fontes, key): + fonte = max( + fontes, + key=lambda f: abs(float(f.get(key, 0.0))), + ) + + return ( + float(fonte.get(key, 0.0)), + fonte["mx_id"], + ) + + def _fundir_fontes(self, fontes, sensores_agree=True): if not fontes: return None - # Se só uma fonte existe, usa ela diretamente. if len(fontes) == 1: f = fontes[0] + return { "roll": f["roll"], "pitch": f["pitch"], @@ -820,24 +842,78 @@ class CameraIMU(ModuloDiagnosticoBase): "roll_rate_dps": f["roll_rate_dps"], "pitch_rate_dps": f["pitch_rate_dps"], "yaw_rate_dps": f["yaw_rate_dps"], - "confidence": f["confidence"], + "confidence": min(float(f["confidence"]), 1.0), "fonte_principal": f["mx_id"], "single_source": True, + "estrategia_fusao": "single_source", } - # Com duas ou mais, média ponderada por confiança. - fonte_principal = max(fontes, key=lambda x: x.get("confidence", 0.0)) + fonte_principal = max( + fontes, + key=lambda f: f.get("confidence", 0.0), + ) + + if sensores_agree: + return { + "roll": self._weighted_mean(fontes, "roll"), + "pitch": self._weighted_mean(fontes, "pitch"), + "yaw": self._fused_yaw(fontes), + "roll_rate_dps": self._weighted_mean( + fontes, + "roll_rate_dps", + ), + "pitch_rate_dps": self._weighted_mean( + fontes, + "pitch_rate_dps", + ), + "yaw_rate_dps": self._weighted_mean( + fontes, + "yaw_rate_dps", + ), + "confidence": min( + max(float(f["confidence"]) for f in fontes), + 1.0, + ), + "fonte_principal": fonte_principal["mx_id"], + "single_source": False, + "estrategia_fusao": "weighted_agreement", + } + + roll, fonte_roll = self._valor_mais_conservador( + fontes, + "roll", + ) + pitch, fonte_pitch = self._valor_mais_conservador( + fontes, + "pitch", + ) + roll_rate, fonte_roll_rate = self._valor_mais_conservador( + fontes, + "roll_rate_dps", + ) + pitch_rate, fonte_pitch_rate = self._valor_mais_conservador( + fontes, + "pitch_rate_dps", + ) return { - "roll": self._weighted_mean(fontes, "roll"), - "pitch": self._weighted_mean(fontes, "pitch"), - "yaw": self._fused_yaw(fontes), - "roll_rate_dps": self._weighted_mean(fontes, "roll_rate_dps"), - "pitch_rate_dps": self._weighted_mean(fontes, "pitch_rate_dps"), - "yaw_rate_dps": self._weighted_mean(fontes, "yaw_rate_dps"), - "confidence": sum(float(f.get("confidence", 0.0)) for f in fontes), + "roll": roll, + "pitch": pitch, + "yaw": fonte_principal["yaw"], + "roll_rate_dps": roll_rate, + "pitch_rate_dps": pitch_rate, + "yaw_rate_dps": fonte_principal["yaw_rate_dps"], + "confidence": min( + float(fonte_principal["confidence"]), + 1.0, + ), "fonte_principal": fonte_principal["mx_id"], + "fonte_roll": fonte_roll, + "fonte_pitch": fonte_pitch, + "fonte_roll_rate": fonte_roll_rate, + "fonte_pitch_rate": fonte_pitch_rate, "single_source": False, + "estrategia_fusao": "conservative_disagreement", } def _atualizar_filtro_fusao(self, fusao): @@ -1272,6 +1348,11 @@ class CameraIMU(ModuloDiagnosticoBase): "confidence": round(float(fusao.get("confidence", 0.0)), 3), "fonte_principal": fonte_principal, "single_source": bool(fusao.get("single_source", False)), + "estrategia_fusao": fusao.get("estrategia_fusao"), + "fonte_roll": fusao.get("fonte_roll"), + "fonte_pitch": fusao.get("fonte_pitch"), + "fonte_roll_rate": fusao.get("fonte_roll_rate"), + "fonte_pitch_rate": fusao.get("fonte_pitch_rate"), "modulos_imu_configurados": [ self._nome_codigo(modulo) for modulo in self.modulos_imu @@ -1294,6 +1375,7 @@ class CameraIMU(ModuloDiagnosticoBase): { "mx_id": f["mx_id"], "modelo": f.get("modelo"), + "referencial_robo": f.get("referencial_robo"), "t_code": f.get("t_code"), "roll": round(f["roll"], 3), "pitch": round(f["pitch"], 3), @@ -1368,6 +1450,11 @@ class CameraIMU(ModuloDiagnosticoBase): and self._modulos_imu_configurados.issubset(modulos_validos) ) + assinatura_fontes = tuple(sorted(f["mx_id"] for f in fontes)) + if assinatura_fontes != self._assinatura_fontes_anterior: + self._resetar_filtro_fusao() + self._assinatura_fontes_anterior = assinatura_fontes + if len(fontes) <= 0: self._resetar_filtro_fusao() data = self._montar_saida_sem_imu( @@ -1376,9 +1463,10 @@ class CameraIMU(ModuloDiagnosticoBase): agora, ) else: - fusao = self._fundir_fontes(fontes) divergencia = self._calcular_divergencia(fontes) + fusao = self._fundir_fontes(fontes, sensores_agree=divergencia["sensors_agree"]) + # Atualiza o filtro antes de avaliar o risco. atitude_filtrada = self._atualizar_filtro_fusao(fusao) diff --git a/AgroBase/OperationControl/ViewModels/Views/ConfigConexao/ConfigConexaoViewModel.cs b/AgroBase/OperationControl/ViewModels/Views/ConfigConexao/ConfigConexaoViewModel.cs index f70ddef50..aace45abe 100644 --- a/AgroBase/OperationControl/ViewModels/Views/ConfigConexao/ConfigConexaoViewModel.cs +++ b/AgroBase/OperationControl/ViewModels/Views/ConfigConexao/ConfigConexaoViewModel.cs @@ -245,7 +245,7 @@ namespace OperationControl.ViewModels try { var interfaces = EthernetService.ObterNomesInterfaces() - .Where(x => x.Contains(AppShell.Mock ? "Ativa" : "Ethernet")) + .Where(x => x.Contains("AG")) .Select(x => x.Split('(')[0].Trim()) .ToList();