From d67d922c6345042cb3dfaedd621a2120fe808b70 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期五, 08 三月 2024 10:21:12 +0800
Subject: [PATCH] v1.9 增加蜂鸣，测试1标签对10基站稳定
---
Src/application/dw_ancapp.c | 380 +++++++++++++++++++++++++++---------------------------
1 files changed, 191 insertions(+), 189 deletions(-)
diff --git a/Src/application/dw_ancapp.c b/Src/application/dw_ancapp.c
index 3b0a160..da3d62e 100644
--- a/Src/application/dw_ancapp.c
+++ b/Src/application/dw_ancapp.c
@@ -83,18 +83,18 @@
extern uint16_t g_Resttimer;
static int32_t filter_dist,g_Tagdist[TAG_NUM_IN_SYS];
double LOS(dwt_rxdiag_t *dia) {
- F1 = dia->firstPathAmp1;
- F2 = dia->firstPathAmp2;
- F3 = dia->firstPathAmp3;
- N = dia->rxPreamCount;
- C = dia->maxGrowthCIR;
-
+ 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));
+// min_power = - 10 * log10((F1 *F1 + F2 * F2 + F3 * F3) / (C *B));
return min_power;
- }
+}
static uint64_t get_tx_timestamp_u64(void)
{
uint8_t ts_tab[5];
@@ -144,26 +144,26 @@
}
void TagListUpdate(void)
{
- uint16_t i,j=0,temp[TAG_NUM_IN_SYS];
- for(i=0;i<taglist_num;i++)
- {
- if(tagofflinetime[i]++<QUIT_SLOT_TIME)
- {
- tagid_list[j]=tagid_list[i];
- tagofflinetime[j++]=tagofflinetime[i];
- }
- }
- taglist_num=j;
+ uint16_t i,j=0,temp[TAG_NUM_IN_SYS];
+ for(i=0; i<taglist_num; i++)
+ {
+ if(tagofflinetime[i]++<QUIT_SLOT_TIME)
+ {
+ tagid_list[j]=tagid_list[i];
+ tagofflinetime[j++]=tagofflinetime[i];
+ }
+ }
+ taglist_num=j;
}
uint16_t CmpTagInList(uint16_t tagid)
-{uint16_t i;
- for(i=0;i<taglist_num;i++)
- {
- if(memcmp(&tagid,&tagid_list[i],2)==0)
- break;
- }
+{ uint16_t i;
+ for(i=0; i<taglist_num; i++)
+ {
+ if(memcmp(&tagid,&tagid_list[i],2)==0)
+ break;
+ }
tagofflinetime[i] = 0;
- return i;
+ return i;
}
static void resp_msg_set_ts(uint8_t *ts_field, const uint64_t ts)
{
@@ -185,44 +185,44 @@
{
uint8_t motorstate;
// tmp_time=TIM3->CNT;
- temp_nearbaspos = ancrec_nearbasepos;
- memcpy(&send_buffer[ANCHOR_ID_IDX],&dev_id,2);
- memcpy(&send_buffer[ANCTIMEMS],&basesystimer,2);
+ temp_nearbaspos = ancrec_nearbasepos;
+ memcpy(&send_buffer[ANCHOR_ID_IDX],&dev_id,2);
+ memcpy(&send_buffer[ANCTIMEMS],&basesystimer,2);
// memcpy(&send_buffer[ANCTIMEUS],&tmp_time,2);
- send_buffer[TAGFREQ_IDX] = tag_frequency;
- memcpy(&send_buffer[TAG_ID_IDX],&tag_id_recv,2);
- poll_rx_ts = get_rx_timestamp_u64();//获得Poll包接收时间T2
+ send_buffer[TAGFREQ_IDX] = tag_frequency;
+ memcpy(&send_buffer[TAG_ID_IDX],&tag_id_recv,2);
+ poll_rx_ts = get_rx_timestamp_u64();//获得Poll包接收时间T2
if(rec_nearbase_num == ancrec_nearbasepos)//抢占模式
{
random_time = (GetLPTime()%100);
- resp_tx_time = (poll_rx_ts+((/*value+*/random_time+rec_nearbase_num*20 + POLL_RX_TO_RESP_TX_DLY_UUS+ancrec_nearbasepos*DELAY_BETWEEN_TWO_FRAME_UUS) * UUS_TO_DWT_TIME)) >> 8;//计算Response发送时间T3。
-
- }else{
- resp_tx_time = (poll_rx_ts + ((/*value+ */rec_nearbase_num*20+POLL_RX_TO_RESP_TX_DLY_UUS+ancrec_nearbasepos*DELAY_BETWEEN_TWO_FRAME_UUS) * UUS_TO_DWT_TIME)) >> 8;//计算Response发送时间T3。
+ resp_tx_time = (poll_rx_ts+((/*value+*/random_time+rec_nearbase_num*20 + POLL_RX_TO_RESP_TX_DLY_UUS+ancrec_nearbasepos*DELAY_BETWEEN_TWO_FRAME_UUS) * UUS_TO_DWT_TIME)) >> 8;//计算Response发送时间T3。
+
+ } else {
+ resp_tx_time = (poll_rx_ts + ((/*value+ */rec_nearbase_num*20+POLL_RX_TO_RESP_TX_DLY_UUS+ancrec_nearbasepos*DELAY_BETWEEN_TWO_FRAME_UUS) * UUS_TO_DWT_TIME)) >> 8;//计算Response发送时间T3。
}
- 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);//接收超时时间
- resp_tx_ts = (((uint64_t)(resp_tx_time & 0xFFFFFFFEUL)) << 8) ;
+ 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);//接收超时时间
+ resp_tx_ts = (((uint64_t)(resp_tx_time & 0xFFFFFFFEUL)) << 8) ;
- /* Write all timestamps in the final message. See NOTE 8 below. */
- resp_msg_set_ts(&send_buffer[RESP_MSG_POLL_RX_TS_IDX], poll_rx_ts);
- resp_msg_set_ts(&send_buffer[RESP_MSG_RESP_TX_TS_IDX], resp_tx_ts);
- memcpy(&send_buffer[RESP_MSG_ANC_DISTOFFSET],&g_com_map[DIST_OFFSET],2);
+ /* Write all timestamps in the final message. See NOTE 8 below. */
+ resp_msg_set_ts(&send_buffer[RESP_MSG_POLL_RX_TS_IDX], poll_rx_ts);
+ resp_msg_set_ts(&send_buffer[RESP_MSG_RESP_TX_TS_IDX], resp_tx_ts);
+ memcpy(&send_buffer[RESP_MSG_ANC_DISTOFFSET],&g_com_map[DIST_OFFSET],2);
- send_buffer[GROUP_ID_IDX] = group_id;
+ send_buffer[GROUP_ID_IDX] = group_id;
// send_buffer[MAINBASE_INDEX]=flag_syncbase;
- send_buffer[MESSAGE_TYPE_IDX]=MBX_RESPONSE;
- send_buffer[MOTORSTATE_INDEX]=motorstate;//(remotesend_state<<4)|motorstate;
- send_buffer[MOTORSTATE_INDEX]&=0x0f;
- remotesend_state = 0;
-
- dwt_forcetrxoff();
- dwt_writetxdata(38, send_buffer, 0);//写入发送数据
- dwt_writetxfctrl(38, 0);//设定发送长度
- result = dwt_starttx(DWT_START_TX_DELAYED);
+ send_buffer[MESSAGE_TYPE_IDX]=MBX_RESPONSE;
+ send_buffer[MOTORSTATE_INDEX]=motorstate;//(remotesend_state<<4)|motorstate;
+ send_buffer[MOTORSTATE_INDEX]&=0x0f;
+ remotesend_state = 0;
+
+ dwt_forcetrxoff();
+ dwt_writetxdata(38, send_buffer, 0);//写入发送数据
+ dwt_writetxfctrl(38, 0);//设定发送长度
+ result = dwt_starttx(DWT_START_TX_DELAYED);
// if(remotesend_state==0)
// {
// dwt_writetxdata(38, send_buffer, 0);//写入发送数据
@@ -231,18 +231,18 @@
//// uwb_timeout = 0;
// if(result == 0)
// {
- uwb_timeout=lp_time;
- while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & SYS_STATUS_TXFRS ))///不断查询芯片状态直到接收成功或者出现错误
- {
+ uwb_timeout=lp_time;
+ while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & SYS_STATUS_TXFRS ))///不断查询芯片状态直到接收成功或者出现错误
+ {
// current_time=HAL_LPTIM_ReadCounter(&hlptim1);
// if(current_time>end_time+((32768/1000)*3)||current_time<end_time+15000)
-// {
+// {
//// printf("debug2 timec:%d timee:%d\r\n",current_time,end_time);
// break;}
- if(lp_time-uwb_timeout>2)
- break;
- };
+ if(lp_time-uwb_timeout>2)
+ break;
+ };
// }
// dwt_rxenable(0);
@@ -257,52 +257,54 @@
uint16_t RX_TIMEOUTs_UWB;
void Anchor_App(void)
{
-
- u8 send_len,i;
- u16 tempid;
- uint32_t rec_syncid;
- uint16_t checksum;
- uint8_t kk;
-
+
+ u8 send_len,i;
+ u16 tempid;
+ uint32_t rec_syncid;
+ uint16_t checksum;
+ uint8_t kk;
+
dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间
- dwt_rxenable(0);//打开接收
-
-
-
+ dwt_rxenable(0);//打开接收
+
+
+
start_time=HAL_LPTIM_ReadCounter(&hlptim1);
- end_time=start_time+((32768/1000)*2);
- if(end_time>=32768)
- {end_time-=32768;}
- current_time=HAL_LPTIM_ReadCounter(&hlptim1);
+ end_time=start_time+((32768/1000)*2);
+ if(end_time>=32768)
+ {
+ end_time-=32768;
+ }
+ current_time=HAL_LPTIM_ReadCounter(&hlptim1);
while(current_time<end_time||current_time>end_time+15000)
+ {
+ current_time=HAL_LPTIM_ReadCounter(&hlptim1);
+ while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到接收成功或者出现错误
{
current_time=HAL_LPTIM_ReadCounter(&hlptim1);
- while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到接收成功或者出现错误
- {
- current_time=HAL_LPTIM_ReadCounter(&hlptim1);
- if(current_time>=end_time&¤t_time<end_time+15000)
- break;
- };
-
- if (status_reg & SYS_STATUS_RXFCG && status_reg!=0xffffffff)//成功接收
- {
+ if(current_time>=end_time&¤t_time<end_time+15000)
+ break;
+ };
+
+ if (status_reg & SYS_STATUS_RXFCG && status_reg!=0xffffffff)//成功接收
+ {
uint16_t tag_recv_interval;
float temp_tagpos;
-
- dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
- frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//获得接收数据长度
- recpoll_len = frame_len;
- 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);
- memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
- //将收到的tag_id分别写入各次通讯的包中，为多标签通讯服务，防止一次通讯中接收到不同ID标签的数据
- memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
+
+ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
+ frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//获得接收数据长度
+ recpoll_len = frame_len;
+ 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);
+ memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
+ //将收到的tag_id分别写入各次通讯的包中，为多标签通讯服务，防止一次通讯中接收到不同ID标签的数据
+ memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
// HAL_GPIO_WritePin(GPIOA, GPIO_PIN_15, GPIO_PIN_SET);
-// printf("ID %x\r\n",tag_id_recv);
- switch(rx_buffer[MESSAGE_TYPE_IDX])
- {uint8_t target_tagi;
+// printf("ID %x\r\n",tag_id_recv);
+ switch(rx_buffer[MESSAGE_TYPE_IDX])
+ { uint8_t target_tagi;
case MBX_REG:
// dwt_readdiagnostics(&d1);
// LOS(&d1);
@@ -317,56 +319,56 @@
// memcpy(&usart_send[22],&checksum,2);
// HexToAsciiSendUDP(usart_send,24);
break;
- case MBX_POLL:
- if(group_id==rx_buffer[GROUP_ID_IDX])
- {
-// printf("timec:%d timee:%d\r\n",current_time,end_time);
- dwt_readdiagnostics(&d1);
- LOS(&d1);
- if(firstpath_power<-100)
+ case MBX_POLL:
+ if(group_id==rx_buffer[GROUP_ID_IDX])
{
- break;
- }
- frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
- battary = rx_buffer[BATTARY_IDX];
- button = rx_buffer[BUTTON_IDX];
- rec_nearbase_num=rx_buffer[FZ_NEARBASENUM_INDEX];
- memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
- memcpy(&rec_ancidlist,&rx_buffer[FZ_NEARBASEID_INDEX],2*rec_nearbase_num);
- memcpy(&rec_ancdistlist,&rx_buffer[FZ_NEARBASEID_INDEX+2*rec_nearbase_num],2*rec_nearbase_num);
- memcpy(&rec_antdelay,&rx_buffer[FZ_NEARBASEID_INDEX+rec_nearbase_num*4],2);
- uwb_losttimer = 0;
+// printf("timec:%d timee:%d\r\n",current_time,end_time);
+ dwt_readdiagnostics(&d1);
+ LOS(&d1);
+ if(firstpath_power<-100)
+ {
+ break;
+ }
+ frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
+ battary = rx_buffer[BATTARY_IDX];
+ button = rx_buffer[BUTTON_IDX];
+ rec_nearbase_num=rx_buffer[FZ_NEARBASENUM_INDEX];
+ memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
+ memcpy(&rec_ancidlist,&rx_buffer[FZ_NEARBASEID_INDEX],2*rec_nearbase_num);
+ memcpy(&rec_ancdistlist,&rx_buffer[FZ_NEARBASEID_INDEX+2*rec_nearbase_num],2*rec_nearbase_num);
+ memcpy(&rec_antdelay,&rx_buffer[FZ_NEARBASEID_INDEX+rec_nearbase_num*4],2);
+ uwb_losttimer = 0;
// wltag_state=RANGE;
- for(u8 i=0;i<rec_nearbase_num;i++)
- {
- if(rec_ancidlist[i] == dev_id)
- {
- for(kk=0;kk<Lora_tx_ancnum;kk++)
+ for(u8 i=0; i<rec_nearbase_num; i++)
+ {
+ if(rec_ancidlist[i] == dev_id)
{
- if(Lora_tx_ancid[kk]==tag_id_recv)
- {
+ for(kk=0; kk<Lora_tx_ancnum; kk++)
+ {
+ if(Lora_tx_ancid[kk]==tag_id_recv)
+ {
// Lora_tx_ancnum--;
- Lora_tx_ancdist[kk]=rec_ancdistlist[i];
- Lora_tx_anc_electricity[kk]=battary;
- break_flag=1;
- break;
- }
- }
- if(break_flag)
- {
- break_flag=0;
- break;
- }
- Lora_tx_ancid[Lora_tx_ancnum]=tag_id_recv;
- Lora_tx_ancdist[Lora_tx_ancnum]=rec_ancdistlist[i];
- Lora_tx_anc_electricity[Lora_tx_ancnum]=battary;
+ Lora_tx_ancdist[kk]=rec_ancdistlist[i];
+ Lora_tx_anc_electricity[kk]=battary;
+ break_flag=1;
+ break;
+ }
+ }
+ if(break_flag)
+ {
+ break_flag=0;
+ break;
+ }
+ Lora_tx_ancid[Lora_tx_ancnum]=tag_id_recv;
+ Lora_tx_ancdist[Lora_tx_ancnum]=rec_ancdistlist[i];
+ Lora_tx_anc_electricity[Lora_tx_ancnum]=battary;
// printf("接收到uwb测距:%d",Lora_tx_ancdist[0]);
- Lora_tx_ancnum++;
- RX_TIMEOUTs_UWB++;
- }
- }
+ Lora_tx_ancnum++;
+ RX_TIMEOUTs_UWB++;
+ }
+ }
//// #ifdef USART_ALL_OUTPUT
-// usart_send[2] = 0x12;//正常模式
+// usart_send[2] = 0x12;//正常模式
// usart_send[3] = 8+rec_nearbase_num*4;//数据段长度
// memcpy(&usart_send[4],&tag_id_recv,2);
// usart_send[6] = frame_seq_nb2;//包序
@@ -376,64 +378,64 @@
// usart_send[8] = lora_yingda_flag;//lora等待应答位
// usart_send[9] = rec_nearbase_num;//lora等待应答位
//// usart_send[9] = lora_yingda_flag;
-// memcpy(&usart_send[10],&rec_ancidlist,2*rec_nearbase_num);
+// memcpy(&usart_send[10],&rec_ancidlist,2*rec_nearbase_num);
// memcpy(&usart_send[10+rec_nearbase_num*2],&rec_ancdistlist,2*rec_nearbase_num);
// checksum = Checksum_u16(&usart_send[2],8+4*rec_nearbase_num);
// memcpy(&usart_send[10+4*rec_nearbase_num],&checksum,2);
//// UART_PushFrame(usart_send,12+rec_nearbase_num*4);
//// #endif
- taglist_pos=CmpTagInList(tag_id_recv);
- if(taglist_num>=TAG_NUM_IN_SYS)
- break;
- if(taglist_pos==taglist_num)
- {
- taglist_pos=taglist_num;
- tagid_list[taglist_num++]=tag_id_recv;
-
- new_tagid=1;
- }else{
- new_tagid=0;
- }
- temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);
- tagpos_rec[(uint8_t)temp_tagpos]=1;
-
-
- if(rec_nearbase_num>ancidlist_num)
- {
- ancidlist_num=rec_nearbase_num;
- memcpy(ancidlist_rec,&rx_buffer[FZ_NEARBASEID_INDEX],rec_nearbase_num*2);
- }
-
- for(i=0;i<rec_nearbase_num;i++)
- {
- memcpy(&tempid,&rx_buffer[FZ_NEARBASEID_INDEX+i*2],2);
- if(tempid==dev_id)
- {
- seize_anchor=0; //非抢占。已存在列表中
- Anchor_RecNearPoll(i);
- break;
- }
- }
-
- if(i==rec_nearbase_num)
- {
- seize_anchor=1; //抢占anchor
- Anchor_RecNearPoll(i);
- }
+ taglist_pos=CmpTagInList(tag_id_recv);
+ if(taglist_num>=TAG_NUM_IN_SYS)
+ break;
+ if(taglist_pos==taglist_num)
+ {
+ taglist_pos=taglist_num;
+ tagid_list[taglist_num++]=tag_id_recv;
+
+ new_tagid=1;
+ } else {
+ new_tagid=0;
+ }
+ temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);
+ tagpos_rec[(uint8_t)temp_tagpos]=1;
+
+
+ if(rec_nearbase_num>ancidlist_num)
+ {
+ ancidlist_num=rec_nearbase_num;
+ memcpy(ancidlist_rec,&rx_buffer[FZ_NEARBASEID_INDEX],rec_nearbase_num*2);
+ }
+
+ for(i=0; i<rec_nearbase_num; i++)
+ {
+ memcpy(&tempid,&rx_buffer[FZ_NEARBASEID_INDEX+i*2],2);
+ if(tempid==dev_id)
+ {
+ seize_anchor=0; //非抢占。已存在列表中
+ Anchor_RecNearPoll(i);
+ break;
+ }
+ }
+
+ if(i==rec_nearbase_num)
+ {
+ seize_anchor=1; //抢占anchor
+ Anchor_RecNearPoll(i);
+ }
}
-
- break;
- }
-
- }else{
+
+ break;
+ }
+
+ } else {
// printf("debug4 timec:%d timee:%d\r\n",current_time,end_time);
- dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
- dwt_rxenable(0);
+ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
+ dwt_rxenable(0);
}
}
- dwt_forcetrxoff();
- dwt_entersleep();
+ dwt_forcetrxoff();
+ dwt_entersleep();
}
--
Gitblit v1.9.3