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_task.c | 134 ++++++++++++++------------------------------
1 files changed, 43 insertions(+), 91 deletions(-)
diff --git a/keil/include/components/app/src/ranging_custom_task.c b/keil/include/components/app/src/ranging_custom_task.c
index 1eb5ad7..79fc851 100644
--- a/keil/include/components/app/src/ranging_custom_task.c
+++ b/keil/include/components/app/src/ranging_custom_task.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.
@@ -46,7 +46,7 @@
#include "ranging_custom.h"
#include "lib_aoa.h"
#include "lib_ranging.h"
-#if KF_EN
+#if FILTER_EN
#include "lib_kf.h"
#endif
#if PDOA_3D_EN
@@ -72,7 +72,6 @@
return;
}
-#if KF_EN
float post_range, post_azimuth, post_elevation;
float azimuth_meas = mk_q7_to_f32(*azimuth);
float elevation_meas = mk_q7_to_f32(*elevation);
@@ -102,19 +101,6 @@
// LOG_INFO(TRACE_MODULE_APP, "$%u %u %d %d %d %d;\r\n", (uint16_t)(range_meas*100),(uint16_t)(post_range*100),(int16_t)azimuth_meas,(int16_t)post_azimuth,
// (int16_t)elevation_meas, (int16_t)post_elevation);
-#else
- 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
@@ -155,9 +141,16 @@
case UWB_PKT_TX_DONE_MSG:
{
const struct UWB_PKT_TX_DONE_IND_T *ind = (const struct UWB_PKT_TX_DONE_IND_T *)param;
+
+ if (ind->status != UWB_TX_OK)
+ {
+ LOG_INFO(TRACE_MODULE_APP, "UWB TX fail 0x%04x\r\n", ind->status);
+ break;
+ }
+
if (ind->ranging_stage == RANGING_POLL)
{
- uint16_t seq_num = READ_SHORT(&ind->tx_data[2]);
+ ranging_env.range_data.sequence_num = READ_SHORT(&ind->tx_data[2]);
#ifdef SE_DEMO_EN
if (ind->tx_len > (MSG_HEADER_LEN + MSG_POLL_USER_DATA_IDX))
@@ -169,11 +162,11 @@
LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\n");
if (uwb_app_config.ranging_flow_mode == (uint8_t)RANGING_FLOW_CONTENTION)
{
- LOG_INFO(TRACE_MODULE_APP, "Custom DS-TWR Contention Initiator SEQ NUM %u\r\n", seq_num);
+ LOG_INFO(TRACE_MODULE_APP, "Custom DS-TWR Contention Initiator SEQ NUM %u\r\n", ranging_env.range_data.sequence_num);
}
else
{
- LOG_INFO(TRACE_MODULE_APP, "Custom DS-TWR Initiator SEQ NUM %u\r\n", seq_num);
+ LOG_INFO(TRACE_MODULE_APP, "Custom DS-TWR Initiator SEQ NUM %u\r\n", ranging_env.range_data.sequence_num);
}
LOG_INFO(TRACE_MODULE_APP, "[TX][%u] Poll %s\r\n", ind->tx_len, ind->status == UWB_TX_OK ? "" : "TX Fail");
}
@@ -191,9 +184,9 @@
{
LOG_INFO(TRACE_MODULE_APP, "[TX][%u] Final\r\n", ind->tx_len);
- // calculate_first_tap_power(1, 1);
- // print_preamble_chest(1, 1);
- // print_sts_ch_taps(1);
+ // calculate_first_tap_power(0, 1);
+ // print_preamble_cir(ranging_env.range_data.sequence_num, 0, 1);
+ // print_sts_cir(0, 1);
}
else if (ind->ranging_stage == RANGING_RESULT)
{
@@ -219,8 +212,14 @@
const struct UWB_PKT_RX_DONE_IND_T *ind = (const struct UWB_PKT_RX_DONE_IND_T *)param;
if (ind->status == UWB_RX_OK)
{
+ if (ranging_env.synced == 0)
+ {
+ LOG_INFO(TRACE_MODULE_APP, "[RX][%u] Not a sync packet\r\n", ind->rx_len);
+ break;
+ }
+
const struct RANGING_USER_PKT_T *usr_pkt = (const struct RANGING_USER_PKT_T *)ind->rx_data;
- if ((ind->ranging_stage == RANGING_POLL) && ranging_env.synced)
+ if (ind->ranging_stage == RANGING_POLL)
{
ranging_env.range_data.sequence_num = usr_pkt->seq_num;
LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\n");
@@ -235,13 +234,6 @@
LOG_INFO(TRACE_MODULE_APP, "[RX][%u][%d] Poll\r\n", ind->rx_len, ranging_env.main_ant_id);
// LOG_INFO(TRACE_MODULE_APP, "[RX]Peer Tx Power Level %d\r\n", usr_pkt->msg.poll_msg[MSG_POLL_TX_PWR_IDX]);
- int32_t freq_offset = phy_freq_offset_get();
- int32_t freq_offset_filter = average_filter(freq_offset);
- LOG_INFO(TRACE_MODULE_APP, "CH Freq Offset %d\r\n", freq_offset_filter);
-#if XTAL_AUTO_TUNE_EN
- int32_t ppm = freq_offset_filter / (int32_t)(ch_center_freq_map[uwb_app_config.ppdu_params.ch_num] * 1e-6);
- calib_xtal38m4_load_cap_auto_tune(ppm);
-#endif
#if USER_DEFINED_DATA_REPORT_EN
uint8_t user_data_len = (uint8_t)(ind->rx_len - MSG_HEADER_LEN - MSG_POLL_USER_DATA_IDX);
if (user_data_len)
@@ -262,15 +254,7 @@
LOG_INFO(TRACE_MODULE_APP, "RSSI: %ddBm, SNR: %ddB \r\n", ind->rssi, ind->snr);
#endif
-#if CSI_EN
- struct RANGING_TAPS_INF_T taps_inf;
- ranging_taps_inf_get(&taps_inf);
- ranging_env.frame[0].NLoS = taps_inf.NLoS;
- ranging_env.frame[0].fom = taps_inf.FoM;
- LOG_INFO(TRACE_MODULE_APP, "NLoS %u FoM %u\r\n", taps_inf.NLoS, taps_inf.FoM);
-#else
ranging_fom_get(&ranging_env.frame[0].NLoS, &ranging_env.frame[0].fom);
-#endif
}
else if (ind->ranging_stage == RANGING_RESPONSE)
{
@@ -289,15 +273,7 @@
LOG_INFO(TRACE_MODULE_APP, "RSSI: %ddBm, SNR: %ddB\r\n", ind->rssi, ind->snr);
#endif
-#if CSI_EN
- struct RANGING_TAPS_INF_T taps_inf;
- ranging_taps_inf_get(&taps_inf);
- ranging_env.frame[responder_idx].NLoS = taps_inf.NLoS;
- ranging_env.frame[responder_idx].fom = taps_inf.FoM;
- LOG_INFO(TRACE_MODULE_APP, "NLoS %u FoM %u\r\n", taps_inf.NLoS, taps_inf.FoM);
-#else
ranging_fom_get(&ranging_env.frame[responder_idx].NLoS, &ranging_env.frame[responder_idx].fom);
-#endif
}
else if (ind->ranging_stage == RANGING_FINAL)
{
@@ -346,7 +322,7 @@
uint32_t distance = (uint32_t)(tof_f * 0.299702547 * VP_VAL - RANGING_CORR);
// update distance result
- struct RANGING_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0];
+ struct TWR_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0];
range_result->status = STATUS_OK;
range_result->distance = (uint16_t)distance;
@@ -390,9 +366,9 @@
}
// float pdoa[3];
- // pdoa[0] = pdoa_select_get(0, 3);
- // pdoa[1] = pdoa_select_get(1, 3);
- // pdoa[2] = pdoa_select_get(2, 3);
+ // pdoa[0] = pdoa_result_get(0, 3);
+ // pdoa[1] = pdoa_result_get(1, 3);
+ // pdoa[2] = pdoa_result_get(2, 3);
// LOG_INFO(TRACE_MODULE_APP, "PDOA: %f %f %f\r\n", pdoa[0], pdoa[1], pdoa[2]);
// float *sts_rssi = sts_rssi_output_get();
@@ -438,51 +414,19 @@
range_result->mac_addr[1] = (target_addr >> 8) & 0xff;
range_result->slot_idx = ranging_env.responder_slot_idx;
-#if CSI_EN
- // need to increase slot duration for log printing
- struct RANGING_TAPS_INF_T taps_inf;
- ranging_taps_inf_get(&taps_inf);
- ranging_env.frame[1].NLoS = taps_inf.NLoS;
- ranging_env.frame[1].fom = taps_inf.FoM;
- LOG_INFO(TRACE_MODULE_APP, "NLoS %u FoM %u\r\n", taps_inf.NLoS, taps_inf.FoM);
-
- // LOG_INFO(TRACE_MODULE_APP, "fap: %d, %f\r\n", taps_inf.fap_loc, taps_inf.fap_pow);
- // LOG_INFO(TRACE_MODULE_APP, "tap1: %d, %f\r\n", taps_inf.tap1_loc, taps_inf.tap1_pow);
- // LOG_INFO(TRACE_MODULE_APP, "tap2: %d, %f\r\n", taps_inf.tap2_loc, taps_inf.tap2_pow);
- // LOG_INFO(TRACE_MODULE_APP, "tap3: %d, %f\r\n", taps_inf.tap3_loc, taps_inf.tap3_pow);
-
- // float chtaps_re[128];
- // float chtaps_im[128];
- // ranging_multi_taps_iq_get(chtaps_re, chtaps_im, 128);
- // for (uint8_t i = 0; i < 128; i++)
- // {
- // LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "%f, %f\r\n", chtaps_re[i], chtaps_im[i]);
- // }
-#else
ranging_fom_get(&ranging_env.frame[1].NLoS, &ranging_env.frame[1].fom);
-#endif
// Retrieve Response-FoM and Response-NLoS from final packet
uint8_t FoM = usr_pkt->msg.final_msg[MSG_FINAL_FOM_IDX(ranging_env.responder_slot_idx)];
uint8_t NLoS = usr_pkt->msg.final_msg[MSG_FINAL_NLOS_IDX(ranging_env.responder_slot_idx)];
-#if RANGING_FOM_FILTER_EN
- uint8_t fap_valid = usr_pkt->msg.final_msg[MSG_FINAL_FAP_VALID_IDX(ranging_env.responder_slot_idx)];
- uint8_t gaps_num = usr_pkt->msg.final_msg[MSG_FINAL_GAPS_NUM_IDX(ranging_env.responder_slot_idx)];
- uint8_t gaps[CIR_LEN / CE_WIN - 1];
- memcpy(gaps, &usr_pkt->msg.final_msg[MSG_FINAL_GAPS_IDX(ranging_env.responder_slot_idx)], (CIR_LEN / CE_WIN - 1));
- debug_csi.ranging_fom = ranging_fom_calculate(&debug_csi, fap_valid, gaps, gaps_num);
-#endif
- LOG_INFO(TRACE_MODULE_APP, "Poll-FoM %d, Response-FoM %d Final-FoM %d, Poll-NLoS %d, Response-NLoS %d Final-NLoS %d\r\n",
- ranging_env.frame[0].fom, FoM, ranging_env.frame[1].fom, ranging_env.frame[0].NLoS, NLoS, ranging_env.frame[1].NLoS);
+ LOG_INFO(TRACE_MODULE_APP, "FoM %d-%d-%d, NLoS %d-%d-%d\r\n", ranging_env.frame[0].fom, FoM, ranging_env.frame[1].fom,
+ ranging_env.frame[0].NLoS, NLoS, ranging_env.frame[1].NLoS);
range_result->NLoS = MAX(NLoS, MAX(ranging_env.frame[0].NLoS, ranging_env.frame[1].NLoS));
// output result
uwbapi_report_ranging_data(range_data);
-
- // int8_t expected_rssi = ranging_expected_rssi_get(ranging_tx_power_get(), range_result->distance, 2, 0);
- // LOG_INFO(TRACE_MODULE_APP, "Expected RSSI: %ddBm\r\n", expected_rssi);
}
else
{
@@ -495,9 +439,17 @@
LOG_INFO(TRACE_MODULE_APP, "RSSI: %ddBm, SNR: %ddB \r\n", ind->rssi, ind->snr);
#endif
- // calculate_first_tap_power(2, 2);
- // print_preamble_chest(2, 2);
- // print_sts_ch_taps(2);
+ int32_t freq_offset = phy_freq_offset_get();
+ int32_t freq_offset_filter = average_filter(freq_offset);
+ LOG_INFO(TRACE_MODULE_APP, "CH Freq Offset %d\r\n", freq_offset_filter);
+
+#if (X38M4_AUTO_TUNE_EN == 1)
+ int32_t ppm = freq_offset_filter / (int32_t)(uwb_ch_freq_table[uwb_app_config.ppdu_params->ch_num] * 1e-6);
+ calib_xtal38m4_load_cap_auto_tune(ppm);
+#endif
+ // calculate_first_tap_power(0, 2);
+ // print_preamble_cir(ranging_env.range_data.sequence_num, 0, 2);
+ // print_sts_cir(0, 2);
}
#if RANGING_RESULT_REPORT_EN
else if (ind->ranging_stage == RANGING_RESULT)
@@ -510,10 +462,9 @@
if (ranging_env.initiator_result_flag & (1 << responder_idx))
{
uint16_t distance = READ_SHORT(&usr_pkt->msg.result_msg[2]);
- LOG_INFO(TRACE_MODULE_APP,
- "Address %X, Distance %ucm, Poll-FoM %d, Response-FoM %d Final-FoM %d, Poll-NLoS %d, Response-NLoS %d Final-NLoS %d\r\n",
- responder_addr, distance, usr_pkt->msg.result_msg[4], ranging_env.frame[responder_idx].fom, usr_pkt->msg.result_msg[6],
- usr_pkt->msg.result_msg[5], ranging_env.frame[responder_idx].NLoS, usr_pkt->msg.result_msg[7]);
+ LOG_INFO(TRACE_MODULE_APP, "Address %X, Distance %ucm, FoM %d-%d-%d, NLoS %d-%d-%d\r\n", responder_addr, distance,
+ usr_pkt->msg.result_msg[4], ranging_env.frame[responder_idx].fom, usr_pkt->msg.result_msg[6], usr_pkt->msg.result_msg[5],
+ ranging_env.frame[responder_idx].NLoS, usr_pkt->msg.result_msg[7]);
}
}
#endif
@@ -533,6 +484,7 @@
else
{
LOG_INFO(TRACE_MODULE_APP, "UWB RX fail 0x%04x\r\n", ind->status);
+ // print_preamble_cir(ranging_env.range_data.sequence_num, 0, 2);
}
}
break;
--
Gitblit v1.9.3