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/app/src/ranging_fira.c | 264 +++++++++++++++++++++++++++-------------------------
1 files changed, 136 insertions(+), 128 deletions(-)
diff --git a/keil/include/components/app/src/ranging_fira.c b/keil/include/components/app/src/ranging_fira.c
index 0502ed5..aebe022 100644
--- a/keil/include/components/app/src/ranging_fira.c
+++ b/keil/include/components/app/src/ranging_fira.c
@@ -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.
@@ -51,7 +51,7 @@
#ifdef RADAR_EN
#include "uwb_radar.h"
#endif
-#if KF_EN
+#if FILTER_EN
#include "lib_kf.h"
#endif
#if PDOA_3D_EN
@@ -61,18 +61,19 @@
#include "board.h"
#if PDOA_3D_EN
-#define PDOA_3D_SUPPORT_NUM 100
+#define PDOA_3D_SUPPORT_NUM 10
#define PDOA_3D_TIMEOUT_MS 2000
static struct PDOA_3D_MAC_ADDR_T mac_addr_cache[PDOA_3D_SUPPORT_NUM];
static struct PDOA_3D_PDOA_DATA_T pdoa_data_cache[PDOA_3D_SUPPORT_NUM];
#endif
-#if KF_EN && FILTER_EN
+#if FILTER_EN
#define KF_SUPPORT_NUM 3
#define KF_TIMEOUT_MS 2000
static struct KF_MAC_ADDR_T kf_mac_addr_cache[KF_SUPPORT_NUM];
static struct KF_CHANNEL_CACHE_T kf_channel_cache[KF_SUPPORT_NUM];
static struct KF_MAT_VALUE_CACHE_T kf_mat_value_cache[KF_SUPPORT_NUM];
+extern void ranging_result_filter(uint8_t *mac_addr, uint16_t *distance, int16_t *azimuth, int16_t *elevation);
#endif
struct RANGING_ENV_T ranging_env;
@@ -91,6 +92,8 @@
.session_responder_num_get = ranging_responder_num_get,
.session_responder_addr_get = ranging_responder_addr_get,
.session_dynamic_update_responder_list = ranging_dynamic_update_respoder_list,
+ .session_initiator_addr_set = ranging_initiator_addr_set,
+ .session_controller_addr_set = ranging_controller_addr_set,
.session_set_ccc_ursk = NULL,
#ifdef RADAR_EN
.vendor_session_configure = uwb_radar_configure,
@@ -141,20 +144,26 @@
void ranging_configure(void)
{
+ uwb_app_config.ranging_stage = RANGING_IDLE;
+
+ uwbs_configure(PHY_TX | PHY_RX, uwb_app_config.session_param.tx_power_level);
+
fira_keys_generate();
aes_update_key(AES_ID0, &fira_key.devPayKey.ukey.keyByte[0]);
mac_update_ccm_key((uint32_t *)&fira_key.devPayKey.ukey.keyWord[0]);
- uwb_app_config.ranging_stage = RANGING_IDLE;
-
- ranging_env.uwb_period_prefetch_time = UWB_PERIOD_PREFETCH_TIME + LPM_PPM_COMPENSATION(LOW_POWER_CLOCK_PPM, uwb_app_config.session_param.ranging_interval);
ranging_env.uwb_evt_prefetch_time = UWB_EVT_PREFETCH_TIME;
ranging_env.uwb_rx_open_in_advance = UWB_RX_OPEN_IN_ADVANCE;
ranging_env.uwb_rx_window = UWB_RX_WINDOW;
- ranging_env.uwb_rx_open_in_advance_wakeup =
- UWB_RX_OPEN_IN_ADVANCE + LPM_PPM_COMPENSATION(LOW_POWER_CLOCK_PPM, uwb_app_config.session_param.ranging_interval);
- ranging_env.uwb_rx_window_wakeup = UWB_RX_WINDOW + LPM_PPM_COMPENSATION(LOW_POWER_CLOCK_PPM, uwb_app_config.session_param.ranging_interval);
+
+ uint32_t compensation = 0;
+#if LOW_POWER_EN
+ compensation += LPM_PPM_COMPENSATION(LOW_POWER_CLOCK_PPM, uwb_app_config.session_param.ranging_interval) + US_TO_PHY_TIMER_COUNT(80);
+#endif
+ ranging_env.uwb_period_prefetch_time = UWB_PERIOD_PREFETCH_TIME + compensation;
+ ranging_env.uwb_rx_open_in_advance_wakeup = UWB_RX_OPEN_IN_ADVANCE + compensation;
+ ranging_env.uwb_rx_window_wakeup = UWB_RX_WINDOW + 2 * compensation;
ranging_env.ranging_period = MS_TO_PHY_TIMER_COUNT(uwb_app_config.session_param.ranging_interval);
ranging_env.slots_per_block = (uint16_t)(uwb_app_config.session_param.ranging_interval / RSTU_TO_MS(uwb_app_config.session_param.slot_duration));
@@ -165,13 +174,19 @@
ranging_env.nround_inblock = ranging_env.slots_per_block / uwb_app_config.session_param.slots_per_round;
ranging_env.enable = 0;
- ranging_env.slot_idx = 0;
- // initial value of one-to-one case
- ranging_env.responder_slot_idx[SLOT_RESPONSE] = 2; // for response
- ranging_env.responder_slot_idx[SLOT_FINAL] = 3; // for final
- ranging_env.responder_slot_idx[SLOT_REPORT] = 4; // for report
- ranging_env.responder_slot_idx[SLOT_RESULT] = 5; // for result
ranging_env.tof = 0;
+ ranging_env.slot_idx = 0;
+
+ if ((uwb_app_config.session_param.device_type == DEV_TYPE_CONTROLLER) && (uwb_app_config.session_param.device_role == DEV_ROLE_RESPONDER))
+ {
+ uint8_t responder_idx = ranging_responder_idx_get(uwbs_local_short_addr_get());
+ uint8_t responder_num = ranging_responder_num_get();
+
+ ranging_env.responder_slot_idx[SLOT_RESPONSE] = RESPONSE_SLOT_IDX(responder_idx, responder_num); // for response
+ ranging_env.responder_slot_idx[SLOT_FINAL] = FINAL_SLOT_IDX(responder_idx, responder_num); // for final
+ ranging_env.responder_slot_idx[SLOT_REPORT] = REPORT_SLOT_IDX(responder_idx, responder_num); // for report
+ ranging_env.responder_slot_idx[SLOT_RESULT] = RESULT_SLOT_IDX(responder_idx, responder_num); // for result
+ }
ranging_env.range_data.ranging_type = 0x1; // TWR (SS-TWR, DS-TWR)
ranging_env.range_data.ranging_interval = uwb_app_config.session_param.ranging_interval;
@@ -189,14 +204,8 @@
ranging_env.phy_sts_index = fira_key.phyStsIdxInit - ranging_env.slots_per_block;
}
- uwbs_configure(PHY_TX | PHY_RX, uwb_app_config.session_param.tx_power_level);
-
-#if CSI_EN
- ranging_aux_out_opt_set(CH_LEN_DEFAULT, 3);
-#endif
-
#if (ANT_PATTERN == ANT_PATTERN_SQUARE)
- struct AOA_ANGLE_SPAN_T aoa_span;
+ angle_span_t aoa_span;
aoa_span.Ndim = 2;
aoa_span.el_low = 90;
aoa_span.el_high = 90;
@@ -208,33 +217,45 @@
#endif
#if AOA_EN
- aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM);
- aoa_steering_vector_set((const float *)((uint32_t)((uwb_app_config.ppdu_params.ch_num == 9) ? svec_ch9_ptr : svec_ch5_ptr) | SRAM_BASE));
+ sts_param_config(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM, STS_BUF_NUM, STS_BUF_SIZE);
+ aoa_steering_vector_set((const float *)((uint32_t)((uwb_app_config.ppdu_params->ch_num == 9) ? svec_ch9_ptr : svec_ch5_ptr) | SRAM_BASE));
#else
- aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI);
+ sts_param_config(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_AUX_ANT_IQ_RSSI, STS_BUF_NUM, STS_BUF_SIZE);
#endif
- aoa_param_config();
-
#if PDOA_3D_EN
- pdoa_3d_param_config(ANT_PATTERN, ANT_LAYOUT, PDOA_3D_AMBIGUITY_LEVEL_HIGH, mac_addr_cache, pdoa_data_cache, PDOA_3D_SUPPORT_NUM, PDOA_3D_TIMEOUT_MS);
+ pdoa_3d_param_config(ANT_PATTERN, ANT_LAYOUT, PDOA_3D_AMBIGUITY_LEVEL_NONE, mac_addr_cache, pdoa_data_cache, PDOA_3D_SUPPORT_NUM, PDOA_3D_TIMEOUT_MS);
// pdoa_angle_reverse_set(1, 0);
#endif
#if FILTER_EN
if (uwb_app_config.filter_en)
{
-#if KF_EN
- loc_post_kf_config(uwb_app_config.session_param.ranging_interval, kf_mac_addr_cache, kf_channel_cache, kf_mat_value_cache, KF_SUPPORT_NUM,
- KF_TIMEOUT_MS);
-#else
- loc_post_filter_config(uwb_app_config.session_param.ranging_interval, 1, uwb_app_config.session_param.aoa_result_req);
-#endif
+ uint32_t update_period_ms = uwb_app_config.session_param.ranging_interval;
+ loc_post_kf_config(update_period_ms, kf_mac_addr_cache, kf_channel_cache, kf_mat_value_cache, KF_SUPPORT_NUM, KF_TIMEOUT_MS);
}
#endif
LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_FIRA, "slot duration %d slots per round %d ranging interval %d\r\n",
uwb_app_config.session_param.slot_duration, uwb_app_config.session_param.slots_per_round, uwb_app_config.session_param.ranging_interval);
+
+#if RANGING_FOM_FILTER_EN
+ ranging_debug_csi_en_set(1);
+#endif
+
+ if (uwb_app_config.session_param.sts_config == STS_DYNAMIC)
+ {
+ mac_ifs_set(PHY_LIFS_PERIOD_MACCLK_LONG, PHY_SIFS_PERIOD_MACCLK_LONG);
+ }
+
+ LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_FIRA, "Local %x\r\n", uwbs_local_short_addr_get());
+ LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_FIRA, "Peer %x\r\n", uwbs_peer_short_addr_get());
+ LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_FIRA, "Controller %x\r\n", ranging_controller_addr_get());
+ LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_FIRA, "Initiator %x\r\n", ranging_initiator_addr_get());
+ for (uint8_t i = 0; i < ranging_responder_num_get(); i++)
+ {
+ LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_FIRA, "Responder[%d] %x\r\n", i, ranging_responder_addr_get(i));
+ }
}
void ranging_start(void)
@@ -243,13 +264,11 @@
ranging_env.lost_cnt = 0xFF;
ranging_env.anchor_point = phy_timer_count_get();
ranging_env.slot_idx = 0;
-#if FIRA_TEST_EN
ranging_env.num_of_measure = 0;
- uwb_app_config.session_param.result_report_config = (uwb_app_config.session_param.aoa_result_req) ? 0x0F : 1;
-#endif
+ uwb_app_config.session_param.result_report_config = (uwb_app_config.session_param.aoa_result_req) ? uwb_app_config.session_param.result_report_config : 1;
+
ranging_env.is_hopping = uwb_app_config.session_param.hopping_mode;
- enum DEV_ROLE_T role = uwb_app_config.session_param.device_role;
- if ((role == DEV_ROLE_INITIATOR) || (role == DEV_ROLE_GATE_CONTROLLER))
+ if (uwb_app_config.session_param.device_type == DEV_TYPE_CONTROLLER)
{
uwb_app_config.ranging_stage = RANGING_RCM;
phy_timer_target_set(ranging_env.anchor_point + ranging_env.ranging_period - ranging_env.uwb_period_prefetch_time, session_timer_callback);
@@ -262,12 +281,8 @@
ranging_env.round_offset_in_block = 0;
ranging_env.next_round_index = 0;
uwb_app_config.ranging_stage = RANGING_SYNC;
- phy_sts_pkt_cfg_set(SP0);
- ranging_update_slot_index(ranging_env.slot_idx);
- power_on_radio(0, 1);
- mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, ranging_env.ranging_period);
- mac_start();
- power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
+ phy_timer_target_set(ranging_env.anchor_point + ranging_env.ranging_period / 2, session_timer_callback);
+ power_mode_request(POWER_UNIT_APP, POWER_MODE_POWER_DOWN);
}
ranging_env.count = 0;
@@ -296,7 +311,7 @@
int8_t ranging_tx_power_get(void)
{
- return uwb_tx_power_get(uwb_app_config.ppdu_params.ch_num, uwb_app_config.session_param.tx_power_level);
+ return uwb_tx_power_get(uwb_app_config.ppdu_params->ch_num, uwb_app_config.session_param.tx_power_level);
}
void ranging_measurements_clear(void)
@@ -310,32 +325,13 @@
void ranging_monitor_start(uint32_t time_ms)
{
- WsfTimerStartMs(&ranging_cb.daemon_timer, time_ms, WSF_TIMER_PERIODIC);
+ mk_timer_list_start_timer(&ranging_cb.daemon_timer, time_ms, MK_TIMER_PERIODIC);
}
void ranging_monitor_stop(void)
{
- WsfTimerStop(&ranging_cb.daemon_timer);
+ mk_timer_list_stop_timer(&ranging_cb.daemon_timer);
}
-
-#if MCTT_TEST_EN
-extern void ranging_round_print_ind(void);
-void ranging_round_print_ind(void)
-{
- struct RANGING_ROUND_PRINT_IND_T *ind;
- if ((ind = WsfMsgAlloc(sizeof(struct RANGING_ROUND_PRINT_IND_T))) != NULL)
- {
- ind->hdr.event = RANGING_ROUND_PRINT_MSG;
-
- // Send the message
- WsfMsgSend(ranging_cb.handle_id, ind);
- }
- else
- {
- LOG_WARNING(TRACE_MODULE_UWB, "memory is not enough for RANGING_ROUND_PRINT_IND_T\r\n");
- }
-}
-#endif
void uwb_pkt_tx_done_ind(const struct MAC_HW_REPORT_T *tx, enum RANGING_STAGE_T stage, uint8_t slot_idx)
{
@@ -346,10 +342,6 @@
ind->ranging_stage = (uint8_t)stage;
ind->slot_idx = slot_idx;
ind->status = tx->err_code;
-#if MCTT_TEST_EN
- ind->timestamp = tx->timestamp;
- ind->phy_shr_duration = phy_shr_duration();
-#endif
ind->tx_len = tx->pkt_len;
if ((ind->tx_len) && (tx->pkt_data != NULL))
@@ -381,12 +373,9 @@
ind->ranging_stage = (uint8_t)stage;
ind->slot_idx = slot_idx;
ind->status = rx->err_code;
- ind->rssi = rx->rssi;
- ind->snr = rx->snr;
-
-#if MCTT_TEST_EN
- ind->timestamp = rx->timestamp;
-#endif
+ // ind->rssi = rx->rssi;
+ ind->rssi = correct_rssi(rx->rssi);
+ ind->snr = correct_snr(rx->snr);
if (rx->err_code == UWB_RX_OK)
{
@@ -403,9 +392,47 @@
ind->rx_len = rx->pkt_len;
}
-#if MCTT_TEST_EN
- ind->phy_header = rx->phy_header;
+#if RANGING_FOM_FILTER_EN
+ if (ranging_debug_csi_en_get())
+ {
+ uint8_t frame_idx = 0;
+ if (ind->ranging_stage == RANGING_RIM)
+ {
+ frame_idx = 1;
+ }
+ else if (ind->ranging_stage == RANGING_RFM)
+ {
+ frame_idx = 2;
+ }
+ else if (ind->ranging_stage == RANGING_MRM)
+ {
+ frame_idx = 3;
+ }
+ else if (ind->ranging_stage == RANGING_RRRM)
+ {
+ frame_idx = 1;
+ }
+
+ debug_csi.frame_idx = frame_idx;
+
+ uint32_t val = REG_READ(0x40003050);
+ debug_csi.frame[frame_idx].rf_gain = (val & 0x07);
+ debug_csi.frame[frame_idx].bb_gain = ((val >> 3) & 0x1f);
+ debug_csi.frame[frame_idx].bd_cnt = phy_bd_cnt_get();
+ debug_csi.frame[frame_idx].sfd_cnt = phy_sfd_cnt_get();
+ debug_csi.frame[frame_idx].error_code = ind->status;
+
+ if (rx->err_code != 0x0830)
+ {
+ debug_csi.frame[frame_idx].rssi = ind->rssi;
+ debug_csi.frame[frame_idx].snr = ind->snr;
+ debug_csi.frame[frame_idx].channel_power = REG_READ(0x40002064);
+ debug_csi.frame[frame_idx].noise_power = REG_READ(0x40002078);
+ debug_csi.frame[frame_idx].freq_offset = phy_freq_offset_get();
+ }
+ }
#endif
+
// Send the message
WsfMsgSend(ranging_cb.handle_id, ind);
return ind->status;
@@ -420,14 +447,20 @@
#pragma clang diagnostic pop
#endif
-void distance_result_filter(uint8_t *mac_addr, uint16_t *distance)
+#if FILTER_EN
+/**
+ * @brief Filter distance result.
+ *
+ * @param [in] mac_addr Pointer of device MAC address
+ * @param [inout] distance Pointer of ranging distance result
+ */
+static void distance_result_filter(uint8_t *mac_addr, uint16_t *distance)
{
if (distance == NULL)
{
return;
}
-#if KF_EN
float post_range;
float range_meas = (float)*distance / 100;
// call filter
@@ -435,17 +468,22 @@
// update distance
*distance = (uint16_t)(post_range * 100);
// LOG_INFO(TRACE_MODULE_APP, "Peer %X, $%u %u;\r\n", READ_SHORT(mac_addr), (uint16_t)(range_meas*100),(uint16_t)(post_range*100));
-#endif
}
-void angle_result_filter(uint8_t *mac_addr, int16_t *angle, uint8_t type)
+/**
+ * @brief Filter angle result.
+ *
+ * @param [in] mac_addr Pointer of device MAC address
+ * @param [inout] angle Pointer of angle result
+ * @param [in] type azimuth or elevation
+ */
+static void angle_result_filter(uint8_t *mac_addr, int16_t *angle, uint8_t type)
{
if (angle == NULL)
{
return;
}
-#if KF_EN
float post_angle;
float angle_meas = mk_q7_to_f32(*angle);
// call filter
@@ -453,12 +491,18 @@
// update angle
*angle = mk_f32_to_q7(post_angle);
// LOG_INFO(TRACE_MODULE_APP, "Peer %X, $%d %d;\r\n", READ_SHORT(mac_addr), (int16_t)angle_meas,(int16_t)post_angle);
-#endif
}
+/**
+ * @brief Filter ranging result.
+ *
+ * @param [in] mac_addr Pointer of device MAC address
+ * @param [inout] distance Pointer of ranging distance result
+ * @param [inout] azimuth Pointer of azimuth result
+ * @param [inout] elevation Pointer of elevation result
+ */
void ranging_result_filter(uint8_t *mac_addr, uint16_t *distance, int16_t *azimuth, int16_t *elevation)
{
-#if KF_EN
if (distance)
{
distance_result_filter(mac_addr, distance);
@@ -471,31 +515,13 @@
{
angle_result_filter(mac_addr, elevation, KF_DATA_TYPE_ELEVATION);
}
-#else
- // do not support multi-channel
- if ((distance == NULL) || (azimuth == NULL))
- {
- return;
- }
-
- float post_range, post_azimuth;
- int azimuth_meas = mk_q7_to_s16(*azimuth);
- float range_meas = (float)*distance;
- // call filter
- loc_post_filter(0, range_meas, azimuth_meas, &post_range, &post_azimuth);
- // update distance
- *distance = (uint16_t)(post_range);
- // update angle
- *azimuth = mk_f32_to_q7(post_azimuth);
-
- // LOG_INFO(TRACE_MODULE_APP, "$%u %u %d %d;\r\n", (uint16_t)(range_meas*100), (uint16_t)(post_range*100),(int16_t)azimuth_meas, (int16_t)post_azimuth);
-#endif
}
+#endif
int measure_report_handler(const struct UWB_PKT_RX_DONE_IND_T *ind)
{
int ret = 0;
- struct RANGING_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0];
+ struct TWR_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0];
if (uwb_app_config.session_param.ranging_round_usage == DS_TWR_DEFERRED || uwb_app_config.session_param.ranging_round_usage == DS_TWR)
{
@@ -549,7 +575,6 @@
}
else if (uwb_app_config.session_param.ranging_round_usage == SS_TWR_DEFERRED)
{
-#if FIRA_TEST_EN
volatile int64_t Tround = 0;
volatile int64_t Treply = ranging_treply(DEV_ROLE_RESPONDER, 0);
// Measurement Report Message Type 2
@@ -566,31 +591,14 @@
break;
}
}
-#else
- int64_t Tround = 0;
- int64_t Treply = ranging_treply(DEV_ROLE_RESPONDER, 0);
- // 34 --> Message Control
- // 35-36 --> Round Index
- // 37... --> Round-trip Time List
- uint8_t responder_num = (ind->rx_data[34] >> 2);
- for (uint8_t n = 0; n < responder_num; n++)
- {
- uint16_t addr = READ_SHORT(&ind->rx_data[37 + 6 * n]);
- if (addr == uwbs_local_short_addr_get())
- {
- Tround = READ_WORD(&ind->rx_data[39 + 6 * n]);
- break;
- }
- }
-#endif
// LOG_INFO(TRACE_MODULE_FIRA, "Tround: %lld, Treply: %lld\r\n", Tround, Treply);
if ((Tround) && (Treply))
{
// corrected by frequency offset
- // Treply = (int64_t)((double)Treply * (1 - (double)ranging_env.freq_offset_filter / ch_center_freq_map[uwb_app_config.ppdu_params.ch_num]));
- Tround = (int64_t)((double)Tround * (1 + (double)ranging_env.freq_offset_filter / ch_center_freq_map[uwb_app_config.ppdu_params.ch_num]));
+ // Treply = (int64_t)((double)Treply * (1 - (double)ranging_env.freq_offset_filter / uwb_ch_freq_table[uwb_app_config.ppdu_params->ch_num]));
+ Tround = (int64_t)((double)Tround * (1 + (double)ranging_env.freq_offset_filter / uwb_ch_freq_table[uwb_app_config.ppdu_params->ch_num]));
int64_t tof_i = (Tround - Treply) / 2;
@@ -711,7 +719,7 @@
uint16_t target_addr = ranging_responder_addr_get(responder_idx);
double tof_f = (double)TIMESTAMP_UNIT_TO_NS(tof);
- struct RANGING_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[responder_idx];
+ struct TWR_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[responder_idx];
range_result->mac_addr[0] = target_addr & 0xff;
range_result->mac_addr[1] = (target_addr >> 8) & 0xff;
--
Gitblit v1.9.3