From 41776246aed24b42e1d05da230f6f114e02c87b6 Mon Sep 17 00:00:00 2001 From: guanjiao <sqrgj@163.com> Date: 星期日, 27 五月 2018 17:39:52 +0800 Subject: [PATCH] 增加低通滤波与渐进报警功能 --- 源码/核心板/Src/application/dw_app.c | 111 +++++++++++++++++-------------------------------------- 1 files changed, 35 insertions(+), 76 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..822a810 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" @@ -89,37 +89,35 @@ }; /* 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; @@ -143,10 +141,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 +165,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 +190,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; @@ -236,8 +234,8 @@ void Tag_App(void)//发送模式(TAG标签) { - uint32 frame_len; - uint32 final_tx_time; + uint32_t frame_len; + uint32_t final_tx_time; /* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */ tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb; @@ -282,7 +280,7 @@ 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写入发送数据 @@ -294,26 +292,6 @@ 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); -// } /* Poll DW1000 until TX frame sent event set. See NOTE 8 below. */ while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成 @@ -344,8 +322,8 @@ 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位没有超时时间 @@ -421,8 +399,8 @@ rx_buffer[ALL_MSG_SN_IDX] = 0; if (rx_buffer[9] == 0x23) //判断是否为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 +414,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;//距离=光速*飞行时间 @@ -453,25 +431,6 @@ // 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); -// } } } -- Gitblit v1.9.3