Merge branch '调试' into 定位-临近

zhyinch

2020-12-18 003aff1766ffd3ccfe9df6096ef137340ebfeabf

 Src/application/dw_app.c

@@ -69,7 +69,7 @@
#define FINAL_MSG_FINAL_TX_TS_IDX 18
#define FINAL_MSG_TS_LEN 4

#define STARTPOLL  REGPOLL
#define STARTPOLL  DISCPOLL
enum enumtagstate
{
   REGPOLL,
@@ -458,7 +458,7 @@
      delay_ms(30);

}
int32_t mainbase_dist,nearbase_distlist[MAX_NEARBASE_NUM],true_nearbase_distlist[MAX_NEARBASE_NUM],true_exsistbase_list[MAX_NEARBASE_NUM],ancsync_time,nextpoll_delaytime,offsettimeus;
int32_t mainbase_dist,nearbase_switchdistlist[MAX_NEARBASE_NUM],nearbase_distlist[MAX_NEARBASE_NUM],true_nearbase_distlist[MAX_NEARBASE_NUM],true_exsistbase_list[MAX_NEARBASE_NUM],ancsync_time,nextpoll_delaytime,offsettimeus;
u8 anclost_times=0 , mainbase_lost_count=0;
u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0;
u8 flag_finalsend,flag_getresponse,flag_rxon;
@@ -552,7 +552,7 @@
         {
            memcpy(&minid, &rx_buffer[ANCHOR_ID_IDX], 4);
            tag_state=GETNEARMSG;
            trygetnearmsg_times=5;
            trygetnearmsg_times=10*tag_frequency;
            mindist=temp_dist;
         }
         if(result==0)
@@ -576,7 +576,7 @@
}
   if(getsync_flag==0)
   {
   tagslotpos--;
//   tagslotpos--;
         if(tagslotpos==0||tagslotpos>max_slotpos)
      {      
         tagslotpos=max_slotpos;   
@@ -584,7 +584,7 @@
      tyncpoll_time=(tagslotpos-1)*slottime;
   }
   mainbase_id=minid;
   //mainbase_id = 0x1;
//   mainbase_id = 0x4;
}
u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32],mainbase_type;;
void GetNearMsg(void)
@@ -628,7 +628,7 @@
         tag_state=NEARPOLL;
      }
   }else{
         tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
//         tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
   }
}
extern u8 userkey_state;
@@ -717,21 +717,19 @@
                           memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2);
                           memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);
                           current_count=HAL_LPTIM_ReadCounter(&hlptim1);
//                           memcpy(&tagslotpos,&rx_buffer[TAGSLOTPOS],2);		
                           tmp_time=tmp_time+450;
                           if(tmp_time>999)
                           {
                              tmp_time-=999;
                              sync_timer++;
                              if(sync_timer>=1010)
                                 {sync_timer=0;}
                           }
                        //   TIM3->CNT=tmp_time;
//                           if(tagslotpos>max_slotpos)
//                              tagslotpos=tagslotpos%(max_slotpos+1);
//                              tyncpoll_time=(tagslotpos-1)*slottime;
                           ////////////////////////////
									
                              dwt_forcetrxoff();
                           final_tx_time = (resp_rx_ts + ((RESP_RX_TO_FINAL_TX_DLY_UUS) * UUS_TO_DWT_TIME)) >> 8;
                           final_tx_ts = (((uint64_t)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;//final包实际发送时间是计算时间加上发送天线delay
                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//将T1，T4，T5写入发送数据
                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+nearbase_num*4], resp_rx_ts);
                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
                           tx_near_msg[MESSAGE_TYPE_IDX]=NEAR_FINAL;
                           dwt_writetxdata(28+nearbase_num*4, tx_near_msg, 0);//将发送数据写入DW1000
                           dwt_writetxfctrl(28+nearbase_num*4, 0);//设定发送数据长度
                           dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5
                           result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送	
                           userkey_state = !GET_USERKEY;

                           memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
                           mainbase_dist=temp_dist;
                           if((rx_buffer[MOTORSTATE_INDEX]&0xf)!=3)
@@ -749,11 +747,16 @@
                              para_len = frame_len-22;
                              memcpy(rec_remotepara,&rx_buffer[REMOTEPARA_INDEX],para_len);
                           }
                           flag_getresponse=1;
                           memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX], resp_rx_ts);
								
                           flag_getresponse=1;								
                           //时间同步
                        tmp_time=tmp_time+450;
                           if(tmp_time>999)
                           {
                              tmp_time-=999;
                              sync_timer++;
                              if(sync_timer>=1000)
                                 {sync_timer=0;}
                           }
                           ancsync_time=((sync_timer)*1000+tmp_time);   
                           last_slotnum=current_slotnum;
                           current_slotnum=((float)(sync_timer*10)/(SLOTTIME_MS*bigslot_num))+1;
@@ -762,33 +765,23 @@
                           lastsync_timer=sync_timer;
                           offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset;
                           SetNextPollTime(tagslotpos);

                           if(result==0)
                                 {while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成
                                 { };}								
                              break;
                        }else{
                           
                           rec_nearbasepos=FindNearBasePos(rec_nearbaseid);                           
                           exsistbase_list[rec_nearbasepos]=KEEP_TIMES;
                           memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
                           memcpy(&nearbase_switchdistlist[rec_nearbasepos],&rx_buffer[NR_NEARSWITCH_DISTANCE],2);
                           nearbase_distlist[rec_nearbasepos]=temp_dist;                        
                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)*4], resp_rx_ts);								
                        if(rec_nearbasepos + 1 == nearbase_num)
                        {
                           dwt_forcetrxoff();
                           final_tx_time = (resp_rx_ts + ((RESP_RX_TO_FINAL_TX_DLY_UUS) * UUS_TO_DWT_TIME)) >> 8;
                           final_tx_ts = (((uint64_t)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;//final包实际发送时间是计算时间加上发送天线delay
                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//将T1，T4，T5写入发送数据
									
                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
                           tx_near_msg[MESSAGE_TYPE_IDX]=NEAR_FINAL;
                           dwt_writetxdata(28+nearbase_num*4, tx_near_msg, 0);//将发送数据写入DW1000
                           dwt_writetxfctrl(28+nearbase_num*4, 0);//设定发送数据长度
                           dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5
                           result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送	
                           userkey_state = !GET_USERKEY;
                           if(result==0)
                                 {while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成
                                 { };}								
                              break;
                           }
                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos)*4], resp_rx_ts);
//                           if(temp_dist<nearbase_switchdistlist[rec_nearbasepos]&&nearbase_switchdistlist[rec_nearbasepos]!=0&&temp_dist>0)
//                           {
//                              mainbase_id = rec_nearbaseid;
//                              tag_state = GETNEARMSG;
//                           }
                        
                  }                        
                     }
@@ -808,7 +801,7 @@
      j=0;
         if(exsistbase_list[0]==0)
         {
//            SetNextPollTime(tagslotpos);
            SetNextPollTime(tagslotpos);
         }
      //   tyncpoll_time=0;
         next_nearbase_num=0;
@@ -847,29 +840,30 @@
               }         
            }
         }

         if(true_nearbase_distlist[0]<mainbase_dist-THRESHOLD_CHANGE_MAINBASE_DIST&&true_exsistbase_list[0])
         {
            changemainbase_count++;      
         }else{
            changemainbase_count = 0;
         }
         if(changemainbase_count>3)
         if(changemainbase_count>2*tag_frequency)
         {
            mainbase_id = true_nearbase_idlist[0];
            tag_state = GETNEARMSG;
         }
         if(mainbase_lost_count>3)
         if(mainbase_lost_count>10*tag_frequency)
         {
            tag_state = DISCPOLL;
            tag_state = STARTPOLL;
         }
            for (i=0;i<nearbase_num;i++)
   {
      if(nearbaseid_list[i]==0)
      {
         tag_state = GETNEARMSG;
      }
	
   }
//            for (i=0;i<nearbase_num;i++)
//   {
//      if(nearbaseid_list[i]==0)
//      {
//         tag_state = GETNEARMSG;
//      }
//	
//   }
//串口输出距离         
//         report_num=0;
//   for (i=0;i<last_nearbase_num;i++)
@@ -883,10 +877,10 @@
//               report_num++;
//            }
//         }
         for(i=0;i<MAX_NEARBASE_NUM;i++)
         {
            nearbase_distlist[i]=0x1ffff;
         }
//         for(i=0;i<MAX_NEARBASE_NUM;i++)
//         {
//            nearbase_distlist[i]=0x1ffff;
//         }
         if(para_update)
         {
         uint8_t pack_length = 0,pack_index = 0,pack_msgtype = 0;
@@ -982,6 +976,7 @@
                           mainbase_id=rec_nearbaseid;
                           nearbase_num=rx_buffer[NEARBASENUM_INDEX];
                           memcpy(nearbaseid_list,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2);
                           if(tagslotpos!=255)
                           tag_state = NEARPOLL;
                        }
                     }
@@ -991,9 +986,13 @@
{
   
   //LED0_ON;
   SPIx_CS_GPIO->BRR = SPIx_CS;
   delay_us(700);
   SPIx_CS_GPIO->BSRR = SPIx_CS;
   id =  dwt_readdevid() ;
       while (DWT_DEVICE_ID != id) 
    {
      //   Dw1000_Init();
         id =  dwt_readdevid() ;
    }
   switch(tag_state)
@@ -1015,13 +1014,11 @@
      LED_LG_OFF;
      if(trygetnearmsg_times==0)
      {
         tag_state = DISCPOLL;
         tag_state = STARTPOLL;
      }
         break;
      case NEARPOLL:
         //GPIO_WriteBit(EU_TX_GPIO, EU_RX_PIN, Bit_SET);
      case NEARPOLL:	
         NearPoll();
         //GPIO_WriteBit(EU_TX_GPIO, EU_RX_PIN, Bit_RESET);
         break;
   }
   dwt_entersleep();