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

ajuste no simulador; mapa dinamico; envio de controle do redis para o controle c#; troca do modo direcional; blindado trajetoria; tempo aguardando

This commit is contained in:
Diego Freitas 2026-07-10 16:27:16 -03:00
parent 4344cbf14a
commit 2936df66fa
11 changed files with 3328 additions and 1137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -4,11 +4,13 @@ using Newtonsoft.Json;
using Newtonsoft.Json.Linq;
using System;
using System.Collections.Generic;
using System.Threading;
using System.IO;
using System.Linq;
using System.Threading.Tasks;
using System.Windows.Forms;
using static AgroBase.Models.Enums;
using System.Diagnostics;
namespace AgroBase.Forms
{
@ -31,6 +33,15 @@ namespace AgroBase.Forms
List<MPCSimulacaoModel> simulacao = new List<MPCSimulacaoModel>();
private int _atualizacaoTelaPendente;
private int _tickEmExecucao;
private readonly Stopwatch _cronometroTela = Stopwatch.StartNew();
private const int IntervaloTelaMs = 100; // 10 FPS para a UI
public frmSimulacaoMapaGPS()
{
InitializeComponent();
@ -70,58 +81,96 @@ namespace AgroBase.Forms
private void frmSimulacaoMapaGPS_FormClosing(object sender, FormClosingEventArgs e)
{
tmrLeitura?.Dispose();
MapaDinamico?.Dispose();
Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.OperacaoIniciada = false;
}
DateTime t0 = DateTime.Now;
private async Task tmrLeitura_Tick()
{
double latencia_tick = (DateTime.Now - t0).TotalSeconds;
t0 = DateTime.Now;
int.TryParse(txtTempoGPS.Text, out int tempo);
if (tempo > 0)
if (Interlocked.Exchange(ref _tickEmExecucao, 1) == 1)
return;
try
{
tempoGps = tempo;
int tempo = 0;
bool automatico = false;
// Esses componentes pertencem à UI.
if (!IsDisposed && IsHandleCreated)
{
Invoke(new Action(() =>
{
int.TryParse(txtTempoGPS.Text, out tempo);
automatico = chbAutomatico.Checked;
}));
}
if (tempo > 0)
tempoGps = tempo;
if (automatico)
{
// Temporariamente mantém o passo na UI porque o método ainda
// acessa vários TextBox diretamente.
Invoke(new Action(() =>
{
btnAcrescentarGPS_Click(
btnAcrescentarGPS,
EventArgs.Empty
);
}));
}
if (_cronometroTela.ElapsedMilliseconds >= IntervaloTelaMs)
{
SolicitarAtualizacaoTela();
_cronometroTela.Restart();
}
// Não reduza para 1 ms quando estiver atrasado.
tmrLeitura.SetInterval(Math.Max((int)tempoGps, 10));
}
finally
{
Interlocked.Exchange(ref _tickEmExecucao, 0);
}
if (!AtualizandoTela)
await Task.CompletedTask;
}
private void SolicitarAtualizacaoTela()
{
if (IsDisposed || !IsHandleCreated)
return;
if (Interlocked.Exchange(ref _atualizacaoTelaPendente, 1) == 1)
return;
BeginInvoke(new Action(() =>
{
Func<bool> funcAtt = new Func<bool>(() =>
try
{
AtualizarDadosTela();
return true;
});
Task.Run(() => FuncoesGlobais.ExecutarMetodoComVerificacaoCrossThread(this, funcAtt));
}
if (chbAutomatico.Checked)
{
btnAcrescentarGPS_Click(btnAcrescentarGPS, new EventArgs());
}
DateTime t1 = DateTime.Now;
double latencia_exec = (t1 - t0).TotalSeconds;
double frequencia_exec = 1.0 / Math.Max(latencia_exec, 1e-6);
double frequencia_tick = 1.0 / Math.Max(latencia_tick, 1e-6);
//Console.WriteLine($"Tempo por tick GPS: {Math.Round(latencia_exec, 4)} s, Frequencia: {Math.Round(frequencia_exec, 2)} Hz | Tempo entre tick GPS: {Math.Round(latencia_tick, 4)}, Frequencia: {Math.Round(frequencia_tick, 2)} Hz");
double novoDelay = Math.Max(tempoGps - (latencia_exec * 1000.0), 1);
tmrLeitura.SetInterval((int)novoDelay);
}
catch (Exception ex)
{
Variaveis.MostrarLog(
"[Simulacao] Erro ao atualizar tela: " + ex
);
}
finally
{
Interlocked.Exchange(ref _atualizacaoTelaPendente, 0);
}
}));
}
bool AtualizandoTela = false;
private void AtualizarDadosTela()
{
if (AtualizandoTela)
return;
AtualizandoTela = true;
var _Controle = Variaveis.OperacaoEmAndamento.Controle;
var _Sensoriamento = Variaveis.OperacaoEmAndamento.Sensoriamento;
var _Trajetoria = Variaveis.OperacaoEmAndamento.Trajetoria;
if (_Trajetoria == null)
{
AtualizandoTela = false;
return;
}
@ -132,13 +181,11 @@ namespace AgroBase.Forms
if (_Trajetoria == null)
{
Variaveis.OperacaoEmAndamento.Mapa.AtualizarRuasSelecionadas();
AtualizandoTela = false;
return;
}
if (_Sensoriamento.Trajetoria == null)
{
AtualizandoTela = false;
return;
}
@ -202,8 +249,6 @@ namespace AgroBase.Forms
Variaveis.OperacaoEmAndamento.Mapa.AtualizarRuasSelecionadas();
}
AtualizandoTela = false;
}
@ -239,20 +284,36 @@ namespace AgroBase.Forms
private void AtualizarPosicaoGPS()
{
GPSService.PenultimaLeitura = new GPSModel()
{
DataHora = GPSService.UltimaLeitura.DataHora,
Longitude = GPSService.UltimaLeitura.Longitude,
LongitudeAnt = GPSService.UltimaLeitura.LongitudeAnt,
Latitude = GPSService.UltimaLeitura.Latitude,
LatitudeAnt = GPSService.UltimaLeitura.LatitudeAnt,
Heartbeat = GPSService.UltimaLeitura.Heartbeat,
OrientacaoReal = GPSService.UltimaLeitura.OrientacaoReal
};
if (!GPSService.Iniciado || (txtLongitude.Text != "" && txtLatitude.Text != ""))
{
GPSService.UltimaLeitura = posicaoAtual;
double orientacaoReal = GPSUtils.NormalizarAngulo(posicaoAtual.OrientacaoReal);
GPSService.UltimaLeitura.Momento = posicaoAtual.Momento;
GPSService.UltimaLeitura.DataHora = posicaoAtual.DataHora;
GPSService.UltimaLeitura.UltimoComandoRespondido = posicaoAtual.UltimoComandoRespondido;
GPSService.UltimaLeitura.Latitude = posicaoAtual.Latitude;
GPSService.UltimaLeitura.Longitude = posicaoAtual.Longitude;
GPSService.UltimaLeitura.LatitudeAnt = posicaoAtual.LatitudeAnt;
GPSService.UltimaLeitura.LongitudeAnt = posicaoAtual.LongitudeAnt;
GPSService.UltimaLeitura.OrientacaoReal = orientacaoReal;
GPSService.UltimaLeitura.AnguloCarroDefinido = orientacaoReal;
GPSService.UltimaLeitura.TipoOrientacao = "A";
GPSService.UltimaLeitura.OrientacaoMovimento =
double.IsNaN(posicaoAtual.OrientacaoMovimento) || double.IsInfinity(posicaoAtual.OrientacaoMovimento)
? orientacaoReal
: GPSUtils.NormalizarAngulo(posicaoAtual.OrientacaoMovimento);
GPSService.UltimaLeitura.Distancia = posicaoAtual.Distancia;
GPSService.UltimaLeitura.Velocidade = posicaoAtual.Velocidade;
GPSService.UltimaLeitura.TimestampOri = posicaoAtual.TimestampOri.Clone();
GPSService.UltimaLeitura.TimestampPos = posicaoAtual.TimestampPos.Clone();
GPSService.UltimaLeitura.Heartbeat = posicaoAtual.Heartbeat;
GPSService.UltimaLeitura.Inicializado = true;
}
GPSService.AtualizarAtrasoPosicoes(Convert.ToInt32(txtDelayPosicao.Text));
@ -277,7 +338,7 @@ namespace AgroBase.Forms
// Obtém os parâmetros
double anguloAtual = double.Parse(txtAnguloGPS.Text);
var _Gps = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps;
var _Gps = GPSService.UltimaLeitura;
double anguloControle = 0.0;
TipoMovimentoDirecional tipoMovimento = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento;
@ -328,8 +389,8 @@ namespace AgroBase.Forms
txtAnguloGPS.Text = posicaoAtual.OrientacaoReal.ToString("0.00");
txtVelocidade.Text = veloccidade.ToString("0.00");
txtDistanciaGPS.Text = posicaoAtual.Distancia.ToString("0.0000");
txtLatitude.Text = posicaoAtual.Latitude.ToString();
txtLongitude.Text = posicaoAtual.Longitude.ToString();
txtLatitude.Text = posicaoAtual.LatitudeAnt.ToString();
txtLongitude.Text = posicaoAtual.LongitudeAnt.ToString();
UltimaAtualizacaoGps = DateTime.Now;
@ -570,7 +631,7 @@ namespace AgroBase.Forms
Heartbeat = GPSService.UltimaLeitura.Heartbeat,
OrientacaoReal = double.Parse(txtAnguloGPS.Text.Replace(".", ",")),
};
GPSService.EnviarCoordenadasParaMapa(VariaveisOperacao.PosicaoBase.Latitude, VariaveisOperacao.PosicaoBase.Longitude, VariaveisOperacao.PosicaoBase.AnguloCarroDefinido, false, 0x01);
GPSService.EnviarCoordenadasParaMapa(VariaveisOperacao.PosicaoBase.Latitude, VariaveisOperacao.PosicaoBase.Longitude, VariaveisOperacao.PosicaoBase.AnguloCarroDefinido, false, 0x02);
}
private void btnLiberarCorredor_Click(object sender, EventArgs e)

2224
AgroBase/AgroBase/Models/MapaDinamicoModel.cs
View File

File diff suppressed because it is too large Load Diff

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

@ -554,7 +554,6 @@ namespace AgroBase.Models
};
op.Controle = new OperacaoControleModel()
{
TipoMovimento = TipoMovimentoDirecional.RodasDianteiras,
Angulo = 0,
EmFreio = false,
PercentualVelocidadeSP = 0,
@ -615,7 +614,7 @@ namespace AgroBase.Models
DirAuxilioSonar = true,
MovAuxilioSonar = true,
MpcHorizonte = 4.0,
AnteciparManobraCorredorM = 10.0,
AnteciparManobraCorredorM = 0.0,
MovVelocidadeSErvasPercent = 50,
MovVelocidadeCErvasPercent = 20,
@ -695,7 +694,6 @@ namespace AgroBase.Models
};
op.Controle = new OperacaoControleModel()
{
TipoMovimento = TipoMovimentoDirecional.RodasDianteiras,
Angulo = 0,
EmFreio = false,
PercentualVelocidadeSP = 0,
@ -836,7 +834,6 @@ namespace AgroBase.Models
};
op.Controle = new OperacaoControleModel()
{
TipoMovimento = TipoMovimentoDirecional.RodasDianteiras,
Angulo = 0,
EmFreio = false,
PercentualVelocidadeSP = 0,
@ -894,7 +891,7 @@ namespace AgroBase.Models
DirAuxilioSonar = true,
MovAuxilioSonar = true,
MpcHorizonte = 4.0,
AnteciparManobraCorredorM = 10.0,
AnteciparManobraCorredorM = 0.0,
MovVelocidadeSErvasPercent = 40,
MovVelocidadeCErvasPercent = 20,
@ -944,7 +941,6 @@ namespace AgroBase.Models
};
op.Controle = new OperacaoControleModel()
{
TipoMovimento = TipoMovimentoDirecional.RodasDianteiras,
Angulo = 0,
EmFreio = false,
PercentualVelocidadeSP = 0,
@ -1059,7 +1055,6 @@ namespace AgroBase.Models
op.Parametros.ParametrosMandatorios = new List<OperacaoParametrosMandatoriosModel>();
op.Controle = new OperacaoControleModel()
{
TipoMovimento = TipoMovimentoDirecional.RodasDianteiras,
Angulo = 0,
EmFreio = false,
PercentualVelocidadeSP = 0,
@ -1093,7 +1088,7 @@ namespace AgroBase.Models
break;
}
op.Controle.DefinirTipoMovimento(TipoMovimentoDirecional.RodasDianteiras, op.DispMvd?.Dados?.Modulos);
op.Parametros.ModulosMandatorios.ForEach(m => m.ComponentesEmUso = op.PreencherComponentesEmUso(m.Dispositivo));
if (CarregarModulos)
{
@ -1799,40 +1794,10 @@ namespace AgroBase.Models
var dispMvd = DispMvd;
if (dispMvd?.Dados?.Modulos != null && value != Controle.TipoMovimento)
if (dispMvd?.Dados?.Modulos != null) // && value != Controle.TipoMovimento
{
switch (value)
{
case TipoMovimentoDirecional.RodasDianteiras:
dispMvd.Dados.Modulos.ForEach(x =>
{
x.MovMotor.Comandar = true;
x.DirMotor.Comandar = x.Modulo_ID.Contains("F");
x.DirMotor.Angulo_SP = x.DirMotor.Comandar ? Controle.Angulo : 0;
});
break;
case TipoMovimentoDirecional.RodasTraseiras:
dispMvd.Dados.Modulos.ForEach(x =>
{
x.MovMotor.Comandar = true;
x.DirMotor.Comandar = x.Modulo_ID.Contains("T");
x.DirMotor.Angulo_SP = x.DirMotor.Comandar ? Controle.Angulo : 0;
});
break;
default:
dispMvd.Dados.Modulos.ForEach(x =>
{
x.MovMotor.Comandar = true;
x.DirMotor.Comandar = true;
x.DirMotor.Angulo_SP = Controle.Angulo;
});
break;
}
Controle.DefinirTipoMovimento(value, dispMvd.Dados.Modulos);
}
Controle.TipoMovimento = value;
}
public void AtualizaInformacoesControleOperacao()
@ -1958,7 +1923,7 @@ namespace AgroBase.Models
//Variaveis.MostrarLog($"Angulo SP alterado de {_ControleAnterior.Angulo:F2} para {_Controle.Angulo:F2}");
_ControleAnterior.Angulo = _Controle.Angulo;
_ControleAnterior.TipoMovimento = _Controle.TipoMovimento;
_ControleAnterior.DefinirTipoMovimento(_Controle.TipoMovimento);
RedisService.AtualizarCampos(
CtxKey.DadosControle,
@ -3640,7 +3605,45 @@ namespace AgroBase.Models
}
public bool EmFreio { get; set; } = false;
public double AlturaBarra { get; set; }
public TipoMovimentoDirecional TipoMovimento { get; set; } = TipoMovimentoDirecional.Diagnostico;
[JsonProperty]
public TipoMovimentoDirecional TipoMovimento { get; private set; } = TipoMovimentoDirecional.Diagnostico;
public void DefinirTipoMovimento(TipoMovimentoDirecional value, List<ModuloMvdModel> modulos = null)
{
if (modulos != null)
{
switch (value)
{
case TipoMovimentoDirecional.RodasDianteiras:
modulos.ForEach(x =>
{
x.MovMotor.Comandar = true;
x.DirMotor.Comandar = x.Modulo_ID.Contains("F");
x.DirMotor.Angulo_SP = x.DirMotor.Comandar ? Angulo : 0;
});
break;
case TipoMovimentoDirecional.RodasTraseiras:
modulos.ForEach(x =>
{
x.MovMotor.Comandar = true;
x.DirMotor.Comandar = x.Modulo_ID.Contains("T");
x.DirMotor.Angulo_SP = x.DirMotor.Comandar ? Angulo : 0;
});
break;
default:
modulos.ForEach(x =>
{
x.MovMotor.Comandar = true;
x.DirMotor.Comandar = true;
x.DirMotor.Angulo_SP = Angulo;
});
break;
}
}
TipoMovimento = value;
}
public List<OperacaoControleTipoModel> TiposControle { get; set; } = new List<OperacaoControleTipoModel>();
public List<MPCSimulacaoModel> SimulacaoMPC { get; set; } = new List<MPCSimulacaoModel>();
public double Latencia { get; set; }
@ -3648,6 +3651,10 @@ namespace AgroBase.Models
public Dictionary<string, ManagerWorkerMessageResponseDebugCustoModel> DebugCustoMpc { get; set; } = new Dictionary<string, ManagerWorkerMessageResponseDebugCustoModel>();
public List<string> Motivos { get; set; } = new List<string>();
public string UltimaSessaoComando { get; set; }
public long? UltimaSeqMovimentoAplicada { get; set; }
public long? UltimaSeqDirecionalAplicada { get; set; }
public void Reiniciar()
{
PercentualVelocidadeSP = 0;
@ -3668,6 +3675,8 @@ namespace AgroBase.Models
Angulo = Angulo,
PercentualVelocidadeSP = PercentualVelocidadeSP,
_pctVelSp = _pctVelSp,
TipoMovimento = TipoMovimento,
AlturaBarra = AlturaBarra,
heartbeat = heartbeat,
ticks_sem_resposta = ticks_sem_resposta,
@ -3687,7 +3696,11 @@ namespace AgroBase.Models
}).ToList() ?? new List<OperacaoControleTipoModel>(),
SimulacaoMPC = new List<MPCSimulacaoModel>(SimulacaoMPC ?? new List<MPCSimulacaoModel>()),
DebugCustoMpc = new Dictionary<string, ManagerWorkerMessageResponseDebugCustoModel>(DebugCustoMpc ?? new Dictionary<string, ManagerWorkerMessageResponseDebugCustoModel>()),
Motivos = new List<string>(Motivos ?? new List<string>())
Motivos = new List<string>(Motivos ?? new List<string>()),
UltimaSessaoComando = UltimaSessaoComando,
UltimaSeqMovimentoAplicada = UltimaSeqMovimentoAplicada,
UltimaSeqDirecionalAplicada = UltimaSeqDirecionalAplicada
};
return clone;
@ -3787,19 +3800,33 @@ namespace AgroBase.Models
tempoDecorridoSeg = Math.Max(0, (fim - Operacao.DataInicio).TotalSeconds);
}
double tempoAguardandoRaw = RedisService.GetField<double>(CtxKey.DadosOperacao, "tempo_aguardando", 0.0);
double tempoAguardandoInicioTs = RedisService.GetField<double>(CtxKey.DadosOperacao, "tempo_aguardando", 0.0);
double tempoAguardarInicio = RedisService.GetField<double>(CtxKey.DadosOperacao, "tempo_aguardar_inicio_operacao", op.TempoIniciarOperacao);
double agoraUnix = DateTimeOffset.Now.ToUnixTimeMilliseconds() / 1000.0;
double tempoAguardandoSeg;
if (tempoAguardandoRaw > 1_000_000_000)
double agoraUnix = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() / 1000.0;
double tempoRestanteInicioSeg = 0.0;
if (tempoAguardandoInicioTs > 1_000_000_000)
{
// Redis está mandando timestamp Unix de início da espera.
tempoAguardandoSeg = Math.Max(0.0, agoraUnix - tempoAguardandoRaw);
/*
* tempo_aguardando é o timestamp Unix
* de quando a espera começou.
*/
double tempoDecorridoEsperaSeg = Math.Max(0.0, agoraUnix - tempoAguardandoInicioTs);
tempoRestanteInicioSeg = Math.Max(0.0, tempoAguardarInicio - tempoDecorridoEsperaSeg);
}
else if (tempoAguardandoInicioTs > 0.0)
{
/*
* Compatibilidade com formato antigo:
* considera o valor como tempo já decorrido.
*/
tempoRestanteInicioSeg = Math.Max(0.0, tempoAguardarInicio - tempoAguardandoInicioTs);
}
else
{
// Compatibilidade: caso algum fluxo antigo mande duração em segundos.
tempoAguardandoSeg = Math.Max(0.0, tempoAguardandoRaw);
/*
* Ainda não existe timestamp de espera.
*/
tempoRestanteInicioSeg = Math.Max(0.0, tempoAguardarInicio);
}
op.TempoIniciarOperacao = Math.Max(0, Convert.ToInt32(Math.Round(tempoAguardarInicio)));
@ -3819,7 +3846,7 @@ namespace AgroBase.Models
DataInicio = Operacao.DataInicio,
DataFim = Operacao.DataFim,
TempoDecorridoSeg = tempoDecorridoSeg,
TempoAguardandoSeg = tempoAguardandoSeg,
TempoAguardandoSeg = tempoRestanteInicioSeg,
TipoCanAtivo = CanManager.TipoServico,
OperacaoLiberada = RedisService.GetField(CtxKey.DadosOperacao, "liberado", false),
@ -3835,10 +3862,7 @@ namespace AgroBase.Models
ModsCalibragemView = Operacao.ModsCalibragem?.ToDictionary(x => x.Key.ToString(), x => new Dictionary<string, bool>(x.Value ?? new Dictionary<string, bool>())) ?? new Dictionary<string, Dictionary<string, bool>>(),
};
if (operacaoAtualizada.StatusOperacaoAtual != Operacao.StatusOperacaoAnterior)
{
AudioAlertaService.AlertaSonoroPorStatus(operacaoAtualizada.StatusOperacaoAtual);
}
bool statusMudou = operacaoAtualizada.StatusOperacaoAtual != Operacao.StatusOperacaoAnterior;
operacaoAtualizada.StatusOperacaoAnterior = operacaoAtualizada.StatusOperacaoAtual;
var saude = OperadorSaude.ModulosSaude;
@ -3877,6 +3901,11 @@ namespace AgroBase.Models
StatusModulo.Operante;
Operacao = operacaoAtualizada;
if (statusMudou)
{
AudioAlertaService.AlertaSonoroPorStatus(operacaoAtualizada.StatusOperacaoAtual);
}
}
// Atu

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

@ -49,6 +49,11 @@ namespace AgroBase.Models.Operadores
public int versao_comando { get; set; } = 1;
public double ts_comando { get; set; } = 0;
public string sessao_comando { get; set; }
public long? seq_movimento { get; set; }
public long? seq_direcional { get; set; }
public bool atualizar_movimento { get; set; } = true;
public bool atualizar_direcional { get; set; } = true;

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

File diff suppressed because it is too large Load Diff

49
AgroBase/AgroBase/Models/Variaveis.cs
View File

@ -27,7 +27,7 @@ namespace AgroBase.Models
{
public class Variaveis
{
public static readonly bool IniciarWorkers = true;
public static readonly bool IniciarWorkers = false;
public static readonly bool Producao = false;
public static bool DebugMode { get; set; } = true;
public static bool Fechando { get; set; } = false;
@ -859,8 +859,8 @@ namespace AgroBase.Models
public static string TopicoMqttParametros { get; } = $"agrobot/v1/rover/<id>/parameters";
public static double LarguraEsquerda { get; } = 44.0; // 62
public static double LarguraDireita { get; } = 44.0; // 22
public static double ComprimentoFrente { get; } = 7.0; // 7
public static double ComprimentoTras { get; } = 107.0; // 107
public static double ComprimentoFrente { get; } = 90.0; // 7
public static double ComprimentoTras { get; } = 40.0; // 107
public static double DistanciaEntreEixos { get; } = 92.0;
public static double LeverArmFrontalCm { get; } = 37.0; // cm
public static double LeverArmLateralCm { get; } = 0.0; // cm
@ -882,7 +882,7 @@ namespace AgroBase.Models
public static int QuantidadeBicosPulverizadores { get; set; } = 7;
public static double AlturaBarraPulverizadoraCm { get; set; } = 80;
public static double ComprimentoBarraPulverizadoraCm { get; set; } = 103.7;
public static double DistanciaEntreBicosCm { get; set; } = 50;
public static double DistanciaEntreBicosCm { get; set; } = 21;
public static double DensidadeHerbicida { get; set; } = 1.0;
public static double CapacidadeReservatorio { get; set; } = 60;
public static double PressaoMinimaCavitacao { get; set; } = 10.0;
@ -962,8 +962,8 @@ namespace AgroBase.Models
{ TipoMovimentoDirecional.MovimentoLateral, 90 },
{ TipoMovimentoDirecional.Diagnostico, 25 },
};
public static double AnguloInclinacaoRollMax { get; set; } = 30.0; // Frontal
public static double AnguloInclinacaoPitchMax { get; set; } = 45.0; // Lateral
public static double AnguloInclinacaoRollMax { get; set; } = 45.0; // Frontal
public static double AnguloInclinacaoPitchMax { get; set; } = 25.0; // Lateral
public static string VozAlerta { get; set; } = "Microsoft Maria Desktop";
@ -1170,32 +1170,51 @@ namespace AgroBase.Models
double dLat = (dy / R_earth) * 180.0 / Math.PI;
double dLon = (dx / (R_earth * Math.Cos(pos.Latitude * Math.PI / 180.0))) * 180.0 / Math.PI;
double latitude = pos.Latitude + dLat;
double longitude = pos.Longitude + dLon;
double latitude = pos.LatitudeAnt + dLat;
double longitude = pos.LongitudeAnt + dLon;
DateTime agora = DateTime.Now;
double agoraMono = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
double orientacaoRealDeg = GPSUtils.NormalizarAngulo(theta * 180.0 / Math.PI);
double orientacaoMovimentoDeg = GPSUtils.NormalizarAngulo(heading * 180.0 / Math.PI);
GPSModel novaPosicao = new GPSModel
{
Momento = DateTime.Now,
Momento = agora,
DataHora = agora,
UltimoComandoRespondido = agora,
Lat0 = GPSService.UltimaLeitura.Lat0,
Lon0 = GPSService.UltimaLeitura.Lon0,
Latitude = latitude,
LatitudeAnt = latitude,
Longitude = longitude,
LongitudeAnt = longitude,
OrientacaoReal = GPSUtils.NormalizarAngulo(theta * 180.0 / Math.PI),
OrientacaoReal = orientacaoRealDeg,
AnguloCarroDefinido = orientacaoRealDeg,
OrientacaoMovimento = orientacaoMovimentoDeg,
TipoOrientacao = "A",
Distancia = velocidadeMs * tempoDelta,
Velocidade = velocidadeMs,
Heartbeat = ultimaPosicao.Heartbeat,
Heartbeat = ultimaPosicao.Heartbeat + 1,
TimestampOri = ultimaPosicao.TimestampOri.Clone(),
TimestampPos = ultimaPosicao.TimestampPos.Clone(),
};
var corrigida =
GPSService.LeverArm.FixLeverArmLatLon_Fast(
novaPosicao.LatitudeAnt,
novaPosicao.LongitudeAnt,
novaPosicao.OrientacaoReal,
5
);
novaPosicao.Latitude = corrigida.lat;
novaPosicao.Longitude = corrigida.lon;
//(double latCor, double lonCorr) = GeoLeverArm.FixLeverArmLatLon_Fast(latitude, longitude, novaPosicao.OrientacaoReal);
//novaPosicao.Latitude = latCor;
//novaPosicao.Longitude = lonCorr;
novaPosicao.TimestampOri.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
novaPosicao.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
novaPosicao.TimestampOri.valor = agoraMono;
novaPosicao.TimestampPos.valor = agoraMono;
return novaPosicao;
}

2
AgroBase/AgroBase/Services/AudioAlertaService.cs
View File

@ -94,7 +94,7 @@ namespace AgroBase.Services
// ------- helpers locais -------
string SegundosFmt(double s)
{
int v = (int)Math.Round(s);
int v = Math.Max(0, (int)Math.Ceiling(s));
return v == 1 ? "1 segundo" : $"{v} segundos";
}

95
AgroBase/AgroBase/Services/GPSService.cs
View File

@ -2189,28 +2189,15 @@ namespace AgroBase.Services
AtualizarTrajetoriaDinamica();
int enderecoEquipamento = 1;
int enderecoEquipamento = 0x02;
EnviarCoordenadasParaMapa(
UltimaLeitura.Latitude,
UltimaLeitura.Longitude,
UltimaLeitura.AnguloCarroDefinido,
op.Sensoriamento.Operacao.OperacaoIniciada,
enderecoEquipamento
);
EnviarCoordenadasParaMapa(UltimaLeitura.Latitude, UltimaLeitura.Longitude, UltimaLeitura.AnguloCarroDefinido, op.Sensoriamento.Operacao.OperacaoIniciada, enderecoEquipamento);
GPSModel posicaoBase = VariaveisOperacao.ObterPosicaoBaseSnapshot();
if ((posicaoBase?.UltimoComandoRespondido ?? DateTime.MinValue) > DateTime.UtcNow.AddSeconds(-60))
{
int enderecoBase = 0;
EnviarCoordenadasParaMapa(
posicaoBase.Latitude,
posicaoBase.Longitude,
posicaoBase.OrientacaoReal,
false,
enderecoBase
);
int enderecoBase = 0x01;
EnviarCoordenadasParaMapa(posicaoBase.Latitude, posicaoBase.Longitude, posicaoBase.OrientacaoReal, false, enderecoBase);
}
PenultimaLeitura.Inicializado = UltimaLeitura.Inicializado;
@ -2375,23 +2362,10 @@ namespace AgroBase.Services
{
if (UltimasLeituras.Count < 2)
{
double ang =
GPSUtils.CalcularOrientacao(
PenultimaLeitura,
UltimaLeitura
);
double d =
GPSUtils.DistanciaEntrePontos(
PenultimaLeitura,
UltimaLeitura
);
PenultimaLeitura.OrientacaoMovimento =
UltimaLeitura.OrientacaoMovimento;
PenultimaLeitura.Distancia =
UltimaLeitura.Distancia;
double ang = GPSUtils.CalcularOrientacao(PenultimaLeitura, UltimaLeitura);
double d = GPSUtils.DistanciaEntrePontos(PenultimaLeitura, UltimaLeitura);
PenultimaLeitura.OrientacaoMovimento = UltimaLeitura.OrientacaoMovimento;
PenultimaLeitura.Distancia = UltimaLeitura.Distancia;
UltimaLeitura.OrientacaoMovimento = ang;
UltimaLeitura.Distancia = d;
@ -2402,24 +2376,19 @@ namespace AgroBase.Services
double sumY = 0;
double distAcum = 0;
for (int i = 0;
i < UltimasLeituras.Count - 1;
i++)
for (int i = 0; i < UltimasLeituras.Count - 1; i++)
{
GPSModel a = UltimasLeituras[i];
GPSModel b = UltimasLeituras[i + 1];
double angSeg =
GPSUtils.CalcularOrientacao(a, b);
double angSeg = GPSUtils.CalcularOrientacao(a, b);
double dSeg =
GPSUtils.DistanciaEntrePontos(a, b);
double dSeg = GPSUtils.DistanciaEntrePontos(a, b);
if (dSeg <= 0)
continue;
double rad =
angSeg * Math.PI / 180.0;
double rad = angSeg * Math.PI / 180.0;
sumX += Math.Cos(rad) * dSeg;
sumY += Math.Sin(rad) * dSeg;
@ -2430,44 +2399,22 @@ namespace AgroBase.Services
if (distAcum <= 0)
{
anguloMovimento =
UltimaLeitura.OrientacaoReal;
anguloMovimento = UltimaLeitura.OrientacaoReal;
}
else
{
anguloMovimento =
Math.Atan2(sumY, sumX) *
180.0 / Math.PI;
anguloMovimento =
GPSUtils.NormalizarAngulo(
anguloMovimento
);
anguloMovimento = Math.Atan2(sumY, sumX) * 180.0 / Math.PI;
anguloMovimento = GPSUtils.NormalizarAngulo(anguloMovimento);
}
GPSModel first =
UltimasLeituras[0];
GPSModel first = UltimasLeituras[0];
GPSModel last = UltimasLeituras[UltimasLeituras.Count - 1];
GPSModel last =
UltimasLeituras[
UltimasLeituras.Count - 1
];
double distLinear =
GPSUtils.DistanciaEntrePontos(
first,
last
);
PenultimaLeitura.OrientacaoMovimento =
UltimaLeitura.OrientacaoMovimento;
PenultimaLeitura.Distancia =
UltimaLeitura.Distancia;
UltimaLeitura.OrientacaoMovimento =
anguloMovimento;
double distLinear = GPSUtils.DistanciaEntrePontos(first, last);
PenultimaLeitura.OrientacaoMovimento = UltimaLeitura.OrientacaoMovimento;
PenultimaLeitura.Distancia = UltimaLeitura.Distancia;
UltimaLeitura.OrientacaoMovimento = anguloMovimento;
UltimaLeitura.Distancia = distLinear;
}

151
AgroBase/AgroBase/Services/Operadores/ManagerWorkerService.cs
View File

@ -236,6 +236,10 @@ namespace AgroBase.Services.Operadores
versao_comando = GetInt(dictParams, "versao_comando", 1),
ts_comando = GetDouble(dictParams, "ts_comando", 0.0),
sessao_comando = dictParams.Value<string>("sessao_comando"),
seq_movimento = dictParams["seq_movimento"]?.Value<long?>(),
seq_direcional = dictParams["seq_direcional"]?.Value<long?>(),
// Default true para compatibilidade com contrato antigo
atualizar_movimento = GetBool(dictParams, "atualizar_movimento", true),
atualizar_direcional = GetBool(dictParams, "atualizar_direcional", true),
@ -283,33 +287,80 @@ namespace AgroBase.Services.Operadores
var op = Variaveis.OperacaoEmAndamento;
var _Controle = op.Controle;
var pControle = op.Parametros.Controle;
if (_Controle == null || pControle == null)
if (op == null)
return;
_Controle.Motivos = (novoComando.motivos ?? new string[0]).ToList();
var controle = op.Controle;
var parametros = op.Parametros?.Controle;
bool podeAtualizarMovimento =
novoComando.atualizar_movimento &&
pControle.MovimentoAutomatico;
if (controle == null || parametros == null)
return;
bool podeAtualizarDirecional =
novoComando.atualizar_direcional &&
pControle.DirecionalAutomatico;
controle.Motivos = (novoComando.motivos ?? Array.Empty<string>()).ToList();
bool movimentoMudou =
_Controle.PercentualVelocidadeSP != novoComando.percentual_velocidade ||
_Controle.EmFreio != novoComando.em_freio;
bool sessaoMudou = !string.IsNullOrWhiteSpace(novoComando.sessao_comando) && !string.Equals(controle.UltimaSessaoComando, novoComando.sessao_comando, StringComparison.Ordinal);
bool direcionalMudou =
_Controle.Angulo != novoComando.angulo ||
_Controle.TipoMovimento != novoComando.tipo_movimento;
bool movimentoMudou = Math.Abs(controle.PercentualVelocidadeSP - novoComando.percentual_velocidade) >= 0.001 || controle.EmFreio != novoComando.em_freio;
if (podeAtualizarMovimento && (movimentoMudou || forcarAplicacao))
bool direcionalMudou = Math.Abs(controle.Angulo - novoComando.angulo) >= 0.001 || controle.TipoMovimento != novoComando.tipo_movimento;
bool seqMovimentoMudou =
novoComando.seq_movimento.HasValue &&
(
sessaoMudou ||
!controle.UltimaSeqMovimentoAplicada.HasValue ||
controle.UltimaSeqMovimentoAplicada.Value !=
novoComando.seq_movimento.Value
);
bool seqDirecionalMudou =
novoComando.seq_direcional.HasValue &&
(
sessaoMudou ||
!controle.UltimaSeqDirecionalAplicada.HasValue ||
controle.UltimaSeqDirecionalAplicada.Value !=
novoComando.seq_direcional.Value
);
/*
* Compatibilidade:
*
* Mensagem nova:
* sequência é a autoridade.
*
* Mensagem antiga:
* usa flag e mudança de valor.
*/
bool mensagemPossuiSequenciaMov = novoComando.seq_movimento.HasValue;
bool mensagemPossuiSequenciaDir = novoComando.seq_direcional.HasValue;
bool aplicarMovimento =
parametros.MovimentoAutomatico &&
(
forcarAplicacao ||
movimentoMudou ||
seqMovimentoMudou ||
(
!mensagemPossuiSequenciaMov &&
novoComando.atualizar_movimento
)
);
bool aplicarDirecional =
parametros.DirecionalAutomatico &&
(
forcarAplicacao ||
direcionalMudou ||
seqDirecionalMudou ||
(
!mensagemPossuiSequenciaDir &&
novoComando.atualizar_direcional
)
);
if (aplicarMovimento)
{
var tipoMov = _Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov);
var tipoMov = controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov);
if (tipoMov != null)
{
@ -319,15 +370,21 @@ namespace AgroBase.Services.Operadores
: Direcao.Parado;
}
_Controle.PercentualVelocidadeSP = novoComando.percentual_velocidade;
_Controle.EmFreio = pControle.FrenagemAutomaticaAoParar && novoComando.em_freio;
controle.PercentualVelocidadeSP = novoComando.percentual_velocidade;
op.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Mov });
controle.EmFreio = parametros.FrenagemAutomaticaAoParar && novoComando.em_freio;
op.AtualizarDadosControle(false, new List<T_Code> { T_Code.Mov });
if (novoComando.seq_movimento.HasValue)
{
controle.UltimaSeqMovimentoAplicada = novoComando.seq_movimento.Value;
}
}
if (podeAtualizarDirecional && (direcionalMudou || forcarAplicacao))
if (aplicarDirecional)
{
var tipoDir = _Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir);
var tipoDir = controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir);
if (tipoDir != null)
{
@ -339,26 +396,38 @@ namespace AgroBase.Services.Operadores
: Direcao.Parado;
}
_Controle.Angulo = novoComando.angulo;
_Controle.TipoMovimento = novoComando.tipo_movimento;
controle.Angulo = novoComando.angulo;
_Controle.SimulacaoMPC = novoComando.simulacao?
.Where(x => x != null && x.Length >= 3)
.Select(x => new MPCSimulacaoModel()
{
latitude = x[0],
longitude = x[1],
orientacao = x[2]
})
.ToList() ?? new List<MPCSimulacaoModel>();
op.DefinirTipoMovimentoControle(novoComando.tipo_movimento);
_Controle.Latencia = novoComando.latencia;
_Controle.ErroLateral = novoComando.erro_lateral;
_Controle.DebugCustoMpc =
novoComando.debug_custo ??
new Dictionary<string, ManagerWorkerMessageResponseDebugCustoModel>();
controle.SimulacaoMPC = novoComando.simulacao?
.Where(x => x != null && x.Length >= 3)
.Select(
x => new MPCSimulacaoModel
{
latitude = x[0],
longitude = x[1],
orientacao = x[2]
}
)
.ToList()
?? new List<MPCSimulacaoModel>();
op.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Dir });
controle.Latencia = novoComando.latencia;
controle.ErroLateral = novoComando.erro_lateral;
controle.DebugCustoMpc = novoComando.debug_custo ?? new Dictionary<string, ManagerWorkerMessageResponseDebugCustoModel>();
op.AtualizarDadosControle(false, new List<T_Code> { T_Code.Dir });
if (novoComando.seq_direcional.HasValue)
{
controle.UltimaSeqDirecionalAplicada = novoComando.seq_direcional.Value;
}
}
if (!string.IsNullOrWhiteSpace(novoComando.sessao_comando))
{
controle.UltimaSessaoComando = novoComando.sessao_comando;
}
}

723
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py
View File

@ -1,4 +1,5 @@
import time
import uuid
from shared.enums import TipoMovimentoDirecional
from shared.contexto_global_redis import ContextoGlobalRedis
@ -16,7 +17,11 @@ class ProcessadorEmAndamento(ProcessadorBase):
def __init__(self):
self.filtro_mov = FiltroVelocidade(alpha=0.7)
ang_max = ContextoGlobalRedis.get_controle().get("angulo_max", 30)
controle = ContextoGlobalRedis.get_controle() or {}
ang_max = self._to_float(
controle.get("angulo_max", 30.0),
30.0
)
self.pid_dir = PIDAdaptativo(
kp_base=0.55,
@ -31,7 +36,14 @@ class ProcessadorEmAndamento(ProcessadorBase):
agora = time.perf_counter()
# Cadências independentes
# Identifica esta execução do Manager.
# Se o Python reiniciar, o C# perceberá uma sessão nova.
self.sessao_comando = uuid.uuid4().hex
# Sequências monotônicas por módulo.
self.seq_movimento = 0
self.seq_direcional = 0
self.ultimo_envio_envelope = agora
self.ultimo_envio_movimento = agora
self.ultimo_envio_direcional = agora
@ -40,81 +52,107 @@ class ProcessadorEmAndamento(ProcessadorBase):
self.intervalo_movimento_s = 0.25
self.intervalo_direcional_s = 0.30
# Memória local para evitar publicar movimento idêntico o tempo todo
self.ultima_velocidade_enviada = None
self.ultimo_freio_enviado = None
self.ultimo_angulo_enviado = None
self.ultimo_tipo_enviado = None
# Evita spam de log de bloqueio
self.ultimo_log_bloqueio = 0.0
self.ultima_msg_bloqueio = ""
self.regras = RegrasTaticas()
def processar(self):
try:
agora = time.perf_counter()
mostrar_log(
"🧭 ProcessadorEmAndamento criado | "
f"id={id(self)} | "
f"sessao={self.sessao_comando} | "
f"perf={agora:.4f}"
)
_controle = ContextoGlobalRedis.get_controle()
def processar(self):
agora = time.perf_counter()
try:
controle = ContextoGlobalRedis.get_controle() or {}
movimento_automatico = bool(
_controle.get("movimento_automatico", False)
controle.get("movimento_automatico", False)
)
direcional_automatico = bool(
_controle.get("direcional_automatico", False)
controle.get("direcional_automatico", False)
)
# Se nenhum controle automático está ativo, este processador não comanda nada.
if not movimento_automatico and not direcional_automatico:
return None
valores_atuais = self._ler_valores_atuais(_controle)
estado = self._ler_valores_atuais(controle)
velocidade = estado["velocidade"]
frear = estado["frear"]
angulo = estado["angulo"]
tipo_movimento = estado["tipo_movimento"]
simulacao = estado["simulacao"]
latencia = estado["latencia"]
erro_lateral = estado["erro_lateral"]
debug_custo = estado["debug_custo"]
motivos = []
erro_geral = False
velocidade = valores_atuais["velocidade"]
frear = valores_atuais["frear"]
angulo = valores_atuais["angulo"]
tipo_movimento = valores_atuais["tipo_movimento"]
simulacao = valores_atuais["simulacao"]
latencia = valores_atuais["latencia"]
erro_lateral = valores_atuais["erro_lateral"]
debug_custo = valores_atuais["debug_custo"]
atualizar_movimento = False
atualizar_direcional = False
# ============================================================
# 1) Regras táticas globais
# 1. Bloqueios táticos
# ============================================================
motivos_parada, freio_necessario = self.regras.verifica_controle_liberado()
motivos_parada, freio_necessario = (
self.regras.verifica_controle_liberado()
)
motivos_parada = motivos_parada or []
if motivos_parada:
self._logar_bloqueio(motivos_parada)
# Bloqueio global: para movimento e centraliza direcional,
# mas respeitando quais módulos estão em automático.
atualizar_movimento = movimento_automatico
atualizar_direcional = direcional_automatico
if atualizar_movimento:
if movimento_automatico:
velocidade = 0.0
frear = bool(freio_necessario)
if atualizar_direcional:
atualizar_movimento = (
self._movimento_mudou(
velocidade,
frear
)
or self._venceu_intervalo_movimento(agora)
)
if direcional_automatico:
angulo = 0.0
tipo_movimento = TipoMovimentoDirecional.RodasDianteiras.value
tipo_movimento = (
TipoMovimentoDirecional
.RodasDianteiras
.value
)
simulacao = []
latencia = 0.0
erro_lateral = 0.0
debug_custo = {}
return self._publicar_comando(
atualizar_direcional = (
self._direcional_mudou(
angulo,
tipo_movimento
)
or self._venceu_intervalo_direcional(agora)
)
return self._publicar_se_necessario(
agora=agora,
velocidade=velocidade,
frear=frear,
@ -131,134 +169,69 @@ class ProcessadorEmAndamento(ProcessadorBase):
)
# ============================================================
# 2) Movimento automático
# 2. Movimento
# ============================================================
if movimento_automatico:
comando_mov = definir_comando_mov(self.filtro_mov)
comando_mov = definir_comando_mov(
self.filtro_mov
)
if isinstance(comando_mov, dict):
velocidade_calculada = self._to_float(
comando_mov.get("velocidade", velocidade),
velocidade
)
freio_calculado = bool(
comando_mov.get("frear", False)
)
velocidade = velocidade_calculada
frear = freio_calculado
movimento_mudou = self._movimento_mudou(
velocidade,
frear
)
envio_movimento_periodico = (
agora - self.ultimo_envio_movimento
) >= self.intervalo_movimento_s
atualizar_movimento = (
movimento_mudou or
envio_movimento_periodico
)
else:
# Falha ao calcular movimento automático: falha segura.
if not isinstance(comando_mov, dict):
erro_geral = True
motivos.append("Falha ao definir comando de movimento automático")
motivos.append(
"Falha ao definir comando de movimento automático"
)
velocidade = 0.0
frear = True
atualizar_movimento = True
else:
velocidade = self._to_float(
comando_mov.get(
"velocidade",
velocidade
),
velocidade
)
frear = bool(
comando_mov.get(
"frear",
frear
)
)
atualizar_movimento = (
self._movimento_mudou(
velocidade,
frear
)
or self._venceu_intervalo_movimento(agora)
)
# ============================================================
# 3) Direcional automático
# 3. Direcional
# ============================================================
if direcional_automatico:
envio_direcional_necessario = (
agora - self.ultimo_envio_direcional
) >= self.intervalo_direcional_s
envio_periodico_necessario = (
self._venceu_intervalo_direcional(agora)
)
comando_dir = definir_comando_dir(
self.pid_dir,
envio_direcional_necessario
envio_periodico_necessario
)
if isinstance(comando_dir, dict):
erro_dir = bool(comando_dir.get("erro", False))
parada_necessaria = bool(
comando_dir.get("parada_necessaria", False)
)
if erro_dir:
erro_geral = True
motivos.extend(
comando_dir.get(
"motivos",
["Erro ao definir comando direcional automático"]
) or []
)
# Se o direcional falhou, não deixa o robô seguir andando
# sob movimento automático.
if movimento_automatico:
velocidade = 0.0
frear = True
atualizar_movimento = True
# Direcional vai para posição segura.
angulo = 0.0
tipo_movimento = TipoMovimentoDirecional.RodasDianteiras.value
simulacao = []
latencia = self._to_float(comando_dir.get("latencia", 0.0), 0.0)
erro_lateral = self._to_float(comando_dir.get("erro_lateral", 0.0), 0.0)
debug_custo = comando_dir.get("debug_custo", {}) or {}
atualizar_direcional = True
else:
enviar_direcional = bool(
comando_dir.get("enviar_comando", False)
)
if enviar_direcional:
angulo = self._to_float(
comando_dir.get("angulo", angulo),
angulo
)
tipo_movimento = self._to_int(
comando_dir.get("tipo", tipo_movimento),
tipo_movimento
)
simulacao = comando_dir.get("simulacao", []) or []
latencia = self._to_float(
comando_dir.get("latencia", latencia),
latencia
)
erro_lateral = self._to_float(
comando_dir.get("erro_lateral", erro_lateral),
erro_lateral
)
debug_custo = comando_dir.get("debug_custo", {}) or {}
atualizar_direcional = True
if parada_necessaria:
motivos.extend(comando_dir.get("motivos", []) or [])
if movimento_automatico:
velocidade = 0.0
frear = True
atualizar_movimento = True
else:
# Falha ao calcular direcional automático: falha segura.
if not isinstance(comando_dir, dict):
erro_geral = True
motivos.append("Falha ao definir comando direcional automático")
motivos.append(
"Falha ao definir comando direcional automático"
)
if movimento_automatico:
velocidade = 0.0
@ -266,35 +239,204 @@ class ProcessadorEmAndamento(ProcessadorBase):
atualizar_movimento = True
angulo = 0.0
tipo_movimento = TipoMovimentoDirecional.RodasDianteiras.value
tipo_movimento = (
TipoMovimentoDirecional
.RodasDianteiras
.value
)
simulacao = []
latencia = 0.0
erro_lateral = 0.0
debug_custo = {}
atualizar_direcional = True
# ============================================================
# 4) Publicação do envelope
# ============================================================
else:
erro_dir = bool(
comando_dir.get("erro", False)
)
precisa_keepalive = (
agora - self.ultimo_envio_envelope
) >= self.intervalo_keepalive_s
if erro_dir:
erro_geral = True
deve_publicar = (
atualizar_movimento or
atualizar_direcional or
precisa_keepalive
)
motivos.extend(
comando_dir.get(
"motivos",
[
"Erro ao definir comando "
"direcional automático"
]
) or []
)
if not deve_publicar:
return None
if movimento_automatico:
velocidade = 0.0
frear = True
atualizar_movimento = True
# Keepalive puro: Manager vivo, mas nenhum módulo precisa reaplicar.
if precisa_keepalive and not atualizar_movimento and not atualizar_direcional:
motivos = []
angulo = 0.0
tipo_movimento = (
TipoMovimentoDirecional
.RodasDianteiras
.value
)
return self._publicar_comando(
simulacao = []
latencia = self._to_float(
comando_dir.get(
"latencia",
0.0
),
0.0
)
erro_lateral = self._to_float(
comando_dir.get(
"erro_lateral",
0.0
),
0.0
)
debug_custo = (
comando_dir.get(
"debug_custo",
{}
)
or {}
)
atualizar_direcional = True
else:
angulo_calculado = self._to_float(
comando_dir.get(
"angulo",
angulo
),
angulo
)
tipo_calculado = self._to_int(
comando_dir.get(
"tipo",
tipo_movimento
),
tipo_movimento
)
simulacao_calculada = (
comando_dir.get(
"simulacao",
[]
)
or []
)
latencia_calculada = self._to_float(
comando_dir.get(
"latencia",
latencia
),
latencia
)
erro_lateral_calculado = self._to_float(
comando_dir.get(
"erro_lateral",
erro_lateral
),
erro_lateral
)
debug_calculado = (
comando_dir.get(
"debug_custo",
{}
)
or {}
)
solicitou_envio_mpc = bool(
comando_dir.get(
"enviar_comando",
False
)
)
direcional_mudou = (
self._direcional_mudou(
angulo_calculado,
tipo_calculado
)
)
envio_periodico = (
self._venceu_intervalo_direcional(
agora
)
)
atualizar_direcional = (
solicitou_envio_mpc
or direcional_mudou
or envio_periodico
)
angulo = angulo_calculado
tipo_movimento = tipo_calculado
simulacao = simulacao_calculada
latencia = latencia_calculada
erro_lateral = erro_lateral_calculado
debug_custo = debug_calculado
if not isinstance(debug_custo, dict):
debug_custo = {}
debug_custo[
"decisao_envio_direcional"
] = {
"solicitou_envio_mpc":
solicitou_envio_mpc,
"direcional_mudou":
direcional_mudou,
"envio_periodico":
envio_periodico,
"atualizar_direcional":
atualizar_direcional,
"angulo_calculado":
round(angulo, 3),
"angulo_ultimo_enviado": (
None
if self.ultimo_angulo_enviado is None
else round(
self.ultimo_angulo_enviado,
3
)
),
"tipo_calculado":
int(tipo_movimento),
"tipo_ultimo_enviado": (
self.ultimo_tipo_enviado
),
"seq_direcional_atual":
self.seq_direcional,
"sessao_comando":
self.sessao_comando,
}
return self._publicar_se_necessario(
agora=agora,
velocidade=velocidade,
frear=frear,
@ -310,30 +452,86 @@ class ProcessadorEmAndamento(ProcessadorBase):
atualizar_direcional=atualizar_direcional,
)
except Exception as e:
mostrar_log(f"Erro no processador EmAndamento: {e}")
except Exception as ex:
mostrar_log(
f"Erro no processador EmAndamento: {ex}"
)
# Falha no orquestrador em operação automática:
# tenta publicar parada segura.
try:
return comando_controle(
percentual_velocidade=0.0,
return self._publicar_comando(
agora=agora,
velocidade=0.0,
frear=True,
angulo=0.0,
tipo_movimento=TipoMovimentoDirecional.RodasDianteiras.value,
tipo_movimento=(
TipoMovimentoDirecional
.RodasDianteiras
.value
),
simulacao=[],
erro=True,
latencia=0.0,
erro_lateral=0.0,
debug_custo={},
motivos=[f"Erro no processador EmAndamento: {e}"],
motivos=[
f"Erro no processador EmAndamento: {ex}"
],
atualizar_movimento=True,
atualizar_direcional=True,
)
except Exception as e2:
mostrar_log(f"Erro ao publicar parada segura no EmAndamento: {e2}")
except Exception as ex2:
mostrar_log(
"Erro ao publicar parada segura no "
f"EmAndamento: {ex2}"
)
return None
def _publicar_se_necessario(
self,
*,
agora,
velocidade,
frear,
angulo,
tipo_movimento,
simulacao,
erro,
latencia,
erro_lateral,
debug_custo,
motivos,
atualizar_movimento,
atualizar_direcional,
):
precisa_keepalive = (
agora - self.ultimo_envio_envelope
) >= self.intervalo_keepalive_s
if not (
atualizar_movimento
or atualizar_direcional
or precisa_keepalive
):
return None
return self._publicar_comando(
agora=agora,
velocidade=velocidade,
frear=frear,
angulo=angulo,
tipo_movimento=tipo_movimento,
simulacao=simulacao,
erro=erro,
latencia=latencia,
erro_lateral=erro_lateral,
debug_custo=debug_custo,
motivos=motivos,
atualizar_movimento=atualizar_movimento,
atualizar_direcional=atualizar_direcional,
)
def _publicar_comando(
self,
*,
@ -351,6 +549,14 @@ class ProcessadorEmAndamento(ProcessadorBase):
atualizar_movimento,
atualizar_direcional,
):
# A sequência aumenta somente quando aquele módulo
# deve reaplicar o comando.
if atualizar_movimento:
self.seq_movimento += 1
if atualizar_direcional:
self.seq_direcional += 1
payload = comando_controle(
percentual_velocidade=velocidade,
frear=frear,
@ -362,77 +568,189 @@ class ProcessadorEmAndamento(ProcessadorBase):
erro_lateral=erro_lateral,
debug_custo=debug_custo,
motivos=motivos or [],
atualizar_movimento=atualizar_movimento,
atualizar_direcional=atualizar_direcional,
seq_movimento=self.seq_movimento,
seq_direcional=self.seq_direcional,
sessao_comando=self.sessao_comando,
)
self.ultimo_envio_envelope = agora
if atualizar_movimento:
self.ultimo_envio_movimento = agora
self.ultima_velocidade_enviada = velocidade
self.ultimo_freio_enviado = frear
self.ultima_velocidade_enviada = float(
velocidade
)
self.ultimo_freio_enviado = bool(frear)
if atualizar_direcional:
self.ultimo_envio_direcional = agora
self.ultimo_angulo_enviado = angulo
self.ultimo_tipo_enviado = tipo_movimento
self.ultimo_angulo_enviado = float(angulo)
self.ultimo_tipo_enviado = int(
tipo_movimento
)
return payload
def _ler_valores_atuais(self, _controle):
def _venceu_intervalo_movimento(self, agora):
return (
agora - self.ultimo_envio_movimento
) >= self.intervalo_movimento_s
def _venceu_intervalo_direcional(self, agora):
return (
agora - self.ultimo_envio_direcional
) >= self.intervalo_direcional_s
def _ler_valores_atuais(self, controle):
return {
"velocidade": self._to_float(
_controle.get("velocidade_sp", 0.0),
0.0
),
"frear": bool(
_controle.get("em_freio", False)
),
"angulo": self._to_float(
_controle.get("angulo_sp", 0.0),
0.0
),
"tipo_movimento": self._to_int(
_controle.get(
"tipo_movimento_direcional",
TipoMovimentoDirecional.RodasDianteiras.value
controle.get(
"velocidade_sp",
0.0
),
TipoMovimentoDirecional.RodasDianteiras.value
0.0
),
"simulacao": _controle.get("simulacao", []) or [],
"frear": bool(
controle.get(
"em_freio",
False
)
),
"angulo": self._to_float(
controle.get(
"angulo_sp",
0.0
),
0.0
),
"tipo_movimento": self._to_int(
controle.get(
"tipo_movimento_direcional",
TipoMovimentoDirecional
.RodasDianteiras
.value
),
TipoMovimentoDirecional
.RodasDianteiras
.value
),
"simulacao": (
controle.get(
"simulacao",
[]
)
or []
),
"latencia": self._to_float(
_controle.get("latencia", 0.0),
controle.get(
"latencia",
0.0
),
0.0
),
"erro_lateral": self._to_float(
_controle.get("erro_lateral", 0.0),
controle.get(
"erro_lateral",
0.0
),
0.0
),
"debug_custo": _controle.get("debug_custo", {}) or {},
"debug_custo": (
controle.get(
"debug_custo",
{}
)
or {}
),
}
def _movimento_mudou(self, velocidade, frear):
def _movimento_mudou(
self,
velocidade,
frear,
tolerancia_velocidade=0.05
):
if self.ultima_velocidade_enviada is None:
return True
if self.ultimo_freio_enviado is None:
return True
if bool(frear) != bool(self.ultimo_freio_enviado):
if bool(frear) != bool(
self.ultimo_freio_enviado
):
return True
return abs(
float(velocidade) - float(self.ultima_velocidade_enviada)
) >= 0.05
float(velocidade)
- float(self.ultima_velocidade_enviada)
) >= tolerancia_velocidade
def _direcional_mudou(
self,
angulo,
tipo_movimento,
tolerancia_angulo=0.10
):
if self.ultimo_angulo_enviado is None:
return True
if self.ultimo_tipo_enviado is None:
return True
tipo_atual = self._to_int(
tipo_movimento,
TipoMovimentoDirecional
.RodasDianteiras
.value
)
tipo_anterior = self._to_int(
self.ultimo_tipo_enviado,
TipoMovimentoDirecional
.RodasDianteiras
.value
)
if tipo_atual != tipo_anterior:
return True
return abs(
float(angulo)
- float(self.ultimo_angulo_enviado)
) >= tolerancia_angulo
def _logar_bloqueio(self, motivos):
agora = time.perf_counter()
msg = " | ".join(map(str, motivos))
mensagem = " | ".join(
map(str, motivos)
)
if msg != self.ultima_msg_bloqueio or (agora - self.ultimo_log_bloqueio) >= 1.0:
mostrar_log(f"⛔ Controle bloqueado: {msg}")
self.ultima_msg_bloqueio = msg
mudou = (
mensagem != self.ultima_msg_bloqueio
)
venceu_intervalo = (
agora - self.ultimo_log_bloqueio
) >= 1.0
if mudou or venceu_intervalo:
mostrar_log(
f"⛔ Controle bloqueado: {mensagem}"
)
self.ultima_msg_bloqueio = mensagem
self.ultimo_log_bloqueio = agora
@staticmethod
@ -440,7 +758,9 @@ class ProcessadorEmAndamento(ProcessadorBase):
try:
if valor is None:
return float(padrao)
return float(valor)
except Exception:
return float(padrao)
@ -454,5 +774,6 @@ class ProcessadorEmAndamento(ProcessadorBase):
return int(valor.value)
return int(valor)
except Exception:
return int(padrao)
return int(padrao)

14
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/padroes.py
View File

@ -29,8 +29,11 @@ def comando_controle(
erro_lateral: float = 0.0,
debug_custo=None,
motivos=None,
atualizar_movimento: bool = True,
atualizar_direcional: bool = True,
atualizar_movimento: bool = False,
atualizar_direcional: bool = False,
seq_movimento: int = 0,
seq_direcional: int = 0,
sessao_comando: str =None,
):
_controle = ContextoGlobalRedis.get_controle()
@ -47,9 +50,14 @@ def comando_controle(
hb_novo = _proximo_heartbeat(hb_atual)
payload = {
"versao_comando": 2,
"versao_comando": 3,
"ts_comando": time.time(),
"sessao_comando": sessao_comando,
"seq_movimento": int(seq_movimento),
"seq_direcional": int(seq_direcional),
# Flags novas do contrato
"atualizar_movimento": bool(atualizar_movimento),
"atualizar_direcional": bool(atualizar_direcional),