UWB_SmallModule_F103.git

			@@ -115,6 +115,7 @@
			}
			}
			}
			extern u16 g_commap_antdelay;
			void Dw1000_Init(void)
			{
			/* Reset and initialise DW1000.
			@@ -131,12 +132,13 @@

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

			/* 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. */
			//è®¾ç½®æ¥æ¶è¶æ¶æ¶é´
			}
			extern u16 group_id;
			void Dw1000_App_Init(void)
			{
			//g_com_map[DEV_ID] = 0x0b;
			@@ -145,6 +147,10 @@
			tx_final_msg[MESSAGE_TYPE_IDX]=FINAL;
			tx_sync_msg[MESSAGE_TYPE_IDX]=SYNC;

			memcpy(&tx_poll_msg[GROUP_ID_IDX], &group_id, 1);
			memcpy(&tx_final_msg[GROUP_ID_IDX], &group_id, 1);
			memcpy(&tx_resp_msg[GROUP_ID_IDX], &group_id, 1);

			memcpy(&tx_poll_msg[TAG_ID_IDX], &dev_id, 2);
			memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 2);
			memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 2);
			@@ -333,6 +339,7 @@
			last_nearbase_num=next_nearbase_num;
			nearbase_num=next_nearbase_num;
			recbase_num=0;
			tx_nearpoll_msg[GROUP_ID_IDX] = group_id;
			tx_nearpoll_msg[BATTARY_IDX] = Get_Battary();
			tx_nearpoll_msg[BUTTON_IDX] = !READ_KEY0;
			tx_nearpoll_msg[SEQUENCE_IDX] = frame_seq_nb++;
			@@ -390,7 +397,7 @@
			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);//æå¼æ¥æ¶
			if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //å¤ææ¥æ¶å°çæ°æ®æ¯å¦æ¯responseæ°æ®
			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;
			poll_tx_ts = get_tx_timestamp_u64(); //è·å¾POLLåéæ¶é´T1
			@@ -451,6 +458,7 @@
			final_msg_set_ts(&tx_nearfinal_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX], resp_rx_ts);
			final_msg_set_ts(&tx_nearfinal_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
			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);//è®¾å®åéæ°æ®é¿åº¦
			memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
			@@ -829,8 +837,11 @@
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			}
			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 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase 1 first near_base
			{
			u8 motorstate;
			@@ -866,6 +877,7 @@
			}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;
			@@ -899,12 +911,13 @@
			{
			return 1;
			}
			if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,2)) //å¤ææ¯å¦ä¸ºFinalå
			if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,2)&&group_id==rx_buffer[GROUP_ID_IDX]) //å¤ææ¯å¦ä¸º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;
			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
			@@ -930,17 +943,27 @@
			LED0_BLINK; //æ¯æåä¸æ¬¡éè®¯åéªçä¸æ¬¡
			dis_after_filter=dist_cm;
			hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10;
			for(si=0;si<10;si++)
			{
			if(smallcar_idlist[si]==tag_id_recv)
			{
			hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]10-(int16_t)g_com_map[NOMOVESLEEP_TIME]10;
			}
			}
			g_flag_Taggetdist[taglist_pos]=0;

			if(hex_dist>-1000&&hex_dist<2000000)
			{
			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;
			filter_dist = pos_predict[taglist_pos]/10;
			filter_speed = vel_predict[taglist_pos]/10;
			newmeasure = 1;
			#else
			filter_dist=hex_dist/10;
			#endif
			@@ -954,8 +977,8 @@
			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]=0;
			if(tagdist_list[taglist_pos]<=0)
			tagdist_list[taglist_pos]=10;
			memcpy(&usart_send[9],&tagdist_list[taglist_pos],4);
			usart_send[13] = battary;
			usart_send[14] = button;
			@@ -1012,6 +1035,8 @@
			memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
			//å°æ¶å°çtag_idåå«åå¥åæ¬¡éè®¯çåä¸ï¼ä¸ºå¤æ ç¾éè®¯æå¡ï¼é²æ¢ä¸æ¬¡éè®¯ä¸æ¥æ¶å°ä¸åIDæ ç¾çæ°æ®
			memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
			if(group_id==rx_buffer[GROUP_ID_IDX])
			{
			switch(rx_buffer[MESSAGE_TYPE_IDX])
			{
			// case POLL:
			@@ -1127,4 +1152,5 @@
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			}
			}