diff --git a/scenarios.json b/scenarios.json index 5880b2e..f5d04d5 100644 --- a/scenarios.json +++ b/scenarios.json @@ -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 + } + ] } } \ No newline at end of file diff --git a/target_simulator/gui/main_view.py b/target_simulator/gui/main_view.py index 0d737f9..936ca36 100644 --- a/target_simulator/gui/main_view.py +++ b/target_simulator/gui/main_view.py @@ -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(): diff --git a/target_simulator/gui/payload_router.py b/target_simulator/gui/payload_router.py index 641be8c..d992cd8 100644 --- a/target_simulator/gui/payload_router.py +++ b/target_simulator/gui/payload_router.py @@ -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"): diff --git a/target_simulator/simulation/simulation_controller.py b/target_simulator/simulation/simulation_controller.py index c92930e..05eb7c3 100644 --- a/target_simulator/simulation/simulation_controller.py +++ b/target_simulator/simulation/simulation_controller.py @@ -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) \ No newline at end of file diff --git a/target_simulator/utils/latency_monitor.py b/target_simulator/utils/latency_monitor.py new file mode 100644 index 0000000..b504da0 --- /dev/null +++ b/target_simulator/utils/latency_monitor.py @@ -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 \ No newline at end of file diff --git a/todo.md b/todo.md index 68df41b..fc0e02f 100644 --- a/todo.md +++ b/todo.md @@ -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 \ No newline at end of file +- [X] sistemare la lentenza intrinseca della interfaccia su pc poco performanti +- [ ] evitare compensazione posizione dei target reali \ No newline at end of file