|
|
请教下，使用官网的ds方式，增加标签后距离相差很大？
刚开始时使用的一个基站对应一个标签，误差大概在 30cm左右，
目前使用一个基站对多高标签(目前测试的是2个标签)，测得的距离貌似比实际距离多了10.5M左右，这个是怎么个情况啊 ？
标签程序如下：
/* 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. */
};
/* Default antenna delay values for 64 MHz PRF. See NOTE 1 below. */
#define TX_ANT_DLY 16436
#define RX_ANT_DLY 16436
/* As "TX then wait for a response" example sends a blink message encoded as per the ISO/IEC 24730-62:2013 standard which includes a bit signalling
* that a response is listened for, this example will respond with a valid frame (that will be ignored anyway) following the same standard. The
* response is a 21-byte frame composed of the following fields:
* - byte 0/1: frame control (0x8C41 to indicate a data frame using 16-bit source addressing and 64-bit destination addressing).
* - byte 2: sequence number, incremented for each new frame.
* - byte 3/4: application ID (0x609A for data frames in this standard).
* - byte 5 -> 12: 64-bit destination address.
* - byte 13/14: 16-bit source address, hard coded in this example to keep it simple.
* - byte 15: function code (0x10 to indicate this is an activity control message).
* - byte 16: activity code (0x00 to indicate activity is finished).
* - byte 17/18: new tag blink rate.
* - byte 19/20: frame check-sum, automatically set by DW1000.
*/
/// *作为“TX然后等待响应”示例发送按照ISO / IEC 24730-62：2013标准编码的闪烁消息，其包括位信令
//?*响应被监听，这个例子将按照相同的标准响应一个有效的帧（无论如何将被忽略）。该
//?*响应是由以下字段组成的21个字节的帧：
//?* - 字节0/1：帧控制（0x8C41表示使用16位源寻址和64位目标寻址的数据帧）。
//?* - 字节2： ID号，
//?* - 字节3/4：应用程序ID（本标准中的数据帧为0x609A）。
//?* - 字节5 - > 12：64位目标地址。
//?* - 字节13/14：16位源地址，在本例中用硬编码保持简单。
//?* - 字节15：功能码（0x10表示这是一个活动控制信息）。
//?* - 字节16：活动代码（0x00表示活动已完成）。
//?* - 字节17/18：新的标签闪烁频率。
//?* - 字节19/20：帧校验和，由DW1000自动设置。 * /
/* Frames used in the ranging process. See NOTE 2 below. */
static uint8 tx_poll_msg[] = {0x41, 0x88, TAG_USER_ID, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0}; // 倒数第3byte位标签编号，也就是TAG_USER_ID
static uint8 rx_resp_msg[] = {0x41, 0x88, TAG_USER_ID, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0};
static uint8 tx_final_msg[] = {0x41, 0x88, TAG_USER_ID, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
/* Length of the common part of the message (up to and including the function code, see NOTE 2 below).
消息的公共部分的长度（直到并包括功能代码，参见下面的注2）*/
#define ALL_MSG_COMMON_LEN 10
/* Index to access some of the fields in the frames involved in the process.
索引来访问过程中涉及的帧中的一些字段 */
#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
#define FINAL_MSG_TS_LEN 4
/* Frame sequence number, incremented after each transmission..帧序列号，在每次传输后递增 */
//static uint8 frame_seq_nb = 0;
/* Buffer to store received messages.
* Its size is adjusted to longest frame that this example code is supposed to handle.
缓冲区来存储收到的消息。 它的大小被调整到这个示例代码应该处理的最长帧 */
#define RX_BUF_LEN 20
static uint8 rx_buffer[RX_BUF_LEN];
/* Hold copy of status register state here for reference so that it can be examined at a debug breakpoint.
在此处保存状态寄存器状态的副本，以便在调试断点处进行检查*/
static uint32 status_reg = 0;
/* UWB microsecond (uus) to device time unit (dtu, around 15.65 ps) conversion factor.
* 1 uus = 512 / 499.2 祍 and 1 祍 = 499.2 * 128 dtu.
UWB微秒（uus）到器件时间单位（dtu，大约15.65 ps）转换因子 */
#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 3100
/* Receive response timeout. See NOTE 5 below. */
#define RESP_RX_TIMEOUT_UUS 2700
/* 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;
/* Declaration of static functions. */
static uint64_t get_tx_timestamp_u64(void);
static uint64_t get_rx_timestamp_u64(void);
static void final_msg_set_ts(uint8_t *ts_field, uint64_t ts);
//uint32_t BeginTime, EndTime;
uint8_t Tag_Number_ID = TAG_USER_ID; // tag 编号，每个 tag 都有自己的编号，固定不变
uint8_t Tag_Number_ID_bak = 0xff; // 赋初值
void bsp_distanceInit(void)
{
reset_DW1000();
// 开始初始化 dw1000 模块
spi_set_rate_low();
// dw1000 初始化完成后，吧spi设置为高
dwt_initialise(DWT_LOADUCODE);
spi_set_rate_high();
dwt_configure(&config);
/* 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. */
// dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);
// dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
}
void bsp_distanceCode(void)
{
/* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
// tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);
dwt_writetxfctrl(sizeof(tx_poll_msg), 0);
dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); //设定发送后打开接收的时间
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); //设定接收超时的时间
/* Start transmission, indicating that a response is expected so that reception is enabled automatically after the frame is sent and the delay
* set by dwt_setrxaftertxdelay() has elapsed. */
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); //立即发送，并在一段时间后打开接收
/* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ }
/* Increment frame sequence number after transmission of the poll message (modulo 256). */
// frame_seq_nb++;
if (status_reg & SYS_STATUS_RXFCG)
{
uint32 frame_len;
/* 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;
if (frame_len <= RX_BUF_LEN)
{
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. */
// rx_buffer[ALL_MSG_SN_IDX] = 0;
// Tag_Number_ID_bak = rx_buffer[ALL_MSG_SN_IDX];
if(rx_buffer[ALL_MSG_SN_IDX] == TAG_USER_ID) // id 号等于本机id继续发送响应信息，否则退出重新从头开始发送poll 包
{
if (memcmp(rx_buffer, rx_resp_msg, ALL_MSG_COMMON_LEN) == 0)
{
uint32 final_tx_time;
/* Retrieve poll transmission and response reception timestamp. */
poll_tx_ts = get_tx_timestamp_u64();
resp_rx_ts = get_rx_timestamp_u64();
/* 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;
dwt_setdelayedtrxtime(final_tx_time);
/* 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;
/* Write all timestamps in the final message. See NOTE 10 below. 在最后的留言写所有的时间戳。见下文注10 */
final_msg_set_ts(&tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);
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. */
tx_final_msg[ALL_MSG_SN_IDX] = TAG_USER_ID; //frame_seq_nb;
dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);
dwt_writetxfctrl(sizeof(tx_final_msg), 0);
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))
{ };
/* Clear TXFRS event. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
/* Increment frame sequence number after transmission of the final message (modulo 256). */
// frame_seq_nb++;
}
}
else{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
else
{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
// ==============================================================================================
// ==============================================================================================
基站程序：
/* 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. */
};
/* Default antenna delay values for 64 MHz PRF. See NOTE 1 below. */
#define TX_ANT_DLY 16436
#define RX_ANT_DLY 16436
/* As "TX then wait for a response" example sends a blink message encoded as per the ISO/IEC 24730-62:2013 standard which includes a bit signalling
* that a response is listened for, this example will respond with a valid frame (that will be ignored anyway) following the same standard. The
* response is a 21-byte frame composed of the following fields:
* - byte 0/1: frame control (0x8C41 to indicate a data frame using 16-bit source addressing and 64-bit destination addressing).
* - byte 2: sequence number, incremented for each new frame.
* - byte 3/4: application ID (0x609A for data frames in this standard).
* - byte 5 -> 12: 64-bit destination address.
* - byte 13/14: 16-bit source address, hard coded in this example to keep it simple.
* - byte 15: function code (0x10 to indicate this is an activity control message).
* - byte 16: activity code (0x00 to indicate activity is finished).
* - byte 17/18: new tag blink rate.
* - byte 19/20: frame check-sum, automatically set by DW1000.
*/
/// *作为“TX然后等待响应”示例发送按照ISO / IEC 24730-62：2013标准编码的闪烁消息，其包括位信令
//?*响应被监听，这个例子将按照相同的标准响应一个有效的帧（无论如何将被忽略）。该
//?*响应是由以下字段组成的21个字节的帧：
//?* - 字节0/1：帧控制（0x8C41表示使用16位源寻址和64位目标寻址的数据帧）。
//?* - 字节2： ID号，
//?* - 字节3/4：应用程序ID（本标准中的数据帧为0x609A）。
//?* - 字节5 - > 12：64位目标地址。
//?* - 字节13/14：16位源地址，在本例中用硬编码保持简单。
//?* - 字节15：功能码（0x10表示这是一个活动控制信息）。
//?* - 字节16：活动代码（0x00表示活动已完成）。
//?* - 字节17/18：新的标签闪烁频率。
//?* - 字节19/20：帧校验和，由DW1000自动设置。 * /
/* Frames used in the ranging process. See NOTE 2 below. */
static uint8 rx_poll_msg[] = {0x41, 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};
/* Length of the common part of the message (up to and including the function code, see NOTE 2 below).
消息的公共部分的长度（直到并包括功能代码，参见下面的注2）*/
#define ALL_MSG_COMMON_LEN 10
/* Index to access some of the fields in the frames involved in the process.
索引来访问过程中涉及的帧中的一些字段 */
#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
#define FINAL_MSG_TS_LEN 4
/* Frame sequence number, incremented after each transmission..帧序列号，在每次传输后递增 */
static uint8 frame_seq_nb = 0;
/* Buffer to store received messages.
* 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];
/* Hold copy of status register state here for reference so that it can be examined at a debug breakpoint.
在此处保存状态寄存器状态的副本，以便在调试断点处进行检查*/
static uint32 status_reg = 0;
/* UWB microsecond (uus) to device time unit (dtu, around 15.65 ps) conversion factor.
* 1 uus = 512 / 499.2 祍 and 1 祍 = 499.2 * 128 dtu.
UWB微秒（uus）到器件时间单位（dtu，大约15.65 ps）转换因子 */
#define UUS_TO_DWT_TIME 65536
/* Delay between frames, in UWB microseconds. See NOTE 4 below.帧之间的延迟，以UWB微秒为单位 */
/* 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.46 ms with above configuration.
这是从帧RX时间戳到用于计算/设置DW1000的延迟TX功能的TX应答时间戳的延迟。 这包括以上配置的帧长约2.46毫秒*/
#define POLL_RX_TO_RESP_TX_DLY_UUS 2600 // 2600, 如果接收不正确，可以更改此设置
/* 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.
这是从帧传输结束到接收器启用的延迟，如为DW1000等待响应功能编程的那样*/
#define RESP_TX_TO_FINAL_RX_DLY_UUS 500
/* Receive final timeout. See NOTE 5 below.接收最后的超时 */
#define FINAL_RX_TIMEOUT_UUS 3300
/* Preamble timeout, in multiple of PAC size. See NOTE 6 below. 序言超时，以PAC大小的倍数*/
#define PRE_TIMEOUT 8
static uint64 poll_rx_ts;
static uint64 resp_tx_ts;
static uint64 final_rx_ts;
/* Speed of light in air, in metres per second. */
#define SPEED_OF_LIGHT 299702547
/* Hold copies of computed time of flight and distance here for reference so that it can be examined at a debug breakpoint.
保存计算出的飞行时间和距离的副本以供参考，以便在调试断点处进行检查*/
static double tof;
static double distance;
static uint64 get_tx_timestamp_u64(void);
static uint64 get_rx_timestamp_u64(void);
static void final_msg_get_ts(const uint8 *ts_field, uint32 *ts);
uint8_t Tag_Number_ID_bak = 0xff; // 赋初值,id 号备份
int main(void)
{
RCC_Configuration();
RCC_GetClocksFreq(&_RCC_Clocks);
SysTick_Configuration();
LED_GPIO_Init();
LCD_ConfigInit();
LCD_Clear();
Sleep(500);
memset(dataseq, 0x0, sizeof(dataseq));
memcpy(dataseq, (const uint8 *) "DECAWAVE Start", 16);
writetoLCD( 40, 1, dataseq);
memcpy(dataseq, (const uint8 *) SOFTWARE_VER_STRING, 16);
writetoLCD( 16, 1, dataseq);
LED_WorkTest();
peripherals_init ();
reset_DW1000(); // Target specific drive of RSTn line into DW1000 low for a period.
spi_set_rate_low();
dwt_initialise(DWT_LOADUCODE);
spi_set_rate_high();
/* Configure GPIOs to show TX/RX activity. See NOTE 6 below. 配置GPIO以显示TX / RX活动*/
dwt_setlnapamode(1, 1);
/* Configure LEDs management. See NOTE 6 below.配置LED管理 */
dwt_setleds(DWT_LEDS_ENABLE);
dwt_configure(&config);
/* Apply default antenna delay value. See NOTE 1 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
/* Set preamble timeout for expected frames. See NOTE 6 below.设置预期帧的前导码超时 */
// dwt_setpreambledetecttimeout(PRE_TIMEOUT); // 这句可以不使用
while(1)
{
/* Clear reception timeout to start next ranging process. 清除接收超时，开始下一个测距过程*/
dwt_setrxtimeout(0);
/* Activate reception immediately. See NOTE 4 below.立即激活接收 */
dwt_rxenable(DWT_START_RX_IMMEDIATE);
/* 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))) //不断查询寄存器直到接收成功
{ };
if (status_reg & SYS_STATUS_RXFCG) // 接收成功
{
uint32 frame_len;
/* 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;
if (frame_len <= RX_BUFFER_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0); //读取第1次接收到的数据
}
/* 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_Number_ID_bak = rx_buffer[ALL_MSG_SN_IDX]; //id 号备份
rx_buffer[ALL_MSG_SN_IDX] = 0; // id 号清0，对比数据时，id 号不用对比
if (memcmp(rx_buffer, rx_poll_msg, ALL_MSG_COMMON_LEN) == 0)
{
uint32 resp_tx_time;
int ret;
/* Retrieve poll reception timestamp. 获得收到数据的时间T2 */
poll_rx_ts = get_rx_timestamp_u64();
/* Set send time for response. See NOTE 8 below.
计算response发送时间 T3
*/
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); //设定发送时间 T3
/* Set expected delay and timeout for final message reception. See NOTE 4 and 5 below.
为最终信息接收设置预期延迟和超时。*/
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.
编写并发送响应消息*/
tx_resp_msg[ALL_MSG_SN_IDX] = Tag_Number_ID_bak; // id 号返回
dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);
dwt_writetxfctrl(sizeof(tx_resp_msg), 0);
// dwt_writetxfctrl_sws(sizeof(tx_resp_msg), 0, 1);
ret = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED); //设定延时发送response包，之后打开接收
/* If dwt_starttx() returns an error, abandon this ranging exchange and proceed to the next one. See NOTE 11 below.
如果dwt_starttx（）返回错误，则放弃这个测距交换并继续下一个*/
if (ret == DWT_ERROR)
{
continue;
}
/* We assume that the transmission is achieved correctly, now poll for reception of expected "final" frame or error/timeout.
* See NOTE 7 below.
设备B（rx/基站） 收到 Final 包，记录时间 T6
*/
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//SYS_STATUS_ALL_RX_TO |
{ };
/* Increment frame sequence number after transmission of the response message (modulo 256). */
frame_seq_nb++;
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;
if (frame_len <= RX_BUF_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0); //读取第2次接收到的数据
}
/* 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(Tag_Number_ID_bak == rx_buffer[ALL_MSG_SN_IDX]) //对比接收到的id与第一次接收到的id号是否一致 ？
{
rx_buffer[ALL_MSG_SN_IDX] = 0;
if (memcmp(rx_buffer, rx_final_msg, ALL_MSG_COMMON_LEN) == 0)
{
uint32 poll_tx_ts, resp_rx_ts, final_tx_ts;
uint32 poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
double Ra, Rb, Da, Db;
int64 tof_dtu;
/* Retrieve response transmission and final reception timestamps. */
resp_tx_ts = get_tx_timestamp_u64();
final_rx_ts = get_rx_timestamp_u64();
/* Get timestamps embedded in the final message. 嵌入在最后消息的时间戳 */
final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);
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);
/* 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;
resp_tx_ts_32 = (uint32)resp_tx_ts;
final_rx_ts_32 = (uint32)final_rx_ts;
Ra = (double)(resp_rx_ts - poll_tx_ts); // Ra = T4 - T1
Rb = (double)(final_rx_ts_32 - resp_tx_ts_32); // Rb = T6 - T3
Da = (double)(final_tx_ts - resp_rx_ts); // Da = T5 - T4
Db = (double)(resp_tx_ts_32 - poll_rx_ts_32); // Db = T3 - T2
tof_dtu = (int64)(((Ra * Rb) - (Da * Db)) / (Ra + Rb + Da + Db)); //计算公式
tof = tof_dtu * DWT_TIME_UNITS;
distance = tof * SPEED_OF_LIGHT; //得到的结果单位是 米
// distance = tof * SPEED_OF_LIGHT * 100; //得到的结果单位是 cm
distance /= 2;
/* Display computed distance on LCD. */
LCD_SetCursor();
memset(dataseq, 0x0, 16);
writetoLCD( 40, 1, (const uint8 *)" dec: ");
sprintf((char *)dataseq, "NO.%02d %3.3fm",
Tag_Number_ID_bak, //显示id号
distance); // 距离数据转换为字符
writetoLCD( 16, 1, dataseq);
// LCD_PutStr(10, 10, "DIST : ", WHITE, BLACK);
// LCD_PutNum(10 + 8 * 7, 10, Type_D, 5, distance * 100, WHITE, BLACK);
// LCD_PutStr(10 + 8 * 13, 10, "mm", WHITE, BLACK);
// sprintf(dist_str, "DIST: %3.2f m", distance);
// lcd_display_str(dist_str);
}
}
else // 如果第2次接收的id号与第一次的不一样，清除所以接收标志，重新开始接收
{
// Clear RX error events in the DW1000 status register.
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
/* Reset RX to properly reinitialise LDE operation. */
dwt_rxreset();
}
}
else
{
// Clear RX error events in the DW1000 status register.
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
/* Reset RX to properly reinitialise LDE operation. */
dwt_rxreset();
}
}
}
else
{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
/* Reset RX to properly reinitialise LDE operation. */
dwt_rxreset();
}
// Sleep(1);
}
} |
|
发表于 2017-11-30 21:57:04
发表于 2017-12-1 02:37:49
楼主
