Initial-Commit - AoA-Projekt
This commit is contained in:
SandraGuefack committed
commit
e63a0d690c
5 files changed
+857
No files matched your search
@@ -0,0 +1,7 @@
|
||||
__pycache__/
|
||||
*.pyc
|
||||
.venv/
|
||||
venv/
|
||||
*.jsonl
|
||||
*.log
|
||||
.vscode/
|
||||
@@ -0,0 +1,165 @@
|
||||
import time
|
||||
import sys
|
||||
import json
|
||||
import struct
|
||||
import queue
|
||||
from datetime import datetime, timezone
|
||||
|
||||
import bgapi
|
||||
|
||||
# --- 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
|
||||
|
||||
# --- Ab hier läuft das Programm, nichts mehr anpassen ---
|
||||
|
||||
lib = None
|
||||
output_file = None
|
||||
sync_handle = None
|
||||
already_synced = False
|
||||
pending_actions = queue.Queue()
|
||||
|
||||
|
||||
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, anzeigen und speichern."""
|
||||
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]
|
||||
|
||||
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]))} ...")
|
||||
|
||||
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,
|
||||
}
|
||||
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}")
|
||||
|
||||
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()
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
+203
@@ -0,0 +1,203 @@
|
||||
/*
|
||||
* 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