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