From b27f5e9cc596aecba798b4a949479a8e775426d3 Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期六, 11 六月 2022 16:01:33 +0800
Subject: [PATCH] 1
---
Src/application/dw_app.c | 86 +++++++++++++-----------------------------
1 files changed, 27 insertions(+), 59 deletions(-)
diff --git a/Src/application/dw_app.c b/Src/application/dw_app.c
index 71a1b17..dad20e6 100644
--- a/Src/application/dw_app.c
+++ b/Src/application/dw_app.c
@@ -30,7 +30,7 @@
#include "beep.h"
#include "modbus.h"
-#define USART_INTEGRATE_OUTPUT
+//#define USART_INTEGRATE_OUTPUT
/*------------------------------------ Marcos ------------------------------------------*/
/* Inter-ranging delay period, in milliseconds. */
#define RNG_DELAY_MS 100
@@ -295,7 +295,7 @@
u8 nearbase_num=0,last_nearbase_num,next_nearbase_num,last_slotnum, para_update,para_len,stationary_flag=0;
u32 rec_tagpos_binary;
int16_t offset=2700;
-u8 motor_state,rec_remotepara_state,rec_remotepara[80];
+u8 motor_state,rec_remotepara_state,rec_remotepara[80],firstrec_flag;
void NearPoll(void)
{
@@ -316,6 +316,7 @@
last_nearbase_num=next_nearbase_num;
nearbase_num=next_nearbase_num;
recbase_num=0;
+ firstrec_flag = 0;
// motor_state=0;
tx_near_msg[GROUP_ID_IDX] = group_id;
tx_near_msg[BATTARY_IDX] = bat_percent;
@@ -353,10 +354,12 @@
dw_systime=dwt_readsystimestamphi32();
if(dw_systime>temp1&&dw_systime<temp2)
{
-
+ flag_finalsend=0;
dwt_forcetrxoff();
flag_rxon=0;
dwt_setdelayedtrxtime(final_tx_time);
+ dwt_writetxdata(28+nearbase_num*4, tx_near_msg, 0);//将发送数据写入DW1000
+ dwt_writetxfctrl(28+nearbase_num*4, 0);//设定发送数据长度
result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送
flag_finalsend=0;
break;
@@ -379,20 +382,24 @@
dwt_rxenable(0);//打开接收
if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_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++;
memcpy(&rec_nearbaseid,&rx_buffer[ANCHOR_ID_IDX],2);
- if(last_nearbase_num==0)
- {
- get_newbase=1;
- nearbaseid_list[0]=rec_nearbaseid;
- nearbase_num=1;
- memcpy(&tx_near_msg[ANCHOR_ID_IDX],&rec_nearbaseid,2);
- }
- if(rec_nearbaseid==nearbaseid_list[0])
- {
- //////////////////////////////////时间同步
+ if(!firstrec_flag)
+ {
+ flag_finalsend = 1;
+ firstrec_flag = 1;
+ poll_tx_ts = get_tx_timestamp_u64();
+ final_tx_time = (poll_tx_ts+65535364 + ((RESP_RX_TO_FINAL_TX_DLY_UUS+DELAY_BETWEEN_TWO_FRAME_UUS*12+500) * UUS_TO_DWT_TIME)) >> 8;
+ temp1=final_tx_time-((650*UUS_TO_DWT_TIME)>>8);
+ temp2=final_tx_time+((100*UUS_TO_DWT_TIME)>>8);
+ final_tx_ts = (((uint64_t)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;//final包实际发送时间是计算时间加上发送天线delay
+ final_msg_set_ts(&tx_near_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//将T1，T4，T5写入发送数据
+ final_msg_set_ts(&tx_near_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
+ tx_near_msg[MESSAGE_TYPE_IDX]=NEAR_FINAL;
+ tx_near_msg[GROUP_ID_IDX] = group_id;
+ //////////////////////////////////时间同步
memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2);
memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);
current_count=HAL_LPTIM_ReadCounter(&hlptim1);
@@ -405,48 +412,16 @@
if(sync_timer>=1010)
{sync_timer=0;}
}
- // TIM3->CNT=tmp_time;
-// if(tagslotpos>max_slotpos)
-// tagslotpos=tagslotpos%(max_slotpos+1);
-// tyncpoll_time=(tagslotpos-1)*slottime;
- ////////////////////////////
- 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;
- rec_remotepara_state=rx_buffer[MOTORSTATE_INDEX]>>4;
-
- mainbase_lost_count=0;
- flag_finalsend=1;
- memcpy(&mainbase_dist,&rx_buffer[DIST_IDX],4);
- final_tx_time = (resp_rx_ts + ((RESP_RX_TO_FINAL_TX_DLY_UUS+DELAY_BETWEEN_TWO_FRAME_UUS*nearbase_num+500) * UUS_TO_DWT_TIME)) >> 8;
- temp1=final_tx_time-((850*UUS_TO_DWT_TIME)>>8);
- temp2=final_tx_time+((100*UUS_TO_DWT_TIME)>>8);
- // dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5
- final_tx_ts = (((uint64_t)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;//final包实际发送时间是计算时间加上发送天线delay
- final_msg_set_ts(&tx_near_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//将T1，T4，T5写入发送数据
- final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX], resp_rx_ts);
- final_msg_set_ts(&tx_near_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
- tx_near_msg[MESSAGE_TYPE_IDX]=NEAR_FINAL;
- tx_near_msg[GROUP_ID_IDX] = group_id;
-
- dwt_writetxdata(28+nearbase_num*4, tx_near_msg, 0);//将发送数据写入DW1000
- dwt_writetxfctrl(28+nearbase_num*4, 0);//设定发送数据长度
- flag_getresponse=1;
- memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
-
-
- //时间同步
- ancsync_time=((sync_timer)*1000+tmp_time);
+ 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);
+ SetNextPollTime(tyncpoll_time);
+ // dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5
+ }
if(rec_remotepara_state==1)
{
memcpy(rec_remotepara,&rx_buffer[REMOTEPARA_INDEX],REMOTEPARA_LEN);
@@ -458,7 +433,7 @@
}
NVIC_SystemReset();
}
- }else{
+
rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
if(rec_nearbasepos>=last_nearbase_num) //发现新的基站
@@ -478,15 +453,8 @@
motor_state=rx_buffer[MOTORSTATE_INDEX];
}
final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos)*4], resp_rx_ts);
- dwt_writetxdata(28+nearbase_num*4, tx_near_msg, 0);//将发送数据写入DW1000
- dwt_writetxfctrl(28+nearbase_num*4, 0);//设定发送数据长度
-
- //dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5
- // result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送
- //dwt_writetxdata(4,&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)*4], FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)*4);//将发送数据写入DW1000
- }
- }
+ }
}else{
dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
if(flag_rxon)
--
Gitblit v1.9.3