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,
@@ -344,6 +344,7 @@
   tx_nearpoll_msg[BUTTON_IDX] = !READ_KEY0;
   tx_nearpoll_msg[SEQUENCE_IDX] = frame_seq_nb++;
   tx_nearpoll_msg[NEARBASENUM_INDEX] = nearbase_num;
    
   memcpy(&tx_nearpoll_msg[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2);
    for(i=0;i<nearbase_num;i++)
    { memcpy(&tx_nearpoll_msg[NEARBASEID_INDEX+nearbase_num*2+i*2],&nearbase_distlist[i],2);}
@@ -437,6 +438,7 @@
                           }
                           TIM3->CNT=tmp_time;
                        }
                                    if(g_com_map[DEV_ROLE])
                           current_slotnum=temp_sync_timer1/g_com_map[COM_INTERVAL];
//                           if(tagslotpos>max_slotpos)
//                              tagslotpos=tagslotpos%(max_slotpos+1);
@@ -464,7 +466,7 @@
                           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&&g_com_map[ALARM_DISTANCE1] == 1)
                        if(temp_dist!=0x1ffff)
                           {
                              g_Resttimer=0;
                                        IWDG_Feed();
@@ -472,14 +474,10 @@
                              usart_send[2] = 1;//正常模式
                              usart_send[3] = 17;//数据段长度
                              usart_send[4] = frame_seq_nb;//数据段长度
                              if(g_com_map[DEV_ROLE])
                              {

                              memcpy(&usart_send[5],&dev_id,2);
                              memcpy(&usart_send[7],&rec_nearbaseid,2);      
                              }else{
                              memcpy(&usart_send[5],&rec_nearbaseid,2);
                              memcpy(&usart_send[7],&dev_id,2);	
                              }											
										
                              memcpy(&usart_send[9],&rx_buffer[DIST_IDX],4);
                              usart_send[13] = battary;
                              usart_send[14] = button;
@@ -516,20 +514,16 @@
                           LED0_BLINK;
//                           memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
//                           tagslotpos=GetRandomSlotPos(rec_tagpos_binary);
                           if(temp_dist!=0x1ffff&&g_com_map[ALARM_DISTANCE1] == 1)
                           if(temp_dist!=0x1ffff)
                           {
                              #ifdef USART_SINGLE_OUTPUT
                              usart_send[2] = 1;//正常模式
                              usart_send[3] = 17;//数据段长度
                              usart_send[4] = frame_seq_nb;//数据段长度
                              if(g_com_map[DEV_ROLE])
                              {
                              memcpy(&usart_send[5],&dev_id,2);
                              memcpy(&usart_send[7],&rec_nearbaseid,2);		
                              }else{
			
                              memcpy(&usart_send[5],&rec_nearbaseid,2);
                              memcpy(&usart_send[7],&dev_id,2);   
                              }											
																					
                              memcpy(&usart_send[9],&rx_buffer[DIST_IDX],4);
                              usart_send[13] = battary;
                              usart_send[14] = button;
@@ -867,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;
@@ -892,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;
@@ -907,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;
@@ -1014,6 +1064,7 @@
                        }else{
                            g_Tagdist[taglist_pos]= 0x2ffff;
                        }
                         AngleCalculate();
            #ifdef USART_SINGLE_OUTPUT
               usart_send[2] = 1;//正常模式 
               usart_send[3] = 17;//数据段长度
@@ -1026,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);
@@ -1045,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)
{
   
@@ -1138,7 +1187,7 @@
            memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
                memcpy(&rec_ancidlist,&rx_buffer[NEARBASEID_INDEX],2*rec_nearbase_num);
                memcpy(&rec_ancdistlist,&rx_buffer[NEARBASEID_INDEX+2*rec_nearbase_num],2*rec_nearbase_num);
                memcpy(&rec_antdelay,&rx_buffer[NEARBASEID_INDEX+nearbase_num*4],2);
                memcpy(&rec_antdelay,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*4],2);
            #ifdef USART_ALL_OUTPUT
                    usart_send[2] = 0x0c;//正常模式 
               usart_send[3] = 8+rec_nearbase_num*4;//数据段长度