434 lines
20 KiB
Python
434 lines
20 KiB
Python
# target_simulator/gui/ppi_display.py
|
|
|
|
"""
|
|
A reusable Tkinter widget that displays a Plan Position Indicator (PPI)
|
|
using Matplotlib, capable of showing both live targets and trajectory previews.
|
|
"""
|
|
|
|
import tkinter as tk
|
|
from tkinter import ttk
|
|
import math
|
|
import numpy as np
|
|
import copy
|
|
from matplotlib.figure import Figure
|
|
from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
|
|
|
|
# Use absolute imports
|
|
from target_simulator.core.models import Target, Waypoint, ManeuverType
|
|
from typing import List, Optional
|
|
|
|
class PPIDisplay(ttk.Frame):
|
|
def clear_previews(self):
|
|
"""Clears all preview artists from the plot."""
|
|
for artist in self.preview_artists:
|
|
artist.set_data([], [])
|
|
self.canvas.draw()
|
|
def _on_motion(self, event):
|
|
# Stub: implementazione futura per tooltip/mouseover
|
|
pass
|
|
"""A custom, reusable widget for the PPI radar display."""
|
|
|
|
def __init__(self, master, max_range_nm: int = 100, scan_limit_deg: int = 60):
|
|
super().__init__(master)
|
|
self.max_range = max_range_nm
|
|
self.scan_limit_deg = scan_limit_deg
|
|
|
|
self.target_artists = []
|
|
self.active_targets: List[Target] = []
|
|
self._target_dots = []
|
|
self.preview_artists = []
|
|
|
|
self._create_controls()
|
|
self._create_plot()
|
|
|
|
def _create_controls(self):
|
|
"""Creates the control widgets for the PPI display."""
|
|
self.controls_frame = ttk.Frame(self)
|
|
self.controls_frame.pack(side=tk.TOP, fill=tk.X, padx=5, pady=5)
|
|
|
|
ttk.Label(self.controls_frame, text="Range (NM):").pack(side=tk.LEFT)
|
|
|
|
# Create a list of valid range steps up to the theoretical max_range
|
|
all_steps = [10, 20, 40, 80, 100, 160]
|
|
valid_steps = sorted([s for s in all_steps if s <= self.max_range])
|
|
if not valid_steps:
|
|
valid_steps = [self.max_range]
|
|
|
|
# Ensure the initial max range is in the list if not a standard step
|
|
if self.max_range not in valid_steps:
|
|
valid_steps.append(self.max_range)
|
|
valid_steps.sort()
|
|
|
|
# The initial value for the combobox is the max_range passed to the constructor
|
|
self.range_var = tk.IntVar(value=self.max_range)
|
|
|
|
self.range_selector = ttk.Combobox(
|
|
self.controls_frame, textvariable=self.range_var,
|
|
values=valid_steps, state="readonly", width=5
|
|
)
|
|
self.range_selector.pack(side=tk.LEFT, padx=5)
|
|
self.range_selector.bind("<<ComboboxSelected>>", self._on_range_selected)
|
|
|
|
def _create_plot(self):
|
|
"""Initializes the Matplotlib polar plot."""
|
|
fig = Figure(figsize=(5, 5), dpi=100, facecolor='#3E3E3E')
|
|
fig.subplots_adjust(left=0.05, right=0.95, top=0.85, bottom=0.05)
|
|
|
|
self.ax = fig.add_subplot(111, projection='polar', facecolor='#2E2E2E')
|
|
|
|
self.ax.set_theta_zero_location('N')
|
|
self.ax.set_theta_direction(-1)
|
|
self.ax.set_rlabel_position(90)
|
|
self.ax.set_ylim(0, self.range_var.get())
|
|
|
|
angles = np.arange(0, 360, 30)
|
|
labels = [f"{angle}°" if angle == 0 else f"+{angle}°" if angle < 180 else "±180°" if angle == 180 else f"-{360 - angle}°" for angle in angles]
|
|
self.ax.set_thetagrids(angles, labels)
|
|
|
|
self.ax.tick_params(axis='x', colors='white', labelsize=8)
|
|
self.ax.tick_params(axis='y', colors='white', labelsize=8)
|
|
self.ax.grid(color='white', linestyle='--', linewidth=0.5, alpha=0.5)
|
|
self.ax.spines['polar'].set_color('white')
|
|
self.ax.set_title("PPI Display", color='white', y=1.08)
|
|
|
|
# --- Artists for drawing ---
|
|
self._start_plot, = self.ax.plot([], [], 'go', markersize=8)
|
|
self._waypoints_plot, = self.ax.plot([], [], 'y+', markersize=10, mew=2, linestyle='None')
|
|
self._path_plot, = self.ax.plot([], [], 'g--', linewidth=1.5)
|
|
self._preview_artist, = self.ax.plot([], [], color="orange", linestyle="--", linewidth=2, alpha=0.7)
|
|
self._heading_artist, = self.ax.plot([], [], color='red', linewidth=1, alpha=0.8)
|
|
self.preview_artists = [self._start_plot, self._waypoints_plot, self._path_plot, self._preview_artist, self._heading_artist]
|
|
|
|
# --- NEW: Create artists for scan lines ---
|
|
limit_rad = np.deg2rad(self.scan_limit_deg)
|
|
self._scan_line_1, = self.ax.plot([limit_rad, limit_rad], [0, self.max_range], color='yellow', linestyle='--', linewidth=1)
|
|
self._scan_line_2, = self.ax.plot([-limit_rad, -limit_rad], [0, self.max_range], color='yellow', linestyle='--', linewidth=1)
|
|
|
|
self._tooltip_label = None
|
|
|
|
self.canvas = FigureCanvasTkAgg(fig, master=self)
|
|
self.canvas.draw()
|
|
self.canvas.get_tk_widget().pack(side=tk.TOP, fill=tk.BOTH, expand=True)
|
|
self.canvas.mpl_connect('motion_notify_event', self._on_motion)
|
|
|
|
# --- NEW: Initial draw of scan lines ---
|
|
self._update_scan_lines()
|
|
|
|
def _update_scan_lines(self):
|
|
"""Updates the length of the scan sector lines to match the current range."""
|
|
current_range_max = self.ax.get_ylim()[1]
|
|
self._scan_line_1.set_ydata([0, current_range_max])
|
|
self._scan_line_2.set_ydata([0, current_range_max])
|
|
|
|
def _on_range_selected(self, event=None):
|
|
"""Handles the selection of a new range."""
|
|
new_range = self.range_var.get()
|
|
self.ax.set_ylim(0, new_range)
|
|
|
|
# --- NEW: Update scan lines on zoom change ---
|
|
self._update_scan_lines()
|
|
|
|
self.update_targets(self.active_targets)
|
|
self.canvas.draw()
|
|
|
|
# ... il resto dei metodi rimane invariato ...
|
|
|
|
def update_targets(self, targets: List[Target]):
|
|
# (This method is unchanged)
|
|
self.active_targets = [t for t in targets if t.active]
|
|
for artist in self.target_artists: artist.remove()
|
|
self.target_artists.clear()
|
|
self._target_dots.clear()
|
|
vector_len = self.range_var.get() / 25
|
|
for target in self.active_targets:
|
|
r = target.current_range_nm
|
|
theta = np.deg2rad(target.current_azimuth_deg)
|
|
dot, = self.ax.plot(theta, r, 'o', markersize=5, color='red')
|
|
self.target_artists.append(dot)
|
|
self._target_dots.append((dot, target))
|
|
x1, y1 = r * np.sin(theta), r * np.cos(theta)
|
|
h_rad = np.deg2rad(target.current_heading_deg)
|
|
dx, dy = vector_len * np.sin(h_rad), vector_len * np.cos(h_rad)
|
|
x2, y2 = x1 + dx, y1 + dy
|
|
r2, th2 = np.sqrt(x2**2 + y2**2), np.arctan2(x2, y2)
|
|
line, = self.ax.plot([theta, th2], [r, r2], color='red', linewidth=1.2)
|
|
self.target_artists.append(line)
|
|
self.canvas.draw()
|
|
|
|
def draw_trajectory_preview(self, trajectory):
|
|
"""
|
|
Simulates and draws a trajectory preview.
|
|
Accepts either a list of Waypoint or a Target object.
|
|
"""
|
|
self.clear_previews()
|
|
self._waypoints_plot.set_data([], [])
|
|
self._start_plot.set_data([], [])
|
|
if hasattr(self, '_preview_artist'):
|
|
self._preview_artist.set_data([], [])
|
|
|
|
# --- Simula la traiettoria anche per manovre dinamiche ---
|
|
waypoints = trajectory if isinstance(trajectory, list) else getattr(trajectory, 'trajectory', [])
|
|
preview_points = []
|
|
# Stato iniziale
|
|
pos_x, pos_y, pos_z = 0.0, 0.0, 0.0
|
|
heading_deg = 0.0
|
|
velocity_fps = 0.0
|
|
altitude_ft = 0.0
|
|
# Usa il primo waypoint come stato iniziale se disponibile
|
|
if waypoints:
|
|
wp0 = waypoints[0]
|
|
if getattr(wp0, 'target_range_nm', None) is not None and getattr(wp0, 'target_azimuth_deg', None) is not None:
|
|
r0 = wp0.target_range_nm
|
|
th0 = math.radians(wp0.target_azimuth_deg)
|
|
pos_x = r0 * 6076.12 * math.sin(th0)
|
|
pos_y = r0 * 6076.12 * math.cos(th0)
|
|
pos_z = wp0.target_altitude_ft or 0.0
|
|
heading_deg = wp0.target_heading_deg or 0.0
|
|
velocity_fps = wp0.target_velocity_fps or 0.0
|
|
altitude_ft = pos_z
|
|
preview_points.append((pos_x, pos_y))
|
|
# Simula ogni waypoint
|
|
for wp in waypoints:
|
|
if wp.maneuver_type == ManeuverType.FLY_TO_POINT:
|
|
if wp.target_range_nm is not None and wp.target_azimuth_deg is not None:
|
|
r = wp.target_range_nm
|
|
th = math.radians(wp.target_azimuth_deg)
|
|
pos_x = r * 6076.12 * math.sin(th)
|
|
pos_y = r * 6076.12 * math.cos(th)
|
|
pos_z = wp.target_altitude_ft or pos_z
|
|
heading_deg = wp.target_heading_deg or heading_deg
|
|
velocity_fps = wp.target_velocity_fps or velocity_fps
|
|
preview_points.append((pos_x, pos_y))
|
|
elif wp.maneuver_type == ManeuverType.FLY_FOR_DURATION:
|
|
# Interpolazione lineare
|
|
dt = wp.duration_s or 1.0
|
|
steps = max(10, int(dt))
|
|
for i in range(steps):
|
|
frac = (i + 1) / steps
|
|
heading_rad = math.radians(heading_deg)
|
|
dist = (wp.target_velocity_fps or velocity_fps) * (dt / steps)
|
|
pos_x += dist * math.sin(heading_rad)
|
|
pos_y += dist * math.cos(heading_rad)
|
|
preview_points.append((pos_x, pos_y))
|
|
heading_deg = wp.target_heading_deg or heading_deg
|
|
velocity_fps = wp.target_velocity_fps or velocity_fps
|
|
elif wp.maneuver_type == ManeuverType.DYNAMIC_MANEUVER:
|
|
# Simula la curva: integra accelerazione laterale
|
|
dt = wp.duration_s or 1.0
|
|
steps = max(20, int(dt * 2))
|
|
velocity = wp.dynamic_velocity_fps or velocity_fps
|
|
heading = heading_deg
|
|
lateral_g = wp.lateral_g or 0.0
|
|
long_g = wp.longitudinal_g or 0.0
|
|
vert_g = wp.vertical_g or 0.0
|
|
G_FT_S2 = 32.174
|
|
lateral_acc = lateral_g * G_FT_S2
|
|
long_acc = long_g * G_FT_S2
|
|
vert_acc = vert_g * G_FT_S2
|
|
for i in range(steps):
|
|
dts = dt / steps
|
|
# Aggiorna heading (virata)
|
|
turn_rate_rad_s = lateral_acc / max(velocity, 1e-3)
|
|
heading += math.degrees(turn_rate_rad_s * dts)
|
|
heading_rad = math.radians(heading)
|
|
# Aggiorna velocità
|
|
velocity += long_acc * dts
|
|
# Aggiorna posizione
|
|
pos_x += velocity * math.sin(heading_rad) * dts
|
|
pos_y += velocity * math.cos(heading_rad) * dts
|
|
pos_z += vert_acc * dts
|
|
preview_points.append((pos_x, pos_y))
|
|
heading_deg = heading
|
|
velocity_fps = velocity
|
|
# Converti i punti in polari per la PPI
|
|
thetas = []
|
|
rs = []
|
|
for x, y in preview_points:
|
|
r = math.sqrt(x**2 + y**2) / 6076.12
|
|
th = math.atan2(x, y)
|
|
rs.append(r)
|
|
thetas.append(th)
|
|
|
|
# Mostra il punto iniziale anche se c'è solo un waypoint
|
|
if len(thetas) == 1:
|
|
self._preview_artist.set_data([], [])
|
|
self._waypoints_plot.set_data(thetas, rs)
|
|
self._start_plot.set_data([thetas[0]], [rs[0]])
|
|
heading_len = 3
|
|
# Usa heading del waypoint se disponibile
|
|
wp_heading = None
|
|
if waypoints and hasattr(waypoints[0], 'target_heading_deg') and waypoints[0].target_heading_deg is not None:
|
|
wp_heading = waypoints[0].target_heading_deg
|
|
if wp_heading is not None:
|
|
# L'heading è rispetto al nord (0°), quindi la freccia va da (r, azimuth) verso (r+len, azimuth+heading)
|
|
start_theta = thetas[0]
|
|
start_r = rs[0]
|
|
heading_theta = start_theta + math.radians(wp_heading)
|
|
heading_r2 = start_r + heading_len
|
|
if hasattr(self, '_heading_artist'):
|
|
self._heading_artist.remove()
|
|
self._heading_artist, = self.ax.plot([start_theta, heading_theta], [start_r, heading_r2], color='red', linewidth=1, alpha=0.8)
|
|
else:
|
|
heading_theta = thetas[0]
|
|
heading_r2 = rs[0] + heading_len
|
|
if hasattr(self, '_heading_artist'):
|
|
self._heading_artist.remove()
|
|
self._heading_artist, = self.ax.plot([thetas[0], heading_theta], [rs[0], heading_r2], color='red', linewidth=1, alpha=0.8)
|
|
self.canvas.draw()
|
|
self._on_range_selected()
|
|
return
|
|
elif len(thetas) >= 2:
|
|
self._preview_artist.set_data(thetas, rs)
|
|
self._waypoints_plot.set_data(thetas, rs)
|
|
self._start_plot.set_data([thetas[0]], [rs[0]])
|
|
heading_len = 3
|
|
heading_theta = thetas[0]
|
|
heading_r2 = rs[0] + heading_len
|
|
if hasattr(self, '_heading_artist'):
|
|
self._heading_artist.remove()
|
|
self._heading_artist, = self.ax.plot([heading_theta, heading_theta], [rs[0], heading_r2], color='red', linewidth=1, alpha=0.8)
|
|
self.canvas.draw()
|
|
self._on_range_selected()
|
|
return
|
|
|
|
# Determine input type
|
|
if isinstance(trajectory, list):
|
|
self._spline_preview_active = False
|
|
waypoints = trajectory
|
|
if not waypoints:
|
|
self.canvas.draw()
|
|
return
|
|
if all(getattr(wp, 'maneuver_type', None) != ManeuverType.FLY_TO_POINT for wp in waypoints):
|
|
self.canvas.draw()
|
|
return
|
|
# Classic preview (polyline) SOLO se spline non attiva
|
|
# (la preview spline cancella la classica)
|
|
# Solo waypoint con azimuth e range (escludi manovre dinamiche)
|
|
thetas = []
|
|
rs = []
|
|
for wp in waypoints:
|
|
az = getattr(wp, 'target_azimuth_deg', None)
|
|
rng = getattr(wp, 'target_range_nm', None)
|
|
if az is not None and rng is not None:
|
|
thetas.append(math.radians(az))
|
|
rs.append(rng)
|
|
if len(thetas) < 2:
|
|
self.canvas.draw()
|
|
return
|
|
# Verifica se la preview è per spline: se sì, non disegnare la linea classica
|
|
if hasattr(self, '_spline_preview_active') and self._spline_preview_active:
|
|
self._preview_artist.set_data([], [])
|
|
else:
|
|
self._preview_artist.set_data(thetas, rs)
|
|
self._waypoints_plot.set_data(thetas, rs)
|
|
start_theta = thetas[0]
|
|
start_r = rs[0]
|
|
self._start_plot.set_data([start_theta], [start_r])
|
|
heading_len = 3
|
|
heading_theta = start_theta
|
|
heading_r2 = start_r + heading_len
|
|
# Cancella eventuali frecce precedenti
|
|
if hasattr(self, '_heading_artist'):
|
|
self._heading_artist.remove()
|
|
self._heading_artist, = self.ax.plot([start_theta, heading_theta], [start_r, heading_r2], color='red', linewidth=1, alpha=0.8)
|
|
else:
|
|
# Assume Target object with trajectory and use_spline
|
|
waypoints = getattr(trajectory, "trajectory", [])
|
|
if len(waypoints) < 2:
|
|
self.canvas.draw()
|
|
return
|
|
if getattr(trajectory, "use_spline", False):
|
|
self._spline_preview_active = True
|
|
from target_simulator.utils.spline import catmull_rom_spline
|
|
# Converte i waypoint da polari a cartesiane (x, y) in NM
|
|
points = []
|
|
for wp in waypoints:
|
|
r_nm = getattr(wp, 'target_range_nm', 0)
|
|
theta_deg = getattr(wp, 'target_azimuth_deg', 0)
|
|
theta_rad = math.radians(theta_deg)
|
|
x_nm = r_nm * math.sin(theta_rad)
|
|
y_nm = r_nm * math.cos(theta_rad)
|
|
points.append((x_nm, y_nm))
|
|
if len(points) < 4:
|
|
spline_pts = points
|
|
else:
|
|
try:
|
|
spline_pts = catmull_rom_spline(points)
|
|
# DEBUG: salva i punti spline in un file CSV
|
|
import csv, os
|
|
temp_path = os.path.join(os.path.dirname(__file__), '..', '..', 'Temp', 'spline_preview.csv')
|
|
with open(temp_path, 'w', newline='') as csvfile:
|
|
writer = csv.writer(csvfile)
|
|
writer.writerow(['x_nm', 'y_nm'])
|
|
for pt in spline_pts:
|
|
writer.writerow([pt[0], pt[1]])
|
|
except Exception:
|
|
spline_pts = points
|
|
# Converte i punti spline da x/y a (theta, r) in NM
|
|
spline_thetas = []
|
|
spline_rs = []
|
|
for x_nm, y_nm in spline_pts:
|
|
r_nm = (x_nm**2 + y_nm**2)**0.5
|
|
theta = math.atan2(x_nm, y_nm)
|
|
spline_thetas.append(theta)
|
|
spline_rs.append(r_nm)
|
|
self._preview_artist.set_data(spline_thetas, spline_rs)
|
|
wp_thetas = []
|
|
wp_rs = []
|
|
for x_nm, y_nm in points:
|
|
r_nm = (x_nm**2 + y_nm**2)**0.5
|
|
theta = math.atan2(x_nm, y_nm)
|
|
wp_thetas.append(theta)
|
|
wp_rs.append(r_nm)
|
|
self._waypoints_plot.set_data(wp_thetas, wp_rs)
|
|
start_x_nm, start_y_nm = points[0]
|
|
start_r_nm = (start_x_nm**2 + start_y_nm**2)**0.5
|
|
start_theta = math.atan2(start_x_nm, start_y_nm)
|
|
self._start_plot.set_data([start_theta], [start_r_nm])
|
|
if hasattr(self, '_heading_artist'):
|
|
self._heading_artist.remove()
|
|
if len(points) > 1:
|
|
dx = points[1][0] - points[0][0]
|
|
dy = points[1][1] - points[0][1]
|
|
norm = (dx**2 + dy**2)**0.5
|
|
if norm > 0:
|
|
heading_len = 3
|
|
hx_nm = start_x_nm + heading_len * dx / norm
|
|
hy_nm = start_y_nm + heading_len * dy / norm
|
|
h_r_nm = (hx_nm**2 + hy_nm**2)**0.5
|
|
h_theta = math.atan2(hx_nm, hy_nm)
|
|
self._heading_artist, = self.ax.plot([start_theta, h_theta], [start_r_nm, h_r_nm], color='red', linewidth=1, alpha=0.8)
|
|
else:
|
|
self._spline_preview_active = False
|
|
xs = [wp.x for wp in waypoints]
|
|
ys = [wp.y for wp in waypoints]
|
|
self._preview_artist, = self.ax.plot(xs, ys, color="orange", linestyle="--", linewidth=2, alpha=0.7)
|
|
# ...rimosso blocco duplicato/non valido...
|
|
self._update_scan_lines()
|
|
self.canvas.draw()
|
|
|
|
def reconfigure_radar(self, max_range_nm: int, scan_limit_deg: int):
|
|
"""
|
|
Updates the radar parameters (range, scan limit) of an existing PPI display.
|
|
"""
|
|
self.max_range = max_range_nm
|
|
self.scan_limit_deg = scan_limit_deg
|
|
|
|
# Update the range combobox values
|
|
steps = [10, 20, 40, 80, 100, 160]
|
|
valid_steps = sorted([s for s in steps if s <= self.max_range])
|
|
if not valid_steps: valid_steps = [self.max_range]
|
|
if self.max_range not in valid_steps:
|
|
valid_steps.append(self.max_range)
|
|
valid_steps.sort()
|
|
|
|
self.range_selector['values'] = valid_steps
|
|
self.range_var.set(self.max_range) # Set to the new max range
|
|
|
|
# Update the scan limit lines
|
|
limit_rad = np.deg2rad(self.scan_limit_deg)
|
|
self._scan_line_1.set_xdata([limit_rad, limit_rad])
|
|
self._scan_line_2.set_xdata([-limit_rad, -limit_rad])
|
|
|
|
# Apply the new range and redraw everything
|
|
self._on_range_selected() |