UWB_SmallModule_F103.git

			@@ -1,5 +1,6 @@
			#include "dw_app.h"
			#include "ADC.h"
			#include "modbus.h"
			#define TDFILTER
			enum enumtagstate
			{
			@@ -48,7 +49,7 @@
			uint32_t g_UWB_com_interval = 0;
			float dis_after_filter; //å½åè·ç¦»å¼
			LPFilter_Frac* p_Dis_Filter; //æµè·ç¨çä½éæ»¤æ³¢å¨
			uint16_t g_Tagdist[TAG_NUM_IN_SYS];
			int32_t g_Tagdist[TAG_NUM_IN_SYS];
			uint8_t g_flag_Taggetdist[256];

			static uint64_t get_tx_timestamp_u64(void)
			@@ -110,11 +111,12 @@
			g_flag_Taggetdist[i]++;
			if(g_flag_Taggetdist[i]>=2)
			{
			g_Tagdist[i]=0xffff;
			g_Tagdist[i]=0x1ffff;
			}
			}
			}
			}
			extern int16_t g_commap_antdelay;
			void Dw1000_Init(void)
			{
			/* Reset and initialise DW1000.
			@@ -131,7 +133,7 @@

			/* Apply default antenna delay value. See NOTE 1 below. */
			dwt_setrxantennadelay(RX_ANT_DLY); //è®¾ç½®æ¥æ¶å¤©çº¿å»¶è¿
			dwt_settxantennadelay(TX_ANT_DLY); //è®¾ç½®åå°å¤©çº¿å»¶è¿
			dwt_settxantennadelay(0); //è®¾ç½®åå°å¤©çº¿å»¶è¿

			/* Set expected response's delay and timeout. See NOTE 4 and 5 below.
			* As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */
			@@ -346,15 +348,17 @@
			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);}


			memcpy(&tx_nearpoll_msg[NEARBASEID_INDEX+nearbase_num*4],&g_commap_antdelay,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+4*nearbase_num, tx_nearpoll_msg, 0);//å°Pollåæ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå»
			dwt_writetxfctrl(13+4*nearbase_num, 0);//è®¾ç½®è¶å®½å¸¦åéæ°æ®é¿åº¦
			dwt_writetxdata(15+4*nearbase_num, tx_nearpoll_msg, 0);//å°Pollåæ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå»
			dwt_writetxfctrl(15+4*nearbase_num, 0);//è®¾ç½®è¶å®½å¸¦åéæ°æ®é¿åº¦
			dwt_starttx(DWT_START_TX_IMMEDIATE \| DWT_RESPONSE_EXPECTED);//å¼å¯åéï¼åéå®æåçå¾ä¸æ®µæ¶é´å¼å¯æ¥æ¶ï¼çå¾æ¶é´å¨dwt_setrxaftertxdelayä¸è®¾ç½®

			flag_finalsend=0;
			@@ -461,22 +465,19 @@
			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);
			IWDG_Feed();
			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;
			@@ -519,14 +520,10 @@
			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;
			@@ -735,7 +732,7 @@
			int8_t correction_time,new_tagid=0;
			extern uint8_t sync_seq;
			u16 taglist_num=0,taglist_pos;
			u16 tagid_list[TAG_NUM_IN_SYS];
			u16 tagid_list[TAG_NUM_IN_SYS],tagstate_list[TAG_NUM_IN_SYS];;
			u8 tagofflinetime[TAG_NUM_IN_SYS];
			int32_t tagdist_list[TAG_NUM_IN_SYS];
			void TagListUpdate(void)
			@@ -763,6 +760,7 @@
			uint32_t frame_len;
			uint32_t resp_tx_time;
			uint8_t rec_nearbase_num,anc_report_num;
			int16_t rec_antdelay;
			void Anchor_RecPoll(void)
			{
			tmp_time=TIM3->CNT;
			@@ -786,6 +784,8 @@

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


			frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
			if(result==0)
			{
			@@ -836,10 +836,31 @@
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			}
			double firstpath_power, rx_power,rec_firstpath_power;
			double f1, f2, r1, r2;
			uint16_t F1,F2,F3,N,C;
			double B = 131072;
			double A = 121.74;
			double min_power;
			dwt_rxdiag_t d1;
			double LOS(dwt_rxdiag_t *dia) {
			F1 = dia->firstPathAmp1;
			F2 = dia->firstPathAmp2;
			F3 = dia->firstPathAmp3;
			N = dia->rxPreamCount;
			C = dia->maxGrowthCIR;

			firstpath_power=10* log10((F1F1+F2F2+F3F3)/(NN))-A;
			rx_power=10log10(CB/(N*N))-A;

			// min_power = - 10 * log10((F1 F1 + F2 F2 + F3 * F3) / (C *B));
			return min_power;
			}
			u16 smallcar_idlist[10]={0x4052,0x4032,0x4055,0x4034,0x4016,0x4010,0x4036,0x4027,0x4024,0x4047};
			extern u16 dist_threshold;
			u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,getrange_success=0;
			int32_t filter_dist,filter_speed;
			u8 newmeasure;
			u8 newmeasure,recpoll_len;
			u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase 1 first near_base
			{
			u8 motorstate;
			@@ -855,7 +876,16 @@
			dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS+(rec_nearbase_num+1-ancrec_nearbasepos)*DELAY_BETWEEN_TWO_FRAME_UUS);//è®¾ç½®åéå®æåå¼å¯æ¥æ¶å»¶è¿æ¶é´
			dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//æ¥æ¶è¶æ¶æ¶é´

			if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE1])

			if(new_tagid)
			{
			tagdist_list[taglist_pos]=0x1ffff;
			memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
			}else{
			memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
			}

			if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE1])
			{
			motorstate =0;
			}else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE2])
			@@ -867,18 +897,12 @@
			}else{
			motorstate =0;
			}

			if(new_tagid)
			{
			tagdist_list[taglist_pos]=0x1ffff;
			memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
			}else{
			memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
			}

			tx_nearresp_msg[GROUP_ID_IDX] = group_id;
			tx_nearresp_msg[MAINBASE_INDEX]=flag_syncbase;
			tx_nearresp_msg[MESSAGE_TYPE_IDX]=NEAR_RESPONSE;
			tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)\|motorstate;
			remotesend_state = 0;
			tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)\|(motorstate&0xf);
			if(remotesend_state)
			{
			memcpy(&tx_nearresp_msg[REMOTEPARA_INDEX],remotetag_para,REMOTEPARA_LEN);
			@@ -915,6 +939,7 @@
			uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
			double Ra, Rb, Da, Db;
			int64_t tof_dtu;
			u8 si;
			resp_tx_ts = get_tx_timestamp_u64();//è·å¾responseåéæ¶é´T3
			final_rx_ts = get_rx_timestamp_u64();//è·å¾finalæ¥æ¶æ¶é´T6
			final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);//ä»æ¥æ¶æ°æ®ä¸è¯»åT1ï¼T4ï¼T5
			@@ -932,7 +957,9 @@
			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 * 1000; //dis ä¸ºåä½ä¸ºcmçè·ç¦»
			dist_cm = dist_no_bias * 1000; //dis ä¸ºåä½ä¸ºcmçè·ç¦»
			dwt_readdiagnostics(&d1);
			LOS(&d1);
			/--------------------------ä»¥ä¸ä¸ºéæµè·é»è¾------------------------/
			//dist_cm=33000;
			getrange_success = 1;
			@@ -940,11 +967,18 @@
			LED0_BLINK; //æ¯æåä¸æ¬¡éè®¯åéªçä¸æ¬¡
			dis_after_filter=dist_cm;
			hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10;

			if(recpoll_len==rec_nearbase_num*4+15)
			{
			hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]10-rec_antdelay10;
			}

			g_flag_Taggetdist[taglist_pos]=0;
			if(hex_dist>-1000&&hex_dist<2000000)

			if(hex_dist>-100000&&hex_dist<2000000)
			{
			if(abs(hex_dist-his_dist[taglist_pos])<dist_threshold\|\|misdist_num[taglist_pos]>3)
			{
			{u8 i=0;

			g_Resttimer=0;
			IWDG_Feed();
			@@ -959,26 +993,60 @@
			misdist_num[taglist_pos]=0;
			tagdist_list[taglist_pos] = filter_dist;
			his_dist[taglist_pos]=hex_dist;
			g_Tagdist[taglist_pos]=filter_dist;
			if(hex_dist>0)
			{
			g_Tagdist[taglist_pos]=hex_dist/10;
			}else{
			// g_Tagdist[taglist_pos]= 1;
			}
			#ifdef USART_SINGLE_OUTPUT
			usart_send[2] = 1;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 17;//æ°æ®æ®µé¿åº¦
			usart_send[4] = frame_seq_nb2;//æ°æ®æ®µé¿åº¦
			memcpy(&usart_send[5],&tag_id_recv,2);
			memcpy(&usart_send[7],&dev_id,2);
			if(tagdist_list[taglist_pos]<=0)
			tagdist_list[taglist_pos]=10;
			if(tagdist_list[taglist_pos]<=0)
			tagdist_list[taglist_pos]=0x2ffff;
			memcpy(&usart_send[9],&tagdist_list[taglist_pos],4);
			usart_send[13] = battary;
			usart_send[14] = button;
			usart_send[15] = firstpath_power;
			usart_send[16] = rx_power;
			checksum = Checksum_u16(&usart_send[2],17);
			memcpy(&usart_send[19],&checksum,2);
			if(g_com_map[MODBUS_MODE]!=1)
			UART_PushFrame(usart_send,21);
			#else
			memcpy(&usart_send_anc[4+6*anc_report_num],&tag_id_recv,2);
			memcpy(&usart_send_anc[6+6*anc_report_num],&tagdist_list[taglist_pos],4);
			anc_report_num++;
			#endif
			tagstate_list[taglist_pos] = (button<<8)\|battary;
			for(i=0;i<taglist_num-1;i++)
			{
			if( tagdist_list[i]>tagdist_list[i+1]&&tagdist_list[i+1]!=0)
			{
			int32_t tempdist;
			u16 tempid,tempstate;
			tempdist = tagdist_list[i];
			tempid = tagid_list[i];
			tempstate = tagstate_list[i];

			tagdist_list[i] = tagdist_list[i+1];
			tagid_list[i] = tagid_list[i+1];
			tagstate_list[i] = tagstate_list[i+1];

			tagdist_list[i+1] = tempdist;
			tagid_list[i+1] = tempid;
			tagstate_list[i+1] = tempstate;
			}
			}
			for(i=0;i<taglist_num;i++)
			{
			Modbus_HoldReg[i*4] = tagid_list[i];
			Modbus_HoldReg[i*4+1] = tagstate_list[i];
			memcpy(&Modbus_HoldReg[i*4+2],&tagdist_list[i],4);
			}
			}else{
			// printf("%d",hex_dist);
			misdist_num[taglist_pos]++;
			@@ -1020,6 +1088,7 @@

			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//æ¸é¤æ å¿ä½
			frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//è·å¾æ¥æ¶æ°æ®é¿åº¦
			recpoll_len = frame_len;
			dwt_readrxdata(rx_buffer, frame_len, 0);//è¯»åæ¥æ¶æ°æ®
			memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
			//å°æ¶å°çtag_idåå«åå¥åæ¬¡éè®¯çåä¸ï¼ä¸ºå¤æ ç¾éè®¯æå¡ï¼é²æ¢ä¸æ¬¡éè®¯ä¸æ¥æ¶å°ä¸åIDæ ç¾çæ°æ®
			@@ -1033,29 +1102,29 @@
			// Anchor_RecPoll();
			// break;
			case SYNC:
			memcpy(&rec_syncid,&rx_buffer[ANCHOR_ID_IDX],4);
			if(rec_syncid<current_syncid)
			{
			current_syncid=rec_syncid;
			flag_syncbase=0;
			sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
			TIM3->CNT = sync_seq*325%1000+15;
			sync_timer = sync_seq*325/1000;
			synclost_timer=0;
			//SyncPoll(sync_seq,rec_syncid);
			}else if(rec_syncid==current_syncid)
			{
			if(rx_buffer[SYNC_SEQ_IDX]<sync_seq)
			{
			flag_syncbase=0;
			sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
			TIM3->CNT = sync_seq*325%1000+5;
			sync_timer = sync_seq*325/1000+995;
			synclost_timer=0;
			//SyncPoll(sync_seq,rec_syncid);
			}

			}
			// memcpy(&rec_syncid,&rx_buffer[ANCHOR_ID_IDX],4);
			// if(rec_syncid<current_syncid)
			// {
			// current_syncid=rec_syncid;
			// flag_syncbase=0;
			// sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
			// TIM3->CNT = sync_seq*325%1000+15;
			// sync_timer = sync_seq*325/1000;
			// synclost_timer=0;
			// //SyncPoll(sync_seq,rec_syncid);
			// }else if(rec_syncid==current_syncid)
			// {
			// if(rx_buffer[SYNC_SEQ_IDX]<sync_seq)
			// {
			// flag_syncbase=0;
			// sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
			// TIM3->CNT = sync_seq*325%1000+5;
			// sync_timer = sync_seq*325/1000+995;
			// synclost_timer=0;
			// //SyncPoll(sync_seq,rec_syncid);
			// }
			//
			// }
			break;
			case NEAR_MSG:
			if(anc_id_recv==flag_syncbase)
			@@ -1081,6 +1150,7 @@
			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);
			memcpy(&rec_antdelay,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*4],2);
			#ifdef USART_ALL_OUTPUT
			usart_send[2] = 0x0c;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 8+rec_nearbase_num*4;//æ°æ®æ®µé¿åº¦