XRange_Tag.git - Gitblit

			@@ -71,7 +71,7 @@
			#define FINAL_MSG_FINAL_TX_TS_IDX 18
			#define FINAL_MSG_TS_LEN 4
			static dwt_config_t config = {
			2, /* Channel number. */
			5, /* Channel number. */
			DWT_PRF_64M, /* Pulse repetition frequency. */
			DWT_PLEN_128, /* Preamble length. */
			DWT_PAC8, /* Preamble acquisition chunk size. Used in RX only. */
			@@ -97,7 +97,6 @@
			static uint64_t poll_rx_ts;
			static uint64_t resp_tx_ts;
			static uint64_t final_rx_ts;
			static double tof;
			int32_t anchor_dist_last_frm[TAG_NUM_IN_SYS],his_dist[TAG_NUM_IN_SYS]; ;
			uint32_t tag_id = 0;
			uint32_t tag_id_recv = 0;
			@@ -304,7 +303,56 @@
			freqlost_count = 0;
			}
			}
			#define FREQ_OFFSET_MULTIPLIER (998.4e6/2.0/1024.0/131072.0)
			#define FREQ_OFFSET_MULTIPLIER_110KB (998.4e6/2.0/8192.0/131072.0)

			#define HERTZ_TO_PPM_MULTIPLIER_CHAN_1 (-1.0e6/3494.4e6)
			#define HERTZ_TO_PPM_MULTIPLIER_CHAN_2 (-1.0e6/3993.6e6)
			#define HERTZ_TO_PPM_MULTIPLIER_CHAN_3 (-1.0e6/4492.8e6)
			#define HERTZ_TO_PPM_MULTIPLIER_CHAN_5 (-1.0e6/6489.6e6)
			u16 rec_maxrangelen;//rec_anc_signalpower[MAX_NEARBASE_ANCNUM];
			float clockOffsetRatio;
			double rtd_init, rtd_resp;
			double tof,distance;
			extern int32_t dwt_readcarrierintegrator(void) ;
			uint32_t testtimer2,anc_pollrx[MAX_NEARBASE_NUM],anc_resptx[MAX_NEARBASE_NUM],tag_resprx[MAX_NEARBASE_NUM];
			int32_t anc_clockoffset[MAX_NEARBASE_NUM];
			int16_t anc_distoffset[MAX_NEARBASE_NUM];
			int32_t test2;
			uint16_t u16_nearbase_distlist[MAX_NEARBASE_NUM];
			uint8_t get_newdist,notenoughdist_count;
			void SetANCTimestap(uint8_t i,uint8_t* pollrx,uint8_t* resptx,uint32_t resprx,uint8_t* distoffset)
			{
			memcpy(&anc_pollrx[i],pollrx,4);
			memcpy(&anc_resptx[i],resptx,4);
			memcpy(&tag_resprx[i],&resprx,4);
			memcpy(&anc_distoffset[i],distoffset,2);
			anc_clockoffset[i] = test2;
			}
			void CalculateDists(void)
			{
			for(int i=0;i<MAX_NEARBASE_NUM;i++)
			{
			//rec_anc_signalpower[i] = exsistbase_list[i];
			if(exsistbase_list[i]==KEEP_TIMES)
			{

			// exsistbase_list[i]--;
			clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_5 / 1.0e6) ;
			rtd_init = tag_resprx[i] - poll_tx_ts;
			rtd_resp = anc_resptx[i] - anc_pollrx[i];
			tof = ((rtd_init - rtd_resp * (1 - clockOffsetRatio)) / 2.0) * DWT_TIME_UNITS;
			distance = tof * SPEED_OF_LIGHT;
			if(distance>-10&&distance<1000)
			nearbase_distlist[i] = distance*100+anc_distoffset[i];
			}else{
			// nearbase_distlist[i] = 0x1ffff;
			}

			}
			}
			float range_lost_time = 0;
			u16 rec_nearbaseid,rec_nearbasepos;
			void NearPoll(void)
			{

			@@ -314,39 +362,45 @@
			u32 start_poll;
			u8 i,j,getsync_flag=0,timeout,flag_ancreadpara=0;
			dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); //设置发送后开启接收，并设定延迟时间
			dwt_setrxtimeout(10000);
			dwt_setrxtimeout(0);
			tag_succ_times = 0;
			//GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_SET);
			if(next_nearbase_num>=MAX_NEARBASE_NUM)
			if(nearbase_num>=MAX_NEARBASE_NUM)
			{
			next_nearbase_num = MAX_NEARBASE_NUM-1;
			nearbase_num = MAX_NEARBASE_NUM-1;
			}
			HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_SET);
			last_nearbase_num=next_nearbase_num;
			nearbase_num=next_nearbase_num;
			// HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_SET);
			// last_nearbase_num=next_nearbase_num;
			// nearbase_num=next_nearbase_num;
			recbase_num=0;
			// motor_state=0;
			for(uint8_t i=0;i<nearbase_num;i++)
			{
			u16_nearbase_distlist[i] = nearbase_distlist[i];
			}
			tx_near_msg[GROUP_ID_IDX] = group_id;
			tx_near_msg[BATTARY_IDX] = bat_percent;
			tx_near_msg[BUTTON_IDX] = !GET_USERKEY\|stationary_flag<<1;
			tx_near_msg[SEQUENCE_IDX] = frame_seq_nb++;
			tx_near_msg[NEARBASENUM_INDEX] = nearbase_num;
			memcpy(&tx_near_msg[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2);
			memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num2],&u16_nearbase_distlist,nearbase_num2);
			tx_near_msg[MESSAGE_TYPE_IDX] = MBX_POLL;
			memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);
			dwt_writetxdata(13+2*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
			dwt_writetxfctrl(13+2*nearbase_num, 0);//设置超宽带发送数据长度
			dwt_writetxdata(13+4*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
			dwt_writetxfctrl(13+4*nearbase_num, 0);//设置超宽带发送数据长度
			dwt_starttx(DWT_START_TX_IMMEDIATE \| DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置

			tx_near_msg[TAGCONFIGSUCCESS_INDEX] =0;
			para_update = 0;
			get_newbase = 0;
			flag_finalsend=0;
			flag_rxon=1;
			flag_getresponse=0;
			start_count=HAL_LPTIM_ReadCounter(&hlptim1);
			recbase_num=0;
			timeout=nearbase_num*SLOT_SCALE+4;
			end_count=start_count+(timeout<<5);
			timeout=nearbase_num*SLOT_SCALE+10;
			end_count=start_count+(timeout<<2);
			if(end_count>=32768)
			{end_count-=32768;}
			mainbase_dist=100000;
			@@ -357,7 +411,7 @@
			current_count=HAL_LPTIM_ReadCounter(&hlptim1);
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误
			{
			if(flag_finalsend)
			if(flag_finalsend&&flag_ancreadpara)
			{
			dw_systime=dwt_readsystimestamphi32();
			if(dw_systime>temp1&&dw_systime<temp2)
			@@ -374,9 +428,10 @@
			memcpy(&tx_near_msg[MESSAGE_PARAREPLY_IDX+SENDANCPARA_LEN],&calCRC,2);
			dwt_writetxdata(14+SENDANCPARA_LEN, tx_near_msg, 0);//将发送数据写入DW1000
			dwt_writetxfctrl(14+SENDANCPARA_LEN, 0);//设定发送数据长度
			}

			dwt_setdelayedtrxtime(final_tx_time);
			result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送
			}
			flag_finalsend=0;
			break;
			}
			@@ -394,10 +449,11 @@
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG \| SYS_STATUS_TXFRS);//清楚寄存器标志位
			frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; //获得接收到的数据长度
			dwt_readrxdata(rx_buffer, frame_len, 0); //读取接收数据
			test2 = dwt_readcarrierintegrator();
			dwt_setrxtimeout(0);//DELAY_BETWEEN_TWO_FRAME_UUS*(nearbase_num+1-recbase_num)+10);//设定接收超时时间，0位没有超时时间
			dwt_rxenable(0);//打开接收
			if (rx_buffer[MESSAGE_TYPE_IDX] == MBX_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)&&rx_buffer[GROUP_ID_IDX]==group_id) //判断接收到的数据是否是response数据
			{ u16 rec_nearbaseid,rec_nearbasepos;
			{
			poll_tx_ts = get_tx_timestamp_u64(); //获得POLL发送时间T1
			resp_rx_ts = get_rx_timestamp_u64(); //获得RESPONSE接收时间T4
			recbase_num++;
			@@ -422,6 +478,9 @@
			}
			if(rec_nearbaseid==nearbaseid_list[0])
			{
			exsistbase_list[0]=KEEP_TIMES;
			SetANCTimestap(0,&rx_buffer[RESP_MSG_POLL_RX_TS_IDX],&rx_buffer[RESP_MSG_RESP_TX_TS_IDX],resp_rx_ts,&rx_buffer[RESP_MSG_ANC_DISTOFFSET]);

			//////////////////////////////////时间同步
			memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2);
			memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);
			@@ -442,8 +501,7 @@
			////////////////////////////
			rec_nearbasepos=0;
			exsistbase_list[rec_nearbasepos]=KEEP_TIMES;
			memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
			nearbase_distlist[rec_nearbasepos]=temp_dist;

			if((rx_buffer[MOTORSTATE_INDEX]&0xf)!=3)
			motor_state=rx_buffer[MOTORSTATE_INDEX]&0xf;

			@@ -469,13 +527,13 @@


			//时间同步
			ancsync_time=((sync_timer)*1000+tmp_time);
			last_slotnum=current_slotnum;
			current_slotnum=((float)sync_timer/g_com_map[COM_INTERVAL])+1;
			if(current_slotnum==last_slotnum-1)
			{flag_getresponse=1;}
			lastsync_timer=sync_timer;
			offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset;
			// ancsync_time=((sync_timer)*1000+tmp_time);
			// last_slotnum=current_slotnum;
			// current_slotnum=((float)sync_timer/g_com_map[COM_INTERVAL])+1;
			// if(current_slotnum==last_slotnum-1)
			// {flag_getresponse=1;}
			// lastsync_timer=sync_timer;
			// offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset;
			// SetNextPollTime(tyncpoll_time);
			// if(rec_remotepara_state==1)
			// {
			@@ -490,8 +548,9 @@
			// }
			}else{
			rec_nearbasepos=FindNearBasePos(rec_nearbaseid);

			if(rec_nearbasepos>=last_nearbase_num) //发现新的基站
			SetANCTimestap(rec_nearbasepos,&rx_buffer[RESP_MSG_POLL_RX_TS_IDX],&rx_buffer[RESP_MSG_RESP_TX_TS_IDX],resp_rx_ts,&rx_buffer[RESP_MSG_ANC_DISTOFFSET]);

			if(rec_nearbasepos>=last_nearbase_num) //发现新的基站
			{
			get_newbase=1;
			nearbase_num++;
			@@ -500,8 +559,7 @@
			}

			exsistbase_list[rec_nearbasepos]=KEEP_TIMES;
			memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
			nearbase_distlist[rec_nearbasepos]=temp_dist;


			if(motor_state<(rx_buffer[MOTORSTATE_INDEX]&0xf)&&(rx_buffer[MOTORSTATE_INDEX]&0xf)!=3)
			{
			@@ -526,24 +584,24 @@
			}
			// dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG\| SYS_STATUS_ALL_RX_ERR);
			}
			if(mainbase_lost_count>tag_frequency*BASELOST_STOPMOTOR_TIME)
			{motor_state=0; }
			dwt_forcetrxoff();
			dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG\| SYS_STATUS_ALL_RX_ERR);
			nearbase_num=recbase_num;
			// tyncpoll_time=0;
			next_nearbase_num=0;
			CalculateDists();
			j = 0;
			for(i=0;i<last_nearbase_num+get_newbase;i++)
			next_nearbase_num = 0;
			for(i=0;i<nearbase_num;i++)
			{
			if(exsistbase_list[i]>0)
			{
			{
			exsistbase_list[i]--;
			next_nearbase_num++;
			true_exsistbase_list[j]=exsistbase_list[i];
			true_nearbase_idlist[j]=nearbaseid_list[i];
			true_nearbase_distlist[j++]=nearbase_distlist[i];
			exsistbase_list[i]--;

			}
			}
			nearbase_num = next_nearbase_num;
			if(mainbase_lost_count>1&&exsistbase_list[0] != 0)
			{
			last_lpcount+= HAL_LPTIM_ReadCounter(&hlptim1)%(g_com_map[COM_INTERVAL]*16);
			@@ -571,33 +629,29 @@
			}

			report_num=0;
			// if(last_nearbase_num>4)
			// {
			// last_nearbase_num = 5;
			// }
			for (i=0;i<last_nearbase_num;i++)
			{
			nearbaseid_list[i]=true_nearbase_idlist[i];
			nearbase_distlist[i]=true_nearbase_distlist[i];
			if(nearbase_distlist[i]!=0x1ffff&&true_exsistbase_list[i]==KEEP_TIMES)
			{
			memcpy(&usart_send[4+6*report_num],&nearbaseid_list[i],2);
			memcpy(&usart_send[6+6*report_num],&nearbase_distlist[i],4);
			report_num++;
			}
			exsistbase_list[i] = true_exsistbase_list[i];
			}
			#ifdef USART_INTEGRATE_OUTPUT
			usart_send[0] = 0x55;
			usart_send[1] = 0xAA;
			usart_send[2] = 4;//正常模式
			usart_send[3] = report_num*6+2;//正常模式
			checksum = Checksum_u16(&usart_send[2],report_num*6+2);
			memcpy(&usart_send[4+report_num*6],&checksum,2);
			USART_puts(usart_send,6+report_num*6);
			#endif
			// #ifdef USART_INTEGRATE_OUTPUT
			// usart_send[0] = 0x55;
			// usart_send[1] = 0xAA;
			// usart_send[2] = 4;//正常模式
			// usart_send[3] = report_num*6+2;//正常模式
			// checksum = Checksum_u16(&usart_send[2],report_num*6+2);
			// memcpy(&usart_send[4+report_num*6],&checksum,2);
			// USART_puts(usart_send,6+report_num*6);
			// #endif

			for(i=0;i<MAX_NEARBASE_NUM;i++)
			{
			nearbase_distlist[i]=0x1ffff;
			}
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR\| SYS_STATUS_TXFRS \|SYS_STATUS_RXFCG);
			//HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_RESET);

			if(para_update==1)
			{
			uint16_t calCRC;
			@@ -605,7 +659,7 @@
			if(memcmp(&calCRC,&rec_remotepara[5],2)==0)
			{ uint8_t pack_msgtype,pack_index,pack_length;
			pack_msgtype = rec_remotepara[0];
			pack_index = rec_remotepara[1]*2;
			pack_index = rec_remotepara[1];
			pack_length = rec_remotepara[2];
			if(pack_msgtype==2)
			{
			@@ -613,7 +667,14 @@
			{

			// motor_keeptime = rec_remotepara[3];
			}else{
			}else if(pack_index == CNT_UPDATE)
			{
			if(rec_remotepara[3]==2)
			{
			EnterUWBUpdateMode();
			}
			}
			else{
			if(pack_index<200)
			{
			memcpy((uint8_t*)&g_com_map + pack_index, &rec_remotepara[3], pack_length);
			@@ -645,6 +706,7 @@
			break;
			}
			}
			delay_us(100);
			g_Resttimer=0;
			NearPoll();
			dwt_entersleep();