zhyinch
2020-03-29 5f2e06850028e286bd95cfd003dd0682af267be3
Ô´Âë/ºËÐİå/Src/application/dw_app.c
@@ -94,7 +94,7 @@
   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. */
   0,               /* Use non-standard SFD (Boolean) */
   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. */
@@ -238,7 +238,7 @@
      for(i=0;i<255;i++)
      {
         g_flag_Taggetdist[i]++;
         if(g_flag_Taggetdist[i]>=2)
         if(g_flag_Taggetdist[i]>=20)
         {
            g_Tagdist[i]=0xffff;
         }
@@ -274,6 +274,10 @@
   tx_resp_msg[MESSAGE_TYPE_IDX]=RESPONSE;
   tx_final_msg[MESSAGE_TYPE_IDX]=FINAL;
   tx_sync_msg[MESSAGE_TYPE_IDX]=SYNC;
   memcpy(&tx_poll_msg[GROUP_ID_IDX], &group_id, 1);
   memcpy(&tx_final_msg[GROUP_ID_IDX], &group_id, 1);
   memcpy(&tx_resp_msg[GROUP_ID_IDX], &group_id, 1);
   
   memcpy(&tx_poll_msg[TAG_ID_IDX], &dev_id, 4);
   memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 4);
@@ -374,7 +378,7 @@
      /* Check that the frame is the expected response from the companion "DS TWR responder" example.
       * As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
      
      if (rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //判断接收到的数据是否是response数据
      if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //判断接收到的数据是否是response数据
      { u16 anc_id_recv;
         /* Retrieve poll transmission and response reception timestamp. */
         poll_tx_ts = get_tx_timestamp_u64();                              //获得POLL发送时间T1
@@ -422,8 +426,10 @@
               memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
//               g_Tagdist[anc_id_recv]=   hex_dist;
//               g_flag_Taggetdist[anc_id_recv]=0;
               if(!g_com_map[MODBUS_MODE])
               if(!g_com_map[MODBUS_MODE]&&hex_dist2!=0xffff)
               {
               hex_dist2 = hex_dist2+(int16_t)g_com_map[DIST_OFFSET];
               usart_send[2] = 1;//正常模式
               usart_send[3] = 17;//数据段长度
               usart_send[4] = frame_seq_nb;//数据段长度
@@ -464,22 +470,24 @@
//   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<g_com_map[MIN_REPORT_ANC_NUM])
//   {
//   //poll_timer +=time32_incr&0x7+3;
//   }
   /* 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;
void Anchor_App(void)
{
   uint32_t frame_len;
   uint32_t resp_tx_time;
   static u8 misdist_num;
   /* Clear reception timeout to start next ranging process. */
   dwt_setrxtimeout(0);//设定接收超时时间,0位没有超时时间
@@ -519,7 +527,7 @@
//         tag_recv_interval = tag_recv_timer + 65535 - tag_time_recv[tag_id_recv];
//      }
      
      if (rx_buffer[MESSAGE_TYPE_IDX] == POLL&&(anchor_type == rx_buffer[ANC_TYPE_IDX])) //判断是否是poll包数据
      if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == POLL&&(anchor_type == rx_buffer[ANC_TYPE_IDX])) //判断是否是poll包数据
      {
         tmp_time=TIM3->CNT;
         memcpy(&tx_resp_msg[ANCTIMEMS],&sync_timer,2);
@@ -541,7 +549,7 @@
         /* 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], &anchor_dist_last_frm[tag_id_recv-TAG_ID_START], 4);
         memcpy(&tx_resp_msg[DIST_IDX], &g_Tagdist[tag_id_recv], 4);
      
         dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//写入发送数据
         dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//设定发送长度
@@ -573,7 +581,7 @@
            /* Check that the frame is a final message sent by "DS TWR initiator" example.
             * As the sequence number field of the frame is not used in this example, it can be zeroed to ease the validation of the frame. */
         
            if (rx_buffer[MESSAGE_TYPE_IDX] == FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,4)&&!memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,4)) //判断是否为Final包
            if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,4)&&!memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,4)) //判断是否为Final包
            {
               uint32_t poll_tx_ts, resp_rx_ts, final_tx_ts;
               uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
@@ -589,6 +597,10 @@
               final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts);
               final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);
               #ifdef CHECK_UID
               if(UID_ERROR==1)
                  poll_rx_ts=0;
               #endif
               /* Compute time of flight. 32-bit subtractions give correct answers even if clock has wrapped. See NOTE 10 below. */
               poll_rx_ts_32 = (uint32_t)poll_rx_ts;//使用32位数据计算
               resp_tx_ts_32 = (uint32_t)resp_tx_ts;
@@ -603,24 +615,30 @@
               distance = tof * SPEED_OF_LIGHT;//距离=光速*飞行时间
               dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //距离减去矫正系数
               dist_cm = dist_no_bias * 100; //dis ä¸ºå•位为cm的距离
               dist_cm = dist_no_bias * 1000; //dis ä¸ºå•位为cm的距离
//               dist[TAG_ID] = LP(dis, TAG_ID); //LP ä¸ºä½Žé€šæ»¤æ³¢å™¨ï¼Œè®©æ•°æ®æ›´ç¨³å®š
               
               /*--------------------------以下为非测距逻辑------------------------*/
               LED0_BLINK; //每成功一次通讯则闪烁一次
               g_UWB_com_interval = 0;
               dis_after_filter=dist_cm;
               hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
               if(hex_dist<100000&&hex_dist>-1000)
               hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10;
               if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
               {
                  if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
                  {
               if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])<1000)
               {
                  g_Tagdist[tag_id_recv-TAG_ID_START] = hex_dist;
                  anchor_dist_last_frm[tag_id_recv-TAG_ID_START] = hex_dist;
                  g_Tagdist[tag_id_recv]=   hex_dist;
               }
               if(hex_dist-his_dist[tag_id_recv-TAG_ID_START]<15000||misdist_num>4)
               {int32_t filter_dist;
                  misdist_num=0;
               if(hex_dist<1000000&&hex_dist>-10000)
               {
                  #ifdef TDFILTER
                  NewTrackingDiffUpdate(tag_id_recv-TAG_ID_START, (float)hex_dist);
                  filter_dist=pos_predict[tag_id_recv-TAG_ID_START]/10;
                  #else
                  filter_dist=hex_dist/10;
                  #endif
                  anchor_dist_last_frm[tag_id_recv-TAG_ID_START]=filter_dist;
                  g_Tagdist[tag_id_recv]=   filter_dist;
               his_dist[tag_id_recv-TAG_ID_START]=hex_dist;
               g_flag_Taggetdist[tag_id_recv]=0;
               if(!g_com_map[MODBUS_MODE])
@@ -639,13 +657,15 @@
                  UART_PushFrame(usart_send,21);
               }   
               //   memcpy(&Modbus_HoldReg[tag_id_recv*2],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
               Modbus_HoldReg[tag_id_recv*2]=anchor_dist_last_frm[tag_id_recv-TAG_ID_START]>>16;
               Modbus_HoldReg[tag_id_recv*2+1]=anchor_dist_last_frm[tag_id_recv-TAG_ID_START];
               Modbus_HoldReg[tag_id_recv*2]=g_Tagdist[tag_id_recv-TAG_ID_START]>>16;
               Modbus_HoldReg[tag_id_recv*2+1]=g_Tagdist[tag_id_recv-TAG_ID_START];
               //dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm);
                  }
               }else{
                  LED0_BLINK;
               }
                  misdist_num++;
               }
            }
            }
         }else{
            /* Clear RX error events in the DW1000 status register. */