From c801533453d93723a228e717bdb4c1d8c0f3be4a Mon Sep 17 00:00:00 2001
From: David <stierint@hotmail.com>
Date: Sun, 4 Oct 2020 16:09:01 -0400
Subject: [PATCH] subnetwork sync implemented
---
src/application/application_definitions.h | 18 ++--
src/application/dw_main.c | 12 +--
src/application/instance.c | 120 +++++++++++++---------
src/application/instance.h | 5 +-
src/application/instance_common.c | 12 ++-
src/application/tdma_handler.c | 105 +++++++++++++------
src/application/tdma_handler.h | 10 +-
7 files changed, 183 insertions(+), 99 deletions(-)
diff --git a/src/application/application_definitions.h b/src/application/application_definitions.h
index 18cec85..3aa5dc4 100644
--- a/src/application/application_definitions.h
+++ b/src/application/application_definitions.h
@@ -61,8 +61,8 @@ enum
POLL,
RESP,
FINAL,
- INF,
- FRAME_TYPE_NB //TODO add in RNG_REPORT? would be useful in calculating how long a frame slot should be... if so, dont forget to update the init_timings function...
+ SYNC,
+ FRAME_TYPE_NB
};
//TWO WAY RANGING function codes
@@ -74,16 +74,17 @@ enum
#define RTLS_DEMO_MSG_INF_INIT (0x14) // TDMA coordination info packet
#define RTLS_DEMO_MSG_INF_SUG (0x15) // TDMA coordination info packet
#define RTLS_DEMO_MSG_INF_UPDATE (0x16) // TDMA coordination info packet
-#define RTLS_DEMO_MSG_RNG_REPORT (0x11) // Report of calculated range back to the Tag
+#define RTLS_DEMO_MSG_RNG_REPORT (0x11) // Report of calculated range to network
+#define RTLS_DEMO_MSG_SYNC (0x22) // Inform other UWBs to sync their TDMA frame start times
//lengths including the Decaranging Message Function Code byte
#define TAG_POLL_MSG_LEN 1 // FunctionCode(1),
#define ANCH_RESPONSE_MSG_LEN 15 //TODO shorten! // FunctionCode(1), RespOption (1), OptionParam(2), Number of Tags(1), Measured_TOF_Time(6), Time Till next window reserved for catching a blink message (4)
#define TAG_FINAL_MSG_LEN 16 // FunctionCode(1), Poll_TxTime(5), Resp_RxTime(5), Final_TxTime(5)
-//#define RANGINGINIT_MSG_LEN 7 // FunctionCode(1), Tag Address (2), Response Time (2) * 2
#define RANGINGINIT_MSG_LEN 1 // FunctionCode(1)
#define RNG_REPORT_MSG_LEN_SHORT 9 // FunctionCode(1), time of flight (6), short address (2)
#define RNG_REPORT_MSG_LEN_LONG 15 // FunctionCode(1), time of flight (6), long address (8)
+#define SYNC_MSG_LEN 8 // FunctionCode (1), framelength (1), time since frame start (6)
#define MAX_MAC_MSG_DATA_LEN (TAG_FINAL_MSG_LEN) //max message len of the above
@@ -126,11 +127,13 @@ enum
#define POLL_FRAME_LEN_BYTES (TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC)
#define RESP_FRAME_LEN_BYTES (ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC)
#define FINAL_FRAME_LEN_BYTES (TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC)
+ #define SYNC_FRAME_LEN_BYTES (SYNC_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC)
#else
#define RNG_INIT_FRAME_LEN_BYTES (RANGINGINIT_MSG_LEN + FRAME_CRTL_AND_ADDRESS_LS + FRAME_CRC)
#define POLL_FRAME_LEN_BYTES (TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC)
#define RESP_FRAME_LEN_BYTES (ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC)
#define FINAL_FRAME_LEN_BYTES (TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC)
+ #define SYNC_FRAME_LEN_BYTES (SYNC_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC)
#endif
#define BLINK_FRAME_CONTROL_BYTES (1)
@@ -189,6 +192,10 @@ enum
#define REPORT_TOF 1 // Offset to put ToF values in the report message. (6 bytes)
#define REPORT_ADDR 7 // Offset to put address of other UWB involved in the range report (2 [short address] or 8 [long address] bytes)
+//Sync
+#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.
@@ -245,7 +252,7 @@ enum
// Default tag turn-around time: cannot be less than 300 us. Defined as 500 us
// so that the tag is not transmitting more than one frame by millisecond (for
// power management purpose).
-#define TAG_TURN_AROUND_TIME_US 1500 //TODO tune again!!! (300)
+#define TAG_TURN_AROUND_TIME_US 2500 //TODO tune again!!! (300)
// "Long" response delays value. Over this limit, special processes must be
// applied.
@@ -275,7 +282,6 @@ enum
#define MIN_FRAMELENGTH 4 //minimum size by TDMA E-ASAP
typedef uint64_t uint64 ;
-
typedef int64_t int64 ;
diff --git a/src/application/dw_main.c b/src/application/dw_main.c
index 9514861..b49a499 100644
--- a/src/application/dw_main.c
+++ b/src/application/dw_main.c
@@ -539,12 +539,12 @@ int dw_main(void)
if(instance_mode == TAG)
{
-// uint64 aaddr = instancenewrangeancadd();
-// uint64 taddr = instancenewrangetagadd();
-//// int n = sprintf((char*)&dataseq[0], "RANGE_COMPLETE,%llX,%llX", taddr, aaddr);
-// int n = sprintf((char*)&dataseq[0], "RANGE_COMPLETE,%04llX,%04llX", taddr, aaddr);
-// send_usbmessage(&dataseq[0], n);
-// usb_run();
+ uint64 aaddr = instancenewrangeancadd();
+ uint64 taddr = instancenewrangetagadd();
+// int n = sprintf((char*)&dataseq[0], "RANGE_COMPLETE,%llX,%llX", taddr, aaddr);
+ int n = sprintf((char*)&dataseq[0], "RANGE_COMPLETE,%04llX,%04llX", taddr, aaddr);
+ send_usbmessage(&dataseq[0], n);
+ usb_run();
}
else
diff --git a/src/application/instance.c b/src/application/instance.c
index d2d5193..b4cb4b6 100644
--- a/src/application/instance.c
+++ b/src/application/instance.c
@@ -69,23 +69,6 @@ int usbtxdebugdataprev_size = 0;
//
void instanceconfigframeheader(instance_data_t *inst)
{
-
-// //configure ranging message(s)
-// for(int i = 0; i < UWB_LIST_SIZE; i++)//TODO only have one msg
-// {
-// inst->msg[i].panID[0] = (inst->panID) & 0xff;
-// inst->msg[i].panID[1] = inst->panID >> 8;
-//
-// //set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
-// inst->msg[i].frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/;
-//#if (USING_64BIT_ADDR==1)
-// //source/dest addressing modes and frame version
-// inst->msg[i].frameCtrl[1] = 0xC /*dest extended address (64bits)*/ | 0xC0 /*src extended address (64bits)*/;
-//#else
-// inst->msg[i].frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0x80 /*src short address (16bits)*/;
-//#endif
-// }
-
//configure ranging message
inst->msg.panID[0] = (inst->panID) & 0xff;
inst->msg.panID[1] = inst->panID >> 8;
@@ -127,7 +110,6 @@ void instanceconfigframeheader(instance_data_t *inst)
//configure RNG_REPORT
-
inst->report_msg.panID[0] = (inst->panID) & 0xff;
inst->report_msg.panID[1] = inst->panID >> 8;
@@ -141,6 +123,20 @@ void instanceconfigframeheader(instance_data_t *inst)
#endif
+ //configure SYNC message
+ inst->sync_msg.panID[0] = (inst->panID) & 0xff;
+ inst->sync_msg.panID[1] = inst->panID >> 8;
+
+ //set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
+ inst->sync_msg.frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/;
+#if (USING_64BIT_ADDR==1)
+ //source/dest addressing modes and frame version
+ inst->sync_msg.frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0xC0 /*src extended address (64bits)*/;
+#else
+ inst->sync_msg.frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0x80 /*src short address (16bits)*/;
+#endif
+
+
//configure BLINK
//blink frames with IEEE EUI-64 tag ID
inst->blinkmsg.frameCtrl = 0xC5 ;
@@ -226,6 +222,11 @@ void instanceconfigmessages(instance_data_t *inst)
inst->report_msg.messageData[FCODE] = RTLS_DEMO_MSG_RNG_REPORT; //message function code (specifies if message is a poll, response or other...)
+ //configure SYNC message
+ memcpy(&inst->sync_msg.sourceAddr[0], &inst->eui64[0], inst->addrByteSize);
+ memcpy(&inst->sync_msg.destAddr[0], &broadcast_address, 2);
+ inst->sync_msg.messageData[FCODE] = RTLS_DEMO_MSG_SYNC; //message function code (specifies if message is a poll, response or other...)
+
//configure BLINK message
memcpy(&inst->blinkmsg.tagID[0], &inst->eui64[0], ADDR_BYTE_SIZE_L);
@@ -960,11 +961,11 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
dwt_writetxdata(psduLength, (uint8 *) &inst->msg, 0) ; // write the frame data
dwt_writetxfctrl(psduLength, 0, 1);
if(dwt_starttx(DWT_START_TX_IMMEDIATE | inst->wait4ack) == 0){
-// uint8 debug_msg[100];
-//// int n = sprintf((char *)&debug_msg, "TX_POLL,%llX,%llX", instance_get_addr(), instance_get_uwbaddr(inst->uwbToRangeWith));
-// int n = sprintf((char *)&debug_msg, "TX_POLL,%04llX,%04llX", instance_get_addr(), instance_get_uwbaddr(inst->uwbToRangeWith));
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
+ uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "TX_POLL,%llX,%llX", instance_get_addr(), instance_get_uwbaddr(inst->uwbToRangeWith));
+ int n = sprintf((char *)&debug_msg, "TX_POLL,%04llX,%04llX", instance_get_addr(), instance_get_uwbaddr(inst->uwbToRangeWith));
+ send_usbmessage(&debug_msg[0], n);
+ usb_run();
}
inst->testAppState = TA_TX_WAIT_CONF ; //TODO move to if statement above? // wait confirmation
@@ -1463,6 +1464,23 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
break;
} //RTLS_DEMO_MSG_RNG_INIT
+ case RTLS_DEMO_MSG_SYNC :
+ {
+ uint8 srcIndex = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
+ uint8 framelength;
+ uint64 timeSinceFrameStart_us = 0;
+ memcpy(&framelength, &messageData[SYNC_FRAMELENGTH], sizeof(uint8));
+ memcpy(&timeSinceFrameStart_us, &messageData[SYNC_TSFS], 6);
+
+ 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
+ tdma_handler->frame_sync(tdma_handler, dw_event, framelength, timeSinceFrameStart_us, srcIndex, FS_EVAL);
+ }
+
+ break;
+ }
case RTLS_DEMO_MSG_INF_UPDATE : //fall through
case RTLS_DEMO_MSG_INF_SUG : //fall through
case RTLS_DEMO_MSG_INF_REG :
@@ -1470,6 +1488,11 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
uint32 time_now = portGetTickCnt();
uint8 srcIndex = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
+ uint8 framelength;
+ uint64 timeSinceFrameStart_us = 0;
+ memcpy(&framelength, &messageData[TDMA_FRAMELENGTH], sizeof(uint8));
+ memcpy(&timeSinceFrameStart_us, &messageData[TDMA_TSFS], 6);
+
//return to dicovery mode if no slots assigned to this UWB
if(inst->mode == ANCHOR || inst->mode == TAG)
{
@@ -1489,7 +1512,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
if(tdma_handler->discovery_mode == WAIT_INF_REG) //treat INF_UPDATE and INF_SUG the same
{
//synchronize the frames //TODO don't need to sync frame at this point...?
- tdma_handler->frame_sync(tdma_handler, dw_event, messageData, srcIndex, FS_ADOPT);
+ tdma_handler->frame_sync(tdma_handler, dw_event, framelength, timeSinceFrameStart_us, 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 COLLECT_INF_REG
@@ -1505,7 +1528,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
//sync to the largest subnetwork...
//synchronize the frames
- tdma_handler->frame_sync(tdma_handler, dw_event, messageData, srcIndex, FS_COLLECT);
+ tdma_handler->frame_sync(tdma_handler, dw_event, framelength, timeSinceFrameStart_us, srcIndex, FS_COLLECT);
//collecting tdma info, append to previously stored info
tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, COPY);
}
@@ -1513,7 +1536,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
{
//process frame sync while waiting to send sug so we maintain syn with selected (sub)network
//also give ourselves the opportunity to detect the need to transmit frame sync rebase messages
- tdma_handler->frame_sync(tdma_handler, dw_event, messageData, srcIndex, FS_AVERAGE);
+ tdma_handler->frame_sync(tdma_handler, dw_event, framelength, timeSinceFrameStart_us, srcIndex, FS_AVERAGE);
}
}
else if(inst->mode == ANCHOR || inst->mode == TAG)
@@ -1525,7 +1548,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
//2.) check for and adopt any tdma changes, sending an INF_UPDATE or INF_REG accordingly
//synchronize the frames
- tdma_handler->frame_sync(tdma_handler, dw_event, messageData, srcIndex, FS_AVERAGE);
+ tdma_handler->frame_sync(tdma_handler, dw_event, framelength, timeSinceFrameStart_us, srcIndex, FS_AVERAGE);
//collecting tdma info, append to previously stored info
bool tdma_modified = tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, CLEAR_LISTED_COPY);
@@ -1599,9 +1622,13 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
// tdma_handler->uwbListTDMAInfo[0].framelength = (uint8)MIN_FRAMELENGTH;
// tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith].framelength = (uint8)MIN_FRAMELENGTH;
+ uint8 framelength;
+ uint64 timeSinceFrameStart_us = 0;
+ memcpy(&framelength, &messageData[TDMA_FRAMELENGTH], sizeof(uint8));
+ memcpy(&timeSinceFrameStart_us, &messageData[TDMA_TSFS], 6);
//synchronise the frames
- tdma_handler->frame_sync(tdma_handler, dw_event, messageData, srcIndex, FS_ADOPT);
+ tdma_handler->frame_sync(tdma_handler, dw_event, framelength, timeSinceFrameStart_us, srcIndex, FS_ADOPT);
//copy the TDMA network configuration directly
tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, CLEAR_ALL_COPY);
//copy new TDMA configuration into the INF message that this UWB will send out
@@ -1820,11 +1847,11 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->lastRangeTimeStamp[inst->uwbToRangeWith] = portGetTickCnt();
-// uint8 debug_msg[100];
-//// 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();
+ uint8 debug_msg[100];
+// 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();
//TODO remember that below is for testing out if its better to send INF as last message
@@ -1907,19 +1934,19 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
}
else if(inst->previousState == TA_TXFINAL_WAIT_SEND)
{
-// uint8 debug_msg[100];
-//// 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);
-// usb_run();
+ uint8 debug_msg[100];
+// 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);
+ usb_run();
}
else if(inst->previousState == TA_TXPOLL_WAIT_SEND)
{
-// uint8 debug_msg[100];
-//// 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);
-// usb_run();
+ uint8 debug_msg[100];
+// 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);
+ usb_run();
}
instance_getevent(17); //get and clear this event
@@ -2038,10 +2065,9 @@ void instance_init_timings(void)
uint32 pre_len;
int sfd_len;
- static const int data_len_bytes[FRAME_TYPE_NB - 1] = {
+ static const int data_len_bytes[FRAME_TYPE_NB] = {
BLINK_FRAME_LEN_BYTES, RNG_INIT_FRAME_LEN_BYTES, POLL_FRAME_LEN_BYTES,
- RESP_FRAME_LEN_BYTES, FINAL_FRAME_LEN_BYTES};
-
+ RESP_FRAME_LEN_BYTES, FINAL_FRAME_LEN_BYTES, SYNC_FRAME_LEN_BYTES};
// Margin used for timeouts computation.
@@ -2094,7 +2120,7 @@ void instance_init_timings(void)
inst->storePreLen_us = CEIL_DIV(pre_len, 100000);
// Second step is data length for all frame types.
- for (int i = 0; i < FRAME_TYPE_NB - 1; i++)//exclude INF message, since it changes size
+ for (int i = 0; i < FRAME_TYPE_NB; i++)
{
inst->frameLengths_us[i] = instance_getmessageduration_us(data_len_bytes[i]);
diff --git a/src/application/instance.h b/src/application/instance.h
index 7ebb8f4..99b10c1 100644
--- a/src/application/instance.h
+++ b/src/application/instance.h
@@ -76,11 +76,14 @@ typedef struct
srd_ext_msg_dlsl msg; // simple 802.15.4 frame structure (used for tx message) - using long addresses
srd_ext_msg_dssl inf_msg; // extended inf message containing frame lengths and slot assignments
srd_ext_msg_dssl report_msg; // extended report message containing the calculated range
+ srd_ext_msg_dssl sync_msg; // extended message indicating the need to resync TDMA frame
+ //TODO make sure the dssl are correct! (ds for broadcast?)
#else
srd_msg_dlss rng_initmsg ; // ranging init message (destination long, source short)
srd_ext_msg_dsss msg; // simple 802.15.4 frame structure (used for tx message) - using short addresses
srd_ext_msg_dsss inf_msg; // extended inf message containing frame lengths and slot assignments
- srd_ext_msg_dsss report_msg; // extended report message containing the calculated range
+ srd_ext_msg_dsss report_msg; // extended report message containing the calculated range
+ srd_ext_msg_dsss sync_msg; // extended message indicating the need to resync TDMA frame
#endif
iso_IEEE_EUI64_blink_msg blinkmsg ; // frame structure (used for tx blink message)
diff --git a/src/application/instance_common.c b/src/application/instance_common.c
index 93c3308..735ee78 100644
--- a/src/application/instance_common.c
+++ b/src/application/instance_common.c
@@ -1220,7 +1220,6 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
{
srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_S;
}
-
}
break;
@@ -1275,13 +1274,12 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
rxd_event = SIG_RX_UNKNOWN;
}
}
-
-
else 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 &&
- dw_event.msgu.frame[fcode_index] != RTLS_DEMO_MSG_RNG_REPORT)
+ dw_event.msgu.frame[fcode_index] != RTLS_DEMO_MSG_RNG_REPORT &&
+ dw_event.msgu.frame[fcode_index] != RTLS_DEMO_MSG_SYNC)
{
rxd_event = SIG_RX_UNKNOWN;
// uint8 debug_msg[150];
@@ -1455,7 +1453,7 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
if(rxd_event == DWT_SIG_RX_OKAY && uwb_index != 255)
{
//always accept.
- if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_REPORT)
+ if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_REPORT || dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_SYNC)
{
// uint8 debug_msg[100];
// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: RNG_REPORT <- uwb_index %d", uwb_index);
@@ -1671,6 +1669,10 @@ 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();
+// }
+// else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_SYNC)
+// {
+//
// }
diff --git a/src/application/tdma_handler.c b/src/application/tdma_handler.c
index 3dda51a..e87cddf 100644
--- a/src/application/tdma_handler.c
+++ b/src/application/tdma_handler.c
@@ -33,11 +33,11 @@ static bool slot_transition(struct TDMAHandler *this)
uint64 frameDuration64 = this->slotDuration_us*this->uwbListTDMAInfo[0].framelength;
if(timeSinceFrameStart64 >= frameDuration64)
{
- 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], "%llX, %llu, %llu", instance_get_addr(), this->lastFST, time_now_us);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// 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], "%llX, %llu, %llu", instance_get_addr(), this->lastFST, time_now_us);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
//TODO think about adopting rebase AFTER all neighbors have had a chance to TX the rebase in their INF message as well.
// this->rebase_pending = FALSE;
@@ -116,7 +116,8 @@ static bool slot_transition(struct TDMAHandler *this)
}
//static void frame_sync(struct TDMAHandler *this, uint8 *messageData, uint16 rxLength, uint8 srcIndex, FRAME_SYNC_MODE mode)
-static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 *messageData, uint8 srcIndex, FRAME_SYNC_MODE mode)
+//static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 *messageData, uint8 srcIndex, FRAME_SYNC_MODE mode)
+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);
@@ -126,13 +127,12 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 *
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 time_now_us = portGetTickCntMicro();
- uint8 framelength;
- uint32 timeSinceFrameStart64 = 0;
-
- memcpy(&framelength, &messageData[TDMA_FRAMELENGTH], 1);
- //timeSinceFrameStart in message
- memcpy(&timeSinceFrameStart64, &messageData[TDMA_TSFS], 6);
+// uint8 framelength;
+// uint64 timeSinceFrameStart_us = 0;
+// memcpy(&framelength, &messageData[TDMA_FRAMELENGTH], sizeof(uint8));
+// //timeSinceFrameStart in message
+// memcpy(&timeSinceFrameStart_us, &messageData[TDMA_TSFS], 6);
//time from message to tx
//assuming zero since we use DWT_START_TX_IMMEDIATE
@@ -140,11 +140,6 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 *
//tx antenna delay
uint64 tx_antenna_delay = (uint64)inst->txAntennaDelay;
- //time for xmission (only count once, happens on both sides near simultaneously)
- //TODO make sure this is correct...
- //easiest way to check would be to see if it is the same as the defines for other standard messages...
-// inst->frameLengths_us[INF] = instance_getmessageduration_us(dw_event->rxLength); //TODO should maybe make sure extended framelength cannot overflow a uint32
-
//time to propagate
//NOTE: assuming zero since difference for speed of light travel time over 10cm and 100m is negligible for frame sync purposes
@@ -156,10 +151,10 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 *
uint64 txrx_delay = (uint64)(convertdevicetimetosec(tx_antenna_delay + rxfs_process_delay)*1000000.0) + inst->storePreLen_us;
- uint64 hisTimeSinceFrameStart_us = timeSinceFrameStart64 + txrx_delay;
+ uint64 hisTimeSinceFrameStart_us = timeSinceFrameStart_us + txrx_delay;
this->uwbFrameStartTimes64[srcIndex] = timestamp_subtract64(time_now_us, hisTimeSinceFrameStart_us); //TODO consider applying the diff!
- if(mode == FS_ADOPT)
+ if(mode == FS_ADOPT) //TODO this might not be right! incoming framelength not always the same as ours!
{
this->uwbFrameStartTimes64[0] = this->uwbFrameStartTimes64[srcIndex];
uint8 slot = hisTimeSinceFrameStart_us/this->slotDuration_us; //integer division rounded down
@@ -169,9 +164,9 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 *
{
return;
}
- else if(mode == FS_AVERAGE) //TODO consider using a weighted average. perhaps use threshholds to determine if instead we should just use the ADOPT logic
+ 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
{
- uint64 myFramelengthDuration_us = this->uwbListTDMAInfo[0].framelength*this->slotDuration_us;
+// uint64 myFramelengthDuration_us = this->uwbListTDMAInfo[0].framelength*this->slotDuration_us;
uint64 myTimeSinceFrameStart_us = get_dt64(this->uwbFrameStartTimes64[0], time_now_us);
////OLD WAY
@@ -283,13 +278,20 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 *
}
}
- uint64 threshold = 2*this->slotStartDelay_us;//TODO make this a permanent variable
- if(diff_us > threshold)
+ if(diff_us > this->frameSyncThreshold_us)
{
- return;//TODO send out a rebase message!
+ this->tx_sync_msg(this);
+ }
+ else if(mode == FS_EVAL)
+ {
+ return;
}
-
+ uint8 div = 2;
+ if(mode == FS_EVAL)
+ {
+ div = 1;
+ }
if(diff_add == TRUE)
{
@@ -298,8 +300,8 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 *
// 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);
+ this->uwbFrameStartTimes64[0] = timestamp_add64(this->uwbFrameStartTimes64[0], diff_us/div);
+ this->lastSlotStartTime64 = timestamp_add64(this->lastSlotStartTime64, diff_us/div);
}
else
{
@@ -307,8 +309,8 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 *
// 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);
+ this->uwbFrameStartTimes64[0] = timestamp_subtract64(this->uwbFrameStartTimes64[0], diff_us/div);
+ this->lastSlotStartTime64 = timestamp_subtract64(this->lastSlotStartTime64, diff_us/div);
}
@@ -316,6 +318,45 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 *
}
}
+
+static bool tx_sync_msg(struct TDMAHandler *this)
+{
+ instance_data_t *inst = instance_get_local_structure_ptr(0);
+ uint64 time_now_us = portGetTickCntMicro();
+ uint64 myTimeSinceFrameStart_us = get_dt64(this->uwbFrameStartTimes64[0], 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);
+ int psduLength = 0;
+
+ inst->sync_msg.seqNum = inst->frameSN++;
+
+
+#if (USING_64BIT_ADDR==1)
+ psduLength = SYNC_MSG_LEN + FRAME_CRTL_AND_ADDRESS_LS + FRAME_CRC;
+#else
+ psduLength = SYNC_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
+#endif
+
+ dwt_writetxdata(psduLength, (uint8 *)&inst->sync_msg, 0) ; // write the frame data
+
+ inst->wait4ack = 0;
+
+ if(instancesendpacket(psduLength, DWT_START_RX_IMMEDIATE, 0))
+ {
+ inst->testAppState = TA_RXE_WAIT;
+ return FALSE;
+ }
+ else
+ {
+
+ inst->previousState = inst->testAppState;
+ inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
+ return TRUE;
+ }
+}
+
+
+
//
////static void frame_sync(struct TDMAHandler *this, uint8 *messageData, uint16 rxLength, uint8 srcIndex, FRAME_SYNC_MODE mode)
//static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 *messageData, uint8 srcIndex, FRAME_SYNC_MODE mode)
@@ -2683,11 +2724,10 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
// }
// }
- uint64 threshold = this->slotStartDelay_us*2; //TODO make a permanent variable for this threshold
//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...
- if(diff_us < threshold)
+ if(diff_us < this->frameSyncThreshold_us)
{
sub_network_members[j]++;
sub_network_membership[i] = j;
@@ -2990,6 +3030,7 @@ static struct TDMAHandler new(){
ret.slot_transition = &slot_transition;
ret.frame_sync = &frame_sync;
+ ret.tx_sync_msg = &tx_sync_msg;
ret.tx_select = &tx_select;
ret.check_blink = &check_blink;
@@ -3013,6 +3054,7 @@ static struct TDMAHandler new(){
ret.check_discovery_mode_expiration = &check_discovery_mode_expiration;
ret.usb_dump_tdma = &usb_dump_tdma;
+
ret.deconflict_slot_assignments = &deconflict_slot_assignments;
ret.deconflict_uwb_pair = &deconflict_uwb_pair;
ret.deconflict_slot_pair = &deconflict_slot_pair;
@@ -3044,6 +3086,7 @@ static struct TDMAHandler new(){
ret.infSentThisSlot = FALSE;
ret.pollSentThisSlot = FALSE;
ret.slotStartDelay_us = 4000;
+ ret.frameSyncThreshold_us = 2*ret.slotStartDelay_us;
ret.infMessageLength = 0;
ret.rebase_pending = FALSE;
ret.rebase_tx = FALSE;
diff --git a/src/application/tdma_handler.h b/src/application/tdma_handler.h
index a5e0aaa..78b7cc4 100644
--- a/src/application/tdma_handler.h
+++ b/src/application/tdma_handler.h
@@ -25,8 +25,8 @@ typedef enum frame_sync_mode
{
FS_ADOPT,
FS_AVERAGE,
- FS_COLLECT
-// FS_REBASE //TODO remove
+ FS_COLLECT,
+ FS_EVAL
}
FRAME_SYNC_MODE;
@@ -58,6 +58,7 @@ struct TDMAHandler
//TODO can probably use a smaller data type...
uint64 slotStartDelay_us; //time between slot start and transmission within that slot
+ uint64 frameSyncThreshold_us;
//discovery variables
DISCOVERY_MODE discovery_mode;
@@ -74,7 +75,9 @@ 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 *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);
bool (*tx_select)(struct TDMAHandler *this);
bool (*check_blink)(struct TDMAHandler *this);
@@ -87,6 +90,7 @@ struct TDMAHandler
void (*check_discovery_mode_expiration)(struct TDMAHandler *this);
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);
--
GitLab