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_custom.c | 178 +++++++++++++++++++++++------------------------------------
1 files changed, 69 insertions(+), 109 deletions(-)
diff --git a/keil/include/components/app/src/ranging_custom.c b/keil/include/components/app/src/ranging_custom.c
index bd4a933..19198f3 100644
--- a/keil/include/components/app/src/ranging_custom.c
+++ b/keil/include/components/app/src/ranging_custom.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.
@@ -48,7 +48,7 @@
#include "ranging_custom.h"
#include "lib_aoa.h"
#include "libc_rom.h"
-#if KF_EN
+#if FILTER_EN
#include "lib_kf.h"
#endif
#if PDOA_3D_EN
@@ -58,13 +58,13 @@
#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 6
#define KF_TIMEOUT_MS 2000
static struct KF_MAC_ADDR_T kf_mac_addr_cache[KF_SUPPORT_NUM];
@@ -161,12 +161,11 @@
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
+ // Reduce inter frame spacing (IFS) for non encrypted packets
+ mac_ifs_set(0, 0);
#if (ANT_PATTERN == ANT_PATTERN_SQUARE)
- struct AOA_ANGLE_SPAN_T aoa_span;
+ angle_span_t aoa_span;
#if AOA_3D_EN
aoa_span.Ndim = 2;
aoa_span.el_low = 0;
@@ -188,28 +187,22 @@
#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
@@ -228,6 +221,7 @@
if (dev_role == DEV_ROLE_INITIATOR)
{
uwb_app_config.ranging_stage = RANGING_POLL;
+ ranging_env.synced = 1;
}
else
{
@@ -239,14 +233,14 @@
ranging_env.count_last = 0;
ranging_env.lost_cnt = 0;
- WsfTimerStartMs(&ranging_cb.daemon_timer, uwb_app_config.session_param.ranging_interval * 10, WSF_TIMER_PERIODIC);
+ mk_timer_list_start_timer(&ranging_cb.daemon_timer, uwb_app_config.session_param.ranging_interval * 10, MK_TIMER_PERIODIC);
LOG_INFO(TRACE_MODULE_APP, "Ranging start, role %d\r\n", dev_role);
}
void ranging_stop(void)
{
ranging_env.enable = 0;
- WsfTimerStop(&ranging_cb.daemon_timer);
+ mk_timer_list_stop_timer(&ranging_cb.daemon_timer);
}
void ranging_restart(void)
@@ -259,7 +253,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_local_addr_set(uint16_t short_addr)
@@ -279,7 +273,7 @@
LOG_INFO(TRACE_MODULE_APP, "Responder %d %04x\r\n", ranging_env.responder_num, addr);
ranging_env.responder_list[ranging_env.responder_num] = addr;
ranging_env.responder_num++;
- ranging_env.main_ant_id = uwb_app_config.ppdu_params.rx_ant_id;
+ ranging_env.main_ant_id = uwb_app_config.ppdu_params->rx_main_ant;
return 1;
}
return 0;
@@ -360,19 +354,6 @@
return (user_response_data_len == data_len) ? 0 : 1;
}
-// ts_a - ts_b
-int64_t ranging_timestamp_diff(int64_t ts_a, int64_t ts_b)
-{
- if (ts_a < ts_b)
- {
- return (0x20000000000 - ts_b + ts_a);
- }
- else
- {
- return (ts_a - ts_b);
- }
-}
-
// unit: 15.6ps
int64_t ranging_tround(enum DEV_ROLE_T role, uint8_t dev_idx)
{
@@ -392,7 +373,7 @@
int64_t tround = ranging_timestamp_diff(rx_timestamp, tx_timestamp);
// correct antenna delay
- tround -= ranging_ant_delays_get(uwb_app_config.ppdu_params.rx_ant_id);
+ tround -= ranging_ant_delays_get(uwb_app_config.ppdu_params->rx_main_ant);
return tround;
}
@@ -416,7 +397,7 @@
int64_t treply = ranging_timestamp_diff(tx_timestamp, rx_timestamp);
// correct antenna delay
- treply += ranging_ant_delays_get(uwb_app_config.ppdu_params.rx_ant_id);
+ treply += ranging_ant_delays_get(uwb_app_config.ppdu_params->rx_main_ant);
return treply;
}
@@ -497,7 +478,7 @@
ranging_user_pkt.msg.result_msg[0] = responder_id & 0xff;
ranging_user_pkt.msg.result_msg[1] = (responder_id >> 8) & 0xff;
- struct RANGING_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0];
+ struct TWR_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0];
ranging_user_pkt.msg.result_msg[2] = range_result->distance & 0xff;
ranging_user_pkt.msg.result_msg[3] = (range_result->distance >> 8) & 0xff;
@@ -551,7 +532,7 @@
static void ranging_timer_callback(void *dev, uint32_t time)
{
- // board_led_on(BOARD_LED_1);
+ // board_led_on(BOARD_LED_2);
if (ranging_env.enable)
{
enum RANGING_STAGE_T stage = uwb_app_config.ranging_stage;
@@ -578,15 +559,11 @@
if (uwb_app_config.ranging_flow_mode == (uint8_t)RANGING_FLOW_CONTENTION)
{
- power_on_radio(1, 1);
- mac_tx(EVT_MODE_MAC_FIX_PHY_ASAP, target_time, 0, (uint8_t *)&ranging_user_pkt,
- user_poll_data_len + MSG_HEADER_LEN + MSG_POLL_USER_DATA_IDX);
+ uwb_tx((uint8_t *)&ranging_user_pkt, user_poll_data_len + MSG_HEADER_LEN + MSG_POLL_USER_DATA_IDX, TX_MODE_CSMACA, target_time);
}
else
{
- power_on_radio(1, 0);
- mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, 0, (uint8_t *)&ranging_user_pkt,
- user_poll_data_len + MSG_HEADER_LEN + MSG_POLL_USER_DATA_IDX);
+ uwb_tx((uint8_t *)&ranging_user_pkt, user_poll_data_len + MSG_HEADER_LEN + MSG_POLL_USER_DATA_IDX, TX_MODE_DEFER, target_time);
}
power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
@@ -597,11 +574,9 @@
{
ranging_env.slot_idx++;
// receive response
- power_on_radio(0, 1);
- sts_lsp_store();
target_time = ranging_env.anchor_point + ranging_env.slot_idx * ranging_env.slot_interval;
- mac_rx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time - UWB_RX_OPEN_IN_ADVANCE, UWB_RX_WINDOW);
+ uwb_rx(RX_MODE_DEFER, target_time - UWB_RX_OPEN_IN_ADVANCE, UWB_RX_WINDOW);
}
break;
@@ -614,8 +589,7 @@
// send final
ranging_final_pkt_construct();
- power_on_radio(1, 0);
- mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, 0, (uint8_t *)&ranging_user_pkt, MSG_HEADER_LEN + MSG_FINAL_USER_DATA_IDX);
+ uwb_tx((uint8_t *)&ranging_user_pkt, MSG_HEADER_LEN + MSG_FINAL_USER_DATA_IDX, TX_MODE_DEFER, target_time);
}
break;
@@ -624,11 +598,9 @@
{
ranging_env.slot_idx++;
// receive result
- power_on_radio(0, 1);
- sts_lsp_store();
target_time = ranging_env.anchor_point + ranging_env.slot_idx * ranging_env.slot_interval;
- mac_rx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time - UWB_RX_OPEN_IN_ADVANCE, UWB_RX_WINDOW);
+ uwb_rx(RX_MODE_DEFER, target_time - UWB_RX_OPEN_IN_ADVANCE, UWB_RX_WINDOW);
}
break;
#endif
@@ -650,21 +622,19 @@
{
ranging_env.anchor_point += ranging_env.ranging_period;
// receive sync
- power_on_radio(0, 1);
if (uwb_app_config.session_param.aoa_result_req)
{
#if DYNAMIC_UPDATE_MAIN_ANTENNA_EN
// dynamic switch main antenna
- phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 1, ranging_env.main_ant_id);
- aoa_param_update(ranging_env.main_ant_id);
+ phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 1, ranging_env.main_ant_id);
+ sts_param_update(ranging_env.main_ant_id);
#else
- phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
+ phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
#endif
}
- sts_lsp_store();
- mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, ranging_env.ranging_period);
+ uwb_rx(RX_MODE_IMMEDIATE, 0, ranging_env.ranging_period);
power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
ranging_env.responder_final_flag = 0;
}
@@ -674,23 +644,21 @@
{
ranging_env.anchor_point += ranging_env.ranging_period;
// receive poll
- power_on_radio(0, 1);
if (uwb_app_config.session_param.aoa_result_req)
{
#if DYNAMIC_UPDATE_MAIN_ANTENNA_EN
// dynamic switch main antenna
- phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 1, ranging_env.main_ant_id);
- aoa_param_update(ranging_env.main_ant_id);
+ phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 1, ranging_env.main_ant_id);
+ sts_param_update(ranging_env.main_ant_id);
#else
- phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
+ phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
#endif
}
- sts_lsp_store();
target_time = ranging_env.anchor_point - LPM_PPM_COMPENSATION(LOW_POWER_CLOCK_PPM, uwb_app_config.session_param.ranging_interval);
- mac_rx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time - UWB_RX_OPEN_IN_ADVANCE,
- UWB_RX_WINDOW + LPM_PPM_COMPENSATION(LOW_POWER_CLOCK_PPM, uwb_app_config.session_param.ranging_interval));
+ uwb_rx(RX_MODE_DEFER, target_time - UWB_RX_OPEN_IN_ADVANCE,
+ UWB_RX_WINDOW + 2 * LPM_PPM_COMPENSATION(LOW_POWER_CLOCK_PPM, uwb_app_config.session_param.ranging_interval));
power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
ranging_env.responder_final_flag = 0;
}
@@ -705,15 +673,11 @@
if (uwb_app_config.ranging_flow_mode == (uint8_t)RANGING_FLOW_CONTENTION)
{
- power_on_radio(1, 1);
- mac_tx(EVT_MODE_MAC_FIX_PHY_ASAP, target_time, 0, (uint8_t *)&ranging_user_pkt,
- user_response_data_len + MSG_HEADER_LEN + MSG_RESPONSE_USER_DATA_IDX);
+ uwb_tx((uint8_t *)&ranging_user_pkt, user_response_data_len + MSG_HEADER_LEN + MSG_RESPONSE_USER_DATA_IDX, TX_MODE_CSMACA, target_time);
}
else
{
- power_on_radio(1, 0);
- mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, 0, (uint8_t *)&ranging_user_pkt,
- user_response_data_len + MSG_HEADER_LEN + MSG_RESPONSE_USER_DATA_IDX);
+ uwb_tx((uint8_t *)&ranging_user_pkt, user_response_data_len + MSG_HEADER_LEN + MSG_RESPONSE_USER_DATA_IDX, TX_MODE_DEFER, target_time);
}
}
break;
@@ -721,22 +685,20 @@
case RANGING_FINAL:
{
// receive final
- power_on_radio(0, 1);
if (uwb_app_config.session_param.aoa_result_req)
{
#if DYNAMIC_UPDATE_MAIN_ANTENNA_EN
// dynamic switch main antenna
- phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 1, ranging_env.main_ant_id);
- aoa_param_update(ranging_env.main_ant_id);
+ phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 1, ranging_env.main_ant_id);
+ sts_param_update(ranging_env.main_ant_id);
#else
- phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
+ phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
#endif
}
- sts_lsp_store();
target_time = ranging_env.anchor_point + (1 + ranging_env.responder_num) * ranging_env.slot_interval;
- mac_rx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time - UWB_RX_OPEN_IN_ADVANCE, UWB_RX_WINDOW);
+ uwb_rx(RX_MODE_DEFER, target_time - UWB_RX_OPEN_IN_ADVANCE, UWB_RX_WINDOW);
}
break;
@@ -745,10 +707,8 @@
{
// send result
ranging_result_pkt_construct();
- power_on_radio(1, 0);
target_time = ranging_env.anchor_point + (1 + ranging_env.responder_num + 1 + ranging_env.responder_slot_idx) * ranging_env.slot_interval;
- mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, 0, (uint8_t *)&ranging_user_pkt,
- user_response_data_len + MSG_HEADER_LEN + MSG_RESULT_INFO_LEN);
+ uwb_tx((uint8_t *)&ranging_user_pkt, user_response_data_len + MSG_HEADER_LEN + MSG_RESULT_INFO_LEN, TX_MODE_DEFER, target_time);
}
break;
#endif
@@ -758,15 +718,13 @@
break;
}
}
-
- mac_start();
}
else
{
power_mode_clear(POWER_UNIT_APP);
LOG_INFO(TRACE_MODULE_APP, "Ranging stop\r\n");
}
- // board_led_off(BOARD_LED_1);
+ // board_led_off(BOARD_LED_2);
}
enum RANGING_STAGE_T ranging_fsm(const struct MAC_HW_REPORT_T *ind)
@@ -777,6 +735,8 @@
// board_led_on(BOARD_LED_2);
power_off_radio();
+ sts_lsp_store_stop();
+
if (ranging_env.enable)
{
ranging_env.count++;
@@ -844,12 +804,6 @@
ranging_env.rx_response_time[responder_idx] = 0;
}
}
-#if RANGING_FOM_FILTER_EN
- // update response taps
- ranging_env.frame[responder_idx].fap_valid =
- first_path_align(ranging_env.frame[responder_idx].gaps, &ranging_env.frame[responder_idx].gaps_num, debug_csi.rframe[0].cir,
- debug_csi.rframe[0].first_tap, PWR_TH, TAP_MARGIN);
-#endif
}
}
@@ -871,7 +825,6 @@
uwb_app_config.ranging_stage = RANGING_POLL;
}
}
- sts_lsp_store_stop();
}
break;
@@ -956,9 +909,8 @@
if (uwb_app_config.session_param.aoa_result_req)
{
- phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_NEVER_SWITCH, 0, 0);
+ phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_NEVER_SWITCH, 0, 0);
}
- sts_lsp_store_stop();
}
break;
@@ -1000,9 +952,8 @@
if (uwb_app_config.session_param.aoa_result_req)
{
- phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_NEVER_SWITCH, 0, 0);
+ phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_NEVER_SWITCH, 0, 0);
}
- sts_lsp_store_stop();
}
break;
@@ -1075,9 +1026,8 @@
if (uwb_app_config.session_param.aoa_result_req)
{
- phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_NEVER_SWITCH, 0, 0);
+ phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_NEVER_SWITCH, 0, 0);
}
- sts_lsp_store_stop();
}
break;
@@ -1112,11 +1062,20 @@
}
else if (RANGING_SYNC == stage)
{
- sts_lsp_store();
// receive sync again
- power_on_radio(0, 1);
- mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, ranging_env.ranging_period);
- mac_start();
+
+ if (uwb_app_config.session_param.aoa_result_req)
+ {
+#if DYNAMIC_UPDATE_MAIN_ANTENNA_EN
+ // dynamic switch main antenna
+ phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 1, ranging_env.main_ant_id);
+ sts_param_update(ranging_env.main_ant_id);
+#else
+ phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
+#endif
+ }
+
+ uwb_rx(RX_MODE_IMMEDIATE, 0, ranging_env.ranging_period);
ranging_env.synced = 0;
}
else
@@ -1176,8 +1135,9 @@
ind->hdr.event = UWB_PKT_RX_DONE_MSG;
ind->ranging_stage = (uint8_t)stage;
ind->status = rx->err_code;
- ind->rssi = rx->rssi;
- ind->snr = rx->snr;
+ // ind->rssi = rx->rssi;
+ ind->rssi = correct_rssi(rx->rssi);
+ ind->snr = correct_snr(rx->snr);
if (rx->err_code == UWB_RX_OK)
{
@@ -1198,8 +1158,9 @@
if (ranging_debug_csi_en_get())
{
uint8_t frame_idx = (ind->ranging_stage == RANGING_FINAL ? 1 : 0);
+
+ debug_csi.antenna = ranging_env.main_ant_id;
debug_csi.frame_idx = frame_idx;
- debug_csi.rframe_idx = frame_idx;
uint32_t val = REG_READ(0x40003050);
debug_csi.frame[frame_idx].rf_gain = (val & 0x07);
@@ -1211,9 +1172,8 @@
{
debug_csi.frame[frame_idx].rssi = ind->rssi;
debug_csi.frame[frame_idx].snr = ind->snr;
-
- dump_preamble_cir(frame_idx, 128);
- dump_sts_cir(frame_idx);
+ debug_csi.frame[frame_idx].channel_power = REG_READ(0x40002064);
+ debug_csi.frame[frame_idx].noise_power = REG_READ(0x40002078);
}
}
#endif
--
Gitblit v1.9.3