From 15853954a9d7424d04943cd4163d21eb8807740d Mon Sep 17 00:00:00 2001
From: chen <15335560115@163.com>
Date: 星期三, 20 十一月 2024 11:07:12 +0800
Subject: [PATCH] V1.0提交未收控制版本
---
keil/dw_app_anchor.c | 57 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 57 insertions(+), 0 deletions(-)
diff --git a/keil/dw_app_anchor.c b/keil/dw_app_anchor.c
new file mode 100644
index 0000000..ff1fa33
--- /dev/null
+++ b/keil/dw_app_anchor.c
@@ -0,0 +1,57 @@
+#include "dw_app_anchor.h"
+#include "stdint.h"
+#include <string.h>
+#if defined (MK_SS_TWR_DW_INIT)
+static float clockOffsetRatio;
+static double rtd_init, rtd_resp;
+static 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];
+static int32_t anc_clockoffset[MAX_NEARBASE_NUM];
+static int16_t anc_distoffset[MAX_NEARBASE_NUM];
+extern uint8_t taglist_keeptime[MAX_TAG_LIST_NUM]; //每个标签存活时间
+extern uint16_t taglist_id[MAX_TAG_LIST_NUM],taglist_dist[MAX_TAG_LIST_NUM]; //标签距离和标签的ID
+extern uint16_t taglist_total_num,taglist_current_index;
+
+extern uint8_t exsistbase_list[MAX_NEARBASE_NUM];
+extern int32_t nearbase_distlist[MAX_NEARBASE_NUM];
+void SetANCTimestap(uint8_t i,uint8_t* pollrx,uint8_t* resptx,uint32_t resprx,uint8_t* distoffset,int32_t anc_clockoffset_from_MK) //写入时间戳信息
+{
+ memcpy(&anc_pollrx[i],pollrx,4);
+ memcpy(&anc_resptx[i],resptx,4);
+ memcpy(&tag_resprx[i],&resprx,4);
+ memcpy(&anc_distoffset[i],distoffset,2);
+ anc_clockoffset[i] = anc_clockoffset_from_MK;//MK8000修改
+}
+void CalculateDists(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;
+ if(distance>-10&&distance<1000)
+ {
+ nearbase_distlist[i] = distance*100+anc_distoffset[i];
+ }
+ } else {
+ // nearbase_distlist[i] = 0x1ffff;
+ }
+
+ }
+
+}
+#endif
+
+
--
Gitblit v1.9.3