sistemato movimento antenna tra north-up e heading up anche con heading aereo diverso da 0
This commit is contained in:
VALLONGOL
parent
14c0501451
commit
4fed8dc983
@ -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 sweep_rad is not None:
|
||||
try:
|
||||
sweep_deg = math.degrees(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())
|
||||
# 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)
|
||||
s_rad = float(sweep_val)
|
||||
|
||||
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
|
||||
# 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
|
||||
|
||||
az_deg = math.degrees(total_az_rad)
|
||||
# LOGGARE IL SUCCESSO PER DEBUG
|
||||
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)
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user