加速计影响测距

zhyinch

2021-10-29 8f8f5ae1fd0a007e331ac909b4280e1e109df091

 Src/application/dw_app.c

@@ -81,7 +81,7 @@
    SINGLEPOLL,
}tag_state=STARTPOLL;
static dwt_config_t config = {
   5,               /* Channel number. */
   2,               /* Channel number. */
   DWT_PRF_64M,     /* Pulse repetition frequency. */
   DWT_PLEN_64,    /* Preamble length. */
   DWT_PAC8,        /* Preamble acquisition chunk size. Used in RX only. */
@@ -624,21 +624,24 @@
//   mainbase_id=minid;
//   mainbase_id = 0x4;
}

#define TAGET_OFFSETTIME 940
#define MAX_NEARBASE_ANCNUM 11
u16 rec_maxrangelen,rec_anc_signalpower[MAX_NEARBASE_ANCNUM];
u8 outrange_times;
extern u32 synctimer;
u32 target_time=100;
u8 pd_i;
u32 temptime,delaytime,expect_anctime,error_time;
u32 temptime,delaytime,expect_anctime;
int32_t error_time;
u16 delaycount;
int16_t poll_offsettime=-6900;
int16_t poll_offsettime=-6900,clockoffset;
extern u16 slotpos_intoatl, slotpos;
void NextPollDelay(u32 anchor_time)
{
      tagslotpos = 0;
   //   error_time = anchor_time-expect_anctime;
   //   tagslotpos = 0;
      error_time = anchor_time-expect_anctime;
   if(error_time<2000&&error_time>0)
      clockoffset += (TAGET_OFFSETTIME-error_time)*0.1;
    for(pd_i=0;pd_i<tag_frequency;pd_i++)
    {
        slotpos_intoatl = pd_i*bigslot_num+tagslotpos;
@@ -646,14 +649,14 @@
        if(anchor_time<temptime-5000)
        { 
               current_slotnum = pd_i;
            delaytime = temptime-anchor_time+poll_offsettime;  
      //            expect_anctime = temptime;
            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;
      expect_anctime = temptime-1000000;
    delaytime = temptime-anchor_time+poll_offsettime+clockoffset;
   return ;
}
u16 lpcount,poll_startcount,last_lpcount;
@@ -677,15 +680,7 @@
    __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
      last_lpcount = lpcount;
   }
void DelayOneSlot(void)
{
         lpcount+=166;
         if(lpcount>=32768)
      {
      lpcount -=32768;
      }
      __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
}

void NextSlotDelayMs(u16 delayms)
{
   current_slotnum++;
@@ -714,7 +709,7 @@
{
   u32 start_poll,frame_len;
    u8 nearmsg_i=0;
   DelayOneSlot();
   NextSlotDelayMs(0);
    for(nearmsg_i=0;nearmsg_i<20;nearmsg_i++)
    {
        nearbase_distlist[nearmsg_i] = 0x1ffff;
@@ -827,7 +822,7 @@
         }
      }
   dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置
   HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_SET);
//   HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_SET);
//    for(i=0;i<nearbase_num+1;i++)
//    {
//        nearbase_distlist[i] = 0x1ffff;
@@ -887,7 +882,7 @@
                           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;
							
                           rec_remotepara_state=rx_buffer[MOTORSTATE_INDEX]>>4;
                        if(rec_remotepara_state&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2))
                           {
                              para_update = 1;
@@ -922,7 +917,7 @@
                                         }
                                     }
                                 
                           rec_remotepara_state=rx_buffer[MOTORSTATE_INDEX]>>4;
									
                           
                           mainbase_lost_count=0;
                           flag_finalsend=1;
@@ -986,7 +981,7 @@
            {flag_finalsend = 2;}
         dwt_forcetrxoff();
dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_RESET);
//HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_RESET);
         if(mainbase_lost_count==0)
         {
            #ifdef SWITCHBASE_DIST
@@ -1005,7 +1000,7 @@
         #endif
         }else if(mainbase_lost_count>5*tag_frequency)
         {
         //   tag_state = STARTPOLL;
            tag_state = STARTPOLL;
         }   
         if(mainbase_lost_count!=0)
         {
@@ -1069,7 +1064,7 @@
            tagslotpos=TOTAL_SLOTNUM;
      }
      //SetNextPollTime(tagslotpos);
      DelayOneSlot();
      NextSlotDelayMs(1);
      
      dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);