/* ********************************************************************************************************* * * static loca_status dwt_init(void) * * 描 述 : 初始化DW1000 * * 输入参数 :none * * * 输出参数 :DW1000状态 * ********************************************************************************************************* */ static loca_status dwt_init(void) { volatile uint32_t devid; BSP_ResetDWT(); //复位DW1000 BSP_ChangeSPIRate(SPI_SLOW); devid = dwt_readdevid(); if (DWT_DEVICE_ID != devid) { BSP_WakeDWT(); devid = dwt_readdevid(); if (DWT_DEVICE_ID != devid) return LOCA_FAULT; } //dw1000_intenable(); BSP_ChangeSPIRate(SPI_FAST); return LOCA_OK; }
void cph_deca_force_wakeup() { reset_DW1000(); spi_set_rate_low(); uint32_t id = dwt_readdevid(); if (id == 0xFFFFFFFF) { TRACE("DW asleep..waking\r\n"); // asleep, wakeup pio_set_pin_high(DW_WAKEUP_PIO_IDX); cph_millis_delay(1); pio_set_pin_low(DW_WAKEUP_PIO_IDX); cph_millis_delay(1); } }
void DWM1000_Anchor::init() { INFO("HSPI"); //_________________________________________________INIT SPI ESP8266 resetChip(); initSpi(); enableIsr(); uint64_t eui = 0xF1F2F3F4F5F6F7F; dwt_seteui((uint8_t*) &eui); dwt_geteui((uint8_t*) &eui); LOG<< HEX << "EUID : "<< eui <<FLUSH; dwt_seteui((uint8_t*) &eui); dwt_geteui((uint8_t*) &eui); LOG<< HEX << "EUID : "<< eui <<FLUSH; // dwt_softreset(); deca_sleep(100); if (dwt_initialise(DWT_LOADUCODE)) { LOG<< " dwt_initialise failed " << FLUSH; } else LOG<< " dwt_initialise done." << FLUSH; if (dwt_configure(&config)) { LOG<< " dwt_configure failed " << FLUSH; } else LOG<< " dwt_configure done." << FLUSH; uint32_t device_id = dwt_readdevid(); uint32_t part_id = dwt_getpartid(); uint32_t lot_id = dwt_getlotid(); LOG<< HEX << " device id : " << device_id << ", part id : " << part_id << ", lot_id :" << lot_id <<FLUSH; /* Apply default antenna delay value. See NOTE 1 below. */ dwt_setrxantennadelay(RX_ANT_DLY); dwt_settxantennadelay(TX_ANT_DLY); /* Set expected response's delay and timeout. See NOTE 4 and 5 below. * As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */ dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); dwt_initialise(DWT_LOADUCODE); // Configure the callbacks from the dwt library dwt_setcallbacks(txcallback, rxcallback); _count = 0; }
int app_dw1000_init ( int HACK_role, int HACK_EUI, void (*txcallback)(const dwt_callback_data_t *), void (*rxcallback)(const dwt_callback_data_t *) ) { uint32_t devID; int err; // Start off DW1000 comms slow REG(SSI0_BASE + SSI_CR1) = 0; REG(SSI0_BASE + SSI_CPSR) = 8; REG(SSI0_BASE + SSI_CR1) |= SSI_CR1_SSE; // Reset the DW1000...for some reason dw1000_reset(); // Make sure we can talk to the DW1000 devID = dwt_readdevid(); if (devID != DWT_DEVICE_ID) { #ifdef DW_DEBUG printf("Could not read Device ID from the DW1000\r\n"); printf("Possible the chip is asleep...\r\n"); #endif return -1; } // Select which of the three antennas on the board to use dw1000_choose_antenna(0); // Init the dw1000 hardware err = dwt_initialise(DWT_LOADUCODE | DWT_LOADLDO | DWT_LOADTXCONFIG | DWT_LOADXTALTRIM); if (err != DWT_SUCCESS) { return -1; } // Setup interrupts // Note: using auto rx re-enable so don't need to trigger on error frames dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | DWT_INT_SFDT | DWT_INT_RFTO | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFSL | DWT_INT_RXPTO | DWT_INT_SFDT, 1); // Configure the callbacks from the dwt library dwt_setcallbacks(txcallback, rxcallback); // Set the parameters of ranging and channel and whatnot global_ranging_config.chan = 2; global_ranging_config.prf = DWT_PRF_64M; global_ranging_config.txPreambLength = DWT_PLEN_64;//DWT_PLEN_4096 // global_ranging_config.txPreambLength = DWT_PLEN_256; global_ranging_config.rxPAC = DWT_PAC8; global_ranging_config.txCode = 9; // preamble code global_ranging_config.rxCode = 9; // preamble code global_ranging_config.nsSFD = 0; global_ranging_config.dataRate = DWT_BR_6M8; global_ranging_config.phrMode = DWT_PHRMODE_EXT; //Enable extended PHR mode (up to 1024-byte packets) global_ranging_config.smartPowerEn = 1; global_ranging_config.sfdTO = 64+8+1;//(1025 + 64 - 32); dwt_configure(&global_ranging_config, 0);//(DWT_LOADANTDLY | DWT_LOADXTALTRIM)); dwt_setsmarttxpower(global_ranging_config.smartPowerEn); // Configure TX power { global_tx_config.PGdly = pgDelay[global_ranging_config.chan]; global_tx_config.power = txPower[global_ranging_config.chan]; dwt_configuretxrf(&global_tx_config); } /* All constants same anyway if(DW1000_ROLE_TYPE == TAG) dwt_xtaltrim(xtaltrim[0]); else dwt_xtaltrim(xtaltrim[ANCHOR_EUI]); */ dwt_xtaltrim(xtaltrim[0]); ////TEST 1: XTAL trim calibration //dwt_configcwmode(global_ranging_config.chan); //dwt_xtaltrim(8); //while(1); //{ // //TEST 2: TX Power level calibration // uint8_t msg[127] = "The quick brown fox jumps over the lazy dog. The quick brown fox jumps over the lazy dog. The quick brown fox jumps over the l"; // dwt_configcontinuousframemode(0x1000); // dwt_writetxdata(127, (uint8 *) msg, 0) ; // dwt_writetxfctrl(127, 0); // dwt_starttx(DWT_START_TX_IMMEDIATE); // while(1); //} // Configure the antenna delay settings { uint16_t antenna_delay; //Antenna delay not really necessary if we're doing an end-to-end calibration antenna_delay = 0; dwt_setrxantennadelay(antenna_delay); dwt_settxantennadelay(antenna_delay); //global_tx_antenna_delay = antenna_delay; //// Shift this over a bit for some reason. Who knows. //// instance_common.c:508 //antenna_delay = dwt_readantennadelay(global_ranging_config.prf) >> 1; //if (antenna_delay == 0) { // printf("resetting antenna delay\r\n"); // // If it's not in the OTP, use a magic value from instance_calib.c // antenna_delay = ((DWT_PRF_64M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS); // dwt_setrxantennadelay(antenna_delay); // dwt_settxantennadelay(antenna_delay); //} //global_tx_antenna_delay = antenna_delay; //printf("tx antenna delay: %u\r\n", antenna_delay); } // // Set the sleep delay. Not sure what this does actually. // instancesettagsleepdelay(POLL_SLEEP_DELAY, BLINK_SLEEP_DELAY); // Configure as either a tag or anchor if (HACK_role == ANCHOR) { uint8_t eui_array[8]; // Enable frame filtering dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN); dw1000_populate_eui(eui_array, HACK_EUI); dwt_seteui(eui_array); dwt_setpanid(DW1000_PANID); // We do want to enable auto RX dwt_setautorxreenable(1); // Let's do double buffering dwt_setdblrxbuffmode(0); // Disable RX timeout by setting to 0 dwt_setrxtimeout(0); // Go for receiving dwt_rxenable(0); } else if (HACK_role == TAG) { uint8_t eui_array[8]; // Allow data and ack frames dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN); dw1000_populate_eui(eui_array, HACK_EUI); dwt_seteui(eui_array); dwt_setpanid(DW1000_PANID); // Do this for the tag too dwt_setautorxreenable(1); dwt_setdblrxbuffmode(1); dwt_enableautoack(5 /*ACK_RESPONSE_TIME*/); // Configure sleep { int mode = DWT_LOADUCODE | DWT_PRESRV_SLEEP | DWT_CONFIG | DWT_TANDV; if (dwt_getldotune() != 0) { // If we need to use LDO tune value from OTP kick it after sleep mode |= DWT_LOADLDO; } // NOTE: on the EVK1000 the DEEPSLEEP is not actually putting the // DW1000 into full DEEPSLEEP mode as XTAL is kept on dwt_configuresleep(mode, DWT_WAKE_CS | DWT_SLP_EN); } } // Make it fast REG(SSI0_BASE + SSI_CR1) = 0; REG(SSI0_BASE + SSI_CPSR) = 2; REG(SSI0_BASE + SSI_CR1) |= SSI_CR1_SSE; return 0; }
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); } } }
// First (generic) init of the DW1000 dw1000_err_e dw1000_init () { // Do the STM setup that initializes pin and peripherals and whatnot. if (!_stm_dw1000_interface_setup) { setup(); } // Reset the dw1000...for some reason dw1000_reset(); uDelay(100); // Make sure we can talk to the DW1000 uint32_t devID; devID = dwt_readdevid(); if (devID != DWT_DEVICE_ID) { //if we can't talk to dw1000, return with an error uDelay(1000); return DW1000_COMM_ERR; } GPIO_WriteBit(STM_GPIO3_PORT, STM_GPIO3_PIN, Bit_SET); uDelay(1000); GPIO_WriteBit(STM_GPIO3_PORT, STM_GPIO3_PIN, Bit_RESET); // Choose antenna 0 as a default dw1000_choose_antenna(0); // Initialize the dw1000 hardware uint32_t err; err = dwt_initialise(DWT_LOADUCODE | DWT_LOADLDO | DWT_LOADTXCONFIG | DWT_LOADXTALTRIM); if (err != DWT_SUCCESS) { return DW1000_COMM_ERR; } // Configure interrupts and callbacks dwt_setinterrupt(0xFFFFFFFF, 0); dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFSL | DWT_INT_RFTO | DWT_INT_RXPTO | DWT_INT_SFDT | DWT_INT_ARFE, 1); dwt_setcallbacks(txcallback, rxcallback); // Set the parameters of ranging and channel and whatnot global_ranging_config.chan = 2; global_ranging_config.prf = DWT_PRF_64M; global_ranging_config.txPreambLength = DWT_PLEN_64; global_ranging_config.rxPAC = DWT_PAC8; global_ranging_config.txCode = 9; // preamble code global_ranging_config.rxCode = 9; // preamble code global_ranging_config.nsSFD = 0; global_ranging_config.dataRate = DWT_BR_6M8; global_ranging_config.phrMode = DWT_PHRMODE_EXT; //Enable extended PHR mode (up to 1024-byte packets) global_ranging_config.smartPowerEn = 1; global_ranging_config.sfdTO = 64+8+1;//(1025 + 64 - 32); #if DW1000_USE_OTP dwt_configure(&global_ranging_config, (DWT_LOADANTDLY | DWT_LOADXTALTRIM)); #else dwt_configure(&global_ranging_config, 0); #endif dwt_setsmarttxpower(global_ranging_config.smartPowerEn); // Configure TX power based on the channel used global_tx_config.PGdly = pgDelay[global_ranging_config.chan]; global_tx_config.power = txPower[global_ranging_config.chan]; dwt_configuretxrf(&global_tx_config); // Need to set some radio properties. Ideally these would come from the // OTP memory on the DW1000 #if DW1000_USE_OTP == 0 // This defaults to 8. Don't know why. dwt_xtaltrim(8); // Antenna delay we don't really care about so we just use 0 dwt_setrxantennadelay(DW1000_ANTENNA_DELAY_RX); dwt_settxantennadelay(DW1000_ANTENNA_DELAY_TX); #endif return DW1000_NO_ERR; }
void listener_run(void) { uint32_t announce_coord_ts = 0; uint32_t elapsed = 0; uint32_t last_ts = 0; uint32_t count = 0; irq_init(); pio_disable_interrupt(DW_IRQ_PIO, DW_IRQ_MASK); // Setup DW1000 dwt_txconfig_t txconfig; // Setup DECAWAVE reset_DW1000(); spi_set_rate_low(); dwt_initialise(DWT_LOADUCODE); spi_set_rate_high(); dwt_configure(&cph_config->dwt_config); dwt_setpanid(0x4350); dwt_setaddress16(0x1234); // Clear CLKPLL_LL dwt_write32bitreg(SYS_STATUS_ID, 0x02000000); uint32_t id = dwt_readdevid(); printf("Device ID: %08X\r\n", id); #if 1 dwt_setcallbacks(0, rxcallback); 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) { elapsed = cph_get_millis() - last_ts; if (elapsed > 5000) { printf("alive %d\r\n", count++); last_ts = cph_get_millis(); } if (trx_signal == SIGNAL_RCV) { printf("[RCV] %d - ", frame_len); for (int i = 0; i < frame_len; i++) { printf("%02X ", rx_buffer[i]); } printf("\r\n"); trx_signal = SIGNAL_EMPTY; dwt_rxenable(0); } else if(trx_signal == SIGNAL_ERR) { printf("ERROR: %08X\r\n", error_status_reg); trx_signal = SIGNAL_EMPTY; dwt_rxenable(0); } else if(trx_signal == SIGNAL_ERR_LEN) { printf("ERROR LENGTH: %08X\r\n", error_status_reg); trx_signal = SIGNAL_EMPTY; dwt_rxenable(0); } } #else while (1) { /* Activate reception immediately. */ 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) { uint32 frame_len; dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG); frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023; if (frame_len <= MAXRXSIXZE) { dwt_readrxdata(rx_buffer, frame_len, 0); } else { frame_len = 0; } if (frame_len > 0) { printf("[RCV] "); for (int i = 0; i < frame_len; i++) { printf("%02X ", rx_buffer[i]); } printf("\r\n"); } else { printf("ERROR: frame_len == %d\r\n", frame_len); } } else { printf("ERROR: dwt_rxenable has status of %08X\r\n", status_reg); /* Clear RX error events in the DW1000 status register. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR | SYS_STATUS_CLKPLL_LL); } } #endif }
// ------------------------------------------------------------------------------------------------------------------- // // the main instance state machine (all the instance modes Tag, Anchor or Listener use the same statemachine....) // // ------------------------------------------------------------------------------------------------------------------- // int testapprun(instance_data_t *inst, int message) { switch (inst->testAppState) { case TA_INIT : #if defined(DEBUG) printf("TA_INIT") ; #endif switch (inst->mode) { case TAG: { uint16 sleep_mode = 0; dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN); //allow data, ack frames; dwt_setpanid(inst->panID); memcpy(inst->eui64, &inst->instanceAddress16, ADDR_BYTE_SIZE_S); dwt_seteui(inst->eui64); //set source address inst->newRangeTagAddress = inst->instanceAddress16 ; dwt_setaddress16(inst->instanceAddress16); //Start off by Sleeping 1st -> set instToSleep to TRUE inst->nextState = TA_TXPOLL_WAIT_SEND; inst->testAppState = TA_TXE_WAIT; inst->instToSleep = TRUE ; inst->rangeNum = 0; inst->tagSleepCorrection = 0; sleep_mode = (DWT_LOADUCODE|DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV); if((dwt_getldotune() != 0)) //if we need to use LDO tune value from OTP kick it after sleep sleep_mode |= DWT_LOADLDO; if(inst->configData.txPreambLength == DWT_PLEN_64) //if using 64 length preamble then use the corresponding OPSet sleep_mode |= DWT_LOADOPSET; #if (DEEP_SLEEP == 1) dwt_configuresleep(sleep_mode, DWT_WAKE_WK|DWT_WAKE_CS|DWT_SLP_EN); //configure the on wake parameters (upload the IC config settings) #endif instanceconfigframeheader16(inst); inst->instanceWakeTime = portGetTickCount(); } break; case ANCHOR: { memcpy(inst->eui64, &inst->instanceAddress16, ADDR_BYTE_SIZE_S); dwt_seteui(inst->eui64); dwt_setpanid(inst->panID); //set source address inst->shortAdd_idx = (inst->instanceAddress16 & 0x3) ; dwt_setaddress16(inst->instanceAddress16); //if address = 0x8000 if(inst->instanceAddress16 == GATEWAY_ANCHOR_ADDR) { inst->gatewayAnchor = TRUE; } dwt_enableframefilter(DWT_FF_NOTYPE_EN); //allow data, ack frames; // First time anchor listens we don't do a delayed RX dwt_setrxaftertxdelay(0); //change to next state - wait to receive a message inst->testAppState = TA_RXE_WAIT ; dwt_setrxtimeout(0); dwt_setpreambledetecttimeout(0); instanceconfigframeheader16(inst); } break; case LISTENER: { dwt_enableframefilter(DWT_FF_NOTYPE_EN); //disable frame filtering dwt_setrxaftertxdelay(0); //no delay of turning on of RX dwt_setrxtimeout(0); dwt_setpreambledetecttimeout(0); //change to next state - wait to receive a message inst->testAppState = TA_RXE_WAIT ; } break ; // end case TA_INIT default: break; } break; // end case TA_INIT case TA_SLEEP_DONE : { event_data_t* dw_event = instance_getevent(10); //clear the event from the queue // waiting for timout from application to wakup IC if (dw_event->type != DWT_SIG_RX_TIMEOUT) { // if no pause and no wake-up timeout continu waiting for the sleep to be done. inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //wait here for sleep timeout break; } inst->done = INST_NOT_DONE_YET; inst->instToSleep = FALSE ; inst->testAppState = inst->nextState; inst->nextState = 0; //clear inst->instanceWakeTime = portGetTickCount(); // Record the time count when we wake-up #if (DEEP_SLEEP == 1) { uint32 x = 0; //wake up device from low power mode //NOTE - in the ARM code just drop chip select for 200us //led_on(LED_PC9); port_SPIx_clear_chip_select(); //CS low instance_data[0].dwIDLE = 0; //reset DW1000 IDLE flag setup_DW1000RSTnIRQ(1); //enable RSTn IRQ Sleep(2); //200 us to wake up - need 2 as Sleep(1) is ~ 175 us //then wait 5ms for DW1000 XTAL to stabilise - instead of wait we wait for RSTn to go high //Sleep(5); Sleep(83); //need to poll to check when the DW1000 is in IDLE, the CPLL interrupt is not reliable //when RSTn goes high the DW1000 is in INIT, it will enter IDLE after PLL lock (in 5 us) /*while(instance_data[0].dwIDLE == 0) // this variable will be sent in the IRQ (process_dwRSTn_irq) { //wait for DW1000 to go to IDLE state RSTn pin to go high x++; }*/ setup_DW1000RSTnIRQ(0); //disable RSTn IRQ port_SPIx_set_chip_select(); //CS high //!!! NOTE it takes ~35us for the DW1000 to download AON and lock the PLL and be in IDLE state //do some dummy reads of the dev ID register to make sure DW1000 is in IDLE before setting LEDs x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz) x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz) x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz) x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz) x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz) /*if(x != DWT_DEVICE_ID) { x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz) }*/ //led_off(LED_PC9); //this is platform dependent - only program if DW EVK/EVB dwt_setleds(1); //MP bug - TX antenna delay needs reprogramming as it is not preserved (only RX) dwt_settxantennadelay(inst->txAntennaDelay) ; //set EUI as it will not be preserved unless the EUI is programmed and loaded from NVM dwt_seteui(inst->eui64); } #else Sleep(3); //to approximate match the time spent in the #if above #endif instancesetantennadelays(); //this will update the antenna delay if it has changed instancesettxpower(); //configure TX power if it has changed } break; case TA_TXE_WAIT : //either go to sleep or proceed to TX a message #if defined(DEBUG) printf("TA_TXE_WAIT\n") ; #endif //if we are scheduled to go to sleep before next transmission then sleep first. if((inst->nextState == TA_TXPOLL_WAIT_SEND) && (inst->instToSleep) //go to sleep before sending the next poll/ starting new ranging exchange ) { inst->rangeNum++; //increment the range number before going to sleep //the app should put chip into low power state and wake up after tagSleepTime_ms time... //the app could go to *_IDLE state and wait for uP to wake it up... inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT_TO; //don't sleep here but kick off the Sleep timer countdown inst->testAppState = TA_SLEEP_DONE; { #if (DEEP_SLEEP == 1) //put device into low power mode dwt_entersleep(); //go to sleep #endif //DW1000 gone to sleep - report the received range inst->newRange = instance_calcranges(&inst->tofArray[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_T2A, &inst->rxResponseMask); inst->rxResponseMaskReport = inst->rxResponseMask; inst->rxResponseMask = 0; inst->newRangeTime = portGetTickCount() ; } } else //proceed to configuration and transmission of a frame { inst->testAppState = inst->nextState; inst->nextState = 0; //clear } break ; // end case TA_TXE_WAIT case TA_TXPOLL_WAIT_SEND : { #if defined(DEBUG) printf("TA_TXPOLL_WAIT_SEND\n") ; #endif inst->msg_f.messageData[POLL_RNUM] = (inst->mode == TAG) ? inst->rangeNum : inst->rangeNumAnc; //copy new range number inst->msg_f.messageData[FCODE] = (inst->mode == TAG) ? RTLS_DEMO_MSG_TAG_POLL : RTLS_DEMO_MSG_ANCH_POLL; //message function code (specifies if message is a poll, response or other...) inst->psduLength = (TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC); inst->msg_f.seqNum = inst->frameSN++; //copy sequence number and then increment inst->msg_f.sourceAddr[0] = inst->eui64[0]; //copy the address inst->msg_f.sourceAddr[1] = inst->eui64[1]; //copy the address inst->msg_f.destAddr[0] = 0xff; //set the destination address (broadcast == 0xffff) inst->msg_f.destAddr[1] = 0xff; //set the destination address (broadcast == 0xffff) dwt_writetxdata(inst->psduLength, (uint8 *) &inst->msg_f, 0) ; // write the frame data //set the delayed rx on time (the response message will be sent after this delay (from A0)) dwt_setrxaftertxdelay((uint32)RX_RESPONSE1_TURNAROUND); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX) if(inst->mode == TAG) { inst->rxResps[inst->rangeNum] = 0; //reset the number of received responses inst->responseTO = MAX_ANCHOR_LIST_SIZE; //expecting 4 responses dwt_setrxtimeout((uint16)inst->fwtoTime_sy * MAX_ANCHOR_LIST_SIZE); //configure the RX FWTO } else { inst->rxResps[inst->rangeNumAnc] = 0; //reset number of responses inst->responseTO = NUM_EXPECTED_RESPONSES_ANC0; //2 responses A1, A2 dwt_setrxtimeout((uint16)inst->fwtoTime_sy * (NUM_EXPECTED_RESPONSES_ANC0)); //units are } inst->rxResponseMask = 0; //reset/clear the mask of received responses when tx poll inst->rxResponseMaskAnc = 0; inst->wait4ack = DWT_RESPONSE_EXPECTED; //response is expected - automatically enable the receiver dwt_writetxfctrl(inst->psduLength, 0); //write frame control dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); //transmit the frame inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation inst->previousState = TA_TXPOLL_WAIT_SEND ; inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set above) } break; case TA_TXFINAL_WAIT_SEND : { //the final has the same range number as the poll (part of the same ranging exchange) inst->msg_f.messageData[POLL_RNUM] = (inst->mode == TAG) ? inst->rangeNum : inst->rangeNumAnc; //the mask is sent so the anchors know whether the response RX time is valid inst->msg_f.messageData[VRESP] = (inst->mode == TAG) ? inst->rxResponseMask : inst->rxResponseMaskAnc; inst->msg_f.messageData[FCODE] = (inst->mode == TAG) ? RTLS_DEMO_MSG_TAG_FINAL : RTLS_DEMO_MSG_ANCH_FINAL; //message function code (specifies if message is a poll, response or other...) inst->psduLength = (TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC); inst->msg_f.seqNum = inst->frameSN++; dwt_writetxdata(inst->psduLength, (uint8 *) &inst->msg_f, 0) ; // write the frame data inst->wait4ack = 0; //clear the flag not using wait for response as this message ends the ranging exchange if(instancesenddlypacket(inst, DWT_START_TX_DELAYED)) { // initiate the re-transmission if(inst->mode == TAG) { inst->testAppState = TA_TXE_WAIT ; //go to TA_TXE_WAIT first to check if it's sleep time inst->nextState = TA_TXPOLL_WAIT_SEND ; } else { //A0 - failed to send Final //A1 - failed to send Final //go back to RX and behave as anchor instance_backtoanchor(inst); } break; //exit this switch case... } else { inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation inst->previousState = TA_TXFINAL_WAIT_SEND; } if(inst->mode == TAG) { inst->instToSleep = TRUE ; } inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set above) } break; case TA_TX_WAIT_CONF : #if defined(DEBUG) printf("TA_TX_WAIT_CONF %d m%d states %08x %08x\n", inst->previousState, message, dwt_read32bitreg(0x19), dwt_read32bitreg(0x0f)) ; #endif { event_data_t* dw_event = instance_getevent(11); //get and clear this event //NOTE: Can get the ACK before the TX confirm event for the frame requesting the ACK //this happens because if polling the ISR the RX event will be processed 1st and then the TX event //thus the reception of the ACK will be processed before the TX confirmation of the frame that requested it. if(dw_event->type != DWT_SIG_TX_DONE) //wait for TX done confirmation { if(dw_event->type != 0) { if(dw_event->type == DWT_SIG_RX_TIMEOUT) //got RX timeout - i.e. did not get the response (e.g. ACK) { #if defined(DEBUG) printf("RX timeout in TA_TX_WAIT_CONF (%d)\n", inst->previousState); #endif //we need to wait for SIG_TX_DONE and then process the timeout and re-send the frame if needed inst->gotTO = 1; } else { inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; } } inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; break; } inst->done = INST_NOT_DONE_YET; if(inst->previousState == TA_TXFINAL_WAIT_SEND) { if(inst->mode == TAG) { inst->testAppState = TA_TXE_WAIT ; inst->nextState = TA_TXPOLL_WAIT_SEND ; break; } else { instance_backtoanchor(inst); } } else if (inst->gotTO == 1) //timeout { #if defined(DEBUG) printf("got TO in TA_TX_WAIT_CONF\n"); #endif inst_processrxtimeout(inst); inst->gotTO = 0; inst->wait4ack = 0 ; //clear this break; } else { inst->txu.txTimeStamp = dw_event->timeStamp; if(inst->previousState == TA_TXPOLL_WAIT_SEND) { uint64 tagCalculatedFinalTxTime ; // Embed into Final message: 40-bit pollTXTime, 40-bit respRxTime, 40-bit finalTxTime if(inst->mode == TAG) { tagCalculatedFinalTxTime = (inst->txu.txTimeStamp + inst->pollTx2FinalTxDelay) & MASK_TXDTS; } else //for anchor make the final half the delay ..... (this is ok, as A0 awaits 2 responses) { tagCalculatedFinalTxTime = (inst->txu.txTimeStamp + inst->pollTx2FinalTxDelayAnc) & MASK_TXDTS; } inst->delayedReplyTime = tagCalculatedFinalTxTime >> 8; //high 32-bits // Calculate Time Final message will be sent and write this field of Final message // Sending time will be delayedReplyTime, snapped to ~125MHz or ~250MHz boundary by // zeroing its low 9 bits, and then having the TX antenna delay added // getting antenna delay from the device and add it to the Calculated TX Time tagCalculatedFinalTxTime = tagCalculatedFinalTxTime + inst->txAntennaDelay; tagCalculatedFinalTxTime &= MASK_40BIT; // Write Calculated TX time field of Final message memcpy(&(inst->msg_f.messageData[FTXT]), (uint8 *)&tagCalculatedFinalTxTime, 5); // Write Poll TX time field of Final message memcpy(&(inst->msg_f.messageData[PTXT]), (uint8 *)&inst->txu.tagPollTxTime, 5); //change the w4r for the second and remaining anchors to 50 us //dwt_setrxaftertxdelay((uint32)RX_RESPONSEX_TURNAROUND); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX) } if(inst->previousState == TA_TXRESPONSE_SENT_TORX) { inst->previousState = TA_TXRESPONSE_WAIT_SEND ; } inst->testAppState = TA_RXE_WAIT ; // After sending, tag expects response/report, anchor waits to receive a final/new poll message = 0; //fall into the next case (turn on the RX) } } //break ; // end case TA_TX_WAIT_CONF case TA_RXE_WAIT : #if defined(DEBUG) printf("TA_RXE_WAIT\n") ; #endif { if(inst->wait4ack == 0) //if this is set the RX will turn on automatically after TX { //turn RX on dwt_rxenable(DWT_START_RX_IMMEDIATE) ; // turn RX on, without delay } else { inst->wait4ack = 0 ; //clear the flag, the next time we want to turn the RX on it might not be auto } if (inst->mode != LISTENER) { inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //using RX FWTO } inst->testAppState = TA_RX_WAIT_DATA; // let this state handle it // end case TA_RXE_WAIT, don't break, but fall through into the TA_RX_WAIT_DATA state to process it immediately. if(message == 0) break; } case TA_RX_WAIT_DATA : // Wait RX data #if defined(DEBUG) printf("TA_RX_WAIT_DATA %d\n", message) ; #endif switch (message) { //if we have received a DWT_SIG_RX_OKAY event - this means that the message is IEEE data type - need to check frame control to know which addressing mode is used case DWT_SIG_RX_OKAY : { event_data_t* dw_event = instance_getevent(15); //get and clear this event uint8 srcAddr[8] = {0,0,0,0,0,0,0,0}; uint8 dstAddr[8] = {0,0,0,0,0,0,0,0}; int fcode = 0; int fn_code = 0; //int srclen = 0; //int fctrladdr_len; uint8 tof_idx = 0; uint8 *messageData; inst->stopTimer = 0; //clear the flag, as we have received a message // handle 16 and 64 bit source and destination addresses switch(dw_event->msgu.frame[1] & 0xCC) { case 0xCC: // memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ll.sourceAddr[0]), ADDR_BYTE_SIZE_L); memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ll.destAddr[0]), ADDR_BYTE_SIZE_L); fn_code = dw_event->msgu.rxmsg_ll.messageData[FCODE]; messageData = &dw_event->msgu.rxmsg_ll.messageData[0]; //srclen = ADDR_BYTE_SIZE_L; //fctrladdr_len = FRAME_CRTL_AND_ADDRESS_L; break; case 0xC8: // memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_sl.sourceAddr[0]), ADDR_BYTE_SIZE_L); memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_sl.destAddr[0]), ADDR_BYTE_SIZE_S); fn_code = dw_event->msgu.rxmsg_sl.messageData[FCODE]; messageData = &dw_event->msgu.rxmsg_sl.messageData[0]; //srclen = ADDR_BYTE_SIZE_L; //fctrladdr_len = FRAME_CRTL_AND_ADDRESS_LS; break; case 0x8C: // memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ls.sourceAddr[0]), ADDR_BYTE_SIZE_S); memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ls.destAddr[0]), ADDR_BYTE_SIZE_L); fn_code = dw_event->msgu.rxmsg_ls.messageData[FCODE]; messageData = &dw_event->msgu.rxmsg_ls.messageData[0]; //srclen = ADDR_BYTE_SIZE_S; //fctrladdr_len = FRAME_CRTL_AND_ADDRESS_LS; break; case 0x88: // memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ss.sourceAddr[0]), ADDR_BYTE_SIZE_S); memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ss.destAddr[0]), ADDR_BYTE_SIZE_S); fn_code = dw_event->msgu.rxmsg_ss.messageData[FCODE]; messageData = &dw_event->msgu.rxmsg_ss.messageData[0]; //srclen = ADDR_BYTE_SIZE_S; //fctrladdr_len = FRAME_CRTL_AND_ADDRESS_S; break; } if((inst->instToSleep == FALSE) && (inst->mode == LISTENER))//update received data, and go back to receiving frames { //do something with message data (e.g. could extract any ToFs and print them) inst->testAppState = TA_RXE_WAIT ; // wait for next frame dwt_setrxaftertxdelay(0); } else { //process ranging messages fcode = fn_code; tof_idx = srcAddr[0] & 0x3 ; switch(fcode) { case RTLS_DEMO_MSG_ANCH_POLL: case RTLS_DEMO_MSG_TAG_POLL: { inst->tagPollRxTime = dw_event->timeStamp ; //save Poll's Rx time if(fcode == RTLS_DEMO_MSG_TAG_POLL) //got poll from Tag { inst->rangeNumA[srcAddr[0]&0x7] = messageData[POLL_RNUM]; //when anchor receives a poll, we need to remember the new range number } else //got poll from Anchor (initiator) { inst->rangeNumAAnc[tof_idx] = messageData[POLL_RNUM]; //when anchor receives poll from another anchor - save the range number } if (A1_ANCHOR_ADDR == inst->instanceAddress16) //this is A1 { if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32)(srcAddr[1] << 8)))) //poll is from A0 { //configure the time A1 will poll A2 (it should be in half slot time from now) inst->a1SlotTime = dw_event->uTimeStamp + (inst->slotPeriod); //inst->instanceTimerEn = 1; - THIS IS ENABLED BELOW AFTER FINAL // - means that if final is not received then A1 will not range to A2 } } //the response has been sent - await TX done event if(dw_event->type_pend == DWT_SIG_TX_PENDING) { inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation inst->previousState = TA_TXRESPONSE_SENT_POLLRX ; //wait for TX confirmation of sent response } //already re-enabled the receiver else if (dw_event->type_pend == DWT_SIG_RX_PENDING) { //stay in RX wait for next frame... //RX is already enabled... inst->testAppState = TA_RX_WAIT_DATA ; // wait for next frame } else //the DW1000 is idle (re-enable from the application level) { //stay in RX wait for next frame... inst->testAppState = TA_RXE_WAIT ; // wait for next frame } } break; //RTLS_DEMO_MSG_TAG_POLL case RTLS_DEMO_MSG_ANCH_RESP2: case RTLS_DEMO_MSG_ANCH_RESP: { uint8 currentRangeNum = (messageData[TOFRN] + 1); //current = previous + 1 if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32)(srcAddr[1] << 8)))) //if response from gateway then use the correction factor { if(inst->mode == TAG) { // casting received bytes to int because this is a signed correction -0.5 periods to +1.5 periods inst->tagSleepCorrection = (int16) (((uint16) messageData[RES_TAG_SLP1] << 8) + messageData[RES_TAG_SLP0]); inst->tagSleepRnd = 0; // once we have initial response from Anchor #0 the slot correction acts and we don't need this anymore } } //the response has been sent - await TX done event if(dw_event->type_pend == DWT_SIG_TX_PENDING) //anchor received response from anchor ID - 1 so is sending it's response now back to tag { inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation inst->previousState = TA_TXRESPONSE_SENT_RESPRX ; //wait for TX confirmation of sent response } //already re-enabled the receiver else if(dw_event->type_pend == DWT_SIG_RX_PENDING) { // stay in TA_RX_WAIT_DATA - receiver is already enabled. } //DW1000 idle - send the final else //if(dw_event->type_pend == DWT_SIG_DW_IDLE) { if(((TAG == inst->mode) && (inst->rxResponseMask & 0x1)) //if A0's response received send the final || ((A1_ANCHOR_ADDR == inst->instanceAddress16) && (inst->rxResponseMaskAnc & 0x4)) || ((GATEWAY_ANCHOR_ADDR == inst->instanceAddress16) && (inst->rxResponseMaskAnc & 0x2)) ) //if A1's response received { inst->testAppState = TA_TXFINAL_WAIT_SEND ; // send our response / the final } else //go to sleep { if(TAG == inst->mode) { inst->testAppState = TA_TXE_WAIT ; //go to TA_TXE_WAIT first to check if it's sleep time inst->nextState = TA_TXPOLL_WAIT_SEND ; inst->instToSleep = TRUE; } else { instance_backtoanchor(inst); } } } /*else { //stay in RX wait for next frame... inst->testAppState = TA_RXE_WAIT ; // wait for next frame }*/ if(fcode == RTLS_DEMO_MSG_ANCH_RESP) //tag to anchor mode { if(currentRangeNum == inst->rangeNum) //these are the previous ranges... { //copy the ToF and put into array (array holds last 4 ToFs) memcpy(&inst->tofArray[(srcAddr[0]&0x3)], &(messageData[TOFR]), 4); //check if the ToF is valid, this makes sure we only report valid ToFs //e.g. consider the case of reception of response from anchor a1 (we are anchor a2) //if a1 got a Poll with previous Range number but got no Final, then the response will have //the correct range number but the range will be INVALID_TOF if(inst->tofArray[(srcAddr[0]&0x3)] != INVALID_TOF) { inst->rxResponseMask |= (0x1 << (srcAddr[0]&0x3)); } } else { if(inst->tofArray[(srcAddr[0]&0x3)] != INVALID_TOF) { inst->tofArray[(srcAddr[0]&0x3)] = INVALID_TOF; } } } else //anchor to anchor (only gateway processes anchor to anchor ToFs) { //report the correct set of ranges (ranges from anchors A1, A2 need to match owns range number) if((inst->gatewayAnchor)&&(currentRangeNum == inst->rangeNumAnc)) //these are the previous ranges... { inst->rangeNumAAnc[0] = inst->rangeNumAnc ; //once A0 receives A2's response then it can report the 3 ToFs. if(inst->rxResps[inst->rangeNumAnc] == 3) //if(A2_ANCHOR_ADDR == (srcAddr[0] | ((uint32)(srcAddr[1] << 8)))) { //copy the ToF and put into array, the array should have 3 ToFs A0-A1, A0-A2 and A1-A2 memcpy(&inst->tofArrayAnc[(srcAddr[0]+dstAddr[0])&0x3], &(messageData[TOFR]), 4); //calculate all anchor - anchor ranges... and report inst->newRange = instance_calcranges(&inst->tofArrayAnc[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_A2A, &inst->rxResponseMaskAnc); inst->rxResponseMaskReport = inst->rxResponseMaskAnc; inst->rxResponseMaskAnc = 0; inst->newRangeTime = dw_event->uTimeStamp ; } else { //copy the ToF and put into array (array holds last 4 ToFs) memcpy(&inst->tofArrayAnc[(srcAddr[0]+dstAddr[0])&0x3], &(messageData[TOFR]), 4); } } } } break; //RTLS_DEMO_MSG_ANCH_RESP case RTLS_DEMO_MSG_ANCH_FINAL: case RTLS_DEMO_MSG_TAG_FINAL: { int64 Rb, Da, Ra, Db ; uint64 tagFinalTxTime = 0; uint64 tagFinalRxTime = 0; uint64 tagPollTxTime = 0; uint64 anchorRespRxTime = 0; uint64 tof = INVALID_TOF; double RaRbxDaDb = 0; double RbyDb = 0; double RayDa = 0; uint8 validResp = messageData[VRESP]; uint8 index = RRXT0 + 5*(inst->shortAdd_idx); if((RTLS_DEMO_MSG_TAG_FINAL == fcode) && (inst->rangeNumA[srcAddr[0]&0x7] != messageData[POLL_RNUM])) //Final's range number needs to match Poll's or else discard this message { inst->testAppState = TA_RXE_WAIT ; // wait for next frame break; } if((RTLS_DEMO_MSG_ANCH_FINAL == fcode) && (((inst->rangeNumAAnc[tof_idx] != messageData[POLL_RNUM]) //Final's range number needs to match Poll's or else discard this message || inst->gatewayAnchor) //gateway can ignore the Final (from A1 to A2 exchange) || (A3_ANCHOR_ADDR == inst->instanceAddress16))) //A3 does not care about Final from A1 or A0 { inst->testAppState = TA_RXE_WAIT ; // wait for next frame break; } if (A1_ANCHOR_ADDR == inst->instanceAddress16) //this is A1 { if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32)(srcAddr[1] << 8)))) //final is from A0 { //ENABLE TIMER ONLY IF FINAL RECEIVED inst->instanceTimerEn = 1; } } //output data over USB... inst->newRangeAncAddress = inst->instanceAddress16; //if we got the final, maybe the tag did not get our response, so //we can use other anchors responses/ToF if there are any.. and output.. //but we cannot calculate new range if(((validResp & (0x1<<(inst->shortAdd_idx))) != 0)) { // time of arrival of Final message tagFinalRxTime = dw_event->timeStamp ; //Final's Rx time /* #if defined(DEBUG) printf("FinalRx Timestamp: %4.15e\n", convertdevicetimetosecu(dw_event.timeStamp)); #endif */ inst->delayedReplyTime = 0 ; // times measured at Tag extracted from the message buffer // extract 40bit times memcpy(&tagPollTxTime, &(messageData[PTXT]), 5); memcpy(&anchorRespRxTime, &(messageData[index]), 5); memcpy(&tagFinalTxTime, &(messageData[FTXT]), 5); // poll response round trip delay time is calculated as // (anchorRespRxTime - tagPollTxTime) - (anchorRespTxTime - tagPollRxTime) Ra = (int64)((anchorRespRxTime - tagPollTxTime) & MASK_40BIT); Db = (int64)((inst->txu.anchorRespTxTime - inst->tagPollRxTime) & MASK_40BIT); // response final round trip delay time is calculated as // (tagFinalRxTime - anchorRespTxTime) - (tagFinalTxTime - anchorRespRxTime) Rb = (int64)((tagFinalRxTime - inst->txu.anchorRespTxTime) & MASK_40BIT); Da = (int64)((tagFinalTxTime - anchorRespRxTime) & MASK_40BIT); RaRbxDaDb = (((double)Ra))*(((double)Rb)) - (((double)Da))*(((double)Db)); RbyDb = ((double)Rb + (double)Db); RayDa = ((double)Ra + (double)Da); tof = (int32) ( RaRbxDaDb/(RbyDb + RayDa) ); } //tag to anchor ranging if(RTLS_DEMO_MSG_TAG_FINAL == fcode) { inst->newRangeTagAddress = srcAddr[0] + ((uint16) srcAddr[1] << 8); //time-of-flight inst->tof[inst->newRangeTagAddress & 0x7] = tof; //calculate all tag - anchor ranges... and report inst->newRange = instance_calcranges(&inst->tofArray[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_T2A, &inst->rxResponseMask); inst->rxResponseMaskReport = inst->rxResponseMask; //copy the valid mask to report inst->rxResponseMask = 0; //we have our range - update the own mask entry... if(tof != INVALID_TOF) //check the last ToF entry is valid and copy into the current array { setTagDist(srcAddr[0], inst->shortAdd_idx); //copy distance from this anchor to the tag into array inst->rxResponseMask = (0x1 << inst->shortAdd_idx); inst->tofArray[inst->shortAdd_idx] = tof; } inst->newRangeTime = dw_event->uTimeStamp ; } else //anchor to anchor ranging { inst->newRangeTagAddress = srcAddr[0] + ((uint16) srcAddr[1] << 8); //time-of-flight inst->tofAnc[tof_idx] = tof; } //reset the response count if(inst->rxResps[inst->rxRespsIdx] >= 0) { inst->rxResps[inst->rxRespsIdx] = -1 * inst->rxResps[inst->rxRespsIdx]; if(inst->rxResps[inst->rxRespsIdx] == 0) //as A0 will have this as 0 when ranging to A1 inst->rxResps[inst->rxRespsIdx] = -1 ; } instancesetantennadelays(); //this will update the antenna delay if it has changed instancesettxpower(); // configure TX power if it has changed inst->testAppState = TA_RXE_WAIT ; // wait for next frame } break; //RTLS_DEMO_MSG_TAG_FINAL default: { //only enable receiver when not using double buffering inst->testAppState = TA_RXE_WAIT ; // wait for next frame dwt_setrxaftertxdelay(0); } break; } //end switch (fcode) if(dw_event->msgu.frame[0] & 0x20) { //as we only pass the received frame with the ACK request bit set after the ACK has been sent instance_getevent(16); //get and clear the ACK sent event } } //end else } break ; //end of DWT_SIG_RX_OKAY case DWT_SIG_RX_TIMEOUT : { event_data_t* dw_event = instance_getevent(17); //get and clear this event #if defined(DEBUG) printf("PD_DATA_TIMEOUT %d\n", inst->previousState) ; #endif //Anchor can time out and then need to send response - so will be in TX pending if(dw_event->type_pend == DWT_SIG_TX_PENDING) { inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation inst->previousState = TA_TXRESPONSE_SENT_TORX ; //wait for TX confirmation of sent response } else if(dw_event->type_pend == DWT_SIG_DW_IDLE) //if timed out and back in receive then don't process as timeout { inst_processrxtimeout(inst); } //else if RX_PENDING then wait for next RX event... message = 0; //clear the message as we have processed the event } break ; case DWT_SIG_TX_AA_DONE: //ignore this event - just process the rx frame that was received before the ACK response case 0: default : { if(message) // == DWT_SIG_TX_DONE) { inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; } if(inst->done == INST_NOT_DONE_YET) inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; } break; } break ; // end case TA_RX_WAIT_DATA default: #if defined(DEBUG) printf("\nERROR - invalid state %d - what is going on??\n", inst->testAppState) ; #endif break; } // end switch on testAppState
/* ********************************************************************************************************* * * 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; }