1

zhyinch

2021-04-06 246d94e2de20b12287f48f40b750697c33222c91

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

@@ -15,6 +15,7 @@
 */

#include <string.h>
#include <math.h>
#include "dw_app.h"
#include "deca_device_api.h"
#include "deca_regs.h"
@@ -28,7 +29,7 @@
#include <stdio.h>
#include "beep.h"
#include "modbus.h"

//#define DEBUG_OUTPUT
/*------------------------------------ Marcos ------------------------------------------*/
/* Inter-ranging delay period, in milliseconds. */
#define RNG_DELAY_MS 100
@@ -46,16 +47,15 @@
#define POLL_TX_TO_RESP_RX_DLY_UUS 150
/* This is the delay from Frame RX timestamp to TX reply timestamp used for calculating/setting the DW1000's delayed TX function. This includes the
 * frame length of approximately 2.66 ms with above configuration. */
#define RESP_RX_TO_FINAL_TX_DLY_UUS 400
#define RESP_RX_TO_FINAL_TX_DLY_UUS 4100
/* Receive response timeout. See NOTE 5 below. */
#define RESP_RX_TIMEOUT_UUS 600
#define RESP_RX_TIMEOUT_UUS 4700

#define POLL_RX_TO_RESP_TX_DLY_UUS 420
#define POLL_RX_TO_RESP_TX_DLY_UUS 3600
/* This is the delay from the end of the frame transmission to the enable of the receiver, as programmed for the DW1000's wait for response feature. */
#define RESP_TX_TO_FINAL_RX_DLY_UUS 200
#define RESP_TX_TO_FINAL_RX_DLY_UUS 500
/* Receive final timeout. See NOTE 5 below. */
#define FINAL_RX_TIMEOUT_UUS 4300

#define SPEED_OF_LIGHT 299702547

/* Indexes to access some of the fields in the frames defined above. */
@@ -81,6 +81,7 @@
#define ANCTIMEMS             14
#define ANCTIMEUS             16
#define ANCSEND_INTERVAL      18
#define SIGNALPOWER           20

#define POLL                    0x01
#define RESPONSE                0x02
@@ -90,16 +91,16 @@
/*------------------------------------ Variables ------------------------------------------*/
/* Default communication configuration. We use here EVK1000's default mode (mode 3). */
static dwt_config_t config = {
   2,               /* Channel number. */
   DWT_PRF_64M,     /* Pulse repetition frequency. */
   DWT_PLEN_128,    /* Preamble length. */
   DWT_PAC8,        /* Preamble acquisition chunk size. Used in RX only. */
   9,               /* TX preamble code. Used in TX only. */
   9,               /* RX preamble code. Used in RX only. */
   1,               /* Use non-standard SFD (Boolean) */
   DWT_BR_6M8,      /* Data rate. */
   DWT_PHRMODE_STD, /* PHY header mode. */
   (129 + 8 - 8)    /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
    2,               /* Channel number. */
    DWT_PRF_64M,     /* Pulse repetition frequency. */
    DWT_PLEN_1024,   /* Preamble length. */
    DWT_PAC32,       /* Preamble acquisition chunk size. Used in RX only. */
    9,               /* TX preamble code. Used in TX only. */
    9,               /* RX preamble code. Used in RX only. */
    1,               /* Use non-standard SFD (Boolean) */
    DWT_BR_110K,     /* Data rate. */
    DWT_PHRMODE_STD, /* PHY header mode. */
    (1025 + 64 - 32) /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
};

/* Frames used in the ranging process. See NOTE 2 below. */
@@ -109,7 +110,7 @@
static uint8_t tx_final_msg[24] = {0};
   
//static uint8_t rx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
static uint8_t tx_resp_msg[22] = {0};
static uint8_t tx_resp_msg[23] = {0};
//static uint8_t rx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
   
/* Frame sequence number, incremented after each transmission. */
@@ -320,6 +321,26 @@
   dwt_writetxfctrl(sizeof(tx_sync_msg), 0);//设置超宽带发送数据长度
   dwt_starttx(DWT_START_TX_IMMEDIATE);
}
 double firstpath_power, rx_power,rec_firstpath_power;
  double f1, f2, r1, r2;
uint16_t F1,F2,F3,N,C;
double B = 131072;
double A = 121.74;
double min_power;
 dwt_rxdiag_t d1;
double LOS(dwt_rxdiag_t *dia) {
     F1 = dia->firstPathAmp1;
     F2 = dia->firstPathAmp2;
     F3 = dia->firstPathAmp3;
     N = dia->rxPreamCount;
     C = dia->maxGrowthCIR;
    
    firstpath_power=10* log10((F1*F1+F2*F2+F3*F3)/(N*N))-A;
//    rx_power=10*log10(C*B/(N*N))-A;

 //   min_power =  - 10 * log10((F1 *F1 + F2 * F2 + F3 * F3) / (C *B));
    return min_power;
  }
uint16_t g_Resttimer;
uint8_t result;
u8 tag_succ_times=0;
@@ -329,6 +350,7 @@
extern uint16_t sync_timer;
u16 tmp_time;
extern float dw_vbat;
extern u16 slottime,max_slotnum,current_slotpos,tyncpoll_time;
void Tag_App(void)//发送模式(TAG标签)
{
   uint32_t frame_len;
@@ -338,7 +360,6 @@
   u8 bat_percent;
   //LED0_ON;
   dwt_forcetrxoff();
   g_Resttimer=0;
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);      
   tag_succ_times = 0;
@@ -361,6 +382,9 @@
    * set by dwt_setrxaftertxdelay() has elapsed. */
   dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置
   start_poll = time32_incr;
               #ifdef DEBUG_OUTPUT
        printf("P包发送,基站ID: %d .\r\n",i);
            #endif
   /* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */
   while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误
   { if(time32_incr - start_poll>20)
@@ -411,6 +435,7 @@
         TIM3->CNT=tmp_time;
      }
         memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);
            rec_firstpath_power = rx_buffer[SIGNALPOWER];
         memcpy(&tx_final_msg[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 4);
         memcpy(&rec_com_interval,&rx_buffer[ANCSEND_INTERVAL],  2);
         if(rec_com_interval>4&&rec_com_interval!=g_com_map[COM_INTERVAL])
@@ -439,16 +464,20 @@
         dwt_writetxfctrl(sizeof(tx_final_msg), 0);//设定发送数据长度
         result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送
         
             #ifdef DEBUG_OUTPUT
            printf("F包发送,基站ID: %d .\r\n",i);
            #endif
            
         tag_succ_times++;
         
            LED0_BLINK;
				
            g_Resttimer=0;
               memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
               if(hex_dist2!=0xffff)
               {   
               g_Tagdist[anc_id_recv]=   hex_dist2;
               g_flag_Taggetdist[anc_id_recv]=0;
					

               if(!g_com_map[MODBUS_MODE])
               {
               hex_dist2 = hex_dist2;      
@@ -461,6 +490,7 @@
               memcpy(&usart_send[9],&hex_dist2,4);
               usart_send[13] = bat_percent;
               usart_send[14] = button;
                    usart_send[15] = rec_firstpath_power;
               checksum = Checksum_u16(&usart_send[2],17);
               memcpy(&usart_send[19],&checksum,2);
               UART_PushFrame(usart_send,21);
@@ -469,7 +499,9 @@
      //         memcpy(&Modbus_HoldReg[anc_id_recv*2],&hex_dist,4);
         /* Poll DW1000 until TX frame sent event set. See NOTE 8 below. */
         if(result==0)
         {while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成
         {
			
            while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成
         { };
      }
         /* Clear TXFRS event. */
@@ -486,6 +518,9 @@
   }
   else
   {
          #ifdef DEBUG_OUTPUT
            printf("R包失败错误信息: %x .\r\n",status_reg);
            #endif
      /* Clear RX error events in the DW1000 status register. */
      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
      random_delay_tim = DFT_RAND_DLY_TIM_MS;
@@ -493,19 +528,21 @@
//   deca_sleep(10);
}
//   dwt_entersleep();
//   if(tag_succ_times<g_com_map[MIN_REPORT_ANC_NUM])
//   {
//   //poll_timer +=time32_incr&0x7+3;
//   }
   if(tag_succ_times<1)
   {
      tyncpoll_time=(current_slotpos--%max_slotnum)*slottime;
   }

   /* Execute a delay between ranging exchanges. */
   
}

int8_t correction_time;
extern uint8_t sync_seq;
#define TDFILTER
//#define CHECK_UID
extern uint8_t UID_ERROR;
extern u16 dist_threshold;
u8 misdist_num[TAG_NUM_IN_SYS];
void Anchor_App(void)
{
@@ -521,7 +558,6 @@
   while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))&&!g_start_send_flag&&!g_start_sync_flag)//不断查询芯片状态直到接收成功或者出现错误
   { 
      IdleTask();
      g_Resttimer=0;
   };

   if (status_reg & SYS_STATUS_RXFCG)//成功接收
@@ -573,7 +609,8 @@
         /* Write and send the response message. See NOTE 9 below.*/
         if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
         memcpy(&tx_resp_msg[DIST_IDX], &g_Tagdist[tag_id_recv], 4);
		
            tx_resp_msg[SIGNALPOWER] = firstpath_power;
            
         dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//写入发送数据
         dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//设定发送长度
         result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送，等待接收
@@ -583,6 +620,9 @@
         frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
         /* We assume that the transmission is achieved correctly, now poll for reception of expected "final" frame or error/timeout.
          * See NOTE 7 below. */
            #ifdef DEBUG_OUTPUT
            printf("收到POLL包，标签ID: %d .\r\n",tag_id_recv);
            #endif
         if(result==0)
         {
            while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))///不断查询芯片状态直到接收成功或者出现错误
@@ -640,15 +680,19 @@

               dist_cm = dist_no_bias * 1000; //dis ä¸ºå•ä½ä¸ºcm的距离
//               dist[TAG_ID] = LP(dis, TAG_ID); //LP ä¸ºä½Žé€šæ»¤æ³¢å™¨ï¼Œè®©æ•°æ®æ›´ç¨³å®š
					
               dwt_readdiagnostics(&d1);
               LOS(&d1);
               /*--------------------------以下为非测距逻辑------------------------*/
             #ifdef DEBUG_OUTPUT
            printf("收到FINAL包，标签ID: %d .\r\n",tag_id_recv);
            #endif
               LED0_BLINK; //每成功一次通讯则闪烁一次
               g_UWB_com_interval = 0;
					
               g_Resttimer=0;
               hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10;
               if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
               {
               if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])<15000||misdist_num[tag_id_recv-TAG_ID_START]>4)
               if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])<dist_threshold||misdist_num[tag_id_recv-TAG_ID_START]>4)
               {
                  int32_t filter_dist;
                  misdist_num[tag_id_recv-TAG_ID_START]=0;
@@ -676,6 +720,7 @@
                  memcpy(&usart_send[9],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
                  usart_send[13] = battary;
                  usart_send[14] = button;
                        usart_send[15] = firstpath_power;
                  checksum = Checksum_u16(&usart_send[2],17);
                  memcpy(&usart_send[19],&checksum,2);
                  UART_PushFrame(usart_send,21);
@@ -694,6 +739,9 @@
            }
         }else{
            /* Clear RX error events in the DW1000 status register. */
            #ifdef DEBUG_OUTPUT
            printf("F包失败错误信息: %x .\r\n",status_reg);
            #endif
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
         }
      }else if(rx_buffer[MESSAGE_TYPE_IDX] == SYNC)
@@ -709,6 +757,9 @@
   }
   else
   {
         #ifdef DEBUG_OUTPUT
            printf("P包失败错误信息: %x .\r\n",status_reg);
            #endif
      /* Clear RX error events in the DW1000 status register. */
      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
   }