From e5f73b1e954eb54e0af326b3a5aee5fab719f0ce Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期六, 08 六月 2019 10:06:45 +0800
Subject: [PATCH] 6月7号

---
 源码/核心板/Src/application/dw_app.h         |   10 ++
 源码/核心板/Src/application/global_param.c   |    4 
 源码/核心板/Src/main.c                       |    6 +
 源码/核心板/Src/application/beep_logic_app.c |    3 
 源码/核心板/Src/ExternalDevices/led.h        |    8 ++
 源码/核心板/Src/stm32f10x_it.c               |    7 +
 源码/核心板/Src/ExternalDevices/led.c        |    5 -
 源码/核心板/Src/application/dw_app.c         |   99 +++++++++++++++++++++++++-------
 8 files changed, 106 insertions(+), 36 deletions(-)

diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/ExternalDevices/led.c" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/ExternalDevices/led.c"
index 29db200..457e7ab 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/ExternalDevices/led.c"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/ExternalDevices/led.c"
@@ -6,9 +6,8 @@
 		
 	/* Enable GPIO clock */
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
-
-    // Enable GPIO used for LED flash
-    GPIO_InitStructure.GPIO_Pin = LED0_PIN;
+	
+	  GPIO_InitStructure.GPIO_Pin = LED0_PIN;
     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
     GPIO_Init(LED0_GPIO, &GPIO_InitStructure);
diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/ExternalDevices/led.h" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/ExternalDevices/led.h"
index 317522c..73c0abe 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/ExternalDevices/led.h"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/ExternalDevices/led.h"
@@ -3,9 +3,17 @@
 #define __LED_H__
 
 #include "stm32f10x.h"
+#include "dw_app.h"
 
+#define NEWBOARD
+
+#if defined(NEWBOARD)  && defined(WORK_MODE_TAG)
+#define LED0_PIN					GPIO_Pin_12
+#define LED0_GPIO					GPIOB
+#else
 #define LED0_PIN					GPIO_Pin_2
 #define LED0_GPIO					GPIOA
+#endif
 #define LED_BLINK(...)				GPIO_Toggle(__VA_ARGS__)
 #define LED0_BLINK					LED_BLINK(LED0_GPIO, LED0_PIN)
 #define LED0_OFF						GPIO_WriteBit(LED0_GPIO, LED0_PIN, Bit_RESET)
diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/beep_logic_app.c" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/beep_logic_app.c"
index 80fc0c0..74c8075 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/beep_logic_app.c"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/beep_logic_app.c"
@@ -37,7 +37,7 @@
 			beep_ontime = ONTIME1;
 			beep_offtime = OFFTIME1;
 			beep_state=1;
-			LED0_ON;
+		
 		}
 //		else if (min_dist <= g_com_map[ALARM_DISTANCE2])
 //		{
@@ -51,7 +51,6 @@
 //			beep_state=1;
 //		}
 		else{
-			LED0_OFF;
 			beep_state=0;
 		}
 		
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 1fa5677..ffb2bc2 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"
@@ -75,6 +75,7 @@
 #define TAG_ID_IDX    				3
 #define MESSAGE_TYPE_IDX 			5
 #define DIST_IDX 							6
+#define ANC_TYPE_IDX 					7
 
 #define POLL     					0x01
 #define RESPONSE 					0x02
@@ -280,32 +281,46 @@
 }
 uint16_t g_Resttimer;
 uint8_t result;
+u8 tag_succ_times=0;
 void Tag_App(void)//发送模式(TAG标签)
 {
 	uint32_t frame_len;
 	uint32_t final_tx_time;
+	u32 start_poll;
+	u8 i;
 	
 	g_Resttimer=0;
 	UART_CheckReceive();
 	GPIO_ResetBits(SPIx_GPIO, SPIx_CS);
 	delay_us(2500);
 	GPIO_SetBits(SPIx_GPIO, SPIx_CS);
-	
+	tag_succ_times = 0;
+	for(i=0;i<REPOET_ANC_NUM;i++)
+	{
 	/* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
 	tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
+	tx_poll_msg[ANC_TYPE_IDX] = i;
+		
 	dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);//将Poll包数据传给DW1000,将在开启发送时传出去
 	dwt_writetxfctrl(sizeof(tx_poll_msg), 0);//设置超宽带发送数据长度
 
 	/* 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);//开启发送,发送完成后等待一段时间开启接收,等待时间在dwt_setrxaftertxdelay中设置
-
+	start_poll = time32_incr;
 	/* 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)))//不断查询芯片状态直到成功接收或者发生错误
-	{ };
+	{ if(time32_incr - start_poll>20)
+		NVIC_SystemReset();
+	
+	};
 
 	/* Increment frame sequence number after transmission of the poll message (modulo 256). */
 	frame_seq_nb++;
+	if(status_reg==0xffffffff)
+	{
+		NVIC_SystemReset();
+	}
 
 	if (status_reg & SYS_STATUS_RXFCG)//如果成功接收
 	{
@@ -321,7 +336,7 @@
 		/* 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[MESSAGE_TYPE_IDX] == RESPONSE) //判断接收到的数据是否是response数据
+		if (rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&rx_buffer[TAG_ID_IDX]==g_com_map[DEV_ID]) //判断接收到的数据是否是response数据
 		{
 			/* Retrieve poll transmission and response reception timestamp. */
 			poll_tx_ts = get_tx_timestamp_u64();										//获得POLL发送时间T1
@@ -347,6 +362,7 @@
 			dwt_writetxfctrl(sizeof(tx_final_msg), 0);//设定发送数据长度
 			result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送
 			
+			tag_succ_times++;
 			
 			/* Poll DW1000 until TX frame sent event set. See NOTE 8 below. */
 			if(result==0)
@@ -357,7 +373,7 @@
 			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//清除标志位
 
 			/* Increment frame sequence number after transmission of the final message (modulo 256). */
-			frame_seq_nb++;
+			frame_seq_nb++;		
 			random_delay_tim = 0;
 		}
 		else
@@ -371,11 +387,32 @@
 		dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
 		random_delay_tim = DFT_RAND_DLY_TIM_MS;
 	}
+//	deca_sleep(10);
+}
+	if(tag_succ_times!=REPOET_ANC_NUM)
+	{
+	random_delay_tim =g_com_map[DEV_ID]*13+7;
+	}else{
+		random_delay_tim=0;
+	}
 	LED0_BLINK;
+	deca_sleep(random_delay_tim);
+	RTC_SET_ALARM(1);
 	/* 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[16];
 extern uint8_t g_pairstart;
 void Anchor_App(void)
 {
@@ -392,11 +429,12 @@
 	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)//成功接收
-	{
+	{ u16 tag_recv_interval;
 		/* Clear good RX frame event in the DW1000 status register. */
 		dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//清除标志位
 
@@ -413,9 +451,13 @@
 		//将收到的tag_id分别写入各次通讯的包中,为多标签通讯服务,防止一次通讯中接收到不同ID标签的数据
 		tag_id_recv = rx_buffer[TAG_ID_IDX];
 		tx_resp_msg[TAG_ID_IDX] = tag_id_recv;
-
+		if(tag_recv_timer>tag_time_recv[tag_id_recv])
+		{	tag_recv_interval =  tag_recv_timer - tag_time_recv[tag_id_recv];
+		}else{
+			tag_recv_interval = tag_recv_timer + 65535 - tag_time_recv[tag_id_recv];
+		}
 		
-		if (rx_buffer[MESSAGE_TYPE_IDX] == POLL&&tag_id_recv!= g_com_map[PAIR_ID]) //判断是否是poll包数据
+		if (rx_buffer[MESSAGE_TYPE_IDX] == POLL&&tag_id_recv!= g_com_map[PAIR_ID]&&(tag_recv_interval>g_com_map[COM_INTERVAL]/2)&&(g_com_map[DEV_ID]%REPOET_ANC_NUM == rx_buffer[ANC_TYPE_IDX])) //判断是否是poll包数据
 		{
 			/* Retrieve poll reception timestamp. */
 			poll_rx_ts = get_rx_timestamp_u64();//获得Poll包接收时间T2
@@ -465,7 +507,8 @@
 					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
@@ -493,27 +536,37 @@
 //					dist[TAG_ID] = LP(dis, TAG_ID); //LP 为低通滤波器,让数据更稳定
 					
 					/*--------------------------以下为非测距逻辑------------------------*/
-			//		LED0_BLINK; //每成功一次通讯则闪烁一次
+					LED0_BLINK; //每成功一次通讯则闪烁一次
 					g_UWB_com_interval = 0;
 					dis_after_filter=dist_cm;
 					g_Tagdist[tag_id_recv]=dist_cm;
-					if(g_pairstart==1&&dist_cm<20)
-					{
-						g_pairstart=0;
-						g_com_map[PAIR_ID]=tag_id_recv;
-						save_com_map_to_flash();
-						BEEP2_ON;
-						delay_ms(1000);
-						printf("Pair Finish PairID: %d. \r\n",g_com_map[PAIR_ID]);
-					}
+//					if(g_pairstart==1&&dist_cm<20)
+//					{
+//						g_pairstart=0;
+//						g_com_map[PAIR_ID]=tag_id_recv;
+//						save_com_map_to_flash();
+//						BEEP2_ON;
+//						delay_ms(1000);
+//						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;
+					#ifdef HEX_OUTPUT
+					usart_send[2] = frame_seq_nb;
+					usart_send[6] = g_com_map[DEV_ID];
+					usart_send[8] = tag_id_recv;
+					hex_dist = dist_cm;
+					memcpy(&usart_send[10],&hex_dist,4);
+					checksum = Checksum_u16(&usart_send[2],12);
+					memcpy(&usart_send[14],&checksum,2);
+					UART_PushFrame(usart_send,16);
+					#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);
 
 				}
-			}
-			else
-			{
+			}else{
 				/* Clear RX error events in the DW1000 status register. */
 				dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
 			}
diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.h" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.h"
index d7add46..2183236 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.h"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.h"
@@ -4,16 +4,22 @@
 
 #include "stm32f10x.h"
 #include "filters.h"
-//#define WORK_MODE_TAG
-#define WORK_MODE_ANCHOR
+#define WORK_MODE_TAG
+//#define WORK_MODE_ANCHOR
+#define HEX_OUTPUT
+#define REPOET_ANC_NUM 4
+#define TOTAL_TAG_NUM  200
 
 #define TAG_NUM_IN_SYS				256
 #define DFT_RAND_DLY_TIM_MS			1
 
+extern volatile uint32_t time32_incr;
+extern u16 tag_recv_timer;
 extern double dist_cm;
 extern uint32_t g_UWB_com_interval;
 extern float dis_after_filter;
 extern LPFilter_Frac* p_Dis_Filter;		//测距用的低通滤波器
+extern u8 usart_send[16];
 
 void Dw1000_Init(void);
 void Tag_App(void);
diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/global_param.c" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/global_param.c"
index 870ee88..1d194b0 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/global_param.c"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/global_param.c"
@@ -29,8 +29,8 @@
 		g_com_map[ALARM_DISTANCE2] = 600;	//默认报警距离50cm
 		g_com_map[ALARM_DISTANCE3] = 900;	//默认报警距离50cm
 		g_com_map[ALARM_DEV] = 1;
-		g_com_map[DEV_ID] =10;//DEFAULT_DEV_ID;	//默认设备ID
-		g_com_map[COM_INTERVAL]=500;
+		g_com_map[DEV_ID] =4;//DEFAULT_DEV_ID;	//默认设备ID
+		g_com_map[COM_INTERVAL]=1000;
 		save_com_map_to_flash();
 	}
 
diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/main.c" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/main.c"
index 640bd3d..b47d1c0 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/main.c"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/main.c"
@@ -65,7 +65,7 @@
 	Device_Init();
 	Program_Init();
 	Dw1000_Init();
-  delay_ms(1000);	
+  delay_ms(10);	
 #ifdef WORK_MODE_TAG
 	RTC_Configuration(g_com_map[COM_INTERVAL]*20);
 	tag_sleep_configuraion();
@@ -73,6 +73,8 @@
 	Dw1000_App_Init();
     /* Loop forever initiating ranging exchanges. */
 	RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE);
+	usart_send[0]=0x55;
+	usart_send[1]=0xAA;
 	while(1)
 	{
 #ifdef WORK_MODE_TAG
@@ -82,7 +84,7 @@
 		Tag_App();
 	}
 //	UART_CheckReceive();
-//	PWR_EnterSTOPMode(PWR_Regulator_LowPower, PWR_STOPEntry_WFI);	
+	PWR_EnterSTOPMode(PWR_Regulator_LowPower, PWR_STOPEntry_WFI);	
 #else 
 		Anchor_App();
 #endif
diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/stm32f10x_it.c" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/stm32f10x_it.c"
index c7bee66..92c2c5b 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/stm32f10x_it.c"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/stm32f10x_it.c"
@@ -33,13 +33,15 @@
 uint8_t tt=0;
 uint8_t g_start_send_flag = 0;
 uint16_t sysscal;
+u16 tag_recv_timer;
 extern uint16_t g_Resttimer;
 extern uint8_t g_pairstart;
 void SysTick_Handler(void)
 {uint16_t i;
   time32_incr++;
 	g_UWB_com_interval++;
-	if(g_Resttimer++>10000)
+	
+	if(g_Resttimer++>2000)
 	{
 		NVIC_SystemReset();
 	}
@@ -48,6 +50,7 @@
 //		dis_after_filter = DEFAULT_DISTANCE;
 //	}
 #ifdef WORK_MODE_ANCHOR
+	tag_recv_timer++;
 	TagDistClear();
 	if(g_pairstart!=1)
 		main_logic();
@@ -75,7 +78,7 @@
 	EXTI_ClearITPendingBit(EXTI_Line17);
 	RTC_ClearITPendingBit(RTC_FLAG_ALR);
 	g_start_send_flag = 1;
-	RTC_SET_ALARM(1);
+	
 	tt++;
   //SYS.wake_id|=1<<17;
 }

--
Gitblit v1.9.3