961 lines
48 KiB
Python
961 lines
48 KiB
Python
import threading
|
|
import time
|
|
import socket
|
|
import ctypes
|
|
import queue
|
|
import struct
|
|
import os
|
|
import logging
|
|
from dataclasses import dataclass, field
|
|
from typing import Dict, Type, Optional, Callable, List
|
|
from types import SimpleNamespace
|
|
from .monitor import MonDecoded, MonitorInfo, MonitorBuffer
|
|
from ..lib1553.message_base import BaseMessage
|
|
from ..lib1553.messages.a1_settings import MsgA1Payload
|
|
from ..lib1553.messages.a2_operation_command import MsgA2Payload
|
|
from ..lib1553.messages.a3_graphic_setting import MsgA3Payload
|
|
from ..lib1553.messages.a4_nav_data_and_cursor import MsgA4Payload
|
|
from ..lib1553.messages.a7_data_link_targets_1 import MsgA7Payload
|
|
from ..lib1553.messages.a8_data_link_targets_2 import MsgA8Payload
|
|
from ..lib1553.messages.inu_high_speed import MsgInuHighSpeedPayload
|
|
from ..lib1553.messages.b2_tws_targets_3_4_5 import MsgB2Payload
|
|
from ..lib1553.messages.b3_tws_targets_6_7_8 import MsgB3Payload
|
|
from ..lib1553.messages.b4_spt_target import MsgB4Payload
|
|
from ..lib1553.messages.b8_bit_report import MsgB8Payload
|
|
from ..lib1553.messages.tws_status_and_targets import TwsStatusAndTargets0102
|
|
from ..lib1553.messages.tracked_target import TrackedTarget01
|
|
from ..lib1553.messages.msg_rdr_settings_and_parameters_tellback import MsgRdrSettingsAndParametersTellback
|
|
from ..lib1553.messages.msg_rdr_status_tellback import MsgRdrStatusTellback
|
|
from ..lib1553.structures import Udp1553Header, Udp1553Message, CommandWord
|
|
from ..lib1553.constants import Marker
|
|
from .introspection import structure_to_dict
|
|
|
|
|
|
# MonDecoded and MonitorInfo are provided by pymsc.core.monitor
|
|
|
|
class AppController:
|
|
def __init__(self):
|
|
self._running = threading.Event()
|
|
self._lock = threading.Lock()
|
|
# Debug flag: when True, receiver prints raw packets and sender addr
|
|
self.debug = True
|
|
|
|
self.messages: Dict[str, BaseMessage] = {}
|
|
self.rx_map: Dict[int, Type[ctypes.Structure]] = {}
|
|
self.monitor_queue = queue.Queue()
|
|
# Per-SA statistics for Bus Monitor summary
|
|
# Keyed by subaddress int -> dict: {name, sa, count, errs, last_sw, last_err, last_time, period_ms, wc, rt}
|
|
self.stats: Dict[int, dict] = {}
|
|
|
|
self._scheduler_thread = None
|
|
self._receiver_thread = None
|
|
|
|
# Monitor buffer (replica comportamento C++ AvbDriverUDP)
|
|
# Use the centralized MonitorBuffer class in pymsc.core.monitor
|
|
self.monitor = MonitorBuffer(buf_size=512)
|
|
# Flag to control whether periodic transmissions are actually sent.
|
|
# Mirrors C++ behaviour where the driver only starts on a explicit `go()`.
|
|
self._sending_enabled = False
|
|
|
|
# MODIFICA 1: Bind su 0.0.0.0 per ascoltare da qualsiasi interfaccia di rete
|
|
# Se il radar invia dati alla tua macchina, li riceverai indipendentemente dall'IP.
|
|
self.udp_ip = "0.0.0.0"
|
|
self.udp_send_port = 51553 # Porta per INVIARE comandi al radar
|
|
self.udp_recv_port = 61553 # Porta per RICEVERE dati dal radar (bus monitor)
|
|
self._sock = None
|
|
|
|
# Flag per controllare se il radar è stato inizializzato
|
|
self.radar_initialized = False
|
|
self.radar_dest_ip = "127.0.0.1" # IP destinazione radar
|
|
|
|
self._init_messages()
|
|
|
|
# Percorsi dei file di log
|
|
try:
|
|
repo_root = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', '..'))
|
|
except Exception:
|
|
repo_root = os.getcwd()
|
|
self.logs_dir = os.path.join(repo_root, 'logs')
|
|
self.debug_log_path = os.path.join(self.logs_dir, 'app_debug.log')
|
|
self.rx_log_path = os.path.join(self.logs_dir, 'rx_messages.log')
|
|
self.tx_log_path = os.path.join(self.logs_dir, 'tx_messages.log')
|
|
self.raw_log_path = os.path.join(self.logs_dir, 'udp_raw.log')
|
|
|
|
# Frame counter per UDP1553Header (incrementato ad ogni frame inviato)
|
|
self.frame_counter = 0
|
|
# Diagnostic dumps limit to avoid flooding logs during normal operation
|
|
self._diag_dump_remaining = 50
|
|
self._diag_log_path = os.path.join(self.logs_dir, 'diag_B_parsing.log')
|
|
# Temporary raw packet dump budget for debugging reception (disabled by default)
|
|
self._raw_dump_remaining = 20
|
|
self._raw_log_path = os.path.join(self.logs_dir, 'udp_raw.log')
|
|
|
|
def _setup_logging(self):
|
|
"""
|
|
Configura il sistema di logging con file azzerati ad ogni avvio.
|
|
Crea 3 file di log nella cartella logs/:
|
|
- app_debug.log: Log generale dell'applicazione
|
|
- rx_messages.log: Messaggi ricevuti dal radar
|
|
- tx_messages.log: Messaggi inviati al radar
|
|
"""
|
|
try:
|
|
os.makedirs(self.logs_dir, exist_ok=True)
|
|
|
|
# Azzera i file di log ad ogni avvio (mode='w')
|
|
with open(self.debug_log_path, 'w', encoding='utf-8') as f:
|
|
f.write(f"=== PyMsc Debug Log - Started {time.strftime('%Y-%m-%d %H:%M:%S')} ===\n")
|
|
|
|
with open(self.rx_log_path, 'w', encoding='utf-8') as f:
|
|
f.write(f"=== RX Messages Log - Started {time.strftime('%Y-%m-%d %H:%M:%S')} ===\n")
|
|
|
|
with open(self.tx_log_path, 'w', encoding='utf-8') as f:
|
|
f.write(f"=== TX Messages Log - Started {time.strftime('%Y-%m-%d %H:%M:%S')} ===\n")
|
|
|
|
with open(self.raw_log_path, 'w', encoding='utf-8') as f:
|
|
f.write(f"=== UDP Raw Packets Log - Started {time.strftime('%Y-%m-%d %H:%M:%S')} ===\n")
|
|
|
|
print(f"Core: Logging initialized in {self.logs_dir}")
|
|
self._log_debug("Application started")
|
|
|
|
except Exception as e:
|
|
print(f"Warning: Could not setup logging: {e}")
|
|
|
|
def _diag_dump(self, text: str):
|
|
"""Write a diagnostic line to the diag log and decrement remaining counter."""
|
|
try:
|
|
if getattr(self, '_diag_dump_remaining', 0) <= 0:
|
|
return
|
|
# ensure logs dir exists
|
|
os.makedirs(self.logs_dir, exist_ok=True)
|
|
with open(self._diag_log_path, 'a', encoding='utf-8') as f:
|
|
f.write(f"[{time.strftime('%H:%M:%S')}] {text}\n")
|
|
# also print to console for immediate feedback
|
|
print(f"[DIAG] {text}")
|
|
self._diag_dump_remaining -= 1
|
|
except Exception:
|
|
pass
|
|
|
|
def _log_debug(self, message):
|
|
"""Scrive nel log di debug."""
|
|
try:
|
|
with open(self.debug_log_path, 'a', encoding='utf-8') as f:
|
|
f.write(f"[{time.strftime('%H:%M:%S.%f')[:-3]}] {message}\n")
|
|
except Exception:
|
|
pass
|
|
|
|
def _log_rx(self, message):
|
|
"""Scrive nel log dei messaggi ricevuti."""
|
|
try:
|
|
with open(self.rx_log_path, 'a', encoding='utf-8') as f:
|
|
f.write(f"[{time.strftime('%H:%M:%S.%f')[:-3]}] {message}\n")
|
|
except Exception:
|
|
pass
|
|
|
|
def _log_tx(self, message):
|
|
"""Scrive nel log dei messaggi trasmessi."""
|
|
try:
|
|
with open(self.tx_log_path, 'a', encoding='utf-8') as f:
|
|
f.write(f"[{time.strftime('%H:%M:%S.%f')[:-3]}] {message}\n")
|
|
except Exception:
|
|
pass
|
|
|
|
def register_message(self, message: BaseMessage):
|
|
with self._lock:
|
|
self.messages[message.label] = message
|
|
# Registra la mappatura per la ricezione (usa il SubAddress del CW)
|
|
sa = message.cw.fields.sa
|
|
self.rx_map[sa] = message.payload.__class__
|
|
|
|
# --- Bus Monitor API (replicate dal driver C++)
|
|
def set_mon_isr(self, callback: Callable[[object], None]):
|
|
"""Register a callback to be invoked when monitor buffer reaches threshold.
|
|
|
|
The callback will be invoked with a snapshot-like object containing
|
|
`buffer` (list of MonDecoded), `total_message` and `processed_message`.
|
|
Invocation is performed in a daemon thread to avoid blocking receiver.
|
|
"""
|
|
# Delegate to MonitorBuffer
|
|
self.monitor.set_isr(callback)
|
|
|
|
def enableMonitorIsr(self, enable: bool = True):
|
|
"""Enable or disable the monitor ISR callback invocation."""
|
|
self.monitor.enable_isr(enable)
|
|
|
|
def getMonitorRawBuffer(self):
|
|
"""Return a snapshot of the current monitor buffer and reset internal storage.
|
|
|
|
Returns an object with attributes: `buffer` (list of MonDecoded),
|
|
`total_message` (int) and `processed_message` (int).
|
|
"""
|
|
return self.monitor.get_raw_buffer()
|
|
|
|
# --- Control methods to mirror legacy driver start/stop behaviour
|
|
def go(self, enable_monitor: bool = True):
|
|
"""Enable periodic sending of messages and (optionally) the monitor ISR.
|
|
|
|
This mirrors the C++ `avbDriver->go()` behaviour: messages are configured
|
|
by `initialize_radar()` but actual periodic transmission starts only when
|
|
`go()` is invoked.
|
|
"""
|
|
self._sending_enabled = True
|
|
if enable_monitor:
|
|
try:
|
|
self.enableMonitorIsr(True)
|
|
except Exception:
|
|
pass
|
|
self._log_debug("Core: go() invoked - sending enabled")
|
|
return True
|
|
|
|
def stop_sending(self):
|
|
"""Disable periodic sending and (optionally) the monitor ISR."""
|
|
self._sending_enabled = False
|
|
try:
|
|
self.enableMonitorIsr(False)
|
|
except Exception:
|
|
pass
|
|
self._log_debug("Core: stop_sending() invoked - sending disabled")
|
|
return True
|
|
|
|
def go_mon(self):
|
|
"""Enable only the monitor ISR (replicates avbDriver->go_mon)."""
|
|
return self.enableMonitorIsr(True)
|
|
|
|
def stop_mon(self):
|
|
"""Disable only the monitor ISR."""
|
|
return self.enableMonitorIsr(False)
|
|
|
|
def start(self):
|
|
print(f"Core: Starting services (Listening on {self.udp_ip}:{self.udp_recv_port})...")
|
|
self._running.set()
|
|
|
|
# Setup logging con file azzerato ad ogni avvio
|
|
self._setup_logging()
|
|
|
|
self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
try:
|
|
self._sock.bind((self.udp_ip, self.udp_recv_port))
|
|
except OSError as e:
|
|
print(f"Error binding socket: {e}")
|
|
|
|
self._scheduler_thread = threading.Thread(target=self._scheduler_loop, daemon=True)
|
|
self._scheduler_thread.start()
|
|
|
|
self._receiver_thread = threading.Thread(target=self._receiver_loop, daemon=True)
|
|
self._receiver_thread.start()
|
|
|
|
def stop(self):
|
|
print("Core: Stopping services...")
|
|
self._running.clear()
|
|
if self._sock:
|
|
self._sock.close()
|
|
if self._scheduler_thread:
|
|
self._scheduler_thread.join(timeout=1.0)
|
|
if self._receiver_thread:
|
|
self._receiver_thread.join(timeout=1.0)
|
|
|
|
def initialize_radar(self):
|
|
"""
|
|
Invia messaggi di inizializzazione al radar.
|
|
Replica il comportamento di test_1553.py per configurare il radar.
|
|
"""
|
|
print("Core: Initializing radar...")
|
|
self._log_debug("=== RADAR INITIALIZATION STARTED ===")
|
|
|
|
# Importa gli enum necessari
|
|
from ..lib1553.datatypes.enums import TargetHistory, RdrModes
|
|
|
|
try:
|
|
# A1 - Settings and Parameters
|
|
a1 = self.messages.get('A1')
|
|
if a1:
|
|
# Imposta valori di default come in test_1553.py
|
|
# Nota: nel nuovo codice l'enum è LEVEL_04 invece di TARGET_HISTORY_LEVEL_04
|
|
a1.payload.settings.bits.target_history = TargetHistory.LEVEL_04
|
|
a1.payload.settings.bits.symbology_intensity = 127
|
|
self._send_single_message(a1)
|
|
self._log_tx("INIT: A1 (Settings) sent - history=LEVEL_04, intensity=127")
|
|
time.sleep(0.05)
|
|
print(" → A1 (Settings) sent")
|
|
|
|
# A2 - Operation Command
|
|
a2 = self.messages.get('A2')
|
|
if a2:
|
|
# Imposta master mode RWS come in test_1553.py
|
|
a2.payload.rdr_mode_command.bits.master_mode = RdrModes.RWS
|
|
# Imposta param1 e param2 come in test_1553.py
|
|
if hasattr(a2.payload, 'param1'):
|
|
a2.payload.param1.raw = 0 # set_gm_submode(0)
|
|
if hasattr(a2.payload, 'param2'):
|
|
a2.payload.param2.raw = 0 # set_spare_0_4(0)
|
|
self._send_single_message(a2)
|
|
time.sleep(0.05)
|
|
print(" → A2 (Operation Command) sent")
|
|
|
|
# A3 - Graphic Setting
|
|
a3 = self.messages.get('A3')
|
|
if a3:
|
|
self._send_single_message(a3)
|
|
time.sleep(0.05)
|
|
print(" → A3 (Graphic Setting) sent")
|
|
|
|
# A4 - Nav Data and Cursor
|
|
a4 = self.messages.get('A4')
|
|
if a4:
|
|
a4.payload.validity_and_slew.raw = 0
|
|
self._send_single_message(a4)
|
|
time.sleep(0.05)
|
|
print(" → A4 (Nav Data) sent")
|
|
|
|
# A5 - INU High Speed
|
|
a5 = self.messages.get('A5')
|
|
if a5:
|
|
a5.payload.timetag.raw = 0
|
|
self._send_single_message(a5)
|
|
time.sleep(0.05)
|
|
print(" → A5 (INU High Speed) sent")
|
|
|
|
# IMPORTANTE: Attiva invio periodico per mantenere la connessione
|
|
# Imposta le frequenze come nel vecchio test_1553.py
|
|
print("Core: Enabling periodic transmission...")
|
|
self._log_debug("Enabling periodic message transmission...")
|
|
self._enable_periodic_transmission()
|
|
|
|
self.radar_initialized = True
|
|
self._log_debug("=== RADAR INITIALIZATION COMPLETE ===")
|
|
print("Core: Radar initialization complete!")
|
|
return True
|
|
|
|
except Exception as e:
|
|
print(f"Core: Error initializing radar: {e}")
|
|
self._log_debug(f"ERROR during radar initialization: {e}")
|
|
import traceback
|
|
traceback.print_exc()
|
|
return False
|
|
|
|
def _send_single_message(self, message: BaseMessage):
|
|
"""
|
|
Invia un singolo messaggio al radar.
|
|
"""
|
|
frame = self._prepare_frame([message])
|
|
self._send_udp(frame, self.radar_dest_ip)
|
|
|
|
def _enable_periodic_transmission(self):
|
|
"""
|
|
Attiva la trasmissione periodica dei messaggi per mantenere la connessione con il radar.
|
|
Imposta le frequenze come nel vecchio test_1553.py:
|
|
- A1: 10 Hz (ogni 100ms)
|
|
- A2: 25 Hz (ogni 40ms)
|
|
- A3: 10 Hz (ogni 100ms)
|
|
- A4: 50 Hz (ogni 20ms)
|
|
- A5: 50 Hz (ogni 20ms)
|
|
"""
|
|
with self._lock:
|
|
# Imposta i periodi in millisecondi
|
|
if 'A1' in self.messages:
|
|
self.messages['A1'].period_ms = 100 # 10 Hz
|
|
print(" → A1: 10 Hz (100ms)")
|
|
|
|
if 'A2' in self.messages:
|
|
self.messages['A2'].period_ms = 40 # 25 Hz
|
|
print(" → A2: 25 Hz (40ms)")
|
|
|
|
if 'A3' in self.messages:
|
|
self.messages['A3'].period_ms = 100 # 10 Hz
|
|
print(" → A3: 10 Hz (100ms)")
|
|
|
|
if 'A4' in self.messages:
|
|
self.messages['A4'].period_ms = 20 # 50 Hz
|
|
print(" → A4: 50 Hz (20ms)")
|
|
|
|
if 'A5' in self.messages:
|
|
self.messages['A5'].period_ms = 20 # 50 Hz
|
|
print(" → A5: 50 Hz (20ms)")
|
|
|
|
def _scheduler_loop(self):
|
|
"""
|
|
Invia messaggi periodici al radar usando la stessa logica del vecchio test_1553.py:
|
|
- Mantiene un contatore manuale di tempo in ms
|
|
- Controlla se mytime % period == 0 per decidere quando inviare
|
|
- Sleep di 20ms per coerenza temporale
|
|
"""
|
|
print("Core: Scheduler started.")
|
|
mytime = 0
|
|
|
|
while self._running.is_set():
|
|
with self._lock:
|
|
active_msgs = list(self.messages.values())
|
|
|
|
# Raggruppa i messaggi che devono essere inviati in questo tick
|
|
due_msgs = []
|
|
for msg in active_msgs:
|
|
if msg.period_ms > 0:
|
|
# Stesso check del vecchio codice: mytime % period == 0
|
|
if mytime % msg.period_ms == 0:
|
|
due_msgs.append(msg)
|
|
|
|
if due_msgs and self._sending_enabled:
|
|
# Aggiorna A5 timetag se presente (come nel vecchio codice)
|
|
a5_msg = next((m for m in due_msgs if m.label == 'A5'), None)
|
|
if a5_msg:
|
|
a5_msg.payload.timetag.raw += 312
|
|
|
|
# Costruiamo un unico frame UDP che contiene tutti i messaggi
|
|
frame = self._prepare_frame(due_msgs)
|
|
# RIMOSSO: logging su file ogni 20ms causa ritardi gravi
|
|
# msg_labels = ", ".join([msg.label for msg in due_msgs])
|
|
# self._log_tx(f"Sending frame with messages: {msg_labels}")
|
|
# Usa radar_dest_ip invece di hardcoded 127.0.0.1
|
|
self._send_udp(frame, self.radar_dest_ip)
|
|
else:
|
|
# If there are due messages but sending is disabled, skip sending.
|
|
# This mimics the legacy driver where `go()` must be called to start.
|
|
if due_msgs and self.debug:
|
|
pass
|
|
|
|
time.sleep(0.02) # 20ms come nel vecchio codice
|
|
mytime += 20 # Incrementa il contatore manuale
|
|
|
|
def _receiver_loop(self):
|
|
"""
|
|
Riceve pacchetti, decodifica header e payload.
|
|
Gestisce anche messaggi sconosciuti (RAW).
|
|
"""
|
|
print("Core: Receiver started.")
|
|
|
|
# Dimensioni fisse degli header definiti in structures.py
|
|
header_size = ctypes.sizeof(Udp1553Header)
|
|
wrapper_size = ctypes.sizeof(Udp1553Message)
|
|
if self.debug:
|
|
print(f"Core: Header size={header_size}, Wrapper size={wrapper_size}")
|
|
|
|
# precompute marker bytes for recovery scanning
|
|
ctrl_begin_bytes = struct.pack("<H", Marker.CTRL_BEGIN)
|
|
ctrl_end_bytes = struct.pack("<H", Marker.CTRL_END)
|
|
end_marker_bytes = struct.pack("<H", Marker.END_1553)
|
|
|
|
# Usa il nuovo sistema di logging già inizializzato in _setup_logging()
|
|
while self._running.is_set():
|
|
try:
|
|
self._sock.settimeout(1.0)
|
|
data, addr = self._sock.recvfrom(4096) # Buffer aumentato
|
|
# Temporary: dump some raw packets for debugging reception issues
|
|
try:
|
|
if getattr(self, '_raw_dump_remaining', 0) > 0:
|
|
os.makedirs(self.logs_dir, exist_ok=True)
|
|
with open(self._raw_log_path, 'a', encoding='utf-8') as rf:
|
|
rf.write(f"[{time.strftime('%H:%M:%S')}] {addr[0]}:{addr[1]} {len(data)} bytes {data[:64].hex()}\n")
|
|
# Also push a short monitor entry so GUI shows we've received something
|
|
try:
|
|
self.monitor_queue.put((time.strftime("%H:%M:%S"), "RAW_RX", f"{addr[0]}:{addr[1]}", f"{len(data)} bytes"))
|
|
except Exception:
|
|
pass
|
|
self._raw_dump_remaining -= 1
|
|
except Exception:
|
|
pass
|
|
|
|
# DEBUG: Salva pacchetti con SA >= 11 (messaggi B) in un file
|
|
if len(data) > 64:
|
|
# Cerca rapidamente se ci sono SA >= 11 nel pacchetto
|
|
for i in range(64, len(data)-4):
|
|
if data[i:i+2] == b'\x3c\x3c': # CTRL_BEGIN
|
|
cw = struct.unpack_from('<H', data, i+2)[0]
|
|
sa = (cw >> 5) & 0x1F
|
|
if sa >= 11: # Messaggio B trovato!
|
|
# Salva il pacchetto per analisi
|
|
debug_path = os.path.join(self.logs_dir, f'packet_with_B_SA{sa}.bin')
|
|
with open(debug_path, 'wb') as f:
|
|
f.write(data)
|
|
print(f"[DEBUG] Packet with B message (SA={sa}) saved to {debug_path}")
|
|
break
|
|
|
|
# RIMOSSO: debug print e file I/O ogni pacchetto causa ritardi gravi
|
|
# if self.debug:
|
|
# print(f"[RX] {len(data)} bytes from {addr}")
|
|
# self._log_rx(f"Received {len(data)} bytes from {addr[0]}:{addr[1]}")
|
|
# with open(self.raw_log_path, 'a', encoding='utf-8') as f:
|
|
# f.write(f"{time.strftime('%Y-%m-%d %H:%M:%S')} {addr[0]}:{addr[1]} {data.hex()}\n")
|
|
|
|
# --- MODIFICA 2: Gestione RAW robusta ---
|
|
|
|
# 1. Validazione minima lunghezza
|
|
if len(data) < header_size + wrapper_size:
|
|
# Pacchetto troppo piccolo o non conforme al protocollo custom
|
|
self.monitor_queue.put((time.strftime("%H:%M:%S"), "INVALID LEN", "RX", data.hex()))
|
|
continue
|
|
|
|
# Parsing Header (Opzionale: possiamo leggere flag, errori, etc)
|
|
# udp_header = Udp1553Header.from_buffer_copy(data[:header_size])
|
|
|
|
# 2. Parsing di eventuali messaggi concatenati nel frame
|
|
offset = header_size
|
|
timestamp = time.strftime("%H:%M:%S")
|
|
|
|
# Helper: cerca il prossimo marker (CTRL_BEGIN/CTRL_END/END_1553)
|
|
def find_next_marker(buf, start=0):
|
|
found = -1
|
|
for marker in (ctrl_begin_bytes, ctrl_end_bytes, end_marker_bytes):
|
|
pos = buf.find(marker, start)
|
|
if pos != -1 and (found == -1 or pos < found):
|
|
found = pos
|
|
return found
|
|
|
|
# Loop finché abbiamo almeno un wrapper disponibile.
|
|
# Support both full wrappers (sizeof(Udp1553Message)) and
|
|
# the short 12-byte variant observed in the capture.
|
|
minimal_wrapper_len = 12
|
|
|
|
# Marker-first parsing: scan the datagram for CTRL_BEGIN markers and
|
|
# attempt to parse either the full Udp1553Message (when available)
|
|
# or the observed short 12-byte wrapper. Accept a short wrapper
|
|
# only when the inverted-CW field matches the bitwise inverse of CW.
|
|
search_offset = offset
|
|
while True:
|
|
# Find next CTRL_BEGIN marker
|
|
next_begin = data.find(ctrl_begin_bytes, search_offset)
|
|
if next_begin == -1:
|
|
break
|
|
offset = next_begin
|
|
|
|
# GENERAL DIAGNOSTIC: if enabled, log marker details (CW raw, expected inverted, positions)
|
|
try:
|
|
if getattr(self, '_diag_dump_remaining', 0) > 0:
|
|
# attempt to read cw_raw and compute related offsets without changing parsing state
|
|
try:
|
|
cw_probe = struct.unpack_from('<H', data, offset+2)[0]
|
|
wc_probe = cw_probe & 0x1F
|
|
tr_probe = (cw_probe >> 10) & 0x1
|
|
payload_len_probe = wc_probe * 2
|
|
inverted_probe_pos = offset + 8 + payload_len_probe
|
|
inverted_probe = None
|
|
ctrl_end_probe = None
|
|
if inverted_probe_pos + 2 <= len(data):
|
|
inverted_probe = struct.unpack_from('<H', data, inverted_probe_pos)[0]
|
|
if inverted_probe_pos + 4 <= len(data):
|
|
ctrl_end_probe = struct.unpack_from('<H', data, inverted_probe_pos+2)[0]
|
|
expected_probe = (~cw_probe) & 0xFFFF
|
|
info = (f"MARKER offset={offset} addr={addr[0]}:{addr[1]} CW=0x{cw_probe:04X} "
|
|
f"sa={(cw_probe>>5)&0x1F} tr={tr_probe} wc={wc_probe} payload_len={payload_len_probe} "
|
|
f"inverted=0x{inverted_probe:04X}" if inverted_probe is not None else
|
|
f"MARKER offset={offset} addr={addr[0]}:{addr[1]} CW=0x{cw_probe:04X} sa={(cw_probe>>5)&0x1F} tr={tr_probe} wc={wc_probe} payload_len={payload_len_probe} inverted=None")
|
|
# We prefer a single-line with expected too
|
|
info2 = f" expected=0x{expected_probe:04X} inv_pos={inverted_probe_pos} ctrl_end={ctrl_end_probe}"
|
|
# write two parts to keep formatting safe
|
|
self._diag_dump(info + info2)
|
|
except Exception:
|
|
# ignore probe failures
|
|
pass
|
|
except Exception:
|
|
pass
|
|
|
|
# Prefer detecting the observed 12-byte short wrapper first
|
|
short_parsed = False
|
|
# Initialize variables that will be used later
|
|
sa = 0
|
|
tr = 0
|
|
rt = 0
|
|
payload_bytes = b''
|
|
payload_len = 0
|
|
msg_wrapper = None
|
|
|
|
if offset + minimal_wrapper_len <= len(data):
|
|
try:
|
|
cw_raw = struct.unpack_from('<H', data, offset+2)[0]
|
|
# Extract WC from CW to determine payload size
|
|
wc_field = cw_raw & 0x1F
|
|
# In MIL-STD-1553: WC=0 means 32 words, but here WC=0 means 0 words
|
|
# Based on captured packets, WC=0 → no payload, WC=N → N words (2*N bytes)
|
|
word_count = wc_field # NOT "32 if wc_field == 0 else wc_field"
|
|
payload_len = word_count * 2
|
|
|
|
# inverted_cw and ctrl_end are AFTER the payload
|
|
inverted_cw_pos = offset + 8 + payload_len
|
|
ctrl_end_pos = inverted_cw_pos + 2
|
|
|
|
if ctrl_end_pos + 2 <= len(data):
|
|
inverted_cw = struct.unpack_from('<H', data, inverted_cw_pos)[0]
|
|
ctrl_end = struct.unpack_from('<H', data, ctrl_end_pos)[0]
|
|
else:
|
|
inverted_cw = None
|
|
ctrl_end = None
|
|
except Exception:
|
|
cw_raw = None
|
|
inverted_cw = None
|
|
ctrl_end = None
|
|
|
|
if cw_raw is not None and inverted_cw is not None and ctrl_end == Marker.CTRL_END:
|
|
expected_inverted = (~cw_raw) & 0xFFFF
|
|
# DEBUG: mostra validazione
|
|
sa_check = (cw_raw >> 5) & 0x1F
|
|
if sa_check >= 11:
|
|
print(f"[PARSER] Offset={offset}, SA={sa_check}, CW=0x{cw_raw:04X}, inverted=0x{inverted_cw:04X}, expected=0x{expected_inverted:04X}, ctrl_end=0x{ctrl_end:04X}, match={inverted_cw == expected_inverted}")
|
|
if getattr(self, '_diag_dump_remaining', 0) > 0:
|
|
info = (f"SHORT offset={offset} addr={addr[0]}:{addr[1]} SA={sa_check} "
|
|
f"CW=0x{cw_raw:04X} inverted=0x{inverted_cw:04X} expected=0x{expected_inverted:04X} "
|
|
f"inverted_pos={inverted_cw_pos} payload_len={payload_len}")
|
|
self._diag_dump(info)
|
|
if inverted_cw == expected_inverted:
|
|
# Valid short wrapper -> build minimal wrapper-like object
|
|
# Create a simple object to hold CW, SW, error_code
|
|
sa_check = (cw_raw >> 5) & 0x1F
|
|
if sa_check >= 11:
|
|
print(f"[INSIDE_IF] SA={sa_check}, about to create SimpleNamespace")
|
|
msg_wrapper = SimpleNamespace()
|
|
if sa_check >= 11:
|
|
print(f"[AFTER_NS] Created SimpleNamespace, creating CommandWord")
|
|
cw_obj = CommandWord()
|
|
if sa_check >= 11:
|
|
print(f"[AFTER_CW] Created CommandWord, setting raw={cw_raw:04X}")
|
|
cw_obj.raw = cw_raw
|
|
if sa_check >= 11:
|
|
print(f"[AFTER_RAW] Set cw_obj.raw successfully")
|
|
msg_wrapper.cw = cw_obj
|
|
if sa_check >= 11:
|
|
print(f"[AFTER_ASSIGN] Assigned cw to msg_wrapper")
|
|
msg_wrapper.sw = struct.unpack_from('<H', data, offset+4)[0]
|
|
msg_wrapper.error_code = struct.unpack_from('<H', data, offset+6)[0]
|
|
|
|
# Extract payload if present
|
|
if payload_len > 0:
|
|
payload_bytes = data[offset+8:offset+8+payload_len]
|
|
else:
|
|
payload_bytes = b''
|
|
|
|
if sa_check >= 11:
|
|
print(f"[BEFORE_FIELDS] About to access msg_wrapper.cw.fields")
|
|
sa = msg_wrapper.cw.fields.sa
|
|
if sa_check >= 11:
|
|
print(f"[GOT_SA] sa={sa}")
|
|
tr = msg_wrapper.cw.fields.tr
|
|
rt = msg_wrapper.cw.fields.rt
|
|
|
|
# DEBUG: Verifica che arriviamo qui
|
|
if sa >= 11:
|
|
print(f"[SHORT_PARSED] SA={sa}, offset advanced to {offset + 8 + payload_len + 4}")
|
|
|
|
# Advance: 8 (wrapper) + payload + 2 (inverted) + 2 (ctrl_end)
|
|
offset = offset + 8 + payload_len + 4
|
|
short_parsed = True
|
|
|
|
if not short_parsed:
|
|
# Fall back to attempting full-wrapper parse
|
|
if offset + wrapper_size <= len(data):
|
|
try:
|
|
msg_wrapper = Udp1553Message.from_buffer_copy(data[offset:offset+wrapper_size])
|
|
except Exception:
|
|
# malformed full wrapper -> skip this marker
|
|
search_offset = offset + 1
|
|
continue
|
|
|
|
# extract CW-derived payload length
|
|
try:
|
|
wc_field = msg_wrapper.cw.fields.wc
|
|
# Treat WC=0 as 0 words for consistency with short-wrapper parsing
|
|
# (captured packets use WC=0 -> no payload)
|
|
word_count = int(wc_field)
|
|
payload_len = word_count * 2
|
|
except Exception:
|
|
payload_len = 0
|
|
|
|
# compute expected end positions
|
|
expected_end = offset + wrapper_size + payload_len + 4
|
|
if expected_end <= len(data):
|
|
# read payload + inverted + footer
|
|
payload_bytes = data[offset+wrapper_size: offset+wrapper_size+payload_len]
|
|
inverted_cw = struct.unpack_from('<H', data, offset+wrapper_size+payload_len)[0]
|
|
ctrl_end = struct.unpack_from('<H', data, offset+wrapper_size+payload_len+2)[0]
|
|
# advance offset past this message
|
|
offset = offset + wrapper_size + payload_len + 4
|
|
else:
|
|
# not enough bytes for full message; skip this marker and continue
|
|
search_offset = offset + 1
|
|
continue
|
|
|
|
# basic validations
|
|
try:
|
|
cw_raw = int(msg_wrapper.cw.raw)
|
|
expected_inverted = (~cw_raw) & 0xFFFF
|
|
except Exception:
|
|
expected_inverted = None
|
|
|
|
if expected_inverted is not None and inverted_cw != expected_inverted:
|
|
# invalid -> report and continue searching after this marker
|
|
try:
|
|
sa_full = getattr(msg_wrapper.cw.fields, 'sa', 0)
|
|
except Exception:
|
|
sa_full = 0
|
|
if sa_full >= 11 and getattr(self, '_diag_dump_remaining', 0) > 0:
|
|
info = (f"FULL_BAD offset={offset} addr={addr[0]}:{addr[1]} SA={sa_full} "
|
|
f"CW=0x{int(msg_wrapper.cw.raw):04X} inverted=0x{inverted_cw:04X} expected=0x{expected_inverted:04X} "
|
|
f"payload_len={payload_len} ctrl_end=0x{ctrl_end:04X}")
|
|
self._diag_dump(info)
|
|
self.monitor_queue.put((timestamp, f"A{getattr(msg_wrapper.cw.fields, 'sa', 0)}", "BC->RT", f"Bad inverted CW (0x{inverted_cw:04X} != 0x{expected_inverted:04X})"))
|
|
search_offset = offset
|
|
continue
|
|
|
|
if ctrl_end != Marker.CTRL_END:
|
|
self.monitor_queue.put((timestamp, f"A{getattr(msg_wrapper.cw.fields, 'sa', 0)}", "BC->RT", "Malformed footer"))
|
|
search_offset = offset
|
|
continue
|
|
|
|
# At this point treat as a valid full wrapper message and fall through
|
|
sa = msg_wrapper.cw.fields.sa if hasattr(msg_wrapper.cw, 'fields') else 0
|
|
tr = msg_wrapper.cw.fields.tr if hasattr(msg_wrapper.cw, 'fields') else 0
|
|
rt = msg_wrapper.cw.fields.rt if hasattr(msg_wrapper.cw, 'fields') else 0
|
|
word_count = word_count
|
|
else:
|
|
# Not enough bytes for either short or full wrapper: advance search
|
|
search_offset = offset + 1
|
|
continue
|
|
|
|
# At this point we have: msg_wrapper, sa, tr, rt, payload_bytes, payload_len, and offset advanced
|
|
# Update search_offset to continue from current offset
|
|
search_offset = offset
|
|
|
|
# DEBUG: Stampa messaggi B (telemetria dal radar)
|
|
if sa >= 11 and sa <= 28:
|
|
print(f"[B MESSAGE] SA={sa} (B{sa-10}), TR={tr}, RT={rt}, payload_len={payload_len}, direction={'RT->BC' if tr==1 else 'BC->RT'}")
|
|
|
|
# Build human readable descriptors and push to monitor as before
|
|
try:
|
|
sw_hex = f"0x{int(getattr(msg_wrapper, 'sw', 0)):04X}"
|
|
except Exception:
|
|
sw_hex = "0x0000"
|
|
try:
|
|
err_hex = f"0x{int(getattr(msg_wrapper, 'error_code', 0)):04X}"
|
|
except Exception:
|
|
err_hex = "0x0000"
|
|
|
|
hex_str = payload_bytes.hex().upper()
|
|
spaced = " ".join(hex_str[i:i+2] for i in range(0, len(hex_str), 2))
|
|
short = spaced if len(spaced) <= 120 else spaced[:120] + "..."
|
|
|
|
# Determine decoded note if possible
|
|
decoded_note = ""
|
|
if sa in self.rx_map:
|
|
payload_cls = self.rx_map[sa]
|
|
struct_size = ctypes.sizeof(payload_cls)
|
|
if struct_size == payload_len and payload_len > 0:
|
|
decoded_note = f"Decoded ({struct_size} bytes)"
|
|
try:
|
|
payload_obj = payload_cls.from_buffer_copy(payload_bytes)
|
|
payload_dict = structure_to_dict(payload_obj)
|
|
import json
|
|
pretty = json.dumps(payload_dict, ensure_ascii=False)
|
|
if len(pretty) > 200:
|
|
pretty = pretty[:200] + '...'
|
|
except Exception as e:
|
|
pretty = f"<decode error: {e}>"
|
|
else:
|
|
decoded_note = f"Payload ({payload_len} bytes)"
|
|
|
|
# Generate correct label: A1..A8 for SA 1..8, B1..B18 for SA 11..28
|
|
if sa in self.rx_map:
|
|
if 1 <= sa <= 8:
|
|
label = f"A{sa}"
|
|
elif 11 <= sa <= 28:
|
|
label = f"B{sa-10}"
|
|
else:
|
|
label = f"UNKNOWN (SA={sa})"
|
|
else:
|
|
label = f"UNKNOWN (SA={sa})"
|
|
dir_str = "RT->BC" if tr == 1 else "BC->RT"
|
|
desc = f"CW(sa={sa},rt={rt},wc={payload_len//2}) SW={sw_hex} ERR={err_hex} {decoded_note}"
|
|
|
|
# Diagnostic: log when we enqueue B messages to monitor_queue
|
|
try:
|
|
if 11 <= sa <= 28 and getattr(self, '_diag_dump_remaining', 0) > 0:
|
|
self._diag_dump(f"ENQUEUED label={('B'+str(sa-10))} dir={dir_str} desc={desc}")
|
|
except Exception:
|
|
pass
|
|
|
|
if sa in self.rx_map and payload_len > 0 and ctypes.sizeof(self.rx_map[sa]) == payload_len and 'pretty' in locals():
|
|
self.monitor_queue.put((timestamp, label, dir_str, pretty))
|
|
# RIMOSSO: debug print e file I/O causa ritardi
|
|
# if self.debug:
|
|
# print(f"[QUEUE] Added {label} (decoded) to monitor_queue")
|
|
# self._log_rx(f"{label} {dir_str} - Decoded payload: {pretty[:100]}")
|
|
else:
|
|
self.monitor_queue.put((timestamp, label, dir_str, desc))
|
|
# RIMOSSO: debug print e file I/O causa ritardi
|
|
# if self.debug:
|
|
# print(f"[QUEUE] Added {label} (raw) to monitor_queue: {desc[:50]}")
|
|
# self._log_rx(f"{label} {dir_str} - {desc}")
|
|
|
|
self.monitor_queue.put((timestamp, f"FROM {addr[0]}:{addr[1]}", dir_str, short))
|
|
# RIMOSSO: file I/O causa ritardi
|
|
# self._log_rx(f" Payload hex: {short[:200]}")
|
|
|
|
# --- Popola il buffer monitor strutturato (replica C++ mon.msg)
|
|
try:
|
|
mon_entry = MonDecoded(
|
|
cw_raw=int(getattr(msg_wrapper.cw, 'raw', 0)),
|
|
sw=int(getattr(msg_wrapper, 'sw', 0)),
|
|
error_code=int(getattr(msg_wrapper, 'error_code', 0)),
|
|
sa=int(sa),
|
|
tr=int(tr),
|
|
rt=int(rt),
|
|
wc=int(payload_len//2),
|
|
data=payload_bytes,
|
|
timetag=time.time(),
|
|
)
|
|
# Delegate buffer handling to MonitorBuffer
|
|
try:
|
|
self.monitor.append(mon_entry)
|
|
except Exception:
|
|
# Do not block receiver on monitor buffer errors
|
|
pass
|
|
except Exception:
|
|
# Non blocchiamo il receiver per errori nel buffer construction
|
|
pass
|
|
|
|
# Update stats
|
|
now = time.time()
|
|
stat = self.stats.get(sa, {
|
|
"name": label,
|
|
"sa": sa,
|
|
"count": 0,
|
|
"errs": 0,
|
|
"last_sw": None,
|
|
"last_err": None,
|
|
"last_time": None,
|
|
"period_ms": None,
|
|
"period_samples": [], # Rolling window of last N intervals
|
|
"wc": payload_len//2,
|
|
"rt": rt,
|
|
})
|
|
stat["count"] += 1
|
|
try:
|
|
stat["last_sw"] = int(getattr(msg_wrapper, 'sw', 0))
|
|
except Exception:
|
|
stat["last_sw"] = None
|
|
stat["last_err"] = int(getattr(msg_wrapper, 'error_code', 0))
|
|
if stat["last_time"] is not None:
|
|
dt = (now - stat["last_time"]) * 1000.0
|
|
# Only update period if interval is significant (> 5ms) to filter duplicates in same datagram
|
|
if dt > 5.0:
|
|
# Add to rolling window (keep last 10 samples)
|
|
samples = stat.get("period_samples", [])
|
|
samples.append(dt)
|
|
if len(samples) > 10:
|
|
samples.pop(0)
|
|
stat["period_samples"] = samples
|
|
# Calculate average period from samples
|
|
stat["period_ms"] = sum(samples) / len(samples)
|
|
stat["last_time"] = now
|
|
stat["wc"] = payload_len//2
|
|
stat["rt"] = rt
|
|
self.stats[sa] = stat
|
|
# Diagnostic: note that we updated stats for this SA (help GUI debugging)
|
|
try:
|
|
if 11 <= sa <= 28:
|
|
# write unbounded short entry to rx_messages.log to confirm stats update
|
|
try:
|
|
with open(self.rx_log_path, 'a', encoding='utf-8') as rf:
|
|
rf.write(f"[{time.strftime('%H:%M:%S')}] STATS_UPDATED SA={sa} name={stat.get('name')} count={stat.get('count')}\n")
|
|
except Exception:
|
|
pass
|
|
if getattr(self, '_diag_dump_remaining', 0) > 0:
|
|
self._diag_dump(f"STATS_UPDATED SA={sa} name={stat.get('name')} count={stat.get('count')}")
|
|
except Exception:
|
|
pass
|
|
|
|
except socket.timeout:
|
|
continue
|
|
except OSError:
|
|
if self._running.is_set():
|
|
print("Socket error in receiver.")
|
|
break
|
|
|
|
def _send_udp(self, data: bytes, dest_ip: str):
|
|
if self._sock:
|
|
try:
|
|
self._sock.sendto(data, (dest_ip, self.udp_send_port))
|
|
except OSError as e:
|
|
print(f"Send error: {e}")
|
|
|
|
def _prepare_frame(self, messages: list[BaseMessage]) -> bytes:
|
|
"""
|
|
Prepara un datagram UDP che replica il comportamento del vecchio codice:
|
|
UDP Header (Udp1553Header) + concatenazione dei messaggi (header1553 + payload + inverted_cw + ctrl_end)
|
|
+ END_1553 (marker finale)
|
|
|
|
IMPORTANTE: Il frame counter deve incrementare ad ogni invio come nel vecchio codice!
|
|
"""
|
|
# Crea header con contatore incrementale
|
|
header = Udp1553Header()
|
|
header.msg_counter = self.frame_counter
|
|
self.frame_counter += 1
|
|
|
|
packed = bytes(header)
|
|
for msg in messages:
|
|
packed += msg.pack()
|
|
# Append global END marker (little-endian uint16)
|
|
packed += struct.pack("<H", Marker.END_1553)
|
|
return packed
|
|
|
|
def _init_messages(self):
|
|
"""
|
|
Register known message types with their payload structures.
|
|
Uses local pymsc.lib1553 definitions instead of importing from _old.
|
|
"""
|
|
def make_placeholder(words=32):
|
|
"""Create a generic placeholder payload for messages not yet implemented."""
|
|
class Placeholder(ctypes.Structure):
|
|
_pack_ = 1
|
|
_fields_ = [("data", ctypes.c_uint16 * words)]
|
|
return Placeholder
|
|
|
|
# Define message registrations: (label, payload_class, sa, freq, is_transmit)
|
|
# NOTE: freq set to 0 to disable automatic transmission (avoid flooding the real radar server)
|
|
registrations = [
|
|
("A1", MsgA1Payload, 1, 0, True), # Radar settings and parameters
|
|
("A2", MsgA2Payload, 2, 0, True), # Radar operation command
|
|
("A3", MsgA3Payload, 3, 0, True), # Graphic setting
|
|
("A4", MsgA4Payload, 4, 0, True), # Nav data and cursor
|
|
("A5", MsgInuHighSpeedPayload, 5, 0, True), # INU high speed data
|
|
("A6", make_placeholder(16), 6, 0, True), # Not used / TODO
|
|
("A7", MsgA7Payload, 7, 0, True), # Data Link Targets #1
|
|
("A8", MsgA8Payload, 8, 0, True), # Data Link Targets #2
|
|
]
|
|
|
|
# B messages: provide payload classes when available, otherwise placeholder
|
|
b_special = {
|
|
1: TwsStatusAndTargets0102, # B1
|
|
2: MsgB2Payload, # B2
|
|
3: MsgB3Payload, # B3
|
|
4: MsgB4Payload, # B4
|
|
5: TrackedTarget01, # B5
|
|
6: MsgRdrSettingsAndParametersTellback, # B6
|
|
7: MsgRdrStatusTellback, # B7
|
|
8: MsgB8Payload, # B8
|
|
}
|
|
for i in range(1, 19):
|
|
sa = 10 + i
|
|
label = f"B{i}"
|
|
payload_cls = b_special.get(i, make_placeholder(27))
|
|
registrations.append((label, payload_cls, sa, 0, False))
|
|
|
|
# Register all messages
|
|
for (label, payload_cls, sa, freq, is_tx) in registrations:
|
|
try:
|
|
msg = BaseMessage(label, payload_cls, sub_addr=sa, frequency=freq, is_transmit=is_tx)
|
|
# Initialize A1 settings to defaults
|
|
if label == 'A1' and hasattr(msg.payload, 'settings'):
|
|
# settings is a ctypes Union with .raw
|
|
try:
|
|
msg.payload.settings.raw = 0
|
|
except Exception:
|
|
pass
|
|
# frequency may be a ctypes scalar (int) or a Union; set appropriately
|
|
if hasattr(msg.payload, 'frequency'):
|
|
try:
|
|
# prefer assigning integer value
|
|
msg.payload.frequency = 0
|
|
except Exception:
|
|
try:
|
|
msg.payload.frequency.raw = 0
|
|
except Exception:
|
|
pass
|
|
self.register_message(msg)
|
|
except Exception as e:
|
|
print(f"Warning: failed to register message {label} (SA={sa}): {e}") |