inserito l'inversione di segno nel platform azimuth
This commit is contained in:
parent
d4cd3c0330
commit
a6d679e7a5
@ -2,8 +2,8 @@
|
|||||||
"general": {
|
"general": {
|
||||||
"scan_limit": 60,
|
"scan_limit": 60,
|
||||||
"max_range": 100,
|
"max_range": 100,
|
||||||
"geometry": "1492x992+459+100",
|
"geometry": "1492x992+84+84",
|
||||||
"last_selected_scenario": "scenario_dritto",
|
"last_selected_scenario": "scenario2",
|
||||||
"connection": {
|
"connection": {
|
||||||
"target": {
|
"target": {
|
||||||
"type": "sfp",
|
"type": "sfp",
|
||||||
|
|||||||
@ -20,6 +20,19 @@ from typing import Dict, Deque, Tuple, Optional, List, Any
|
|||||||
# Module-level logger for this module
|
# Module-level logger for this module
|
||||||
logger = logging.getLogger(__name__)
|
logger = logging.getLogger(__name__)
|
||||||
|
|
||||||
|
|
||||||
|
def _invert_azimuth_deg(az_deg: Optional[float]) -> Optional[float]:
|
||||||
|
"""Invert azimuth sign (deg) and normalize to [0,360).
|
||||||
|
|
||||||
|
Returns None if input is None or invalid.
|
||||||
|
"""
|
||||||
|
try:
|
||||||
|
if az_deg is None:
|
||||||
|
return None
|
||||||
|
return (-(float(az_deg))) % 360
|
||||||
|
except Exception:
|
||||||
|
return None
|
||||||
|
|
||||||
# A state tuple can contain (timestamp, x, y, z, vx, vy, vz, ...)
|
# A state tuple can contain (timestamp, x, y, z, vx, vy, vz, ...)
|
||||||
# For now, we focus on timestamp and position in feet.
|
# For now, we focus on timestamp and position in feet.
|
||||||
TargetState = Tuple[float, float, float, float]
|
TargetState = Tuple[float, float, float, float]
|
||||||
@ -85,6 +98,8 @@ class SimulationStateHub:
|
|||||||
# These are optional and used by the GUI to render antenna orientation.
|
# These are optional and used by the GUI to render antenna orientation.
|
||||||
self._antenna_azimuth_deg = None
|
self._antenna_azimuth_deg = None
|
||||||
self._antenna_azimuth_ts = None
|
self._antenna_azimuth_ts = None
|
||||||
|
# Keep the raw antenna/platform azimuth value when available
|
||||||
|
self._antenna_azimuth_raw = None
|
||||||
|
|
||||||
def add_simulated_state(
|
def add_simulated_state(
|
||||||
self, target_id: int, timestamp: float, state: Tuple[float, ...]
|
self, target_id: int, timestamp: float, state: Tuple[float, ...]
|
||||||
@ -274,12 +289,23 @@ class SimulationStateHub:
|
|||||||
"""
|
"""
|
||||||
try:
|
try:
|
||||||
ts = float(timestamp) if timestamp is not None else time.monotonic()
|
ts = float(timestamp) if timestamp is not None else time.monotonic()
|
||||||
az = float(azimuth_deg) % 360
|
# Apply platform azimuth inversion: many external sources use an
|
||||||
|
# opposite angular convention. Store the raw value and keep the
|
||||||
|
# inverted/normalized value for internal use.
|
||||||
|
raw = None
|
||||||
|
try:
|
||||||
|
raw = float(azimuth_deg)
|
||||||
|
except Exception:
|
||||||
|
raw = None
|
||||||
|
az_inverted = _invert_azimuth_deg(raw)
|
||||||
|
if az_inverted is None:
|
||||||
|
return
|
||||||
except Exception:
|
except Exception:
|
||||||
return
|
return
|
||||||
with self._lock:
|
with self._lock:
|
||||||
self._antenna_azimuth_deg = az
|
self._antenna_azimuth_deg = az_inverted
|
||||||
self._antenna_azimuth_ts = ts
|
self._antenna_azimuth_ts = ts
|
||||||
|
self._antenna_azimuth_raw = raw
|
||||||
|
|
||||||
def get_antenna_azimuth(self) -> Tuple[Optional[float], Optional[float]]:
|
def get_antenna_azimuth(self) -> Tuple[Optional[float], Optional[float]]:
|
||||||
"""
|
"""
|
||||||
@ -445,8 +471,34 @@ class SimulationStateHub:
|
|||||||
state: A dictionary containing ownship state information, e.g.,
|
state: A dictionary containing ownship state information, e.g.,
|
||||||
{'position_xy_ft': (x, y), 'heading_deg': 90.0}.
|
{'position_xy_ft': (x, y), 'heading_deg': 90.0}.
|
||||||
"""
|
"""
|
||||||
|
# Apply normalization/inversion for platform azimuth/heading in a
|
||||||
|
# central place to keep the rest of the codebase consistent. Preserve
|
||||||
|
# the raw value for debugging and archival purposes (avoid side-effects).
|
||||||
|
try:
|
||||||
|
st = dict(state) if state is not None else {}
|
||||||
|
except Exception:
|
||||||
|
st = {}
|
||||||
|
|
||||||
|
# Handle common heading/azimuth keys: 'heading_deg' is used by many callers.
|
||||||
|
try:
|
||||||
|
if "heading_deg" in st:
|
||||||
|
raw_h = st.get("heading_deg")
|
||||||
|
try:
|
||||||
|
raw_h_f = float(raw_h)
|
||||||
|
except Exception:
|
||||||
|
raw_h_f = None
|
||||||
|
inv = _invert_azimuth_deg(raw_h_f)
|
||||||
|
if inv is not None:
|
||||||
|
# preserve raw
|
||||||
|
st["heading_deg_raw"] = raw_h_f
|
||||||
|
st["heading_deg"] = inv
|
||||||
|
except Exception:
|
||||||
|
# Do not allow normalization logic to raise
|
||||||
|
pass
|
||||||
|
|
||||||
with self._lock:
|
with self._lock:
|
||||||
self._ownship_state.update(state)
|
# Merge into stored ownship state
|
||||||
|
self._ownship_state.update(st)
|
||||||
|
|
||||||
def get_ownship_state(self) -> Dict[str, Any]:
|
def get_ownship_state(self) -> Dict[str, Any]:
|
||||||
"""
|
"""
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user