diff --git a/network_backend.py b/network_backend.py index 81423d1..22ec4bb 100644 --- a/network_backend.py +++ b/network_backend.py @@ -5,6 +5,7 @@ import time from msg_container import MsgContainer from receiver.nmea_packetizer import NmeaPacketizer from receiver.ubx_packetizer import UbxPacketizer +from receiver.ubx_receiver import UbxReceiver from transceiver import Transceiver from a_backend import ABackend from nmea.GLL import Gll @@ -12,7 +13,6 @@ from nmea.GSA import Gsa from nmea.GSV import Gsv from nmea.GGA import Gga from nmea.RMC import Rmc -from ubx.ubx import UbxReceiver class NetworkBackend(ABackend): @@ -116,7 +116,7 @@ if __name__ == "__main__": # Create NMEA-Packetizer nmea_packetizer = NmeaPacketizer() - ubx_packetizer = UbxPacketizer(sync_bytes=b'\xb5\x62') + ubx_packetizer = UbxPacketizer() # Register NMEA-Packetizer to transceiver transceiver.register_recv(nmea_packetizer) diff --git a/receiver/ubx_packetizer.py b/receiver/ubx_packetizer.py index 7770313..0f4db7c 100644 --- a/receiver/ubx_packetizer.py +++ b/receiver/ubx_packetizer.py @@ -1,34 +1,44 @@ +import struct from msg_container import MsgContainer from msg_sink import MsgSink from msg_source import MsgSource -from struct import pack +from ubx.ubx import UBX_SYNC_WORD, frame_parse_hdr, frame_create +from receiver.ubx_receiver import UbxReceiver class UbxPacketizer(MsgSink, MsgSource): - def __init__(self, sync_bytes: bytes): + def __init__(self, sync_bytes: bytes = UBX_SYNC_WORD): MsgSink.__init__(self) MsgSource.__init__(self) self.sync_bytes = sync_bytes self.packet = b'' self.wait_sync = True - self.latency = None + self.sync_count = 0 + + def reset(self): + self.packet = b'' + self.wait_sync = True + self.sync_count = 0 + def on_recv(self, msg: MsgContainer): name = msg.name - data = msg.data - w = len(self.sync_bytes) - self.latency = 0 - for n in range (0, len(data)): - d = data[n:n+w] - if d == self.sync_bytes: - self.latency = w-1 - print("UBX-sync") - timestamp = msg.timestamp - self.wait_sync = False - elif not self.wait_sync: - if self.latency == 0: - self.packet += d - else: - self.latency -= 1 - + timestamp = msg.timestamp + for n in range (0, len(msg.data)): + d = msg.data[n] + if d == self.sync_bytes[self.sync_count]: + self.sync_count += 1 + if self.sync_count == len(self.sync_bytes): + print("UBX-sync") + self.sync_count = 0 + self.packet = b'' + self.wait_sync = False + else: + if not self.wait_sync: + self.packet += struct.pack('B', d) + hdr = frame_parse_hdr(self.packet) + if hdr is not None: + if (6 + hdr.length) == len(self.packet): + self.call_sinks(MsgContainer(name, timestamp, self.packet)) + self.packet = b'' def is_msg(self, msg: MsgContainer): @@ -36,11 +46,21 @@ class UbxPacketizer(MsgSink, MsgSource): if __name__ == "__main__": - _sync = b'JA' - _data = b'31' - pkt = UbxPacketizer(sync_bytes=_sync) - frame1 = _sync + b'31' + _sync + b'31' - pkt.on_recv(MsgContainer('test', 31101970.0208, frame1)) + class MySink(MsgSink): + def __init__(self): + MsgSink.__init__(self) - frame2 = _sync + b'3' + _sync + b'10' - pkt.on_recv(MsgContainer('test', 31101970.0208, frame2)) + def on_recv(self, msg: MsgContainer): + print(msg) + + def is_msg(self, msg: MsgContainer): + return True + + ubx_rx = UbxReceiver('test', 1, 2) + + pkt = UbxPacketizer() + pkt.register_recv(ubx_rx) + + pkt.reset() + frame = frame_create(_class=1, _id=2, _data=b'Hallo') + frame_create(_class=1, _id=2, _data=b',') + frame_create(_class=1, _id=2, _data=b'UBX') + frame_create(_class=1, _id=2, _data=b'World') + pkt.on_recv(MsgContainer('test', 31101970.0208, frame)) diff --git a/receiver/ubx_receiver.py b/receiver/ubx_receiver.py new file mode 100644 index 0000000..8545a24 --- /dev/null +++ b/receiver/ubx_receiver.py @@ -0,0 +1,20 @@ +from msg_container import MsgContainer +from msg_sink import MsgSink +from ubx.ubx import frame_parse, frame_parse_hdr + +class UbxReceiver(MsgSink): + def __init__(self, name: str, mid: int, mclass: int): + MsgSink.__init__(self) + self.name = name + self.mid = mid + self.mclass = mclass + + def on_recv(self, msg: MsgContainer): + data, hdr = frame_parse(msg.data) + print(f"{hdr.mclass}:{hdr.mid}:{data}") + + def is_msg(self, msg: MsgContainer): + hdr = frame_parse_hdr(msg.data) + if hdr is not None: + return self.mclass == hdr.mclass and self.mid == hdr.mid + return False diff --git a/ubx/ubx.py b/ubx/ubx.py index 1253d4b..8a5aac8 100644 --- a/ubx/ubx.py +++ b/ubx/ubx.py @@ -1,8 +1,6 @@ import struct -from msg_container import MsgContainer -from msg_sink import MsgSink - +UBX_SYNC_WORD = b'\xb5\x62' def checksum(data: bytes) -> bytes: ck_a = 0 @@ -22,59 +20,49 @@ def frame_create(_class: int, _id: int, _data: bytes) -> bytes: return _sync + _hdr + _data + _check +class Hdr: + def __init__(self, _class: int, _id: int, _length: int): + self.mclass = _class + self.mid = _id + self.length = _length + + +def frame_parse_hdr(frame: bytes) -> Hdr|None: + hdr = Hdr(0,0,0) + + if len(frame) < 4: + return None + + hdr.mclass = struct.unpack('B', frame[0:1])[0] + hdr.mid = struct.unpack('B', frame[1:2])[0] + hdr.length = struct.unpack('