0.6.8移植完成版本，初步调好两边sts配置，但只能接受一包，基站因为中断中多加了些sts校验计算的语句导致耗时tx时间卡在过去问题未解决

chen

2025-07-04 147b7f967cb84086a53c8bee7d4acf81c49a2bb1

 keil/include/components/algo/inc/lib_ranging.h

@@ -1,5 +1,5 @@
/*
 * Copyright (c) 2019-2023 Beijing Hanwei Innovation Technology Ltd. Co. and
 * Copyright (c) 2019-2025 Beijing Hanwei Innovation Technology Ltd. Co. and
 * its subsidiaries and affiliates (collectly called MKSEMI).
 *
 * All rights reserved.
@@ -40,8 +40,7 @@

#ifndef RANGING_LIB_H
#define RANGING_LIB_H
#include "mk_mac.h"
#include "mk_phy.h"
#include "mk_uwb.h"

/**
 * @addtogroup MK8000_ALGO_Ranging
@@ -52,15 +51,41 @@
#define CHEST_DUMP_EN (1)
#endif

/* When debugging offline channel estimation, enable this macro */
#ifndef OFFLINE_CHEST_EN
#define OFFLINE_CHEST_EN (0)
#ifndef CHEST_DUMP_STS_EN
#define CHEST_DUMP_STS_EN (1)
#endif

// total channel tap length
// total channel tap number
#define MLAGS_LENGTH 160
// output channel tap length
#define CH_LEN_DEFAULT (128)
// output channel tap number
#define CE_LEN (128)

// Maximum STS segment number
#define MAX_STS_SEG_NUM (1)
// Maximum STS segment length
#define MAX_STS_SEG_LEN (64)

// STS buffer number
#define STS_BUF_NUM (1)

/* for 4 symbol based switching, max size is 2*11*256, buffer size = (11*2*Nsegs*seg_len/4)*4 Byte */
/* for segment based switching, max size is 4*11*256 = 11KB (never switch mode use the same formula to calculate buffer size) */
/* if set 11k, it has memory issue when run time */
/* we may not use segment based switching */
#define STS_BUF_SIZE (11 * MAX_STS_SEG_NUM * MAX_STS_SEG_LEN + 2)

struct STS_INF_T
{
    float sts_short_ce[11];
    float sts_IQ[22];
    float sts_lsp_result[STS_BUF_SIZE];
};

struct RANGING_INF_T
{
    uint32_t CE_CIR[64];
    struct MAC_HW_REPORT_T RPT;
};

/** Ranging channel status information */
struct RANGING_CSI_T
@@ -74,11 +99,13 @@
    int16_t azimuth;
    uint8_t ranging_fom;
    uint8_t azimuth_fom;
    uint8_t antenna;
    uint8_t frame_idx;
    uint8_t rframe_idx;
    uint16_t reserved;
    uint8_t reserved[2];

    // pre-poll | poll | final | final-data
    // poll | final
    // RCM | RIM | RFM | MRM
    struct FRAME_INF_T
    {
        int8_t rssi;
@@ -87,71 +114,176 @@
        uint8_t bb_gain;
        uint16_t bd_cnt;
        uint16_t sfd_cnt;
        uint16_t error_code;
    } frame[4];

    // poll | final
    struct RFRAME_INF_T
    {
        int32_t freq_offset;
        float kfactor;
        uint32_t channel_power;
        uint32_t noise_power;
        int8_t main_tap_power;
        int8_t first_tap_power;
        int16_t main_tap_power;
        int16_t first_tap_power;
        int16_t fap_delta;
        uint8_t main_tap;
        uint8_t first_tap;
        int16_t sts_fap_delta;
        uint8_t sts_main_tap;
        uint8_t sts_first_tap;
        uint16_t error_code;
        uint8_t nlos;
        uint8_t fom;
        int8_t cir[128][2];
        float sts_taps[11];
    } rframe[2];
        float sts_taps[11][2];
    } frame[4];
};

typedef struct
{
    int16_t tap1_loc;
    int16_t tap2_loc;
    int16_t tap3_loc;
    float tap1_re;
    float tap1_im;
    float tap2_re;
    float tap2_im;
    float tap3_re;
    float tap3_im;
    int16_t fap_loc;
    float fap_pow;
} ranging_aux_t;
    uint8_t en_rfg_adj;      ///> Enable RF gain adjust logic. Default value: 1
    uint8_t en_hpm;          ///> Enable high performance mode. Default value: 0
    int16_t sfd_offset_db;   ///> Offset to be used when SFD metric is used. Default value: 132.0
    int16_t agc_offset_db;   ///> Offset to be used when AGC gains are used. Default value: 32.0
    int16_t snr_offset_init; ///> Initial offset for SNR calculation. Default value: 10.0
} comp_rssi_t;

typedef struct
{
    float Kfactor;          ///> ratio of the main tap to the total energy
    int16_t loc_deviation;  ///> Deviation in main tap location from the expected one
    float fom1;             ///> Mean excess delay spread
    float fom2;             ///> RMS delay spread
    float fom3;             ///> Channel type: 0/1 : LoS, 2: Multipath, 3: NLos
    ranging_aux_t aux_data; ///> First path location and energy
    float mean_npwr;        ///> Mean noise power (in offline CE mode) -- linear scale
    float max_npwr;         ///> Max noise power (in offline CE mode) -- linear scale
    float chpwr;            ///> Total channel power (in offline CE mode) -- linear scale
} ranging_FoM_t;
    int32_t ce_delta;
    uint16_t ce_map_loc;
    uint16_t ce_fap_loc;
    int32_t sts_delta;
    uint16_t sts_map_loc;
    uint16_t sts_fap_loc;
    uint8_t ce_fom;
    uint8_t ce_nlos;
    uint8_t sts_fom;
    uint8_t sts_nlos;

struct RANGING_TAPS_INF_T
    uint32_t ce_map_pwr;
    uint32_t ce_fap_pwr;
    uint32_t ce_ch_pwr;
    uint32_t ce_mean_npwr;
    uint32_t ce_max_npwr;

    uint32_t sts_map_pwr;
    uint32_t sts_fap_pwr;
    uint32_t sts_ch_pwr;
    uint32_t sts_mean_npwr;
    uint32_t sts_max_npwr;

    uint8_t use_ce_sts;
    uint8_t sts_valid;
    uint8_t fap_fom;
    uint8_t angle_valid;

    int8_t rssi;
    int8_t snr;

    float all_ant_sts_taps[4 * 11];
    float per_ant_sts_pwr[4];

    float sts_rssi[4];
    float sts_fap_iq[8];

    float theta_est;
    float phi_est;
    float Kfactor;

} ranging_result_t;

typedef struct
{
    int16_t fap_loc;
    int16_t tap1_loc;
    int16_t tap2_loc;
    int16_t tap3_loc;
    float fap_pow;
    float tap1_pow;
    float tap2_pow;
    float tap3_pow;
    float Kfactor;         // ratio of the main tap to the total energy
    int16_t loc_deviation; // Deviation in main tap location from the expected one
    uint8_t NLoS;          // 0/1: LoS, 2: Multipath, 3: NLoS
    uint8_t FoM;           // 0 ~ 100
    uint8_t local_search_win;      ///> Local search window (configured register value). Default: 2
    uint8_t en_sts_fap_det;        ///> Flag to enable first path detection: 1: Enabled, 0: Disabled
    uint8_t init_detect_opt;       ///> FAP detect option. 0: Original, 1: Enhanced. Default: 1
    uint8_t THR_SF1;               ///> !!! scale factor 1 for threshold. Default: 8 (Internal test purpose only, not accessible to user)
    uint8_t THR_SF2;               ///> !!! scale factor 2 for threshold. Default: 16 (Internal test purpose only, not accessible to user)
    uint8_t ant_switching_enabled; ///> 1: Antenna switching is enabled, 0: Antenna switching is not enabled (single antenna mode)
    uint8_t num_of_antennas;       ///> Number of antenna: tied to the mk_phy parameter. Range 1 to 4
    uint8_t ant_offset;            ///> Antenna offset indicating the main antenna. Tied to the mk_phy parameter. Range 0 to 3
    uint8_t en_skip_ant_ports;     ///> 0: Disabled (Default), 1: Enabled. When 1, IQ samples from skip_port_idx are not used in computation
    uint8_t skip_port_idx; ///> 0 to 3. Only used when en_skp_ant_ports = 1. The port index (relative to main port) whose data is not used. Main port is always
                           /// at index 0
    uint8_t en_dynamic_port_sel; ///> 0: Disabled (Default), 1: Enabled. When 1, SNR metric for all ports are generated and ports will small SNR (based on
                                 /// thres_skip_port) are skipped
    uint8_t opt_sts_ce;          ///> Option for STS CE. 0 (default) - Use sts_short_ce from STS valid module, 1 - Regenerate CE internally using sts store data
                                 ///(original method)
    float total_energy_thres;    ///> total energy threshold. Default: 5.0e-5
    float par_th;                ///> peak to average threshold. default: 6.0
    float preecho_thres_lin;     ///> Pre-echo threshold linear (take configured register value and generate linear value). Range 0.1 to 0.001. Default 0.01
    float ENER_THR1;             ///> !!! Energy threshold 1, Usually around 0.05 or 0.06. Default: 0.05 (Internal test purpose only, not accessible to user)
    float ENER_THR2;             ///> !!! Energy threshold 2, Usually around 0.01 or 0.02. Default: 0.01 (Internal test purpose only, not accessible to user)
    float thres_skip_port;       ///> Default: 0.1. When dynamic port selection is enabled then this threshold is used to select good ports

} sts_fap_detect_t;

typedef struct
{
    uint8_t ch_number;             // RF channel number
    uint8_t tx_sts_config;         // STS packet configurator ( Valid values 0 to 3) - only applicable in BPRF and HPRF modes.
                                   // Refer to Table 41 of 15.4z standard - specifies where STS is placed
    uint8_t tx_sts_seg_num;        // STS segment number (Valid values 0 to 3). BPRF: 1 segment (parameter value = 0). HPRF:
                                   // 1, 2, 3, or 4 segments
    uint8_t tx_sts_seg_len;        // STS segment length (in units of 512 chips). Valid values 0 to 3. BPRF: 64 (parameter
                                   // value = 1). HPRF: 0 (32), 1 (64), 2 (128), or 3 (256)
    uint8_t tx_en_AoD_mode;        // 1: Enable AoD mode (assumes that the TX has multiple antennas)
    uint8_t antenna_array_size;    // Number of antenna elements
    uint8_t xdim_Nrx_ant;          // Number of antennas along x-axis
    uint8_t ant_sw_period;         // switching period in symbols
    uint8_t rx_syms_avail;         // Number of symbols (512 chips) available on each dwell
    uint8_t alg_option;            // 1 - classical beamforming, 2 - Capon, 3 - Music
    uint8_t gen_steering_vec_file; // Generate steering vector file; 0 - Use pre-generated steering vector file, 1 -
                                   // Re-generate steering vector file
    uint8_t en_calib;              // Enable calibration option - Only used during steering vector generation
    uint8_t module_opt;            // Module Option - 0: ISA based, 1: Matrix based
    int8_t tau1;                   // FE_tp - Default: -12
    int8_t tau2;                   // FE_ti - Default: -10
    int8_t tau3;                   // FE_Fp - Default: -11 (when FE opt = 0)
                                   //         Default: -8 (When FE opt = 1)
    int8_t tau4;                   // FE_Fi - Default: -3 (when FE opt = 0)
                                   //         Default: -11 (when FE opt = 1)
    uint8_t fe_opt_Fp;             // FE_opt_Fp - 0 or 1
    uint8_t cal_mode;              // cal_mode
    uint8_t ant_offset;            // antenna offset to indicate the first antenna in STS field
    uint8_t ant_board_pos;         // antenna board position
    uint8_t sym_cnt_mode;          // 0: auto (based on STS parameters), 1: Custom
    uint8_t aoa_sym_cnt;           // Used only when symbol count mode = 1 (i.e. custom). The number of AoA symbols should be less than the total STS symbols

    uint16_t sts_inlen;
    sts_fap_detect_t fap_params;

} sts_params_t;

// Structure holding the angle information for steering vector generation
typedef struct
{
    uint8_t Ndim;    ///> Number of dimensions. 1 - Azimuth only, 2 - Azimuth and Elevation
    int16_t az_low;  ///> Lower end of azimuth scan range (in degrees)
    int16_t az_high; ///> Higher end of azimuth scan range (in degrees)
    uint8_t az_step; ///> Azimuth step size in degrees. Normally 3
    int16_t el_low;  ///> Lower end of elevation scan range (in degrees)
    int16_t el_high; ///> Higher end of elevation scan range (in degrees)
    uint8_t el_step; ///> Elevation step size in degrees. Normally 3
} angle_span_t;

// (aux_mode << 4) | (alg_option)
enum STS_AUX_OPT_T
{
    // 135us
    STS_AUX_ANT_IQ_RSSI = ((1 << 4) | 3),
    // 239us
    STS_AUX_ANT_IQ_RSSI_PDOA = ((1 << 4) | 0),
    // 312us
    STS_AUX_ANT_IQ_RSSI_PDOA_AOA = ((1 << 4) | 2),
    // 930us
    STS_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM = ((2 << 4) | 2),
};

// Ranging auxiliary information option
enum RANGING_AUX_OPT_T
{
    // 10us
    CE_AUX_CH_PWR_NLOS = 1,
    // 140us
    CE_AUX_FOM = 2,
    // 150us
    CE_AUX_CH_PWR_NLOS_FOM = 3,
};

#ifdef __cplusplus
@@ -166,20 +298,41 @@
#define TAP_MARGIN 4

extern struct RANGING_CSI_T debug_csi;
uint8_t first_path_align(uint8_t *ce_chest_gaps, uint8_t *ce_chest_gaps_num, int8_t ce_chest[CIR_LEN][2], uint8_t ce_fap, uint8_t th, uint8_t margin);
uint8_t ranging_fom_calculate(struct RANGING_CSI_T *csi, uint8_t response_fom, uint8_t *response_tap_gaps, uint8_t response_tap_gaps_num);
uint8_t ranging_fom_calculate(struct RANGING_CSI_T *csi, uint8_t frame_start, uint8_t response_fom, uint8_t *response_tap_gaps, uint8_t response_tap_gaps_num);
void dump_preamble_cir(uint8_t idx, uint8_t taps_num);
void dump_sts_cir(uint8_t idx);
void print_preamble_chest(uint8_t rx_pkt_num, uint8_t rframe_num);
void print_sts_ch_taps(uint8_t rframe_num);
void calculate_first_tap_power(uint8_t rx_pkt_num, uint8_t rframe_num);
void print_preamble_cir(uint32_t seq_num, uint8_t frame_start, uint8_t frame_num);
int8_t correct_rssi(int8_t rssi);
int8_t correct_snr(int8_t snr);
void calculate_first_tap_power(uint8_t frame_start, uint8_t frame_num);
int8_t calculate_noise_floor(uint8_t rf_gain, uint8_t bb_gain);
uint32_t auto_adjust_filter_coeff(uint8_t idx);

#if CHEST_DUMP_STS_EN
void dump_sts_cir(uint8_t idx);
void print_sts_cir(uint8_t frame_start, uint8_t frame_num);
#endif
#endif

extern int16_t fap_ones_zeros[256] __attribute__((aligned(4)));

extern ranging_result_t g_ranging_result;

extern sts_params_t g_sts_params;

extern struct STS_INF_T *g_sts_cir_buff;

extern struct STS_INF_T g_sts_inf[STS_BUF_NUM];

/**
 * @brief Initialize all the global variables that will be used in the ranging lib.
 *
 * @param[in] rframe    Ranging frame type, SP0 ~ SP3
 * @param [in] opt      Options for auxiliary information
 *    0 (000)    -    No auxiliary data is generated
 *    1 (001)    -    (ch pwr, fap/map pwr, Kfactor, LoS/NLoS)
 *    2 (002)    -    (FoM)
 */
void ranging_lib_init(void);
void ranging_lib_init(uint8_t rframe, enum RANGING_AUX_OPT_T opt);

/**
 * @brief Enable or disable ranging debug CSI output
@@ -194,23 +347,20 @@
uint8_t ranging_debug_csi_en_get(void);

/**
 * @brief Set ranging frame type.
 * @param[in] type    ranging frame type, SP0 ~ SP3
 */
void ranging_frame_type_set(uint8_t type);

/**
 * @brief Get ranging frame type.
 * @return ranging frame type SP0 ~ SP3
 */
uint8_t ranging_frame_type_get(void);

/**
 * @brief Detect the fisrt path of ranging frame.
 * @param[in] rssi    Rx packet RSSI
 * @param[in] rssi      Rx packet RSSI
 * @param[in] snr       Rx packet SNR
 * @return delta of the first path
 */
int32_t ranging_first_path_detect(int8_t rssi);
int32_t ranging_first_path_detect(int8_t rssi, int8_t snr);

/**
 * @brief Calculate timestamp difference.
 * @param[in] end           End timestamp
 * @param[in] begin         Begin timestamp
 * @return end - begin
 */
int64_t ranging_timestamp_diff(int64_t end, int64_t begin);

/**
 * @brief Calculate TX timestamp of the ranging frame.
@@ -229,6 +379,22 @@
int64_t ranging_rx_time(const struct MAC_HW_REPORT_T *ind);

/**
 * @brief Calculate TX timestamp of the ranging frame.
 *
 * @param[in] timestamp    PHY timer count of TX
 * @return TX timestamp (unit: 2ns)
 */
int64_t ranging_tx_time_in_2ns(uint32_t timestamp);

/**
 * @brief Calculate RX timestamp of the ranging frame.
 *
 * @param[in] ind    MAC RX report
 * @return RX timestamp (unit: 2ns)
 */
int64_t ranging_rx_time_in_2ns(const struct MAC_HW_REPORT_T *ind);

/**
 * @brief Get ranging FAP FoM.
 *
 * @param[out] NLoS     Non-Line of sight flag, 0/1: LoS, 2: Multipath, 3: NLoS
@@ -237,37 +403,12 @@
void ranging_fom_get(uint8_t *NLoS, uint8_t *FoM);

/**
 * @brief Get multi-taps information.
 *
 * @param[out] inf    multi-taps information
 */
void ranging_taps_inf_get(struct RANGING_TAPS_INF_T *inf);

/**
 * @brief Get multi-taps I/Q result.
 *
 * @param[out] chtaps_re pointer of output buffer of multi-taps real part
 * @param[out] chtaps_im pointer of output buffer of multi-taps imagine part
 * @param[in] taps_num number of taps to be get, the maximum value is 128
 */
void ranging_multi_taps_iq_get(float *chtaps_re, float *chtaps_im, uint8_t taps_num);

#if OFFLINE_CHEST_EN
/**
 * @brief Enable offline channel estimate.
 *
 * @param[in] en enable or disable
 */
void ranging_offline_chest_enable(uint8_t en);
#endif

/**
 * @brief Set antenna delay for ranging.
 *
 * @param[in] ant_idx       antenna port index, from 0 to 3
 * @param[in] delay_ps      antenna delay, unit 15.6ps
 * @param[in] delay_rstu    antenna delay, unit 15.65ps
 */
void ranging_ant_delays_set(uint8_t ant_idx, int16_t delay_ps);
void ranging_ant_delays_set(uint8_t ant_idx, int16_t delay_rstu);

/**
 * @brief Get antenna delay for ranging.
@@ -278,22 +419,6 @@
int16_t ranging_ant_delays_get(uint8_t ant_idx);

/**
 * @brief Select aux information output
 *
 * @param [in] len       Length of samples to be processed (64, 32)
 * @param [in] opt       Options for aux information
 *    0 (000)    -    No auxiliary data is generated
 *    1 (001)    -    combo 1 (Kfactor, location deviation, LoS/NLoS)
 *    2 (010)    -    combo 2 (3 largest taps info)
 *    4 (100)    -    combo 3 (Mean excess delay, RMS excess delay)
 *    3 (011)    -    combo 1 and combo 2
 *    5 (101)    -    combo 1 and combo 3
 *    7 (111)    -    combo 1, 2 and 3
 *
 */
void ranging_aux_out_opt_set(uint8_t len, uint8_t opt);

/**
 * @brief Get UWB RX RSSI.
 *
 * @param[out] rssi    RSSI (-110 ~ -10) dBm
@@ -302,14 +427,85 @@
void ranging_rssi_get(int8_t *rssi, int8_t *snr);

/**
 * @brief Compute expected RSSI based on TX power and ranging distance result
 *
 * @param[in] uwb_tx_power      Ranging UWB TX power in dBm
 * @param[in] distance          Ranging distance result in cm
 * @param[in] path_loss_exp     Ranges from 2 to 5 (including fractional values, simplified here) 2 - for outdoor, 3 or 4 for multipath channels
 * @param[in] ant_gain_loss     Antenna gain or loss in dB
 * @brief Configure STS parameters.
 * @param[in] rframe            Ranging frame type, SP0 ~ SP3
 * @param [in] opt              STS auxiliary option @ref enum STS_AUX_OPT_T
 * @param [in] sts_buff_num     STS buffer number @ref STS_BUF_NUM
 * @param [in] sts_buff_len     STS buffer length @ref STS_BUF_SIZE
 * @return Size of samples that need LSP to process
 */
int8_t ranging_expected_rssi_get(int8_t uwb_tx_power, uint16_t distance, uint8_t path_loss_exp, int8_t ant_gain_loss);
uint16_t sts_param_config(uint8_t rframe, enum STS_AUX_OPT_T opt, uint8_t sts_buff_num, uint16_t sts_buff_len);

/**
 * @brief Update STS parameters.
 * @param [in] main_ant         RX main antenna
 */
void sts_param_update(uint8_t main_ant);

/**
 * @brief Enable or disable dynamic port selection
 * @param [in] enable           0: disable, 1: enable
 */
void sts_dynamic_port_sel(uint8_t enable);

/**
 * @brief Store LSP result of RX ranging frame.
 */
void sts_lsp_store(void);

/**
 * @brief Stop storing LSP result of RX ranging frame.
 */
void sts_lsp_store_stop(void);

/**
 * @brief Validate STS.
 * @return 1 represents STS is valid
 */
uint8_t sts_valid_check(void);

/**
 * @brief Detect the fisrt path of ranging frame based on STS.
 * @param [in] rssi                 RSSI
 * @param [in] snr                  SNR
 * @return delta of the first path
 */
int32_t sts_first_path_detect(int8_t rssi, int8_t snr);

/**
 * @brief Calculate RX main antenna based on STS RSSI.
 * @param [in][out]                 Input current main antenna ID, ouput updated main antenna ID
 */
void sts_rx_main_ant_get(uint8_t *id);

/**
 * @brief Get 4 antenna port RSSI.
 * @return array of 4 antenna port RSSI
 */
float *sts_4ant_rssi_get(void);

/**
 * @brief Get RSSI result.
 *
 * @return RSSI
 */
float *sts_rssi_output_get(void);

/**
 * @brief Get STS first path IQ of each antenna port, needs to call AoA calculation or PDoA calculation in advanced.
 *
 * @return STS first path IQ
 */
float *sts_first_path_iq_get(void);

/**
 * @brief Function to compute 10*log10 of a number
 * This function takes in 32-bit integer and outputs 10*log10 in floating point
 *
 * @param[in] num              Input number
 * @return the result of 10*log10(num)
 */
float fast_10log10(uint32_t num);

/**
 * @brief Get ranging library version.