From 815ea18d897111c4bf70861277acf94201ccf60a Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期五, 14 十月 2022 16:25:59 +0800
Subject: [PATCH] V1.72 增加滤波器 增加频率可调
---
源码/核心板/Src/application/dw_app.c | 76 +++++++++++++++++++++++++-------------
1 files changed, 50 insertions(+), 26 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 3b22bba..72fcfc2 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"
@@ -1,6 +1,6 @@
#include "dw_app.h"
#include "ADC.h"
-#define TDFILTER
+//#define TDFILTER
enum enumtagstate
{
DISCPOLL,
@@ -313,11 +313,13 @@
}
return max_slotpos-1;
}
-u8 test=0,dissucc_flag1,dissucc_flag2,lastdist1,lastdist2;
-u8 anclost_times=0,usartoutput_count,usartoutput_notenough=0;
+u8 test=0,dissucc_times,lastdist1,lastdist2;
+u8 anclost_times=0,usartoutput_count;
+u16 usartoutput_notenough=0;
u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0,rec_extratag_battary;
-u16 temp_sync_timer1,temp_sync_timer2,rec_extratag_id;
+u16 temp_sync_timer1,temp_sync_timer2,rec_extratag_id,success_ancid;
int32_t rec_extratag_dist;
+uint8_t usart_send_array[5][21],usart_array_num;
void NearPoll(void)
{
static u8 mainbase_lost_count=0,flag_finalsend,flag_rxon;
@@ -471,10 +473,14 @@
dwt_writetxdata(28+nearbase_num*4, tx_nearfinal_msg, 0);//将发送数据写入DW1000
dwt_writetxfctrl(28+nearbase_num*4, 0);//设定发送数据长度
memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
- if(temp_dist!=0x1ffff&&lastdist1!=temp_dist&&!dissucc_flag1)//没有成功测距，且距离和上一次不相等
+ if(temp_dist!=0x1ffff)//没有成功测距，且距离和上一次不相等
{
- dissucc_flag1 = 1;
- lastdist1 = temp_dist;
+ if(success_ancid!=rec_nearbaseid&&dissucc_times<2)
+ {
+ success_ancid = rec_nearbaseid;
+ dissucc_times++;
+
+
g_Resttimer=0;
IWDG_Feed();
@@ -489,7 +495,8 @@
usart_send[14] = button;
checksum = Checksum_u16(&usart_send[2],17);
memcpy(&usart_send[19],&checksum,2);
- UART_PushFrame(usart_send,21);
+ //UART_PushFrame(usart_send,21);
+ memcpy(&usart_send_array[usart_array_num++],usart_send,21);
usartoutput_count++;
#endif
@@ -506,9 +513,11 @@
usart_send[14] = button;
checksum = Checksum_u16(&usart_send[2],17);
memcpy(&usart_send[19],&checksum,2);
- UART_PushFrame(usart_send,21);
+ memcpy(&usart_send_array[usart_array_num++],usart_send,21);
+ //UART_PushFrame(usart_send,21);
#endif
}
+ }
}
}else{
@@ -536,11 +545,13 @@
LED0_BLINK;
// memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
// tagslotpos=GetRandomSlotPos(rec_tagpos_binary);
- if(temp_dist!=0x1ffff&&lastdist2!=temp_dist&&!dissucc_flag2)//没有成功测距，且距离和上一次不相等
+ if(temp_dist!=0x1ffff)//没有成功测距，且距离和上一次不相等
{
- dissucc_flag2 = 1;
- lastdist2 = temp_dist;
-
+ if(success_ancid!=rec_nearbaseid&&dissucc_times<2)
+ {
+ success_ancid = rec_nearbaseid;
+ dissucc_times++;
+
#ifdef USART_SINGLE_OUTPUT
usart_send[2] = 1;//正常模式
usart_send[3] = 17;//数据段长度
@@ -558,10 +569,11 @@
usart_send[14] = button;
checksum = Checksum_u16(&usart_send[2],17);
memcpy(&usart_send[19],&checksum,2);
- UART_PushFrame(usart_send,21);
+ //UART_PushFrame(usart_send,21);
+ memcpy(&usart_send_array[usart_array_num++],usart_send,21);
usartoutput_count++;
#endif
-
+
//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
@@ -585,9 +597,11 @@
usart_send[14] = button;
checksum = Checksum_u16(&usart_send[2],17);
memcpy(&usart_send[19],&checksum,2);
- UART_PushFrame(usart_send,21);
+ memcpy(&usart_send_array[usart_array_num++],usart_send,21);
+ //UART_PushFrame(usart_send,21);
#endif
}
+ }
}
}
}
@@ -701,8 +715,8 @@
{
usartoutput_notenough = 0;
}else{
- sync_timer += time32_incr%5;
- if(usartoutput_notenough++>50)
+ sync_timer += time32_incr%2;
+ if(usartoutput_notenough++>500)
{
SCB->AIRCR = 0X05FA0000|(unsigned int)0x04; //软复位回到bootloader
}
@@ -798,7 +812,7 @@
u16 tagid_list[TAG_NUM_IN_SYS];
u8 tagofflinetime[TAG_NUM_IN_SYS];
int32_t tagdist_list[TAG_NUM_IN_SYS];
-u8 tagseq_list[TAG_NUM_IN_SYS];
+u8 tagseq_list[TAG_NUM_IN_SYS],tagnewdist_list[TAG_NUM_IN_SYS];
void TagListUpdate(void)
{
u16 i,j=0,temp[TAG_NUM_IN_SYS];
@@ -925,6 +939,7 @@
u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,getrange_success=0;
int32_t filter_dist,filter_speed;
u8 newmeasure,recpoll_len;
+float fiter_p;
u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase 1 first near_base
{
u8 motorstate;
@@ -952,18 +967,21 @@
}else{
motorstate =0;
}
-// if(frame_seq_nb2!=tagseq_list[taglist_pos]+1)
-// {
-// tagdist_list[taglist_pos]=0x1ffff;
-// }
if(new_tagid)
{
tagdist_list[taglist_pos]=0x1ffff;
memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
}else{
- memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
+ if(tagnewdist_list[taglist_pos])
+ {
+ tagnewdist_list[taglist_pos] = 0;
+ memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
+ }else{
+ int32_t nodistvalue = 0x1ffff;
+ memcpy(&tx_nearresp_msg[DIST_IDX], &nodistvalue, 4);
+ }
}
- tx_nearresp_msg[GROUP_ID_IDX] = group_id;
+ 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]=0;//(remotesend_state<<4)|motorstate;
@@ -1062,7 +1080,12 @@
//filter_speed = vel_predict[taglist_pos];
newmeasure = 1;
#else
- filter_dist=hex_dist/10;
+ if(tagdist_list[taglist_pos]>0&&tagdist_list[taglist_pos]<50000)
+ {
+ filter_dist=(hex_dist/10)*fiter_p+(1-fiter_p)*tagdist_list[taglist_pos];
+ }else{
+ filter_dist = hex_dist/10;
+ }
#endif
misdist_num[taglist_pos]=0;
tagdist_list[taglist_pos] = filter_dist;
@@ -1073,6 +1096,7 @@
}else{
g_Tagdist[taglist_pos]= 1;
}
+ tagnewdist_list[taglist_pos] = 1;
tagseq_list[taglist_pos] = frame_seq_nb2;
#ifdef USART_SINGLE_OUTPUT
usart_send[2] = 1;//正常模式
--
Gitblit v1.9.3