diff --git a/bg22_iq_logger.py b/bg22_iq_logger.py new file mode 100644 index 0000000..d22ef0d --- /dev/null +++ b/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()