diff --git a/src/application/application_definitions.h b/src/application/application_definitions.h
index 3aa5dc4fbc9bea827226839a5377a56496b4249e..a1389593139117e62e2905dd0dd4709f7aa1353f 100644
--- a/src/application/application_definitions.h
+++ b/src/application/application_definitions.h
@@ -144,8 +144,12 @@ enum
#define BLINK_FRAME_CRTL_AND_ADDRESS (BLINK_FRAME_SOURCE_ADDRESS + BLINK_FRAME_CTRLP) //10 bytes
#define BLINK_FRAME_LEN_BYTES (BLINK_FRAME_CRTL_AND_ADDRESS + BLINK_FRAME_CRC)
-#define UWB_LIST_SIZE (10) //maximum number of UWBs to range with (valid options are 0 through 253)//TODO check what works based on data sizes needed for INF packets and 16 or 64 bit addres
-#define UWB_COMM_TIMEOUT 3000 //ms //TODO maybe make this a function of other defines?
+#define UWB_LIST_SIZE 10 //the maximum size of the UWB network
+ //do not use a number larger than 80 if USING_64BIT_ADDR==1 and 127 if USING_64BIT_ADDR==0
+ //these are the largest UWB network sizes that the firmware will support.
+ //USING_64BIT_ADDR==1: Limiting factor is the max length of the INF messages
+ //USING_64BIT_ADDR==0: Limiting factor is the max power of 2 that fits in a uint8 (framelength is stored as uint8)
+#define UWB_COMM_TIMEOUT 3000 //ms //TODO make this a function of UWB_LIST_SIZE, twice the max framelengt? maybe this shouldn't be a define?
//UWB_LIST entry types
#define UWB_LIST_SELF 0 //entry for self in list
@@ -154,8 +158,8 @@ enum
#define UWB_LIST_TWICE_HIDDEN 3 //uwb in list that is active, out of range, and a hidden neighbor is slotted to range with
#define UWB_LIST_INACTIVE 4 //uwb in list that is not active (could have previously been neighbor, hidden, or twice hidden)
-#define BLINK_SLEEP_DELAY 0 //ms //how long the tag should sleep after blinking
-#define POLL_SLEEP_DELAY 25 //ms //how long the tag should sleep after ranging
+#define BLINK_SLEEP_DELAY 0 //ms //how long the tag should sleep after blinking
+#define POLL_SLEEP_DELAY 25 //ms //how long the tag should sleep after ranging
@@ -165,15 +169,15 @@ enum
// NOTE: the maximum RX timeout is ~ 65ms
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-#define INST_DONE_WAIT_FOR_NEXT_EVENT 1 //this signifies that the current event has been processed and instance is ready for next one
+#define INST_DONE_WAIT_FOR_NEXT_EVENT 1 //this signifies that the current event has been processed and instance is ready for next one
#define INST_DONE_WAIT_FOR_NEXT_EVENT_TO 2 //this signifies that the current event has been processed and that instance is waiting for next one with a timeout
- //which will trigger if no event coming in specified time
-#define INST_NOT_DONE_YET 0 //this signifies that the instance is still processing the current event
+ //which will trigger if no event coming in specified time
+#define INST_NOT_DONE_YET 0 //this signifies that the instance is still processing the current event
// Function code byte offset (valid for all message types).
#define FCODE 0 // Function code is 1st byte of messageData
-//INF message byte offsets
+//INF message byte offsets //TODO remove the +6
#define TDMA_TSFS 1+6 // offset to put time since TDMA frame start in the INF message
#define TDMA_TSFS_REBASE 7+6 // offset to put whether the receiving UWB needs to rebase its TDMA frame to the transmitted TSFS
#define TDMA_NUMN 8+6 // offset to put the number of this UWB's neighbors in the INF message
@@ -196,16 +200,6 @@ enum
#define SYNC_FRAMELENGTH 1 // offset to put the framelength in the sync message
#define SYNC_TSFS 2 // offset to put the time since TDMA frame start in the sync message
-//TODO remove below
-// Ranging init message byte offsets. Composed of tag short address, anchor
-// response delay and tag response delay.
-//#define RNG_INIT_TAG_SHORT_ADDR_LO 1
-//#define RNG_INIT_TAG_SHORT_ADDR_HI 2
-//#define RNG_INIT_ANC_RESP_DLY_LO 3
-//#define RNG_INIT_ANC_RESP_DLY_HI 4
-//#define RNG_INIT_TAG_RESP_DLY_LO 5
-//#define RNG_INIT_TAG_RESP_DLY_HI 6
-
// Response delay values coded in ranging init message.
// This is a bitfield composed of:
// - bits 0 to 14: value
diff --git a/src/application/dw_main.c b/src/application/dw_main.c
index b49a499a2bbba994947e84001e17d6e5cd8a1646..c4f079a8f2551d73de69ed85cf3ce326b6f54d06 100644
--- a/src/application/dw_main.c
+++ b/src/application/dw_main.c
@@ -302,7 +302,7 @@ int dw_main(void)
{
int i = 0;
double range_result = 0;
- uint64 range_addr = 0;
+// uint64 range_addr = 0;
int canSleep;
//LCD variables
@@ -579,8 +579,8 @@ int dw_main(void)
memset(dataseq1, ' ', LCD_BUFF_LEN);
uint64 addr = instance_get_addr();
- uint8 num_neighbors = instfindnumactiveneighbors(inst);
- uint8 num_hidden = instfindnumactivehidden(inst);
+ uint8 num_neighbors = instfindnumneighbors(inst);
+ uint8 num_hidden = instfindnumhidden(inst);
char status[10];
if(instance_mode == DISCOVERY)
diff --git a/src/application/instance.c b/src/application/instance.c
index 9751dc1c9f79a66408412d5619ee70507371fe21..3c75779dce12f2164ca8dcc58acfbe085a970716 100644
--- a/src/application/instance.c
+++ b/src/application/instance.c
@@ -395,11 +395,11 @@ void send_statetousb(instance_data_t *inst, struct TDMAHandler *tdma_handler)
usbdebugdataprev_size = usbdebugdata_size;
memcpy(usbdebugdataprev, usbdebugdata, usbdebugdata_size);
- int num_neighbors = instfindnumactiveneighbors(inst);
- uint8 debug_msg[150];
- int n = sprintf((char*)&debug_msg[0], "mode: %s, num_neighbors %d, discovery_mode %s", get_instanceModes_string(inst->mode), num_neighbors, get_discovery_modes_string(tdma_handler->discovery_mode));
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// int num_neighbors = instfindnumneighbors(inst);
+// uint8 debug_msg[150];
+// int n = sprintf((char*)&debug_msg[0], "mode: %s, num_neighbors %d, discovery_mode %s", get_instanceModes_string(inst->mode), num_neighbors, get_discovery_modes_string(tdma_handler->discovery_mode));
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
}
}
@@ -624,10 +624,11 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
- //select a TX action, return if TRUE if we should move on to another state
+ //select a TX action, return TRUE if we should move on to another state
if(tdma_handler->tx_select(tdma_handler) == TRUE)
{
dwt_forcetrxoff();
+
}
else
{
@@ -954,7 +955,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
//set the delayed rx on time (the response message will be sent after this delay)
dwt_setrxaftertxdelay(inst->txToRxDelayTag_sy*0); //TODO fix this! remove *0
- dwt_setrxtimeout((uint16)inst->fwtoTime_sy*0); //TODO fix this! remove *2
+ dwt_setrxtimeout((uint16)inst->fwtoTime_sy*2); //TODO fix this! remove *2
//response is expected
inst->wait4ack = DWT_RESPONSE_EXPECTED;
@@ -1317,6 +1318,18 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->testAppState = TA_RX_WAIT_DATA; // let this state handle it
inst->rxCheckOnTime = portGetTickCnt() + RX_CHECK_ON_PERIOD;
+
+// //see if tx is actually enabled
+// //read SYS_STATE, getting second byte
+// uint8 regval = dwt_read8bitoffsetreg(SYS_STATE_ID,1);
+// //get the first 5 bytes
+// regval &= 0x1F;
+// uint8 debug_msg[200];
+// int n = sprintf((char *)&debug_msg, "RX STATUS: regval %u", regval);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+
// 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)
{
@@ -1499,6 +1512,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
{
if(tdma_handler->uwbListTDMAInfo[0].slotsLength == 0)
{
+ inst->mode = DISCOVERY;
tdma_handler->set_discovery_mode(tdma_handler, WAIT_INF_REG, time_now);
}
}
@@ -1650,39 +1664,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
break;
}//RTLS_DEMO_MSG_INF
-// case RTLS_DEMO_MSG_INF_SUG :
-// {
-// //TODO: process differently depending on if we are or aren't in discovery
-//
-// //process the INF_SUG packet and return to RX
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "process RTLS_DEMO_MSG_INF_SUG :");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-//
-//// uint8 srcIndex = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
-////
-//// //synchronise the frames
-//// tdma_handler->frame_sync(tdma_handler, messageData, dw_event->rxLength, srcIndex, FS_ADOPT);
-//// //initialize collection of tdma info, clear any previously stored info
-//// tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, CLEAR_ALL_COPY);
-//// //set discovery mode to EXIT
-//// tdma_handler->set_discovery_mode(tdma_handler, EXIT, time_now); //TODO do I really need EXIT?
-//
-//
-// uint8 srcIndex = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
-//
-// //TODO implement
-//// tdma_handler->process_sug_msg(tdma_handler, messageData, &srcAddr);
-// tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, CLEAR_LISTED_COPY);
-//
-// //TODO add frame sync!
-//
-// //stay in RX wait for next frame...
-// inst->testAppState = TA_RXE_WAIT ; // wait for next frame
-//
-// break;
-// }//RTLS_DEMO_MSG_INF_SUG
case RTLS_DEMO_MSG_TAG_POLL:
{
if(dw_event->typePend == DWT_SIG_TX_PENDING)
@@ -1848,7 +1829,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
}
}
-// inst->lastRangeTimeStamp[inst->uwbToRangeWith] = portGetTickCnt();
tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith].lastRange = portGetTickCnt();
uint8 debug_msg[100];
@@ -1857,9 +1837,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
send_usbmessage(&debug_msg[0], n);
usb_run();
-
- //TODO remember that below is for testing out if its better to send INF as last message
-// inst->testAppState = TA_TXINF_WAIT_SEND;
+ tdma_handler->firstPollComplete = TRUE;
inst->testAppState = TA_TX_SELECT;
inst->previousState = TA_INIT;
inst->nextState = TA_INIT;
diff --git a/src/application/instance.h b/src/application/instance.h
index 04c3ba9843a2a8b6f8462c4a3ffd8f0f91b426ba..c3d5149be3cb1f22cbc376b8f2ab4727cb9af9a6 100644
--- a/src/application/instance.h
+++ b/src/application/instance.h
@@ -127,12 +127,12 @@ typedef struct
int lateTX;
int lateRX;
- double adist[RTD_MED_SZ] ;
- double adist4[4] ;
- double longTermRangeSum ;
- int longTermRangeCount ;
- int tofIndex ;
- int tofCount ;
+// double adist[RTD_MED_SZ] ;
+// double adist4[4] ;
+// double longTermRangeSum ;
+// int longTermRangeCount ;
+// int tofIndex ;
+// int tofCount ;
uint8 newRangeUWBIndex; //index for most recent ranging exchange
int newRange;
@@ -156,10 +156,10 @@ typedef struct
// uint8 uwbNumActive[UWB_LIST_SIZE]; //number of TAGs each tracked ANCHOR is actively ranging with. //TODO remove?
// keep track of when final messages so we can drop uwbs that we haven't communicated with in a while
- uint32 lastCommTimeStamp[UWB_LIST_SIZE]; //TODO move into tdma_handler?
- uint32 lastHiddenTimeStamp[UWB_LIST_SIZE];
- uint32 lastTwiceHiddenTimeStamp[UWB_LIST_SIZE];
- uint8 uwbTimeout[UWB_LIST_SIZE] ; //TODO remove and use list type instead
+// uint32 lastCommTimeStamp[UWB_LIST_SIZE]; //TODO move into tdma_handler?
+// uint32 lastHiddenTimeStamp[UWB_LIST_SIZE];
+// uint32 lastTwiceHiddenTimeStamp[UWB_LIST_SIZE];
+// uint8 uwbTimeout[UWB_LIST_SIZE] ; //TODO remove and use list type instead
// uint32 lastRangeTimeStamp[UWB_LIST_SIZE];
@@ -214,9 +214,10 @@ int instaddactivateuwbinlist(instance_data_t *inst, uint8 *uwbAddr);
int instcheckactiveuwbinlist(instance_data_t *inst, uint8 *uwbAddr);
int instfindfirstactiveuwbinlist(instance_data_t *inst, uint8 startindex);
int instfindnumactiveuwbinlist(instance_data_t *inst);
-int instfindnumactiveneighbors(instance_data_t *inst);
-int instfindnumactivehidden(instance_data_t *inst);
+int instfindnumneighbors(instance_data_t *inst);
+int instfindnumhidden(instance_data_t *inst);
int instgetuwblistindex(instance_data_t *inst, uint8 *uwbAddr, uint8 addrByteSize);
+void instremoveuwb(instance_data_t *inst, uint8 uwb_index);
void instance_readaccumulatordata(void);
diff --git a/src/application/instance_common.c b/src/application/instance_common.c
index b9b6dd9a3ba0e8ce80e88dfbb215a040c06fc247..ac1cb51f1b7cc8de31a1d504e6a9186bfa3895ab 100644
--- a/src/application/instance_common.c
+++ b/src/application/instance_common.c
@@ -296,7 +296,7 @@ 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;
+// inst->uwbTimeout[i] = 1;
return i;
}
}
@@ -309,142 +309,195 @@ int instgetuwblistindex(instance_data_t *inst, uint8 *uwbAddr, uint8 addrByteSiz
return 255;
}
-
// -------------------------------------------------------------------------------------------------------------------
//
-// function to add new UWBs to the uwb list or remove timeout status from UWBs already in uwb list
+// function to remove the UWB at a given index from our list. Take care when calling this inside of a loop iterating over the uwbList!
//
// -------------------------------------------------------------------------------------------------------------------
-// return 1 if UWB added to list or removed from timeout status
-// return 0 if UWB not added list or removed from timeout status
-int instaddactivateuwbinlist(instance_data_t *inst, uint8 *uwbAddr)
+// return index of UWB
+void instremoveuwb(instance_data_t *inst, uint8 uwb_index)
{
- uint8 i;
- uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+ if(uwb_index > 0 && uwb_index < inst->uwbListLen)
+ {
+ if(inst->uwbToRangeWith == uwb_index)
+ {
+ uwb_index = 255;
+ }
+ else if(inst->uwbToRangeWith > uwb_index && inst->uwbToRangeWith < inst->uwbListLen)
+ {
+ inst->uwbToRangeWith--;
+ }
- //add the new UWB to the list, if not already there and there is space
- for(i=1; i<UWB_LIST_SIZE; i++)//0 reserved for self. timeout status not used for self
- {
- if(memcmp(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) != 0)
- {
- if(memcmp(&inst->uwbList[i][0], &blank[0], inst->addrByteSize) == 0) //blank entry
- {
- memcpy(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) ;
- inst->uwbListLen = i + 1 ;
- inst->uwbToRangeWith = i;
- inst->uwbTimeout[i] = 0;
- return 1;
- }
- }
- else
- {
- if(inst->uwbTimeout[i])
+ if(inst->newRange == 1)
+ {
+ if(inst->newRangeUWBIndex == uwb_index)
{
- //uwb has timed out, wake it up
- inst->uwbToRangeWith = i;
- inst->uwbTimeout[i] = 0;
- return 1;
+ uwb_index = 0;
+ inst->newRange = 0;
}
- else
+ else if(inst->newRangeUWBIndex > uwb_index && inst->newRangeUWBIndex < inst->uwbListLen)
{
- //we already have this uwb in the list and it has not timed out
- break;
+ inst->newRangeUWBIndex--;
}
- }
- }
+ }
- return 0;
-}
-// -------------------------------------------------------------------------------------------------------------------
-//
-// function to check if a UWB is already in our list and not in a timeout status
-//
-// -------------------------------------------------------------------------------------------------------------------
-// return index if UWB in list and not timed out
-// return 255 if UWB not in list or is but has timed out
-int instcheckactiveuwbinlist(instance_data_t *inst, uint8 *uwbAddr)
-{
- uint8 i;
- uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+ for(int i=uwb_index; i<inst->uwbListLen-1; i++)
+ {
+ memcpy(&inst->uwbList[i][0], &inst->uwbList[i+1][0], inst->addrByteSize);
+ inst->tof[i] = inst->tof[i+1];
+ inst->idistance[i] = inst->idistance[i+1];
+ inst->idistanceraw[i] = inst->idistanceraw[i];
+ }
+ uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+ memcpy(&inst->uwbList[inst->uwbListLen-1][0], &blank[0], 8);
+ inst->tof[inst->uwbListLen-1] = 0;
+ inst->idistance[inst->uwbListLen-1] = 0;
+ inst->idistanceraw[inst->uwbListLen-1] = 0;
- //add the new UWB to the list, if not already there and there is space
- for(i=1; i<UWB_LIST_SIZE; i++)//0 reserved for self, timeout not used for self
- {
- if(memcmp(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) == 0)
- {
- if(inst->uwbTimeout[i])
- {
- //UWB in list, but timed out
- break;
- }
- else
- {
- return i; //we already have this UWB in the list
- }
- }
- else
- {
- if(memcmp(&inst->uwbList[i][0], &blank[0], 8) == 0) //blank entry
- {
- break;
- }
- }
- }
+ //update the tdma_handler before reducing the size of the uwbList
+ tdma_handler.remove_uwbinfo(&tdma_handler, uwb_index);
- return 255;
+ inst->uwbListLen--;
+ }
}
+
// -------------------------------------------------------------------------------------------------------------------
+////
+//// function to add new UWBs to the uwb list or remove timeout status from UWBs already in uwb list
+////
+//// -------------------------------------------------------------------------------------------------------------------
+//// return 1 if UWB added to list or removed from timeout status
+//// return 0 if UWB not added list or removed from timeout status
+//int instaddactivateuwbinlist(instance_data_t *inst, uint8 *uwbAddr)
+//{
+// uint8 i;
+// uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
//
-// function to find the first UWB in our list that is not in a timeout status, starting with the given index
+// //add the new UWB to the list, if not already there and there is space
+// for(i=1; i<UWB_LIST_SIZE; i++)//0 reserved for self. timeout status not used for self
+// {
+// if(memcmp(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) != 0)
+// {
+// if(memcmp(&inst->uwbList[i][0], &blank[0], inst->addrByteSize) == 0) //blank entry
+// {
+// memcpy(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) ;
+// inst->uwbListLen = i + 1 ;
+// inst->uwbToRangeWith = i;
+// inst->uwbTimeout[i] = 0;
+// return 1;
+// }
+// }
+// else
+// {
+// if(inst->uwbTimeout[i])
+// {
+// //uwb has timed out, wake it up
+// inst->uwbToRangeWith = i;
+// inst->uwbTimeout[i] = 0;
+// return 1;
+// }
+// else
+// {
+// //we already have this uwb in the list and it has not timed out
+// break;
+// }
+// }
+// }
//
-// -------------------------------------------------------------------------------------------------------------------
-// return index for first UWB in list that is not timed out
-// return 255 if all UWBs in list (from the starting index) are timed out
-int instfindfirstactiveuwbinlist(instance_data_t *inst, uint8 startindex)
-{
- uint8 i;
-
- for(i=startindex; i<inst->uwbListLen; i++)
- {
- if(!inst->uwbTimeout[i])
- {
- return i;
- }
- }
+// return 0;
+//}
- return 255;
-}
+//// -------------------------------------------------------------------------------------------------------------------
+////
+//// function to check if a UWB is already in our list and not in a timeout status
+////
+//// -------------------------------------------------------------------------------------------------------------------
+//// return index if UWB in list and not timed out
+//// return 255 if UWB not in list or is but has timed out
+//int instcheckactiveuwbinlist(instance_data_t *inst, uint8 *uwbAddr)
+//{
+// uint8 i;
+// uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+//
+// //add the new UWB to the list, if not already there and there is space
+// for(i=1; i<UWB_LIST_SIZE; i++)//0 reserved for self, timeout not used for self
+// {
+// if(memcmp(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) == 0)
+// {
+// if(inst->uwbTimeout[i])
+// {
+// //UWB in list, but timed out
+// break;
+// }
+// else
+// {
+// return i; //we already have this UWB in the list
+// }
+// }
+// else
+// {
+// if(memcmp(&inst->uwbList[i][0], &blank[0], 8) == 0) //blank entry
+// {
+// break;
+// }
+// }
+// }
+//
+// return 255;
+//}
+
+//// -------------------------------------------------------------------------------------------------------------------
+////
+//// function to find the first UWB in our list that is not in a timeout status, starting with the given index
+////
+//// -------------------------------------------------------------------------------------------------------------------
+//// return index for first UWB in list that is not timed out
+//// return 255 if all UWBs in list (from the starting index) are timed out
+//int instfindfirstactiveuwbinlist(instance_data_t *inst, uint8 startindex)
+//{
+// uint8 i;
+//
+// for(i=startindex; i<inst->uwbListLen; i++)
+// {
+// if(!inst->uwbTimeout[i])
+// {
+// return i;
+// }
+// }
+//
+// return 255;
+//}
-// -------------------------------------------------------------------------------------------------------------------
+//// -------------------------------------------------------------------------------------------------------------------
+////
+//// function to find the number of UWBs in our list that are not in a timeout status
+////
+//// -------------------------------------------------------------------------------------------------------------------
+//int instfindnumactiveuwbinlist(instance_data_t *inst)
+//{
+// uint8 num = 0;
//
-// function to find the number of UWBs in our list that are not in a timeout status
+// for(int i=1; i<inst->uwbListLen; i++) //0 reserved for self, timeout status not applicable
+// {
+// if(!inst->uwbTimeout[i])
+// {
+// num++;
+// }
+// }
//
-// -------------------------------------------------------------------------------------------------------------------
-int instfindnumactiveuwbinlist(instance_data_t *inst)
-{
- uint8 num = 0;
-
- for(int i=1; i<inst->uwbListLen; i++) //0 reserved for self, timeout status not applicable
- {
- if(!inst->uwbTimeout[i])
- {
- num++;
- }
- }
-
- return num;
-}
+// return num;
+//}
// -------------------------------------------------------------------------------------------------------------------
//
// function to find the number of neighbor UWBs in our list that are not in a timeout status
//
// -------------------------------------------------------------------------------------------------------------------
-int instfindnumactiveneighbors(instance_data_t *inst)
+int instfindnumneighbors(instance_data_t *inst)
{
uint8 num = 0;
@@ -465,7 +518,7 @@ int instfindnumactiveneighbors(instance_data_t *inst)
// function to find the number of hidden neighbor UWBs in our list that are not in a timeout status
//
// -------------------------------------------------------------------------------------------------------------------
-int instfindnumactivehidden(instance_data_t *inst)
+int instfindnumhidden(instance_data_t *inst)
{
uint8 num = 0;
@@ -474,7 +527,6 @@ int instfindnumactivehidden(instance_data_t *inst)
// if(inst->uwbListType[i] == UWB_LIST_HIDDEN)
if(tdma_handler.uwbListTDMAInfo[i].connectionType == UWB_LIST_HIDDEN)
{
-
num++;
}
}
@@ -499,15 +551,7 @@ void instclearuwbList(void)
for(int i=0; i<UWB_LIST_SIZE; i++)
{
- instance_data[instance].lastCommTimeStamp[i] = 0;
-// instance_data[instance].lastRangeTimeStamp[i] = 0;
- tdma_handler.uwbListTDMAInfo[i].lastRange = 0;
- instance_data[instance].uwbTimeout[i] = 0;
-// instance_data[instance].time_till_next_reported[i] = 0;
-
memcpy(&instance_data[instance].uwbList[i][0], &blank[0], 8);
-// instance_data[instance].uwbListType[i] = UWB_LIST_INACTIVE;
- tdma_handler.uwbListTDMAInfo[i].connectionType = UWB_LIST_INACTIVE;
}
}
@@ -579,16 +623,16 @@ void instanceclearcounts(void)
instance_data[instance].frameSN = 0;
- instance_data[instance].tofCount = 0 ;
- instance_data[instance].tofIndex = 0 ;
+// instance_data[instance].tofCount = 0 ;
+// instance_data[instance].tofIndex = 0 ;
instance_data[instance].txmsgcount = 0;
instance_data[instance].rxmsgcount = 0;
instance_data[instance].lateTX = 0;
instance_data[instance].lateRX = 0;
- instance_data[instance].longTermRangeSum = 0;
- instance_data[instance].longTermRangeCount = 0;
+// instance_data[instance].longTermRangeSum = 0;
+// instance_data[instance].longTermRangeCount = 0;
instclearuwbList();
@@ -610,8 +654,8 @@ int instance_init(void)
instance_data[instance].ranging = 0;
instance_data[instance].goToSleep = 0;
- instance_data[instance].tofIndex = 0;
- instance_data[instance].tofCount = 0;
+// instance_data[instance].tofIndex = 0;
+// instance_data[instance].tofCount = 0;
for(uint8 i=0; i<UWB_LIST_SIZE; i++)
{
instance_data[instance].tof[i] = 0;
@@ -1322,6 +1366,16 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
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;
+ }
+
+
// uint8 debug_msg[100];
//// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK RECEIVED: BLINK uwb_index: %d, uwbToRangeWith: %d ", uwb_index, instance_data[instance].uwbToRangeWith);
@@ -1334,12 +1388,15 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
//must be a neighbor
uwb_index = instgetuwblistindex(&instance_data[instance], &dw_event.msgu.frame[srcAddr_index], instance_data[instance].addrByteSize);
- //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;
- instance_data[instance].uwbTimeout[uwb_index] = 0;
-
+ 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;
+ }
// uint8 debug_msg[100];
@@ -1349,25 +1406,26 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
//// uint16 *addr_ptr = &instance_data[instance].uwbShortAdd;
//// int n = sprintf((char*)&debug_msg[0], "RX CB RX: DWT_SIG_RX_OKAY-%s,%u,xxxx,%p", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]), my_addr, (void *) addr_ptr);
//// int n = sprintf((char*)&debug_msg[0], "RX CB RX: DWT_SIG_RX_OKAY-%s,%u,xxxx", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]), my_addr);
-// int n = sprintf((char*)&debug_msg[0], "RX CB RX: DWT_SIG_RX_OKAY-%s,%04X,xxxx", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]), my_addr);
+// int n = sprintf((char*)&debug_msg[0], "RX CB RX: DWT_SIG_RX_OKAY-%s,%04X,uwb_index: %04d, uwb_trw: %04d", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]), my_addr,uwb_index, instance_data[instance].uwbToRangeWith);
//// int n = sprintf((char*)&debug_msg[0], "%u,%p", my_addr, (void *)addr_ptr);
// send_usbmessage(&debug_msg[0], n);
// usb_run();
}
- if(uwb_index > UWB_LIST_SIZE - 1)
- {
- instance_data_t *inst = &instance_data[instance];
+// if(uwb_index > UWB_LIST_SIZE - 1 || uwb_index <= 0)
+// {
+// instance_data_t *inst = &instance_data[instance];
- uint8 debug_msg[100];
- uint16 my_addr = instance_data[instance].uwbShortAdd;
- int n = sprintf((char*)&debug_msg[0], "uwb_index:%u of %u",uwb_index, instance_data[instance].uwbListLen);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint8 debug_msg[100];
+// uint16 my_addr = instance_data[instance].uwbShortAdd;
+// int n = sprintf((char*)&debug_msg[0], "uwb_index:%u of %u",uwb_index, instance_data[instance].uwbListLen);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
//TODO solve this problem. other, (incorrect) addresses are making it into our UWB list for some reason...
- }
+ //think i took care of it, it was incorrect indexing in create/read INF messages
+// }
bool accept_inf = FALSE;
//check if the incoming message indicates that we should range with the source UWB or just accept and process the message
@@ -1397,7 +1455,7 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
if(tdma_handler.discovery_mode == WAIT_RNG_INIT)
{
//only accept RNG_INIT if no other active neighbors exist (and we are waiting for RNG_INIT)
- int num_active = instfindnumactiveneighbors(&instance_data[instance]);
+ int num_active = instfindnumneighbors(&instance_data[instance]);
if(num_active <= 1)
{
instance_data[instance].uwbToRangeWith = uwb_index;
@@ -1432,7 +1490,7 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
{
if(tdma_handler.discovery_mode == WAIT_INF_REG)
{
- int num_neighbors = instfindnumactiveneighbors(&instance_data[instance]);
+ int num_neighbors = instfindnumneighbors(&instance_data[instance]);
// uint8 debug_msg[100];
// int n = sprintf((char*)&debug_msg[0], "num_neighbors %d", num_neighbors);
// send_usbmessage(&debug_msg[0], n);
@@ -1466,8 +1524,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: RNG_REPORT <- uwb_index %d", uwb_index);
// send_usbmessage(&debug_msg[0], n);
// usb_run();
- instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
- instance_data[instance].uwbTimeout[uwb_index] = 0;
+// instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
+// instance_data[instance].uwbTimeout[uwb_index] = 0;
place_event = 1;
}
}
@@ -1485,8 +1543,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
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;
+// instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
+// instance_data[instance].uwbTimeout[uwb_index] = 0;
place_event = 1;
@@ -1515,8 +1573,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
// send_usbmessage(&debug_msg[0], n);
// usb_run();
- instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
- instance_data[instance].uwbTimeout[uwb_index] = 0;
+// instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
+// instance_data[instance].uwbTimeout[uwb_index] = 0;
uint16 frameLength = 0;
@@ -1643,8 +1701,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
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;
+// 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);
@@ -1652,8 +1710,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
}
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;
+// 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);
@@ -1661,8 +1719,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
}
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;
+// 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);
@@ -1709,6 +1767,11 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
if(place_event)
{
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "event placed");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
instance_putevent(dw_event);
#if (DEEP_SLEEP == 1)
@@ -1839,78 +1902,94 @@ int instance_run(void)
message = 0;
}
- //check if lastCommTimeStamp has expired for any of the uwbs in our list
- for(int i=1; i < instance_data[instance].uwbListLen; i++)//0 reserved for self, timeout not applicable
+ uint32 time_now = portGetTickCnt();
+ //check timeouts. handle case where we timeout and no longer have any neighbors
+ if(tdma_handler.check_timeouts(&tdma_handler, time_now))
{
- //TODO could also have a timer to put into DISCOVERY if we haven't spoken to anyone in a while
- uint32 time_now = portGetTickCnt();
-
- uint32 delta_t = get_dt32(instance_data[instance].lastCommTimeStamp[i], time_now);
- if(delta_t > UWB_COMM_TIMEOUT) //TODO handle number wrapping
- {
-// if(instance_data[instance].uwbListType[i] == UWB_LIST_NEIGHBOR) //what about hidden?
- if(tdma_handler.uwbListTDMAInfo[i].connectionType == UWB_LIST_NEIGHBOR) //what about hidden?
- {
-// instance_data[instance].uwbListType[i] = UWB_LIST_INACTIVE; //TODO release TDMA slots as well
- tdma_handler.uwbListTDMAInfo[i].connectionType = UWB_LIST_INACTIVE; //TODO release TDMA slots as well
- instance_data[instance].uwbTimeout[i] = 1;
-
- //NEW
- //if no more active neighbors exist, transition to DISCOVERY
- uint8 numNeighbors = instfindnumactiveneighbors(&instance_data[instance]);
- if(numNeighbors <= 0)
- {
- tdma_handler.tdma_free_all_slots(&tdma_handler);
-
+ tdma_handler.tdma_free_all_slots(&tdma_handler);
+ instance_data[instance].mode = DISCOVERY; //TODO clear the TDMA slot information!
+ tdma_handler.discoveryStartTime = portGetTickCnt();
+ tdma_handler.enter_discovery_mode(&tdma_handler);
+ inst_processtxrxtimeout(&instance_data[instance]);
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "inst_processtxrxtimeout(inst) after free_all_slots");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
- instance_data[instance].mode = DISCOVERY; //TODO clear the TDMA slot information!
- tdma_handler.discoveryStartTime = portGetTickCnt();
- tdma_handler.enter_discovery_mode(&tdma_handler);
- inst_processtxrxtimeout(&instance_data[instance]);
-
- instance_data[instance].canPrintInfo = 0;
- instance_data[instance].ranging = 0;
- }
- else
- {
- //TODO check if able to reconfigure tdma assignements. shrink frame size, reassign emptied slots, etc
- tdma_handler.uwblist_free_slots(&tdma_handler, i);
- }
-
-
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "TIMEOUT: uwb %i", i);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
- }
-
- if(instance_data[instance].uwbToRangeWith == i)
- {
- instance_data[instance].uwbToRangeWith = 255;
- //NOTE this might need to be changed for TAG operations
- if(instance_data[instance].mode == ANCHOR || instance_data[instance].mode == DISCOVERY) //TODO maybe send to TA_MODE_SELECT?
- {
- instance_data[instance].testAppState = TA_RXE_WAIT;
- }
- else
- {
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "instance_run(void) TIMEOUT");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
- instance_data[instance].testAppState = TA_TX_SELECT;
- }
- }
- }
-
+ instance_data[instance].canPrintInfo = 0;
+ instance_data[instance].ranging = 0;
}
+// //check if lastCommTimeStamp has expired for any of the uwbs in our list
+// for(int i=1; i < instance_data[instance].uwbListLen; i++)//0 reserved for self, timeout not applicable
+// {
+// //TODO could also have a timer to put into DISCOVERY if we haven't spoken to anyone in a while
+// uint32 time_now = portGetTickCnt();
+//
+//
+// uint32 delta_t = get_dt32(instance_data[instance].lastCommTimeStamp[i], time_now);
+// if(delta_t > UWB_COMM_TIMEOUT) //TODO handle number wrapping
+// {
+//// if(instance_data[instance].uwbListType[i] == UWB_LIST_NEIGHBOR) //what about hidden?
+// if(tdma_handler.uwbListTDMAInfo[i].connectionType == UWB_LIST_NEIGHBOR) //what about hidden?
+// {
+//// instance_data[instance].uwbListType[i] = UWB_LIST_INACTIVE; //TODO release TDMA slots as well
+// tdma_handler.uwbListTDMAInfo[i].connectionType = UWB_LIST_INACTIVE; //TODO release TDMA slots as well
+// instance_data[instance].uwbTimeout[i] = 1;
+//
+// //NEW
+// //if no more active neighbors exist, transition to DISCOVERY
+// uint8 numNeighbors = instfindnumactiveneighbors(&instance_data[instance]);
+// if(numNeighbors <= 0)
+// {
+// tdma_handler.tdma_free_all_slots(&tdma_handler);
+//
+//
+//
+//// uint8 debug_msg[100];
+//// int n = sprintf((char *)&debug_msg, "inst_processtxrxtimeout(inst) after free_all_slots");
+//// send_usbmessage(&debug_msg[0], n);
+//// usb_run();
+//
+// instance_data[instance].mode = DISCOVERY; //TODO clear the TDMA slot information!
+// tdma_handler.discoveryStartTime = portGetTickCnt();
+// tdma_handler.enter_discovery_mode(&tdma_handler);
+// inst_processtxrxtimeout(&instance_data[instance]);
+//
+// instance_data[instance].canPrintInfo = 0;
+// instance_data[instance].ranging = 0;
+// }
+// else
+// {
+// //TODO check if able to reconfigure tdma assignements. shrink frame size, reassign emptied slots, etc
+// tdma_handler.uwblist_free_slots(&tdma_handler, i);
+// }
+//
+//
+//// uint8 debug_msg[100];
+//// int n = sprintf((char*)&debug_msg[0], "TIMEOUT: uwb %i", i);
+//// send_usbmessage(&debug_msg[0], n);
+//// usb_run();
+// }
+//
+// if(instance_data[instance].uwbToRangeWith == i)
+// {
+// instance_data[instance].uwbToRangeWith = 255;
+// //NOTE this might need to be changed for TAG operations
+// if(instance_data[instance].mode == ANCHOR || instance_data[instance].mode == DISCOVERY) //TODO maybe send to TA_MODE_SELECT?
+// {
+// instance_data[instance].testAppState = TA_RXE_WAIT;
+// }
+// else
+// {
+//// uint8 debug_msg[100];
+//// int n = sprintf((char*)&debug_msg[0], "instance_run(void) TIMEOUT");
+//// send_usbmessage(&debug_msg[0], n);
+//// usb_run();
+// instance_data[instance].testAppState = TA_TX_SELECT;
+// }
+// }
+// }
+//
+// }
+
if(done == INST_DONE_WAIT_FOR_NEXT_EVENT_TO) //we are in RX and need to timeout (Tag needs to send another poll if no Rx frame)
{
if(instance_data[instance].mode == DISCOVERY)
diff --git a/src/application/tdma_handler.c b/src/application/tdma_handler.c
index 735feb075a52dbf7683a2291db9432f8caf5e6c2..e5c111811aceefb3087955e36e2a71aba3aee9c0 100644
--- a/src/application/tdma_handler.c
+++ b/src/application/tdma_handler.c
@@ -22,7 +22,9 @@ static bool slot_transition(struct TDMAHandler *this)
if(timeSinceSlotStart64 >= this->slotDuration_us) //TODO should i check frame separate from slot?
{
transition = TRUE;
- this->pollSentThisSlot = FALSE;
+ this->firstPollSentThisSlot = FALSE;
+ this->firstPollComplete = FALSE;
+ this->secondPollSentThisSlot = FALSE;
this->infSentThisSlot = FALSE;
//we have transitioned into the next slot.
@@ -30,6 +32,7 @@ static bool slot_transition(struct TDMAHandler *this)
// uint64 timeSinceFrameStart64 = get_dt64(this->uwbFrameStartTimes64[0], time_now_us);
uint64 timeSinceFrameStart64 = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now_us);
+ //TODO this->uwbListTDMAInfo[0].frameStartTime is sometimes getting set to a time in the future... figure out!
uint64 frameDuration64 = this->slotDuration_us*this->uwbListTDMAInfo[0].framelength;
if(timeSinceFrameStart64 >= frameDuration64)
@@ -113,7 +116,9 @@ static bool slot_transition(struct TDMAHandler *this)
else if(inst->mode == DISCOVERY)
{
this->infSentThisSlot = FALSE;
- this->pollSentThisSlot = FALSE;
+ this->firstPollSentThisSlot = FALSE;
+ this->firstPollComplete = FALSE;
+ this->secondPollSentThisSlot = FALSE;
}
return transition;
@@ -125,6 +130,19 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 f
{
instance_data_t *inst = instance_get_local_structure_ptr(0);
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "frame sync");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ //do not process erroneous tsfs
+ //can happen if frame start time is shifted ahead of time_now_us in transmitting UWB
+ if(timeSinceFrameStart_us > 1000000000 || timeSinceFrameStart_us == 0)
+ {
+ return;
+ }
+
+
uint8 sys_time_arr[5] = {0, 0, 0, 0, 0};
dwt_readsystime(sys_time_arr);
uint64 dwt_time_now = 0;
@@ -159,17 +177,19 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 f
// this->uwbFrameStartTimes64[srcIndex] = timestamp_subtract64(time_now_us, hisTimeSinceFrameStart_us); //TODO consider applying the diff!
this->uwbListTDMAInfo[srcIndex].frameStartTime = timestamp_subtract64(time_now_us, hisTimeSinceFrameStart_us); //TODO consider applying the diff!
- if(mode == FS_ADOPT) //TODO this might not be right! incoming framelength not always the same as ours!
+ if(mode == FS_COLLECT)
{
-// this->uwbFrameStartTimes64[0] = this->uwbFrameStartTimes64[srcIndex];
- this->uwbListTDMAInfo[0].frameStartTime = this->uwbListTDMAInfo[srcIndex].frameStartTime;
- uint8 slot = hisTimeSinceFrameStart_us/this->slotDuration_us; //integer division rounded down
-// this->lastSlotStartTime64 = this->uwbFrameStartTimes64[0] + (uint64)((this->slotDuration_us)*slot);
- this->lastSlotStartTime64 = this->uwbListTDMAInfo[0].frameStartTime + (uint64)((this->slotDuration_us)*slot);
+ return;
}
- else if(mode == FS_COLLECT)
+ else if(mode == FS_ADOPT) //TODO this might not be right! incoming framelength not always the same as ours!
{
- return;
+// this->uwbFrameStartTimes64[0] = this->uwbFrameStartTimes64[srcIndex];
+// this->uwbListTDMAInfo[0].frameStartTime = this->uwbListTDMAInfo[srcIndex].frameStartTime;
+// uint8 slot = hisTimeSinceFrameStart_us/this->slotDuration_us; //integer division rounded down
+// this->lastSlotStartTime64 = this->uwbFrameStartTimes64[0] + (uint64)((this->slotDuration_us)*slot);
+// this->lastSlotStartTime64 = this->uwbListTDMAInfo[0].frameStartTime + (uint64)((this->slotDuration_us)*slot);
+
+ this->uwbListTDMAInfo[0].frameStartTime = this->uwbListTDMAInfo[srcIndex].frameStartTime;//NOTE gets processed further at end of function
}
else// if(mode == FS_AVERAGE || mode == FS_EVAL) //TODO consider using a weighted average. perhaps use threshholds to determine if instead we should just use the ADOPT logic
{
@@ -177,69 +197,7 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 f
// uint64 myTimeSinceFrameStart_us = get_dt64(this->uwbFrameStartTimes64[0], time_now_us);
uint64 myTimeSinceFrameStart_us = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now_us);
- ////OLD WAY
-
-// if(this->uwbListTDMAInfo[0].framelength <= framelength)
-// {
-// uint64 hisTimeSinceFrameStartMod_us = hisTimeSinceFrameStart_us%myFramelengthDuration_us;
-//
-// if(myTimeSinceFrameStart_us > hisTimeSinceFrameStartMod_us)
-// {
-// uint64 diff_us = myTimeSinceFrameStart_us - hisTimeSinceFrameStartMod_us;
-//
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "add %llu", diff_us);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-//
-// this->uwbFrameStartTimes64[0] = timestamp_add64(this->uwbFrameStartTimes64[0], diff_us/2);
-// this->lastSlotStartTime64 = timestamp_add64(this->lastSlotStartTime64, diff_us/2);
-// }
-// else
-// {
-// uint64 diff_us = hisTimeSinceFrameStartMod_us - myTimeSinceFrameStart_us;
-//
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "subtract %llu", diff_us);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-//
-// this->uwbFrameStartTimes64[0] = timestamp_subtract64(this->uwbFrameStartTimes64[0], diff_us/2);
-// this->lastSlotStartTime64 = timestamp_subtract64(this->lastSlotStartTime64, diff_us/2);
-// }
-// }
-// else
-// {
-// uint64 hisFramelengthDuration_us = framelength*this->slotDuration_us;
-// uint64 myTimeSinceFrameStartMod_us = myTimeSinceFrameStart_us%hisFramelengthDuration_us;
-//
-// if(hisTimeSinceFrameStart_us > myTimeSinceFrameStartMod_us)
-// {
-// uint64 diff_us = hisTimeSinceFrameStart_us - myTimeSinceFrameStartMod_us;
-//
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "subtract %llu", diff_us);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-//
-// this->uwbFrameStartTimes64[0] = timestamp_subtract64(this->uwbFrameStartTimes64[0], diff_us/2);
-// this->lastSlotStartTime64 = timestamp_subtract64(this->lastSlotStartTime64, diff_us/2);
-// }
-// else
-// {
-// uint64 diff_us = myTimeSinceFrameStartMod_us - hisTimeSinceFrameStart_us;
-//
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "add %llu", diff_us);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-//
-// this->uwbFrameStartTimes64[0] = timestamp_add64(this->uwbFrameStartTimes64[0], diff_us/2);
-// this->lastSlotStartTime64 = timestamp_add64(this->lastSlotStartTime64, diff_us/2);
-// }
-// }
-
- //NEW WAY SELF VS INCOMING
+ //SELF VS INCOMING
uint8 min_fl = this->uwbListTDMAInfo[0].framelength;
if(framelength < min_fl)
@@ -310,7 +268,7 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 f
// this->uwbFrameStartTimes64[0] = timestamp_add64(this->uwbFrameStartTimes64[0], diff_us/div);
this->uwbListTDMAInfo[0].frameStartTime = timestamp_add64(this->uwbListTDMAInfo[0].frameStartTime, diff_us/div);
- this->lastSlotStartTime64 = timestamp_add64(this->lastSlotStartTime64, diff_us/div);
+// this->lastSlotStartTime64 = timestamp_add64(this->lastSlotStartTime64, diff_us/div);
}
else
{
@@ -320,11 +278,27 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 f
// usb_run();
// this->uwbFrameStartTimes64[0] = timestamp_subtract64(this->uwbFrameStartTimes64[0], diff_us/div);
this->uwbListTDMAInfo[0].frameStartTime = timestamp_subtract64(this->uwbListTDMAInfo[0].frameStartTime, diff_us/div);
- this->lastSlotStartTime64 = timestamp_subtract64(this->lastSlotStartTime64, diff_us/div);
+// this->lastSlotStartTime64 = timestamp_subtract64(this->lastSlotStartTime64, diff_us/div);
}
+ }
+ uint64 myFrameDuration = this->slotDuration_us*this->uwbListTDMAInfo[0].framelength;
+ uint64 myTimeSinceFrameStart_us = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now_us); //TODO what if added to be past time_now???
+ if(myTimeSinceFrameStart_us > 100000000)
+ {
+ //if this is a very large number, then the frame start time was likely moved head of time_now_us.
+ this->lastSlotStartTime64 = this->uwbListTDMAInfo[0].frameStartTime - (uint64)(this->slotDuration_us);
+ }
+ else
+ {
+ while(myTimeSinceFrameStart_us >= myFrameDuration)
+ {
+ this->uwbListTDMAInfo[0].frameStartTime = timestamp_add64(this->uwbListTDMAInfo[0].frameStartTime, myFrameDuration);
+ myTimeSinceFrameStart_us -= myFrameDuration;
+ }
-
+ uint8 slot = hisTimeSinceFrameStart_us/this->slotDuration_us; //integer division rounded down
+ this->lastSlotStartTime64 = this->uwbListTDMAInfo[0].frameStartTime + (uint64)(this->slotDuration_us*slot);
}
}
@@ -548,7 +522,7 @@ static bool tx_sync_msg(struct TDMAHandler *this)
//}
-static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
+static bool tx_select(struct TDMAHandler *this)
{
// uint8 debug_msg[100];
// int n = sprintf((char *)&debug_msg, "tx_select,");
@@ -559,12 +533,6 @@ static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
uint32 time_now = portGetTickCnt(); //TODO come back and perhaps use only one or the other
uint32 time_now_us = portGetTickCntMicro(); //TODO see time_now
-// uint32 offset = 0;
-// //force number wrapping in case the timestamp is getting close to wrapping
-// if(time_now > 4000000000){
-// offset = 1000000000;
-// }
-// uint32 time_now_offset = time_now + offset;
//DISCOVERY pauses for BLINK_DELAY <-added
// if(this->waitForInf == TRUE || this->waitForRngInit == TRUE)
@@ -623,8 +591,7 @@ static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
return TRUE;
}
}
-// else if(this->discovery_mode == WAIT_SEND_SUG)//TODO left off here
- else if(this->discovery_mode == SEND_SUG)//TODO left off here
+ else if(this->discovery_mode == SEND_SUG)
{
// uint8 debug_msg[100];
// int n = sprintf((char *)&debug_msg, "discovery send sug");
@@ -656,47 +623,70 @@ static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
}
else if(inst->mode == TAG)
{
-
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "tag");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
//get time since slot start and make sure that's greater than delay
uint64 timeSinceSlotStart = get_dt64(this->lastSlotStartTime64, time_now_us);
-
//TAG pauses for INF_POLL_DELAY <-added at beginning of slot
-// if(this->poll_delay(this, time_now_offset, offset) == TRUE)
if(timeSinceSlotStart <= this->slotStartDelay_us)
{
uwb_index = -1;
}
else
{
- //check which neighbor UWB hasn't been ranged with
+ if(this->firstPollSentThisSlot == TRUE && this->secondPollSentThisSlot == FALSE && this->firstPollComplete == FALSE)
+ {
+ int test = 1;
+ test++;
+ }
+
+ //check which neighbor UWB hasn't been ranged with for the longest amount of time
uint32 timeSinceOldestRange = 0;
+ uint32 timeSinceSecondOldestRange = 0;
+ uint8 second_oldest_uwb_index = 255;
for(int i = 1; i < inst->uwbListLen; i++)//0 reserved for self
{
-// if(inst->uwbListType[i] == UWB_LIST_NEIGHBOR)
if(this->uwbListTDMAInfo[i].connectionType == UWB_LIST_NEIGHBOR)
{
-// uint32 timeSinceRange = get_dt32(inst->lastRangeTimeStamp[i], time_now);
uint32 timeSinceRange = get_dt32(this->uwbListTDMAInfo[i].lastRange, time_now);
- if(timeSinceRange > timeSinceOldestRange)
+
+ if(timeSinceOldestRange == 0)
{
timeSinceOldestRange = timeSinceRange;
+ timeSinceSecondOldestRange = timeSinceRange;
+
uwb_index = i;
+ second_oldest_uwb_index = i;
+ }
+ else if(timeSinceRange > timeSinceOldestRange)
+ {
+ timeSinceSecondOldestRange = timeSinceOldestRange;
+ second_oldest_uwb_index = uwb_index;
+ timeSinceOldestRange = timeSinceRange;
+ uwb_index = i;
+ }
+ else if(timeSinceRange > timeSinceSecondOldestRange)
+ {
+ timeSinceSecondOldestRange = timeSinceRange;
+ second_oldest_uwb_index = i;
+ }
+ else if(timeSinceOldestRange == timeSinceSecondOldestRange)
+ {
+ timeSinceSecondOldestRange = timeSinceRange;
+ second_oldest_uwb_index = i;
}
}
}
- if(uwb_index == 255 && inst->uwbListLen > 0) //TODO what is the purpose of this exactly?
+ if(this->firstPollSentThisSlot == TRUE && this->secondPollSentThisSlot == FALSE && this->firstPollComplete == FALSE)
+ {
+ uwb_index = second_oldest_uwb_index;
+ }
+
+ if(uwb_index == 255 && inst->uwbListLen > 1)
{
uwb_index = 1;
}
}
-
}
else
{
@@ -711,40 +701,33 @@ static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
// usb_run();
- if(uwb_index < 1){
+ if(uwb_index < 1) //set to -1 when waiting for the slotStartDelay to pass
+ {
//do nothing
- //TODO would be best to sleep until next possible poll and then select again...
-// done = INST_DONE_WAIT_FOR_NEXT_EVENT; //TODO make sure that this is the right thing to do.
-
-// debug_msg[100];
-// n = sprintf((char *)&debug_msg, "1");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
return FALSE;
-
- }else if(uwb_index > 254){
+ }
+ else if(uwb_index > 254)
+ {
inst->testAppState = TA_TXBLINK_WAIT_SEND;
inst->uwbToRangeWith = (uint8)255;
-
-// debug_msg[100];
-// n = sprintf((char *)&debug_msg, "2");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
- }else{
-
+ }
+ else
+ {
uint8 fcode = RTLS_DEMO_MSG_INF_INIT;
if(memcmp(&inst->inf_msg.messageData[FCODE], &fcode, sizeof(uint8)) == 0)
{
this->infSentThisSlot = TRUE;
inst->testAppState = TA_TXINF_WAIT_SEND;
}
- else if(this->pollSentThisSlot == FALSE)
+ else if(this->firstPollSentThisSlot == FALSE)
+ {
+ this->firstPollSentThisSlot = TRUE;
+ inst->testAppState = TA_TXPOLL_WAIT_SEND;
+ }
+ else if(this->secondPollSentThisSlot == FALSE && this->firstPollComplete == FALSE)
{
- this->pollSentThisSlot = TRUE;
- inst->testAppState = TA_TXPOLL_WAIT_SEND;//TODO modify any other related variables with this change if we keep it.
- //in case the ranging event fails, dont just wait till end of slot, move on toe TA_TXINF_WAIT_SEND
+ this->secondPollSentThisSlot = TRUE;
+ inst->testAppState = TA_TXPOLL_WAIT_SEND; //select new uwb index!
}
else if(this->infSentThisSlot == FALSE)
{
@@ -756,13 +739,6 @@ static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
return TRUE;
}
-
-// debug_msg[100];
-// n = sprintf((char *)&debug_msg, "3");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
-
inst->uwbToRangeWith = (uint8)uwb_index;
}
@@ -806,8 +782,8 @@ static void populate_inf_msg(struct TDMAHandler *this, uint8 inf_msg_type)
{
instance_data_t *inst = instance_get_local_structure_ptr(0);
- int num_neighbors = instfindnumactiveneighbors(inst);
- int num_hidden = instfindnumactivehidden(inst);
+ int num_neighbors = instfindnumneighbors(inst);
+ int num_hidden = instfindnumhidden(inst);
// uint32 time_now = portGetTickCnt();
//fcode
@@ -927,36 +903,26 @@ static void update_inf_tsfs(struct TDMAHandler *this)
instance_data_t *inst = instance_get_local_structure_ptr(0);
uint64 time_now_us = portGetTickCntMicro();
int msgDataIndex = TDMA_TSFS;
-// uint64 timeSinceFrameStart64 = get_dt64(this->uwbFrameStartTimes64[0], time_now_us);
uint64 timeSinceFrameStart64 = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now_us);
uint64 frameDuration = this->slotDuration_us*this->uwbListTDMAInfo[0].framelength;
- while(timeSinceFrameStart64 > frameDuration)
+
+ //if frameStartTime ahead of time_now_us, just send the erroneously large number over.
+ //the receiver will detect and disregard
+ if(timeSinceFrameStart64 < 10000000)
+ {
+ while(timeSinceFrameStart64 > frameDuration)
+ {
+ this->uwbListTDMAInfo[0].frameStartTime = timestamp_add64(this->uwbListTDMAInfo[0].frameStartTime, frameDuration);
+ timeSinceFrameStart64 -= frameDuration;
+ }
+ }
+ else
{
-// this->uwbFrameStartTimes64[0] = timestamp_add64(this->uwbFrameStartTimes64[0], frameDuration);
- this->uwbListTDMAInfo[0].frameStartTime = timestamp_add64(this->uwbListTDMAInfo[0].frameStartTime, frameDuration);
- timeSinceFrameStart64 -= frameDuration;
+ timeSinceFrameStart64 = 0;
}
memcpy(&inst->inf_msg.messageData[msgDataIndex], &timeSinceFrameStart64, 6);
-
- uint64 tsinf = get_dt64(this->lastINFtx, time_now_us);
- memcpy(&inst->inf_msg.messageData[1], &tsinf, 6);
- this->lastINFtx = time_now_us;
-
- //indicate whether the TSFS being transmitted is a rebase.
-// memcpy(&inst->inf_msg.messageData[TDMA_TSFS_REBASE], &this->rebase_pending, sizeof(bool));
-
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "TSFS %llu", timeSinceFrameStart64 );
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
-// uint8 sys_time_arr[5] = {0, 0, 0, 0, 0};
-// dwt_readsystime(sys_time_arr);
-// uint64 dwt_time_now = (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);
-// uint64 timeSinceFrameStart_dwt = dwt_getdt(this->uwbFrameStartTimes_dwt[0], dwt_time_now);
-// memcpy(&inst->inf_msg.messageData[100], &timeSinceFrameStart_dwt, 5);
}
@@ -980,6 +946,12 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
{
//NOTE: this function does not handle TDMA deconflict
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "process INF");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+
bool tdma_modified = FALSE;
uint32 time_now = portGetTickCnt();
@@ -1012,7 +984,6 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
}
instance_data_t *inst = instance_get_local_structure_ptr(0);
-// inst->uwbListType[srcIndex] = UWB_LIST_NEIGHBOR;
this->uwbListTDMAInfo[srcIndex].connectionType = UWB_LIST_NEIGHBOR;
uint8 numNeighbors;
@@ -1044,7 +1015,6 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
tdma_modified = TRUE;
}
-// msgDataIndex = TDMA_NUMS + 1;
//copy slot assignments for source UWB
info = &this->uwbListTDMAInfo[srcIndex];
if(framelength != info->framelength)
@@ -1115,15 +1085,14 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
this->uwbListTDMAInfo[uwb_index].connectionType = UWB_LIST_HIDDEN;
}
- inst->lastHiddenTimeStamp[uwb_index] = time_now;
+// inst->lastHiddenTimeStamp[uwb_index] = time_now;
+ this->uwbListTDMAInfo[uwb_index].lastCommHidden = time_now;
}
info = &this->uwbListTDMAInfo[uwb_index];
uwbListInMsg[uwb_index] = TRUE;
-
-
memcpy(&framelength, &messageData[msgDataIndex], sizeof(uint8));
msgDataIndex++;
memcpy(&numSlots, &messageData[msgDataIndex], sizeof(uint8));
@@ -1192,7 +1161,8 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
this->uwbListTDMAInfo[uwb_index].connectionType = UWB_LIST_TWICE_HIDDEN;
}
- inst->lastTwiceHiddenTimeStamp[uwb_index] = time_now;
+// inst->lastTwiceHiddenTimeStamp[uwb_index] = time_now;
+ this->uwbListTDMAInfo[uwb_index].lastCommTwiceHidden = time_now;
}
uwbListInMsg[uwb_index] = TRUE;
@@ -1294,76 +1264,17 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
}
}
- uint32 deltat = get_dt32(time_now, portGetTickCnt());
-
- if(deltat > 1)
- {
- uint8 debug_msg[100];
- int n = sprintf((char *)&debug_msg, "process_inf_time:%lu,%04X,xxxx", deltat, inst->uwbShortAdd);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
- }
-
- return tdma_modified;
-
- //handle TDMA frame sync
+// uint32 deltat = get_dt32(time_now, portGetTickCnt());
- //TODO keep this but turn back on, dw_event not declared...
-
-// //account for the delay between transmission and reception of the INF message
-// //TODO make this a function of the data rate as well!
-// //NOTE experimentally found this number to be +- 1 millisecond for some reason. //TODO figure out why
-// //experimentally found formula is 0.079667x + 0.85611
-// uint32 txrx_delay = (uint32)(0.079667*(float)(dw_event->rxLength) + 0.85611); //TODO make this function of message length!
-//
-//
-// tdma_handler->uwbFrameStartTimes[srcIndex] = time_now - (timeSinceFrameStart + txrx_delay);
-//
-//
-// uint32 myFramelengthDuration = tdma_handler->framelength*tdma_handler->slotDuration;
-// uint32 myTimeSinceFrameStart = get_dt32(tdma_handler->frameStartTime, time_now);
-//
-// //NEW
-// if(tdma_handler->framelength <= tdma_handler->uwbFramelengths[srcIndex])
+// if(deltat > 1)
// {
-// uint32 timeSinceFrameStartMod = (timeSinceFrameStart + txrx_delay)%myFramelengthDuration;
-//
-// if(myTimeSinceFrameStart > timeSinceFrameStartMod)
-// {
-// uint32 diff = myTimeSinceFrameStart - timeSinceFrameStartMod;
-// tdma_handler->frameStartTime += diff/2;
-// tdma_handler->lastSlotStartTime += diff/2;
-// }
-// else
-// {
-// uint32 diff = timeSinceFrameStartMod - myTimeSinceFrameStart;
-// tdma_handler->frameStartTime -= diff/2;
-// tdma_handler->lastSlotStartTime -= diff/2;
-//
-// }
-// }
-// else
-// {
-// uint32 hisFramelengthDuration = tdma_handler->uwbFramelengths[srcIndex]*tdma_handler->slotDuration;
-// uint32 myTimeSinceFrameStartMod = (myTimeSinceFrameStart - txrx_delay)%hisFramelengthDuration;
-//
-// if(timeSinceFrameStart > myTimeSinceFrameStartMod)
-// {
-// uint32 diff = timeSinceFrameStart - myTimeSinceFrameStartMod;
-// tdma_handler->frameStartTime -= diff/2;
-// tdma_handler->lastSlotStartTime -= diff/2;
-// }
-// else
-// {
-// uint32 diff = myTimeSinceFrameStartMod - timeSinceFrameStart;
-// tdma_handler->frameStartTime += diff/2;
-// tdma_handler->lastSlotStartTime += diff/2;
-// }
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "process_inf_time:%lu,%04X,xxxx", deltat, inst->uwbShortAdd);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
// }
-
-
-
+ return tdma_modified;
}
@@ -2620,8 +2531,6 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
// send_usbmessage(&debug_msg[0], n);
// usb_run();
-
-
switch (discovery_mode)//TODO make sure all modes are captured here...
{
case WAIT_INF_REG:
@@ -2669,7 +2578,7 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
//keep track of which number each UWB belongs to
//then keep track of which was selected so we can iterate over it later in this function
uint8 num_sub_networks = 0;
- uint8 sub_network_membership[UWB_LIST_SIZE] = {0};
+ uint8 sub_network_membership[UWB_LIST_SIZE] = {};
uint8 sub_network_selected = 0;
uint8 sub_network_members[UWB_LIST_SIZE-1] = {0}; //cannot be more subnetworks than other UWBs
uint64 sub_network_tsfs[UWB_LIST_SIZE-1] = {0};
@@ -2686,7 +2595,7 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
// if(inst->uwbListType[i] != UWB_LIST_NEIGHBOR)
if(this->uwbListTDMAInfo[i].connectionType != UWB_LIST_NEIGHBOR)
{
- continue; //TODO consider a way to accoutn for hidden and twice hidden to determine which netowrk is larger
+ continue; //TODO consider a way to account for hidden and twice hidden to determine which network is larger
}
struct TDMAInfo *info_i = &this->uwbListTDMAInfo[i];
@@ -2694,6 +2603,12 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
// uint64 timeSinceFrameStart_us = get_dt64(this->uwbFrameStartTimes64[i], time_now_us);
uint64 timeSinceFrameStart_us = get_dt64(this->uwbListTDMAInfo[i].frameStartTime, time_now_us);
+ if(timeSinceFrameStart_us > 100000000)
+ {
+ sub_network_membership[i] = 255; //disregard
+ continue;
+ }
+
//if tsfs > framelengthDuration_us, reduce it by framelengthDuration_us
//test the ith UWB against the jth subnetwork
@@ -2701,7 +2616,6 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
{
uint64 diff_us = 0;
-
uint8 min_fl = info_i->framelength;
if(sub_network_base_framelength[j] < min_fl)
{
@@ -2711,7 +2625,7 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
uint64 min_framelengthDuration = min_fl*this->slotDuration_us;
uint64 diff_tsfs = 0;
- if(timeSinceFrameStart_us <= sub_network_tsfs[j])//TODO left off here this appears to be the problem area, the subnetwork timings arent always lining up like they should
+ if(timeSinceFrameStart_us <= sub_network_tsfs[j])
{
diff_tsfs = sub_network_tsfs[j] - timeSinceFrameStart_us;
}
@@ -2732,37 +2646,6 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
}
-
-
-
-// if(info_i->framelength <= sub_network_base_framelength[j])//TODO left off here this appears to be the problem area, the subnetwork timings arent always lining up like they should
-// {
-// uint64 sub_network_tsfs_us_mod = sub_network_tsfs[j]%framelengthDuration_us; //<--this doesn't really make sense...
-//
-// if(timeSinceFrameStart_us > sub_network_tsfs_us_mod)
-// {
-// diff_us = timeSinceFrameStart_us - sub_network_tsfs_us_mod;
-// }
-// else
-// {
-// diff_us = sub_network_tsfs_us_mod - timeSinceFrameStart_us;
-// }
-// }
-// else
-// {
-// uint64 sub_network_FramelengthDuration_us = sub_network_base_framelength[j]*this->slotDuration_us;
-// uint64 timeSinceFrameStartMod_us = timeSinceFrameStart_us%sub_network_FramelengthDuration_us;
-//
-// if(sub_network_tsfs[j] > timeSinceFrameStartMod_us)
-// {
-// diff_us = sub_network_tsfs[j] - timeSinceFrameStartMod_us;
-// }
-// else
-// {
-// diff_us = timeSinceFrameStartMod_us - sub_network_tsfs[j];
-// }
-// }
-
//if difference is below the threshold, it belongs to this subnetwork
//if not, it may belong to another one already listed,
//if not, create a new one...
@@ -2777,7 +2660,6 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
//reached the last listed sub_netowrk, list a new subnetwork.
sub_network_members[num_sub_networks] = 1;
sub_network_base_framelength[num_sub_networks] = this->uwbListTDMAInfo[i].framelength;
-// sub_network_tsfs[num_sub_networks] = get_dt64(this->uwbFrameStartTimes64[i], time_now_us);
sub_network_tsfs[num_sub_networks] = get_dt64(this->uwbListTDMAInfo[i].frameStartTime, time_now_us);
sub_network_membership[num_sub_networks] = num_sub_networks;
num_sub_networks++;
@@ -2905,9 +2787,9 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
// this->uwbFrameStartTimes64[0] -= myFrameDuration;
this->uwbListTDMAInfo[0].frameStartTime -= myFrameDuration;
}
-// uint8 slot = this->uwbFrameStartTimes64[0]/this->slotDuration_us; //integer division rounded down
- uint8 slot = this->uwbListTDMAInfo[0].frameStartTime/this->slotDuration_us; //integer division rounded down
-// this->lastSlotStartTime64 = this->uwbFrameStartTimes64[0] + (uint64)(this->slotDuration_us*slot);
+
+ uint64 myTimeSinceFrameStart = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now);
+ uint8 slot = myTimeSinceFrameStart/this->slotDuration_us; //integer division rounded down
this->lastSlotStartTime64 = this->uwbListTDMAInfo[0].frameStartTime + (uint64)(this->slotDuration_us*slot);
// this->lastSlotStartTime64 = this->uwbFrameStartTimes64[0];
@@ -2982,6 +2864,226 @@ static void check_discovery_mode_expiration(struct TDMAHandler *this)
}
}
+
+static bool check_timeouts(struct TDMAHandler *this, uint32 time_now)
+{
+ instance_data_t *inst = instance_get_local_structure_ptr(0);
+ uint32 delta_t = 0;
+ bool rangingUWBTimeout = FALSE;
+ bool setInactive = FALSE;
+ bool updateINF = FALSE;
+ bool noNeighbors = FALSE;
+
+ for(int i=1; i < inst->uwbListLen; i++)//0 reserved for self, timeout not applicable
+ {
+ struct TDMAInfo *info = &this->uwbListTDMAInfo[i];
+
+ switch (info->connectionType)
+ {
+ case UWB_LIST_NEIGHBOR:
+ {
+ delta_t = get_dt32(info->lastCommNeighbor, time_now);
+
+ if(delta_t > UWB_COMM_TIMEOUT)
+ {
+ if(info->lastCommHidden != 0)
+ {
+ info->connectionType = UWB_LIST_HIDDEN;
+ updateINF = TRUE;
+ }
+ else if(info->lastCommTwiceHidden != 0)
+ {
+ info->connectionType = UWB_LIST_TWICE_HIDDEN;
+ updateINF = TRUE;
+ }
+ else
+ {
+ info->connectionType = UWB_LIST_INACTIVE;
+ updateINF = TRUE;
+ setInactive = TRUE;
+ this->free_slots(info);
+ }
+
+ if(instfindnumneighbors(inst) <= 0)
+ {
+ noNeighbors = TRUE;
+ }
+
+ if(inst->uwbToRangeWith == i)
+ {
+ rangingUWBTimeout = TRUE;
+ }
+ }
+
+ break;
+ }
+ case UWB_LIST_HIDDEN:
+ {
+ delta_t = get_dt32(info->lastCommHidden, time_now);
+
+ if(delta_t > UWB_COMM_TIMEOUT)
+ {
+ if(info->lastCommTwiceHidden != 0)
+ {
+ info->connectionType = UWB_LIST_TWICE_HIDDEN;
+ updateINF = TRUE;
+ }
+ else
+ {
+ info->connectionType = UWB_LIST_INACTIVE;
+ this->free_slots(info);
+ setInactive = TRUE;
+ updateINF = TRUE;
+ }
+ }
+
+ break;
+ }
+ case UWB_LIST_TWICE_HIDDEN:
+ {
+ delta_t = get_dt32(info->lastCommTwiceHidden, time_now);
+
+ if(delta_t > UWB_COMM_TIMEOUT)
+ {
+ info->connectionType = UWB_LIST_INACTIVE;
+ this->free_slots(info);
+ setInactive = TRUE;
+ updateINF = TRUE;
+ }
+
+ break;
+ }
+ case UWB_LIST_INACTIVE:
+ {
+ //TODO figure out the why this doesn't work.
+ //specifically when instremoveuwb(inst, i); and i--; are used
+
+
+// //find the most recent comm...
+// uint32 newest = 0;
+// if(info->lastCommNeighbor != 0)
+// {
+// newest = info->lastCommNeighbor;
+// }
+//
+// if(info->lastCommHidden != 0)
+// {
+// if(info->lastCommHidden > newest)//TODO number wrapping
+// {
+// newest = info->lastCommHidden;
+// }
+// }
+//
+// if(info->lastCommTwiceHidden != 0)
+// {
+// if(info->lastCommTwiceHidden > newest)//TODO number wrapping
+// {
+// newest = info->lastCommTwiceHidden;
+// }
+// }
+//
+// if(newest != 0)
+// {
+// delta_t = get_dt32(newest, time_now);
+//
+// if(delta_t > UWB_COMM_TIMEOUT*2)
+// {
+// uint64 uwb_addr = 0;
+// uwb_addr = instance_get_uwbaddr(i);
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "REMOVE UWB: %d, %llX", i, uwb_addr);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+// instremoveuwb(inst, i); //TODO finish testing to make sure it works like it should.
+// i--; //TODO is this a probelm if i=1 then i=0?
+//
+// }
+// }
+
+
+ break;
+ }
+ default:
+ {
+ //invalid list type
+ break;
+ }
+ }
+ }
+
+ //one of the UWBs we were tracking became inactive,
+ //reoptimize our TDMA assignments and repopulate the inf message
+ if(setInactive == TRUE)
+ {
+ this->free_slots(&this->uwbListTDMAInfo[0]);
+ this->find_assign_slot(this);
+ }
+
+ //adjust the INF message to reflect any changes
+ if(updateINF == TRUE)
+ {
+ this->populate_inf_msg(this, RTLS_DEMO_MSG_INF_UPDATE);
+ }
+
+
+ if(rangingUWBTimeout == TRUE)
+ {
+ inst->uwbToRangeWith = 255;
+ if(inst->mode == ANCHOR || inst->mode == DISCOVERY)
+ {
+ inst->testAppState = TA_RXE_WAIT;
+ }
+ else
+ {
+ inst->testAppState = TA_TX_SELECT;
+ }
+ }
+
+ return noNeighbors;
+}
+
+
+static void remove_uwbinfo(struct TDMAHandler *this, uint8 uwb_index)
+{
+ instance_data_t *inst = instance_get_local_structure_ptr(0);
+
+ if(uwb_index > 0 && uwb_index < inst->uwbListLen)
+ {
+ this->free_slots(&this->uwbListTDMAInfo[uwb_index]);
+
+ for(int i=uwb_index; i<inst->uwbListLen-1; i++)
+ {
+ struct TDMAInfo *info_a = &this->uwbListTDMAInfo[i];
+ struct TDMAInfo *info_b = &this->uwbListTDMAInfo[i+1];
+
+ info_a->connectionType = info_b->connectionType;
+ info_a->lastCommNeighbor = info_b->lastCommNeighbor;
+ info_a->lastCommHidden = info_b->lastCommHidden;
+ info_a->lastCommTwiceHidden = info_b->lastCommTwiceHidden;
+ info_a->lastRange = info_b->lastRange;
+ info_a->frameStartTime = info_b->frameStartTime;
+ info_a->framelength = info_b->framelength;
+ info_a->slotsLength = info_b->slotsLength;
+ info_a->slots = info_b->slots;
+ }
+
+
+ struct TDMAInfo *info_end = &this->uwbListTDMAInfo[inst->uwbListLen-1]; //maybe this!
+ info_end->connectionType = UWB_LIST_INACTIVE;
+ info_end->lastCommNeighbor = 0;
+ info_end->lastCommHidden = 0;
+ info_end->lastCommTwiceHidden = 0;
+ info_end->lastRange = 0;
+ info_end->frameStartTime = 0;
+ info_end->framelength = MIN_FRAMELENGTH;
+ this->free_slots(info_end);
+ }
+}
+
+
+
+
+
static void usb_dump_tdma(struct TDMAHandler *this)
{
instance_data_t *inst = instance_get_local_structure_ptr(0);
@@ -3099,6 +3201,8 @@ static struct TDMAHandler new(){
ret.enter_discovery_mode = &enter_discovery_mode;
ret.set_discovery_mode = &set_discovery_mode;
ret.check_discovery_mode_expiration = &check_discovery_mode_expiration;
+ ret.check_timeouts = &check_timeouts;
+ ret.remove_uwbinfo = &remove_uwbinfo;
ret.usb_dump_tdma = &usb_dump_tdma;
@@ -3121,20 +3225,25 @@ static struct TDMAHandler new(){
for(int i = 0; i < UWB_LIST_SIZE; i++)
{
- ret.uwbListTDMAInfo[i].uwbIndex = i;
+// ret.uwbListTDMAInfo[i].uwbIndex = i;
ret.uwbListTDMAInfo[i].framelength = (uint8)MIN_FRAMELENGTH;
ret.uwbListTDMAInfo[i].slots = NULL;
ret.uwbListTDMAInfo[i].slotsLength = 0;
ret.uwbListTDMAInfo[i].frameStartTime = time_now_us;
ret.uwbListTDMAInfo[i].connectionType = UWB_LIST_INACTIVE;
+ ret.uwbListTDMAInfo[i].lastCommNeighbor = 0;
+ ret.uwbListTDMAInfo[i].lastCommHidden = 0;
+ ret.uwbListTDMAInfo[i].lastCommTwiceHidden = 0;
+ ret.uwbListTDMAInfo[i].lastRange = 0;
}
ret.uwbListTDMAInfo[0].connectionType = UWB_LIST_SELF;
-// ret.uwbFrameStartTimes64[0] = time_now_us;
ret.lastFST = time_now_us;
ret.lastSlotStartTime64 = time_now_us;
ret.infSentThisSlot = FALSE;
- ret.pollSentThisSlot = FALSE;
+ ret.firstPollSentThisSlot = FALSE;
+ ret.firstPollComplete = FALSE;
+ ret.secondPollSentThisSlot = FALSE;
ret.slotStartDelay_us = 4000;
ret.frameSyncThreshold_us = 2*ret.slotStartDelay_us;
ret.infMessageLength = 0;
@@ -3147,9 +3256,6 @@ static struct TDMAHandler new(){
ret.collectInfDuration = 10000; //TODO change this back
ret.waitInfDuration = ret.collectInfDuration;
- ret.lastINFtx = time_now_us;
- ret.lastINFrx = time_now_us;
-
return ret;
}
diff --git a/src/application/tdma_handler.h b/src/application/tdma_handler.h
index ec9eac97b08ae7076cfd4e6f9e399651411762cf..9e7001cbefc851c96fbefd474498e82fdc893774 100644
--- a/src/application/tdma_handler.h
+++ b/src/application/tdma_handler.h
@@ -7,13 +7,13 @@
struct TDMAInfo
{
- //lastCommNeighbor
- //lastCommHidden
- //lastComm2Hidden
- uint64 frameStartTime;
- uint32 lastRange;
+// uint8 uwbIndex; //TODO remove if unused
uint8 connectionType; //UWB_LIST_SELF, UWB_LIST_NEIGHBOR, UWB_LIST_HIDDEN, UWB_LIST_INACTIVE
- uint8 uwbIndex; //TODO remove if unused
+ uint32 lastCommNeighbor; //milliseconds
+ uint32 lastCommHidden; //milliseconds
+ uint32 lastCommTwiceHidden; //milliseconds
+ uint32 lastRange; //milliseconds
+ uint64 frameStartTime; //microseconds
uint8 framelength;
uint8 slotsLength;
uint8 *slots;
@@ -47,17 +47,14 @@ struct TDMAHandler
//TODO use smaller data types where possible
- uint64 lastINFtx;
- uint64 lastINFrx;
-
-
-// uint64 uwbFrameStartTimes64[UWB_LIST_SIZE];
uint64 lastFST;
uint64 lastSlotStartTime64;
uint32 slotDuration_ms; //TODO make variable in duration based on UWB_LIST_SIZE
uint32 slotDuration_us; //TODO make variable in duration based on UWB_LIST_SIZE
bool infSentThisSlot;
- bool pollSentThisSlot;
+ bool firstPollSentThisSlot;
+ bool firstPollComplete;
+ bool secondPollSentThisSlot;
bool rebase_pending;
bool rebase_tx;
uint64 rebase_frameStartTime64; //TODO rename 64 to us!
@@ -81,7 +78,6 @@ struct TDMAHandler
//class functions
bool (*slot_transition)(struct TDMAHandler *this);
-// void (*frame_sync)(struct TDMAHandler *this, event_data_t *dw_event, uint8 *messageData, uint8 srcIndex, FRAME_SYNC_MODE mode);
void (*frame_sync)(struct TDMAHandler *this, event_data_t *dw_event, uint8 framelength, uint64 timeSinceFrameStart_us, uint8 srcIndex, FRAME_SYNC_MODE mode);
bool (*tx_sync_msg)(struct TDMAHandler *this);
void (*update_inf_tsfs)(struct TDMAHandler *this);
@@ -94,11 +90,9 @@ struct TDMAHandler
void (*enter_discovery_mode)(struct TDMAHandler *this);
void (*set_discovery_mode)(struct TDMAHandler *this, DISCOVERY_MODE mode, uint32 time_now);
void (*check_discovery_mode_expiration)(struct TDMAHandler *this);
+ bool (*check_timeouts)(struct TDMAHandler *this, uint32 time_now);
+ void (*remove_uwbinfo)(struct TDMAHandler *this, uint8 uwb_index);
void (*usb_dump_tdma)(struct TDMAHandler *this);
-
-
- //TODO left off here. updating messageData as well as
- //TODO revisit anything that works with messageData!!!
bool (*slot_assigned)(struct TDMAInfo *info, uint8 slot);
bool (*assign_slot)(struct TDMAInfo *info, uint8 slot, bool safeAssign);
void (*free_slot)(struct TDMAInfo *info, uint8 slot);