From 75868d549c6f6abdea79569831da59c80a2bed5a Mon Sep 17 00:00:00 2001 From: SandraGuefack Date: Sat, 12 Sep 2026 14:04:22 +0200 Subject: [PATCH] =?UTF-8?q?Hinzuf=C3=BCgen=20der=20MUSIC-=20und=20AoA-Sch?= =?UTF-8?q?=C3=A4tzungsskripte?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- bg22_get_AoA.py | 198 +++++++++++++++++++++++++++++++ music_estimator.py | 284 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 482 insertions(+) create mode 100644 bg22_get_AoA.py create mode 100644 music_estimator.py diff --git a/bg22_get_AoA.py b/bg22_get_AoA.py new file mode 100644 index 0000000..0401bce --- /dev/null +++ b/bg22_get_AoA.py @@ -0,0 +1,198 @@ +import time +import sys +import json +import struct +import queue +from datetime import datetime, timezone + +import bgapi +import music_estimator + +# --- Einstellungen, bei Bedarf anpassen --- + +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" + +OUTPUT_FILE = "iq_samples.jsonl" + +SLOT_DURATIONS = 2 # 1 oder 2 (Mikrosekunden pro Slot) +SWITCHING_PATTERN = bytes(range(0, 16)) # welche Antennen der Reihe nach genutzt werden +CTE_COUNT = 0 # 0 = unbegrenzt weiter empfangen + +CALIBRATION_PACKET_COUNT = 20 # wie viele Pakete für die Auto-Kalibrierung gesammelt werden + +# --- Ab hier läuft das Programm, nichts mehr anpassen --- + +lib = None +output_file = None +sync_handle = None +already_synced = False +pending_actions = queue.Queue() + +calibrating = "--calibrate" in sys.argv +calibration_samples = [] + + +def on_event(event): + """Diese Funktion wird jedes Mal aufgerufen, wenn der BG22 uns etwas meldet.""" + global sync_handle + + name = str(event).split("(")[0] + + if name in ("bt_evt_scanner_scan_report", "bt_evt_scanner_extended_advertisement_report"): + look_for_periodic_advertising(event) + + elif name in ("bt_evt_sync_opened", "bt_evt_sync_scanner_opened", "bt_evt_periodic_sync_opened"): + sync_handle = event.sync + print(f"Verbindung zum Tag hergestellt (sync={sync_handle}). Aktiviere CTE ...") + pending_actions.put(("enable_cte", sync_handle)) + + elif name in ("bt_evt_sync_closed", "bt_evt_sync_scanner_closed", "bt_evt_periodic_sync_closed"): + print("Verbindung zum Tag wurde beendet.") + + 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 != 0: + print(f"Fehler vom BG22: {event}") + + # alles andere (andere Bluetooth-Geräte in der Umgebung) wird ignoriert + + +def start_scan(): + """Sucht nach Bluetooth-Geräten in der Umgebung.""" + print("Suche nach einem Tag ...") + lib.bt.scanner.set_parameters(1, 160, 160) + lib.bt.scanner.start(1, 2) + + +def look_for_periodic_advertising(event): + """Prüft, ob ein gefundenes Gerät ein Positionierungs-Tag mit CTE ist.""" + global already_synced + + if already_synced: + return # wir haben schon ein Tag gefunden, weitere ignorieren + + periodic_interval = getattr(event, "periodic_interval", None) + adv_sid = getattr(event, "adv_sid", None) + + if not periodic_interval: + return # dieses Gerät sendet kein periodisches Advertising -> kein Tag für uns + + print(f"Tag gefunden! RSSI={event.rssi} dBm, Advertising alle {periodic_interval} ms") + + already_synced = True + pending_actions.put(("connect_to_tag", event.address, event.address_type, adv_sid)) + + +def enable_cte(sync): + """Schaltet den Empfang der Richtungs-Signale (CTE) ein.""" + lib.bt.cte_receiver.enable_connectionless_cte(sync, SLOT_DURATIONS, CTE_COUNT, SWITCHING_PATTERN) + print("CTE-Empfang läuft. Warte auf IQ-Daten ...\n") + + +def save_iq_report(event): + """Ein IQ-Paket ist angekommen: dekodieren, Winkel schätzen, anzeigen und speichern.""" + global calibrating + + raw_bytes = event.samples + values = list(struct.unpack(f"{len(raw_bytes)}b", raw_bytes)) + i_values = values[0::2] + q_values = values[1::2] + + if calibrating: + calibration_samples.append((i_values, q_values)) + print(f"Kalibrierung: Paket {len(calibration_samples)}/{CALIBRATION_PACKET_COUNT} " + f"(Tag muss bei 0°/0° stehen)") + if len(calibration_samples) >= CALIBRATION_PACKET_COUNT: + print("\nGenug Pakete gesammelt, berechne Kalibrierung ...") + music_estimator.compute_calibration_from_samples(calibration_samples, 0, 0) + music_estimator.reset_calibration_cache() + calibrating = False + print("Kalibrierung aktiv. Normaler Betrieb läuft weiter.\n") + return + + azimuth, elevation = music_estimator.estimate_angle(i_values, q_values) + + print(f"IQ-Paket #{event.event_counter} Kanal={event.channel} " + f"RSSI={event.rssi} dBm {len(i_values)} Samples") + print(f" Rohbytes: {raw_bytes[:8]} ...") + print(f" I/Q-Werte: {list(zip(i_values[:4], q_values[:4]))} ...") + if azimuth is not None: + print(f" Winkel: Azimut={azimuth:.0f}° Elevation={elevation:.0f}°") + else: + print(" Winkel: zu wenige Samples für eine Schätzung") + + entry = { + "zeit": datetime.now(timezone.utc).isoformat(), + "counter": event.event_counter, + "kanal": event.channel, + "rssi": event.rssi, + "slot_dauer_us": SLOT_DURATIONS, + "antennen_pattern": list(SWITCHING_PATTERN), + "i": i_values, + "q": q_values, + "azimut": azimuth, + "elevation": elevation, + } + output_file.write(json.dumps(entry) + "\n") + output_file.flush() + + +def main(): + global lib, output_file + + output_file = open(OUTPUT_FILE, "a", encoding="utf-8") + print(f"IQ-Samples werden gespeichert in: {OUTPUT_FILE}") + + if calibrating: + print(f"\n*** KALIBRIERUNGSMODUS ***") + print(f"Tag jetzt bei 0° Azimut / 0° Elevation vor dem Array platzieren!") + print(f"Sammle automatisch {CALIBRATION_PACKET_COUNT} Pakete ...\n") + + connector = bgapi.SerialConnector(COM_PORT, baudrate=BAUD_RATE) + lib = bgapi.BGLib(connector, XAPI_PATH, on_event) + lib.open() + print(f"Verbunden mit {COM_PORT}.") + + lib.bt.system.reboot() + time.sleep(2.0) + + lib.bt.system.hello() + print("BG22 antwortet, alles bereit.") + start_scan() + + # Die Ereignisse (Events) kommen im Hintergrund automatisch bei on_event() an. + # Hier verarbeiten wir nur die Aktionen, die NICHT direkt aus einem Event + # heraus aufgerufen werden dürfen (sonst blockiert sich das Programm selbst). + while True: + try: + action, *details = pending_actions.get(timeout=0.1) + except queue.Empty: + continue + + if action == "connect_to_tag": + address, address_type, adv_sid = details + lib.bt.scanner.stop() + lib.bt.sync_scanner.open(address, address_type, adv_sid) + + elif action == "enable_cte": + enable_cte(details[0]) + + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + print("\nBeendet.") + except Exception as error: + print(f"Es ist ein Fehler aufgetreten: {error}") + sys.exit(1) + finally: + if lib is not None: + lib.close() + if output_file is not None: + output_file.close() diff --git a/music_estimator.py b/music_estimator.py new file mode 100644 index 0000000..0fba5e9 --- /dev/null +++ b/music_estimator.py @@ -0,0 +1,284 @@ +""" +music_estimator.py + +Eigene 2D-MUSIC-Implementierung (Azimut + Elevation) fuer ein 4x4 URA +(Uniform Rectangular Array), passend zu den Daten aus bg22_iq_logger.py +(Datei iq_samples.jsonl). + +Diese Implementierung basiert auf der allgemeinen, oeffentlich bekannten +MUSIC-Theorie (Schmidt, 1986) und wurde von Grund auf fuer dieses Projekt +geschrieben - keine Uebernahme aus einem bestehenden Repository, da die +gefundenen Beispiele im Netz auf andere Array-Geometrien/Datenformate +(Radar, 1D-Array) zugeschnitten sind. + +Voraussetzungen: + pip install numpy --break-system-packages + +Kalibrierung (Phasenoffsets pro Antenne): + 1. Tag bei bekanntem Winkel (Standard: 0°/0°, direkt vor dem Array) + platzieren und mit bg22_iq_logger.py eine eigene Datei aufnehmen, + z. B. "iq_samples_0deg.jsonl". + 2. Kalibrierung berechnen und speichern: + python music_estimator.py calibrate iq_samples_0deg.jsonl + Das schreibt "calibration.json" mit den Phasenoffsets pro Antenne. + 3. Normale Auswertung (nutzt calibration.json automatisch, falls + vorhanden): + python music_estimator.py +""" + +import sys +import json +import numpy as np + + +# --- Einstellungen --- + +INPUT_FILE = "iq_samples.jsonl" +CALIBRATION_FILE = "calibration.json" + +FREQUENCY_HZ = 2.44e9 # Bluetooth-Mittenfrequenz (Kanal ~19, ca. 2.44 GHz) +SPEED_OF_LIGHT = 3e8 +WAVELENGTH = SPEED_OF_LIGHT / FREQUENCY_HZ + +# WICHTIG: Der reale Antennenabstand des BRD4191A ist NICHT lambda/2! +# Laut AN1195 (Antenna Array Design Guidelines): Patch-Antennen 27,9 x 27,9 mm, +# mit 12,1 mm Abstand zueinander platziert -> Mitte-zu-Mitte-Abstand = 40,0 mm. +# (lambda/2 waere hier ca. 61,5 mm gewesen - deutlich zu groß!) +ANTENNA_SPACING = 0.0400 # Meter, aus AN1195 (27.9mm Patch + 12.1mm Abstand) + +ARRAY_ROWS = 4 +ARRAY_COLS = 4 +NUM_ANTENNAS = ARRAY_ROWS * ARRAY_COLS # 16, entspricht SWITCHING_PATTERN + +# Vorzeichen-Konvention testen/anpassen, falls sich beim Vergleich mit der +# RTL-Bibliothek ein systematisches Vorzeichen-Muster zeigt (siehe Chat- +# Diskussion: (-37,-65) eigener Algo. vs (50,58) RTL-Referenz -> Verdacht +# auf umgekehrte Vorzeichen-Konvention). +AZIMUTH_SIGN = -1 # auf -1 setzen, um das Vorzeichen zu testen/korrigieren +ELEVATION_SIGN = -1 # auf -1 setzen, um das Vorzeichen zu testen/korrigieren + +REFERENCE_SAMPLES = 8 # feste Referenzperiode, siehe Chat-Diskussion +NUM_SOURCES = 1 # nur ein Tag sendet gleichzeitig + +# Suchraster fuer die Winkelsuche (Grad) +AZIMUTH_RANGE = np.arange(-90, 91, 1) +ELEVATION_RANGE = np.arange(-90, 91, 1) + + +def antenna_positions(): + """Liefert die (x, y)-Position jeder Antenne im 4x4-Raster, + in der Reihenfolge des SWITCHING_PATTERN (0..15).""" + positions = [] + for row in range(ARRAY_ROWS): + for col in range(ARRAY_COLS): + x = col * ANTENNA_SPACING + y = row * ANTENNA_SPACING + positions.append((x, y)) + return np.array(positions) # Form: (16, 2) + + +def steering_vector(azimuth_deg, elevation_deg, positions): + """Berechnet den theoretischen Steering-Vector fuer einen Winkel. + Noch OHNE Phasenkalibrierung der einzelnen Antennen.""" + az = np.radians(azimuth_deg) * AZIMUTH_SIGN + el = np.radians(elevation_deg) * ELEVATION_SIGN + + # Richtungsvektor der ankommenden Welle + u = np.cos(el) * np.sin(az) + v = np.sin(el) + + phase = (2 * np.pi / WAVELENGTH) * (positions[:, 0] * u + positions[:, 1] * v) + return np.exp(1j * phase) # Form: (16,) + + +def extract_snapshots(i_values, q_values): + """Wandelt die IQ-Rohwerte eines Pakets in mehrere Antennen-Snapshots um. + Die Referenzperiode wird uebersprungen; danach wird zyklisch durch die + 16 Antennen des Switching-Patterns gegangen. Jeder volle Durchlauf durch + alle 16 Antennen ergibt einen Snapshot (eine Spalte der Datenmatrix).""" + complex_samples = np.array(i_values) + 1j * np.array(q_values) + switching_samples = complex_samples[REFERENCE_SAMPLES:] + + num_full_cycles = len(switching_samples) // NUM_ANTENNAS + if num_full_cycles == 0: + return None # zu wenige Samples fuer einen vollen Durchlauf + + usable = switching_samples[: num_full_cycles * NUM_ANTENNAS] + snapshots = usable.reshape(num_full_cycles, NUM_ANTENNAS).T + return snapshots # Form: (16, num_full_cycles) + + +def circular_mean(angles): + """Mittelwert von Winkeln (Radiant), robust gegen den Sprung bei ±π.""" + return np.angle(np.mean(np.exp(1j * angles))) + + +def compute_calibration_from_samples(samples, known_azimuth, known_elevation, positions=None): + """Wie compute_calibration(), aber nimmt eine Liste von (i_values, q_values) + Tupeln direkt entgegen, statt eine Datei zu lesen. Nützlich für eine + LIVE-Kalibrierung waehrend das Skript laeuft (siehe bg22_iq_logger.py).""" + if positions is None: + positions = antenna_positions() + + theoretical = steering_vector(known_azimuth, known_elevation, positions) + theoretical_phase = np.angle(theoretical / theoretical[0]) + + offsets_per_antenna = [[] for _ in range(NUM_ANTENNAS)] + + for i_values, q_values in samples: + snapshots = extract_snapshots(i_values, q_values) + if snapshots is None: + continue + for snapshot in snapshots.T: + observed_phase = np.angle(snapshot / snapshot[0]) + error = observed_phase - theoretical_phase + for antenna in range(NUM_ANTENNAS): + offsets_per_antenna[antenna].append(error[antenna]) + + offsets = [circular_mean(np.array(values)) if values else 0.0 + for values in offsets_per_antenna] + + with open(CALIBRATION_FILE, "w", encoding="utf-8") as f: + json.dump({ + "referenz_azimut": known_azimuth, + "referenz_elevation": known_elevation, + "phasenoffsets_rad": offsets, + }, f, indent=2) + + print(f"Kalibrierung gespeichert in {CALIBRATION_FILE}") + print(f"Phasenoffsets (Grad): {np.degrees(offsets).round(1).tolist()}") + return offsets + + +def compute_calibration(input_file, known_azimuth, known_elevation, positions): + """Berechnet den Phasenoffset jeder Antenne relativ zu Antenne 0, aus + einer Aufnahme bei bekanntem Winkel (Standard: 0°/0°), gelesen aus + einer JSON-Lines-Datei.""" + samples = [] + with open(input_file, "r", encoding="utf-8") as f: + for line in f: + record = json.loads(line) + samples.append((record["i"], record["q"])) + return compute_calibration_from_samples(samples, known_azimuth, known_elevation, positions) + + +def reset_calibration_cache(): + """Erzwingt, dass estimate_angle() die Kalibrierungsdatei beim naechsten + Aufruf neu einliest (z. B. nachdem gerade eine neue Kalibrierung + gespeichert wurde).""" + global _calibration_loaded + _calibration_loaded = False + + +def load_calibration(): + """Laedt gespeicherte Phasenoffsets, falls vorhanden. Sonst None.""" + try: + with open(CALIBRATION_FILE, "r", encoding="utf-8") as f: + data = json.load(f) + return np.array(data["phasenoffsets_rad"]) + except FileNotFoundError: + return None + + +def apply_calibration(snapshots, offsets): + """Entfernt die bekannten Phasenoffsets aus den Rohdaten, bevor MUSIC + darauf angewendet wird.""" + correction = np.exp(-1j * offsets).reshape(-1, 1) + return snapshots * correction + + +def music_spectrum(snapshots, positions): + """Berechnet das MUSIC-Pseudospektrum ueber das Winkelraster und gibt + den Winkel mit dem staerksten Peak zurueck.""" + # Kovarianzmatrix des Signals (16x16) + covariance = snapshots @ snapshots.conj().T / snapshots.shape[1] + + eigenvalues, eigenvectors = np.linalg.eigh(covariance) + # Sortierung: groesster Eigenwert zuletzt -> Rauschraum sind die ersten + noise_subspace = eigenvectors[:, : NUM_ANTENNAS - NUM_SOURCES] + + best_azimuth, best_elevation, best_power = 0.0, 0.0, -np.inf + + for az in AZIMUTH_RANGE: + for el in ELEVATION_RANGE: + a = steering_vector(az, el, positions) + denom = a.conj().T @ noise_subspace @ noise_subspace.conj().T @ a + power = 1.0 / np.abs(denom) + if power > best_power: + best_power = power + best_azimuth, best_elevation = az, el + + return int(best_azimuth), int(best_elevation) + + +# --- Öffentliche Funktion zum direkten Aufruf aus anderem Code --- + +_cached_positions = None +_cached_offsets = None +_calibration_loaded = False + + +def estimate_angle(i_values, q_values): + """Nimmt die dekodierten I- und Q-Werte EINES IQ-Reports entgegen + (genau das, was decode_samples() in bg22_iq_logger.py liefert) und + gibt (azimuth, elevation) in Grad zurück, oder (None, None), falls + zu wenige Samples für einen vollen Antennen-Durchlauf vorliegen. + + Kalibrierung wird beim ersten Aufruf automatisch geladen (falls + calibration.json existiert) und danach zwischengespeichert. + """ + global _cached_positions, _cached_offsets, _calibration_loaded + + if _cached_positions is None: + _cached_positions = antenna_positions() + + if not _calibration_loaded: + _cached_offsets = load_calibration() + _calibration_loaded = True + + snapshots = extract_snapshots(i_values, q_values) + if snapshots is None or snapshots.shape[1] < 2: + return None, None + + if _cached_offsets is not None: + snapshots = apply_calibration(snapshots, _cached_offsets) + + return music_spectrum(snapshots, _cached_positions) + + +def main(): + positions = antenna_positions() + + if len(sys.argv) >= 2 and sys.argv[1] == "calibrate": + calibration_file = sys.argv[2] if len(sys.argv) >= 3 else INPUT_FILE + compute_calibration(calibration_file, known_azimuth=0, known_elevation=0, + positions=positions) + return + + offsets = load_calibration() + if offsets is None: + print("Keine Kalibrierung gefunden (calibration.json fehlt). " + "Ergebnisse sind unkalibriert und möglicherweise verzerrt.\n" + "Kalibrierung erstellen mit: python music_estimator.py calibrate \n") + else: + print("Kalibrierung geladen, wird angewendet.\n") + + with open(INPUT_FILE, "r", encoding="utf-8") as f: + for line_number, line in enumerate(f, start=1): + record = json.loads(line) + snapshots = extract_snapshots(record["i"], record["q"]) + + if snapshots is None or snapshots.shape[1] < 2: + print(f"Zeile {line_number}: zu wenige Snapshots, übersprungen.") + continue + + if offsets is not None: + snapshots = apply_calibration(snapshots, offsets) + + azimuth, elevation = music_spectrum(snapshots, positions) + print(f"Zeile {line_number} (counter={record['counter']}): " + f"Azimut={azimuth:.0f}° Elevation={elevation:.0f}°") + + +if __name__ == "__main__": + main()