Zendion/agrobot_base
Zendion
/
agrobot_base
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

robustez contra first, ajuste no fps da camera traseira, ajuste no imu traseiro, reinciar mesma operacao,

This commit is contained in:
Diego Freitas 2026-08-09 19:08:33 -03:00
parent b148d987b7
commit 6ca8aa5985
34 changed files with 1466 additions and 325 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
AgroBase/AgroBase/Forms/Operacoes/frmOperacaoManual.cs
View File

@ -679,7 +679,8 @@ namespace AgroBase.Forms.Operacoes
status: comando
);
pnlAtuadores.Controls.OfType<Label>().First(x => x.Name == "lblCmd_" + ID).Text = "Comando: " + (comando ? "Ligado" : "Desligado");
var lbl = pnlAtuadores.Controls.OfType<Label>().FirstOrDefault(x => x.Name == "lblCmd_" + ID);
if (lbl != null) lbl.Text = "Comando: " + (comando ? "Ligado" : "Desligado");
btn.Text = comando ? "Desligar" : "Ligar";
}
}

149
AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs
View File

@ -1516,7 +1516,8 @@ namespace AgroBase.Forms.Operacoes
lblMagnetometro.ForeColor = !LogImu.Iniciado ? Color.Red : Color.Black;
pnlOrientacaoMagnetometro.Invalidate();
pnlInclinacao.Invalidate();
visualizador3D.AtualizarAngulos(LogImu.PitchSeguro, LogImu.RollSeguro, -LogsGPS[idxMomentoAtual].OrientacaoReal);
double yaw = -LogsGPS[idxMomentoAtual].OrientacaoReal;
visualizador3D.AtualizarAngulos(Lateral: LogImu.RollSeguro, Frontal: LogImu.PitchSeguro, Rotacao: LogImu.YawSeguro);
//txtTemperatura.Text = LogImu.Temperatura.ToString("0.00");
//txtAltitude.Text = LogImu.Altitude.ToString("0.00");
//txtPressao.Text = LogImu.Pressao + " Pa (" + (LogImu.Pressao / 101300.0).ToString("0.00") + " atm)";
@ -1921,62 +1922,128 @@ namespace AgroBase.Forms.Operacoes
private void pnlInclinacao_Paint(object sender, PaintEventArgs e)
{
Graphics g = e.Graphics;
int centerX = pnlInclinacao.Width / 2;
int centerY = pnlInclinacao.Height / 2;
int radius = 10;
// Desenhar fundo com bordas arredondadas
GraphicsPath path = new GraphicsPath();
int cornerRadius = 20;
path.AddArc(0, 0, cornerRadius, cornerRadius, 180, 90);
path.AddArc(pnlInclinacao.Width - cornerRadius, 0, cornerRadius, cornerRadius, 270, 90);
path.AddArc(pnlInclinacao.Width - cornerRadius, pnlInclinacao.Height - cornerRadius, cornerRadius, cornerRadius, 0, 90);
path.AddArc(0, pnlInclinacao.Height - cornerRadius, cornerRadius, cornerRadius, 90, 90);
path.CloseAllFigures();
g.SmoothingMode = SmoothingMode.AntiAlias;
g.FillPath(new SolidBrush(Color.LightBlue), path);
g.DrawPath(new Pen(Color.DarkBlue, 2), path);
g.TextRenderingHint = System.Drawing.Text.TextRenderingHint.ClearTypeGridFit;
// Desenhar linhas de referência
Pen pen = new Pen(Color.Gray, 1);
g.DrawLine(pen, centerX, 0, centerX, pnlInclinacao.Height);
g.DrawLine(pen, 0, centerY, pnlInclinacao.Width, centerY);
int largura = pnlInclinacao.ClientSize.Width;
int altura = pnlInclinacao.ClientSize.Height;
// Desenhar linhas adicionais para ângulos
for (int i = -90; i <= 90; i += 15)
if (largura <= 0 || altura <= 0)
return;
float centerX = largura / 2f;
float centerY = altura / 2f;
float radius = 10f;
float margemX = radius + 5f;
float margemY = radius + 5f;
// X = lateral = Roll
double limiteLateral = Math.Max(1.0, Math.Abs(VariaveisEquipamento.AnguloInclinacaoRollMax));
// Y = frontal = Pitch
double limiteFrontal = Math.Max(1.0, Math.Abs(VariaveisEquipamento.AnguloInclinacaoPitchMax));
float escalaX = (largura / 2f - margemX) / (float)limiteLateral;
float escalaY = (altura / 2f - margemY) / (float)limiteFrontal;
GraphicsPath path = CriarRetanguloArredondado(new RectangleF(1, 1, largura - 2, altura - 2), 20f);
SolidBrush fundoBrush = new SolidBrush(Color.LightBlue);
Pen bordaPen = new Pen(Color.DarkBlue, 2f);
Pen eixoPen = new Pen(Color.Gray, 1f);
Pen gradePen = new Pen(Color.FromArgb(100, Color.Gray), 1f);
SolidBrush pontoBrush = new SolidBrush(Color.Red);
Font fonte = new Font("Arial", 8f);
StringFormat textoCentralizado = new StringFormat { Alignment = StringAlignment.Center, LineAlignment = StringAlignment.Center };
// Fundo
g.FillPath(fundoBrush, path);
g.DrawPath(bordaPen, path);
// Evita que desenhos ultrapassem o painel arredondado
Region estadoClip = g.Clip;
g.SetClip(path);
// Eixos centrais
g.DrawLine(eixoPen, centerX, margemY, centerX, altura - margemY);
g.DrawLine(eixoPen, margemX, centerY, largura - margemX, centerY);
const int quantidadeDivisoes = 4;
// Régua horizontal: Roll / lateral
for (int i = -quantidadeDivisoes; i <= quantidadeDivisoes; i++)
{
int posX = centerX + (int)(i * (pnlInclinacao.Width / 2 - radius) / 90);
int posY = centerY - (int)(i * (pnlInclinacao.Height / 2 - radius) / 90);
g.DrawLine(pen, centerX, centerY, posX, centerY);
g.DrawLine(pen, centerX, centerY, centerX, posY);
double angulo = limiteLateral * i / quantidadeDivisoes;
float posX = centerX + (float)angulo * escalaX;
g.DrawLine(gradePen, posX, margemY, posX, altura - margemY);
g.DrawLine(eixoPen, posX, centerY - 5f, posX, centerY + 5f);
if (i != 0)
{
RectangleF areaTexto = new RectangleF(posX - 25f, centerY + 7f, 50f, 18f);
g.DrawString($"{angulo:0.#}°", fonte, Brushes.Black, areaTexto, textoCentralizado);
}
}
// Desenhar régua na linha central
for (int i = -90; i <= 90; i += 15)
// Régua vertical: Pitch / frontal
for (int i = -quantidadeDivisoes; i <= quantidadeDivisoes; i++)
{
int posX = centerX + (int)(i * (pnlInclinacao.Width / 2 - radius) / 90);
int posY = centerY - (int)(i * (pnlInclinacao.Height / 2 - radius) / 90);
double angulo = limiteFrontal * i / quantidadeDivisoes;
float posY = centerY - (float)angulo * escalaY;
// Marcas horizontais
g.DrawLine(pen, posX, centerY - 5, posX, centerY + 5);
g.DrawString(i.ToString(), new Font("Arial", 8), Brushes.Black, posX - 10, centerY + 10);
g.DrawLine(gradePen, margemX, posY, largura - margemX, posY);
g.DrawLine(eixoPen, centerX - 5f, posY, centerX + 5f, posY);
// Marcas verticais
g.DrawLine(pen, centerX - 5, posY, centerX + 5, posY);
g.DrawString(i.ToString(), new Font("Arial", 8), Brushes.Black, centerX + 10, posY - 5);
if (i != 0)
{
RectangleF areaTexto = new RectangleF(centerX + 7f, posY - 9f, 50f, 18f);
g.DrawString($"{angulo:0.#}°", fonte, Brushes.Black, areaTexto, textoCentralizado);
}
}
// Calcular a posição do círculo com base nos ângulos
var Log = LogsIMU[idxMomentoAtual];
int circlePosX = centerX + (int)((Log?.RollSeguro ?? 0) * (pnlInclinacao.Width / 2 - radius) / 90);
int circlePosY = centerY - (int)((Log?.PitchSeguro ?? 0) * (pnlInclinacao.Height / 2 - radius) / 90);
// X horizontal = lateral / Roll
double anguloLateralExibido = FuncoesMatematicas.Clamp(Log.RollSeguro, -limiteLateral, limiteLateral);
// Desenhar o círculo
Brush brush = new SolidBrush(Color.Red);
g.FillEllipse(brush, circlePosX - radius, circlePosY - radius, radius * 2, radius * 2);
// Y vertical = frontal / Pitch
double anguloFrontalExibido = FuncoesMatematicas.Clamp(-Log.PitchSeguro, -limiteFrontal, limiteFrontal);
float circlePosX = centerX + (float)anguloLateralExibido * escalaX;
float circlePosY = centerY + (float)anguloFrontalExibido * escalaY;
g.FillEllipse(pontoBrush, circlePosX - radius, circlePosY - radius, radius * 2f, radius * 2f);
// Restaura o recorte
g.Clip = estadoClip;
}
private static GraphicsPath CriarRetanguloArredondado(RectangleF retangulo, float raio)
{
GraphicsPath path = new GraphicsPath();
float diametro = Math.Min(raio, Math.Min(retangulo.Width, retangulo.Height));
RectangleF arco = new RectangleF(retangulo.X, retangulo.Y, diametro, diametro);
path.AddArc(arco, 180, 90);
arco.X = retangulo.Right - diametro;
path.AddArc(arco, 270, 90);
arco.Y = retangulo.Bottom - diametro;
path.AddArc(arco, 0, 90);
arco.X = retangulo.Left;
path.AddArc(arco, 90, 90);
path.CloseFigure();
return path;
}
private void pnlCaminho_Paint(object sender, PaintEventArgs e)
{
if (!chbDesenharCorredor.Checked) return;

2
AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs
View File

@ -349,7 +349,7 @@ namespace AgroBase.Forms
double anguloControle = 0.0;
TipoMovimentoDirecional tipoMovimento = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento;
double veloccidade = (Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.StatusOperacaoAtual == StatusOperacao.Concluido || Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.StatusOperacaoAtual == StatusOperacao.Aguardando) ? 0 : velocidadeCarroMs;
double veloccidade = (new List<StatusOperacao>() { StatusOperacao.Concluido, StatusOperacao.Aguardando, StatusOperacao.Parametrizando }.Contains(Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.StatusOperacaoAtual)) ? 0 : velocidadeCarroMs;
var tipoControle = Variaveis.OperacaoEmAndamento.Parametros.Controle.DirTipoMovimento;

5
AgroBase/AgroBase/Forms/frmAcompanhamento.cs
View File

@ -161,7 +161,10 @@ namespace AgroBase.Forms
try
{
Chart chart = (Chart)(pnlFundo.Controls.Find("chart" + Modulo.Modulo_ID, false).First());
Chart chart = (Chart)(pnlFundo.Controls.Find("chart" + Modulo.Modulo_ID, false)?.FirstOrDefault());
if (chart == null)
return;
chart.Titles.Clear();
chart.Titles.Add(Modulo.Modulo_ID);
var Logs = (Modulo.MovMotor.LogGrafico.Count > MovimentacaoUnificadaModel.LogsManter ? Modulo.MovMotor.LogGrafico.Skip(Math.Max(0, Modulo.MovMotor.LogGrafico.Count - MovimentacaoUnificadaModel.LogsManter)).Take(MovimentacaoUnificadaModel.LogsManter).ToList() : Modulo.MovMotor.LogGrafico).Where(log => log.Modulo_ID == Modulo.Modulo_ID).ToList();

12
AgroBase/AgroBase/Forms/frmWT901C.Designer.cs generated
View File

@ -115,9 +115,9 @@
this.label1.Location = new System.Drawing.Point(692, 397);
this.label1.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.label1.Name = "label1";
this.label1.Size = new System.Drawing.Size(27, 26);
this.label1.Size = new System.Drawing.Size(79, 26);
this.label1.TabIndex = 51;
this.label1.Text = "X";
this.label1.Text = "Frontal";
//
// label2
//
@ -127,9 +127,9 @@
this.label2.Location = new System.Drawing.Point(692, 468);
this.label2.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.label2.Name = "label2";
this.label2.Size = new System.Drawing.Size(28, 26);
this.label2.Size = new System.Drawing.Size(78, 26);
this.label2.TabIndex = 53;
this.label2.Text = "Y";
this.label2.Text = "Lateral";
//
// txtY
//
@ -151,9 +151,9 @@
this.label3.Location = new System.Drawing.Point(690, 539);
this.label3.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.label3.Name = "label3";
this.label3.Size = new System.Drawing.Size(25, 26);
this.label3.Size = new System.Drawing.Size(93, 26);
this.label3.TabIndex = 55;
this.label3.Text = "Z";
this.label3.Text = "Rotação";
//
// txtZ
//

146
AgroBase/AgroBase/Forms/frmWT901C.cs
View File

@ -272,7 +272,7 @@ namespace AgroBase.Forms
picBussola.Invalidate();
//glControl.Invalidate();
pnlInclinacao.Invalidate();
visualizador3D.AtualizarAngulos(AnguloX, AnguloY, AnguloZ);
visualizador3D.AtualizarAngulos(Frontal: AnguloX, Lateral: AnguloY, Rotacao: AnguloZ);
txtX.Text = AnguloX.ToString("0.00");
txtY.Text = AnguloY.ToString("0.00");
@ -324,59 +324,123 @@ namespace AgroBase.Forms
private void pnlInclinacao_Paint(object sender, PaintEventArgs e)
{
Graphics g = e.Graphics;
int centerX = pnlInclinacao.Width / 2;
int centerY = pnlInclinacao.Height / 2;
int radius = 10;
// Desenhar fundo com bordas arredondadas
GraphicsPath path = new GraphicsPath();
int cornerRadius = 20;
path.AddArc(0, 0, cornerRadius, cornerRadius, 180, 90);
path.AddArc(pnlInclinacao.Width - cornerRadius, 0, cornerRadius, cornerRadius, 270, 90);
path.AddArc(pnlInclinacao.Width - cornerRadius, pnlInclinacao.Height - cornerRadius, cornerRadius, cornerRadius, 0, 90);
path.AddArc(0, pnlInclinacao.Height - cornerRadius, cornerRadius, cornerRadius, 90, 90);
path.CloseAllFigures();
g.SmoothingMode = SmoothingMode.AntiAlias;
g.FillPath(new SolidBrush(Color.LightBlue), path);
g.DrawPath(new Pen(Color.DarkBlue, 2), path);
g.TextRenderingHint = System.Drawing.Text.TextRenderingHint.ClearTypeGridFit;
// Desenhar linhas de referência
Pen pen = new Pen(Color.Gray, 1);
g.DrawLine(pen, centerX, 0, centerX, pnlInclinacao.Height);
g.DrawLine(pen, 0, centerY, pnlInclinacao.Width, centerY);
int largura = pnlInclinacao.ClientSize.Width;
int altura = pnlInclinacao.ClientSize.Height;
// Desenhar linhas adicionais para ângulos
for (int i = -90; i <= 90; i += 15)
if (largura <= 0 || altura <= 0)
return;
float centerX = largura / 2f;
float centerY = altura / 2f;
float radius = 10f;
float margemX = radius + 5f;
float margemY = radius + 5f;
// X = lateral = Roll
double limiteLateral = Math.Max(1.0, Math.Abs(VariaveisEquipamento.AnguloInclinacaoRollMax));
// Y = frontal = Pitch
double limiteFrontal = Math.Max(1.0, Math.Abs(VariaveisEquipamento.AnguloInclinacaoPitchMax));
float escalaX = (largura / 2f - margemX) / (float)limiteLateral;
float escalaY = (altura / 2f - margemY) / (float)limiteFrontal;
GraphicsPath path = CriarRetanguloArredondado(new RectangleF(1, 1, largura - 2, altura - 2), 20f);
SolidBrush fundoBrush = new SolidBrush(Color.LightBlue);
Pen bordaPen = new Pen(Color.DarkBlue, 2f);
Pen eixoPen = new Pen(Color.Gray, 1f);
Pen gradePen = new Pen(Color.FromArgb(100, Color.Gray), 1f);
SolidBrush pontoBrush = new SolidBrush(Color.Red);
Font fonte = new Font("Arial", 8f);
StringFormat textoCentralizado = new StringFormat { Alignment = StringAlignment.Center, LineAlignment = StringAlignment.Center };
// Fundo
g.FillPath(fundoBrush, path);
g.DrawPath(bordaPen, path);
// Evita que desenhos ultrapassem o painel arredondado
Region estadoClip = g.Clip;
g.SetClip(path);
// Eixos centrais
g.DrawLine(eixoPen, centerX, margemY, centerX, altura - margemY);
g.DrawLine(eixoPen, margemX, centerY, largura - margemX, centerY);
const int quantidadeDivisoes = 4;
// Régua horizontal: Roll / lateral
for (int i = -quantidadeDivisoes; i <= quantidadeDivisoes; i++)
{
int posX = centerX + (int)(i * (pnlInclinacao.Width / 2 - radius) / 90);
int posY = centerY - (int)(i * (pnlInclinacao.Height / 2 - radius) / 90);
g.DrawLine(pen, centerX, centerY, posX, centerY);
g.DrawLine(pen, centerX, centerY, centerX, posY);
double angulo = limiteLateral * i / quantidadeDivisoes;
float posX = centerX + (float)angulo * escalaX;
g.DrawLine(gradePen, posX, margemY, posX, altura - margemY);
g.DrawLine(eixoPen, posX, centerY - 5f, posX, centerY + 5f);
if (i != 0)
{
RectangleF areaTexto = new RectangleF(posX - 25f, centerY + 7f, 50f, 18f);
g.DrawString($"{angulo:0.#}°", fonte, Brushes.Black, areaTexto, textoCentralizado);
}
}
// Desenhar régua na linha central
for (int i = -90; i <= 90; i += 15)
// Régua vertical: Pitch / frontal
for (int i = -quantidadeDivisoes; i <= quantidadeDivisoes; i++)
{
int posX = centerX + (int)(i * (pnlInclinacao.Width / 2 - radius) / 90);
int posY = centerY - (int)(i * (pnlInclinacao.Height / 2 - radius) / 90);
double angulo = limiteFrontal * i / quantidadeDivisoes;
float posY = centerY - (float)angulo * escalaY;
// Marcas horizontais
g.DrawLine(pen, posX, centerY - 5, posX, centerY + 5);
g.DrawString(i.ToString(), new Font("Arial", 8), Brushes.Black, posX - 10, centerY + 10);
g.DrawLine(gradePen, margemX, posY, largura - margemX, posY);
g.DrawLine(eixoPen, centerX - 5f, posY, centerX + 5f, posY);
// Marcas verticais
g.DrawLine(pen, centerX - 5, posY, centerX + 5, posY);
g.DrawString(i.ToString(), new Font("Arial", 8), Brushes.Black, centerX + 10, posY - 5);
if (i != 0)
{
RectangleF areaTexto = new RectangleF(centerX + 7f, posY - 9f, 50f, 18f);
g.DrawString($"{angulo:0.#}°", fonte, Brushes.Black, areaTexto, textoCentralizado);
}
}
// Calcular a posição do círculo com base nos ângulos
int circlePosX = centerX + (int)(AnguloX * (pnlInclinacao.Width / 2 - radius) / 90);
int circlePosY = centerY - (int)(AnguloY * (pnlInclinacao.Height / 2 - radius) / 90);
// X horizontal = lateral / Roll
double anguloLateralExibido = FuncoesMatematicas.Clamp(AnguloY, -limiteLateral, limiteLateral);
// Desenhar o círculo
Brush brush = new SolidBrush(Color.Red);
g.FillEllipse(brush, circlePosX - radius, circlePosY - radius, radius * 2, radius * 2);
// Y vertical = frontal / Pitch
double anguloFrontalExibido = FuncoesMatematicas.Clamp(AnguloX, -limiteFrontal, limiteFrontal);
float circlePosX = centerX + (float)anguloLateralExibido * escalaX;
float circlePosY = centerY + (float)anguloFrontalExibido * escalaY;
g.FillEllipse(pontoBrush, circlePosX - radius, circlePosY - radius, radius * 2f, radius * 2f);
// Restaura o recorte
g.Clip = estadoClip;
}
private static GraphicsPath CriarRetanguloArredondado(RectangleF retangulo, float raio)
{
GraphicsPath path = new GraphicsPath();
float diametro = Math.Min(raio, Math.Min(retangulo.Width, retangulo.Height));
RectangleF arco = new RectangleF(retangulo.X, retangulo.Y, diametro, diametro);
path.AddArc(arco, 180, 90);
arco.X = retangulo.Right - diametro;
path.AddArc(arco, 270, 90);
arco.Y = retangulo.Bottom - diametro;
path.AddArc(arco, 0, 90);
arco.X = retangulo.Left;
path.AddArc(arco, 90, 90);
path.CloseFigure();
return path;
}
private void PicBussola_Paint(object sender, PaintEventArgs e)

8
AgroBase/AgroBase/Models/GPSModel.cs
View File

@ -300,6 +300,9 @@ namespace AgroBase.Models
public static GPSModel ProjetarPontoDeslocado(GPSModel pontoOriginal, double distancia, double angulo)
{
if (pontoOriginal == null)
return null;
// Convertendo ângulo em radianos
double anguloRad = (Math.PI / 180) * angulo;
@ -526,7 +529,10 @@ namespace AgroBase.Models
var resultado = trecho
.Select((p, i) => (ponto: p, index: i, dist: DistanciaEntrePontos(pontoRef, p)))
.OrderBy(x => x.dist)
.First();
.FirstOrDefault();
if (resultado.ponto == null)
return (null, -1);
return (resultado.ponto, resultado.index);
}

6
AgroBase/AgroBase/Models/GeneralJoystick.cs
View File

@ -692,7 +692,8 @@ namespace AgroBase
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();
var cmd = DeParaComandos.FirstOrDefault(x => x.botaoJoy == botao)?.Clone();
if (cmd == null) return;
cmd.CalcularValor(direcao == Direcao.Parado, valorJoy: valor);
bool mudou = direcao != _ultimaDirecaoMovEnviada || double.IsNaN(_ultimoValorMovEnviado) || Math.Abs(cmd.ValorAplicar - _ultimoValorMovEnviado) >= VariacaoMinimaMov;
bool heartbeat = (DateTime.Now - _ultimoEnvioMov).TotalMilliseconds >= HeartbeatComandoMs;
@ -712,7 +713,8 @@ namespace AgroBase
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();
var cmd = DeParaComandos.FirstOrDefault(x => x.botaoJoy == botao)?.Clone();
if (cmd == null) return;
cmd.CalcularValor(solto, valorJoy: valor);
bool mudou = double.IsNaN(_ultimoAnguloDirEnviado) || Math.Abs(cmd.ValorAplicar - _ultimoAnguloDirEnviado) >= VariacaoMinimaDir;
bool heartbeat = (DateTime.Now - _ultimoEnvioDir).TotalMilliseconds >= HeartbeatComandoMs;

22
AgroBase/AgroBase/Models/Modules/AtuadorModel.cs
View File

@ -1261,6 +1261,10 @@ namespace AgroBase.Models.Modules
x.TempoAtuadoEmMovimento = 0;
x.TempoComFluxo = 0;
x.UltimoStatusDesejado = false;
x.UltimoAnguloDesejado = x.AnguloNeutro;
x.UltimoComandoEnviado = DateTime.MinValue;
x.ReiniciarHistoricoAtuacao(true);
x.ValoresLeituras.ForEach(l =>
@ -1279,6 +1283,12 @@ namespace AgroBase.Models.Modules
x.TempoComPotencia = 0;
x.PotenciaAcumuladaPctSeg = 0;
x.PotenciaMediaAtiva = 0;
x.UltimoStatusDesejado = false;
x.UltimaPotenciaDesejada = 0;
x.UltimaPressaoDesejada = 0;
x.UltimoComandoEnviado = DateTime.MinValue;
x.ValoresLeituras.ForEach(l =>
{
l.atual = new CanCommandFreqModel<dynamic>();
@ -2313,8 +2323,13 @@ namespace AgroBase.Models.Modules
public Estado _EstadoLeitura => (Estado)(ValoresLeituras?.FirstOrDefault(x => x.posicao == CanMessagePosicaoDados.Dados1 && x.funcao == FuncoesPinout.EstadoLeitura)?.atual?.valor ?? 0);
public double _AnguloBicoLeitura => ValoresLeituras?.FirstOrDefault(x => x.posicao == CanMessagePosicaoDados.Dados1 && x.funcao == FuncoesPinout.ServoAnguloLeitura)?.atual? .valor ?? 0;
[JsonIgnore]
public DateTime UltimoComandoEnviado { get; set; } = DateTime.MinValue;
[JsonIgnore]
public bool UltimoStatusDesejado { get; set; } = false;
[JsonIgnore]
public double UltimoAnguloDesejado { get; set; } = 0;
public (CanMessagePosicaoDados, byte[]) ProtocoloConfiguracao(PinoutDataModel pinout, bool conectar)
@ -2597,9 +2612,16 @@ namespace AgroBase.Models.Modules
public double PotenciaMediaAtiva { get; set; }
[JsonIgnore]
public DateTime UltimoComandoEnviado { get; set; } = DateTime.MinValue;
[JsonIgnore]
public bool UltimoStatusDesejado { get; set; } = false;
[JsonIgnore]
public double UltimaPotenciaDesejada { get; set; } = 0;
[JsonIgnore]
public double UltimaPressaoDesejada { get; set; } = 0;

2
AgroBase/AgroBase/Models/Modules/DirecionalModel.cs
View File

@ -81,7 +81,7 @@ namespace AgroBase.Models.Modules
}
public double AnguloFolgaCompensar { get; set; }
public double? Reducao { get; set; }
public double FatorVelocidadeReducao => (Reducao ?? VariaveisEquipamento.ReducaoDirecional) / 20.0;
public double FatorVelocidadeReducao => (Reducao ?? VariaveisEquipamento.ReducaoDirecional) / 40.0;
public double OffsetAnguloReal { get; set; }
public double RPM
{

9
AgroBase/AgroBase/Models/Modules/SensoriamentoModel.cs
View File

@ -2386,9 +2386,12 @@ namespace AgroBase.Models.Modules
public void AtualizarStatusSinaleiros(ModoOperacao Modo, StatusOperacao _statusOperacao)
{
Sinaleiros.First(x => x.ID == "LEDMN").Comportamento = VariaveisEquipamento.ComportamentoLedPorStatus(Modo == ModoOperacao.Manual ? StatusLED.Aceso : StatusLED.Apagado);
Sinaleiros.First(x => x.ID == "LEDAT").Comportamento = VariaveisEquipamento.ComportamentoLedPorStatus(Modo == ModoOperacao.Manual ? StatusLED.Apagado : StatusLED.Aceso);
Sinaleiros.First(x => x.ID == "LEDOP").Comportamento = VariaveisEquipamento.ComportamentoLedPorStatus(StatusLED.Piscando, status_operacao: _statusOperacao);
var ledMn = Sinaleiros.FirstOrDefault(x => x.ID == "LEDMN");
var ledAt = Sinaleiros.FirstOrDefault(x => x.ID == "LEDAT");
var ledOp = Sinaleiros.FirstOrDefault(x => x.ID == "LEDOP");
if (ledMn != null) ledMn.Comportamento = VariaveisEquipamento.ComportamentoLedPorStatus(Modo == ModoOperacao.Manual ? StatusLED.Aceso : StatusLED.Apagado);
if (ledAt != null) ledAt.Comportamento = VariaveisEquipamento.ComportamentoLedPorStatus(Modo == ModoOperacao.Manual ? StatusLED.Apagado : StatusLED.Aceso);
if (ledOp != null) ledOp.Comportamento = VariaveisEquipamento.ComportamentoLedPorStatus(StatusLED.Piscando, status_operacao: _statusOperacao);
}
}

31
AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs
View File

@ -307,6 +307,8 @@ namespace AgroBase.Models
public double SimulacaoRpmControle { get; set; } = 0;
public int TempoIniciarOperacao { get; set; } = 10;
public int TempoAguardarRetomada { get; set; } = 5;
public string ID { get; set; }
private static string _discoverySessionId = Guid.NewGuid().ToString("N");
@ -493,6 +495,7 @@ namespace AgroBase.Models
op.Parametros.Controle = new OperacaoParametrosControleModel()
{
RegistrarDadosPosProcessamento = false,
ControleManualAcionado = true,
MovimentoAutomatico = false,
DirecionalAutomatico = false,
@ -512,10 +515,11 @@ namespace AgroBase.Models
MovVelocidadeSErvasPercent = 35,
MovVelocidadeCErvasPercent = 12,
DirAnguloMaximo = 30,
DirVelocidadeMovimento = 40,
DirVelocidadeMovimento = 80,
DirTipoMovimento = TiposControladorDirecional.Manual,
DistanciaManobra = 3.0,
AtuAlturaAreaPulverizacao = 10,
AtuDuracaoAtuacao = 200,
AtuPercentualErvasBicoOff = 2,
@ -603,6 +607,7 @@ namespace AgroBase.Models
op.Parametros.Controle = new OperacaoParametrosControleModel()
{
RegistrarDadosPosProcessamento = false,
ControleManualAcionado = false,
MovimentoAutomatico = true,
DirecionalAutomatico = true,
@ -622,10 +627,11 @@ namespace AgroBase.Models
MovVelocidadeSErvasPercent = 50,
MovVelocidadeCErvasPercent = 20,
DirAnguloMaximo = 30,
DirVelocidadeMovimento = 50,
DirVelocidadeMovimento = 80,
DirTipoMovimento = TiposControladorDirecional.MPC,
DistanciaManobra = 3.0,
AtuAlturaAreaPulverizacao = 10,
AtuDuracaoAtuacao = 200,
AtuPercentualErvasBicoOff = 2,
@ -743,6 +749,7 @@ namespace AgroBase.Models
op.Parametros.Controle = new OperacaoParametrosControleModel()
{
RegistrarDadosPosProcessamento = false,
ControleManualAcionado = false,
MovimentoAutomatico = true,
DirecionalAutomatico = true,
@ -762,9 +769,11 @@ namespace AgroBase.Models
MovVelocidadeSErvasPercent = 50,
MovVelocidadeCErvasPercent = 20,
DirAnguloMaximo = 30,
DirVelocidadeMovimento = 50,
DirVelocidadeMovimento = 80,
DirTipoMovimento = TiposControladorDirecional.IA,
DistanciaManobra = 3.0,
AtuAlturaAreaPulverizacao = 10,
AtuDuracaoAtuacao = 200,
AtuPercentualErvasBicoOff = 2,
@ -880,6 +889,7 @@ namespace AgroBase.Models
op.Parametros.Controle = new OperacaoParametrosControleModel()
{
RegistrarDadosPosProcessamento = false,
ControleManualAcionado = false,
MovimentoAutomatico = true,
DirecionalAutomatico = true,
@ -899,10 +909,11 @@ namespace AgroBase.Models
MovVelocidadeSErvasPercent = 40,
MovVelocidadeCErvasPercent = 20,
DirAnguloMaximo = 30,
DirVelocidadeMovimento = 50,
DirVelocidadeMovimento = 80,
DirTipoMovimento = TiposControladorDirecional.PID,
DistanciaManobra = 3.0,
AtuAlturaAreaPulverizacao = 10,
AtuDuracaoAtuacao = 200,
AtuPercentualErvasBicoOff = 2,
@ -978,6 +989,7 @@ namespace AgroBase.Models
op.Parametros.Controle = new OperacaoParametrosControleModel()
{
RegistrarDadosPosProcessamento = false,
ControleManualAcionado = false,
MovimentoAutomatico = true,
DirecionalAutomatico = true,
@ -997,10 +1009,11 @@ namespace AgroBase.Models
MovVelocidadeSErvasPercent = 60,
MovVelocidadeCErvasPercent = 20,
DirAnguloMaximo = 30,
DirVelocidadeMovimento = 50,
DirVelocidadeMovimento = 80,
DirTipoMovimento = TiposControladorDirecional.MPC,
DistanciaManobra = 3.0,
AtuAlturaAreaPulverizacao = 10,
AtuDuracaoAtuacao = 200,
AtuPercentualErvasBicoOff = 2,

1
AgroBase/AgroBase/Models/Operadores/ManagerWorkerModel.cs
View File

@ -163,6 +163,7 @@ namespace AgroBase.Models.Operadores
AtualizarDadosControle = 3,
EnviarDadosControle = 4,
FinalizarOperacao = 5,
ReiniciarMPC = 6,
}
}

75
AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs
View File

@ -1,6 +1,7 @@
using AgroBase.Models.Operadores;
using AgroBase.Services;
using AgroBase.Services.Operadores;
using BlackSharp.Core.Extensions;
using Newtonsoft.Json;
using Newtonsoft.Json.Linq;
using System;
@ -999,19 +1000,51 @@ namespace AgroBase.Models
{
if (_TrajetoriaFixaDefinida && !VerificacaoInicialMeioRuaConcluida && ((considerarStatus && new List<StatusOperacao>() { StatusOperacao.Aguardando, StatusOperacao.EmAndamento }.Contains(Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.StatusOperacaoAtual)) || !considerarStatus))
{
var _posicaoAtual = GPSPosicaoAtual;
if (_posicaoAtual == null)
return false;
VerificacaoInicialMeioRuaConcluida = true;
(bool noCoredor, var idxPontoMaisProximo) = VerificaEquipamentoDentroCorredorMontado(_TrajetoriaFixa[0].Posicao);
if (noCoredor)
string msg = "";
var ultimoPonto = _TrajetoriaFixa[_TrajetoriaFixa.Count - 1];
if (ultimoPonto?.Posicao == null)
return false;
double distanciaUltimoPonto = GPSUtils.DistanciaEntrePontos(_posicaoAtual, ultimoPonto.Posicao);
//int idxCorredorDentro = VerificaEquipamentoDentroCorredor(_posicaoAtual);
if (!double.IsNaN(distanciaUltimoPonto) && !double.IsInfinity(distanciaUltimoPonto) && distanciaUltimoPonto < 5.0)
{
msg = $"Início no meio da rua ignorado: robô está a {distanciaUltimoPonto:0.00}m do fim. Nova operação começará do zero.";
Variaveis.OperacaoEmAndamento?.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Operante, 100, msg);
Variaveis.MostrarLog($"[TRJ] {msg}");
return false;
}
(bool noCoredor, var idxPontoMaisProximo) = VerificaEquipamentoDentroCorredorMontado(_posicaoAtual);
if (noCoredor && idxPontoMaisProximo >= 0 && idxPontoMaisProximo < _TrajetoriaFixa.Count)
{
for (int i = 0; i <= idxPontoMaisProximo; i++)
{
_TrajetoriaFixa[i].Visitado = true;
}
CorredorAtual.AtualizarDados();
CorredorAtual?.AtualizarDados();
msg = $"Início no meio da rua confirmado: idx={idxPontoMaisProximo}/{_TrajetoriaFixa.Count - 1}, marcados={idxPontoMaisProximo + 1}/{_TrajetoriaFixa.Count}.";
Variaveis.OperacaoEmAndamento?.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Alerta, 100, msg);
Variaveis.MostrarLog($"[TRJ] {msg}");
return true;
}
msg = $"Início no meio da rua ignorado: robô está fora do corredor. Nova operação começará do zero.";
Variaveis.OperacaoEmAndamento?.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Operante, 100, msg);
Variaveis.MostrarLog($"[TRJ] {msg}");
}
return false;
@ -2099,7 +2132,7 @@ namespace AgroBase.Models
// 2. Para cada ponto da rua1 interpolada, encontrar o mais próximo na rua2 interpolada
foreach (var ponto1 in rua1Interpolada)
{
var pontoMaisProximo = rua2Interpolada.OrderBy(ponto2 => GPSUtils.DistanciaEntrePontos(ponto1, ponto2)).First();
var pontoMaisProximo = rua2Interpolada.OrderBy(ponto2 => GPSUtils.DistanciaEntrePontos(ponto1, ponto2)).FirstOrDefault();
double distancia = GPSUtils.DistanciaEntrePontos(ponto1, pontoMaisProximo);
@ -2191,8 +2224,8 @@ namespace AgroBase.Models
CorredorAtual.Reverse();
}
double anguloProjetar1 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(1).First(), CorredorAtual.First());
GPSModel PrimeiroPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.First(), DistanciaProjecaoRua, anguloProjetar1);
double anguloProjetar1 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(1).FirstOrDefault(), CorredorAtual.FirstOrDefault());
GPSModel PrimeiroPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.FirstOrDefault(), DistanciaProjecaoRua, anguloProjetar1);
var ultimoPontoTrajetoria = _trajetoriaFixa.LastOrDefault();
@ -2220,8 +2253,8 @@ namespace AgroBase.Models
{
int pontosAdicionr = Convert.ToInt32(CorredoresLarguras[idx] / (DistanciaEntrePontosCurva * _FatorLarguraCorredor));
var _penultimoPonto = _trajetoriaFixa[_trajetoriaFixa.Count() - 2].Posicao;
double anguloProjecao = GPSUtils.CalcularOrientacao(Corredores[idx - 1].First(), Corredores[idx - 1].Last());
GPSModel _ultimoPonto = GPSUtils.ProjetarPontoDeslocado(Corredores[idx - 1].Last(), DistanciaProjecaoRua, anguloProjecao);
double anguloProjecao = GPSUtils.CalcularOrientacao(Corredores[idx - 1].FirstOrDefault(), Corredores[idx - 1].LastOrDefault());
GPSModel _ultimoPonto = GPSUtils.ProjetarPontoDeslocado(Corredores[idx - 1].LastOrDefault(), DistanciaProjecaoRua, anguloProjecao);
List<GPSModel> CurvaConexao = CriarCurvaEntrePontos(ultimoPontoTrajetoria.Posicao, PrimeiroPonto, DistanciaProjecaoRua * 2.0, anguloProjecao, pontosAdicionr, false);
for (int i = 0; i < CurvaConexao.Count; i++)
@ -2242,7 +2275,7 @@ namespace AgroBase.Models
idxPontoCorredor = _trajetoriaFixa.Count(x => x.idxCorredor == idx),
Posicao = ponto,
Direcao = pontoLigacao ? direcaoAtual : DirecaoCarroRua.Manobra,
Orientacao = GPSUtils.CalcularOrientacao(_trajetoriaFixa.Last().Posicao, ponto),
Orientacao = GPSUtils.CalcularOrientacao(_trajetoriaFixa.LastOrDefault().Posicao, ponto),
Visitado = false,
LarguraCorredor = larguraCorredorMenor * 0.9
});
@ -2284,7 +2317,7 @@ namespace AgroBase.Models
double distancia_projetar = !ultimoCorredor ? (DistanciaProjecaoRua * percentualAcrescimoPontoAberturaCurva) : (DistanciaProjecaoRua * percentualAcrescimoPontoAberturaCurva);
GPSModel ultimoPontoCorredorAtual = CorredorAtual.Last();
double anguloProjetar2 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(CorredorAtual.Count() - 2).First(), ultimoPontoCorredorAtual);
double anguloProjetar2 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(CorredorAtual.Count() - 2).FirstOrDefault(), ultimoPontoCorredorAtual);
GPSModel UltimoPonto = GPSUtils.ProjetarPontoDeslocado(ultimoPontoCorredorAtual, distancia_projetar, anguloProjetar2);
// Realiza um acréscimo no angulo à projetar, permitindo que o equipamento faça uma curva mais aberta entre corredores
@ -2798,6 +2831,8 @@ namespace AgroBase.Models
{
var lista = new List<GPSModel>();
if (a == null || b == null) return lista;
double dist = GPSUtils.DistanciaEntrePontos(a, b);
if (dist < espacamento_m)
{
@ -2850,8 +2885,10 @@ namespace AgroBase.Models
bool angCorretoI = Math.Abs(difAngI) <= limiar;
if (!angCorreto && !angCorretoI) continue;
var pInicio = corredor.First();
var pFim = corredor.Last();
var pInicio = corredor.FirstOrDefault();
var pFim = corredor.LastOrDefault();
if (pInicio == null || pFim == null) continue;
double dBaseInicio = GPSUtils.DistanciaEntrePontos(posBase, pInicio);
double dBaseFim = GPSUtils.DistanciaEntrePontos(posBase, pFim);
@ -2888,9 +2925,11 @@ namespace AgroBase.Models
{
var corredor = Corredores[c.idxCorredor];
var ponto =
c.condicaoCarro == TipoCondicaoRoboRua.EntraPeloInicio || c.condicaoCarro == TipoCondicaoRoboRua.SaiPeloInicio ? corredor.First() :
c.condicaoCarro == TipoCondicaoRoboRua.EntraPeloFim || c.condicaoCarro == TipoCondicaoRoboRua.SaiPeloFim ? corredor.Last() :
corredor.First();
c.condicaoCarro == TipoCondicaoRoboRua.EntraPeloInicio || c.condicaoCarro == TipoCondicaoRoboRua.SaiPeloInicio ? corredor.FirstOrDefault() :
c.condicaoCarro == TipoCondicaoRoboRua.EntraPeloFim || c.condicaoCarro == TipoCondicaoRoboRua.SaiPeloFim ? corredor.LastOrDefault() :
corredor.FirstOrDefault();
if (ponto == null) continue;
double comprimento = 0;
for (int i = 1; i < corredor.Count; i++)
@ -2920,8 +2959,8 @@ namespace AgroBase.Models
var corredor = Corredores[idxCorredor];
bool sentidoCorreto = condicaoCarro == TipoCondicaoRoboRua.EntraPeloInicio || condicaoCarro == TipoCondicaoRoboRua.SaiPeloInicio;
GPSModel entrada = sentidoCorreto ? corredor.First() : corredor.Last();
GPSModel saida = sentidoCorreto ? corredor.Last() : corredor.First();
GPSModel entrada = sentidoCorreto ? corredor.FirstOrDefault() : corredor.LastOrDefault();
GPSModel saida = sentidoCorreto ? corredor.LastOrDefault() : corredor.FirstOrDefault();
// 1️⃣ robô → entrada do corredor
pontos.AddRange(GerarTrechoReto(posRobo, entrada, DistanciaEntrePontos));
@ -3063,7 +3102,7 @@ namespace AgroBase.Models
if (pts.Count < 3) return pts;
// pivot: menor y, depois menor x
var pivot = pts.OrderBy(p => p.y).ThenBy(p => p.x).First();
var pivot = pts.OrderBy(p => p.y).ThenBy(p => p.x).FirstOrDefault();
// ordena por ângulo polar com pivot
var sorted = pts

4
AgroBase/AgroBase/Services/CANServiceSerial.cs
View File

@ -696,12 +696,12 @@ namespace AgroBase.Services
if (!_ultimoIdEnviado.HasValue || !idsDisponiveis.Contains(_ultimoIdEnviado.Value))
{
proximoId = idsDisponiveis.First();
proximoId = idsDisponiveis.FirstOrDefault();
}
else
{
var index = idsDisponiveis.IndexOf(_ultimoIdEnviado.Value);
proximoId = (index + 1 < idsDisponiveis.Count) ? idsDisponiveis[index + 1] : idsDisponiveis.First();
proximoId = (index + 1 < idsDisponiveis.Count) ? idsDisponiveis[index + 1] : idsDisponiveis.FirstOrDefault();
}
// Seleciona a mensagem mais antiga com esse IdTx

4
AgroBase/AgroBase/Services/CANServiceWaveshare.cs
View File

@ -510,12 +510,12 @@ namespace AgroBase.Services
if (!_ultimoIdEnviado.HasValue || !idsDisponiveis.Contains(_ultimoIdEnviado.Value))
{
proximoId = idsDisponiveis.First();
proximoId = idsDisponiveis.FirstOrDefault();
}
else
{
var index = idsDisponiveis.IndexOf(_ultimoIdEnviado.Value);
proximoId = (index + 1 < idsDisponiveis.Count) ? idsDisponiveis[index + 1] : idsDisponiveis.First();
proximoId = (index + 1 < idsDisponiveis.Count) ? idsDisponiveis[index + 1] : idsDisponiveis.FirstOrDefault();
}
// Seleciona a mensagem mais antiga com esse IdTx

4
AgroBase/AgroBase/Services/CanServiceEthernet.cs
View File

@ -480,11 +480,11 @@ namespace AgroBase.Services
uint proximoId;
if (!_ultimoIdEnviado.HasValue || !idsDisponiveis.Contains(_ultimoIdEnviado.Value))
proximoId = idsDisponiveis.First();
proximoId = idsDisponiveis.FirstOrDefault();
else
{
int index = idsDisponiveis.IndexOf(_ultimoIdEnviado.Value);
proximoId = (index + 1 < idsDisponiveis.Count) ? idsDisponiveis[index + 1] : idsDisponiveis.First();
proximoId = (index + 1 < idsDisponiveis.Count) ? idsDisponiveis[index + 1] : idsDisponiveis.FirstOrDefault();
}
return baseLista.Where(x => x.IdTx == proximoId).OrderBy(x => x.Momento).FirstOrDefault();

315
AgroBase/AgroBase/Services/CoulombCounterService.cs
View File

@ -33,9 +33,38 @@ namespace AgroBase.Services
private const double JanelaAutonomia_m = 150.0;
private const double JanelaAutonomiaTempo_s = 300.0;
private const double AlphaHistorico = 0.20;
private const double FatorConservadorHistorico = 1.10;
/*
* A EMA recente só é atualizada depois de acumular este deslocamento.
* Isso evita atualizar centenas de vezes com janelas quase idênticas.
*/
private const double PassoAtualizacaoEma_m = 5.0;
/*
* Reação assimétrica:
* - sobe rápido quando o consumo piora;
* - desce devagar quando o consumo melhora.
*/
private const double AlphaEmaSubida = 0.35;
private const double AlphaEmaDescida = 0.08;
/*
* Peso da condição recente na fusão.
* Se o trecho recente estiver pior que a média global,
* seu peso aumenta dinamicamente.
*/
private const double PesoRecenteNormal = 0.30;
private const double PesoRecenteMaximo = 0.60;
/*
* Barreiras contra divisões instáveis e amostras absurdas.
* O limite máximo deve ser generoso e serve apenas para rejeitar
* corrupção de dados, salto de distância ou intervalo elétrico inválido.
*/
private const double MinWhPorMetroValido = 0.01;
private const double MaxWhPorMetroValido = 5.00;
private const double DiferencaTensaoAlerta_V = 2.0;
private const double DiferencaCorrenteMinAlerta_A = 3.0;
private const double DiferencaCorrenteRelativaAlerta = 0.35;
@ -60,6 +89,31 @@ namespace AgroBase.Services
private readonly double _dalyCorrenteDescargaSinal;
private readonly double _sensorCorrenteDescargaSinal;
private double _somaDistanciaOperacao_m;
private double _somaTempoOperacao_s;
private double _somaEnergiaOperacao_Wh;
private double _distanciaDesdeAtualizacaoEma_m;
public double DistanciaHistoricoGlobal_m => _somaDistanciaOperacao_m;
public double TempoHistoricoGlobal_s => _somaTempoOperacao_s;
public double EnergiaHistoricoGlobal_Wh => _somaEnergiaOperacao_Wh;
public double DistanciaHistoricoJanela_m => _somaDistanciaJanela_m;
public double TempoHistoricoJanela_s => _somaTempoJanela_s;
public double EnergiaHistoricoJanela_Wh => _somaEnergiaJanela_Wh;
public double WhPorMetroGlobal { get; private set; }
public double WhPorMetroRecenteEma { get; private set; }
public double WhPorMetroCombinado { get; private set; }
public double PesoRecenteAplicado { get; private set; }
public bool HistoricoGlobalValido { get; private set; }
public bool HistoricoRecenteValido { get; private set; }
private class FonteEnergiaSnapshot
{
public string Nome { get; set; } = "";
@ -84,7 +138,7 @@ namespace AgroBase.Services
double max_Ah,
double wh_m_base = 0.30,
double tensao_nominal_V = 36.0,
double daly_corrente_descarga_sinal = -1.0,
double daly_corrente_descarga_sinal = 1.0,
double sensor_corrente_descarga_sinal = 1.0,
double timeout_bms_s = 6.0,
double timeout_sensor_s = 4.0)
@ -1113,10 +1167,15 @@ namespace AgroBase.Services
_distanciaPendenteAutonomia_m += deltaDist_m;
}
private void FecharIntervaloAutonomia(
double deltaTempo_s,
double deltaEnergia_Wh)
private void FecharIntervaloAutonomia(double deltaTempo_s, double deltaEnergia_Wh)
{
if (!OperacaoAtiva || OperacaoFinalizada)
{
DescartarIntervaloPendente("fora_de_operacao");
RecalcularAutonomia();
return;
}
if (_aguardandoBaselineAutonomia)
{
DescartarIntervaloPendente("baseline_autonomia");
@ -1136,7 +1195,7 @@ namespace AgroBase.Services
return;
}
AtualizarJanelaAutonomia(
AtualizarHistoricoAutonomia(
_distanciaPendenteAutonomia_m,
deltaTempo_s,
deltaEnergia_Wh
@ -1191,42 +1250,144 @@ namespace AgroBase.Services
return deltaDist_m;
}
private void AtualizarJanelaAutonomia(double deltaDist_m, double deltaTempo_s, double deltaEnergia_Wh)
private void AtualizarHistoricoAutonomia(double deltaDist_m, double deltaTempo_s, double deltaEnergia_Wh)
{
if (!ValorFinito(deltaDist_m) ||
!ValorFinito(deltaTempo_s) ||
!ValorFinito(deltaEnergia_Wh))
{
EstadoIntervaloAutonomia = "amostra_nao_finita";
return;
}
deltaDist_m = Math.Max(0, deltaDist_m);
deltaTempo_s = Math.Max(0, deltaTempo_s);
deltaEnergia_Wh = Math.Max(0, deltaEnergia_Wh);
/*
* Mesmo com distância zero, a energia entra na janela.
* Assim espera, calibração, manobras paradas e processamento
* continuam compondo o custo real da operação.
* O histórico global e a janela recebem exatamente os mesmos
* intervalos elétricos considerados confiáveis.
*
* Energia gasta parado também entra. Quando o robô voltar a andar,
* esse custo fará parte do Wh/m real da operação.
*/
_janelaAutonomia.Enqueue((deltaDist_m, deltaTempo_s, deltaEnergia_Wh));
_somaDistanciaOperacao_m += deltaDist_m;
_somaTempoOperacao_s += deltaTempo_s;
_somaEnergiaOperacao_Wh += deltaEnergia_Wh;
_janelaAutonomia.Enqueue(
(deltaDist_m, deltaTempo_s, deltaEnergia_Wh)
);
_somaDistanciaJanela_m += deltaDist_m;
_somaTempoJanela_s += deltaTempo_s;
_somaEnergiaJanela_Wh += deltaEnergia_Wh;
while ((_somaDistanciaJanela_m > JanelaAutonomia_m || _somaTempoJanela_s > JanelaAutonomiaTempo_s) && _janelaAutonomia.Count > 0)
/*
* Remove amostras antigas até a janela voltar aos limites.
* Ela representa no máximo os últimos 150 m ou 300 s.
*/
while ((_somaDistanciaJanela_m > JanelaAutonomia_m ||
_somaTempoJanela_s > JanelaAutonomiaTempo_s) &&
_janelaAutonomia.Count > 1)
{
var old = _janelaAutonomia.Dequeue();
_somaDistanciaJanela_m -= old.dist_m;
_somaTempoJanela_s -= old.tempo_s;
_somaEnergiaJanela_Wh -= old.energia_Wh;
var antiga = _janelaAutonomia.Dequeue();
_somaDistanciaJanela_m -= antiga.dist_m;
_somaTempoJanela_s -= antiga.tempo_s;
_somaEnergiaJanela_Wh -= antiga.energia_Wh;
}
if (_somaDistanciaJanela_m > MinDistanciaTaxa_m)
/*
* Corrige pequenos resíduos negativos de ponto flutuante.
*/
_somaDistanciaJanela_m =
Math.Max(0, _somaDistanciaJanela_m);
_somaTempoJanela_s =
Math.Max(0, _somaTempoJanela_s);
_somaEnergiaJanela_Wh =
Math.Max(0, _somaEnergiaJanela_Wh);
if (_somaDistanciaOperacao_m >= MinDistanciaTaxa_m)
{
WhPorMetroJanela = _somaEnergiaJanela_Wh / _somaDistanciaJanela_m;
double taxaGlobal =
_somaEnergiaOperacao_Wh /
_somaDistanciaOperacao_m;
if (TaxaWhPorMetroValida(taxaGlobal))
{
WhPorMetroGlobal = taxaGlobal;
}
}
if (_somaDistanciaJanela_m >= MinDistanciaHistorico_m && WhPorMetroJanela > 0)
if (_somaDistanciaJanela_m >= MinDistanciaTaxa_m)
{
WhPorMetroHistorico = WhPorMetroHistorico <= 0 ? WhPorMetroJanela : WhPorMetroHistorico * (1.0 - AlphaHistorico) + WhPorMetroJanela * AlphaHistorico;
double taxaJanela =
_somaEnergiaJanela_Wh /
_somaDistanciaJanela_m;
AutonomiaComHistorico = true;
if (TaxaWhPorMetroValida(taxaJanela))
{
WhPorMetroJanela = taxaJanela;
}
}
HistoricoGlobalValido =
_somaDistanciaOperacao_m >= MinDistanciaHistorico_m &&
TaxaWhPorMetroValida(WhPorMetroGlobal);
HistoricoRecenteValido =
_somaDistanciaJanela_m >= MinDistanciaHistorico_m &&
TaxaWhPorMetroValida(WhPorMetroJanela);
_distanciaDesdeAtualizacaoEma_m += deltaDist_m;
/*
* Atualiza a EMA a cada 5 m, e não a cada frame.
* Isso evita dar peso repetido à mesma janela sobreposta.
*/
if (HistoricoRecenteValido &&
(_distanciaDesdeAtualizacaoEma_m >= PassoAtualizacaoEma_m ||
WhPorMetroRecenteEma <= 0))
{
AtualizarEmaRecente(WhPorMetroJanela);
_distanciaDesdeAtualizacaoEma_m = 0;
}
AutonomiaComHistorico =
HistoricoGlobalValido &&
HistoricoRecenteValido &&
TaxaWhPorMetroValida(WhPorMetroRecenteEma);
}
private void AtualizarEmaRecente(double taxaAtual)
{
if (!TaxaWhPorMetroValida(taxaAtual))
return;
if (!TaxaWhPorMetroValida(WhPorMetroRecenteEma))
{
WhPorMetroRecenteEma = taxaAtual;
return;
}
double alpha =
taxaAtual > WhPorMetroRecenteEma
? AlphaEmaSubida
: AlphaEmaDescida;
WhPorMetroRecenteEma =
WhPorMetroRecenteEma * (1.0 - alpha) +
taxaAtual * alpha;
}
private static bool TaxaWhPorMetroValida(double taxa)
{
return ValorFinito(taxa) &&
taxa >= MinWhPorMetroValido &&
taxa <= MaxWhPorMetroValido;
}
private void RecalcularAutonomia()
@ -1237,38 +1398,115 @@ namespace AgroBase.Services
return;
}
if (AutonomiaComHistorico && WhPorMetroHistorico > 0)
if (AutonomiaComHistorico)
{
FonteAutonomia = "historico_operacional";
FonteAutonomia = "historico_global_recente";
double consumoObservado = Math.Max(WhPorMetroJanela, WhPorMetroHistorico);
/*
* Normalmente o recente recebe 30%.
*
* Se o recente estiver consumindo mais que a média global,
* seu peso cresce progressivamente até 60%.
*
* Se estiver consumindo menos, permanece em 30%, evitando que
* um pequeno trecho leve gere uma autonomia otimista demais.
*/
double razaoRecenteGlobal =
WhPorMetroRecenteEma /
Math.Max(WhPorMetroGlobal, MinWhPorMetroValido);
WhPorMetroReferencia = consumoObservado * FatorConservadorHistorico;
if (razaoRecenteGlobal > 1.0)
{
double agravamento =
Clamp(razaoRecenteGlobal - 1.0, 0, 1.0);
PesoRecenteAplicado =
PesoRecenteNormal +
agravamento *
(PesoRecenteMaximo - PesoRecenteNormal);
}
else
{
PesoRecenteAplicado = PesoRecenteNormal;
}
double pesoGlobal = 1.0 - PesoRecenteAplicado;
WhPorMetroCombinado =
WhPorMetroGlobal * pesoGlobal +
WhPorMetroRecenteEma * PesoRecenteAplicado;
WhPorMetroHistorico = WhPorMetroCombinado;
WhPorMetroReferencia =
WhPorMetroCombinado *
FatorConservadorHistorico;
}
else if (HistoricoGlobalValido)
{
/*
* A operação já tem distância global suficiente,
* mas a janela/EMA recente ainda está amadurecendo.
*/
FonteAutonomia = "historico_global_inicial";
PesoRecenteAplicado = 0;
WhPorMetroCombinado = WhPorMetroGlobal;
WhPorMetroHistorico = WhPorMetroGlobal;
WhPorMetroReferencia =
WhPorMetroGlobal *
FatorConservadorHistorico;
}
else if (WhPorMetroJanela > 0)
{
FonteAutonomia = "janela_inicial";
WhPorMetroReferencia = Math.Max(WhPorMetroBase, WhPorMetroJanela);
PesoRecenteAplicado = 0;
WhPorMetroCombinado = 0;
WhPorMetroHistorico = 0;
/*
* Antes de 20 m, o fallback ainda funciona como piso.
*/
WhPorMetroReferencia =
Math.Max(
WhPorMetroBase,
WhPorMetroJanela
);
}
else
{
FonteAutonomia = "fallback_campo";
PesoRecenteAplicado = 0;
WhPorMetroCombinado = 0;
WhPorMetroHistorico = 0;
WhPorMetroReferencia = WhPorMetroBase;
}
WhPorMetro = WhPorMetroReferencia;
if (WhUtilRestante > 0 && WhPorMetroReferencia > 0)
if (WhUtilRestante > 0 &&
WhPorMetroReferencia > 0)
{
DistanciaMaximaTeorica_m = WhUtilRestante / WhPorMetroReferencia;
DistanciaMaximaTeorica_m =
WhUtilRestante /
WhPorMetroReferencia;
double margem = Clamp(MargemSegurancaAutonomia, 0, 0.90);
double margem =
Clamp(
MargemSegurancaAutonomia,
0,
0.90
);
DistanciaMaximaSegura_m = DistanciaMaximaTeorica_m * (1.0 - margem);
DistanciaMaximaSegura_m =
DistanciaMaximaTeorica_m *
(1.0 - margem);
DistanciaEstimadaRestante_m = DistanciaMaximaSegura_m;
DistanciaEstimadaRestante_m =
DistanciaMaximaSegura_m;
}
else
{
@ -1438,6 +1676,21 @@ namespace AgroBase.Services
DistanciaMaximaTeorica_m = 0;
DistanciaMaximaSegura_m = 0;
DistanciaEstimadaRestante_m = 0;
_somaDistanciaOperacao_m = 0;
_somaTempoOperacao_s = 0;
_somaEnergiaOperacao_Wh = 0;
_distanciaDesdeAtualizacaoEma_m = 0;
WhPorMetroGlobal = 0;
WhPorMetroRecenteEma = 0;
WhPorMetroCombinado = 0;
PesoRecenteAplicado = 0;
HistoricoGlobalValido = false;
HistoricoRecenteValido = false;
}
private void ResetarTudo()

397
AgroBase/AgroBase/Services/HerbicideCounterService.cs
View File

@ -312,9 +312,15 @@ namespace AgroBase.Services
private double _somaTempoPulverizacao_s;
private double _somaVolumePulverizacaoTempo_L;
private double _somaDistanciaOperacional_m;
private double _somaTempoOperacional_s;
private double _somaVolumeOperacional_L;
private double _somaDistanciaOperacionalJanela_m;
private double _somaTempoOperacionalJanela_s;
private double _somaVolumeOperacionalJanela_L;
private double _somaDistanciaOperacionalGlobal_m;
private double _somaTempoOperacionalGlobal_s;
private double _somaVolumeOperacionalGlobal_L;
private double _distanciaDesdeAtualizacaoEmaOperacional_m;
private double _volumeAnterior_L;
private double _distanciaTotalAnterior_m;
@ -329,6 +335,42 @@ namespace AgroBase.Services
private int _atuacoesInicioOperacao;
private const double PassoAtualizacaoEmaOperacional_m = 5.0;
private const double AlphaEmaOperacionalSubida = 0.35;
private const double AlphaEmaOperacionalDescida = 0.08;
private const double PesoRecenteOperacionalNormal = 0.30;
private const double PesoRecenteOperacionalMaximo = 0.60;
/*
* Limites amplos para rejeitar corrupção de dados.
* Não devem limitar condições reais de campo.
*/
private const double MinLPorMetroOperacionalValido = 0.00001;
private const double MaxLPorMetroOperacionalValido = 1.00;
public double DistanciaOperacionalGlobal_m => _somaDistanciaOperacionalGlobal_m;
public double TempoOperacionalGlobal_s => _somaTempoOperacionalGlobal_s;
public double VolumeOperacionalGlobal_L => _somaVolumeOperacionalGlobal_L;
public double DistanciaOperacionalJanela_m => _somaDistanciaOperacionalJanela_m;
public double TempoOperacionalJanela_s => _somaTempoOperacionalJanela_s;
public double VolumeOperacionalJanela_L => _somaVolumeOperacionalJanela_L;
public double LPorMetroOperacionalGlobal { get; private set; }
public double LPorMetroOperacionalJanela { get; private set; }
public double LPorMetroOperacionalRecenteEma { get; private set; }
public double LPorMetroOperacionalCombinado { get; private set; }
public double PesoRecenteOperacionalAplicado { get; private set; }
public bool HistoricoOperacionalGlobalValido { get; private set; }
public bool HistoricoOperacionalRecenteValido { get; private set; }
// ============================================================
// Ciclo de vida
// ============================================================
@ -1132,20 +1174,42 @@ namespace AgroBase.Services
}
}
private void AtualizarJanelaOperacional(
double deltaDist_m,
double deltaTempo_s,
double deltaVolume_L)
private void AtualizarJanelaOperacional(double deltaDist_m, double deltaTempo_s, double deltaVolume_L)
{
deltaDist_m =
Math.Max(0, deltaDist_m);
if (!OperacaoAtiva || OperacaoFinalizada)
return;
deltaTempo_s =
Math.Max(0, deltaTempo_s);
if (!ValorFinito(deltaDist_m) ||
!ValorFinito(deltaTempo_s) ||
!ValorFinito(deltaVolume_L))
{
return;
}
deltaVolume_L =
Math.Max(0, deltaVolume_L);
deltaDist_m = Math.Max(0, deltaDist_m);
deltaTempo_s = Math.Max(0, deltaTempo_s);
deltaVolume_L = Math.Max(0, deltaVolume_L);
if (deltaDist_m <= 0 &&
deltaTempo_s <= 0 &&
deltaVolume_L <= 0)
{
return;
}
/*
* Histórico global da operação corrente.
*
* Todo deslocamento entra, mesmo sem pulverização.
* O volume entra somente quando o consumo foi validado.
*/
_somaDistanciaOperacionalGlobal_m += deltaDist_m;
_somaTempoOperacionalGlobal_s += deltaTempo_s;
_somaVolumeOperacionalGlobal_L += deltaVolume_L;
/*
* Janela operacional recente.
*/
_janelaOperacional.Enqueue(
(
deltaDist_m,
@ -1154,66 +1218,161 @@ namespace AgroBase.Services
)
);
_somaDistanciaOperacional_m +=
deltaDist_m;
_somaTempoOperacional_s +=
deltaTempo_s;
_somaVolumeOperacional_L +=
deltaVolume_L;
_somaDistanciaOperacionalJanela_m += deltaDist_m;
_somaTempoOperacionalJanela_s += deltaTempo_s;
_somaVolumeOperacionalJanela_L += deltaVolume_L;
while (
(
_somaDistanciaOperacional_m >
_somaDistanciaOperacionalJanela_m >
JanelaOperacional_m ||
_somaTempoOperacional_s >
_somaTempoOperacionalJanela_s >
JanelaOperacionalTempo_s
) &&
_janelaOperacional.Count > 0
_janelaOperacional.Count > 1
)
{
var old =
_janelaOperacional.Dequeue();
var antiga = _janelaOperacional.Dequeue();
_somaDistanciaOperacional_m -=
old.dist_m;
_somaTempoOperacional_s -=
old.tempo_s;
_somaVolumeOperacional_L -=
old.vol_L;
_somaDistanciaOperacionalJanela_m -= antiga.dist_m;
_somaTempoOperacionalJanela_s -= antiga.tempo_s;
_somaVolumeOperacionalJanela_L -= antiga.vol_L;
}
if (
_somaDistanciaOperacional_m >
MinDistanciaTaxa_m
)
_somaDistanciaOperacionalJanela_m =
Math.Max(0, _somaDistanciaOperacionalJanela_m);
_somaTempoOperacionalJanela_s =
Math.Max(0, _somaTempoOperacionalJanela_s);
_somaVolumeOperacionalJanela_L =
Math.Max(0, _somaVolumeOperacionalJanela_L);
/*
* Média global da operação.
*/
if (_somaDistanciaOperacionalGlobal_m >=
MinDistanciaTaxa_m)
{
LPorMetroOperacional =
_somaVolumeOperacional_L /
_somaDistanciaOperacional_m;
double taxaGlobal =
_somaVolumeOperacionalGlobal_L /
_somaDistanciaOperacionalGlobal_m;
if (TaxaLPorMetroOperacionalValida(taxaGlobal))
{
LPorMetroOperacionalGlobal = taxaGlobal;
}
}
if (
deltaDist_m > 0 &&
_somaDistanciaOperacional_m >=
/*
* Média da janela recente.
*/
if (_somaDistanciaOperacionalJanela_m >=
MinDistanciaTaxa_m)
{
double taxaJanela =
_somaVolumeOperacionalJanela_L /
_somaDistanciaOperacionalJanela_m;
/*
* A propriedade antiga continua representando a janela.
*/
LPorMetroOperacional = taxaJanela;
LPorMetroOperacionalJanela = taxaJanela;
}
HistoricoOperacionalGlobalValido =
_somaDistanciaOperacionalGlobal_m >=
MinDistanciaHistoricoOperacional_m &&
LPorMetroOperacional > 0
TaxaLPorMetroOperacionalValida(
LPorMetroOperacionalGlobal
);
HistoricoOperacionalRecenteValido =
_somaDistanciaOperacionalJanela_m >=
MinDistanciaHistoricoOperacional_m &&
TaxaLPorMetroOperacionalValida(
LPorMetroOperacionalJanela
);
_distanciaDesdeAtualizacaoEmaOperacional_m +=
deltaDist_m;
/*
* Atualiza a EMA apenas a cada 5 m.
* Isso evita aplicar peso repetido à mesma janela sobreposta.
*/
if (
HistoricoOperacionalRecenteValido &&
(
_distanciaDesdeAtualizacaoEmaOperacional_m >=
PassoAtualizacaoEmaOperacional_m ||
!TaxaLPorMetroOperacionalValida(
LPorMetroOperacionalRecenteEma
)
)
)
{
LPorMetroOperacionalHistorico =
LPorMetroOperacionalHistorico <= 0
? LPorMetroOperacional
: LPorMetroOperacionalHistorico *
(1.0 - AlphaHistorico) +
LPorMetroOperacional *
AlphaHistorico;
AtualizarEmaOperacionalRecente(
LPorMetroOperacionalJanela
);
AutonomiaOperacionalComHistorico =
true;
_distanciaDesdeAtualizacaoEmaOperacional_m = 0;
}
AutonomiaOperacionalComHistorico =
HistoricoOperacionalGlobalValido &&
HistoricoOperacionalRecenteValido &&
TaxaLPorMetroOperacionalValida(
LPorMetroOperacionalRecenteEma
);
}
private void AtualizarEmaOperacionalRecente(double taxaAtual_LPorMetro)
{
if (!TaxaLPorMetroOperacionalValida(
taxaAtual_LPorMetro))
{
return;
}
if (!TaxaLPorMetroOperacionalValida(
LPorMetroOperacionalRecenteEma))
{
LPorMetroOperacionalRecenteEma =
taxaAtual_LPorMetro;
return;
}
double alpha =
taxaAtual_LPorMetro >
LPorMetroOperacionalRecenteEma
? AlphaEmaOperacionalSubida
: AlphaEmaOperacionalDescida;
LPorMetroOperacionalRecenteEma =
LPorMetroOperacionalRecenteEma *
(1.0 - alpha) +
taxaAtual_LPorMetro *
alpha;
}
private static bool TaxaLPorMetroOperacionalValida(double taxa_LPorMetro)
{
return
ValorFinito(taxa_LPorMetro) &&
taxa_LPorMetro >=
MinLPorMetroOperacionalValido &&
taxa_LPorMetro <=
MaxLPorMetroOperacionalValido;
}
private static bool ValorFinito(double valor)
{
return
!double.IsNaN(valor) &&
!double.IsInfinity(valor);
}
// ============================================================
@ -1247,33 +1406,100 @@ namespace AgroBase.Services
private double CalcularLPorMetroOperacionalReferencia()
{
if (
AutonomiaOperacionalComHistorico &&
LPorMetroOperacionalHistorico > 0
)
if (AutonomiaOperacionalComHistorico)
{
FonteAutonomiaOperacional =
"historico_operacional";
"historico_global_recente";
double global =
LPorMetroOperacionalGlobal;
double recente =
LPorMetroOperacionalRecenteEma;
/*
* Usa o maior entre a janela recente e o histórico suavizado.
* Assim uma região recente mais carregada de ervas eleva
* imediatamente a estimativa, mas uma região limpa não
* derruba a autonomia de forma otimista demais.
* Se a situação recente estiver pior, ela ganha mais peso.
* O aumento é progressivo e limitado a 60%.
*/
double razaoRecenteGlobal =
recente /
Math.Max(
global,
MinLPorMetroOperacionalValido
);
if (razaoRecenteGlobal > 1.0)
{
double agravamento =
Clamp(
razaoRecenteGlobal - 1.0,
0,
1.0
);
PesoRecenteOperacionalAplicado =
PesoRecenteOperacionalNormal +
agravamento *
(
PesoRecenteOperacionalMaximo -
PesoRecenteOperacionalNormal
);
}
else
{
/*
* Um trecho recente mais limpo pode melhorar a autonomia,
* mas recebe apenas 30% e a EMA já desce lentamente.
*/
PesoRecenteOperacionalAplicado =
PesoRecenteOperacionalNormal;
}
double pesoGlobal =
1.0 -
PesoRecenteOperacionalAplicado;
LPorMetroOperacionalCombinado =
global * pesoGlobal +
recente * PesoRecenteOperacionalAplicado;
LPorMetroOperacionalHistorico =
LPorMetroOperacionalCombinado;
return LPorMetroOperacionalCombinado;
}
if (HistoricoOperacionalGlobalValido)
{
FonteAutonomiaOperacional =
"historico_global_inicial";
PesoRecenteOperacionalAplicado = 0;
LPorMetroOperacionalCombinado =
LPorMetroOperacionalGlobal;
LPorMetroOperacionalHistorico =
LPorMetroOperacionalGlobal;
return Math.Max(
LPorMetroOperacional,
LPorMetroOperacionalHistorico
LPorMetroOperacionalGlobal,
LPorMetroBase
);
}
if (LPorMetroOperacional > 0)
if (TaxaLPorMetroOperacionalValida(
LPorMetroOperacionalJanela))
{
FonteAutonomiaOperacional =
"janela_operacional_inicial";
PesoRecenteOperacionalAplicado = 0;
LPorMetroOperacionalCombinado = 0;
LPorMetroOperacionalHistorico = 0;
return Math.Max(
LPorMetroOperacional,
LPorMetroOperacionalJanela,
LPorMetroBase
);
}
@ -1281,6 +1507,10 @@ namespace AgroBase.Services
FonteAutonomiaOperacional =
"fallback_campo";
PesoRecenteOperacionalAplicado = 0;
LPorMetroOperacionalCombinado = 0;
LPorMetroOperacionalHistorico = 0;
return LPorMetroBase;
}
@ -1567,23 +1797,38 @@ namespace AgroBase.Services
_somaTempoPulverizacao_s = 0;
_somaVolumePulverizacaoTempo_L = 0;
_somaDistanciaOperacional_m = 0;
_somaTempoOperacional_s = 0;
_somaVolumeOperacional_L = 0;
_somaDistanciaOperacionalGlobal_m = 0;
_somaTempoOperacionalGlobal_s = 0;
_somaVolumeOperacionalGlobal_L = 0;
_somaDistanciaOperacionalJanela_m = 0;
_somaTempoOperacionalJanela_s = 0;
_somaVolumeOperacionalJanela_L = 0;
_distanciaDesdeAtualizacaoEmaOperacional_m = 0;
LPorMetroOperacional = 0;
LPorMetroOperacionalGlobal = 0;
LPorMetroOperacionalJanela = 0;
LPorMetroOperacionalRecenteEma = 0;
LPorMetroOperacionalCombinado = 0;
LPorMetroOperacionalHistorico = 0;
PesoRecenteOperacionalAplicado = 0;
HistoricoOperacionalGlobalValido = false;
HistoricoOperacionalRecenteValido = false;
AutonomiaOperacionalComHistorico = false;
FonteAutonomiaOperacional = "fallback_campo";
LPorMetro = 0;
LPorMetroHistorico = 0;
LPorMetroOperacional = 0;
LPorMetroOperacionalHistorico = 0;
LPorMetroReferenciaOperacional = 0;
LPorMetroReferenciaPulverizacaoContinua = 0;
LPorMinuto = 0;
AutonomiaOperacionalComHistorico = false;
FonteAutonomiaOperacional = "fallback_campo";
}
private static double Clamp(

5
AgroBase/AgroBase/Services/MapasService.cs
View File

@ -223,11 +223,12 @@ namespace AgroBase.Services
* Mantemos a compatibilidade com o fluxo atual:
* a seleção usa IDs internos começando em zero.
*/
feature.geometry.id = i.ToString();
string id = (i + 1).ToString();
feature.geometry.id = id;
if (string.IsNullOrWhiteSpace(feature.properties.Id))
{
feature.properties.Id = (i + 1).ToString();
feature.properties.Id = id;
}
}
}

1
AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs
View File

@ -306,6 +306,7 @@ namespace AgroBase.Services.Operadores
("configurado", true),
("modo", op.Parametros?.Modo ?? ModoOperacao.NaoDefinido),
("tempo_aguardar_inicio_operacao", op.TempoIniciarOperacao),
("tempo_aguardar_retomada", op.TempoAguardarRetomada),
("modulos_mandatorios", _Parametros.ModulosMandatorios.Where(x => x.Mandatorio && x.Utilizar).Select(x => (int)x.Dispositivo).ToArray()),
("modulos_opcionais", _Parametros.ModulosMandatorios.Where(x => !x.Mandatorio && x.Utilizar).Select(x => (int)x.Dispositivo).ToArray()),
("parametros_mandatorios", _Parametros.ParametrosMandatorios.Where(x => x.Mandatorio && x.Utilizar).Select(x => (int)x.Parametro).ToArray()),

16
AgroBase/AgroBase/Services/Visualizador3DService.cs
View File

@ -26,18 +26,18 @@ namespace AgroBase.Services
private float positionX = 0.0f;
private float positionY = 0.0f;
private bool isMiddleButtonDragging = false;
private double AnguloX { get; set; }
private double AnguloY { get; set; }
private double AnguloX { get; set; }
private double AnguloZ { get; set; }
private string Path3D { get; set; }
public void AtualizarAngulos(double X, double Y, double Z, double Tolerance = 1.0)
public void AtualizarAngulos(double Lateral, double Frontal, double Rotacao, double Tolerance = 1.0)
{
if (Math.Abs(AnguloX - X) > Tolerance || Math.Abs(AnguloY - Y) > Tolerance || Math.Abs(AnguloZ - Z) > Tolerance)
if (Math.Abs(AnguloX - Frontal) > Tolerance || Math.Abs(AnguloY - Lateral) > Tolerance || Math.Abs(AnguloZ - Rotacao) > Tolerance)
{
AnguloX = X;
AnguloY = Y;
AnguloZ = Z;
AnguloX = Frontal;
AnguloY = -Lateral;
AnguloZ = -Rotacao;
glControl.Invalidate();
glControl.SwapBuffers();
}
@ -158,8 +158,8 @@ namespace AgroBase.Services
DrawText("O", -10.0f, 0.0f, -0.05f);
// Aplicar inclinação ao objeto 3D
GL.Rotate(AnguloX, 0.0, 0.0, 1.0);
GL.Rotate(AnguloY, 1.0, 0.0, 0.0);
GL.Rotate(AnguloY, 0.0, 0.0, 1.0);
GL.Rotate(AnguloX, 1.0, 0.0, 0.0);
GL.Rotate(AnguloZ - 180, 0.0, 1.0, 0.0);
// Desenhar o objeto 3D

40
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_imu.py
View File

@ -46,6 +46,9 @@ class IMUCamera(ModuloDiagnosticoBase):
raw_acc_alpha=0.04,
raw_max_acc_erro_g=0.18,
referencial_robo="r,p,y",
sinal_roll=1.0,
sinal_pitch=1.0,
sinal_yaw=1.0,
):
self.mx_id = mx_id
self.imu_queue = queue
@ -139,6 +142,9 @@ class IMUCamera(ModuloDiagnosticoBase):
self._calib_pitch_hist = deque(maxlen=200)
self.referencial_robo = str(referencial_robo or "r,p,y").lower().replace(" ", "")
self.sinal_roll = -1.0 if float(sinal_roll) < 0 else 1.0
self.sinal_pitch = -1.0 if float(sinal_pitch) < 0 else 1.0
self.sinal_yaw = -1.0 if float(sinal_yaw) < 0 else 1.0
self.matriz_sensor_para_robo = (self._criar_matriz_sensor_para_robo(self.referencial_robo))
if queue is None:
@ -455,8 +461,10 @@ class IMUCamera(ModuloDiagnosticoBase):
self.yaw_filtrado_deg = yaw
return
self.roll_filtrado_deg = (1.0 - a) * self.roll_filtrado_deg + a * roll
self.pitch_filtrado_deg = (1.0 - a) * self.pitch_filtrado_deg + a * pitch
droll = self._wrap_angle_delta_deg(roll, self.roll_filtrado_deg)
dpitch = self._wrap_angle_delta_deg(pitch, self.pitch_filtrado_deg)
self.roll_filtrado_deg = self._normalizar_angulo_deg(self.roll_filtrado_deg + a * droll)
self.pitch_filtrado_deg = self._normalizar_angulo_deg(self.pitch_filtrado_deg + a * dpitch)
# Yaw com wrap
dyaw = self._wrap_angle_delta_deg(yaw, self.yaw_filtrado_deg)
@ -575,6 +583,9 @@ class IMUCamera(ModuloDiagnosticoBase):
return R.from_matrix(matriz_robo)
def _normalizar_angulo_deg(self, angulo):
return (float(angulo) + 180.0) % 360.0 - 180.0
# ==========================================================
# Calibração
# ==========================================================
@ -928,27 +939,34 @@ class IMUCamera(ModuloDiagnosticoBase):
# Correção por accelerômetro apenas quando parece gravidade.
if self._raw_acc_confiavel(a):
alpha_acc = self.raw_acc_alpha
self.raw_roll_abs_deg = (1.0 - alpha_acc) * roll_pred + alpha_acc * roll_acc_abs
self.raw_pitch_abs_deg = (1.0 - alpha_acc) * pitch_pred + alpha_acc * pitch_acc_abs
erro_roll = self._wrap_angle_delta_deg(roll_acc_abs, roll_pred)
self.raw_roll_abs_deg = self._normalizar_angulo_deg(roll_pred + alpha_acc * erro_roll)
erro_pitch = self._wrap_angle_delta_deg(pitch_acc_abs, pitch_pred)
self.raw_pitch_abs_deg = self._normalizar_angulo_deg(pitch_pred + alpha_acc * erro_pitch)
else:
self.raw_roll_abs_deg = roll_pred
self.raw_pitch_abs_deg = pitch_pred
self.raw_yaw_abs_deg = (yaw_pred + 180.0) % 360.0 - 180.0
roll = self.raw_roll_abs_deg - self.raw_roll_offset_deg
pitch = self.raw_pitch_abs_deg - self.raw_pitch_offset_deg
yaw = self.raw_yaw_abs_deg - self.raw_yaw_offset_deg
yaw = (yaw + 180.0) % 360.0 - 180.0
roll = self._wrap_angle_delta_deg(self.raw_roll_abs_deg, self.raw_roll_offset_deg)
pitch = self._wrap_angle_delta_deg(self.raw_pitch_abs_deg, self.raw_pitch_offset_deg)
yaw = self._wrap_angle_delta_deg(self.raw_yaw_abs_deg, self.raw_yaw_offset_deg)
roll = self._normalizar_angulo_deg(roll * self.sinal_roll)
pitch = self._normalizar_angulo_deg(pitch * self.sinal_pitch)
yaw = self._normalizar_angulo_deg(yaw * self.sinal_yaw)
self.roll_deg = float(roll)
self.pitch_deg = float(pitch)
self.yaw_deg = float(yaw)
# Para raw, é melhor usar o gyro como rate direto.
self.roll_rate_dps = gx
self.pitch_rate_dps = gy
self.yaw_rate_dps = gz
self.roll_rate_dps = gx * self.sinal_roll
self.pitch_rate_dps = gy * self.sinal_pitch
self.yaw_rate_dps = gz * self.sinal_yaw
self._last_angle_ts = ts_integracao
self._last_roll_deg = roll

13
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py
View File

@ -55,8 +55,7 @@ class CameraMultispectral:
self.imu_freq_hz = 200
self.imu_publish_hz = 50
self.imu_sensor_type = "GAME_ROTATION_VECTOR"
self.imu_modo = "rotation_vector"
self.imu_modo = "raw_6axis" # rotation_vector
self.iniciado = False
self.rodando = False
@ -149,6 +148,9 @@ class CameraMultispectral:
module_calibration_json=self.module_calibration_json,
sync_mode="best",
sync_tolerance_ms=25.0,
imu_modo=self.imu_modo,
imu_freq_hz=self.imu_freq_hz,
)
resp = self.client.start(print_debug=False)
@ -178,8 +180,11 @@ class CameraMultispectral:
max_queue_drain=20,
nome_sensor=f"{self.modelo}_imu",
modo=self.imu_modo,
gyro_unidade="deg_s",
referencial_robo="-r,p,-y",
gyro_unidade="rad_s",
referencial_robo="r,p,y",
sinal_roll=1.0,
sinal_pitch=-1.0,
sinal_yaw=-1.0,
)
else:
self.tem_imu = False

56
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py
View File

@ -28,10 +28,12 @@ class CameraOak:
self.timestamp_ultima_segmentacao = None
self.tem_depth = False
self.tem_imu = False
self.iniciar_imu = bool(iniciar_imu)
self.imu = None
self.imu_freq_hz = 200
self.imu_publish_hz = 50
self.imu_sensor_type = "GAME_ROTATION_VECTOR"
self.imu_modo = "raw_6axis"
self.imu_sensor_type = None
self.rodando = False
self.iniciado = False
@ -139,6 +141,9 @@ class CameraOak:
modo=self.imu_modo,
gyro_unidade="rad_s",
referencial_robo="y,r,p",
sinal_roll=1.0,
sinal_pitch=1.0,
sinal_yaw=-1.0,
)
if self.modelo_ia_seg is not None:
@ -795,30 +800,57 @@ class CameraOak:
self.mostrar_log(f"[WARN] Falha ao montar pipeline depth: {e}")
# IMU (somente se presente)
if self.tem_imu:
if self.tem_imu and self.iniciar_imu:
try:
imu = pipeline.create(dai.node.IMU)
imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_RAW, self.imu_freq_hz)
imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_RAW, self.imu_freq_hz)
if self.imu_modo == "raw_6axis":
imu.enableIMUSensor(
dai.IMUSensor.ACCELEROMETER_RAW,
self.imu_freq_hz,
)
imu.enableIMUSensor(
dai.IMUSensor.GYROSCOPE_RAW,
self.imu_freq_hz,
)
self.imu_sensor_type = (
"ACCELEROMETER_RAW_GYROSCOPE_RAW"
)
elif self.imu_modo == "rotation_vector":
imu.enableIMUSensor(
dai.IMUSensor.GAME_ROTATION_VECTOR,
self.imu_freq_hz,
)
self.imu_sensor_type = "GAME_ROTATION_VECTOR"
else:
raise ValueError(
f"imu_modo inválido: {self.imu_modo!r}. "
"Use 'raw_6axis' ou 'rotation_vector'."
)
imu.setBatchReportThreshold(1)
imu.setMaxBatchReports(5)
xoutImu = pipeline.create(dai.node.XLinkOut)
xoutImu.setStreamName("imu")
imu.out.link(xoutImu.input)
self.imu_sensor_type = "ACCELEROMETER_RAW_GYROSCOPE_RAW"
self.imu_modo = "raw_6axis"
xout_imu = pipeline.create(dai.node.XLinkOut)
xout_imu.setStreamName("imu")
imu.out.link(xout_imu.input)
self.mostrar_log(
f"Pipeline imu criado | sensor={self.imu_sensor_type} | freq={self.imu_freq_hz}Hz"
"Pipeline IMU criado"
f" | modo={self.imu_modo}"
f" | sensor={self.imu_sensor_type}"
f" | freq={self.imu_freq_hz}Hz"
)
except Exception as e:
self.tem_imu = False
self.mostrar_log(f"[WARN] Falha ao montar pipeline imu: {e}")
self.mostrar_log(
f"[WARN] Falha ao montar pipeline IMU: {e}"
)
script = None
if self.modelo_ia_seg is not None or self.modelo_ia_det is not None:

6
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/benchmark.py
View File

@ -393,12 +393,12 @@ def main():
ap.add_argument(
"--module_params",
default=r"C:\ZendionInc\agrobot_base\Python\OAK\datasets\oak-fcc-3\calibration\module_params.json",
default=r"C:\AgroBaseModels\Ervas\modelmp-4_0.json",
help="Caminho do module_params.json.",
)
ap.add_argument("--width", type=int, default=1024, help="Largura final do tensor.")
ap.add_argument("--height", type=int, default=640, help="Altura final do tensor.")
ap.add_argument("--width", type=int, default=640, help="Largura final do tensor.")
ap.add_argument("--height", type=int, default=400, help="Altura final do tensor.")
ap.add_argument("--fps", type=float, default=30.0)
ap.add_argument("--seconds", type=float, default=20.0)
ap.add_argument("--timeout", type=float, default=3.0)

9
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_client.py
View File

@ -24,6 +24,8 @@ class OakFcc3Client:
sync_mode="best",
sync_tolerance_ms=25.0,
mx_id=None,
imu_modo="rotation_vector",
imu_freq_hz=200,
**kwargs,
):
self.width = width
@ -38,6 +40,9 @@ class OakFcc3Client:
self.module_params = self._load_module_params(module_calibration_json)
self.fusion_config = self.module_params.get("fusion_config", {}) or {}
self.imu_modo = str(imu_modo).strip().lower()
self.imu_freq_hz = int(imu_freq_hz)
self.mx_id = str(mx_id) if mx_id else None
self.svc = OakFcc3Service(
@ -53,6 +58,10 @@ class OakFcc3Client:
sync_tolerance_ms=sync_tolerance_ms,
mx_id=self.mx_id,
module_calibration_json=module_calibration_json,
imu_modo=self.imu_modo,
imu_freq_hz=self.imu_freq_hz,
**kwargs,
)

88
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_manager.py
View File

@ -49,6 +49,8 @@ class OakFcc3Manager:
mx_id=None,
module_calibration_json=None,
module_params=None,
imu_modo="rotation_vector",
imu_freq_hz=200,
):
self.fps = fps
@ -86,6 +88,16 @@ class OakFcc3Manager:
self.buffers = {}
self.camera_info = {}
self.imu_modo = self._validate_imu_modo(imu_modo)
self.imu_freq_hz = int(imu_freq_hz)
if self.imu_freq_hz <= 0:
raise ValueError(
f"imu_freq_hz deve ser maior que zero: {self.imu_freq_hz}"
)
self.imu_sensor_type = None
self.has_imu_pipeline = False
self.tem_imu = False
self.q_imu = None
@ -335,19 +347,40 @@ class OakFcc3Manager:
def _create_imu_node(self, pipeline):
self.has_imu_pipeline = False
self.imu_sensor_type = None
try:
imu = pipeline.create(dai.node.IMU)
# Para inclinação do rover:
# GAME_ROTATION_VECTOR entrega quaternion pronto com roll/pitch
# referenciados à gravidade e sem depender de magnetômetro.
imu.enableIMUSensor(dai.IMUSensor.GAME_ROTATION_VECTOR, 200)
if self.imu_modo == "raw_6axis":
imu.enableIMUSensor(
dai.IMUSensor.ACCELEROMETER_RAW,
self.imu_freq_hz,
)
imu.enableIMUSensor(
dai.IMUSensor.GYROSCOPE_RAW,
self.imu_freq_hz,
)
self.imu_sensor_type = (
"ACCELEROMETER_RAW_GYROSCOPE_RAW"
)
elif self.imu_modo == "rotation_vector":
imu.enableIMUSensor(
dai.IMUSensor.GAME_ROTATION_VECTOR,
self.imu_freq_hz,
)
self.imu_sensor_type = "GAME_ROTATION_VECTOR"
else:
# Proteção adicional; normalmente o __init__ já impede isso.
raise RuntimeError(
f"imu_modo não suportado: {self.imu_modo!r}"
)
# Envia assim que tiver amostra.
imu.setBatchReportThreshold(1)
# Evita rajadas grandes e reduz backlog.
imu.setMaxBatchReports(5)
xout_imu = pipeline.create(dai.node.XLinkOut)
@ -355,11 +388,45 @@ class OakFcc3Manager:
imu.out.link(xout_imu.input)
self.has_imu_pipeline = True
print("[OAK] Pipeline IMU criado | GAME_ROTATION_VECTOR | 200Hz")
print(
"[OAK] Pipeline IMU criado"
f" | modo={self.imu_modo}"
f" | sensor={self.imu_sensor_type}"
f" | freq={self.imu_freq_hz}Hz"
)
except Exception as e:
self.has_imu_pipeline = False
print(f"[WARN] IMU indisponível no pipeline: {e}")
self.imu_sensor_type = None
print(
"[WARN] IMU indisponível no pipeline"
f" | modo={self.imu_modo}"
f" | freq={self.imu_freq_hz}Hz"
f" | erro={e}"
)
def _validate_imu_modo(self, modo):
modo = str(modo).strip().lower()
aliases = {
"raw": "raw_6axis",
"raw_6_axis": "raw_6axis",
"raw_6axis": "raw_6axis",
"rotation": "rotation_vector",
"game_rotation_vector": "rotation_vector",
"rotation_vector": "rotation_vector",
}
modo_normalizado = aliases.get(modo)
if modo_normalizado is None:
raise ValueError(
f"imu_modo inválido: {modo!r}. "
"Use 'raw_6axis' ou 'rotation_vector'."
)
return modo_normalizado
# ============================================================
# Pipeline creation, MULTISPEC aligned on OAK
@ -1037,6 +1104,9 @@ class OakFcc3Manager:
"async_capture": self.get_async_capture_status(),
"tem_imu": bool(getattr(self, "tem_imu", False)),
"has_imu_pipeline": bool(getattr(self, "has_imu_pipeline", False)),
"imu_modo": self.imu_modo,
"imu_freq_hz": self.imu_freq_hz,
"imu_sensor_type": self.imu_sensor_type,
}
def _serializable_aligned_geometry(self):

3
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_service.py
View File

@ -67,6 +67,9 @@ class OakFcc3Service:
"raw_policy": self.manager.raw_policy,
"sync_mode": getattr(self.manager, "sync_mode", "best"),
"sync_tolerance_ms": self.manager.sync_tolerance_ms,
"imu_modo": self.manager.imu_modo,
"imu_freq_hz": self.manager.imu_freq_hz,
"imu_sensor_type": self.manager.imu_sensor_type,
}
def set_fps(self, fps):

6
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/main.py
View File

@ -74,6 +74,12 @@ def main():
ContextoGlobalRedis()._atualizar_mpc(do_manager=True, iniciar_operacao=False)
except Exception as e:
mostrar_log(f"Erro ao Iniciar MPC: {e}")
elif acao == ManagerWorkerCommandType.ReiniciarMPC:
try:
from manager_worker.modulos.mpc import resetar as resetar_mpc
_mpc_iniciado = resetar_mpc()
except Exception as e:
mostrar_log(f"Erro ao Resetar MPC: {e}")
elif acao == ManagerWorkerCommandType.AtualizarDadosControle:
#mostrar_log("Adicionado na fila")
fila.adicionar(acao)

133
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py
View File

@ -7,6 +7,7 @@ import time
from typing import Any, Callable, Dict, Iterable, List, Optional, Sequence, Tuple
import numpy as np
import copy
from shared.enums import StatusCarroMapa, TipoMovimentoDirecional
from manager_worker.config import mostrar_log
@ -102,32 +103,128 @@ def _safe_motivos(motivos: Any) -> list:
_mpc = None
_iniciado = False
def inicializar(parametros, mapa, p_ref, forcar):
"""Inicializa ou recria o controlador MPC sem derrubar a instância anterior em caso de erro."""
def inicializar(parametros, mapa, p_ref, forcar=False):
global _mpc, _iniciado
mapa = list(mapa or [])
parametros = _as_dict(parametros)
len_atual = _safe_len(getattr(_mpc, "pontos_info", [])) if _mpc is not None else 0
len_mapa = _safe_len(mapa)
precisa_recriar = (not _iniciado) or (_mpc is not None and len_atual != len_mapa) or bool(forcar)
assinatura_nova = _assinatura_mapa(mapa, p_ref)
_log(f"[MPC] Inicializar chamado. iniciado: {_iniciado}, len_pontosinfo: {len_atual}, len_mapa: {len_mapa}, forcar: {forcar}")
assinatura_atual = None
if _mpc is not None:
assinatura_atual = _assinatura_mapa(
_mpc.pontos_mapa,
_mpc.p_ref,
)
mapa_mudou = assinatura_atual != assinatura_nova
precisa_recriar = (
not _iniciado
or _mpc is None
or mapa_mudou
or bool(forcar)
)
_log(
f"[MPC] Inicializar chamado | "
f"iniciado={_iniciado} | "
f"forcar={bool(forcar)} | "
f"mapa_mudou={mapa_mudou} | "
f"pontos_novos={len(mapa)}"
)
if not precisa_recriar:
return
return True
try:
novo_mpc = ControladorMPC(parametros_mpc=parametros, pontos_mapa=mapa, p_ref=p_ref)
novo_mpc = ControladorMPC(
parametros_mpc=parametros,
pontos_mapa=mapa,
p_ref=p_ref,
)
_mpc = novo_mpc
_iniciado = True
_log(f"[MPC] Iniciado. Trajetoria com {_safe_len(_mpc.pontos_info)} pontos")
_log(
f"[MPC] Nova instância criada | "
f"pontos={len(_mpc.pontos_info)} | "
f"visitados={np.count_nonzero(_mpc.visitados_execucao)}"
)
return True
except Exception as e:
_log(f"❌ Erro ao inicializar MPC. Instância anterior preservada: {e}")
_log(
f"❌ Erro ao inicializar MPC. "
f"Instância anterior preservada: {e}"
)
if _mpc is None:
_iniciado = False
return False
def resetar() -> bool:
global _mpc, _iniciado
mpc_atual = _mpc
if mpc_atual is None or not _iniciado:
_log("[MPC] Reset ignorado: MPC ainda não foi inicializado.")
return False
try:
parametros = copy.deepcopy(mpc_atual.parametros_mpc)
mapa = copy.deepcopy(mpc_atual.pontos_mapa)
p_ref = tuple(mpc_atual.p_ref)
except Exception as e:
_log(f"❌ Não foi possível obter os dados para resetar o MPC: {e}")
return False
inicializar(
parametros=parametros,
mapa=mapa,
p_ref=p_ref,
forcar=True,
)
reiniciado = _mpc is not mpc_atual
if reiniciado:
_log(
f"[MPC] Reset concluído. Nova instância criada com "
f"{_safe_len(_mpc.pontos_info)} pontos."
)
else:
_log("❌ Reset do MPC falhou. Instância anterior preservada.")
return reiniciado
def _assinatura_mapa(mapa, p_ref):
pontos = []
for ponto in mapa or []:
xy = ponto.get("xy", (0.0, 0.0))
pontos.append((
round(_safe_float(xy[0]), 3),
round(_safe_float(xy[1]), 3),
_safe_int(ponto.get("tipo", 0)),
round(_safe_float(
ponto.get("distanciaMargem", 0.7)
), 3),
))
referencia = (
round(_safe_float(p_ref[0]), 8),
round(_safe_float(p_ref[1]), 8),
)
return referencia, tuple(pontos)
def get_mpc():
global _mpc
return _mpc
@ -149,13 +246,23 @@ class ControladorMPC:
- mesma estrutura de `pontos_info` esperada pelo MPC.
"""
parametros_mpc = _as_dict(parametros_mpc)
self.pontos_info = list(pontos_mapa or [])
if not isinstance(p_ref, (list, tuple)) or len(p_ref) < 2:
raise ValueError("p_ref inválido. Esperado (lat0, lon0).")
self.lat0 = _safe_float(p_ref[0])
self.lon0 = _safe_float(p_ref[1])
# Fotografia da configuração usada para criar esta instância.
self.parametros_mpc = copy.deepcopy(parametros_mpc)
self.pontos_mapa = copy.deepcopy(list(pontos_mapa or []))
self.p_ref = (
_safe_float(p_ref[0]),
_safe_float(p_ref[1]),
)
# Dados de trabalho desta instância.
self.pontos_info = copy.deepcopy(self.pontos_mapa)
self.lat0 = self.p_ref[0]
self.lon0 = self.p_ref[1]
self.gps_handler = GPSHandler(self.lat0, self.lon0)
self.ultima_atualizacao = None

5
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py
View File

@ -1127,9 +1127,7 @@ class ContextoGlobalRedis:
if bloqueio_pendente_desde > 0:
updates_base.update({
"ultimo_bloqueio_transitorio_em": agora,
"ultimo_bloqueio_transitorio_motivo": str(
operacao.get("bloqueio_pendente_motivo", "")
),
"ultimo_bloqueio_transitorio_motivo": str(operacao.get("bloqueio_pendente_motivo", "")),
"bloqueio_pendente_desde": 0.0,
"bloqueio_pendente_motivo": "",
})
@ -1187,6 +1185,7 @@ class ContextoGlobalRedis:
restante = (retomada_desde + tempo_retomada) - agora
if restante > 0:
updates_base["tempo_aguardando"] = agora
_commit(
StatusOperacao.Aguardando,
f"Bloqueio removido; retomada segura em {restante:.1f}s",

1
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/enums.py
View File

@ -23,6 +23,7 @@ class ManagerWorkerCommandType(IntEnum):
AtualizarDadosControle = 3
EnviarDadosControle = 4
FinalizarOperacao = 5
ReiniciarMPC = 6
class WeedWorkerCommandType(IntEnum):
ScriptCarregado = 1

214
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py
View File

@ -158,6 +158,13 @@ class CameraManager:
self._ultimo_config_update_ts = 0.0
# Diagnostico fino do pipeline. Estes campos sao somente observabilidade:
# nao alteram frequencias, caches, gates ou comandos dos bicos.
self._diag_tensor_overwrites = 0
self._diag_pred_overwrites = 0
self._diag_last_tensor_lock_wait_ms = 0.0
self._diag_last_pred_lock_wait_ms = 0.0
self._dbg_img = None
self._dbg_layer = None
self._dbg_shape = (706, 560)
@ -854,28 +861,30 @@ class CameraManager:
self._ultima_saude_ts = time.time()
try:
# Mantém a referência estável durante leitura/publicação da saúde.
# fechar_camera_manager aguarda esta seção curta terminar.
# O lock protege somente a identidade da sessão da câmera.
with self._vida_lock:
camera_atual = self.camera
generation = self._camera_generation
if camera_atual is not None:
if pipeline_ia is None:
pipeline_ia = self._avaliar_saude_pipeline_ia()
camera_atual.atualizar_saude(pipeline_ia=pipeline_ia)
if camera_atual is not None:
if pipeline_ia is None:
pipeline_ia = self._avaliar_saude_pipeline_ia()
elif self.mx_id is not None:
from camera_worker.manager import definir_saude_camera
definir_saude_camera(
self.mx_id,
StatusModulo.DESCONECTADO,
0,
["desconectado"],
False,
{},
disp=T_Code.Cam,
conectado=False,
)
camera_atual.atualizar_saude(pipeline_ia=pipeline_ia)
elif self.mx_id is not None:
from camera_worker.manager import definir_saude_camera
definir_saude_camera(
self.mx_id,
StatusModulo.DESCONECTADO,
0,
["desconectado"],
False,
{},
disp=T_Code.Cam,
conectado=False,
)
except Exception as e:
self.mostrar_log(f"[saude] erro: {e}")
@ -987,7 +996,11 @@ class CameraManager:
# ============================================================
def _get_tensor_novo_para_inferencia(self):
t_lock0 = time.perf_counter()
with self._lock_tensor:
self._diag_last_tensor_lock_wait_ms = (
time.perf_counter() - t_lock0
) * 1000.0
tensor5 = self._tensor_pronto
res = self._tensor_res
ts_tensor = self._tensor_ts
@ -1006,7 +1019,11 @@ class CameraManager:
return tensor5, ts_tensor, res, generation
def _get_prediction_nova_para_deteccao(self):
t_lock0 = time.perf_counter()
with self._pred_lock:
self._diag_last_pred_lock_wait_ms = (
time.perf_counter() - t_lock0
) * 1000.0
cache = dict(self._pred_cache)
pred_ts = float(cache.get("ts", 0.0) or 0.0)
@ -1323,23 +1340,39 @@ class CameraManager:
def _iniciar_loop_captura_tensor(self, freq=25.0):
def loop():
periodo = 1.0 / max(float(freq), 0.1)
ultimo_inicio = None
sleep_req_ms_ant = 0.0
sleep_real_ms_ant = 0.0
while True:
t0_wall = time.time()
t0_perf = time.perf_counter()
t0_cpu = time.thread_time()
loop_gap_ms = (
(t0_perf - ultimo_inicio) * 1000.0
if ultimo_inicio is not None else 0.0
)
ultimo_inicio = t0_perf
camera_atual = None
generation = -1
vida_lock_wait_ms = 0.0
try:
if self.iniciando or self._em_warmup or not self.operante or self.camera is None:
time.sleep(0.05)
continue
t_lock0 = time.perf_counter()
with self._vida_lock:
vida_lock_wait_ms = (time.perf_counter() - t_lock0) * 1000.0
camera_atual = self.camera
generation = self._camera_generation
t_request0 = time.perf_counter()
t_request_cpu0 = time.thread_time()
tensor5, res = camera_atual.requisitar_tensor_multispec(force=True)
request_wall_ms = (time.perf_counter() - t_request0) * 1000.0
request_cpu_ms = (time.thread_time() - t_request_cpu0) * 1000.0
erro = res.get("erro") if isinstance(res, dict) else None
if erro:
@ -1367,6 +1400,11 @@ class CameraManager:
perf = res.get("perf", {}) if isinstance(res, dict) else {}
with self._lock_tensor:
if (
self._tensor_pronto is not None
and self._tensor_ts > self._tensor_consumido_ts
):
self._diag_tensor_overwrites += 1
self._tensor_pronto = tensor5
self._tensor_res = res
self._tensor_ts = agora
@ -1374,10 +1412,12 @@ class CameraManager:
self._ultimo_tensor_ok_ts = agora
t1_perf = time.perf_counter()
thread_cpu_ms = (time.thread_time() - t0_cpu) * 1000.0
wall_ms = (t1_perf - t0_perf) * 1000.0
self.perf.tick(
"tensor",
latencia_ms=(t1_perf - t0_perf) * 1000.0,
latencia_ms=wall_ms,
tensor_core_ms=float(perf.get("total_ms", 0.0) or 0.0),
get_decoded_ms=float(perf.get("get_decoded_ms", 0.0) or 0.0),
build_ms=float(perf.get("build_ms", 0.0) or 0.0),
@ -1387,6 +1427,17 @@ class CameraManager:
frame_ts=agora,
idade_frame_ms=0.0,
generation=generation,
loop_gap_ms=loop_gap_ms,
thread_cpu_ms=thread_cpu_ms,
off_cpu_ms=max(0.0, wall_ms - thread_cpu_ms),
vida_lock_wait_ms=vida_lock_wait_ms,
request_wall_ms=request_wall_ms,
request_cpu_ms=request_cpu_ms,
request_off_cpu_ms=max(0.0, request_wall_ms - request_cpu_ms),
sleep_req_ms=sleep_req_ms_ant,
sleep_real_ms=sleep_real_ms_ant,
sleep_overshoot_ms=max(0.0, sleep_real_ms_ant - sleep_req_ms_ant),
tensor_overwrites=self._diag_tensor_overwrites,
)
except Exception as e:
@ -1403,17 +1454,31 @@ class CameraManager:
finally:
gasto = time.time() - t0_wall
restante = periodo - gasto
sleep_req_ms_ant = max(0.0, restante) * 1000.0
t_sleep0 = time.perf_counter()
if restante > 0:
time.sleep(restante)
sleep_real_ms_ant = (time.perf_counter() - t_sleep0) * 1000.0
threading.Thread(target=loop, name="WeedTensorCapture", daemon=True).start()
def _iniciar_loop_inferencia(self, freq=25.0):
def loop():
periodo = 1.0 / max(float(freq), 0.1)
ultimo_inicio = None
ultima_inferencia_inicio = None
polls_sem_tensor = 0
sleep_req_ms_ant = 0.0
sleep_real_ms_ant = 0.0
while True:
t_loop0 = time.perf_counter()
t_cpu0 = time.thread_time()
loop_gap_ms = (
(t_loop0 - ultimo_inicio) * 1000.0
if ultimo_inicio is not None else 0.0
)
ultimo_inicio = t_loop0
try:
if self.iniciando:
@ -1433,6 +1498,7 @@ class CameraManager:
if tensor5 is None or tensor_ts is None:
self.perf.inc("infer_sem_tensor_novo")
polls_sem_tensor += 1
time.sleep(0.001)
continue
@ -1441,11 +1507,13 @@ class CameraManager:
continue
t_inf0 = time.perf_counter()
infer_cpu0 = time.thread_time()
predictions = self.model_svc.infer_tensor_fast(
tensor5,
keep_probs=False,
)
t_inf1 = time.perf_counter()
infer_cpu_ms = (time.thread_time() - infer_cpu0) * 1000.0
if not self._sessao_camera_valida(camera_atual, generation):
self.perf.inc("infer_descartada_geracao_depois")
@ -1469,6 +1537,12 @@ class CameraManager:
pred_ts = time.time()
with self._pred_lock:
if (
self._pred_cache.get("predictions") is not None
and float(self._pred_cache.get("ts", 0.0) or 0.0)
> self._pred_consumido_ts
):
self._diag_pred_overwrites += 1
self._pred_cache = {
"ts": pred_ts,
"tensor_ts": tensor_ts,
@ -1489,9 +1563,16 @@ class CameraManager:
self._ultimo_predictions_full = infer_full
t_loop1 = time.perf_counter()
thread_cpu_ms = (time.thread_time() - t_cpu0) * 1000.0
wall_ms = (t_loop1 - t_loop0) * 1000.0
infer_start_gap_ms = (
(t_inf0 - ultima_inferencia_inicio) * 1000.0
if ultima_inferencia_inicio is not None else 0.0
)
ultima_inferencia_inicio = t_inf0
self.perf.tick(
"inferencia",
latencia_ms=(t_loop1 - t_loop0) * 1000.0,
latencia_ms=wall_ms,
get_tensor_ms=(t_get1 - t_get0) * 1000.0,
infer_ms=infer_ms,
infer_forward_ms=float(infer_forward_ms or 0.0),
@ -1502,12 +1583,28 @@ class CameraManager:
frame_ts=pred_ts,
idade_tensor_ms=(time.time() - tensor_ts) * 1000.0 if tensor_ts else None,
generation=generation,
loop_gap_ms=loop_gap_ms,
infer_start_gap_ms=infer_start_gap_ms,
thread_cpu_ms=thread_cpu_ms,
off_cpu_ms=max(0.0, wall_ms - thread_cpu_ms),
infer_cpu_ms=infer_cpu_ms,
infer_off_cpu_ms=max(0.0, infer_ms - infer_cpu_ms),
tensor_lock_wait_ms=self._diag_last_tensor_lock_wait_ms,
polls_sem_tensor=polls_sem_tensor,
sleep_req_ms=sleep_req_ms_ant,
sleep_real_ms=sleep_real_ms_ant,
sleep_overshoot_ms=max(0.0, sleep_real_ms_ant - sleep_req_ms_ant),
pred_overwrites=self._diag_pred_overwrites,
)
polls_sem_tensor = 0
gasto = time.perf_counter() - t_loop0
restante = periodo - gasto
sleep_req_ms_ant = max(0.0, restante) * 1000.0
t_sleep0 = time.perf_counter()
if restante > 0:
time.sleep(restante)
sleep_real_ms_ant = (time.perf_counter() - t_sleep0) * 1000.0
except Exception as e:
self.mostrar_log(f"❌ Erro no loop_inferencia weed: {e}")
@ -1517,9 +1614,20 @@ class CameraManager:
def _iniciar_loop_deteccao_weed(self, freq=25.0):
def loop():
ultimo_inicio = None
ultima_deteccao_inicio = None
polls_sem_pred = 0
sleep_req_ms_ant = 0.0
sleep_real_ms_ant = 0.0
while True:
t0_wall = time.time()
t_loop0 = time.perf_counter()
t_cpu0 = time.thread_time()
loop_gap_ms = (
(t_loop0 - ultimo_inicio) * 1000.0
if ultimo_inicio is not None else 0.0
)
ultimo_inicio = t_loop0
try:
if self.iniciando:
@ -1532,7 +1640,12 @@ class CameraManager:
time.sleep(0.2)
continue
t_cfg0 = time.perf_counter()
cfg_cpu0 = time.thread_time()
self._atualizar_config_dinamica_detector(intervalo_s=0.20)
t_cfg1 = time.perf_counter()
config_ms = (t_cfg1 - t_cfg0) * 1000.0
config_cpu_ms = (time.thread_time() - cfg_cpu0) * 1000.0
t_pred0 = time.perf_counter()
pred_cache = self._get_prediction_nova_para_deteccao()
@ -1540,6 +1653,7 @@ class CameraManager:
if pred_cache is None:
self.perf.inc("det_sem_prediction_nova")
polls_sem_pred += 1
# Se o estado operacional mudou para não permitido, não espera prediction nova
# para desligar bicos.
@ -1566,8 +1680,10 @@ class CameraManager:
continue
t_det0 = time.perf_counter()
det_cpu0 = time.thread_time()
analise_completa = self.detectar_ervas(predictions)
t_det1 = time.perf_counter()
detector_cpu_ms = (time.thread_time() - det_cpu0) * 1000.0
if not isinstance(analise_completa, dict):
self.mostrar_log("[WEED] análise inválida retornada pelo WeedDetector")
@ -1637,6 +1753,12 @@ class CameraManager:
t_loop1 = time.perf_counter()
total_ms = (t_loop1 - t_loop0) * 1000.0
thread_cpu_ms = (time.thread_time() - t_cpu0) * 1000.0
det_start_gap_ms = (
(t_det0 - ultima_deteccao_inicio) * 1000.0
if ultima_deteccao_inicio is not None else 0.0
)
ultima_deteccao_inicio = t_det0
self._ultimo_loop_analise_fps = 1000.0 / max(total_ms, 1e-6)
self._perf_weed = {
@ -1673,6 +1795,9 @@ class CameraManager:
"deteccao",
latencia_ms=total_ms,
get_pred_ms=(t_pred1 - t_pred0) * 1000.0,
config_ms=config_ms,
config_cpu_ms=config_cpu_ms,
config_off_cpu_ms=max(0.0, config_ms - config_cpu_ms),
detector_ms=detector_ms,
convert_ms=(t_conv1 - t_conv0) * 1000.0,
ctx_read_ms=float(debug_pulverizacao.get("ctx_read_ms", 0.0) or 0.0),
@ -1688,7 +1813,19 @@ class CameraManager:
frame_ts=pred_ts,
idade_frame_ms=(time.time() - pred_ts) * 1000.0 if pred_ts else None,
idade_tensor_ms=(time.time() - tensor_ts) * 1000.0 if tensor_ts else None,
loop_gap_ms=loop_gap_ms,
det_start_gap_ms=det_start_gap_ms,
thread_cpu_ms=thread_cpu_ms,
off_cpu_ms=max(0.0, total_ms - thread_cpu_ms),
detector_cpu_ms=detector_cpu_ms,
detector_off_cpu_ms=max(0.0, detector_ms - detector_cpu_ms),
pred_lock_wait_ms=self._diag_last_pred_lock_wait_ms,
polls_sem_pred=polls_sem_pred,
sleep_req_ms=sleep_req_ms_ant,
sleep_real_ms=sleep_real_ms_ant,
sleep_overshoot_ms=max(0.0, sleep_real_ms_ant - sleep_req_ms_ant),
)
polls_sem_pred = 0
except Exception as e:
self.mostrar_log(f"❌ Erro no loop_deteccao_weed: {e}")
@ -1700,7 +1837,11 @@ class CameraManager:
finally:
lat = time.time() - t0_wall
time.sleep(max(0.0, (1.0 / freq) - lat))
sleep_s = max(0.0, (1.0 / freq) - lat)
sleep_req_ms_ant = sleep_s * 1000.0
t_sleep0 = time.perf_counter()
time.sleep(sleep_s)
sleep_real_ms_ant = (time.perf_counter() - t_sleep0) * 1000.0
threading.Thread(target=loop, daemon=True).start()
@ -2406,6 +2547,7 @@ class CameraManager:
f"post={self._fmt(self._m(inf, 'infer_post_ms'))} "
f"gpu={self._fmt(self._m(inf, 'infer_gpu_ms'))} | "
f"DET total={self._fmt(self._lat(det))} "
f"config={self._fmt(self._m(det, 'config_ms'))} "
f"get_pred={self._fmt(self._m(det, 'get_pred_ms'))} "
f"detector={self._fmt(self._m(det, 'detector_ms'))} "
f"ctx={self._fmt(self._m(det, 'ctx_read_ms'))} "
@ -2420,6 +2562,34 @@ class CameraManager:
f"redis_dbg={pub_dbg.get('redis_ms', 0):.1f}ms"
)
self.mostrar_log(
"[WEED_SCHED] "
f"TENSOR gap={self._fmt(self._m(tensor, 'loop_gap_ms'))} "
f"cpu={self._fmt(self._m(tensor, 'thread_cpu_ms'))} "
f"offcpu={self._fmt(self._m(tensor, 'off_cpu_ms'))} "
f"req_wall={self._fmt(self._m(tensor, 'request_wall_ms'))} "
f"req_cpu={self._fmt(self._m(tensor, 'request_cpu_ms'))} "
f"lock={self._fmt(self._m(tensor, 'vida_lock_wait_ms'))} "
f"sleep={self._fmt(self._m(tensor, 'sleep_req_ms'))}/"
f"{self._fmt(self._m(tensor, 'sleep_real_ms'))} | "
f"INF gap={self._fmt(self._m(inf, 'infer_start_gap_ms'))} "
f"cpu={self._fmt(self._m(inf, 'thread_cpu_ms'))} "
f"offcpu={self._fmt(self._m(inf, 'off_cpu_ms'))} "
f"fwd_cpu={self._fmt(self._m(inf, 'infer_cpu_ms'))} "
f"lock={self._fmt(self._m(inf, 'tensor_lock_wait_ms'))} "
f"poll={self._fmt(self._m(inf, 'polls_sem_tensor'), casas=0)} "
f"sleep={self._fmt(self._m(inf, 'sleep_req_ms'))}/"
f"{self._fmt(self._m(inf, 'sleep_real_ms'))} | "
f"DET gap={self._fmt(self._m(det, 'det_start_gap_ms'))} "
f"cpu={self._fmt(self._m(det, 'thread_cpu_ms'))} "
f"offcpu={self._fmt(self._m(det, 'off_cpu_ms'))} "
f"det_cpu={self._fmt(self._m(det, 'detector_cpu_ms'))} "
f"lock={self._fmt(self._m(det, 'pred_lock_wait_ms'))} "
f"poll={self._fmt(self._m(det, 'polls_sem_pred'), casas=0)} "
f"sleep={self._fmt(self._m(det, 'sleep_req_ms'))}/"
f"{self._fmt(self._m(det, 'sleep_real_ms'))}"
)
@staticmethod
def _fmt(v, casas=1, default=0.0):
try:
@ -2710,4 +2880,4 @@ class CameraManager:
def _finalizar_salvamento_posprocessamento(self):
with self._posproc_save_lock:
self._posproc_salvando = False
self._posproc_salvando = False