11

zhyinch

2020-06-01 9d2282974b45aecf66adbd707fcde1ea83456703

 Ô´Âë/ºËÐÄ°å/Src/application/dw_app.c

@@ -5,7 +5,7 @@
   DISCPOLL,
   GETNEARMSG,
   NEARPOLL,
}tag_state;
}tag_state=GETNEARMSG;
static dwt_config_t config = {
   2,               /* Channel number. */
   DWT_PRF_64M,     /* Pulse repetition frequency. */
@@ -25,6 +25,8 @@
static uint8_t tx_nearpoll_msg[80] = {0};
static uint8_t tx_nearresp_msg[80] = {0};
static uint8_t tx_nearfinal_msg[80] = {0};

static uint8_t tx_near_msg[80] = {0};

static uint32_t frame_seq_nb = 0;   
static uint32_t status_reg = 0;
@@ -219,36 +221,36 @@
}
u8 recbase_num=0;
#define CHANGE_BASE_THRESHOLD  5
void NearAncSelect(void)
{static u16 last_mainbase_id,change_base_count;
   int32_t nearbase_mindist=99999, nearbase_minpos;
   u8 i;
   for(i=0;i<recbase_num-1;i++)
   {
      if(nearbase_mindist>nearbase_distlist[i])
      {
         nearbase_mindist=nearbase_distlist[i];
         nearbase_minpos=i;
      }
   }
   if(nearbase_mindist<mainbase_dist-THRESHOLD_CHANGE_MAINBASE_DIST)
   {
      if(last_mainbase_id==nearbaseid_list[nearbase_minpos])
      {
         change_base_count++;
         if(change_base_count>CHANGE_BASE_THRESHOLD)
         {
            mainbase_id=last_mainbase_id;
            tag_state=GETNEARMSG;
         }
      }else{
         change_base_count=0;
      }
      last_mainbase_id=nearbaseid_list[nearbase_minpos];		
   }else{
      change_base_count=0;
   }
}
//void NearAncSelect(void)
//{static u16 last_mainbase_id,change_base_count;
//   int32_t nearbase_mindist=99999, nearbase_minpos;
//   u8 i;
//   for(i=0;i<recbase_num-1;i++)
//   {
//      if(nearbase_mindist>nearbase_distlist[i])
//      {
//         nearbase_mindist=nearbase_distlist[i];
//         nearbase_minpos=i;
//      }
//   }
//   if(nearbase_mindist<mainbase_dist-THRESHOLD_CHANGE_MAINBASE_DIST)
//   {
//      if(last_mainbase_id==nearbaseid_list[nearbase_minpos])
//      {
//         change_base_count++;
//         if(change_base_count>CHANGE_BASE_THRESHOLD)
//         {
//            mainbase_id=last_mainbase_id;
//            tag_state=GETNEARMSG;
//         }
//      }else{
//         change_base_count=0;
//      }
//      last_mainbase_id=nearbaseid_list[nearbase_minpos];		
//   }else{
//      change_base_count=0;
//   }
//}
u8 test=0;
u8 anclost_times=0;
u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0;;
@@ -551,51 +553,72 @@


}

//void GetNearMsg(void)
//{
//   u32 start_poll,frame_len;
//   memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);
//   memcpy(&tx_near_msg[TAG_ID_IDX],&dev_id,2);
//   tx_near_msg[MESSAGE_TYPE_IDX] = NEAR_MSG;
//	
//   dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
//  dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);	
//   dwt_writetxdata(12, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
//   dwt_writetxfctrl(12, 0);//设置超宽带发送数据长度
//   dwt_starttx(DWT_START_TX_IMMEDIATE| DWT_RESPONSE_EXPECTED);
//   start_poll = time32_incr;
//   /* 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(time32_incr - start_poll>20)
//      NVIC_SystemReset();
//   };
//   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] == NEAR_MSG&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
//      {
//         nearbase_num=rx_buffer[NEARBASENUM_INDEX];
//         tagslotpos=rx_buffer[TAGSLOTPOS];
//         memcpy(nearbaseid_list,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2);
//         slottime=ceil((nearbase_num+2)*0.3)+1;
//         tyncpoll_time=tagslotpos*slottime;
//         tag_state=NEARPOLL;
//      }
//   }
//}
u8 nearmsg_mainbase=0;
u32 rec_tagpos_binary;
void GetNearMsg(void)
{
   u32 start_poll,frame_len;
   if(nearmsg_mainbase)
   {nearmsg_mainbase=0;
   }else{
   nearmsg_mainbase=1;
   }
   memcpy(&tx_near_msg[ANCHOR_ID_IDX],&nearmsg_mainbase,2);
   memcpy(&tx_near_msg[TAG_ID_IDX],&dev_id,2);
   tx_near_msg[MESSAGE_TYPE_IDX] = NEAR_MSG;
	
   dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
  dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);	
   dwt_writetxdata(12, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
   dwt_writetxfctrl(12, 0);//设置超宽带发送数据长度
   dwt_starttx(DWT_START_TX_IMMEDIATE| DWT_RESPONSE_EXPECTED);
   start_poll = time32_incr;
   /* 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(time32_incr - start_poll>20)
      NVIC_SystemReset();
   };
   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] == NEAR_MSG&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
      {
         next_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
         memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
         tagslotpos=rx_buffer[TAGSLOTPOS];
         memcpy(nearbaseid_list,&rx_buffer[NEARBASEID_INDEX],next_nearbase_num*2);
         slottime=ceil((nearbase_num+2)*0.3)+1;
         tyncpoll_time=tagslotpos*slottime;
         tag_state=NEARPOLL;
      }
   }
}

void Tag_App(void)//发送模式(TAG标签)
{
   //LED0_ON;
   g_Resttimer=0;
   NearPoll();
   switch(tag_state)
   {
      case DISCPOLL:
			
         break;
      case GETNEARMSG:
         GetNearMsg();
         break;
      case NEARPOLL:
            NearPoll();
         break;
	
	
   }

}

int8_t correction_time,new_tagid=0;
@@ -702,7 +725,7 @@
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
         }
}
u8 misdist_num,seize_anchor;
u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor;
u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase  1 first near_base
{
         tmp_time=TIM3->CNT;
@@ -785,9 +808,9 @@
               g_flag_Taggetdist[taglist_pos]=0;
               if(hex_dist>-1000&&hex_dist<100000)
               {
               if(abs(hex_dist-his_dist[taglist_pos])<1500||misdist_num>3)
               if(abs(hex_dist-his_dist[taglist_pos])<1500||misdist_num[taglist_pos]>3)
               {
                  misdist_num=0;
                  misdist_num[taglist_pos]=0;
               tagdist_list[taglist_pos] = hex_dist;   
               his_dist[taglist_pos]=hex_dist;   
            #ifndef USART_INTEGRATE_OUTPUT
@@ -809,7 +832,7 @@
            #endif
               }else{
               //   printf("%d",hex_dist);
                  misdist_num++;
                  misdist_num[taglist_pos]++;
               }
            }
         }
@@ -822,6 +845,9 @@
}
uint32_t current_syncid=0xffffffff,synclost_timer;
extern u8 flag_syncbase;
extern uint32_t tagpos_binary;
u8 tagpos_rec[50],tagpos_send[50],ancidlist_num;
u16 ancidlist_rec[20],ancidlist_send[20];
void Anchor_App(void)
{
   
@@ -841,7 +867,7 @@

   if (status_reg & SYS_STATUS_RXFCG)//成功接收
   { u16 tag_recv_interval;

      float temp_tagpos;
      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//清除标志位
      frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//获得接收数据长度
      dwt_readrxdata(rx_buffer, frame_len, 0);//读取接收数据
@@ -879,21 +905,22 @@
            
            }
            break;
//         case NEAR_MSG:
//            if(anc_id_recv==dev_id)
//            {
//               rx_buffer[TAGSLOTPOS]=taglist_pos;
//               tx_near_msg[MESSAGE_TYPE_IDX] = NEAR_MSG;
//               memcpy(&tx_near_msg[TAG_ID_IDX],&tag_id_recv,2);
//               tx_near_msg[NEARBASENUM_INDEX]=g_com_map[NEARBASE_NUM];
//               memcpy(&tx_near_msg[NEARBASEID_INDEX],&g_com_map[NEARBASE_ID1],g_com_map[NEARBASE_NUM]*2);
//               send_len=11+g_com_map[NEARBASE_NUM]*2+2;
//               dwt_writetxdata(send_len, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
//               dwt_writetxfctrl(send_len, 0);//设置超宽带发送数据长度
//               dwt_starttx(DWT_START_TX_IMMEDIATE);
//            }
//            break;
//		
         case NEAR_MSG:
            if(anc_id_recv==flag_syncbase)
            {
               rx_buffer[TAGSLOTPOS]=taglist_pos;
               tx_near_msg[MESSAGE_TYPE_IDX] = NEAR_MSG;
               memcpy(&tx_near_msg[TAG_ID_IDX],&tag_id_recv,2);
               memcpy(&tx_near_msg[NEARMSG_EMPTYSLOTPOS_INDEX],&tagpos_binary,4);
               tx_near_msg[NEARBASENUM_INDEX]=ancidlist_num;
               memcpy(&tx_near_msg[NEARBASEID_INDEX],&ancidlist_send,ancidlist_num*2);
               send_len=11+ancidlist_num*2+2;
               dwt_writetxdata(send_len, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
               dwt_writetxfctrl(send_len, 0);//设置超宽带发送数据长度
               dwt_starttx(DWT_START_TX_IMMEDIATE);
            }
            break;
		
         case NEAR_POLL:
            
            memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
@@ -909,8 +936,8 @@
               new_tagid=0;
            }
            tagofflinetime[taglist_pos]=0;
		
				
           temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);
            tagpos_rec[(u8)temp_tagpos]=1;
            rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
         
            for(i=0;i<rec_nearbase_num;i++)
@@ -928,6 +955,11 @@
                  seize_anchor=1;   //抢占anchor
                  Anchor_RecNearPoll(i);   
               }
               if(rec_nearbase_num>ancidlist_num)
               {
                  ancidlist_num=rec_nearbase_num;
                  memcpy(ancidlist_rec,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
               }
            
            break;