zhyinch
2019-12-18 3db912651e297c662fcd190b79d2ca90a34a8f66
Ô´Âë/ºËÐİå/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. */
@@ -474,7 +474,9 @@
}
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;
@@ -589,6 +591,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,7 +609,7 @@
               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 ä¸ºä½Žé€šæ»¤æ³¢å™¨ï¼Œè®©æ•°æ®æ›´ç¨³å®š
               
               /*--------------------------以下为非测距逻辑------------------------*/
@@ -611,18 +617,26 @@
               g_UWB_com_interval = 0;
               dis_after_filter=dist_cm;
               hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
               if(hex_dist-anchor_dist_last_frm[tag_id_recv-TAG_ID_START]<1500||misdist_num>4)
               {
               if(hex_dist-anchor_dist_last_frm[tag_id_recv-TAG_ID_START]<15000||misdist_num>4)
               {int32_t filter_dist;
                  misdist_num=0;
               if(hex_dist<100000&&hex_dist>-1000)
               if(hex_dist<1000000&&hex_dist>-10000)
               {
                  if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
                  {
               if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])<1000)
               if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])<10000)
               {
                  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;
                  #ifdef TDFILTER
                  TrackingDiffUpdate(pUWBDistanceTrackingDiff, (float)hex_dist);
                  filter_dist=pUWBDistanceTrackingDiff->pos_predict/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]=   anchor_dist_last_frm[tag_id_recv-TAG_ID_START];
               }
               his_dist[tag_id_recv-TAG_ID_START]=hex_dist;
               g_flag_Taggetdist[tag_id_recv]=0;