From 54a0b191c9e60a63562bff03b73d04c707be46e3 Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期五, 17 九月 2021 15:52:08 +0800
Subject: [PATCH] V1.54 测试通过
---
源码/核心板/Src/application/dw_app.c | 34 +++++++++++++++++++++++++++-------
1 files changed, 27 insertions(+), 7 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 f4287d6..9293bfc 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"
@@ -137,6 +137,7 @@
* As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */
//设置接收超时时间
}
+extern u16 group_id;
void Dw1000_App_Init(void)
{
//g_com_map[DEV_ID] = 0x0b;
@@ -145,6 +146,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, 2);
memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 2);
memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 2);
@@ -333,6 +338,7 @@
last_nearbase_num=next_nearbase_num;
nearbase_num=next_nearbase_num;
recbase_num=0;
+ tx_nearpoll_msg[GROUP_ID_IDX] = group_id;
tx_nearpoll_msg[BATTARY_IDX] = Get_Battary();
tx_nearpoll_msg[BUTTON_IDX] = !READ_KEY0;
tx_nearpoll_msg[SEQUENCE_IDX] = frame_seq_nb++;
@@ -390,7 +396,7 @@
dwt_readrxdata(rx_buffer, frame_len, 0); //读取接收数据
dwt_setrxtimeout(0);//DELAY_BETWEEN_TWO_FRAME_UUS*(nearbase_num+1-recbase_num)+10);//设定接收超时时间，0位没有超时时间
dwt_rxenable(0);//打开接收
- if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
+ if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)&&rx_buffer[GROUP_ID_IDX] == group_id) //判断接收到的数据是否是response数据
{ u16 rec_nearbaseid,rec_nearbasepos;
int32_t temp_dist;
poll_tx_ts = get_tx_timestamp_u64(); //获得POLL发送时间T1
@@ -451,6 +457,7 @@
final_msg_set_ts(&tx_nearfinal_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX], resp_rx_ts);
final_msg_set_ts(&tx_nearfinal_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
tx_nearfinal_msg[MESSAGE_TYPE_IDX]=NEAR_FINAL;
+ tx_nearfinal_msg[GROUP_ID_IDX] = group_id;
dwt_writetxdata(28+nearbase_num*4, tx_nearfinal_msg, 0);//将发送数据写入DW1000
dwt_writetxfctrl(28+nearbase_num*4, 0);//设定发送数据长度
memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
@@ -831,6 +838,8 @@
}
extern u16 dist_threshold;
u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,getrange_success=0;
+int32_t filter_dist,filter_speed;
+u8 newmeasure;
u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase 1 first near_base
{
u8 motorstate;
@@ -866,6 +875,7 @@
}else{
memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
}
+ tx_nearresp_msg[GROUP_ID_IDX] = group_id;
tx_nearresp_msg[MAINBASE_INDEX]=flag_syncbase;
tx_nearresp_msg[MESSAGE_TYPE_IDX]=NEAR_RESPONSE;
tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)|motorstate;
@@ -899,7 +909,7 @@
{
return 1;
}
- if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,2)) //判断是否为Final包
+ if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,2)&&group_id==rx_buffer[GROUP_ID_IDX]) //判断是否为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;
@@ -931,16 +941,18 @@
dis_after_filter=dist_cm;
hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10;
g_flag_Taggetdist[taglist_pos]=0;
- if(hex_dist>0&&hex_dist<2000000)
+ if(hex_dist>-1000&&hex_dist<2000000)
{
if(abs(hex_dist-his_dist[taglist_pos])<dist_threshold||misdist_num[taglist_pos]>3)
{
- int32_t filter_dist;
+
g_Resttimer=0;
IWDG_Feed();
#ifdef TDFILTER
NewTrackingDiffUpdate(taglist_pos, (float)hex_dist);
- filter_dist=pos_predict[taglist_pos]/10;
+ filter_dist = pos_predict[taglist_pos]/10;
+ filter_speed = vel_predict[taglist_pos]/10;
+ newmeasure = 1;
#else
filter_dist=hex_dist/10;
#endif
@@ -948,12 +960,16 @@
tagdist_list[taglist_pos] = filter_dist;
his_dist[taglist_pos]=hex_dist;
g_Tagdist[taglist_pos]=filter_dist;
+ if(!g_com_map[MODBUS_MODE])
+ {
#ifdef USART_SINGLE_OUTPUT
usart_send[2] = 1;//正常模式
usart_send[3] = 17;//数据段长度
usart_send[4] = frame_seq_nb2;//数据段长度
memcpy(&usart_send[5],&tag_id_recv,2);
- memcpy(&usart_send[7],&dev_id,2);
+ memcpy(&usart_send[7],&dev_id,2);
+ if(tagdist_list[taglist_pos]<=0)
+ tagdist_list[taglist_pos]=10;
memcpy(&usart_send[9],&tagdist_list[taglist_pos],4);
usart_send[13] = battary;
usart_send[14] = button;
@@ -964,7 +980,8 @@
memcpy(&usart_send_anc[4+6*anc_report_num],&tag_id_recv,2);
memcpy(&usart_send_anc[6+6*anc_report_num],&tagdist_list[taglist_pos],4);
anc_report_num++;
- #endif
+ #endif
+ }
}else{
// printf("%d",hex_dist);
misdist_num[taglist_pos]++;
@@ -1010,6 +1027,8 @@
memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
//将收到的tag_id分别写入各次通讯的包中，为多标签通讯服务，防止一次通讯中接收到不同ID标签的数据
memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
+ if(group_id==rx_buffer[GROUP_ID_IDX])
+ {
switch(rx_buffer[MESSAGE_TYPE_IDX])
{
// case POLL:
@@ -1125,4 +1144,5 @@
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
+}
--
Gitblit v1.9.3