VALLONGOL/S1005403_RisCC
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

aggiunto nuovo calcolo della latenza, tolto compensazione ownship su dati target real

Browse Source
This commit is contained in:
VALLONGOL 2025-11-21 09:58:21 +01:00
parent 7e982a6e06
commit 87488897e6
6 changed files with 369 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

138
scenarios.json
View File

@ -452,5 +452,143 @@
"use_spline": false
}
]
},
"Scenario_60gradi": {
"name": "Scenario_60gradi",
"targets": [
{
"target_id": 0,
"active": true,
"traceable": true,
"trajectory": [
{
"maneuver_type": "Fly to Point",
"duration_s": 1.0,
"target_range_nm": 5.0,
"target_azimuth_deg": 60.0,
"target_altitude_ft": 10000.0,
"target_velocity_fps": 506.343,
"target_heading_deg": 60.0,
"longitudinal_acceleration_g": 0.0,
"lateral_acceleration_g": 0.0,
"vertical_acceleration_g": 0.0,
"turn_direction": "Right"
},
{
"maneuver_type": "Fly to Point",
"duration_s": 120.0,
"target_range_nm": 100.0,
"target_azimuth_deg": 60.0,
"target_altitude_ft": 10000.0,
"longitudinal_acceleration_g": 0.0,
"lateral_acceleration_g": 0.0,
"vertical_acceleration_g": 0.0,
"turn_direction": "Right"
}
],
"use_spline": false
},
{
"target_id": 1,
"active": true,
"traceable": true,
"trajectory": [
{
"maneuver_type": "Fly to Point",
"duration_s": 1.0,
"target_range_nm": 5.0,
"target_azimuth_deg": -60.0,
"target_altitude_ft": 10000.0,
"target_velocity_fps": 506.343,
"target_heading_deg": -60.0,
"longitudinal_acceleration_g": 0.0,
"lateral_acceleration_g": 0.0,
"vertical_acceleration_g": 0.0,
"turn_direction": "Right"
},
{
"maneuver_type": "Fly to Point",
"duration_s": 120.0,
"target_range_nm": 100.0,
"target_azimuth_deg": -60.0,
"target_altitude_ft": 10000.0,
"longitudinal_acceleration_g": 0.0,
"lateral_acceleration_g": 0.0,
"vertical_acceleration_g": 0.0,
"turn_direction": "Right"
}
],
"use_spline": false
}
]
},
"Scenario_30gradi": {
"name": "Scenario_30gradi",
"targets": [
{
"target_id": 0,
"active": true,
"traceable": true,
"trajectory": [
{
"maneuver_type": "Fly to Point",
"duration_s": 1.0,
"target_range_nm": 5.0,
"target_azimuth_deg": 30.0,
"target_altitude_ft": 10000.0,
"target_velocity_fps": 506.343,
"target_heading_deg": 30.0,
"longitudinal_acceleration_g": 0.0,
"lateral_acceleration_g": 0.0,
"vertical_acceleration_g": 0.0,
"turn_direction": "Right"
},
{
"maneuver_type": "Fly to Point",
"duration_s": 120.0,
"target_range_nm": 100.0,
"target_azimuth_deg": 30.0,
"target_altitude_ft": 10000.0,
"longitudinal_acceleration_g": 0.0,
"lateral_acceleration_g": 0.0,
"vertical_acceleration_g": 0.0,
"turn_direction": "Right"
}
],
"use_spline": false
},
{
"target_id": 1,
"active": true,
"traceable": true,
"trajectory": [
{
"maneuver_type": "Fly to Point",
"duration_s": 1.0,
"target_range_nm": 5.0,
"target_azimuth_deg": -30.0,
"target_altitude_ft": 10000.0,
"target_velocity_fps": 506.343,
"target_heading_deg": -30.0,
"longitudinal_acceleration_g": 0.0,
"lateral_acceleration_g": 0.0,
"vertical_acceleration_g": 0.0,
"turn_direction": "Right"
},
{
"maneuver_type": "Fly to Point",
"duration_s": 120.0,
"target_range_nm": 100.0,
"target_azimuth_deg": -30.0,
"target_altitude_ft": 10000.0,
"longitudinal_acceleration_g": 0.0,
"lateral_acceleration_g": 0.0,
"vertical_acceleration_g": 0.0,
"turn_direction": "Right"
}
],
"use_spline": false
}
]
}
}

23
target_simulator/gui/main_view.py
View File

@ -54,6 +54,7 @@ from target_simulator.core.models import Scenario, Target
from target_simulator.utils.logger import get_logger, shutdown_logging_system
from target_simulator.utils.config_manager import ConfigManager
from target_simulator.utils.latency_monitor import LatencyMonitor
from target_simulator.gui.sfp_debug_window import SfpDebugWindow
from target_simulator.gui.logger_panel import LoggerPanel
from target_simulator.core.sfp_communicator import SFPCommunicator
@ -138,6 +139,7 @@ class MainView(tk.Tk):
self.current_scenario_name: Optional[str] = None
self.sfp_debug_window: Optional[SfpDebugWindow] = None
self.analysis_window: Optional[AnalysisWindow] = None
self.latency_monitor: Optional[LatencyMonitor] = None
# --- Simulation Engine ---
self.simulation_engine: Optional[SimulationEngine] = None
@ -164,6 +166,15 @@ class MainView(tk.Tk):
# --- Post-UI Initialization ---
self._initialize_communicators()
if isinstance(self.target_communicator, SFPCommunicator):
router = self.target_communicator.router()
if router:
self.latency_monitor = LatencyMonitor(
self, router, self.target_communicator
)
self.latency_monitor.start()
self._load_scenarios_into_ui()
# Determine initial scenario to load. Prefer last_selected_scenario
# from settings if it exists and is valid; otherwise use the
@ -751,6 +762,9 @@ class MainView(tk.Tk):
# Stop the simulation if it's running
if self.is_simulation_running.get():
self._on_stop_simulation()
if self.latency_monitor:
self.latency_monitor.stop()
# Save window geometry and last scenario
settings_to_save = {
@ -1011,10 +1025,9 @@ class MainView(tk.Tk):
"""Periodically updates the latency display in the status bar."""
try:
latency_s = 0.0
if self.target_communicator and hasattr(self.target_communicator, "router"):
router = self.target_communicator.router()
if router and hasattr(router, "get_estimated_latency_s"):
latency_s = router.get_estimated_latency_s()
# --- MODIFICA PRINCIPALE: Usa il LatencyMonitor ---
if self.latency_monitor:
latency_s = self.latency_monitor.get_stable_latency_s()
# Update the status bar display
if hasattr(self, "latency_status_var") and self.latency_status_var:
@ -1022,7 +1035,7 @@ class MainView(tk.Tk):
latency_ms = latency_s * 1000
self.latency_status_var.set(f"Latency: {latency_ms:.1f} ms")
else:
self.latency_status_var.set("") # Clear if no latency
self.latency_status_var.set("Latency: --")
# Update the simulation engine's prediction horizon if it's running
if self.simulation_engine and self.simulation_engine.is_running():

65
target_simulator/gui/payload_router.py
View File

@ -25,6 +25,14 @@ from target_simulator.analysis.simulation_state_hub import SimulationStateHub
from target_simulator.core.models import Target
from target_simulator.utils.clock_synchronizer import ClockSynchronizer
try:
from pyproj import Geod
_GEOD = Geod(ellps="WGS84")
_HAS_PYPROJ = True
except ImportError:
_GEOD = None
_HAS_PYPROJ = False
logger = logging.getLogger(__name__)
PayloadHandler = Callable[[bytearray], None]
@ -250,28 +258,57 @@ class DebugPayloadRouter:
if self._hub:
try:
sc = parsed_payload.scenario
delta_t = 0.0
if self._last_ownship_update_time is not None:
delta_t = reception_timestamp - self._last_ownship_update_time
self._last_ownship_update_time = reception_timestamp
old_state = self._hub.get_ownship_state()
old_pos_xy = old_state.get("position_xy_ft", (0.0, 0.0))
# Server sends vx, vy with its convention (x=N, y=W). This matches our internal convention.
ownship_vx_fps = float(sc.vx) * M_TO_FT # North velocity
ownship_vy_fps = float(sc.vy) * M_TO_FT # West velocity
new_pos_x_ft = old_pos_xy[0] + ownship_vx_fps * delta_t
new_pos_y_ft = old_pos_xy[1] + ownship_vy_fps * delta_t
# Prendi la posizione di partenza della simulazione
sim_origin = self._hub.get_simulation_origin()
origin_lat = sim_origin.get("latitude")
origin_lon = sim_origin.get("longitude")
origin_pos_xy_ft = sim_origin.get("position_xy_ft", (0.0, 0.0))
# Posizione attuale dell'ownship inviata dal server
current_lat = float(sc.latitude)
current_lon = float(sc.longitude)
new_pos_x_ft, new_pos_y_ft = origin_pos_xy_ft
# Calcola la posizione cartesiana assoluta basata sulla Lat/Lon
# solo se abbiamo un'origine valida. Altrimenti, la posizione rimane quella dell'origine.
if origin_lat is not None and origin_lon is not None:
delta_north_m = 0.0
delta_east_m = 0.0
if _HAS_PYPROJ and _GEOD:
# Metodo accurato con pyproj
fwd_az, back_az, dist = _GEOD.inv(origin_lon, origin_lat, current_lon, current_lat)
delta_north_m = dist * math.cos(math.radians(fwd_az))
delta_east_m = dist * math.sin(math.radians(fwd_az))
else:
# Metodo di approssimazione equirettangolare (fallback)
R = 6378137.0
dlat = math.radians(current_lat - origin_lat)
dlon = math.radians(current_lon - origin_lon)
delta_north_m = dlat * R
delta_east_m = dlon * R * math.cos(math.radians(origin_lat))
# Converti lo spostamento in metri in coordinate del nostro sistema (X=Nord, Y=Ovest)
# e aggiungilo alla posizione cartesiana dell'origine.
new_pos_x_ft = origin_pos_xy_ft[0] + (delta_north_m * M_TO_FT)
new_pos_y_ft = origin_pos_xy_ft[1] - (delta_east_m * M_TO_FT) # Negativo perché Y è Ovest
ownship_heading_deg = math.degrees(float(sc.true_heading)) % 360
ownship_state = {
"timestamp": reception_timestamp,
"position_xy_ft": (new_pos_x_ft, new_pos_y_ft),
"altitude_ft": float(sc.baro_altitude) * M_TO_FT,
"velocity_xy_fps": (ownship_vx_fps, ownship_vy_fps),
"velocity_xy_fps": (float(sc.vx) * M_TO_FT, float(sc.vy) * M_TO_FT),
"heading_deg": ownship_heading_deg,
"latitude": float(sc.latitude),
"longitude": float(sc.longitude),
"latitude": current_lat,
"longitude": current_lon,
}
self._hub.set_ownship_state(ownship_state)
with self._lock:
archive = self.active_archive
if archive and hasattr(archive, "add_ownship_state"):

88
target_simulator/simulation/simulation_controller.py
View File

@ -201,25 +201,17 @@ class SimulationController:
def _stop_or_finish_simulation(self, main_view, was_stopped_by_user: bool):
"""Unified logic for handling simulation end, either by user or naturally."""
if self.current_archive:
# --- NUOVA AGGIUNTA INIZIO ---
# Retrieve estimated latency before saving the archive
estimated_latency_s = 0.0
extra_metadata = {}
try:
target_comm = getattr(
self.communicator_manager, "target_communicator", None
)
if target_comm and hasattr(target_comm, "router"):
router = target_comm.router()
if router and hasattr(router, "get_estimated_latency_s"):
estimated_latency_s = router.get_estimated_latency_s()
if estimated_latency_s > 0:
extra_metadata["estimated_latency_ms"] = round(
estimated_latency_s * 1000, 2
)
# Retrieve prediction offset from config
# Recupera la latenza stabile dal nuovo LatencyMonitor (basato su SYNC)
if main_view.latency_monitor:
estimated_latency_s = main_view.latency_monitor.get_stable_latency_s()
if estimated_latency_s > 0:
extra_metadata["estimated_latency_ms"] = round(
estimated_latency_s * 1000, 2
)
# Recupera l'offset di predizione manuale
conn_settings = self.config_manager.get_connection_settings()
target_sfp_cfg = conn_settings.get("target", {}).get("sfp", {})
offset_ms = target_sfp_cfg.get("prediction_offset_ms", 0.0)
@ -230,69 +222,37 @@ class SimulationController:
self.logger.warning(
f"Could not retrieve estimated latency for archive: {e}"
)
# Also attempt to include latency statistics and recent samples
# --- RIMOSSO: Salvataggio delle vecchie statistiche di latenza da ClockSynchronizer ---
# Questa parte è stata rimossa per evitare di salvare dati di latenza ambigui o ridondanti.
# Recupera i dati di performance profiling (se abilitati)
try:
target_comm = getattr(self.communicator_manager, "target_communicator", None)
if target_comm and hasattr(target_comm, "router"):
router = target_comm.router()
if router and hasattr(router, "get_latency_stats"):
stats = router.get_latency_stats(sample_limit=500)
if stats and stats.get("count", 0) > 0:
extra_metadata["latency_summary"] = stats
if router and hasattr(router, "get_latency_samples"):
samples = router.get_latency_samples(
limit=None
) # Get all available samples
if samples:
# Convert to [timestamp, latency_ms] format
samples_with_time = [
[round(ts, 3), round(lat * 1000.0, 3)]
for ts, lat in samples
]
extra_metadata["latency_samples"] = samples_with_time
# Save performance profiling data if available
if router and hasattr(router, "get_performance_samples"):
perf_samples = router.get_performance_samples()
self.logger.debug(
f"Retrieved {len(perf_samples) if perf_samples else 0} performance samples from router"
)
if perf_samples:
extra_metadata["performance_samples"] = perf_samples
self.logger.info(
f"Saved {len(perf_samples)} performance samples to archive"
)
else:
self.logger.warning(
"No performance samples available to save"
)
except Exception as e:
self.logger.warning(
f"Could not collect latency samples for archive: {e}"
f"Could not collect performance samples for archive: {e}"
)
# Add simulation parameters (client update interval / send rate)
try:
update_interval_s = None
if hasattr(main_view, "update_time"):
try:
# update_time is a Tk variable (DoubleVar) in the UI
update_interval_s = float(main_view.update_time.get())
except Exception:
update_interval_s = None
if update_interval_s is not None:
extra_metadata["client_update_interval_s"] = round(
update_interval_s, 6
)
# Aggiunge i parametri di simulazione
try:
if hasattr(main_view, "update_time"):
update_interval_s = float(main_view.update_time.get())
extra_metadata["client_update_interval_s"] = round(update_interval_s, 6)
if update_interval_s > 0:
extra_metadata["client_update_rate_hz"] = round(
1.0 / update_interval_s, 3
)
extra_metadata["client_update_rate_hz"] = round(1.0 / update_interval_s, 3)
except Exception as e:
self.logger.warning(
f"Could not read client update interval for archive: {e}"
)
# --- NUOVA AGGIUNTA FINE ---
# Salva l'archivio con i metadati puliti e corretti
self.current_archive.save(extra_metadata=extra_metadata)
self.current_archive = main_view.current_archive = None
@ -330,4 +290,4 @@ class SimulationController:
if not main_view.is_simulation_running.get():
return
self.logger.info("Simulation engine finished execution.")
self._stop_or_finish_simulation(main_view, was_stopped_by_user=False)
self._stop_or_finish_simulation(main_view, was_stopped_by_user=False)

134
target_simulator/utils/latency_monitor.py Normal file
View File

@ -0,0 +1,134 @@
# target_simulator/utils/latency_monitor.py
"""
Provides a LatencyMonitor for actively measuring network latency using SYNC packets.
"""
import tkinter as tk
import time
import random
import statistics
from collections import deque
from typing import Optional
from target_simulator.core.sfp_communicator import SFPCommunicator
from target_simulator.gui.payload_router import DebugPayloadRouter
class LatencyMonitor:
"""
Manages periodic SYNC requests to actively measure and average network latency.
This utility runs on the Tkinter event loop, sending a SYNC packet at a
configured interval and processing replies to maintain a moving average of
the one-way latency (RTT/2).
"""
def __init__(
self,
master: tk.Tk,
router: DebugPayloadRouter,
communicator: SFPCommunicator,
interval_ms: int = 1000,
history_size: int = 20,
):
"""
Initializes the LatencyMonitor.
Args:
master: The root Tkinter window, used for scheduling 'after' events.
router: The DebugPayloadRouter to get SYNC replies from.
communicator: The SFPCommunicator to send SYNC requests with.
interval_ms: The interval in milliseconds between SYNC requests.
history_size: The number of recent latency samples to average.
"""
self.master = master
self.router = router
self.communicator = communicator
self.interval_ms = interval_ms
self._pending_requests = {} # {cookie: send_timestamp}
self._latency_history = deque(maxlen=history_size)
self._is_running = False
self._after_id_send = None
self._after_id_process = None
def start(self):
"""Starts the periodic sending and processing loops."""
if self._is_running:
return
self._is_running = True
self._schedule_send()
self._schedule_process()
def stop(self):
"""Stops the periodic loops."""
if not self._is_running:
return
self._is_running = False
if self._after_id_send:
self.master.after_cancel(self._after_id_send)
self._after_id_send = None
if self._after_id_process:
self.master.after_cancel(self._after_id_process)
self._after_id_process = None
def _schedule_send(self):
"""Schedules the next SYNC packet send."""
if not self._is_running:
return
self._send_sync_request()
self._after_id_send = self.master.after(self.interval_ms, self._schedule_send)
def _schedule_process(self):
"""Schedules the next reply processing check."""
if not self._is_running:
return
self._process_replies()
# Process replies more frequently than sending requests
self._after_id_process = self.master.after(100, self._schedule_process)
def _send_sync_request(self):
"""Sends a single SYNC request if the communicator is connected."""
if not self.communicator or not self.communicator.is_open:
return
cookie = random.randint(0, 2**32 - 1)
send_time = time.monotonic()
if self.communicator.send_sync_request(cookie):
self._pending_requests[cookie] = send_time
def _process_replies(self):
"""Processes all available SYNC replies from the router's queue."""
while True:
result = self.router.get_sync_result()
if not result:
break # No more results in the queue
cookie = result.get("cookie")
reception_time = result.get("reception_timestamp")
if cookie in self._pending_requests:
send_time = self._pending_requests.pop(cookie)
rtt_s = reception_time - send_time
latency_s = rtt_s / 2.0
if latency_s >= 0:
self._latency_history.append(latency_s)
def get_stable_latency_s(self) -> float:
"""
Returns the moving average of the one-way latency in seconds.
Returns 0.0 if not enough data is available.
"""
if not self._latency_history:
return 0.0
try:
return statistics.mean(self._latency_history)
except statistics.StatisticsError:
return 0.0

7
todo.md
View File

@ -16,7 +16,7 @@
- [x] creare una procedura di allineamento tra server e client usando il comando di ping da implementare anche sul server
- [x] funzione di sincronizzazione: è stato aggiunto al server la possibilità di gestire dei messaggi che sono di tipo SY (tag) che sono fatti per gestire il sincronismo tra client e server. In questa nuova tipologia di messaggi io invio un mio timetag che poi il server mi restituirà subito appena lo riceve, facendo così sappiamo in quanto tempo il messaggio che spedisco è arrivato al server, viene letto, e viene risposto il mio numero con anche il timetag del server. Facendo così misurando i delta posso scroprire esattamente il tempo che intercorre tra inviare un messaggio al server e ricevere una risposta. Per come è fatto il server il tempo di applicazione dei nuovi valori per i target sarà al massimo di 1 batch, che può essere variabile, ma a quel punto lo potremmo calibrare in altro modo. Con l'analisi sui sync possiamo sapere come allineare gli orologi.
-[ ] cercare di capire come inserire il comando di sync per allineare gli orologi
- [X] cercare di capire come inserire il comando di sync per allineare gli orologi
- [x] Aggiungere un tasto per duplicare uno scenario da uno già presente e dargli un nome diverso
- [ ] aggiungere una funzione automatica durante il salvataggio dello scenario che cancelli quelli più vecchi di 10 salvataggi fa, per evitare che aumentino in numero senza controllo
@ -41,6 +41,7 @@
- [X] IMPORTANTE: verificare la rotazione dei target quando durante la simulazione ruota l'aereo, in questo caso se ruota l'aereo ed i target sono parttiti con un certo angolo rispetto allo 0, poi la traiettoria dei target deve essere aggiornata rispetto al momento iniziale e non calcolata ad ogni step di rotazione. Al momento dello start, devo memorizzare l'angolo di rotazione dell'aereo e quindi quello è l'angolo con cui dovranno essere aggiornate sempre le traiettorie dei target e non quella corrente dell'aereo che potrà girare dove vuole ma a quel punto le tracce sono partite e quindi seguiranno la loro strada.
- [x] salvare i dati di perfomance della simulazione in altro file per evitare di appesantire file di salvataggio simulazione
- [x] caricare solo i dati dei file ce ci interessano quando passo all'analisi
- [ ] verificare nel caso di simulazione su server come mai le talenze ballano così tanto.
- [x] verificare nel caso di simulazione su server come mai le talenze ballano così tanto.
- [x] al posto di mettere il timestamp, nei grafici mettiamo il tempo relativo al punto di partenza della simulazione visualizzata, così da avere un riferimento effettico tra il tempo del punto in oggetto e l'inizio della simulazione (poter dire "dopo 10 secondi succede questo", con i timetag la cosa non è immediata)
- [X] sistemare la lentenza intrinseca della interfaccia su pc poco performanti
- [X] sistemare la lentenza intrinseca della interfaccia su pc poco performanti
- [ ] evitare compensazione posizione dei target reali