375 lines
12 KiB
Python
375 lines
12 KiB
Python
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()
|