fix spline request min 4 waypoint
This commit is contained in:
parent
77a1d87366
commit
288c4f73be
@ -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,
|
||||||
|
|||||||
@ -174,7 +174,7 @@ class Target:
|
|||||||
target_x = target_range_ft * math.sin(target_az_rad)
|
target_x = target_range_ft * math.sin(target_az_rad)
|
||||||
target_y = target_range_ft * math.cos(target_az_rad)
|
target_y = target_range_ft * math.cos(target_az_rad)
|
||||||
target_z = wp.target_altitude_ft or self._pos_z_ft
|
target_z = wp.target_altitude_ft or self._pos_z_ft
|
||||||
|
|
||||||
dx, dy, dz = target_x - self._pos_x_ft, target_y - self._pos_y_ft, target_z - self._pos_z_ft
|
dx, dy, dz = target_x - self._pos_x_ft, target_y - self._pos_y_ft, target_z - self._pos_z_ft
|
||||||
dist_3d = math.sqrt(dx**2 + dy**2 + dz**2)
|
dist_3d = math.sqrt(dx**2 + dy**2 + dz**2)
|
||||||
dist_2d = math.sqrt(dx**2 + dy**2)
|
dist_2d = math.sqrt(dx**2 + dy**2)
|
||||||
@ -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
|
||||||
|
|
||||||
|
|||||||
@ -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:
|
||||||
|
|||||||
@ -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")
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user