Compare commits
2 Commits
7a1b6bd51d
...
19392738f7
| Author | SHA1 | Date |
|---|---|---|
Diego Freitas
|
19392738f7 | |
|
Diego Freitas
|
42b9058df1 |
|
|
@ -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,13 +217,30 @@ namespace AgroBase.Forms.IHM.Operacao
|
|||
}
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
if (operacaoIniciada)
|
||||
{
|
||||
op?.AtualizarParametrosParciais(operacaoParametrizando.Parametros, op);
|
||||
}
|
||||
else
|
||||
{
|
||||
OperacaoModel.CarregarParamerosOperacaoBase(operacaoParametrizando.Parametros);
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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<double>();
|
||||
var pitches = new List<double>();
|
||||
|
||||
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()
|
||||
|
|
|
|||
|
|
@ -3196,6 +3196,21 @@ namespace AgroBase.Models
|
|||
// ============================================================================
|
||||
|
||||
private void ExecutarComandoMovDirGenerico(OperacaoComandoBaseModel controleBase, OperacaoModel op)
|
||||
{
|
||||
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);
|
||||
|
||||
|
|
@ -3204,8 +3219,33 @@ namespace AgroBase.Models
|
|||
|
||||
op.DefinirPausa(controleBase.Pausa);
|
||||
|
||||
if (op.Sensoriamento.Operacao.Pausa)
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* Emergência sempre possui autoridade máxima.
|
||||
*/
|
||||
if (op.Sensoriamento.Operacao.Emergencia)
|
||||
return;
|
||||
|
||||
bool controleManualAcionado =
|
||||
op.Parametros?.Controle?.ControleManualAcionado == true;
|
||||
|
||||
/*
|
||||
* ============================================================
|
||||
* 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;
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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"}
|
||||
{"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}
|
||||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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(),
|
||||
|
|
|
|||
|
|
@ -713,12 +713,8 @@ class ContextoGlobalRedis:
|
|||
ultimo_publicado == cls._int(ultimo_esperado, -999999)
|
||||
)
|
||||
|
||||
mapa_mpc_ok = (
|
||||
not requer_confirmacao_mapa_mpc
|
||||
or (
|
||||
not mapa_barreira_critica
|
||||
and mapa_confirmado_flag
|
||||
and status_mapa == "aplicado"
|
||||
ack_mapa_exato = (
|
||||
status_mapa == "aplicado"
|
||||
and revisao_solicitada > 0
|
||||
and health_rev == revisao_solicitada
|
||||
and manager_rev == revisao_solicitada
|
||||
|
|
@ -732,17 +728,47 @@ class ContextoGlobalRedis:
|
|||
and manager_qtd == qtd_esperada
|
||||
and indices_extremos_ok
|
||||
)
|
||||
|
||||
mapa_mpc_ok = (
|
||||
not requer_confirmacao_mapa_mpc
|
||||
or (
|
||||
ack_mapa_exato
|
||||
and mapa_confirmado_flag
|
||||
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,
|
||||
|
|
|
|||
|
|
@ -32,34 +32,60 @@ namespace OperationControl.ViewModels.Views.Operacao.Monitoramento
|
|||
get => _habilitado;
|
||||
set
|
||||
{
|
||||
if (_habilitado != value)
|
||||
{
|
||||
_habilitado = value;
|
||||
OnPropertyChanged(nameof(StatusTexto));
|
||||
OnPropertyChanged(nameof(TextoAjuda));
|
||||
OnPropertyChanged(nameof(StatusBrush));
|
||||
if (_habilitado == value)
|
||||
return;
|
||||
|
||||
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
|
||||
});
|
||||
}
|
||||
var r =
|
||||
VariaveisControleOperacao.RoverEmFoco;
|
||||
|
||||
/*
|
||||
* Ao abandonar o manual, primeiro neutralizamos tudo
|
||||
* enquanto o rover ainda reconhece autoridade manual.
|
||||
*/
|
||||
if (!value)
|
||||
{
|
||||
LimparTeclas();
|
||||
EnviarNeutros();
|
||||
}
|
||||
else
|
||||
|
||||
_habilitado = value;
|
||||
|
||||
OnPropertyChanged(nameof(StatusTexto));
|
||||
OnPropertyChanged(nameof(TextoAjuda));
|
||||
OnPropertyChanged(nameof(StatusBrush));
|
||||
|
||||
if (r != null)
|
||||
{
|
||||
VariaveisControleOperacao.EnviarComandoIniciarUDP(VariaveisControleOperacao.RoverEmFoco.IP, true);
|
||||
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
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -653,6 +653,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
|
||||
// ============================================================
|
||||
|
|
@ -752,22 +773,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
|
||||
|
|
@ -809,14 +830,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)
|
||||
{
|
||||
Prioridade = 700,
|
||||
Fonte = "Trajetoria",
|
||||
Titulo = "TRAJETÓRIA BLOQUEADA",
|
||||
Descricao = motivo,
|
||||
StatusVisual = StatusModulo.Alerta
|
||||
});
|
||||
bool relacionadoTrajetoria =
|
||||
linha.Contains(
|
||||
"Mapa/MPC",
|
||||
StringComparison.OrdinalIgnoreCase)
|
||||
||
|
||||
linha.Contains(
|
||||
"trajetória",
|
||||
StringComparison.OrdinalIgnoreCase)
|
||||
||
|
||||
linha.Contains(
|
||||
"trajetoria",
|
||||
StringComparison.OrdinalIgnoreCase);
|
||||
|
||||
if (relacionadoTrajetoria)
|
||||
{
|
||||
AdicionarEventoTrajetoria(
|
||||
linha,
|
||||
prioridade: 710);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
|
|
|
|||
Loading…
Reference in New Issue