初步测试完成

yincheng.zhong

2023-05-05 737a981dc473a1e6018bd7e0db35fee69e5b3b01

 Ô´Âë/ºËÐÄ°å/Src/application/dw_app.c

@@ -1,6 +1,6 @@
#include "dw_app.h"
#include "ADC.h"
#define TDFILTER
//#define TDFILTER
enum enumtagstate
{
   DISCPOLL,
@@ -861,6 +861,52 @@
u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,getrange_success=0;
int32_t filter_dist,filter_speed;
u8 newmeasure,recpoll_len;
uint32_t current_syncid=0xffffffff,synclost_timer;
extern uint8_t flag_syncbase;
uint8_t tagpos_rec[50],tagpos_send[50],ancidlist_num;
uint16_t ancidlist_rec[20],ancidlist_send[20],rec_ancidlist[20],rec_ancdistlist[20];
uint16_t samegroup_ancid,anc_mount_dist;
uint8_t getvaluetimes = 0;
uint16_t first_dist,second_dist,delta_dist;
int16_t tag_angle[TAG_NUM_IN_SYS];
float p_dist;
void AngleCalculate(void)
{
    getvaluetimes = 0;
    for(uint8_t i=0;i<rec_nearbase_num;i++)
    {
        if(rec_ancidlist[i]==dev_id)
        {
            
            first_dist = rec_ancdistlist[i];
            if(first_dist!=0xffff)
            {
                getvaluetimes++;
            }
        }else if(rec_ancidlist[i]==samegroup_ancid)
        {
            
            second_dist = rec_ancdistlist[i];
            if(second_dist != 0xffff)
            {
                getvaluetimes++;
            }
        }
    }
    if(getvaluetimes==2)
    {
       p_dist = ((float)first_dist-(float)second_dist)/anc_mount_dist;
        if(p_dist>1)
        {
            p_dist = 1;
        }else if(p_dist<-1)
        {
            p_dist = -1;
        }
       tag_angle[taglist_pos] = -asin(p_dist)*57.3; 
    }
}
extern int16_t alarm_angle;
u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase  1 first near_base
{
   u8 motorstate;
@@ -892,14 +938,14 @@
            motorstate =0;
         }else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE2])
         {
            if(flag_tag_distsmooth[taglist_pos])
            if(flag_tag_distsmooth[taglist_pos]&&tag_angle[taglist_pos]>alarm_angle)
            {motorstate =2;
            }else{
               motorstate =0;
            }
         }else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE3])
         {
            if(flag_tag_distsmooth[taglist_pos])
            if(flag_tag_distsmooth[taglist_pos]&&tag_angle[taglist_pos]>alarm_angle)
            {motorstate =1;
               }else{
               motorstate =0;
@@ -1012,6 +1058,7 @@
                        }else{
                            g_Tagdist[taglist_pos]= 0x2ffff;
                        }
                         AngleCalculate();
            #ifdef USART_SINGLE_OUTPUT
               usart_send[2] = 1;//正常模式 
               usart_send[3] = 17;//数据段长度
@@ -1024,7 +1071,7 @@
               usart_send[13] = battary;
               usart_send[14] = button;
               usart_send[15] = firstpath_power;
               usart_send[16] = rx_power;
               usart_send[16] = tag_angle[taglist_pos];
               checksum = Checksum_u16(&usart_send[2],17);
               memcpy(&usart_send[19],&checksum,2);
               UART_PushFrame(usart_send,21);
@@ -1043,11 +1090,9 @@
               //printf("%x/n",status_reg);
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
         }
           
}
uint32_t current_syncid=0xffffffff,synclost_timer;
extern u8 flag_syncbase;
u8 tagpos_rec[50],tagpos_send[50],ancidlist_num;
u16 ancidlist_rec[20],ancidlist_send[20],rec_ancidlist[20],rec_ancdistlist[20];

void Anchor_App(void)
{