zhyinch
2020-04-11 66bdd9977d6bfe014389d2cf67e83cecedb6cd7e
Ô´Âë/ºËÐİå/Src/application/dw_app.c
@@ -65,20 +65,22 @@
#define FINAL_MSG_TS_LEN 4
#define SYNC_SEQ_IDX             5
//common
#define GROUP_ID_IDX               0
#define ANCHOR_ID_IDX             1
#define TAG_ID_IDX                5
#define MESSAGE_TYPE_IDX          9   
#define DIST_IDX                      10
//Poll
#define ANC_TYPE_IDX                14
#define BATTARY_IDX                  15
#define BUTTON_IDX                  16
#define SEQUENCE_IDX               17
//respose
#define DIST_IDX                      10
#define ANCTIMEMS             14
#define ANCTIMEUS             16
#define ANCSEND_INTERVAL      18
#define POLL                    0x01
#define RESPONSE                0x02
@@ -94,7 +96,7 @@
   DWT_PAC8,        /* Preamble acquisition chunk size. Used in RX only. */
   9,               /* TX preamble code. Used in TX only. */
   9,               /* RX preamble code. Used in RX only. */
   0,               /* Use non-standard SFD (Boolean) */
   1,               /* Use non-standard SFD (Boolean) */
   DWT_BR_6M8,      /* Data rate. */
   DWT_PHRMODE_STD, /* PHY header mode. */
   (129 + 8 - 8)    /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
@@ -104,14 +106,14 @@
static uint8_t tx_poll_msg[20] = {0};
static uint8_t tx_sync_msg[14] = {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};
static uint8_t tx_final_msg[24] = {0};
   
//static uint8_t rx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
static uint8_t tx_resp_msg[20] = {0};
static uint8_t tx_resp_msg[22] = {0};
//static uint8_t rx_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};
   
/* Frame sequence number, incremented after each transmission. */
static uint32_t frame_seq_nb = 0;
static uint32_t frame_seq_nb = 0,frame_seq_nb2=0;
   
/* Hold copy of status register state here for reference, so reader can examine it at a breakpoint. */
static uint32_t status_reg = 0;
@@ -134,7 +136,7 @@
static double tof;
   
uint32_t anchor_dist_last_frm[TAG_NUM_IN_SYS],his_dist[TAG_NUM_IN_SYS];   ;
int32_t anchor_dist_last_frm[TAG_NUM_IN_SYS],his_dist[TAG_NUM_IN_SYS];   ;
uint32_t tag_id = 0;
uint32_t tag_id_recv = 0;
uint8_t random_delay_tim = 0;
@@ -145,7 +147,7 @@
float dis_after_filter;            //当前距离值
LPFilter_Frac* p_Dis_Filter;      //测距用的低通滤波器
uint16_t g_Tagdist[TAG_NUM_IN_SYS];
int32_t g_Tagdist[TAG_NUM_IN_SYS];
uint8_t g_flag_Taggetdist[256];
/*------------------------------------ Functions ------------------------------------------*/
@@ -275,10 +277,16 @@
   tx_final_msg[MESSAGE_TYPE_IDX]=FINAL;
   tx_sync_msg[MESSAGE_TYPE_IDX]=SYNC;
   
   memcpy(&tx_poll_msg[GROUP_ID_IDX], &group_id, 1);
   memcpy(&tx_final_msg[GROUP_ID_IDX], &group_id, 1);
   memcpy(&tx_resp_msg[GROUP_ID_IDX], &group_id, 1);
   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);
   memcpy(&tx_sync_msg[ANCHOR_ID_IDX], &dev_id, 4);
   memcpy(&tx_resp_msg[ANCSEND_INTERVAL], &g_com_map[COM_INTERVAL], 2);
}   
uint16_t Checksum_u16(uint8_t* pdata, uint32_t len) 
{
@@ -313,7 +321,7 @@
uint16_t g_Resttimer;
uint8_t result;
u8 tag_succ_times=0;
int32_t hex_dist;
int32_t hex_dist,hex_dist2;
u16 checksum;
int8_t tag_delaytime;
extern uint16_t sync_timer;
@@ -350,7 +358,7 @@
   while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误
   { if(time32_incr - start_poll>20)
      NVIC_SystemReset();
      UART_CheckReceive();
      IdleTask();
      
   };
@@ -374,8 +382,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. */
      
      if (rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //判断接收到的数据是否是response数据
      {
      if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //判断接收到的数据是否是response数据
      { u16 anc_id_recv,rec_com_interval;
         /* Retrieve poll transmission and response reception timestamp. */
         poll_tx_ts = get_tx_timestamp_u64();                              //获得POLL发送时间T1
         resp_rx_ts = get_rx_timestamp_u64();                              //获得RESPONSE接收时间T4
@@ -395,8 +403,17 @@
         }
         TIM3->CNT=tmp_time;
      }
         memcpy(&anchor_dist_last_frm[0], &rx_buffer[DIST_IDX], 4);
         memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);
         memcpy(&tx_final_msg[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 4);
         memcpy(&rec_com_interval,&rx_buffer[ANCSEND_INTERVAL],  2);
         if(rec_com_interval>4&&rec_com_interval!=g_com_map[COM_INTERVAL])
         {
            g_com_map[COM_INTERVAL]=rec_com_interval;
            save_com_map_to_flash();
            delay_ms(100);
            SCB->AIRCR = 0X05FA0000|(unsigned int)0x04; //软复位回到bootloader
         }
         /* Compute final message transmission time. See NOTE 9 below. */
         final_tx_time = (resp_rx_ts + (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//计算final包发送时间,T5=T4+Treply2
         dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5
@@ -418,22 +435,31 @@
         tag_succ_times++;
         
            LED0_BLINK;
               memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
               if(hex_dist2!=0xffff)
               {
               g_Tagdist[anc_id_recv]=   hex_dist2;
               g_flag_Taggetdist[anc_id_recv]=0;
               
               if(!g_com_map[MODBUS_MODE])
               {
               hex_dist2 = hex_dist2;
               usart_send[2] = 1;//正常模式
               usart_send[3] = 17;//数据段长度
               usart_send[4] = frame_seq_nb;//数据段长度
               memcpy(&usart_send[5],&dev_id,2);
               memcpy(&usart_send[7],&rx_buffer[ANCHOR_ID_IDX],2);
               hex_dist = anchor_dist_last_frm[0]+(int16_t)g_com_map[DIST_OFFSET];
               memcpy(&usart_send[9],&hex_dist,4);
               memcpy(&usart_send[9],&hex_dist2,4);
               usart_send[13] = battary;
               usart_send[14] = button;
               checksum = Checksum_u16(&usart_send[2],17);
               memcpy(&usart_send[19],&checksum,2);
               UART_PushFrame(usart_send,21);
               memcpy(&Modbus_HoldReg[tag_id_recv*2],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
               }
            }
      //         memcpy(&Modbus_HoldReg[anc_id_recv*2],&hex_dist,4);
         /* Poll DW1000 until TX frame sent event set. See NOTE 8 below. */
         if(result==0)
         {while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成
@@ -460,22 +486,24 @@
//   deca_sleep(10);
}
//   dwt_entersleep();
   if(tag_succ_times<g_com_map[MIN_REPORT_ANC_NUM])
   {
   //poll_timer +=time32_incr&0x7+3;
   }
//   if(tag_succ_times<g_com_map[MIN_REPORT_ANC_NUM])
//   {
//   //poll_timer +=time32_incr&0x7+3;
//   }
   /* Execute a delay between ranging exchanges. */
   
}
int8_t correction_time;
extern uint8_t sync_seq;
#define TDFILTER
//#define CHECK_UID
extern uint8_t UID_ERROR;
void Anchor_App(void)
{
   uint32_t frame_len;
   uint32_t resp_tx_time;
   static u8 misdist_num;
   /* Clear reception timeout to start next ranging process. */
   dwt_setrxtimeout(0);//设定接收超时时间,0位没有超时时间
@@ -515,7 +543,7 @@
//         tag_recv_interval = tag_recv_timer + 65535 - tag_time_recv[tag_id_recv];
//      }
      
      if (rx_buffer[MESSAGE_TYPE_IDX] == POLL&&(anchor_type == rx_buffer[ANC_TYPE_IDX])) //判断是否是poll包数据
      if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == POLL&&(anchor_type == rx_buffer[ANC_TYPE_IDX])) //判断是否是poll包数据
      {
         tmp_time=TIM3->CNT;
         memcpy(&tx_resp_msg[ANCTIMEMS],&sync_timer,2);
@@ -537,7 +565,7 @@
         /* Write and send the response message. See NOTE 9 below.*/
         if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
         memcpy(&tx_resp_msg[DIST_IDX], &anchor_dist_last_frm[tag_id_recv-TAG_ID_START], 4);
         memcpy(&tx_resp_msg[DIST_IDX], &g_Tagdist[tag_id_recv], 4);
      
         dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//写入发送数据
         dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//设定发送长度
@@ -545,7 +573,7 @@
         battary = rx_buffer[BATTARY_IDX];
         button = rx_buffer[BUTTON_IDX];
         frame_seq_nb = rx_buffer[SEQUENCE_IDX];
         frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
         /* We assume that the transmission is achieved correctly, now poll for reception of expected "final" frame or error/timeout.
          * See NOTE 7 below. */
         if(result==0)
@@ -569,7 +597,7 @@
            /* 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. */
         
            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包
            if (rx_buffer[GROUP_ID_IDX] == group_id&&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;
@@ -585,6 +613,10 @@
               final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts);
               final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);
               #ifdef CHECK_UID
               if(UID_ERROR==1)
                  poll_rx_ts=0;
               #endif
               /* Compute time of flight. 32-bit subtractions give correct answers even if clock has wrapped. See NOTE 10 below. */
               poll_rx_ts_32 = (uint32_t)poll_rx_ts;//使用32位数据计算
               resp_tx_ts_32 = (uint32_t)resp_tx_ts;
@@ -599,40 +631,57 @@
               distance = tof * SPEED_OF_LIGHT;//距离=光速*飞行时间
               dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //距离减去矫正系数
               dist_cm = dist_no_bias * 100; //dis ä¸ºå•位为cm的距离
               dist_cm = dist_no_bias * 1000; //dis ä¸ºå•位为cm的距离
//               dist[TAG_ID] = LP(dis, TAG_ID); //LP ä¸ºä½Žé€šæ»¤æ³¢å™¨ï¼Œè®©æ•°æ®æ›´ç¨³å®š
               
               /*--------------------------以下为非测距逻辑------------------------*/
               LED0_BLINK; //每成功一次通讯则闪烁一次
               g_UWB_com_interval = 0;
               dis_after_filter=dist_cm;
               hex_dist = (int16_t)(dist_cm+g_com_map[DIST_OFFSET]);
                  if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
               if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])<1000)
               hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10;
               if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
               {
                  g_Tagdist[tag_id_recv-TAG_ID_START] = hex_dist;
                  anchor_dist_last_frm[tag_id_recv-TAG_ID_START] = hex_dist;
               }
               if(hex_dist-his_dist[tag_id_recv-TAG_ID_START]<15000||misdist_num>4)
               {int32_t filter_dist;
                  misdist_num=0;
               if(hex_dist<1000000&&hex_dist>-10000)
               {
                  #ifdef TDFILTER
                  NewTrackingDiffUpdate(tag_id_recv-TAG_ID_START, (float)hex_dist);
                  filter_dist=pos_predict[tag_id_recv-TAG_ID_START]/10;
                  #else
                  filter_dist=hex_dist/10;
                  #endif
                  anchor_dist_last_frm[tag_id_recv-TAG_ID_START]=filter_dist;
                  g_Tagdist[tag_id_recv]=   filter_dist;
               his_dist[tag_id_recv-TAG_ID_START]=hex_dist;
               g_flag_Taggetdist[tag_id_recv]=0;
               if(!g_com_map[MODBUS_MODE])
               {
                  usart_send[2] = 1;//正常模式
                  usart_send[3] = 17;//数据段长度
                  usart_send[4] = frame_seq_nb2;//数据段长度
                  memcpy(&usart_send[5],&tag_id_recv,2);
                  memcpy(&usart_send[7],&dev_id,2);
                  
               usart_send[2] = 1;//正常模式
               usart_send[3] = 17;//数据段长度
               usart_send[4] = frame_seq_nb;//数据段长度
               memcpy(&usart_send[5],&tag_id_recv,2);
               memcpy(&usart_send[7],&dev_id,2);
               memcpy(&usart_send[9],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
               usart_send[13] = battary;
               usart_send[14] = button;
               checksum = Checksum_u16(&usart_send[2],17);
               memcpy(&usart_send[19],&checksum,2);
               UART_PushFrame(usart_send,21);
               memcpy(&Modbus_HoldReg[tag_id_recv*2],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
                  memcpy(&usart_send[9],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
                  usart_send[13] = battary;
                  usart_send[14] = button;
                  checksum = Checksum_u16(&usart_send[2],17);
                  memcpy(&usart_send[19],&checksum,2);
                  UART_PushFrame(usart_send,21);
               }
               //   memcpy(&Modbus_HoldReg[tag_id_recv*2],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
               Modbus_HoldReg[tag_id_recv*2]=g_Tagdist[tag_id_recv-TAG_ID_START]>>16;
               Modbus_HoldReg[tag_id_recv*2+1]=g_Tagdist[tag_id_recv-TAG_ID_START];
               //dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm);
                  }
               }else{
                  misdist_num++;
               }
            }
            }
         }else{
            /* Clear RX error events in the DW1000 status register. */
@@ -640,8 +689,7 @@
         }
      }else if(rx_buffer[MESSAGE_TYPE_IDX] == SYNC)
      {
         if(rx_buffer[SYNC_SEQ_IDX]<sync_seq)
      //   if(rx_buffer[SYNC_SEQ_IDX]==2)
         if(rx_buffer[SYNC_SEQ_IDX]<sync_seq&&sync_mainbase==0)
         {
            sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
            TIM3->CNT = sync_seq*325%1000+15;