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
|
# (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.
|
||||||
|
|||||||
@ -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)
|
||||||
|
|
||||||
|
|||||||
@ -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}"
|
||||||
)
|
)
|
||||||
@ -144,13 +149,14 @@ class DebugPayloadRouter:
|
|||||||
# Propagate heading information (if available) into the hub so
|
# Propagate heading information (if available) into the hub so
|
||||||
# GUI builders that reconstruct lightweight Target objects
|
# GUI builders that reconstruct lightweight Target objects
|
||||||
# from the hub can also pick up the last known heading.
|
# from the hub can also pick up the last known heading.
|
||||||
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)
|
||||||
except Exception:
|
self._hub.set_real_heading(target.target_id, getattr(target, 'current_heading_deg', 0.0), raw_value=raw_val)
|
||||||
# Never allow heading propagation to break payload handling
|
except Exception:
|
||||||
self._logger.debug("Failed to propagate heading to hub", exc_info=True)
|
# Never allow heading propagation to break payload handling
|
||||||
|
self._logger.debug("Failed to propagate heading to hub", exc_info=True)
|
||||||
if self._update_queue:
|
if self._update_queue:
|
||||||
try:
|
try:
|
||||||
self._update_queue.put_nowait([])
|
self._update_queue.put_nowait([])
|
||||||
|
|||||||
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