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

fix spline request min 4 waypoint

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
settings.json
View File

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

13
target_simulator/core/models.py
View File

@ -188,6 +188,19 @@ class Target:
else:
self.current_velocity_fps = 0
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:
self.current_pitch_deg = 0

35
target_simulator/gui/ppi_display.py
View File

@ -294,15 +294,34 @@ class PPIDisplay(ttk.Frame):
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
# Costruisci la lista dei punti: waypoint espliciti + punti finali calcolati per FLY_FOR_DURATION
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))
curr_r = None
curr_theta = None
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))
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:
spline_pts = points
else:

31
target_simulator/gui/trajectory_editor_window.py
View File

@ -1,5 +1,6 @@
import tkinter as tk
from tkinter import ttk, messagebox
import math
from typing import List, Optional
import copy
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="OK", command=self._on_ok).pack(side=tk.RIGHT, padx=5)
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()
# Aggiorna la label tempo simulazione all'apertura
total_time = self._get_total_simulation_time()
self.sim_time_label.config(text=f"0.0s / {total_time:.1f}s")