From 0017519b68e7c8f47ccc1df3d92eaf07a2b21050 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期五, 26 五月 2023 13:59:19 +0800
Subject: [PATCH] 修改标签测距0bug
---
Src/application/dw_app.c | 29 ++++++++++++++++-------------
1 files changed, 16 insertions(+), 13 deletions(-)
diff --git a/Src/application/dw_app.c b/Src/application/dw_app.c
index 646b349..42dec63 100644
--- a/Src/application/dw_app.c
+++ b/Src/application/dw_app.c
@@ -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)
{
@@ -336,13 +337,13 @@
if(exsistbase_list[i]>0)
{
- 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;
@@ -373,13 +374,17 @@
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，将在开启发送时传出去
@@ -543,7 +548,7 @@
// }
}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) //发现新的基站
{
@@ -602,8 +607,6 @@
{
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,10 +630,10 @@
}
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];
--
Gitblit v1.9.3