fix spline request min 4 waypoint

This commit is contained in:
VALLONGOL 2025-10-14 14:36:37 +02:00
parent 77a1d87366
commit 288c4f73be
4 changed files with 72 additions and 11 deletions

View File

@ -114,7 +114,7 @@
"target_azimuth_deg": 45.0 "target_azimuth_deg": 45.0
} }
], ],
"use_spline": false "use_spline": true
}, },
{ {
"target_id": 1, "target_id": 1,

View File

@ -188,6 +188,19 @@ class Target:
else: else:
self.current_velocity_fps = 0 self.current_velocity_fps = 0
wp._calculated_duration_s = 999999 wp._calculated_duration_s = 999999
elif wp.maneuver_type == ManeuverType.FLY_FOR_DURATION:
# Aggiorna la posizione iniziale per il prossimo segmento
heading_rad = math.radians(wp.target_heading_deg or self.current_heading_deg)
pitch_rad = math.radians(self.current_pitch_deg)
dist_moved = (wp.target_velocity_fps or self.current_velocity_fps) * (wp.duration_s or 0.0)
dist_2d = dist_moved * math.cos(pitch_rad)
self._pos_x_ft += dist_2d * math.sin(heading_rad)
self._pos_y_ft += dist_2d * math.cos(heading_rad)
self._pos_z_ft += dist_moved * math.sin(pitch_rad)
self._update_current_polar_coords()
self.current_heading_deg = wp.target_heading_deg or self.current_heading_deg
self.current_velocity_fps = wp.target_velocity_fps or self.current_velocity_fps
wp._calculated_duration_s = wp.duration_s or 0.0
else: else:
self.current_pitch_deg = 0 self.current_pitch_deg = 0

View File

@ -294,15 +294,34 @@ class PPIDisplay(ttk.Frame):
if getattr(trajectory, "use_spline", False): if getattr(trajectory, "use_spline", False):
self._spline_preview_active = True self._spline_preview_active = True
from target_simulator.utils.spline import catmull_rom_spline from target_simulator.utils.spline import catmull_rom_spline
# Converte i waypoint da polari a cartesiane (x, y) in NM # Costruisci la lista dei punti: waypoint espliciti + punti finali calcolati per FLY_FOR_DURATION
points = [] points = []
for wp in waypoints: curr_r = None
r_nm = getattr(wp, 'target_range_nm', 0) curr_theta = None
theta_deg = getattr(wp, 'target_azimuth_deg', 0) for i, wp in enumerate(waypoints):
theta_rad = math.radians(theta_deg) if getattr(wp, 'maneuver_type', None) == ManeuverType.FLY_TO_POINT:
x_nm = r_nm * math.sin(theta_rad) curr_r = getattr(wp, 'target_range_nm', 0)
y_nm = r_nm * math.cos(theta_rad) curr_theta = math.radians(getattr(wp, 'target_azimuth_deg', 0))
points.append((x_nm, y_nm)) x_nm = curr_r * math.sin(curr_theta)
y_nm = curr_r * math.cos(curr_theta)
points.append((x_nm, y_nm))
elif getattr(wp, 'maneuver_type', None) == ManeuverType.FLY_FOR_DURATION:
# Se non c'è punto iniziale, ignora
if curr_r is None or curr_theta is None:
continue
vel_fps = getattr(wp, 'target_velocity_fps', 0)
vel_nmps = vel_fps / NM_TO_FT if vel_fps else 0
duration = getattr(wp, 'duration_s', 0)
heading_deg = getattr(wp, 'target_heading_deg', 0)
heading_rad = math.radians(heading_deg)
dr = vel_nmps * duration
theta1 = curr_theta + heading_rad
r1 = curr_r + dr
x_nm = r1 * math.sin(theta1)
y_nm = r1 * math.cos(theta1)
points.append((x_nm, y_nm))
curr_r = r1
curr_theta = theta1
if len(points) < 4: if len(points) < 4:
spline_pts = points spline_pts = points
else: else:

View File

@ -1,5 +1,6 @@
import tkinter as tk import tkinter as tk
from tkinter import ttk, messagebox from tkinter import ttk, messagebox
import math
from typing import List, Optional from typing import List, Optional
import copy import copy
from queue import Queue, Empty from queue import Queue, Empty
@ -128,8 +129,36 @@ class TrajectoryEditorWindow(tk.Toplevel):
ttk.Button(button_frame, text="Cancel", command=self._on_cancel).pack(side=tk.RIGHT) ttk.Button(button_frame, text="Cancel", command=self._on_cancel).pack(side=tk.RIGHT)
ttk.Button(button_frame, text="OK", command=self._on_ok).pack(side=tk.RIGHT, padx=5) ttk.Button(button_frame, text="OK", command=self._on_ok).pack(side=tk.RIGHT, padx=5)
def _on_spline_toggle(self): def _on_spline_toggle(self):
# Conta i punti effettivi (waypoint + calcolati)
waypoints = self.waypoints
points = []
curr_r = None
curr_theta = None
from target_simulator.core.models import ManeuverType, NM_TO_FT
for i, wp in enumerate(waypoints):
if getattr(wp, 'maneuver_type', None) == ManeuverType.FLY_TO_POINT:
curr_r = getattr(wp, 'target_range_nm', 0)
curr_theta = math.radians(getattr(wp, 'target_azimuth_deg', 0))
points.append((curr_r, curr_theta))
elif getattr(wp, 'maneuver_type', None) == ManeuverType.FLY_FOR_DURATION:
if curr_r is None or curr_theta is None:
continue
vel_fps = getattr(wp, 'target_velocity_fps', 0)
vel_nmps = vel_fps / NM_TO_FT if vel_fps else 0
duration = getattr(wp, 'duration_s', 0)
heading_deg = getattr(wp, 'target_heading_deg', 0)
heading_rad = math.radians(heading_deg)
dr = vel_nmps * duration
theta1 = curr_theta + heading_rad
r1 = curr_r + dr
points.append((r1, theta1))
curr_r = r1
curr_theta = theta1
if len(points) < 4:
self.use_spline_var.set(False)
messagebox.showinfo("Spline not available", "Spline mode requires at least 4 points (explicit or calculated waypoints).")
return
self._update_static_preview() self._update_static_preview()
# Aggiorna la label tempo simulazione all'apertura # Aggiorna la label tempo simulazione all'apertura
total_time = self._get_total_simulation_time() total_time = self._get_total_simulation_time()
self.sim_time_label.config(text=f"0.0s / {total_time:.1f}s") self.sim_time_label.config(text=f"0.0s / {total_time:.1f}s")