1064 lines
54 KiB
Python
1064 lines
54 KiB
Python
#!/usr/bin/env python3
|
|
"""BG22 IQ-Logger + Winkel-Schätzung, NEU AUFGESETZT nur nach den Prinzipien aus
|
|
zwei öffentlichen GitHub-Referenzprojekten (keine eigene Switching-Pattern-Debatte
|
|
mehr, kein separates Kalibrierfile -- siehe Begründung unten):
|
|
|
|
1) Navigine/Direction-of-Arrival-DoA-Estimation-Algorithm
|
|
(github.com/Navigine/Direction-of-Arrival-DoA-Estimation-Algorithm)
|
|
-> Kernidee übernommen: das 2D-Problem (Azimut, Elevation) wird NICHT als ein
|
|
einziges 2D-Gitter gelöst, sondern in zwei UNABHÄNGIGE 1D-MUSIC-Scans
|
|
aufgeteilt, je über ein virtuelles 2-Antennen-Array aus benachbarten
|
|
Antennenpaaren. Phasendrift wird live aus einer Referenzperiode pro Paket
|
|
geschätzt und abgezogen (dort: phase_ref aus 8 Referenz-Samples) --
|
|
das machen wir hier identisch über compensate_references().
|
|
|
|
2) MSBeni/AoA_IQsamples (github.com/MSBeni/AoA_IQsamples, Ordner AoA/DoA_Final)
|
|
-> Kernidee übernommen: mehrere klassische DOA-Schätzer (Bartlett, MUSIC)
|
|
nebeneinander auf DERSELBEN Kovarianzmatrix laufen lassen, als
|
|
gegenseitige Plausibilitätsprüfung (Bartlett ist unempfindlicher
|
|
gegenüber Rangproblemen der Kovarianzmatrix, aber unschärfer als MUSIC).
|
|
|
|
Warum neu aufgesetzt: das bisherige Skript (bg22_AoA.py) hatte zu viele bewegliche,
|
|
gegenseitig abhängige Teile (3 Switching-Pattern-Hypothesen, Pro-Kanal-Phasen-
|
|
kalibrierungsdatei, 3 Winkel-Algorithmen) -- jede Kombination aus falschem Pattern
|
|
+ falscher/fehlender Kalibrierungsdatei hat zu stundenlangem Debugging geführt.
|
|
Dieses Skript hier:
|
|
- nutzt NUR das offizielle Silicon-Labs-Doku-Beispiel-Switching-Pattern
|
|
(0,1,2,...,15 -- keine Alternative mehr, keine Flags dafür),
|
|
- braucht KEINE separate Kalibrierungsdatei (Phasendrift wird pro Paket live
|
|
aus der Referenzperiode geschätzt, wie bei Navigine),
|
|
- schätzt die Winkel NUR über die paarweise 1D-Methode (kein 2D-Gitter, kein
|
|
ESPRIT, kein Forward-Backward-Averaging -- bewusst reduziert auf das, was in
|
|
den beiden Referenzprojekten tatsächlich gezeigt und getestet wurde).
|
|
"""
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import json
|
|
import queue
|
|
import struct
|
|
import sys
|
|
import time
|
|
from collections import deque
|
|
from dataclasses import dataclass
|
|
from datetime import datetime, timezone
|
|
from typing import Any
|
|
|
|
import numpy as np
|
|
import bgapi
|
|
|
|
# SWITCHING-PATTERN, korrigiert 08.10.26 nach der Silicon-Labs-Doku zur BRD4191A
|
|
# ("Silicon Labs Dual Polarized Antenna Array Board", RTL-Lib App Development,
|
|
# Appendix): 16 Patches mit je einem VERTIKALEN und einem HORIZONTALEN Element,
|
|
# also 32 Antennen. Ein Pattern-Wert waehlt ein ELEMENT, nicht einen Patch.
|
|
# Offizielle Reihenfolge V1,H1,V2,H2,...,V16,H16:
|
|
#
|
|
# Patch 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
|
|
# V,H 2,3 0,1 14,15 12,13 4,5 8,24 20,21 10,11 6,7 22,23 16,17 19,18 9,25 26,27 28,29 31,30
|
|
#
|
|
# Frueher stand hier range(0, 16) mit der Annahme "Wert k = ANT(k+1)". Tatsaechlich
|
|
# wurden damit nur 9 der 16 Patches abgetastet, 7 davon in BEIDEN Polarisationen, und
|
|
# auf Rasterplaetze gelegt, an denen sie nicht liegen. Das erklaert alle Befunde seit
|
|
# dem 02.10.26: Amplitudenfaktor 7, Paar-Kohaerenz 0.24, Phasenschritt 133.7 Grad
|
|
# ueber dem physikalischen Maximum von 117, Boresight richtig, abseits falsch.
|
|
#
|
|
# Jetzt: EIN Element pro Patch, eine Polarisation, in Patch-Reihenfolge -- damit gilt
|
|
# wieder Spalte k == Patch ANT(k+1), und antenna_position() bleibt unveraendert.
|
|
# NACH JEDEM WECHSEL NEU KALIBRIEREN: die Offsets haengen am gewaehlten Element.
|
|
_PATTERN_V = (2, 0, 14, 12, 4, 8, 20, 10, 6, 22, 16, 19, 9, 26, 28, 31)
|
|
_PATTERN_H = (3, 1, 15, 13, 5, 24, 21, 11, 7, 23, 17, 18, 25, 27, 29, 30)
|
|
POLARISATION = "V" # "V" oder "H": die mit der hoeheren, gleichmaessigeren Amplitude
|
|
RF_ORDER = _PATTERN_V if POLARISATION == "V" else _PATTERN_H
|
|
CP_RF_VALUE = 40 # Patch 6 zirkular polarisiert (CP_MODE | 8) -- laut Doku richtig
|
|
|
|
|
|
@dataclass
|
|
class Config:
|
|
references: int = 8 # Anzahl Referenz-Samples am Paketanfang (fuer Phasendrift-Schaetzung)
|
|
expected_samples: int = 45
|
|
slot_us: float = 2.0
|
|
reference_interval_us: float = 1.0
|
|
phase_multiplier: float = 2.0
|
|
pitch_x_m: float = 0.040 # Spaltenabstand (Azimut-Achse)
|
|
pitch_y_m: float = 0.040 # Zeilenabstand (Elevation-Achse)
|
|
flip_azimuth: bool = True # UMGESTELLT 08.10.26 (vorher False): kehrt das Vorzeichen des
|
|
# Azimuts um. Technisch wird u gespiegelt (links/rechts); da
|
|
# az = atan2(u, -v) gilt, ergibt -u exakt -az, ueber den ganzen
|
|
# Kreis und auch bei +-180. Mit --no-flip-azimuth abschaltbar.
|
|
flip_elevation: bool = True # Spiegelt v (oben/unten). Achtung: in der Kegel-Konvention dreht das
|
|
# den AZIMUT (0 <-> 180), nicht die Elevation -- die ist jetzt immer >= 0.
|
|
# BESTAETIGT per Realtest (29.09-01.10.26) gegen den Silicon Labs
|
|
# AoA Analyzer: v-Vorzeichen war invertiert, per Default korrigiert.
|
|
swap_axes: bool = False # Zeilen/Spalten-Rollen tauschen (Test: Array evtl. um 90° gedreht/row-col vertauscht)
|
|
azimuth_mirror: bool = False # OHNE WIRKUNG seit der Kegel-Konvention (07.10.26). Die Spiegelung
|
|
# war die Heuristik, mit der der halbe Azimutbereich [-90,+90] auf die
|
|
# andere Hemisphaere abgebildet wurde. Der Azimut laeuft jetzt direkt
|
|
# ueber den vollen Kreis, damit ist sie gegenstandslos. Das Feld bleibt
|
|
# nur stehen, damit aeltere Aufrufe mit --azimuth-mirror nicht brechen.
|
|
azimuth_offset_deg: float = 0.0 # Konstanter Azimut-Offset, ADDIERT ganz am Ende. Dreht die
|
|
# Nullrichtung. Referenz ist der Silicon Labs AoA Analyzer: gleiches
|
|
# Array, gleiche Firmware, gleiche Pakete, nur anderer Algorithmus.
|
|
# Am 07.10.26 gegen den Analyzer geprueft (Pfeil im Oberen-Rechten,
|
|
# Anzeige 132,97 Grad, Aufdruck dort 135 Grad): die Nullrichtung stimmt
|
|
# bereits. Dieser Wert gehoert also auf 0; ein grosser Wert waere ein
|
|
# Fehler in der Rechenkette, kein Konventionsunterschied.
|
|
elevation_offset_deg: float = 0.0 # Analog fuer Elevation.
|
|
|
|
|
|
def ble_frequency_hz(channel: int) -> float:
|
|
"""BLE-Kanal (0..39) auf Mittenfrequenz in Hz abbilden."""
|
|
if 0 <= channel <= 10:
|
|
mhz = 2404 + 2 * channel
|
|
elif 11 <= channel <= 36:
|
|
mhz = 2428 + 2 * (channel - 11)
|
|
elif channel == 37:
|
|
mhz = 2402
|
|
elif channel == 38:
|
|
mhz = 2426
|
|
elif channel == 39:
|
|
mhz = 2480
|
|
else:
|
|
raise ValueError(f"BLE Kanal ausserhalb 0..39: {channel}")
|
|
return mhz * 1e6
|
|
|
|
|
|
def iq_complex(raw: Any) -> np.ndarray:
|
|
"""I/Q-Paare zu komplexem Signal, Konvention I - jQ (Silicon Labs)."""
|
|
a = np.asarray(raw)
|
|
if a.ndim != 2 or a.shape[1] != 2:
|
|
raise ValueError("iq muss eine Liste von [I, Q] sein")
|
|
return a[:, 0].astype(float) - 1j * a[:, 1].astype(float)
|
|
|
|
|
|
DRIFT_GLAETTUNG = 0.05 # Gewicht des neuen Werts im gleitenden Mittel der Phasendrift.
|
|
# 1.0 = kein Mittel, nur der Wert des aktuellen Pakets (frueheres
|
|
# Verhalten). 0.05 entspricht einem Gedaechtnis von rund 20 Paketen,
|
|
# also etwa einer Sekunde bei 50 ms Sendeintervall.
|
|
_drift_zeiger: complex = 0j
|
|
|
|
|
|
def drift_schaetzen(samples: np.ndarray, cfg: Config) -> float:
|
|
"""Phasendrift je Referenz-Sample aus der Referenzperiode schätzen.
|
|
|
|
Das ist das Prinzip von Navigine's phase_ref: der Mittelwert der Phasenschritte
|
|
zwischen aufeinanderfolgenden Referenz-Samples, hier über alle Referenz-Samples
|
|
gemittelt statt nur über die ersten beiden. In der Referenzperiode sendet immer
|
|
dieselbe Antenne, jede Drehung dort kommt also allein vom Frequenzunterschied
|
|
zwischen Tag und Locator.
|
|
"""
|
|
nref = cfg.references
|
|
if len(samples) < nref:
|
|
raise ValueError(f"benötige {nref} Referenz-Samples, habe {len(samples)}")
|
|
refs = samples[:nref]
|
|
if np.any(np.abs(refs) < 1e-12):
|
|
raise ValueError("Referenz-Sample mit Betrag 0")
|
|
adjacent_products = refs[1:] * np.conj(refs[:-1])
|
|
return float(np.angle(np.mean(np.exp(1j * np.angle(adjacent_products)))))
|
|
|
|
|
|
def geglaettete_drift(phase_step: float) -> float:
|
|
"""Gleitendes Mittel der Phasendrift über die Pakete hinweg.
|
|
|
|
Warum: der Frequenzversatz zweier Quarze ändert sich über Sekunden kaum, die
|
|
Schätzung aus nur 8 Referenz-Samples streut aber. Gemessen am 03.10.26 über drei
|
|
Aufnahmen: Streuung der Drift je Paket rund 0.4 Grad, zwischen den Kanälen dagegen
|
|
nur 0.16 Grad -- die Streuung ist also Messrauschen und keine echte Änderung, und
|
|
ein kanalübergreifendes Mittel ist zulässig.
|
|
|
|
Dieses Rauschen wird in compensate_references mit phase_multiplier * slot_us
|
|
multipliziert und schlägt dann auf alle 16 Antennenphasen durch. Mit dem gleitenden
|
|
Mittel stieg die Stabilität der Antennenphasen über die Pakete von 0.942 auf 0.967
|
|
(aufnahme_a), von 0.923 auf 0.982 (aufnahme_b) und von 0.904 auf 0.983 (aufnahme_c).
|
|
|
|
Gemittelt werden Einheitszeiger, damit der Sprung bei plus oder minus 180 Grad nicht
|
|
stört -- die gemessene Drift liegt mit rund 174 Grad dicht daran.
|
|
"""
|
|
global _drift_zeiger
|
|
neu = np.exp(1j * phase_step)
|
|
if _drift_zeiger == 0j:
|
|
_drift_zeiger = neu
|
|
else:
|
|
_drift_zeiger = (1.0 - DRIFT_GLAETTUNG) * _drift_zeiger + DRIFT_GLAETTUNG * neu
|
|
return float(np.angle(_drift_zeiger))
|
|
|
|
|
|
def compensate_references(samples: np.ndarray, cfg: Config,
|
|
phase_step: float | None = None) -> np.ndarray:
|
|
"""Phasendrift aus dem Paket herausrechnen.
|
|
|
|
phase_step: Drift je Referenz-Sample. Ohne Angabe schätzt die Funktion sie allein
|
|
aus diesem Paket (Verhalten bis 03.10.26); process_packet übergibt den geglätteten
|
|
Wert.
|
|
"""
|
|
nref = cfg.references
|
|
if phase_step is None:
|
|
phase_step = drift_schaetzen(samples, cfg)
|
|
slope = phase_step / cfg.reference_interval_us
|
|
corrected = samples.copy()
|
|
corrected[nref:] *= np.exp(-1j * slope * cfg.phase_multiplier *
|
|
(np.arange(len(samples) - nref) + 1) * cfg.slot_us)
|
|
return corrected
|
|
|
|
|
|
def snapshots_from_samples(data: np.ndarray, cfg: Config) -> np.ndarray:
|
|
"""Daten in der RF-Schaltreihenfolge gruppieren (16 Antennen pro Snapshot-Spalte).
|
|
|
|
WICHTIG -- Zykluslaenge ist 17, nicht 16:
|
|
Das an den Stack uebergebene Switching-Pattern ist (CP_RF_VALUE,) + RF_ORDER
|
|
und damit 17 Eintraege lang. Die Hardware durchlaeuft ALLE 17, auch waehrend
|
|
der Switching-Phase; die Referenzantenne laeuft also im Zyklus mit.
|
|
|
|
Gemessen am 02.10.26 (struktur_check.py, 97 Pakete auf einem Kanal):
|
|
Deckung aufeinanderfolgender Zyklen 0.999 bei Zykluslaenge 17, dagegen 0.519
|
|
bei der naechstbesten Laenge. Mit dem frueheren 16er-Schnitt verrutschte die
|
|
Antennenzuordnung je Zyklus um eine Stelle. Folge auf denselben Paketen:
|
|
Eigenwert-Verhaeltnis der Kovarianz 3.6 statt 2313, Kohaerenz je Antennenpaar
|
|
0.796 statt 1.000, Pseudospektrum flach, Ausgabe nur noch conf = 1.0 mit
|
|
Werten in den Ecken des Suchrasters.
|
|
|
|
Bei cfg.references = 8 beginnt der hier uebergebene Ausschnitt mit
|
|
RF_ORDER[0]; CP_RF_VALUE liegt dann an der letzten Stelle des Zyklus und
|
|
wird verworfen. Die Spalten 0..15 entsprechen damit weiterhin genau den
|
|
Antennenindizes aus antenna_position().
|
|
"""
|
|
n_ant = len(RF_ORDER) # 16 Rasterantennen
|
|
n_cycle = n_ant + 1 # 17: CP_RF_VALUE laeuft im Zyklus mit
|
|
n_full = len(data) // n_cycle
|
|
if n_full < 1:
|
|
raise ValueError(f"nur {len(data)} Antennen-Samples; brauche {n_cycle}")
|
|
used = data[:n_full * n_cycle]
|
|
return used.reshape(n_full, n_cycle)[:, :n_ant].T
|
|
|
|
|
|
def antenna_position(ant_index: int) -> tuple[float, float, float]:
|
|
"""Physische Position (x, y, z) einer Antenne in Metern, 0-basiert (ant_index=0 -> ANT1).
|
|
Board in der Y-Z-Ebene, Blickrichtung entlang X. Layout bestätigt über den
|
|
Silicon Labs AoA Analyzer (3D-View, "Dual Polarized Board" / "Silicon Labs
|
|
Ref. 4x4 Uniform Dual Polarized Rectangular Array"):
|
|
|
|
ANT13 ANT9 ANT5 ANT1
|
|
ANT14 ANT10 ANT6 ANT2
|
|
ANT15 ANT11 ANT7 ANT3
|
|
ANT16 ANT12 ANT8 ANT4
|
|
|
|
ANT1..16 spaltenweise gruppiert, Spalten von RECHTS nach LINKS; innerhalb
|
|
einer Gruppe von oben nach unten. Da wir hier NUR das Simple-Pattern (0..15
|
|
identisch) nutzen, ist logische Position == physischer Index, d.h. diese
|
|
Funktion bestimmt die Geometrie direkt (keine rf_order-Indirektion mehr nötig).
|
|
"""
|
|
group = ant_index // 4
|
|
row_in_col = ant_index % 4
|
|
col = 3 - group
|
|
return (0.0, col, row_in_col) # in Pitch-Einheiten, s. coordinates()
|
|
|
|
|
|
def coordinates(cfg: Config) -> np.ndarray:
|
|
"""Antennenpositionen in Metern, in Snapshot-Spaltenreihenfolge (0..15).
|
|
cfg.swap_axes tauscht testweise die Rollen von Spalte (Azimut) und Zeile
|
|
(Elevation) -- falls das physische Array um 90° gedreht montiert ist oder
|
|
die Zeilen/Spalten-Zuordnung aus dem AoA-Analyzer-Screenshot nicht zur
|
|
tatsächlichen Verbauung passt."""
|
|
xyz_units = np.array([antenna_position(i) for i in range(16)], dtype=float)
|
|
if cfg.swap_axes:
|
|
xyz_units = xyz_units[:, [0, 2, 1]] # (x, col, row) -> (x, row, col)
|
|
return xyz_units * np.array([1.0, cfg.pitch_x_m, cfg.pitch_y_m])
|
|
|
|
|
|
def find_pairs(xyz: np.ndarray, cfg: Config) -> tuple[list[tuple[int, int]], list[tuple[int, int]]]:
|
|
"""Vertikale Nachbarn (gleiche Spalte, fuer Elevation) und horizontale Nachbarn
|
|
(gleiche Zeile, fuer Azimut) finden -- das sind die "Antennenpaare", auf denen
|
|
Navigine's Methode aufbaut."""
|
|
n_ant = xyz.shape[0]
|
|
tol = 1e-6
|
|
vertical_pairs, horizontal_pairs = [], []
|
|
for i in range(n_ant):
|
|
for j in range(n_ant):
|
|
dx, dy, dz = xyz[j] - xyz[i]
|
|
if abs(dx) < tol and abs(dy) < tol and abs(dz - cfg.pitch_y_m) < tol:
|
|
vertical_pairs.append((i, j))
|
|
if abs(dx) < tol and abs(dy - cfg.pitch_x_m) < tol and abs(dz) < tol:
|
|
horizontal_pairs.append((i, j))
|
|
return vertical_pairs, horizontal_pairs
|
|
|
|
|
|
def scan_1d(X: np.ndarray, pairs: list[tuple[int, int]], wave: float, spacing: float,
|
|
coupling: float = 1.0, step_deg: float = 1.0) -> dict:
|
|
"""Virtuelles 2-Antennen-Array aus ALLEN (Paar x Snapshot)-Kombinationen bauen
|
|
und sowohl MUSIC als auch Bartlett über -90..+90 Grad laufen lassen (Navigine
|
|
fuer MUSIC, MSBeni fuer Bartlett als Gegenprobe). Gibt beide Schätzungen
|
|
zurueck, damit man sie im Log vergleichen kann."""
|
|
ref = np.array([X[i, :] for i, j in pairs])
|
|
nxt = np.array([X[j, :] for i, j in pairs])
|
|
Xv = np.stack([ref.reshape(-1), nxt.reshape(-1)]) # 2 x (n_pairs*n_snap)
|
|
n = Xv.shape[1]
|
|
R = (Xv @ Xv.conj().T) / n
|
|
R = R + (1e-3 * np.trace(R).real / 2) * np.eye(2)
|
|
|
|
vals, vecs = np.linalg.eigh(R)
|
|
order = np.argsort(vals)[::-1]
|
|
vals, vecs = vals[order], vecs[:, order]
|
|
En = vecs[:, 1:] # 1 Quelle, 2 Antennen -> 1-dim. Rauschunterraum
|
|
|
|
angles = np.arange(-90.0, 90.0 + step_deg / 2, step_deg)
|
|
music_power = np.empty(len(angles))
|
|
bartlett_power = np.empty(len(angles))
|
|
for idx, ang in enumerate(np.radians(angles)):
|
|
a = np.array([1.0, np.exp(1j * wave * spacing * coupling * np.sin(ang))])[:, None]
|
|
denom = float(np.linalg.norm(En.conj().T @ a) ** 2)
|
|
music_power[idx] = 1.0 / max(denom, 1e-15)
|
|
bartlett_power[idx] = float(np.abs((a.conj().T @ R @ a))[0, 0]) # Bartlett/Fourier-Beamformer
|
|
|
|
music_peak = int(np.argmax(music_power))
|
|
bartlett_peak = int(np.argmax(bartlett_power))
|
|
return {
|
|
"music_deg": float(angles[music_peak]),
|
|
"music_confidence": float(music_power[music_peak] / (np.median(music_power) + 1e-15)),
|
|
"bartlett_deg": float(angles[bartlett_peak]),
|
|
"bartlett_confidence": float(bartlett_power[bartlett_peak] / (np.median(bartlett_power) + 1e-15)),
|
|
}
|
|
|
|
|
|
def mirror_azimuth(az_deg: float) -> float:
|
|
"""Spiegelt einen im sicheren Bereich [-90,+90] gemessenen Azimut an der 90°-Achse
|
|
auf die physikalisch korrekte Vorne/Hinten-Hemisphaere (siehe Config.azimuth_mirror):
|
|
25° -> 155°, -25° -> -155°, 90°/-90° bleiben an der Mehrdeutigkeitsgrenze (dort ist
|
|
cos(az)~0, Vorne/Hinten ohnehin nicht unterscheidbar -- s. on_boundary-Check)."""
|
|
if az_deg >= 0:
|
|
return 180.0 - az_deg
|
|
return -180.0 - az_deg
|
|
|
|
|
|
def doa_pairs(X: np.ndarray, frequency_hz: float, cfg: Config) -> dict:
|
|
"""Haupt-Winkel-Schaetzung in der KEGEL-KONVENTION (umgestellt 07.10.26).
|
|
|
|
Ausgabe:
|
|
Azimut 0 .. +360 Grad -- Drehwinkel UM die Boresight-Achse
|
|
(Bereich umstellbar, s. AZIMUT_0_360)
|
|
Elevation 0 .. +90 Grad -- Winkel ueber der ARRAY-EBENE
|
|
(90 = Boresight, 0 = in der Array-Ebene),
|
|
gleiche Achse wie der Silicon Labs AoA Analyzer
|
|
|
|
Warum diese Konvention die richtige fuer ein planares Array ist:
|
|
gemessen werden zwei Richtungskosinus, u entlang der Spaltenachse (+y) und
|
|
v entlang der Zeilenachse (+z). Beide zusammen legen den Drehwinkel um die
|
|
Boresight-Achse EINDEUTIG fest -- daher der volle Kreis beim Azimut. Was ein
|
|
planares Array prinzipiell NICHT unterscheiden kann, ist vorne von hinten
|
|
(eine Quelle bei +w und bei -w erzeugt dieselben Phasen). Genau diese
|
|
Mehrdeutigkeit ist der Grund, warum die Elevation nur 0 bis 90 Grad
|
|
abdeckt: der gesamte Messbereich ist die vordere Halbkugel.
|
|
|
|
Die Elevation zaehlt ab der ARRAY-EBENE, nicht ab der Boresight-Achse: 90 Grad
|
|
heisst frontal, 0 Grad heisst in der Ebene liegend. Das ist die Achse des
|
|
Silicon Labs AoA Analyzer, am 07.10.26 am Aufbau bestaetigt (Tag frontal ->
|
|
Analyzer zeigt nahe 90). Damit sind beide Anzeigen direkt vergleichbar.
|
|
|
|
Die alte az/el-Konvention (beide Achsen -90..+90) warf dagegen Information
|
|
weg: sie presste den vollen Drehwinkel in einen Halbkreis und brauchte dafuer
|
|
mirror_azimuth() als Heuristik. Das entfaellt jetzt.
|
|
|
|
Nullrichtung des Azimuts: 0 Grad zeigt NACH UNTEN, +90 Grad nach rechts
|
|
(Blick auf die Antennenseite), 180 Grad nach oben, -90 Grad nach links --
|
|
also so, wie es auf der Karte aufgedruckt ist. Stimmt das bei dir nicht,
|
|
verdreht cfg.azimuth_offset_deg die Nullrichtung.
|
|
|
|
Nebeneffekt: die Azimut-Paare laufen jetzt mit coupling = -1 statt
|
|
-cos(Elevation). Der cos-Faktor war ein Artefakt der alten Parametrisierung.
|
|
Die beiden 1D-Scans messen u und v unabhaengig voneinander, wie es der
|
|
Physik eines planaren Arrays entspricht; die Kopplung entsteht erst bei der
|
|
Umrechnung in Winkel.
|
|
"""
|
|
xyz = coordinates(cfg)
|
|
n_ant, n_snap = X.shape
|
|
if n_ant != 16 or n_snap < 2:
|
|
raise ValueError("doa_pairs braucht 16 Antennen und mind. 2 Snapshots")
|
|
vertical_pairs, horizontal_pairs = find_pairs(xyz, cfg)
|
|
wave = 2 * np.pi * frequency_hz / 299_792_458.0
|
|
|
|
hor = scan_1d(X, horizontal_pairs, wave, cfg.pitch_x_m, coupling=-1.0)
|
|
ver = scan_1d(X, vertical_pairs, wave, cfg.pitch_y_m)
|
|
|
|
u_sign = -1.0 if cfg.flip_azimuth else 1.0
|
|
v_sign = -1.0 if cfg.flip_elevation else 1.0
|
|
|
|
def in_winkel(h_deg: float, v_deg: float) -> tuple[float, float]:
|
|
u = u_sign * float(np.sin(np.radians(h_deg)))
|
|
v = v_sign * float(np.sin(np.radians(v_deg)))
|
|
return winkel_aus_richtungskosinus(u, v)
|
|
|
|
az_deg, el_deg = in_winkel(hor["music_deg"], ver["music_deg"])
|
|
az_bartlett_deg, el_bartlett_deg = in_winkel(hor["bartlett_deg"], ver["bartlett_deg"])
|
|
|
|
# Konstante Offsets zuletzt. Azimut wird auf -180..180 zurueckgefaltet,
|
|
# Elevation auf 0..90 begrenzt -- ausserhalb gibt es keine Messung.
|
|
def falten(w: float) -> float:
|
|
w = (w + cfg.azimuth_offset_deg) % 360.0
|
|
return w if AZIMUT_0_360 else (w + 180.0) % 360.0 - 180.0
|
|
az_deg = falten(az_deg)
|
|
az_bartlett_deg = falten(az_bartlett_deg)
|
|
el_deg = min(max(el_deg + cfg.elevation_offset_deg, 0.0), 90.0)
|
|
el_bartlett_deg = min(max(el_bartlett_deg + cfg.elevation_offset_deg, 0.0), 90.0)
|
|
|
|
return {
|
|
"azimuth_deg": az_deg,
|
|
"elevation_deg": el_deg,
|
|
"confidence": float(min(ver["music_confidence"], hor["music_confidence"])),
|
|
# Bartlett-Gegenprobe (MSBeni): grosse Abweichung von MUSIC = Schaetzung unsicher
|
|
"azimuth_bartlett_deg": az_bartlett_deg,
|
|
"elevation_bartlett_deg": el_bartlett_deg,
|
|
# Seit der Umstellung identisch mit azimuth_deg: es gibt keinen getrennten
|
|
# "sicheren" Bereich mehr, der Azimut ist ueber den vollen Kreis eindeutig.
|
|
"azimuth_internal_deg": az_deg,
|
|
}
|
|
|
|
|
|
KALIBRIER_DATEI = "antennen_kalibrierung.json"
|
|
_kalibrierung: dict | None = None
|
|
|
|
|
|
def lade_kalibrierung() -> dict | None:
|
|
"""Antennen-Phasenoffsets aus antennen_kalibrierung.json laden, falls vorhanden.
|
|
|
|
Fehlt die Datei, laeuft alles wie frueher weiter -- dann allerdings mit dem
|
|
bekannten Problem, dass die Mittelung ueber die Antennenpaare in scan_1d an
|
|
den unterschiedlichen Geraeteoffsets zerbricht (gemessen 02.10.26: Kohaerenz
|
|
je Paar 1.000, nach Mittelung 0.474, Ergebnis conf = 1.0).
|
|
|
|
Erzeugt wird die Datei von kalibrierung.py aus einer Boresight-Aufnahme.
|
|
"""
|
|
try:
|
|
with open(KALIBRIER_DATEI, "r", encoding="utf-8") as f:
|
|
daten = json.load(f)
|
|
except FileNotFoundError:
|
|
return None
|
|
except (json.JSONDecodeError, OSError) as fehler:
|
|
print(f"{KALIBRIER_DATEI} nicht lesbar ({fehler}) -- laufe ohne Kalibrierung.",
|
|
file=sys.stderr)
|
|
return None
|
|
|
|
if list(daten.get("rf_order", [])) != list(RF_ORDER):
|
|
print(f"{KALIBRIER_DATEI} wurde mit einem ANDEREN Switching-Pattern aufgenommen "
|
|
f"({daten.get('rf_order', 'alt, 0..15')}) -- wird NICHT verwendet. "
|
|
f"Neu kalibrieren: Boresight-Aufnahme, dann kalibrierung.py.", file=sys.stderr)
|
|
return None
|
|
global_rad = np.asarray(daten.get("global_rad", []), dtype=float)
|
|
if global_rad.shape != (len(RF_ORDER),):
|
|
print(f"{KALIBRIER_DATEI} passt nicht zu {len(RF_ORDER)} Antennen "
|
|
f"-- laufe ohne Kalibrierung.", file=sys.stderr)
|
|
return None
|
|
|
|
kanal = {}
|
|
for schluessel, werte in (daten.get("kanal_rad") or {}).items():
|
|
reihe = np.asarray(werte, dtype=float)
|
|
if reihe.shape == global_rad.shape:
|
|
kanal[int(schluessel)] = reihe
|
|
|
|
return {"global": global_rad, "kanal": kanal,
|
|
"quelle": daten.get("quelle", "?"),
|
|
"stabilitaet": daten.get("stabilitaet"),
|
|
"pakete": daten.get("pakete")}
|
|
|
|
|
|
def kalibrierung_anwenden(X: np.ndarray, channel: int) -> np.ndarray:
|
|
"""Feste Phasenoffsets der Antennen herausrechnen.
|
|
|
|
Die Offsets sind leicht frequenzabhaengig (Leiterbahnlaufzeit), deshalb wird
|
|
der zum Kanal passende Satz bevorzugt; fuer Kanaele ohne eigene Messung
|
|
dienen die kanaluebergreifenden Werte als Rueckfallebene.
|
|
"""
|
|
if _kalibrierung is None:
|
|
return X
|
|
offsets = _kalibrierung["kanal"].get(channel, _kalibrierung["global"])
|
|
return X * np.exp(-1j * offsets)[:, None]
|
|
|
|
|
|
# ------------------------------------------------------------------ Selbstkalibrierung
|
|
# Gemessen am 08.10.26 auf 1008 Paketen (iq_samples.jsonl, 37 Kanaele):
|
|
#
|
|
# Stabilitaet des Phasenvektors INNERHALB eines Kanals 0.944 (19.4 Grad)
|
|
# Stabilitaet ueber alle 37 Kanaele gemischt 0.830 (35.0 Grad)
|
|
# Streuung der Antennenoffsets ZWISCHEN den Kanaelen 32.4 Grad RMS
|
|
#
|
|
# Die Offsets sind also kanalabhaengig, und antennen_kalibrierung.json stammt aus
|
|
# einem ANDEREN Programmlauf, in dem sie andere Werte hatten (gemessen 37.3 Grad
|
|
# RMS Unterschied zwischen Laeufen). Ergebnis: die Datei korrigiert zwar etwas,
|
|
# aber nicht die Kanalabhaengigkeit DIESES Laufs.
|
|
#
|
|
# Abhilfe hier: die Kanaltabelle wird AUS DEM LAUFENDEN LAUF selbst geschaetzt.
|
|
# Pro Kanal wird der mittlere (normierte) Phasenvektor gesammelt; die Korrektur
|
|
# ist die Abweichung dieses Kanals vom Mittel ueber alle Kanaele. Dafuer muss die
|
|
# Richtung NICHT bekannt sein -- es genuegt, dass der Tag waehrend der Lernphase
|
|
# still steht. Nach SELBSTKAL_LERNPAKETE wird die Tabelle eingefroren, damit ein
|
|
# spaeter bewegter Tag sie nicht verdirbt.
|
|
#
|
|
# WICHTIG, was das NICHT tut: die festen Offsets je Antenne (Leiterbahnlaufzeit)
|
|
# bleiben unangetastet, denn die stecken im Mittel ueber alle Kanaele und werden
|
|
# hier gerade herausgerechnet. Gegen den raeumlichen Fehler (Kohaerenz je Paar
|
|
# 0.38, Ebene-Welle-Fit 0.57) hilft das also nicht -- gemessen: der Fit aendert
|
|
# sich dadurch um 0.001. Diese Funktion verbessert die STREUUNG, nicht die
|
|
# absolute Richtung.
|
|
# Dimensionierung aus einem Out-of-Sample-Test am 08.10.26 (Tabelle auf den ersten
|
|
# N Paketen gelernt, Fehler auf ALLEN UEBRIGEN gemessen, 1008 Pakete / 37 Kanaele):
|
|
#
|
|
# N Kanaele ohne Korrektur mit Korrektur nach Block von 30
|
|
# 200 22 33.4 Grad 11.5 Grad 6.1 -> 2.1 Grad
|
|
# 400 36 32.6 Grad 9.0 Grad 6.0 -> 1.7 Grad
|
|
# 600 37 32.9 Grad 7.9 Grad 6.0 -> 1.4 Grad
|
|
# Decke (Streuung innerhalb eines Kanals) 7.4 Grad -> 1.4 Grad
|
|
#
|
|
# Ab 600 Paketen ist die Decke praktisch erreicht, deshalb dieser Wert: rund 12 s
|
|
# bei 20 ms Advertising-Intervall. Die Korrektur wird ERST NACH der Lernphase
|
|
# angewendet, nie mit einer noch wandernden Tabelle -- eine sich aendernde
|
|
# Korrektur verschmiert sonst genau das, was sie reparieren soll (gemessen:
|
|
# Streuung innerhalb eines Kanals fiel dabei von 0.992 auf 0.881).
|
|
SELBSTKAL_AKTIV = True
|
|
SELBSTKAL_MIN_PAKETE = 8 # Pakete auf einem Kanal, damit er in die Tabelle kommt
|
|
SELBSTKAL_LERNPAKETE = 600 # danach ist die Tabelle fest und wird angewendet
|
|
|
|
_selbstkal_summe: dict[int, np.ndarray] = {}
|
|
_selbstkal_anzahl: dict[int, int] = {}
|
|
_selbstkal_tabelle: dict[int, np.ndarray] = {}
|
|
_selbstkal_gesehen = 0
|
|
_selbstkal_fertig = False
|
|
|
|
|
|
def _selbstkal_tabelle_rechnen() -> None:
|
|
"""Kanaltabelle aus den bisher gesammelten Mittelwerten neu aufbauen."""
|
|
reif = {c: summe / _selbstkal_anzahl[c]
|
|
for c, summe in _selbstkal_summe.items()
|
|
if _selbstkal_anzahl[c] >= SELBSTKAL_MIN_PAKETE}
|
|
if len(reif) < 2:
|
|
return
|
|
stapel = []
|
|
for vektor in reif.values():
|
|
betrag = np.abs(vektor)
|
|
if np.any(betrag < 1e-12):
|
|
return
|
|
stapel.append(vektor / betrag)
|
|
mittel = np.array(stapel).mean(axis=0)
|
|
betrag = np.abs(mittel)
|
|
if np.any(betrag < 1e-12):
|
|
return
|
|
mittel = mittel / betrag
|
|
_selbstkal_tabelle.clear()
|
|
for kanal, vektor in reif.items():
|
|
korrektur = (vektor / np.abs(vektor)) * np.conj(mittel)
|
|
_selbstkal_tabelle[kanal] = korrektur / np.abs(korrektur)
|
|
|
|
|
|
def selbstkalibrierung_lernen(X: np.ndarray, channel: int) -> None:
|
|
"""Phasenvektor dieses Pakets in die Kanalstatistik einrechnen."""
|
|
global _selbstkal_gesehen, _selbstkal_fertig
|
|
if not SELBSTKAL_AKTIV or _selbstkal_fertig:
|
|
return
|
|
vektor = X.mean(axis=1)
|
|
betrag = np.abs(vektor)
|
|
if np.any(betrag < 1e-12):
|
|
return
|
|
vektor = vektor / betrag
|
|
# VERANKERN: jedes CTE-Paket startet mit beliebiger Gesamtphase. Ohne diesen
|
|
# Schritt mittelt sich ueber die Pakete genau diese Zufallsphase mit ein und
|
|
# die Kanaltabelle wird unbrauchbar (gemessen 08.10.26: 16.0 statt 7.9 Grad
|
|
# Restfehler). Fuer die Richtungsschaetzung sind nur Phasen-DIFFERENZEN
|
|
# zwischen Antennen relevant, eine gemeinsame Drehung ist also unschaedlich.
|
|
anker = vektor.mean()
|
|
if abs(anker) < 1e-3:
|
|
anker = vektor[0] # Rueckfall, falls der Mittelwert ausloescht
|
|
vektor = vektor * np.conj(anker / abs(anker))
|
|
if channel in _selbstkal_summe:
|
|
_selbstkal_summe[channel] = _selbstkal_summe[channel] + vektor
|
|
_selbstkal_anzahl[channel] += 1
|
|
else:
|
|
_selbstkal_summe[channel] = vektor
|
|
_selbstkal_anzahl[channel] = 1
|
|
_selbstkal_gesehen += 1
|
|
if _selbstkal_gesehen >= SELBSTKAL_LERNPAKETE:
|
|
_selbstkal_tabelle_rechnen()
|
|
_selbstkal_fertig = True
|
|
fehlt = sorted(k for k, n in _selbstkal_anzahl.items() if n < SELBSTKAL_MIN_PAKETE)
|
|
print(f"Selbstkalibrierung: aktiv nach {_selbstkal_gesehen} Paketen, "
|
|
f"{len(_selbstkal_tabelle)} Kanaele in der Tabelle"
|
|
+ (f", ohne Korrektur bleiben {fehlt}" if fehlt else "") + ".", flush=True)
|
|
|
|
|
|
def selbstkalibrierung_anwenden(X: np.ndarray, channel: int) -> np.ndarray:
|
|
"""Kanalabhaengigen Anteil der Antennenoffsets herausrechnen."""
|
|
if not SELBSTKAL_AKTIV or not _selbstkal_fertig:
|
|
return X # waehrend der Lernphase unveraendert, s. Kommentar oben
|
|
korrektur = _selbstkal_tabelle.get(channel)
|
|
if korrektur is None:
|
|
return X
|
|
return X * np.conj(korrektur)[:, None]
|
|
|
|
|
|
def process_packet(packet: dict, cfg: Config) -> dict | None:
|
|
"""Ein IQ-Paket verarbeiten und Snapshots fuer die Winkel-Schätzung liefern (oder None)."""
|
|
ch = int(packet["channel"])
|
|
f = ble_frequency_hz(ch)
|
|
samples = iq_complex(packet["iq"])
|
|
if len(samples) != cfg.expected_samples:
|
|
return None
|
|
fixed = compensate_references(samples, cfg, geglaettete_drift(drift_schaetzen(samples, cfg)))
|
|
X = snapshots_from_samples(fixed[cfg.references:], cfg)
|
|
X = kalibrierung_anwenden(X, ch)
|
|
selbstkalibrierung_lernen(X, ch) # lernt auf dem noch unkorrigierten Vektor
|
|
X = selbstkalibrierung_anwenden(X, ch)
|
|
return {"X": X, "frequency_hz": f}
|
|
|
|
|
|
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"
|
|
IQ_FILE = "iq_samples.jsonl"
|
|
ANGLE_TABLE_FILE = "winkel_tabelle.json"
|
|
SLOT_DURATION_US = 2
|
|
CTE_COUNT = 0
|
|
ACCUMULATE_PACKETS = 1 # MUSIC nach JEDEM Paket (03.10.26). Ein Paket liefert 2 Snapshots
|
|
# (2 volle 17er-Zyklen aus 45 Samples), also 2 x 12 Paare = 24 Spalten
|
|
# im virtuellen Array von scan_1d -- genug fuer die 2x2-Kovarianz, aber
|
|
# deutlich verrauschter als vorher mit 8 Paketen. Die Streuung faengt
|
|
# dafuer die Glaettung ueber FILTER_WINDOW Werte auf.
|
|
FILTER_WINDOW = 30 # Blockgroesse (07.10.26, vorher 100): je 50 angenommene Schaetzungen ergeben EINEN Mittelwert.
|
|
# Bei ACCUMULATE_PACKETS = 1 und 40 ms Sendeintervall sind das rund
|
|
# 4 Sekunden -- vorausgesetzt, alle Pakete kommen durch den Filter.
|
|
FILTER_MAX_ALTER_S = 8.0 # Zusaetzliche Altersgrenze (05.10.26). FILTER_WINDOW allein zaehlt
|
|
# ANGENOMMENE Schaetzungen, nicht Pakete: faellt die Annahmequote auf
|
|
# 20 %, umfasst das Fenster 20 s statt 4 s und der Ausgabewert haengt
|
|
# minutenlang an einer alten Kartenstellung fest. Werte, die aelter
|
|
# als FILTER_MAX_ALTER_S sind, verfallen deshalb unabhaengig davon.
|
|
AUSREISSER_SIGMA = 2.5 # Verwirft Werte, die weiter als 2.5 Sigma vom gewichteten Schwerpunkt
|
|
# liegen (05.10.26). CONFIDENCE_MIN allein genuegt nicht: ein Multipath-
|
|
# Peak ist scharf, also hoch-konfident UND falsch.
|
|
PRINT_EVERY_N_PACKETS = 10
|
|
CONFIDENCE_MIN = 1.0 # Praktisch AUS seit 07.10.26 (vorher 5.0). Der AoA Analyzer kennt keine
|
|
# solche Schwelle und liefert auch bei streifendem Einfall einen Wert; dieses
|
|
# Skript soll sich genauso verhalten. Gemessen an der Aufnahme von 23:14:
|
|
# mit 5.0 wurden 966 von 969 Paketen verworfen, die Winkel-Tabelle blieb leer.
|
|
# Die Konfidenz steht weiter in jeder Logzeile -- sie bewertet die Schaetzung,
|
|
# verwirft sie aber nicht mehr.
|
|
|
|
lib = None
|
|
iq_file = None
|
|
already_synced = False
|
|
pending_actions = queue.Queue()
|
|
snapshot_buffer: dict[int, list[np.ndarray]] = {}
|
|
angle_table: list[dict] = []
|
|
last_azimut = None
|
|
last_elevation = None
|
|
# Ein gemeinsamer Puffer statt zwei getrennter (05.10.26): Azimut und Elevation gehoeren
|
|
# zusammen, weil der Azimut-Scan mit coupling = -cos(el) der GLEICHEN Schaetzung rechnet.
|
|
# Eintrag: (Zeitstempel, u, v, Gewicht) -- u/v sind Richtungskosinus, s. richtungskosinus().
|
|
_recent: deque = deque() # kein maxlen: der Block wird nach FILTER_WINDOW Werten geleert
|
|
_letzter_block: tuple | None = None # (az, el, streuung, n) des zuletzt fertigen Blocks
|
|
ACTIVE_CONFIG = Config()
|
|
SHUTTING_DOWN = False # verhindert Schreibzugriff auf iq_file nach dessen Schliessung beim Beenden
|
|
|
|
|
|
def rssi_weight(rssi_dbm: int) -> float:
|
|
"""Gewichtungsfaktor aus RSSI: stärkeres Signal zählt mehr im gewichteten Median."""
|
|
return max(rssi_dbm + 100.0, 1.0)
|
|
|
|
|
|
|
|
AZIMUT_0_360 = False # Ausgabebereich des Azimuts (08.10.26, umgestellt): -180 bis +180,
|
|
# wie der AoA Analyzer. Keine Werte ueber 180 Grad mehr. True: 0 bis 360, keine
|
|
# negativen Werte. False: -180 bis +180. Nur die DARSTELLUNG aendert sich,
|
|
# gerechnet wird weiter in Richtungskosinus -- 350 und -10 sind dieselbe
|
|
# Richtung. Die Mittelung bleibt davon unberuehrt, weil sie u und v mittelt
|
|
# und nicht Gradzahlen; ein Mittelwert um 0 herum kippt also nicht.
|
|
|
|
|
|
def richtungskosinus(az_deg: float, el_deg: float) -> tuple[float, float]:
|
|
"""(Azimut, Elevation) -> Richtungskosinus (u, v) in der Array-Ebene.
|
|
|
|
u = Anteil entlang +y (Spaltenachse, nach rechts)
|
|
v = Anteil entlang +z (Zeilenachse, nach oben)
|
|
|
|
Kegel-Konvention (07.10.26): el ist der Kegelwinkel zur Boresight-Achse
|
|
(0 = frontal, 90 = in der Array-Ebene), az der Drehwinkel um diese Achse mit
|
|
0 Grad nach unten und +90 Grad nach rechts.
|
|
|
|
u und v sind das, was das Array physikalisch misst: die Phasenrampe ueber die
|
|
Antennen ist LINEAR in u bzw. v, nicht in Grad. Deshalb wird in diesem Raum
|
|
gemittelt und erst am Ende zurueckgerechnet.
|
|
"""
|
|
el = np.radians(min(max(el_deg, 0.0), 90.0))
|
|
az = np.radians(az_deg)
|
|
r = np.cos(el) # 90 Grad = Boresight -> r = 0; 0 Grad = Array-Ebene -> r = 1
|
|
return float(r * np.sin(az)), float(-r * np.cos(az))
|
|
|
|
|
|
def winkel_aus_richtungskosinus(u: float, v: float) -> tuple[float, float]:
|
|
"""Rueckweg von (u, v) nach (Azimut -180..180, Elevation 0..90).
|
|
|
|
u^2 + v^2 > 1 kann durch Rauschen entstehen und hat keine Richtung mehr:
|
|
der Punkt wird dann auf den Einheitskreis gezogen, also auf Elevation 90.
|
|
"""
|
|
r = float(np.hypot(u, v))
|
|
if r > 1.0:
|
|
u, v, r = u / r, v / r, 1.0
|
|
el_deg = float(np.degrees(np.arccos(r)))
|
|
if r < 1e-9:
|
|
return 0.0, el_deg
|
|
az_deg = float(np.degrees(np.arctan2(u, -v)))
|
|
if AZIMUT_0_360:
|
|
az_deg = az_deg % 360.0
|
|
return az_deg, el_deg
|
|
|
|
|
|
def schaetz_gewicht(confidence: float) -> float:
|
|
"""Gewicht einer Einzelschaetzung fuer die Glaettung.
|
|
|
|
Bis 04.10.26 wurde mit rssi_weight() gewichtet -- das war der Fehler in der
|
|
Glaettung: RSSI sagt, wie STARK das Signal war, nicht wie GUT die Schaetzung
|
|
ist. Ueber den praktischen Bereich (-45 bis -65 dBm) spreizt rssi_weight nur
|
|
55:35, also faktisch gar nicht, waehrend die Konfidenz zwischen 5 und 105
|
|
schwankt. Eine Schaetzung, die den Filter gerade so passiert hat (conf 5.1),
|
|
zaehlte damit genauso viel wie eine saubere (conf 105).
|
|
"""
|
|
return (max(confidence - CONFIDENCE_MIN, 0.0) + 1.0) ** 2
|
|
|
|
|
|
def glaettung_zuruecksetzen() -> None:
|
|
"""Angefangenen Block und letztes Ergebnis verwerfen -- nach dem Umstellen der
|
|
Karte aufrufen, dann vermischt der naechste Block keine zwei Stellungen."""
|
|
global _letzter_block
|
|
_recent.clear()
|
|
_letzter_block = None
|
|
|
|
|
|
def smoothed_angle(new_az: float, new_el: float,
|
|
weight: float) -> tuple[float | None, float | None, float | None, int, bool]:
|
|
"""BLOCKMITTELWERT (05.10.26): sammelt FILTER_WINDOW Schaetzungen und gibt dann
|
|
EINEN Mittelwert aus, danach beginnt ein neuer, leerer Block.
|
|
|
|
Rueckgabe: (Azimut, Elevation, Streuung in Grad, Anzahl, neu).
|
|
"neu" ist nur in dem Paket True, in dem ein Block fertig geworden ist -- nur
|
|
dann kommt ein Wert in die Winkel-Tabelle. Solange der erste Block laeuft,
|
|
sind Azimut/Elevation/Streuung None; danach wird zwischen zwei fertigen
|
|
Bloecken der zuletzt fertige Wert weitergereicht, damit die Anzeige nicht leer
|
|
faellt.
|
|
|
|
Unterschied zum gleitenden Mittel (Stand bis 05.10.26): dort lief ein Fenster
|
|
von 100 Werten mit, und jedes Paket lieferte einen neuen, mit dem vorherigen
|
|
stark korrelierten Ausgabewert. Hier ist jeder Ausgabewert aus 100 FRISCHEN,
|
|
nicht wiederverwendeten Schaetzungen gerechnet. Die Bloecke sind damit
|
|
voneinander unabhaengig, was sie fuer eine Messreihe erst auswertbar macht:
|
|
zwei aufeinanderfolgende Bloecke sind zwei echte Wiederholungsmessungen.
|
|
Preis dafuer: ein Ausgabewert nur noch alle FILTER_WINDOW Pakete, bei rund
|
|
19 Paketen/s also etwa alle 2,6 Sekunden
|
|
(und entsprechend laenger, wenn der Konfidenzfilter Pakete verwirft).
|
|
|
|
Gerechnet wird weiter in Richtungskosinus (s. richtungskosinus()), damit
|
|
Azimut und Elevation ihre Kopplung ueber cos(el) behalten.
|
|
"""
|
|
global _letzter_block
|
|
jetzt = time.monotonic()
|
|
|
|
# Lange Pause: angefangenen Block verwerfen, sonst mischt er zwei Stellungen.
|
|
if _recent and (jetzt - _recent[-1][0]) > FILTER_MAX_ALTER_S:
|
|
_recent.clear()
|
|
|
|
u, v = richtungskosinus(new_az, new_el)
|
|
_recent.append((jetzt, u, v, max(weight, 1e-6)))
|
|
|
|
if len(_recent) < FILTER_WINDOW:
|
|
if _letzter_block is None:
|
|
return None, None, None, len(_recent), False
|
|
az, el, s, n = _letzter_block
|
|
return az, el, s, n, False
|
|
|
|
daten = np.asarray(_recent, dtype=float)
|
|
_recent.clear() # Block abgeschlossen, naechster faengt bei 0 an
|
|
uu, vv, ww = daten[:, 1], daten[:, 2], daten[:, 3]
|
|
u_m = float(np.average(uu, weights=ww))
|
|
v_m = float(np.average(vv, weights=ww))
|
|
|
|
# Eine Ausreisser-Runde: Mittelwert bleibt Mittelwert, aber ein einzelner
|
|
# Multipath-Treffer kippt den Block nicht.
|
|
abstand = np.hypot(uu - u_m, vv - v_m)
|
|
sigma = float(np.sqrt(np.average(abstand ** 2, weights=ww)))
|
|
if sigma > 1e-9:
|
|
gut = abstand <= AUSREISSER_SIGMA * sigma
|
|
if int(gut.sum()) >= 4:
|
|
uu, vv, ww = uu[gut], vv[gut], ww[gut]
|
|
u_m = float(np.average(uu, weights=ww))
|
|
v_m = float(np.average(vv, weights=ww))
|
|
|
|
streuung = float(np.degrees(np.sqrt(np.average((uu - u_m) ** 2 + (vv - v_m) ** 2, weights=ww))))
|
|
az_deg, el_deg = winkel_aus_richtungskosinus(u_m, v_m)
|
|
_letzter_block = (az_deg, el_deg, streuung, int(len(ww)))
|
|
return az_deg, el_deg, streuung, int(len(ww)), True
|
|
|
|
|
|
def signed_iq(raw_bytes: bytes) -> tuple[list[int], list[int]]:
|
|
"""Rohbytes in vorzeichenbehaftete I- und Q-Listen entpacken."""
|
|
values = struct.unpack(f"{len(raw_bytes)}b", raw_bytes)
|
|
return list(values[0::2]), list(values[1::2])
|
|
|
|
|
|
# ---------------------------------------------------------------- Ausgabe-Konvention
|
|
# Intern rechnet alles in der KEGEL-Konvention: Elevation 90 = Boresight, 0 = in der
|
|
# Array-Ebene. In dieser Form wird auch gemittelt (ueber u und v). Sie bleibt
|
|
# unangetastet: richtungskosinus() und winkel_aus_richtungskosinus() sind zueinander
|
|
# invers und duerfen nicht auseinanderlaufen, sonst bricht die Glaettung.
|
|
#
|
|
# Der AoA Analyzer misst die Elevation VOM Boresight aus: dort 0 statt 90. Beide
|
|
# beschreiben dieselbe Richtung, sie sind komplementaer. Abgeleitet aus zwei Messungen
|
|
# am selben Aufbau (08.10.26):
|
|
#
|
|
# Boresight Analyzer Az 1.54 El 2.06
|
|
# Stellung "133" Analyzer Az 133.67 El 10.83
|
|
#
|
|
# Ein Azimut von 133 Grad schliesst aus, dass der Azimut ein horizontaler Winkel ist
|
|
# -- ein solcher waere auf +-90 begrenzt. Der Analyzer benutzt also dieselbe polare
|
|
# Form wie dieses Skript (Azimut = Drehung um die Boresight-Achse, -180..180), nur mit
|
|
# dem anderen Nullpunkt der Elevation.
|
|
#
|
|
# NOCH ZU BESTAETIGEN: die Elevation stimmt mit beiden Messpunkten ueberein, das
|
|
# Vorzeichen des Azimuts abseits der Achse nicht -- am Boresight ist der Azimut
|
|
# bedeutungslos und taugt nicht als Beleg. Dafuer eine Messung in Stellung "133" mit
|
|
# der frischen Kalibrierung gegen Az 133.67 / El 10.83 halten.
|
|
AUSGABE_KONVENTION = "analyzer" # = interne Kegel-Form: Elevation 90 = Boresight, wie der Analyzer
|
|
|
|
|
|
def ausgabe_winkel(az_deg: float, el_deg: float) -> tuple[float, float]:
|
|
"""Ausgabe in der Konvention des AoA Analyzers -- sie ist IDENTISCH mit der
|
|
internen Kegel-Konvention, es wird nichts umgerechnet.
|
|
|
|
Belegt am 08.10.26 durch zwei Analyzer-Ansichten am selben Aufbau:
|
|
- Tag frontal vor der Karte (Pfeil von vorn als Kreisscheibe zu sehen):
|
|
Az 4.39, El 82.29 -> Elevation 90 = Boresight
|
|
- Azimut-Marken 0/45/90/135/180/-135/-90/-45 rings um die Karte, 0 unten,
|
|
90 rechts -> Azimut = Drehung um die Boresight-Achse
|
|
Genau so rechnet dieses Skript intern. Zwei zwischenzeitliche Umrechnungen
|
|
(90 - el, dann sphaerisch) beruhten auf der Fehldeutung einer Analyzer-Anzeige
|
|
El 2.06 als "frontal" -- der Tag lag dort tatsaechlich fast in der Array-Ebene.
|
|
"""
|
|
return az_deg, el_deg
|
|
|
|
|
|
def add_to_angle_table(zeit: str, counter: int, azimut: float, elevation: float) -> None:
|
|
"""Neuen Winkel nur speichern, wenn er sich vom letzten unterscheidet."""
|
|
global last_azimut, last_elevation
|
|
if azimut == last_azimut and elevation == last_elevation:
|
|
return
|
|
last_azimut, last_elevation = azimut, elevation
|
|
angle_table.append({"zeit": zeit, "counter": counter, "azimut": azimut, "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: Any) -> None:
|
|
"""IQ-Paket speichern, Pakete akkumulieren und Winkel schätzen."""
|
|
global snapshot_buffer
|
|
i_values, q_values = signed_iq(event.samples)
|
|
channel = int(event.channel)
|
|
packet = {"channel": channel, "rssi": int(event.rssi), "iq": list(zip(i_values, q_values))}
|
|
zeit = datetime.now(timezone.utc).isoformat()
|
|
record = {"zeit": zeit, "counter": int(event.event_counter), "channel": channel,
|
|
"rssi": int(event.rssi), "i": i_values, "q": q_values,
|
|
"azimut": None, "elevation": None}
|
|
|
|
result = process_packet(packet, ACTIVE_CONFIG)
|
|
if result is not None:
|
|
channel_buffer = snapshot_buffer.setdefault(channel, [])
|
|
channel_buffer.append(result["X"])
|
|
if len(channel_buffer) >= ACCUMULATE_PACKETS:
|
|
combined = np.concatenate(channel_buffer, axis=1)
|
|
snapshot_buffer[channel] = []
|
|
try:
|
|
angles = doa_pairs(combined, result["frequency_hz"], ACTIVE_CONFIG)
|
|
az_raw, el_raw, conf = angles["azimuth_deg"], angles["elevation_deg"], angles["confidence"]
|
|
# Randwert-Pruefung auf dem UNGESPIEGELTEN Azimut: nahe +-90° ist cos(az)~0, dort ist
|
|
# die Vorne/Hinten-Spiegelung selbst instabil (kleine Fehler koennen die Hemisphaere
|
|
# kippen) -- unabhaengig davon, wie weit der gespiegelte, berichtete Wert az_raw liegt.
|
|
# Randwert-Filter ENTFERNT am 07.10.26 auf Wunsch: streifende Einfaelle
|
|
# (Elevation nahe 0, Quelle in der Array-Ebene) werden nicht mehr verworfen.
|
|
# Der AoA Analyzer liefert dort ebenfalls einen Wert, also soll dieses Skript
|
|
# es auch. Der Azimut ist dort allerdings empfindlicher, weil u^2+v^2 gegen 1
|
|
# geht -- sichtbar an der Konfidenz, nicht mehr an einem harten Filter.
|
|
on_boundary = False
|
|
low_confidence = conf < CONFIDENCE_MIN
|
|
bartlett_diff = (abs(angles["azimuth_bartlett_deg"] - az_raw) +
|
|
abs(angles["elevation_bartlett_deg"] - el_raw))
|
|
if on_boundary or low_confidence:
|
|
reason = "Randwert" if on_boundary else "geringe Konfidenz"
|
|
print(f">>> AoA roh (verworfen, {reason}, conf={conf:.1f}): "
|
|
f"Azimut={az_raw:+.1f}° Elevation={el_raw:+.1f}°",
|
|
file=sys.stderr, flush=True)
|
|
else:
|
|
az_smooth, el_smooth, streuung, n_used, block_fertig = smoothed_angle(
|
|
az_raw, el_raw, schaetz_gewicht(conf))
|
|
record["azimut"] = az_smooth
|
|
record["elevation"] = el_smooth
|
|
record["streuung"] = streuung
|
|
record["n_block"] = n_used
|
|
record["block_fertig"] = block_fertig
|
|
# Bartlett-Werte werden seit 05.10.26 nicht mehr ausgegeben (zu unruhig
|
|
# auf der Konsole). Die Gegenprobe LAEUFT weiter, nur die Zahlen bleiben
|
|
# unsichtbar; weicht sie stark ab, erscheint weiterhin ein kurzer Hinweis.
|
|
bart = " [ACHTUNG: Gegenprobe weicht ab]" if bartlett_diff > 15.0 else ""
|
|
az_raw_out, el_raw_out = ausgabe_winkel(az_raw, el_raw)
|
|
roh = (f">>> AoA roh: Azimut={az_raw_out:+.1f}° Elevation={el_raw_out:+.1f}° "
|
|
f"(conf={conf:.1f})")
|
|
if az_smooth is None:
|
|
# Erster Block laeuft noch, es gibt schlicht noch keinen Mittelwert.
|
|
print(f"{roh} | sammelt {n_used}/{FILTER_WINDOW}{bart}",
|
|
file=sys.stderr, flush=True)
|
|
else:
|
|
marke = " <<<<< BLOCK FERTIG" if block_fertig else ""
|
|
stand = (f"{FILTER_WINDOW}/{FILTER_WINDOW}" if block_fertig
|
|
else f"sammelt {len(_recent)}/{FILTER_WINDOW}")
|
|
az_s_out, el_s_out = ausgabe_winkel(az_smooth, el_smooth)
|
|
print(f"{roh} | Block-Mittel: Azimut={az_s_out:+.1f}° "
|
|
f"Elevation={el_s_out:+.1f}° (n={n_used}, "
|
|
f"Streuung={streuung:.1f}°, {stand}){bart}{marke}",
|
|
file=sys.stderr, flush=True)
|
|
# Nur fertige Bloecke in die Tabelle: eine Zeile = eine
|
|
# unabhaengige Messung aus FILTER_WINDOW frischen Schaetzungen.
|
|
if block_fertig:
|
|
add_to_angle_table(zeit, int(event.event_counter), az_s_out, el_s_out)
|
|
except ValueError:
|
|
pass
|
|
|
|
if SHUTTING_DOWN:
|
|
return
|
|
try:
|
|
iq_file.write(json.dumps(record, separators=(",", ":")) + "\n")
|
|
iq_file.flush()
|
|
except ValueError:
|
|
# Datei wurde waehrend des Beendens (Strg+C) bereits geschlossen; der BGAPI-
|
|
# Event-Thread kann noch kurz nachlaufen, das ist harmlos.
|
|
return
|
|
if int(event.event_counter) % PRINT_EVERY_N_PACKETS == 0:
|
|
print(f"IQ-Paket #{event.event_counter} Kanal={channel} RSSI={event.rssi} dBm "
|
|
f"Samples={len(i_values)}", file=sys.stderr, flush=True)
|
|
|
|
|
|
def on_event(event: Any) -> None:
|
|
"""BGAPI-Ereignisse behandeln: Scan, Sync, CTE-Empfang."""
|
|
global already_synced
|
|
name = str(event).split("(")[0]
|
|
if name in ("bt_evt_scanner_scan_report", "bt_evt_scanner_extended_advertisement_report"):
|
|
interval = getattr(event, "periodic_interval", None)
|
|
if interval and not already_synced:
|
|
already_synced = True
|
|
print(f"Tag gefunden RSSI={event.rssi} dBm Intervall={interval} ms", file=sys.stderr)
|
|
pending_actions.put(("sync", event.address, event.address_type, getattr(event, "adv_sid", 0)))
|
|
elif name in ("bt_evt_sync_opened", "bt_evt_sync_scanner_opened", "bt_evt_periodic_sync_opened"):
|
|
print(f"Synchronisation hergestellt (sync={event.sync})", file=sys.stderr)
|
|
pending_actions.put(("cte", event.sync))
|
|
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:
|
|
print(f"Fehler BG22: {event}", file=sys.stderr)
|
|
|
|
|
|
def main() -> int:
|
|
"""Verbindung zum BG22 aufbauen, Tag suchen und CTE-Empfang starten."""
|
|
global lib, iq_file, ACTIVE_CONFIG, ANGLE_TABLE_FILE, CONFIDENCE_MIN, ACCUMULATE_PACKETS
|
|
global _kalibrierung
|
|
parser = argparse.ArgumentParser(
|
|
description="BG22 IQ-Logger + Winkel-Schätzung (Navigine/MSBeni-Prinzip, kein Switching-"
|
|
"Pattern-Streit, keine Kalibrierdatei)")
|
|
parser.add_argument("--port", default=COM_PORT)
|
|
parser.add_argument("--baud", type=int, default=BAUD_RATE)
|
|
parser.add_argument("--xapi", default=XAPI_PATH)
|
|
parser.add_argument("--iq-out", default=IQ_FILE)
|
|
parser.add_argument("--angle-out", default=ANGLE_TABLE_FILE)
|
|
parser.add_argument("--pitch-x-m", type=float, default=0.040)
|
|
parser.add_argument("--pitch-y-m", type=float, default=0.040)
|
|
parser.add_argument("--phase-multiplier", type=float, default=2.0)
|
|
parser.add_argument("--accumulate", type=int, default=ACCUMULATE_PACKETS,
|
|
help="Anzahl Pakete pro Winkel-Schätzung akkumuliert (mehr = stabiler, langsamer)")
|
|
parser.add_argument("--confidence-min", type=float, default=CONFIDENCE_MIN,
|
|
help="Minimale Konfidenz (Peak/Median-Verhältnis) zum Akzeptieren einer Schätzung")
|
|
parser.add_argument("--flip-azimuth", action="store_true", dest="flip_azimuth", default=True,
|
|
help="Azimut-Vorzeichenumkehr EINSCHALTEN (seit 08.10.26 ohnehin der Default)")
|
|
parser.add_argument("--no-flip-azimuth", action="store_false", dest="flip_azimuth",
|
|
help="Azimut-Vorzeichenumkehr AUSSCHALTEN, liefert wieder das alte Vorzeichen")
|
|
parser.add_argument("--no-flip-elevation", action="store_false", dest="flip_elevation", default=True,
|
|
help="Spiegelung von v AUSSCHALTEN (per Default AN -- per Realtest am "
|
|
"01.10.26 gegen den AoA Analyzer bestätigt)")
|
|
parser.add_argument("--swap-axes", action="store_true",
|
|
help="Zeilen/Spalten-Rollen tauschen (Test: Array evtl. um 90° gedreht montiert)")
|
|
parser.add_argument("--no-azimuth-mirror", action="store_false", dest="azimuth_mirror", default=False,
|
|
help="Azimut-Hemisphaeren-Spiegelung AUSSCHALTEN (seit 03.10.26 ohnehin der "
|
|
"Default, Flag nur noch zur Abwaertskompatibilitaet)")
|
|
parser.add_argument("--azimuth-mirror", action="store_true", dest="azimuth_mirror", default=False,
|
|
help="Ohne Wirkung seit der Kegel-Konvention (07.10.26), Flag nur noch "
|
|
"zur Abwaertskompatibilitaet, s. Config.azimuth_mirror.")
|
|
parser.add_argument("--az-offset", type=float, default=0.0,
|
|
help="Konstanter Azimut-Offset in Grad, dreht die Nullrichtung "
|
|
"(s. Config.azimuth_offset_deg). Gegen den AoA Analyzer herleiten, "
|
|
"nicht raten.")
|
|
parser.add_argument("--el-offset", type=float, default=0.0,
|
|
help="Analog fuer Elevation (s. Config.elevation_offset_deg).")
|
|
a = parser.parse_args()
|
|
|
|
ACTIVE_CONFIG = Config(pitch_x_m=a.pitch_x_m, pitch_y_m=a.pitch_y_m, phase_multiplier=a.phase_multiplier,
|
|
flip_azimuth=a.flip_azimuth, flip_elevation=a.flip_elevation, swap_axes=a.swap_axes,
|
|
azimuth_mirror=a.azimuth_mirror, azimuth_offset_deg=a.az_offset,
|
|
elevation_offset_deg=a.el_offset)
|
|
CONFIDENCE_MIN = a.confidence_min
|
|
ACCUMULATE_PACKETS = a.accumulate
|
|
_kalibrierung = lade_kalibrierung()
|
|
if _kalibrierung is None:
|
|
print(f"Antennenkalibrierung: keine ({KALIBRIER_DATEI} nicht gefunden). "
|
|
f"Die Winkel werden dadurch systematisch falsch sein.", file=sys.stderr)
|
|
else:
|
|
print(f"Antennenkalibrierung: geladen aus {KALIBRIER_DATEI} "
|
|
f"(Quelle {_kalibrierung['quelle']}, {_kalibrierung['pakete']} Pakete, "
|
|
f"Stabilitaet {_kalibrierung['stabilitaet']:.2f}, "
|
|
f"{len(_kalibrierung['kanal'])} Kanaele einzeln)", file=sys.stderr)
|
|
switching_pattern = bytes((CP_RF_VALUE,) + RF_ORDER)
|
|
ANGLE_TABLE_FILE = a.angle_out
|
|
|
|
with open(ANGLE_TABLE_FILE, "w", encoding="utf-8") as f:
|
|
json.dump([], f, indent=2, ensure_ascii=False)
|
|
iq_file = open(a.iq_out, "a", encoding="utf-8")
|
|
print(f"IQ-Datei: {a.iq_out} Winkel-Tabelle: {ANGLE_TABLE_FILE} "
|
|
f"RF-Pattern: simple (0..15, fix) Konfidenzschwelle: {CONFIDENCE_MIN} "
|
|
f"flip_az={a.flip_azimuth} flip_el={a.flip_elevation} swap_axes={a.swap_axes}", file=sys.stderr)
|
|
|
|
try:
|
|
connector = bgapi.SerialConnector(a.port, baudrate=a.baud)
|
|
lib = bgapi.BGLib(connector, a.xapi, on_event)
|
|
lib.open()
|
|
print(f"Verbunden mit BG22 auf {a.port}", file=sys.stderr)
|
|
lib.bt.system.reboot()
|
|
time.sleep(2.0)
|
|
lib.bt.system.hello()
|
|
lib.bt.scanner.set_parameters(1, 160, 160)
|
|
lib.bt.scanner.start(1, 2)
|
|
print("Suche nach periodischem Tag...", file=sys.stderr)
|
|
while True:
|
|
try:
|
|
action, *args = pending_actions.get(timeout=0.1)
|
|
except queue.Empty:
|
|
continue
|
|
if action == "sync":
|
|
lib.bt.scanner.stop()
|
|
lib.bt.sync_scanner.open(*args)
|
|
elif action == "cte":
|
|
lib.bt.cte_receiver.enable_connectionless_cte(args[0], SLOT_DURATION_US,
|
|
CTE_COUNT, switching_pattern)
|
|
print(f"CTE-Empfang aktiv. RF-Pattern: {list(switching_pattern)}", file=sys.stderr)
|
|
elif action == "stop":
|
|
break
|
|
except KeyboardInterrupt:
|
|
print("Beende auf Benutzerwunsch...", file=sys.stderr)
|
|
finally:
|
|
global SHUTTING_DOWN
|
|
SHUTTING_DOWN = True # ab hier verwirft save_iq_report() neue Events, statt auf iq_file zu schreiben
|
|
if lib:
|
|
lib.close() # stoppt zuerst den BGAPI-Event-Thread, damit er iq_file nicht mehr anfasst
|
|
if iq_file:
|
|
iq_file.close()
|
|
return 0
|
|
|
|
|
|
if __name__ == "__main__":
|
|
sys.exit(main())
|