From ae53ad9142f8092693118eac64a138d050082afa Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期二, 25 十月 2022 17:11:46 +0800
Subject: [PATCH] 测试延迟差=0 精度
---
源码/核心板/Src/application/dw_app.c | 82 +++++++++++++++++++++++++++++++++++++----
1 files changed, 74 insertions(+), 8 deletions(-)
diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c"
index 117e158..e363661 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c"
@@ -15,6 +15,7 @@
*/
#include <string.h>
+#include <math.h>
#include "dw_app.h"
#include "deca_device_api.h"
#include "deca_regs.h"
@@ -28,7 +29,9 @@
#include <stdio.h>
#include "beep.h"
#include "modbus.h"
-#define DEBUG_OUTPUT
+//#define DEBUG_OUTPUT
+//#define TDFILTER
+//#define CONSTANT_FILTER
/*------------------------------------ Marcos ------------------------------------------*/
/* Inter-ranging delay period, in milliseconds. */
#define RNG_DELAY_MS 100
@@ -46,7 +49,7 @@
#define POLL_TX_TO_RESP_RX_DLY_UUS 150
/* This is the delay from Frame RX timestamp to TX reply timestamp used for calculating/setting the DW1000's delayed TX function. This includes the
* frame length of approximately 2.66 ms with above configuration. */
-#define RESP_RX_TO_FINAL_TX_DLY_UUS 400
+#define RESP_RX_TO_FINAL_TX_DLY_UUS 420
/* Receive response timeout. See NOTE 5 below. */
#define RESP_RX_TIMEOUT_UUS 600
@@ -81,6 +84,7 @@
#define ANCTIMEMS 14
#define ANCTIMEUS 16
#define ANCSEND_INTERVAL 18
+#define SIGNALPOWER 20
#define POLL 0x01
#define RESPONSE 0x02
@@ -109,7 +113,7 @@
static uint8_t tx_final_msg[24] = {0};
//static uint8_t rx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
-static uint8_t tx_resp_msg[22] = {0};
+static uint8_t tx_resp_msg[23] = {0};
//static uint8_t rx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
/* Frame sequence number, incremented after each transmission. */
@@ -320,6 +324,53 @@
dwt_writetxfctrl(sizeof(tx_sync_msg), 0);//设置超宽带发送数据长度
dwt_starttx(DWT_START_TX_IMMEDIATE);
}
+ double firstpath_power, rx_power,rec_firstpath_power;
+ double f1, f2, r1, r2;
+uint16_t F1,F2,F3,N,C;
+double B = 131072;
+double A = 121.74;
+double min_power;
+ dwt_rxdiag_t d1;
+double LOS(dwt_rxdiag_t *dia) {
+ F1 = dia->firstPathAmp1;
+ F2 = dia->firstPathAmp2;
+ F3 = dia->firstPathAmp3;
+ N = dia->rxPreamCount;
+ C = dia->maxGrowthCIR;
+
+ firstpath_power=10* log10((F1*F1+F2*F2+F3*F3)/(N*N))-A;
+ rx_power=10*log10(C*B/(N*N))-A;
+
+ // min_power = - 10 * log10((F1 *F1 + F2 * F2 + F3 * F3) / (C *B));
+ return min_power;
+ }
+#define CONSTANT_LEN 50
+extern u16 dist_threshold;
+int32_t ConstantFilter(int32_t currentdist,u8 channel)
+ {
+ static int32_t cs_lastdist[10],cs_lastvalid[10];
+ static u8 cfstart_flag[10] = {1};
+ static u8 constant_count[10] = {100};
+ if(cfstart_flag[channel])
+ {
+ cfstart_flag[channel] = 0;
+ cs_lastdist[channel] = currentdist;
+ }
+ if( abs(currentdist - cs_lastdist[channel])<100)
+ {
+ if(constant_count[channel]<CONSTANT_LEN)
+ constant_count[channel]++;
+ }else{
+ constant_count[channel] = 0;
+ }
+ if(constant_count[channel] == CONSTANT_LEN)
+ {
+ cs_lastvalid[channel] = currentdist;
+ }
+ cs_lastdist[channel] = currentdist;
+ return cs_lastvalid[channel];
+ }
+
uint16_t g_Resttimer;
uint8_t result;
u8 tag_succ_times=0;
@@ -414,6 +465,7 @@
TIM3->CNT=tmp_time;
}
memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);
+ rec_firstpath_power = rx_buffer[SIGNALPOWER];
memcpy(&tx_final_msg[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 4);
memcpy(&rec_com_interval,&rx_buffer[ANCSEND_INTERVAL], 2);
if(rec_com_interval>4&&rec_com_interval!=g_com_map[COM_INTERVAL])
@@ -455,7 +507,8 @@
{
g_Tagdist[anc_id_recv]= hex_dist2;
g_flag_Taggetdist[anc_id_recv]=0;
-
+ Modbus_HoldReg[anc_id_recv*2]=hex_dist2>>16;
+ Modbus_HoldReg[anc_id_recv*2+1]=hex_dist2;
if(!g_com_map[MODBUS_MODE])
{
hex_dist2 = hex_dist2;
@@ -468,9 +521,11 @@
memcpy(&usart_send[9],&hex_dist2,4);
usart_send[13] = bat_percent;
usart_send[14] = button;
+ usart_send[15] = rec_firstpath_power;
checksum = Checksum_u16(&usart_send[2],17);
memcpy(&usart_send[19],&checksum,2);
UART_PushFrame(usart_send,21);
+
}
}
// memcpy(&Modbus_HoldReg[anc_id_recv*2],&hex_dist,4);
@@ -513,12 +568,13 @@
/* Execute a delay between ranging exchanges. */
}
+
int8_t correction_time;
extern uint8_t sync_seq;
-#define TDFILTER
+
//#define CHECK_UID
extern uint8_t UID_ERROR;
-extern u16 dist_threshold;
+
u8 misdist_num[TAG_NUM_IN_SYS];
void Anchor_App(void)
{
@@ -585,7 +641,8 @@
/* Write and send the response message. See NOTE 9 below.*/
if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
memcpy(&tx_resp_msg[DIST_IDX], &g_Tagdist[tag_id_recv], 4);
-
+ tx_resp_msg[SIGNALPOWER] = firstpath_power;
+
dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//写入发送数据
dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//设定发送长度
result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送，等待接收
@@ -655,7 +712,8 @@
dist_cm = dist_no_bias * 1000; //dis 为单位为cm的距离
// dist[TAG_ID] = LP(dis, TAG_ID); //LP 为低通滤波器，让数据更稳定
-
+ dwt_readdiagnostics(&d1);
+ LOS(&d1);
/*--------------------------以下为非测距逻辑------------------------*/
#ifdef DEBUG_OUTPUT
printf("收到FINAL包，标签ID: %d .\r\n",tag_id_recv);
@@ -678,9 +736,15 @@
#else
filter_dist=hex_dist/10;
#endif
+
+ #ifdef CONSTANT_FILTER
+ filter_dist = ConstantFilter(filter_dist,tag_id_recv-TAG_ID_START);
+ #endif
anchor_dist_last_frm[tag_id_recv-TAG_ID_START]=filter_dist;
g_Tagdist[tag_id_recv]= filter_dist;
+
+
his_dist[tag_id_recv-TAG_ID_START]=hex_dist;
g_flag_Taggetdist[tag_id_recv]=0;
if(!g_com_map[MODBUS_MODE])
@@ -694,6 +758,8 @@
memcpy(&usart_send[9],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
usart_send[13] = battary;
usart_send[14] = button;
+ usart_send[15] = firstpath_power;
+ usart_send[16] = rx_power;
checksum = Checksum_u16(&usart_send[2],17);
memcpy(&usart_send[19],&checksum,2);
UART_PushFrame(usart_send,21);
--
Gitblit v1.9.3