#include "dw_mbx_anc.h"
|
///////////////////////////////////Ãâ²¼Ïßʱ¼äͬ²½´úÂë
|
|
|
|
typedef enum{ Sync_Lost, //ͬ²½¶ªÊ§ ͬ²½Ê±¼ä 100Ãëͬ²½Ò»´Î£¬Ã¿´Îͬ²½¿ªÆô2Ã룬·¢Æð²â¾àÇëÇó
|
Sync_Range,//ͬ²½×´Ì¬ ²â¾à ͬ²½Ê±¼ä 10Ãëͬ²½Ò»´Î£¬¿ªÆô10ms£¬·¢Æð²â¾àÇëÇó
|
Sync_Config,//ͬ²½×´Ì¬ ÅäÖà ͬ²½Ê±¼ä 1Ãëͬ²½Ò»´Î£¬²»·¢Æð²â¾àÇëÇó
|
Sync_Sleep, //ͬ²½×´Ì¬ ÐÝÃß Í¬²½Ê±¼ä 100Ãëͬ²½Ò»´Î£¬²»·¢Æð²â¾àÇëÇó
|
}sync_state_enum;
|
sync_state_enum sync_state = Sync_Lost;
|
|
static uint8_t sync_interval_s,range_state = 0;
|
static uint16_t sync_listen_ms;
|
|
void SyncStateSwitch(void) //ͬ²½Çл»Âß¼£¬¿ª»úLOST״̬£¬ÊÕµ½Ðźźó½øÈëͬ²½×´Ì¬£¬ÔÚͬ²½×´Ì¬ÖУ¬¸ù¾ÝÊÕµ½µÄÍø¹ØÐÅÏ¢Åжϲâ¾à»¹ÊÇÐÝÃß ²â¾à ÅäÖÃ״̬
|
{
|
switch(sync_state)
|
{
|
case Sync_Lost:
|
sync_interval_s = 100;
|
sync_listen_ms =2000;
|
range_state = 1;
|
break;
|
case Sync_Range:
|
sync_interval_s = 10;
|
sync_listen_ms =5;
|
range_state = 1;
|
break;
|
case Sync_Config:
|
sync_interval_s = 1;
|
sync_listen_ms =5;
|
range_state = 0;
|
break;
|
case Sync_Sleep:
|
sync_interval_s = 100;
|
sync_listen_ms =5;
|
range_state = 0;
|
break;
|
}
|
}
|
uint8_t GetRangeState(void)
|
{
|
return range_state;
|
}
|
#define SYNCLOST_THRES 10
|
|
|
|
static uint16_t current_count,poll_startcount,start_count,end_count,poll_timeout;
|
static uint8_t sync_interval_count = 0,flag_sync_success,sync_lost_count;
|
void SetFlagSyncSuccess(uint8_t value)
|
{
|
flag_sync_success = value;
|
}
|
|
static uint16_t sync_wg_id,rec_sync_count;
|
uint8_t lora_recbuffer[255],lora_recbuffer_len;
|
static uint16_t synclock_zero_count,wg_sync_polltime_100us,uwbpoll_time_count;
|
uint16_t GetUwbPollCount(void)
|
{
|
return uwbpoll_time_count;
|
}
|
uint16_t GetWGRecSyncCount(void)
|
{
|
return rec_sync_count;
|
}
|
void SyncClockProcess(void) //ͬ²½Ê±ÖÓº¯Êý»ñÈ¡Ï´ÎUWBpollµÄ¶¨Ê±Æ÷count¡£
|
{
|
memcpy(&wg_sync_polltime_100us,&lora_recbuffer[SYSTIME_IDX],2); //WGÏ·¢µÄͬ²½Ê±¼ä£¬µ¥Î»0.1ms¡£
|
synclock_zero_count = rec_sync_count-(uint32_t)wg_sync_polltime_100us*100/LPTIMER_LSB;
|
if(synclock_zero_count<0)
|
synclock_zero_count+=32768;
|
uwbpoll_time_count = synclock_zero_count + (uint32_t)g_com_map[UWBPOLLTIME_MS_IDX]*1000/LPTIMER_LSB;
|
if(uwbpoll_time_count>32768)
|
uwbpoll_time_count-=32768;
|
}
|
void LoraSyncRecPoll(void)
|
{
|
if(sync_interval_count++>=sync_interval_s)
|
{
|
sync_interval_count = 0;
|
flag_sync_success = 0;
|
SwitchLoraToSyncRec(); //Çл»lora½ÓÊÕƵµã
|
|
start_count=HAL_LPTIM_ReadCounter(&hlptim1); //µÈ´ý½ÓÊÕÍê³ÉÖ±µ½³¬Ê±
|
poll_timeout=sync_listen_ms*10; //µ¥Î»0.1ms
|
end_count=start_count+(poll_timeout<<2);
|
if(end_count>=32768)
|
{end_count-=32768;}
|
current_count=HAL_LPTIM_ReadCounter(&hlptim1);
|
while(GetFlagLoraWaitSync())
|
{
|
current_count=HAL_LPTIM_ReadCounter(&hlptim1);
|
if(current_count>=end_count&¤t_count<end_count+15000)
|
{
|
break;
|
}
|
}
|
if(flag_sync_success) //ͬ²½³É¹¦
|
{
|
sync_lost_count = 0;
|
lora_recbuffer_len = GetLoraBufferAndLen(lora_recbuffer,&rec_sync_count);//»ñÈ¡LORAÊÕµ½µÄÊý¾ÝºÍ³¤¶È
|
if(lora_recbuffer[MSG_TYPE_IDX]==LORA_MSGTYPE_SYNC)
|
{
|
memcpy(&g_com_map[SYNCWG_ID_IDX],&lora_recbuffer[WG_ID_IDX],2);
|
if(lora_recbuffer[RANGE_STATE_IDX])
|
{
|
sync_state = Sync_Range;
|
}else{
|
sync_state = Sync_Sleep;
|
}
|
switch(lora_recbuffer[SUBMSGTYPE_IDX])
|
{
|
case SUBMSG_READ_ANCPARA:
|
|
break;
|
case SUBMSG_WRITE_ANCPARA:
|
|
break;
|
case SUBMSG_UPDATE_ANC:
|
|
break;
|
}
|
}
|
}else{
|
if(sync_lost_count++>SYNCLOST_THRES)
|
{
|
sync_state = Sync_Lost;
|
SyncStateSwitch();
|
}
|
}
|
}
|
// Radio.Sleep( ); //Èç¹û³¬Ê±¾Ísleep
|
}
|
////////////////////////////Ãâ²¼Ïß²â¾àÏà¹Ø´úÂë
|
|
|
#define TAG_KEEPTIMES 30 //±êÇ©´æ»îʱ¼ä
|
#define MAX_TAG_LIST_NUM 200 //ͬʱͨѶ±êÇ©×î´óÊýÁ¿
|
|
static uint8_t uwb_sendbuffer[150];
|
static uint8_t rx_buffer[150];
|
|
uint16_t taglist_total_num,taglist_current_index; //µ±Ç°Áбí×ÜÊýÁ¿
|
static uint8_t taglist_keeptime[MAX_TAG_LIST_NUM]; //ÿ¸ö±êÇ©´æ»îʱ¼ä
|
static uint16_t taglist_id[MAX_TAG_LIST_NUM],taglist_dist[MAX_TAG_LIST_NUM]; //±êÇ©¾àÀëºÍ±êÇ©µÄID
|
|
|
static u8 frame_seq_nb,frame_len;
|
|
static uint32_t status_reg = 0;
|
extern int32_t dwt_readcarrierintegrator(void);
|
static int32_t test2;
|
static uint64_t poll_tx_ts;
|
static uint64_t resp_rx_ts;
|
static uint64_t get_tx_timestamp_u64(void)
|
{
|
uint8_t ts_tab[5];
|
uint64_t ts = 0;
|
int i;
|
dwt_readtxtimestamp(ts_tab);
|
for (i = 4; i >= 0; i--)
|
{
|
ts <<= 8;
|
ts |= ts_tab[i];
|
}
|
return ts;
|
}
|
|
static uint64_t get_rx_timestamp_u64(void)
|
{
|
uint8_t ts_tab[5];
|
uint64_t ts = 0;
|
int i;
|
dwt_readrxtimestamp(ts_tab);
|
for (i = 4; i >= 0; i--)
|
{
|
ts <<= 8;
|
ts |= ts_tab[i];
|
}
|
return ts;
|
}
|
//×¢Ò⣺ÒòΪÐÂ×¢²áµÄ±êÇ©²¢²»ÖªµÀ»ùÕ¾ÒѾÊÕµ½ÁË£¬ËùÒÔÒªÔÚÏÂÒ»°üÁ¢¿Ì½«ÐµıêÇ©ID·¢³ö£¬·ñÔò±êÇ©»áÒ»Ö±ÇÀÕ¼»ùÕ¾µÄpoll°ü¡£
|
//·½·¨Êǽ«ÐÂ×¢²áµÄ±êÇ©Ìí¼Óµ½Ï´η¢Ë͵ıêÇ©ÁбíÍ·²¿£¬ÆäËû±êÇ©ÒÀ´ÎÏòºóŲ¶¯¡£
|
uint16_t AddNewTagIntoList(uint16_t tagid)
|
{
|
u16 newindex_cal = taglist_current_index+25;
|
if(newindex_cal>=taglist_total_num) //Èç¹ûµ±Ç°Ë÷ÒýÊÇ×îºóÒ»°ü£¬ÄÇô¾Í¼Óµ½×ÜË÷ÒýµÄºóÃ棬Èç¹û²»ÊǾͼӵ½ÏÂÒ»´ÎÆðʼË÷ÒýλÖá£
|
{
|
newindex_cal = taglist_total_num;
|
|
}else{
|
for(u16 i=taglist_total_num;i>newindex_cal+1;i--) //´ÓºóÍùǰŲµØ·½
|
{
|
taglist_id[i]=taglist_id[i-1];
|
taglist_dist[i]=taglist_dist[i-1];
|
taglist_keeptime[i]=taglist_keeptime[i-1];
|
}
|
}
|
taglist_total_num++;
|
taglist_id[newindex_cal] = tagid;
|
taglist_dist[newindex_cal] = 0;
|
taglist_keeptime[newindex_cal] = TAG_KEEPTIMES;
|
return newindex_cal;
|
}
|
uint16_t CmpTagInList(uint16_t tagid)
|
{
|
u16 temp;
|
for(u8 i=0;i<taglist_total_num;i++)
|
{
|
if(memcmp(&tagid,&taglist_id[i],2)==0)
|
{
|
taglist_keeptime[i] = TAG_KEEPTIMES;
|
return i;
|
}
|
}
|
if(taglist_total_num<MAX_TAG_LIST_NUM)
|
temp = AddNewTagIntoList(tagid);
|
return temp;
|
}
|
static uint32_t anc_pollrx[MAX_NEARBASE_NUM],anc_resptx[MAX_NEARBASE_NUM],tag_resprx[MAX_NEARBASE_NUM];
|
static int32_t anc_clockoffset[MAX_NEARBASE_NUM];
|
static int16_t anc_distoffset[MAX_NEARBASE_NUM];
|
static 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;
|
}
|
static float clockOffsetRatio;
|
#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)
|
static float clockOffsetRatio;
|
static double rtd_init, rtd_resp;
|
static double tof,distance;
|
#define SPEED_OF_LIGHT 299702547
|
u16 count1,count2;
|
static void CalculateDists(void) //¼ÆËã¾àÀë
|
{
|
for(int i=0;i<MAX_NEARBASE_NUM+10;i++) //ÒòΪÔö¼ÓÁË5¸öÇÀռλÖã¬ËùÒÔ×îÖյļÆËãÊÇ×ÜÊýÁ¿+10.¿¼ÂÇʵ¼ÊÇÀÕ¼¿ÉÄܳ¬¹ý5¸ö
|
{
|
//rec_anc_signalpower[i] = exsistbase_list[i];
|
if(taglist_keeptime[i+taglist_current_index]==TAG_KEEPTIMES)
|
{
|
|
// exsistbase_list[i]--;
|
#ifdef _UWB_4G
|
clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_2 / 1.0e6) ;
|
#else
|
clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_5 / 1.0e6) ;
|
#endif
|
rtd_init = tag_resprx[i] - poll_tx_ts&0xffffffff;
|
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)
|
{
|
count2++;
|
taglist_dist[i+taglist_current_index] = distance*100+anc_distoffset[i];
|
}
|
}else{
|
// nearbase_distlist[i] = 0x1ffff;
|
}
|
|
}
|
}
|
void testprogram(void)
|
{
|
// taglist_total_num = 30;
|
// taglist_id[0] = 7;
|
// for(uint8_t i=0;i<30;i++)
|
// {
|
// taglist_keeptime[i] = 100;
|
// }
|
}
|
void TagListUpdate(void)
|
{
|
uint16_t i,j=0,temp[TAG_NUM_IN_SYS];
|
for(i=0;i<taglist_total_num;i++)
|
{
|
if(taglist_keeptime[i]-->0)
|
{
|
taglist_id[j]=taglist_id[i];
|
taglist_keeptime[j++]=taglist_keeptime[i];
|
}
|
}
|
taglist_total_num=j;
|
}
|
static u16 rec_nearbaseid,temp_index;
|
static u8 rec_tag_index,tag_num_tosend;
|
void MBXANCPoll(void) //½«Òª·¢µÄ±êÇ©ÐòºÅ£¬ÔÚÁбíÖеÄλÖÃ
|
{
|
|
tag_num_tosend = taglist_total_num-taglist_current_index;
|
if(tag_num_tosend>25)
|
{
|
tag_num_tosend = 25;
|
}
|
count1++;
|
memcpy(&uwb_sendbuffer[TAG_ID_IDX],&dev_id,2);
|
uwb_sendbuffer[GROUP_ID_IDX] = group_id;
|
uwb_sendbuffer[BATTARY_IDX] = bat_percent;
|
uwb_sendbuffer[SEQUENCE_IDX] = frame_seq_nb++;
|
uwb_sendbuffer[NEARBASENUM_INDEX] = tag_num_tosend;
|
memcpy(&uwb_sendbuffer[NEARBASEID_INDEX],&taglist_id[taglist_current_index],tag_num_tosend*2);
|
memcpy(&uwb_sendbuffer[NEARBASEID_INDEX+tag_num_tosend*2],&taglist_dist[taglist_current_index],tag_num_tosend*2);
|
uwb_sendbuffer[MESSAGE_TYPE_IDX] = MBX_POLL;
|
dwt_writetxdata(13+4*tag_num_tosend, uwb_sendbuffer, 0);//½«Poll°üÊý¾Ý´«¸øDW1000£¬½«ÔÚ¿ªÆô·¢ËÍʱ´«³öÈ¥
|
dwt_writetxfctrl(13+4*tag_num_tosend, 0);//ÉèÖó¬¿í´ø·¢ËÍÊý¾Ý³¤¶È
|
|
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//¿ªÆô·¢ËÍ£¬·¢ËÍÍê³ÉºóµÈ´ýÒ»¶Îʱ¼ä¿ªÆô½ÓÊÕ£¬µÈ´ýʱ¼äÔÚdwt_setrxaftertxdelayÖÐÉèÖÃ
|
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
|
|
start_count=HAL_LPTIM_ReadCounter(&hlptim1);
|
poll_timeout=tag_num_tosend*SLOT_SCALE+30; //µ¥Î»0.1ms
|
end_count=start_count+(poll_timeout<<2);
|
if(end_count>=32768)
|
{end_count-=32768;}
|
current_count=HAL_LPTIM_ReadCounter(&hlptim1);
|
while(current_count<end_count||current_count>end_count+15000)
|
{
|
current_count=HAL_LPTIM_ReadCounter(&hlptim1);
|
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//²»¶Ï²éѯоƬ״ֱ̬µ½³É¹¦½ÓÊÕ»òÕß·¢Éú´íÎó
|
{
|
|
current_count=HAL_LPTIM_ReadCounter(&hlptim1);
|
if(current_count>=end_count&¤t_count<end_count+15000)
|
break;
|
};
|
if(status_reg==0xffffffff)
|
{
|
NVIC_SystemReset();
|
}
|
if (status_reg & SYS_STATUS_RXFCG)//Èç¹û³É¹¦½ÓÊÕ
|
{
|
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();
|
poll_tx_ts = get_tx_timestamp_u64(); //»ñµÃPOLL·¢ËÍʱ¼äT1
|
resp_rx_ts = get_rx_timestamp_u64();
|
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Êý¾Ý
|
{
|
memcpy(&rec_nearbaseid,&rx_buffer[ANCHOR_ID_IDX],2);
|
rec_tag_index = CmpTagInList(rec_nearbaseid);
|
temp_index =rec_tag_index -taglist_current_index;
|
SetANCTimestap(temp_index,&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]);
|
}
|
|
}
|
}
|
dwt_forcetrxoff();
|
CalculateDists();
|
TagListUpdate();
|
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
|
taglist_current_index+=tag_num_tosend;
|
if(taglist_current_index>=taglist_total_num||taglist_total_num<=25) //Èç¹û±êÇ©µ±Ç°index´óÓÚ×Üindex£¬»òÕß×Ü»ùÕ¾ÊýÁ¿Ð¡ÓÚµÈÓÚ25£¬Ôò´Ó0¿ªÊ¼index¡£
|
taglist_current_index = 0;
|
}
|
