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

fix simulation on spline

Browse Source
This commit is contained in:
VALLONGOL 2025-10-13 12:55:32 +02:00
parent 04cf083a74
commit 3f1fddd87d
1 changed files with 103 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

155
target_simulator/core/models.py
View File

@ -85,46 +85,73 @@ class Target:
self.reset_simulation() self.reset_simulation()
def reset_simulation(self): def reset_simulation(self):
# Se use_spline è attivo, calcola la traiettoria spline e la memorizza self._sim_time_s = 0.0
self._time_in_waypoint_s = 0.0
self._current_waypoint_index = -1
if self.use_spline and self.trajectory: if self.use_spline and self.trajectory:
from target_simulator.utils.spline import catmull_rom_spline from target_simulator.utils.spline import catmull_rom_spline
# Estrai i waypoint come punti (x, y, z)
# Convert all waypoints to cartesian points in feet
pts = [] pts = []
for wp in self.trajectory: for wp in self.trajectory:
range_ft = (wp.target_range_nm or 0.0) * NM_TO_FT if wp.maneuver_type == ManeuverType.FLY_TO_POINT and wp.target_range_nm is not None and wp.target_azimuth_deg is not None:
az_rad = math.radians(wp.target_azimuth_deg or 0.0) range_ft = (wp.target_range_nm or 0.0) * NM_TO_FT
x = range_ft * math.sin(az_rad) az_rad = math.radians(wp.target_azimuth_deg or 0.0)
y = range_ft * math.cos(az_rad) x = range_ft * math.sin(az_rad)
z = wp.target_altitude_ft or 0.0 y = range_ft * math.cos(az_rad)
pts.append([x, y, z]) z = wp.target_altitude_ft or 0.0
pts.append([x, y, z])
if not pts:
self._pos_x_ft = self._pos_y_ft = self._pos_z_ft = 0.0
self.current_velocity_fps = 0.0
self.current_heading_deg = 0.0
self._spline_path = None
return
# Generate spline path from points in feet
self._spline_path = catmull_rom_spline(pts, num_points=200) self._spline_path = catmull_rom_spline(pts, num_points=200)
self._spline_index = 0 self._spline_index = 0
"""Resets the target to its initial state defined by the first waypoint."""
self._sim_time_s = 0.0
self._current_waypoint_index = -1
self._time_in_waypoint_s = 0.0
if not self.trajectory or self.trajectory[0].maneuver_type != ManeuverType.FLY_TO_POINT: # Set initial position from the first point of the spline
self._pos_x_ft = self._pos_y_ft = self._pos_z_ft = 0.0 initial_pos = self._spline_path[0]
self.current_velocity_fps = 0.0; self.current_heading_deg = 0.0; self.current_pitch_deg = 0.0 self._pos_x_ft, self._pos_y_ft, self._pos_z_ft = initial_pos[0], initial_pos[1], initial_pos[2]
# Calculate total duration from waypoints
total_duration = sum(wp.duration_s or 0.0 for wp in self.trajectory if wp.duration_s)
# Create a time mapping for each point in the spline path
self._spline_point_times = [i * (total_duration / (len(self._spline_path) - 1)) for i in range(len(self._spline_path))]
if not self._spline_point_times: self._spline_point_times = [0.0]
self._update_current_polar_coords()
else: else:
first_wp = self.trajectory[0] # Waypoint-based simulation reset
self.current_range_nm = first_wp.target_range_nm or 0.0 if not self.trajectory or self.trajectory[0].maneuver_type != ManeuverType.FLY_TO_POINT:
self.current_azimuth_deg = first_wp.target_azimuth_deg or 0.0 self._pos_x_ft = self._pos_y_ft = self._pos_z_ft = 0.0
self.current_altitude_ft = first_wp.target_altitude_ft or 0.0 self.current_velocity_fps = 0.0
self.current_velocity_fps = first_wp.target_velocity_fps or 0.0 self.current_heading_deg = 0.0
self.current_heading_deg = first_wp.target_heading_deg or 0.0 self.current_pitch_deg = 0.0
self.current_pitch_deg = 0.0 # Pitch is calculated, not set directly at start else:
first_wp = self.trajectory[0]
self.current_range_nm = first_wp.target_range_nm or 0.0
self.current_azimuth_deg = first_wp.target_azimuth_deg or 0.0
self.current_altitude_ft = first_wp.target_altitude_ft or 0.0
self.current_velocity_fps = first_wp.target_velocity_fps or 0.0
self.current_heading_deg = first_wp.target_heading_deg or 0.0
self.current_pitch_deg = 0.0
range_ft = self.current_range_nm * NM_TO_FT range_ft = self.current_range_nm * NM_TO_FT
az_rad = math.radians(self.current_azimuth_deg) az_rad = math.radians(self.current_azimuth_deg)
self._pos_x_ft = range_ft * math.sin(az_rad) self._pos_x_ft = range_ft * math.sin(az_rad)
self._pos_y_ft = range_ft * math.cos(az_rad) self._pos_y_ft = range_ft * math.cos(az_rad)
self._pos_z_ft = self.current_altitude_ft self._pos_z_ft = self.current_altitude_ft
self._activate_next_waypoint() self._activate_next_waypoint()
self._update_current_polar_coords() self._update_current_polar_coords()
def _activate_next_waypoint(self): def _activate_next_waypoint(self):
"""Pre-calculates and activates the next waypoint in the trajectory.""" """Pre-calculates and activates the next waypoint in the trajectory."""
@ -150,47 +177,67 @@ class Target:
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)
# These become the constant kinematics for this segment
self.current_heading_deg = math.degrees(math.atan2(dx, dy)) % 360 self.current_heading_deg = math.degrees(math.atan2(dx, dy)) % 360
self.current_pitch_deg = math.degrees(math.atan2(dz, dist_2d)) if dist_2d > 1 else 90.0 if dz > 0 else -90.0 self.current_pitch_deg = math.degrees(math.atan2(dz, dist_2d)) if dist_2d > 1 else 90.0 if dz > 0 else -90.0
if wp.duration_s and wp.duration_s > 0: if wp.duration_s and wp.duration_s > 0:
self.current_velocity_fps = dist_3d / wp.duration_s self.current_velocity_fps = dist_3d / wp.duration_s
wp._calculated_duration_s = wp.duration_s wp._calculated_duration_s = wp.duration_s
else: # Invalid waypoint, make it static else:
self.current_velocity_fps = 0 self.current_velocity_fps = 0
wp._calculated_duration_s = 999999 wp._calculated_duration_s = 999999
else: # ManeuverType.FLY_FOR_DURATION else:
# For duration-based flights, assume level flight (pitch = 0) since there's no target point.
self.current_pitch_deg = 0 self.current_pitch_deg = 0
def update_state(self, delta_time_s: float): def update_state(self, delta_time_s: float):
if not self.active: if not self.active:
return return
if self.use_spline and self._spline_path: if self.use_spline and self._spline_path:
# Avanza lungo la spline
self._sim_time_s += delta_time_s self._sim_time_s += delta_time_s
# Calcola l'indice successivo in base alla velocità media tra i waypoint
# (per ora: avanza di 1 punto per tick, si può migliorare con tempo totale) total_duration = self._spline_point_times[-1]
if not hasattr(self, '_spline_index'): if total_duration <= 0 or self._sim_time_s >= total_duration:
self._spline_index = 0 # Simulation finished, stay at the last point
if self._spline_index < len(self._spline_path) - 1: self._spline_index = len(self._spline_path) - 1
self._spline_index += 1
pt = self._spline_path[self._spline_index]
prev_pt = self._spline_path[self._spline_index-1]
self._pos_x_ft, self._pos_y_ft, self._pos_z_ft = pt
# Calcola heading e pitch tra i due punti
dx, dy, dz = pt[0] - prev_pt[0], pt[1] - prev_pt[1], pt[2] - prev_pt[2]
dist_2d = math.sqrt(dx**2 + dy**2)
self.current_heading_deg = math.degrees(math.atan2(dx, dy)) % 360
self.current_pitch_deg = math.degrees(math.atan2(dz, dist_2d)) if dist_2d > 1 else 90.0 if dz > 0 else -90.0
self.current_altitude_ft = pt[2]
self.current_range_nm = math.sqrt(pt[0]**2 + pt[1]**2) / NM_TO_FT
# Velocità istantanea (approssimata)
self.current_velocity_fps = math.sqrt(dx**2 + dy**2 + dz**2) / delta_time_s if delta_time_s > 0 else 0.0
# Se finita la spline, il target si ferma
else: else:
pass # Find the current index in the spline path based on time
progress = self._sim_time_s / total_duration
self._spline_index = int(progress * (len(self._spline_path) - 1))
pt = self._spline_path[self._spline_index]
# Determine previous point for calculating heading/pitch
prev_pt = self._spline_path[self._spline_index - 1] if self._spline_index > 0 else pt
# Update position (path is already in feet)
self._pos_x_ft, self._pos_y_ft, self._pos_z_ft = pt[0], pt[1], pt[2]
# Update kinematics
dx = self._pos_x_ft - prev_pt[0]
dy = self._pos_y_ft - prev_pt[1]
dz = self._pos_z_ft - prev_pt[2]
dist_moved_since_last_tick = math.sqrt(dx**2 + dy**2 + dz**2)
dist_2d = math.sqrt(dx**2 + dy**2)
if dist_2d > 0.1: # Avoid division by zero or noise
self.current_heading_deg = math.degrees(math.atan2(dx, dy)) % 360
self.current_pitch_deg = math.degrees(math.atan2(dz, dist_2d))
if delta_time_s > 0:
self.current_velocity_fps = dist_moved_since_last_tick / delta_time_s
self._update_current_polar_coords()
# --- DEBUG LOG ---
#import logging
#logging.getLogger("target_simulator.spline_debug").info(
# f"SplinePos: t={self._sim_time_s:.2f}s idx={self._spline_index} x={self._pos_x_ft:.2f} y={self._pos_y_ft:.2f} z={self._pos_z_ft:.2f}")
# --- DEBUG LOG ---
#import logging
#logging.getLogger("target_simulator.spline_debug").info(
# f"SplinePos: t={self._sim_time_s:.2f}s idx={self._spline_index} x={self._pos_x_ft:.2f} y={self._pos_y_ft:.2f} z={self._pos_z_ft:.2f}")
else: else:
# Logica classica # Logica classica
self._sim_time_s += delta_time_s self._sim_time_s += delta_time_s