sistemata visualizzazione target che arrivano dal server e fatta inversione azimuth +a sinistra
This commit is contained in:
VALLONGOL
parent
db645b13ce
commit
ebdbbb3de3
@ -107,9 +107,16 @@ class Target:
|
||||
dist_3d, dist_2d = math.sqrt(dx**2 + dy**2 + dz**2), math.sqrt(
|
||||
dx**2 + dy**2
|
||||
)
|
||||
# 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:
|
||||
# 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
|
||||
)
|
||||
# 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:
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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:
|
||||
|
||||
65
tests/test_command_ris_roundtrip.py
Normal file
65
tests/test_command_ris_roundtrip.py
Normal file
@ -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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user