32位ID

zhyinch

2019-06-22 92ffaba03621e312b0620f22cd1f184522ee8f3e

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

@@ -64,7 +64,6 @@
#define SPEED_OF_LIGHT 299702547

/* Indexes to access some of the fields in the frames defined above. */
#define ALL_MSG_SN_IDX 2
#define FINAL_MSG_POLL_TX_TS_IDX 10
#define FINAL_MSG_RESP_RX_TS_IDX 14
#define FINAL_MSG_FINAL_TX_TS_IDX 18
@@ -72,10 +71,12 @@

#define GROUP_ID_IDX               0
#define ANCHOR_ID_IDX             1
#define TAG_ID_IDX                3
#define MESSAGE_TYPE_IDX          5
#define DIST_IDX                      6
#define ANC_TYPE_IDX                7
#define TAG_ID_IDX                5
#define MESSAGE_TYPE_IDX          9	
#define DIST_IDX                      10
#define ANC_TYPE_IDX                14
#define BATTARY_IDX                  15
#define BUTTON_IDX                  16

#define POLL                    0x01
#define RESPONSE                0x02
@@ -97,7 +98,7 @@
};

/* Frames used in the ranging process. See NOTE 2 below. */
static uint8_t tx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
static uint8_t tx_poll_msg[18] = {0};
//static uint8_t rx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0};
static uint8_t tx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
   
@@ -130,8 +131,8 @@
static double tof;
   
uint16_t anchor_dist_last_frm[TAG_NUM_IN_SYS];   
uint8_t tag_id = 0;
uint8_t tag_id_recv = 0;
uint32_t tag_id = 0;
uint32_t tag_id_recv = 0;
uint8_t random_delay_tim = 0;

double distance, dist_no_bias, dist_cm;
@@ -140,7 +141,7 @@
float dis_after_filter;            //当前距离值
LPFilter_Frac* p_Dis_Filter;      //测距用的低通滤波器

uint16_t g_Tagdist[256];
uint16_t g_Tagdist[TOTAL_TAG_NUM];
uint8_t g_flag_Taggetdist[256];
/*------------------------------------ Functions ------------------------------------------*/

@@ -269,9 +270,9 @@
   tx_poll_msg[MESSAGE_TYPE_IDX]=POLL;
   tx_resp_msg[MESSAGE_TYPE_IDX]=RESPONSE;
   tx_final_msg[MESSAGE_TYPE_IDX]=FINAL;
   memcpy(&tx_poll_msg[TAG_ID_IDX], &g_com_map[DEV_ID], 2);
   memcpy(&tx_final_msg[TAG_ID_IDX], &g_com_map[DEV_ID], 2);
   memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &g_com_map[DEV_ID], 2);
   memcpy(&tx_poll_msg[TAG_ID_IDX], &dev_id, 4);
   memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 4);
   memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 4);
   
}   
void tag_sleep_configuraion(void)
@@ -298,7 +299,6 @@
   for(i=0;i<REPOET_ANC_NUM;i++)
   {
   /* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
   tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
   tx_poll_msg[ANC_TYPE_IDX] = i;
      
   dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
@@ -335,8 +335,8 @@

      /* Check that the frame is the expected response from the companion "DS TWR responder" example.
       * As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
      rx_buffer[ALL_MSG_SN_IDX] = 0;
      if (rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&rx_buffer[TAG_ID_IDX]==g_com_map[DEV_ID]) //判断接收到的数据是否是response数据
		
      if (rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //判断接收到的数据是否是response数据
      {
         /* Retrieve poll transmission and response reception timestamp. */
         poll_tx_ts = get_tx_timestamp_u64();                              //获得POLL发送时间T1
@@ -357,7 +357,7 @@
         final_msg_set_ts(&tx_final_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);

         /* Write and send final message. See NOTE 7 below. */
         tx_final_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
		
         dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);//将发送数据写入DW1000
         dwt_writetxfctrl(sizeof(tx_final_msg), 0);//设定发送数据长度
         result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送
@@ -391,7 +391,7 @@
}
   if(tag_succ_times!=REPOET_ANC_NUM)
   {
   random_delay_tim =g_com_map[DEV_ID]*13+7;
   random_delay_tim =time32_incr&0x8f+7;
   }else{
      random_delay_tim=0;
   }
@@ -412,7 +412,8 @@
}

u16 tag_time_recv[TOTAL_TAG_NUM];
u8 usart_send[16];
u8 usart_send[25];
u8 battary,button;
extern uint8_t g_pairstart;
void Anchor_App(void)
{
@@ -446,18 +447,20 @@

      /* Check that the frame is a poll sent by "DS TWR initiator" example.
       * As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
      rx_buffer[ALL_MSG_SN_IDX] = 0;
	
      
      //将收到的tag_id分别写入各次通讯的包中，为多标签通讯服务，防止一次通讯中接收到不同ID标签的数据
      tag_id_recv = rx_buffer[TAG_ID_IDX];
      tx_resp_msg[TAG_ID_IDX] = tag_id_recv;
      if(tag_recv_timer>tag_time_recv[tag_id_recv])
      {   tag_recv_interval =  tag_recv_timer - tag_time_recv[tag_id_recv];
      }else{
         tag_recv_interval = tag_recv_timer + 65535 - tag_time_recv[tag_id_recv];
      }
      //tag_id_recv = rx_buffer[TAG_ID_IDX];
      memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],4);
      memcpy(&tx_resp_msg[TAG_ID_IDX],&tag_id_recv,4);
      //tx_resp_msg[TAG_ID_IDX] = tag_id_recv;
//      if(tag_recv_timer>tag_time_recv[tag_id_recv-TAG_ID_START])
//      {   tag_recv_interval =  tag_recv_timer - tag_time_recv[tag_id_recv];
//      }else{
//         tag_recv_interval = tag_recv_timer + 65535 - tag_time_recv[tag_id_recv];
//      }
      
      if (rx_buffer[MESSAGE_TYPE_IDX] == POLL&&tag_id_recv!= g_com_map[PAIR_ID]&&(tag_recv_interval>g_com_map[COM_INTERVAL]/2)&&(g_com_map[DEV_ID]%REPOET_ANC_NUM == rx_buffer[ANC_TYPE_IDX])) //判断是否是poll包数据
      if (rx_buffer[MESSAGE_TYPE_IDX] == POLL&&(anchor_type == rx_buffer[ANC_TYPE_IDX])) //判断是否是poll包数据
      {
         /* Retrieve poll reception timestamp. */
         poll_rx_ts = get_rx_timestamp_u64();//获得Poll包接收时间T2
@@ -472,7 +475,7 @@

         /* Write and send the response message. See NOTE 9 below.*/
         memcpy(&tx_resp_msg[DIST_IDX], &anchor_dist_last_frm[tag_id_recv], 2);
         tx_resp_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
		
         dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//写入发送数据
         dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//设定发送长度
         result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送，等待接收
@@ -500,8 +503,8 @@

            /* Check that the frame is a final message sent by "DS TWR initiator" example.
             * As the sequence number field of the frame is not used in this example, it can be zeroed to ease the validation of the frame. */
            rx_buffer[ALL_MSG_SN_IDX] = 0;
            if (rx_buffer[MESSAGE_TYPE_IDX] == FINAL&&rx_buffer[TAG_ID_IDX]==tag_id_recv&&rx_buffer[ANCHOR_ID_IDX]==g_com_map[DEV_ID]) //判断是否为Final包
			
            if (rx_buffer[MESSAGE_TYPE_IDX] == FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,4)&&!memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,4)) //判断是否为Final包
            {
               uint32_t poll_tx_ts, resp_rx_ts, final_tx_ts;
               uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
@@ -539,7 +542,7 @@
               LED0_BLINK; //每成功一次通讯则闪烁一次
               g_UWB_com_interval = 0;
               dis_after_filter=dist_cm;
               g_Tagdist[tag_id_recv]=dist_cm;
      //         g_Tagdist[tag_id_recv-TAG_ID_START]=dist_cm;
//               if(g_pairstart==1&&dist_cm<20)
//               {
//                  g_pairstart=0;
@@ -549,17 +552,21 @@
//                  delay_ms(1000);
//                  printf("Pair Finish PairID: %d. \r\n",g_com_map[PAIR_ID]);
//               }
                tag_time_recv[tag_id_recv] = tag_recv_timer;
               // tag_time_recv[tag_id_recv] = tag_recv_timer;
               g_flag_Taggetdist[tag_id_recv]=0;
               #ifdef HEX_OUTPUT
               usart_send[2] = frame_seq_nb;
               usart_send[6] = tag_id_recv;
               usart_send[8] = g_com_map[DEV_ID];
               //usart_send[6] = tag_id_recv;
               //usart_send[8] = g_com_map[DEV_ID];
               memcpy(&usart_send[3],&dev_id,4);
               memcpy(&usart_send[7],&dev_id,4);
               hex_dist = dist_cm;
               memcpy(&usart_send[10],&hex_dist,4);
               checksum = Checksum_u16(&usart_send[2],12);
               memcpy(&usart_send[14],&checksum,2);
               UART_PushFrame(usart_send,16);
               memcpy(&usart_send[11],&hex_dist,4);
               usart_send[15] = battary;
               usart_send[16] = button;
               checksum = Checksum_u16(&usart_send[2],19);
               memcpy(&usart_send[20],&checksum,2);
               UART_PushFrame(usart_send,23);
               #else
               printf("Anchor ID: %d, Tag ID: %d, Dist = %d cm\n", g_com_map[DEV_ID], tag_id_recv, (uint16_t)dis_after_filter);
               #endif