#include "wirless_uwb_app.h"
|
|
#define MAXSEARCHSYNCTIMES 300
|
E_RangProc rangproc;
|
static u16 current_count,start_count,end_count,poll_startcount;
|
static u16 lpcount,last_lpcount;
|
static u16 waketopolltimeus = 3200;
|
#define BASENUM_COMTIME 244
|
static u32 last_anchor_time;
|
static int32_t error_time;
|
static u32 temptime,delaytime,expect_anctime;
|
static int16_t poll_offsettime=-6900,clockoffset1S;
|
int16_t target_offsettime = 850;
|
static u16 searchsync_times;
|
static u16 stime_time,syncbase_ms,range_ms,stime_lp_difftime;//ͬ²½ÏµÍ³Ê±¼äºÁÃë ΢Ãͬ²½»ùվʱ¼ä£¬²â¾àʱ¼ä¡£
|
void CalibClockOffset(int16_t clockerror)
|
{
|
if(clockerror<2000&&error_time>-2000)
|
clockoffset1S+=clockerror*0.1;
|
}
|
void SetWkupTimeBaseNow(u16 current_lpcount,u32 delaytime) //Ï´λ½ÐÑʱ¼äºÍµ±Ç°Ê±¼äµÄ²îÖµ¡£
|
{
|
//tagslotpos = 0;
|
|
//CalibClockOffset(error_time);
|
|
delaytime += poll_offsettime+clockoffset1S*delaytime/1000000;
|
lpcount = current_lpcount+delaytime/LPTIMER_LSB;
|
if(lpcount>=32768)
|
{
|
lpcount -=32768;
|
}
|
poll_startcount = lpcount+105;//105=(3200)/LPTIMER_LSB;
|
if(poll_startcount>=32768)
|
{
|
poll_startcount -=32768;
|
}
|
__HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
|
last_lpcount = lpcount;
|
|
return ;
|
}
|
|
void SetWkupTimeBaseLast(u32 delaytime) //ÏÂÒ»´ÎµÄ»½ÐÑʱ¼ä±ÈÉÏÒ»´ÎµÄ»½ÐÑʱ¼ä²î¶àÉÙ£¬Èç¹ûÊÇ0»òÕßÊÇ1000msÔò²»Óõ÷Óá£
|
{
|
lpcount = last_lpcount+delaytime/LPTIMER_LSB;
|
if(lpcount>=32768)
|
{
|
lpcount -=32768;
|
}
|
poll_startcount = lpcount+105;//105=(3200)/LPTIMER_LSB;
|
if(poll_startcount>=32768)
|
{
|
poll_startcount -=32768;
|
}
|
__HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
|
last_lpcount = lpcount;
|
}
|
void WkupUWB(void)
|
{ u32 id;
|
u8 error_times;
|
SPIx_CS_GPIO->BRR = SPIx_CS;
|
delay_us(700);
|
SPIx_CS_GPIO->BSRR = SPIx_CS;
|
id = dwt_readdevid() ;
|
while (DWT_DEVICE_ID != id)
|
{
|
id = dwt_readdevid() ;
|
IdleTask();
|
if(error_times++>20)
|
{
|
printf("DW ID ERROR.\r\n");
|
SCB->AIRCR = 0X05FA0000|(unsigned int)0x04; //Èí¸´Î»»Øµ½bootloader
|
}
|
}
|
}
|
void WaitUntilTargetTime(void)
|
{
|
|
current_count=HAL_LPTIM_ReadCounter(&hlptim1);
|
|
while(current_count<poll_startcount||current_count>poll_startcount+16384)
|
{
|
current_count=HAL_LPTIM_ReadCounter(&hlptim1);
|
if(current_count<poll_startcount-300)
|
{
|
break;
|
}
|
}
|
}
|
static u32 status_reg;
|
static u8 frame_len;
|
static u8 rx_buffer[130];
|
void WaitConfigProc(void) //¿ªÆô½ÓÊÕ1ms£¬ÐÞ¸ÄÅäÖá£
|
{
|
WkupUWB();
|
// WaitUntilTargetTime();
|
dwt_setrxtimeout(1000);//DELAY_BETWEEN_TWO_FRAME_UUS*(nearbase_num+1-recbase_num)+10);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä10
|
dwt_rxenable(0);//´ò¿ª½ÓÊÕ
|
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//²»¶Ï²éѯоƬ״ֱ̬µ½³É¹¦½ÓÊÕ»òÕß·¢Éú´íÎó
|
{}
|
if(status_reg & SYS_STATUS_RXFCG)
|
{
|
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//Çå³ý±ê־λ
|
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//»ñµÃ½ÓÊÕÊý¾Ý³¤¶È
|
dwt_readrxdata(rx_buffer, frame_len, 0);//¶ÁÈ¡½ÓÊÕÊý¾Ý
|
}else{
|
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
|
}
|
}
|
void SearchSyncProc(void)
|
{
|
WkupUWB();
|
WaitUntilTargetTime();
|
dwt_setrxtimeout(10000);//½ÓÊÕ10msÊý¾Ý
|
dwt_rxenable(0);//´ò¿ª½ÓÊÕ
|
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//²»¶Ï²éѯоƬ״ֱ̬µ½³É¹¦½ÓÊÕ»òÕß·¢Éú´íÎó
|
{}
|
if(status_reg & SYS_STATUS_RXFCG)
|
{
|
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//Çå³ý±ê־λ
|
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//»ñµÃ½ÓÊÕÊý¾Ý³¤¶È
|
dwt_readrxdata(rx_buffer, frame_len, 0);//¶ÁÈ¡½ÓÊÕÊý¾Ý
|
}else{
|
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
|
}
|
SetWkupTimeBaseLast(980);
|
searchsync_times++;
|
if(searchsync_times>MAXSEARCHSYNCTIMES)
|
{
|
|
}
|
}
|
|
void WaitSyncProc(void) //Õý³£Í¬²½
|
{
|
WkupUWB();
|
WaitUntilTargetTime();
|
dwt_setrxtimeout(1000);//½ÓÊÕ10msÊý¾Ý
|
dwt_rxenable(0);//´ò¿ª½ÓÊÕ
|
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//²»¶Ï²éѯоƬ״ֱ̬µ½³É¹¦½ÓÊÕ»òÕß·¢Éú´íÎó
|
{}
|
if(status_reg & SYS_STATUS_RXFCG)
|
{
|
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//Çå³ý±ê־λ
|
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//»ñµÃ½ÓÊÕÊý¾Ý³¤¶È
|
dwt_readrxdata(rx_buffer, frame_len, 0);//¶ÁÈ¡½ÓÊÕÊý¾Ý
|
}else{
|
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
|
}
|
}
|
u32 DelayBetweenTimes(u32 target_time,u32 current_time)
|
{
|
int32_t temp_time;
|
temp_time = target_time-current_time;
|
while(temp_time>1000000)
|
{
|
temp_time -= 1000000;
|
}
|
while(temp_time<=0)
|
{
|
temp_time += 1000000;
|
}
|
|
return temp_time;
|
}
|
void NealPoll(void)
|
{
|
|
}
|
void RangingProc(void)
|
{
|
u32 current_lptime;
|
current_lptime = HAL_LPTIM_ReadCounter(&hlptim1)*LPTIMER_LSB; //µÍ¹¦ºÄʱÖÓ»»Ëãʱ¼ä
|
stime_time = current_lptime + stime_lp_difftime; //ͬ²½Ê±¼ä
|
if(DelayBetweenTimes(syncbase_ms*1000,stime_time)>DelayBetweenTimes(range_ms*1000,stime_time))
|
{
|
rangproc = RANGPROC_NEARPOLL;
|
}else{
|
rangproc = RANGPROC_WAITSYNC;
|
}
|
}
|
