UWB_SmallModule_F103.git

			@@ -28,7 +28,7 @@
			#include <stdio.h>
			#include "beep.h"
			#include "modbus.h"

			//#define DEBUG_OUTPUT
			/------------------------------------ Marcos ------------------------------------------/
			/* Inter-ranging delay period, in milliseconds. */
			#define RNG_DELAY_MS 100
			@@ -65,20 +65,22 @@
			#define FINAL_MSG_TS_LEN 4

			#define SYNC_SEQ_IDX 5

			//common
			#define GROUP_ID_IDX 0
			#define ANCHOR_ID_IDX 1
			#define TAG_ID_IDX 5
			#define MESSAGE_TYPE_IDX 9
			#define DIST_IDX 10

			//Poll
			#define ANC_TYPE_IDX 14
			#define BATTARY_IDX 15
			#define BUTTON_IDX 16
			#define SEQUENCE_IDX 17
			//respose
			#define DIST_IDX 10
			#define ANCTIMEMS 14
			#define ANCTIMEUS 16
			#define ANCSEND_INTERVAL 18

			#define POLL 0x01
			#define RESPONSE 0x02
			@@ -104,10 +106,10 @@
			static uint8_t tx_poll_msg[20] = {0};
			static uint8_t tx_sync_msg[14] = {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_t tx_final_msg[24] = {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[20] = {0};
			static uint8_t tx_resp_msg[22] = {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. */
			@@ -235,12 +237,14 @@
			if(clear_judge_cnt++>1000) //è®¾å®1Såé¢ï¼æ¯ç§è¿ä¸æ¬¡ãå¤ææ å¿ä½å¤§äºçäº2ï¼2sæ²¡æ¶å°æ°æ®å°±ææ°æ®åæ0xffffï¼ä¸è§¦åè¦æ¥ã
			{
			clear_judge_cnt=0;
			for(i=0;i<255;i++)
			for(i=0;i<100;i++)
			{
			g_flag_Taggetdist[i]++;
			if(g_flag_Taggetdist[i]>=20)
			if(g_flag_Taggetdist[i]>=2)
			{
			g_Tagdist[i]=0xffff;
			Modbus_HoldReg[i*2]=1;
			Modbus_HoldReg[i*2+1]=0xffff;
			}
			}
			}
			@@ -275,10 +279,16 @@
			tx_final_msg[MESSAGE_TYPE_IDX]=FINAL;
			tx_sync_msg[MESSAGE_TYPE_IDX]=SYNC;

			memcpy(&tx_poll_msg[GROUP_ID_IDX], &group_id, 1);
			memcpy(&tx_final_msg[GROUP_ID_IDX], &group_id, 1);
			memcpy(&tx_resp_msg[GROUP_ID_IDX], &group_id, 1);

			memcpy(&tx_poll_msg[TAG_ID_IDX], &dev_id, 4);
			memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 4);
			memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 4);
			memcpy(&tx_sync_msg[ANCHOR_ID_IDX], &dev_id, 4);

			memcpy(&tx_resp_msg[ANCSEND_INTERVAL], &g_com_map[COM_INTERVAL], 2);
			}
			uint16_t Checksum_u16(uint8_t* pdata, uint32_t len)
			{
			@@ -318,19 +328,24 @@
			int8_t tag_delaytime;
			extern uint16_t sync_timer;
			u16 tmp_time;
			extern float dw_vbat;
			extern u16 slottime,max_slotnum,current_slotpos,tyncpoll_time;
			void Tag_App(void)//åéæ¨¡å¼(TAGæ ç¾)
			{
			uint32_t frame_len;
			uint32_t final_tx_time;
			u32 start_poll;
			u8 i,getsync_flag=0;
			u8 bat_percent;
			//LED0_ON;
			dwt_forcetrxoff();
			g_Resttimer=0;
			dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); //è®¾ç½®åéåå¼å¯æ¥æ¶ï¼å¹¶è®¾å®å»¶è¿æ¶é´
			dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
			tag_succ_times = 0;
			tx_poll_msg[BATTARY_IDX] = Get_Battary();
			bat_percent=(dw_vbat-2.8)/0.5*100;
			if(bat_percent>100)
			bat_percent=100;
			tx_poll_msg[BATTARY_IDX] = bat_percent;//Get_Battary();
			tx_poll_msg[BUTTON_IDX] = !READ_KEY0;
			tx_poll_msg[SEQUENCE_IDX] = frame_seq_nb++;
			GPIO_WriteBit(GPIOA, GPIO_Pin_9, Bit_RESET);
			@@ -346,6 +361,9 @@
			* set by dwt_setrxaftertxdelay() has elapsed. */
			dwt_starttx(DWT_START_TX_IMMEDIATE \| DWT_RESPONSE_EXPECTED);//å¼å¯åéï¼åéå®æåçå¾ä¸æ®µæ¶é´å¼å¯æ¥æ¶ï¼çå¾æ¶é´å¨dwt_setrxaftertxdelayä¸è®¾ç½®
			start_poll = time32_incr;
			#ifdef DEBUG_OUTPUT
			printf("Pååé,åºç«ID: %d .\r\n",i);
			#endif
			/* 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)
			@@ -374,8 +392,8 @@
			/* 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. */

			if (rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //å¤ææ¥æ¶å°çæ°æ®æ¯å¦æ¯responseæ°æ®
			{ u16 anc_id_recv;
			if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //å¤ææ¥æ¶å°çæ°æ®æ¯å¦æ¯responseæ°æ®
			{ u16 anc_id_recv,rec_com_interval;
			/* 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
			@@ -397,6 +415,15 @@
			}
			memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);
			memcpy(&tx_final_msg[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 4);
			memcpy(&rec_com_interval,&rx_buffer[ANCSEND_INTERVAL], 2);
			if(rec_com_interval>4&&rec_com_interval!=g_com_map[COM_INTERVAL])
			{
			g_com_map[COM_INTERVAL]=rec_com_interval;
			save_com_map_to_flash();
			delay_ms(100);
			SCB->AIRCR = 0X05FA0000\|(unsigned int)0x04; //è½¯å¤ä½åå°bootloader
			}

			/* 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
			@@ -415,17 +442,23 @@
			dwt_writetxfctrl(sizeof(tx_final_msg), 0);//è®¾å®åéæ°æ®é¿åº¦
			result=dwt_starttx(DWT_START_TX_DELAYED);//è®¾å®ä¸ºå»¶è¿åé

			#ifdef DEBUG_OUTPUT
			printf("Fååé,åºç«ID: %d .\r\n",i);
			#endif

			tag_succ_times++;

			LED0_BLINK;

			g_Resttimer=0;
			memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
			// g_Tagdist[anc_id_recv]= hex_dist;
			// g_flag_Taggetdist[anc_id_recv]=0;

			if(!g_com_map[MODBUS_MODE]&&hex_dist2!=0xffff)
			if(hex_dist2!=0xffff)
			{
			g_Tagdist[anc_id_recv]= hex_dist2;
			g_flag_Taggetdist[anc_id_recv]=0;

			if(!g_com_map[MODBUS_MODE])
			{
			hex_dist2 = hex_dist2+(int16_t)g_com_map[DIST_OFFSET];
			hex_dist2 = hex_dist2;
			usart_send[2] = 1;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 17;//æ°æ®æ®µé¿åº¦
			usart_send[4] = frame_seq_nb;//æ°æ®æ®µé¿åº¦
			@@ -433,16 +466,19 @@
			memcpy(&usart_send[7],&rx_buffer[ANCHOR_ID_IDX],2);

			memcpy(&usart_send[9],&hex_dist2,4);
			usart_send[13] = battary;
			usart_send[13] = bat_percent;
			usart_send[14] = button;
			checksum = Checksum_u16(&usart_send[2],17);
			memcpy(&usart_send[19],&checksum,2);
			UART_PushFrame(usart_send,21);
			}
			}
			// memcpy(&Modbus_HoldReg[anc_id_recv*2],&hex_dist,4);
			/* Poll DW1000 until TX frame sent event set. See NOTE 8 below. */
			if(result==0)
			{while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//ä¸ææ¥è¯¢è¯çç¶æç´å°åéå®æ
			{

			while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//ä¸ææ¥è¯¢è¯çç¶æç´å°åéå®æ
			{ };
			}
			/* Clear TXFRS event. */
			@@ -459,6 +495,9 @@
			}
			else
			{
			#ifdef DEBUG_OUTPUT
			printf("Råå¤±è´¥éè¯¯ä¿¡æ¯: %x .\r\n",status_reg);
			#endif
			/* Clear RX error events in the DW1000 status register. */
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			random_delay_tim = DFT_RAND_DLY_TIM_MS;
			@@ -466,9 +505,9 @@
			// deca_sleep(10);
			}
			// dwt_entersleep();
			if(tag_succ_times<g_com_map[MIN_REPORT_ANC_NUM])
			if(tag_succ_times<1)
			{
			//poll_timer +=time32_incr&0x7+3;
			tyncpoll_time=(current_slotpos--%max_slotnum)*slottime;
			}

			/* Execute a delay between ranging exchanges. */
			@@ -479,11 +518,12 @@
			#define TDFILTER
			//#define CHECK_UID
			extern uint8_t UID_ERROR;
			extern u16 dist_threshold;
			u8 misdist_num[TAG_NUM_IN_SYS];
			void Anchor_App(void)
			{
			uint32_t frame_len;
			uint32_t resp_tx_time;
			static u8 misdist_num;
			/* Clear reception timeout to start next ranging process. */
			dwt_setrxtimeout(0);//è®¾å®æ¥æ¶è¶æ¶æ¶é´ï¼0ä½æ²¡æè¶æ¶æ¶é´

			@@ -494,7 +534,6 @@
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR))&&!g_start_send_flag&&!g_start_sync_flag)//ä¸ææ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæèåºç°éè¯¯
			{
			IdleTask();
			g_Resttimer=0;
			};

			if (status_reg & SYS_STATUS_RXFCG)//æåæ¥æ¶
			@@ -523,7 +562,7 @@
			// tag_recv_interval = tag_recv_timer + 65535 - tag_time_recv[tag_id_recv];
			// }

			if (rx_buffer[MESSAGE_TYPE_IDX] == POLL&&(anchor_type == rx_buffer[ANC_TYPE_IDX])) //å¤ææ¯å¦æ¯pollåæ°æ®
			if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == POLL&&(anchor_type == rx_buffer[ANC_TYPE_IDX])) //å¤ææ¯å¦æ¯pollåæ°æ®
			{
			tmp_time=TIM3->CNT;
			memcpy(&tx_resp_msg[ANCTIMEMS],&sync_timer,2);
			@@ -556,6 +595,9 @@
			frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
			/* We assume that the transmission is achieved correctly, now poll for reception of expected "final" frame or error/timeout.
			* See NOTE 7 below. */
			#ifdef DEBUG_OUTPUT
			printf("æ¶å°POLLåï¼æ ç¾ID: %d .\r\n",tag_id_recv);
			#endif
			if(result==0)
			{
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR)))///ä¸ææ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæèåºç°éè¯¯
			@@ -577,7 +619,7 @@
			/* 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 (rx_buffer[MESSAGE_TYPE_IDX] == FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,4)&&!memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,4)) //å¤ææ¯å¦ä¸ºFinalå
			if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,4)&&!memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,4)) //å¤ææ¯å¦ä¸ºFinalå
			{
			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;
			@@ -615,15 +657,19 @@
			// dist[TAG_ID] = LP(dis, TAG_ID); //LP ä¸ºä½éæ»¤æ³¢å¨ï¼è®©æ°æ®æ´ç¨³å®

			/--------------------------ä»¥ä¸ä¸ºéæµè·é»è¾------------------------/
			#ifdef DEBUG_OUTPUT
			printf("æ¶å°FINALåï¼æ ç¾ID: %d .\r\n",tag_id_recv);
			#endif
			LED0_BLINK; //æ¯æåä¸æ¬¡éè®¯åéªçä¸æ¬¡
			g_UWB_com_interval = 0;

			g_Resttimer=0;
			hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10;
			if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
			{
			if(hex_dist-his_dist[tag_id_recv-TAG_ID_START]<15000\|\|misdist_num>4)
			{int32_t filter_dist;
			misdist_num=0;
			if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])<dist_threshold\|\|misdist_num[tag_id_recv-TAG_ID_START]>4)
			{
			int32_t filter_dist;
			misdist_num[tag_id_recv-TAG_ID_START]=0;
			if(hex_dist<1000000&&hex_dist>-10000)
			{
			#ifdef TDFILTER
			@@ -658,19 +704,22 @@
			//dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm);
			}

			}else{
			misdist_num++;
			}
			else{
			misdist_num[tag_id_recv-TAG_ID_START]++;
			}
			}
			}
			}else{
			/* Clear RX error events in the DW1000 status register. */
			#ifdef DEBUG_OUTPUT
			printf("Fåå¤±è´¥éè¯¯ä¿¡æ¯: %x .\r\n",status_reg);
			#endif
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			}else if(rx_buffer[MESSAGE_TYPE_IDX] == SYNC)
			{
			if(rx_buffer[SYNC_SEQ_IDX]<sync_seq)
			// if(rx_buffer[SYNC_SEQ_IDX]==2)
			if(rx_buffer[SYNC_SEQ_IDX]<sync_seq&&sync_mainbase==0)
			{
			sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
			TIM3->CNT = sync_seq*325%1000+15;
			@@ -681,6 +730,9 @@
			}
			else
			{
			#ifdef DEBUG_OUTPUT
			printf("Påå¤±è´¥éè¯¯ä¿¡æ¯: %x .\r\n",status_reg);
			#endif
			/* Clear RX error events in the DW1000 status register. */
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}