diff --git a/target_simulator/core/models.py b/target_simulator/core/models.py index 8d8302e..7ac62d3 100644 --- a/target_simulator/core/models.py +++ b/target_simulator/core/models.py @@ -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 + + self._activate_next_waypoint() - 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._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