aoa-commissionings/bg22_live_hybrid.py

"""Live-Messung mit dem BG22 und dem hybriden Algorithmus (aoa_music_komplett.py).

Verbindung, Tag-Suche und CTE-Empfang kommen aus bg22_AoA_v2.py (unveraendert).
Beide Dateien muessen im selben Ordner liegen.

1. Kalibrierung aufnehmen (Tag vor dem Board, ca. 60 s, Analyzer-Werte notieren):
       python bg22_live_hybrid.py --nur-aufnehmen --iq-out kalibrierung_neu.jsonl
2. Messen (ohne --kalibrierung: Test ohne Kalibrierung):
       python bg22_live_hybrid.py --kalibrierung kalibrierung_neu.jsonl \
           --kal-az <Az> --kal-el <El> --ref-az <Az Analyzer> --ref-el <El Analyzer> --iq-out test_neu_a.jsonl
"""
import argparse
import importlib.util
import json
import os
import sys
import time as _time
from datetime import datetime, timezone

import numpy as np

hier = os.path.dirname(os.path.abspath(__file__))
sys.path.insert(0, hier)
import aoa_music_komplett as ab                          # noqa: E402

# =============================================================================
# Kommandozeile
# =============================================================================

ap = argparse.ArgumentParser(add_help=False)
ap.add_argument("--kalibrierung")
ap.add_argument("--kal-az", type=float, default=0.0)
ap.add_argument("--kal-el", type=float, default=90.0)
ap.add_argument("--polar", default="VH", choices=["V", "VH"])
ap.add_argument("--abstand-mm", type=float, default=ab.ABSTAND_M * 1000)
ap.add_argument("--ref-az", type=float)
ap.add_argument("--ref-el", type=float)
ap.add_argument("--nur-aufnehmen", action="store_true")
ap.add_argument("--winkel-out", default="winkel_tabelle.json")
ap.add_argument("--iq-out", default="iq_samples.jsonl")
a, rest = ap.parse_known_args()
sys.argv = [sys.argv[0], "--iq-out", a.iq_out, "--angle-out", "winkel_tabelle_alt.json"] + rest

# =============================================================================
# Verbindung aus bg22_AoA_v2.py uebernehmen
# =============================================================================

spec = importlib.util.spec_from_file_location("bg22_AoA_v2", os.path.join(hier, "bg22_AoA_v2.py"))
bg22 = importlib.util.module_from_spec(spec)
sys.modules["bg22_AoA_v2"] = bg22
spec.loader.exec_module(bg22)

N_ANT = 16 if a.polar == "V" else 32
if a.polar == "VH":
    bg22.RF_ORDER = tuple(ab.MUSTER_V) + tuple(ab.MUSTER_H)
with open(a.iq_out + ".meta.json", "w", encoding="utf-8") as f:
    json.dump({"n_antennes": N_ANT, "polarisations": a.polar, "slot_us": bg22.SLOT_DURATION_US,
               "motif": [bg22.CP_RF_VALUE] + list(bg22.RF_ORDER)}, f, indent=1)
bg22.SELBSTKAL_AKTIV = False
bg22.KALIBRIER_DATEI = "__nicht_verwendet__.json"   # alte Kalibrierung abgeschaltet
bg22.process_packet = lambda paket, cfg: None       # alte MUSIC-Berechnung abgeschaltet
open(a.iq_out, "w", encoding="utf-8").close()       # IQ-Datei bei jedem Start leeren

# =============================================================================
# Schaetzer vorbereiten
# =============================================================================

schaetzer = None
if not a.nur_aufnehmen:
    array = ab.brd4191a(a.polar, a.abstand_mm / 1000)
    if a.kalibrierung:
        if not os.path.exists(a.kalibrierung):
            sys.exit(f"Kalibrierdatei nicht gefunden: {a.kalibrierung}")
        kal = ab.Kalibrierung.aus_aufnahme(array, ab.lese_iq_datei(a.kalibrierung, N_ANT), a.kal_az, a.kal_el)
    else:
        kal = ab.Kalibrierung(array.n)              # ohne Kalibrierung: alle Faktoren = 1
        print("bg22_live: OHNE Kalibrierung (nur zum Test)", file=sys.stderr)
    einst = ab.Einstellungen(polarisationen=(0,) if a.polar == "V" else (0, 1))
    schaetzer = ab.MusicSchaetzer(array, kal, einst)
    print(f"bg22_live: Hybrid (Beamformer + MUSIC), {N_ANT} Antennen, {a.abstand_mm:.1f} mm, "
          f"Kalibrierung {a.kalibrierung or 'keine'} ({len(kal.je_kanal)} Kanaele)", file=sys.stderr)
else:
    print(f"bg22_live: nur Aufnahme ({N_ANT} Antennen) nach {a.iq_out}", file=sys.stderr)

tabelle = []

# =============================================================================
# Verarbeitung je Paket
# =============================================================================

def paket_verarbeiten(i_werte, q_werte, kanal, t):
    if schaetzer is None:
        return
    p = ab.paket_aus_iq(i_werte, q_werte, kanal, t, N_ANT, float(bg22.SLOT_DURATION_US))
    s = schaetzer.verarbeiten(p) if p is not None else None
    if s is None or not np.isfinite(s.az_grad):
        return
    vergleich = ""
    if a.ref_az is not None and a.ref_el is not None:
        d = ab.winkelabstand(ab.richtung(s.az_grad * ab.GRAD, s.el_grad * ab.GRAD),
                             ab.richtung(a.ref_az * ab.GRAD, a.ref_el * ab.GRAD)) / ab.GRAD
        vergleich = f"  | Abweichung {d:4.1f}"
    print(f">>> Az {s.az_grad:+7.1f} +-{s.unsicherheit_az:4.1f}  El {s.el_grad:5.1f} +-{s.unsicherheit_el:4.1f}  "
          f"Konfidenz {s.konfidenz:.2f}{vergleich}  {', '.join(s.gruende)}", file=sys.stderr, flush=True)
    tabelle.append({"zeit": datetime.fromtimestamp(t, timezone.utc).isoformat(),
                    "azimut": round(s.az_grad, 2), "elevation": round(s.el_grad, 2),
                    "unsicherheit_az": round(s.unsicherheit_az, 2), "unsicherheit_el": round(s.unsicherheit_el, 2),
                    "konfidenz": round(s.konfidenz, 3), "gruende": s.gruende})
    if len(tabelle) % 5 == 0:
        speichern()


def speichern():
    with open(a.winkel_out, "w", encoding="utf-8") as f:
        json.dump(tabelle, f, indent=1, ensure_ascii=False)


_original_speichern = bg22.save_iq_report


def save_iq_report(event):
    _original_speichern(event)                      # IQ-Datei wie bisher schreiben
    try:
        i_werte, q_werte = bg22.signed_iq(event.samples)
        paket_verarbeiten(i_werte, q_werte, int(event.channel), _time.time())
    except Exception as fehler:                     # Rechenfehler darf die Messung nicht abbrechen
        print(f"bg22_live: Paket uebersprungen ({fehler})", file=sys.stderr)


bg22.save_iq_report = save_iq_report

# =============================================================================
# Zusammenfassung beim Beenden (Strg+C)
# =============================================================================

def zusammenfassung():
    T = tabelle[len(tabelle) // 10:]                # Einschwingphase weglassen
    if not T:
        return
    S = [ab.Schaetzung(0, t["azimut"], t["elevation"], 0, 0, t["konfidenz"], 0, 0, 0, 0) for t in T]
    r = ab.ergebnis(S, a.ref_az, a.ref_el)
    txt = f"\n===== ERGEBNIS: Azimut {r['az']:+.1f}, Elevation {r['el']:.1f}, Konfidenz {r['konfidenz']:.2f}"
    if "abweichung" in r:
        txt += (f"\n===== ANALYZER: Azimut {a.ref_az:+.1f}, Elevation {a.ref_el:.1f} -> "
                f"ABWEICHUNG {r['abweichung']:.1f} Grad (Az {r['abw_az']:+.1f}, El {r['abw_el']:+.1f})")
    print(txt + "\n", file=sys.stderr, flush=True)

# =============================================================================
# Start (5 s Bootzeit, bis zu 3 Versuche)
# =============================================================================

class _LaengereBootzeit:
    def __getattr__(self, name):
        return getattr(_time, name)

    def sleep(self, sekunden):
        _time.sleep(5.0 if sekunden == 2.0 else sekunden)


bg22.time = _LaengereBootzeit()

if __name__ == "__main__":
    try:
        from bgapi.bglib import CommandError
    except ImportError:
        CommandError = Exception
    try:
        for versuch in range(1, 4):
            try:
                bg22.main()
                break
            except CommandError as fehler:
                print(f"bg22_live: Versuch {versuch}/3, BG22 antwortet nicht ({fehler}).", file=sys.stderr)
                if versuch == 3:
                    sys.exit("Abbruch: AoA Analyzer trennen, Reset am BG22, neu starten.")
                bg22.SHUTTING_DOWN = False
                _time.sleep(3.0)
    finally:
        speichern()
        print(f"bg22_live: {len(tabelle)} Schaetzungen in {a.winkel_out}", file=sys.stderr)
        zusammenfassung()