车载帧间隔随着设备低功耗优化到400us，能实现较稳定测距

chen

2024-10-18 0bcb013a7bae7815ac59c8b59302e2d9ed0b3f30

 APL/dw_ancapp.c

@@ -26,7 +26,7 @@
#define POLL_TX_TO_RESP_RX_DLY_UUS 150
#define RESP_RX_TO_FINAL_TX_DLY_UUS 410
#define RESP_RX_TIMEOUT_UUS 2000
#define DELAY_BETWEEN_TWO_FRAME_UUS 1000
#define DELAY_BETWEEN_TWO_FRAME_UUS 400  //yuan1200
#define POLL_RX_TO_RESP_TX_DLY_UUS 670
#define RESP_TX_TO_FINAL_RX_DLY_UUS 10
#define FINAL_RX_TIMEOUT_UUS 4300
@@ -54,11 +54,11 @@
static uint16_t ancidlist_rec[TAG_NUM_IN_SYS],ancidlist_send[TAG_NUM_IN_SYS],rec_ancidlist[TAG_NUM_IN_SYS];
static int16_t rec_ancdistlist[TAG_NUM_IN_SYS];
static int16_t rec_antdelay;
static uint16_t taglist_num=0,taglist_pos,tmp_time;
static uint16_t tagid_list[TAG_NUM_IN_SYS];
 uint16_t taglist_num=0,taglist_pos;
 uint16_t tagid_list[TAG_NUM_IN_SYS];
uint8_t tagbattary_list[TAG_NUM_IN_SYS];
static uint8_t tagofflinetime[TAG_NUM_IN_SYS];
static int32_t tagdist_list[TAG_NUM_IN_SYS];
 int32_t tagdist_list[TAG_NUM_IN_SYS];
static uint8_t tagpos_rec[50];
static uint64_t poll_rx_ts;
static uint8_t g_flag_Taggetdist[TAG_NUM_IN_SYS],flag_tag_distsmooth[TAG_NUM_IN_SYS],misdist_num[TAG_NUM_IN_SYS];
@@ -76,7 +76,7 @@
const double A = 121.74;
double min_power;
static dwt_rxdiag_t d1;
static int32_t hex_dist,his_dist[TAG_NUM_IN_SYS];
static int32_t hex_dist;
extern uint16_t g_Resttimer;
static int32_t filter_dist,g_Tagdist[TAG_NUM_IN_SYS];
double LOS(dwt_rxdiag_t *dia) {
@@ -139,6 +139,7 @@
        *ts += ts_field[i] << (i * 8);
    }
}
int16_t his_dist[100];
void TagListUpdate(void)
{
   uint16_t i,j=0,temp[TAG_NUM_IN_SYS];
@@ -147,7 +148,11 @@
      if(tagofflinetime[i]++<QUIT_SLOT_TIME)
      {
         tagid_list[j]=tagid_list[i];
//         tagid_distlist[j] = 
         tagdist_list[j]=tagdist_list[i];
         his_dist[j] = his_dist[i];
         tagofflinetime[j++]=tagofflinetime[i];
			
      }
   }
   taglist_num=j;
@@ -177,18 +182,18 @@
uint8_t Anchor_RecNearPoll(uint8_t ancrec_nearbasepos) //0 mainbase  1 first near_base
{
    uint8_t motorstate;
         tmp_time=TIM3->CNT;
      //   tmp_time=TIM3->CNT;
      temp_nearbaspos = ancrec_nearbasepos;
      memcpy(&send_buffer[ANCHOR_ID_IDX],&dev_id,2);
         memcpy(&send_buffer[ANCTIMEMS],&basesystimer,2);
         memcpy(&send_buffer[ANCTIMEUS],&tmp_time,2);
      //   memcpy(&send_buffer[ANCTIMEUS],&tmp_time,2);
         send_buffer[TAGFREQ_IDX] = tag_frequency;
         memcpy(&send_buffer[TAG_ID_IDX],&tag_id_recv,2);
         poll_rx_ts = get_rx_timestamp_u64();//获得Poll包接收时间T2

    if(rec_nearbase_num == ancrec_nearbasepos)//抢占模式
    {
        random_time = (HIDO_TimerGetTick()%100);
     //   random_time = (HIDO_TimerGetTick()%100);
           resp_tx_time = (poll_rx_ts+((rec_nearbase_num*20 + POLL_RX_TO_RESP_TX_DLY_UUS+ancrec_nearbasepos*DELAY_BETWEEN_TWO_FRAME_UUS) * UUS_TO_DWT_TIME)) >> 8;//计算Response发送时间T3。
  
    }else{
@@ -202,15 +207,15 @@
            /* Write all timestamps in the final message. See NOTE 8 below. */
            resp_msg_set_ts(&send_buffer[RESP_MSG_POLL_RX_TS_IDX], poll_rx_ts);
            resp_msg_set_ts(&send_buffer[RESP_MSG_RESP_TX_TS_IDX], resp_tx_ts);
                  g_com_map[DIST_OFFSET]=0;//测试记得删
                  //g_com_map[DIST_OFFSET]=0;//测试记得删
            memcpy(&send_buffer[RESP_MSG_ANC_DISTOFFSET],&g_com_map[DIST_OFFSET],2);
         if(new_tagid)
         {
            tagdist_list[taglist_pos]=0x1ffff;
            memcpy(&send_buffer[DIST_IDX], &tagdist_list[taglist_pos], 4);
         }else{
            memcpy(&send_buffer[DIST_IDX], &tagdist_list[taglist_pos], 4);
         }
//         if(new_tagid)
//         {
//            tagdist_list[taglist_pos]=0x1ffff;
//            memcpy(&send_buffer[DIST_IDX], &tagdist_list[taglist_pos], 4);
//         }else{
//            memcpy(&send_buffer[DIST_IDX], &tagdist_list[taglist_pos], 4);
//         }
//         motorstate =0;
//            if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE1]||tagdist_list[taglist_pos]<0)
//            {
@@ -510,7 +515,26 @@
//         }
//        }
}

int16_t DistFilter(int16_t dist,int8_t pos)
{
   if(abs(dist-his_dist[pos])<1000||misdist_num[pos]>3)
   {
//      if(tagdist_list[pos]>0&&tagdist_list[pos]<50000)
//      {
//         filter_dist=(dist/10)*0.5+0.5*tagdist_list[pos];
//      }else{
//         filter_dist = dist/10;
//      }
      misdist_num[pos]=0;
      his_dist[pos]=dist;        
   }else{
      misdist_num[pos]++;
   }
   return his_dist[pos];
}
uint16_t target_tagid[12] = {0x1316,0x1394,0x1265,0x1318,0x1306,0x1304,0x1376,0x1312,0x1277,0x1388,0x1140,0x1392};
int16_t anchor_dist;
void Anchor_App(void)
{
   u8 send_len,i;
@@ -558,13 +582,18 @@
                {
                    if(rec_ancidlist[i] == dev_id)
                    {      //rec_ancdistlist[i]+=(int16_t)g_com_map[DIST_OFFSET];
                        PushAnchorDataArray(tag_id_recv,rec_ancdistlist[i],-(int8_t)firstpath_power);  
											
                                 anchor_dist = rec_ancdistlist[i]+(int16_t)g_com_map[DIST_OFFSET];
                                 break;
                      //  PushAnchorDataArray(tag_id_recv,rec_ancdistlist[i],-(int8_t)firstpath_power);  
                        
                       // PushAnchorDataArray(tag_id_recv,rec_ancdistlist[i],battary);  
                    }
                }
            taglist_pos=CmpTagInList(tag_id_recv);
                if(taglist_num>=TAG_NUM_IN_SYS)
            if(rec_ancdistlist[i]!=-1)			
            tagdist_list[taglist_pos] = DistFilter(anchor_dist,taglist_pos);
//                if(taglist_num>=TAG_NUM_IN_SYS)
//                    break;
            if(taglist_pos==taglist_num)
            {