198 lines
8.3 KiB
Python
198 lines
8.3 KiB
Python
import time
|
|
import threading
|
|
import binascii
|
|
import logging
|
|
from .packet_builder import PacketBuilder
|
|
from .packet_builder_simple import SimplePacketBuilder
|
|
|
|
logger = logging.getLogger(__name__)
|
|
|
|
# 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"
|
|
logger.info(f"Multi-rate traffic generation started ({mode} format, {BASE_TICK_HZ}Hz base tick)")
|
|
logger.info(f"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()
|
|
logger.info("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)
|
|
if not QUIET_MODE:
|
|
logger.debug(f"Sent {msg_obj.__class__.__name__} (SA {msg_obj.SUBADDRESS})")
|
|
if DEBUG_PACKETS:
|
|
logger.debug(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)
|
|
if not QUIET_MODE:
|
|
logger.debug(f"Requested {msg_obj.__class__.__name__} (SA {msg_obj.SUBADDRESS})")
|
|
if DEBUG_PACKETS:
|
|
logger.debug(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."""
|
|
# Log A2 raw data when sending (for debugging mode command)
|
|
for msg in messages:
|
|
if msg.__class__.__name__ == 'MsgA2':
|
|
word0 = msg._data[0]
|
|
# Extract using ICD MSB=0 bit numbering: shift = 16 - (start_bit + width)
|
|
master_mode = (word0 >> 12) & 0xF # Bits 0-3
|
|
standby_cmd = (word0 >> 6) & 1 # Bit 9
|
|
logger.debug(f"[TX] A2 Word[0] = 0x{word0:04X} | master_mode={master_mode} standby={standby_cmd}")
|
|
|
|
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]
|
|
logger.info(f"Sent UDP1553 FRAME [{', '.join(msg_names)}] -> {self.target_ip}:{self.target_port}")
|
|
if DEBUG_PACKETS:
|
|
logger.debug(hex_dump(pkt[:100], " TX-FRAME (first 100B): "))
|
|
logger.debug(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() |