From affb754b56216235932464dbaffbaf34638cd8f2 Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期四, 30 一月 2020 10:02:15 +0800
Subject: [PATCH] 增加标签第一个数据不输出
---
源码/核心板/Src/application/dw_app.c | 56 +++++++++++++++++++++++++++++++++-----------------------
1 files changed, 33 insertions(+), 23 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 653b3d7..1268afd 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"
@@ -94,7 +94,7 @@
DWT_PAC8, /* Preamble acquisition chunk size. Used in RX only. */
9, /* TX preamble code. Used in TX only. */
9, /* RX preamble code. Used in RX only. */
- 0, /* Use non-standard SFD (Boolean) */
+ 1, /* Use non-standard SFD (Boolean) */
DWT_BR_6M8, /* Data rate. */
DWT_PHRMODE_STD, /* PHY header mode. */
(129 + 8 - 8) /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
@@ -422,8 +422,10 @@
memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
// g_Tagdist[anc_id_recv]= hex_dist;
// g_flag_Taggetdist[anc_id_recv]=0;
- if(!g_com_map[MODBUS_MODE])
+
+ if(!g_com_map[MODBUS_MODE]&&hex_dist2!=0xffff)
{
+ hex_dist2 = hex_dist2+(int16_t)g_com_map[DIST_OFFSET];
usart_send[2] = 1;//正常模式
usart_send[3] = 17;//数据段长度
usart_send[4] = frame_seq_nb;//数据段长度
@@ -474,7 +476,9 @@
}
int8_t correction_time;
extern uint8_t sync_seq;
-
+#define TDFILTER
+//#define CHECK_UID
+extern uint8_t UID_ERROR;
void Anchor_App(void)
{
uint32_t frame_len;
@@ -541,7 +545,7 @@
/* 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], &anchor_dist_last_frm[tag_id_recv-TAG_ID_START], 4);
+ memcpy(&tx_resp_msg[DIST_IDX], &g_Tagdist[tag_id_recv], 4);
dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//写入发送数据
dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//设定发送长度
@@ -589,6 +593,10 @@
final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts);
final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);
+ #ifdef CHECK_UID
+ if(UID_ERROR==1)
+ poll_rx_ts=0;
+ #endif
/* Compute time of flight. 32-bit subtractions give correct answers even if clock has wrapped. See NOTE 10 below. */
poll_rx_ts_32 = (uint32_t)poll_rx_ts;//使用32位数据计算
resp_tx_ts_32 = (uint32_t)resp_tx_ts;
@@ -603,27 +611,30 @@
distance = tof * SPEED_OF_LIGHT;//距离=光速*飞行时间
dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //距离减去矫正系数
- dist_cm = dist_no_bias * 100; //dis 为单位为cm的距离
+ dist_cm = dist_no_bias * 1000; //dis 为单位为cm的距离
// dist[TAG_ID] = LP(dis, TAG_ID); //LP 为低通滤波器，让数据更稳定
/*--------------------------以下为非测距逻辑------------------------*/
LED0_BLINK; //每成功一次通讯则闪烁一次
g_UWB_com_interval = 0;
- dis_after_filter=dist_cm;
- hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
- if(hex_dist-anchor_dist_last_frm[tag_id_recv-TAG_ID_START]<1500||misdist_num>4)
+
+ hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10;
+ if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
{
+ if(hex_dist-his_dist[tag_id_recv-TAG_ID_START]<15000||misdist_num>4)
+ {int32_t filter_dist;
misdist_num=0;
- if(hex_dist<100000&&hex_dist>-1000)
- {
- if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
- {
- if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])<1000)
- {
- g_Tagdist[tag_id_recv-TAG_ID_START] = hex_dist;
- anchor_dist_last_frm[tag_id_recv-TAG_ID_START] = hex_dist;
- g_Tagdist[tag_id_recv]= hex_dist;
- }
+ if(hex_dist<1000000&&hex_dist>-10000)
+ {
+ #ifdef TDFILTER
+ NewTrackingDiffUpdate(tag_id_recv-TAG_ID_START, (float)hex_dist);
+ filter_dist=pos_predict[tag_id_recv-TAG_ID_START]/10;
+ #else
+ filter_dist=hex_dist/10;
+ #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])
@@ -642,16 +653,15 @@
UART_PushFrame(usart_send,21);
}
// memcpy(&Modbus_HoldReg[tag_id_recv*2],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
- Modbus_HoldReg[tag_id_recv*2]=anchor_dist_last_frm[tag_id_recv-TAG_ID_START]>>16;
- Modbus_HoldReg[tag_id_recv*2+1]=anchor_dist_last_frm[tag_id_recv-TAG_ID_START];
+ Modbus_HoldReg[tag_id_recv*2]=g_Tagdist[tag_id_recv-TAG_ID_START]>>16;
+ Modbus_HoldReg[tag_id_recv*2+1]=g_Tagdist[tag_id_recv-TAG_ID_START];
//dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm);
}
- }
+
}else{
misdist_num++;
}
- if(anchor_dist_last_frm[tag_id_recv-TAG_ID_START]>1000)
- {anchor_dist_last_frm[tag_id_recv-TAG_ID_START]=anchor_dist_last_frm[tag_id_recv-TAG_ID_START];}
+ }
}
}else{
/* Clear RX error events in the DW1000 status register. */
--
Gitblit v1.9.3