MUSIC-Algorithmus mit Kalibrierung, Filterung und Vorzeichenkorrektur

bg22_iq_logger.py

@@ -0,0 +1,374 @@
+import time
+import sys
+import json
+import struct
+import queue
+from datetime import datetime, timezone
+
+import numpy as np
+import bgapi
+
+
+# =============================================================================
+# EINSTELLUNGEN: BG22 / CTE
+# =============================================================================
+
+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"
+ANGLE_TABLE_FILE = "winkel_tabelle.json"   # nur Azimut/Elevation, ein Eintrag pro neuem Wert
+
+SLOT_DURATIONS = 2                        # 1 oder 2 Mikrosekunden pro Slot
+SWITCHING_PATTERN = bytes(range(0, 16))   # Reihenfolge der Antennen
+CTE_COUNT = 0                             # 0 = unbegrenzt weiter empfangen
+
+
+# =============================================================================
+# EINSTELLUNGEN: EIGENER MUSIC-ALGORITHMUS
+# =============================================================================
+
+CALIBRATION_FILE = "calibration.json"
+
+FREQUENCY_HZ = 2.44e9
+SPEED_OF_LIGHT = 3e8
+WAVELENGTH = SPEED_OF_LIGHT / FREQUENCY_HZ
+
+ANTENNA_SPACING = 0.0400   # 40 mm, laut AN1195 (nicht lambda/2)
+
+ARRAY_ROWS = 4
+ARRAY_COLS = 4
+NUM_ANTENNAS = ARRAY_ROWS * ARRAY_COLS
+
+AZIMUTH_SIGN = 1
+ELEVATION_SIGN = 1
+
+REFERENCE_SAMPLES = 8   # feste Referenzperiode am Anfang jedes CTE
+NUM_SOURCES = 1         # nur ein Tag sendet gleichzeitig
+
+AZIMUTH_RANGE = np.arange(-180, 181, 1)
+ELEVATION_RANGE = np.arange(-90, 91, 1)
+
+ACCUMULATE_PACKETS = 20   # Pakete pro Winkel-Update
+
+
+# =============================================================================
+# GLOBALER ZUSTAND
+# =============================================================================
+
+lib = None
+output_file = None
+sync_handle = None
+already_synced = False
+pending_actions = queue.Queue()
+
+_cached_positions = None
+_cached_offsets = None
+_calibration_loaded = False
+
+_snapshot_buffer = []
+_latest_stable_angle = (None, None)
+_angle_table = []
+
+
+# =============================================================================
+# BLUETOOTH-EVENTS
+# =============================================================================
+
+def on_event(event):
+    """Wird bei jedem eingehenden Ereignis vom BG22 aufgerufen."""
+    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}).")
+        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}")
+
+
+def look_for_periodic_advertising(event):
+    """Prüft ein gefundenes Gerät: sendet es CTE über periodisches Advertising?"""
+    global already_synced
+
+    if already_synced:
+        return
+
+    periodic_interval = getattr(event, "periodic_interval", None)
+    adv_sid = getattr(event, "adv_sid", None)
+
+    if not periodic_interval:
+        return
+
+    print(f"Tag gefunden! RSSI={event.rssi} dBm, Interval={periodic_interval} ms")
+
+    already_synced = True
+    pending_actions.put(("connect_to_tag", event.address, event.address_type, adv_sid))
+
+
+# =============================================================================
+# BLUETOOTH-BEFEHLE
+# =============================================================================
+
+def start_scan():
+    """Startet die Suche 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 connect_to_tag(address, address_type, adv_sid):
+    """Beendet die Suche und verbindet sich mit dem gefundenen Tag."""
+    lib.bt.scanner.stop()
+    lib.bt.sync_scanner.open(address, address_type, adv_sid)
+
+
+def enable_cte(sync):
+    """Aktiviert den CTE-Empfang auf der bestehenden Synchronisation."""
+    lib.bt.cte_receiver.enable_connectionless_cte(
+        sync, SLOT_DURATIONS, CTE_COUNT, SWITCHING_PATTERN
+    )
+    print("CTE-Empfang läuft. Warte auf IQ-Daten ...\n")
+
+
+# =============================================================================
+# EIGENER MUSIC-ALGORITHMUS (2D, Azimut + Elevation, 4x4 URA)
+# =============================================================================
+
+def antenna_positions():
+    """(x, y)-Position jeder Antenne im 4x4-Raster."""
+    positions = []
+    for row in range(ARRAY_ROWS):
+        for col in range(ARRAY_COLS):
+            positions.append((row * ANTENNA_SPACING, col * ANTENNA_SPACING))
+    return np.array(positions)
+
+
+def steering_vector(azimuth_deg, elevation_deg, positions):
+    """Theoretischer Steering-Vector für einen Winkel."""
+    az = np.radians(azimuth_deg) * AZIMUTH_SIGN
+    el = np.radians(elevation_deg) * ELEVATION_SIGN
+    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)
+
+
+def extract_snapshots(i_values, q_values):
+    """Wandelt IQ-Rohwerte in Antennen-Snapshots um."""
+    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
+
+    usable = switching_samples[: num_full_cycles * NUM_ANTENNAS]
+    return usable.reshape(num_full_cycles, NUM_ANTENNAS).T
+
+
+def circular_mean(angles):
+    """Mittelwert von Winkeln, robust gegen den Sprung bei ±π."""
+    return np.angle(np.mean(np.exp(1j * angles)))
+
+
+def music_spectrum(snapshots, positions):
+    """MUSIC-Pseudospektrum, gibt den stärksten Peak zurück."""
+    covariance = snapshots @ snapshots.conj().T / snapshots.shape[1]
+    eigenvalues, eigenvectors = np.linalg.eigh(covariance)
+    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)
+
+
+def load_calibration():
+    """Lädt gespeicherte Phasenoffsets, falls vorhanden."""
+    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 bekannte Phasenoffsets aus den Rohdaten."""
+    correction = np.exp(-1j * offsets).reshape(-1, 1)
+    return snapshots * correction
+
+
+def add_packet_and_maybe_estimate(i_values, q_values):
+    """Sammelt Snapshots; berechnet alle ACCUMULATE_PACKETS Pakete einen Winkel."""
+    global _cached_positions, _cached_offsets, _calibration_loaded
+    global _snapshot_buffer, _latest_stable_angle
+
+    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 not None:
+        _snapshot_buffer.append(snapshots)
+
+    if len(_snapshot_buffer) < ACCUMULATE_PACKETS:
+        return _latest_stable_angle
+
+    combined = np.concatenate(_snapshot_buffer, axis=1)
+    _snapshot_buffer = []
+
+    if _cached_offsets is not None:
+        combined = apply_calibration(combined, _cached_offsets)
+
+    _latest_stable_angle = music_spectrum(combined, _cached_positions)
+    return _latest_stable_angle
+
+
+# =============================================================================
+# IQ-DATEN VERARBEITEN
+# =============================================================================
+
+def decode_samples(raw_bytes):
+    """Wandelt die rohen Bytes in I- und Q-Wertelisten um."""
+    values = list(struct.unpack(f"{len(raw_bytes)}b", raw_bytes))
+    return values[0::2], values[1::2]
+
+
+def add_to_angle_table(zeit, counter, azimuth, elevation):
+    """Fügt einen neuen Winkel-Eintrag hinzu und schreibt die Tabelle neu."""
+    _angle_table.append({
+        "zeit": zeit, "counter": counter,
+        "azimut": azimuth, "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):
+    """Dekodiert, berechnet den Winkel, zeigt an, speichert."""
+    i_values, q_values = decode_samples(event.samples)
+
+    azimuth, elevation = add_packet_and_maybe_estimate(i_values, q_values)
+
+    print(f"IQ-Paket #{event.event_counter}  Kanal={event.channel}  "
+          f"RSSI={event.rssi} dBm  {len(i_values)} Samples  "
+          f"({len(_snapshot_buffer)}/{ACCUMULATE_PACKETS} gesammelt)")
+    print(f"   Rohbytes:   {event.samples[:8]} ...")
+    print(f"   I/Q-Werte:  {list(zip(i_values[:4], q_values[:4]))} ...")
+    if azimuth is not None:
+        print(f"   Azimut={azimuth}°  Elevation={elevation}°")
+        if not _angle_table or (azimuth, elevation) != (
+            _angle_table[-1]["azimut"], _angle_table[-1]["elevation"]
+        ):
+            add_to_angle_table(
+                datetime.now(timezone.utc).isoformat(),
+                event.event_counter, azimuth, elevation,
+            )
+    else:
+        print("   sammle noch Pakete ...")
+
+    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()
+
+
+# =============================================================================
+# HAUPTPROGRAMM
+# =============================================================================
+
+def connect_to_bg22():
+    """Öffnet die serielle Verbindung, stellt sauberen Zustand her."""
+    connector = bgapi.SerialConnector(COM_PORT, baudrate=BAUD_RATE)
+    module = bgapi.BGLib(connector, XAPI_PATH, on_event)
+    module.open()
+    print(f"Verbunden mit {COM_PORT}.")
+
+    module.bt.system.reboot()
+    time.sleep(2.0)
+
+    module.bt.system.hello()
+    print("BG22 antwortet, alles bereit.")
+    return module
+
+
+def process_pending_actions():
+    """Führt Aktionen aus, die nicht direkt im Event-Handler laufen dürfen."""
+    while True:
+        try:
+            action, *details = pending_actions.get(timeout=0.1)
+        except queue.Empty:
+            continue
+
+        if action == "connect_to_tag":
+            connect_to_tag(*details)
+        elif action == "enable_cte":
+            enable_cte(*details)
+
+
+def main():
+    global lib, output_file
+
+    output_file = open(OUTPUT_FILE, "a", encoding="utf-8")
+    print(f"IQ-Samples werden gespeichert in: {OUTPUT_FILE}")
+
+    lib = connect_to_bg22()
+    start_scan()
+    process_pending_actions()
+
+
+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()