From a5e16e925923fa444276ca51f0e7c0cbbe07a96c Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期六, 27 五月 2023 15:51:54 +0800
Subject: [PATCH] V1.58 香港机场测试，有BUG
---
Src/application/dw_app.c | 107 ++++++++++++++++++++++++++---------------------------
1 files changed, 53 insertions(+), 54 deletions(-)
diff --git a/Src/application/dw_app.c b/Src/application/dw_app.c
index 646b349..7c6eb1f 100644
--- a/Src/application/dw_app.c
+++ b/Src/application/dw_app.c
@@ -73,14 +73,14 @@
static dwt_config_t config = {
5, /* Channel number. */
DWT_PRF_64M, /* Pulse repetition frequency. */
- DWT_PLEN_64, /* Preamble length. */
+ DWT_PLEN_128, /* Preamble length. */
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. */
1, /* Use non-standard SFD (Boolean) */
DWT_BR_6M8, /* Data rate. */
DWT_PHRMODE_STD, /* PHY header mode. */
- (65 + 8 - 8) /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
+ (129 + 8 - 8) /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
};
static uint8_t tx_poll_msg[20] = {0};
static uint8_t tx_sync_msg[14] = {0};
@@ -315,10 +315,11 @@
double rtd_init, rtd_resp;
double tof,distance;
extern int32_t dwt_readcarrierintegrator(void) ;
-uint32_t testtimer[10],testtimer2,anc_pollrx[11],anc_resptx[11],tag_resprx[11];
-int32_t anc_clockoffset[11];
-int16_t anc_distoffset[11];
+uint32_t testtimer2,anc_pollrx[MAX_NEARBASE_NUM],anc_resptx[MAX_NEARBASE_NUM],tag_resprx[MAX_NEARBASE_NUM];
+int32_t anc_clockoffset[MAX_NEARBASE_NUM];
+int16_t anc_distoffset[MAX_NEARBASE_NUM];
int32_t test2;
+uint16_t u16_nearbase_distlist[MAX_NEARBASE_NUM];
uint8_t get_newdist,notenoughdist_count;
void SetANCTimestap(uint8_t i,uint8_t* pollrx,uint8_t* resptx,uint32_t resprx,uint8_t* distoffset)
{
@@ -330,22 +331,22 @@
}
void CalculateDists(void)
{
- for(int i=0;i<11;i++)
+ for(int i=0;i<MAX_NEARBASE_NUM;i++)
{
//rec_anc_signalpower[i] = exsistbase_list[i];
- if(exsistbase_list[i]>0)
+ if(exsistbase_list[i]==KEEP_TIMES)
{
- exsistbase_list[i]--;
+ // exsistbase_list[i]--;
clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_5 / 1.0e6) ;
rtd_init = tag_resprx[i] - poll_tx_ts;
rtd_resp = anc_resptx[i] - anc_pollrx[i];
tof = ((rtd_init - rtd_resp * (1 - clockOffsetRatio)) / 2.0) * DWT_TIME_UNITS;
distance = tof * SPEED_OF_LIGHT;
- if(distance>-1000&&distance<100000)
+ if(distance>-10&&distance<1000)
nearbase_distlist[i] = distance*100+anc_distoffset[i];
}else{
- nearbase_distlist[i] = 0x1ffff;
+ // nearbase_distlist[i] = 0x1ffff;
}
}
@@ -361,25 +362,29 @@
u32 start_poll;
u8 i,j,getsync_flag=0,timeout,flag_ancreadpara=0;
dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); //设置发送后开启接收，并设定延迟时间
- dwt_setrxtimeout(10000);
+ dwt_setrxtimeout(0);
tag_succ_times = 0;
//GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_SET);
- if(next_nearbase_num>=MAX_NEARBASE_NUM)
+ if(nearbase_num>=MAX_NEARBASE_NUM)
{
- next_nearbase_num = MAX_NEARBASE_NUM-1;
+ nearbase_num = MAX_NEARBASE_NUM-1;
}
- HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_SET);
- last_nearbase_num=next_nearbase_num;
- nearbase_num=next_nearbase_num;
+// HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_SET);
+// last_nearbase_num=next_nearbase_num;
+// nearbase_num=next_nearbase_num;
recbase_num=0;
// motor_state=0;
+ for(uint8_t i=0;i<nearbase_num;i++)
+ {
+ u16_nearbase_distlist[i] = nearbase_distlist[i];
+ }
tx_near_msg[GROUP_ID_IDX] = group_id;
tx_near_msg[BATTARY_IDX] = bat_percent;
tx_near_msg[BUTTON_IDX] = !GET_USERKEY|stationary_flag<<1;
tx_near_msg[SEQUENCE_IDX] = frame_seq_nb++;
tx_near_msg[NEARBASENUM_INDEX] = nearbase_num;
memcpy(&tx_near_msg[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2);
- memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*2],&nearbase_distlist,nearbase_num*2);
+ memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*2],&u16_nearbase_distlist,nearbase_num*2);
tx_near_msg[MESSAGE_TYPE_IDX] = MBX_POLL;
memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);
dwt_writetxdata(13+4*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
@@ -394,8 +399,8 @@
flag_getresponse=0;
start_count=HAL_LPTIM_ReadCounter(&hlptim1);
recbase_num=0;
- timeout=nearbase_num*SLOT_SCALE+4;
- end_count=start_count+(timeout<<5);
+ timeout=nearbase_num*SLOT_SCALE+10;
+ end_count=start_count+(timeout<<2);
if(end_count>=32768)
{end_count-=32768;}
mainbase_dist=100000;
@@ -406,7 +411,7 @@
current_count=HAL_LPTIM_ReadCounter(&hlptim1);
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误
{
- if(flag_finalsend)
+ if(flag_finalsend&&flag_ancreadpara)
{
dw_systime=dwt_readsystimestamphi32();
if(dw_systime>temp1&&dw_systime<temp2)
@@ -423,9 +428,10 @@
memcpy(&tx_near_msg[MESSAGE_PARAREPLY_IDX+SENDANCPARA_LEN],&calCRC,2);
dwt_writetxdata(14+SENDANCPARA_LEN, tx_near_msg, 0);//将发送数据写入DW1000
dwt_writetxfctrl(14+SENDANCPARA_LEN, 0);//设定发送数据长度
+
+ dwt_setdelayedtrxtime(final_tx_time);
+ result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送
}
-// dwt_setdelayedtrxtime(final_tx_time);
-// result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送
flag_finalsend=0;
break;
}
@@ -495,8 +501,7 @@
////////////////////////////
rec_nearbasepos=0;
exsistbase_list[rec_nearbasepos]=KEEP_TIMES;
- memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
- nearbase_distlist[rec_nearbasepos]=temp_dist;
+
if((rx_buffer[MOTORSTATE_INDEX]&0xf)!=3)
motor_state=rx_buffer[MOTORSTATE_INDEX]&0xf;
@@ -522,13 +527,13 @@
//时间同步
- ancsync_time=((sync_timer)*1000+tmp_time);
- last_slotnum=current_slotnum;
- current_slotnum=((float)sync_timer/g_com_map[COM_INTERVAL])+1;
- if(current_slotnum==last_slotnum-1)
- {flag_getresponse=1;}
- lastsync_timer=sync_timer;
- offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset;
+// ancsync_time=((sync_timer)*1000+tmp_time);
+// last_slotnum=current_slotnum;
+// current_slotnum=((float)sync_timer/g_com_map[COM_INTERVAL])+1;
+// if(current_slotnum==last_slotnum-1)
+// {flag_getresponse=1;}
+// lastsync_timer=sync_timer;
+// offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset;
// SetNextPollTime(tyncpoll_time);
// if(rec_remotepara_state==1)
// {
@@ -543,9 +548,9 @@
// }
}else{
rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
- SetANCTimestap(rec_nearbasepos+1,&rx_buffer[RESP_MSG_POLL_RX_TS_IDX],&rx_buffer[RESP_MSG_RESP_TX_TS_IDX],resp_rx_ts,&rx_buffer[RESP_MSG_ANC_DISTOFFSET]);
+ SetANCTimestap(rec_nearbasepos,&rx_buffer[RESP_MSG_POLL_RX_TS_IDX],&rx_buffer[RESP_MSG_RESP_TX_TS_IDX],resp_rx_ts,&rx_buffer[RESP_MSG_ANC_DISTOFFSET]);
- if(rec_nearbasepos>=last_nearbase_num) //发现新的基站
+ if(rec_nearbasepos>=last_nearbase_num) //发现新的基站
{
get_newbase=1;
nearbase_num++;
@@ -554,8 +559,7 @@
}
exsistbase_list[rec_nearbasepos]=KEEP_TIMES;
- memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
- nearbase_distlist[rec_nearbasepos]=temp_dist;
+
if(motor_state<(rx_buffer[MOTORSTATE_INDEX]&0xf)&&(rx_buffer[MOTORSTATE_INDEX]&0xf)!=3)
{
@@ -580,30 +584,28 @@
}
// dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
}
- if(mainbase_lost_count>tag_frequency*BASELOST_STOPMOTOR_TIME)
- {motor_state=0; }
+ dwt_forcetrxoff();
dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
- nearbase_num=recbase_num;
- // tyncpoll_time=0;
- next_nearbase_num=0;
+ CalculateDists();
j = 0;
- for(i=0;i<last_nearbase_num+get_newbase;i++)
+ next_nearbase_num = 0;
+ for(i=0;i<nearbase_num;i++)
{
if(exsistbase_list[i]>0)
- {
+ {
+ exsistbase_list[i]--;
next_nearbase_num++;
true_exsistbase_list[j]=exsistbase_list[i];
true_nearbase_idlist[j]=nearbaseid_list[i];
true_nearbase_distlist[j++]=nearbase_distlist[i];
- exsistbase_list[i]--;
+
}
}
+ nearbase_num = next_nearbase_num;
if(mainbase_lost_count>1&&exsistbase_list[0] != 0)
{
last_lpcount+= HAL_LPTIM_ReadCounter(&hlptim1)%(g_com_map[COM_INTERVAL]*16);
}
- if(nearbase_num==4)
- {nearbase_num=4;}
last_nearbase_num = next_nearbase_num;
for(i=0;i<last_nearbase_num-1;i++)
{
@@ -627,14 +629,15 @@
}
report_num=0;
- if(last_nearbase_num>4)
- {
- last_nearbase_num = 5;
- }
+// if(last_nearbase_num>4)
+// {
+// last_nearbase_num = 5;
+// }
for (i=0;i<last_nearbase_num;i++)
{
nearbaseid_list[i]=true_nearbase_idlist[i];
nearbase_distlist[i]=true_nearbase_distlist[i];
+ exsistbase_list[i] = true_exsistbase_list[i];
}
// #ifdef USART_INTEGRATE_OUTPUT
// usart_send[0] = 0x55;
@@ -646,13 +649,9 @@
// USART_puts(usart_send,6+report_num*6);
// #endif
- for(i=0;i<MAX_NEARBASE_NUM;i++)
- {
- nearbase_distlist[i]=0x1ffff;
- }
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
//HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_RESET);
- CalculateDists();
+
if(para_update==1)
{
uint16_t calCRC;
--
Gitblit v1.9.3