115 lines
4.8 KiB
Python
115 lines
4.8 KiB
Python
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
|
|
|
|
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
|
|
|
|
def start(self):
|
|
if self._running:
|
|
return
|
|
self._running = True
|
|
self._thread = threading.Thread(target=self._loop, daemon=True)
|
|
self._thread.start()
|
|
mode = "SIMPLE" if USE_SIMPLE_FORMAT else "FULL UDP1553"
|
|
print(f"[Scheduler] Traffic generation started ({mode} format, 1Hz).")
|
|
|
|
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")
|
|
|
|
def _loop(self):
|
|
"""
|
|
Scheduling loop.
|
|
Sends configuration messages (A) and requests status (B).
|
|
"""
|
|
while self._running:
|
|
# Send all messages in a single frame
|
|
messages = [
|
|
self.controller.msg_a1, # SA 1 - Settings
|
|
self.controller.msg_a2, # SA 2 - Operation Command
|
|
self.controller.msg_a3, # SA 3 - Graphics
|
|
self.controller.msg_a4, # SA 4 - Nav Data
|
|
self.controller.msg_a5, # SA 5 - INU Data
|
|
# Tell-back requests (TR=1)
|
|
self.controller.msg_b6, # SA 16 - Settings Tell-Back (request)
|
|
self.controller.msg_b7, # SA 17 - Status Tell-Back (request)
|
|
]
|
|
|
|
if USE_SIMPLE_FORMAT:
|
|
self._send_frame_simple(messages)
|
|
else:
|
|
self._send_frame_udp1553(messages)
|
|
|
|
time.sleep(1.0) |