ajustes no calculo de risco do imu, autonomia de reservatorio, bateria e trajetoria.

This commit is contained in:
Diego Freitas 2026-07-14 15:54:20 -03:00
parent ded951d733
commit a59bee710d
8 changed files with 5008 additions and 864 deletions

View File

@ -1763,7 +1763,7 @@ namespace AgroBase.Forms.Operacoes
}
// agrupar por tipo
var porTipo = dict.Where(x => x.Key.Contains("_")).GroupBy(kv => CandMpcKey.ParseKey(kv.Key).Tipo);
var porTipo = dict.Where(x => x.Key.Contains("_") && !x.Key.Contains("d")).GroupBy(kv => CandMpcKey.ParseKey(kv.Key).Tipo);
foreach (var g in porTipo.OrderBy(gr => gr.Key.ToString()))
{
var tipoNode = tv.Nodes.Add(g.Key.ToString());

File diff suppressed because it is too large Load Diff

View File

@ -27,7 +27,7 @@ namespace AgroBase.Models
{
public class Variaveis
{
public static readonly bool IniciarWorkers = false;
public static readonly bool IniciarWorkers = true;
public static readonly bool Producao = false;
public static bool DebugMode { get; set; } = true;
public static bool Fechando { get; set; } = false;
@ -873,7 +873,7 @@ namespace AgroBase.Models
}
public static double TensaoMinimaBateria { get; set; } = 30.0;
public static double TensaoMaximaBateria { get; set; } = 42.0;
public static double CorrenteMaximaBateria { get; set; } = 20.0;
public static double CorrenteMaximaBateria { get; set; } = 100.0;
public static double PercentualTensaoBateriaMin { get; set; } = 25.0;
public static double PercentualReservatorioMin { get; set; } = 8.0;
public static double PercentualReservatorioMinCritio { get; set; } = 5.0;
@ -962,8 +962,8 @@ namespace AgroBase.Models
{ TipoMovimentoDirecional.MovimentoLateral, 90 },
{ TipoMovimentoDirecional.Diagnostico, 25 },
};
public static double AnguloInclinacaoRollMax { get; set; } = 45.0; // Frontal
public static double AnguloInclinacaoPitchMax { get; set; } = 25.0; // Lateral
public static double AnguloInclinacaoRollMax { get; set; } = 30.0; // Frontal 40 graus max
public static double AnguloInclinacaoPitchMax { get; set; } = 22.0; // Lateral
public static string VozAlerta { get; set; } = "Microsoft Maria Desktop";

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

View File

@ -35,20 +35,28 @@ class CameraIMU(ModuloDiagnosticoBase):
timeout_imu_s=0.40,
timeout_camera_s=2.0,
max_disagreement_deg=4.0,
roll_attention_deg=7.0,
roll_risk_deg=11.0,
roll_critical_deg=16.0,
roll_emergency_deg=22.0,
pitch_attention_deg=8.0,
pitch_risk_deg=14.0,
pitch_critical_deg=20.0,
pitch_emergency_deg=28.0,
# Limites principais vindos do equipamento.
# No rover:
# roll = inclinação frontal
# pitch = inclinação lateral
roll_max_default_deg=45.0,
pitch_max_default_deg=25.0,
# Níveis derivados do limite máximo configurado.
angulo_attention_factor=0.50,
angulo_risk_factor=0.75,
angulo_emergency_factor=1.20,
limites_refresh_s=1.0,
# Taxas continuam independentes do ângulo máximo.
roll_rate_attention_dps=25.0,
roll_rate_risk_dps=40.0,
roll_rate_critical_dps=65.0,
pitch_rate_attention_dps=25.0,
pitch_rate_risk_dps=45.0,
pitch_rate_critical_dps=75.0,
filtro_alpha=0.35,
historico_len=100,
modulos_imu=None,
@ -61,15 +69,35 @@ class CameraIMU(ModuloDiagnosticoBase):
self.timeout_camera_s = float(timeout_camera_s)
self.max_disagreement_deg = float(max_disagreement_deg)
self.roll_attention_deg = float(roll_attention_deg)
self.roll_risk_deg = float(roll_risk_deg)
self.roll_critical_deg = float(roll_critical_deg)
self.roll_emergency_deg = float(roll_emergency_deg)
# ============================================================
# Limites angulares do equipamento
# ============================================================
self.pitch_attention_deg = float(pitch_attention_deg)
self.pitch_risk_deg = float(pitch_risk_deg)
self.pitch_critical_deg = float(pitch_critical_deg)
self.pitch_emergency_deg = float(pitch_emergency_deg)
self.roll_max_default_deg = float(roll_max_default_deg)
self.pitch_max_default_deg = float(pitch_max_default_deg)
self.angulo_attention_factor = float(angulo_attention_factor)
self.angulo_risk_factor = float(angulo_risk_factor)
self.angulo_emergency_factor = float(angulo_emergency_factor)
self.limites_refresh_s = max(float(limites_refresh_s), 0.25)
self._ultimo_refresh_limites = 0.0
# Valores iniciais seguros. Serão substituídos pelos dados do Redis.
self.roll_max_deg = self.roll_max_default_deg
self.pitch_max_deg = self.pitch_max_default_deg
self.roll_attention_deg = 0.0
self.roll_risk_deg = 0.0
self.roll_critical_deg = 0.0
self.roll_emergency_deg = 0.0
self.pitch_attention_deg = 0.0
self.pitch_risk_deg = 0.0
self.pitch_critical_deg = 0.0
self.pitch_emergency_deg = 0.0
self._recalcular_faixas_angulares()
self.roll_rate_attention_dps = float(roll_rate_attention_dps)
self.roll_rate_risk_dps = float(roll_rate_risk_dps)
@ -108,6 +136,9 @@ class CameraIMU(ModuloDiagnosticoBase):
self.roll_rate_filtrado = 0.0
self.pitch_rate_filtrado = 0.0
# Não usar self.last_data para controlar a inicialização.
# Pode existir last_data de uma condição "sem_imu".
self._filtro_fusao_inicializado = False
self.roll_hist = deque(maxlen=historico_len)
self.pitch_hist = deque(maxlen=historico_len)
@ -596,6 +627,120 @@ class CameraIMU(ModuloDiagnosticoBase):
return fontes, ignoradas, desabilitadas
def _validar_limite_angulo(self, valor, valor_atual):
"""
Valida um limite angular vindo do Redis.
Em caso de valor ausente ou inválido, mantém o último valor válido.
Isso impede que uma falha momentânea no contexto altere a segurança.
"""
try:
valor = float(valor)
if not math.isfinite(valor):
return float(valor_atual)
# Evita configurações claramente inválidas.
if valor < 5.0 or valor > 85.0:
return float(valor_atual)
return valor
except Exception:
return float(valor_atual)
def _recalcular_faixas_angulares(self):
"""
Deriva attention, risk, critical e emergency usando os dois
limites principais definidos pelo operador.
No contrato físico deste rover:
- roll = inclinação frontal
- pitch = inclinação lateral
"""
self.roll_attention_deg = (
self.roll_max_deg * self.angulo_attention_factor
)
self.roll_risk_deg = (
self.roll_max_deg * self.angulo_risk_factor
)
self.roll_critical_deg = self.roll_max_deg
self.roll_emergency_deg = min(
89.0,
max(
self.roll_max_deg + 2.0,
self.roll_max_deg * self.angulo_emergency_factor,
)
)
self.pitch_attention_deg = (
self.pitch_max_deg * self.angulo_attention_factor
)
self.pitch_risk_deg = (
self.pitch_max_deg * self.angulo_risk_factor
)
self.pitch_critical_deg = self.pitch_max_deg
self.pitch_emergency_deg = min(
89.0,
max(
self.pitch_max_deg + 2.0,
self.pitch_max_deg * self.angulo_emergency_factor,
)
)
def _atualizar_limites_equipamento(self, forcar=False):
"""
Atualiza periodicamente os limites definidos pelo equipamento.
Não consulta o Redis a 50 Hz, pois esses parâmetros mudam raramente.
"""
agora = time.perf_counter()
if (
not forcar
and (agora - self._ultimo_refresh_limites) < self.limites_refresh_s
):
return
self._ultimo_refresh_limites = agora
equipamento = ContextoGlobalRedis.get_equipamento() or {}
roll_anterior = self.roll_max_deg
pitch_anterior = self.pitch_max_deg
novo_roll_max = self._validar_limite_angulo(
equipamento.get("angulo_roll_max"),
self.roll_max_deg,
)
novo_pitch_max = self._validar_limite_angulo(
equipamento.get("angulo_pitch_max"),
self.pitch_max_deg,
)
mudou = (
abs(novo_roll_max - self.roll_max_deg) > 1e-6
or abs(novo_pitch_max - self.pitch_max_deg) > 1e-6
)
self.roll_max_deg = novo_roll_max
self.pitch_max_deg = novo_pitch_max
self._recalcular_faixas_angulares()
if mudou:
self.mostrar_log(
"Limites angulares atualizados | "
f"roll/frontal={roll_anterior:.1f}°→{self.roll_max_deg:.1f}° | "
f"pitch/lateral={pitch_anterior:.1f}°→{self.pitch_max_deg:.1f}° | "
f"critical=({self.roll_critical_deg:.1f}°, "
f"{self.pitch_critical_deg:.1f}°) | "
f"emergency=({self.roll_emergency_deg:.1f}°, "
f"{self.pitch_emergency_deg:.1f}°)"
)
# ============================================================
# Fusão
# ============================================================
@ -695,6 +840,87 @@ class CameraIMU(ModuloDiagnosticoBase):
"single_source": False,
}
def _atualizar_filtro_fusao(self, fusao):
"""
Atualiza uma única vez por ciclo os valores filtrados da atitude final.
Os ângulos filtrados serão usados tanto:
- na decisão de risco;
- quanto na publicação e nos logs.
As taxas brutas continuam disponíveis para detectar movimentos rápidos.
"""
roll = float(fusao.get("roll", 0.0))
pitch = float(fusao.get("pitch", 0.0))
yaw = float(fusao.get("yaw", 0.0))
roll_rate = float(fusao.get("roll_rate_dps", 0.0))
pitch_rate = float(fusao.get("pitch_rate_dps", 0.0))
alpha = self.clamp(float(self.filtro_alpha), 0.0, 1.0)
if not self._filtro_fusao_inicializado:
self.roll_filtrado = roll
self.pitch_filtrado = pitch
self.yaw_filtrado = yaw
self.roll_rate_filtrado = roll_rate
self.pitch_rate_filtrado = pitch_rate
self._filtro_fusao_inicializado = True
else:
self.roll_filtrado = self._ema(
self.roll_filtrado,
roll,
alpha,
)
self.pitch_filtrado = self._ema(
self.pitch_filtrado,
pitch,
alpha,
)
# Yaw precisa respeitar o wrap -180° / +180°.
dyaw = self._angle_delta_deg(
yaw,
self.yaw_filtrado,
)
self.yaw_filtrado += alpha * dyaw
self.yaw_filtrado = (
self.yaw_filtrado + 180.0
) % 360.0 - 180.0
self.roll_rate_filtrado = self._ema(
self.roll_rate_filtrado,
roll_rate,
alpha,
)
self.pitch_rate_filtrado = self._ema(
self.pitch_rate_filtrado,
pitch_rate,
alpha,
)
return {
"roll": float(self.roll_filtrado),
"pitch": float(self.pitch_filtrado),
"yaw": float(self.yaw_filtrado),
"roll_rate_dps": float(self.roll_rate_filtrado),
"pitch_rate_dps": float(self.pitch_rate_filtrado),
}
def _resetar_filtro_fusao(self):
"""
Faz a próxima leitura válida inicializar o filtro diretamente
com a atitude atual, evitando carregar valores antigos.
"""
self._filtro_fusao_inicializado = False
# ============================================================
# Risco e ação sugerida
# ============================================================
@ -918,6 +1144,7 @@ class CameraIMU(ModuloDiagnosticoBase):
ignoradas,
desabilitadas,
fusao,
atitude_filtrada,
divergencia,
risco,
redundancia_disponivel,
@ -931,24 +1158,12 @@ class CameraIMU(ModuloDiagnosticoBase):
pitch_rate = float(fusao["pitch_rate_dps"])
yaw_rate = float(fusao["yaw_rate_dps"])
a = self.filtro_alpha
roll_filtrado = float(atitude_filtrada["roll"])
pitch_filtrado = float(atitude_filtrada["pitch"])
yaw_filtrado = float(atitude_filtrada["yaw"])
if self.last_data is None:
self.roll_filtrado = roll
self.pitch_filtrado = pitch
self.yaw_filtrado = yaw
self.roll_rate_filtrado = roll_rate
self.pitch_rate_filtrado = pitch_rate
else:
self.roll_filtrado = self._ema(self.roll_filtrado, roll, a)
self.pitch_filtrado = self._ema(self.pitch_filtrado, pitch, a)
dyaw = self._angle_delta_deg(yaw, self.yaw_filtrado)
self.yaw_filtrado = self.yaw_filtrado + a * dyaw
self.yaw_filtrado = (self.yaw_filtrado + 180.0) % 360.0 - 180.0
self.roll_rate_filtrado = self._ema(self.roll_rate_filtrado, roll_rate, a)
self.pitch_rate_filtrado = self._ema(self.pitch_rate_filtrado, pitch_rate, a)
roll_rate_filtrado = float(atitude_filtrada["roll_rate_dps"])
pitch_rate_filtrado = float(atitude_filtrada["pitch_rate_dps"])
self.roll_hist.append(roll)
self.pitch_hist.append(pitch)
@ -988,21 +1203,21 @@ class CameraIMU(ModuloDiagnosticoBase):
"yaw": round(yaw, 3),
# Dado levemente suavizado.
"roll_filtrado": round(self.roll_filtrado, 3),
"pitch_filtrado": round(self.pitch_filtrado, 3),
"yaw_filtrado": round(self.yaw_filtrado, 3),
"roll_filtrado": round(roll_filtrado, 3),
"pitch_filtrado": round(pitch_filtrado, 3),
"yaw_filtrado": round(yaw_filtrado, 3),
# Compatibilidade com contrato antigo.
"roll_seg": round(self.roll_filtrado, 3),
"pitch_seg": round(self.pitch_filtrado, 3),
"roll_seg": round(roll_filtrado, 3),
"pitch_seg": round(pitch_filtrado, 3),
# Tendência angular.
"roll_rate_dps": round(roll_rate, 3),
"pitch_rate_dps": round(pitch_rate, 3),
"yaw_rate_dps": round(yaw_rate, 3),
"roll_rate_filtrado_dps": round(self.roll_rate_filtrado, 3),
"pitch_rate_filtrado_dps": round(self.pitch_rate_filtrado, 3),
"roll_rate_filtrado_dps": round(roll_rate_filtrado, 3),
"pitch_rate_filtrado_dps": round(pitch_rate_filtrado, 3),
# Segurança.
"risk_level": risco["risk_level"],
@ -1016,6 +1231,43 @@ class CameraIMU(ModuloDiagnosticoBase):
"stop": risco["stop"],
"emergency_stop": risco["emergency_stop"],
"convencao_eixos": {
"roll": "frontal",
"pitch": "lateral",
},
"limites_angulo": {
"roll": {
"semantica": "frontal",
"attention_deg": round(self.roll_attention_deg, 3),
"risk_deg": round(self.roll_risk_deg, 3),
"critical_deg": round(self.roll_critical_deg, 3),
"emergency_deg": round(self.roll_emergency_deg, 3),
"max_configurado_deg": round(self.roll_max_deg, 3),
},
"pitch": {
"semantica": "lateral",
"attention_deg": round(self.pitch_attention_deg, 3),
"risk_deg": round(self.pitch_risk_deg, 3),
"critical_deg": round(self.pitch_critical_deg, 3),
"emergency_deg": round(self.pitch_emergency_deg, 3),
"max_configurado_deg": round(self.pitch_max_deg, 3),
},
},
"roll_max_configurado_deg": round(self.roll_max_deg, 3),
"pitch_max_configurado_deg": round(self.pitch_max_deg, 3),
"roll_attention_deg": round(self.roll_attention_deg, 3),
"roll_risk_deg": round(self.roll_risk_deg, 3),
"roll_critical_deg": round(self.roll_critical_deg, 3),
"roll_emergency_deg": round(self.roll_emergency_deg, 3),
"pitch_attention_deg": round(self.pitch_attention_deg, 3),
"pitch_risk_deg": round(self.pitch_risk_deg, 3),
"pitch_critical_deg": round(self.pitch_critical_deg, 3),
"pitch_emergency_deg": round(self.pitch_emergency_deg, 3),
# Diagnóstico de fusão.
"confidence": round(float(fusao.get("confidence", 0.0)), 3),
"fonte_principal": fonte_principal,
@ -1102,6 +1354,8 @@ class CameraIMU(ModuloDiagnosticoBase):
try:
agora = time.time()
self._atualizar_limites_equipamento()
fontes, ignoradas, desabilitadas = self._ler_fontes_imu()
modulos_validos = {
@ -1115,6 +1369,7 @@ class CameraIMU(ModuloDiagnosticoBase):
)
if len(fontes) <= 0:
self._resetar_filtro_fusao()
data = self._montar_saida_sem_imu(
ignoradas,
desabilitadas,
@ -1124,11 +1379,20 @@ class CameraIMU(ModuloDiagnosticoBase):
fusao = self._fundir_fontes(fontes)
divergencia = self._calcular_divergencia(fontes)
# Atualiza o filtro antes de avaliar o risco.
atitude_filtrada = self._atualizar_filtro_fusao(fusao)
risco = self._avaliar_risco(
roll=fusao["roll"],
pitch=fusao["pitch"],
# Ângulos filtrados evitam parada por um único pico de leitura.
roll=atitude_filtrada["roll"],
pitch=atitude_filtrada["pitch"],
# Taxas brutas continuam rápidas.
# Se o robô começar a tombar rapidamente, a segurança não fica
# esperando o filtro angular acompanhar.
roll_rate=fusao["roll_rate_dps"],
pitch_rate=fusao["pitch_rate_dps"],
sensores_agree=divergencia["sensors_agree"],
redundancia_disponivel=redundancia_disponivel,
)
@ -1138,6 +1402,7 @@ class CameraIMU(ModuloDiagnosticoBase):
ignoradas=ignoradas,
desabilitadas=desabilitadas,
fusao=fusao,
atitude_filtrada=atitude_filtrada,
divergencia=divergencia,
risco=risco,
redundancia_disponivel=redundancia_disponivel,

View File

@ -441,6 +441,10 @@ class CameraManager:
return True
def reiniciar_deteccoes(self):
self._reset_runtime_state()
return True, "estado operacional limpo"
# ============================================================
# Warmup
# ============================================================

View File

@ -81,8 +81,12 @@ def main():
tipos = dados.get("params", {}).get("tipos", [])
get_camera_manager().salvar_frames(tipos, nome, pasta)
elif acao == WeedWorkerCommandType.ReiniciarDeteccoes:
if get_camera_manager().weed_detector is not None:
get_camera_manager().weed_detector._reiniciar_deteccoes()
manager = get_camera_manager()
ok, motivo = manager.reiniciar_deteccoes()
if ok:
mostrar_log(f"[weed] Detecções reiniciadas: {motivo}")
else:
mostrar_log(f"[weed] Não foi possível reiniciar detecções: {motivo}")
else:
mostrar_log(f"⚠️ Comando desconhecido: {acao.name}")
except Exception as e: