da sistemare i versi delle punte dei target a +-90 gradi

This commit is contained in:
VALLONGOL 2025-10-24 13:26:28 +02:00
parent d820faad9c
commit 0bc190a257
4 changed files with 140 additions and 8 deletions

View File

@ -37,6 +37,8 @@ class SimulationStateHub:
# (e.g. RIS payloads) without modifying the canonical stored position # (e.g. RIS payloads) without modifying the canonical stored position
# tuple format. # tuple format.
self._latest_real_heading = {} self._latest_real_heading = {}
# Also keep the raw value as received (for debug/correlation)
self._latest_raw_heading = {}
def add_simulated_state(self, target_id: int, timestamp: float, state: Tuple[float, ...]): def add_simulated_state(self, target_id: int, timestamp: float, state: Tuple[float, ...]):
""" """
@ -100,7 +102,7 @@ class SimulationStateHub:
# Never allow diagnostic logging to break hub behavior # Never allow diagnostic logging to break hub behavior
pass pass
def set_real_heading(self, target_id: int, heading_deg: float): def set_real_heading(self, target_id: int, heading_deg: float, raw_value: float = None):
""" """
Store the latest real heading (in degrees) for a specific target id. Store the latest real heading (in degrees) for a specific target id.
This keeps the hub backwards-compatible (position tuples unchanged) This keeps the hub backwards-compatible (position tuples unchanged)
@ -115,6 +117,12 @@ class SimulationStateHub:
tid = int(target_id) tid = int(target_id)
hdg = float(heading_deg) % 360 hdg = float(heading_deg) % 360
self._latest_real_heading[tid] = hdg self._latest_real_heading[tid] = hdg
if raw_value is not None:
try:
self._latest_raw_heading[tid] = float(raw_value)
except Exception:
# ignore invalid raw value
pass
except Exception: except Exception:
# On error, do nothing (silently ignore invalid heading) # On error, do nothing (silently ignore invalid heading)
pass pass
@ -126,6 +134,13 @@ class SimulationStateHub:
with self._lock: with self._lock:
return self._latest_real_heading.get(int(target_id)) return self._latest_real_heading.get(int(target_id))
def get_raw_heading(self, target_id: int) -> Optional[float]:
"""
Retrieve the last stored raw heading value for a target, or None if not set.
"""
with self._lock:
return self._latest_raw_heading.get(int(target_id))
def get_target_history(self, target_id: int) -> Optional[Dict[str, List[TargetState]]]: def get_target_history(self, target_id: int) -> Optional[Dict[str, List[TargetState]]]:
""" """
Retrieves a copy of the historical data for a specific target. Retrieves a copy of the historical data for a specific target.

View File

@ -9,6 +9,7 @@ from tkinter import ttk, scrolledtext, messagebox
from queue import Queue, Empty from queue import Queue, Empty
from typing import Optional, Dict, Any, List from typing import Optional, Dict, Any, List
import time import time
import math
# Use absolute imports for robustness and clarity # Use absolute imports for robustness and clarity
from target_simulator.gui.ppi_display import PPIDisplay from target_simulator.gui.ppi_display import PPIDisplay
@ -1240,6 +1241,48 @@ class MainView(tk.Tk):
except Exception: except Exception:
pass pass
# Correlation log: show raw->hub->used to help debugging pipeline
try:
if self.simulation_hub and hasattr(self.simulation_hub, 'get_raw_heading'):
raw_h = self.simulation_hub.get_raw_heading(tid)
else:
raw_h = None
# Compute theta0/theta1 using same conventions as PPIDisplay
try:
az_deg = float(real_target.current_azimuth_deg)
r_nm = float(real_target.current_range_nm)
az_rad = math.radians(az_deg)
x_start = r_nm * math.sin(az_rad)
y_start = r_nm * math.cos(az_rad)
vector_len = (
self.ppi_widget.range_var.get() / 20.0
if hasattr(self, 'ppi_widget') and hasattr(self.ppi_widget, 'range_var')
else 1.0
)
hdg_used = float(real_target.current_heading_deg)
hdg_rad = math.radians(hdg_used)
dx = vector_len * math.sin(hdg_rad)
dy = vector_len * math.cos(hdg_rad)
x_end = x_start + dx
y_end = y_start + dy
theta0_deg = -math.degrees(math.atan2(x_start, y_start))
theta1_deg = -math.degrees(math.atan2(x_end, y_end))
except Exception:
theta0_deg = None
theta1_deg = None
self.logger.debug(
"Heading pipeline: TID %s raw=%s hub=%s used=%s theta0=%.3f theta1=%.3f",
tid,
raw_h,
getattr(self.simulation_hub, 'get_real_heading')(tid) if self.simulation_hub else None,
real_target.current_heading_deg,
theta0_deg if theta0_deg is not None else float('nan'),
theta1_deg if theta1_deg is not None else float('nan'),
)
except Exception:
pass
real_target.active = True real_target.active = True
real_targets_for_ppi.append(real_target) real_targets_for_ppi.append(real_target)

View File

@ -115,6 +115,11 @@ class DebugPayloadRouter:
hdg_deg = raw_h hdg_deg = raw_h
unit = 'deg' unit = 'deg'
target.current_heading_deg = hdg_deg % 360 target.current_heading_deg = hdg_deg % 360
# Store the raw value on the Target for later correlation
try:
setattr(target, '_raw_heading', raw_h)
except Exception:
pass
self._logger.debug( self._logger.debug(
f"Parsed RIS heading for target {i}: raw={raw_h} assumed={unit} hdg_deg={target.current_heading_deg:.6f}" f"Parsed RIS heading for target {i}: raw={raw_h} assumed={unit} hdg_deg={target.current_heading_deg:.6f}"
) )
@ -147,7 +152,8 @@ class DebugPayloadRouter:
try: try:
for target in real_targets: for target in real_targets:
if hasattr(self._hub, 'set_real_heading'): if hasattr(self._hub, 'set_real_heading'):
self._hub.set_real_heading(target.target_id, getattr(target, 'current_heading_deg', 0.0)) raw_val = getattr(target, '_raw_heading', None)
self._hub.set_real_heading(target.target_id, getattr(target, 'current_heading_deg', 0.0), raw_value=raw_val)
except Exception: except Exception:
# Never allow heading propagation to break payload handling # Never allow heading propagation to break payload handling
self._logger.debug("Failed to propagate heading to hub", exc_info=True) self._logger.debug("Failed to propagate heading to hub", exc_info=True)

68
tools/test_ris_inject.py Normal file
View File

@ -0,0 +1,68 @@
# Minimal test harness to inject a fake RIS payload into DebugPayloadRouter
import logging
logging.basicConfig(level=logging.DEBUG, format='%(asctime)s [%(levelname)s] %(name)s: %(message)s')
from target_simulator.gui.payload_router import DebugPayloadRouter
from target_simulator.analysis.simulation_state_hub import SimulationStateHub
import target_simulator.gui.payload_router as pr_mod
# Build fake ris target objects with expected attributes
class FakeRisTarget:
def __init__(self, flags, x, y, z, heading):
self.flags = flags
self.x = x
self.y = y
self.z = z
self.heading = heading
class FakeScenario:
def __init__(self, timetag=123456789):
self.timetag = timetag
class FakeParsed:
def __init__(self, tgt_list):
# parsed.tgt.tgt is iterated in payload_router
self.tgt = type('T', (), {'tgt': tgt_list})()
self.scenario = FakeScenario()
@staticmethod
def from_buffer_copy(payload):
# This will be monkeypatched per-call by assigning a closure; but keep for safety
raise RuntimeError('Should be monkeypatched')
# Prepare fake targets using your example headings (radians)
rads = [0.0, 1.5707964897155762, -3.141592502593994, -1.5707964897155762]
# Example positions (x,y,z) in meters approx; router expects M_TO_FT conversion inside
fake_targets = []
for i, h in enumerate(rads):
# Set flags non-zero
# Use x,y values that roughly match earlier logs (meters)
x_m = 9600 + i * 6000
y_m = 37000 - i * 5000
z_m = 3048.0 # 10000 ft ~= 3048 m
fake_targets.append(FakeRisTarget(flags=1, x=x_m, y=y_m, z=z_m, heading=h))
# Monkeypatch the SfpRisStatusPayload.from_buffer_copy in payload_router module
original_parser = pr_mod.SfpRisStatusPayload
def fake_from_buffer_copy(payload):
return FakeParsed(fake_targets)
pr_mod.SfpRisStatusPayload = type('P', (), {'from_buffer_copy': staticmethod(fake_from_buffer_copy)})
# Run the router handling
hub = SimulationStateHub()
router = DebugPayloadRouter(simulation_hub=hub, update_queue=None)
print('Invoking _handle_ris_status with fake payload...')
router._handle_ris_status(b'FAKE')
# Show hub contents and stored headings
for tid in range(len(rads)):
hist = hub.get_target_history(tid)
hdg = hub.get_real_heading(tid)
print(f'TID {tid} -> history entries: sim={len(hist["simulated"]) if hist else 0} real={len(hist["real"]) if hist else 0}, heading_in_hub={hdg}')
# Restore original parser symbol to avoid side effects
pr_mod.SfpRisStatusPayload = original_parser
print('Done')