From df3db0b978b1cab3555d6878a274b0f382bf707c Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期一, 01 十月 2018 15:42:49 +0800
Subject: [PATCH] 标签开机死机bug改好，基站增加软件超时复位
---
源码/核心板/Src/application/dw_app.c | 252 +++++++++++++++++++++++++++----------------------
1 files changed, 138 insertions(+), 114 deletions(-)
diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c"
index 91bdfa4..2499fa3 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c"
@@ -26,6 +26,11 @@
#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"
+
/*------------------------------------ Marcos ------------------------------------------*/
@@ -45,13 +50,13 @@
#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
@@ -65,8 +70,10 @@
#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
@@ -74,52 +81,49 @@
/*------------------------------------ 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;
@@ -130,6 +134,12 @@
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 ------------------------------------------*/
@@ -143,10 +153,10 @@
*
* @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--)
@@ -167,10 +177,10 @@
*
* @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--)
@@ -192,17 +202,17 @@
*
* @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;
@@ -211,7 +221,23 @@
*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.
@@ -223,7 +249,9 @@
/* 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); //设置发射天线延迟
@@ -233,11 +261,31 @@
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]=0x03;
+ 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;
@@ -269,20 +317,20 @@
/* 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写入发送数据
@@ -293,32 +341,14 @@
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);//清除标志位
@@ -339,13 +369,14 @@
}
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位没有超时时间
@@ -355,7 +386,11 @@
/* 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)//成功接收
{
@@ -373,11 +408,11 @@
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
@@ -391,17 +426,19 @@
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++;
@@ -419,10 +456,10 @@
/* 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;
@@ -436,14 +473,14 @@
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;//距离=光速*飞行时间
@@ -452,26 +489,13 @@
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;
+ //dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm);
}
}
--
Gitblit v1.9.3