import time import threading import binascii from .packet_builder import PacketBuilder from .packet_builder_simple import SimplePacketBuilder # Debug flag - set to True for detailed packet logging DEBUG_PACKETS = False # Quiet mode - set to True to suppress scheduler output (for GUI mode) QUIET_MODE = False # Use simple format (like qg1553overudp.cpp) instead of full UDP1553 protocol # Set to False to use the full UDP1553 protocol (like avddriverudp.cpp) USE_SIMPLE_FORMAT = False # Server expects 0x1553 marker, so use full format # Scheduler base tick rate (must be divisor of all message rates) # Using 200 Hz (5ms) as base tick allows: # - 50 Hz = every 4 ticks (20ms) # - 25 Hz = every 8 ticks (40ms) # - 6.25 Hz = every 32 ticks (160ms) # - 1.5625 Hz = every 128 ticks (640ms) BASE_TICK_HZ = 200 BASE_TICK_PERIOD = 1.0 / BASE_TICK_HZ # 5ms def hex_dump(data: bytes, prefix: str = "") -> str: """Create a hex dump of bytes for debugging.""" hex_str = binascii.hexlify(data).decode('ascii') # Format in groups of 4 bytes (8 hex chars) formatted = ' '.join(hex_str[i:i+4] for i in range(0, len(hex_str), 4)) return f"{prefix}[{len(data)} bytes] {formatted}" class TrafficScheduler: """ Manages the periodic transmission of 1553 messages (BC -> RT) and requests for tell-backs (RT -> BC) using the RadarController state. """ def __init__(self, udp_handler, radar_controller, target_ip, target_port): self.udp = udp_handler self.controller = radar_controller self.target_ip = target_ip self.target_port = target_port self.builder = PacketBuilder() self.simple_builder = SimplePacketBuilder() self._running = False self._thread = None self._on_message_sent = None # Callback for sent messages self._tick_counter = 0 # Tabella di scheduling: mappa (message, ticks_divisor) # ticks_divisor = BASE_TICK_HZ / message_rate_hz self._schedule_table = self._build_schedule_table() self._on_message_sent = None # Callback for sent messages def register_sent_callback(self, callback_func): """ Registers a function to be called when messages are sent. Signature: callback_func(message_obj) """ self._on_message_sent = callback_func def _build_schedule_table(self): """ Costruisce la tabella di scheduling basata sui rate ICD dei messaggi. Ritorna lista di tuple (message_obj, divisor) dove divisor indica ogni quanti tick inviare. """ schedule = [] # Messaggi A (BC->RT) con i loro rate ICD msg_configs = [ (self.controller.msg_a1, 6.25), # 6.25 Hz -> ogni 32 tick (160ms) (self.controller.msg_a2, 25), # 25 Hz -> ogni 8 tick (40ms) (self.controller.msg_a3, 6.25), # 6.25 Hz -> ogni 32 tick (160ms) (self.controller.msg_a4, 50), # 50 Hz -> ogni 4 tick (20ms) (self.controller.msg_a5, 50), # 50 Hz -> ogni 4 tick (20ms) (self.controller.msg_a7, 6.25), # 6.25 Hz -> ogni 32 tick (160ms) (self.controller.msg_a8, 6.25), # 6.25 Hz -> ogni 32 tick (160ms) # B6, B7 sono richieste tell-back (self.controller.msg_b6, 6.25), # 6.25 Hz -> ogni 32 tick (160ms) (self.controller.msg_b7, 25), # 25 Hz -> ogni 8 tick (40ms) ] for msg_obj, rate_hz in msg_configs: divisor = int(BASE_TICK_HZ / rate_hz) schedule.append((msg_obj, divisor)) return schedule def start(self): if self._running: return self._running = True self._tick_counter = 0 self._thread = threading.Thread(target=self._loop, daemon=True) self._thread.start() mode = "SIMPLE" if USE_SIMPLE_FORMAT else "FULL UDP1553" print(f"[Scheduler] Multi-rate traffic generation started ({mode} format, {BASE_TICK_HZ}Hz base tick).") print(f"[Scheduler] Message rates: A1/A3/A7/A8/B6=6.25Hz, A2/B7=25Hz, A4/A5=50Hz") def stop(self): self._running = False if self._thread: self._thread.join() print("[Scheduler] Traffic generation stopped.") def _send_a(self, msg_obj): """Sends an 'A' message (Data to Server).""" pkt = self.builder.build_packet(msg_obj, is_request=False) self.udp.send(pkt, self.target_ip, self.target_port) print(f"[Scheduler] Sent {msg_obj.__class__.__name__} (SA {msg_obj.SUBADDRESS}) -> {self.target_ip}:{self.target_port}") if DEBUG_PACKETS: print(hex_dump(pkt, " TX: ")) def _req_b(self, msg_obj): """Sends a request for a 'B' message (Ask Server for Data).""" pkt = self.builder.build_packet(msg_obj, is_request=True) self.udp.send(pkt, self.target_ip, self.target_port) print(f"[Scheduler] Requested {msg_obj.__class__.__name__} (SA {msg_obj.SUBADDRESS}, TR={msg_obj.IS_TRANSMIT}) -> {self.target_ip}:{self.target_port}") if DEBUG_PACKETS: print(hex_dump(pkt, " TX-REQ: ")) def _send_frame_simple(self, messages): """Sends all messages in a single frame using simple format.""" pkt = self.simple_builder.build_frame(messages) self.udp.send(pkt, self.target_ip, self.target_port) msg_names = [m.__class__.__name__ for m in messages] print(f"[Scheduler] Sent FRAME [{', '.join(msg_names)}] -> {self.target_ip}:{self.target_port}") if DEBUG_PACKETS: print(hex_dump(pkt[:100], " TX-FRAME (first 100B): ")) print(f" Frame total size: {len(pkt)} bytes") def _send_frame_udp1553(self, messages): """Sends all messages in a single frame using full UDP1553 format.""" pkt = self.builder.build_frame(messages) self.udp.send(pkt, self.target_ip, self.target_port) if not QUIET_MODE: msg_names = [m.__class__.__name__ for m in messages] print(f"[Scheduler] Sent UDP1553 FRAME [{', '.join(msg_names)}] -> {self.target_ip}:{self.target_port}") if DEBUG_PACKETS: print(hex_dump(pkt[:100], " TX-FRAME (first 100B): ")) print(f" Frame total size: {len(pkt)} bytes") # Notify callback for each sent message if self._on_message_sent: for msg in messages: self._on_message_sent(msg) def _loop(self): """ Scheduling loop multi-rate. Tick a 200Hz (5ms), ogni messaggio viene inviato secondo il suo rate specifico. """ next_tick_time = time.perf_counter() while self._running: # Determina quali messaggi inviare in questo tick messages_to_send = [] for msg_obj, divisor in self._schedule_table: # Invia se tick_counter รจ multiplo del divisor if self._tick_counter % divisor == 0: messages_to_send.append(msg_obj) # Invia i messaggi di questo tick in un unico frame if messages_to_send: if USE_SIMPLE_FORMAT: self._send_frame_simple(messages_to_send) else: self._send_frame_udp1553(messages_to_send) # Incrementa tick counter self._tick_counter += 1 if self._tick_counter >= 800: # LCM di tutti i divisor (4,8,32) = 32, safe con 800 self._tick_counter = 0 # Reset per evitare overflow # Sleep preciso fino al prossimo tick next_tick_time += BASE_TICK_PERIOD sleep_time = next_tick_time - time.perf_counter() if sleep_time > 0: time.sleep(sleep_time) else: # Siamo in ritardo, resync next_tick_time = time.perf_counter()