From e9b9d6cf141c6de1450f1c4f36a0438a0f96c84c Mon Sep 17 00:00:00 2001
From: David <stierint@hotmail.com>
Date: Wed, 21 Oct 2020 19:04:52 -0400
Subject: [PATCH] add random timing offset to blink frequency
---
src/application/application_definitions.h | 12 +-
src/application/instance.c | 238 ++++++----------------
src/application/instance.h | 2 -
src/application/instance_common.c | 163 ++-------------
src/application/tdma_handler.c | 93 ++++-----
src/application/tdma_handler.h | 1 +
6 files changed, 128 insertions(+), 381 deletions(-)
diff --git a/src/application/application_definitions.h b/src/application/application_definitions.h
index a138959..cc4c2c9 100644
--- a/src/application/application_definitions.h
+++ b/src/application/application_definitions.h
@@ -264,16 +264,20 @@ enum
#define BLINK_DURATION_MS RNG_INIT_REPLY_DLY_MS + 1
#define RANGE_DURATION_MS MAX(18, POLL_SLEEP_DELAY) //to increase time between ranging, modify POLL_SLEEP_DELAY
-//#define BLINK_FREQUENCY 0.0001
-#define BLINK_PERIOD_MS 1000 //time to wait between sending blink messages
+#define BLINK_PERIOD_MS 900 //time to wait between sending blink messages
+#define BLINK_PERIOD_RAND_MS 200
-#define RX_CHECK_ON_PERIOD 200 //TODO modify
+
+#define RANGE_INIT_RAND 2000 //in DW1000 device time
+
+
+#define RX_CHECK_ON_PERIOD 200 //TODO modify
// Reception start-up time, in symbols.
#define RX_START_UP_SY 16
//TDMA defines
-#define MIN_FRAMELENGTH 4 //minimum size by TDMA E-ASAP
+#define MIN_FRAMELENGTH 4 //minimum size by TDMA E-ASAP
typedef uint64_t uint64 ;
typedef int64_t int64 ;
diff --git a/src/application/instance.c b/src/application/instance.c
index 3c75779..1311237 100644
--- a/src/application/instance.c
+++ b/src/application/instance.c
@@ -469,33 +469,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
{
case TA_INIT :
{
- //TODO REMOVE test below
-// //test out how improved timestamping works!!!
-// uint64 tsu1 = portGetTickCntMicro();
-// uint32 tsm1 = portGetTickCnt();
-// int a = 0;
-// while(a < 130)
-// {
-// a = a + 1;
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "%i :", a);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// }
-// uint64 tsu2 = portGetTickCntMicro();
-// uint32 tsm2 = portGetTickCnt();
-//
-// uint64 dtsu = tsu2 - tsu1;//systick counts down!!!
-// uint32 dtsm = tsm2 - tsm1;
-//
-//
-// int b = a;
-//
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "%llu %lu %i :", dtsu, dtsm, b);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
switch (inst->mode)
{
case DISCOVERY:
@@ -508,19 +481,20 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
dwt_seteui(inst->eui64);
dwt_setpanid(inst->panID);
- inst->uwbShortAdd = inst->eui64[0] + (inst->eui64[1] << 8);//TODO use a hashing algorithm
+ //seed random number generator with our 64-bit address
+ uint64 seed = 0;
+ seed |= (uint64) inst->eui64[0];
+ seed |= (uint64) inst->eui64[1] << 8;
+ seed |= (uint64) inst->eui64[2] << 16;
+ seed |= (uint64) inst->eui64[3] << 24;
+ seed |= (uint64) inst->eui64[4] << 32;
+ seed |= (uint64) inst->eui64[5] << 40;
+ seed |= (uint64) inst->eui64[6] << 48;
+ seed |= (uint64) inst->eui64[7] << 56;
+ srand(seed);
-// RX CB RX: DWT_SIG_RX_OKAY-RTLS_DEMO_MSG_TAG_POLL,4492,xxxx
-// while(TRUE){
-// uint8 debug_msg[100];
-// uint64 my_addr = inst->uwbShortAdd;
-// char *msg = "RTLS_DEMO_MSG_TAG_POLL";
-// int n = sprintf((char*)&debug_msg[0], "RX CB RX: DWT_SIG_RX_OKAY-%s,%llu,xxxx", msg, my_addr);
-//// int n = sprintf((char*)&debug_msg[0], "ADDR,%llu", my_addr);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// Sleep(100);
-// }
+
+ inst->uwbShortAdd = inst->eui64[0] + (inst->eui64[1] << 8);//TODO use a hashing algorithm
#if (USING_64BIT_ADDR==0)
dwt_setaddress16(inst->uwbShortAdd);
@@ -529,7 +503,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
memcpy(&inst->uwbList[0][0], &inst->eui64, inst->addrByteSize);
#endif
inst->uwbListLen = 1;
-// inst->uwbListType[0] = UWB_LIST_SELF;
tdma_handler->uwbListTDMAInfo[0].connectionType = UWB_LIST_SELF;
mode = (DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV);
@@ -546,8 +519,9 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
instanceconfigframeheader(inst);
instanceconfigmessages(inst);
- // First time listening we don't do a delayed RX
+ // First time listening, do not delay RX
dwt_setrxaftertxdelay(0);
+
//change to next state - wait to receive a message
tdma_handler->discoveryStartTime = portGetTickCnt();
tdma_handler->last_blink_time = portGetTickCnt();
@@ -557,52 +531,13 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->canPrintInfo = 1;
inst->wait4ack = 0;
-// uint16 resp_dly_us, resp_dly;
-// uint32 final_reply_delay_us;
-//
-// // First response delay to send is anchor's response delay.
-// resp_dly_us = ANC_TURN_AROUND_TIME_US + inst->frameLengths_us[POLL];
-// resp_dly = ((RESP_DLY_UNIT_US << RESP_DLY_UNIT_SHIFT) & RESP_DLY_UNIT_MASK)
-// + ((resp_dly_us << RESP_DLY_VAL_SHIFT) & RESP_DLY_VAL_MASK);
-// inst->resp_dly[RESP_DLY_ANC] = resp_dly & 0xFFFF; //TODO make sure that we only want the 16 lowest bits
-//
-//
-// // Second response delay to send is tag's response delay.
-// // Get response delays from message and update internal timings accordingly
-// resp_dly_us = TAG_TURN_AROUND_TIME_US + inst->frameLengths_us[RESP];
-// resp_dly = ((RESP_DLY_UNIT_US << RESP_DLY_UNIT_SHIFT) & RESP_DLY_UNIT_MASK)
-// + ((resp_dly_us << RESP_DLY_VAL_SHIFT) & RESP_DLY_VAL_MASK);
-// inst->resp_dly[RESP_DLY_TAG] = resp_dly & 0xFFFF; //TODO make sure that we only want the 16 lowest bits
-//
-//
-// for (int i = 0; i < RESP_DLY_NB; i++)
-// {
-// if (((inst->resp_dly[i] & RESP_DLY_UNIT_MASK) >> RESP_DLY_UNIT_SHIFT) == RESP_DLY_UNIT_MS)
-// {
-// // Remove unit bit and convert to microseconds.
-// inst->resp_dly[i] &= ~RESP_DLY_UNIT_MASK;
-// inst->resp_dly[i] *= 1000;
-// }
-// }
-//
-// // Update delay between poll transmission and response reception.
-// // Use uint64 for resp_dly here to avoid overflows if it is more than 400 ms.
-// inst->txToRxDelayTag_sy = US_TO_SY_INT((uint64)inst->resp_dly[RESP_DLY_ANC] - inst->frameLengths_us[POLL]) - RX_START_UP_SY;
-//
-// // Update delay between poll transmission and final transmission.
-// final_reply_delay_us = inst->resp_dly[RESP_DLY_ANC] + inst->resp_dly[RESP_DLY_TAG];
-// inst->finalReplyDelay = convertmicrosectodevicetimeu(final_reply_delay_us);
-// inst->finalReplyDelay_ms = CEIL_DIV(final_reply_delay_us, 1000);
-
- // If we are using long response delays, deactivate sleep.
+ // If we are using long response delays, disable sleep.
if (inst->resp_dly[RESP_DLY_ANC] >= LONG_RESP_DLY_LIMIT_US
|| inst->resp_dly[RESP_DLY_TAG] >= LONG_RESP_DLY_LIMIT_US)
{
inst->sleepingEabled = 0;
}
-
-
}
break;
default:
@@ -618,11 +553,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
case TA_TX_SELECT :
{
// send_statetousb(inst);
- // select whether to blink or send out a range poll message.
- // select a uwb from the list if sending out a range poll message
-// int uwb_index = inst->tx_scheduler.tx_select(&inst->tx_scheduler);
-
-
//select a TX action, return TRUE if we should move on to another state
if(tdma_handler->tx_select(tdma_handler) == TRUE)
@@ -727,50 +657,50 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->testAppState = inst->nextState;
inst->nextState = 0; //clear
}
- break ; // end case TA_TXE_WAIT
- }
+ break ;
+ }// end case TA_TXE_WAIT
case TA_TXBLINK_WAIT_SEND :
- {
- int flength = (BLINK_FRAME_CRTL_AND_ADDRESS + FRAME_CRC);
+ {
+ int flength = (BLINK_FRAME_CRTL_AND_ADDRESS + FRAME_CRC);
// //blink frames with IEEE EUI-64 tag ID
- inst->blinkmsg.seqNum = inst->frameSN++;
+ inst->blinkmsg.seqNum = inst->frameSN++;
- //using wait for response to do delayed receive
- inst->wait4ack = DWT_RESPONSE_EXPECTED;
+ //using wait for response to do delayed receive
+ inst->wait4ack = DWT_RESPONSE_EXPECTED;
- dwt_setrxtimeout((uint16)inst->fwtoTimeB_sy*0); //units are symbols
- //set the delayed rx on time (the ranging init will be sent after this delay)
- dwt_setrxaftertxdelay((uint32)inst->rnginitW4Rdelay_sy*0); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
+ //TODO modify timeout values to account for max value of RANGE_INIT_RAND
+ dwt_setrxtimeout((uint16)inst->fwtoTimeB_sy*0); //units are symbols
+ //set the delayed rx on time (the ranging init will be sent after this delay)
+ dwt_setrxaftertxdelay((uint32)inst->rnginitW4Rdelay_sy*0); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
- dwt_writetxdata(flength, (uint8 *) (&inst->blinkmsg), 0) ; // write the frame data
- dwt_writetxfctrl(flength, 0, 1);
+ dwt_writetxdata(flength, (uint8 *) (&inst->blinkmsg), 0) ; // write the frame data
+ dwt_writetxfctrl(flength, 0, 1);
- if(dwt_starttx(DWT_START_TX_IMMEDIATE | inst->wait4ack) == 0) //always using immediate TX and enable delayed RX
- {
- uint8 debug_msg[100];
+ if(dwt_starttx(DWT_START_TX_IMMEDIATE | inst->wait4ack) == 0) //always using immediate TX and enable delayed RX
+ {
+ uint8 debug_msg[100];
// int n = sprintf((char *)&debug_msg, "TX_BLINK,%llX,NULL", instance_get_addr());
- int n = sprintf((char *)&debug_msg, "TX_BLINK,%04llX,NULL", instance_get_addr());
- send_usbmessage(&debug_msg[0], n);
- usb_run();
-
- inst->blink0range1 = 0;
- inst->blink_start = portGetTickCnt();
- inst->timeofTx = portGetTickCnt();
- tdma_handler->last_blink_time = portGetTickCnt();
+ int n = sprintf((char *)&debug_msg, "TX_BLINK,%04llX,NULL", instance_get_addr());
+ send_usbmessage(&debug_msg[0], n);
+ usb_run();
- }
+ inst->blink_start = portGetTickCnt();
+ inst->timeofTx = portGetTickCnt();
+ tdma_handler->last_blink_time = portGetTickCnt();
+ tdma_handler->blinkPeriodRand = (uint32)rand()%BLINK_PERIOD_RAND_MS;
+ }
- inst->goToSleep = 1; //go to Sleep after this blink
- inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation //TODO move this into the if statement above?
- inst->previousState = TA_TXBLINK_WAIT_SEND ; //TODO move this into the ...
- done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
- }
- break ; // end case TA_TXBLINK_WAIT_SEND
+ inst->goToSleep = 1; //go to Sleep after this blink
+ inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation //TODO move this into the if statement above?
+ inst->previousState = TA_TXBLINK_WAIT_SEND ; //TODO move this into the ...
+ 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_TXRANGINGINIT_WAIT_SEND :
{
int psduLength = RANGINGINIT_MSG_LEN;
@@ -975,7 +905,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
inst->range_start = portGetTickCnt();
- inst->blink0range1 = 1; //TODO check if this is still needed...
// //test between 0CD6 and 118C
@@ -1352,8 +1281,8 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->canPrintInfo = 1;
- //if using longer reply delay time (e.g. if interworking with a PC application)
- inst->delayedReplyTime = (dw_event->timeStamp + inst->rnginitReplyDelay) >> 8 ; // time we should send the blink response
+ //TODO add a small random number to this to reduce chance of collisions
+ inst->delayedReplyTime = (dw_event->timeStamp + inst->rnginitReplyDelay + (uint32)rand()%RANGE_INIT_RAND) >> 8 ; // time we should send the blink response
//set destination address
memcpy(&inst->rng_initmsg.destAddr[0], &(dw_event->msgu.rxblinkmsg.tagID[0]), BLINK_FRAME_SOURCE_ADDRESS); //remember who to send the reply to
@@ -1524,6 +1453,12 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
//1.) sync our frame start time to the local network
//2.) collect and combine tdma info so we can construct a SUG packet and send it out
+ if(tdma_handler->discovery_mode == WAIT_INF_INIT)
+ {
+ //if we receive network traffic while waiting for INF_INIT, transition to collecting INF messages
+ tdma_handler->set_discovery_mode(tdma_handler, COLLECT_INF_REG, time_now);
+ }
+
if(tdma_handler->discovery_mode == WAIT_INF_REG) //treat INF_UPDATE and INF_SUG the same
{
//synchronize the frames //TODO don't need to sync frame at this point...?
@@ -1535,13 +1470,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
}
else if(tdma_handler->discovery_mode == COLLECT_INF_REG)
{
- //TODO handle the case where we are collecting from two different UWB networks that are not synchronized
- //in case there are multiple subnetworks... only collect average frame start times for one of them...
- //send out INF_SUG in that 0th frame with rebase to the other subnetwork's frame. but what if there are two other networks?
- //instead send out INF fug in that 0th frame with rebase to that frame. Hopefully the other networks will get the message.
- //if not, the discrepency will eventually be taken care of the ANCHOR and TAG logic, regarless of how many subnetworks there are.
- //sync to the largest subnetwork...
-
//synchronize the frames
tdma_handler->frame_sync(tdma_handler, dw_event, framelength, timeSinceFrameStart_us, srcIndex, FS_COLLECT);
//collecting tdma info, append to previously stored info
@@ -1568,48 +1496,11 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
//collecting tdma info, append to previously stored info
bool tdma_modified = tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, CLEAR_LISTED_COPY);
-
-// uint16 dest_addr = 0x118C;
-// uint16 my_addr = inst->uwbShortAdd;
-// if(memcmp(&my_addr, &dest_addr, sizeof(uint16))==0)
-// {
-// uint16 test_addr = 0x0CD6;
-//
-// if(memcmp(&test_addr, &srcAddr, sizeof(uint16))==0)
-// {
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "range_end,xxxx,xxxx");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// }
-// }
-
-
-
if(tdma_modified)
{
+ //only repopulate the INF message if there was a modification to the TDMA configuration
tdma_handler->populate_inf_msg(tdma_handler, RTLS_DEMO_MSG_INF_UPDATE);//TODO populate at slot start while waiting for buffer to expire
}
-
-// //clear all tdma information
-// tdma_handler->tdma_free_all_slots(tdma_handler);
-//
-// //build the initial TDMA
-// tdma_handler->uwbListTDMAInfo[0].framelength = (uint8)MIN_FRAMELENGTH;
-// tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith].framelength = (uint8)MIN_FRAMELENGTH;
-//
-// tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith], 1);
-// tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[0], 2);
-// tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith], 3);
-//
-// tdma_handler->populate_inf_msg(tdma_handler, RTLS_DEMO_MSG_INF_UPDATE);
-
- //TODO left off here! make sure the logic below is enforced in the RX callback
- //NOTE i can think of some differences between SUG/UPDATE/REG...
- //when waiting to send INF_SUG, ignore everything else?
- //when waiting to send INF_UPDATE, ignore INF_REG?
-
-
}
//wait for next RX
@@ -1630,32 +1521,19 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
uint32 time_now = portGetTickCnt();
uint8 srcIndex = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
-// //clear all tdma information
-// tdma_handler->tdma_free_all_slots(tdma_handler);
-//
-// //build the initial TDMA
-// tdma_handler->uwbListTDMAInfo[0].framelength = (uint8)MIN_FRAMELENGTH;
-// tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith].framelength = (uint8)MIN_FRAMELENGTH;
-
uint8 framelength;
uint64 timeSinceFrameStart_us = 0;
memcpy(&framelength, &messageData[TDMA_FRAMELENGTH], sizeof(uint8));
memcpy(&timeSinceFrameStart_us, &messageData[TDMA_TSFS], 6);
- //synchronise the frames
+ //synchronize the frames
tdma_handler->frame_sync(tdma_handler, dw_event, framelength, timeSinceFrameStart_us, srcIndex, FS_ADOPT);
//copy the TDMA network configuration directly
tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, CLEAR_ALL_COPY);
//copy new TDMA configuration into the INF message that this UWB will send out
- tdma_handler->populate_inf_msg(tdma_handler, RTLS_DEMO_MSG_INF_UPDATE); //TODO set some sort of flag that tells us to send INF_UPDATE?
+ tdma_handler->populate_inf_msg(tdma_handler, RTLS_DEMO_MSG_INF_UPDATE);
//set discovery mode to EXIT
- tdma_handler->set_discovery_mode(tdma_handler, EXIT, time_now); //TODO do I really need EXIT?
-
-// tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith], 1);
-// tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[0], 2);
-// tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith], 3);
-//
-// tdma_handler->populate_inf_msg(tdma_handler, RTLS_DEMO_MSG_INF_UPDATE);
+ tdma_handler->set_discovery_mode(tdma_handler, EXIT, time_now);
inst->mode = ANCHOR;
diff --git a/src/application/instance.h b/src/application/instance.h
index c3d5149..d827e63 100644
--- a/src/application/instance.h
+++ b/src/application/instance.h
@@ -179,8 +179,6 @@ typedef struct
uint8 dweventPeek;
uint8 monitor;
uint32 timeofTx;
- uint64 timeofRxCallback;
- uint64 timeofRxCallback_dwtime;
uint8 smartPowerEn;
uint32 currentStateStartTime;
diff --git a/src/application/instance_common.c b/src/application/instance_common.c
index ac1cb51..3874e13 100644
--- a/src/application/instance_common.c
+++ b/src/application/instance_common.c
@@ -280,7 +280,6 @@ int instgetuwblistindex(instance_data_t *inst, uint8 *uwbAddr, uint8 addrByteSiz
if(match == FALSE)
{
-// instance_data_t *inst = &instance_data[instance];
uint8 debug_msg[100];
int n = sprintf((char*)&debug_msg[0], "match not found");
send_usbmessage(&debug_msg[0], n);
@@ -296,7 +295,6 @@ int instgetuwblistindex(instance_data_t *inst, uint8 *uwbAddr, uint8 addrByteSiz
{
memcpy(&inst->uwbList[i][0], &uwbAddr[0], addrByteSize) ;
inst->uwbListLen = i + 1 ;
-// inst->uwbTimeout[i] = 1;
return i;
}
}
@@ -1310,11 +1308,6 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
{
uint8 destAddr_index = FRAME_CTRLP;
-//#if (USING_64BIT_ADDR==0)
-// if(memcmp(&instance_data[instance].uwbShortAdd, &dw_event.msgu.frame[destAddr_index], instance_data[instance].addrByteSize) != 0)
-//#else
-// if(memcmp(&instance_data[instance].eui64[0], &dw_event.msgu.frame[destAddr_index], instance_data[instance].addrByteSize) != 0)
-//#endif
if(memcmp(&instance_data[instance].uwbList[0][0], &dw_event.msgu.frame[destAddr_index], instance_data[instance].addrByteSize) != 0)
{
// uint8 debug_msg[150];
@@ -1362,17 +1355,12 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
#endif
//must be a neighbor
-// uwb_index = instgetuwblistindex(&instance_data[instance], &dw_event.msgu.rxblinkmsg.tagID[0], instance_data[instance].addrByteSize);
uwb_index = instgetuwblistindex(&instance_data[instance], &blink_address[0], instance_data[instance].addrByteSize);
-// instance_data[instance].uwbListType[uwb_index] = UWB_LIST_NEIGHBOR;
if(uwb_index > 0 && uwb_index < UWB_LIST_SIZE)
{
- // instance_data[instance].uwbListType[uwb_index] = UWB_LIST_NEIGHBOR;
tdma_handler.uwbListTDMAInfo[uwb_index].connectionType = UWB_LIST_NEIGHBOR;
- // instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
tdma_handler.uwbListTDMAInfo[uwb_index].lastCommNeighbor = time_now;
-// instance_data[instance].uwbTimeout[uwb_index] = 0;
}
@@ -1391,11 +1379,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
if(uwb_index > 0 && uwb_index < UWB_LIST_SIZE)
{
//TODO maybe do this somewhere else...
- // instance_data[instance].uwbListType[uwb_index] = UWB_LIST_NEIGHBOR;
tdma_handler.uwbListTDMAInfo[uwb_index].connectionType = UWB_LIST_NEIGHBOR;
- // instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
tdma_handler.uwbListTDMAInfo[uwb_index].lastCommNeighbor = time_now;
-// instance_data[instance].uwbTimeout[uwb_index] = 0;
}
@@ -1475,7 +1460,7 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_INF_UPDATE ||
dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_INF_SUG)
{
- if(tdma_handler.discovery_mode == WAIT_INF_REG || tdma_handler.discovery_mode == COLLECT_INF_REG)
+ if(tdma_handler.discovery_mode == WAIT_INF_REG || tdma_handler.discovery_mode == COLLECT_INF_REG || tdma_handler.discovery_mode == WAIT_INF_INIT)
{
accept_inf = TRUE;
}
@@ -1496,10 +1481,10 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
// send_usbmessage(&debug_msg[0], n);
// usb_run();
- if(num_neighbors <= 1)
- {
+// if(num_neighbors <= 1) //TODO i don't think I want this
+// {
instance_data[instance].uwbToRangeWith = uwb_index;
- }
+// }
}
}
}
@@ -1532,19 +1517,6 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
if(accept_inf == TRUE)
{
- //NOTE: these could get messed up if other messages trigger rxcallback before being processed by frame sync...
- //should these be folded into the dw_event?
- //TODO only do this when we need to (we are accepting and INF message)
- instance_data[instance].timeofRxCallback = time_now_us;
-
- //TODO only do this when we need to (we are accepting and INF message)
- uint8 sys_time_arr[5] = {0, 0, 0, 0, 0};
- dwt_readsystime(sys_time_arr);
- instance_data[instance].timeofRxCallback_dwtime = (uint64)sys_time_arr[0] + ((uint64)sys_time_arr[1] << 8) + ((uint64)sys_time_arr[2] << 16) + ((uint64)sys_time_arr[3] << 24) + ((uint64)sys_time_arr[4] << 32);
-
-
-// instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
-// instance_data[instance].uwbTimeout[uwb_index] = 0;
place_event = 1;
@@ -1556,9 +1528,6 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
// usb_run();
}
-
-
-
}
else if(uwb_index != 255 && instance_data[instance].uwbToRangeWith == uwb_index)
{
@@ -1568,14 +1537,6 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
//process RTLS_DEMO_MSG_TAG_POLL immediately.
if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_POLL)
{
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "RX_POLL");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
-// instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
-// instance_data[instance].uwbTimeout[uwb_index] = 0;
-
uint16 frameLength = 0;
instance_data[instance].tagPollRxTime = dw_event.timeStamp ; //Poll's Rx time
@@ -1592,48 +1553,6 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
#endif
-// instance_data[instance].msg[uwb_index].messageData[FCODE] = RTLS_DEMO_MSG_ANCH_RESP; //message function code (specifies if message is a poll, response or other...)
-// //program option octet and parameters (not used currently)
-// instance_data[instance].msg[uwb_index].messageData[RES_R1] = 0x2; // "activity"
-// instance_data[instance].msg[uwb_index].messageData[RES_R2] = 0x0; //
-// instance_data[instance].msg[uwb_index].messageData[RES_R3] = 0x0;
-//
-//
-// memcpy(&instance_data[instance].msg[uwb_index].destAddr[0], &dw_event.msgu.frame[srcAddr_index], instance_data[instance].addrByteSize); //remember who to send the reply to (set destination address)
-//
-// //if tof not zero, set tof, else memset 0
-// //also include next TX_ACCEPT node time (offset from now)
-// // Write calculated TOF into response message
-// memcpy(&instance_data[instance].msg[uwb_index].messageData[TOFR], &instance_data[instance].tof[uwb_index], 6); //TODO fix number of bytes...
-//
-// uint8 num_active = 0;
-// for(int i = 0; i < instance_data[instance].uwbListLen; i++){
-// if(!instance_data[instance].uwbTimeout[i]){
-// num_active++;
-// }
-// }
-// memcpy(&instance_data[instance].msg[uwb_index].messageData[NTAG], &num_active, 1);
-//
-// //get time till the next RX_ACCEPT
-//// uint32 time_till = 0;
-//// if(instance_data[instance].time_till_next_reported[uwb_index] == 0){
-//// time_till = instance_data[instance].rx_scheduler.get_time_till_next_rx_accept(&instance_data[instance].rx_scheduler);
-//// instance_data[instance].time_till_next_reported[uwb_index] = 1;
-//// }
-//// memcpy(&instance_data[instance].msg[uwb_index].messageData[TIME_TILL], &time_till, 4);
-//
-//
-//// uint8 debug_msg[100];
-//// uint64 mtof = instance_data[instance].msg[uwb_index].messageData[TOFR];
-//// int n = sprintf((char *)&debug_msg, "TOF in resp %llu", mtof);
-//// send_usbmessage(&debug_msg[0], n);
-//// usb_run();
-//
-//
-//// instance_data[instance].tof[uwb_index] = 0; //clear ToF ..
-////
-// instance_data[instance].msg[uwb_index].seqNum = instance_data[instance].frameSN++;
-
instance_data[instance].msg.messageData[FCODE] = RTLS_DEMO_MSG_ANCH_RESP; //message function code (specifies if message is a poll, response or other...)
memcpy(&instance_data[instance].msg.destAddr[0], &dw_event.msgu.frame[srcAddr_index], instance_data[instance].addrByteSize); //remember who to send the reply to (set destination address)
@@ -1649,7 +1568,6 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
instance_data[instance].wait4ack = DWT_RESPONSE_EXPECTED;
dwt_writetxfctrl(frameLength, 0, 1);
-// dwt_writetxdata(frameLength, (uint8 *) &instance_data[instance].msg[uwb_index], 0) ; // write the frame data
dwt_writetxdata(frameLength, (uint8 *) &instance_data[instance].msg, 0) ; // write the frame data
#if (IMMEDIATE_RESPONSE == 1)
@@ -1668,64 +1586,22 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
dw_event.typePend = DWT_SIG_TX_PENDING ; // exit this interrupt and notify the application/instance that TX is in progress.
instance_data[instance].timeofTx = time_now;
}
- //report out which message is being sent!
-// send_txmsgtousb(get_msg_fcode_string((int)instance_data[instance].msg[uwb_index].messageData[FCODE]));
-
-// int64 mtof = 0;
-// memcpy(&mtof, &instance_data[instance].msg[uwb_index].messageData[TOFR], sizeof(int64));
-// int64 mtof2 = 0;
-// memcpy(&mtof2, &instance_data[instance].tof[uwb_index], sizeof(int64));
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, ".instance_data[instance].uwbToRangeWith %d, tof[uwb_index] %lld, .msg[uwb_index].messageData[TOFR] %lld",instance_data[instance].uwbToRangeWith, mtof2, mtof);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
+ instance_data[instance].tof[uwb_index] = 0; //clear ToF .. //TODO does this need to be here?
-// uint32 time_till_cpy = 0;
-// memcpy(&time_till_cpy, &instance_data[instance].msg[uwb_index].messageData[TIME_TILL], sizeof(uint32));
-//
-// uint8 debug_msg[200];
-// int n = 0;
-// n = sprintf((char*)&debug_msg[0], "time_till %lu, memcpy time_till %lu", time_till, time_till_cpy);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
- instance_data[instance].tof[uwb_index] = 0; //clear ToF ..
-
-
-// uint8 debug_msg[100];
-// n = sprintf((char*)&debug_msg[0], "RX TAG_POLL ACCEPTED ANCH_RESP sent <- uwb %d", uwb_index);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
- }
- else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_ANCH_RESP)
- {
-// uint32 response_time = portGetTickCnt() - instance_data[instance].range_start;
-// instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
-// instance_data[instance].uwbTimeout[uwb_index] = 0;
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: ANCH_RESP <- uwb_index %d", uwb_index);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
- }
- else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_INIT)
- {
-// instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
-// instance_data[instance].uwbTimeout[uwb_index] = 0;
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: RNG_INIT <- uwb_index %d", uwb_index);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
- }
- else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_FINAL)
- {
-// instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
-// instance_data[instance].uwbTimeout[uwb_index] = 0;
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: TAG_FINAL <- uwb_index %d", uwb_index);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
}
+// else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_ANCH_RESP)
+// {
+//
+// }
+// else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_INIT)
+// {
+//
+// }
+// else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_FINAL)
+// {
+//
+// }
// else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_REPORT)
// {
// instance_data[instance].lastCommTimeStamp[uwb_index] = portGetTickCnt();
@@ -1745,11 +1621,6 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
}
else if (rxd_event == DWT_SIG_RX_BLINK)
{
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: BLINK <- uwb %d", uwb_index);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
place_event = 1;
}
}
diff --git a/src/application/tdma_handler.c b/src/application/tdma_handler.c
index e5c1118..6da5ec2 100644
--- a/src/application/tdma_handler.c
+++ b/src/application/tdma_handler.c
@@ -546,50 +546,54 @@ static bool tx_select(struct TDMAHandler *this)
{
if(this->discovery_mode == WAIT_INF_REG)
{
- //start blinking if enough time has passed since entering DISCOVERY mode
- uint32 timeSinceDiscoverStart = get_dt32(this->discoveryStartTime, time_now);
-
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "discovery wait inf reg");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
- // uint8 debug_msg[100];
- // int n = sprintf((char *)&debug_msg, "timeSinceDiscoverStart %lu, maxFrameDuration %lu ", timeSinceDiscoverStart, maxFrameDuration);
- // send_usbmessage(&debug_msg[0], n);
- // usb_run();
-
- if(timeSinceDiscoverStart > this->waitInfDuration)
+ if(this->check_blink(this))
{
- //enforce blink period
- uint32 timeSinceLastBlink = get_dt32(this->last_blink_time, time_now);
-
- // uint8 debug_msg[100];
- // int n = sprintf((char *)&debug_msg, "timeSinceLastBlink %lu, BLINK_PERIOD_MS %lu ", timeSinceLastBlink, (uint32)BLINK_PERIOD_MS);
- // send_usbmessage(&debug_msg[0], n);
- // usb_run();
-
- if(timeSinceLastBlink > (uint32)BLINK_PERIOD_MS + (uint32)(rand() % 100))
- {
- //time to blink
- uwb_index = 255;
- this->set_discovery_mode(this, WAIT_RNG_INIT, time_now);
- }
- else
- {
- //not time to blink yet, keep waiting for RNG_INIT
- inst->wait4ack = 0;
- inst->testAppState = TA_RXE_WAIT;
- return TRUE;
- }
+ //time to blink
+ uwb_index = 255;
+ this->set_discovery_mode(this, WAIT_RNG_INIT, time_now);
}
else
{
- //shouldn't be in this mode, should be listening for INF messages
+ //not time to blink yet, keep waiting for RNG_INIT
inst->wait4ack = 0;
inst->testAppState = TA_RXE_WAIT;
return TRUE;
}
+
+// //start blinking if enough time has passed since entering DISCOVERY mode
+// uint32 timeSinceDiscoverStart = get_dt32(this->discoveryStartTime, time_now);
+//
+// if(timeSinceDiscoverStart > this->waitInfDuration)
+// {
+// //enforce blink period
+// uint32 timeSinceLastBlink = get_dt32(this->last_blink_time, time_now);
+//
+// // uint8 debug_msg[100];
+// // int n = sprintf((char *)&debug_msg, "timeSinceLastBlink %lu, BLINK_PERIOD_MS %lu ", timeSinceLastBlink, (uint32)BLINK_PERIOD_MS);
+// // send_usbmessage(&debug_msg[0], n);
+// // usb_run();
+//
+// if(timeSinceLastBlink > (uint32)BLINK_PERIOD_MS + this->blinkPeriodRand)
+// {
+// //time to blink
+// uwb_index = 255;
+// this->set_discovery_mode(this, WAIT_RNG_INIT, time_now);
+// }
+// else
+// {
+// //not time to blink yet, keep waiting for RNG_INIT
+// inst->wait4ack = 0;
+// inst->testAppState = TA_RXE_WAIT;
+// return TRUE;
+// }
+// }
+// else
+// {
+// //shouldn't be in this mode, should be listening for INF messages
+// inst->wait4ack = 0;
+// inst->testAppState = TA_RXE_WAIT;
+// return TRUE;
+// }
}
else if(this->discovery_mode == SEND_SUG)
{
@@ -755,21 +759,11 @@ static bool check_blink(struct TDMAHandler *this)
{
uint32 time_now = portGetTickCnt();
uint32 timeSinceDiscoveryStart = get_dt32(this->discoveryStartTime, time_now);
- if(timeSinceDiscoveryStart > this->maxFramelength*this->slotDuration_ms )
+ if(timeSinceDiscoveryStart > this->waitInfDuration )
{
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "in RX_WAIT_DATA, portGetTickCnt(): %lu, inst->last_blink_time: %lu, BLINK_PERIOD_MS: %lu", portGetTickCnt(), inst->last_blink_time, (uint32)BLINK_PERIOD_MS);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
uint32 timeSinceBlink = get_dt32(this->last_blink_time, time_now);
- if(timeSinceBlink > (uint32)BLINK_PERIOD_MS + (uint32)(rand()%100)){
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "in RX_WAIT_DATA, siwtch to TA_TX_SELECT");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
-// this->discovery_mode = WAIT_RNG_INIT;
+ if(timeSinceBlink > (uint32)BLINK_PERIOD_MS + this->blinkPeriodRand)
+ {
retval = TRUE;
}
}
@@ -3255,6 +3249,7 @@ static struct TDMAHandler new(){
// ret.collectInfDuration = ret.maxFramelength*ret.slotDuration;
ret.collectInfDuration = 10000; //TODO change this back
ret.waitInfDuration = ret.collectInfDuration;
+ ret.blinkPeriodRand = (uint32)rand()%BLINK_PERIOD_RAND_MS;
return ret;
}
diff --git a/src/application/tdma_handler.h b/src/application/tdma_handler.h
index 9e7001c..0f2bfad 100644
--- a/src/application/tdma_handler.h
+++ b/src/application/tdma_handler.h
@@ -66,6 +66,7 @@ struct TDMAHandler
//discovery variables
DISCOVERY_MODE discovery_mode;
uint32 last_blink_time; //timestamp of most recent blink
+ uint32 blinkPeriodRand; //random number used to vary blink message transmission interval
uint32 discoveryStartTime; //time that we started listening for other UWBs
uint32 discovery_mode_start_time;
uint32 discovery_mode_duration;
--
GitLab