| | |
| | | #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 <stdio.h> |
| | | |
| | | |
| | | |
| | | /*------------------------------------ Marcos ------------------------------------------*/ |
| | |
| | | #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 4100 |
| | | #define RESP_RX_TO_FINAL_TX_DLY_UUS 1500 |
| | | /* Receive response timeout. See NOTE 5 below. */ |
| | | #define RESP_RX_TIMEOUT_UUS 14700 |
| | | #define RESP_RX_TIMEOUT_UUS 2700 |
| | | |
| | | #define POLL_RX_TO_RESP_TX_DLY_UUS 3600 |
| | | #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 500 |
| | | #define RESP_TX_TO_FINAL_RX_DLY_UUS 200 |
| | | /* Receive final timeout. See NOTE 5 below. */ |
| | | #define FINAL_RX_TIMEOUT_UUS 4300 |
| | | |
| | |
| | | #define FINAL_MSG_TS_LEN 4 |
| | | |
| | | #define GROUP_ID_IDX 0 |
| | | #define SOURCE_ID_IDX 1 |
| | | #define MESSAGE_TYPE_IDX 3 |
| | | #define ANCHOR_ID_IDX 1 |
| | | #define TAG_ID_IDX 3 |
| | | #define MESSAGE_TYPE_IDX 5 |
| | | #define DIST_IDX 6 |
| | | |
| | | #define POLL 0x01 |
| | | #define RESPONSE 0x02 |
| | |
| | | |
| | | /*------------------------------------ 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_1024, /* Preamble length. */ |
| | | DWT_PAC32, /* 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_110K, /* Data rate. */ |
| | | DWT_PHRMODE_STD, /* PHY header mode. */ |
| | | (1025 + 64 - 32) /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */ |
| | | static dwt_config_t config = { |
| | | 5, /* 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. */ |
| | | 0, /* 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 tx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0}; |
| | | //static uint8 rx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0}; |
| | | static uint8 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_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 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 rx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0}; |
| | | static uint8 tx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0}; |
| | | //static uint8 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}; |
| | | //static uint8_t rx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0}; |
| | | static uint8_t tx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 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 frame_seq_nb = 0; |
| | | static uint32_t frame_seq_nb = 0; |
| | | |
| | | /* Hold copy of status register state here for reference, so reader can examine it at a breakpoint. */ |
| | | static uint32 status_reg = 0; |
| | | 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 24 |
| | | static uint8 rx_buffer[RX_BUF_LEN]; |
| | | 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. */ |
| | | typedef unsigned long long uint64; |
| | | static uint64 poll_tx_ts; |
| | | static uint64 resp_rx_ts; |
| | | static uint64 final_tx_ts; |
| | | 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). */ |
| | | typedef signed long long int64; |
| | | static uint64 poll_rx_ts; |
| | | static uint64 resp_tx_ts; |
| | | static uint64 final_rx_ts; |
| | | static uint64_t poll_rx_ts; |
| | | static uint64_t resp_tx_ts; |
| | | static uint64_t final_rx_ts; |
| | | |
| | | static double tof; |
| | | |
| | |
| | | |
| | | double distance, dist_no_bias, dist_cm; |
| | | |
| | | uint32_t g_UWB_com_interval = 0; |
| | | float dis_after_filter; //å½åè·ç¦»å¼ |
| | | LPFilter_Frac* p_Dis_Filter; //æµè·ç¨çä½éæ»¤æ³¢å¨ |
| | | |
| | | uint16_t g_Tagdist[256]; |
| | | uint8_t g_flag_Taggetdist[256]; |
| | | /*------------------------------------ Functions ------------------------------------------*/ |
| | | |
| | | |
| | |
| | | * |
| | | * @return 64-bit value of the read time-stamp. |
| | | */ |
| | | static uint64 get_tx_timestamp_u64(void) |
| | | static uint64_t get_tx_timestamp_u64(void) |
| | | { |
| | | uint8 ts_tab[5]; |
| | | uint64 ts = 0; |
| | | uint8_t ts_tab[5]; |
| | | uint64_t ts = 0; |
| | | int i; |
| | | dwt_readtxtimestamp(ts_tab); |
| | | for (i = 4; i >= 0; i--) |
| | |
| | | * |
| | | * @return 64-bit value of the read time-stamp. |
| | | */ |
| | | static uint64 get_rx_timestamp_u64(void) |
| | | static uint64_t get_rx_timestamp_u64(void) |
| | | { |
| | | uint8 ts_tab[5]; |
| | | uint64 ts = 0; |
| | | uint8_t ts_tab[5]; |
| | | uint64_t ts = 0; |
| | | int i; |
| | | dwt_readrxtimestamp(ts_tab); |
| | | for (i = 4; i >= 0; i--) |
| | |
| | | * |
| | | * @return none |
| | | */ |
| | | static void final_msg_set_ts(uint8 *ts_field, uint64 ts) |
| | | 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) ts; |
| | | ts_field[i] = (uint8_t) ts; |
| | | ts >>= 8; |
| | | } |
| | | } |
| | | |
| | | static void final_msg_get_ts(const uint8 *ts_field, uint32 *ts) |
| | | static void final_msg_get_ts(const uint8_t *ts_field, uint32_t *ts) |
| | | { |
| | | int i; |
| | | *ts = 0; |
| | |
| | | *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<255;i++) |
| | | { |
| | | g_flag_Taggetdist[i]++; |
| | | if(g_flag_Taggetdist[i]>=2) |
| | | { |
| | | g_Tagdist[i]=0xffff; |
| | | } |
| | | } |
| | | } |
| | | } |
| | | void Dw1000_Init(void) |
| | | { |
| | | /* Reset and initialise DW1000. |
| | |
| | | |
| | | /* 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); //设置åå°å¤©çº¿å»¶è¿ |
| | |
| | | dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); //设置åéåå¼å¯æ¥æ¶ï¼å¹¶è®¾å®å»¶è¿æ¶é´ |
| | | dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); //è®¾ç½®æ¥æ¶è¶
æ¶æ¶é´ |
| | | } |
| | | |
| | | void Dw1000_App_Init(void) |
| | | { |
| | | g_com_map[DEV_ID] = 0x04; |
| | | 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); |
| | | |
| | | } |
| | | void tag_sleep_configuraion(void) |
| | | { |
| | | dwt_configuresleep(0x940, 0x7); |
| | | dwt_entersleep(); |
| | | } |
| | | uint8_t result; |
| | | void Tag_App(void)//å鿍¡å¼(TAGæ ç¾) |
| | | { |
| | | uint32 frame_len; |
| | | uint32 final_tx_time; |
| | | uint32_t frame_len; |
| | | uint32_t final_tx_time; |
| | | |
| | | GPIO_ResetBits(SPIx_GPIO, SPIx_CS); |
| | | delay_us(2500); |
| | | GPIO_SetBits(SPIx_GPIO, SPIx_CS); |
| | | |
| | | /* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */ |
| | | tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb; |
| | |
| | | /* 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[9] == 0x10) //å¤ææ¥æ¶å°çæ°æ®æ¯å¦æ¯responseæ°æ® |
| | | if (rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE) //å¤ææ¥æ¶å°çæ°æ®æ¯å¦æ¯responseæ°æ® |
| | | { |
| | | /* 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 |
| | | |
| | | memcpy(&anchor_dist_last_frm[tag_id], &rx_buffer[11], 2); |
| | | |
| | | |
| | | memcpy(&anchor_dist_last_frm[tag_id], &rx_buffer[DIST_IDX], 2); |
| | | memcpy(&tx_final_msg[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 2); |
| | | /* 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)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;//finalå
å®é
åéæ¶é´æ¯è®¡ç®æ¶é´å ä¸åé天线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åå
¥åéæ°æ® |
| | |
| | | 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);//设å®åéæ°æ®é¿åº¦ |
| | | dwt_starttx(DWT_START_TX_DELAYED);//设å®ä¸ºå»¶è¿åé |
| | | |
| | | //è¿é为串å£è¾åº |
| | | // if (GPIO_ReadInputDataBit(GPIOA, SW2) != RESET) //éè¿æ¨ç å¼å
³å¤ææ°æ®è¾åºæ ¼å¼ |
| | | // { |
| | | // dID = TAG_ID; |
| | | // printf("TAG_ID: %2.0f ", dID); |
| | | // dID = ANCHOR_ID; |
| | | // printf("ANCHOR_ID: %2.0f ", dID); |
| | | // printf("Distance: %5.0f cm\n", (double)dist[TAG_ID]); |
| | | // } |
| | | // else |
| | | // { |
| | | // send[2] = ANCHOR_ID; |
| | | // send[3] = TAG_ID; |
| | | |
| | | // memcpy(&send[4], &dist[TAG_ID], 2); |
| | | // check = Checksum_u16(&send[2], 6); |
| | | // memcpy(&send[8], &check, 2); |
| | | // USART_puts(send, 10); |
| | | // } |
| | | result=dwt_starttx(DWT_START_TX_DELAYED);//设å®ä¸ºå»¶è¿åé |
| | | |
| | | |
| | | /* Poll DW1000 until TX frame sent event set. See NOTE 8 below. */ |
| | | while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//䏿æ¥è¯¢è¯çç¶æç´å°åé宿 |
| | | if(result==0) |
| | | {while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//䏿æ¥è¯¢è¯çç¶æç´å°åé宿 |
| | | { }; |
| | | |
| | | } |
| | | /* Clear TXFRS event. */ |
| | | dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//æ¸
餿 å¿ä½ |
| | | |
| | |
| | | } |
| | | LED0_BLINK; |
| | | /* Execute a delay between ranging exchanges. */ |
| | | deca_sleep(RNG_DELAY_MS + random_delay_tim); //ä¼ç åºå®æ¶é´ |
| | | dwt_entersleep(); |
| | | |
| | | } |
| | | |
| | | uint16_t g_Resttimer; |
| | | void Anchor_App(void) |
| | | { |
| | | uint32 frame_len; |
| | | uint32 resp_tx_time; |
| | | uint32_t frame_len; |
| | | uint32_t resp_tx_time; |
| | | |
| | | /* Clear reception timeout to start next ranging process. */ |
| | | dwt_setrxtimeout(0);//è®¾å®æ¥æ¶è¶
æ¶æ¶é´ï¼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)))//䏿æ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæè
åºç°é误 |
| | | { }; |
| | | { |
| | | UART_CheckReceive(); |
| | | UART_CheckSend(); |
| | | g_Resttimer=0; |
| | | }; |
| | | |
| | | if (status_reg & SYS_STATUS_RXFCG)//æåæ¥æ¶ |
| | | { |
| | |
| | | rx_buffer[ALL_MSG_SN_IDX] = 0; |
| | | |
| | | //å°æ¶å°çtag_idåå«åå
¥å次é讯çå
ä¸ï¼ä¸ºå¤æ ç¾é讯æå¡ï¼é²æ¢ä¸æ¬¡éè®¯ä¸æ¥æ¶å°ä¸åIDæ ç¾çæ°æ® |
| | | tag_id_recv = rx_buffer[5]; |
| | | tx_resp_msg[5] = tag_id_recv; |
| | | tag_id_recv = rx_buffer[TAG_ID_IDX]; |
| | | tx_resp_msg[TAG_ID_IDX] = tag_id_recv; |
| | | |
| | | |
| | | if (rx_buffer[9] == 0x21) //夿æ¯å¦æ¯pollå
æ°æ® |
| | | if (rx_buffer[MESSAGE_TYPE_IDX] == POLL) //夿æ¯å¦æ¯pollå
æ°æ® |
| | | { |
| | | /* Retrieve poll reception timestamp. */ |
| | | poll_rx_ts = get_rx_timestamp_u64();//è·å¾Pollå
æ¥æ¶æ¶é´T2 |
| | |
| | | dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//æ¥æ¶è¶
æ¶æ¶é´ |
| | | |
| | | /* Write and send the response message. See NOTE 9 below.*/ |
| | | memcpy(&tx_resp_msg[11], &anchor_dist_last_frm[tag_id_recv], 2); |
| | | 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);//设å®åéé¿åº¦ |
| | | dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//å»¶è¿åéï¼çå¾
æ¥æ¶ |
| | | result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//å»¶è¿åéï¼çå¾
æ¥æ¶ |
| | | |
| | | /* We assume that the transmission is achieved correctly, now poll for reception of expected "final" frame or error/timeout. |
| | | * See NOTE 7 below. */ |
| | | while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))///䏿æ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæè
åºç°é误 |
| | | 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). */ |
| | | frame_seq_nb++; |
| | | |
| | |
| | | /* 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[9] == 0x23) //夿æ¯å¦ä¸ºFinalå
|
| | | 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å
|
| | | { |
| | | uint32 poll_tx_ts, resp_rx_ts, final_tx_ts; |
| | | uint32 poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32; |
| | | 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; |
| | | |
| | |
| | | final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts); |
| | | |
| | | /* Compute time of flight. 32-bit subtractions give correct answers even if clock has wrapped. See NOTE 10 below. */ |
| | | poll_rx_ts_32 = (uint32)poll_rx_ts;//使ç¨32使°æ®è®¡ç® |
| | | resp_tx_ts_32 = (uint32)resp_tx_ts; |
| | | final_rx_ts_32 = (uint32)final_rx_ts; |
| | | 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)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));//计ç®å
¬å¼ |
| | | tof_dtu = (int64_t)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));//计ç®å
¬å¼ |
| | | |
| | | tof = tof_dtu * DWT_TIME_UNITS; |
| | | distance = tof * SPEED_OF_LIGHT;//è·ç¦»=å
é*é£è¡æ¶é´ |
| | |
| | | dist_cm = dist_no_bias * 100; //dis 为åä½ä¸ºcmçè·ç¦» |
| | | // dist[TAG_ID] = LP(dis, TAG_ID); //LP 为ä½é滤波å¨ï¼è®©æ°æ®æ´ç¨³å® |
| | | |
| | | /*--------------------------以ä¸ä¸ºéæµè·é»è¾------------------------*/ |
| | | LED0_BLINK; //æ¯æå䏿¬¡é讯åéªç䏿¬¡ |
| | | //è¿éä¾ä¸²å£è¾åº |
| | | // if (GPIO_ReadInputDataBit(GPIOA, SW2) != RESET) //éè¿æ¨ç å¼å
³å¤ææ°æ®è¾åºæ ¼å¼ |
| | | // { |
| | | // dID = TAG_ID; |
| | | // printf("TAG_ID: %2.0f ", dID); |
| | | // dID = ANCHOR_ID; |
| | | // printf("ANCHOR_ID: %2.0f ", dID); |
| | | // printf("Distance: %5.0f cm\n", (double)dist[TAG_ID]); |
| | | // } |
| | | // else |
| | | // { |
| | | // send[2] = ANCHOR_ID; |
| | | // send[3] = TAG_ID; |
| | | |
| | | // memcpy(&send[4], &dist[TAG_ID], 2); |
| | | // check = Checksum_u16(&send[2], 6); |
| | | // memcpy(&send[8], &check, 2); |
| | | // USART_puts(send, 10); |
| | | // } |
| | | g_UWB_com_interval = 0; |
| | | dis_after_filter=dist_cm; |
| | | g_Tagdist[tag_id_recv]=dist_cm; |
| | | g_flag_Taggetdist[tag_id_recv]=0; |
| | | printf("Anchor ID: %d, Tag ID: %d, Dist = %d cm\n", g_com_map[DEV_ID], tag_id_recv, (uint16_t)dis_after_filter); |
| | | //dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm); |
| | | |
| | | } |
| | | } |