SXXXXXXX_PyBusMonitor1553/pybusmonitor1553/core/scheduler.py

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."""
        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()