199 lines
6.6 KiB
Python
199 lines
6.6 KiB
Python
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()
|