V2.18 增加转点切换和距离切换2钟模式，测试长时间是否丢失临近基站

zhyinch

2020-12-23 f520882cbe60829964cbed519ce4fb0484f90cd9

 Src/application/dw_app.c

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

#define STARTPOLL  DISCPOLL
#define STARTPOLL  REGPOLL
#define SWITCHBASE_DIST
//#define SWITCHBASE_ZHUANDIAN
enum enumtagstate
{
   REGPOLL,
@@ -552,7 +554,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 +578,7 @@
}
   if(getsync_flag==0)
   {
   tagslotpos--;
//   tagslotpos--;
         if(tagslotpos==0||tagslotpos>max_slotpos)
      {      
         tagslotpos=max_slotpos;   
@@ -584,7 +586,7 @@
      tyncpoll_time=(tagslotpos-1)*slottime;
   }
   mainbase_id=minid;
//   mainbase_id = 0x9804;
//   mainbase_id = 0x4;
}
u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32],mainbase_type;;
void GetNearMsg(void)
@@ -628,16 +630,17 @@
         tag_state=NEARPOLL;
      }
   }else{
         tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
//         tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
   }
}
extern u8 userkey_state;
extern float motor_keeptime;
uint8_t changemainbase_count=0,gotosleep_flag;
uint32_t frame_len;
void NearPoll(void)
{
   
   uint32_t temp1,temp2,dw_systime;
   uint32_t temp1,temp2,dw_systime,salvebase_mindist=999999,mindist_slavebaseid;

   uint32_t final_tx_time;
   u32 start_poll;
@@ -650,7 +653,7 @@
   {
      next_nearbase_num = MAX_NEARBASE_NUM-1;
   }
   HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_SET);
//   HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_SET);

   recbase_num=0;
//   motor_state=0;
@@ -777,12 +780,18 @@
                           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)*4], resp_rx_ts);
                           if(temp_dist<nearbase_switchdistlist[rec_nearbasepos]&&nearbase_switchdistlist[rec_nearbasepos]!=0&&temp_dist!=0)
                           if(temp_dist<salvebase_mindist)
                           {
                              salvebase_mindist = temp_dist;
                              mindist_slavebaseid = rec_nearbaseid;
                           }
                           #ifdef SWITCHBASE_ZHUANDIAN
                           if(temp_dist<nearbase_switchdistlist[rec_nearbasepos]&&nearbase_switchdistlist[rec_nearbasepos]!=0&&temp_dist>0)
                           {
                              mainbase_id = rec_nearbaseid;
                              tag_state = GETNEARMSG;
                           }
								
                        #endif
                  }                        
                     }
                  }else{
@@ -798,89 +807,27 @@
         }
         dwt_forcetrxoff();
dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
      j=0;
         if(exsistbase_list[0]==0)

         if(mainbase_lost_count==0)
         {
//            SetNextPollTime(tagslotpos);
         }
      //   tyncpoll_time=0;
//         next_nearbase_num=0;
//         for(i=0;i<nearbase_num;i++)
//         {
//            if(exsistbase_list[i]>0)
//            {		
//               next_nearbase_num++;
//               true_exsistbase_list[j]=exsistbase_list[i];
//               true_nearbase_idlist[j]=nearbaseid_list[i];			
//               true_nearbase_distlist[j++]=nearbase_distlist[i];
//               exsistbase_list[i]--;
//            }
//         }
            #ifdef SWITCHBASE_DIST
            if(salvebase_mindist<mainbase_dist- THRESHOLD_CHANGE_MAINBASE_DIST )
            {
               changemainbase_count++;
               if(changemainbase_count>tag_frequency*2)
               {
                              mainbase_id = mindist_slavebaseid;
                              tag_state = GETNEARMSG;
               }
            }else{
               changemainbase_count = 0;
            }
         #endif
         }else if(mainbase_lost_count>10*tag_frequency)
         {
            tag_state = STARTPOLL;
         }			

//         if(recbase_num<3)
//         {next_nearbase_num=next_nearbase_num;}
//               last_nearbase_num = next_nearbase_num;
//         for(i=0;i<last_nearbase_num-1;i++)
//         {
//            for(j=0;j<last_nearbase_num-1;j++)
//            {
//               if(true_nearbase_distlist[j]>true_nearbase_distlist[j+1])
//               {
//                  u32 temp_dist,temp_id,temp_exsis;
//                  temp_dist=true_nearbase_distlist[j];
//                  temp_id = true_nearbase_idlist[j];
//                  temp_exsis=true_exsistbase_list[i];
//                  true_nearbase_distlist[j]=true_nearbase_distlist[j+1];
//                  true_nearbase_idlist[j]=true_nearbase_idlist[j+1];
//                  true_exsistbase_list[j]=true_exsistbase_list[j+1];
//						
//                  true_nearbase_distlist[j+1]=temp_dist;
//                  true_nearbase_idlist[j+1]=temp_id;
//                  true_exsistbase_list[j+1]=temp_exsis;
//               }			
//            }
//         }

//         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)
//         {
//            mainbase_id = true_nearbase_idlist[0];
//            tag_state = GETNEARMSG;
//         }
//         if(mainbase_lost_count>3)
//         {
//            tag_state = DISCPOLL;
//         }
//            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++)
//         {
//            nearbaseid_list[i]=true_nearbase_idlist[i];
//            nearbase_distlist[i]=true_nearbase_distlist[i];
//            if(nearbase_distlist[i]!=0x1ffff&&true_exsistbase_list[i]==0xa)
//            {
//               memcpy(&usart_send[4+6*report_num],&nearbaseid_list[i],2);
//               memcpy(&usart_send[6+6*report_num],&nearbase_distlist[i],4);
//               report_num++;
//            }
//         }
//         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;
@@ -889,16 +836,21 @@
            pack_length = rec_remotepara[2];
            if(pack_msgtype==2)
            {
               if( pack_index == 0x10)
               {
               motor_keeptime = rec_remotepara[3];
               }else{
                  memcpy((uint8_t*)&g_com_map + pack_index, &rec_remotepara[3], pack_length);
               //返回一个error状态
               //SendComMap(pack_datalen,pack_index);
               save_com_map_to_flash();
               delay_ms(100);
               NVIC_SystemReset();
               }
            }
         }
   dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_RESET);
//HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_RESET);

}
extern uint8_t module_power,imu_enable,motor_enable;
@@ -976,6 +928,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;
                        }
                     }
@@ -985,10 +938,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();
      //   Dw1000_Init();
         id =  dwt_readdevid() ;
    }
   switch(tag_state)
@@ -1010,13 +966,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();