UWB_SmallModule_F103.git

			@@ -1,5 +1,6 @@
			#include "dw_app.h"
			#include "ADC.h"
			#include "modbus.h"
			#define TDFILTER
			enum enumtagstate
			{
			@@ -464,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);
			if(temp_dist!=0x1ffff&&g_com_map[ALARM_DISTANCE1] == 1)
			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;
			@@ -516,20 +514,16 @@
			LED0_BLINK;
			// memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
			// tagslotpos=GetRandomSlotPos(rec_tagpos_binary);
			if(temp_dist!=0x1ffff&&g_com_map[ALARM_DISTANCE1] == 1)
			if(temp_dist!=0x1ffff)
			{
			#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;
			@@ -738,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)
			@@ -842,6 +836,26 @@
			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;
			@@ -862,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])
			@@ -874,18 +897,11 @@
			}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;
			tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)\|(motorstate&0xf);
			if(remotesend_state)
			{
			memcpy(&tx_nearresp_msg[REMOTEPARA_INDEX],remotetag_para,REMOTEPARA_LEN);
			@@ -940,7 +956,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;
			@@ -959,7 +977,7 @@
			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();
			@@ -978,7 +996,7 @@
			{
			g_Tagdist[taglist_pos]=hex_dist/10;
			}else{
			g_Tagdist[taglist_pos]= 1;
			// g_Tagdist[taglist_pos]= 1;
			}
			#ifdef USART_SINGLE_OUTPUT
			usart_send[2] = 1;//æ£å¸¸æ¨¡å¼
			@@ -986,19 +1004,48 @@
			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]++;
			@@ -1054,29 +1101,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)
			@@ -1102,7 +1149,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+nearbase_num*4],2);
			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;//æ°æ®æ®µé¿åº¦