UWB_SmallModule_F103.git

			@@ -313,10 +313,11 @@
			}
			return max_slotpos-1;
			}
			u8 test=0;
			u8 anclost_times=0;
			u8 test=0,dissucc_times,lastdist1,lastdist2;
			u8 anclost_times=0,usartoutput_count;
			u16 usartoutput_notenough=0;
			u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0,rec_extratag_battary;
			u16 temp_sync_timer1,temp_sync_timer2,rec_extratag_id;
			u16 temp_sync_timer1,temp_sync_timer2,rec_extratag_id,success_ancid;
			int32_t rec_extratag_dist;
			void NearPoll(void)
			{
			@@ -330,6 +331,7 @@
			dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); //è®¾ç½®åéåå¼å¯æ¥æ¶ï¼å¹¶è®¾å®å»¶è¿æ¶é´
			dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
			tag_succ_times = 0;
			usartoutput_count = 0;
			GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_SET);
			if(next_nearbase_num>=MAX_NEARBASE_NUM)
			{
			@@ -421,23 +423,23 @@
			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;
			}
			current_slotnum=temp_sync_timer1/g_com_map[COM_INTERVAL];
			// 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);
			// tyncpoll_time=(tagslotpos-1)*slottime;
			@@ -470,44 +472,40 @@
			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)
			{
			if(temp_dist!=0x1ffff)//æ²¡ææåæµè·ï¼ä¸è·ç¦»åä¸ä¸æ¬¡ä¸ç¸ç
			{
			if(success_ancid!=rec_nearbaseid&&dissucc_times<2)
			{
			success_ancid = rec_nearbaseid;
			dissucc_times++;



			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[7],&rec_nearbaseid,2);
			memcpy(&usart_send[9],&temp_dist,4);
			usart_send[13] = rx_buffer[DIST_IDX+3];
			usart_send[14] = button;
			checksum = Checksum_u16(&usart_send[2],17);
			memcpy(&usart_send[19],&checksum,2);
			UART_PushFrame(usart_send,21);
			usartoutput_count++;
			#endif
			}

			if(rec_extratag_dist!=0x1ffff&&rec_extratag_dist!=0&&rx_buffer[GROUP_ID_IDX]!=supergroupid)
			{
			#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],&rec_extratag_id,2);
			memcpy(&usart_send[7],&rec_nearbaseid,2);
			}else{
			memcpy(&usart_send[5],&rec_nearbaseid,2);
			memcpy(&usart_send[7],&rec_extratag_id,2);
			}
			memcpy(&usart_send[7],&rec_nearbaseid,2);
			memcpy(&usart_send[9],&rec_extratag_dist,4);
			usart_send[13] = rx_buffer[EXTRA_TAGDIST_INDEX+3];
			usart_send[14] = button;
			@@ -516,7 +514,8 @@
			UART_PushFrame(usart_send,21);
			#endif
			}

			}
			}

			}else{
			rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
			@@ -543,8 +542,13 @@
			LED0_BLINK;
			// memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
			// tagslotpos=GetRandomSlotPos(rec_tagpos_binary);
			if(temp_dist!=0x1ffff)
			{
			if(temp_dist!=0x1ffff)//æ²¡ææåæµè·ï¼ä¸è·ç¦»åä¸ä¸æ¬¡ä¸ç¸ç
			{
			if(success_ancid!=rec_nearbaseid&&dissucc_times<2)
			{
			success_ancid = rec_nearbaseid;
			dissucc_times++;

			#ifdef USART_SINGLE_OUTPUT
			usart_send[2] = 1;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 17;//æ°æ®æ®µé¿åº¦
			@@ -563,12 +567,13 @@
			checksum = Checksum_u16(&usart_send[2],17);
			memcpy(&usart_send[19],&checksum,2);
			UART_PushFrame(usart_send,21);
			usartoutput_count++;
			#endif


			//dwt_setdelayedtrxtime(final_tx_time);//è®¾ç½®finalååéæ¶é´T5
			// result=dwt_starttx(DWT_START_TX_DELAYED);//è®¾å®ä¸ºå»¶è¿åé
			//dwt_writetxdata(4,&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)4], FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)4);//å°åéæ°æ®åå¥DW1000
			}

			if(rec_extratag_dist!=0x1ffff&&rec_extratag_dist!=0&&rx_buffer[GROUP_ID_IDX]!=supergroupid)
			{
			#ifdef USART_SINGLE_OUTPUT
			@@ -591,6 +596,8 @@
			UART_PushFrame(usart_send,21);
			#endif
			}
			}
			}
			}
			}
			}else{
			@@ -699,6 +706,16 @@
			// }else{
			// anclost_times=0;
			// }
			if(usartoutput_count>=2)
			{
			usartoutput_notenough = 0;
			}else{
			sync_timer += time32_incr%2;
			if(usartoutput_notenough++>500)
			{
			SCB->AIRCR = 0X05FA0000\|(unsigned int)0x04; //è½¯å¤ä½åå°bootloader
			}
			}
			dwt_forcetrxoff();
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR\| SYS_STATUS_TXFRS \|SYS_STATUS_RXFCG);
			}
			@@ -779,7 +796,7 @@
			break;
			}

			SetNextPollTime(tyncpoll_time);
			// SetNextPollTime(tyncpoll_time);
			g_start_send_flag = 0;
			GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_RESET);
			}
			@@ -790,6 +807,7 @@
			u16 tagid_list[TAG_NUM_IN_SYS];
			u8 tagofflinetime[TAG_NUM_IN_SYS];
			int32_t tagdist_list[TAG_NUM_IN_SYS];
			u8 tagseq_list[TAG_NUM_IN_SYS],tagnewdist_list[TAG_NUM_IN_SYS];
			void TagListUpdate(void)
			{
			u16 i,j=0,temp[TAG_NUM_IN_SYS];
			@@ -943,15 +961,18 @@
			}else{
			motorstate =0;
			}

			if(new_tagid)
			{
			tagdist_list[taglist_pos]=0x1ffff;
			memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
			}else{
			if(tagnewdist_list[taglist_pos])
			{
			tagnewdist_list[taglist_pos] = 0;
			memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
			}
			}
			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]=0;//(remotesend_state<<4)\|motorstate;
			@@ -1001,7 +1022,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;
			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
			@@ -1047,13 +1068,13 @@
			#ifdef TDFILTER
			NewTrackingDiffUpdate(taglist_pos, (float)hex_dist);
			filter_dist = pos_predict[taglist_pos]/10;
			filter_speed = vel_predict[taglist_pos]/10;
			//filter_speed = vel_predict[taglist_pos];
			newmeasure = 1;
			#else
			filter_dist=hex_dist/10;
			#endif
			misdist_num[taglist_pos]=0;
			tagdist_list[taglist_pos] = filter_dist;
			tagdist_list[taglist_pos] = filter_dist;
			his_dist[taglist_pos]=hex_dist;
			if(hex_dist>0)
			{
			@@ -1061,6 +1082,8 @@
			}else{
			g_Tagdist[taglist_pos]= 1;
			}
			tagnewdist_list[taglist_pos] = 1;
			tagseq_list[taglist_pos] = frame_seq_nb2;
			#ifdef USART_SINGLE_OUTPUT
			usart_send[2] = 1;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 17;//æ°æ®æ®µé¿åº¦
			@@ -1070,7 +1093,7 @@
			if(tagdist_list[taglist_pos]<=0)
			tagdist_list[taglist_pos]=10;
			memcpy(&usart_send[9],&tagdist_list[taglist_pos],4);
			tagdist_list[taglist_pos] =tagdist_list[taglist_pos]\| (battary<<24);
			tagdist_list[taglist_pos] =tagdist_list[taglist_pos]\| (battary<<24);
			usart_send[13] = battary;
			usart_send[14] = button;
			usart_send[15] = firstpath_power;