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; }