Entfernen von noch zum testen AoA Skripten aus main(erstmal nur als Branch)
This commit is contained in:
SandraGuefack committed
1 parent
45080d5959
commit
d093026a82
2 files changed
-401
No files matched your search
@@ -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()
|
||||
-203
@@ -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 <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
Reference in new issue
Block a user