From adfc7e798b9cbdd022bf8df971843436912a0fe5 Mon Sep 17 00:00:00 2001
From: chen <15335560115@163.com>
Date: 星期日, 20 七月 2025 16:58:30 +0800
Subject: [PATCH] 成功移植g_com_map表逻辑，初步测试能读能写，并且TDOA效果和官方一致
---
keil/include/components/algo/inc/lib_ranging.h | 452 ++++++++++++++++++++++++++++++++++++++++----------------
1 files changed, 324 insertions(+), 128 deletions(-)
diff --git a/keil/include/components/algo/inc/lib_ranging.h b/keil/include/components/algo/inc/lib_ranging.h
index 5910425..9a30273 100644
--- a/keil/include/components/algo/inc/lib_ranging.h
+++ b/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.
--
Gitblit v1.9.3