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