UWB_SmallModule_F103.git

			@@ -1,11 +1,12 @@
			#include "dw_app.h"
			#include "ADC.h"
			#define TDFILTER
			enum enumtagstate
			{
			DISCPOLL,
			GETNEARMSG,
			NEARPOLL,
			}tag_state=GETNEARMSG;
			}tag_state=NEARPOLL;
			static dwt_config_t config = {
			2, /* Channel number. */
			DWT_PRF_64M, /* Pulse repetition frequency. */
			@@ -28,7 +29,7 @@

			static uint8_t tx_near_msg[80] = {0};

			static uint32_t frame_seq_nb = 0;
			uint32_t frame_seq_nb = 0, frame_seq_nb2 = 0;
			static uint32_t status_reg = 0;
			static uint8_t rx_buffer[100];
			static uint64_t poll_tx_ts;
			@@ -337,17 +338,24 @@
			tx_nearpoll_msg[SEQUENCE_IDX] = frame_seq_nb++;
			tx_nearpoll_msg[NEARBASENUM_INDEX] = nearbase_num;
			memcpy(&tx_nearpoll_msg[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2);
			for(i=0;i<nearbase_num;i++)
			{ memcpy(&tx_nearpoll_msg[NEARBASEID_INDEX+nearbase_num2+i2],&nearbase_distlist[i],2);}

			for(i=0;i<MAX_NEARBASE_NUM;i++)
			{
			nearbase_distlist[i]=0x1ffff;
			}
			tx_nearpoll_msg[MESSAGE_TYPE_IDX] = NEAR_POLL;
			memcpy(&tx_nearpoll_msg[ANCHOR_ID_IDX],&mainbase_id,2);
			dwt_writetxdata(13+2*nearbase_num, tx_nearpoll_msg, 0);//å°Pollåæ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå»
			dwt_writetxfctrl(13+2*nearbase_num, 0);//è®¾ç½®è¶å®½å¸¦åéæ°æ®é¿åº¦
			dwt_writetxdata(13+4*nearbase_num, tx_nearpoll_msg, 0);//å°Pollåæ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå»
			dwt_writetxfctrl(13+4*nearbase_num, 0);//è®¾ç½®è¶å®½å¸¦åéæ°æ®é¿åº¦
			dwt_starttx(DWT_START_TX_IMMEDIATE \| DWT_RESPONSE_EXPECTED);//å¼å¯åéï¼åéå®æåçå¾ä¸æ®µæ¶é´å¼å¯æ¥æ¶ï¼çå¾æ¶é´å¨dwt_setrxaftertxdelayä¸è®¾ç½®

			flag_finalsend=0;
			flag_rxon=1;
			neartimout_timer=0;
			get_newbase=0;
			timeout=ceil((float)nearbase_num*0.4)+3;
			timeout=ceil((float)nearbase_num*SLOT_SCALE)+3;
			//timeout=5;
			mainbase_dist=100000;
			mainbase_lost_count++;
			@@ -446,7 +454,34 @@
			dwt_writetxdata(28+nearbase_num*4, tx_nearfinal_msg, 0);//å°åéæ°æ®åå¥DW1000
			dwt_writetxfctrl(28+nearbase_num*4, 0);//è®¾å®åéæ°æ®é¿åº¦
			memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);

			if(temp_dist!=0x1ffff)
			{
			g_Resttimer=0;
			IWDG_Feed();
			#ifdef USART_SINGLE_OUTPUT
			usart_send[2] = 1;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 17;//æ°æ®æ®µé¿åº¦
			usart_send[4] = frame_seq_nb;//æ°æ®æ®µé¿åº¦
			if(g_com_map[DEV_ROLE])
			{
			memcpy(&usart_send[5],&dev_id,2);
			memcpy(&usart_send[7],&rec_nearbaseid,2);
			}else{
			memcpy(&usart_send[5],&rec_nearbaseid,2);
			memcpy(&usart_send[7],&dev_id,2);
			}
			memcpy(&usart_send[9],&rx_buffer[DIST_IDX],4);
			usart_send[13] = battary;
			usart_send[14] = button;
			checksum = Checksum_u16(&usart_send[2],17);
			memcpy(&usart_send[19],&checksum,2);
			UART_PushFrame(usart_send,21);
			#endif

			//dwt_setdelayedtrxtime(final_tx_time);//è®¾ç½®finalååéæ¶é´T5
			// result=dwt_starttx(DWT_START_TX_DELAYED);//è®¾å®ä¸ºå»¶è¿åé
			//dwt_writetxdata(4,&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)4], FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)4);//å°åéæ°æ®åå¥DW1000
			}


			}else{
			@@ -473,18 +508,24 @@
			// tagslotpos=GetRandomSlotPos(rec_tagpos_binary);
			if(temp_dist!=0x1ffff)
			{
			#ifndef USART_INTEGRATE_OUTPUT
			#ifdef USART_SINGLE_OUTPUT
			usart_send[2] = 1;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 17;//æ°æ®æ®µé¿åº¦
			usart_send[4] = frame_seq_nb;//æ°æ®æ®µé¿åº¦
			if(g_com_map[DEV_ROLE])
			{
			memcpy(&usart_send[5],&dev_id,2);
			memcpy(&usart_send[7],&rec_nearbaseid,2);
			memcpy(&usart_send[7],&rec_nearbaseid,2);
			}else{
			memcpy(&usart_send[5],&rec_nearbaseid,2);
			memcpy(&usart_send[7],&dev_id,2);
			}
			memcpy(&usart_send[9],&rx_buffer[DIST_IDX],4);
			usart_send[13] = battary;
			usart_send[14] = button;
			checksum = Checksum_u16(&usart_send[2],17);
			memcpy(&usart_send[19],&checksum,2);
			// UART_PushFrame(usart_send,21);
			UART_PushFrame(usart_send,21);
			#endif

			//dwt_setdelayedtrxtime(final_tx_time);//è®¾ç½®finalååéæ¶é´T5
			@@ -522,7 +563,7 @@
			// }
			tagslotpos=GetRandomSlotPos(rec_tagpos_binary\|tagpos_binary);
			tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
			tag_state=GETNEARMSG;
			tag_state=NEARPOLL;
			}
			// tyncpoll_time=0;
			next_nearbase_num=0;
			@@ -574,19 +615,16 @@
			report_num++;
			}
			}
			for(i=0;i<MAX_NEARBASE_NUM;i++)
			{
			nearbase_distlist[i]=0x1ffff;
			}


			// printf("%d,%d",temp_sync_timer2,temp_sync_timer1);
			// #ifdef USART_INTEGRATE_OUTPUT
			// usart_send[2] = 4;//æ£å¸¸æ¨¡å¼
			// usart_send[3] = report_num*6+2;//æ£å¸¸æ¨¡å¼
			// checksum = Checksum_u16(&usart_send[2],report_num*6+2);
			// memcpy(&usart_send[4+report_num*6],&checksum,2);
			// UART_PushFrame(usart_send,6+report_num*6);
			// #endif
			#ifdef USART_INTEGRATE_OUTPUT
			usart_send[2] = 4;//æ£å¸¸æ¨¡å¼
			usart_send[3] = report_num*6+2;//æ£å¸¸æ¨¡å¼
			checksum = Checksum_u16(&usart_send[2],report_num*6+2);
			memcpy(&usart_send[4+report_num*6],&checksum,2);
			UART_PushFrame(usart_send,6+report_num*6);
			#endif
			// if(mainbase_lost_count>5)
			// {
			// //tag_state=DISCPOLL;
			@@ -655,13 +693,19 @@
			tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
			}

			#ifdef USART_INTEGRATE_OUTPUT
			usart_send[2] = 4;//æ£å¸¸æ¨¡å¼
			usart_send[3] = report_num*6+2;//æ£å¸¸æ¨¡å¼
			checksum = Checksum_u16(&usart_send[2],report_num*6+2);
			memcpy(&usart_send[4+report_num*6],&checksum,2);
			UART_PushFrame(usart_send,6+report_num*6);
			#endif
			}

			void Tag_App(void)//åéæ¨¡å¼(TAGæ ç¾)
			{
			//LED0_ON;
			dwt_forcetrxoff();
			g_Resttimer=0;
			GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_SET);
			switch(tag_state)
			{
			@@ -735,7 +779,7 @@

			battary = rx_buffer[BATTARY_IDX];
			button = rx_buffer[BUTTON_IDX];
			frame_seq_nb = rx_buffer[SEQUENCE_IDX];
			frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
			if(result==0)
			{
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR)))///ä¸ææ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæèåºç°éè¯¯
			@@ -785,7 +829,8 @@
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			}
			u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor;
			extern u16 dist_threshold;
			u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,getrange_success=0;
			u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase 1 first near_base
			{
			u8 motorstate;
			@@ -835,9 +880,7 @@
			}
			result = dwt_starttx(DWT_START_TX_DELAYED \| DWT_RESPONSE_EXPECTED);//å»¶è¿åéï¼çå¾æ¥æ¶

			battary = rx_buffer[BATTARY_IDX];
			button = rx_buffer[BUTTON_IDX];
			frame_seq_nb = rx_buffer[SEQUENCE_IDX];

			if(result==0)
			{
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR))&&!g_start_sync_flag)///ä¸ææ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæèåºç°éè¯¯
			@@ -879,26 +922,36 @@
			tof = tof_dtu * DWT_TIME_UNITS;
			distance = tof * SPEED_OF_LIGHT;//è·ç¦»=åé*é£è¡æ¶é´
			dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //è·ç¦»åå»ç«æ£ç³»æ°
			dist_cm = dist_no_bias * 100; //dis ä¸ºåä½ä¸ºcmçè·ç¦»
			dist_cm = dist_no_bias * 1000; //dis ä¸ºåä½ä¸ºcmçè·ç¦»
			/--------------------------ä»¥ä¸ä¸ºéæµè·é»è¾------------------------/
			//dist_cm=33000;
			getrange_success = 1;

			LED0_BLINK; //æ¯æåä¸æ¬¡éè®¯åéªçä¸æ¬¡
			dis_after_filter=dist_cm;
			hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
			hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10;
			g_flag_Taggetdist[taglist_pos]=0;
			if(hex_dist>-1000&&hex_dist<100000)
			if(hex_dist>0&&hex_dist<2000000)
			{
			if(abs(hex_dist-his_dist[taglist_pos])<1500\|\|misdist_num[taglist_pos]>3)
			if(abs(hex_dist-his_dist[taglist_pos])<dist_threshold\|\|misdist_num[taglist_pos]>3)
			{
			int32_t filter_dist;
			g_Resttimer=0;
			IWDG_Feed();
			#ifdef TDFILTER
			NewTrackingDiffUpdate(taglist_pos, (float)hex_dist);
			filter_dist=pos_predict[taglist_pos]/10;
			#else
			filter_dist=hex_dist/10;
			#endif
			misdist_num[taglist_pos]=0;
			tagdist_list[taglist_pos] = hex_dist;
			tagdist_list[taglist_pos] = filter_dist;
			his_dist[taglist_pos]=hex_dist;
			g_Tagdist[taglist_pos]=hex_dist;
			#ifndef USART_INTEGRATE_OUTPUT
			g_Tagdist[taglist_pos]=filter_dist;
			#ifdef USART_SINGLE_OUTPUT
			usart_send[2] = 1;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 17;//æ°æ®æ®µé¿åº¦
			usart_send[4] = frame_seq_nb;//æ°æ®æ®µé¿åº¦
			usart_send[4] = frame_seq_nb2;//æ°æ®æ®µé¿åº¦
			memcpy(&usart_send[5],&tag_id_recv,2);
			memcpy(&usart_send[7],&dev_id,2);
			memcpy(&usart_send[9],&tagdist_list[taglist_pos],4);
			@@ -926,7 +979,7 @@
			uint32_t current_syncid=0xffffffff,synclost_timer;
			extern u8 flag_syncbase;
			u8 tagpos_rec[50],tagpos_send[50],ancidlist_num;
			u16 ancidlist_rec[20],ancidlist_send[20];
			u16 ancidlist_rec[20],ancidlist_send[20],rec_ancidlist[20],rec_ancdistlist[20];
			void Anchor_App(void)
			{

			@@ -942,6 +995,7 @@
			// GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_SET);
			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();
			};
			//GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_RESET);
			@@ -949,7 +1003,7 @@
			{ u16 tag_recv_interval;
			float temp_tagpos;

			g_Resttimer=0;

			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//æ¸é¤æ å¿ä½
			frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//è·å¾æ¥æ¶æ°æ®é¿åº¦
			dwt_readrxdata(rx_buffer, frame_len, 0);//è¯»åæ¥æ¶æ°æ®
			@@ -962,7 +1016,7 @@
			// if (anchor_type == rx_buffer[ANC_TYPE_IDX])
			// Anchor_RecPoll();
			// break;
			case SYNC:
			case SYNC:
			memcpy(&rec_syncid,&rx_buffer[ANCHOR_ID_IDX],4);
			if(rec_syncid<current_syncid)
			{
			@@ -1003,8 +1057,30 @@
			}
			break;

			case NEAR_POLL:
			case NEAR_POLL:
			frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
			battary = rx_buffer[BATTARY_IDX];
			button = rx_buffer[BUTTON_IDX];
			rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
			memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
			memcpy(&rec_ancidlist,&rx_buffer[NEARBASEID_INDEX],2*rec_nearbase_num);
			memcpy(&rec_ancdistlist,&rx_buffer[NEARBASEID_INDEX+2rec_nearbase_num],2rec_nearbase_num);
			#ifdef USART_ALL_OUTPUT
			usart_send[2] = 0x0c;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 8+rec_nearbase_num*4;//æ°æ®æ®µé¿åº¦
			usart_send[4] = frame_seq_nb2;//æ°æ®æ®µé¿åº¦
			usart_send[5] = battary;
			usart_send[6] = button;
			usart_send[7] = rec_nearbase_num;
			memcpy(&usart_send[8],&tag_id_recv,2);
			memcpy(&usart_send[10],&rec_ancidlist,2*rec_nearbase_num);
			memcpy(&usart_send[10+rec_nearbase_num2],&rec_ancdistlist,2rec_nearbase_num);
			checksum = Checksum_u16(&usart_send[2],8+4*rec_nearbase_num);
			memcpy(&usart_send[10+4*rec_nearbase_num],&checksum,2);
			UART_PushFrame(usart_send,12+rec_nearbase_num*4);
			#endif
			if(tag_id_recv>=g_com_map[WHITELIST1_START]&&tag_id_recv<=g_com_map[WHITELIST1_END])
			{break;}
			taglist_pos=CmpTagInList(tag_id_recv);
			if(taglist_pos==taglist_num)
			{
			@@ -1016,9 +1092,9 @@
			new_tagid=0;
			}
			tagofflinetime[taglist_pos]=0;
			temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);
			temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);
			tagpos_rec[(u8)temp_tagpos]=1;
			rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];


			if(rec_nearbase_num>ancidlist_num)
			{