zhyinch
2020-01-13 9b5beb905d2c3846b912adcce8051ce4ea3377de
Ô´Âë/ºËÐİå/Src/application/dw_app.c
@@ -422,7 +422,8 @@
               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])
         hex_dist2 = hex_dist2+(int16_t)g_com_map[DIST_OFFSET];
               if(!g_com_map[MODBUS_MODE]&&hex_dist2!=65555)
               {
               usart_send[2] = 1;//正常模式
               usart_send[3] = 17;//数据段长度
@@ -475,7 +476,7 @@
int8_t correction_time;
extern uint8_t sync_seq;
#define TDFILTER
//#define CHECK_UID
#define CHECK_UID
extern uint8_t UID_ERROR;
void Anchor_App(void)
{
@@ -617,17 +618,13 @@
               g_UWB_com_interval = 0;
               
               hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10;
               if(hex_dist-anchor_dist_last_frm[tag_id_recv-TAG_ID_START]<15000||misdist_num>4)
               if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
               {
               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)
               {
                  if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
                  {
               if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])<10000)
               {
                  //anchor_dist_last_frm[tag_id_recv-TAG_ID_START] = hex_dist;
               {
                  #ifdef TDFILTER
                  NewTrackingDiffUpdate(tag_id_recv-TAG_ID_START, (float)hex_dist);
                  filter_dist=pos_predict[tag_id_recv-TAG_ID_START]/10;
@@ -635,8 +632,8 @@
                  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];
               }
                  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])
@@ -655,15 +652,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{
                  misdist_num++;
               }
            }
            }
         }else{
            /* Clear RX error events in the DW1000 status register. */