/*! ---------------------------------------------------------------------------- * @file main.c * @brief Double-sided two-way ranging (DS TWR) initiator example code * * * * @attention * * Copyright 2015 (c) Decawave Ltd, Dublin, Ireland. * * All rights reserved. * * @author Decawave */ #include #include #include "dw_app.h" #include "deca_device_api.h" #include "deca_regs.h" #include "dw_driver.h" #include "Spi.h" #include "led.h" #include "serial_at_cmd_app.h" #include "Usart.h" #include "global_param.h" #include "filters.h" #include #include "beep.h" #include "modbus.h" //#define DEBUG_OUTPUT #define TDFILTER #define CONSTANT_FILTER /*------------------------------------ Marcos ------------------------------------------*/ /* Inter-ranging delay period, in milliseconds. */ #define RNG_DELAY_MS 100 /* Default antenna delay values for 64 MHz PRF. See NOTE 1 below. */ #define TX_ANT_DLY 0 #define RX_ANT_DLY 32899 /* UWB microsecond (uus) to device time unit (dtu, around 15.65 ps) conversion factor. * 1 uus = 512 / 499.2 µs and 1 µs = 499.2 * 128 dtu. */ #define UUS_TO_DWT_TIME 65536 /* Delay between frames, in UWB microseconds. See NOTE 4 below. */ /* This is the delay from the end of the frame transmission to the enable of the receiver, as programmed for the DW1000's wait for response feature. */ #define POLL_TX_TO_RESP_RX_DLY_UUS 150 /* This is the delay from Frame RX timestamp to TX reply timestamp used for calculating/setting the DW1000's delayed TX function. This includes the * frame length of approximately 2.66 ms with above configuration. */ #define RESP_RX_TO_FINAL_TX_DLY_UUS 400 /* Receive response timeout. See NOTE 5 below. */ #define RESP_RX_TIMEOUT_UUS 9600 #define POLL_RX_TO_RESP_TX_DLY_UUS 420 /* This is the delay from the end of the frame transmission to the enable of the receiver, as programmed for the DW1000's wait for response feature. */ #define RESP_TX_TO_FINAL_RX_DLY_UUS 200 /* Receive final timeout. See NOTE 5 below. */ #define FINAL_RX_TIMEOUT_UUS 4300 #define SPEED_OF_LIGHT 299702547 /* Indexes to access some of the fields in the frames defined above. */ #define FINAL_MSG_POLL_TX_TS_IDX 10 #define FINAL_MSG_RESP_RX_TS_IDX 14 #define FINAL_MSG_FINAL_TX_TS_IDX 18 #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 //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 SIGNALPOWER 20 //DATA POLL #define CURENTPACKNUM_IDX 7 #define TOTALPACKNUM_IDX 8 #define DATA_IDX 10 #define POLL 0x01 #define RESPONSE 0x02 #define FINAL 0x03 #define SYNC 0x04 #define DATA_POLL 0x21 #define DATA_RESPONSE 0x22 /*------------------------------------ Variables ------------------------------------------*/ /* Default communication configuration. We use here EVK1000's default mode (mode 3). */ static dwt_config_t config = { 2, /* Channel number. */ DWT_PRF_64M, /* Pulse repetition frequency. */ DWT_PLEN_128, /* Preamble length. */ 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. */ 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. */ }; /* Frames used in the ranging process. See NOTE 2 below. */ static uint8_t tx_poll_msg[20] = {0}; static uint8_t tx_sync_msg[140] = {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[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[23] = {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,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; /* Buffer to store received response message. * Its size is adjusted to longest frame that this example code is supposed to handle. */ #define RX_BUF_LEN 224 static uint8_t rx_buffer[RX_BUF_LEN]; /* Time-stamps of frames transmission/reception, expressed in device time units. * As they are 40-bit wide, we need to define a 64-bit int type to handle them. */ static uint64_t poll_tx_ts; static uint64_t resp_rx_ts; static uint64_t final_tx_ts; /* Length of the common part of the message (up to and including the function code, see NOTE 2 below). */ static uint64_t poll_rx_ts; static uint64_t resp_tx_ts; static uint64_t final_rx_ts; static double tof; 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; double distance, dist_no_bias, dist_cm; uint32_t g_UWB_com_interval = 0; float dis_after_filter; //µ±Ç°¾àÀëÖµ LPFilter_Frac* p_Dis_Filter; //²â¾àÓõĵÍͨÂ˲¨Æ÷ int32_t g_Tagdist[TAG_NUM_IN_SYS]; uint8_t g_flag_Taggetdist[256]; /*------------------------------------ Functions ------------------------------------------*/ /*! ------------------------------------------------------------------------------------------------------------------ * @fn get_tx_timestamp_u64() * * @brief Get the TX time-stamp in a 64-bit variable. * /!\ This function assumes that length of time-stamps is 40 bits, for both TX and RX! * * @param none * * @return 64-bit value of the read time-stamp. */ static uint64_t get_tx_timestamp_u64(void) { uint8_t ts_tab[5]; uint64_t ts = 0; int i; dwt_readtxtimestamp(ts_tab); for (i = 4; i >= 0; i--) { ts <<= 8; ts |= ts_tab[i]; } return ts; } /*! ------------------------------------------------------------------------------------------------------------------ * @fn get_rx_timestamp_u64() * * @brief Get the RX time-stamp in a 64-bit variable. * /!\ This function assumes that length of time-stamps is 40 bits, for both TX and RX! * * @param none * * @return 64-bit value of the read time-stamp. */ static uint64_t get_rx_timestamp_u64(void) { uint8_t ts_tab[5]; uint64_t ts = 0; int i; dwt_readrxtimestamp(ts_tab); for (i = 4; i >= 0; i--) { ts <<= 8; ts |= ts_tab[i]; } return ts; } /*! ------------------------------------------------------------------------------------------------------------------ * @fn final_msg_set_ts() * * @brief Fill a given timestamp field in the final message with the given value. In the timestamp fields of the final * message, the least significant byte is at the lower address. * * @param ts_field pointer on the first byte of the timestamp field to fill * ts timestamp value * * @return none */ static void final_msg_set_ts(uint8_t *ts_field, uint64_t ts) { int i; for (i = 0; i < FINAL_MSG_TS_LEN; i++) { ts_field[i] = (uint8_t) ts; ts >>= 8; } } static void final_msg_get_ts(const uint8_t *ts_field, uint32_t *ts) { int i; *ts = 0; for (i = 0; i < FINAL_MSG_TS_LEN; i++) { *ts += ts_field[i] << (i * 8); } } void TagDistClear(void) { static uint16_t clear_judge_cnt; uint16_t i; if(clear_judge_cnt++>1000) //É趨1S·ÖƵ£¬Ã¿Ãë½øÒ»´Î¡£Åжϱê־λ´óÓÚµÈÓÚ2£¬2sûÊÕµ½Êý¾Ý¾Í°ÑÊý¾Ý±ä³É0xffff£¬²»´¥·¢¾¯±¨¡£ { clear_judge_cnt=0; for(i=0;i<100;i++) { g_flag_Taggetdist[i]++; if(g_flag_Taggetdist[i]>=2) { g_Tagdist[i]=0xffff; Modbus_HoldReg[i*2]=1; Modbus_HoldReg[i*2+1]=0xffff; } } } } void Dw1000_Init(void) { /* Reset and initialise DW1000. * For initialisation, DW1000 clocks must be temporarily set to crystal speed. After initialisation SPI rate can be increased for optimum * performance. */ Reset_DW1000();//ÖØÆôDW1000 /* Target specific drive of RSTn line into DW1000 low for a period. */ dwt_initialise(DWT_LOADUCODE);//³õʼ»¯DW1000 Spi_ChangePrescaler(SPIx_PRESCALER_FAST); //ÉèÖÃΪ¿ìËÙģʽ /* Configure DW1000. See NOTE 6 below. */ dwt_configure(&config);//ÅäÖÃDW1000 /* Apply default antenna delay value. See NOTE 1 below. */ dwt_setrxantennadelay(RX_ANT_DLY); //ÉèÖýÓÊÕÌìÏßÑÓ³Ù dwt_settxantennadelay(TX_ANT_DLY); //ÉèÖ÷¢ÉäÌìÏßÑÓ³Ù /* Set expected response's delay and timeout. See NOTE 4 and 5 below. * As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */ //ÉèÖýÓÊÕ³¬Ê±Ê±¼ä } void Dw1000_App_Init(void) { //g_com_map[DEV_ID] = 0x0b; tx_poll_msg[MESSAGE_TYPE_IDX]=POLL; tx_resp_msg[MESSAGE_TYPE_IDX]=RESPONSE; 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_sync_msg[TAG_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) { uint16_t sum = 0; uint32_t i; for(i=0; ifirstPathAmp1; F2 = dia->firstPathAmp2; F3 = dia->firstPathAmp3; N = dia->rxPreamCount; C = dia->maxGrowthCIR; firstpath_power=10* log10((F1*F1+F2*F2+F3*F3)/(N*N))-A; // rx_power=10*log10(C*B/(N*N))-A; // min_power = - 10 * log10((F1 *F1 + F2 * F2 + F3 * F3) / (C *B)); return min_power; } #define CONSTANT_LEN 50 extern u16 dist_threshold; int32_t ConstantFilter(int32_t currentdist,u8 channel) { static int32_t cs_lastdist[10],cs_lastvalid[10]; static u8 cfstart_flag[10] = {1}; static u8 constant_count[10] = {100}; if(cfstart_flag[channel]) { cfstart_flag[channel] = 0; cs_lastdist[channel] = currentdist; } if( abs(currentdist - cs_lastdist[channel])<100) { if(constant_count[channel]100) bat_percent=100; tx_poll_msg[BATTARY_IDX] = bat_percent;//Get_Battary(); tx_poll_msg[BUTTON_IDX] = !READ_KEY0; tx_poll_msg[SEQUENCE_IDX] = frame_seq_nb++; GPIO_WriteBit(GPIOA, GPIO_Pin_9, Bit_RESET); for(i=0;i20) NVIC_SystemReset(); IdleTask(); }; /* Increment frame sequence number after transmission of the poll message (modulo 256). */ if(status_reg==0xffffffff) { NVIC_SystemReset(); } if (status_reg & SYS_STATUS_RXFCG)//Èç¹û³É¹¦½ÓÊÕ { /* Clear good RX frame event and TX frame sent in the DW1000 status register. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//Çå³þ¼Ä´æÆ÷±ê־λ /* A frame has been received, read it into the local buffer. */ frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; //»ñµÃ½ÓÊÕµ½µÄÊý¾Ý³¤¶È dwt_readrxdata(rx_buffer, frame_len, 0); //¶ÁÈ¡½ÓÊÕÊý¾Ý /* 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[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 if(getsync_flag==0&&g_com_map[DEV_ROLE]) { getsync_flag=1; memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2); memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2); tmp_time=tmp_time+450; if(tmp_time>999) { tmp_time-=999; sync_timer++; if(sync_timer>=1010) {sync_timer=0;} } TIM3->CNT=tmp_time; } memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4); rec_firstpath_power = rx_buffer[SIGNALPOWER]; 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 /* Final TX timestamp is the transmission time we programmed plus the TX antenna delay. */ final_tx_ts = (((uint64_t)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;//final°üʵ¼Ê·¢ËÍʱ¼äÊǼÆËãʱ¼ä¼ÓÉÏ·¢ËÍÌìÏßdelay /* Write all timestamps in the final message. See NOTE 10 below. */ final_msg_set_ts(&tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//½«T1£¬T4£¬T5дÈë·¢ËÍÊý¾Ý final_msg_set_ts(&tx_final_msg[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts); 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. */ 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);//É趨ΪÑÓ³Ù·¢ËÍ #ifdef DEBUG_OUTPUT printf("F°ü·¢ËÍ,»ùÕ¾ID: %d .\r\n",i); #endif tag_succ_times++; LED0_BLINK; g_Resttimer=0; 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); memcpy(&usart_send[9],&hex_dist2,4); usart_send[13] = bat_percent; usart_send[14] = button; usart_send[15] = rec_firstpath_power; checksum = Checksum_u16(&usart_send[2],17); memcpy(&usart_send[19],&checksum,2); UART_PushFrame(usart_send,21); } } // 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))//²»¶Ï²éѯоƬ״ֱ̬µ½·¢ËÍÍê³É { }; } /* Clear TXFRS event. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//Çå³ý±ê־λ /* Increment frame sequence number after transmission of the final message (modulo 256). */ random_delay_tim = 0; } else { random_delay_tim = DFT_RAND_DLY_TIM_MS; //Èç¹ûͨѶʧ°Ü£¬½«¼ä¸ôʱ¼äÔö¼Ó5ms£¬±Ü¿ªÒòΪ¶à±êǩͬʱ·¢ËÍÒýÆðµÄ³åÍ»¡£ } } else { #ifdef DEBUG_OUTPUT printf("R°üʧ°Ü´íÎóÐÅÏ¢: %x .\r\n",status_reg); #endif /* Clear RX error events in the DW1000 status register. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); random_delay_tim = DFT_RAND_DLY_TIM_MS; } // deca_sleep(10); } // dwt_entersleep(); if(tag_succ_times<1) { tyncpoll_time=(current_slotpos--%max_slotnum)*slottime; } /* Execute a delay between ranging exchanges. */ } int8_t correction_time; extern uint8_t sync_seq; //#define CHECK_UID extern uint8_t UID_ERROR; u8 misdist_num[TAG_NUM_IN_SYS]; void Anchor_App(void) { uint32_t frame_len; uint32_t resp_tx_time; /* Clear reception timeout to start next ranging process. */ dwt_setrxtimeout(0);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä /* Activate reception immediately. */ dwt_rxenable(0);//´ò¿ª½ÓÊÕ /* Poll for reception of a frame or error/timeout. See NOTE 7 below. */ while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))&&!g_start_send_flag&&!g_start_sync_flag)//²»¶Ï²éѯоƬ״ֱ̬µ½½ÓÊճɹ¦»òÕß³öÏÖ´íÎó { IdleTask(); }; if (status_reg & SYS_STATUS_RXFCG)//³É¹¦½ÓÊÕ { u16 tag_recv_interval; /* Clear good RX frame event in the DW1000 status register. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//Çå³ý±ê־λ /* A frame has been received, read it into the local buffer. */ frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//»ñµÃ½ÓÊÕÊý¾Ý³¤¶È dwt_readrxdata(rx_buffer, frame_len, 0);//¶ÁÈ¡½ÓÊÕÊý¾Ý /* 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. */ //½«ÊÕµ½µÄtag_id·Ö±ðдÈë¸÷´ÎͨѶµÄ°üÖУ¬Îª¶à±êǩͨѶ·þÎñ£¬·ÀÖ¹Ò»´ÎͨѶÖнÓÊÕµ½²»Í¬ID±êÇ©µÄÊý¾Ý //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[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); memcpy(&tx_resp_msg[ANCTIMEUS],&tmp_time,2); // if(correction_time>10) // {correction_time++;} /* Retrieve poll reception timestamp. */ poll_rx_ts = get_rx_timestamp_u64();//»ñµÃPoll°ü½ÓÊÕʱ¼äT2 /* Set send time for response. See NOTE 8 below. */ resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//¼ÆËãResponse·¢ËÍʱ¼äT3¡£ dwt_setdelayedtrxtime(resp_tx_time);//ÉèÖÃResponse·¢ËÍʱ¼äT3 /* Set expected delay and timeout for final message reception. */ dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);//ÉèÖ÷¢ËÍÍê³Éºó¿ªÆô½ÓÊÕÑÓ³Ùʱ¼ä dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//½ÓÊÕ³¬Ê±Ê±¼ä /* 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], &g_Tagdist[tag_id_recv], 4); tx_resp_msg[SIGNALPOWER] = firstpath_power; 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);//ÑÓ³Ù·¢ËÍ£¬µÈ´ý½ÓÊÕ battary = rx_buffer[BATTARY_IDX]; button = rx_buffer[BUTTON_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. */ #ifdef DEBUG_OUTPUT printf("ÊÕµ½POLL°ü£¬±êÇ©ID: %d .\r\n",tag_id_recv); #endif if(result==0) { while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))///²»¶Ï²éѯоƬ״ֱ̬µ½½ÓÊճɹ¦»òÕß³öÏÖ´íÎó { }; } /* Increment frame sequence number after transmission of the response message (modulo 256). */ if (status_reg & SYS_STATUS_RXFCG)//½ÓÊճɹ¦ { /* Clear good RX frame event and TX frame sent in the DW1000 status register. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//Çå³þ±ê־λ /* A frame has been received, read it into the local buffer. */ frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;//Êý¾Ý³¤¶È dwt_readrxdata(rx_buffer, frame_len, 0);//¶ÁÈ¡½ÓÊÕÊý¾Ý /* 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[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; double Ra, Rb, Da, Db; int64_t tof_dtu; /* Retrieve response transmission and final reception timestamps. */ resp_tx_ts = get_tx_timestamp_u64();//»ñµÃresponse·¢ËÍʱ¼äT3 final_rx_ts = get_rx_timestamp_u64();//»ñµÃfinal½ÓÊÕʱ¼äT6 /* Get timestamps embedded in the final message. */ final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);//´Ó½ÓÊÕÊý¾ÝÖжÁÈ¡T1£¬T4£¬T5 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; final_rx_ts_32 = (uint32_t)final_rx_ts; Ra = (double)(resp_rx_ts - poll_tx_ts);//Tround1 = T4 - T1 Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);//Tround2 = T6 - T3 Da = (double)(final_tx_ts - resp_rx_ts);//Treply2 = T5 - T4 Db = (double)(resp_tx_ts_32 - poll_rx_ts_32);//Treply1 = T3 - T2 tof_dtu = (int64_t)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));//¼ÆË㹫ʽ tof = tof_dtu * DWT_TIME_UNITS; distance = tof * SPEED_OF_LIGHT;//¾àÀë=¹âËÙ*·ÉÐÐʱ¼ä dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //¾àÀë¼õÈ¥½ÃÕýϵÊý dist_cm = dist_no_bias * 1000; //dis Ϊµ¥Î»ÎªcmµÄ¾àÀë // dist[TAG_ID] = LP(dis, TAG_ID); //LP ΪµÍͨÂ˲¨Æ÷£¬ÈÃÊý¾Ý¸üÎȶ¨ dwt_readdiagnostics(&d1); LOS(&d1); /*--------------------------ÒÔÏÂΪ·Ç²â¾àÂß¼­------------------------*/ #ifdef DEBUG_OUTPUT printf("ÊÕµ½FINAL°ü£¬±êÇ©ID: %d .\r\n",tag_id_recv); #endif LED0_BLINK; //ÿ³É¹¦Ò»´ÎͨѶÔòÉÁ˸һ´Î g_UWB_com_interval = 0; g_Resttimer=0; hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10; if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS) { if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])4) { int32_t filter_dist; misdist_num[tag_id_recv-TAG_ID_START]=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 #ifdef CONSTANT_FILTER filter_dist = ConstantFilter(filter_dist,tag_id_recv-TAG_ID_START); #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); memcpy(&usart_send[9],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4); usart_send[13] = battary; usart_send[14] = button; usart_send[15] = firstpath_power; 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[tag_id_recv-TAG_ID_START]++; } } } }else{ /* Clear RX error events in the DW1000 status register. */ #ifdef DEBUG_OUTPUT printf("F°üʧ°Ü´íÎóÐÅÏ¢: %x .\r\n",status_reg); #endif dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); } }else if(rx_buffer[MESSAGE_TYPE_IDX] == SYNC) { if(rx_buffer[SYNC_SEQ_IDX]CNT = sync_seq*325%1000+15; sync_timer = sync_seq*325/1000; SyncPoll(sync_seq); } } } else { #ifdef DEBUG_OUTPUT printf("P°üʧ°Ü´íÎóÐÅÏ¢: %x .\r\n",status_reg); #endif /* Clear RX error events in the DW1000 status register. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); } } u8 frame_len,jumptime; u8 rec_gpsdata[1000]; extern u8 RTCMdata[2000]; u16 recgpsdata_i,recdata_len; void UWBSendOnePackData(u8* data_addr,u8 len) { delay_ms(jumptime); g_Resttimer = 0; LED0_BLINK; g_start_sync_flag=1; dwt_forcetrxoff(); dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); //ÉèÖ÷¢Ëͺó¿ªÆô½ÓÊÕ£¬²¢É趨ÑÓ³Ùʱ¼ä dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); tx_sync_msg[MESSAGE_TYPE_IDX]=DATA_POLL; memcpy(&tx_sync_msg[DATA_IDX],data_addr,len); dwt_writetxdata(len+14, tx_sync_msg, 0);//½«Poll°üÊý¾Ý´«¸øDW1000£¬½«ÔÚ¿ªÆô·¢ËÍʱ´«³öÈ¥ dwt_writetxfctrl(len+14, 0);//ÉèÖó¬¿í´ø·¢ËÍÊý¾Ý³¤¶È dwt_starttx(DWT_START_TX_IMMEDIATE| DWT_RESPONSE_EXPECTED); start_poll = time32_incr; recgpsdata_i = 0; while(time32_incr - start_poll<100) { 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)//Èç¹û³É¹¦½ÓÊÕ { /* Clear good RX frame event and TX frame sent in the DW1000 status register. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//Çå³þ¼Ä´æÆ÷±ê־λ /* A frame has been received, read it into the local buffer. */ frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; //»ñµÃ½ÓÊÕµ½µÄÊý¾Ý³¤¶È dwt_readrxdata(rx_buffer, frame_len, 0); //¶ÁÈ¡½ÓÊÕÊý¾Ý dwt_rxenable(0); /* 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] == DATA_RESPONSE ) //ÅжϽÓÊÕµ½µÄÊý¾ÝÊÇ·ñÊÇresponseÊý¾Ý { recdata_len = frame_len-14; memcpy(&rec_gpsdata[recgpsdata_i],&rx_buffer[DATA_IDX],recdata_len); recgpsdata_i+=recdata_len; if(rx_buffer[CURENTPACKNUM_IDX]==1) { USART_puts(rec_gpsdata,recgpsdata_i); break; } }else{ jumptime=time32_incr%20; } }else{ jumptime=time32_incr%20; } } } char *HIDO_UtilStrnchr(const char *_pcStr, char _cChr, HIDO_UINT32 u32N) { int32_t i = 0; for (i = 0; i < u32N; i++) { if (_pcStr[i] == _cChr) { return (char *) (_pcStr + i); } } return NULL; } HIDO_UINT8 HIDO_UtilCharToHex(HIDO_CHAR _cCh) { return (_cCh >= '0' && _cCh <= '9') ? (_cCh - '0') : (_cCh >= 'A' && _cCh <= 'F' ? (_cCh - 'A' + 0x0A) : (_cCh >= 'a' && _cCh <= 'f' ? _cCh - 'a' + 0x0A : 0)); } HIDO_UINT32 HIDO_UtilStrBufToInt(HIDO_CHAR *_pcStringBuf, HIDO_UINT32 _u32BufLen) { HIDO_UINT32 u32Temp = 0; while (_u32BufLen != 0) { u32Temp = u32Temp * 10 + HIDO_UtilCharToHex(*_pcStringBuf); _pcStringBuf++; _u32BufLen--; } return u32Temp; } /****************************************************************************** Description : Get gps latitude from gps data parameter : data: gps data pointer, len: gps data length, gps: ST_GPS pointer Return : TRUE on success, FALSE on fail author : DuJian history : 2012-7-27 new ******************************************************************************/ static int32_t GPS_ParseLat(HIDO_DataStruct *_pstLatData, double *lat) { uint32_t u32Len = _pstLatData->m_u32Len; char *pcStart = (char *) _pstLatData->m_pData; char *pcDot = NULL; uint32_t u32TempLen = 0; double dd; double mmmm; double mm; if (u32Len < 9) { return HIDO_ERR; } pcDot = HIDO_UtilStrnchr(pcStart, '.', u32Len); if (NULL == pcDot || (pcDot - pcStart) != 4) { return HIDO_ERR; } dd = HIDO_UtilStrBufToInt(pcStart, 2); mm = HIDO_UtilStrBufToInt(pcStart + 2, 2); u32TempLen = u32Len - (pcDot + 1 - pcStart); mmmm = HIDO_UtilStrBufToInt(pcDot + 1, u32TempLen); while(u32TempLen != 0) { mmmm /= 10.0; u32TempLen--; } mm = mm + mmmm; *lat = dd + (mm / 60.0); return HIDO_OK; } static int32_t GPS_ParseHeight(HIDO_DataStruct *_pstLatData, double *height) { uint32_t u32Len = _pstLatData->m_u32Len; char *pcStart = (char *) _pstLatData->m_pData; char *pcDot = NULL; uint32_t u32TempLen = 0; double dd; double mmmm; double mm; pcDot = HIDO_UtilStrnchr(pcStart, '.', u32Len); u32TempLen = pcDot - pcStart; mm = HIDO_UtilStrBufToInt(pcStart, u32TempLen); u32TempLen = u32Len - (pcDot + 1 - pcStart); mmmm = HIDO_UtilStrBufToInt(pcDot + 1, u32TempLen); while(u32TempLen != 0) { mmmm /= 10.0; u32TempLen--; } *height = mm + mmmm; return HIDO_OK; } /****************************************************************************** Description : Get gps longitude from gps data parameter : data: gps data pointer, len: gps data length, gps: ST_GPS pointer Return : TRUE on success, FALSE on fail author : DuJian history : 2012-7-27 new ******************************************************************************/ static int32_t GPS_ParseLon(HIDO_DataStruct *_pstLonData, double *lon) { uint32_t u32Len = _pstLonData->m_u32Len; char *pcStart = (char *) _pstLonData->m_pData; char *pcDot = NULL; uint32_t u32TempLen = 0; double ddd; double mmmm; double mm; if (u32Len < 10) { return HIDO_ERR; } pcDot = HIDO_UtilStrnchr(pcStart, '.', u32Len); if (NULL == pcDot || (pcDot - pcStart) != 5) { return HIDO_ERR; } ddd = HIDO_UtilStrBufToInt(pcStart, 3); mm = HIDO_UtilStrBufToInt(pcStart + 3, 2); u32TempLen = u32Len - (pcDot + 1 - pcStart); mmmm = HIDO_UtilStrBufToInt(pcDot + 1, u32TempLen); while(u32TempLen != 0) { mmmm /= 10.0; u32TempLen--; } mm = mm + mmmm; *lon = ddd + (mm / 60.0); return HIDO_OK; } extern u8 gpsdataready_flag; extern u16 gps_packlen; u8 totalpack_num,currentpack_num; u16 sendtimes; u32 rec_urtid; void RecOnePackData(void) { dwt_setrxtimeout(0);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä /* Activate reception immediately. */ dwt_rxenable(0);//´ò¿ª½ÓÊÕ /* Poll for reception of a frame or error/timeout. See NOTE 7 below. */ while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))&&!g_start_send_flag&&!g_start_sync_flag)//²»¶Ï²éѯоƬ״ֱ̬µ½½ÓÊճɹ¦»òÕß³öÏÖ´íÎó { IdleTask(); }; if (status_reg & SYS_STATUS_RXFCG)//³É¹¦½ÓÊÕ { u16 tag_recv_interval,send_i=0,remain_i=0; /* Clear good RX frame event in the DW1000 status register. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//Çå³ý±ê־λ /* A frame has been received, read it into the local buffer. */ frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//»ñµÃ½ÓÊÕÊý¾Ý³¤¶È g_Resttimer = 0; dwt_readrxdata(rx_buffer, frame_len, 0);//¶ÁÈ¡½ÓÊÕÊý¾Ý if (rx_buffer[MESSAGE_TYPE_IDX] == DATA_POLL ) //ÅжϽÓÊÕµ½µÄÊý¾ÝÊÇ·ñÊÇresponseÊý¾Ý { recdata_len = frame_len-14; memcpy(rec_gpsdata,&rx_buffer[DATA_IDX],recdata_len); // if(gpsdataready_flag) { gpsdataready_flag = 0; tx_sync_msg[MESSAGE_TYPE_IDX]=DATA_RESPONSE; remain_i = gps_packlen; currentpack_num = 0; totalpack_num = gps_packlen/110; while(remain_i>0) { if(remain_i>=110) { tx_sync_msg[CURENTPACKNUM_IDX] = 0; memcpy(&tx_sync_msg[DATA_IDX],&RTCMdata[send_i],110); send_i+=110; remain_i-=110; dwt_writetxdata(110+14, tx_sync_msg, 0);//½«Poll°üÊý¾Ý´«¸øDW1000£¬½«ÔÚ¿ªÆô·¢ËÍʱ´«³öÈ¥ dwt_writetxfctrl(110+14, 0);//ÉèÖó¬¿í´ø·¢ËÍÊý¾Ý³¤¶È dwt_starttx(DWT_START_TX_IMMEDIATE); sendtimes++; }else{ tx_sync_msg[CURENTPACKNUM_IDX] = 1; memcpy(&tx_sync_msg[DATA_IDX],&RTCMdata[send_i],remain_i); dwt_writetxdata(remain_i+14, tx_sync_msg, 0);//½«Poll°üÊý¾Ý´«¸øDW1000£¬½«ÔÚ¿ªÆô·¢ËÍʱ´«³öÈ¥ dwt_writetxfctrl(remain_i+14, 0);//ÉèÖó¬¿í´ø·¢ËÍÊý¾Ý³¤¶È dwt_starttx(DWT_START_TX_IMMEDIATE); remain_i = 0; sendtimes++; } delay_us(1000); } } LED0_BLINK; extern HIDO_DataStruct stPosState[4]; GPS_ParseGGA(rec_gpsdata,recdata_len); const char *fmt = "{\"battery\":4.2,\"dev_type\":\"11\",\"device_sn\":\"%d\",\"gps_type\":%d,\"high\":%.8lf,\"lat\":%.8lf,\"lng\":%.8lf}"; double lat = 0; double lon = 0; double high = 0; uint8_t gps_type; gps_type = HIDO_UtilStrBufToInt(stPosState[2].m_pData,1); if(gps_type!=0) { GPS_ParseLon(&stPosState[1], &lon); GPS_ParseLat(&stPosState[0], &lat); GPS_ParseHeight(&stPosState[3], &high); } memcpy(&rec_urtid,&rx_buffer[ANCHOR_ID_IDX],4); printf(fmt,rec_urtid,gps_type, high, lat, lon); //USART_puts(rec_gpsdata,recdata_len); } }else{ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); } }