From 3674e113e9923184e8f1bd497c2f7832fc959d29 Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期一, 21 二月 2022 14:47:50 +0800
Subject: [PATCH] V1.66 修改标签报警逻辑bug
---
源码/核心板/Src/application/dw_app.c | 74 ++++++++++++++++++++++++++++--------
1 files changed, 57 insertions(+), 17 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 7c3db08..3589abd 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"
@@ -49,7 +49,7 @@
float dis_after_filter; //当前距离值
LPFilter_Frac* p_Dis_Filter; //测距用的低通滤波器
int32_t g_Tagdist[TAG_NUM_IN_SYS];
-uint8_t g_flag_Taggetdist[256];
+uint8_t g_flag_Taggetdist[256],flag_tag_distsmooth[TAG_NUM_IN_SYS];
static uint64_t get_tx_timestamp_u64(void)
{
@@ -842,6 +842,26 @@
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
+ double firstpath_power, rx_power,rec_firstpath_power;
+ double f1, f2, r1, r2;
+uint16_t F1,F2,F3,N,C;
+double B = 131072;
+double A = 121.74;
+double min_power;
+ dwt_rxdiag_t d1;
+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;
+ }
u16 smallcar_idlist[10]={0x4052,0x4032,0x4055,0x4034,0x4016,0x4010,0x4036,0x4027,0x4024,0x4047};
extern u16 dist_threshold;
u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,getrange_success=0;
@@ -862,18 +882,7 @@
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);//接收超时时间
- if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE1])
- {
- motorstate =0;
- }else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE2])
- {
- motorstate =2;
- }else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE3])
- {
- motorstate =1;
- }else{
- motorstate =0;
- }
+
if(new_tagid)
{
@@ -882,7 +891,28 @@
}else{
memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
}
- tx_nearresp_msg[GROUP_ID_IDX] = group_id;
+
+ if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE1]||tagdist_list[taglist_pos]==0)
+ {
+ motorstate =0;
+ }else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE2])
+ {
+ if(flag_tag_distsmooth[taglist_pos])
+ {motorstate =2;
+ }else{
+ motorstate =0;
+ }
+ }else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE3])
+ {
+ if(flag_tag_distsmooth[taglist_pos])
+ {motorstate =1;
+ }else{
+ motorstate =0;
+ }
+ }else{
+ motorstate =0;
+ }
+ tx_nearresp_msg[GROUP_ID_IDX] = group_id;
tx_nearresp_msg[MAINBASE_INDEX]=flag_syncbase;
tx_nearresp_msg[MESSAGE_TYPE_IDX]=NEAR_RESPONSE;
tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)|motorstate;
@@ -940,7 +970,9 @@
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 * 1000; //dis 为单位为cm的距离
+ dist_cm = dist_no_bias * 1000; //dis 为单位为cm的距离
+ dwt_readdiagnostics(&d1);
+ LOS(&d1);
/*--------------------------以下为非测距逻辑------------------------*/
//dist_cm=33000;
getrange_success = 1;
@@ -958,6 +990,12 @@
if(hex_dist>-100000&&hex_dist<2000000)
{
+ if(abs(hex_dist-his_dist[taglist_pos])<10000)
+ {
+ flag_tag_distsmooth[taglist_pos] =1;
+ }else{
+ flag_tag_distsmooth[taglist_pos] =0;
+ }
if(abs(hex_dist-his_dist[taglist_pos])<dist_threshold||misdist_num[taglist_pos]>3)
{
@@ -978,7 +1016,7 @@
{
g_Tagdist[taglist_pos]=hex_dist/10;
}else{
- g_Tagdist[taglist_pos]= 1;
+ g_Tagdist[taglist_pos]= 999999;
}
#ifdef USART_SINGLE_OUTPUT
usart_send[2] = 1;//正常模式
@@ -987,10 +1025,12 @@
memcpy(&usart_send[5],&tag_id_recv,2);
memcpy(&usart_send[7],&dev_id,2);
if(tagdist_list[taglist_pos]<=0)
- tagdist_list[taglist_pos]=10;
+ tagdist_list[taglist_pos]=999999;
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;
checksum = Checksum_u16(&usart_send[2],17);
memcpy(&usart_send[19],&checksum,2);
UART_PushFrame(usart_send,21);
--
Gitblit v1.9.3