aoa-commissionings/rtl_bridge.c

/*
 * 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);
}