From d093026a825804c763fed2929686e4162f4a6e55 Mon Sep 17 00:00:00 2001 From: SandraGuefack Date: Sat, 12 Sep 2026 14:31:30 +0200 Subject: [PATCH] Entfernen von noch zum testen AoA Skripten aus main(erstmal nur als Branch) --- bg22_iq_logger.py | 198 -------------------------------------------- rtl_bridge.c | 203 ---------------------------------------------- 2 files changed, 401 deletions(-) delete mode 100644 bg22_iq_logger.py delete mode 100644 rtl_bridge.c diff --git a/bg22_iq_logger.py b/bg22_iq_logger.py deleted file mode 100644 index 0401bce..0000000 --- a/bg22_iq_logger.py +++ /dev/null @@ -1,198 +0,0 @@ -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/rtl_bridge.c b/rtl_bridge.c deleted file mode 100644 index e66c1be..0000000 --- a/rtl_bridge.c +++ /dev/null @@ -1,203 +0,0 @@ -/* - * 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); -}