From 5f2e06850028e286bd95cfd003dd0682af267be3 Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期日, 29 三月 2020 11:29:07 +0800
Subject: [PATCH] 增加组ID模式
---
源码/核心板/Src/application/dw_app.c | 58 +++++++++++++++++++++++++++++-----------------------------
1 files changed, 29 insertions(+), 29 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 e7c4a18..97bb644 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"
@@ -275,6 +275,10 @@
tx_final_msg[MESSAGE_TYPE_IDX]=FINAL;
tx_sync_msg[MESSAGE_TYPE_IDX]=SYNC;
+ memcpy(&tx_poll_msg[GROUP_ID_IDX], &group_id, 1);
+ memcpy(&tx_final_msg[GROUP_ID_IDX], &group_id, 1);
+ memcpy(&tx_resp_msg[GROUP_ID_IDX], &group_id, 1);
+
memcpy(&tx_poll_msg[TAG_ID_IDX], &dev_id, 4);
memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 4);
memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 4);
@@ -374,7 +378,7 @@
/* Check that the frame is the expected response from the companion "DS TWR responder" example.
* As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
- if (rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //判断接收到的数据是否是response数据
+ if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //判断接收到的数据是否是response数据
{ u16 anc_id_recv;
/* Retrieve poll transmission and response reception timestamp. */
poll_tx_ts = get_tx_timestamp_u64(); //获得POLL发送时间T1
@@ -422,8 +426,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;//数据段长度
@@ -464,10 +470,10 @@
// deca_sleep(10);
}
// dwt_entersleep();
- if(tag_succ_times<g_com_map[MIN_REPORT_ANC_NUM])
- {
- //poll_timer +=time32_incr&0x7+3;
- }
+// if(tag_succ_times<g_com_map[MIN_REPORT_ANC_NUM])
+// {
+// //poll_timer +=time32_incr&0x7+3;
+// }
/* Execute a delay between ranging exchanges. */
@@ -521,7 +527,7 @@
// tag_recv_interval = tag_recv_timer + 65535 - tag_time_recv[tag_id_recv];
// }
- if (rx_buffer[MESSAGE_TYPE_IDX] == POLL&&(anchor_type == rx_buffer[ANC_TYPE_IDX])) //判断是否是poll包数据
+ if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == POLL&&(anchor_type == rx_buffer[ANC_TYPE_IDX])) //判断是否是poll包数据
{
tmp_time=TIM3->CNT;
memcpy(&tx_resp_msg[ANCTIMEMS],&sync_timer,2);
@@ -543,7 +549,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);//设定发送长度
@@ -575,7 +581,7 @@
/* Check that the frame is a final message sent by "DS TWR initiator" example.
* As the sequence number field of the frame is not used in this example, it can be zeroed to ease the validation of the frame. */
- if (rx_buffer[MESSAGE_TYPE_IDX] == FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,4)&&!memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,4)) //判断是否为Final包
+ if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,4)&&!memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,4)) //判断是否为Final包
{
uint32_t poll_tx_ts, resp_rx_ts, final_tx_ts;
uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
@@ -615,29 +621,24 @@
/*--------------------------以下为非测距逻辑------------------------*/
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]<15000||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<1000000&&hex_dist>-10000)
- {
- if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
- {
- if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])<10000)
- {
- g_Tagdist[tag_id_recv-TAG_ID_START] = hex_dist;
- anchor_dist_last_frm[tag_id_recv-TAG_ID_START] = hex_dist;
-
+ {
#ifdef TDFILTER
- TrackingDiffUpdate(pUWBDistanceTrackingDiff, (float)hex_dist);
- filter_dist=pUWBDistanceTrackingDiff->pos_predict/10;
+ 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]= anchor_dist_last_frm[tag_id_recv-TAG_ID_START];
- }
+ 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])
@@ -656,16 +657,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