diff --git a/src/application/dw_main.c b/src/application/dw_main.c
index 61d84323f225b1e99e05e64a4fb5739c89127025..b4ffc23c3a6509a3a9e2bf27fde7d67542d4d163 100644
--- a/src/application/dw_main.c
+++ b/src/application/dw_main.c
@@ -30,12 +30,6 @@ extern void send_usbmessage(uint8*, int);
#define SOFTWARE_VER_STRING "TDMA Version 1.0" //
-//#define SWS1_TXSPECT_MODE 0x80 //Continuous TX spectrum mode TODO
-//#define SWS1_ANC_MODE 0x08 //anchor mode
-//#define SWS1_SHF_MODE 0x10 //short frame mode (6.81M) (switch S1-5)
-//#define SWS1_64M_MODE 0x20 //64M PRF mode (switch S1-6)
-//#define SWS1_CH5_MODE 0x40 //channel 5 mode (switch S1-7)
-
int instance_mode = DISCOVERY;
uint8 s1switch = 0;
@@ -86,16 +80,6 @@ uint32 inittestapplication(uint8 mode_switch)
return(-1) ;
}
-// if(mode_switch & SWS1_ANC_MODE) TODO
-// {
-// led_on(LED_PC6);
-//
-// }
-// else
-// {
-// led_on(LED_PC7);
-// }
-
instance_init_s();
int dr_mode = decarangingmode(mode_switch);
instance_data_t* inst = instance_get_local_structure_ptr(0);
@@ -107,7 +91,6 @@ uint32 inittestapplication(uint8 mode_switch)
instance_init_timings();
-// result = tdma_init_s(inst->durationSlotMax_us); //call after instance_init_timings() to get slot duration
inst->mode = DISCOVERY;
return devID;
@@ -444,141 +427,6 @@ int dw_main(void)
port_EnableEXT_IRQ();
-
-
-// uint8 debug_msg[100];//TODO remove
-// int n = sprintf((char *)&debug_msg, "INITIAL CONFIG");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// Sleep(5000);
-//
-// //dump the dwt_registers
-// dwt_dumpregisterstousb();
-//
-//
-//
-//// port_DisableEXT_IRQ();
-////// port_set_dw1000_slowrate();
-//// inittestapplication(s1switch);
-////// dwt_softreset();
-//// Sleep(1000);
-//// //dump the dwt_registers
-//// dwt_dumpregisterstousb();
-////// port_set_dw1000_fastrate();
-//// port_EnableEXT_IRQ();
-//
-//
-// port_DisableEXT_IRQ(); //disable IRQ until we configure the device
-//
-// uint32 devID ;
-// int result;
-//
-// port_set_dw1000_slowrate(); //max SPI before PLLs configured is ~4M
-//
-// dwt_softreset();
-//
-// //reset the DW1000 by driving the RSTn line low
-// reset_DW1000();
-//
-//
-// //result = instance_init() ;
-// // Reset the IC (might be needed if not getting here from POWER ON)
-//// dwt_softreset();
-//
-// //we can enable any configuration loading from OTP/ROM on initialization
-// dwt_initialise(DWT_LOADUCODE) ;
-//
-// //this is platform dependent - only program if DW EVK/EVB
-// dwt_setleds(3) ; //configure the GPIOs which control the leds on EVBs
-//
-// //enable TX, RX states on GPIOs 6 and 5
-// dwt_setlnapamode(1,1);
-// //result = instance_init() ;
-//
-// port_set_dw1000_fastrate();
-//
-//// instance_init_s();
-// // if using auto CRC check (DWT_INT_RFCG and DWT_INT_RFCE) are used instead of DWT_INT_RDFR flag
-// // other errors which need to be checked (as they disable receiver) are
-// dwt_setinterrupt(SYS_MASK_VAL, 1);
-//
-// //this is platform dependent - only program if DW EVK/EVB
-//// dwt_setleds(3) ; //configure the GPIOs which control the LEDs on EVBs
-//
-// dwt_setcallbacks(instance_txcallback, instance_rxgoodcallback, instance_rxtimeoutcallback, instance_rxerrorcallback, instance_irqstuckcallback);
-//// instance_init_s();
-//
-// dr_mode = decarangingmode(s1switch);
-//
-//// chan = inst->chConfig[dr_mode].channelNumber ;
-//// prf = (inst->chConfig[dr_mode].pulseRepFreq == DWT_PRF_16M)? 16 : 64 ;
-//
-// instance_config(&inst->chConfig[dr_mode]) ; // Set operating channel etc
-// port_EnableEXT_IRQ(); //enable IRQ before starting
-//
-// n = sprintf((char *)&debug_msg, "INITIAL CONFIG");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// Sleep(5000);
-//
-// //dump the dwt_registers
-// dwt_dumpregisterstousb();
-
- //FOLLOWING DIFFERENT FOR SOFTRESET ONLY! //FOLLOWING DIFFERENT FOR PARTIAL INIT
- //0x04 SYS_CFG_ID //
- //0x06 SYS_TIME_ID (only difference for init) //XXX
- //0x08 TX_FCTRL_ID //
- //0x0E SYS_MASK_ID //
- //0x18 TX_ANTD_ID //
- //0x1D TX_POWER_ID //
- //0x1F CHAN_CTRL_ID //
- //0x21 USR_SFD_ID //
- //0x23 AGC_CTRL_ID //
- //0x26 GPIO_CTRL_ID //
- //0x28 RF_CONT_ID //
- //0x2A-0x0B TX_CAL_ID-TC_PGDELAY_OFFSET //
- //0x2B FS_CTRL_ID //
- //0x2D OTP_IF_ID //instead 0x2C AON_ID (difference in reserved bits, shouldnt matter!)
- //0x2E-0x1804 DIG_DIAG_ID-LDE_RXANTD_OFFSET //
- //0x2E-0x2804 DIG_DIAG_ID-LDE_REPC_OFFSET //
- //0x2F DIG_DIAG_ID //
- //0x36 PMSC_ID //
-
-// n = sprintf((char *)&debug_msg, "HARD SOFTRESET");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-//
-// port_DisableEXT_IRQ();
-// port_set_dw1000_slowrate();
-// reset_DW1000();
-// Sleep(1000);
-// //dump the dwt_registers
-// dwt_dumpregisterstousb();
-// port_set_dw1000_fastrate();
-// port_EnableEXT_IRQ();
-
-
-//
-// port_set_dw1000_slowrate(); //max SPI before PLLs configured is ~4M
-//
-// //this is called here to wake up the device (i.e. if it was in sleep mode before the restart)
-// devID = instancereaddeviceid() ;
-// if(DWT_DEVICE_ID != devID) //if the read of device ID fails, the DW1000 could be asleep
-// {
-// port_wakeup_dw1000();
-//
-// devID = instancereaddeviceid() ;
-// // SPI not working or Unsupported Device ID
-// if(DWT_DEVICE_ID != devID)
-// return(-1) ;
-// //clear the sleep bit - so that after the hard reset below the DW does not go into sleep
-// dwt_softreset();
-// }
-//
-// //reset the DW1000 by driving the RSTn line low
-// reset_DW1000();
-
-
// main loop
while(1)
{
@@ -587,7 +435,6 @@ int dw_main(void)
instance_run(); //run the state machine!!!
instance_mode = inst->mode;
-// if(inst->canPrintInfo == TRUE) TODO
if(inst->canPrintUSB == TRUE)
{
if(instancenewrange())
@@ -611,7 +458,7 @@ int dw_main(void)
}
}
- //self address, ranging anchor address, ranging tag address, range
+ //self address, ranging anchor address, ranging tag address, range TODO
// n = sprintf((char*)&dataseq[0], "%016llX %016llX %016llX %08X %08X", saddr, aaddr, taddr, rng, rng_raw);
// n = sprintf((char*)&dataseq[0], "%016llX,%016llX,%016llX,%08X,%08X", saddr, aaddr, taddr, rng, rng_raw);
// n = sprintf((char*)&dataseq[0], "%016llX %016llX %016llX %08X", saddr, aaddr, taddr, rng);
@@ -619,30 +466,13 @@ int dw_main(void)
// n = sprintf((char*)&dataseq[0], "%08d,xxxx,xxxx", rng);
- // if(instance_mode == TAG) //TODO remove
- // {
- // uint64 aaddr = instancenewrangeancadd();
- // uint64 taddr = instancenewrangetagadd();
- //// int n = sprintf((char*)&dataseq[0], "RANGE_COMPLETE,%llX,%llX", taddr, aaddr);
-// n = sprintf((char*)&dataseq[0], "RANGE_COMPLETE,%04llX,%04llX", taddr, aaddr);
- // }
-
-#ifdef USB_SUPPORT //this is set in the port.h file
-// if(instance_mode == TAG) //TODO remove
-// {
-// send_usbmessage(&dataseq[0], n);
-// usb_run();
-// }
-
send_usbmessage(&dataseq[0], n);
usb_run();
-#endif
}
}
//only write to LCD if we aren't in the middle of ranging messages
//the sleep messages embedded in the LCD calls mess up the timing otherwise
-// if(enableLCD == TRUE && inst->canPrintInfo == TRUE)
if(enableLCD == TRUE && inst->canPrintLCD == TRUE)
{
@@ -690,12 +520,6 @@ int dw_main(void)
if(updateLCD == TRUE)
{
-
-// uint8 debug_msg[100]; //TODO
-// int n = sprintf((char*)&debug_msg[0], "updateLCD");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
dataseq[0] = 0x2 ; //return cursor home
writetoLCD( 1, 0, dataseq);
@@ -708,7 +532,7 @@ int dw_main(void)
sprintf((char*)&dataseq[0], "%s ", status);
sprintf((char*)&dataseq1[0], "N%02u FL%03u %05.2fm", num_neighbors, framelength, range_result);
}
- else //TODO i think number of hidden nodes might be more useful than FL...
+ else
{
sprintf((char*)&dataseq[0], "%llX %s", addr, status);
sprintf((char*)&dataseq1[0], "N%02u FL%03d %05.2fm", num_neighbors, framelength, range_result);
@@ -730,7 +554,7 @@ int dw_main(void)
}
- writetoLCD(40, 1, dataseq); //send some data TODO
+ writetoLCD(40, 1, dataseq); //send some data
writetoLCD(16, 1, dataseq1); //send some data
}
}
diff --git a/src/application/instance.c b/src/application/instance.c
index 960e562ab0400e7d8c73d39e09be1c08bd0b82b1..4b24a4091376882f28762a6788805f45c821d5b9 100644
--- a/src/application/instance.c
+++ b/src/application/instance.c
@@ -191,8 +191,6 @@ void instancerxon(instance_data_t *inst, int delayed, uint64 delayedReceiveTime)
int instancesendpacket(uint16 length, uint8 txmode, uint32 dtime)
{
-
-
int result = 0;
dwt_writetxfctrl(length, 0, 1);
@@ -207,15 +205,7 @@ int instancesendpacket(uint16 length, uint8 txmode, uint32 dtime)
result = 1; //late/error
}
-// if(result != 1){
-// uint8 debug_msg[100]; //TODO remove
-// int n = sprintf((char *)&debug_msg, "bytes,xxxx,%04d", length);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// }
-
return result; // state changes
-
}
//debug helper function to print the testAppState
@@ -319,37 +309,6 @@ char* get_msg_fcode_string(int fcode)
}
}
-//TODO remove!
-#define USB_DEBUG_BUFF_LEN (100)
-uint8 usbdebugdata[USB_DEBUG_BUFF_LEN]; //data to be sent over usb for debug purposes
-int usbdebugdata_size = 0;
-uint8 usbdebugdataprev[USB_DEBUG_BUFF_LEN]; //previous message sent
-int usbdebugdataprev_size = 0;
-uint8 usbrxdebugdata[USB_DEBUG_BUFF_LEN];
-int usbrxdebugdata_size = 0;
-uint8 usbrxdebugdataprev[USB_DEBUG_BUFF_LEN];
-int usbrxdebugdataprev_size = 0;
-uint8 usbtxdebugdata[USB_DEBUG_BUFF_LEN];
-int usbtxdebugdata_size = 0;
-uint8 usbtxdebugdataprev[USB_DEBUG_BUFF_LEN];
-int usbtxdebugdataprev_size = 0;
-
-
-//TODO remove this funciton
-void send_statetousb(instance_data_t *inst, struct TDMAHandler *tdma_handler)
-{
-
- usbdebugdata_size = sprintf((char*)&usbdebugdata[0], "%s , %s , %s , %s", get_inst_states_string(inst->testAppState), get_inst_states_string(inst->previousState), get_inst_states_string(inst->nextState), get_instanceModes_string(inst->mode));
-
- if (memcmp(usbdebugdataprev, usbdebugdata, usbdebugdata_size) != 0 || usbdebugdata_size != usbdebugdataprev_size)
- {
- send_usbmessage(&usbdebugdata[0], usbdebugdata_size);
- usb_run();
- usbdebugdataprev_size = usbdebugdata_size;
- memcpy(usbdebugdataprev, usbdebugdata, usbdebugdata_size);
- }
-}
-
@@ -443,7 +402,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
dwt_setrxtimeout(0);
inst->wait4ack = 0;
-// inst->canPrintInfo = TRUE; TODO
inst->canPrintUSB = TRUE;
inst->canPrintLCD = TRUE;
}
@@ -547,11 +505,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
dwt_writetxdata(psduLength, (uint8 *) &inst->rng_initmsg, 0) ; // write the frame data
if(instancesendpacket(psduLength, DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED, inst->delayedReplyTime))
{
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "RNG_INIT late tx!");
- send_usbmessage(&debug_msg[0], n);
- usb_run(); //TODO
-
inst->previousState = TA_INIT;
inst->nextState = TA_INIT;
inst->testAppState = TA_RXE_WAIT ; // wait to receive a new blink or poll message
@@ -586,19 +539,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
dwt_setrxtimeout(0); //units are 1.0256us //no timeout (keep RX on until instructed otherwise)
inst->wait4ack = 0;
- //TODO remove
-// uint8 sys_time_arr[5] = {0, 0, 0, 0, 0};
-// dwt_readsystime(sys_time_arr);
-// uint64 dwt_time_now = 0;
-// 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);
-
-// inst->delayedReplyTime = (dwt_time_now + inst->rnginitReplyDelay + convertmicrosectodevicetimeu(rand()%RANGE_INIT_RAND_US)) >> 8 ; // time we should send the blink response
-
-
-// uint64 delayedReplyTime = (dwt_time_now + convertmicrosectodevicetimeu(MIN_DELAYED_TX_DLY_US + inst->storedPreLen_us + 1000)) >> 8;
-
dwt_writetxdata(psduLength, (uint8 *) &inst->inf_msg, 0) ; // write the frame data
-// if(instancesendpacket(psduLength, DWT_START_TX_DELAYED | inst->wait4ack, delayedReplyTime))
if(instancesendpacket(psduLength, DWT_START_TX_IMMEDIATE | inst->wait4ack, 0))
{
//get the message FCODE
@@ -610,11 +551,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
tdma_handler->set_discovery_mode(tdma_handler, WAIT_SEND_SUG, portGetTickCnt());
}
- uint8 debug_msg[100]; //TODO
- int n = sprintf((char*)&debug_msg[0], "TX_INF_LATE");
- send_usbmessage(&debug_msg[0], n);
- usb_run();
-
inst->previousState = TA_INIT;
inst->nextState = TA_INIT;
inst->testAppState = TA_RXE_WAIT;
@@ -627,23 +563,11 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->timeofTx = portGetTickCnt();
-// uint8 debug_msg[100]; //TODO
-// int n = sprintf((char*)&debug_msg[0], "tx_INF");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
-// inst->canPrintUSB = TRUE; TODO
-// inst->canPrintLCD = TRUE;
-
uint64 margin_us = 1000;
uint64 framelength_us = instance_getmessageduration_us(psduLength);
inst->txDoneTimeoutDuration = CEIL_DIV(TX_CMD_TO_TX_CB_DLY_US + framelength_us + margin_us, 1000); //tx cmd to tx cb
}
-// if(tdma_handler->toHidden == TRUE){
-// tdma_handler->toHidden = FALSE;
-// }
-
break;
}
case TA_TXPOLL_WAIT_SEND :
@@ -686,7 +610,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->testAppState = TA_TX_WAIT_CONF ;
inst->previousState = TA_TXPOLL_WAIT_SEND ;
done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
-// inst->canPrintInfo = FALSE; //don't print to LCD or USB while ranging TODO
inst->canPrintUSB = FALSE;
inst->canPrintLCD = FALSE;
@@ -698,11 +621,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
}
case TA_TXREPORT_WAIT_SEND :
{
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "TA_TXREPORT_WAIT_SEND");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run(); //TODO
-
int psduLength = REPORT_FRAME_LEN_BYTES;
// Write calculated TOF into response message
@@ -731,8 +649,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->timeofTx = portGetTickCnt();
inst->txDoneTimeoutDuration = inst->durationReportTxDoneTimeout_ms;
-// inst->canPrintInfo = TRUE; //ranging complete, allow print to USB and LCD TODO
- inst->canPrintUSB = TRUE;
+// inst->canPrintUSB = TRUE;
inst->canPrintLCD = FALSE;
}
@@ -782,11 +699,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
{
if(inst->previousState == TA_TXINF_WAIT_SEND)
{
-// uint8 debug_msg[100]; //TODO
-// int n = sprintf((char *)&debug_msg, "confirm tx_inf");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
inst->canPrintUSB = TRUE;
inst->canPrintLCD = TRUE;
@@ -933,13 +845,10 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
memcpy(&framelength, &messageData[SYNC_FRAMELENGTH], sizeof(uint8));
memcpy(&timeSinceFrameStart_us, &messageData[SYNC_TSFS], 6);
-// tdma_handler->update_frame_start(tdma_handler);
-
if(inst->mode == ANCHOR || inst->mode == TAG)
{
//evaluate our frame synchronization to see if we need to snap to the incoming value
//and rebroadcast a SYNC message
- //TODO reenable!!!
tdma_handler->frame_sync(tdma_handler, dw_event, framelength, timeSinceFrameStart_us, srcIndex, FS_EVAL);
}
@@ -957,34 +866,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
memcpy(&framelength, &messageData[TDMA_FRAMELENGTH], sizeof(uint8));
memcpy(&timeSinceFrameStart_us, &messageData[TDMA_TSFS], 6);
-
-
-// if(tdma_handler->discovery_mode == COLLECT_INF_REG || tdma_handler->discovery_mode == WAIT_INF_REG){
-// uint8 debug_msg[100]; TODO
-// int n = sprintf((char*)&debug_msg[0], "fcode %s", get_msg_fcode_string(fcode));
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// }
-
-
-// uint8 debug_msg[100]; TODO
-// int n = sprintf((char*)&debug_msg[0], "fcode %s", get_msg_fcode_string(fcode));
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
-// if( dw_event->msgu.rxmsg_ss.messageData[FCODE] == RTLS_DEMO_MSG_INF_SUG){
-// uint64 time_now_us = portGetTickCntMicro();
-// uint64 myTimeSinceSlotStart = get_dt64(tdma_handler->lastSlotStartTime64, time_now_us);
-// uint64 myTimeSinceFrameStart = get_dt64(tdma_handler->uwbListTDMAInfo[0].frameStartTime, time_now_us);
-// uint8 slot = myTimeSinceFrameStart/(tdma_handler->slotDuration_us); //integer division rounded down
-//
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "SUG tsfs: %llu, mytsfs: %llu, mytsss: %llu, slot: %u", timeSinceFrameStart_us, myTimeSinceFrameStart, myTimeSinceSlotStart, slot);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// }
-
-
//return to discovery mode if no slots assigned to this UWB
if(inst->mode == ANCHOR || inst->mode == TAG)
{
@@ -1051,7 +932,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
}
- //inst->canPrintInfo = TRUE; //INF is last message in slot, we can print after processing TODO
+ //INF is last message in slot, we can print after processing
inst->canPrintUSB = TRUE;
inst->canPrintLCD = TRUE;
@@ -1121,12 +1002,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
} //RTLS_DEMO_MSG_ANCH_RESP
case RTLS_DEMO_MSG_TAG_FINAL :
{
-// uint8 debug_msg[100]; TODO
-// int n = sprintf((char *)&debug_msg, "RTLS_DEMO_MSG_TAG_FINAL");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
-
int64 Rb, Da, Ra, Db ;
uint64 tagFinalTxTime = 0;
uint64 tagFinalRxTime = 0;
@@ -1167,13 +1042,10 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->tof[inst->uwbToRangeWith] = (int64) ( RaRbxDaDb/(RbyDb + RayDa) );
inst->newRangeUWBIndex = inst->uwbToRangeWith;
-// if(inst->tof[inst->newRangeUWBIndex] > 0) //if ToF == 0 - then no new range to report
-// {
- if(reportTOF(inst, inst->newRangeUWBIndex)==0)
- {
- inst->newRange = 1; //TODO comment out to turn off ANCHOR outputting range to USB
- }
-// }
+ if(reportTOF(inst, inst->newRangeUWBIndex)==0)
+ {
+ inst->newRange = 1;
+ }
tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith].lastRange = portGetTickCnt();
@@ -1191,25 +1063,22 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
uint8 anchor_index = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
//for now only process if we are the TAG that ranged with the reporting ANCHOR
- //TODO make it so all are processed
-// if(tag_index == 0)
-// {
- inst->tof[anchor_index] = 0;
+ inst->tof[anchor_index] = 0;
- //copy previously calculated ToF
- memcpy(&inst->tof[anchor_index], &messageData[REPORT_TOF], 6);
+ //copy previously calculated ToF
+ memcpy(&inst->tof[anchor_index], &messageData[REPORT_TOF], 6);
- inst->newRangeAncAddress = instance_get_uwbaddr(anchor_index);
- inst->newRangeTagAddress = instance_get_uwbaddr(tag_index);
+ inst->newRangeAncAddress = instance_get_uwbaddr(anchor_index);
+ inst->newRangeTagAddress = instance_get_uwbaddr(tag_index);
- inst->newRangeUWBIndex = anchor_index;
- if(inst->tof[inst->newRangeUWBIndex] > 0) //if ToF == 0 - then no new range to report
+ inst->newRangeUWBIndex = anchor_index;
+ if(inst->tof[inst->newRangeUWBIndex] > 0) //if ToF == 0 - then no new range to report
+ {
+ if(reportTOF(inst, inst->newRangeUWBIndex)==0)
{
- if(reportTOF(inst, inst->newRangeUWBIndex)==0)
- {
- inst->newRange = 1;
- }
+ inst->newRange = 1;
}
+ }
if(tag_index == 0)
{
@@ -1235,79 +1104,11 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->uwbToRangeWith = 255;
}
-// inst->canPrintInfo = TRUE; //ranging complete, allow print to USB and LCD TODO
inst->canPrintUSB = TRUE;
inst->canPrintLCD = FALSE;
-// dwt_setrxaftertxdelay(0); //units are 1.0256us
-// done = INST_DONE_WAIT_FOR_NEXT_EVENT; TODO
break;
} //RTLS_DEMO_MSG_RNG_REPORT
-// case RTLS_DEMO_MSG_RNG_REPORT: TODO
-// {
-// uint8 tag_index = instgetuwblistindex(inst, &messageData[REPORT_ADDR], inst->addrByteSize);
-// uint8 anchor_index = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
-//
-// //for now only process if we are the TAG that ranged with the reporting ANCHOR
-// //TODO make it so all are processed
-//// if(tag_index == 0)
-//// {
-// inst->tof[anchor_index] = 0;
-//
-// //copy previously calculated ToF
-// memcpy(&inst->tof[anchor_index], &messageData[REPORT_TOF], 6);
-//
-// inst->newRangeAncAddress = instance_get_uwbaddr(anchor_index);
-// inst->newRangeTagAddress = instance_get_uwbaddr(tag_index);
-//
-// inst->newRangeUWBIndex = anchor_index;
-//// if(inst->tof[inst->newRangeUWBIndex] > 0) //if ToF == 0 - then no new range to report
-//// {
-// if(reportTOF(inst, inst->newRangeUWBIndex)==0)
-// {
-// inst->newRange = 1;
-// }
-//// }
-//
-// tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith].lastRange = portGetTickCnt();
-//
-//// uint8 debug_msg[100];//TODO remove
-////// int n = sprintf((char *)&debug_msg, "POLL_COMPLETE,%llX,%llX", inst->newRangeTagAddress, inst->newRangeAncAddress);
-//// int n = sprintf((char *)&debug_msg, "POLL_COMPLETE,%04llX,%04llX", inst->newRangeTagAddress, inst->newRangeAncAddress);
-//// send_usbmessage(&debug_msg[0], n);
-//// usb_run();
-//
-//
-//
-// inst->previousState = TA_INIT;
-// inst->nextState = TA_INIT; //TODO remove nextState???
-// inst->uwbToRangeWith = 255;
-//
-// if(tag_index == 0)
-// {
-// tdma_handler->firstPollComplete = TRUE;
-// inst->testAppState = TA_TX_SELECT;
-// }
-// else
-// {
-// inst->testAppState = TA_RXE_WAIT ;
-// }
-//// }
-//// else
-//// {
-//// inst->testAppState = TA_RXE_WAIT ;
-//// inst->previousState = TA_INIT;
-//// inst->nextState = TA_INIT;
-//// inst->uwbToRangeWith = 255;
-//// }
-//
-// //TODO turn this on after rx INF!
-//// inst->canPrintInfo = TRUE; //ranging complete, allow print to USB and LCD
-// dwt_setrxaftertxdelay(0); //units are 1.0256us
-//
-//// done = INST_DONE_WAIT_FOR_NEXT_EVENT; TODO
-// break;
-// } //RTLS_DEMO_MSG_RNG_REPORT
default:
{
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
@@ -1332,7 +1133,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
int n;
if(inst->previousState == TA_TXBLINK_WAIT_SEND)
{
-// uint8 debug_msg[100];
+// uint8 debug_msg[100]; TODO
// n = sprintf((char *)&debug_msg, "TX_BLINK_TIMEOUT,%04llX,NULL", instance_get_addr());
// send_usbmessage(&debug_msg[0], n);
// usb_run();
@@ -1345,7 +1146,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
}
else if(inst->previousState == TA_TXFINAL_WAIT_SEND)
{
-// uint8 debug_msg[100];
+// uint8 debug_msg[100]; TODO
//// n = sprintf((char *)&debug_msg, "TX_POLL_TIMEOUT,%llX,%llX", instance_get_addr(), instance_get_uwbaddr(inst->uwbToRangeWith));
// n = sprintf((char *)&debug_msg, "TX_POLL_TIMEOUT,%04llX,%04llX", instance_get_addr(), instance_get_uwbaddr(inst->uwbToRangeWith));
// send_usbmessage(&debug_msg[0], n);
@@ -1353,7 +1154,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
}
else if(inst->previousState == TA_TXPOLL_WAIT_SEND)
{
-// uint8 debug_msg[100];
+// uint8 debug_msg[100]; TODO
//// n = sprintf((char *)&debug_msg, "TX_POLL_TIMEOUT,%llX,%llX", instance_get_addr(), instance_get_uwbaddr(inst->uwbToRangeWith));
// n = sprintf((char *)&debug_msg, "TX_POLL_TIMEOUT,%04llX,%04llX", instance_get_addr(), instance_get_uwbaddr(inst->uwbToRangeWith));
// send_usbmessage(&debug_msg[0], n);
diff --git a/src/application/instance.h b/src/application/instance.h
index 2586892866d8b0c7d409e92c0b233ab6a8f2ee72..41e9fe06596aa7f8a7fe6747be6cfb5d37722dd6 100644
--- a/src/application/instance.h
+++ b/src/application/instance.h
@@ -27,8 +27,6 @@ extern "C" {
typedef struct
{
- //TODO remove this!
- uint64 report_rx_us;
INST_MODE mode; //instance mode (tag or anchor)
diff --git a/src/application/instance_common.c b/src/application/instance_common.c
index d13c97f4d53c90244e376d549d5fc06ad1efec83..068410b5f7abf76ef1cb35f580ac235f0e8a5055 100644
--- a/src/application/instance_common.c
+++ b/src/application/instance_common.c
@@ -374,12 +374,7 @@ void instanceclearcounts(void)
int instance_init(void)
{
int instance = 0 ;
-
int result;
-
-// instance_data[instance].mode = DISCOVERY;
-// instance_data[instance].ranging = 0; TODO
-// instance_data[instance].goToSleep = 0;
for(uint8 i=0; i<UWB_LIST_SIZE; i++)
{
@@ -697,12 +692,6 @@ int instance_get_rxl(void) //get number of late Tx frames
void inst_processtxrxtimeout(instance_data_t *inst)
{
-// uint8 debug_msg[100]; //TODO
-// int n = sprintf((char *)&debug_msg, "inst_processtxrxtimeout");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
-
if(inst->mode == DISCOVERY)
{
inst->wait4ack = 0;
@@ -734,7 +723,6 @@ void inst_processtxrxtimeout(instance_data_t *inst)
inst->testAppState = TA_TX_SELECT ;
}
-// inst->canPrintInfo = TRUE; TODO
inst->canPrintUSB = TRUE;
inst->canPrintLCD = TRUE;
inst->previousState = TA_INIT;
@@ -746,7 +734,7 @@ void inst_processtxrxtimeout(instance_data_t *inst)
void instance_irqstuckcallback()
{
-// uint8 debug_msg[100];
+// uint8 debug_msg[100]; TODO test this when networks meet!
// int n = sprintf((char*)&debug_msg[0], "irq stuck callback");
// send_usbmessage(&debug_msg[0], n);
// usb_run();
@@ -1000,52 +988,6 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
//don't process unknown signals or non-broadcast messages that aren't addressed to this UWB
if(rxd_event == DWT_SIG_RX_OKAY)
{
-
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "RX CB RX: DWT_SIG_RX_OKAY-%s", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]));
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
- uint8 idx = instgetuwblistindex(&instance_data[instance], &dw_event.msgu.frame[srcAddr_index], instance_data[instance].addrByteSize);
-
-// if(instance_data[instance].mode == DISCOVERY){ //TODO
-// if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_INF_REG ||
-// dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_INF_INIT ||
-// dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_INF_SUG ||
-// dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_INF_UPDATE
-// )
-// {
-
-// uint64 dt = get_dt64(instance_data[instance].report_rx_us, portGetTickCntMicro());
-// if(dt > tdma_handler.slotDuration_us*0.75){
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "NOT BOTH, dt: %llu", dt);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// }
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "RX CB RX: DWT_SIG_RX_OKAY-%s, ts: %llu, saddr: %016llX", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]), portGetTickCntMicro(), instance_get_uwbaddr(idx));
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// }
-// }
-
-// if(dw_event.msgu.frame[fcode_index] != RTLS_DEMO_MSG_RNG_REPORT){
-// instance_data[instance].report_rx_us = portGetTickCntMicro();
-//
-//
-// }
-
-// if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_POLL){
-// uint64 add = 0;
-// add = (dw_event.msgu.frame[FRAME_CTRLP + 1] << 8) + dw_event.msgu.frame[FRAME_CTRLP];
-//
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "POLL destination: %04llX", add);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// }
-
if((dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_INIT ) ||
(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_POLL ) ||
(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_ANCH_RESP) ||
@@ -1101,10 +1043,6 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
tdma_handler.uwbListTDMAInfo[uwb_index].connectionType = UWB_LIST_NEIGHBOR;
tdma_handler.uwbListTDMAInfo[uwb_index].lastCommNeighbor = time_now;
}
-
-
-
-
}
bool accept_inf = FALSE;
@@ -1206,11 +1144,6 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
dwt_writetxdata(psduLength, (uint8 *) &instance_data[instance].msg, 0) ; // write the frame data
if(instancesendpacket(psduLength, DWT_START_TX_IMMEDIATE | instance_data[instance].wait4ack, 0))
{
-// uint8 debug_msg[100];//TODO
-// int n = sprintf((char*)&debug_msg[0], "RESP late tx!");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
instance_data[0].tx_anch_resp = FALSE;
instance_data[instance].previousState = TA_INIT;
instance_data[instance].nextState = TA_INIT;
@@ -1224,7 +1157,6 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
instance_data[instance].timeofTx = time_now;
instance_data[instance].txDoneTimeoutDuration = instance_data[instance].durationRespTxDoneTimeout_ms;
-// instance_data[instance].canPrintInfo = FALSE; //don't print to LCD or USB while ranging
instance_data[instance].canPrintUSB = FALSE;
instance_data[instance].canPrintLCD = FALSE;
}
@@ -1382,11 +1314,7 @@ int instance_run(void)
message = 0;
}
-// uint32 time_now = portGetTickCnt();
-
//only check timeouts if we aren't in the middle of ranging messages
- //the TDMA reoptimization may mess up the timing
-// if(instance_data[instance].canPrintInfo == TRUE) TODO
if(instance_data[instance].canPrintLCD == TRUE)
{
//check timeouts.
diff --git a/src/application/tdma_handler.c b/src/application/tdma_handler.c
index 6fb8715fcf041cc5905ed730e64824e3a8da9fa0..a372c3847bdd3f24b03c22ef118326a297125234 100644
--- a/src/application/tdma_handler.c
+++ b/src/application/tdma_handler.c
@@ -27,64 +27,28 @@ static bool slot_transition(struct TDMAHandler *this)
this->firstPollComplete = FALSE;
this->secondPollSentThisSlot = FALSE;
this->infSentThisSlot = FALSE;
-// inst->canPrintInfo = TRUE; TODO
inst->canPrintUSB = TRUE;
inst->canPrintLCD = TRUE;
//we have transitioned into the next slot.
//get the slot number and set the start time appropriately
-
-
-
uint64 timeSinceFrameStart64 = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now_us);
-
uint64 frameDuration64 = this->slotDuration_us*this->uwbListTDMAInfo[0].framelength;
-// if(timeSinceFrameStart64 >= frameDuration64)
-// {
-// //TODO remove
-//// uint8 debug_msg[100];
-////// int n = sprintf((char*)&debug_msg[0], "NEW FRAME, %llX, this->frameStartTime: %lu, this->slotDuration*this->framelength: %lu", instance_get_addr(), this->frameStartTime, (this->slotDuration*this->framelength));
-////// int n = sprintf((char*)&debug_msg[0], "NEW FRAME, xxxx, xxxx");
-//// int n = sprintf((char*)&debug_msg[0], "%llX, %llu, %llu", instance_get_addr(), this->lastFST, time_now_us);
-//// send_usbmessage(&debug_msg[0], n);
-//// usb_run();
-//
-// }
-
if(timeSinceFrameStart64 >= frameDuration64)
{
int div = timeSinceFrameStart64/frameDuration64;
this->uwbListTDMAInfo[0].frameStartTime = timestamp_add64(this->uwbListTDMAInfo[0].frameStartTime, frameDuration64*div);
timeSinceFrameStart64 -= frameDuration64*div;
}
-// uint64 timeSinceFrameStart64 = this->update_frame_start(this);
-
-
- this->lastFST = this->uwbListTDMAInfo[0].frameStartTime; //TODO remove
uint8 slot = timeSinceFrameStart64/(this->slotDuration_us); //integer division rounded down
this->lastSlotStartTime64 = this->uwbListTDMAInfo[0].frameStartTime + (uint64)(this->slotDuration_us*slot);
-
-// uint8 debug_msg[100]; //TODO
-// int n = sprintf((char *)&debug_msg, "new slot: %u, ts: %llu", slot, time_now_us);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
-
-
if(inst->mode != DISCOVERY)
{
if(this->slot_assigned(&this->uwbListTDMAInfo[0], slot) == TRUE)
{
-// uint8 debug_msg[100]; //TODO
-// int n = sprintf((char *)&debug_msg, "MY SLOT STARTS");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
-
-
inst->mode = TAG;
inst->testAppState = TA_TX_SELECT;
//go to TX select, select the oldest uwb, send INF, then send POLL
@@ -122,7 +86,6 @@ static uint64 update_frame_start(struct TDMAHandler *this){
if(timeSinceFrameStart_us > 1000000000) //if very large number, assume frame start time accidentally moved ahead of time now
{
uint64 diff_us = get_dt64(time_now_us, this->uwbListTDMAInfo[0].frameStartTime);
-// int div = CEIL_DIV(diff_us,frameDuration_us); TODO
int div = diff_us/frameDuration_us;
if(diff_us%frameDuration_us != 0)
{
@@ -144,7 +107,6 @@ static uint64 update_frame_start(struct TDMAHandler *this){
static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 framelength, uint64 timeSinceFrameStart_us, uint8 srcIndex, FRAME_SYNC_MODE mode)
{
instance_data_t *inst = instance_get_local_structure_ptr(0);
-// uint64 myTimeSinceFrameStart_us = 0; TODO
//do not process erroneous tsfs
//can happen if frame start time is shifted ahead of time_now_us in transmitting UWB
@@ -180,27 +142,6 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 f
this->uwbListTDMAInfo[srcIndex].frameStartTime = timestamp_subtract64(time_now_us, hisTimeSinceFrameStart_us);
uint64 myFrameDuration = this->slotDuration_us*this->uwbListTDMAInfo[0].framelength;
-// if(inst->mode == DISCOVERY){ TODO
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "uwb_index: %d, fst: %llu", srcIndex, this->uwbListTDMAInfo[srcIndex].frameStartTime);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// }
-
-
-
-// if( dw_event->msgu.rxmsg_ss.messageData[FCODE] == RTLS_DEMO_MSG_INF_SUG){ TODO
-// uint64 myTimeSinceSlotStart = get_dt64(this->lastSlotStartTime64, time_now_us);
-// uint64 myTimeSinceFrameStart = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now_us);
-// uint8 slot = myTimeSinceFrameStart/(this->slotDuration_us); //integer division rounded down
-//
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "SUG tsfs: %llu, tsfsoffset: %llu, mytsfs: %llu, mytsss: %llu, slot: %u", timeSinceFrameStart_us, hisTimeSinceFrameStart_us, myTimeSinceFrameStart, myTimeSinceSlotStart, slot);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// }
-
-
if(mode == FS_COLLECT)
{
return;
@@ -213,16 +154,6 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 f
{
uint64 myTimeSinceFrameStart_us = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now_us);
- //first make sure my frame start time is up to date TODO
-// if(myTimeSinceFrameStart_us >= myFrameDuration)
-// {
-// int div = myTimeSinceFrameStart_us/myFrameDuration;
-// this->uwbListTDMAInfo[0].frameStartTime = timestamp_add64(this->uwbListTDMAInfo[0].frameStartTime, myFrameDuration*div);
-// myTimeSinceFrameStart_us -= myFrameDuration*div;
-// }
-
-// myTimeSinceFrameStart_us = this->update_frame_start(this);
-
//SELF VS INCOMING
uint8 min_fl = this->uwbListTDMAInfo[0].framelength;
@@ -272,24 +203,9 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 f
//check if frame sync out of tolerance (don't xmit sync message in case of 0th slot misalignment)
if(diff_us%(MIN_FRAMELENGTH*this->slotDuration_us) > this->frameSyncThreshold_us){
if(MIN_FRAMELENGTH*this->slotDuration_us - diff_us%(MIN_FRAMELENGTH*this->slotDuration_us) > this->frameSyncThreshold_us){
-// uint64 min_mod = diff_us%(MIN_FRAMELENGTH*this->slotDuration_us);
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "diff_us: %llu, min_mod: %llu", diff_us, min_mod);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
this->tx_sync_msg(this);
}
}
-// else if(diff_us > this->frameSyncThreshold_us) TODO
-// {
-//
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "diff_us: %llu", diff_us);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-//
-//
-// }
else if(mode == FS_EVAL)
{
return;
@@ -324,22 +240,12 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 f
}
else
{
- while(myTimeSinceFrameStart_us >= myFrameDuration)//TODO remove one
+ while(myTimeSinceFrameStart_us >= myFrameDuration)
{
this->uwbListTDMAInfo[0].frameStartTime = timestamp_add64(this->uwbListTDMAInfo[0].frameStartTime, myFrameDuration);
myTimeSinceFrameStart_us -= myFrameDuration;
}
- //first make sure my frame start time is up to date
-// if(myTimeSinceFrameStart_us >= myFrameDuration)
-// {
-// int div = myTimeSinceFrameStart_us/myFrameDuration;
-// this->uwbListTDMAInfo[0].frameStartTime = timestamp_add64(this->uwbListTDMAInfo[0].frameStartTime, myFrameDuration*div);
-// myTimeSinceFrameStart_us -= myFrameDuration*div;
-// }
-
-// myTimeSinceFrameStart_us = this->update_frame_start(this);
}
-// myTimeSinceFrameStart_us = this->update_frame_start(this);
uint8 slot = myTimeSinceFrameStart_us/this->slotDuration_us; //integer division rounded down
this->lastSlotStartTime64 = timestamp_add64(this->uwbListTDMAInfo[0].frameStartTime, (uint64)(this->slotDuration_us*slot));
@@ -350,28 +256,11 @@ static bool tx_sync_msg(struct TDMAHandler *this)
{
int psduLength = SYNC_FRAME_LEN_BYTES;
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "stuck tx sync msg");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
instance_data_t *inst = instance_get_local_structure_ptr(0);
- uint64 time_now_us = portGetTickCntMicro(); //TODO
+ uint64 time_now_us = portGetTickCntMicro();
- uint64 myFrameDuration = this->slotDuration_us*this->uwbListTDMAInfo[0].framelength; //TODO remove
uint64 myTimeSinceFrameStart_us = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now_us);
- //first make sure my frame start time is up to date
-// if(myTimeSinceFrameStart_us >= myFrameDuration)
-// {
-// int div = myTimeSinceFrameStart_us/myFrameDuration;
-// this->uwbListTDMAInfo[0].frameStartTime = timestamp_add64(this->uwbListTDMAInfo[0].frameStartTime, myFrameDuration*div);
-// myTimeSinceFrameStart_us -= myFrameDuration*div;
-// }
-
-// uint64 myTimeSinceFrameStart_us = this->update_frame_start(this);
-
-// uint64 myTimeSinceFrameStart_us = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now_us);
memcpy(&inst->sync_msg.messageData[SYNC_FRAMELENGTH], &this->uwbListTDMAInfo[0].framelength, sizeof(uint8));
memcpy(&inst->sync_msg.messageData[SYNC_TSFS], &myTimeSinceFrameStart_us, 6);
inst->sync_msg.seqNum = inst->frameSN++;
@@ -698,31 +587,12 @@ static void update_inf_tsfs(struct TDMAHandler *this)
instance_data_t *inst = instance_get_local_structure_ptr(0);
uint64 time_now_us = portGetTickCntMicro();
uint64 timeSinceFrameStart64 = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now_us);
- uint64 frameDuration = this->slotDuration_us*this->uwbListTDMAInfo[0].framelength;
- //if frameStartTime ahead of time_now_us, just send the erroneously large number over.
- //the receiver will detect and disregard
- if(timeSinceFrameStart64 < 10000000)
- {
-// timeSinceFrameStart64 = this->update_frame_start(this);
-
-// while(timeSinceFrameStart64 > frameDuration) //TODO remove (maybe turn this back on for testing!)
-// {
-// this->uwbListTDMAInfo[0].frameStartTime = timestamp_add64(this->uwbListTDMAInfo[0].frameStartTime, frameDuration);
-// timeSinceFrameStart64 -= frameDuration;
-// }
- }
- else
+ if(timeSinceFrameStart64 > 10000000)
{
timeSinceFrameStart64 = 0;
}
-// if(this->toHidden == TRUE){
-// this->toHidden = FALSE;
-// }
-
-// uint64 timeSinceFrameStart64 = this->update_frame_start(this);
-
memcpy(&inst->inf_msg.messageData[TDMA_TSFS], &timeSinceFrameStart64, 6);
}
@@ -1662,7 +1532,6 @@ static void enter_discovery_mode(struct TDMAHandler *this)
this->collectInfStartTime = time_now;
instance_data_t *inst = instance_get_local_structure_ptr(0);
-// inst->canPrintInfo = TRUE; TODO
inst->canPrintUSB = TRUE;
inst->canPrintLCD = TRUE;
@@ -1740,9 +1609,6 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
continue;
}
- //TODO bring frame start time up here?
-
-
//test the ith UWB against the jth subnetwork
for(int j=0; j < num_sub_networks; j++)
{
@@ -1860,10 +1726,8 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
tnext[i] += frameduration;
}
-// if(tnext[i] > tcommon && tnext[i] - tcommon >= shortestFrameDuration) //(should this be the frame buffer???) TODO
- if(tnext[i] > tcommon && tnext[i] - tcommon >= this->frameSyncThreshold_us) //(should this be the frame buffer???)
+ if(tnext[i] > tcommon && tnext[i] - tcommon >= this->frameSyncThreshold_us)
{
-// tcommon = tnext[i];
//increment the value based on tnext[0] to guarantee all uwbs
//in this subnetwork will have frame sync errors within the threshold
tcommon += shortestFrameDuration;
@@ -1898,7 +1762,6 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
}
uint64 myTimeSinceFrameStart = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now);
-// uint64 myTimeSinceFrameStart = this->update_frame_start(this);
uint8 slot = myTimeSinceFrameStart/this->slotDuration_us; //integer division rounded down
this->lastSlotStartTime64 = this->uwbListTDMAInfo[0].frameStartTime + (uint64)(this->slotDuration_us*slot);
@@ -1983,11 +1846,6 @@ static bool check_timeouts(struct TDMAHandler *this)
{
if(info->lastCommHidden != 0)
{
- //TODO remove this test variable
- this->toHidden = TRUE;
- this->toHiddenFST = this->uwbListTDMAInfo[0].frameStartTime;
- this->toHiddenFL = this->uwbListTDMAInfo[0].framelength;
-
info->connectionType = UWB_LIST_HIDDEN;
updateINF = TRUE;
}
@@ -2069,41 +1927,8 @@ static bool check_timeouts(struct TDMAHandler *this)
//re-optimize our TDMA assignments and repopulate the inf message
if(setInactive == TRUE)
{
-// uint64 time_now_us = portGetTickCntMicro();
-// uint64 timeSinceFrameStart64 = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now_us);
-// uint64 frameDuration64 = this->slotDuration_us*this->uwbListTDMAInfo[0].framelength;
-// if(timeSinceFrameStart64 >= frameDuration64)
-// {
-// int div = timeSinceFrameStart64/frameDuration64;
-// timeSinceFrameStart64 -= frameDuration64*div;
-// }
-// uint8 slot_now = timeSinceFrameStart64/(this->slotDuration_us);
-//
-//
-// char slots_bef[10];
-// for(int s=0; s < this->uwbListTDMAInfo[0].slotsLength; s++){
-// sprintf((char *)&slots_bef[s*3], ", %d", this->uwbListTDMAInfo[0].slots[s]);
-// }
-//
-// uint8 fl_bef = this->uwbListTDMAInfo[0].framelength;
-
this->free_slots(&this->uwbListTDMAInfo[0]);
this->find_assign_slot(this);
-
-// char slots_aft[10];
-// for(int s=0; s < this->uwbListTDMAInfo[0].slotsLength; s++){
-//
-// sprintf((char *)&slots_aft[s*3], ", %d", this->uwbListTDMAInfo[0].slots[s]);
-// }
-
-// uint8 fl_aft = this->uwbListTDMAInfo[0].framelength;
-
-
-// uint8 debug_msg[100]; //TODO
-// int n = sprintf((char *)&debug_msg, "set inactive, slot_now: (%d), slots_bef: (%s)/%d, slots_aft: (%s)/%d", slot_now, slots_bef, fl_bef, slots_aft, fl_aft);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
}
//adjust the INF message to reflect any changes
@@ -2191,7 +2016,6 @@ static struct TDMAHandler new(uint64 slot_duration){
}
ret.uwbListTDMAInfo[0].connectionType = UWB_LIST_SELF;
- ret.lastFST = time_now_us;
ret.lastSlotStartTime64 = time_now_us;
ret.infSentThisSlot = FALSE;
ret.firstPollSentThisSlot = FALSE;
@@ -2208,11 +2032,6 @@ static struct TDMAHandler new(uint64 slot_duration){
ret.waitInfDuration = ret.collectInfDuration;
ret.blinkPeriodRand = (uint32)rand()%BLINK_PERIOD_RAND_MS;
- //TODO remove these test variables
- ret.toHidden = FALSE;
- ret.toHiddenFST = 0;
- ret.toHiddenFL = 0;
-
return ret;
}
diff --git a/src/application/tdma_handler.h b/src/application/tdma_handler.h
index 1854d90b0a817aad7d55e1133cf08e263d887bd2..51b2333fef06d2f607f85a04564b042ed5f6dcdc 100644
--- a/src/application/tdma_handler.h
+++ b/src/application/tdma_handler.h
@@ -37,18 +37,12 @@ FRAME_SYNC_MODE;
struct TDMAHandler
{
- //TODO remove these test variables
- bool toHidden;
- uint64 toHiddenFST;
- uint8 toHiddenFL;
-
//TDMA class variables
uint8 maxFramelength;
uint8 slotAssingmentSelfIndex;
struct TDMAInfo uwbListTDMAInfo[UWB_LIST_SIZE];
- uint64 lastFST;
uint64 lastSlotStartTime64;
uint32 slotDuration_ms;
uint32 slotDuration_us;