Hinzufügen der MUSIC- und AoA-Schätzungsskripte

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()

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 <datei>\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()