UWB_SmallModule_F103.git

			@@ -30,6 +30,8 @@
			#include "beep.h"
			#include "modbus.h"
			//#define DEBUG_OUTPUT
			#define TDFILTER
			#define CONSTANT_FILTER
			/------------------------------------ Marcos ------------------------------------------/
			/* Inter-ranging delay period, in milliseconds. */
			#define RNG_DELAY_MS 100
			@@ -77,6 +79,8 @@
			#define BATTARY_IDX 15
			#define BUTTON_IDX 16
			#define SEQUENCE_IDX 17
			#define DISTNUM_INDEX 18
			#define DISTARRAY_INDEX 19
			//respose
			#define DIST_IDX 10
			#define ANCTIMEMS 14
			@@ -342,14 +346,43 @@
			// min_power = - 10 * log10((F1 F1 + F2 F2 + F3 * F3) / (C *B));
			return min_power;
			}
			uint16_t g_Resttimer;
			#define CONSTANT_LEN 50
			extern u16 dist_threshold;
			int32_t ConstantFilter(int32_t currentdist,u8 channel)
			{
			static int32_t cs_lastdist[10],cs_lastvalid[10];
			static u8 cfstart_flag[10] = {1};
			static u8 constant_count[10] = {100};
			if(cfstart_flag[channel])
			{
			cfstart_flag[channel] = 0;
			cs_lastdist[channel] = currentdist;
			}
			if( abs(currentdist - cs_lastdist[channel])<100)
			{
			if(constant_count[channel]<CONSTANT_LEN)
			constant_count[channel]++;
			}else{
			constant_count[channel] = 0;
			}
			if(constant_count[channel] == CONSTANT_LEN)
			{
			cs_lastvalid[channel] = currentdist;
			}
			cs_lastdist[channel] = currentdist;
			return cs_lastvalid[channel];
			}
			#define TAGARRAY_LEN 20
			uint16_t g_Resttimer,tag_IDarray[TAGARRAY_LEN];
			uint8_t result;
			u8 tag_succ_times=0;
			int32_t hex_dist,hex_dist2;
			int32_t hex_dist,hex_dist2,tag_distarray[TAGARRAY_LEN];
			u16 checksum;
			int8_t tag_delaytime;
			extern uint16_t sync_timer;
			u16 tmp_time;
			u32 getdatatimer[TAGARRAY_LEN];
			#define DATALOSTTIME 2000
			extern float dw_vbat;
			extern u16 slottime,max_slotnum,current_slotpos,tyncpoll_time;
			void Tag_App(void)//åéæ¨¡å¼(TAGæ ç¾)
			@@ -370,14 +403,24 @@
			tx_poll_msg[BATTARY_IDX] = bat_percent;//Get_Battary();
			tx_poll_msg[BUTTON_IDX] = !READ_KEY0;
			tx_poll_msg[SEQUENCE_IDX] = frame_seq_nb++;
			tx_poll_msg[DISTNUM_INDEX] = g_com_map[MAX_REPORT_ANC_NUM];
			for(i=0;i<g_com_map[MAX_REPORT_ANC_NUM];i++)
			{
			if(time32_incr-getdatatimer[i]>DATALOSTTIME)
			{
			tag_distarray[i] = 0x1ffff;
			}
			}
			memcpy(&tx_poll_msg[DISTARRAY_INDEX],tag_IDarray,2*g_com_map[MAX_REPORT_ANC_NUM]);
			memcpy(&tx_poll_msg[DISTARRAY_INDEX+2g_com_map[MAX_REPORT_ANC_NUM]],tag_distarray,4g_com_map[MAX_REPORT_ANC_NUM]);
			GPIO_WriteBit(GPIOA, GPIO_Pin_9, Bit_RESET);
			for(i=0;i<g_com_map[MAX_REPORT_ANC_NUM];i++)
			{
			/* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
			tx_poll_msg[ANC_TYPE_IDX] = i;

			dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);//å°Pollåæ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå»
			dwt_writetxfctrl(sizeof(tx_poll_msg), 0);//è®¾ç½®è¶å®½å¸¦åéæ°æ®é¿åº¦
			dwt_writetxdata(22+6*g_com_map[MAX_REPORT_ANC_NUM], tx_poll_msg, 0);//å°Pollåæ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå»
			dwt_writetxfctrl(22+6*g_com_map[MAX_REPORT_ANC_NUM], 0);//è®¾ç½®è¶å®½å¸¦åéæ°æ®é¿åº¦

			/* Start transmission, indicating that a response is expected so that reception is enabled automatically after the frame is sent and the delay
			* set by dwt_setrxaftertxdelay() has elapsed. */
			@@ -414,7 +457,7 @@
			/* 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[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //å¤ææ¥æ¶å°çæ°æ®æ¯å¦æ¯responseæ°æ®
			if ((rx_buffer[GROUP_ID_IDX]==0\|\|rx_buffer[GROUP_ID_IDX] == group_id\|\|group_id==0)&&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
			@@ -435,7 +478,9 @@
			}
			TIM3->CNT=tmp_time;
			}
			memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);
			memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);
			memcpy(&tag_distarray[i*4], &rx_buffer[DIST_IDX], 4);
			memcpy(&tag_IDarray[i*2],&rx_buffer[ANCHOR_ID_IDX],2);
			rec_firstpath_power = rx_buffer[SIGNALPOWER];
			memcpy(&tx_final_msg[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 4);
			memcpy(&rec_com_interval,&rx_buffer[ANCSEND_INTERVAL], 2);
			@@ -468,7 +513,7 @@
			#ifdef DEBUG_OUTPUT
			printf("Fååé,åºç«ID: %d .\r\n",i);
			#endif

			getdatatimer[i] = time32_incr;
			tag_succ_times++;

			LED0_BLINK;
			@@ -540,10 +585,12 @@

			int8_t correction_time;
			extern uint8_t sync_seq;
			#define TDFILTER

			//#define CHECK_UID
			extern uint8_t UID_ERROR;
			extern u16 dist_threshold;
			u16 ancrec_IDarray[TAGARRAY_LEN];
			u32 ancrec_distarray[TAGARRAY_LEN];
			u32 ancrec_distnum;
			u8 misdist_num[TAG_NUM_IN_SYS];
			void Anchor_App(void)
			{
			@@ -587,7 +634,7 @@
			// tag_recv_interval = tag_recv_timer + 65535 - tag_time_recv[tag_id_recv];
			// }

			if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == POLL&&(anchor_type == rx_buffer[ANC_TYPE_IDX])) //å¤ææ¯å¦æ¯pollåæ°æ®
			if ((rx_buffer[GROUP_ID_IDX]==0\|\|rx_buffer[GROUP_ID_IDX] == group_id\|\|group_id==0)&&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);
			@@ -598,7 +645,9 @@

			/* Retrieve poll reception timestamp. */
			poll_rx_ts = get_rx_timestamp_u64();//è·å¾Pollåæ¥æ¶æ¶é´T2

			ancrec_distnum = rx_buffer[DISTNUM_INDEX];
			memcpy(ancrec_IDarray,&rx_buffer[DISTARRAY_INDEX],2*g_com_map[MAX_REPORT_ANC_NUM]);
			memcpy(ancrec_distarray,&rx_buffer[DISTARRAY_INDEX+2g_com_map[MAX_REPORT_ANC_NUM]],4g_com_map[MAX_REPORT_ANC_NUM]);
			/* Set send time for response. See NOTE 8 below. */
			resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//è®¡ç®Responseåéæ¶é´T3ã
			dwt_setdelayedtrxtime(resp_tx_time);//è®¾ç½®Responseåéæ¶é´T3
			@@ -645,7 +694,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[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å
			if ((rx_buffer[GROUP_ID_IDX]==0\|\|rx_buffer[GROUP_ID_IDX] == group_id\|\|group_id==0)&&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;
			@@ -694,7 +743,7 @@
			if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
			{
			if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])<dist_threshold\|\|misdist_num[tag_id_recv-TAG_ID_START]>4)
			{
			{u8 i;
			int32_t filter_dist;
			misdist_num[tag_id_recv-TAG_ID_START]=0;
			if(hex_dist<1000000&&hex_dist>-10000)
			@@ -705,31 +754,63 @@
			#else
			filter_dist=hex_dist/10;
			#endif

			#ifdef CONSTANT_FILTER
			filter_dist = ConstantFilter(filter_dist,tag_id_recv-TAG_ID_START);
			#endif
			anchor_dist_last_frm[tag_id_recv-TAG_ID_START]=filter_dist;
			g_Tagdist[tag_id_recv]= filter_dist;



			his_dist[tag_id_recv-TAG_ID_START]=hex_dist;
			g_flag_Taggetdist[tag_id_recv]=0;
			if(!g_com_map[MODBUS_MODE])
			{
			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);

			memcpy(&usart_send[9],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
			usart_send[13] = battary;
			usart_send[14] = button;
			usart_send[15] = firstpath_power;
			checksum = Checksum_u16(&usart_send[2],17);
			memcpy(&usart_send[19],&checksum,2);
			UART_PushFrame(usart_send,21);
			{
			for(i=0;i<g_com_map[MAX_REPORT_ANC_NUM];i++)
			{
			if(ancrec_distarray[i] !=0x1ffff)
			{
			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],&ancrec_IDarray[i],2);

			memcpy(&usart_send[9],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
			usart_send[13] = battary;
			usart_send[14] = button;
			usart_send[15] = firstpath_power;
			checksum = Checksum_u16(&usart_send[2],17);
			memcpy(&usart_send[19],&checksum,2);
			UART_PushFrame(usart_send,21);

			}
			}
			// 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);
			//
			// memcpy(&usart_send[9],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
			// usart_send[13] = battary;
			// usart_send[14] = button;
			// usart_send[15] = firstpath_power;
			// checksum = Checksum_u16(&usart_send[2],17);
			// memcpy(&usart_send[19],&checksum,2);
			// UART_PushFrame(usart_send,21);
			}
			// memcpy(&Modbus_HoldReg[tag_id_recv*2],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
			Modbus_HoldReg[tag_id_recv*2]=g_Tagdist[tag_id_recv-TAG_ID_START]>>16;
			Modbus_HoldReg[tag_id_recv*2+1]=g_Tagdist[tag_id_recv-TAG_ID_START];
			//dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm);

			// Modbus_HoldReg[tag_id_recv*2]=g_Tagdist[tag_id_recv-TAG_ID_START]>>16;
			// Modbus_HoldReg[tag_id_recv*2+1]=g_Tagdist[tag_id_recv-TAG_ID_START];
			for(i=0;i<g_com_map[MAX_REPORT_ANC_NUM];i++)
			{
			Modbus_HoldReg[tag_id_recv3g_com_map[MAX_REPORT_ANC_NUM]+3*i] = ancrec_IDarray[i];
			Modbus_HoldReg[tag_id_recv3g_com_map[MAX_REPORT_ANC_NUM]+3*i+1] = ancrec_distarray[i]>>16;
			Modbus_HoldReg[tag_id_recv3g_com_map[MAX_REPORT_ANC_NUM]+3*i+2] = ancrec_distarray[i];
			}

			}

			}