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 | 28 +++++++++++++++++++++-------
1 files changed, 21 insertions(+), 7 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 61a2ad7..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,
@@ -319,6 +319,7 @@
u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0,rec_extratag_battary;
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;
@@ -494,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
@@ -511,7 +513,8 @@
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
}
}
@@ -566,7 +569,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
@@ -593,7 +597,8 @@
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
}
}
@@ -934,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;
@@ -969,7 +975,10 @@
if(tagnewdist_list[taglist_pos])
{
tagnewdist_list[taglist_pos] = 0;
- memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
+ 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;
@@ -1071,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;
--
Gitblit v1.9.3