diff --git a/target_simulator/core/models.py b/target_simulator/core/models.py index 6a88fc7..2f21ef5 100644 --- a/target_simulator/core/models.py +++ b/target_simulator/core/models.py @@ -107,9 +107,16 @@ class Target: dist_3d, dist_2d = math.sqrt(dx**2 + dy**2 + dz**2), math.sqrt( dx**2 + dy**2 ) - self.current_heading_deg = ( - math.degrees(math.atan2(dx, dy)) % 360 if dist_2d > 0.1 else 0.0 - ) + # Compute heading using atan2(dy, dx) so heading degrees correspond + # to azimuth convention where 0 is North and positive is left (CCW). + # Restore original heading calculation (atan2(dx, dy)) to match + # the path generation convention (x = R*sin(az), y = R*cos(az)). + try: + self.current_heading_deg = ( + math.degrees(math.atan2(dx, dy)) % 360 if dist_2d > 0.1 else 0.0 + ) + except Exception: + self.current_heading_deg = 0.0 self.current_pitch_deg = ( math.degrees(math.atan2(dz, dist_2d)) if dist_2d > 0.1 else 0.0 ) @@ -146,6 +153,7 @@ class Target: vertices = [] total_duration_s = 0.0 first_wp = waypoints[0] + # Convert polar (range, azimuth) to Cartesian (x East, y North) pos_ft = [ (first_wp.target_range_nm or 0.0) * NM_TO_FT @@ -168,6 +176,7 @@ class Target: duration = wp.duration_s or 0.0 if wp.maneuver_type == ManeuverType.FLY_TO_POINT: + # Convert polar waypoint to Cartesian using same convention pos_ft = [ (wp.target_range_nm or 0.0) * NM_TO_FT @@ -331,7 +340,11 @@ class Target: if delta_time_s > 0: self.current_velocity_fps = dist_3d / delta_time_s if dist_2d > 0.1: - self.current_heading_deg = math.degrees(math.atan2(dx, dy)) % 360 + # Restore original update_state heading computation using atan2(dx, dy). + try: + self.current_heading_deg = math.degrees(math.atan2(dx, dy)) % 360 + except Exception: + self.current_heading_deg = 0.0 self.current_pitch_deg = math.degrees(math.atan2(dz, dist_2d)) self._update_current_polar_coords() @@ -339,9 +352,15 @@ class Target: self.current_range_nm = ( math.sqrt(self._pos_x_ft**2 + self._pos_y_ft**2) / NM_TO_FT ) - self.current_azimuth_deg = ( - math.degrees(math.atan2(self._pos_x_ft, self._pos_y_ft)) % 360 - ) + # Compute azimuth using atan2(y, x) so positive azimuths follow the + # convention used by the GUI (positive to the left of 0°). + # Use atan2(x, y) to match the x=R*sin, y=R*cos convention used elsewhere. + try: + self.current_azimuth_deg = ( + math.degrees(math.atan2(self._pos_x_ft, self._pos_y_ft)) % 360 + ) + except Exception: + self.current_azimuth_deg = 0.0 self.current_altitude_ft = self._pos_z_ft def to_dict(self) -> Dict: diff --git a/target_simulator/gui/ppi_display.py b/target_simulator/gui/ppi_display.py index 31b879e..e141edb 100644 --- a/target_simulator/gui/ppi_display.py +++ b/target_simulator/gui/ppi_display.py @@ -67,25 +67,31 @@ class PPIDisplay(ttk.Frame): self.ax = fig.add_subplot(111, projection="polar", facecolor="#2E2E2E") + # Set zero at North and make theta increase counter-clockwise so + # positive azimuths are to the left of 0° (CCW positive) self.ax.set_theta_zero_location("N") - self.ax.set_theta_direction(-1) + self.ax.set_theta_direction(1) self.ax.set_rlabel_position(90) self.ax.set_ylim(0, self.range_var.get()) + # Create labels so that positive azimuths appear to the LEFT of 0°. + # We map internal angles (0..360) to displayed labels where angles + # increasing CCW from 0 are shown as +, and the right side is negative. angles = np.arange(0, 360, 30) - # --- MODIFICATION START --- - # Logic to format labels as 0, +30, ..., +/-180, ..., -30 labels = [] for angle in angles: - if angle == 0: + # Convert internal angle to centered signed value in [-180,180] + a = ((angle + 180) % 360) - 180 + # With CCW positive, a is already positive on the left + disp = a + if disp == 0: labels.append("0°") - elif 0 < angle < 180: - labels.append(f"+{angle}°") - elif angle == 180: + elif abs(disp) == 180: labels.append("±180°") - else: # angle > 180 - labels.append(f"-{360 - angle}°") - # --- MODIFICATION END --- + elif disp > 0: + labels.append(f"+{int(disp)}°") + else: + labels.append(f"{int(disp)}°") self.ax.set_thetagrids(angles, labels) self.ax.tick_params(axis="x", colors="white", labelsize=8) diff --git a/target_simulator/gui/sfp_debug_window.py b/target_simulator/gui/sfp_debug_window.py index 9714fbe..0d9b89c 100644 --- a/target_simulator/gui/sfp_debug_window.py +++ b/target_simulator/gui/sfp_debug_window.py @@ -200,9 +200,17 @@ class SfpDebugWindow(tk.Toplevel): traceable=True, ) t.current_range_nm = (ris_tgt.x ** 2 + ris_tgt.y ** 2) ** 0.5 / 6076.12 - t.current_azimuth_deg = math.degrees(math.atan2(ris_tgt.x, ris_tgt.y)) + try: + # Use atan2(y, x) so azimuth follows the conventional (x East, y North) + t.current_azimuth_deg = math.degrees(math.atan2(ris_tgt.y, ris_tgt.x)) + except Exception: + t.current_azimuth_deg = 0.0 t.current_altitude_ft = ris_tgt.z t.current_heading_deg = getattr(ris_tgt, 'heading', 0.0) + try: + self._log_to_widget(f"RIS JSON heading raw[{i}]: {getattr(ris_tgt, 'heading', None)}", "DEBUG") + except Exception: + pass t.active = True targets.append(t) self.update_ppi_targets(targets) @@ -744,12 +752,16 @@ class SfpDebugWindow(tk.Toplevel): vel_fps = vel_kn * KNOTS_TO_FPS + is_active = self.tgt_active_var.get() + is_traceable = self.tgt_traceable_var.get() updates = { "range_nm": f"{range_nm:.2f}", "azimuth_deg": f"{az_deg:.2f}", "velocity_fps": f"{vel_fps:.2f}", "heading_deg": f"{hdg_deg:.2f}", "altitude_ft": f"{alt_ft:.2f}", + "active": is_active, + "traceable": is_traceable, } command_str = command_builder.build_tgtset_selective(target_id, updates) command_str = command_str.strip() @@ -1026,7 +1038,8 @@ class SfpDebugWindow(tk.Toplevel): zm = float(tz) rng_m = (xm * xm + ym * ym) ** 0.5 rng_nm = rng_m / 1852.0 - az_deg = math.degrees(math.atan2(xm, ym)) + # Use atan2(y, x) for conventional azimuth (x East, y North) + az_deg = math.degrees(math.atan2(ym, xm)) # elevation from z and slant range elev_rad = math.atan2(zm, rng_m) if rng_m > 0 else 0.0 elev_deg = math.degrees(elev_rad) @@ -1050,8 +1063,8 @@ class SfpDebugWindow(tk.Toplevel): range_nm = ((x ** 2 + y ** 2) ** 0.5) / METERS_PER_NM if range_nm > current_range: continue - # Compute azimuth in degrees using same convention as PPIDisplay - az_deg = math.degrees(math.atan2(x, y)) + # Compute azimuth in degrees using atan2(y, x) + az_deg = math.degrees(math.atan2(y, x)) # Heading in JSON is in radians -> convert to degrees heading = t.get("heading", 0.0) try: diff --git a/tests/test_command_ris_roundtrip.py b/tests/test_command_ris_roundtrip.py new file mode 100644 index 0000000..f246d6b --- /dev/null +++ b/tests/test_command_ris_roundtrip.py @@ -0,0 +1,65 @@ +import math +from target_simulator.core.command_builder import build_tgtinit +from target_simulator.core.models import Waypoint, Target, NM_TO_FT + +METERS_PER_NM = 1852.0 + + +def test_build_tgtinit_contains_parameters(): + wp = Waypoint( + maneuver_type=Waypoint.maneuver_type, # placeholder, not used by builder + target_range_nm=30.0, + target_azimuth_deg=10.0, + target_altitude_ft=10000.0, + target_velocity_fps=506.34, + target_heading_deg=25.0, + ) + t = Target(target_id=0, trajectory=[wp]) + # Make sure flags are set as expected + t.active = True + t.traceable = True + t.restart = True + # Set current kinematics to match waypoint (build_tgtinit reads current_* fields) + t.current_range_nm = 30.0 + t.current_azimuth_deg = 10.0 + t.current_heading_deg = 25.0 + t.current_altitude_ft = 10000.0 + t.current_velocity_fps = 506.34 + + cmd = build_tgtinit(t) + # command should start with tgtinit and contain the numeric fields in order + parts = cmd.split() + assert parts[0] == "tgtinit" + assert parts[1] == "0" + assert parts[2] == "30.00" + assert parts[3] == "10.00" + # velocity is formatted as fps + assert parts[4] == "506.34" + assert parts[5] == "25.00" + assert parts[6] == "10000.00" + # qualifiers should contain /s /t /r (order may vary for /r appended) + qualifiers = parts[7:] + assert "/s" in qualifiers + assert "/t" in qualifiers + assert "/r" in qualifiers + + +def test_command_to_ris_coordinate_roundtrip(): + """Simula il percorso: command params -> RIS coords (meters) -> GUI azimuth -> reconstruct coords. + Verifica che la ricostruzione torni alle stesse coordinate (round-trip) entro tolleranza.""" + range_nm = 40.0 + az_deg = 20.0 + alt_ft = 10000.0 + + # Simulate server: produce RIS X,Y in meters using conventional conversion + rad = math.radians(az_deg) + x_m = range_nm * METERS_PER_NM * math.sin(rad) + y_m = range_nm * METERS_PER_NM * math.cos(rad) + + # GUI receives x_m,y_m and computes az_gui = degrees(atan2(y,x)). + # Given server uses x = R*sin(az) and y = R*cos(az), + # az_gui should equal (90° - az) modulo 360. + az_gui = math.degrees(math.atan2(y_m, x_m)) % 360 + expected = (90.0 - az_deg) % 360 + assert math.isclose(az_gui, expected, rel_tol=1e-9, abs_tol=1e-6) +