From fc057b986700bbb5dd6bac94c40b4093f2751f7f Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期六, 31 十月 2020 16:13:51 +0800
Subject: [PATCH] V2.7 固定时间片9标签20HZ测试通过
---
源码/核心板/Src/application/dw_app.c | 83 +++++++++++++++++++++++++++--------------
1 files changed, 55 insertions(+), 28 deletions(-)
diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c"
index bed88eb..919ddf1 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c"
@@ -176,14 +176,14 @@
extern uint8_t g_start_send_flag;
u8 g_start_sync_flag;
-void SyncPoll(u8 sync_seq,uint32_t sync_baseid)
+void SyncPoll(u8 sync_seq)
{u8 result;
g_start_sync_flag=1; //中断模式，退出终端后，需要重新来过
dwt_forcetrxoff(); //关闭接收，以防在RX ON 状态
tx_sync_msg[SYNC_SEQ_IDX]=sync_seq;
- memcpy(&tx_sync_msg[ANCHOR_ID_IDX],&sync_baseid,4);
+ memcpy(&tx_sync_msg[ANCHOR_ID_IDX],&dev_id,4);
dwt_writetxdata(sizeof(tx_sync_msg), tx_sync_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
dwt_writetxfctrl(sizeof(tx_sync_msg), 0);//设置超宽带发送数据长度
dwt_starttx(DWT_START_TX_IMMEDIATE);
@@ -309,11 +309,30 @@
}
return max_slotpos-1;
}
+dwt_rxdiag_t d1;
u8 test=0;
u8 anclost_times=0;
u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0;
u16 temp_sync_timer1,temp_sync_timer2;
+ double firstpath_power, rx_power;
+ double f1, f2, r1, r2;
+uint16_t F1,F2,F3,N,C;
+double B = 131072;
+double A = 121.74;
+double min_power;
+double LOS(dwt_rxdiag_t *dia) {
+ 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));
+ return min_power;
+ }
void Poll(void)
{
uint32_t frame_len;
@@ -896,6 +915,7 @@
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
+uint32_t time_monitor[10];
extern uint16_t configremotetagID;
extern u8 remotetag_paralen;
u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse;
@@ -913,7 +933,7 @@
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);//接收超时时间
-
+ dwt_readdiagnostics(&d1);
if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE1])
{
motorstate =0;
@@ -1003,6 +1023,10 @@
int64_t tof_dtu;
resp_tx_ts = get_tx_timestamp_u64();//获得response发送时间T3
final_rx_ts = get_rx_timestamp_u64();//获得final接收时间T6
+
+// dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间
+// dwt_rxenable(0);//打开接收
+
final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);//从接收数据中读取T1，T4，T5
final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_NEARBASE_IDX+ancrec_nearbasepos*4], &resp_rx_ts);
final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);
@@ -1010,6 +1034,7 @@
poll_rx_ts_32 = (uint32_t)poll_rx_ts;//使用32位数据计算
resp_tx_ts_32 = (uint32_t)resp_tx_ts;
final_rx_ts_32 = (uint32_t)final_rx_ts;
+ time_monitor[1] = sync_timer*1000+TIM3->CNT;
Ra = (double)(resp_rx_ts - poll_tx_ts);//Tround1 = T4 - T1
Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);//Tround2 = T6 - T3
Da = (double)(final_tx_ts - resp_rx_ts);//Treply2 = T5 - T4
@@ -1018,10 +1043,14 @@
tof = tof_dtu * DWT_TIME_UNITS;
distance = tof * SPEED_OF_LIGHT;//距离=光速*飞行时间
dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //距离减去矫正系数
- dist_cm = dist_no_bias * 100; //dis 为单位为cm的距离
+ dist_cm = dist_no_bias * 100; //dis 为单位为cm的距离
+ dwt_readdiagnostics(&d1);
+// time_monitor[2] = sync_timer*1000+TIM3->CNT;
+ LOS(&d1);
+ time_monitor[3] = sync_timer*1000+TIM3->CNT;
/*--------------------------以下为非测距逻辑------------------------*/
//dist_cm=33000;
-
+ GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_RESET);
LED0_BLINK; //每成功一次通讯则闪烁一次
dis_after_filter=dist_cm;
hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
@@ -1043,6 +1072,8 @@
memcpy(&usart_send[9],&tagdist_list[taglist_pos],4);
usart_send[13] = battary;
usart_send[14] = button;
+ usart_send[15] = firstpath_power;
+ usart_send[16] = (rx_power-firstpath_power)*10;
checksum = Checksum_u16(&usart_send[2],17);
memcpy(&usart_send[19],&checksum,2);
UART_PushFrame(usart_send,21);
@@ -1079,8 +1110,9 @@
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
+extern u16 synclost_count;
uint32_t current_syncid=0xffffffff,synclost_timer;
-extern u8 flag_syncbase;
+extern u8 flag_syncbase,waitsync_flag;
u8 tagpos_rec[50],tagpos_send[50],ancidlist_num;
u16 ancidlist_rec[20],ancidlist_send[20];
void Anchor_App(void)
@@ -1104,7 +1136,7 @@
if (status_reg & SYS_STATUS_RXFCG)//成功接收
{ u16 tag_recv_interval;
float temp_tagpos;
-
+ time_monitor[0] = sync_timer*1000+TIM3->CNT;
g_Resttimer=0;
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//清除标志位
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//获得接收数据长度
@@ -1120,28 +1152,17 @@
break;
case SYNC:
memcpy(&rec_syncid,&rx_buffer[ANCHOR_ID_IDX],4);
- if(rec_syncid<current_syncid)
- {
- current_syncid=rec_syncid;
+ if(anc_id_recv==g_com_map[SYNCBASEID])
+ {
+ synclost_count = 0;
flag_syncbase=0;
sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
- TIM3->CNT = sync_seq*325%1000+15;
- sync_timer = sync_seq*325/1000;
+ TIM3->CNT = (sync_seq*325%1000)-80;
+ sync_timer = (sync_seq*325/1000);
synclost_timer=0;
- //SyncPoll(sync_seq,rec_syncid);
- }else if(rec_syncid==current_syncid)
- {
- if(rx_buffer[SYNC_SEQ_IDX]<sync_seq)
- {
- flag_syncbase=0;
- sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
- TIM3->CNT = sync_seq*325%1000+5;
- sync_timer = sync_seq*325/1000+995;
- synclost_timer=0;
- //SyncPoll(sync_seq,rec_syncid);
- }
-
- }
+ if(g_com_map[BASESYNCSEQ]==2)
+ SyncPoll(sync_seq);
+ }
break;
case NEAR_MSG:
if(anc_id_recv==dev_id)
@@ -1160,7 +1181,8 @@
}
break;
- case NEAR_POLL:
+ case NEAR_POLL:
+ // GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);
memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
taglist_pos=CmpTagInList(tag_id_recv);
if(taglist_pos==taglist_num)
@@ -1180,6 +1202,7 @@
if(anc_id_recv==dev_id)
{
+ GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);
Anchor_RecNearPoll(0);
}else{
@@ -1188,6 +1211,7 @@
memcpy(&tempid,&rx_buffer[NEARBASEID_INDEX+i*2],2);
if(tempid==dev_id)
{
+
seize_anchor=0; //非抢占。已存在列表中
Anchor_RecNearPoll(i+1);
break;
@@ -1202,7 +1226,10 @@
// }
- break;
+ break;
+ default:
+
+ break;
}
}else{
#ifdef DEBUG_INF
--
Gitblit v1.9.3