119 lines
3.3 KiB
Python
119 lines
3.3 KiB
Python
import os
|
|
import sys
|
|
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'src')))
|
|
from Grifo_E_1553lib.mil1553udp import UDP1553Header ,UDP1553Message, cw
|
|
from Grifo_E_1553lib.udp_receiver import UDPReceiver
|
|
import socket
|
|
import ctypes
|
|
import time
|
|
from pynput.keyboard import Key, Listener
|
|
import threading
|
|
# from Grifo_E_1553lib.messages.messages import B7
|
|
|
|
mcounter = 0
|
|
DEBUG = False
|
|
|
|
def prepare_udp_message(msg_list):
|
|
global mcounter
|
|
udp_header = UDP1553Header()
|
|
udp_header.marker1553 = ctypes.c_uint16(udp_header.k_marker_1553)
|
|
udp_header.otype = ctypes.c_uint16(udp_header.k_otype_bc) # BC
|
|
udp_header.mcounter = mcounter
|
|
udp_header.ta = 0
|
|
|
|
packed_header = bytes(udp_header)
|
|
|
|
packed_messages = packed_header
|
|
for msg in msg_list:
|
|
packed_messages+= msg.get_packed_message() #bytes(msg)
|
|
|
|
packed_messages+= bytes(ctypes.c_uint16(udp_header.k_marker_end_1553))
|
|
# print(f"size of message: {len(packed_messages)} bytes.")
|
|
mcounter+=1
|
|
return packed_messages
|
|
|
|
|
|
|
|
UDP_IP = "127.0.0.1"
|
|
UDP_SEND_PORT = 51553
|
|
UDP_RECV_PORT = 61553
|
|
|
|
|
|
# def init_recv_sock():
|
|
# sock.bind((UDP_IP, UDP_RECV_PORT))
|
|
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
sock.bind((UDP_IP,UDP_RECV_PORT))
|
|
|
|
def send_udp_msg(msg):
|
|
|
|
# print(f"Sending to {UDP_IP}:{UDP_SEND_PORT}")
|
|
sock.sendto(msg, (UDP_IP, UDP_SEND_PORT))
|
|
# print("Message sent!")
|
|
|
|
def current_milli_time():
|
|
return round(time.time() * 1000)
|
|
|
|
|
|
STOP_ALL = threading.Event()
|
|
STOP_ALL.clear()
|
|
canSend = threading.Event()
|
|
|
|
def keypressed(key):
|
|
|
|
if key == Key.esc:
|
|
print("Quitting the program...")
|
|
STOP_ALL.set()
|
|
return False
|
|
|
|
def receive_messages():
|
|
# r = UDPReceiver(UDP_IP, UDP_RECV_PORT)
|
|
# while not STOP_ALL.is_set():
|
|
|
|
|
|
# msg =r.receive()
|
|
data, addr = sock.recvfrom(4096)
|
|
# print(f"RECEIVED FROM: {addr} ")
|
|
# print(f"RECEIVEDDDDDDDDDDD {len(data)}" )
|
|
# print(len(data))
|
|
UDPHeader =UDP1553Header.from_buffer_copy(bytes(data)[0:ctypes.sizeof(UDP1553Header)])
|
|
# print(UDPHeader.marker1553)
|
|
header1553 = UDP1553Message.from_buffer_copy(bytes(data)[ctypes.sizeof(UDP1553Header):ctypes.sizeof(UDP1553Header) + ctypes.sizeof(UDP1553Message)])
|
|
raw_data = bytes(data)[ctypes.sizeof(UDP1553Header) + ctypes.sizeof(UDP1553Message):]
|
|
# Debug print: show received packet summary for troubleshooting
|
|
try:
|
|
sa = header1553.cw.str.sa
|
|
wc = header1553.cw.str.wc
|
|
raw_cw = header1553.cw.raw
|
|
except Exception:
|
|
sa = None
|
|
wc = None
|
|
raw_cw = None
|
|
if DEBUG:
|
|
print(f"[UDP RECV] from={addr} SA={sa} WC={wc} CW_raw={hex(raw_cw) if raw_cw is not None else None} UDP_mcounter={UDPHeader.mcounter}")
|
|
# also dump SW if present
|
|
try:
|
|
sw = header1553.sw
|
|
if DEBUG:
|
|
print(f"[UDP RECV] SW={hex(sw)} errcode={header1553.errcode}")
|
|
except Exception:
|
|
pass
|
|
|
|
return [UDPHeader,header1553,raw_data]
|
|
|
|
|
|
|
|
|
|
|
|
# print(ctypes.c_uint16.from_buffer_copy(bytes(data)[-ctypes.sizeof(ctypes.c_uint16):]))
|
|
|
|
|
|
# print(ctypes.sizeof(UDP1553Header))
|
|
|
|
def isA(msg):
|
|
return (msg.head.cw.str.sa < 10)
|
|
|
|
|
|
# print(ctypes.sizeof(UDP1553Header))
|
|
# print(ctypes.sizeof(UDP1553Message))
|
|
# print(ctypes.sizeof(UDP1553Header)+ ctypes.sizeof(UDP1553Message))
|
|
# # :ctypes.sizeof(UDP1553Header) + |