From bccfff0ace10f85619e9f1fd4b2806f99e9390c2 Mon Sep 17 00:00:00 2001
From: chen <15335560115@163.com>
Date: 星期六, 16 八月 2025 18:33:27 +0800
Subject: [PATCH] 防丢标签V1.0,demo版本实现震动和测距
---
keil/dw_tag.c | 83 +++++++++++++++++++++--------------------
1 files changed, 43 insertions(+), 40 deletions(-)
diff --git a/keil/dw_tag.c b/keil/dw_tag.c
index b48c51d..a27738a 100644
--- a/keil/dw_tag.c
+++ b/keil/dw_tag.c
@@ -8,7 +8,7 @@
#include "global_param.h"
static float clockOffsetRatio;
static double rtd_init, rtd_resp;
-double tof,distance_tag;
+double tof,distance;
#define SPEED_OF_LIGHT 299702547
#define DWT_TIME_UNITS (1.0/499.2e6/128.0) //!< = 15.65e-12 s
static uint32_t anc_pollrx[MAX_NEARBASE_NUM],anc_resptx[MAX_NEARBASE_NUM],tag_resprx[MAX_NEARBASE_NUM],tag_polltx[MAX_NEARBASE_NUM];
@@ -36,34 +36,36 @@
memcpy(&tag_resprx[i],&resprx,4);
memcpy(&anc_distoffset[i],distoffset,2);
memcpy(&tag_polltx[i],&polltx,4);
- //anc_clockoffset[i] = anc_clockoffset_from_MK;//MK8000修改
+ anc_clockoffset[i] = freq_offset;//MK8000修改
}
-//void CalculateDists1(int64_t poll_tx_ts)
-//{
-//for(int i=0;i<MAX_NEARBASE_NUM;i++)
-// {
-// //rec_anc_signalpower[i] = exsistbase_list[i];
-// if(exsistbase_list[i]==KEEP_TIMES)
-// {
-// // exsistbase_list[i]--;
-//#ifdef _UWB_4G
-// clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_2 / 1.0e6) ;
-//#else
-// clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_5 / 1.0e6) ;//MK8000修改
-//#endif
-// rtd_init = tag_resprx[i] - (uint32_t)poll_tx_ts;
-// rtd_resp = anc_resptx[i] - anc_pollrx[i];
-// tof = ((rtd_init - rtd_resp * (1 - clockOffsetRatio)) / 2.0) * DWT_TIME_UNITS;
-// distance_tag = tof * SPEED_OF_LIGHT/100;
-// if(distance_tag>-10&&distance_tag<1000)
-// {
-// nearbase_distlist[i] = distance_tag+anc_distoffset[i];
-// }
-// }else{
-// nearbase_distlist[i] = 0x1ffff;
-// }
-// }
-//}
+void CalculateDists1(int64_t poll_tx_ts)
+{
+for(int i=0;i<MAX_NEARBASE_NUM;i++)
+ {
+ //rec_anc_signalpower[i] = exsistbase_list[i];
+ if(exsistbase_list[i]==KEEP_TIMES)
+ {
+ // exsistbase_list[i]--;
+#ifdef _UWB_4G
+ clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_2 / 1.0e6) ;
+#else
+ clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_5 / 1.0e6) ;//MK8000修改
+#endif
+ rtd_init = tag_resprx[i] - (uint32_t)poll_tx_ts;
+ rtd_resp = anc_resptx[i] - anc_pollrx[i];
+ tof = ((rtd_init - rtd_resp * (1 - clockOffsetRatio)) / 2.0) * DWT_TIME_UNITS;
+ distance = tof * SPEED_OF_LIGHT/100;
+ if(distance>-10&&distance<1000)
+ {
+ nearbase_distlist[i] = distance+anc_distoffset[i];
+ }
+ }else{
+ nearbase_distlist[i] = 0x1ffff;
+ }
+
+ }
+
+}
// ts_a - ts_b
int64_t ranging_timestamp_diff(int64_t ts_a, int64_t ts_b)
{
@@ -78,10 +80,9 @@
}
void CalculateDists(int64_t poll_tx_ts)
{
- for(int i=0;i<MAX_NEARBASE_NUM;i++)
+ for(int i=0;i<MAX_NEARBASE_NUM+10;i++)
{
-
- if(exsistbase_list[i]==KEEP_TIMES)
+ if(taglist_keeptime[i+taglist_current_index]==KEEP_TIMES)
{
//temp_freq_offset=freq_offset_filter;//测试
@@ -107,7 +108,7 @@
// correct antenna delay
// tround -= ranging_ant_delays_get(uwb_app_config.ppdu_params.rx_ant_id);
// corrected by frequency offset
- tround = (int64_t)((double)tround * (1 - (double)freq_offset_filter / ch_center_freq_map[UWB_CH_NUM]));
+ tround = (int64_t)((double)tround * (1 - (double)anc_clockoffset[i] / ch_center_freq_map[UWB_CH_NUM]));
tround_temp=tround;
treply_temp=treply;
tof_i = (int32_t)(tround - treply) / 2;
@@ -120,17 +121,19 @@
// }
//tof_f = (double)TIMESTAMP_UNIT_TO_NS((uint32_t)tof_i);//yuan
tof_f = (double)TIMESTAMP_UNIT_TO_NS(tof_i);
- //distance_tag=(uint16_t)(tof_f * 0.299702547 * VP_VAL - RANGING_CORR);//yuan
- distance_tag=tof_f * 0.299702547 * VP_VAL - RANGING_CORR;
- distance_tag=distance_tag+30;//测试加的offset
- //LOG_INFO(TRACE_MODULE_APP, "distance_tag is %lf\r\n",distance_tag);
+ //distance=(uint16_t)(tof_f * 0.299702547 * VP_VAL - RANGING_CORR);//yuan
+ distance=tof_f * 0.299702547 * VP_VAL - RANGING_CORR;
+ distance=distance+30;//测试加的offset
+ //LOG_INFO(TRACE_MODULE_APP, "distance is %lf\r\n",distance);
- if(distance_tag>-1000&&distance_tag<100000)
+ if(distance>-1000&&distance<100000)
{
- nearbase_distlist[i] = distance_tag+(int16_t)g_com_map[OFFSET_AS_TAG];//offset加入上位机校准
- }
+ taglist_dist[i+taglist_current_index] = distance+(int16_t)g_com_map[DIST_OFFSET];//offset未加
+ }else{
+ taglist_dist[i] = 0x2ffff;
+ }
}else{
- nearbase_distlist[i] = 0x1ffff;
+ //nearbase_distlist[i] = 0x7fff;
}
}
--
Gitblit v1.9.3