refactored
This commit is contained in:
+3
-102
@@ -6,7 +6,6 @@ TCP/IP-Verbindungen werden in einer persistenten LRU-Liste (max. 5) gespeichert.
|
||||
"""
|
||||
|
||||
import sys
|
||||
import struct
|
||||
import socket
|
||||
import math
|
||||
import serial
|
||||
@@ -23,18 +22,12 @@ from PyQt5.QtGui import (
|
||||
QPainter, QPen, QBrush, QColor, QFont,
|
||||
QRadialGradient,
|
||||
)
|
||||
from ubx_parser import UbxPacketizer
|
||||
from ubx import UbxPacketizer, build_poll, extract_satellites, UBX_CLASS_NAV, UBX_ID_NAV_SAT, UBX_ID_NAV_SVINFO
|
||||
ubx_packetizer = UbxPacketizer()
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# UBX Protokoll-Konstanten
|
||||
# ---------------------------------------------------------------------------
|
||||
UBX_SYNC1 = 0xB5
|
||||
UBX_SYNC2 = 0x62
|
||||
UBX_CLASS_NAV = 0x01
|
||||
UBX_ID_NAV_SAT = 0x35
|
||||
UBX_ID_NAV_SVINFO = 0x30
|
||||
|
||||
GNSS_NAMES = {
|
||||
0: "GPS",
|
||||
1: "SBAS",
|
||||
@@ -43,6 +36,7 @@ GNSS_NAMES = {
|
||||
4: "IMES",
|
||||
5: "QZSS",
|
||||
6: "GLONASS",
|
||||
7: "NavIC"
|
||||
}
|
||||
|
||||
GNSS_COLORS = {
|
||||
@@ -53,102 +47,9 @@ GNSS_COLORS = {
|
||||
4: QColor("#9C27B0"),
|
||||
5: QColor("#00BCD4"),
|
||||
6: QColor("#FF4081"),
|
||||
7: QColor("#808080")
|
||||
}
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# UBX Hilfsfunktionen
|
||||
# ---------------------------------------------------------------------------
|
||||
def ubx_checksum(payload: bytes) -> tuple[int, int]:
|
||||
ck_a = ck_b = 0
|
||||
for b in payload:
|
||||
ck_a = (ck_a + b) & 0xFF
|
||||
ck_b = (ck_b + ck_a) & 0xFF
|
||||
return ck_a, ck_b
|
||||
|
||||
|
||||
def build_ubx(cls: int, msg_id: int, payload: bytes = b"") -> bytes:
|
||||
header = bytes([UBX_SYNC1, UBX_SYNC2, cls, msg_id])
|
||||
length = struct.pack("<H", len(payload))
|
||||
raw = bytes([cls, msg_id]) + length + payload
|
||||
ck_a, ck_b = ubx_checksum(raw)
|
||||
return header + length + payload + bytes([ck_a, ck_b])
|
||||
|
||||
|
||||
def build_poll(cls: int, msg_id: int) -> bytes:
|
||||
return build_ubx(cls, msg_id, b"")
|
||||
|
||||
|
||||
def parse_ubx_frames(data: bytes) -> list[dict]:
|
||||
frames, i = [], 0
|
||||
while i < len(data) - 7:
|
||||
if data[i] == UBX_SYNC1 and data[i + 1] == UBX_SYNC2:
|
||||
if i + 6 > len(data):
|
||||
break
|
||||
cls = data[i + 2]
|
||||
msg_id = data[i + 3]
|
||||
length = struct.unpack_from("<H", data, i + 4)[0]
|
||||
end = i + 6 + length + 2
|
||||
if end > len(data):
|
||||
break
|
||||
payload = data[i + 6: i + 6 + length]
|
||||
raw_chk = data[i + 2: i + 6 + length]
|
||||
ck_a, ck_b = ubx_checksum(raw_chk)
|
||||
if ck_a == data[end - 2] and ck_b == data[end - 1]:
|
||||
frames.append({"cls": cls, "id": msg_id, "payload": payload})
|
||||
i = end
|
||||
else:
|
||||
i += 1
|
||||
return frames
|
||||
|
||||
|
||||
def parse_nav_sat(payload: bytes) -> list[dict]:
|
||||
if len(payload) < 8:
|
||||
return []
|
||||
num_svs = payload[5]
|
||||
sats = []
|
||||
for n in range(num_svs):
|
||||
off = 8 + n * 12
|
||||
if off + 12 > len(payload):
|
||||
break
|
||||
gnss_id, sv_id, cno, elev, azim, _, flags = struct.unpack_from(
|
||||
"<BBBBhhI", payload[off:off + 12]
|
||||
)
|
||||
sats.append({
|
||||
"gnssId": gnss_id, "svId": sv_id, "cno": cno,
|
||||
"elev": elev, "azim": azim, "used": bool(flags & 0x08),
|
||||
})
|
||||
return sats
|
||||
|
||||
|
||||
def parse_nav_svinfo(payload: bytes) -> list[dict]:
|
||||
if len(payload) < 8:
|
||||
return []
|
||||
num_ch = payload[4]
|
||||
sats = []
|
||||
for n in range(num_ch):
|
||||
off = 8 + n * 12
|
||||
if off + 12 > len(payload):
|
||||
break
|
||||
_, svid, flags, _, cno, elev, azim, _ = struct.unpack_from(
|
||||
"<BBBBBbhI", payload, off
|
||||
)
|
||||
sats.append({
|
||||
"gnssId": 0, "svId": svid, "cno": cno,
|
||||
"elev": elev, "azim": azim, "used": bool(flags & 0x01),
|
||||
})
|
||||
return sats
|
||||
|
||||
|
||||
def extract_satellites(frames: list[dict]) -> list[dict]:
|
||||
for f in frames:
|
||||
if f["cls"] == UBX_CLASS_NAV and f["id"] == UBX_ID_NAV_SAT:
|
||||
return parse_nav_sat(f["payload"])
|
||||
if f["cls"] == UBX_CLASS_NAV and f["id"] == UBX_ID_NAV_SVINFO:
|
||||
return parse_nav_svinfo(f["payload"])
|
||||
return []
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Transport-Abstraktionen
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
@@ -0,0 +1,167 @@
|
||||
import struct
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# UBX Protokoll-Konstanten
|
||||
# ---------------------------------------------------------------------------
|
||||
UBX_SYNC1 = 0xB5
|
||||
UBX_SYNC2 = 0x62
|
||||
UBX_CLASS_NAV = 0x01
|
||||
UBX_ID_NAV_SAT = 0x35
|
||||
UBX_ID_NAV_SVINFO = 0x30
|
||||
UBX_SYNC_WORD = b'\xb5\x62'
|
||||
|
||||
|
||||
def checksum(data: bytes) -> bytes:
|
||||
ck_a = 0
|
||||
ck_b = 0
|
||||
for i in range(len(data)):
|
||||
ck_a = (ck_a + data[i]) & 0xFF
|
||||
ck_b = (ck_b + ck_a) & 0xFF
|
||||
|
||||
return struct.pack("BB", ck_a, ck_b)
|
||||
|
||||
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('<H', frame[2:4])[0]
|
||||
|
||||
return hdr
|
||||
|
||||
def frame_parse(frame: bytes):
|
||||
_data = None
|
||||
_hdr = Hdr(0,0,0)
|
||||
|
||||
while True:
|
||||
_hdr = frame_parse_hdr(frame[0:4])
|
||||
if _hdr is None:
|
||||
break
|
||||
|
||||
if (4+_hdr.length+2) != len(frame):
|
||||
break
|
||||
|
||||
_check_soll = frame[-2:]
|
||||
_check_ist = checksum(frame[0:-2])
|
||||
if _check_soll != _check_ist:
|
||||
break
|
||||
|
||||
_data = frame[4:-2]
|
||||
break
|
||||
|
||||
return _data, _hdr
|
||||
|
||||
class UbxPacketizer:
|
||||
def __init__(self, sync_bytes: bytes = UBX_SYNC_WORD):
|
||||
self.sync_bytes = sync_bytes
|
||||
self.packet = b''
|
||||
self.wait_sync = True
|
||||
self.sync_count = 0
|
||||
self.d_save = b''
|
||||
|
||||
def reset(self):
|
||||
self.packet = b''
|
||||
self.wait_sync = True
|
||||
self.sync_count = 0
|
||||
|
||||
def on_recv(self, msg: bytes):
|
||||
frames = []
|
||||
for d in msg:
|
||||
if d == self.sync_bytes[self.sync_count]:
|
||||
self.d_save += struct.pack('B', d)
|
||||
self.sync_count += 1
|
||||
if self.sync_count == len(self.sync_bytes):
|
||||
self.sync_count = 0
|
||||
self.d_save = b''
|
||||
self.packet = b''
|
||||
self.wait_sync = False
|
||||
else:
|
||||
if self.sync_count > 0:
|
||||
self.packet += self.d_save
|
||||
self.d_save = b''
|
||||
self.sync_count = 0
|
||||
if not self.wait_sync:
|
||||
self.packet += struct.pack('B', d)
|
||||
data, hdr = frame_parse(self.packet)
|
||||
if data is not None:
|
||||
if hdr.length == len(data):
|
||||
frames.append({"cls": hdr.mclass, "id": hdr.mid, "payload": data})
|
||||
self.packet = b''
|
||||
self.wait_sync = True
|
||||
return frames
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# UBX Hilfsfunktionen
|
||||
# ---------------------------------------------------------------------------
|
||||
def ubx_checksum(payload: bytes) -> tuple[int, int]:
|
||||
ck_a = ck_b = 0
|
||||
for b in payload:
|
||||
ck_a = (ck_a + b) & 0xFF
|
||||
ck_b = (ck_b + ck_a) & 0xFF
|
||||
return ck_a, ck_b
|
||||
|
||||
def build_ubx(cls: int, msg_id: int, payload: bytes = b"") -> bytes:
|
||||
header = bytes([UBX_SYNC1, UBX_SYNC2, cls, msg_id])
|
||||
length = struct.pack("<H", len(payload))
|
||||
raw = bytes([cls, msg_id]) + length + payload
|
||||
ck_a, ck_b = ubx_checksum(raw)
|
||||
return header + length + payload + bytes([ck_a, ck_b])
|
||||
|
||||
def build_poll(cls: int, msg_id: int) -> bytes:
|
||||
return build_ubx(cls, msg_id, b"")
|
||||
|
||||
def parse_nav_sat(payload: bytes) -> list[dict]:
|
||||
if len(payload) < 8:
|
||||
return []
|
||||
num_svs = payload[5]
|
||||
sats = []
|
||||
for n in range(num_svs):
|
||||
off = 8 + n * 12
|
||||
if off + 12 > len(payload):
|
||||
break
|
||||
gnss_id, sv_id, cno, elev, azim, _, flags = struct.unpack_from(
|
||||
"<BBBBhhI", payload[off:off + 12]
|
||||
)
|
||||
sats.append({
|
||||
"gnssId": gnss_id, "svId": sv_id, "cno": cno,
|
||||
"elev": elev, "azim": azim, "used": bool(flags & 0x08),
|
||||
})
|
||||
return sats
|
||||
|
||||
def parse_nav_svinfo(payload: bytes) -> list[dict]:
|
||||
if len(payload) < 8:
|
||||
return []
|
||||
num_ch = payload[4]
|
||||
sats = []
|
||||
for n in range(num_ch):
|
||||
off = 8 + n * 12
|
||||
if off + 12 > len(payload):
|
||||
break
|
||||
_, svid, flags, _, cno, elev, azim, _ = struct.unpack_from(
|
||||
"<BBBBBbhI", payload, off
|
||||
)
|
||||
sats.append({
|
||||
"gnssId": 0, "svId": svid, "cno": cno,
|
||||
"elev": elev, "azim": azim, "used": bool(flags & 0x01),
|
||||
})
|
||||
return sats
|
||||
|
||||
def extract_satellites(frames: list[dict]) -> list[dict]:
|
||||
for f in frames:
|
||||
if f["cls"] == UBX_CLASS_NAV and f["id"] == UBX_ID_NAV_SAT:
|
||||
return parse_nav_sat(f["payload"])
|
||||
if f["cls"] == UBX_CLASS_NAV and f["id"] == UBX_ID_NAV_SVINFO:
|
||||
return parse_nav_svinfo(f["payload"])
|
||||
return []
|
||||
|
||||
|
||||
@@ -1,92 +0,0 @@
|
||||
import struct
|
||||
|
||||
UBX_SYNC_WORD = b'\xb5\x62'
|
||||
|
||||
def checksum(data: bytes) -> bytes:
|
||||
ck_a = 0
|
||||
ck_b = 0
|
||||
for i in range(len(data)):
|
||||
ck_a = (ck_a + data[i]) & 0xFF
|
||||
ck_b = (ck_b + ck_a) & 0xFF
|
||||
|
||||
return struct.pack("BB", ck_a, ck_b)
|
||||
|
||||
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('<H', frame[2:4])[0]
|
||||
|
||||
return hdr
|
||||
|
||||
def frame_parse(frame: bytes):
|
||||
_data = None
|
||||
_hdr = Hdr(0,0,0)
|
||||
|
||||
while True:
|
||||
_hdr = frame_parse_hdr(frame[0:4])
|
||||
if _hdr is None:
|
||||
break
|
||||
|
||||
if (4+_hdr.length+2) != len(frame):
|
||||
break
|
||||
|
||||
_check_soll = frame[-2:]
|
||||
_check_ist = checksum(frame[0:-2])
|
||||
if _check_soll != _check_ist:
|
||||
break
|
||||
|
||||
_data = frame[4:-2]
|
||||
break
|
||||
|
||||
return _data, _hdr
|
||||
|
||||
class UbxPacketizer:
|
||||
def __init__(self, sync_bytes: bytes = UBX_SYNC_WORD):
|
||||
self.sync_bytes = sync_bytes
|
||||
self.packet = b''
|
||||
self.wait_sync = True
|
||||
self.sync_count = 0
|
||||
self.d_save = b''
|
||||
|
||||
def reset(self):
|
||||
self.packet = b''
|
||||
self.wait_sync = True
|
||||
self.sync_count = 0
|
||||
|
||||
def on_recv(self, msg: bytes):
|
||||
frames = []
|
||||
for d in msg:
|
||||
if d == self.sync_bytes[self.sync_count]:
|
||||
self.d_save += struct.pack('B', d)
|
||||
self.sync_count += 1
|
||||
if self.sync_count == len(self.sync_bytes):
|
||||
self.sync_count = 0
|
||||
self.d_save = b''
|
||||
self.packet = b''
|
||||
self.wait_sync = False
|
||||
else:
|
||||
if self.sync_count > 0:
|
||||
self.packet += self.d_save
|
||||
self.d_save = b''
|
||||
self.sync_count = 0
|
||||
if not self.wait_sync:
|
||||
self.packet += struct.pack('B', d)
|
||||
data, hdr = frame_parse(self.packet)
|
||||
if data is not None:
|
||||
if hdr.length == len(data):
|
||||
frames.append({"cls": hdr.mclass, "id": hdr.mid, "payload": data})
|
||||
self.packet = b''
|
||||
self.wait_sync = True
|
||||
return frames
|
||||
Reference in New Issue
Block a user