diff --git a/target_simulator/gui/payload_router.py b/target_simulator/gui/payload_router.py index 061e667..a7a8e90 100644 --- a/target_simulator/gui/payload_router.py +++ b/target_simulator/gui/payload_router.py @@ -360,21 +360,29 @@ class DebugPayloadRouter: # --- Propagate antenna azimuth to hub --- if self._hub: - # Get platform heading and relative antenna sweep - heading_rad = scenario_dict.get( - "true_heading", scenario_dict.get("platform_azimuth") - ) + # FINAL UNDERSTANDING after testing: + # ant_nav_az appears to be in BODY coordinates (relative to platform) + # The PPI display handles the rotation based on North-Up vs Heading-Up mode + # + # In North-Up: antenna should stay within scan cone centered on heading + # In Heading-Up: antenna should stay within scan cone centered on nose + # + # Just pass ant_nav_az directly - let PPI handle the transformations sweep_rad = scenario_dict.get("ant_nav_az") - total_az_rad = None - if heading_rad is not None: - total_az_rad = float(heading_rad) - if sweep_rad is not None: - total_az_rad += float(sweep_rad) - - if total_az_rad is not None: - az_deg = math.degrees(total_az_rad) - self._hub.set_antenna_azimuth(az_deg, timestamp=time.monotonic()) + if sweep_rad is not None: + try: + sweep_deg = math.degrees(float(sweep_rad)) + + # Normalize to [-180, 180] + while sweep_deg > 180: + sweep_deg -= 360 + while sweep_deg < -180: + sweep_deg += 360 + + self._hub.set_antenna_azimuth(sweep_deg, timestamp=time.monotonic()) + except Exception: + pass except Exception: self._logger.exception("Failed to generate text/JSON for RIS debug view.") @@ -415,42 +423,36 @@ class DebugPayloadRouter: return sc[key] return None - # Find platform heading and relative antenna sweep using multiple potential keys - heading_val = _find_val( - ["true_heading", "platform_azimuth", "heading"], obj - ) + # Just pass ant_nav_az directly - PPI handles transformations sweep_val = _find_val( ["ant_nav_az", "antenna_azimuth", "sweep_azimuth"], obj ) - total_az_val = None - if heading_val is not None: + if sweep_val is not None: try: - h_rad = float(heading_val) - # If heading is in degrees (> 2*pi), convert to radians - if abs(h_rad) > (2 * math.pi + 0.01): - h_rad = math.radians(h_rad) - - total_az_rad = h_rad - if sweep_val is not None: - s_rad = float(sweep_val) - # If sweep is in degrees, convert - if abs(s_rad) > (2 * math.pi + 0.01): - s_rad = math.radians(s_rad) - total_az_rad += s_rad - - az_deg = math.degrees(total_az_rad) - # LOGGARE IL SUCCESSO PER DEBUG + s_rad = float(sweep_val) + + # If value is in degrees, convert + if abs(s_rad) > (2 * math.pi + 0.01): + s_rad = math.radians(s_rad) + + sweep_deg = math.degrees(s_rad) + + # Normalize to [-180, 180] + while sweep_deg > 180: + sweep_deg -= 360 + while sweep_deg < -180: + sweep_deg += 360 + self._logger.debug( - f"Found azimuth info in JSON: heading={heading_val}, sweep={sweep_val} -> total_deg={az_deg}" + f"Found antenna azimuth in JSON: ant_nav_az={sweep_deg:.1f}°" ) - self._hub.set_antenna_azimuth(az_deg, timestamp=time.monotonic()) + self._hub.set_antenna_azimuth(sweep_deg, timestamp=time.monotonic()) except Exception as e: - self._logger.debug(f"Error processing azimuth values: {e}") - pass + self._logger.debug(f"Error processing antenna azimuth: {e}") else: pass - # self._logger.debug("No heading info found in JSON payload") + # self._logger.debug("No complete antenna azimuth info in JSON") # Optionally capture elevation for future UI use # plat_el = _find_val(["ant_nav_el", "platform_elevation"], obj) diff --git a/target_simulator/gui/ppi_display.py b/target_simulator/gui/ppi_display.py index 83662d7..9402349 100644 --- a/target_simulator/gui/ppi_display.py +++ b/target_simulator/gui/ppi_display.py @@ -696,21 +696,32 @@ class PPIDisplay(ttk.Frame): else: az_float = float(az_deg) - final_az_for_plot = az_float - if self.display_mode_var.get() == "Heading-Up": - # The incoming az_deg is absolute. To display it relative to the - # ownship (which is fixed at 0 deg), we subtract the ownship heading. - final_az_for_plot -= self.ownship_heading_deg + # WORKING SOLUTION confirmed by user: + # North-Up with heading=+10°: + # - Server sends ant_nav_az from +50° to -70° + # - Add 2×heading: result is +70° to -50° ✓ CORRECT + # + # Heading-Up with heading=+10°: + # - Grid is rotated by -10° (via set_theta_offset) + # - Scan lines are drawn at heading ± limit + # - Antenna needs same transformation as scan lines + # + # Solution: use SAME formula for both modes + # - North-Up: add 2×heading + # - Heading-Up: add 2×heading (counter-rotates for the rotated grid) + + final_az_for_plot = az_float + (2.0 * self.ownship_heading_deg) + + # Normalize to [-180, 180] range + while final_az_for_plot > 180: + final_az_for_plot -= 360 + while final_az_for_plot < -180: + final_az_for_plot += 360 # Convert final angle to theta for Matplotlib (0=N, positive=CCW) theta = np.deg2rad(final_az_for_plot) max_r = self.ax.get_ylim()[1] - # logger.debug( - # f"Rendering antenna: az_in={az_deg}, mode={self.display_mode_var.get()}, " - # f"own_hdg={self.ownship_heading_deg}, final_az={final_az_for_plot}, theta={theta}" - # ) - self._antenna_line_artist.set_data([theta, theta], [0, max_r]) self._antenna_line_artist.set_visible(True)