diff --git a/.gitignore b/.gitignore index 2483976..61c48e7 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ .idea/ __pycache__/ +log/ diff --git a/main.py b/main.py new file mode 100644 index 0000000..2103ca9 --- /dev/null +++ b/main.py @@ -0,0 +1,135 @@ +import time +import signal +import argparse + +from network_backend import NetworkBackend +from msg.frame_logger import FrameLogger +from transceiver import Transceiver + +from nmea.packetizer import NmeaPacketizer +from nmea.messages import Gll, Gsa, Gsv, Gga, Rmc + +from ubx.ubx import frame_create +from ubx.msg_types import * +from ubx.packetizer import UbxPacketizer +from ubx.messages import Sig, Sat, RawX, UbxMsg +from ubx.listener import UbxListener + + +def handler(signum, frame, be: NetworkBackend, loggers: list): + sig_name = signal.Signals(signum).name + print(f'Signal handler called with signal {sig_name} ({signum})') + be.stop() + be.disconnect() + for lg in loggers: + lg.close() + + +if __name__ == "__main__": + ap = argparse.ArgumentParser() + ap.add_argument('--log-dir', default='./log', help='Directory for frame log files (default: ./log)') + args = ap.parse_args() + + server_list = [ + {'name': " neo-f9p", 'host': "192.168.22.93", 'port': 8721}, + {'name': "zed-x20p", 'host': "192.168.22.93", 'port': 8731} + ] + + transceiver = {} + loggers = [] + + # TCPIP multi source receiver + mc = NetworkBackend(server_list) + + # Set the signal handler + signal.signal(signal.SIGINT, lambda signum, frame: handler(signum, frame, mc, loggers)) + + for grm in [" neo-f9p", "zed-x20p"]: + nmea_rx_list = [ + Gll(grm, 'GN'), + Gsa(grm, 'GN'), + Gsv(grm, 'GP'), + Gga(grm, 'GP'), + Rmc(grm, 'GN'), + Gga(grm, 'GN'), + ] + ubx_rx_list = [ + UbxListener(Sig()), + UbxListener(Sat()), + UbxListener(RawX()), + + UbxListener(UbxMsg(UBX_CLASS_ACK, UBX_ID_ACK_ACK)), + UbxListener(UbxMsg(UBX_CLASS_ACK, UBX_ID_ACK_NACK)), + ] + + # Topology + # Receiver-0.0 \ + # Receiver-0.1 <- Packetizer-0 <- Transceiver-0 \ + # / \ + # Send-0->/ \ + # + Backend + # Send-1->\ / + # \ / + # Receiver-1.0 <- Packetizer-1 <- Transceiver-1 / + # Receiver-1.1 / + + # Create transceiver + transceiver[grm] = Transceiver(grm) + mc.register_xcvr(transceiver[grm]) + + # Create and register frame logger + lg = FrameLogger(grm, args.log_dir) + loggers.append(lg) + transceiver[grm].register_listener(lg) + + # Create NMEA-Packetizer + nmea_packetizer = NmeaPacketizer() + ubx_packetizer = UbxPacketizer() + + # Register NMEA-Packetizer to transceiver + transceiver[grm].register_listener(nmea_packetizer) + transceiver[grm].register_listener(ubx_packetizer) + + # Register Receiver to NMEA-Packetizer + for rx in nmea_rx_list: + nmea_packetizer.register_listener(rx) + + # Register Receiver to UBX-Packetizer + for rx in ubx_rx_list: + ubx_packetizer.register_listener(rx) + + + # Connect all sources + mc.connect() + + # Start thread + mc.start() + + while True: + try: + for grm in [" neo-f9p", "zed-x20p"]: + # Poll UBX-NAV-CLOCK (0x01 0x22) + frame = frame_create(UBX_CLASS_NAV, UBX_ID_CLOCK) + transceiver[grm].send(frame) + # Poll UBX-NAV-SIG (0x01 0x43) + frame = frame_create(UBX_CLASS_NAV, UBX_ID_NAV_SIG) + transceiver[grm].send(frame) + # Poll UBX-NAV-SAT (0x01 0x35) + frame = frame_create(UBX_CLASS_NAV, UBX_ID_NAV_SAT) + transceiver[grm].send(frame) + # Poll UBX-RXM-RAWX (0x02 0x15) + frame = frame_create(UBX_CLASS_RXM, UBX_ID_RXM_RAW) + transceiver[grm].send(frame) + except ValueError: + break + + time.sleep(1) + + # Stop thread + mc.stop() + + # code never reached + mc.disconnect() + + for lg in loggers: + lg.close() \ No newline at end of file diff --git a/msg/frame_logger.py b/msg/frame_logger.py new file mode 100644 index 0000000..ae09560 --- /dev/null +++ b/msg/frame_logger.py @@ -0,0 +1,23 @@ +import os +from datetime import datetime +from msg.listener import MsgListener +from msg.container import MsgContainer + + +class FrameLogger(MsgListener): + def __init__(self, source_id: str, log_dir: str = './log'): + super().__init__() + os.makedirs(log_dir, exist_ok=True) + ts = datetime.now().strftime('%Y_%m_%d_%H_%M_%S') + filename = f"{ts}_{source_id.strip()}.log" + self._file = open(os.path.join(log_dir, filename), 'wb') + + def is_msg(self, msg: MsgContainer) -> bool: + return True + + def on_recv(self, msg: MsgContainer): + self._file.write(msg.data) + self._file.flush() + + def close(self): + self._file.close() diff --git a/network_backend.py b/network_backend.py index c6c0a4e..9a026d9 100644 --- a/network_backend.py +++ b/network_backend.py @@ -1,7 +1,5 @@ import socket import selectors -import time -import signal from threading import Thread from queue import Queue @@ -10,15 +8,6 @@ from transceiver import Transceiver, ATransceiver from a_backend import ABackend -from nmea.packetizer import NmeaPacketizer -from nmea.messages import Gll, Gsa, Gsv, Gga, Rmc - -from ubx.ubx import frame_create -from ubx.msg_types import * -from ubx.packetizer import UbxPacketizer -from ubx.messages import Sig, Sat, RawX, UbxMsg -from ubx.listener import UbxListener - class NetworkBackend(ABackend): def __init__(self, servers: list[dict]): @@ -98,7 +87,6 @@ class NetworkBackend(ABackend): name = descr['name'] sock = self.find_by_name(name) if mask & selectors.EVENT_READ: - timestamp = time.time() data = key.fileobj.recv(64) sock['xcvr'].on_recv(MsgContainer(data, sock['xcvr'])) if mask & selectors.EVENT_WRITE: @@ -111,108 +99,4 @@ class NetworkBackend(ABackend): except KeyboardInterrupt: print("Caught keyboard interrupt, exiting") finally: - self.sel.close() - -def handler(signum, frame, be: NetworkBackend): - sig_name = signal.Signals(signum).name - print(f'Signal handler called with signal {sig_name} ({signum})') - be.stop() - be.disconnect() - -if __name__ == "__main__": - server_list = [ - {'name': " neo-f9p", 'host': "192.168.22.93", 'port': 8721}, - {'name': "zed-x20p", 'host': "192.168.22.93", 'port': 8731} - ] - - transceiver = {} - - # TCPIP multi source receiver - mc = NetworkBackend(server_list) - - # Set the signal handler - signal.signal(signal.SIGINT, lambda signum, frame: handler(signum, frame, mc)) - - for grm in [" neo-f9p", "zed-x20p"]: - nmea_rx_list = [ - Gll(grm, 'GN'), - Gsa(grm, 'GN'), - Gsv(grm, 'GP'), - Gga(grm, 'GP'), - Rmc(grm, 'GN'), - Gga(grm, 'GN'), - ] - ubx_rx_list = [ - UbxListener(Sig()), - UbxListener(Sat()), - UbxListener(RawX()), - - UbxListener(UbxMsg(UBX_CLASS_ACK, UBX_ID_ACK_ACK)), - UbxListener(UbxMsg(UBX_CLASS_ACK, UBX_ID_ACK_NACK)), - ] - - # Topology - # Receiver-0.0 \ - # Receiver-0.1 <- Packetizer-0 <- Transceiver-0 \ - # / \ - # Send-0->/ \ - # + Backend - # Send-1->\ / - # \ / - # Receiver-1.0 <- Packetizer-1 <- Transceiver-1 / - # Receiver-1.1 / - - # Create transceiver - transceiver[grm] = Transceiver(grm) - mc.register_xcvr(transceiver[grm]) - - # Create NMEA-Packetizer - nmea_packetizer = NmeaPacketizer() - ubx_packetizer = UbxPacketizer() - - # Register NMEA-Packetizer to transceiver - transceiver[grm].register_listener(nmea_packetizer) - transceiver[grm].register_listener(ubx_packetizer) - - # Register Receiver to NMEA-Packetizer - for rx in nmea_rx_list: - nmea_packetizer.register_listener(rx) - - # Register Receiver to UBX-Packetizer - for rx in ubx_rx_list: - ubx_packetizer.register_listener(rx) - - - # Connect all sources - mc.connect() - - # Start thread - mc.start() - - while True: - try: - for grm in [" neo-f9p", "zed-x20p"]: - # Poll UBX-NAV-CLOCK (0x01 0x22) - frame = frame_create(UBX_CLASS_NAV, UBX_ID_CLOCK) - transceiver[grm].send(frame) - # Poll UBX-NAV-SIG (0x01 0x43) - frame = frame_create(UBX_CLASS_NAV, UBX_ID_NAV_SIG) - transceiver[grm].send(frame) - # Poll UBX-NAV-SAT (0x01 0x35) - frame = frame_create(UBX_CLASS_NAV, UBX_ID_NAV_SAT) - transceiver[grm].send(frame) - # Poll UBX-RXM-RAWX (0x02 0x15) - frame = frame_create(UBX_CLASS_RXM, UBX_ID_RXM_RAW) - transceiver[grm].send(frame) - except ValueError: - break - - time.sleep(1) - - # Stop thread - mc.stop() - - # code never reached - mc.disconnect() - - + self.sel.close() \ No newline at end of file