commit 937835572fd0d3e2d90d489cdb71e770716ddad2 Author: SandraGuefack Date: Sat Sep 12 13:09:31 2026 +0200 Initial commit - projet AoA diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c126469 --- /dev/null +++ b/.gitignore @@ -0,0 +1,7 @@ +__pycache__/ +*.pyc +.venv/ +venv/ +*.jsonl +*.log +.vscode/ diff --git a/bg22_get-iq_samples.py b/bg22_get-iq_samples.py new file mode 100644 index 0000000..5f4b575 --- /dev/null +++ b/bg22_get-iq_samples.py @@ -0,0 +1,165 @@ +import time +import sys +import json +import struct +import queue +from datetime import datetime, timezone + +import bgapi + +# --- 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 + +# --- Ab hier läuft das Programm, nichts mehr anpassen --- + +lib = None +output_file = None +sync_handle = None +already_synced = False +pending_actions = queue.Queue() + + +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, anzeigen und speichern.""" + 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] + + 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]))} ...") + + 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, + } + 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}") + + 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/bg22_iq_logger.py b/bg22_iq_logger.py new file mode 100644 index 0000000..0401bce --- /dev/null +++ b/bg22_iq_logger.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() diff --git a/rtl_bridge.c b/rtl_bridge.c new file mode 100644 index 0000000..e66c1be --- /dev/null +++ b/rtl_bridge.c @@ -0,0 +1,203 @@ +/* + * rtl_bridge.c + * ============ + * Kleiner C-Wrapper um die Silicon Labs "Real-Time Locationing" (RTL) library + * (sl_rtl_clib_api.h), damit Python (via ctypes) daraus AoA-Winkel berechnen + * kann. + * + * Warum dieser Umweg? Die RTL library wird nur als STATISCHE Bibliothek + * ausgeliefert (librtl_static.a), nicht als DLL. ctypes kann aber nur DLLs + * (bzw. .so) zur Laufzeit laden. Diese Datei wird deshalb selbst zu einer + * kleinen DLL kompiliert, die librtl_static.a einbindet und eine einfache, + * ctypes-freundliche API nach außen zeigt. + * + * Gefundene Installation (bei dir): + * Header: C:\Users\sandr\.silabs\slt\installs\conan\p\simpleca33d691c539\p\ + * realtime_locationing\inc\sl_rtl_clib_api.h + * Lib: C:\Users\sandr\.silabs\slt\installs\conan\p\simpleca33d691c539\p\ + * realtime_locationing\lib\gcc\windows\x86-64\release\librtl_static.a + * (+ librtl_log_static.a im selben Ordner) + * + * Bauen (mit MinGW-w64 gcc, x86-64, unter Windows): + * + * set RTL=C:\Users\sandr\.silabs\slt\installs\conan\p\simpleca33d691c539\p\realtime_locationing + * gcc -O2 -shared -o rtl_bridge.dll rtl_bridge.c ^ + * -I"%RTL%\inc" ^ + * -L"%RTL%\lib\gcc\windows\x86-64\release" ^ + * -lrtl_static -lrtl_log_static -lm ^ + * -Wl,--out-implib,rtl_bridge.lib + * + * Falls der Linker "undefined reference"-Fehler zu rtl_log_* Funktionen + * wirft: -lrtl_log_static ist bereits dabei; ansonsten bitte Fehlermeldung + * schicken, dann passen wir es an. + * + * rtl_bridge.dll muss danach im selben Ordner wie aoa_estimator.py liegen. + */ + +#include +#include + +#include "sl_rtl_clib_api.h" + +#if defined(_WIN32) + #define RTL_API __declspec(dllexport) +#else + #define RTL_API __attribute__((visibility("default"))) +#endif + +typedef struct { + sl_rtl_aox_libitem item; + uint32_t num_antennas; + uint32_t num_snapshots; + float **i_snapshots; /* [snapshot][antenna] */ + float **q_snapshots; + int estimator_ready; +} rtl_aoa_handle; + +static void free_snapshot_buffers(rtl_aoa_handle *h) +{ + if (!h->estimator_ready) { + return; + } + for (uint32_t s = 0; s < h->num_snapshots; s++) { + free(h->i_snapshots[s]); + free(h->q_snapshots[s]); + } + free(h->i_snapshots); + free(h->q_snapshots); + h->i_snapshots = NULL; + h->q_snapshots = NULL; + h->estimator_ready = 0; +} + +/* + * array_type : siehe enum sl_rtl_aox_array_type (0 = 4x4 URA, 1 = 3x3 URA, + * 2 = 1x4 ULA, ...) in sl_rtl_clib_api.h + * mode : siehe enum sl_rtl_aox_mode, z.B. 11 = SL_RTL_AOA_MODE_REAL_TIME_BASIC + * num_antennas: Anzahl der Antennen im Switching-Pattern (z.B. 16 fuer 4x4 URA) + * + * Rueckgabewert: Handle (Zeiger) oder NULL bei Fehler. + */ +RTL_API void *rtl_aoa_create(uint32_t array_type, uint32_t mode, uint32_t num_antennas) +{ + rtl_aoa_handle *h = (rtl_aoa_handle *)calloc(1, sizeof(rtl_aoa_handle)); + if (!h) { + return NULL; + } + h->num_antennas = num_antennas; + + if (sl_rtl_aox_init(&h->item) != SL_RTL_ERROR_SUCCESS) { + free(h); + return NULL; + } + if (sl_rtl_aox_set_array_type(&h->item, (enum sl_rtl_aox_array_type)array_type) != SL_RTL_ERROR_SUCCESS) { + sl_rtl_aox_deinit(&h->item); + free(h); + return NULL; + } + if (sl_rtl_aox_set_mode(&h->item, (enum sl_rtl_aox_mode)mode) != SL_RTL_ERROR_SUCCESS) { + sl_rtl_aox_deinit(&h->item); + free(h); + return NULL; + } + /* Standard-Abtastrate der BG22-CTE-Rohdaten: 1 Sample/Mikrosekunde (1 MHz). */ + sl_rtl_aox_set_sample_rate(&h->item, 1000000.0f); + sl_rtl_aox_set_num_radio_channels(&h->item, 1); + + return h; +} + +/* + * Verarbeitet EIN IQ-Paket (ein bt_evt_..._iq_report) und liefert, falls + * schon moeglich, einen Winkel zurueck. + * + * raw_i / raw_q : rohe I- bzw. Q-Werte des Pakets (unveraendert von + * decode_samples() in bg22_iq_logger.py), Laenge = num_raw_samples + * start_offset : Anzahl Samples am Anfang, die zur Referenzperiode + * gehoeren und uebersprungen werden (Standard: 8) + * downsampling_factor : Verhaeltnis Rohrate/Snapshot-Rate (= SLOT_DURATIONS) + * tone_frequency_hz : Mittenfrequenz des BLE-Kanals in Hz + * + * Rueckgabewert (enum sl_rtl_error_code als int): + * 0 = SL_RTL_ERROR_SUCCESS -> az_out/el_out gueltig + * 3 = SL_RTL_ERROR_ESTIMATION_IN_PROGRESS -> noch kein Ergebnis, weiter Pakete senden + * <0 = interner Fehler dieser Bridge (siehe Kommentare unten) + * sonst = anderer sl_rtl_error_code (siehe sl_rtl_clib_api.h) + */ +RTL_API int rtl_aoa_process_packet(void *handle, + const float *raw_i, const float *raw_q, uint32_t num_raw_samples, + uint32_t start_offset, float downsampling_factor, + float tone_frequency_hz, + float *az_out, float *el_out) +{ + rtl_aoa_handle *h = (rtl_aoa_handle *)handle; + if (!h) { + return -1; /* ungueltiges Handle */ + } + + uint32_t num_snapshots = 0; + if (sl_rtl_aox_calculate_number_of_snapshots(&h->item, num_raw_samples, start_offset, + downsampling_factor, 1, &num_snapshots) + != SL_RTL_ERROR_SUCCESS) { + return -2; /* Snapshot-Anzahl konnte nicht berechnet werden */ + } + if (num_snapshots == 0) { + return -3; /* zu wenige Rohdaten (z.B. nur Referenzperiode) */ + } + + /* Estimator (neu) anlegen, wenn er noch nicht existiert oder sich die + * Paketlaenge (und damit die Snapshot-Anzahl) geaendert hat. */ + if (!h->estimator_ready || h->num_snapshots != num_snapshots) { + free_snapshot_buffers(h); + + h->num_snapshots = num_snapshots; + sl_rtl_aox_set_num_snapshots(&h->item, num_snapshots); + if (sl_rtl_aox_create_estimator(&h->item) != SL_RTL_ERROR_SUCCESS) { + return -4; + } + + h->i_snapshots = (float **)calloc(num_snapshots, sizeof(float *)); + h->q_snapshots = (float **)calloc(num_snapshots, sizeof(float *)); + if (!h->i_snapshots || !h->q_snapshots) { + return -5; + } + for (uint32_t s = 0; s < num_snapshots; s++) { + h->i_snapshots[s] = (float *)calloc(h->num_antennas, sizeof(float)); + h->q_snapshots[s] = (float *)calloc(h->num_antennas, sizeof(float)); + } + h->estimator_ready = 1; + } + + enum sl_rtl_error_code rc; + + rc = sl_rtl_aox_convert_raw_samples(&h->item, start_offset, downsampling_factor, + (float *)raw_i, (float *)raw_q, num_raw_samples, + h->i_snapshots, h->q_snapshots, num_snapshots); + if (rc != SL_RTL_ERROR_SUCCESS) { + return (int)rc; + } + + rc = sl_rtl_aox_process(&h->item, h->i_snapshots, h->q_snapshots, tone_frequency_hz, az_out, el_out); + return (int)rc; +} + +/* Standardabweichung (Konfidenzmass) der letzten Winkelschaetzung. */ +RTL_API int rtl_aoa_get_std_dev(void *handle, float *az_std_out, float *el_std_out) +{ + rtl_aoa_handle *h = (rtl_aoa_handle *)handle; + if (!h) { + return -1; + } + return (int)sl_rtl_aox_get_latest_aox_standard_deviation(&h->item, az_std_out, el_std_out); +} + +RTL_API void rtl_aoa_destroy(void *handle) +{ + rtl_aoa_handle *h = (rtl_aoa_handle *)handle; + if (!h) { + return; + } + free_snapshot_buffers(h); + sl_rtl_aox_deinit(&h->item); + free(h); +}