diff --git a/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs b/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs index 5bb24f774..19c325ba7 100644 --- a/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs +++ b/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs @@ -206,7 +206,7 @@ namespace AgroBase.Forms.IHM.Operacao ?? operacaoParametrizando.Mapa?.RuasPercorrer?.Count ?? 0; - if (qtdRuas == 0) + if (qtdRuas < 2) { FuncoesIHM.MostrarAlerta( "Trajetória não parametrizada", @@ -217,12 +217,29 @@ namespace AgroBase.Forms.IHM.Operacao } } - if (operacaoIniciada) - op?.AtualizarParametrosParciais(operacaoParametrizando.Parametros, op); - else - OperacaoModel.CarregarParamerosOperacaoBase(operacaoParametrizando.Parametros); + try + { + if (operacaoIniciada) + { + op?.AtualizarParametrosParciais(operacaoParametrizando.Parametros, op); + } + else + { + OperacaoModel.CarregarParamerosOperacaoBase(operacaoParametrizando.Parametros); + } - ParametrosSalvos?.Invoke(this, EventArgs.Empty); + ParametrosSalvos?.Invoke(this, EventArgs.Empty); + } + catch (InvalidOperationException ex) + { + Variaveis.MostrarLog("[ucOperacaoParametros.BtnSalvar] " + ex); + FuncoesIHM.MostrarAlerta("Não foi possível salvar a operação", ex.Message, MessageBoxIcon.Warning); + } + catch (Exception ex) + { + Variaveis.MostrarLog("[ucOperacaoParametros.BtnSalvar][ERRO] " + ex); + FuncoesIHM.MostrarAlerta("Erro ao salvar parâmetros", "Não foi possível aplicar os parâmetros da operação. Consulte os logs para mais detalhes.", MessageBoxIcon.Error); + } } private async void BtnRedefinir_Click(object sender, EventArgs e) diff --git a/AgroBase/AgroBase/Forms/IHM/frmAjustes.Designer.cs b/AgroBase/AgroBase/Forms/IHM/frmAjustes.Designer.cs index 8c76c2f33..7f4775c55 100644 --- a/AgroBase/AgroBase/Forms/IHM/frmAjustes.Designer.cs +++ b/AgroBase/AgroBase/Forms/IHM/frmAjustes.Designer.cs @@ -61,6 +61,10 @@ namespace AgroBase.Forms.IHM this.fldEntreEixos = new AgroBase.Forms.IHM.Operacao.Parametros.Controls.HmiCompactNumericField(); this.fldAnguloFrontal = new AgroBase.Forms.IHM.Operacao.Parametros.Controls.HmiCompactNumericField(); this.fldAnguloLateral = new AgroBase.Forms.IHM.Operacao.Parametros.Controls.HmiCompactNumericField(); + this.pnlReferenciaImu = new System.Windows.Forms.Panel(); + this.lblOffsetImuRoll = new System.Windows.Forms.Label(); + this.lblOffsetImuPitch = new System.Windows.Forms.Label(); + this.btnReferenciarImu = new AgroBase.Forms.IHM.Controls.HmiSidebarButton(); this.pnlBateria = new AgroBase.Forms.IHM.Controls.HmiRoundedPanel(); this.bateriaLayout = new System.Windows.Forms.TableLayoutPanel(); this.lblBateria = new System.Windows.Forms.Label(); @@ -94,6 +98,7 @@ namespace AgroBase.Forms.IHM this.rightLayout.SuspendLayout(); this.pnlGeometria.SuspendLayout(); this.geometriaLayout.SuspendLayout(); + this.pnlReferenciaImu.SuspendLayout(); this.pnlBateria.SuspendLayout(); this.bateriaLayout.SuspendLayout(); this.pnlReservatorio.SuspendLayout(); @@ -524,6 +529,7 @@ namespace AgroBase.Forms.IHM this.geometriaLayout.Controls.Add(this.fldEntreEixos, 2, 1); this.geometriaLayout.Controls.Add(this.fldAnguloFrontal, 0, 2); this.geometriaLayout.Controls.Add(this.fldAnguloLateral, 1, 2); + this.geometriaLayout.Controls.Add(this.pnlReferenciaImu, 2, 2); this.geometriaLayout.Dock = System.Windows.Forms.DockStyle.Fill; this.geometriaLayout.Location = new System.Drawing.Point(10, 6); this.geometriaLayout.Margin = new System.Windows.Forms.Padding(0); @@ -660,6 +666,58 @@ namespace AgroBase.Forms.IHM this.fldAnguloLateral.Titulo = "Ângulo máx. lateral"; this.fldAnguloLateral.Unidade = "°"; // + // pnlReferenciaImu + // + this.pnlReferenciaImu.BackColor = System.Drawing.Color.FromArgb(((int)(((byte)(30)))), ((int)(((byte)(36)))), ((int)(((byte)(41))))); + this.pnlReferenciaImu.Controls.Add(this.lblOffsetImuRoll); + this.pnlReferenciaImu.Controls.Add(this.lblOffsetImuPitch); + this.pnlReferenciaImu.Controls.Add(this.btnReferenciarImu); + this.pnlReferenciaImu.Dock = System.Windows.Forms.DockStyle.Fill; + this.pnlReferenciaImu.Location = new System.Drawing.Point(247, 84); + this.pnlReferenciaImu.Margin = new System.Windows.Forms.Padding(3, 2, 2, 2); + this.pnlReferenciaImu.Name = "pnlReferenciaImu"; + this.pnlReferenciaImu.Padding = new System.Windows.Forms.Padding(4, 2, 4, 3); + this.pnlReferenciaImu.Size = new System.Drawing.Size(119, 54); + this.pnlReferenciaImu.TabIndex = 5; + // + // lblOffsetImuRoll + // + this.lblOffsetImuRoll.Font = new System.Drawing.Font("Segoe UI", 7F); + this.lblOffsetImuRoll.ForeColor = System.Drawing.Color.FromArgb(((int)(((byte)(204)))), ((int)(((byte)(218)))), ((int)(((byte)(211))))); + this.lblOffsetImuRoll.Location = new System.Drawing.Point(5, 2); + this.lblOffsetImuRoll.Name = "lblOffsetImuRoll"; + this.lblOffsetImuRoll.Size = new System.Drawing.Size(54, 15); + this.lblOffsetImuRoll.TabIndex = 0; + this.lblOffsetImuRoll.Text = "Roll: 0.00°"; + this.lblOffsetImuRoll.TextAlign = System.Drawing.ContentAlignment.MiddleLeft; + // + // lblOffsetImuPitch + // + this.lblOffsetImuPitch.Font = new System.Drawing.Font("Segoe UI", 7F); + this.lblOffsetImuPitch.ForeColor = System.Drawing.Color.FromArgb(((int)(((byte)(204)))), ((int)(((byte)(218)))), ((int)(((byte)(211))))); + this.lblOffsetImuPitch.Location = new System.Drawing.Point(59, 2); + this.lblOffsetImuPitch.Name = "lblOffsetImuPitch"; + this.lblOffsetImuPitch.Size = new System.Drawing.Size(56, 15); + this.lblOffsetImuPitch.TabIndex = 1; + this.lblOffsetImuPitch.Text = "Pitch: 0.00°"; + this.lblOffsetImuPitch.TextAlign = System.Drawing.ContentAlignment.MiddleLeft; + // + // btnReferenciarImu + // + this.btnReferenciarImu.AccentColor = System.Drawing.Color.FromArgb(((int)(((byte)(155)))), ((int)(((byte)(106)))), ((int)(((byte)(222))))); + this.btnReferenciarImu.BackColor = System.Drawing.Color.Transparent; + this.btnReferenciarImu.BorderColor = System.Drawing.Color.FromArgb(((int)(((byte)(120)))), ((int)(((byte)(86)))), ((int)(((byte)(170))))); + this.btnReferenciarImu.Cursor = System.Windows.Forms.Cursors.Hand; + this.btnReferenciarImu.FillColor = System.Drawing.Color.FromArgb(((int)(((byte)(30)))), ((int)(((byte)(36)))), ((int)(((byte)(41))))); + this.btnReferenciarImu.IconText = "↻"; + this.btnReferenciarImu.Location = new System.Drawing.Point(4, 18); + this.btnReferenciarImu.Margin = new System.Windows.Forms.Padding(0); + this.btnReferenciarImu.MinimumSize = new System.Drawing.Size(60, 34); + this.btnReferenciarImu.Name = "btnReferenciarImu"; + this.btnReferenciarImu.Size = new System.Drawing.Size(111, 34); + this.btnReferenciarImu.TabIndex = 2; + this.btnReferenciarImu.Text = "ZERAR"; + // // pnlBateria // this.pnlBateria.AccentColor = System.Drawing.Color.FromArgb(((int)(((byte)(72)))), ((int)(((byte)(210)))), ((int)(((byte)(122))))); @@ -1019,6 +1077,7 @@ namespace AgroBase.Forms.IHM this.rightLayout.ResumeLayout(false); this.pnlGeometria.ResumeLayout(false); this.geometriaLayout.ResumeLayout(false); + this.pnlReferenciaImu.ResumeLayout(false); this.pnlBateria.ResumeLayout(false); this.bateriaLayout.ResumeLayout(false); this.pnlReservatorio.ResumeLayout(false); @@ -1065,6 +1124,10 @@ namespace AgroBase.Forms.IHM private AgroBase.Forms.IHM.Operacao.Parametros.Controls.HmiCompactNumericField fldEntreEixos; private AgroBase.Forms.IHM.Operacao.Parametros.Controls.HmiCompactNumericField fldAnguloFrontal; private AgroBase.Forms.IHM.Operacao.Parametros.Controls.HmiCompactNumericField fldAnguloLateral; + private System.Windows.Forms.Panel pnlReferenciaImu; + private System.Windows.Forms.Label lblOffsetImuRoll; + private System.Windows.Forms.Label lblOffsetImuPitch; + private AgroBase.Forms.IHM.Controls.HmiSidebarButton btnReferenciarImu; private AgroBase.Forms.IHM.Controls.HmiRoundedPanel pnlBateria; private System.Windows.Forms.TableLayoutPanel bateriaLayout; diff --git a/AgroBase/AgroBase/Forms/IHM/frmAjustes.cs b/AgroBase/AgroBase/Forms/IHM/frmAjustes.cs index 5be709c09..727e90fed 100644 --- a/AgroBase/AgroBase/Forms/IHM/frmAjustes.cs +++ b/AgroBase/AgroBase/Forms/IHM/frmAjustes.cs @@ -1,6 +1,7 @@ using AgroBase.Forms.IHM.Controls; using AgroBase.Forms.IHM.Operacao.Parametros.Controls; using AgroBase.Models; +using AgroBase.Models.Components; using AgroBase.Services.Operadores; using System; using System.Collections.Generic; @@ -56,6 +57,7 @@ namespace AgroBase.Forms.IHM public void AtualizarDadosTela() { lblRover.Text = $"ROVER {VariaveisEquipamento.Parametros.serial_number}\r\nConfiguração local"; + AtualizarReferenciaImuTela(); } @@ -102,6 +104,11 @@ namespace AgroBase.Forms.IHM await ExecutarSeguroAsync("Erro ao testar comunicação!", TestarComunicacaoAsync); }; + btnReferenciarImu.Click += async delegate + { + await ExecutarSeguroAsync("Erro ao referenciar IMU!", ReferenciarImuAsync); + }; + cameraFrontal.CameraChanged += cameraFrontal_CameraChanged; cameraErvas.CameraChanged += cameraErvas_CameraChanged; @@ -179,6 +186,94 @@ namespace AgroBase.Forms.IHM campo.DecimalPlaces = casas; } + + private void AtualizarReferenciaImuTela() + { + lblOffsetImuRoll.Text = $"L: {VariaveisEquipamento.Parametros.imu_roll_offset_deg:+0.00;-0.00;0.00}°"; + lblOffsetImuPitch.Text = $"F: {VariaveisEquipamento.Parametros.imu_pitch_offset_deg:+0.00;-0.00;0.00}°"; + } + + private async Task ReferenciarImuAsync() + { + var resposta = CustomDialog.ShowDialog( + "Referenciar IMU", + "Posicione o rover em uma superfície nivelada e mantenha-o imóvel.\r\n\r\n" + + "A referência atual de inclinação será recalculada.", + MessageBoxIcon.Information, + new Dictionary<(string, DialogResult?), Action>() + { + { ("OK", DialogResult.OK), () => { } }, + { ("Cancelar", DialogResult.Cancel), () => { } }, + } + ); + + if (resposta != DialogResult.OK) + return; + + btnReferenciarImu.Enabled = false; + btnReferenciarImu.Text = "LENDO..."; + + try + { + const int quantidadeAmostras = 12; + const int intervaloMs = 100; + var rolls = new List(); + var pitches = new List(); + + for (int i = 0; i < quantidadeAmostras; i++) + { + var imu = Variaveis.OperacaoEmAndamento?.Sensoriamento?.IMU; + + if (imu != null && imu.valido && imu.IdadeSegundos <= 1.0) + { + rolls.Add(imu.roll); + pitches.Add(imu.pitch); + } + + await Task.Delay(intervaloMs); + } + + if (rolls.Count < 8) + throw new InvalidOperationException("Não há amostras válidas e recentes suficientes do IMU para realizar a referência."); + + double mediaRoll = rolls.Average(); + double mediaPitch = pitches.Average(); + + double desvioRoll = Math.Sqrt(rolls.Average(x => Math.Pow(x - mediaRoll, 2))); + double desvioPitch = Math.Sqrt(pitches.Average(x => Math.Pow(x - mediaPitch, 2))); + + if (desvioRoll > 0.50 || desvioPitch > 0.50) + throw new InvalidOperationException( + $"O rover parece estar em movimento. Estabilidade insuficiente (σ roll {desvioRoll:0.00}°, σ pitch {desvioPitch:0.00}°). Mantenha-o imóvel e tente novamente."); + + double novoRoll = VariaveisEquipamento.Parametros.imu_roll_offset_deg + mediaRoll; + double novoPitch = VariaveisEquipamento.Parametros.imu_pitch_offset_deg + mediaPitch; + + var parametros = VariaveisEquipamento.Parametros; + DefinirParametroGlobal(parametros, novoRoll, "imu_roll_offset_deg"); + DefinirParametroGlobal(parametros, novoPitch, "imu_pitch_offset_deg"); + + if (!parametros.SalvarParametros()) + throw new InvalidOperationException("Não foi possível salvar os novos offsets do IMU."); + + parametros.AtualizarParametros(); + HealthWorkerService.AtualizarDadosOperacao(); + AtualizarReferenciaImuTela(); + + FuncoesIHM.MostrarAlerta( + "IMU referenciado", + $"Referência atualizada com sucesso.\r\n\r\n" + + $"Roll: {novoRoll:+0.00;-0.00;0.00}°\r\n" + + $"Pitch: {novoPitch:+0.00;-0.00;0.00}°", + MessageBoxIcon.Information); + } + finally + { + btnReferenciarImu.Text = "ZERAR"; + btnReferenciarImu.Enabled = true; + } + } + #region Câmeras private async Task AtualizarCameras() diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs index e67ac73aa..025823bdd 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs @@ -3197,15 +3197,55 @@ namespace AgroBase.Models private void ExecutarComandoMovDirGenerico(OperacaoComandoBaseModel controleBase, OperacaoModel op) { - op.DefinirEmergencia(controleBase.Emergencia); + if (op == null) + return; + /* + * ============================================================ + * COMANDO OPERACIONAL DE PARADA + * ============================================================ + * + * Pausa/Emergência só podem ser alteradas por um comando + * explicitamente destinado a isso. + * + * MOV/DIR nunca devem limpar esses estados. + */ + if (controleBase.Dispositivo == T_Code.Vzo) + { + op.DefinirEmergencia(controleBase.Emergencia); + + if (op.Sensoriamento.Operacao.Emergencia) + return; + + op.DefinirPausa(controleBase.Pausa); + + return; + } + + /* + * Emergência sempre possui autoridade máxima. + */ if (op.Sensoriamento.Operacao.Emergencia) return; - op.DefinirPausa(controleBase.Pausa); + bool controleManualAcionado = + op.Parametros?.Controle?.ControleManualAcionado == true; - if (op.Sensoriamento.Operacao.Pausa) + /* + * ============================================================ + * PAUSA OPERACIONAL + * ============================================================ + * + * Pausa impede o AUTOMÁTICO, mas não impede uma intervenção + * manual explicitamente assumida pelo operador. + * + * Também permitimos comandos de soltura/neutro mesmo após + * retirar o controle manual, para nunca deixar comando preso. + */ + if (op.Sensoriamento.Operacao.Pausa && !controleManualAcionado && !controleBase.Solto) + { return; + } if (controleBase.Controle == null) return; @@ -3215,12 +3255,20 @@ namespace AgroBase.Models if (cmdRover == null) return; - if (cmdRover.Dispositivo == T_Code.Mov && (op.Parametros?.Controle?.MovimentoAutomatico ?? false) && op.Sensoriamento.Operacao.OperacaoIniciada) + if ( + cmdRover.Dispositivo == T_Code.Mov && + (op.Parametros?.Controle?.MovimentoAutomatico ?? false) && + op.Sensoriamento.Operacao.OperacaoIniciada + ) { return; } - if (cmdRover.Dispositivo == T_Code.Dir && (op.Parametros?.Controle?.DirecionalAutomatico ?? false) && op.Sensoriamento.Operacao.OperacaoIniciada) + if ( + cmdRover.Dispositivo == T_Code.Dir && + (op.Parametros?.Controle?.DirecionalAutomatico ?? false) && + op.Sensoriamento.Operacao.OperacaoIniciada + ) { return; } @@ -3231,16 +3279,24 @@ namespace AgroBase.Models return; if (controleBase.Controle.PercentualVelocidadeSP != null) - controle.PercentualVelocidadeSP = (double)controleBase .Controle.PercentualVelocidadeSP; + controle.PercentualVelocidadeSP = + (double)controleBase.Controle.PercentualVelocidadeSP; if (controleBase.Controle.AnguloSP != null) - controle.Angulo = (double)controleBase.Controle.AnguloSP; + controle.Angulo = + (double)controleBase.Controle.AnguloSP; if (controleBase.Controle.TipoMovimentoDirecional != null) - op.DefinirTipoMovimentoControle((TipoMovimentoDirecional)controleBase.Controle.TipoMovimentoDirecional); + { + op.DefinirTipoMovimentoControle( + (TipoMovimentoDirecional) + controleBase.Controle.TipoMovimentoDirecional + ); + } if (controleBase.Controle.EmFreio != null) - controle.EmFreio = (bool)controleBase.Controle.EmFreio; + controle.EmFreio = + (bool)controleBase.Controle.EmFreio; switch (cmdRover.Dispositivo) { @@ -3248,7 +3304,8 @@ namespace AgroBase.Models GeneralJoystick.ProcessarDadosControle( cmdRover.botaoJoy, controleBase.Solto, - valorDesejado: controle.PercentualVelocidadeSP, + valorDesejado: + controle.PercentualVelocidadeSP, ForcarComando: true ); break; @@ -3257,7 +3314,8 @@ namespace AgroBase.Models GeneralJoystick.ProcessarDadosControle( cmdRover.botaoJoy, controleBase.Solto, - valorDesejado: controle.Angulo, + valorDesejado: + controle.Angulo, ForcarComando: true ); break; diff --git a/AgroBase/AgroBase/Models/Variaveis.cs b/AgroBase/AgroBase/Models/Variaveis.cs index e2244ed04..281f020a0 100644 --- a/AgroBase/AgroBase/Models/Variaveis.cs +++ b/AgroBase/AgroBase/Models/Variaveis.cs @@ -1123,7 +1123,7 @@ namespace AgroBase.Models { TipoMovimentoDirecional.Diagnostico, 25 }, }; public static double AnguloInclinacaoRollMax { get; set; } = 22.0; // Lateral - public static double AnguloInclinacaoPitchMax { get; set; } = 30.0; // Frontal 40 graus max + public static double AnguloInclinacaoPitchMax { get; set; } = 30.0; // Frontal public static string VozAlerta { get; set; } = "Microsoft Maria Desktop"; public static bool Pronto => Variaveis.OperacaoEmAndamento?.Parametros?.Controle != null; @@ -1527,6 +1527,9 @@ namespace AgroBase.Models public string can_gateway { get; set; } public string can_submask { get; set; } + public double imu_roll_offset_deg { get; set; } = 0.0; // Lateral + public double imu_pitch_offset_deg { get; set; } = 0.0; // Frontal + public void AtualizarParametros() { string config_path = Path.Combine(Variaveis.CaminhoSistema, Variaveis.CaminhoParametros, "config.json"); @@ -1558,6 +1561,9 @@ namespace AgroBase.Models camera_caminhho_id = par.camera_caminhho_id; camera_ervas_id = par.camera_ervas_id; + imu_pitch_offset_deg = par.imu_pitch_offset_deg; + imu_roll_offset_deg = par.imu_roll_offset_deg; + Carregado = true; } catch (Exception ex) diff --git a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs index a031d3c91..dbeeba794 100644 --- a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs +++ b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs @@ -429,7 +429,9 @@ namespace AgroBase.Services.Operadores ("dir_angulo_direita_sinal", 1), ("imu_dir_roll_min_aux", 6.0), ("imu_dir_bias_attention_deg", 4.0), - ("imu_dir_bias_risk_deg", 9.0) + ("imu_dir_bias_risk_deg", 9.0), + ("imu_roll_offset_deg", VariaveisEquipamento.Parametros.imu_roll_offset_deg), + ("imu_pitch_offset_deg", VariaveisEquipamento.Parametros.imu_pitch_offset_deg) ); } catch (Exception ex) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Parametros/config.json b/AgroBase/AgroBase/bin/x64/Debug/Parametros/config.json index 9a8100aa3..7dd331ddc 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Parametros/config.json +++ b/AgroBase/AgroBase/bin/x64/Debug/Parametros/config.json @@ -1 +1 @@ -{"Carregado":true,"serial_number":"01","base_ip":"20.1.0.2","base_porta_caminho":5000,"base_porta_ervas":5002,"comunicacao_interface":"AG_Hallow","rover_ip":"20.1.0.10","rover_gateway":"20.1.0.1","rover_submask":"255.255.255.0","camera_caminhho_id":"14442C10C143E2D600","camera_ervas_id":["194430108133AC2F00"],"can_interface":"AG_Can","can_ip_pc":"192.168.0.5","can_ip_modulo":"192.168.0.7","can_porta":8235,"can_gateway":"192.168.0.1","can_submask":"255.255.255.0"} \ No newline at end of file +{"Carregado":true,"serial_number":"01","base_ip":"20.1.0.2","base_porta_caminho":5000,"base_porta_ervas":5002,"comunicacao_interface":"AG_Hallow","rover_ip":"20.1.0.10","rover_gateway":"20.1.0.1","rover_submask":"255.255.255.0","camera_caminhho_id":"14442C10C143E2D600","camera_ervas_id":["194430108133AC2F00"],"can_interface":"AG_Can","can_ip_pc":"192.168.0.5","can_ip_modulo":"192.168.0.7","can_porta":8235,"can_gateway":"192.168.0.1","can_submask":"255.255.255.0","imu_roll_offset_deg":0.0,"imu_pitch_offset_deg":0.0} \ No newline at end of file diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/direcional.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/direcional.py index 0717734e5..8b724278f 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/direcional.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/direcional.py @@ -159,14 +159,27 @@ class ModuloDirecional(ModuloDiagnosticoBase): condicoes = [] if not conectado: + condicoes = [] + + if comandar: + condicoes.append({ + "valor": 0, + "severidade": 100, + "descricao": ( + f"Driver direcional {endereco_str} desconectado " + "enquanto está em uso" + ), + }) + return { "id": endereco_str, "label": mod_id, "status": StatusModulo.DESCONECTADO.value, "saude": 0, "motivos": ["Desconectado"], - "condicoes_operacionais": [], + "condicoes_operacionais": condicoes, "em_uso": comandar, + "referenciando": referenciando, "componentes": { "conectividade": 0, "feedback": 0, 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 efa2fa365..cd25ad1a8 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 @@ -49,6 +49,7 @@ class CameraIMU(ModuloDiagnosticoBase): angulo_risk_factor=0.75, angulo_emergency_factor=1.20, limites_refresh_s=1.0, + offsets_refresh_s=1.0, # Taxas continuam independentes do ângulo máximo. roll_rate_attention_dps=25.0, @@ -87,6 +88,13 @@ class CameraIMU(ModuloDiagnosticoBase): self.limites_refresh_s = max(float(limites_refresh_s), 0.25) self._ultimo_refresh_limites = 0.0 + # Offsets persistentes de atitude, definidos no equipamento. + # Convenção: valor corrigido = valor medido - offset. + self.offsets_refresh_s = max(float(offsets_refresh_s), 0.25) + self._ultimo_refresh_offsets = 0.0 + self.roll_offset_deg = 0.0 + self.pitch_offset_deg = 0.0 + # Valores iniciais seguros. Serão substituídos pelos dados do Redis. self.roll_max_deg = self.roll_max_default_deg self.pitch_max_deg = self.pitch_max_default_deg @@ -154,6 +162,10 @@ class CameraIMU(ModuloDiagnosticoBase): self.max_skew_imu_s = float(max_skew_imu_s) + # Carrega configuração persistente antes da primeira atitude publicada. + self._atualizar_limites_equipamento(forcar=True) + self._atualizar_offsets_equipamento(forcar=True) + self.thread = threading.Thread( target=self.imu_task_loop, name="CameraIMUFinalWorker", @@ -789,6 +801,77 @@ class CameraIMU(ModuloDiagnosticoBase): f"{self.pitch_emergency_deg:.1f}°)" ) + def _validar_offset_angulo(self, valor, valor_atual): + """Valida offset persistente de roll/pitch vindo do equipamento.""" + try: + valor = float(valor) + if not math.isfinite(valor): + return float(valor_atual) + # Guard rail: offset mecânico maior que isso indica configuração errada. + if valor < -45.0 or valor > 45.0: + return float(valor_atual) + return valor + except Exception: + return float(valor_atual) + + def _atualizar_offsets_equipamento(self, forcar=False): + """ + Atualiza os offsets persistentes da referência mecânica do rover. + + Campos esperados em equipamento: + imu_roll_offset_deg + imu_pitch_offset_deg + + Convenção: + roll_corrigido = roll_fundido - imu_roll_offset_deg + pitch_corrigido = pitch_fundido - imu_pitch_offset_deg + """ + agora = time.perf_counter() + if ( + not forcar + and (agora - self._ultimo_refresh_offsets) < self.offsets_refresh_s + ): + return + + self._ultimo_refresh_offsets = agora + equipamento = ContextoGlobalRedis.get_equipamento() or {} + + roll_anterior = self.roll_offset_deg + pitch_anterior = self.pitch_offset_deg + + self.roll_offset_deg = self._validar_offset_angulo( + equipamento.get("imu_roll_offset_deg", 0.0), + self.roll_offset_deg, + ) + self.pitch_offset_deg = self._validar_offset_angulo( + equipamento.get("imu_pitch_offset_deg", 0.0), + self.pitch_offset_deg, + ) + + mudou = ( + abs(self.roll_offset_deg - roll_anterior) > 1e-6 + or abs(self.pitch_offset_deg - pitch_anterior) > 1e-6 + ) + if mudou: + # Não deixa o EMA carregar a referência antiga depois de recalibrar. + self._resetar_filtro_fusao() + self.mostrar_log( + "Offsets IMU atualizados | " + f"roll={roll_anterior:.3f}°→{self.roll_offset_deg:.3f}° | " + f"pitch={pitch_anterior:.3f}°→{self.pitch_offset_deg:.3f}°" + ) + + def _aplicar_offsets_fusao(self, fusao): + """Aplica a referência mecânica depois da fusão e antes do filtro/risco.""" + if fusao is None: + return None + + corrigida = dict(fusao) + corrigida["roll"] = float(fusao.get("roll", 0.0)) - self.roll_offset_deg + corrigida["pitch"] = float(fusao.get("pitch", 0.0)) - self.pitch_offset_deg + # Offset constante não altera taxas angulares nem yaw. + return corrigida + # ============================================================ # Fusão # ============================================================ @@ -1384,6 +1467,16 @@ class CameraIMU(ModuloDiagnosticoBase): "roll_max_configurado_deg": round(self.roll_max_deg, 3), "pitch_max_configurado_deg": round(self.pitch_max_deg, 3), + # Referência mecânica persistente aplicada à atitude final. + "offsets_imu": { + "roll_deg": round(self.roll_offset_deg, 4), + "pitch_deg": round(self.pitch_offset_deg, 4), + "convencao": "corrigido=medido-offset", + "origem": "equipamento", + }, + "roll_offset_deg": round(self.roll_offset_deg, 4), + "pitch_offset_deg": round(self.pitch_offset_deg, 4), + "roll_attention_deg": round(self.roll_attention_deg, 3), "roll_risk_deg": round(self.roll_risk_deg, 3), "roll_critical_deg": round(self.roll_critical_deg, 3), @@ -1487,6 +1580,7 @@ class CameraIMU(ModuloDiagnosticoBase): agora = time.time() self._atualizar_limites_equipamento() + self._atualizar_offsets_equipamento() fontes, ignoradas, desabilitadas = self._ler_fontes_imu() @@ -1520,6 +1614,11 @@ class CameraIMU(ModuloDiagnosticoBase): fusao = self._fundir_fontes(fontes, sensores_agree=divergencia["sensors_agree"]) + # Corrige a atitude final pela referência mecânica persistente do rover. + # A divergência entre sensores continua sendo avaliada antes do offset, + # pois um offset global não deve mascarar desacordo entre IMUs. + fusao = self._aplicar_offsets_fusao(fusao) + # Atualiza o filtro antes de avaliar o risco. atitude_filtrada = self._atualizar_filtro_fusao(fusao) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/movimentacao.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/movimentacao.py index 968cf5430..8d67c43a9 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/movimentacao.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/movimentacao.py @@ -318,13 +318,31 @@ class ModuloMovimentacao(ModuloDiagnosticoBase): ) if not conectado and estado_desconectado["nivel"] == "confirmado": + condicoes_desconectado = [] + + if comandar: + condicoes_desconectado.append({ + "label": "Conexão OID", + "valor": estado_desconectado["duracao_s"], + "severidade": 100, + "descricao": ( + f"Driver de movimentação {mod_id} desconectado " + "enquanto está em uso" + ), + "acoes": [ + "Verificar comunicação CAN com o OID.", + "Verificar alimentação do driver.", + "Inspecionar conectores e cabeamento do motor.", + ], + }) + return { "id": endereco_str, "label": mod_id, "status": StatusModulo.DESCONECTADO.value, "saude": 0, "motivos": ["Desconectado"], - "condicoes_operacionais": [], + "condicoes_operacionais": condicoes_desconectado, "em_uso": comandar, "conectado_efetivo": False, "componentes": self._componentes_zero(), diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py index d0744e543..f5c029330 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py @@ -713,36 +713,62 @@ class ContextoGlobalRedis: ultimo_publicado == cls._int(ultimo_esperado, -999999) ) + ack_mapa_exato = ( + status_mapa == "aplicado" + and revisao_solicitada > 0 + and health_rev == revisao_solicitada + and manager_rev == revisao_solicitada + and command_solicitado != "" + and health_cmd == command_solicitado + and manager_cmd == command_solicitado + and health_status == "aplicado" + and manager_status == "aplicado" + and qtd_esperada > 0 + and qtd_publicada == qtd_esperada + and manager_qtd == qtd_esperada + and indices_extremos_ok + ) + mapa_mpc_ok = ( not requer_confirmacao_mapa_mpc or ( - not mapa_barreira_critica + ack_mapa_exato and mapa_confirmado_flag - and status_mapa == "aplicado" - and revisao_solicitada > 0 - and health_rev == revisao_solicitada - and manager_rev == revisao_solicitada - and command_solicitado != "" - and health_cmd == command_solicitado - and manager_cmd == command_solicitado - and health_status == "aplicado" - and manager_status == "aplicado" - and qtd_esperada > 0 - and qtd_publicada == qtd_esperada - and manager_qtd == qtd_esperada - and indices_extremos_ok + and not mapa_barreira_critica ) ) if not mapa_mpc_ok: motivos.append( "Mapa/MPC ainda não confirmado: " - f"rev solicitada/health/manager={revisao_solicitada}/{health_rev}/{manager_rev}; " - f"cmd solicitado/health/manager={command_solicitado}/{health_cmd}/{manager_cmd}; " - f"pontos publicados/esperados/manager={qtd_publicada}/{qtd_esperada}/{manager_qtd}; " + f"confirmado={mapa_confirmado_flag}; " + f"barreira={mapa_barreira_critica}; " + f"rev solicitada/health/manager=" + f"{revisao_solicitada}/{health_rev}/{manager_rev}; " + f"cmd solicitado/health/manager=" + f"{command_solicitado}/{health_cmd}/{manager_cmd}; " + f"pontos publicados/esperados/manager=" + f"{qtd_publicada}/{qtd_esperada}/{manager_qtd}; " + f"indices publicado={primeiro_publicado}->{ultimo_publicado}, " + f"esperado={primeiro_esperado}->{ultimo_esperado}, " + f"ok={indices_extremos_ok}; " f"status={status_mapa}/{health_status}/{manager_status}" ) + if ack_mapa_exato and not mapa_confirmado_flag: + mostrar_log( + "[MAPSYNC/INCONSISTENCIA] " + f"ACK exato confirmado, mas mapa_confirmado=false. " + f"rev={revisao_solicitada}; cmd={command_solicitado}" + ) + + if ack_mapa_exato and mapa_barreira_critica: + mostrar_log( + "[MAPSYNC/INCONSISTENCIA] " + f"ACK exato confirmado, mas mapa_barreira_critica permanece ativa. " + f"rev={revisao_solicitada}; cmd={command_solicitado}" + ) + detalhes_regras["mapa_mpc"] = { "requer_confirmacao": requer_confirmacao_mapa_mpc, "ok": mapa_mpc_ok, diff --git a/AgroBase/OperationControl/ViewModels/Views/Operacao/Monitoramento/ControleManualViewModel.cs b/AgroBase/OperationControl/ViewModels/Views/Operacao/Monitoramento/ControleManualViewModel.cs index d68ef0380..9a6b9a2dc 100644 --- a/AgroBase/OperationControl/ViewModels/Views/Operacao/Monitoramento/ControleManualViewModel.cs +++ b/AgroBase/OperationControl/ViewModels/Views/Operacao/Monitoramento/ControleManualViewModel.cs @@ -32,34 +32,60 @@ namespace OperationControl.ViewModels.Views.Operacao.Monitoramento get => _habilitado; set { - if (_habilitado != value) + if (_habilitado == value) + return; + + var r = + VariaveisControleOperacao.RoverEmFoco; + + /* + * Ao abandonar o manual, primeiro neutralizamos tudo + * enquanto o rover ainda reconhece autoridade manual. + */ + if (!value) { - _habilitado = value; - OnPropertyChanged(nameof(StatusTexto)); - OnPropertyChanged(nameof(TextoAjuda)); - OnPropertyChanged(nameof(StatusBrush)); + LimparTeclas(); + EnviarNeutros(); + } - var r = VariaveisControleOperacao.RoverEmFoco; - if (r != null) - { - r.Controle.ControleManualAcionado = value; - r.Controle.MovimentoAutomatico = value ? false : r.ControleAutomatico; - r.Controle.DirecionalAutomatico = value ? false : r.ControleAutomatico; - VariaveisControleOperacao.EnviarParametrosOperacao(new AgroBase.Models.Operacoes.OperacaoParametrosModel() - { - Controle = VariaveisControleOperacao.RoverEmFoco?.Controle - }); - } + _habilitado = value; - if (!value) - { - LimparTeclas(); - EnviarNeutros(); - } - else - { - VariaveisControleOperacao.EnviarComandoIniciarUDP(VariaveisControleOperacao.RoverEmFoco.IP, true); - } + OnPropertyChanged(nameof(StatusTexto)); + OnPropertyChanged(nameof(TextoAjuda)); + OnPropertyChanged(nameof(StatusBrush)); + + if (r != null) + { + r.Controle.ControleManualAcionado = + value; + + r.Controle.MovimentoAutomatico = + value + ? false + : r.ControleAutomatico; + + r.Controle.DirecionalAutomatico = + value + ? false + : r.ControleAutomatico; + + VariaveisControleOperacao + .EnviarParametrosOperacao( + new AgroBase.Models.Operacoes + .OperacaoParametrosModel() + { + Controle = r.Controle + } + ); + } + + if (value && r != null) + { + VariaveisControleOperacao + .EnviarComandoIniciarUDP( + r.IP, + true + ); } } } diff --git a/AgroBase/OperationControl/ViewModels/Windows/DockWindowViewModel.cs b/AgroBase/OperationControl/ViewModels/Windows/DockWindowViewModel.cs index f1c5d79f2..7bd7cd66e 100644 --- a/AgroBase/OperationControl/ViewModels/Windows/DockWindowViewModel.cs +++ b/AgroBase/OperationControl/ViewModels/Windows/DockWindowViewModel.cs @@ -605,6 +605,27 @@ namespace OperationControl.ViewModels return "A operação não foi liberada pelo rover."; } + void AdicionarEventoTrajetoria( + string descricao, + int prioridade = 700, + StatusModulo status = StatusModulo.Alerta) + { + descricao = NormalizarTexto(descricao); + + if (string.IsNullOrWhiteSpace(descricao)) + return; + + eventos.Add(new MotivoTopBarModel + { + Prioridade = prioridade, + Fonte = "Trajetoria", + Titulo = "TRAJETÓRIA BLOQUEADA", + Descricao = descricao, + StatusVisual = status, + Modulo = T_Code.Trj + }); + } + // ============================================================ // 0) Comunicação // ============================================================ @@ -704,22 +725,22 @@ namespace OperationControl.ViewModels // Parâmetro obrigatório ausente // ============================================================ - if ( - obj.Operacao != null && - !obj.Operacao.Liberada && - obj.Operacao.Pausa != true && - obj.Operacao.Emergencia != true - ) - { - eventos.Add(new MotivoTopBarModel - { - Prioridade = 850, - Fonte = "Operacao", - Titulo = "OPERAÇÃO BLOQUEADA", - Descricao = MotivoOperacaoAtual(), - StatusVisual = StatusModulo.Falha - }); - } + //if ( + // obj.Operacao != null && + // !obj.Operacao.Liberada && + // obj.Operacao.Pausa != true && + // obj.Operacao.Emergencia != true + //) + //{ + // eventos.Add(new MotivoTopBarModel + // { + // Prioridade = 850, + // Fonte = "Operacao", + // Titulo = "OPERAÇÃO BLOQUEADA", + // Descricao = MotivoOperacaoAtual(), + // StatusVisual = StatusModulo.Falha + // }); + //} // ============================================================ // 5) Controle @@ -761,14 +782,42 @@ namespace OperationControl.ViewModels foreach (var motivo in motivosTrajetoria) { - eventos.Add(new MotivoTopBarModel + AdicionarEventoTrajetoria(motivo); + } + + var impedimentoOperacao = obj.Operacao?.ImpedimentoErro ?? ""; + + var linhasImpedimento = + impedimentoOperacao + .Split( + new[] { "\r\n", "\n", "\r" }, + StringSplitOptions.RemoveEmptyEntries) + .Select(x => x.Trim()) + .Where(x => !string.IsNullOrWhiteSpace(x)) + .Distinct() + .ToList(); + + foreach (var linha in linhasImpedimento) + { + bool relacionadoTrajetoria = + linha.Contains( + "Mapa/MPC", + StringComparison.OrdinalIgnoreCase) + || + linha.Contains( + "trajetória", + StringComparison.OrdinalIgnoreCase) + || + linha.Contains( + "trajetoria", + StringComparison.OrdinalIgnoreCase); + + if (relacionadoTrajetoria) { - Prioridade = 700, - Fonte = "Trajetoria", - Titulo = "TRAJETÓRIA BLOQUEADA", - Descricao = motivo, - StatusVisual = StatusModulo.Alerta - }); + AdicionarEventoTrajetoria( + linha, + prioridade: 710); + } } // ============================================================