From 4109f0604cd4d1221929497df94b69ec2c54a4ed Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期四, 20 四月 2023 17:05:14 +0800
Subject: [PATCH] V1.55 香港L3-L4测试调通
---
Src/application/dw_app.c | 107 ++++++++++++++++++++++++++++++++++++++++-------------
1 files changed, 81 insertions(+), 26 deletions(-)
diff --git a/Src/application/dw_app.c b/Src/application/dw_app.c
index 2308719..646b349 100644
--- a/Src/application/dw_app.c
+++ b/Src/application/dw_app.c
@@ -71,16 +71,16 @@
#define FINAL_MSG_FINAL_TX_TS_IDX 18
#define FINAL_MSG_TS_LEN 4
static dwt_config_t config = {
- 2, /* Channel number. */
+ 5, /* Channel number. */
DWT_PRF_64M, /* Pulse repetition frequency. */
- DWT_PLEN_128, /* Preamble length. */
+ DWT_PLEN_64, /* 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. */
- (129 + 8 - 8) /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
+ (65 + 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};
@@ -97,7 +97,6 @@
static uint64_t poll_rx_ts;
static uint64_t resp_tx_ts;
static uint64_t final_rx_ts;
-static double tof;
int32_t anchor_dist_last_frm[TAG_NUM_IN_SYS],his_dist[TAG_NUM_IN_SYS]; ;
uint32_t tag_id = 0;
uint32_t tag_id_recv = 0;
@@ -304,7 +303,55 @@
freqlost_count = 0;
}
}
+#define FREQ_OFFSET_MULTIPLIER (998.4e6/2.0/1024.0/131072.0)
+#define FREQ_OFFSET_MULTIPLIER_110KB (998.4e6/2.0/8192.0/131072.0)
+#define HERTZ_TO_PPM_MULTIPLIER_CHAN_1 (-1.0e6/3494.4e6)
+#define HERTZ_TO_PPM_MULTIPLIER_CHAN_2 (-1.0e6/3993.6e6)
+#define HERTZ_TO_PPM_MULTIPLIER_CHAN_3 (-1.0e6/4492.8e6)
+#define HERTZ_TO_PPM_MULTIPLIER_CHAN_5 (-1.0e6/6489.6e6)
+u16 rec_maxrangelen;//rec_anc_signalpower[MAX_NEARBASE_ANCNUM];
+float clockOffsetRatio;
+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];
+int32_t test2;
+uint8_t get_newdist,notenoughdist_count;
+void SetANCTimestap(uint8_t i,uint8_t* pollrx,uint8_t* resptx,uint32_t resprx,uint8_t* distoffset)
+{
+ memcpy(&anc_pollrx[i],pollrx,4);
+ memcpy(&anc_resptx[i],resptx,4);
+ memcpy(&tag_resprx[i],&resprx,4);
+ memcpy(&anc_distoffset[i],distoffset,2);
+ anc_clockoffset[i] = test2;
+}
+void CalculateDists(void)
+{
+ for(int i=0;i<11;i++)
+ {
+ //rec_anc_signalpower[i] = exsistbase_list[i];
+ if(exsistbase_list[i]>0)
+ {
+
+ 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)
+ nearbase_distlist[i] = distance*100+anc_distoffset[i];
+ }else{
+ nearbase_distlist[i] = 0x1ffff;
+ }
+
+ }
+}
+float range_lost_time = 0;
+u16 rec_nearbaseid,rec_nearbasepos;
void NearPoll(void)
{
@@ -332,14 +379,16 @@
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);
tx_near_msg[MESSAGE_TYPE_IDX] = MBX_POLL;
memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);
- dwt_writetxdata(13+2*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
- dwt_writetxfctrl(13+2*nearbase_num, 0);//设置超宽带发送数据长度
+ dwt_writetxdata(13+4*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
+ dwt_writetxfctrl(13+4*nearbase_num, 0);//设置超宽带发送数据长度
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置
tx_near_msg[TAGCONFIGSUCCESS_INDEX] =0;
para_update = 0;
+ get_newbase = 0;
flag_finalsend=0;
flag_rxon=1;
flag_getresponse=0;
@@ -375,8 +424,8 @@
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;
}
@@ -394,10 +443,11 @@
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清楚寄存器标志位
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; //获得接收到的数据长度
dwt_readrxdata(rx_buffer, frame_len, 0); //读取接收数据
+ test2 = dwt_readcarrierintegrator();
dwt_setrxtimeout(0);//DELAY_BETWEEN_TWO_FRAME_UUS*(nearbase_num+1-recbase_num)+10);//设定接收超时时间，0位没有超时时间
dwt_rxenable(0);//打开接收
if (rx_buffer[MESSAGE_TYPE_IDX] == MBX_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)&&rx_buffer[GROUP_ID_IDX]==group_id) //判断接收到的数据是否是response数据
- { u16 rec_nearbaseid,rec_nearbasepos;
+ {
poll_tx_ts = get_tx_timestamp_u64(); //获得POLL发送时间T1
resp_rx_ts = get_rx_timestamp_u64(); //获得RESPONSE接收时间T4
recbase_num++;
@@ -422,6 +472,9 @@
}
if(rec_nearbaseid==nearbaseid_list[0])
{
+ exsistbase_list[0]=KEEP_TIMES;
+ SetANCTimestap(0,&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]);
+
//////////////////////////////////时间同步
memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2);
memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);
@@ -490,7 +543,8 @@
// }
}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]);
+
if(rec_nearbasepos>=last_nearbase_num) //发现新的基站
{
get_newbase=1;
@@ -548,6 +602,8 @@
{
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++)
{
@@ -571,26 +627,24 @@
}
report_num=0;
+ 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];
- if(nearbase_distlist[i]!=0x1ffff&&true_exsistbase_list[i]==KEEP_TIMES)
- {
- memcpy(&usart_send[4+6*report_num],&nearbaseid_list[i],2);
- memcpy(&usart_send[6+6*report_num],&nearbase_distlist[i],4);
- report_num++;
- }
}
- #ifdef USART_INTEGRATE_OUTPUT
- usart_send[0] = 0x55;
- usart_send[1] = 0xAA;
- usart_send[2] = 4;//正常模式
- usart_send[3] = report_num*6+2;//正常模式
- checksum = Checksum_u16(&usart_send[2],report_num*6+2);
- memcpy(&usart_send[4+report_num*6],&checksum,2);
- USART_puts(usart_send,6+report_num*6);
- #endif
+// #ifdef USART_INTEGRATE_OUTPUT
+// usart_send[0] = 0x55;
+// usart_send[1] = 0xAA;
+// usart_send[2] = 4;//正常模式
+// usart_send[3] = report_num*6+2;//正常模式
+// checksum = Checksum_u16(&usart_send[2],report_num*6+2);
+// memcpy(&usart_send[4+report_num*6],&checksum,2);
+// USART_puts(usart_send,6+report_num*6);
+// #endif
for(i=0;i<MAX_NEARBASE_NUM;i++)
{
@@ -598,6 +652,7 @@
}
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;
@@ -605,7 +660,7 @@
if(memcmp(&calCRC,&rec_remotepara[5],2)==0)
{ uint8_t pack_msgtype,pack_index,pack_length;
pack_msgtype = rec_remotepara[0];
- pack_index = rec_remotepara[1]*2;
+ pack_index = rec_remotepara[1];
pack_length = rec_remotepara[2];
if(pack_msgtype==2)
{
--
Gitblit v1.9.3