#include "dw_app.h"
|
#include "stdint.h"
|
#include <string.h>
|
#include "mk_uwb.h"
|
#include "mk_misc.h"
|
static float clockOffsetRatio;
|
static double rtd_init, rtd_resp;
|
double tof,distance;
|
#define SPEED_OF_LIGHT 299702547
|
#define DWT_TIME_UNITS (1.0/499.2e6/128.0) //!< = 15.65e-12 s
|
static uint32_t anc_pollrx[MAX_NEARBASE_NUM],anc_resptx[MAX_NEARBASE_NUM],tag_resprx[MAX_NEARBASE_NUM],tag_polltx[MAX_NEARBASE_NUM];
|
static int32_t anc_clockoffset[MAX_NEARBASE_NUM];
|
static int16_t anc_distoffset[MAX_NEARBASE_NUM];
|
extern uint8_t taglist_keeptime[MAX_TAG_LIST_NUM]; //ÿ¸ö±êÇ©´æ»îʱ¼ä
|
extern uint16_t taglist_id[MAX_TAG_LIST_NUM],taglist_dist[MAX_TAG_LIST_NUM]; //±êÇ©¾àÀëºÍ±êÇ©µÄID
|
extern uint16_t taglist_total_num,taglist_current_index;
|
//#if defined (MK_SS_TWR_DW_INIT)
|
extern uint8_t exsistbase_list[MAX_NEARBASE_NUM];
|
extern int32_t nearbase_distlist[MAX_NEARBASE_NUM];
|
static int64_t tround,treply;
|
static int64_t poll_tx_timestamp,resp_rx_timestamp,resp_tx_timestamp,poll_rx_timestamp;
|
|
int32_t temp_freq_offset;
|
int64_t tround_temp,treply_temp;
|
int32_t tof_i;
|
uint32_t tof_i_ui;
|
double tof_f;
|
extern int32_t freq_offset,freq_offset_filter;
|
void SetANCTimestap(uint8_t i,uint8_t* pollrx,uint8_t* resptx,uint32_t resprx,uint8_t* distoffset,int32_t anc_clockoffset_from_MK,uint32_t polltx) //дÈëʱ¼ä´ÁÐÅÏ¢
|
{
|
memcpy(&anc_pollrx[i],pollrx,4);
|
memcpy(&anc_resptx[i],resptx,4);
|
memcpy(&tag_resprx[i],&resprx,4);
|
memcpy(&anc_distoffset[i],distoffset,2);
|
memcpy(&tag_polltx[i],&polltx,4);
|
//anc_clockoffset[i] = anc_clockoffset_from_MK;//MK8000ÐÞ¸Ä
|
}
|
void CalculateDists1(int64_t poll_tx_ts)
|
{
|
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]--;
|
#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) ;//MK8000ÐÞ¸Ä
|
#endif
|
rtd_init = tag_resprx[i] - (uint32_t)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;
|
}
|
|
}
|
|
}
|
// ts_a - ts_b
|
int64_t ranging_timestamp_diff(int64_t ts_a, int64_t ts_b)
|
{
|
if (ts_a < ts_b)
|
{
|
return (0x20000000000 - ts_b + ts_a);
|
}
|
else
|
{
|
return (ts_a - ts_b);
|
}
|
}
|
void CalculateDists(int64_t poll_tx_ts)
|
{
|
for(int i=0;i<MAX_NEARBASE_NUM;i++)
|
{
|
|
if(exsistbase_list[i]==KEEP_TIMES)
|
{
|
|
//temp_freq_offset=freq_offset_filter;//²âÊÔ
|
memcpy(&resp_tx_timestamp,&anc_resptx[i],4);
|
memcpy(&poll_rx_timestamp,&anc_pollrx[i],4);
|
treply = ranging_timestamp_diff(resp_tx_timestamp, poll_rx_timestamp);
|
//»ùÕ¾
|
// // correct antenna delay
|
// treply += ranging_ant_delays_get((uint8_t)(RX_MAIN_ANT_PORT));
|
|
// uint32_t tag_tx_temp=(uint32_t)poll_tx_ts;//²âÊÔ
|
// memcpy(&tag_polltx[0],&tag_tx_temp,4);//²âÊÔ
|
|
//poll_tx_timestamp=poll_tx_ts;
|
|
memcpy(&poll_tx_timestamp,&tag_polltx[i],4);
|
memcpy(&resp_rx_timestamp,&tag_resprx[i],4);
|
|
tround=ranging_timestamp_diff(resp_rx_timestamp, poll_tx_timestamp);
|
tround_temp=tround;
|
treply_temp=treply;
|
//±êÇ©
|
// correct antenna delay
|
// tround -= ranging_ant_delays_get(uwb_app_config.ppdu_params.rx_ant_id);
|
// corrected by frequency offset
|
tround = (int64_t)((double)tround * (1 - (double)freq_offset_filter / ch_center_freq_map[UWB_CH_NUM]));
|
tround_temp=tround;
|
treply_temp=treply;
|
tof_i = (int32_t)(tround - treply) / 2;
|
// tof_i_ui=(uint32_t)tof_i;
|
|
// outlier filterÈ¥µô¸ºÖµ¹ýÂËδÌí¼Óoffset
|
// if (tof_i < 0)
|
// {
|
// tof_i = 0;
|
// }
|
//tof_f = (double)TIMESTAMP_UNIT_TO_NS((uint32_t)tof_i);//yuan
|
tof_f = (double)TIMESTAMP_UNIT_TO_NS(tof_i);
|
//distance=(uint16_t)(tof_f * 0.299702547 * VP_VAL - RANGING_CORR);//yuan
|
distance=tof_f * 0.299702547 * VP_VAL - RANGING_CORR;
|
if(distance>-10&&distance<1000)
|
{
|
nearbase_distlist[i] = distance*100+anc_distoffset[i];//offsetδ¼Ó
|
}
|
}else{
|
nearbase_distlist[i] = 0x1ffff;
|
}
|
|
}
|
}
|
//#endif
|
