fix simulation on spline
This commit is contained in:
VALLONGOL
parent
04cf083a74
commit
3f1fddd87d
@ -85,46 +85,73 @@ class Target:
|
||||
self.reset_simulation()
|
||||
|
||||
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:
|
||||
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 = []
|
||||
for wp in self.trajectory:
|
||||
range_ft = (wp.target_range_nm or 0.0) * NM_TO_FT
|
||||
az_rad = math.radians(wp.target_azimuth_deg or 0.0)
|
||||
x = range_ft * math.sin(az_rad)
|
||||
y = range_ft * math.cos(az_rad)
|
||||
z = wp.target_altitude_ft or 0.0
|
||||
pts.append([x, y, z])
|
||||
if wp.maneuver_type == ManeuverType.FLY_TO_POINT and wp.target_range_nm is not None and wp.target_azimuth_deg is not None:
|
||||
range_ft = (wp.target_range_nm or 0.0) * NM_TO_FT
|
||||
az_rad = math.radians(wp.target_azimuth_deg or 0.0)
|
||||
x = range_ft * math.sin(az_rad)
|
||||
y = range_ft * math.cos(az_rad)
|
||||
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_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:
|
||||
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.current_pitch_deg = 0.0
|
||||
# Set initial position from the first point of the spline
|
||||
initial_pos = self._spline_path[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:
|
||||
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 # Pitch is calculated, not set directly at start
|
||||
# Waypoint-based simulation reset
|
||||
if not self.trajectory or self.trajectory[0].maneuver_type != ManeuverType.FLY_TO_POINT:
|
||||
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.current_pitch_deg = 0.0
|
||||
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
|
||||
az_rad = math.radians(self.current_azimuth_deg)
|
||||
self._pos_x_ft = range_ft * math.sin(az_rad)
|
||||
self._pos_y_ft = range_ft * math.cos(az_rad)
|
||||
self._pos_z_ft = self.current_altitude_ft
|
||||
range_ft = self.current_range_nm * NM_TO_FT
|
||||
az_rad = math.radians(self.current_azimuth_deg)
|
||||
self._pos_x_ft = range_ft * math.sin(az_rad)
|
||||
self._pos_y_ft = range_ft * math.cos(az_rad)
|
||||
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):
|
||||
"""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_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_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:
|
||||
self.current_velocity_fps = dist_3d / wp.duration_s
|
||||
wp._calculated_duration_s = wp.duration_s
|
||||
else: # Invalid waypoint, make it static
|
||||
else:
|
||||
self.current_velocity_fps = 0
|
||||
wp._calculated_duration_s = 999999
|
||||
else: # ManeuverType.FLY_FOR_DURATION
|
||||
# For duration-based flights, assume level flight (pitch = 0) since there's no target point.
|
||||
else:
|
||||
self.current_pitch_deg = 0
|
||||
|
||||
def update_state(self, delta_time_s: float):
|
||||
if not self.active:
|
||||
return
|
||||
|
||||
if self.use_spline and self._spline_path:
|
||||
# Avanza lungo la spline
|
||||
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)
|
||||
if not hasattr(self, '_spline_index'):
|
||||
self._spline_index = 0
|
||||
if 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
|
||||
|
||||
total_duration = self._spline_point_times[-1]
|
||||
if total_duration <= 0 or self._sim_time_s >= total_duration:
|
||||
# Simulation finished, stay at the last point
|
||||
self._spline_index = len(self._spline_path) - 1
|
||||
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:
|
||||
# Logica classica
|
||||
self._sim_time_s += delta_time_s
|
||||
|
||||
Loading…
Reference in New Issue
Block a user