文件重新排版

yincheng.zhong

2023-08-16 aceee9b89a9a012321ef68357e7ddd94684b14b3

 Src/application/dw_app.c

@@ -368,7 +368,9 @@
            tmp_time-=999;
            sync_timer++;
            if(sync_timer>=1010)
               {sync_timer=0;}
                        {
                            sync_timer=0;
                        }
         }
      //   TIM3->CNT=tmp_time;
      }
@@ -484,151 +486,6 @@
int32_t salvebase_mindist;
int16_t intheight;
static uint8_t send_buffer[200];
void DiscPoll(void)
{
   uint32_t frame_len;
   uint32_t final_tx_time;
   u32 start_poll;
   int32_t minddist=0x1ffff,minid=-1,temp_dist;
   u8 i,j,getsync_flag=0;
    u16 mindist_ancid,random_delay_tim,tempval;
    NextSlotDelayMs(-2);
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);		
   frame_seq_nb++;
   for(uint8_t singlepoll_i=0;singlepoll_i<10;singlepoll_i++)
   {
       

   /* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
    nearbase_num = 0;
        
   // send_buffer[BATTARY_IDX] = bat_percent;
   //send_buffer[BUTTON_IDX] = !GET_USERKEY|stationary_flag<<1|gotosleep_flag<<2;
   send_buffer[SEQUENCE_IDX] = frame_seq_nb;
    send_buffer[SEQUENCEH_IDX] = (frame_seq_nb)>>8;
   send_buffer[NEARBASENUM_INDEX] = nearbase_num;
     
//   memcpy(&send_buffer[NEARBASEID_INDEX],&singlepoll_baseid,nearbase_num*2);
//    memcpy(&send_buffer[NEARBASEID_INDEX+nearbase_num*2],&singlepoll_basedist,nearbase_num*4+4);
//    intheight+=g_com_map[HEIGHTOFFEST_INDEX];
 //   memcpy(&send_buffer[NEARBASEID_INDEX+nearbase_num*6],&intheight,2);
   send_buffer[MESSAGE_TYPE_IDX] = DISCOVERPOLL;	
//   singlepoll_basedist[singlepoll_i]= 0x1ffff;
   send_buffer[NEARP_TAGFREQ_INDEX] = tag_frequency;	
   send_buffer[NEARP_TAGSLOTPOS_INDEX] = tagslotpos;	
    send_buffer[ANCHOR_ID_IDX] = singlepoll_i;
   memcpy(&send_buffer[TAG_ID_IDX],&dev_id,2);	
   dwt_writetxdata(28+6*nearbase_num, send_buffer, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
   dwt_writetxfctrl(28+6*nearbase_num, 0);//设置超宽带发送数据长度
   dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置

   
   /* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */
   while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误
   { 
   };
   if(status_reg==0xffffffff)
   {
      NVIC_SystemReset();
   }
   if (status_reg & SYS_STATUS_RXFCG)//如果成功接收
   {
      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清楚寄存器标志位
      frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;   //获得接收到的数据长度
      dwt_readrxdata(rx_buffer, frame_len, 0);   //读取接收数据
        
      if (rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
      {
         poll_tx_ts = get_tx_timestamp_u64();                              //获得POLL发送时间T1
         resp_rx_ts = get_rx_timestamp_u64();                              //获得RESPONSE接收时间T4			
         if(getsync_flag==0)
         {
            getsync_flag=1;
                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;}
                }

                //offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset;
//                SetNextPollTime(tagslotpos);
      }
         memcpy(&send_buffer[NEARBASEID_INDEX+singlepoll_i*2],&rx_buffer[ANCHOR_ID_IDX],2);
         memcpy(&temp_dist, &rx_buffer[DIST_IDX], 4);
         memcpy(&send_buffer[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 4);
           // singlepoll_basedist[singlepoll_i] = temp_dist;
       // memcpy(&singlepoll_baseid[singlepoll_i], &rx_buffer[ANCHOR_ID_IDX], 2);
            
            if(temp_dist<minddist)
            {
                minddist = temp_dist;
                memcpy(&mindist_ancid,&rx_buffer[ANCHOR_ID_IDX],2);
                
            }
            send_buffer[MESSAGE_TYPE_IDX] = DISCOVERFINAL;	
         /* Compute final message transmission time. See NOTE 9 below. */
         final_tx_time = (resp_rx_ts + (600 * UUS_TO_DWT_TIME)) >> 8;//计算final包发送时间，T5=T4+Treply2
         dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5
         final_tx_ts = (((uint64_t)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;//final包实际发送时间是计算时间加上发送天线delay
         final_msg_set_ts(&send_buffer[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//将T1，T4，T5写入发送数据
         final_msg_set_ts(&send_buffer[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts);
         final_msg_set_ts(&send_buffer[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
         dwt_writetxdata(25,send_buffer, 0);//将发送数据写入DW1000
         dwt_writetxfctrl(25, 0);//设定发送数据长度
         result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送			
			
 
         if(result==0)
         {while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成
         { };
      }
         /* Clear TXFRS event. */
         dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//清除标志位
         random_delay_tim = 0;
      }
      else
      {
           //  singlepoll_basedist[singlepoll_i] = 0x1ffff;
         random_delay_tim = DFT_RAND_DLY_TIM_MS; //如果通讯失败，将间隔时间增加5ms，避开因为多标签同时发送引起的冲突。
      }
   }
   else
   {
     //   singlepoll_basedist[singlepoll_i] = 0x1ffff;
   dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
      random_delay_tim = DFT_RAND_DLY_TIM_MS;
   }
   // delay_ms(1);
}
    if(minddist!=0x1ffff&&minddist!=0)
    {
                  try_reg_times = 0;
            tag_state = REGPOLL;            
            mainbase_id = mindist_ancid;  
                  Registor_Poll();			
            //GetNearMsg();
    }
   
   if(getsync_flag==0)
   {
//   tagslotpos--;
//         if(tagslotpos==0||tagslotpos>max_slotpos)
//      {		
//         tagslotpos=max_slotpos;	
//      }
//      tyncpoll_time=(tagslotpos-1)*slottime;
   }
//   mainbase_id=minid;
//   mainbase_id = 0x4;
}
int16_t target_offsettime = 850;
//#define TAGET_OFFSETTIME 940
#define MAX_NEARBASE_ANCNUM 11
@@ -849,6 +706,281 @@
    
    }
}
void DiscPoll2(void)
{
    uint32_t temp1,temp2,dw_systime,mindist_slavebaseid;
    static u8 notenoughdist_count=0;
    uint32_t final_tx_time;
    u32 start_poll;
    u8 i,j,getsync_flag=0,timeout,get_newdist;
    // USART_putc(current_slotnum);
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
    tag_succ_times = 0;
    salvebase_mindist=999999;
    get_newdist = 0;
    if(next_nearbase_num>=MAX_NEARBASE_NUM)
    {
        next_nearbase_num = MAX_NEARBASE_NUM-1;
    }
    nearbase_num = 10;
    recbase_num=0;

    tx_near_msg[BATTARY_IDX] = bat_percent;
    tx_near_msg[BUTTON_IDX] = userkey_state|stationary_flag<<1|gotosleep_flag<<2|motor_flag<<5;
    tx_near_msg[SEQUENCE_IDX] = frame_seq_nb;
    tx_near_msg[SEQUENCEH_IDX] = (frame_seq_nb++)>>8;
    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*4+4);
    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*6+4],&rec_anc_signalpower,nearbase_num*2+2);

    if(intheight!=0)
        intheight+=g_com_map[HEIGHTOFFEST_INDEX];

    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*8+6],&intheight,2);
    tx_near_msg[MESSAGE_TYPE_IDX] = POS_POLL;

    tx_near_msg[NEARP_TAGFREQ_INDEX] = tag_frequency;
    tx_near_msg[NEARP_TAGSLOTPOS_INDEX] = tagslotpos;

    memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);
    dwt_writetxdata(29+8*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
    dwt_writetxfctrl(29+8*nearbase_num, 0);//设置超宽带发送数据长度
    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中设置
    tx_near_msg[TAGCONFIGSUCCESS_INDEX] =0;
    para_update = 0;
    flag_finalsend=0;
    flag_rxon=1;
    flag_getresponse=0;
    start_count=HAL_LPTIM_ReadCounter(&hlptim1);
    recbase_num=0;
    timeout=ceil((float)nearbase_num*SLOT_SCALE)+3;
    end_count=start_count+(timeout<<5);
    if(end_count>=32768)
    {
        end_count-=32768;
    }
    mainbase_dist=100000;
    mainbase_lost_count++;
    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
    while(current_count<end_count||current_count>end_count+15000)
    {
        current_count=HAL_LPTIM_ReadCounter(&hlptim1);
        while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误
        {
            current_count=HAL_LPTIM_ReadCounter(&hlptim1);
            if(current_count>=end_count&&current_count<end_count+15000)
                break;
        };
        if(status_reg==0xffffffff)
        {
            NVIC_SystemReset();
        }
        if (status_reg & SYS_STATUS_RXFCG)//如果成功接收
        {
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清楚寄存器标志位
            frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;   //获得接收到的数据长度
            dwt_readrxdata(rx_buffer, frame_len, 0);   //读取接收数据
            test2 = dwt_readcarrierintegrator();
            dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间
            dwt_rxenable(0);//打开接收
            if (rx_buffer[MESSAGE_TYPE_IDX] == POS_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
            {   u16 rec_nearbaseid,rec_nearbasepos;
                poll_tx_ts = get_tx_timestamp_u64();                              //获得POLL发送时间T1
                resp_rx_ts = get_rx_timestamp_u64();                              //获得RESPONSE接收时间T4
                recbase_num++;
                memcpy(&rec_nearbaseid,&rx_buffer[ANCHOR_ID_IDX],2);
                if(rec_nearbaseid==mainbase_id)
                {
                    exsistbase_list[0]=KEEP_TIMES;
                    SetANCTimestap(0,&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]);

                    //////////////////////////////////时间同步
                    memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2);
                    memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);
                    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
                    dwt_forcetrxoff();
                    rec_remotepara_state=rx_buffer[MOTORSTATE_INDEX]>>4;
                    if(rec_remotepara_state&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2))
                    {
                        para_update = 1;
                        tx_near_msg[TAGCONFIGSUCCESS_INDEX] = 1;
                        para_len = frame_len-22;
                        memcpy(rec_remotepara,&rx_buffer[REMOTEPARA_INDEX],para_len);
                    }
                    userkey_state = !GET_USERKEY;

                    //memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
                    temp_dist = nearbase_distlist[0];
                    if(temp_dist!=nearbase_distlist[0])
                    {
                        get_newdist++;
                    }
                    mainbase_dist=temp_dist;
                    nearbase_distlist[0] = temp_dist;
                    base_mindist = temp_dist;
                    if((rx_buffer[MOTORSTATE_INDEX]&0xf)!=3)
                        motor_state=rx_buffer[MOTORSTATE_INDEX]&0xf;
                    if(frame_len==38)
                    {
                        memcpy(&rec_maxrangelen,&rx_buffer[MAXRANGE_DISTANCE],2);
                        memcpy(&rec_anc_signalpower[0],&rx_buffer[ANC_SIGNALPOWER],2);
                        if(mainbase_dist>rec_maxrangelen&&mainbase_dist>0&&rec_maxrangelen!=0)
                        {
                            if(outrange_times++>OUTRANGE_RESTARTTIMES)
                            {
                                tag_state = STARTPOLL;
                            }
                        } else {
                            outrange_times = 0;
                        }
                    }



                    mainbase_lost_count=0;
                    flag_finalsend=1;
                    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);
                    SetLPTimer(ancsync_time);
                    break;
                } 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]);

                    exsistbase_list[rec_nearbasepos+1]=KEEP_TIMES;
                    //memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
                    temp_dist = nearbase_distlist[rec_nearbasepos+1];
                    if(frame_len==38)
                        memcpy(&rec_anc_signalpower[rec_nearbasepos+1],&rx_buffer[ANC_SIGNALPOWER],2);
                    memcpy(&nearbase_switchdistlist[rec_nearbasepos],&rx_buffer[NR_NEARSWITCH_DISTANCE],2);
                    if(temp_dist!=nearbase_distlist[rec_nearbasepos+1])
                    {
                        get_newdist++;
                    }
                    nearbase_distlist[rec_nearbasepos+1]=temp_dist;   //   nearbase_distlist[1]对应   rec_nearbaseid[0]的距离
                    final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos)*4], resp_rx_ts);
                    if(temp_dist<salvebase_mindist&&nearbase_switchdistlist[rec_nearbasepos]!=0&&(nearbase_switchdistlist[rec_nearbasepos]==1||temp_dist<nearbase_switchdistlist[rec_nearbasepos]))
                    {
                        salvebase_mindist = temp_dist;
                        mindist_slavebaseid = rec_nearbaseid;
                    }
                }
            }
        } else {
            dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
            if(recbase_num!=nearbase_num+1)
            {
                dwt_rxenable(0);
            }
        }
    }
    dwt_forcetrxoff();
    dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);

    if(mainbase_lost_count==0)
    {
#ifdef SWITCHBASE_DIST
        if(salvebase_mindist<mainbase_dist- THRESHOLD_CHANGE_MAINBASE_DIST )
        {
            changemainbase_count++;
            if(changemainbase_count>2)
            {
                changemainbase_count = 0;
                mainbase_id = mindist_slavebaseid;
                tag_state = GETNEARMSG;
                trygetnearmsg_times = 0;
                GetNearMsg();
            }
        } else {
            changemainbase_count = 0;
        }
#endif
    } else if(mainbase_lost_count>10)
    {
        mainbase_lost_count = 0;
        tag_state = DISCPOLL;
        DiscPoll();
    }
    if(mainbase_lost_count!=0)
    {
        if(mainbase_lost_count<=5)
        {   NextSlotDelayMs(0);
        } else {
            NextSlotDelayMs(0);
        }
    }
    CalculateDists();
    if(para_update)
    {

        pack_msgtype = rec_remotepara[0];
        pack_index = rec_remotepara[1];
        pack_length = rec_remotepara[2];
        if(pack_msgtype==2)
        {
            if( pack_index == MOTOR_ONTIME_INDEX)
            {
                if(motor_keeptime==0)
                    motor_keeptime = rec_remotepara[3];
            } else if( pack_index == 2*COM_INTERVAL)
            {
                if(memcmp(&g_com_map[COM_INTERVAL],&rec_remotepara[3],2)!=0)
                {
                    memcpy((uint8_t*)&g_com_map + pack_index, &rec_remotepara[3], pack_length);
                    save_com_map_to_flash();
                    tag_frequency = 1000/g_com_map[COM_INTERVAL];
                    bigslot_num = TOTAL_SLOTNUM/tag_frequency;
                }
            }
            else {
                if(pack_index<200)
                {
                    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<nearbase_num; i++)
    {
        if(nearbaseid_list[i]!=nearbaseid_list2[i])
        {
            tag_state = GETNEARMSG;
            GetNearMsg();
            trygetnearmsg_times = 0;
            nearbaseid_list0[i]=1;
        }
    }
    dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
}
void NearPoll(void)
{
   
@@ -868,7 +1000,9 @@
      next_nearbase_num = MAX_NEARBASE_NUM-1;
   }
   if(nearbase_num>10)
   {nearbase_num = 10;}
    {
        nearbase_num = 10;
    }
//   HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_SET);

   recbase_num=0;
@@ -929,7 +1063,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);
@@ -1032,7 +1168,9 @@
                              tmp_time-=999;
                              sync_timer++;
                              if(sync_timer>=1000)
                                 {sync_timer=0;}
                        {
                            sync_timer=0;
                        }
                           }
                           ancsync_time=((sync_timer)*1000+tmp_time);   
                           SetLPTimer(ancsync_time);
@@ -1078,7 +1216,9 @@
                  }else{
                  dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
                  if(recbase_num!=nearbase_num+1)
                     {dwt_rxenable(0);   }
            {
                dwt_rxenable(0);
            }
                     //                  if(flag_rxon)
//                     {
                  //      dwt_rxenable(0);   
@@ -1087,7 +1227,9 @@
               //   dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
         }
            if(flag_finalsend!=1)
            {flag_finalsend = 2;}
    {
        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_SET);
@@ -1248,7 +1390,9 @@
                              tmp_time-=999;
                              sync_timer++;
                              if(sync_timer>=1010)
                                 {sync_timer=0;}
                {
                    sync_timer=0;
                }
                           }
                           ancsync_time=((sync_timer)*1000+tmp_time);                                                         
                           SetLPTimer(ancsync_time);
@@ -1349,7 +1493,7 @@
         break;
      case DISCPOLL:
         LED_LG_ON;      
         DiscPoll();
        DiscPoll2();
         LED_LG_OFF;
         break;
      case GETNEARMSG: