skyplot/ubx.py

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_ID_NAV_DOP    = 0x04

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 parse_nav_dop(payload: bytes) -> dict:
	if len(payload) < 18:
		return {}

	i_tow, p_dop, g_dop, t_dop, v_dop, h_dop, n_dop, e_dop = struct.unpack_from(
		"<IHHHHHHH", payload
	)
	scale = 0.01
	res = {
		"p_dop": p_dop*scale, "g_dop": g_dop*scale, "t_dop": t_dop*scale,
		"v_dop": v_dop*scale, "h_dop": h_dop*scale, "n_dop": n_dop*scale, "e_dop": e_dop*scale
	}

	return res

def extract_satellites(frames: list[dict]) -> dict:
	for f in frames:
		if f["cls"] == UBX_CLASS_NAV and f["id"] == UBX_ID_NAV_SAT:
			return {'sat': parse_nav_sat(f["payload"])}
		if f["cls"] == UBX_CLASS_NAV and f["id"] == UBX_ID_NAV_SVINFO:
			return {'sat': parse_nav_svinfo(f["payload"])}
		if f["cls"] == UBX_CLASS_NAV and f["id"] == UBX_ID_NAV_DOP:
			return {'dop': parse_nav_dop(f["payload"])}
	return {}