From 66b13717fce3890b464d0b43eb61b94ea7cdf56e Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期二, 28 九月 2021 17:09:23 +0800
Subject: [PATCH] V2.30
---
源码/核心板/Src/application/dw_app.c | 232 +++++++++++++++++++++++++++++++++++++++++++++++++--------
1 files changed, 198 insertions(+), 34 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 088b3aa..c34e398 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"
@@ -47,7 +47,7 @@
uint32_t g_UWB_com_interval = 0;
float dis_after_filter; //当前距离值
LPFilter_Frac* p_Dis_Filter; //测距用的低通滤波器
-uint16_t g_Tagdist[TAG_NUM_IN_SYS];
+int32_t g_Tagdist[TAG_NUM_IN_SYS];
uint8_t g_flag_Taggetdist[256];
static uint64_t get_tx_timestamp_u64(void)
@@ -97,6 +97,7 @@
*ts += ts_field[i] << (i * 8);
}
}
+extern int32_t tagdist_list[TAG_NUM_IN_SYS];
void TagDistClear(void)
{
static uint16_t clear_judge_cnt;
@@ -109,7 +110,7 @@
g_flag_Taggetdist[i]++;
if(g_flag_Taggetdist[i]>=2)
{
- g_Tagdist[i]=0xffff;
+ tagdist_list[i]=0x1ffff;
}
}
}
@@ -165,7 +166,7 @@
}
u16 tag_time_recv[TAG_NUM_IN_SYS];
-u8 usart_send[100],usart_send_anc[100];
+u8 usart_send[150],usart_send_anc[100];
u8 battary,button,tag_frequency,tag_slotpos;
extern uint8_t g_pairstart;
void tag_sleep_configuraion(void)
@@ -839,6 +840,7 @@
}
return i;
}
+u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse,motorstate=0;
uint32_t frame_len;
uint32_t resp_tx_time;
uint8_t rec_nearbase_num,anc_report_num;
@@ -863,8 +865,7 @@
dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//设定发送长度
result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送，等待接收
- battary = rx_buffer[BATTARY_IDX];
- button = rx_buffer[BUTTON_IDX];
+
frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
if(result==0)
{
@@ -905,11 +906,44 @@
LED0_BLINK; //每成功一次通讯则闪烁一次
dis_after_filter=dist_cm;
hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
- if(abs(hex_dist-his_dist[taglist_pos])<1000)
- {
- tagdist_list[taglist_pos] = hex_dist;
- }
- his_dist[taglist_pos]=hex_dist;
+if(hex_dist>-1000&&hex_dist<200000)
+ {
+ g_flag_Taggetdist[taglist_pos]=0;
+ if(abs(hex_dist-his_dist[taglist_pos])<1500||misdist_num[taglist_pos]>3)
+ {
+ misdist_num[taglist_pos]=0;
+ tagdist_list[taglist_pos] = hex_dist;
+ his_dist[taglist_pos]=hex_dist;
+ g_Tagdist[taglist_pos]=hex_dist;
+ #ifndef USART_INTEGRATE_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[9],&tagdist_list[taglist_pos],4);
+ usart_send[13] = battary;
+ usart_send[14] = button;
+ usart_send[15] = firstpath_power;
+ usart_send[16] = (rx_power-firstpath_power)*10;
+ usart_send[17] = tag_frequency;
+ usart_send[18] = tag_slotpos;
+ checksum = Checksum_u16(&usart_send[2],17);
+ memcpy(&usart_send[19],&checksum,2);
+ UART_PushFrame(usart_send,21);
+ #else
+ 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
+ #ifdef DEBUG_INF
+ printf("序号：%d\r\n",frame_seq_nb2);
+ #endif
+ }else{
+ // printf("%d",hex_dist);
+ misdist_num[taglist_pos]++;
+ }
+ }
}
}else{
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
@@ -919,7 +953,7 @@
extern uint16_t configremotetagID;
extern u8 remotetag_paralen;
u8 finalsend=0;
-u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse,motorstate=0;
+u16 signalpower_list[TAG_NUM_IN_SYS];
u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase 1 first near_base
{
@@ -935,15 +969,14 @@
dwt_setdelayedtrxtime(resp_tx_time);//设置Response发送时间T3
dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS+(rec_nearbase_num+1-ancrec_nearbasepos)*DELAY_BETWEEN_TWO_FRAME_UUS);//设置发送完成后开启接收延迟时间
dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//接收超时时间
- dwt_readdiagnostics(&d1);
+ //dwt_readdiagnostics(&d1);
if(new_tagid)
{
- tagdist_list[taglist_pos]=0x1ffff;
- memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
- }else{
- memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
+ tagdist_list[taglist_pos]=0x1ffff;
}
+ memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
+ memcpy(&tx_nearresp_msg[ANC_SIGNALPOWER],&signalpower_list[taglist_pos],2);
tx_nearresp_msg[MAINBASE_INDEX]=flag_syncbase;
tx_nearresp_msg[MESSAGE_TYPE_IDX]=NEAR_RESPONSE;
tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)|motorstate;
@@ -970,12 +1003,15 @@
break;
}
}
+ }else{
+
}
if(finalsend == 0)
{
- dwt_writetxdata(24, tx_nearresp_msg, 0);//写入发送数据
- dwt_writetxfctrl(24, 0);//设定发送长度
+ memcpy(&tx_nearresp_msg[MAXRANGE_DISTANCE],&g_com_map[MAXRANGDIST_INDEX],2);
+ dwt_writetxdata(28, tx_nearresp_msg, 0);//写入发送数据
+ dwt_writetxfctrl(28, 0);//设定发送长度
}
// if(remotesend_state&&tag_id_recv==configremotetagID)
@@ -988,11 +1024,6 @@
result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送，等待接收
- battary = rx_buffer[BATTARY_IDX];
- button = rx_buffer[BUTTON_IDX];
- frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
- tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
- tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];
if(result==0)
{
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))&&!g_start_sync_flag)///不断查询芯片状态直到接收成功或者出现错误
@@ -1050,15 +1081,17 @@
LED0_BLINK; //每成功一次通讯则闪烁一次
dis_after_filter=dist_cm;
hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
- g_flag_Taggetdist[taglist_pos]=0;
+
if(hex_dist>-1000&&hex_dist<200000)
{
+ g_flag_Taggetdist[taglist_pos]=0;
if(abs(hex_dist-his_dist[taglist_pos])<1500||misdist_num[taglist_pos]>3)
{
misdist_num[taglist_pos]=0;
tagdist_list[taglist_pos] = hex_dist;
his_dist[taglist_pos]=hex_dist;
- g_Tagdist[taglist_pos]=hex_dist;
+ g_Tagdist[taglist_pos]=hex_dist;
+ signalpower_list[taglist_pos] = ((uint8_t)firstpath_power<<8)|(uint8_t)((rx_power-firstpath_power)*10);
#ifndef USART_INTEGRATE_OUTPUT
usart_send[2] = 1;//正常模式
usart_send[3] = 17;//数据段长度
@@ -1074,7 +1107,7 @@
usart_send[18] = tag_slotpos;
checksum = Checksum_u16(&usart_send[2],17);
memcpy(&usart_send[19],&checksum,2);
- UART_PushFrame(usart_send,21);
+ // UART_PushFrame(usart_send,21);
#else
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);
@@ -1108,12 +1141,15 @@
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
-
+extern int32_t intheight;
extern u16 synclost_count;
uint32_t current_syncid=0xffffffff,synclost_timer;
extern u8 flag_syncbase,waitsync_flag;
-u8 tagpos_rec[50],tagpos_send[50],ancidlist_num;
+u8 tagpos_rec[50],tagpos_send[50],ancidlist_num,zero_matrix[100]={0};
u16 ancidlist_rec[20],ancidlist_send[20];
+u16 recnearbaselist_id[30],rec_taganc_signalpower_list[11];
+int32_t recnearbaselist_dist[30];
+int16_t rec_tagheight;
void Anchor_App(void)
{
@@ -1146,9 +1182,93 @@
switch(rx_buffer[MESSAGE_TYPE_IDX])
{
uint16_t checksum;
- case POLL:
- if (anchor_type == rx_buffer[ANC_TYPE_IDX])
+ case DISCOVERPOLL:
+ if (anchor_type == rx_buffer[ANCHOR_ID_IDX])
+ {
+ taglist_pos=CmpTagInList(tag_id_recv);
+ if(taglist_pos==taglist_num)
+ {
+ taglist_pos=taglist_num;
+ tagid_list[taglist_num++]=tag_id_recv;
+
+ new_tagid=1;
+ }else{
+ new_tagid=0;
+ }
+ tagofflinetime[taglist_pos]=0;
+
+ rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
+ if(rec_nearbase_num>=30)
+ {
+ break;
+ }
+ battary = rx_buffer[BATTARY_IDX];
+ button = rx_buffer[BUTTON_IDX];
+ frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
+ tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
+ tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];
+
+ ////////////////基站汇总模式数据
+ memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6],2);
+ memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
+ memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*2],rec_nearbase_num*4);
Anchor_RecPoll();
+ }
+ break;
+ case SPOLL:
+ //if (anchor_type == rx_buffer[ANC_TYPE_IDX])
+ if(dev_id==anc_id_recv)
+ {
+ taglist_pos=CmpTagInList(tag_id_recv);
+ if(taglist_pos==taglist_num)
+ {
+ taglist_pos=taglist_num;
+ tagid_list[taglist_num++]=tag_id_recv;
+
+ new_tagid=1;
+ }else{
+ new_tagid=0;
+ }
+ tagofflinetime[taglist_pos]=0;
+
+ rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
+ if(rec_nearbase_num>=30)
+ {
+ break;
+ }
+ battary = rx_buffer[BATTARY_IDX];
+ button = rx_buffer[BUTTON_IDX];
+ frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
+ tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
+ tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];
+
+ ////////////////基站汇总模式数据
+ memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6],2);
+ memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
+ memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*2],rec_nearbase_num*4);
+ usart_send[2] = 0x0c;//正常模式
+ usart_send[3] = 15+8*(rec_nearbase_num);//数据段长度
+ memcpy(&usart_send[4],&tag_id_recv,2);
+ usart_send[6] = rx_buffer[SEQUENCE_IDX];
+ usart_send[7] = rx_buffer[SEQUENCEH_IDX];
+ usart_send[8] = battary;
+ usart_send[9] = button;
+ memcpy(&usart_send[10],&rec_tagheight,2);
+ usart_send[12] = tag_frequency;
+ usart_send[13] = tag_slotpos;
+ usart_send[14] = 0;
+ usart_send[15] = 0;
+ usart_send[16] = rec_nearbase_num;
+
+ memcpy(&usart_send[17],&recnearbaselist_id,2*rec_nearbase_num);
+ memcpy(&usart_send[17+rec_nearbase_num*2],&recnearbaselist_dist,4*rec_nearbase_num);
+ memcpy(&usart_send[17+rec_nearbase_num*6],zero_matrix,2*rec_nearbase_num);
+
+ checksum = Checksum_u16(&usart_send[2],15+8*rec_nearbase_num);
+ memcpy(&usart_send[17+8*rec_nearbase_num],&checksum,2);
+ UART_PushFrame(usart_send,19+8*rec_nearbase_num);
+ Anchor_RecPoll();
+ }
break;
case SYNC:
memcpy(&rec_syncid,&rx_buffer[ANCHOR_ID_IDX],4);
@@ -1212,13 +1332,15 @@
memcpy(&usart_send[6],&tag_id_recv,2);
memcpy(&usart_send[8],&rx_buffer[10],9);
usart_send[17] = firstpath_power;
+ memcpy(&usart_send[18],&rx_buffer[19],2);
checksum = Checksum_u16(&usart_send[2],20);
memcpy(&usart_send[22],&checksum,2);
UART_PushFrame(usart_send,24);
}
break;
case NEAR_POLL:
- // GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);
+ // GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);
+
memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
taglist_pos=CmpTagInList(tag_id_recv);
if(taglist_pos==taglist_num)
@@ -1231,10 +1353,51 @@
new_tagid=0;
}
tagofflinetime[taglist_pos]=0;
- temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);
+ temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);
tagpos_rec[(u8)temp_tagpos]=1;
rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
-
+ if(rec_nearbase_num>30)
+ {
+ break;
+ }
+ battary = rx_buffer[BATTARY_IDX];
+ button = rx_buffer[BUTTON_IDX];
+ frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
+ tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
+ tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];
+ ////////////////基站汇总模式数据
+ memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*8+6],2);
+ memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
+ memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*2],rec_nearbase_num*4+4);
+ memcpy(&rec_taganc_signalpower_list,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6+4],rec_nearbase_num*2+2);
+ if(anc_id_recv == dev_id )
+ {
+ usart_send[2] = 0x0c;//正常模式
+ usart_send[3] = 15+8*(rec_nearbase_num+1);//数据段长度
+ memcpy(&usart_send[4],&tag_id_recv,2);
+ usart_send[6] = rx_buffer[SEQUENCE_IDX];
+ usart_send[7] = rx_buffer[SEQUENCEH_IDX];
+ usart_send[8] = battary;
+ usart_send[9] = button;
+
+ memcpy(&usart_send[10],&rec_tagheight,2);
+ usart_send[12] = tag_frequency;
+ usart_send[13] = tag_slotpos;
+ usart_send[14] = 0;
+ usart_send[15] = 0;
+ usart_send[16] = rec_nearbase_num+1;
+ memcpy(&usart_send[17],&anc_id_recv,2);
+ memcpy(&usart_send[19],&recnearbaselist_id,2*rec_nearbase_num);
+ memcpy(&usart_send[19+rec_nearbase_num*2],&recnearbaselist_dist,4*rec_nearbase_num+4);
+ memcpy(&usart_send[19+rec_nearbase_num*6+4],&rec_taganc_signalpower_list,2*rec_nearbase_num+2);
+
+ checksum = Checksum_u16(&usart_send[2],23+8*rec_nearbase_num);
+ memcpy(&usart_send[25+8*rec_nearbase_num],&checksum,2);
+ UART_PushFrame(usart_send,27+8*rec_nearbase_num);
+ }
+ //////////////////////////////////////////////////////////////
+
+
GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);
if(anc_id_recv==dev_id)
{
@@ -1259,7 +1422,8 @@
// seize_anchor=1; //抢占anchor
// Anchor_RecNearPoll(i);
// }
-
+// if(tag_id_recv==0x4008)
+// printf("标签高度: %d,基站高度: %d,高度差： %d. \r\n",rec_tagheight,intheight,intheight-rec_tagheight);
break;
default:
--
Gitblit v1.9.3