// Tell the anchor to start its job of being an anchor
dw1000_err_e oneway_anchor_start () {
dw1000_err_e err;
// Make sure the DW1000 is awake.
err = dw1000_wakeup();
if (err == DW1000_WAKEUP_SUCCESS) {
// We did wake the chip, so reconfigure it properly
// Put back the ANCHOR settings.
oneway_anchor_init();
} else if (err) {
// Chip did not seem to wakeup. This is not good, so we have
// to reset the application.
return err;
}
// Also we start over in case the anchor was doing anything before
_state = ASTATE_IDLE;
// Choose to wait in the first default position.
// This could change to wait in any of the first NUM_CHANNEL-1 positions.
oneway_set_ranging_broadcast_subsequence_settings(ANCHOR, 0);
// Obviously we want to be able to receive packets
dwt_rxenable(0);
return DW1000_NO_ERR;
}
void rx_mode (data_t *data)
{
dwt_setrxtimeout(data->rxTimeouts);
//Put DW1000 in rx
dwt_rxenable(0);
data->previous_state = data->current_state;
data->current_state = RX_RESP_WAIT;
}
// Tell the anchor to start its job of being an anchor
void dw1000_anchor_start () {
// Choose to wait in the first default position.
// This could change to wait in any of the first NUM_CHANNEL-1 positions.
dw1000_set_ranging_broadcast_subsequence_settings(ANCHOR, 0, TRUE);
// Also we start over in case the anchor was doing anything before
_state = ASTATE_IDLE;
// Obviously we want to be able to receive packets
dwt_rxenable(0);
}
// This is called by the periodic timer that tracks the tag's periodic
// broadcast ranging poll messages. This is responsible for setting the
// antenna and channel properties for the anchor.
static void ranging_broadcast_subsequence_task () {
// When this timer is called it is time to start a new subsequence
// slot, so we must increment our counter
_ranging_broadcast_ss_num++;
// Check if we are done listening for packets from the TAG. If we get
// a packet on the last subsequence we won't get here, but if we
// don't get that packet we need this check.
if (_ranging_broadcast_ss_num > _ranging_operation_config.reply_after_subsequence) {
ranging_listening_window_setup();
} else {
// Update the anchor listening settings
oneway_set_ranging_broadcast_subsequence_settings(ANCHOR, _ranging_broadcast_ss_num);
// And re-enable RX. The set_broadcast_settings function disables tx and rx.
dwt_rxenable(0);
}
}
dw1000_err_e dw1000_anchor_init () {
uint8_t eui_array[8];
// Make sure the radio starts off
dwt_forcetrxoff();
// Set the anchor so it only receives data and ack packets
dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN);
// Set the ID and PAN ID for this anchor
dw1000_read_eui(eui_array);
dwt_seteui(eui_array);
dwt_setpanid(POLYPOINT_PANID);
// Automatically go back to receive
dwt_setautorxreenable(TRUE);
// Don't use these
dwt_setdblrxbuffmode(FALSE);
dwt_setrxtimeout(FALSE);
// Don't receive at first
dwt_rxenable(FALSE);
// Load our EUI into the outgoing packet
dw1000_read_eui(pp_anc_final_pkt.ieee154_header_unicast.sourceAddr);
// Need a timer
_ranging_broadcast_timer = timer_init();
// Init the PRNG for determining when to respond to the tag
raninit(&_prng_state, eui_array[0]<<8|eui_array[1]);
// Make SPI fast now that everything has been setup
dw1000_spi_fast();
return DW1000_NO_ERR;
}
bool DWM1000_Anchor::dispatch(Msg& msg) {
PT_BEGIN()
PT_WAIT_UNTIL(msg.is(0, SIG_INIT));
init();
while (true) {
WAIT_POLL: {
dwt_setrxtimeout(0); /* Clear reception timeout to start next ranging process. */
dwt_rxenable(0); /* Activate reception immediately. */
// dwt_setinterrupt(DWT_INT_RFCG, 1); // enable RXD interrupt
while (true) { /* Poll for reception of a frame or error/timeout. See NOTE 7 below. */
timeout(1000);/* This is the delay from the end of the frame transmission to the enable of the receiver, as programmed for the DW1000's wait for response feature. */
clearInterrupt();
PT_YIELD_UNTIL(timeout() || isInterruptDetected());
status_reg = _status_reg;
LOG<< HEX << " status reg.:" << status_reg << " ,interrupts : " << interruptCount << FLUSH;
status_reg = dwt_read32bitreg(SYS_STATUS_ID);
LOG<< HEX << " IRQ pin : " << digitalRead(D2) << " status_reg DWM1000 " << status_reg << FLUSH;// PULL LOW
if (status_reg & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))
break;
}
}
///____________________________________________________________________________
if (status_reg & SYS_STATUS_RXFCG) {
LOG<< " $ "<<FLUSH;
uint32 frame_len;
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG); /* Clear good RX frame event in the DW1000 status register. */
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
if (frame_len <= RX_BUFFER_LEN) {
dwt_readrxdata(rx_buffer, frame_len, 0);
}
/* Check that the frame is a poll sent by "DS TWR initiator" example.
* As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
rx_buffer[ALL_MSG_SN_IDX] = 0;
if (memcmp(rx_buffer, rx_poll_msg, ALL_MSG_COMMON_LEN) == 0) {
LOG<< " $$ "<<FLUSH;
uint32 resp_tx_time;
poll_rx_ts = get_rx_timestamp_u64(); /* Retrieve poll reception timestamp. */
/* Set send time for response. See NOTE 8 below. */
resp_tx_time = (poll_rx_ts
+ (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;
dwt_setdelayedtrxtime(resp_tx_time);
/* Set expected delay and timeout for final message reception. */
dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);
dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);
/* Write and send the response message. See NOTE 9 below.*/
tx_resp_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);
dwt_writetxfctrl(sizeof(tx_resp_msg), 0);
dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);
/* We assume that the transmission is achieved correctly, now poll for reception of expected "final" frame or error/timeout.
* See NOTE 7 below. */
// while (true) { /* Poll for reception of a frame or error/timeout. See NOTE 7 below. */
timeout(10);
dwt_setinterrupt(DWT_INT_RFCG, 1);// enable
clearInterrupt();
// PT_YIELD_UNTIL(timeout() || isInterruptDetected());
status_reg = dwt_read32bitreg(SYS_STATUS_ID);
// status_reg = _status_reg;
LOG<< HEX << " status reg2:" << status_reg << FLUSH;
// if (status_reg & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))
// break;
// }
// while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
// { };
/* Increment frame sequence number after transmission of the response message (modulo 256). */
frame_seq_nb++;
if (status_reg & SYS_STATUS_RXFCG) {
LOG<< " $$$ "<<FLUSH;
/* Clear good RX frame event and TX frame sent in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID,
SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(
RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
if (frame_len <= RX_BUF_LEN) {
dwt_readrxdata(rx_buffer, frame_len, 0);
}
/* Check that the frame is a final message sent by "DS TWR initiator" example.
* As the sequence number field of the frame is not used in this example, it can be zeroed to ease the validation of the frame. */
rx_buffer[ALL_MSG_SN_IDX] = 0;
if (memcmp(rx_buffer, rx_final_msg, ALL_MSG_COMMON_LEN)
== 0) {
uint32 poll_tx_ts, resp_rx_ts, final_tx_ts;
uint32 poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
double Ra, Rb, Da, Db;
int64 tof_dtu;
/* Retrieve response transmission and final reception timestamps. */
resp_tx_ts = get_tx_timestamp_u64();
final_rx_ts = get_rx_timestamp_u64();
/* Get timestamps embedded in the final message. */
final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX],
&poll_tx_ts);
final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX],
&resp_rx_ts);
final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX],
&final_tx_ts);
/* Compute time of flight. 32-bit subtractions give correct answers even if clock has wrapped. See NOTE 10 below. */
poll_rx_ts_32 = (uint32) poll_rx_ts;
resp_tx_ts_32 = (uint32) resp_tx_ts;
final_rx_ts_32 = (uint32) final_rx_ts;
Ra = (double) (resp_rx_ts - poll_tx_ts);
Rb = (double) (final_rx_ts_32 - resp_tx_ts_32);
Da = (double) (final_tx_ts - resp_rx_ts);
Db = (double) (resp_tx_ts_32 - poll_rx_ts_32);
tof_dtu = (int64) ((Ra * Rb - Da * Db)
/ (Ra + Rb + Da + Db));
tof = tof_dtu * DWT_TIME_UNITS;
distance = tof * SPEED_OF_LIGHT;
/* Display computed distance on LCD. */
// char dist_str[20];
// sprintf(dist_str,"%3.2f", distance);
// lcd_display_str(dist_str);
LOG<< " distance : " << (float)distance << "m. " << FLUSH;
}
} else {
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
} else {
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);
}
}
}
// -------------------------------------------------------------------------------------------------------------------
//
// the main instance state machine (for Tag instance mode only!)
//
// -------------------------------------------------------------------------------------------------------------------
//
int testapprun_tf(instance_data_t *inst, int message)
{
switch (inst->testAppState)
{
case TA_INIT :
// printf("TA_INIT") ;
switch (inst->mode)
{
case TAG:
{
int mode = 0;
dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN); //allow data and ack frames;
inst->frameFilteringEnabled = 1 ;
dwt_setpanid(inst->panid);
dwt_seteui(inst->eui64);
inst->msg_f.panID[0] = (inst->panid) & 0xff;
inst->msg_f.panID[1] = inst->panid >> 8;
#if (DR_DISCOVERY == 1)
inst->mode = TAG_TDOA ;
inst->testAppState = TA_TXBLINK_WAIT_SEND;
memcpy(&inst->blinkmsg.tagID[0], &inst->eui64[0], BLINK_FRAME_SOURCE_ADDRESS);
#else
inst->testAppState = TA_TXPOLL_WAIT_SEND;
#endif
//can use RX auto re-enable when not logging/plotting errored frames
inst->rxautoreenable = 1;
dwt_setautorxreenable(inst->rxautoreenable); //not necessary to auto RX re-enable as the receiver is on for a short time (Tag knows when the response is coming)
//disable double buffer for a Tag - not needed....
dwt_setdblrxbuffmode(0); //enable/disable double RX buffer
//NOTE - Auto ACK only works if frame filtering is enabled!
dwt_enableautoack(ACK_RESPONSE_TIME); //wait for ACK_RESPONSE_TIME symbols (e.g. 5) before replying with the ACK
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;
if(inst->configData.txPreambLength == DWT_PLEN_64) //if using 64 length preamble then use the corresponding OPSet
mode |= DWT_LOADOPSET;
//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); //configure the on wake parameters (upload the IC config settings)
}
break;
default:
break;
}
break; // end case TA_INIT
case TA_SLEEP_DONE :
{
event_data_t* dw_event = instance_getevent(20); //clear the event from the queue
// waiting for timout from application to wakup IC
if (dw_event->type != DWT_SIG_RX_TIMEOUT)
{
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //wait here for sleep timeout
break;
}
inst->done = INST_NOT_DONE_YET;
inst->instToSleep = 0;
inst->testAppState = inst->nextState;
inst->nextState = 0; //clear
inst->canprintinfo = 0;
//wake up from DEEP SLEEP
{
//wake up device from low power mode
//NOTE - in the ARM code just drop chip select for 200us
port_SPIx_clear_chip_select(); //CS low
instance_data[0].dwIDLE = 0; //reset
setup_DW1000RSTnIRQ(1); //enable RSTn IRQ
Sleep(1); //200 us to wake up then waits 5ms for DW1000 XTAL to stabilise
port_SPIx_set_chip_select(); //CS high
#if (DW_IDLE_CHK==1) //Wait (sleep) to give DW1000 time to get to IDLE state
Sleep(5);
//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
dwt_settxantennadelay(inst->txantennaDelay) ;
//set EUI as it will not be preserved unless the EUI is programmed and loaded from NVM
/*if((inst->mode == TAG) || (inst->mode == TAG_TDOA))
{
dwt_setpanid(inst->panid);
dwt_seteui(inst->eui64);
}*/
#elif (DW_IDLE_CHK==2) //Use RSTn pin to notify the micro that DW1000 is in IDLE
//need to poll to check when the DW1000 is in IDLE, the CPLL interrupt is not reliable
while(instance_data[0].dwIDLE == 0); //wait for DW1000 to go to IDLE state RSTn pin to go high
if(dwt_read32bitreg(0x0) != 0xDECA0130)
{
//error?
int x = 0;
}
setup_DW1000RSTnIRQ(0); //disable RSTn IRQ
#else
//need to poll to check when the DW1000 is in IDLE, the CPLL interrupt is not reliable
while(dwt_read32bitreg(0x0) != 0xDECA0130);
//Sleep(2);
#endif
dwt_entersleepaftertx(0);
dwt_setinterrupt(DWT_INT_TFRS, 1); //re-enable the TX/RX interrupts
}
}
break;
case TA_TXE_WAIT : //either go to sleep or proceed to TX a message
// printf("TA_TXE_WAIT") ;
//if we are scheduled to go to sleep before next sending then sleep first.
if(((inst->nextState == TA_TXPOLL_WAIT_SEND)
|| (inst->nextState == TA_TXBLINK_WAIT_SEND))
&& (inst->instToSleep) //go to sleep before sending the next poll
)
{
//the app should put chip into low power state and wake up in 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 TagTimeoutTimer (instancetimer)
inst->testAppState = TA_SLEEP_DONE;
if(inst->nextState == TA_TXBLINK_WAIT_SEND)
inst->canprintinfo = 1;
//put device into low power mode
dwt_entersleep(); //go to sleep
}
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_TXBLINK_WAIT_SEND :
{
//blink frames with IEEE EUI-64 tag ID
inst->blinkmsg.frameCtrl = 0xC5 ;
inst->blinkmsg.seqNum = inst->frame_sn++;
dwt_writetxdata((BLINK_FRAME_CRTL_AND_ADDRESS + FRAME_CRC), (uint8 *) (&inst->blinkmsg), 0) ; // write the frame data
dwt_writetxfctrl((BLINK_FRAME_CRTL_AND_ADDRESS + FRAME_CRC), 0);
//response will be sent after 500us (thus delay the receiver turn on by 290sym ~ 299us)
//use delayed rx on (wait4resp timer) - this value is applied when the TX frame is done/sent, so this value can be written after TX is started
dwt_setrxaftertxdelay(inst->rnginitW4Rdelay_sy); //units are ~us - wait for wait4respTIM before RX on (delay RX)
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); //always using immediate TX
dwt_setrxtimeout(inst->fwtoTimeB_sy); //units are us - wait for BLINKRX_FWTO_TIME after RX on before timing out
#if (DW_IDLE_CHK==2)
//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
dwt_settxantennadelay(inst->txantennaDelay) ;
#endif
inst->sentSN = inst->blinkmsg.seqNum;
inst->wait4ack = DWT_RESPONSE_EXPECTED; //Poll is coming soon after...
inst->instToSleep = 1;
inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation
inst->previousState = TA_TXBLINK_WAIT_SEND ;
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
}
break ; // end case TA_TXBLINK_WAIT_SEND
case TA_TXPOLL_WAIT_SEND :
{
#if (DR_DISCOVERY == 1)
//NOTE the anchor address is set after receiving the ranging initialisation message
inst->instToSleep = 1; //go to Sleep after this poll
#else
//set destination address
if(destaddress(inst))
{
break;
}
//copy anchor address to short message structure
inst->msg_f.destAddr[0] = inst->msg.destAddr[0];
inst->msg_f.destAddr[1] = inst->msg.destAddr[1];
#endif
inst->msg_f.messageData[FCODE] = RTLS_DEMO_MSG_TAG_POLLF;
inst->psduLength = TAG_POLL_F_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC + EXTRA_LENGTH;
//set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
inst->msg_f.frameCtrl[0] = 0x41 /*PID comp*/;
//short address for both
inst->msg_f.frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0x80 /*src short address (16bits)*/;
inst->msg_f.seqNum = inst->frame_sn++;
inst->wait4ack = DWT_RESPONSE_EXPECTED; //Response is coming after 275 us...
//500 -> 485, 800 -> 765
dwt_writetxfctrl(inst->psduLength, 0);
//if the response is expected there is a 1ms timeout to stop RX if no response (ACK or other frame) coming
dwt_setrxtimeout(inst->fwtoTime_sy); //units are us - wait for 215us after RX on
//use delayed rx on (wait4resp timer)
dwt_setrxaftertxdelay(inst->fixedReplyDelay_sy); //units are ~us - wait for wait4respTIM before RX on (delay RX)
dwt_writetxdata(inst->psduLength, (uint8 *) &inst->msg_f, 0) ; // write the poll frame data
//start TX of frame
dwt_starttx(DWT_START_TX_IMMEDIATE | inst->wait4ack);
#if (DW_IDLE_CHK==2)
//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
dwt_settxantennadelay(inst->txantennaDelay) ;
#endif
inst->sentSN = inst->msg_f.seqNum;
//write the final function code
inst->msg_f.messageData[FCODE] = RTLS_DEMO_MSG_TAG_FINALF;
//increment the sequence number for the final message
inst->msg_f.seqNum = inst->frame_sn;
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 below)
inst->responseRxNum = 0;
}
break;
case TA_TX_WAIT_CONF :
//printf("TA_TX_WAIT_CONF") ;
{
//uint8 temp[5];
event_data_t* dw_event = instance_getevent(5); //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 == DWT_SIG_RX_TIMEOUT) //got RX timeout - i.e. did not get the response (e.g. ACK)
{
//we need to wait for SIG_TX_DONE and then process the timeout and re-send the frame if needed
inst->gotTO = 1;
}
if(dw_event->type == SIG_RX_ACK)
{
inst->wait4ack = 0 ; //clear the flag as the ACK has been received
inst_processackmsg(inst, dw_event->msgu.rxackmsg.seqNum);
//printf("RX ACK in TA_TX_WAIT_CONF... wait for TX confirm before changing state\n");
}
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT;
break;
}
inst->done = INST_NOT_DONE_YET;
if(inst->previousState == TA_TXFINAL_WAIT_SEND) //tag will do immediate receive when waiting for report (as anchor sends it without delay)
{
#if (DR_DISCOVERY == 1)
//in Discovery mode anchor is not sending the report to tag go to sleep
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT_TO; //kick off the TagTimeoutTimer (instance timer) to initiate wakeup
inst->nextState = TA_TXPOLL_WAIT_SEND;
inst->testAppState = TA_TXE_WAIT; //we are going manually to sleep - change to TA_TXE_WAIT state
#else
//wait for report when non-Discovery mode
if(inst->wait4ack == 0)
dwt_rxenable(0) ; // turn receiver on,
#endif
break;
}
else if (inst->gotTO) //timeout
{
inst_processrxtimeout(inst);
inst->gotTO = 0;
}
else
{
if(inst->previousState == TA_TXPOLL_WAIT_SEND)
{
// write the final's frame control and address tx data (add CRC as the function will write length - 2)
dwt_writetxdata((FRAME_CRTL_AND_ADDRESS_S + 1 + FRAME_CRC), (uint8 *) &inst->msg_f, FINAL_MSG_OFFSET) ; // write the final frame data
dwt_entersleepaftertx(1);
dwt_setinterrupt(DWT_INT_TFRS, 0); //disable all the interrupts (wont be able to enter sleep if interrupts are pending)
inst->tagPollTxTime32l = dw_event->timeStamp32l;
inst->relpyAddress[0] = inst->msg_f.destAddr[0];
inst->relpyAddress[1] = inst->msg_f.destAddr[1];
inst->canprintinfo = 2;
}
if(inst->previousState == TA_TXRANGINGINIT_WAIT_SEND) //set frame control for the response message
{
dwt_writetxfctrl((ANCH_RESPONSE_F_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC + EXTRA_LENGTH), RESPONSE_MSG_OFFSET);
}
inst->testAppState = TA_RX_WAIT_DATA ; // wait for next frame
//turn RX on
if(inst->wait4ack == 0)
dwt_rxenable(0) ; // turn receiver on, immediate = 0/delayed = 1
inst->wait4ack = 0 ;
//dwt_readfromdevice(0x19, 0, 5, temp);
//sprintf((char*)&usbdata[20], "T2R%d %02x%02x%02x%02x%02x ", count, temp[4], temp[3], temp[2], temp[1], temp[0]);
//send_usbmessage(&usbdata[20], 16);
//count=0;
}
}
break ; // end case TA_TX_WAIT_CONF
case TA_RXE_WAIT :
//printf("TA_RXE_WAIT") ;
{
// - with "fast" ranging - we only get here after frame timeout...
//turn RX on
instancerxon(inst, 0, 0) ; // turn RX on, with/without delay
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
//printf("TA_RX_WAIT_DATA") ;
switch (message)
{
case SIG_RX_BLINK :
{
instance_getevent(6); //get and clear this event
//else //not initiating ranging - continue to receive
{
inst->testAppState = TA_RX_WAIT_DATA ; // wait for next frame
//turn RX on
dwt_rxenable(0) ; // turn receiver on, immediate = 0/delayed = 1
inst->done = INST_NOT_DONE_YET;
}
}
break;
case SIG_RX_ACK :
{
event_data_t* dw_event = instance_getevent(7); //get and clear this event
inst_processackmsg(inst, dw_event->msgu.rxackmsg.seqNum);
//else we did not expect this ACK turn the RX on again
//only enable receiver when not using double buffering
inst->testAppState = TA_RX_WAIT_DATA ; // wait for next frame
//turn RX on
dwt_rxenable(0) ; // turn receiver on, immediate = 0/delayed = 1
inst->done = INST_NOT_DONE_YET;
}
break;
case DWT_SIG_RX_OKAY :
{
event_data_t* dw_event = instance_getevent(8); //get and clear this event
uint8 srcAddr[8] = {0,0,0,0,0,0,0,0};
int fcode = 0;
int fn_code = 0;
int srclen = 0;
int fctrladdr_len;
uint8 *messageData;
inst->stoptimer = 0; //clear the flag, as we have received a message
// 16 or 64 bit addresses
switch(dw_event->msgu.frame[1])
{
case 0xCC: //
memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ll.sourceAddr[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);
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);
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);
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->ackexpected) && (inst->ackTO)) //ACK frame was expected but we got a good frame - treat as ACK timeout
{
//printf("got good frame instead of ACK in DWT_SIG_RX_OKAY - pretend TO\n");
inst_processrxtimeout(inst);
message = 0; //clear the message as we have processed the event
}
else
{
inst->ackexpected = 0; //clear this as we got good frame (but as not using ACK TO) we prob missed the ACK - check if it has been addressed to us
fcode = fn_code; //tag has address filtering so if it received a frame it must be addressed to it
switch(fcode)
{
case RTLS_DEMO_MSG_RNG_INIT:
{
if(inst->mode == TAG_TDOA) //only start ranging with someone if not ranging already
{
//double delay = rxrngmsg->messageData[RES_T1] + (rxrngmsg->messageData[RES_T2] << 8); //in ms
inst->testAppState = TA_TXPOLL_WAIT_SEND ; // send next poll
//remember the anchor address
inst->msg_f.destAddr[0] = srcAddr[0];
inst->msg_f.destAddr[1] = srcAddr[1];
inst->msg_f.sourceAddr[0] = messageData[RES_R1];
inst->msg_f.sourceAddr[1] = messageData[RES_R1+1];
inst->tagShortAdd = (uint16)messageData[RES_R1] + ((uint16)messageData[RES_R2] << 8) ;
dwt_setaddress16(inst->tagShortAdd);
//instancesetreplydelay(delay); //
inst->mode = TAG ;
inst->rxTimeouts = 0; //reset timeout count
inst->instToSleep = 0; //don't go to sleep - start ranging instead and then sleep after 1 range is done
inst->done = INST_NOT_DONE_YET;
}
}
break; //RTLS_DEMO_MSG_RNG_INITF
case RTLS_DEMO_MSG_ANCH_RESPF:
{
#if (TWSYMRANGE == 1)
//need to write the delayed time before starting transmission
inst->delayedReplyTime32 = ((uint32)dw_event->timeStamp32h + (uint32)inst->fixedFastReplyDelay32h) ;
dwt_setdelayedtrxtime(inst->delayedReplyTime32) ;
dwt_writetxfctrl((TAG_FINAL_F_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC), FINAL_MSG_OFFSET);
if(dwt_starttx(DWT_START_TX_DELAYED))
{
//error - TX FAILED
// initiate the re-transmission
inst->testAppState = TA_TXE_WAIT ;
inst->nextState = TA_TXPOLL_WAIT_SEND;
dwt_entersleepaftertx(0);
inst->wait4ack = 0; //clear the flag as the TX has
inst->lateTX++;
break; //exit this switch case...
}
else
{
rtd_t rtd;
//calculate the difference between response rx and final tx
//here we just need to subtract the low 32 bits as the response delay is < 32bits (actually it is < 26 bits)
rtd.diffRmP = (uint32)dw_event->timeStamp32l - (uint32)inst->tagPollTxTime32l ;
//calculate difference between final tx and response rx
rtd.diffFmR = (uint32)inst->txantennaDelay + ((uint32)inst->fixedFastReplyDelay32h << 8) - ((uint32)dw_event->timeStamp32l & 0x1FF);
//write the rest of the message (the two response time differences (low 32 bits)
dwt_writetxdata((TAG_FINAL_F_MSG_LEN - 1 + FRAME_CRC), (uint8 *) &rtd, (FINAL_MSG_OFFSET+FRAME_CRTL_AND_ADDRESS_S+1)) ; // write the frame data
inst->sentSN = inst->msg_f.seqNum;
inst->previousState = TA_TXFINAL_WAIT_SEND;
//if Tag is not waiting for report - it will go to sleep automatically after the final is sent
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT_TO; //kick off the TagTimeoutTimer (instancetimer) to initiate wakeup
inst->testAppState = TA_SLEEP_DONE; //we are going automatically to sleep so no TX confirm interrupt (next state will be TA_SLEEP_DONE)
inst->canprintinfo = 1;
inst->txmsgcount ++;
inst->frame_sn++ ; //increment as final is sent
}
inst->respPSC = (dwt_read16bitoffsetreg(0x10, 2) >> 4);
inst->wait4ack = 0; //no response
inst->ackexpected = !ACK_REQUESTED ; //used to ignore unexpected ACK frames
//inst->rxu.anchorRespRxTime = inst->rxu.rxTimeStamp ; //Response's Rx time
inst->nextState = TA_TXPOLL_WAIT_SEND;
#else
if(inst->responseRxNum == 0) // this is first response
{
dwt_setrxtimeout(5000); //~5ms
//turn RX on
dwt_rxenable(0) ; // turn receiver on, immediate = 0/delayed = 1
inst->anchResp1RxTime32l = dw_event->timeStamp32l;
inst->responseRxNum++;
}
else // we have two responses and can calculate ToF
{
//the first response will be sent time X after reception of the poll, but as the tx time is snapped to 8ns
//we need to account for the low 9 bits of poll rx time in the RTD calculation
uint32 pollrxlowbits = (uint32)messageData[1] + (uint32)(messageData[2] << 8);
//RTD = (RxResp1 - TxPoll) - (RxResp2 - RxResp1)
//ToF = RTD/2 = RxResp1 - 0.5 * (TxPoll + RxResp2)
inst->tof32 = ((uint32)inst->anchResp1RxTime32l - (uint32)inst->tagPollTxTime32l + pollrxlowbits) - ((uint32)dw_event->timeStamp32l - (uint32)inst->anchResp1RxTime32l);
inst->tof32 <<= 1; //to make it compatible with reportTOF() which expects ToF*4
reportTOF_f(inst);
inst->newrange = 1;
inst->testAppState = TA_TXE_WAIT ;
inst->nextState = TA_TXPOLL_WAIT_SEND;
}
#endif
}
break; //RTLS_DEMO_MSG_ANCH_RESPF
case RTLS_DEMO_MSG_ANCH_TOFRF:
{
inst->tof32 = messageData[TOFR];
inst->tof32 += (uint32)messageData[TOFR+1] << 8;
inst->tof32 += (uint32)messageData[TOFR+2] << 16;
inst->tof32 += (uint32)messageData[TOFR+3] << 24;
if(dw_event->msgu.rxmsg_ss.seqNum != inst->lastReportSN)
{
reportTOF_f(inst);
inst->newrange = 1;
inst->lastReportSN = dw_event->msgu.rxmsg_ss.seqNum;
inst->newrangetagaddress = srcAddr[0] + ((uint16) srcAddr[1] << 8);
inst->newrangeancaddress = inst->eui64[0] + ((uint16) inst->eui64[1] << 8);
}
inst->testAppState = TA_TXE_WAIT;
inst->nextState = TA_TXPOLL_WAIT_SEND ; // send next poll
}
break; //RTLS_DEMO_MSG_ANCH_TOFRF
default:
{
//only enable receiver when not using double buffering
inst->testAppState = TA_RX_WAIT_DATA ; // wait for next frame
//turn RX on
dwt_rxenable(0) ; // turn receiver on, immediate = 0/delayed = 1
}
break;
} //end switch (rxmsg->functionCode)
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(9); //get and clear the ACK sent event
}
}
}
break ;
case DWT_SIG_RX_TIMEOUT :
//printf("PD_DATA_TIMEOUT") ;
instance_getevent(26); //get and clear this event
inst_processrxtimeout(inst);
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: //no event - wait in receive...
{
//stay in Rx (fall-through from above state)
//if(DWT_SIG_TX_AA_DONE == message) printf("Got SIG_TX_AA_DONE in RX wait - ignore\n");
if(inst->done == INST_NOT_DONE_YET) inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT;
}
break;
default :
{
//printf("\nERROR - Unexpected message %d ??\n", message) ;
//assert(0) ; // Unexpected Primitive what is going on ?
}
break ;
}
break ; // end case TA_RX_WAIT_DATA
default:
//printf("\nERROR - invalid state %d - what is going on??\n", inst->testAppState) ;
break;
} // end switch on testAppState
return inst->done;
} // end testapprun()
/**
* Application entry point.
*/
int rxWait(void)
{
/* Reset and initialise DW1000.
* For initialisation, DW1000 clocks must be temporarily set to crystal speed. After initialisation SPI rate can be increased for optimum
* performance. */
int i;
reset_DW1000(); /* Target specific drive of RSTn line into DW1000 low for a period. */
//spi_set_rate_low();
dwt_initialise(DWT_LOADNONE);
//spi_set_rate_high();
/* Configure DW1000. See NOTE 2 below. */
dwt_configure(&config);
/* Loop forever sending and receiving frames periodically. */
while (1)
{
/* Activate reception immediately. See NOTE 3 below. */
dwt_rxenable(0);
/* Poll until a frame is properly received or an error occurs. See NOTE 4 below.
* STATUS register is 5 bytes long but, as the events we are looking at are in the lower bytes of the register, we can use this simplest API
* function to access it. */
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };
printf("Status reg now 0x%x\r\n",status_reg);
if (status_reg & SYS_STATUS_RXFCG)
{
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
if (frame_len <= FRAME_LEN_MAX)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}
/* Clear good RX frame event in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);
for (i=0;i<frame_len;i++) {
printf("%x ",rx_buffer[i]);
}
printf("\r\n");
/* Validate the frame is the one expected as sent by "TX then wait for a response" example. */
if ((frame_len == 14) && (rx_buffer[0] == 0xC5) && (rx_buffer[10] == 0x43) && (rx_buffer[11] == 0x2))
{
int i;
/* Copy source address of blink in response destination address. */
for (i = 0; i < 8; i++)
{
tx_msg[DATA_FRAME_DEST_IDX + i] = rx_buffer[BLINK_FRAME_SRC_IDX + i];
}
/* Write response frame data to DW1000 and prepare transmission. See NOTE 5 below.*/
dwt_writetxdata(sizeof(tx_msg), tx_msg, 0);
dwt_writetxfctrl(sizeof(tx_msg), 0);
/* Send the response. */
dwt_starttx(DWT_START_TX_IMMEDIATE);
/* Poll DW1000 until TX frame sent event set. */
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ };
/* Clear TX frame sent event. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
/* Increment the data frame sequence number (modulo 256). */
tx_msg[DATA_FRAME_SN_IDX]++;
}
}
else
{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
printf("Some RX errors ...\r\n");
}
}
}
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
static void App_TaskStart(void *p_arg)
{
volatile uint32_t status;
int8u bufp[7] = {0xc4,0x01,0x01,0x00,0x00,0x00,0x55};
uint8_t *ptr;
uint8_t err;
(void) p_arg;
//系统滴答安装
OS_CPU_SysTickInit();
//初始化信号量
app_eventcreate();
//初始化其他任务
app_taskcreate();
//初始化基础硬件
BSP_Init();
dw1000_init();
{
dwtconf temp;
temp.config.chan = 3;
temp.config.prf = DWT_PRF_16M;
temp.config.txPreambLength = DWT_PLEN_256;
temp.config.rxPAC = DWT_PAC16;
temp.config.rxCode = 5;
temp.config.txCode = 5;
temp.config.nsSFD = 1;
temp.config.dataRate = DWT_BR_850K;
temp.config.smartPowerEn = 0;
temp.config.phrMode = 0;
temp.mode = LOCA_LISTENER;
dw1000_config(&temp);
}
dwt_rxenable(0);
USBD_Init(&USB_OTG_dev,
USB_OTG_FS_CORE_ID,
&USR_desc,
&USBD_CDC_cb,
&USR_cb);
//dw1000_intenable();
//发送log
//Shell_SendDatas(strlen(logstr), logstr);
//改变自己优先级 变为工作指示灯
//OSTaskDel(OS_PRIO_SELF);
OSTaskChangePrio(OS_PRIO_SELF, OS_LOWEST_PRIO - 4);
while(TURE)
{
// OSTimeDlyHMSM(0, 0, 0, 10);
//dwt_writetxdata(127,msg,0);
//dwt_writetxfctrl(127,0);
//dwt_starttx(DWT_START_TX_IMMEDIATE);
// bufp[1] ++;
//status = dwt_read32bitreg(SYS_STATUS_ID);
ptr = OSQPend(qmsg, 0, &err);
if (err == OS_ERR_NONE)
{
VCP_fops.pIf_DataTx(ptr+1, ptr[0]);
free(ptr);
}
}
}
// Triggered when we receive a packet
void app_dw1000_rxcallback (const dwt_callback_data_t *rxd) {
// First grab a copy of local time when this arrived
rtimer_clock_t rt_timestamp = RTIMER_NOW();
DEBUG_B6_HIGH;
if (rxd->event == DWT_SIG_RX_OKAY) {
leds_toggle(LEDS_BLUE);
uint8_t packet_type;
uint64_t dw_timestamp;
uint8_t recv_pkt_buf[RX_PKT_BUF_LEN];
// Get the dw_timestamp first
uint8_t txTimeStamp[5] = {0, 0, 0, 0, 0};
dwt_readrxtimestamp(txTimeStamp);
dw_timestamp = ((uint64_t) (*((uint32_t*) txTimeStamp))) | (((uint64_t) txTimeStamp[4]) << 32);
// Get the packet
dwt_readrxdata(recv_pkt_buf, MIN(RX_PKT_BUF_LEN, rxd->datalength), 0);
packet_type = recv_pkt_buf[offsetof(struct pp_tag_poll, message_type)];
global_round_num = recv_pkt_buf[offsetof(struct pp_tag_poll, roundNum)];
if (packet_type == MSG_TYPE_PP_ONEWAY_TAG_POLL) {
struct pp_tag_poll* pkt = (struct pp_tag_poll*) recv_pkt_buf;
DEBUG_P("TAG_POLL rx: %u\r\n", pkt->subsequence);
if (global_subseq_num == pkt->subsequence) {
// Configurations matched, record arrival time
pp_anc_final_pkt.TOAs[global_subseq_num] = dw_timestamp;
} else {
// Tag/anchor weren't on same settings, so we
// drop this sample and catch the anchor up
global_subseq_num = pkt->subsequence;
}
if (!global_round_active) {
DEBUG_B4_LOW;
DEBUG_B5_LOW;
memset(antenna_statistics, 0, sizeof(antenna_statistics));
global_round_active = true;
start_of_new_subseq = true;
substate_timer_fired = true;
/*
subseq_start_time = rt_timestamp - US_TO_RT(TAG_SQ_START_TO_POLL_SFD_HIGH_US);
rtimer_clock_t set_to = subseq_start_time + US_TO_RT(POLL_TO_SS_US+SS_TO_SQ_US);
*/
rtimer_clock_t set_to = rt_timestamp + US_TO_RT(
POLL_TO_SS_US + SS_TO_SQ_US
- TAG_SQ_START_TO_POLL_SFD_HIGH_US
- ANC_MYSTERY_STARTUP_DELAY_US);
rtimer_set(&subsequence_timer,
set_to,
1,
(rtimer_callback_t)subsequence_task,
NULL);
}
//Keep a running total of the number of packets seen from each antenna
antenna_statistics[subseq_num_to_anchor_sel(global_subseq_num)]++;
} else if (packet_type == MSG_TYPE_PP_ONEWAY_TAG_FINAL) {
if (!global_round_active) {
// The first packet we happened to receive was
// an ANC_FINAL. We have nothing interesting to
// reply with, so don't. But we *do* need to set
// receive mode again so that a new poll will be
// caught
dwt_rxenable(0);
return;
}
//We're likely in RX mode, so we need to exit before transmission
dwt_forcetrxoff();
pp_anc_final_pkt.TOAs[NUM_MEASUREMENTS] = dw_timestamp;
pp_anc_final_pkt.header.seqNum++;
const uint16 frame_len = sizeof(struct pp_anc_final);
dwt_writetxfctrl(frame_len, 0);
//Schedule this transmission for our scheduled time slot
DEBUG_B6_LOW;
uint32_t temp = dwt_readsystimestamphi32();
uint32_t delay_time = temp +
DW_DELAY_FROM_US(
ANC_FINAL_INITIAL_DELAY_HACK_VALUE +
(ANC_FINAL_RX_TIME_ON_TAG*(ANCHOR_EUI-1))
);
/* I don't understand what exactly is going on
* here. The chip seems to want way longer for
* this preamble than others -- maybe something
* to do with the large payload? From empirical
* measurements, the 300 base delay is about the
* minimum (250 next tested as not working)
DW_DELAY_FROM_US(
REVISED_DELAY_FROM_PKT_LEN_US(frame_len) +
(2*ANC_FINAL_RX_TIME_ON_TAG*(ANCHOR_EUI-1))
);
*/
delay_time &= 0xFFFFFFFE;
pp_anc_final_pkt.dw_time_sent = delay_time;
dwt_setdelayedtrxtime(delay_time);
int err = dwt_starttx(DWT_START_TX_DELAYED);
dwt_settxantennadelay(TX_ANTENNA_DELAY);
dwt_writetxdata(frame_len, (uint8_t*) &pp_anc_final_pkt, 0);
DEBUG_B6_HIGH;
#ifdef DW_DEBUG
// No printing until after all dwt timing op's
struct pp_tag_poll* pkt = (struct pp_tag_poll*) recv_pkt_buf;
DEBUG_P("TAG_FINAL rx: %u\r\n", pkt->subsequence);
#endif
if (err) {
#ifdef DW_DEBUG
uint32_t now = dwt_readsystimestamphi32();
DEBUG_P("Could not send anchor response\r\n");
DEBUG_P(" -- sched time %lu, now %lu (diff %lu)\r\n", delay_time, now, now-delay_time);
leds_on(LEDS_RED);
#endif
} else {
DEBUG_P("Send ANC_FINAL\r\n");
leds_off(LEDS_RED);
}
} else {
DEBUG_P("*** ERR: RX Unknown packet type: 0x%X\r\n", packet_type);
}
} else {
// Called when the radio has received a packet.
static void anchor_rxcallback (const dwt_callback_data_t *rxd) {
if (rxd->event == DWT_SIG_RX_OKAY) {
// Read in parameters of this packet reception
uint64_t dw_rx_timestamp;
uint8_t buf[ONEWAY_ANCHOR_MAX_RX_PKT_LEN];
uint8_t message_type;
// Get the received time of this packet first
dwt_readrxtimestamp(buf);
dw_rx_timestamp = DW_TIMESTAMP_TO_UINT64(buf);
// Get the actual packet bytes
dwt_readrxdata(buf, MIN(ONEWAY_ANCHOR_MAX_RX_PKT_LEN, rxd->datalength), 0);
// We process based on the first byte in the packet. How very active
// message like...
message_type = buf[offsetof(struct pp_tag_poll, message_type)];
if (message_type == MSG_TYPE_PP_NOSLOTS_TAG_POLL) {
// This is one of the broadcast ranging packets from the tag
struct pp_tag_poll* rx_poll_pkt = (struct pp_tag_poll*) buf;
// Decide what to do with this packet
if (_state == ASTATE_IDLE) {
// We are currently not ranging with any tags.
if (rx_poll_pkt->subsequence < NUM_RANGING_CHANNELS) {
// We are idle and this is one of the first packets
// that the tag sent. Start listening for this tag's
// ranging broadcast packets.
_state = ASTATE_RANGING;
// Clear memory for this new tag ranging event
memset(pp_anc_final_pkt.TOAs, 0, sizeof(pp_anc_final_pkt.TOAs));
memset(_anchor_antenna_recv_num, 0, sizeof(_anchor_antenna_recv_num));
// Record the EUI of the tag so that we don't get mixed up
memcpy(pp_anc_final_pkt.ieee154_header_unicast.destAddr, rx_poll_pkt->header.sourceAddr, 8);
// Record which ranging subsequence the tag is on
_ranging_broadcast_ss_num = rx_poll_pkt->subsequence;
// Record the timestamp. Need to subtract off the TX+RX delay from each recorded
// timestamp.
pp_anc_final_pkt.TOAs[_ranging_broadcast_ss_num] =
dw_rx_timestamp - oneway_get_txrxdelay_from_subsequence(ANCHOR, _ranging_broadcast_ss_num);
// Also record parameters the tag has sent us about how to respond
// (or other operational parameters).
_ranging_operation_config.reply_after_subsequence = rx_poll_pkt->reply_after_subsequence;
_ranging_operation_config.anchor_reply_window_in_us = rx_poll_pkt->anchor_reply_window_in_us;
_ranging_operation_config.anchor_reply_slot_time_in_us = rx_poll_pkt->anchor_reply_slot_time_in_us;
// Update the statistics we keep about which antenna
// receives the most packets from the tag
uint8_t recv_antenna_index = oneway_subsequence_number_to_antenna(ANCHOR, rx_poll_pkt->subsequence);
_anchor_antenna_recv_num[recv_antenna_index]++;
// Now we need to start our own state machine to iterate
// through the antenna / channel combinations while listening
// for packets from the same tag.
timer_start(_anchor_timer, RANGING_BROADCASTS_PERIOD_US, ranging_broadcast_subsequence_task);
} else {
// We found this tag ranging sequence late. We don't want
// to use this because we won't get enough range estimates.
// Just stay idle, but we do need to re-enable RX to
// keep receiving packets.
dwt_rxenable(0);
}
} else if (_state == ASTATE_RANGING) {
// We are currently ranging with a tag, waiting for the various
// ranging broadcast packets.
// First check if this is from the same tag
if (memcmp(pp_anc_final_pkt.ieee154_header_unicast.destAddr, rx_poll_pkt->header.sourceAddr, 8) == 0) {
// Same tag
if (rx_poll_pkt->subsequence == _ranging_broadcast_ss_num) {
// This is the packet we were expecting from the tag.
// Record the TOA, and adjust it with the calibration value.
pp_anc_final_pkt.TOAs[_ranging_broadcast_ss_num] =
dw_rx_timestamp - oneway_get_txrxdelay_from_subsequence(ANCHOR, _ranging_broadcast_ss_num);
// Update the statistics we keep about which antenna
// receives the most packets from the tag
uint8_t recv_antenna_index = oneway_subsequence_number_to_antenna(ANCHOR, _ranging_broadcast_ss_num);
_anchor_antenna_recv_num[recv_antenna_index]++;
} else {
// Some how we got out of sync with the tag. Ignore the
// range and catch up.
_ranging_broadcast_ss_num = rx_poll_pkt->subsequence;
}
// Check to see if we got the last of the ranging broadcasts
if (_ranging_broadcast_ss_num == _ranging_operation_config.reply_after_subsequence) {
// We did!
ranging_listening_window_setup();
}
} else {
// Not the same tag, ignore
}
} else {
// We are in some other state, not sure what that means
}
} else {
// Other message types go here, if they get added
// We do want to enter RX mode again, however
dwt_rxenable(0);
}
} else {
/*! ------------------------------------------------------------------------------------------------------------------
* @fn main()
*
* @brief Application entry point.
*
* @param none
*
* @return none
*/
int ssTwrResp(void)
{
/* Reset and initialise DW1000.
* For initialisation, DW1000 clocks must be temporarily set to crystal speed. After initialisation SPI rate can be increased for optimum
* performance. */
int i;
int status;
reset_DW1000(); /* Target specific drive of RSTn line into DW1000 low for a period. */
//spi_set_rate_low();
dwt_initialise(DWT_LOADUCODE);
//spi_set_rate_high();
/* Configure DW1000. See NOTE 5 below. */
dwt_configure(&config);
uint32_t otpVal[0x20];
dwt_otpread(0,otpVal,0x20);
printf("OTP 6: 0x%x\r\n",otpVal[6]);
printf("OTP 7: 0x%x\r\n",otpVal[7]);
printf("OTP x16: 0x%x\r\n",otpVal[0x16]);
printf("OTP x17: 0x%x\r\n",otpVal[0x17]);
/* Apply default antenna delay value. See NOTE 2 below. */
printf("antenna delays: default TX: %d, default RX: %d, evk 16m: %d, evk 64m: %d\r\n",TX_ANT_DLY,RX_ANT_DLY,DWT_RF_DELAY_16M,DWT_RF_DELAY_64M);
tx_delay = TX_ANT_DLY;
rx_delay = RX_ANT_DLY;
dwt_setrxantennadelay(rx_delay);
dwt_settxantennadelay(tx_delay);
btn = buttons();
printf("%s entering main loop\r\n",__FUNCTION__);
/* Loop forever responding to ranging requests. */
while (1)
{
/* Activate reception immediately. */
dwt_rxenable(0);
/* Poll for reception of a frame or error/timeout. See NOTE 6 below. */
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ } ; //printf("Waiting. status reg 0x%x\r\n",status_reg); };
//printf("Status reg now 0x%x\r\n",status_reg);
if (status_reg & SYS_STATUS_RXFCG)
{
uint32 frame_len;
//printf("Check RX\r\n");
/* Clear good RX frame event in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
//printf("Frame length %d\r\n",frame_len);
if (frame_len <= RX_BUFFER_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}
/*
for (i=0;i<frame_len;i++) {
printf("%x ",rx_buffer[i]);
}
printf("\r\n");
*/
/* Check that the frame is a poll sent by "SS TWR initiator" example.
* As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
rx_buffer[ALL_MSG_SN_IDX] = 0;
if (memcmp(rx_buffer, rx_poll_msg, ALL_MSG_COMMON_LEN) == 0)
{
uint32 resp_tx_time;
//printf("Poll MSG\r\n");
/* Retrieve poll reception timestamp. */
poll_rx_ts = get_rx_timestamp_u64();
//printf("RX timestamp: %lld\r\n",poll_rx_ts);
/* Compute final message transmission time. See NOTE 7 below. */
resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;
dwt_setdelayedtrxtime(resp_tx_time);
//printf("TX time: %d\r\n",resp_tx_time);
/* Response TX timestamp is the transmission time we programmed plus the antenna delay. */
resp_tx_ts = (((uint64)(resp_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;
//printf("TX timestamp: %lld\r\n",resp_tx_ts);
/* Write all timestamps in the final message. See NOTE 8 below. */
resp_msg_set_ts(&tx_resp_msg[RESP_MSG_POLL_RX_TS_IDX], poll_rx_ts);
resp_msg_set_ts(&tx_resp_msg[RESP_MSG_RESP_TX_TS_IDX], resp_tx_ts);
/* Write and send the response message. See NOTE 9 below. */
tx_resp_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
status = dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);
if (DWT_SUCCESS != status) printf("API error line %d\r\n",__LINE__);
status = dwt_writetxfctrl(sizeof(tx_resp_msg), 0);
if (DWT_SUCCESS != status) printf("API error line %d\r\n",__LINE__);
status = dwt_starttx(DWT_START_TX_DELAYED);
if (DWT_SUCCESS != status) {
printf("API error line %d\r\n",__LINE__);
printf("RX timestamp: %llu\r\n",poll_rx_ts);
printf("TX time: %llu\r\n",((uint64)resp_tx_time) << 8);
printf("TX timestamp: %llu\r\n",resp_tx_ts);
}
// poll only if starttx was OK
if (DWT_SUCCESS == status) {
/* Poll DW1000 until TX frame sent event set. See NOTE 6 below. */
u32 tx_stat;
while (!(tx_stat = dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ }; //printf("Waiting. status reg 0x%x\r\n",tx_stat); }
//printf("After Poll: status reg now 0x%x\r\n",tx_stat);
}
/* Clear TXFRS event. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
/* Increment frame sequence number after transmission of the poll message (modulo 256). */
frame_seq_nb++;
}
}
