增加基站转点距离，增加主基站最后回复。

zhyinch

2020-11-12 f603e50972a701a41c17fc97c9c82f85b6639200

 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  DISCPOLL
#define STARTPOLL  REGPOLL
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;
@@ -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,32 +765,22 @@
                           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;
                           }
                        
                  }                        
@@ -811,65 +804,66 @@
//            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]--;
            }
         }
//         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]--;
//            }
//         }

         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;
      }
	
   }
//         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++)
@@ -883,26 +877,26 @@
//               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;
            pack_msgtype = rec_remotepara[0];
            pack_index = rec_remotepara[1];
            pack_length = rec_remotepara[2];
            if(pack_msgtype==2)
            {
                  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();
            }
         }
//         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;
//            pack_msgtype = rec_remotepara[0];
//            pack_index = rec_remotepara[1];
//            pack_length = rec_remotepara[2];
//            if(pack_msgtype==2)
//            {
//                  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);