测试40米异常bug

zhyinch

2019-11-27 cdd16c76f1af9803f8015fd928ff9c6deda8de65

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

@@ -111,7 +111,7 @@
//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. */
static uint32_t frame_seq_nb = 0;	
static uint32_t frame_seq_nb = 0,frame_seq_nb2=0;	
   
/* Hold copy of status register state here for reference, so reader can examine it at a breakpoint. */
static uint32_t status_reg = 0;
@@ -134,7 +134,7 @@

static double tof;
   
uint32_t anchor_dist_last_frm[TAG_NUM_IN_SYS],his_dist[TAG_NUM_IN_SYS];   ;	
int32_t anchor_dist_last_frm[TAG_NUM_IN_SYS],his_dist[TAG_NUM_IN_SYS];   ;	
uint32_t tag_id = 0;
uint32_t tag_id_recv = 0;
uint8_t random_delay_tim = 0;
@@ -145,7 +145,7 @@
float dis_after_filter;            //当前距离值
LPFilter_Frac* p_Dis_Filter;      //测距用的低通滤波器

uint16_t g_Tagdist[TAG_NUM_IN_SYS];
int32_t g_Tagdist[TAG_NUM_IN_SYS];
uint8_t g_flag_Taggetdist[256];
/*------------------------------------ Functions ------------------------------------------*/

@@ -313,7 +313,7 @@
uint16_t g_Resttimer;
uint8_t result;
u8 tag_succ_times=0;
int32_t hex_dist;
int32_t hex_dist,hex_dist2;
u16 checksum;
int8_t tag_delaytime;
extern uint16_t sync_timer;
@@ -350,7 +350,7 @@
   while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误
   { if(time32_incr - start_poll>20)
      NVIC_SystemReset();
      UART_CheckReceive();
      IdleTask();
      
   };

@@ -395,7 +395,7 @@
         }
         TIM3->CNT=tmp_time;
      }
         memcpy(&anchor_dist_last_frm[0], &rx_buffer[DIST_IDX], 4);
         memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);
         memcpy(&tx_final_msg[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 4);
         /* Compute final message transmission time. See NOTE 9 below. */
         final_tx_time = (resp_rx_ts + (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//计算final包发送时间，T5=T4+Treply2
@@ -418,7 +418,7 @@
         tag_succ_times++;
         
            LED0_BLINK;
            hex_dist = anchor_dist_last_frm[0];
				
               memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
//               g_Tagdist[anc_id_recv]=   hex_dist;
//               g_flag_Taggetdist[anc_id_recv]=0;
@@ -430,7 +430,7 @@
               memcpy(&usart_send[5],&dev_id,2);
               memcpy(&usart_send[7],&rx_buffer[ANCHOR_ID_IDX],2);
            
               memcpy(&usart_send[9],&hex_dist,4);
               memcpy(&usart_send[9],&hex_dist2,4);
               usart_send[13] = battary;
               usart_send[14] = button;
               checksum = Checksum_u16(&usart_send[2],17);
@@ -549,7 +549,7 @@

         battary = rx_buffer[BATTARY_IDX];
         button = rx_buffer[BUTTON_IDX];
         frame_seq_nb = rx_buffer[SEQUENCE_IDX];
         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. */
         if(result==0)
@@ -610,21 +610,26 @@
               LED0_BLINK; //每成功一次通讯则闪烁一次
               g_UWB_com_interval = 0;
               dis_after_filter=dist_cm;
               hex_dist = (int16_t)(dist_cm+g_com_map[DIST_OFFSET]);
               hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
               if(hex_dist>1000)
               {hex_dist=hex_dist;}
               if(hex_dist<100000&&hex_dist>-1000)
               {
                  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;
               }
               his_dist[tag_id_recv-TAG_ID_START]=hex_dist;
               g_Tagdist[tag_id_recv]=   hex_dist;
               g_flag_Taggetdist[tag_id_recv]=0;
               if(!g_com_map[MODBUS_MODE])
               {
                  usart_send[2] = 1;//正常模式
                  usart_send[3] = 17;//数据段长度
                  usart_send[4] = frame_seq_nb;//数据段长度
                  usart_send[4] = frame_seq_nb2;//数据段长度
                  memcpy(&usart_send[5],&tag_id_recv,2);
                  memcpy(&usart_send[7],&dev_id,2);
                  
@@ -635,11 +640,14 @@
                  memcpy(&usart_send[19],&checksum,2);
                  UART_PushFrame(usart_send,21);
               }   
                  memcpy(&Modbus_HoldReg[tag_id_recv*2],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
					
					
               //   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];
               //dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm);

                  }
               }else{
                  LED0_BLINK;
               }						
            }
         }else{
            /* Clear RX error events in the DW1000 status register. */