#!/usr/bin/env python3 """BG22 IQ-Logger + Winkel-Schätzung, NEU AUFGESETZT nur nach den Prinzipien aus zwei öffentlichen GitHub-Referenzprojekten (keine eigene Switching-Pattern-Debatte mehr, kein separates Kalibrierfile -- siehe Begründung unten): 1) Navigine/Direction-of-Arrival-DoA-Estimation-Algorithm (github.com/Navigine/Direction-of-Arrival-DoA-Estimation-Algorithm) -> Kernidee übernommen: das 2D-Problem (Azimut, Elevation) wird NICHT als ein einziges 2D-Gitter gelöst, sondern in zwei UNABHÄNGIGE 1D-MUSIC-Scans aufgeteilt, je über ein virtuelles 2-Antennen-Array aus benachbarten Antennenpaaren. Phasendrift wird live aus einer Referenzperiode pro Paket geschätzt und abgezogen (dort: phase_ref aus 8 Referenz-Samples) -- das machen wir hier identisch über compensate_references(). 2) MSBeni/AoA_IQsamples (github.com/MSBeni/AoA_IQsamples, Ordner AoA/DoA_Final) -> Kernidee übernommen: mehrere klassische DOA-Schätzer (Bartlett, MUSIC) nebeneinander auf DERSELBEN Kovarianzmatrix laufen lassen, als gegenseitige Plausibilitätsprüfung (Bartlett ist unempfindlicher gegenüber Rangproblemen der Kovarianzmatrix, aber unschärfer als MUSIC). Warum neu aufgesetzt: das bisherige Skript (bg22_AoA.py) hatte zu viele bewegliche, gegenseitig abhängige Teile (3 Switching-Pattern-Hypothesen, Pro-Kanal-Phasen- kalibrierungsdatei, 3 Winkel-Algorithmen) -- jede Kombination aus falschem Pattern + falscher/fehlender Kalibrierungsdatei hat zu stundenlangem Debugging geführt. Dieses Skript hier: - nutzt NUR das offizielle Silicon-Labs-Doku-Beispiel-Switching-Pattern (0,1,2,...,15 -- keine Alternative mehr, keine Flags dafür), - braucht KEINE separate Kalibrierungsdatei (Phasendrift wird pro Paket live aus der Referenzperiode geschätzt, wie bei Navigine), - schätzt die Winkel NUR über die paarweise 1D-Methode (kein 2D-Gitter, kein ESPRIT, kein Forward-Backward-Averaging -- bewusst reduziert auf das, was in den beiden Referenzprojekten tatsächlich gezeigt und getestet wurde). """ from __future__ import annotations import argparse import json import queue import struct import sys import time from collections import deque from dataclasses import dataclass from datetime import datetime, timezone from typing import Any import numpy as np import bgapi # SWITCHING-PATTERN, korrigiert 08.10.26 nach der Silicon-Labs-Doku zur BRD4191A # ("Silicon Labs Dual Polarized Antenna Array Board", RTL-Lib App Development, # Appendix): 16 Patches mit je einem VERTIKALEN und einem HORIZONTALEN Element, # also 32 Antennen. Ein Pattern-Wert waehlt ein ELEMENT, nicht einen Patch. # Offizielle Reihenfolge V1,H1,V2,H2,...,V16,H16: # # Patch 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 # V,H 2,3 0,1 14,15 12,13 4,5 8,24 20,21 10,11 6,7 22,23 16,17 19,18 9,25 26,27 28,29 31,30 # # Frueher stand hier range(0, 16) mit der Annahme "Wert k = ANT(k+1)". Tatsaechlich # wurden damit nur 9 der 16 Patches abgetastet, 7 davon in BEIDEN Polarisationen, und # auf Rasterplaetze gelegt, an denen sie nicht liegen. Das erklaert alle Befunde seit # dem 02.10.26: Amplitudenfaktor 7, Paar-Kohaerenz 0.24, Phasenschritt 133.7 Grad # ueber dem physikalischen Maximum von 117, Boresight richtig, abseits falsch. # # Jetzt: EIN Element pro Patch, eine Polarisation, in Patch-Reihenfolge -- damit gilt # wieder Spalte k == Patch ANT(k+1), und antenna_position() bleibt unveraendert. # NACH JEDEM WECHSEL NEU KALIBRIEREN: die Offsets haengen am gewaehlten Element. _PATTERN_V = (2, 0, 14, 12, 4, 8, 20, 10, 6, 22, 16, 19, 9, 26, 28, 31) _PATTERN_H = (3, 1, 15, 13, 5, 24, 21, 11, 7, 23, 17, 18, 25, 27, 29, 30) POLARISATION = "V" # "V" oder "H": die mit der hoeheren, gleichmaessigeren Amplitude RF_ORDER = _PATTERN_V if POLARISATION == "V" else _PATTERN_H CP_RF_VALUE = 40 # Patch 6 zirkular polarisiert (CP_MODE | 8) -- laut Doku richtig @dataclass class Config: references: int = 8 # Anzahl Referenz-Samples am Paketanfang (fuer Phasendrift-Schaetzung) expected_samples: int = 45 slot_us: float = 2.0 reference_interval_us: float = 1.0 phase_multiplier: float = 2.0 pitch_x_m: float = 0.040 # Spaltenabstand (Azimut-Achse) pitch_y_m: float = 0.040 # Zeilenabstand (Elevation-Achse) flip_azimuth: bool = True # UMGESTELLT 08.10.26 (vorher False): kehrt das Vorzeichen des # Azimuts um. Technisch wird u gespiegelt (links/rechts); da # az = atan2(u, -v) gilt, ergibt -u exakt -az, ueber den ganzen # Kreis und auch bei +-180. Mit --no-flip-azimuth abschaltbar. flip_elevation: bool = True # Spiegelt v (oben/unten). Achtung: in der Kegel-Konvention dreht das # den AZIMUT (0 <-> 180), nicht die Elevation -- die ist jetzt immer >= 0. # BESTAETIGT per Realtest (29.09-01.10.26) gegen den Silicon Labs # AoA Analyzer: v-Vorzeichen war invertiert, per Default korrigiert. swap_axes: bool = False # Zeilen/Spalten-Rollen tauschen (Test: Array evtl. um 90° gedreht/row-col vertauscht) azimuth_mirror: bool = False # OHNE WIRKUNG seit der Kegel-Konvention (07.10.26). Die Spiegelung # war die Heuristik, mit der der halbe Azimutbereich [-90,+90] auf die # andere Hemisphaere abgebildet wurde. Der Azimut laeuft jetzt direkt # ueber den vollen Kreis, damit ist sie gegenstandslos. Das Feld bleibt # nur stehen, damit aeltere Aufrufe mit --azimuth-mirror nicht brechen. azimuth_offset_deg: float = 0.0 # Konstanter Azimut-Offset, ADDIERT ganz am Ende. Dreht die # Nullrichtung. Referenz ist der Silicon Labs AoA Analyzer: gleiches # Array, gleiche Firmware, gleiche Pakete, nur anderer Algorithmus. # Am 07.10.26 gegen den Analyzer geprueft (Pfeil im Oberen-Rechten, # Anzeige 132,97 Grad, Aufdruck dort 135 Grad): die Nullrichtung stimmt # bereits. Dieser Wert gehoert also auf 0; ein grosser Wert waere ein # Fehler in der Rechenkette, kein Konventionsunterschied. elevation_offset_deg: float = 0.0 # Analog fuer Elevation. def ble_frequency_hz(channel: int) -> float: """BLE-Kanal (0..39) auf Mittenfrequenz in Hz abbilden.""" if 0 <= channel <= 10: mhz = 2404 + 2 * channel elif 11 <= channel <= 36: mhz = 2428 + 2 * (channel - 11) elif channel == 37: mhz = 2402 elif channel == 38: mhz = 2426 elif channel == 39: mhz = 2480 else: raise ValueError(f"BLE Kanal ausserhalb 0..39: {channel}") return mhz * 1e6 def iq_complex(raw: Any) -> np.ndarray: """I/Q-Paare zu komplexem Signal, Konvention I - jQ (Silicon Labs).""" a = np.asarray(raw) if a.ndim != 2 or a.shape[1] != 2: raise ValueError("iq muss eine Liste von [I, Q] sein") return a[:, 0].astype(float) - 1j * a[:, 1].astype(float) DRIFT_GLAETTUNG = 0.05 # Gewicht des neuen Werts im gleitenden Mittel der Phasendrift. # 1.0 = kein Mittel, nur der Wert des aktuellen Pakets (frueheres # Verhalten). 0.05 entspricht einem Gedaechtnis von rund 20 Paketen, # also etwa einer Sekunde bei 50 ms Sendeintervall. _drift_zeiger: complex = 0j def drift_schaetzen(samples: np.ndarray, cfg: Config) -> float: """Phasendrift je Referenz-Sample aus der Referenzperiode schätzen. Das ist das Prinzip von Navigine's phase_ref: der Mittelwert der Phasenschritte zwischen aufeinanderfolgenden Referenz-Samples, hier über alle Referenz-Samples gemittelt statt nur über die ersten beiden. In der Referenzperiode sendet immer dieselbe Antenne, jede Drehung dort kommt also allein vom Frequenzunterschied zwischen Tag und Locator. """ nref = cfg.references if len(samples) < nref: raise ValueError(f"benötige {nref} Referenz-Samples, habe {len(samples)}") refs = samples[:nref] if np.any(np.abs(refs) < 1e-12): raise ValueError("Referenz-Sample mit Betrag 0") adjacent_products = refs[1:] * np.conj(refs[:-1]) return float(np.angle(np.mean(np.exp(1j * np.angle(adjacent_products))))) def geglaettete_drift(phase_step: float) -> float: """Gleitendes Mittel der Phasendrift über die Pakete hinweg. Warum: der Frequenzversatz zweier Quarze ändert sich über Sekunden kaum, die Schätzung aus nur 8 Referenz-Samples streut aber. Gemessen am 03.10.26 über drei Aufnahmen: Streuung der Drift je Paket rund 0.4 Grad, zwischen den Kanälen dagegen nur 0.16 Grad -- die Streuung ist also Messrauschen und keine echte Änderung, und ein kanalübergreifendes Mittel ist zulässig. Dieses Rauschen wird in compensate_references mit phase_multiplier * slot_us multipliziert und schlägt dann auf alle 16 Antennenphasen durch. Mit dem gleitenden Mittel stieg die Stabilität der Antennenphasen über die Pakete von 0.942 auf 0.967 (aufnahme_a), von 0.923 auf 0.982 (aufnahme_b) und von 0.904 auf 0.983 (aufnahme_c). Gemittelt werden Einheitszeiger, damit der Sprung bei plus oder minus 180 Grad nicht stört -- die gemessene Drift liegt mit rund 174 Grad dicht daran. """ global _drift_zeiger neu = np.exp(1j * phase_step) if _drift_zeiger == 0j: _drift_zeiger = neu else: _drift_zeiger = (1.0 - DRIFT_GLAETTUNG) * _drift_zeiger + DRIFT_GLAETTUNG * neu return float(np.angle(_drift_zeiger)) def compensate_references(samples: np.ndarray, cfg: Config, phase_step: float | None = None) -> np.ndarray: """Phasendrift aus dem Paket herausrechnen. phase_step: Drift je Referenz-Sample. Ohne Angabe schätzt die Funktion sie allein aus diesem Paket (Verhalten bis 03.10.26); process_packet übergibt den geglätteten Wert. """ nref = cfg.references if phase_step is None: phase_step = drift_schaetzen(samples, cfg) slope = phase_step / cfg.reference_interval_us corrected = samples.copy() corrected[nref:] *= np.exp(-1j * slope * cfg.phase_multiplier * (np.arange(len(samples) - nref) + 1) * cfg.slot_us) return corrected def snapshots_from_samples(data: np.ndarray, cfg: Config) -> np.ndarray: """Daten in der RF-Schaltreihenfolge gruppieren (16 Antennen pro Snapshot-Spalte). WICHTIG -- Zykluslaenge ist 17, nicht 16: Das an den Stack uebergebene Switching-Pattern ist (CP_RF_VALUE,) + RF_ORDER und damit 17 Eintraege lang. Die Hardware durchlaeuft ALLE 17, auch waehrend der Switching-Phase; die Referenzantenne laeuft also im Zyklus mit. Gemessen am 02.10.26 (struktur_check.py, 97 Pakete auf einem Kanal): Deckung aufeinanderfolgender Zyklen 0.999 bei Zykluslaenge 17, dagegen 0.519 bei der naechstbesten Laenge. Mit dem frueheren 16er-Schnitt verrutschte die Antennenzuordnung je Zyklus um eine Stelle. Folge auf denselben Paketen: Eigenwert-Verhaeltnis der Kovarianz 3.6 statt 2313, Kohaerenz je Antennenpaar 0.796 statt 1.000, Pseudospektrum flach, Ausgabe nur noch conf = 1.0 mit Werten in den Ecken des Suchrasters. Bei cfg.references = 8 beginnt der hier uebergebene Ausschnitt mit RF_ORDER[0]; CP_RF_VALUE liegt dann an der letzten Stelle des Zyklus und wird verworfen. Die Spalten 0..15 entsprechen damit weiterhin genau den Antennenindizes aus antenna_position(). """ n_ant = len(RF_ORDER) # 16 Rasterantennen n_cycle = n_ant + 1 # 17: CP_RF_VALUE laeuft im Zyklus mit n_full = len(data) // n_cycle if n_full < 1: raise ValueError(f"nur {len(data)} Antennen-Samples; brauche {n_cycle}") used = data[:n_full * n_cycle] return used.reshape(n_full, n_cycle)[:, :n_ant].T def antenna_position(ant_index: int) -> tuple[float, float, float]: """Physische Position (x, y, z) einer Antenne in Metern, 0-basiert (ant_index=0 -> ANT1). Board in der Y-Z-Ebene, Blickrichtung entlang X. Layout bestätigt über den Silicon Labs AoA Analyzer (3D-View, "Dual Polarized Board" / "Silicon Labs Ref. 4x4 Uniform Dual Polarized Rectangular Array"): ANT13 ANT9 ANT5 ANT1 ANT14 ANT10 ANT6 ANT2 ANT15 ANT11 ANT7 ANT3 ANT16 ANT12 ANT8 ANT4 ANT1..16 spaltenweise gruppiert, Spalten von RECHTS nach LINKS; innerhalb einer Gruppe von oben nach unten. Da wir hier NUR das Simple-Pattern (0..15 identisch) nutzen, ist logische Position == physischer Index, d.h. diese Funktion bestimmt die Geometrie direkt (keine rf_order-Indirektion mehr nötig). """ group = ant_index // 4 row_in_col = ant_index % 4 col = 3 - group return (0.0, col, row_in_col) # in Pitch-Einheiten, s. coordinates() def coordinates(cfg: Config) -> np.ndarray: """Antennenpositionen in Metern, in Snapshot-Spaltenreihenfolge (0..15). cfg.swap_axes tauscht testweise die Rollen von Spalte (Azimut) und Zeile (Elevation) -- falls das physische Array um 90° gedreht montiert ist oder die Zeilen/Spalten-Zuordnung aus dem AoA-Analyzer-Screenshot nicht zur tatsächlichen Verbauung passt.""" xyz_units = np.array([antenna_position(i) for i in range(16)], dtype=float) if cfg.swap_axes: xyz_units = xyz_units[:, [0, 2, 1]] # (x, col, row) -> (x, row, col) return xyz_units * np.array([1.0, cfg.pitch_x_m, cfg.pitch_y_m]) def find_pairs(xyz: np.ndarray, cfg: Config) -> tuple[list[tuple[int, int]], list[tuple[int, int]]]: """Vertikale Nachbarn (gleiche Spalte, fuer Elevation) und horizontale Nachbarn (gleiche Zeile, fuer Azimut) finden -- das sind die "Antennenpaare", auf denen Navigine's Methode aufbaut.""" n_ant = xyz.shape[0] tol = 1e-6 vertical_pairs, horizontal_pairs = [], [] for i in range(n_ant): for j in range(n_ant): dx, dy, dz = xyz[j] - xyz[i] if abs(dx) < tol and abs(dy) < tol and abs(dz - cfg.pitch_y_m) < tol: vertical_pairs.append((i, j)) if abs(dx) < tol and abs(dy - cfg.pitch_x_m) < tol and abs(dz) < tol: horizontal_pairs.append((i, j)) return vertical_pairs, horizontal_pairs def scan_1d(X: np.ndarray, pairs: list[tuple[int, int]], wave: float, spacing: float, coupling: float = 1.0, step_deg: float = 1.0) -> dict: """Virtuelles 2-Antennen-Array aus ALLEN (Paar x Snapshot)-Kombinationen bauen und sowohl MUSIC als auch Bartlett über -90..+90 Grad laufen lassen (Navigine fuer MUSIC, MSBeni fuer Bartlett als Gegenprobe). Gibt beide Schätzungen zurueck, damit man sie im Log vergleichen kann.""" ref = np.array([X[i, :] for i, j in pairs]) nxt = np.array([X[j, :] for i, j in pairs]) Xv = np.stack([ref.reshape(-1), nxt.reshape(-1)]) # 2 x (n_pairs*n_snap) n = Xv.shape[1] R = (Xv @ Xv.conj().T) / n R = R + (1e-3 * np.trace(R).real / 2) * np.eye(2) vals, vecs = np.linalg.eigh(R) order = np.argsort(vals)[::-1] vals, vecs = vals[order], vecs[:, order] En = vecs[:, 1:] # 1 Quelle, 2 Antennen -> 1-dim. Rauschunterraum angles = np.arange(-90.0, 90.0 + step_deg / 2, step_deg) music_power = np.empty(len(angles)) bartlett_power = np.empty(len(angles)) for idx, ang in enumerate(np.radians(angles)): a = np.array([1.0, np.exp(1j * wave * spacing * coupling * np.sin(ang))])[:, None] denom = float(np.linalg.norm(En.conj().T @ a) ** 2) music_power[idx] = 1.0 / max(denom, 1e-15) bartlett_power[idx] = float(np.abs((a.conj().T @ R @ a))[0, 0]) # Bartlett/Fourier-Beamformer music_peak = int(np.argmax(music_power)) bartlett_peak = int(np.argmax(bartlett_power)) return { "music_deg": float(angles[music_peak]), "music_confidence": float(music_power[music_peak] / (np.median(music_power) + 1e-15)), "bartlett_deg": float(angles[bartlett_peak]), "bartlett_confidence": float(bartlett_power[bartlett_peak] / (np.median(bartlett_power) + 1e-15)), } def mirror_azimuth(az_deg: float) -> float: """Spiegelt einen im sicheren Bereich [-90,+90] gemessenen Azimut an der 90°-Achse auf die physikalisch korrekte Vorne/Hinten-Hemisphaere (siehe Config.azimuth_mirror): 25° -> 155°, -25° -> -155°, 90°/-90° bleiben an der Mehrdeutigkeitsgrenze (dort ist cos(az)~0, Vorne/Hinten ohnehin nicht unterscheidbar -- s. on_boundary-Check).""" if az_deg >= 0: return 180.0 - az_deg return -180.0 - az_deg def doa_pairs(X: np.ndarray, frequency_hz: float, cfg: Config) -> dict: """Haupt-Winkel-Schaetzung in der KEGEL-KONVENTION (umgestellt 07.10.26). Ausgabe: Azimut 0 .. +360 Grad -- Drehwinkel UM die Boresight-Achse (Bereich umstellbar, s. AZIMUT_0_360) Elevation 0 .. +90 Grad -- Winkel ueber der ARRAY-EBENE (90 = Boresight, 0 = in der Array-Ebene), gleiche Achse wie der Silicon Labs AoA Analyzer Warum diese Konvention die richtige fuer ein planares Array ist: gemessen werden zwei Richtungskosinus, u entlang der Spaltenachse (+y) und v entlang der Zeilenachse (+z). Beide zusammen legen den Drehwinkel um die Boresight-Achse EINDEUTIG fest -- daher der volle Kreis beim Azimut. Was ein planares Array prinzipiell NICHT unterscheiden kann, ist vorne von hinten (eine Quelle bei +w und bei -w erzeugt dieselben Phasen). Genau diese Mehrdeutigkeit ist der Grund, warum die Elevation nur 0 bis 90 Grad abdeckt: der gesamte Messbereich ist die vordere Halbkugel. Die Elevation zaehlt ab der ARRAY-EBENE, nicht ab der Boresight-Achse: 90 Grad heisst frontal, 0 Grad heisst in der Ebene liegend. Das ist die Achse des Silicon Labs AoA Analyzer, am 07.10.26 am Aufbau bestaetigt (Tag frontal -> Analyzer zeigt nahe 90). Damit sind beide Anzeigen direkt vergleichbar. Die alte az/el-Konvention (beide Achsen -90..+90) warf dagegen Information weg: sie presste den vollen Drehwinkel in einen Halbkreis und brauchte dafuer mirror_azimuth() als Heuristik. Das entfaellt jetzt. Nullrichtung des Azimuts: 0 Grad zeigt NACH UNTEN, +90 Grad nach rechts (Blick auf die Antennenseite), 180 Grad nach oben, -90 Grad nach links -- also so, wie es auf der Karte aufgedruckt ist. Stimmt das bei dir nicht, verdreht cfg.azimuth_offset_deg die Nullrichtung. Nebeneffekt: die Azimut-Paare laufen jetzt mit coupling = -1 statt -cos(Elevation). Der cos-Faktor war ein Artefakt der alten Parametrisierung. Die beiden 1D-Scans messen u und v unabhaengig voneinander, wie es der Physik eines planaren Arrays entspricht; die Kopplung entsteht erst bei der Umrechnung in Winkel. """ xyz = coordinates(cfg) n_ant, n_snap = X.shape if n_ant != 16 or n_snap < 2: raise ValueError("doa_pairs braucht 16 Antennen und mind. 2 Snapshots") vertical_pairs, horizontal_pairs = find_pairs(xyz, cfg) wave = 2 * np.pi * frequency_hz / 299_792_458.0 hor = scan_1d(X, horizontal_pairs, wave, cfg.pitch_x_m, coupling=-1.0) ver = scan_1d(X, vertical_pairs, wave, cfg.pitch_y_m) u_sign = -1.0 if cfg.flip_azimuth else 1.0 v_sign = -1.0 if cfg.flip_elevation else 1.0 def in_winkel(h_deg: float, v_deg: float) -> tuple[float, float]: u = u_sign * float(np.sin(np.radians(h_deg))) v = v_sign * float(np.sin(np.radians(v_deg))) return winkel_aus_richtungskosinus(u, v) az_deg, el_deg = in_winkel(hor["music_deg"], ver["music_deg"]) az_bartlett_deg, el_bartlett_deg = in_winkel(hor["bartlett_deg"], ver["bartlett_deg"]) # Konstante Offsets zuletzt. Azimut wird auf -180..180 zurueckgefaltet, # Elevation auf 0..90 begrenzt -- ausserhalb gibt es keine Messung. def falten(w: float) -> float: w = (w + cfg.azimuth_offset_deg) % 360.0 return w if AZIMUT_0_360 else (w + 180.0) % 360.0 - 180.0 az_deg = falten(az_deg) az_bartlett_deg = falten(az_bartlett_deg) el_deg = min(max(el_deg + cfg.elevation_offset_deg, 0.0), 90.0) el_bartlett_deg = min(max(el_bartlett_deg + cfg.elevation_offset_deg, 0.0), 90.0) return { "azimuth_deg": az_deg, "elevation_deg": el_deg, "confidence": float(min(ver["music_confidence"], hor["music_confidence"])), # Bartlett-Gegenprobe (MSBeni): grosse Abweichung von MUSIC = Schaetzung unsicher "azimuth_bartlett_deg": az_bartlett_deg, "elevation_bartlett_deg": el_bartlett_deg, # Seit der Umstellung identisch mit azimuth_deg: es gibt keinen getrennten # "sicheren" Bereich mehr, der Azimut ist ueber den vollen Kreis eindeutig. "azimuth_internal_deg": az_deg, } KALIBRIER_DATEI = "antennen_kalibrierung.json" _kalibrierung: dict | None = None def lade_kalibrierung() -> dict | None: """Antennen-Phasenoffsets aus antennen_kalibrierung.json laden, falls vorhanden. Fehlt die Datei, laeuft alles wie frueher weiter -- dann allerdings mit dem bekannten Problem, dass die Mittelung ueber die Antennenpaare in scan_1d an den unterschiedlichen Geraeteoffsets zerbricht (gemessen 02.10.26: Kohaerenz je Paar 1.000, nach Mittelung 0.474, Ergebnis conf = 1.0). Erzeugt wird die Datei von kalibrierung.py aus einer Boresight-Aufnahme. """ try: with open(KALIBRIER_DATEI, "r", encoding="utf-8") as f: daten = json.load(f) except FileNotFoundError: return None except (json.JSONDecodeError, OSError) as fehler: print(f"{KALIBRIER_DATEI} nicht lesbar ({fehler}) -- laufe ohne Kalibrierung.", file=sys.stderr) return None if list(daten.get("rf_order", [])) != list(RF_ORDER): print(f"{KALIBRIER_DATEI} wurde mit einem ANDEREN Switching-Pattern aufgenommen " f"({daten.get('rf_order', 'alt, 0..15')}) -- wird NICHT verwendet. " f"Neu kalibrieren: Boresight-Aufnahme, dann kalibrierung.py.", file=sys.stderr) return None global_rad = np.asarray(daten.get("global_rad", []), dtype=float) if global_rad.shape != (len(RF_ORDER),): print(f"{KALIBRIER_DATEI} passt nicht zu {len(RF_ORDER)} Antennen " f"-- laufe ohne Kalibrierung.", file=sys.stderr) return None kanal = {} for schluessel, werte in (daten.get("kanal_rad") or {}).items(): reihe = np.asarray(werte, dtype=float) if reihe.shape == global_rad.shape: kanal[int(schluessel)] = reihe return {"global": global_rad, "kanal": kanal, "quelle": daten.get("quelle", "?"), "stabilitaet": daten.get("stabilitaet"), "pakete": daten.get("pakete")} def kalibrierung_anwenden(X: np.ndarray, channel: int) -> np.ndarray: """Feste Phasenoffsets der Antennen herausrechnen. Die Offsets sind leicht frequenzabhaengig (Leiterbahnlaufzeit), deshalb wird der zum Kanal passende Satz bevorzugt; fuer Kanaele ohne eigene Messung dienen die kanaluebergreifenden Werte als Rueckfallebene. """ if _kalibrierung is None: return X offsets = _kalibrierung["kanal"].get(channel, _kalibrierung["global"]) return X * np.exp(-1j * offsets)[:, None] # ------------------------------------------------------------------ Selbstkalibrierung # Gemessen am 08.10.26 auf 1008 Paketen (iq_samples.jsonl, 37 Kanaele): # # Stabilitaet des Phasenvektors INNERHALB eines Kanals 0.944 (19.4 Grad) # Stabilitaet ueber alle 37 Kanaele gemischt 0.830 (35.0 Grad) # Streuung der Antennenoffsets ZWISCHEN den Kanaelen 32.4 Grad RMS # # Die Offsets sind also kanalabhaengig, und antennen_kalibrierung.json stammt aus # einem ANDEREN Programmlauf, in dem sie andere Werte hatten (gemessen 37.3 Grad # RMS Unterschied zwischen Laeufen). Ergebnis: die Datei korrigiert zwar etwas, # aber nicht die Kanalabhaengigkeit DIESES Laufs. # # Abhilfe hier: die Kanaltabelle wird AUS DEM LAUFENDEN LAUF selbst geschaetzt. # Pro Kanal wird der mittlere (normierte) Phasenvektor gesammelt; die Korrektur # ist die Abweichung dieses Kanals vom Mittel ueber alle Kanaele. Dafuer muss die # Richtung NICHT bekannt sein -- es genuegt, dass der Tag waehrend der Lernphase # still steht. Nach SELBSTKAL_LERNPAKETE wird die Tabelle eingefroren, damit ein # spaeter bewegter Tag sie nicht verdirbt. # # WICHTIG, was das NICHT tut: die festen Offsets je Antenne (Leiterbahnlaufzeit) # bleiben unangetastet, denn die stecken im Mittel ueber alle Kanaele und werden # hier gerade herausgerechnet. Gegen den raeumlichen Fehler (Kohaerenz je Paar # 0.38, Ebene-Welle-Fit 0.57) hilft das also nicht -- gemessen: der Fit aendert # sich dadurch um 0.001. Diese Funktion verbessert die STREUUNG, nicht die # absolute Richtung. # Dimensionierung aus einem Out-of-Sample-Test am 08.10.26 (Tabelle auf den ersten # N Paketen gelernt, Fehler auf ALLEN UEBRIGEN gemessen, 1008 Pakete / 37 Kanaele): # # N Kanaele ohne Korrektur mit Korrektur nach Block von 30 # 200 22 33.4 Grad 11.5 Grad 6.1 -> 2.1 Grad # 400 36 32.6 Grad 9.0 Grad 6.0 -> 1.7 Grad # 600 37 32.9 Grad 7.9 Grad 6.0 -> 1.4 Grad # Decke (Streuung innerhalb eines Kanals) 7.4 Grad -> 1.4 Grad # # Ab 600 Paketen ist die Decke praktisch erreicht, deshalb dieser Wert: rund 12 s # bei 20 ms Advertising-Intervall. Die Korrektur wird ERST NACH der Lernphase # angewendet, nie mit einer noch wandernden Tabelle -- eine sich aendernde # Korrektur verschmiert sonst genau das, was sie reparieren soll (gemessen: # Streuung innerhalb eines Kanals fiel dabei von 0.992 auf 0.881). SELBSTKAL_AKTIV = True SELBSTKAL_MIN_PAKETE = 8 # Pakete auf einem Kanal, damit er in die Tabelle kommt SELBSTKAL_LERNPAKETE = 600 # danach ist die Tabelle fest und wird angewendet _selbstkal_summe: dict[int, np.ndarray] = {} _selbstkal_anzahl: dict[int, int] = {} _selbstkal_tabelle: dict[int, np.ndarray] = {} _selbstkal_gesehen = 0 _selbstkal_fertig = False def _selbstkal_tabelle_rechnen() -> None: """Kanaltabelle aus den bisher gesammelten Mittelwerten neu aufbauen.""" reif = {c: summe / _selbstkal_anzahl[c] for c, summe in _selbstkal_summe.items() if _selbstkal_anzahl[c] >= SELBSTKAL_MIN_PAKETE} if len(reif) < 2: return stapel = [] for vektor in reif.values(): betrag = np.abs(vektor) if np.any(betrag < 1e-12): return stapel.append(vektor / betrag) mittel = np.array(stapel).mean(axis=0) betrag = np.abs(mittel) if np.any(betrag < 1e-12): return mittel = mittel / betrag _selbstkal_tabelle.clear() for kanal, vektor in reif.items(): korrektur = (vektor / np.abs(vektor)) * np.conj(mittel) _selbstkal_tabelle[kanal] = korrektur / np.abs(korrektur) def selbstkalibrierung_lernen(X: np.ndarray, channel: int) -> None: """Phasenvektor dieses Pakets in die Kanalstatistik einrechnen.""" global _selbstkal_gesehen, _selbstkal_fertig if not SELBSTKAL_AKTIV or _selbstkal_fertig: return vektor = X.mean(axis=1) betrag = np.abs(vektor) if np.any(betrag < 1e-12): return vektor = vektor / betrag # VERANKERN: jedes CTE-Paket startet mit beliebiger Gesamtphase. Ohne diesen # Schritt mittelt sich ueber die Pakete genau diese Zufallsphase mit ein und # die Kanaltabelle wird unbrauchbar (gemessen 08.10.26: 16.0 statt 7.9 Grad # Restfehler). Fuer die Richtungsschaetzung sind nur Phasen-DIFFERENZEN # zwischen Antennen relevant, eine gemeinsame Drehung ist also unschaedlich. anker = vektor.mean() if abs(anker) < 1e-3: anker = vektor[0] # Rueckfall, falls der Mittelwert ausloescht vektor = vektor * np.conj(anker / abs(anker)) if channel in _selbstkal_summe: _selbstkal_summe[channel] = _selbstkal_summe[channel] + vektor _selbstkal_anzahl[channel] += 1 else: _selbstkal_summe[channel] = vektor _selbstkal_anzahl[channel] = 1 _selbstkal_gesehen += 1 if _selbstkal_gesehen >= SELBSTKAL_LERNPAKETE: _selbstkal_tabelle_rechnen() _selbstkal_fertig = True fehlt = sorted(k for k, n in _selbstkal_anzahl.items() if n < SELBSTKAL_MIN_PAKETE) print(f"Selbstkalibrierung: aktiv nach {_selbstkal_gesehen} Paketen, " f"{len(_selbstkal_tabelle)} Kanaele in der Tabelle" + (f", ohne Korrektur bleiben {fehlt}" if fehlt else "") + ".", flush=True) def selbstkalibrierung_anwenden(X: np.ndarray, channel: int) -> np.ndarray: """Kanalabhaengigen Anteil der Antennenoffsets herausrechnen.""" if not SELBSTKAL_AKTIV or not _selbstkal_fertig: return X # waehrend der Lernphase unveraendert, s. Kommentar oben korrektur = _selbstkal_tabelle.get(channel) if korrektur is None: return X return X * np.conj(korrektur)[:, None] def process_packet(packet: dict, cfg: Config) -> dict | None: """Ein IQ-Paket verarbeiten und Snapshots fuer die Winkel-Schätzung liefern (oder None).""" ch = int(packet["channel"]) f = ble_frequency_hz(ch) samples = iq_complex(packet["iq"]) if len(samples) != cfg.expected_samples: return None fixed = compensate_references(samples, cfg, geglaettete_drift(drift_schaetzen(samples, cfg))) X = snapshots_from_samples(fixed[cfg.references:], cfg) X = kalibrierung_anwenden(X, ch) selbstkalibrierung_lernen(X, ch) # lernt auf dem noch unkorrigierten Vektor X = selbstkalibrierung_anwenden(X, ch) return {"X": X, "frequency_hz": f} COM_PORT = "COM13" BAUD_RATE = 115200 XAPI_PATH = r"C:\Users\sandr\.silabs\slt\installs\conan\p\simpleca33d691c539\p\bluetooth_le_host\api\sl_bt.xapi" IQ_FILE = "iq_samples.jsonl" ANGLE_TABLE_FILE = "winkel_tabelle.json" SLOT_DURATION_US = 2 CTE_COUNT = 0 ACCUMULATE_PACKETS = 1 # MUSIC nach JEDEM Paket (03.10.26). Ein Paket liefert 2 Snapshots # (2 volle 17er-Zyklen aus 45 Samples), also 2 x 12 Paare = 24 Spalten # im virtuellen Array von scan_1d -- genug fuer die 2x2-Kovarianz, aber # deutlich verrauschter als vorher mit 8 Paketen. Die Streuung faengt # dafuer die Glaettung ueber FILTER_WINDOW Werte auf. FILTER_WINDOW = 30 # Blockgroesse (07.10.26, vorher 100): je 50 angenommene Schaetzungen ergeben EINEN Mittelwert. # Bei ACCUMULATE_PACKETS = 1 und 40 ms Sendeintervall sind das rund # 4 Sekunden -- vorausgesetzt, alle Pakete kommen durch den Filter. FILTER_MAX_ALTER_S = 8.0 # Zusaetzliche Altersgrenze (05.10.26). FILTER_WINDOW allein zaehlt # ANGENOMMENE Schaetzungen, nicht Pakete: faellt die Annahmequote auf # 20 %, umfasst das Fenster 20 s statt 4 s und der Ausgabewert haengt # minutenlang an einer alten Kartenstellung fest. Werte, die aelter # als FILTER_MAX_ALTER_S sind, verfallen deshalb unabhaengig davon. AUSREISSER_SIGMA = 2.5 # Verwirft Werte, die weiter als 2.5 Sigma vom gewichteten Schwerpunkt # liegen (05.10.26). CONFIDENCE_MIN allein genuegt nicht: ein Multipath- # Peak ist scharf, also hoch-konfident UND falsch. PRINT_EVERY_N_PACKETS = 10 CONFIDENCE_MIN = 1.0 # Praktisch AUS seit 07.10.26 (vorher 5.0). Der AoA Analyzer kennt keine # solche Schwelle und liefert auch bei streifendem Einfall einen Wert; dieses # Skript soll sich genauso verhalten. Gemessen an der Aufnahme von 23:14: # mit 5.0 wurden 966 von 969 Paketen verworfen, die Winkel-Tabelle blieb leer. # Die Konfidenz steht weiter in jeder Logzeile -- sie bewertet die Schaetzung, # verwirft sie aber nicht mehr. lib = None iq_file = None already_synced = False pending_actions = queue.Queue() snapshot_buffer: dict[int, list[np.ndarray]] = {} angle_table: list[dict] = [] last_azimut = None last_elevation = None # Ein gemeinsamer Puffer statt zwei getrennter (05.10.26): Azimut und Elevation gehoeren # zusammen, weil der Azimut-Scan mit coupling = -cos(el) der GLEICHEN Schaetzung rechnet. # Eintrag: (Zeitstempel, u, v, Gewicht) -- u/v sind Richtungskosinus, s. richtungskosinus(). _recent: deque = deque() # kein maxlen: der Block wird nach FILTER_WINDOW Werten geleert _letzter_block: tuple | None = None # (az, el, streuung, n) des zuletzt fertigen Blocks ACTIVE_CONFIG = Config() SHUTTING_DOWN = False # verhindert Schreibzugriff auf iq_file nach dessen Schliessung beim Beenden def rssi_weight(rssi_dbm: int) -> float: """Gewichtungsfaktor aus RSSI: stärkeres Signal zählt mehr im gewichteten Median.""" return max(rssi_dbm + 100.0, 1.0) AZIMUT_0_360 = False # Ausgabebereich des Azimuts (08.10.26, umgestellt): -180 bis +180, # wie der AoA Analyzer. Keine Werte ueber 180 Grad mehr. True: 0 bis 360, keine # negativen Werte. False: -180 bis +180. Nur die DARSTELLUNG aendert sich, # gerechnet wird weiter in Richtungskosinus -- 350 und -10 sind dieselbe # Richtung. Die Mittelung bleibt davon unberuehrt, weil sie u und v mittelt # und nicht Gradzahlen; ein Mittelwert um 0 herum kippt also nicht. def richtungskosinus(az_deg: float, el_deg: float) -> tuple[float, float]: """(Azimut, Elevation) -> Richtungskosinus (u, v) in der Array-Ebene. u = Anteil entlang +y (Spaltenachse, nach rechts) v = Anteil entlang +z (Zeilenachse, nach oben) Kegel-Konvention (07.10.26): el ist der Kegelwinkel zur Boresight-Achse (0 = frontal, 90 = in der Array-Ebene), az der Drehwinkel um diese Achse mit 0 Grad nach unten und +90 Grad nach rechts. u und v sind das, was das Array physikalisch misst: die Phasenrampe ueber die Antennen ist LINEAR in u bzw. v, nicht in Grad. Deshalb wird in diesem Raum gemittelt und erst am Ende zurueckgerechnet. """ el = np.radians(min(max(el_deg, 0.0), 90.0)) az = np.radians(az_deg) r = np.cos(el) # 90 Grad = Boresight -> r = 0; 0 Grad = Array-Ebene -> r = 1 return float(r * np.sin(az)), float(-r * np.cos(az)) def winkel_aus_richtungskosinus(u: float, v: float) -> tuple[float, float]: """Rueckweg von (u, v) nach (Azimut -180..180, Elevation 0..90). u^2 + v^2 > 1 kann durch Rauschen entstehen und hat keine Richtung mehr: der Punkt wird dann auf den Einheitskreis gezogen, also auf Elevation 90. """ r = float(np.hypot(u, v)) if r > 1.0: u, v, r = u / r, v / r, 1.0 el_deg = float(np.degrees(np.arccos(r))) if r < 1e-9: return 0.0, el_deg az_deg = float(np.degrees(np.arctan2(u, -v))) if AZIMUT_0_360: az_deg = az_deg % 360.0 return az_deg, el_deg def schaetz_gewicht(confidence: float) -> float: """Gewicht einer Einzelschaetzung fuer die Glaettung. Bis 04.10.26 wurde mit rssi_weight() gewichtet -- das war der Fehler in der Glaettung: RSSI sagt, wie STARK das Signal war, nicht wie GUT die Schaetzung ist. Ueber den praktischen Bereich (-45 bis -65 dBm) spreizt rssi_weight nur 55:35, also faktisch gar nicht, waehrend die Konfidenz zwischen 5 und 105 schwankt. Eine Schaetzung, die den Filter gerade so passiert hat (conf 5.1), zaehlte damit genauso viel wie eine saubere (conf 105). """ return (max(confidence - CONFIDENCE_MIN, 0.0) + 1.0) ** 2 def glaettung_zuruecksetzen() -> None: """Angefangenen Block und letztes Ergebnis verwerfen -- nach dem Umstellen der Karte aufrufen, dann vermischt der naechste Block keine zwei Stellungen.""" global _letzter_block _recent.clear() _letzter_block = None def smoothed_angle(new_az: float, new_el: float, weight: float) -> tuple[float | None, float | None, float | None, int, bool]: """BLOCKMITTELWERT (05.10.26): sammelt FILTER_WINDOW Schaetzungen und gibt dann EINEN Mittelwert aus, danach beginnt ein neuer, leerer Block. Rueckgabe: (Azimut, Elevation, Streuung in Grad, Anzahl, neu). "neu" ist nur in dem Paket True, in dem ein Block fertig geworden ist -- nur dann kommt ein Wert in die Winkel-Tabelle. Solange der erste Block laeuft, sind Azimut/Elevation/Streuung None; danach wird zwischen zwei fertigen Bloecken der zuletzt fertige Wert weitergereicht, damit die Anzeige nicht leer faellt. Unterschied zum gleitenden Mittel (Stand bis 05.10.26): dort lief ein Fenster von 100 Werten mit, und jedes Paket lieferte einen neuen, mit dem vorherigen stark korrelierten Ausgabewert. Hier ist jeder Ausgabewert aus 100 FRISCHEN, nicht wiederverwendeten Schaetzungen gerechnet. Die Bloecke sind damit voneinander unabhaengig, was sie fuer eine Messreihe erst auswertbar macht: zwei aufeinanderfolgende Bloecke sind zwei echte Wiederholungsmessungen. Preis dafuer: ein Ausgabewert nur noch alle FILTER_WINDOW Pakete, bei rund 19 Paketen/s also etwa alle 2,6 Sekunden (und entsprechend laenger, wenn der Konfidenzfilter Pakete verwirft). Gerechnet wird weiter in Richtungskosinus (s. richtungskosinus()), damit Azimut und Elevation ihre Kopplung ueber cos(el) behalten. """ global _letzter_block jetzt = time.monotonic() # Lange Pause: angefangenen Block verwerfen, sonst mischt er zwei Stellungen. if _recent and (jetzt - _recent[-1][0]) > FILTER_MAX_ALTER_S: _recent.clear() u, v = richtungskosinus(new_az, new_el) _recent.append((jetzt, u, v, max(weight, 1e-6))) if len(_recent) < FILTER_WINDOW: if _letzter_block is None: return None, None, None, len(_recent), False az, el, s, n = _letzter_block return az, el, s, n, False daten = np.asarray(_recent, dtype=float) _recent.clear() # Block abgeschlossen, naechster faengt bei 0 an uu, vv, ww = daten[:, 1], daten[:, 2], daten[:, 3] u_m = float(np.average(uu, weights=ww)) v_m = float(np.average(vv, weights=ww)) # Eine Ausreisser-Runde: Mittelwert bleibt Mittelwert, aber ein einzelner # Multipath-Treffer kippt den Block nicht. abstand = np.hypot(uu - u_m, vv - v_m) sigma = float(np.sqrt(np.average(abstand ** 2, weights=ww))) if sigma > 1e-9: gut = abstand <= AUSREISSER_SIGMA * sigma if int(gut.sum()) >= 4: uu, vv, ww = uu[gut], vv[gut], ww[gut] u_m = float(np.average(uu, weights=ww)) v_m = float(np.average(vv, weights=ww)) streuung = float(np.degrees(np.sqrt(np.average((uu - u_m) ** 2 + (vv - v_m) ** 2, weights=ww)))) az_deg, el_deg = winkel_aus_richtungskosinus(u_m, v_m) _letzter_block = (az_deg, el_deg, streuung, int(len(ww))) return az_deg, el_deg, streuung, int(len(ww)), True def signed_iq(raw_bytes: bytes) -> tuple[list[int], list[int]]: """Rohbytes in vorzeichenbehaftete I- und Q-Listen entpacken.""" values = struct.unpack(f"{len(raw_bytes)}b", raw_bytes) return list(values[0::2]), list(values[1::2]) # ---------------------------------------------------------------- Ausgabe-Konvention # Intern rechnet alles in der KEGEL-Konvention: Elevation 90 = Boresight, 0 = in der # Array-Ebene. In dieser Form wird auch gemittelt (ueber u und v). Sie bleibt # unangetastet: richtungskosinus() und winkel_aus_richtungskosinus() sind zueinander # invers und duerfen nicht auseinanderlaufen, sonst bricht die Glaettung. # # Der AoA Analyzer misst die Elevation VOM Boresight aus: dort 0 statt 90. Beide # beschreiben dieselbe Richtung, sie sind komplementaer. Abgeleitet aus zwei Messungen # am selben Aufbau (08.10.26): # # Boresight Analyzer Az 1.54 El 2.06 # Stellung "133" Analyzer Az 133.67 El 10.83 # # Ein Azimut von 133 Grad schliesst aus, dass der Azimut ein horizontaler Winkel ist # -- ein solcher waere auf +-90 begrenzt. Der Analyzer benutzt also dieselbe polare # Form wie dieses Skript (Azimut = Drehung um die Boresight-Achse, -180..180), nur mit # dem anderen Nullpunkt der Elevation. # # NOCH ZU BESTAETIGEN: die Elevation stimmt mit beiden Messpunkten ueberein, das # Vorzeichen des Azimuts abseits der Achse nicht -- am Boresight ist der Azimut # bedeutungslos und taugt nicht als Beleg. Dafuer eine Messung in Stellung "133" mit # der frischen Kalibrierung gegen Az 133.67 / El 10.83 halten. AUSGABE_KONVENTION = "analyzer" # = interne Kegel-Form: Elevation 90 = Boresight, wie der Analyzer def ausgabe_winkel(az_deg: float, el_deg: float) -> tuple[float, float]: """Ausgabe in der Konvention des AoA Analyzers -- sie ist IDENTISCH mit der internen Kegel-Konvention, es wird nichts umgerechnet. Belegt am 08.10.26 durch zwei Analyzer-Ansichten am selben Aufbau: - Tag frontal vor der Karte (Pfeil von vorn als Kreisscheibe zu sehen): Az 4.39, El 82.29 -> Elevation 90 = Boresight - Azimut-Marken 0/45/90/135/180/-135/-90/-45 rings um die Karte, 0 unten, 90 rechts -> Azimut = Drehung um die Boresight-Achse Genau so rechnet dieses Skript intern. Zwei zwischenzeitliche Umrechnungen (90 - el, dann sphaerisch) beruhten auf der Fehldeutung einer Analyzer-Anzeige El 2.06 als "frontal" -- der Tag lag dort tatsaechlich fast in der Array-Ebene. """ return az_deg, el_deg def add_to_angle_table(zeit: str, counter: int, azimut: float, elevation: float) -> None: """Neuen Winkel nur speichern, wenn er sich vom letzten unterscheidet.""" global last_azimut, last_elevation if azimut == last_azimut and elevation == last_elevation: return last_azimut, last_elevation = azimut, elevation angle_table.append({"zeit": zeit, "counter": counter, "azimut": azimut, "elevation": elevation}) with open(ANGLE_TABLE_FILE, "w", encoding="utf-8") as f: json.dump(angle_table, f, indent=2, ensure_ascii=False) def save_iq_report(event: Any) -> None: """IQ-Paket speichern, Pakete akkumulieren und Winkel schätzen.""" global snapshot_buffer i_values, q_values = signed_iq(event.samples) channel = int(event.channel) packet = {"channel": channel, "rssi": int(event.rssi), "iq": list(zip(i_values, q_values))} zeit = datetime.now(timezone.utc).isoformat() record = {"zeit": zeit, "counter": int(event.event_counter), "channel": channel, "rssi": int(event.rssi), "i": i_values, "q": q_values, "azimut": None, "elevation": None} result = process_packet(packet, ACTIVE_CONFIG) if result is not None: channel_buffer = snapshot_buffer.setdefault(channel, []) channel_buffer.append(result["X"]) if len(channel_buffer) >= ACCUMULATE_PACKETS: combined = np.concatenate(channel_buffer, axis=1) snapshot_buffer[channel] = [] try: angles = doa_pairs(combined, result["frequency_hz"], ACTIVE_CONFIG) az_raw, el_raw, conf = angles["azimuth_deg"], angles["elevation_deg"], angles["confidence"] # Randwert-Pruefung auf dem UNGESPIEGELTEN Azimut: nahe +-90° ist cos(az)~0, dort ist # die Vorne/Hinten-Spiegelung selbst instabil (kleine Fehler koennen die Hemisphaere # kippen) -- unabhaengig davon, wie weit der gespiegelte, berichtete Wert az_raw liegt. # Randwert-Filter ENTFERNT am 07.10.26 auf Wunsch: streifende Einfaelle # (Elevation nahe 0, Quelle in der Array-Ebene) werden nicht mehr verworfen. # Der AoA Analyzer liefert dort ebenfalls einen Wert, also soll dieses Skript # es auch. Der Azimut ist dort allerdings empfindlicher, weil u^2+v^2 gegen 1 # geht -- sichtbar an der Konfidenz, nicht mehr an einem harten Filter. on_boundary = False low_confidence = conf < CONFIDENCE_MIN bartlett_diff = (abs(angles["azimuth_bartlett_deg"] - az_raw) + abs(angles["elevation_bartlett_deg"] - el_raw)) if on_boundary or low_confidence: reason = "Randwert" if on_boundary else "geringe Konfidenz" print(f">>> AoA roh (verworfen, {reason}, conf={conf:.1f}): " f"Azimut={az_raw:+.1f}° Elevation={el_raw:+.1f}°", file=sys.stderr, flush=True) else: az_smooth, el_smooth, streuung, n_used, block_fertig = smoothed_angle( az_raw, el_raw, schaetz_gewicht(conf)) record["azimut"] = az_smooth record["elevation"] = el_smooth record["streuung"] = streuung record["n_block"] = n_used record["block_fertig"] = block_fertig # Bartlett-Werte werden seit 05.10.26 nicht mehr ausgegeben (zu unruhig # auf der Konsole). Die Gegenprobe LAEUFT weiter, nur die Zahlen bleiben # unsichtbar; weicht sie stark ab, erscheint weiterhin ein kurzer Hinweis. bart = " [ACHTUNG: Gegenprobe weicht ab]" if bartlett_diff > 15.0 else "" az_raw_out, el_raw_out = ausgabe_winkel(az_raw, el_raw) roh = (f">>> AoA roh: Azimut={az_raw_out:+.1f}° Elevation={el_raw_out:+.1f}° " f"(conf={conf:.1f})") if az_smooth is None: # Erster Block laeuft noch, es gibt schlicht noch keinen Mittelwert. print(f"{roh} | sammelt {n_used}/{FILTER_WINDOW}{bart}", file=sys.stderr, flush=True) else: marke = " <<<<< BLOCK FERTIG" if block_fertig else "" stand = (f"{FILTER_WINDOW}/{FILTER_WINDOW}" if block_fertig else f"sammelt {len(_recent)}/{FILTER_WINDOW}") az_s_out, el_s_out = ausgabe_winkel(az_smooth, el_smooth) print(f"{roh} | Block-Mittel: Azimut={az_s_out:+.1f}° " f"Elevation={el_s_out:+.1f}° (n={n_used}, " f"Streuung={streuung:.1f}°, {stand}){bart}{marke}", file=sys.stderr, flush=True) # Nur fertige Bloecke in die Tabelle: eine Zeile = eine # unabhaengige Messung aus FILTER_WINDOW frischen Schaetzungen. if block_fertig: add_to_angle_table(zeit, int(event.event_counter), az_s_out, el_s_out) except ValueError: pass if SHUTTING_DOWN: return try: iq_file.write(json.dumps(record, separators=(",", ":")) + "\n") iq_file.flush() except ValueError: # Datei wurde waehrend des Beendens (Strg+C) bereits geschlossen; der BGAPI- # Event-Thread kann noch kurz nachlaufen, das ist harmlos. return if int(event.event_counter) % PRINT_EVERY_N_PACKETS == 0: print(f"IQ-Paket #{event.event_counter} Kanal={channel} RSSI={event.rssi} dBm " f"Samples={len(i_values)}", file=sys.stderr, flush=True) def on_event(event: Any) -> None: """BGAPI-Ereignisse behandeln: Scan, Sync, CTE-Empfang.""" global already_synced name = str(event).split("(")[0] if name in ("bt_evt_scanner_scan_report", "bt_evt_scanner_extended_advertisement_report"): interval = getattr(event, "periodic_interval", None) if interval and not already_synced: already_synced = True print(f"Tag gefunden RSSI={event.rssi} dBm Intervall={interval} ms", file=sys.stderr) pending_actions.put(("sync", event.address, event.address_type, getattr(event, "adv_sid", 0))) elif name in ("bt_evt_sync_opened", "bt_evt_sync_scanner_opened", "bt_evt_periodic_sync_opened"): print(f"Synchronisation hergestellt (sync={event.sync})", file=sys.stderr) pending_actions.put(("cte", event.sync)) elif name in ("bt_evt_cte_receiver_connectionless_iq_report", "bt_evt_periodic_sync_iq_report", "bt_evt_cte_receiver_iq_report"): save_iq_report(event) elif hasattr(event, "result") and event.result: print(f"Fehler BG22: {event}", file=sys.stderr) def main() -> int: """Verbindung zum BG22 aufbauen, Tag suchen und CTE-Empfang starten.""" global lib, iq_file, ACTIVE_CONFIG, ANGLE_TABLE_FILE, CONFIDENCE_MIN, ACCUMULATE_PACKETS global _kalibrierung parser = argparse.ArgumentParser( description="BG22 IQ-Logger + Winkel-Schätzung (Navigine/MSBeni-Prinzip, kein Switching-" "Pattern-Streit, keine Kalibrierdatei)") parser.add_argument("--port", default=COM_PORT) parser.add_argument("--baud", type=int, default=BAUD_RATE) parser.add_argument("--xapi", default=XAPI_PATH) parser.add_argument("--iq-out", default=IQ_FILE) parser.add_argument("--angle-out", default=ANGLE_TABLE_FILE) parser.add_argument("--pitch-x-m", type=float, default=0.040) parser.add_argument("--pitch-y-m", type=float, default=0.040) parser.add_argument("--phase-multiplier", type=float, default=2.0) parser.add_argument("--accumulate", type=int, default=ACCUMULATE_PACKETS, help="Anzahl Pakete pro Winkel-Schätzung akkumuliert (mehr = stabiler, langsamer)") parser.add_argument("--confidence-min", type=float, default=CONFIDENCE_MIN, help="Minimale Konfidenz (Peak/Median-Verhältnis) zum Akzeptieren einer Schätzung") parser.add_argument("--flip-azimuth", action="store_true", dest="flip_azimuth", default=True, help="Azimut-Vorzeichenumkehr EINSCHALTEN (seit 08.10.26 ohnehin der Default)") parser.add_argument("--no-flip-azimuth", action="store_false", dest="flip_azimuth", help="Azimut-Vorzeichenumkehr AUSSCHALTEN, liefert wieder das alte Vorzeichen") parser.add_argument("--no-flip-elevation", action="store_false", dest="flip_elevation", default=True, help="Spiegelung von v AUSSCHALTEN (per Default AN -- per Realtest am " "01.10.26 gegen den AoA Analyzer bestätigt)") parser.add_argument("--swap-axes", action="store_true", help="Zeilen/Spalten-Rollen tauschen (Test: Array evtl. um 90° gedreht montiert)") parser.add_argument("--no-azimuth-mirror", action="store_false", dest="azimuth_mirror", default=False, help="Azimut-Hemisphaeren-Spiegelung AUSSCHALTEN (seit 03.10.26 ohnehin der " "Default, Flag nur noch zur Abwaertskompatibilitaet)") parser.add_argument("--azimuth-mirror", action="store_true", dest="azimuth_mirror", default=False, help="Ohne Wirkung seit der Kegel-Konvention (07.10.26), Flag nur noch " "zur Abwaertskompatibilitaet, s. Config.azimuth_mirror.") parser.add_argument("--az-offset", type=float, default=0.0, help="Konstanter Azimut-Offset in Grad, dreht die Nullrichtung " "(s. Config.azimuth_offset_deg). Gegen den AoA Analyzer herleiten, " "nicht raten.") parser.add_argument("--el-offset", type=float, default=0.0, help="Analog fuer Elevation (s. Config.elevation_offset_deg).") a = parser.parse_args() ACTIVE_CONFIG = Config(pitch_x_m=a.pitch_x_m, pitch_y_m=a.pitch_y_m, phase_multiplier=a.phase_multiplier, flip_azimuth=a.flip_azimuth, flip_elevation=a.flip_elevation, swap_axes=a.swap_axes, azimuth_mirror=a.azimuth_mirror, azimuth_offset_deg=a.az_offset, elevation_offset_deg=a.el_offset) CONFIDENCE_MIN = a.confidence_min ACCUMULATE_PACKETS = a.accumulate _kalibrierung = lade_kalibrierung() if _kalibrierung is None: print(f"Antennenkalibrierung: keine ({KALIBRIER_DATEI} nicht gefunden). " f"Die Winkel werden dadurch systematisch falsch sein.", file=sys.stderr) else: print(f"Antennenkalibrierung: geladen aus {KALIBRIER_DATEI} " f"(Quelle {_kalibrierung['quelle']}, {_kalibrierung['pakete']} Pakete, " f"Stabilitaet {_kalibrierung['stabilitaet']:.2f}, " f"{len(_kalibrierung['kanal'])} Kanaele einzeln)", file=sys.stderr) switching_pattern = bytes((CP_RF_VALUE,) + RF_ORDER) ANGLE_TABLE_FILE = a.angle_out with open(ANGLE_TABLE_FILE, "w", encoding="utf-8") as f: json.dump([], f, indent=2, ensure_ascii=False) iq_file = open(a.iq_out, "a", encoding="utf-8") print(f"IQ-Datei: {a.iq_out} Winkel-Tabelle: {ANGLE_TABLE_FILE} " f"RF-Pattern: simple (0..15, fix) Konfidenzschwelle: {CONFIDENCE_MIN} " f"flip_az={a.flip_azimuth} flip_el={a.flip_elevation} swap_axes={a.swap_axes}", file=sys.stderr) try: connector = bgapi.SerialConnector(a.port, baudrate=a.baud) lib = bgapi.BGLib(connector, a.xapi, on_event) lib.open() print(f"Verbunden mit BG22 auf {a.port}", file=sys.stderr) lib.bt.system.reboot() time.sleep(2.0) lib.bt.system.hello() lib.bt.scanner.set_parameters(1, 160, 160) lib.bt.scanner.start(1, 2) print("Suche nach periodischem Tag...", file=sys.stderr) while True: try: action, *args = pending_actions.get(timeout=0.1) except queue.Empty: continue if action == "sync": lib.bt.scanner.stop() lib.bt.sync_scanner.open(*args) elif action == "cte": lib.bt.cte_receiver.enable_connectionless_cte(args[0], SLOT_DURATION_US, CTE_COUNT, switching_pattern) print(f"CTE-Empfang aktiv. RF-Pattern: {list(switching_pattern)}", file=sys.stderr) elif action == "stop": break except KeyboardInterrupt: print("Beende auf Benutzerwunsch...", file=sys.stderr) finally: global SHUTTING_DOWN SHUTTING_DOWN = True # ab hier verwirft save_iq_report() neue Events, statt auf iq_file zu schreiben if lib: lib.close() # stoppt zuerst den BGAPI-Event-Thread, damit er iq_file nicht mehr anfasst if iq_file: iq_file.close() return 0 if __name__ == "__main__": sys.exit(main())