da sistemare i versi delle punte dei target a +-90 gradi
This commit is contained in:
parent
d820faad9c
commit
0bc190a257
@ -37,6 +37,8 @@ class SimulationStateHub:
|
||||
# (e.g. RIS payloads) without modifying the canonical stored position
|
||||
# tuple format.
|
||||
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, ...]):
|
||||
"""
|
||||
@ -100,7 +102,7 @@ class SimulationStateHub:
|
||||
# Never allow diagnostic logging to break hub behavior
|
||||
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.
|
||||
This keeps the hub backwards-compatible (position tuples unchanged)
|
||||
@ -115,6 +117,12 @@ class SimulationStateHub:
|
||||
tid = int(target_id)
|
||||
hdg = float(heading_deg) % 360
|
||||
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:
|
||||
# On error, do nothing (silently ignore invalid heading)
|
||||
pass
|
||||
@ -126,6 +134,13 @@ class SimulationStateHub:
|
||||
with self._lock:
|
||||
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]]]:
|
||||
"""
|
||||
Retrieves a copy of the historical data for a specific target.
|
||||
|
||||
@ -9,6 +9,7 @@ from tkinter import ttk, scrolledtext, messagebox
|
||||
from queue import Queue, Empty
|
||||
from typing import Optional, Dict, Any, List
|
||||
import time
|
||||
import math
|
||||
|
||||
# Use absolute imports for robustness and clarity
|
||||
from target_simulator.gui.ppi_display import PPIDisplay
|
||||
@ -1240,6 +1241,48 @@ class MainView(tk.Tk):
|
||||
except Exception:
|
||||
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_targets_for_ppi.append(real_target)
|
||||
|
||||
|
||||
@ -115,6 +115,11 @@ class DebugPayloadRouter:
|
||||
hdg_deg = raw_h
|
||||
unit = 'deg'
|
||||
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(
|
||||
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:
|
||||
for target in real_targets:
|
||||
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:
|
||||
# Never allow heading propagation to break payload handling
|
||||
self._logger.debug("Failed to propagate heading to hub", exc_info=True)
|
||||
|
||||
68
tools/test_ris_inject.py
Normal file
68
tools/test_ris_inject.py
Normal 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')
|
||||
Loading…
Reference in New Issue
Block a user