| | |
| | | * @file main.c |
| | | * @brief Double-sided two-way ranging (DS TWR) initiator example code |
| | | * |
| | | * This is a simple code example which acts as the initiator in a DS TWR distance measurement exchange. This application sends a "poll" |
| | | * frame (recording the TX time-stamp of the poll), and then waits for a "response" message expected from the "DS TWR responder" example |
| | | * code (companion to this application). When the response is received its RX time-stamp is recorded and we send a "final" message to |
| | | * complete the exchange. The final message contains all the time-stamps recorded by this application, including the calculated/predicted TX |
| | | * time-stamp for the final message itself. The companion "DS TWR responder" example application works out the time-of-flight over-the-air |
| | | * and, thus, the estimated distance between the two devices. |
| | | * |
| | | * |
| | | * @attention |
| | | * |
| | |
| | | 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 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 tx_resp_msg[16] = {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 double tof; |
| | | |
| | | uint16_t anchor_dist_last_frm[TAG_NUM_IN_SYS]; |
| | | uint32_t anchor_dist_last_frm[TAG_NUM_IN_SYS]; |
| | | uint32_t tag_id = 0; |
| | | uint32_t tag_id_recv = 0; |
| | | uint8_t random_delay_tim = 0; |
| | |
| | | memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 4); |
| | | |
| | | } |
| | | uint16_t Checksum_u16(uint8_t* pdata, uint32_t len) |
| | | { |
| | | uint16_t sum = 0; |
| | | uint32_t i; |
| | | for(i=0; i<len; i++) |
| | | sum += pdata[i]; |
| | | sum = ~sum; |
| | | return sum; |
| | | } |
| | | |
| | | u16 tag_time_recv[TOTAL_TAG_NUM]; |
| | | u8 usart_send[25]; |
| | | u8 battary,button; |
| | | extern uint8_t g_pairstart; |
| | | void tag_sleep_configuraion(void) |
| | | { |
| | | dwt_configuresleep(0x940, 0x7); |
| | |
| | | uint16_t g_Resttimer; |
| | | uint8_t result; |
| | | u8 tag_succ_times=0; |
| | | u32 hex_dist; |
| | | u16 checksum; |
| | | void Tag_App(void)//å鿍¡å¼(TAGæ ç¾) |
| | | { |
| | | uint32_t frame_len; |
| | |
| | | u8 i; |
| | | |
| | | g_Resttimer=0; |
| | | UART_CheckReceive(); |
| | | GPIO_ResetBits(SPIx_GPIO, SPIx_CS); |
| | | delay_us(2500); |
| | | GPIO_SetBits(SPIx_GPIO, SPIx_CS); |
| | |
| | | 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(); |
| | | }; |
| | | |
| | | /* Increment frame sequence number after transmission of the poll message (modulo 256). */ |
| | | frame_seq_nb++; |
| | | if(status_reg==0xffffffff) |
| | | { |
| | | NVIC_SystemReset(); |
| | |
| | | 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[DIST_IDX], 2); |
| | | memcpy(&tx_final_msg[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 2); |
| | | memcpy(&anchor_dist_last_frm[tag_id], &rx_buffer[DIST_IDX], 4); |
| | | memcpy(&tx_final_msg[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 4); |
| | | /* 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 |
| | |
| | | dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//æ¸
餿 å¿ä½ |
| | | |
| | | /* Increment frame sequence number after transmission of the final message (modulo 256). */ |
| | | frame_seq_nb++; |
| | | |
| | | random_delay_tim = 0; |
| | | #ifdef TAG_OUTPUT |
| | | #ifdef HEX_OUTPUT |
| | | usart_send[2] = frame_seq_nb++; |
| | | //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],&tx_final_msg[ANCHOR_ID_IDX],4); |
| | | hex_dist = rx_buffer[DIST_IDX]; |
| | | 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[21],&checksum,2); |
| | | USART_puts(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 |
| | | #endif |
| | | } |
| | | else |
| | | { |
| | |
| | | /* Execute a delay between ranging exchanges. */ |
| | | dwt_entersleep(); |
| | | } |
| | | uint16_t Checksum_u16(uint8_t* pdata, uint32_t len) |
| | | { |
| | | uint16_t sum = 0; |
| | | uint32_t i; |
| | | for(i=0; i<len; i++) |
| | | sum += pdata[i]; |
| | | sum = ~sum; |
| | | return sum; |
| | | } |
| | | |
| | | u16 tag_time_recv[TOTAL_TAG_NUM]; |
| | | u8 usart_send[25]; |
| | | u8 battary,button; |
| | | extern uint8_t g_pairstart; |
| | | void Anchor_App(void) |
| | | { |
| | | uint32_t frame_len; |
| | |
| | | dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//æ¥æ¶è¶
æ¶æ¶é´ |
| | | |
| | | /* Write and send the response message. See NOTE 9 below.*/ |
| | | memcpy(&tx_resp_msg[DIST_IDX], &anchor_dist_last_frm[tag_id_recv], 2); |
| | | if(tag_id_recv-TAG_ID_START<=TOTAL_TAG_NUM) |
| | | memcpy(&tx_resp_msg[DIST_IDX], &anchor_dist_last_frm[tag_id_recv-TAG_ID_START], 4); |
| | | |
| | | dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//åå
¥åéæ°æ® |
| | | dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//设å®åéé¿åº¦ |
| | |
| | | { }; |
| | | } |
| | | /* Increment frame sequence number after transmission of the response message (modulo 256). */ |
| | | frame_seq_nb++; |
| | | |
| | | if (status_reg & SYS_STATUS_RXFCG)//æ¥æ¶æå |
| | | { |
| | |
| | | uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32; |
| | | double Ra, Rb, Da, Db; |
| | | int64_t tof_dtu; |
| | | u32 hex_dist; |
| | | u16 checksum; |
| | | |
| | | /* 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 |
| | |
| | | // printf("Pair Finish PairID: %d. \r\n",g_com_map[PAIR_ID]); |
| | | // } |
| | | // tag_time_recv[tag_id_recv] = tag_recv_timer; |
| | | g_flag_Taggetdist[tag_id_recv]=0; |
| | | g_flag_Taggetdist[tag_id_recv-TAG_ID_START]=0; |
| | | anchor_dist_last_frm[tag_id_recv-TAG_ID_START] = dist_cm; |
| | | #ifdef HEX_OUTPUT |
| | | usart_send[2] = frame_seq_nb; |
| | | usart_send[2] = frame_seq_nb++; |
| | | //usart_send[6] = tag_id_recv; |
| | | //usart_send[8] = g_com_map[DEV_ID]; |
| | | memcpy(&usart_send[3],&dev_id,4); |
| | | memcpy(&usart_send[3],&tag_id_recv,4); |
| | | memcpy(&usart_send[7],&dev_id,4); |
| | | hex_dist = dist_cm; |
| | | 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); |
| | | memcpy(&usart_send[21],&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 |
| | | |
| | | //dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm); |
| | | |
| | | } |
| | |
| | | } |
| | | } |
| | | |
| | | /***************************************************************************************************************************************************** |
| | | * NOTES: |
| | | * |
| | | * 1. The sum of the values is the TX to RX antenna delay, experimentally determined by a calibration process. Here we use a hard coded typical value |
| | | * but, in a real application, each device should have its own antenna delay properly calibrated to get the best possible precision when performing |
| | | * range measurements. |
| | | * 2. The messages here are similar to those used in the DecaRanging ARM application (shipped with EVK1000 kit). They comply with the IEEE |
| | | * 802.15.4 standard MAC data frame encoding and they are following the ISO/IEC:24730-62:2013 standard. The messages used are: |
| | | * - a poll message sent by the initiator to trigger the ranging exchange. |
| | | * - a response message sent by the responder allowing the initiator to go on with the process |
| | | * - a final message sent by the initiator to complete the exchange and provide all information needed by the responder to compute the |
| | | * time-of-flight (distance) estimate. |
| | | * The first 10 bytes of those frame are common and are composed of the following fields: |
| | | * - byte 0/1: frame control (0x8841 to indicate a data frame using 16-bit addressing). |
| | | * - byte 2: sequence number, incremented for each new frame. |
| | | * - byte 3/4: PAN TAG_ID (0xDECA). |
| | | * - byte 5/6: destination address, see NOTE 3 below. |
| | | * - byte 7/8: source address, see NOTE 3 below. |
| | | * - byte 9: function code (specific values to indicate which message it is in the ranging process). |
| | | * The remaining bytes are specific to each message as follows: |
| | | * Poll message: |
| | | * - no more data |
| | | * Response message: |
| | | * - byte 10: activity code (0x02 to tell the initiator to go on with the ranging exchange). |
| | | * - byte 11/12: activity parameter, not used here for activity code 0x02. |
| | | * Final message: |
| | | * - byte 10 -> 13: poll message transmission timestamp. |
| | | * - byte 14 -> 17: response message reception timestamp. |
| | | * - byte 18 -> 21: final message transmission timestamp. |
| | | * All messages end with a 2-byte checksum automatically set by DW1000. |
| | | * 3. Source and destination addresses are hard coded constants in this example to keep it simple but for a real product every device should have a |
| | | * unique TAG_ID. Here, 16-bit addressing is used to keep the messages as short as possible but, in an actual application, this should be done only |
| | | * after an exchange of specific messages used to define those short addresses for each device participating to the ranging exchange. |
| | | * 4. Delays between frames have been chosen here to ensure proper synchronisation of transmission and reception of the frames between the initiator |
| | | * and the responder and to ensure a correct accuracy of the computed distance. The user is referred to DecaRanging ARM Source Code Guide for more |
| | | * details about the timings involved in the ranging process. |
| | | * 5. This timeout is for complete reception of a frame, i.e. timeout duration must take into account the length of the expected frame. Here the value |
| | | * is arbitrary but chosen large enough to make sure that there is enough time to receive the complete response frame sent by the responder at the |
| | | * 110k data rate used (around 3 ms). |
| | | * 6. In a real application, for optimum performance within regulatory limits, it may be necessary to set TX pulse bandwidth and TX power, (using |
| | | * the dwt_configuretxrf API call) to per device calibrated values saved in the target system or the DW1000 OTP memory. |
| | | * 7. dwt_writetxdata() takes the full size of the message as a parameter but only copies (size - 2) bytes as the check-sum at the end of the frame is |
| | | * automatically appended by the DW1000. This means that our variable could be two bytes shorter without losing any data (but the sizeof would not |
| | | * work anymore then as we would still have to indicate the full length of the frame to dwt_writetxdata()). It is also to be noted that, when using |
| | | * delayed send, the time set for transmission must be far enough in the future so that the DW1000 IC has the time to process and start the |
| | | * transmission of the frame at the wanted time. If the transmission command is issued too late compared to when the frame is supposed to be sent, |
| | | * this is indicated by an error code returned by dwt_starttx() API call. Here it is not tested, as the values of the delays between frames have |
| | | * been carefully defined to avoid this situation. |
| | | * 8. We use polled mode of operation here to keep the example as simple as possible but all status events can be used to generate interrupts. Please |
| | | * refer to DW1000 User Manual for more details on "interrupts". It is also to be noted that STATUS register is 5 bytes long but, as the event we |
| | | * use are all in the first bytes of the register, we can use the simple dwt_read32bitreg() API call to access it instead of reading the whole 5 |
| | | * bytes. |
| | | * 9. As we want to send final TX timestamp in the final message, we have to compute it in advance instead of relying on the reading of DW1000 |
| | | * register. Timestamps and delayed transmission time are both expressed in device time units so we just have to add the desired response delay to |
| | | * response RX timestamp to get final transmission time. The delayed transmission time resolution is 512 device time units which means that the |
| | | * lower 9 bits of the obtained value must be zeroed. This also allows to encode the 40-bit value in a 32-bit words by shifting the all-zero lower |
| | | * 8 bits. |
| | | * 10. In this operation, the high order byte of each 40-bit timestamps is discarded. This is acceptable as those time-stamps are not separated by |
| | | * more than 2**32 device time units (which is around 67 ms) which means that the calculation of the round-trip delays (needed in the |
| | | * time-of-flight computation) can be handled by a 32-bit subtraction. |
| | | * 11. The user is referred to DecaRanging ARM application (distributed with EVK1000 product) for additional practical example of usage, and to the |
| | | * DW1000 API Guide for more details on the DW1000 driver functions. |
| | | ****************************************************************************************************************************************************/ |