UWB_SmallModule_F103.git

			@@ -1,6 +1,12 @@
			#include "dw_app.h"
			#include "ADC.h"
			#define TDFILTER
			#include "stm32f10x_it.h"
			#include "serial_at_cmd_app.h"
			#define STRX(x) #x
			#define STR(x) STRX(x)
			#define __debug_info__ __FILE__ ":" STR(__LINE__)
			//#define TDFILTER
			#define DBGMSG_OUTPUT
			enum enumtagstate
			{
			DISCPOLL,
			@@ -43,14 +49,14 @@
			uint32_t tag_id = 0;
			uint32_t tag_id_recv = 0;
			uint32_t anc_id_recv = 0;
			uint8_t random_delay_tim = 0;
			uint8_t random_delay_tim = 0,dbgmsg_len;
			double distance, dist_no_bias, dist_cm;
			uint32_t g_UWB_com_interval = 0;
			float dis_after_filter; //å½åè·ç¦»å¼
			LPFilter_Frac* p_Dis_Filter; //æµè·ç¨çä½éæ»¤æ³¢å¨
			int32_t g_Tagdist[TAG_NUM_IN_SYS];
			uint8_t g_flag_Taggetdist[256],flag_tag_distsmooth[TAG_NUM_IN_SYS];

			char dbgmsg[100];
			static uint64_t get_tx_timestamp_u64(void)
			{
			uint8_t ts_tab[5];
			@@ -315,10 +321,10 @@
			}
			u8 test=0;
			u8 anclost_times=0;
			u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0;
			u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0,uwbrec_userdata[20],uwbrec_userdatalen;
			u16 temp_sync_timer1,temp_sync_timer2;

			void NearPoll(void)
			extern u8 usartrec_userdatalen,heartbeat_timer;
			void NearPoll(void)
			{
			static u8 mainbase_lost_count=0,flag_finalsend,flag_rxon;
			uint32_t temp1,temp2,dw_systime;
			@@ -344,20 +350,23 @@
			tx_nearpoll_msg[BUTTON_IDX] = !READ_KEY0;
			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);
			tx_nearpoll_msg[MESSAGE_TYPE_IDX] = NEAR_POLL;
			memcpy(&tx_nearpoll_msg[ANCHOR_ID_IDX],&mainbase_id,2);
			tx_nearpoll_msg[POLL_USERDATA_IDX] = usartrec_userdatalen;
			memcpy(&tx_nearpoll_msg[POLL_USERDATA_IDX+1],&uartrec_userdata,usartrec_userdatalen);
			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);
			memcpy(&tx_nearpoll_msg[KUIXIANLEN_INDEX],&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(15+4*nearbase_num, tx_nearpoll_msg, 0);//å°Pollåæ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå»
			dwt_writetxfctrl(15+4*nearbase_num, 0);//è®¾ç½®è¶å®½å¸¦åéæ°æ®é¿åº¦

			dwt_writetxdata(27+4*nearbase_num, tx_nearpoll_msg, 0);//å°Pollåæ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå»
			dwt_writetxfctrl(27+4*nearbase_num, 0);//è®¾ç½®è¶å®½å¸¦åéæ°æ®é¿åº¦
			dwt_starttx(DWT_START_TX_IMMEDIATE \| DWT_RESPONSE_EXPECTED);//å¼å¯åéï¼åéå®æåçå¾ä¸æ®µæ¶é´å¼å¯æ¥æ¶ï¼çå¾æ¶é´å¨dwt_setrxaftertxdelayä¸è®¾ç½®

			flag_finalsend=0;
			@@ -381,7 +390,14 @@
			flag_rxon=0;
			dwt_forcetrxoff();
			dwt_setdelayedtrxtime(final_tx_time);
			result=dwt_starttx(DWT_START_TX_DELAYED);//è®¾å®ä¸ºå»¶è¿åé
			result=dwt_starttx(DWT_START_TX_DELAYED);//è®¾å®ä¸ºå»¶è¿åé
			if(result!=DWT_SUCCESS&&g_com_map[MODBUS_ID1]==1)
			{
			#ifdef DBGMSG_OUTPUT
			dbgmsg_len = sprintf(dbgmsg,"%s status_reg:%x",__debug_info__,status_reg);
			SendDbgMsg(dbgmsg,dbgmsg_len);
			#endif
			}
			break;
			}
			}
			@@ -390,15 +406,17 @@
			};
			if(status_reg==0xffffffff)
			{
			NVIC_SystemReset();
			//NVIC_SystemReset();
			}
			if (status_reg & SYS_STATUS_RXFCG)//å¦ææåæ¥æ¶
			{
			rflosttimer = 0;
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG \| SYS_STATUS_TXFRS);//æ¸æ¥å¯åå¨æ å¿ä½
			frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; //è·å¾æ¥æ¶å°çæ°æ®é¿åº¦
			dwt_readrxdata(rx_buffer, frame_len, 0); //è¯»åæ¥æ¶æ°æ®
			dwt_setrxtimeout(0);//DELAY_BETWEEN_TWO_FRAME_UUS*(nearbase_num+1-recbase_num)+10);//è®¾å®æ¥æ¶è¶æ¶æ¶é´ï¼0ä½æ²¡æè¶æ¶æ¶é´
			dwt_rxenable(0);//æå¼æ¥æ¶
			rflosttimer = 0;
			if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)&&rx_buffer[GROUP_ID_IDX] == group_id) //å¤ææ¥æ¶å°çæ°æ®æ¯å¦æ¯responseæ°æ®
			{ u16 rec_nearbaseid,rec_nearbasepos;
			int32_t temp_dist;
			@@ -421,22 +439,22 @@
			memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);


			if(g_com_map[DEV_ROLE]!=0)
			{
			sync_timer=temp_sync_timer1;
			//// memcpy(&tagslotpos,&rx_buffer[TAGSLOTPOS],2);
			// if(g_com_map[DEV_ROLE]!=0)
			// {
			// sync_timer=temp_sync_timer1;
			////// memcpy(&tagslotpos,&rx_buffer[TAGSLOTPOS],2);
			////
			//

			tmp_time=tmp_time+650;
			if(tmp_time>999)
			{
			tmp_time-=999;
			sync_timer++;
			if(sync_timer>=1000)
			{sync_timer=0;}
			}
			TIM3->CNT=tmp_time;
			}
			// tmp_time=tmp_time+650;
			// if(tmp_time>999)
			// {
			// tmp_time-=999;
			// sync_timer++;
			// if(sync_timer>=1000)
			// {sync_timer=0;}
			// }
			// TIM3->CNT=tmp_time;
			// }
			current_slotnum=temp_sync_timer1/g_com_map[COM_INTERVAL];
			// if(tagslotpos>max_slotpos)
			// tagslotpos=tagslotpos%(max_slotpos+1);
			@@ -462,30 +480,35 @@
			tx_nearfinal_msg[MESSAGE_TYPE_IDX]=NEAR_FINAL;
			tx_nearfinal_msg[GROUP_ID_IDX] = group_id;
			dwt_writetxdata(28+nearbase_num*4, tx_nearfinal_msg, 0);//å°åéæ°æ®åå¥DW1000
			dwt_writetxfctrl(28+nearbase_num*4, 0);//è®¾å®åéæ°æ®é¿åº¦
			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)
			ClearUWBUserData();
			uwbrec_userdatalen = rx_buffer[RESPONSE_USERDATA_IDX];
			if(uwbrec_userdatalen<=USERDATA_MAXLEN)
			{
			memcpy(uwbrec_userdata,&rx_buffer[RESPONSE_USERDATA_IDX+1],uwbrec_userdatalen);
			}
			if(temp_dist!=0x1ffff )
			{
			g_Resttimer=0;
			IWDG_Feed();
			#ifdef USART_SINGLE_OUTPUT
			usart_send[2] = 1;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 17;//æ°æ®æ®µé¿åº¦
			usart_send[3] = 29;//æ°æ®æ®µé¿åº¦
			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);
			memcpy(&usart_send[17],selftest,2);
			memcpy(&usart_send[21],uwbrec_userdata,10);
			checksum = Checksum_u16(&usart_send[2],29);
			memcpy(&usart_send[31],&checksum,2);
			UART_PushFrame(usart_send,33);
			heartbeat_timer = 0;
			#endif

			//dwt_setdelayedtrxtime(final_tx_time);//è®¾ç½®finalååéæ¶é´T5
			@@ -506,6 +529,12 @@

			memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
			nearbase_distlist[rec_nearbasepos]=temp_dist;
			ClearUWBUserData();
			uwbrec_userdatalen = rx_buffer[RESPONSE_USERDATA_IDX];
			if(uwbrec_userdatalen<=USERDATA_MAXLEN)
			{
			memcpy(uwbrec_userdata,&rx_buffer[RESPONSE_USERDATA_IDX+1],uwbrec_userdatalen);
			}

			if(temp_dist!=0x1ffff)
			exsistbase_list[rec_nearbasepos]=KEEP_TIMES;
			@@ -516,26 +545,26 @@
			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[3] = 29;//æ°æ®æ®µé¿åº¦
			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);
			memcpy(&usart_send[17],selftest,2);
			memcpy(&usart_send[21],uwbrec_userdata,10);
			checksum = Checksum_u16(&usart_send[2],29);
			memcpy(&usart_send[31],&checksum,2);
			UART_PushFrame(usart_send,33);
			heartbeat_timer = 0;
			#endif

			//dwt_setdelayedtrxtime(final_tx_time);//è®¾ç½®finalååéæ¶é´T5
			@@ -547,7 +576,15 @@
			}else{
			dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG\| SYS_STATUS_ALL_RX_ERR);
			if(flag_rxon)
			{dwt_rxenable(0);
			{
			dwt_rxenable(0);
			if(g_com_map[MODBUS_ID1]==1)
			{
			#ifdef DBGMSG_OUTPUT
			dbgmsg_len = sprintf(dbgmsg,"%s status_reg:%x",__debug_info__,status_reg);
			SendDbgMsg(dbgmsg,dbgmsg_len);
			#endif
			}
			}
			}
			}
			@@ -571,9 +608,10 @@
			// temp_adc=Get_ADC_Value();
			// random_value=random_value\|((temp_adc&0x01)<<i);
			// }
			pollsendtimer+=time32_incr%11;
			tagslotpos=GetRandomSlotPos(rec_tagpos_binary\|tagpos_binary);
			tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
			tag_state=NEARPOLL;
			tag_state=NEARPOLL;
			}
			// tyncpoll_time=0;
			next_nearbase_num=0;
			@@ -729,7 +767,6 @@
			NearPoll();
			break;
			}

			SetNextPollTime(tyncpoll_time);
			g_start_send_flag = 0;
			GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_RESET);
			@@ -868,9 +905,11 @@
			u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,getrange_success=0;
			int32_t filter_dist,filter_speed;
			u8 newmeasure,recpoll_len;
			u8 motorstate=0;
			u8 tagpos_rec[50],tagpos_send[50],ancidlist_num;
			u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase 1 first near_base
			{
			u8 motorstate;

			tmp_time=TIM3->CNT;
			memcpy(&tx_nearresp_msg[ANCTIMEMS],&sync_timer,2);
			memcpy(&tx_nearresp_msg[ANCTIMEUS],&tmp_time,2);
			@@ -908,29 +947,39 @@
			motorstate =0;
			}
			}
			tx_nearresp_msg[GROUP_ID_IDX] = group_id;
			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]=motorstate;//(remotesend_state<<4)\|motorstate;
			tx_nearresp_msg[MOTORSTATE_INDEX]&=0x0f;
			if(usartrec_userdatalen<=USERDATA_MAXLEN)
			{tx_nearresp_msg[RESPONSE_USERDATA_IDX] = usartrec_userdatalen;
			memcpy(&tx_nearresp_msg[RESPONSE_USERDATA_IDX+1],uartrec_userdata,usartrec_userdatalen);
			}
			// if(remotesend_state)
			// {
			// memcpy(&tx_nearresp_msg[REMOTEPARA_INDEX],remotetag_para,REMOTEPARA_LEN);
			// dwt_writetxdata(22+REMOTEPARA_LEN, tx_nearresp_msg, 0);//åå¥åéæ°æ®
			// dwt_writetxfctrl(22+REMOTEPARA_LEN, 0);//è®¾å®åéé¿åº¦
			// }else{
			dwt_writetxdata(24, tx_nearresp_msg, 0);//åå¥åéæ°æ®
			dwt_writetxfctrl(24, 0);//è®¾å®åéé¿åº¦
			dwt_writetxdata(24+11, tx_nearresp_msg, 0);//åå¥åéæ°æ®
			dwt_writetxfctrl(24+11, 0);//è®¾å®åéé¿åº¦
			// }
			result = dwt_starttx(DWT_START_TX_DELAYED \| DWT_RESPONSE_EXPECTED);//å»¶è¿åéï¼çå¾æ¥æ¶


			if(result==0)
			if(result==DWT_SUCCESS)
			{
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR))&&!g_start_sync_flag)///ä¸ææ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæèåºç°éè¯¯
			{ };
			}else{
			result++;
			if(g_com_map[MODBUS_ID1]==1)
			{
			#ifdef DBGMSG_OUTPUT
			dbgmsg_len = sprintf(dbgmsg,"%s status_reg:%x",__debug_info__,status_reg);
			SendDbgMsg(dbgmsg,dbgmsg_len);
			#endif
			}
			}

			if (status_reg & SYS_STATUS_RXFCG)//æ¥æ¶æå
			@@ -1016,8 +1065,9 @@
			g_Tagdist[taglist_pos]= 0x2ffff;
			}
			#ifdef USART_SINGLE_OUTPUT

			usart_send[2] = 1;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 17;//æ°æ®æ®µé¿åº¦
			usart_send[3] = 29;//æ°æ®æ®µé¿åº¦
			usart_send[4] = frame_seq_nb2;//æ°æ®æ®µé¿åº¦
			memcpy(&usart_send[5],&tag_id_recv,2);
			memcpy(&usart_send[7],&dev_id,2);
			@@ -1028,11 +1078,12 @@
			usart_send[14] = button;
			usart_send[15] = firstpath_power;
			usart_send[16] = rx_power;
			memcpy(&usart_send[19],selftest,2);
			memcpy(&usart_send[21],uartrec_userdata,10);
			memcpy(&usart_send[17],selftest,2);
			memcpy(&usart_send[21],uwbrec_userdata,10);
			checksum = Checksum_u16(&usart_send[2],29);
			memcpy(&usart_send[31],&checksum,2);
			UART_PushFrame(usart_send,33);
			heartbeat_timer = 0;
			#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);
			@@ -1046,12 +1097,19 @@
			}
			}else{
			//printf("%x/n",status_reg);
			if(g_com_map[MODBUS_ID1]==1)
			{
			#ifdef DBGMSG_OUTPUT
			dbgmsg_len = sprintf(dbgmsg,"%s status_reg:%x",__debug_info__,status_reg);
			SendDbgMsg(dbgmsg,dbgmsg_len);
			#endif
			}
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			}
			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],rec_ancidlist[20],rec_ancdistlist[20];
			void Anchor_App(void)
			{
			@@ -1076,7 +1134,7 @@
			{ u16 tag_recv_interval;
			float temp_tagpos;


			rflosttimer = 0;
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//æ¸é¤æ å¿ä½
			frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//è·å¾æ¥æ¶æ°æ®é¿åº¦
			recpoll_len = frame_len;
			@@ -1141,7 +1199,11 @@
			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[KUIXIANLEN_INDEX],2);
			ClearUWBUserData();
			uwbrec_userdatalen = rx_buffer[POLL_USERDATA_IDX];
			if(uwbrec_userdatalen<=20)
			memcpy(uwbrec_userdata,&rx_buffer[POLL_USERDATA_IDX+1],uwbrec_userdatalen);
			#ifdef USART_ALL_OUTPUT
			usart_send[2] = 0x0c;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 8+rec_nearbase_num*4;//æ°æ®æ®µé¿åº¦
			@@ -1198,9 +1260,15 @@

			break;
			}
			}else{
			}
			}else{
			#ifdef DBGMSG_OUTPUT
			if((status_reg&SYS_STATUS_ALL_RX_ERR)&&g_com_map[MODBUS_ID1]==1)
			{ dbgmsg_len = sprintf(dbgmsg,"%s status_reg:%x",__debug_info__,status_reg);
			SendDbgMsg(dbgmsg,dbgmsg_len);
			}
			#endif
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			}
			}