UWB_SmallModule_F103.git

			@@ -7,7 +7,7 @@
			NEARPOLL,
			}tag_state=GETNEARMSG;
			static dwt_config_t config = {
			2, /* Channel number. */
			5, /* Channel number. */
			DWT_PRF_64M, /* Pulse repetition frequency. */
			DWT_PLEN_64, /* Preamble length. */
			DWT_PAC8, /* Preamble acquisition chunk size. Used in RX only. */
			@@ -129,7 +129,7 @@
			/* Configure DW1000. See NOTE 6 below. */
			dwt_configure(&config);//éç½®DW1000


			dwt_setinterrupt( DWT_INT_RFCG \| (DWT_INT_ARFE \| DWT_INT_RFSL \| DWT_INT_SFDT \| DWT_INT_RPHE \| DWT_INT_RFCE \| DWT_INT_RFTO \| DWT_INT_RXPTO), 1);

			/* Apply default antenna delay value. See NOTE 1 below. */
			dwt_setrxantennadelay(RX_ANT_DLY); //è®¾ç½®æ¥æ¶å¤©çº¿å»¶è¿
			@@ -840,6 +840,7 @@
			}
			return i;
			}
			u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse,motorstate=0;
			uint32_t frame_len;
			uint32_t resp_tx_time;
			uint8_t rec_nearbase_num,anc_report_num;
			@@ -905,11 +906,44 @@
			LED0_BLINK; //æ¯æåä¸æ¬¡éè®¯åéªçä¸æ¬¡
			dis_after_filter=dist_cm;
			hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
			if(abs(hex_dist-his_dist[taglist_pos])<1000)
			{
			tagdist_list[taglist_pos] = hex_dist;
			}
			his_dist[taglist_pos]=hex_dist;
			if(hex_dist>-1000&&hex_dist<200000)
			{
			g_flag_Taggetdist[taglist_pos]=0;
			if(abs(hex_dist-his_dist[taglist_pos])<1500\|\|misdist_num[taglist_pos]>3)
			{
			misdist_num[taglist_pos]=0;
			tagdist_list[taglist_pos] = hex_dist;
			his_dist[taglist_pos]=hex_dist;
			g_Tagdist[taglist_pos]=hex_dist;
			#ifndef USART_INTEGRATE_OUTPUT
			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],&tagdist_list[taglist_pos],4);
			usart_send[13] = battary;
			usart_send[14] = button;
			usart_send[15] = firstpath_power;
			usart_send[16] = (rx_power-firstpath_power)*10;
			usart_send[17] = tag_frequency;
			usart_send[18] = tag_slotpos;
			checksum = Checksum_u16(&usart_send[2],17);
			memcpy(&usart_send[19],&checksum,2);
			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
			#ifdef DEBUG_INF
			printf("åºå·ï¼%d\r\n",frame_seq_nb2);
			#endif
			}else{
			// printf("%d",hex_dist);
			misdist_num[taglist_pos]++;
			}
			}
			}
			}else{
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			@@ -919,7 +953,7 @@
			extern uint16_t configremotetagID;
			extern u8 remotetag_paralen;
			u8 finalsend=0;
			u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse,motorstate=0;
			u16 signalpower_list[TAG_NUM_IN_SYS];
			u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase 1 first near_base
			{

			@@ -939,11 +973,10 @@

			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);
			tagdist_list[taglist_pos]=0x1ffff;
			}
			memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
			memcpy(&tx_nearresp_msg[ANC_SIGNALPOWER],&signalpower_list[taglist_pos],2);
			tx_nearresp_msg[MAINBASE_INDEX]=flag_syncbase;
			tx_nearresp_msg[MESSAGE_TYPE_IDX]=NEAR_RESPONSE;
			tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)\|motorstate;
			@@ -970,12 +1003,15 @@
			break;
			}
			}
			}else{

			}

			if(finalsend == 0)
			{
			dwt_writetxdata(24, tx_nearresp_msg, 0);//åå¥åéæ°æ®
			dwt_writetxfctrl(24, 0);//è®¾å®åéé¿åº¦
			memcpy(&tx_nearresp_msg[MAXRANGE_DISTANCE],&g_com_map[MAXRANGDIST_INDEX],2);
			dwt_writetxdata(28, tx_nearresp_msg, 0);//åå¥åéæ°æ®
			dwt_writetxfctrl(28, 0);//è®¾å®åéé¿åº¦
			}

			// if(remotesend_state&&tag_id_recv==configremotetagID)
			@@ -1025,7 +1061,7 @@
			poll_rx_ts_32 = (uint32_t)poll_rx_ts;//ä½¿ç¨32ä½æ°æ®è®¡ç®
			resp_tx_ts_32 = (uint32_t)resp_tx_ts;
			final_rx_ts_32 = (uint32_t)final_rx_ts;
			time_monitor[1] = sync_timer*1000+TIM3->CNT;
			// time_monitor[1] = sync_timer*1000+TIM3->CNT;
			Ra = (double)(resp_rx_ts - poll_tx_ts);//Tround1 = T4 - T1
			Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);//Tround2 = T6 - T3
			Da = (double)(final_tx_ts - resp_rx_ts);//Treply2 = T5 - T4
			@@ -1035,13 +1071,13 @@
			distance = tof * SPEED_OF_LIGHT;//è·ç¦»=åé*é£è¡æ¶é´
			dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //è·ç¦»åå»ç«æ£ç³»æ°
			dist_cm = dist_no_bias * 100; //dis ä¸ºåä½ä¸ºcmçè·ç¦»
			dwt_readdiagnostics(&d1);
			// dwt_readdiagnostics(&d1);
			// time_monitor[2] = sync_timer*1000+TIM3->CNT;
			LOS(&d1);
			time_monitor[3] = sync_timer*1000+TIM3->CNT;
			// LOS(&d1);
			// time_monitor[3] = sync_timer*1000+TIM3->CNT;
			/--------------------------ä»¥ä¸ä¸ºéæµè·é»è¾------------------------/
			//dist_cm=33000;
			GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_RESET);
			//GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_RESET);
			LED0_BLINK; //æ¯æåä¸æ¬¡éè®¯åéªçä¸æ¬¡
			dis_after_filter=dist_cm;
			hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
			@@ -1054,7 +1090,8 @@
			misdist_num[taglist_pos]=0;
			tagdist_list[taglist_pos] = hex_dist;
			his_dist[taglist_pos]=hex_dist;
			g_Tagdist[taglist_pos]=hex_dist;
			g_Tagdist[taglist_pos]=hex_dist;
			signalpower_list[taglist_pos] = ((uint8_t)firstpath_power<<8)\|(uint8_t)((rx_power-firstpath_power)*10);
			#ifndef USART_INTEGRATE_OUTPUT
			usart_send[2] = 1;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 17;//æ°æ®æ®µé¿åº¦
			@@ -1104,15 +1141,28 @@
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			}
			void Anchor_Start(void)
			{
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR\| SYS_STATUS_TXFRS \|SYS_STATUS_RXFCG);
			g_start_sync_flag=0;

			dwt_setrxtimeout(1000);//è®¾å®æ¥æ¶è¶æ¶æ¶é´ï¼0ä½æ²¡æè¶æ¶æ¶é´
			dwt_rxenable(0);//æå¼æ¥æ¶
			// while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR))&&!g_start_send_flag&&!g_start_sync_flag)//ä¸ææ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæèåºç°éè¯¯
			// {
			// //IdleTask();
			// };
			}
			extern int32_t intheight;
			extern u16 synclost_count;
			uint32_t current_syncid=0xffffffff,synclost_timer;
			extern u8 flag_syncbase,waitsync_flag;
			u8 tagpos_rec[50],tagpos_send[50],ancidlist_num;
			u8 tagpos_rec[50],tagpos_send[50],ancidlist_num,zero_matrix[100]={0};
			u16 ancidlist_rec[20],ancidlist_send[20];
			u16 recnearbaselist_id[30];
			u16 recnearbaselist_id[30],rec_taganc_signalpower_list[11];
			int32_t recnearbaselist_dist[30];
			int16_t rec_tagheight;
			u16 timm = 0;
			void Anchor_App(void)
			{

			@@ -1120,21 +1170,23 @@
			u16 tempid;
			uint32_t rec_syncid;

			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR\| SYS_STATUS_TXFRS \|SYS_STATUS_RXFCG);
			g_start_sync_flag=0;
			// dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR\| SYS_STATUS_TXFRS \|SYS_STATUS_RXFCG);
			// g_start_sync_flag=0;

			dwt_setrxtimeout(0);//è®¾å®æ¥æ¶è¶æ¶æ¶é´ï¼0ä½æ²¡æè¶æ¶æ¶é´
			dwt_rxenable(0);//æå¼æ¥æ¶
			// dwt_setrxtimeout(0);//è®¾å®æ¥æ¶è¶æ¶æ¶é´ï¼0ä½æ²¡æè¶æ¶æ¶é´
			// dwt_rxenable(0);//æå¼æ¥æ¶
			// GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_SET);
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR))&&!g_start_send_flag&&!g_start_sync_flag)//ä¸ææ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæèåºç°éè¯¯
			{
			IdleTask();
			};
			// while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR)))//ä¸ææ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæèåºç°éè¯¯
			// {
			// //IdleTask();
			// };
			//GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_RESET);
			dwt_setinterrupt( DWT_INT_RFCG \| (DWT_INT_ARFE \| DWT_INT_RFSL \| DWT_INT_SFDT \| DWT_INT_RPHE \| DWT_INT_RFCE \| DWT_INT_RFTO \| DWT_INT_RXPTO), 0);
			status_reg = dwt_read32bitreg(SYS_STATUS_ID);
			if (status_reg & SYS_STATUS_RXFCG)//æåæ¥æ¶
			{ u16 tag_recv_interval;
			float temp_tagpos;
			time_monitor[0] = sync_timer*1000+TIM3->CNT;
			// time_monitor[0] = sync_timer*1000+TIM3->CNT;
			g_Resttimer=0;
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//æ¸é¤æ å¿ä½
			frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//è·å¾æ¥æ¶æ°æ®é¿åº¦
			@@ -1145,9 +1197,93 @@
			switch(rx_buffer[MESSAGE_TYPE_IDX])
			{
			uint16_t checksum;
			case POLL:
			if (anchor_type == rx_buffer[ANC_TYPE_IDX])
			case DISCOVERPOLL:
			if (anchor_type == rx_buffer[ANCHOR_ID_IDX])
			{
			taglist_pos=CmpTagInList(tag_id_recv);
			if(taglist_pos==taglist_num)
			{
			taglist_pos=taglist_num;
			tagid_list[taglist_num++]=tag_id_recv;

			new_tagid=1;
			}else{
			new_tagid=0;
			}
			tagofflinetime[taglist_pos]=0;

			rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
			if(rec_nearbase_num>=30)
			{
			break;
			}
			battary = rx_buffer[BATTARY_IDX];
			button = rx_buffer[BUTTON_IDX];
			frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
			tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
			tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];

			////////////////åºç«æ±æ»æ¨¡å¼æ°æ®
			memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6],2);
			memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
			memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num2],rec_nearbase_num4);
			Anchor_RecPoll();
			}
			break;
			case SPOLL:
			//if (anchor_type == rx_buffer[ANC_TYPE_IDX])
			if(dev_id==anc_id_recv)
			{
			taglist_pos=CmpTagInList(tag_id_recv);
			if(taglist_pos==taglist_num)
			{
			taglist_pos=taglist_num;
			tagid_list[taglist_num++]=tag_id_recv;

			new_tagid=1;
			}else{
			new_tagid=0;
			}
			tagofflinetime[taglist_pos]=0;

			rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
			if(rec_nearbase_num>=30)
			{
			break;
			}
			battary = rx_buffer[BATTARY_IDX];
			button = rx_buffer[BUTTON_IDX];
			frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
			tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
			tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];

			////////////////åºç«æ±æ»æ¨¡å¼æ°æ®
			memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6],2);
			memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
			memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num2],rec_nearbase_num4);
			usart_send[2] = 0x0c;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 15+8*(rec_nearbase_num);//æ°æ®æ®µé¿åº¦
			memcpy(&usart_send[4],&tag_id_recv,2);
			usart_send[6] = rx_buffer[SEQUENCE_IDX];
			usart_send[7] = rx_buffer[SEQUENCEH_IDX];
			usart_send[8] = battary;
			usart_send[9] = button;
			memcpy(&usart_send[10],&rec_tagheight,2);
			usart_send[12] = tag_frequency;
			usart_send[13] = tag_slotpos;
			usart_send[14] = 0;
			usart_send[15] = 0;
			usart_send[16] = rec_nearbase_num;

			memcpy(&usart_send[17],&recnearbaselist_id,2*rec_nearbase_num);
			memcpy(&usart_send[17+rec_nearbase_num2],&recnearbaselist_dist,4rec_nearbase_num);
			memcpy(&usart_send[17+rec_nearbase_num6],zero_matrix,2rec_nearbase_num);

			checksum = Checksum_u16(&usart_send[2],15+8*rec_nearbase_num);
			memcpy(&usart_send[17+8*rec_nearbase_num],&checksum,2);
			UART_PushFrame(usart_send,19+8*rec_nearbase_num);
			Anchor_RecPoll();
			}
			break;
			case SYNC:
			memcpy(&rec_syncid,&rx_buffer[ANCHOR_ID_IDX],4);
			@@ -1211,6 +1347,7 @@
			memcpy(&usart_send[6],&tag_id_recv,2);
			memcpy(&usart_send[8],&rx_buffer[10],9);
			usart_send[17] = firstpath_power;
			memcpy(&usart_send[18],&rx_buffer[19],2);
			checksum = Checksum_u16(&usart_send[2],20);
			memcpy(&usart_send[22],&checksum,2);
			UART_PushFrame(usart_send,24);
			@@ -1234,15 +1371,22 @@
			temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);
			tagpos_rec[(u8)temp_tagpos]=1;
			rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
			if(rec_nearbase_num>30)
			{
			break;
			}
			battary = rx_buffer[BATTARY_IDX];
			button = rx_buffer[BUTTON_IDX];
			frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
			tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
			tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];
			////////////////åºç«æ±æ»æ¨¡å¼æ°æ®
			memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6+4],2);
			memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*8+6],2);
			memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
			memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num2],rec_nearbase_num4+4);
			memcpy(&rec_taganc_signalpower_list,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num6+4],rec_nearbase_num2+2);
			if(anc_id_recv == dev_id )
			{
			usart_send[2] = 0x0c;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 15+8*(rec_nearbase_num+1);//æ°æ®æ®µé¿åº¦
			memcpy(&usart_send[4],&tag_id_recv,2);
			@@ -1250,6 +1394,7 @@
			usart_send[7] = rx_buffer[SEQUENCEH_IDX];
			usart_send[8] = battary;
			usart_send[9] = button;

			memcpy(&usart_send[10],&rec_tagheight,2);
			usart_send[12] = tag_frequency;
			usart_send[13] = tag_slotpos;
			@@ -1259,11 +1404,15 @@
			memcpy(&usart_send[17],&anc_id_recv,2);
			memcpy(&usart_send[19],&recnearbaselist_id,2*rec_nearbase_num);
			memcpy(&usart_send[19+rec_nearbase_num2],&recnearbaselist_dist,4rec_nearbase_num+4);

			memcpy(&usart_send[19+rec_nearbase_num6+4],&rec_taganc_signalpower_list,2rec_nearbase_num+2);

			checksum = Checksum_u16(&usart_send[2],23+8*rec_nearbase_num);
			memcpy(&usart_send[25+8*rec_nearbase_num],&checksum,2);
			UART_PushFrame(usart_send,27+8*rec_nearbase_num);

			// UART_CheckSend();

			timm++;
			}
			//////////////////////////////////////////////////////////////


			@@ -1305,5 +1454,10 @@
			#endif
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			UART_CheckSend();
			dwt_setinterrupt( DWT_INT_RFCG \| (DWT_INT_ARFE \| DWT_INT_RFSL \| DWT_INT_SFDT \| DWT_INT_RPHE \| DWT_INT_RFCE \| DWT_INT_RFTO \| DWT_INT_RXPTO), 1);
			dwt_setrxtimeout(0);//è®¾å®æ¥æ¶è¶æ¶æ¶é´ï¼0ä½æ²¡æè¶æ¶æ¶é´
			dwt_rxenable(0);//æå¼æ¥æ¶

			}