void cs_listener_run(void) {
uint8_t ts[8];
uint32 chirp_ts;
int64_t rcv_tag_ts = 0;
uint32 sync_rate = cph_config->sender_period;
uint8 functionCode = 0;
double diff_val = 0;
double diff_val_prev = 0;
double diff_val_var = 0;
double diff_ts =0;
double adjusted_ts = 0;
double relative_tag_ts = 0;
int64_t rcv_curr_ts = 0;
int64_t rcv_prev_ts = 0;
int64_t rcv_diff_ts = 0;
double rcv_diff = 0;
int64_t blink_curr_ts = 0;
int64_t blink_prev_ts = 0;
int64_t blink_diff_ts = 0;
double blink_diff = 0;
double rcv_blink_diff = 0;
uint32 rcv_interval;
uint32 elapsed;
uint8 count = 1;
irq_init();
pio_disable_interrupt(DW_IRQ_PIO, DW_IRQ_MASK);
cph_deca_init_device();
cph_deca_init_network(cph_config->panid, cph_config->shortid);
printf("Device ID: %08X\r\n", dwt_readdevid());
// Enable external sync
uint32_t ec_ctrl;
dwt_readfromdevice(EXT_SYNC_ID, EC_CTRL_OFFSET, 4, (uint8_t*) &ec_ctrl);
ec_ctrl &= EC_CTRL_WAIT_MASK; // clear WAIT field
ec_ctrl |= EC_CTRL_OSTRM | (33 << 3); // turn on OSTRM and put 33 in WAIT field
dwt_writetodevice(EXT_SYNC_ID, EC_CTRL_OFFSET, 4, (uint8_t*) &ec_ctrl);
dwt_setcallbacks(0, rxcallback);
chirp_ts = cph_get_millis();
dwt_setinterrupt( DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO /*| DWT_INT_RXPTO*/), 1);
pio_enable_interrupt(DW_IRQ_PIO, DW_IRQ_MASK);
dwt_rxenable(0);
while(1)
{
if(AWAKE_CHIRP) {
elapsed = cph_get_millis() - chirp_ts;
if(elapsed > CHIRP_PERIOD) {
TRACE("Awake %d\r\n", count);
count++;
chirp_ts = cph_get_millis();
}
}
if(irq_status == STATUS_RCV){
functionCode = ((cph_deca_msg_header_t*)rx_buffer)->functionCode;
switch(functionCode){
case FUNC_CS_SYNC:
dwt_readrxtimestamp(ts);
TRACE("end sys: ");
for (int i = 4; i >= 0; i--) {
TRACE("%02X", ts[i]);
}
TRACE("\r\n");
// // Difference in RX timestamps
// rcv_curr_ts = get_rx_timestamp();
// rcv_diff_ts = rcv_curr_ts - rcv_prev_ts;
// rcv_prev_ts = rcv_curr_ts;
// rcv_diff = (int64_t)rcv_diff_ts * DWT_TIME_UNITS * 1000;
//
// // Difference in TX timestamps
// blink_curr_ts = ((int64_t)((cph_deca_msg_blink_t*)rx_buffer)->blinkTxTs) << 8;
// blink_diff_ts = blink_curr_ts - blink_prev_ts;
// blink_prev_ts = blink_curr_ts;
// blink_diff = blink_diff_ts * DWT_TIME_UNITS * 1000;
//
// rcv_blink_diff = rcv_diff - blink_diff;
//
//// TRACE("TS0: %08X \t TS1: %08X \t V: %08X \t %+.08f ms\r\n", blink_ts_prev, blink_ts, blink_ts_var, blink_diff);
// TRACE("Bdiff: %+.08f ms \t Rdiff: %+.08f ms \t RBdiff: %+.08f ms\r\n", blink_diff, rcv_diff, rcv_blink_diff);
break;
case FUNC_CS_TAG:
rcv_tag_ts = get_rx_timestamp();
rcv_tag_ts = rcv_tag_ts - rcv_curr_ts;
relative_tag_ts = DWU_to_MS(rcv_tag_ts);
// TRACE("%02X : Raw Tag TS: %.08f ", ((cph_deca_msg_header_t*)rx_buffer)->seq, relative_tag_ts);
if(relative_tag_ts < 100) {
// relative_tag_ts = ((relative_tag_ts - diff_ts) - (int)(relative_tag_ts - diff_ts)) * 1000000; //Truncates leading value, and converts to ns
relative_tag_ts = ((relative_tag_ts - diff_ts) - (int)(relative_tag_ts - diff_ts)) * 1000; //Truncates leading value, and converts to μs
// relative_tag_ts = (relative_tag_ts - diff_ts); //Account for offset from source clock in ms
TRACE("%02X:%.08f\r\n", ((cph_deca_msg_header_t*)rx_buffer)->seq, relative_tag_ts);
}
break;
case FUNC_CS_COORD:
break;
default:
break;
}
irq_status = STATUS_EMPTY;
dwt_rxenable(0);
} else if (irq_status == STATUS_ERR) {
// TRACE("INVALID LENGTH: %d\r\n", frame_len);
irq_status = STATUS_EMPTY;
dwt_rxenable(0);
}
}
}
/*
*********************************************************************************************************
*
* loca_status dwt_config(eCHAN channel, ePRF prf, ePLEN PreambleLength, eCODE code, uint8_t nsSFD, DataRate db)
*
* 描 述 : 配置DW1000
*
* 输入参数 :
* channel：DW1000通道
prf：
PreambleLength：
code：
nsSFD：
db：
*
* 输出参数 : DW1000状态
*********************************************************************************************************
*/
static loca_status dwt_config(eCHAN channel, ePRF prf, ePLEN PreambleLength, eCODE code, uint8_t nsSFD, DataRate db)
{
dwt_config_t config;
dwt_txconfig_t txconfig;
uint8_t buf[2];
uint64_t eui64;
uint16_t panid;
eui64 = LOC_CFG_EUI;
panid = LOC_CFG_PANID;
config.chan = channel;
config.prf = prf;
config.txPreambLength = PreambleLength;
switch (PreambleLength)
{
case PLEN_64:
case PLEN_128:
config.rxPAC = DWT_PAC8;
break;
case PLEN_256:
case PLEN_512:
config.rxPAC = DWT_PAC16;
break;
case PLEN_1024:
config.rxPAC = DWT_PAC32;
break;
default:
config.rxPAC = DWT_PAC64;
break;
}
config.rxCode = code;
config.txCode = code;
config.nsSFD = nsSFD;
config.dataRate = db;
config.smartPowerEn = 0;
config.phrMode = 0;
txconfig.PGdly = dwt_pgdelay[channel];
txconfig.power = dwt_manualpwr[prf - 1][channel];
BSP_ChangeSPIRate(SPI_SLOW);
dwt_softreset();
if (dwt_initialise(DWT_LOADNONE) != DWT_SUCCESS)
{
BSP_ChangeSPIRate(SPI_FAST);
return LOCA_FAULT;
}
BSP_ChangeSPIRate(SPI_FAST);
if (dwt_readdevid() != DWT_DEVICE_ID)
{
return LOCA_FAULT;
}
buf[0] = 0x10;
dwt_writetodevice(GPIO_CTRL_ID,0x02,1,&buf[0]);
buf[0] = 0xf0;
buf[1] = 0xf0;
dwt_writetodevice(GPIO_CTRL_ID,0x08,2,&buf[0]);
dwt_configure(&config, 0);
dwt_xtaltrim(LOC_CFG_XTALRIM);
dwt_setrxantennadelay(0);
dwt_settxantennadelay(0);
dwt_setsmarttxpower(0);
dwt_configuretxrf(&txconfig);
dwt_enableframefilter(DWT_FF_NOTYPE_EN);
dwt_seteui((uint8_t *)&eui64);
dwt_setpanid(panid);
//开启硬件中断
//BSP_DWTIntEnable();
// dwt_setinterrupt( DWT_INT_TFRS, 1);
dwt_setrxmode(DWT_RX_NORMAL,0,0);
return LOCA_OK;
}
