UWB_SmallModule_F103.git

			@@ -15,6 +15,7 @@
			*/

			#include <string.h>
			#include <math.h>
			#include "dw_app.h"
			#include "deca_device_api.h"
			#include "deca_regs.h"
			@@ -28,7 +29,9 @@
			#include <stdio.h>
			#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
			@@ -76,11 +79,14 @@
			#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
			#define ANCTIMEUS 16
			#define ANCSEND_INTERVAL 18
			#define SIGNALPOWER 20

			#define POLL 0x01
			#define RESPONSE 0x02
			@@ -109,7 +115,7 @@
			static uint8_t tx_final_msg[24] = {0};

			//static uint8_t rx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
			static uint8_t tx_resp_msg[22] = {0};
			static uint8_t tx_resp_msg[23] = {0};
			//static uint8_t rx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

			/* Frame sequence number, incremented after each transmission. */
			@@ -320,15 +326,65 @@
			dwt_writetxfctrl(sizeof(tx_sync_msg), 0);//è®¾ç½®è¶å®½å¸¦åéæ°æ®é¿åº¦
			dwt_starttx(DWT_START_TX_IMMEDIATE);
			}
			uint16_t g_Resttimer;
			double firstpath_power, rx_power,rec_firstpath_power;
			double f1, f2, r1, r2;
			uint16_t F1,F2,F3,N,C;
			double B = 131072;
			double A = 121.74;
			double min_power;
			dwt_rxdiag_t d1;
			double LOS(dwt_rxdiag_t *dia) {
			F1 = dia->firstPathAmp1;
			F2 = dia->firstPathAmp2;
			F3 = dia->firstPathAmp3;
			N = dia->rxPreamCount;
			C = dia->maxGrowthCIR;

			firstpath_power=10* log10((F1F1+F2F2+F3F3)/(NN))-A;
			// rx_power=10log10(CB/(N*N))-A;

			// min_power = - 10 * log10((F1 F1 + F2 F2 + F3 * F3) / (C *B));
			return min_power;
			}
			#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 u8 supergroupid;
			extern u16 slottime,max_slotnum,current_slotpos,tyncpoll_time;
			void Tag_App(void)//åéæ¨¡å¼(TAGæ ç¾)
			{
			@@ -348,19 +404,32 @@
			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. */
			dwt_starttx(DWT_START_TX_IMMEDIATE \| DWT_RESPONSE_EXPECTED);//å¼å¯åéï¼åéå®æåçå¾ä¸æ®µæ¶é´å¼å¯æ¥æ¶ï¼çå¾æ¶é´å¨dwt_setrxaftertxdelayä¸è®¾ç½®
			start_poll = time32_incr;
			#ifdef DEBUG_OUTPUT
			printf("Pååé,åºç«ID: %d .\r\n",i);
			#endif
			/* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR)))//ä¸ææ¥è¯¢è¯çç¶æç´å°æåæ¥æ¶æèåçéè¯¯
			{ if(time32_incr - start_poll>20)
			@@ -389,7 +458,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]==supergroupid\|\|rx_buffer[GROUP_ID_IDX] == group_id\|\|group_id==supergroupid)&&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
			@@ -410,7 +479,10 @@
			}
			TIM3->CNT=tmp_time;
			}
			memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);
			memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);
			memcpy(&tag_distarray[i], &rx_buffer[DIST_IDX], 4);
			memcpy(&tag_IDarray[i],&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);
			if(rec_com_interval>4&&rec_com_interval!=g_com_map[COM_INTERVAL])
			@@ -439,6 +511,10 @@
			dwt_writetxfctrl(sizeof(tx_final_msg), 0);//è®¾å®åéæ°æ®é¿åº¦
			result=dwt_starttx(DWT_START_TX_DELAYED);//è®¾å®ä¸ºå»¶è¿åé

			#ifdef DEBUG_OUTPUT
			printf("Fååé,åºç«ID: %d .\r\n",i);
			#endif
			getdatatimer[i] = time32_incr;
			tag_succ_times++;

			LED0_BLINK;
			@@ -461,10 +537,13 @@
			memcpy(&usart_send[9],&hex_dist2,4);
			usart_send[13] = bat_percent;
			usart_send[14] = button;
			usart_send[15] = rec_firstpath_power;
			checksum = Checksum_u16(&usart_send[2],17);
			memcpy(&usart_send[19],&checksum,2);
			UART_PushFrame(usart_send,21);
			}
			Modbus_HoldReg[anc_id_recv*2]=hex_dist2>>16;
			Modbus_HoldReg[anc_id_recv*2+1]=hex_dist2;
			}
			// memcpy(&Modbus_HoldReg[anc_id_recv*2],&hex_dist,4);
			/* Poll DW1000 until TX frame sent event set. See NOTE 8 below. */
			@@ -488,6 +567,9 @@
			}
			else
			{
			#ifdef DEBUG_OUTPUT
			printf("Råå¤±è´¥éè¯¯ä¿¡æ¯: %x .\r\n",status_reg);
			#endif
			/* Clear RX error events in the DW1000 status register. */
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			random_delay_tim = DFT_RAND_DLY_TIM_MS;
			@@ -503,12 +585,15 @@
			/* Execute a delay between ranging exchanges. */

			}

			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)
			{
			@@ -552,7 +637,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]==supergroupid\|\|rx_buffer[GROUP_ID_IDX] == group_id\|\|group_id==supergroupid)&&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);
			@@ -563,7 +648,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
			@@ -575,7 +662,8 @@
			/* Write and send the response message. See NOTE 9 below.*/
			if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
			memcpy(&tx_resp_msg[DIST_IDX], &g_Tagdist[tag_id_recv], 4);

			tx_resp_msg[SIGNALPOWER] = firstpath_power;

			dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//åå¥åéæ°æ®
			dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//è®¾å®åéé¿åº¦
			result = dwt_starttx(DWT_START_TX_DELAYED \| DWT_RESPONSE_EXPECTED);//å»¶è¿åéï¼çå¾æ¥æ¶
			@@ -585,6 +673,9 @@
			frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
			/* We assume that the transmission is achieved correctly, now poll for reception of expected "final" frame or error/timeout.
			* See NOTE 7 below. */
			#ifdef DEBUG_OUTPUT
			printf("æ¶å°POLLåï¼æ ç¾ID: %d .\r\n",tag_id_recv);
			#endif
			if(result==0)
			{
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR)))///ä¸ææ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæèåºç°éè¯¯
			@@ -606,7 +697,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]==supergroupid\|\|rx_buffer[GROUP_ID_IDX] == group_id\|\|group_id==supergroupid)&&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;
			@@ -642,8 +733,12 @@

			dist_cm = dist_no_bias * 1000; //dis ä¸ºåä½ä¸ºcmçè·ç¦»
			// dist[TAG_ID] = LP(dis, TAG_ID); //LP ä¸ºä½éæ»¤æ³¢å¨ï¼è®©æ°æ®æ´ç¨³å®

			dwt_readdiagnostics(&d1);
			LOS(&d1);
			/--------------------------ä»¥ä¸ä¸ºéæµè·é»è¾------------------------/
			#ifdef DEBUG_OUTPUT
			printf("æ¶å°FINALåï¼æ ç¾ID: %d .\r\n",tag_id_recv);
			#endif
			LED0_BLINK; //æ¯æåä¸æ¬¡éè®¯åéªçä¸æ¬¡
			g_UWB_com_interval = 0;
			g_Resttimer=0;
			@@ -651,7 +746,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)
			@@ -662,30 +757,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;
			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],&ancrec_distarray[i],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];
			}

			}

			}
			@@ -696,6 +824,9 @@
			}
			}else{
			/* Clear RX error events in the DW1000 status register. */
			#ifdef DEBUG_OUTPUT
			printf("Fåå¤±è´¥éè¯¯ä¿¡æ¯: %x .\r\n",status_reg);
			#endif
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			}else if(rx_buffer[MESSAGE_TYPE_IDX] == SYNC)
			@@ -711,6 +842,9 @@
			}
			else
			{
			#ifdef DEBUG_OUTPUT
			printf("Påå¤±è´¥éè¯¯ä¿¡æ¯: %x .\r\n",status_reg);
			#endif
			/* Clear RX error events in the DW1000 status register. */
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}