修改标签测距0bug

yincheng.zhong

2023-05-26 0017519b68e7c8f47ccc1df3d92eaf07a2b21050

 Src/application/dw_app.c

@@ -315,10 +315,11 @@
double rtd_init, rtd_resp;
double tof,distance;
extern int32_t dwt_readcarrierintegrator(void) ;
uint32_t testtimer[10],testtimer2,anc_pollrx[11],anc_resptx[11],tag_resprx[11];
int32_t anc_clockoffset[11];
int16_t anc_distoffset[11];
uint32_t testtimer2,anc_pollrx[MAX_NEARBASE_NUM],anc_resptx[MAX_NEARBASE_NUM],tag_resprx[MAX_NEARBASE_NUM];
int32_t anc_clockoffset[MAX_NEARBASE_NUM];
int16_t anc_distoffset[MAX_NEARBASE_NUM];
int32_t test2;
uint16_t u16_nearbase_distlist[MAX_NEARBASE_NUM];
uint8_t get_newdist,notenoughdist_count;
void SetANCTimestap(uint8_t i,uint8_t* pollrx,uint8_t* resptx,uint32_t resprx,uint8_t* distoffset)
{
@@ -336,13 +337,13 @@
      if(exsistbase_list[i]>0)
      {

          exsistbase_list[i]--;
         // exsistbase_list[i]--;
          clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_5 / 1.0e6) ;
          rtd_init = tag_resprx[i] - poll_tx_ts;
          rtd_resp = anc_resptx[i] - anc_pollrx[i];
          tof = ((rtd_init - rtd_resp * (1 - clockOffsetRatio)) / 2.0) * DWT_TIME_UNITS;
          distance = tof * SPEED_OF_LIGHT;
          if(distance>-1000&&distance<100000)
          if(distance>-10&&distance<1000)
            nearbase_distlist[i] = distance*100+anc_distoffset[i];
      }else{
          nearbase_distlist[i] = 0x1ffff;
@@ -373,13 +374,17 @@
   nearbase_num=next_nearbase_num;
   recbase_num=0;
//   motor_state=0;
    for(uint8_t i=0;i<nearbase_num;i++)
    {
        u16_nearbase_distlist[i] = nearbase_distlist[i];
    }
   tx_near_msg[GROUP_ID_IDX] = group_id;
   tx_near_msg[BATTARY_IDX] = bat_percent;
   tx_near_msg[BUTTON_IDX] = !GET_USERKEY|stationary_flag<<1;
   tx_near_msg[SEQUENCE_IDX] = frame_seq_nb++;
   tx_near_msg[NEARBASENUM_INDEX] = nearbase_num;
   memcpy(&tx_near_msg[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2);
    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*2],&nearbase_distlist,nearbase_num*2);
    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*2],&u16_nearbase_distlist,nearbase_num*2);
   tx_near_msg[MESSAGE_TYPE_IDX] = MBX_POLL;   
   memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);   
   dwt_writetxdata(13+4*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
@@ -543,7 +548,7 @@
//                           } 
                        }else{
                           rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
                           SetANCTimestap(rec_nearbasepos+1,&rx_buffer[RESP_MSG_POLL_RX_TS_IDX],&rx_buffer[RESP_MSG_RESP_TX_TS_IDX],resp_rx_ts,&rx_buffer[RESP_MSG_ANC_DISTOFFSET]);
                           SetANCTimestap(rec_nearbasepos,&rx_buffer[RESP_MSG_POLL_RX_TS_IDX],&rx_buffer[RESP_MSG_RESP_TX_TS_IDX],resp_rx_ts,&rx_buffer[RESP_MSG_ANC_DISTOFFSET]);

                              if(rec_nearbasepos>=last_nearbase_num)          //发现新的基站
                           {
@@ -602,8 +607,6 @@
         {
                last_lpcount+= HAL_LPTIM_ReadCounter(&hlptim1)%(g_com_map[COM_INTERVAL]*16);
         }
            if(nearbase_num==4)
            {nearbase_num=4;}
            last_nearbase_num = next_nearbase_num;
         for(i=0;i<last_nearbase_num-1;i++)
         {
@@ -627,10 +630,10 @@
         }
         
         report_num=0;
            if(last_nearbase_num>4)
            {
                last_nearbase_num = 5;
            }
//            if(last_nearbase_num>4)
//            {
//                last_nearbase_num = 5;
//            }
   for (i=0;i<last_nearbase_num;i++)
         {
            nearbaseid_list[i]=true_nearbase_idlist[i];