From e328ebef585cea2351b37117b2d5ac4978ecd3c0 Mon Sep 17 00:00:00 2001
From: WXK <287788329@qq.com>
Date: 星期二, 11 二月 2025 14:57:23 +0800
Subject: [PATCH] 1111111
---
keil/include/components/app/src/ranging_ss_twr_task.c | 49 ++++++++++++++++++++++++++-----------------------
1 files changed, 26 insertions(+), 23 deletions(-)
diff --git a/keil/include/components/app/src/ranging_ss_twr_task.c b/keil/include/components/app/src/ranging_ss_twr_task.c
index 1ca8edc..1b97dfe 100644
--- a/keil/include/components/app/src/ranging_ss_twr_task.c
+++ b/keil/include/components/app/src/ranging_ss_twr_task.c
@@ -118,10 +118,6 @@
* \return None.
*/
/*************************************************************************************************/
- int32_t tof_i;
- double tof_f;
- int64_t Tround;
- int64_t Treply;
void ranging_handler(wsfEventMask_t event, const void *param)
{
const wsfMsgHdr_t *msg = (const wsfMsgHdr_t *)param;
@@ -148,6 +144,12 @@
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]);
@@ -160,6 +162,15 @@
{
LOG_INFO(TRACE_MODULE_APP, "[TX][%u] Response\r\n", ind->tx_len);
LOG_INFO(TRACE_MODULE_APP, "Treply %u \r\n", (uint32_t)ranging_treply());
+
+ 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
+ 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 PRINT_PAYLOAD_EN
@@ -208,13 +219,7 @@
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);
#endif
- 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
if (ind->rx_len - POLL_MSG_USER_DATA_IDX)
{
@@ -234,8 +239,8 @@
int32_t freq_offset_filter = average_filter(freq_offset);
LOG_INFO(TRACE_MODULE_APP, "CH Freq Offset %d\r\n", freq_offset_filter);
- Tround = ranging_tround();
- Treply = 0;
+ int64_t Tround = ranging_tround();
+ int64_t Treply = 0;
for (int i = 4; i >= 0; i--)
{
@@ -248,25 +253,23 @@
{
// corrected by frequency offset
Tround = (int64_t)((double)Tround * (1 - (double)freq_offset_filter / ch_center_freq_map[uwb_app_config.ppdu_params.ch_num]));
- tof_i = (int32_t)(Tround - Treply) / 2;
+ int32_t tof_i = (int32_t)(Tround - Treply) / 2;
// outlier filter
if (tof_i < 0)
{
tof_i = 0;
}
-
- ranging_env.tof = (uint32_t)(tof_i);
-
- tof_f = (double)TIMESTAMP_UNIT_TO_NS(ranging_env.tof);
- struct RANGING_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0];
+ ranging_env.tof = (uint32_t)(tof_i);
+
+ double tof_f = (double)TIMESTAMP_UNIT_TO_NS(ranging_env.tof);
+
+ struct TWR_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0];
// update distance result
range_result->distance = (uint16_t)(tof_f * 0.299702547 * VP_VAL - RANGING_CORR);
-
- LOG_INFO(TRACE_MODULE_APP, "tof_f %lf,tof_i %d\r\n", tof_f,tof_i);
-
+
range_result->status = STATUS_OK;
if (ranging_frame_type_get() == SP1)
@@ -323,7 +326,7 @@
struct RANGE_DATA_T *range_data = &ranging_env.range_data;
- range_data->ranging_type = 0x01;
+ range_data->ranging_type = 0x01; // TWR (SS-TWR, DS-TWR)
range_data->measurements_num = 1;
uint16_t target_addr = uwbs_peer_short_addr_get();
range_result->mac_addr[0] = target_addr & 0xff;
--
Gitblit v1.9.3