增加时间同步功能，和ID最小的基站同步，同步代码和定位相同

yincheng.zhong

2023-09-25 62c98aa748df7e6393958c7250afc8c3b5c69807

 Src/application/dw_app.c

@@ -78,7 +78,7 @@
   NEARPOLL,
}tag_state=GETNEARMSG;
static dwt_config_t config = {
   2,               /* Channel number. */
    5,               /* Channel number. */
   DWT_PRF_64M,     /* Pulse repetition frequency. */
   DWT_PLEN_128,    /* Preamble length. */
   DWT_PAC8,        /* Preamble acquisition chunk size. Used in RX only. */
@@ -234,7 +234,7 @@
extern uint16_t sync_timer;
u16 tmp_time;
int32_t temp_dist;
u16 tagslotpos;
u16 tagslotpos,rec_tagslotpos;

u16 anclist_num=0,anclist_pos; //list 总数量和当前位置
u16 ancid_list[TAG_NUM_IN_SYS];
@@ -291,12 +291,98 @@
u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0;
u8 flag_finalsend,flag_getresponse,flag_rxon;
uint16_t current_count,start_count,end_count,lastsync_timer;
int16_t target_offsettime = 850;
u32 target_time=100;
u8 pd_i;
u32 temptime,delaytime,expect_anctime;
int32_t error_time;
u16 delaycount,slotpos_intoatl;
int16_t poll_offsettime=-6900,clockoffset;
extern u16 slotpos,bigslot_num;
void NextPollDelay(u32 anchor_time)
{
    //tagslotpos = 0;
    error_time = anchor_time-expect_anctime;
    if(error_time<2000&&error_time>0)
        clockoffset += (target_offsettime-error_time)*0.1;
    for(pd_i=0; pd_i<tag_frequency; pd_i++)
    {
        slotpos_intoatl = pd_i*bigslot_num+tagslotpos;
        temptime = (slotpos_intoatl*SLOTTIME_MS+100)*100;
        if(anchor_time<temptime-5000)
        {
            current_slotnum = pd_i;
            delaytime = temptime-anchor_time+poll_offsettime+clockoffset;
            expect_anctime = temptime;
            return ;
        }
    }
    temptime = (tagslotpos*SLOTTIME_MS+100+10000)*100;
    expect_anctime = temptime-1000000;
    delaytime = temptime-anchor_time+poll_offsettime+clockoffset;
    return ;
}
u16 lpcount,poll_startcount,last_lpcount;
u16 waketopolltimeus = 4500;
#define BASENUM_COMTIME 244
u32 last_anchor_time;
void SetLPTimer(u32 anchor_time)
{
    NextPollDelay(anchor_time);
    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
    lpcount = current_count+delaytime/LPTIMER_LSB+(10-nearbase_num)*8;
    poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
    if(poll_startcount>=32768)
    {
        poll_startcount -=32768;
    }
    if(lpcount>=32768)
    {
        lpcount -=32768;
    }
    __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
    last_lpcount = lpcount;
}

void NextSlotDelayMs(int16_t delayms)
{
    current_slotnum++;
    if(current_slotnum>=tag_frequency)
    {
        current_slotnum = 0;
        lpcount = last_lpcount+31457/tag_frequency+1311+32.767*delayms;//(float)(delayms+40)*32.767 ;//(200/tag_frequency+delayms)*4.8*1000/LPTIMER_LSB
    } else {
        lpcount = last_lpcount+31457/tag_frequency+32.767*delayms;//(float)(200/tag_frequency*48+delayms*10)*3.3 ;
    }
    if(lpcount>=32768)
    {
        lpcount -=32768;
    }
    poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
    if(poll_startcount>=32768)
    {
        poll_startcount -=32768;
    }
    __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
    last_lpcount = lpcount;
}
u8 nearbase_num=0,last_nearbase_num,next_nearbase_num,last_slotnum, para_update,para_len,stationary_flag=0;
u32 rec_tagpos_binary;
int16_t offset=2700;
extern int16_t g_commap_antdelay;
u8 motor_state,rec_remotepara_state,rec_remotepara[80];
u16 sync_anc_id = 0xffff,sync_anc_losttime = 0;
int32_t ancsync_time,tpos;
void SyncAncUpdate(u16 ancid,int32_t anctime,u16 slotpos)
{
   if(ancid<=sync_anc_id)
   {
      SetLPTimer(ancsync_time);
      tagslotpos = slotpos;
      sync_anc_losttime = 0;
      sync_anc_id = ancid;	
   }
}
void NearPoll(void)
{
   
@@ -308,7 +394,7 @@
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);      
   tag_succ_times = 0;
      //GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_SET);
    //
   if(next_nearbase_num>=MAX_NEARBASE_NUM)
   {
      next_nearbase_num = MAX_NEARBASE_NUM-1;
@@ -317,6 +403,10 @@
   last_nearbase_num=next_nearbase_num;
   nearbase_num=next_nearbase_num;
   recbase_num=0;
      if(sync_anc_losttime++>10)
      {
         sync_anc_id = 0xffff;
      }
//   motor_state=0;
   tx_near_msg[GROUP_ID_IDX] = group_id;
   tx_near_msg[BATTARY_IDX] = bat_percent;
@@ -329,8 +419,19 @@
   memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);   
   dwt_writetxdata(15+4*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
   dwt_writetxfctrl(15+4*nearbase_num, 0);//设置超宽带发送数据长度
   dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置
   
      current_count=HAL_LPTIM_ReadCounter(&hlptim1);		
      while(current_count<poll_startcount||current_count>poll_startcount+16384)
      {
         current_count=HAL_LPTIM_ReadCounter(&hlptim1);
         if(current_count<poll_startcount-300)
         {
            break;
         }
      }
		
    dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置
      HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_SET);
   tx_near_msg[TAGCONFIGSUCCESS_INDEX] =0;
   para_update = 0;
   flag_finalsend=0;
@@ -341,7 +442,9 @@
   timeout=ceil((float)nearbase_num*SLOT_SCALE)+3;
   end_count=start_count+(timeout<<5);
   if(end_count>=32768)
   {end_count-=32768;}
    {
        end_count-=32768;
    }
   mainbase_dist=100000;
   mainbase_lost_count++;
   current_count=HAL_LPTIM_ReadCounter(&hlptim1);
@@ -398,7 +501,7 @@
                           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);		
                              memcpy(&rec_tagslotpos,&rx_buffer[TAGSLOTPOS],2);
                           tmp_time=tmp_time+450;
                           if(tmp_time>999)
                           {
@@ -407,6 +510,8 @@
                              if(sync_timer>=1010)
                                 {sync_timer=0;}
                           }
                              ancsync_time=((sync_timer)*1000+tmp_time);																			
                              SyncAncUpdate(rec_nearbaseid,ancsync_time,rec_tagslotpos);
                        //   TIM3->CNT=tmp_time;
//                           if(tagslotpos>max_slotpos)
//                              tagslotpos=tagslotpos%(max_slotpos+1);
@@ -445,10 +550,12 @@
                           last_slotnum=current_slotnum;
                           current_slotnum=((float)sync_timer/g_com_map[COM_INTERVAL])+1;
                           if(current_slotnum==last_slotnum-1)
                           {flag_getresponse=1;}
                    {
                        flag_getresponse=1;
                    }
                           lastsync_timer=sync_timer;
                           offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset;
                           SetNextPollTime(tyncpoll_time);
//                           SetNextPollTime(tyncpoll_time);
                           if(rec_remotepara_state==1)
                           {
                                      memcpy(rec_remotepara,&rx_buffer[REMOTEPARA_INDEX],REMOTEPARA_LEN);
@@ -461,8 +568,22 @@
                                        NVIC_SystemReset();
                           } 
                        }else{
                           rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
                             memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2);
                    memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);
                    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
                              memcpy(&rec_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;}
                              }
                              ancsync_time=((sync_timer)*1000+tmp_time);																			
                              SyncAncUpdate(rec_nearbaseid,ancsync_time,rec_tagslotpos);
                           
                    rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
                              if(rec_nearbasepos>=last_nearbase_num)          //发现新的基站
                           {
                              get_newbase=1;
@@ -498,22 +619,15 @@
               //   dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
         }
         if(mainbase_lost_count>tag_frequency*BASELOST_STOPMOTOR_TIME)
         {motor_state=0; }
    {
        motor_state=0;
    }
            dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
         nearbase_num=recbase_num;
            j=0;
         if(exsistbase_list[0]==0)
      if(sync_anc_losttime!=0)
         {
//          u8 temp_adc,random_value;
//            random_value=0;
//            for(i=0;i<8;i++)
//            {
//               temp_adc=Get_ADC_Value();
//               random_value=random_value|((temp_adc&0x01)<<i);
//            }
            tagslotpos=GetRandomSlotPos(rec_tagpos_binary);
            tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;	
            SetNextPollTime(tyncpoll_time);
         NextSlotDelayMs(0);
         }
      //   tyncpoll_time=0;
         next_nearbase_num=0;
@@ -530,7 +644,9 @@
         }

         if(recbase_num<3)
         {next_nearbase_num=next_nearbase_num;}
    {
        next_nearbase_num=next_nearbase_num;
    }
               last_nearbase_num = next_nearbase_num;
         for(i=0;i<last_nearbase_num-1;i++)
         {
@@ -581,7 +697,7 @@
         }
   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(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_RESET);
}
u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32];
void GetNearMsg(void)