V1.9

yincheng.zhong

2023-05-11 2cb63340687ae2ada98c545227ffb247f48f3a9a

 Ô´Âë/ºËÐÄ°å/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,58 @@
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],tag_dist[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_dist[taglist_pos] = second_dist;
        tag_angle[taglist_pos]  = asin(p_dist)*57.3; 
        OUT485_ENABLE;
      delay_us(10);
        printf("角度：%d,距离差：%d,距离1：%d,距离2:%d......\r\n",tag_angle[taglist_pos],first_dist-second_dist,first_dist,second_dist);
      delay_us(10);
        OUT485_DISABLE;
    }
}
extern int16_t alarm_angle;
u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase  1 first near_base
{
   u8 motorstate;
@@ -886,14 +938,18 @@
            memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
         }
         motorstate =0;
      if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE2])
		
            if(g_Tagdist[taglist_pos]<g_com_map[ALARM_DISTANCE1]||g_Tagdist[taglist_pos]==0)
         {
            motorstate =0;
         }else if(g_Tagdist[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])
         }else if(g_Tagdist[taglist_pos]<g_com_map[ALARM_DISTANCE3])
         {
            if(flag_tag_distsmooth[taglist_pos])
            {motorstate =1;
@@ -901,7 +957,7 @@
               motorstate =0;
            }
         }
      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]=motorstate;//(remotesend_state<<4)|motorstate;
@@ -1008,6 +1064,7 @@
                        }else{
                            g_Tagdist[taglist_pos]= 0x2ffff;
                        }
                         AngleCalculate();
            #ifdef USART_SINGLE_OUTPUT
               usart_send[2] = 1;//正常模式 
               usart_send[3] = 17;//数据段长度
@@ -1020,7 +1077,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);
@@ -1039,11 +1096,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)
{