V1.72 增加滤波器 增加频率可调

zhyinch

2022-10-14 815ea18d897111c4bf70861277acf94201ccf60a

 源码/核心板/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;