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 UDP1553Message, cw, UDP1553Header from Grifo_E_1553lib.messages.msg_rdr_settings_and_parameters import MsgRdrSettingsAndParameters from Grifo_E_1553lib.messages.msg_rdr_operation_command import MsgRdrOperationCommand from Grifo_E_1553lib.messages.msg_inu_high_speed import MsgInuHighSpeed from Grifo_E_1553lib.messages.msg_nav_data_and_cursor import MsgNavDataAndCursor from Grifo_E_1553lib.messages.msg_graphic_setting import MsgGraphicSetting from Grifo_E_1553lib.messages.tracked_target import TrackedTarget01,TrackedTarget02_10 from Grifo_E_1553lib.messages.msg_rdr_status_tellback import MsgRdrStatusTellback from Grifo_E_1553lib.messages.msg_rdr_settings_and_parameters_tellback import MsgRdrSettingsAndParametersTellback from Grifo_E_1553lib.data_types.enums import TargetHistory, RdrModes from Grifo_E_1553lib.connection.utils import * import socket import ctypes import time from Grifo_E_1553lib.connection.utils import prepare_udp_message # mcounter = 0 # 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) lock = threading.Lock() class MessageDB(): messages_buffer1 = {} messages_buffer2 = {} def buffercopy(): with lock: MessageDB.messages_buffer2 = MessageDB.messages_buffer1.copy() return MessageDB.messages_buffer2 def saveMessage(label,msg): MessageDB.messages_buffer1[label] = msg def getMessage(label): return MessageDB.messages_buffer1[label] def getAllMessages(): return MessageDB.messages_buffer1 class MessageNew(): # _instance = None global majorFrame # label = "" # def __new__(cls, *args, **kwargs): # if cls._instance is None: # # Crea una sola volta l'istanza # cls._instance = super(MessageNew, cls).__new__(cls) # return cls._instance def __init__(self, label, Message_class, freq, subAddr, request = False): # if hasattr(self, "_initialized"): # return # evita re-inizializzazione # super().__init__(MsgRdrSettingsAndParameters, 1) self.label = label self.freq = freq self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) MessageDB.saveMessage(self.label,self) # def __init__(self,Message_class, subAddr,request = False): self.message = Message_class() self.size = ctypes.sizeof(self.message) self.head = UDP1553Message(0x3C3C, cw(int(self.size/2), subAddr, 0 if not request else 1, 20)) self.__request = request self.inverted_cw = ~self.head.cw.raw & 0xFFFF # telemetry counters self.sent_count = 0 self.recv_count = 0 self.last_sent_ms = 0 self.last_recv_ms = 0 def get_packed_message(self): if self.__request: return self.request_message() # print(f"1553 header size: {ctypes.sizeof(self.head)}, message size: {ctypes.sizeof(self.message)} ,+2 ,+2 ") return bytes(self.head) + bytes(self.message)+ bytes(ctypes.c_uint16(self.inverted_cw)) + bytes(ctypes.c_uint16(0x3E3E)) def request_message(self): return bytes(self.head) + bytes([]) + bytes(ctypes.c_uint16(self.inverted_cw)) + bytes(ctypes.c_uint16(0x3E3E)) def set_message(self, data): ctypes.memmove(ctypes.addressof(self.message), data, ctypes.sizeof(self.message)) def recv_messages(): while not STOP_ALL.is_set(): rv = receive_messages() if not rv: # receive returned no data (socket closed or not initialized) -> exit loop break UDPHeader, header1553, data = rv # defensive: if header1553 is None, exit if header1553 is None: break match getattr(header1553.cw.str, 'sa', None): case 11: # print("B1 RECEIVEDDDDDDDDDDDDDDDDDDDDDDDDD") None case 12: print("B2") case 13: print("B3") case 14: print("B4") case 15: print("B5") case 16: # print("B6") try: m = MessageDB.getMessage("B6") m.set_message(data) m.recv_count = getattr(m, 'recv_count', 0) + 1 import time m.last_recv_ms = round(time.time() * 1000) except Exception: pass case 17: # print("B7 RECEIVEDDDDDDDDDD") # with open('LOG.txt', 'a') as file: # file.writelines('B7 RECEIVEDDDDDd!!!') # b7_message = B7().message.from_buffer_copy(bytes(data)[ctypes.sizeof(UDP1553Header):ctypes.sizeof(UDP1553Header) + ctypes.sizeof(UDP1553Message)]) # B7().set_message(data) try: m = MessageDB.getMessage("B7") m.set_message(data) m.recv_count = getattr(m, 'recv_count', 0) + 1 import time m.last_recv_ms = round(time.time() * 1000) except Exception: pass case 18: print("B8") case 19: print("B9") case 20: print("B10") case 21: print("B11") class Message(): def __init__(self,Message_class, subAddr,request = False): self.message = Message_class() self.size = ctypes.sizeof(self.message) self.head = UDP1553Message(0x3C3C, cw(int(self.size/2), subAddr, 0 if not request else 1, 20)) self.__request = request self.inverted_cw = ~self.head.cw.raw & 0xFFFF # telemetry counters self.sent_count = 0 self.recv_count = 0 self.last_sent_ms = 0 self.last_recv_ms = 0 def get_packed_message(self): if self.__request: return self.request_message() # print(f"1553 header size: {ctypes.sizeof(self.head)}, message size: {ctypes.sizeof(self.message)} ,+2 ,+2 ") return bytes(self.head) + bytes(self.message)+ bytes(ctypes.c_uint16(self.inverted_cw)) + bytes(ctypes.c_uint16(0x3E3E)) def request_message(self): return bytes(self.head) + bytes([]) + bytes(ctypes.c_uint16(self.inverted_cw)) + bytes(ctypes.c_uint16(0x3E3E)) def set_message(self, data): ctypes.memmove(ctypes.addressof(self.message), data, ctypes.sizeof(self.message)) try: import time self.recv_count = getattr(self, 'recv_count', 0) + 1 self.last_recv_ms = round(time.time() * 1000) except Exception: pass # self.message = self.message.class.from_buffer_copy(data) # print(Message(None,2).message) # print(bytes([])) # A2 = Message(MsgRdrOperationCommand) # print(f"WC: {A2.inverted_cw}") # class CommonFreqFrame(): # def __init__(self, freq, ): # self.freq = freq # self._time_ms = 1/freq * 1000 # self.messages = [] # def addMessage(self,msg): # if not self.messages.__contains__(msg): # self.messages.append(msg) # # return True # # else: # # return False # class MajorFrame(): # commonFreqFrames = [] # def put_in_right_frame(msg): # if len(MajorFrame.commonFreqFrames) == 0: # MajorFrame.commonFreqFrames.append(CommonFreqFrame(msg.freq)) # found = False # for commFr in MajorFrame.commonFreqFrames: # if commFr.freq == msg.freq: # commFr.addMessage(msg) # found = True # break # if not found: # MajorFrame.commonFreqFrames.append(CommonFreqFrame(msg.freq)) # def __init__(self): # pass class MajorFrame(): __majorFrame = {} def put_in_right_frame(msg): MajorFrame.__majorFrame.setdefault(msg._time_ms,[]).append(msg) __majorFrame = {} def put_in_right_frame_new(msgs): MajorFrame.__majorFrame = {} for msg in msgs.values(): MajorFrame.__majorFrame.setdefault(msg._time_ms,[]).append(msg) def get_majorFrame(): return MajorFrame.__majorFrame def send_frame_at_right_time(stop, canSend): print("Sending frames...") mytime = 0 while not stop.is_set(): # print(MajorFrame.get_majorFrame()) for key in MajorFrame.__majorFrame.keys(): if mytime % key == 0 and canSend.is_set(): MessageDB.getMessage("A5").message.timetag.raw+=312 # increment sent counters for telemetry try: for m in MajorFrame.__majorFrame[key]: m.sent_count = getattr(m, 'sent_count', 0) + 1 m.last_sent_ms = round(time.time() * 1000) except Exception: pass udp_msg = prepare_udp_message(MajorFrame.__majorFrame[key]) send_udp_msg(udp_msg) time.sleep(0.02) mytime = mytime + 20 def send_frame_at_right_time_new(stop, canSend): print("Sending frames...") mytime = 0 while not stop.is_set(): # print(MajorFrame.get_majorFrame()) MajorFrame.put_in_right_frame_new(MessageDB.buffercopy()) for key in MajorFrame.__majorFrame.keys(): if mytime % key == 0 and canSend.is_set(): MessageDB.getMessage("A5").message.timetag.raw+=312 # increment sent counters for telemetry try: for m in MajorFrame.__majorFrame[key]: m.sent_count = getattr(m, 'sent_count', 0) + 1 m.last_sent_ms = round(time.time() * 1000) except Exception: pass udp_msg = prepare_udp_message(MajorFrame.__majorFrame[key]) send_udp_msg(udp_msg) time.sleep(0.02) mytime = mytime + 20 class A1(Message): _instance = None global majorFrame def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(A1, cls).__new__(cls) return cls._instance def __init__(self, *args, **kwargs): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(MsgRdrSettingsAndParameters, 1) self.freq = 10 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) class A2(Message): _instance = None global majorFrame def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(A2, cls).__new__(cls) return cls._instance def __init__(self, *args, **kwargs): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(MsgRdrOperationCommand, 2) self.freq = 25 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) class A3(Message): _instance = None global majorFrame def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(A3, cls).__new__(cls) return cls._instance def __init__(self, *args, **kwargs): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(MsgGraphicSetting, 3) self.freq = 10 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) class A4(Message): _instance = None def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(A4, cls).__new__(cls) return cls._instance def __init__(self, *args, **kwargs): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(MsgNavDataAndCursor, 4) self.freq = 50 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) # def __init__(self): # super().__init__(MsgNavDataAndCursor, 4) # self.freq = 50 class A5(Message): _instance = None def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(A5, cls).__new__(cls) return cls._instance def __init__(self, *args, **kwargs): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(MsgInuHighSpeed, 5) self.freq = 50 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) def update_A5_time(self): self.message.timetag.raw+=312 class B4(Message): _instance = None def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(B4, cls).__new__(cls) return cls._instance def __init__(self): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(TrackedTarget02_10, 14,True) self.freq = 50 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) class B5(Message): _instance = None def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(B5, cls).__new__(cls) return cls._instance def __init__(self): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(TrackedTarget01, 15,True) self.freq = 50 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) class B7(Message): _instance = None def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(B7, cls).__new__(cls) return cls._instance def __init__(self): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(MsgRdrStatusTellback, 17,True) self.freq = 25 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) class B11(Message): _instance = None def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(B11, cls).__new__(cls) return cls._instance def __init__(self): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(TrackedTarget02_10, 21,True) self.freq = 50 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) class B12(Message): _instance = None def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(B12, cls).__new__(cls) return cls._instance def __init__(self): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(TrackedTarget02_10, 22,True) self.freq = 50 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) class B13(Message): _instance = None def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(B13, cls).__new__(cls) return cls._instance def __init__(self): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(TrackedTarget02_10, 23,True) self.freq = 50 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) class B14(Message): _instance = None def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(B14, cls).__new__(cls) return cls._instance def __init__(self): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(TrackedTarget02_10, 24,True) self.freq = 50 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) class B15(Message): _instance = None def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(B15, cls).__new__(cls) return cls._instance def __init__(self): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(TrackedTarget02_10, 25,True) self.freq = 50 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) class B16(Message): _instance = None def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(B16, cls).__new__(cls) return cls._instance def __init__(self): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(TrackedTarget02_10, 26,True) self.freq = 50 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) class B17(Message): _instance = None def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(B17, cls).__new__(cls) return cls._instance def __init__(self): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(TrackedTarget02_10, 27,True) self.freq = 50 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) class B18(Message): _instance = None def __new__(cls, *args, **kwargs): if cls._instance is None: # Crea una sola volta l'istanza cls._instance = super(B18, cls).__new__(cls) return cls._instance def __init__(self): if hasattr(self, "_initialized"): return # evita re-inizializzazione super().__init__(TrackedTarget02_10, 28,True) self.freq = 50 self._time_ms = 1/self.freq * 1000 self._initialized = True # majorFrame.setdefault(self.freq,[]).append(self) MajorFrame.put_in_right_frame(self) # A2_ = A2() # A4_ = A4() # A5_ = A5() def print_size(msgs): for msg in msgs: print(f"SIZE OF {msg.__class__.__name__} : {ctypes.sizeof(msg)}") # print_size([A2_,A4_,A5_]) # print(Message.) # a = A2() # b = A2() # print(A2().freq) # print(b.freq) # print (a is b) # majorFrame.setdefault(A2().freq,[]).append(A2()) # A2() # A2() # A2() # print(A2()) # print(A4()) # print(A5()) # # print(A2()) # # majorFrame.setdefault(A4().freq,[]).append(A4()) # # majorFrame.setdefault(A5().freq,[]).append(A5()) # print(MajorFrame.get_majorFrame())