146 lines
5.0 KiB
Python
146 lines
5.0 KiB
Python
import threading
|
|
import time
|
|
import socket
|
|
from typing import Dict
|
|
from ..lib1553.message_base import BaseMessage
|
|
from ..lib1553.message_base import BaseMessage
|
|
from ..lib1553.messages.a1_settings import MsgA1Payload
|
|
|
|
class AppController:
|
|
"""
|
|
Central Logic Controller.
|
|
Manages state, network threads, and message database.
|
|
Replaces global variables and scattered logic.
|
|
"""
|
|
|
|
def __init__(self):
|
|
self._running = threading.Event()
|
|
self._lock = threading.Lock()
|
|
|
|
# "Database" of messages: { "A1": BaseMessageInstance, ... }
|
|
self.messages: Dict[str, BaseMessage] = {}
|
|
|
|
# Threads
|
|
self._scheduler_thread = None
|
|
self._receiver_thread = None
|
|
|
|
# Network Config (TODO: Move to a config file)
|
|
self.udp_ip = "127.0.0.1"
|
|
self.udp_send_port = 51553
|
|
self.udp_recv_port = 61553
|
|
self._sock = None
|
|
|
|
def register_message(self, message: BaseMessage):
|
|
"""Adds a message to the active list."""
|
|
with self._lock:
|
|
self.messages[message.label] = message
|
|
|
|
def start(self):
|
|
"""Starts the network threads."""
|
|
print("Core: Starting services...")
|
|
self._running.set()
|
|
|
|
# Init Socket
|
|
self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
# Bind for receiving
|
|
try:
|
|
self._sock.bind((self.udp_ip, self.udp_recv_port))
|
|
except OSError as e:
|
|
print(f"Error binding socket: {e}")
|
|
|
|
self._init_messages()
|
|
|
|
# Start Scheduler
|
|
self._scheduler_thread = threading.Thread(target=self._scheduler_loop, daemon=True)
|
|
self._scheduler_thread.start()
|
|
|
|
# Start Receiver
|
|
self._receiver_thread = threading.Thread(target=self._receiver_loop, daemon=True)
|
|
self._receiver_thread.start()
|
|
|
|
def stop(self):
|
|
"""Stops threads and cleans up."""
|
|
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 _scheduler_loop(self):
|
|
"""
|
|
Manages the transmission of periodic messages.
|
|
Uses perf_counter for better precision than time.sleep().
|
|
"""
|
|
print("Core: Scheduler started.")
|
|
start_time = time.perf_counter()
|
|
|
|
while self._running.is_set():
|
|
current_time = time.perf_counter()
|
|
elapsed_ms = (current_time - start_time) * 1000
|
|
|
|
with self._lock:
|
|
# Copy values to avoid locking during I/O
|
|
active_msgs = list(self.messages.values())
|
|
|
|
for msg in active_msgs:
|
|
if msg.period_ms > 0:
|
|
# Check if it's time to send (Simple logic, can be improved for phase control)
|
|
# Using a modulus logic on integer ms for simplicity similar to original
|
|
if int(elapsed_ms) % int(msg.period_ms) < 20: # Window of 20ms tolerance
|
|
# In a real real-time system, we track 'next_wake_time' for each msg
|
|
# For now, we simulate the previous 'MajorFrame' logic
|
|
self._send_udp(msg.pack())
|
|
|
|
time.sleep(0.01) # Sleep 10ms to prevent CPU hogging
|
|
|
|
def _receiver_loop(self):
|
|
"""
|
|
Listens for incoming UDP packets.
|
|
"""
|
|
print("Core: Receiver started.")
|
|
while self._running.is_set():
|
|
try:
|
|
# Blocking call with timeout to allow checking _running flag
|
|
self._sock.settimeout(1.0)
|
|
data, addr = self._sock.recvfrom(2048)
|
|
|
|
# TODO: Parse data using lib1553 structures
|
|
# For now just print to verify connectivity
|
|
# print(f"Rx {len(data)} bytes from {addr}")
|
|
|
|
except socket.timeout:
|
|
continue
|
|
except OSError:
|
|
if self._running.is_set():
|
|
print("Socket error in receiver.")
|
|
break
|
|
|
|
def _send_udp(self, data: bytes):
|
|
if self._sock:
|
|
try:
|
|
self._sock.sendto(data, (self.udp_ip, self.udp_send_port))
|
|
except OSError as e:
|
|
print(f"Send error: {e}")
|
|
|
|
def _init_messages(self):
|
|
"""Creates the initial instances of the 1553 messages."""
|
|
|
|
# Creazione Messaggio A1
|
|
a1_msg = BaseMessage(
|
|
label="A1 - Settings",
|
|
payload_cls=MsgA1Payload,
|
|
sub_addr=1,
|
|
frequency=10, # 10 Hz
|
|
is_transmit=True
|
|
)
|
|
|
|
# Impostiamo valori di default per testare
|
|
a1_msg.payload.settings.raw = 0 # Reset
|
|
# a1_msg.payload.settings.history = TargetHistory.LEVEL_03 # Se importato enum
|
|
|
|
self.register_message(a1_msg)
|
|
print("Core: Messages initialized.") |