diff --git a/DecaRanging.coproj b/DecaRanging.coproj
index 983056d8e9136f30b6debdd1afa54cdcc4e69953..be9541106ba3ab279f864ed5dd4e7736df073fe9 100644
--- a/DecaRanging.coproj
+++ b/DecaRanging.coproj
@@ -4,9 +4,9 @@
<Device manufacturerId="9" manufacturerName="ST" chipId="334" chipName="STM32F105RC" boardId="" boardName=""/>
<BuildOption>
<Compile>
- <Option name="OptimizationLevel" value="4"/>
+ <Option name="OptimizationLevel" value="0"/>
<Option name="UseFPU" value="0"/>
- <Option name="UserEditCompiler" value="-ffunction-sections; -fdata-sections; -c; -fmessage-length=0;"/>
+ <Option name="UserEditCompiler" value="-ffunction-sections; -fdata-sections; -c; -fmessage-length=0"/>
<Includepaths>
<Includepath path="."/>
<Includepath path="Libraries/CMSIS/CM3/CoreSupport"/>
diff --git a/Libraries/STM32_USB_OTG_Driver/src/usb_core.c b/Libraries/STM32_USB_OTG_Driver/src/usb_core.c
index e257e73dc4c0166c9acc32c8b3349d0a0cf5afcc..9b35db089e5cc63b8959aec32ef3d4d8b2bdbc61 100644
--- a/Libraries/STM32_USB_OTG_Driver/src/usb_core.c
+++ b/Libraries/STM32_USB_OTG_Driver/src/usb_core.c
@@ -1961,7 +1961,7 @@ void USB_OTG_ActiveRemoteWakeup(USB_OTG_CORE_HANDLE *pdev)
if(pdev->cfg.low_power)
{
/* un-gate USB Core clock */
- power.d32 = USB_OTG_READ_REG32(&pdev->regs.PCGCCTL);
+ power.d32 = USB_OTG_READ_REG32(pdev->regs.PCGCCTL);
power.b.gatehclk = 0;
power.b.stoppclk = 0;
USB_OTG_WRITE_REG32(pdev->regs.PCGCCTL, power.d32);
@@ -1995,7 +1995,7 @@ void USB_OTG_UngateClock(USB_OTG_CORE_HANDLE *pdev)
if(dsts.b.suspsts == 1)
{
/* un-gate USB Core clock */
- power.d32 = USB_OTG_READ_REG32(&pdev->regs.PCGCCTL);
+ power.d32 = USB_OTG_READ_REG32(pdev->regs.PCGCCTL);
power.b.gatehclk = 0;
power.b.stoppclk = 0;
USB_OTG_WRITE_REG32(pdev->regs.PCGCCTL, power.d32);
diff --git a/Libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c b/Libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c
index 131979493ddefc653a6d112663b159bfe14eafed..6fee2e12df8fb665a18804d18c057815a0d3ce4b 100644
--- a/Libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c
+++ b/Libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c
@@ -352,7 +352,7 @@ static uint32_t DCD_HandleResume_ISR(USB_OTG_CORE_HANDLE *pdev)
if(pdev->cfg.low_power)
{
/* un-gate USB Core clock */
- power.d32 = USB_OTG_READ_REG32(&pdev->regs.PCGCCTL);
+ power.d32 = USB_OTG_READ_REG32(pdev->regs.PCGCCTL);
power.b.gatehclk = 0;
power.b.stoppclk = 0;
USB_OTG_WRITE_REG32(pdev->regs.PCGCCTL, power.d32);
diff --git a/src/application/application_definitions.h b/src/application/application_definitions.h
index da3d693372ee3a101c5ec4b0ea63f9b60a8d8aa8..501a5ee8caa54cc93e1e587e7a566530057bf160 100644
--- a/src/application/application_definitions.h
+++ b/src/application/application_definitions.h
@@ -22,6 +22,7 @@
#define NUM_INST 1
#define SPEED_OF_LIGHT (299704644.54) //(299702547.0) // in m/s in air
#define MASK_40BIT (0x00FFFFFFFFFF) // DW1000 counter is 40 bits
+#define MASK_42BIT (0x000003FFFFFFFFFF) //stm32 microsecond timestamps are 42 bits
#define MASK_TXDTS (0x00FFFFFFFE00) //The TX timestamp will snap to 8 ns resolution - mask lower 9 bits.
#define SYS_MASK_VAL (DWT_INT_TFRS | DWT_INT_RFCG | DWT_INT_RXOVRR | DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO)
#define DELAY_CALIB (0) // when set to 1 - the LCD display will show information used for TX/RX delay calibration
@@ -31,7 +32,6 @@
#define RX_ANT_DELAY 0
#define USING_64BIT_ADDR (0) //when set to 0 - the DecaRanging application will use 16-bit addresses
-#define USING_LCD (1) //when set to 0 - the DecaRanging application will not use the LCD display
//! callback events
#define DWT_SIG_RX_NOERR 0
@@ -78,10 +78,12 @@ enum
//lengths including the Decaranging Message Function Code byte
#define TAG_POLL_MSG_LEN 1 // FunctionCode(1),
-#define ANCH_RESPONSE_MSG_LEN 15 // 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 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 RNG_REPORT_MSG_LEN 7 // FunctionCode(1), time of flight (6)
+//#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 MAX_MAC_MSG_DATA_LEN (TAG_FINAL_MSG_LEN) //max message len of the above
@@ -154,7 +156,7 @@ enum
-#define IMMEDIATE_RESPONSE (1)
+#define IMMEDIATE_RESPONSE 1
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
// NOTE: the maximum RX timeout is ~ 65ms
@@ -169,11 +171,11 @@ enum
#define FCODE 0 // Function code is 1st byte of messageData
//INF message byte offsets
-#define TDMA_TSFS 1 // offset to put time since TDMA frame start in the INF message
-#define TDMA_NUMN 5 // offset to put the number of this UWB's neighbors in the INF message
-#define TDMA_NUMH 6 // offset to put the number of this UWB's hidden neighbors in the INF message
-#define TDMA_FRAMELENGTH 7 // offset to put this UWB's TDMA framelength in the INF message
-#define TDMA_NUMS 8 // offset to put the number of this UWB's TDMA slot assignments in the INF message
+#define TDMA_TSFS 1+6 // offset to put time since TDMA frame start in the INF message
+#define TDMA_NUMN 7+6 // offset to put the number of this UWB's neighbors in the INF message
+#define TDMA_NUMH 8+6 // offset to put the number of this UWB's hidden neighbors in the INF message
+#define TDMA_FRAMELENGTH 9+6 // offset to put this UWB's TDMA framelength in the INF message
+#define TDMA_NUMS 10+6 // offset to put the number of this UWB's TDMA slot assignments in the INF message
// Final message byte offsets.
@@ -182,22 +184,19 @@ enum
#define FTXT 11
-// Anchor response byte offsets.
-#define RES_R1 1 // Response option octet 0x02 (1),
-#define RES_R2 2 // Response option parameter 0x00 (1) - used to notify Tag that the report is coming
-#define RES_R3 3 // Response option parameter 0x00 (1),
-#define NTAG 4 // Offset to put number of active TAGs in the report message. (1 byte)
-#define TOFR 5 // Offset to put ToF values in the report message. (6 bytes)
-#define TIME_TILL 11 // Offset to put time until next RX_ACCEPT in the report message (4 bytes)
+// Range report byte offsets.
+#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)
+//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
+//#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:
@@ -233,7 +232,7 @@ enum
#define US_TO_SY_INT(x) (((x) * 10000) / 10256)
// Minimum delay between reception and following transmission.
-#define RX_TO_TX_TIME_US 150 //TODO tune again (150)
+#define RX_TO_TX_TIME_US 1500 //TODO tune again (150)
#define RXTOTXTIME ((int)(50.0 / 1.0256)) //e.g. Poll RX to Response TX time
// Default anchor turn-around time: has to be RX_TO_TX_TIME_US when using
@@ -279,7 +278,7 @@ typedef uint64_t uint64 ;
typedef int64_t int64 ;
-typedef enum instanceModes{DISCOVERY, CONNECTION_PENDING, TAG, ANCHOR, NUM_MODES} INST_MODE;
+typedef enum instanceModes{DISCOVERY, TAG, ANCHOR, NUM_MODES} INST_MODE;
typedef enum discovery_modes
{
diff --git a/src/application/dw_main.c b/src/application/dw_main.c
index 336fdbaa6ee607893a6a8d6c8fde15b03ef391df..ab20a56654b539e5f9aa0364ef93ea2ff75bc994 100644
--- a/src/application/dw_main.c
+++ b/src/application/dw_main.c
@@ -27,7 +27,7 @@ extern void usb_printconfig(int, uint8*, int);
extern void send_usbmessage(uint8*, int);
// "1234567890123456" - 16 bytes long LCD
-#define SOFTWARE_VER_STRING "Version 3.11 " //
+#define SOFTWARE_VER_STRING "TDMA Version 1.0" //
#define SWS1_TXSPECT_MODE 0x80 //Continuous TX spectrum mode
#define SWS1_ANC_MODE 0x08 //anchor mode
@@ -263,14 +263,17 @@ void setLCDline1(uint8 s1switch)
**/
void configure_continuous_txspectrum_mode(uint8 s1switch)
{
-#if (USING_LCD == 1)
- uint8 command = 0x2 ; //return cursor home
- writetoLCD( 1, 0, &command);
- sprintf((char*)&dataseq[0], "Conti TX %s:%d:%d ", (s1switch & SWS1_SHF_MODE) ? "S" : "L", chan, prf);
- writetoLCD( 40, 1, dataseq); //send some data
- memcpy(dataseq, (const uint8 *) "Spectrum Test ", 16);
- writetoLCD( 16, 1, dataseq); //send some data
-#endif
+ if(port_is_switch_on(TA_SW1_4) == S1_SWITCH_ON)
+ {
+ //LCD display ON
+
+ uint8 command = 0x2 ; //return cursor home
+ writetoLCD(1, 0, &command);
+ sprintf((char*)&dataseq[0], "Conti TX %s:%d:%d ", (s1switch & SWS1_SHF_MODE) ? "S" : "L", chan, prf);
+ writetoLCD(LCD_BUFF_LEN, 1, dataseq); //send some data
+ memcpy(dataseq, (const uint8 *) "Spectrum Test ", LCD_BUFF_LEN);
+ writetoLCD(LCD_BUFF_LEN, 1, dataseq); //send some data
+ }
//configure DW1000 into Continuous TX mode
instance_starttxtest(0x1000);
@@ -298,30 +301,61 @@ int dw_main(void)
{
int i = 0;
double range_result = 0;
+ uint64 range_addr = 0;
int canSleep;
+ //LCD variables
+ bool enableLCD = FALSE;
+ int toggle = 0;
+ int toggle_counter = 0;
+ int toggle_step = 5;
+ uint8 dataseq[LCD_BUFF_LEN];
+// bool new_range = FALSE;
+ uint8 command = 0x0;
+
led_off(LED_ALL); //turn off all the LEDs
peripherals_init();
-#if (USING_LCD == 1)
- int toggle = 1;
- spi_peripheral_init(1);
-#else
- spi_peripheral_init(0);
-#endif
+ s1switch = port_is_boot1_on(0) << 1
+ | port_is_switch_on(TA_SW1_3) << 2
+ | port_is_switch_on(TA_SW1_4) << 3
+ | port_is_switch_on(TA_SW1_5) << 4
+ | port_is_switch_on(TA_SW1_6) << 5
+ | port_is_switch_on(TA_SW1_7) << 6
+ | port_is_switch_on(TA_SW1_8) << 7;
+
+
+ if(port_is_switch_on(TA_SW1_4) == S1_SWITCH_ON)
+ {
+ //display ON
+ enableLCD = TRUE;
+ }
+
+ if(enableLCD == TRUE)
+ {
+ spi_peripheral_init(1);
+ }
+ else
+ {
+ spi_peripheral_init(0);
+ }
+
Sleep(1000); //wait for LCD to power on
-#if (USING_LCD == 1)
- initLCD();
- memset(dataseq, 0x0, sizeof(dataseq));
- memcpy(dataseq, (const uint8 *) "DECAWAVE ", 16);
- writetoLCD( 40, 1, dataseq); //send some data
- memcpy(dataseq, (const uint8 *) SOFTWARE_VER_STRING, 16); // Also set at line #26 (Should make this from single value !!!)
- writetoLCD( 16, 1, dataseq); //send some data
-#endif
+ if(enableLCD == TRUE)
+ {
+ initLCD();
+
+ memset(dataseq, 0x0, sizeof(dataseq));
+ writetoLCD(1, 0, dataseq);
+ memcpy(dataseq, (const uint8 *) "GGRG UWB RANGING", LCD_BUFF_LEN);
+ writetoLCD(40, 1, dataseq); //send some data
+ memcpy(dataseq, (const uint8 *) SOFTWARE_VER_STRING, LCD_BUFF_LEN); // Also set at line #26 (TODO Should make this from single value !!!)
+ writetoLCD(16, 1, dataseq); //send some data
+ }
Sleep(1000);
@@ -332,248 +366,170 @@ int dw_main(void)
usb_init();
Sleep(1000);
usb_run();
- Sleep(10000);
+ Sleep(10000); //TODO remove this for the final build
#endif
- s1switch = port_is_boot1_on(0) << 1 // is_switch_on(TA_SW1_2) << 2
- | port_is_switch_on(TA_SW1_3) << 2
- | port_is_switch_on(TA_SW1_4) << 3
- | port_is_switch_on(TA_SW1_5) << 4
- | port_is_switch_on(TA_SW1_6) << 5
- | port_is_switch_on(TA_SW1_7) << 6
- | port_is_switch_on(TA_SW1_8) << 7;
-
-// uint8 s13 = port_is_switch_on(TA_SW1_3);
-// uint8 s14 = port_is_switch_on(TA_SW1_4);
-// uint8 s15 = port_is_switch_on(TA_SW1_5);
-// uint8 s16 = port_is_switch_on(TA_SW1_6);
-// uint8 s17 = port_is_switch_on(TA_SW1_7);
-// uint8 s18 = port_is_switch_on(TA_SW1_8);
-// uint8 debug_msg[8];
-// int n = sprintf((char*)&debug_msg[0], "s1switch: %d ", s1switch);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
- if(port_is_switch_on(TA_SW1_3) == S1_SWITCH_OFF)
- {
- int j = 1000000;
-#if (USING_LCD == 1)
- uint8 command;
- memset(dataseq, 0, LCD_BUFF_LEN);
-
- while(j--);
- command = 0x2 ; //return cursor home
- writetoLCD( 1, 0, &command);
+ //run DecaRanging application
- memcpy(dataseq, (const uint8 *) "DECAWAVE ", 12);
- writetoLCD( 40, 1, dataseq); //send some data
-#endif
-#ifdef USB_SUPPORT //this is set in the port.h file
- memcpy(dataseq, (const uint8 *) "USB to SPI ", 12);
-#else
-#endif
-#if (USING_LCD == 1)
- writetoLCD( 16, 1, dataseq); //send some data
-#endif
- j = 1000000;
+ led_off(LED_ALL);
- while(j--);
-#if (USING_LCD == 1)
- command = 0x2 ; //return cursor home
- writetoLCD( 1, 0, &command);
-#endif
#ifdef USB_SUPPORT //this is set in the port.h file
- // Do nothing in foreground -- allow USB application to run, I guess on the basis of USB interrupts?
- while (1) // loop forever
- {
- //usb_run();
- }
-#endif
- return 1;
- }
- else //run DecaRanging application
- {
-#if (USING_LCD == 1)
- uint8 dataseq[LCD_BUFF_LEN];
- uint8 command = 0x0;
-
- command = 0x2 ; //return cursor home
- writetoLCD( 1, 0, &command);
- memset(dataseq, ' ', LCD_BUFF_LEN);
- memcpy(dataseq, (const uint8 *) "DECAWAVE RANGE", 16);
- writetoLCD( 16, 1, dataseq); //send some data
+ usb_printconfig(16, (uint8 *)SOFTWARE_VER_STRING, s1switch);
#endif
- led_off(LED_ALL);
+ if(inittestapplication(s1switch) == (uint32)-1)
+ {
+ if(enableLCD == TRUE)
+ {
+ led_on(LED_ALL); //to display error....
+ dataseq[0] = 0x2 ; //return cursor home
+ writetoLCD(1, 0, &dataseq[0]);
+ memset(dataseq, ' ', LCD_BUFF_LEN);
+ memcpy(dataseq, (const uint8 *) "ERROR ", LCD_BUFF_LEN);
+ writetoLCD( LCD_BUFF_LEN, 1, dataseq); //send some data
+ memcpy(dataseq, (const uint8 *) "INIT FAIL ", LCD_BUFF_LEN);
+ writetoLCD( LCD_BUFF_LEN, 1, dataseq); //send some data
+ }
+ return 0; //error
+ }
-#ifdef USB_SUPPORT //this is set in the port.h file
- usb_printconfig(16, (uint8 *)SOFTWARE_VER_STRING, s1switch);
-#endif
+ //test EVB1000 - used in EVK1000 production
+ if((s1switch & SWS1_TXSPECT_MODE) == SWS1_TXSPECT_MODE) //to test TX power
+ {
+ //this function does not return!
+ configure_continuous_txspectrum_mode(s1switch);
+ }
- if(inittestapplication(s1switch) == (uint32)-1)
- {
-#if (USING_LCD == 1)
-
- led_on(LED_ALL); //to display error....
- dataseq[0] = 0x2 ; //return cursor home
- writetoLCD( 1, 0, &dataseq[0]);
- memset(dataseq, ' ', LCD_BUFF_LEN);
- memcpy(dataseq, (const uint8 *) "ERROR ", 12);
- writetoLCD( 40, 1, dataseq); //send some data
- memcpy(dataseq, (const uint8 *) " INIT FAIL ", 12);
- writetoLCD( 40, 1, dataseq); //send some data
-#endif
- return 0; //error
- }
+ //sleep for 5 seconds displaying "Decawave"
+ i=30;
+ while(i--)
+ {
+ if(enableLCD == TRUE)
+ {
+ if (i & 1) led_off(LED_ALL);
+ else led_on(LED_ALL);
+ }
+ Sleep(200);
+ }
+ i = 0;
+ led_off(LED_ALL);
- //test EVB1000 - used in EVK1000 production
- if((s1switch & SWS1_TXSPECT_MODE) == SWS1_TXSPECT_MODE) //to test TX power
- {
- //this function does not return!
- configure_continuous_txspectrum_mode(s1switch);
- }
+ if(enableLCD == TRUE)
+ {
+ command = 0x2 ; //return cursor home
+ writetoLCD( 1, 0, &command);
+ memset(dataseq, ' ', LCD_BUFF_LEN);
+ }
- //sleep for 5 seconds displaying "Decawave"
- i=30;
- while(i--)
- {
-#if (USING_LCD == 1)
- if (i & 1) led_off(LED_ALL);
- else led_on(LED_ALL);
-#endif
- Sleep(200);
- }
- i = 0;
- led_off(LED_ALL);
-#if (USING_LCD == 1)
- command = 0x2 ; //return cursor home
- writetoLCD( 1, 0, &command);
+// if(enableLCD == TRUE)
+// {
+//
+// memcpy(&dataseq[0], (const uint8 *) " DISCOVERY MODE ", LCD_BUFF_LEN);
+// writetoLCD(LCD_BUFF_LEN, 1, dataseq); //send some data
+// sprintf((char*)&dataseq[0], "%llX", instance_get_addr());
+// writetoLCD(LCD_BUFF_LEN, 1, dataseq); //send some data
+//
+// command = 0x2 ; //return cursor home
+// writetoLCD( 1, 0, &command);
+// }
- memset(dataseq, ' ', LCD_BUFF_LEN);
-#endif
- if(s1switch & SWS1_ANC_MODE)
- {
-// instance_mode = ANCHOR;
- led_on(LED_PC6);
- }
- else
- {
-// instance_mode = TAG;
- led_on(LED_PC7);
- }
-#if (USING_LCD == 1)
-// if(instance_mode == TAG)
-// {
- memcpy(&dataseq[0], (const uint8 *) " DISCOVERY MODE ", 16);
-
- writetoLCD( 40, 1, dataseq); //send some data
- sprintf((char*)&dataseq[0], "%llX", instance_get_addr());
- writetoLCD( 16, 1, dataseq); //send some data
-// }
-// else
-// {
-// memcpy(&dataseq[0], (const uint8 *) " AWAITING ", 16);
-// writetoLCD( 40, 1, dataseq); //send some data
-// memcpy(&dataseq[0], (const uint8 *) " POLL ", 16);
-// writetoLCD( 16, 1, dataseq); //send some data
-// }
-
- command = 0x2 ; //return cursor home
- writetoLCD( 1, 0, &command);
-#endif
+ if(enableLCD == TRUE)
+ {
+ memset(dataseq, ' ', LCD_BUFF_LEN);
+ memset(dataseq1, ' ', LCD_BUFF_LEN);
+ }
- }
+ port_EnableEXT_IRQ();
-#if (USING_LCD == 1)
- memset(dataseq, ' ', LCD_BUFF_LEN);
- memset(dataseq1, ' ', LCD_BUFF_LEN);
-#endif
+ //TODO remove
+ instance_data_t* inst1 = instance_get_local_structure_ptr(0);
+ inst1->testTimer = portGetTickCntMicro();
- port_EnableEXT_IRQ();
// main loop
while(1)
{
-
+ bool updateLCD = FALSE;
+ //TODO reenable optimization in the compiler settings!!!
instance_data_t* inst = instance_get_local_structure_ptr(0);
canSleep = instance_run(); //run the state machine!!!
instance_mode = inst->mode; //TODO modify how the rest of this works with DISCOVER, TAG, and ANCHOR!
if(instancenewrange())
{
- int n, rng, rng_raw;
- uint64 aaddr, taddr;
+ updateLCD = TRUE;
+// int n, rng, rng_raw;
+// uint64 aaddr, taddr;
// ranging = 1;
//send the new range information to LCD and/or USB
range_result = instance_get_idist(inst->newRangeUWBIndex);
-// uint8 debug_msg[200];
-// n = 0;
-// n = sprintf((char*)&debug_msg[0], "inst->newRangeUWBIndex %d", inst->newRangeUWBIndex);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
+ range_addr = instance_get_uwbaddr(inst->newRangeUWBIndex);
+
+
//set_rangeresult(range_result);
-#if (USING_LCD == 1)
+ if(enableLCD == TRUE)
+ {
#if (DELAY_CALIB_OUTPUT == 1)
- dataseq[0] = 0x2 ; //return cursor home
- writetoLCD( 1, 0, dataseq);
-
- dataseq[0] = 0x2 ; //return cursor home
- writetoLCD( 1, 0, dataseq);
- memset(dataseq, ' ', LCD_BUFF_LEN);
- memset(dataseq1, ' ', LCD_BUFF_LEN);
-
- int toggle_step = 5;
-
- if(toggle <= toggle_step)
- {
- sprintf((char*)&dataseq[0], "ADDRESS - SELF ");
- sprintf((char*)&dataseq1[0], "%llX", instance_get_addr());
- }
- else if(toggle <= toggle_step*2)
- {
- sprintf((char*)&dataseq[0], "RANGING WITH ");
- if(inst->mode == TAG)
- {
- sprintf((char*)&dataseq1[0], "%.3u ANCHORS ", instfindnumactiveuwbinlist(inst));
- }
- else if(inst->mode == ANCHOR)
- {
- sprintf((char*)&dataseq1[0], "%.3u TAGS ", instfindnumactiveuwbinlist(inst));
- }
- }
- else
- {
- sprintf((char*)&dataseq[0], "TX DELAY: %.5u ", inst->txAntennaDelay);
- sprintf((char*)&dataseq1[0], "RX DELAY: %.5u ", inst->rxAntennaDelay);
- }
- toggle++;
- if(toggle > toggle_step*3)
- {
- toggle = 0;
- }
-
- writetoLCD( 40, 1, dataseq); //send some data
- writetoLCD( 16, 1, dataseq1); //send some data
+ dataseq[0] = 0x2 ; //return cursor home
+ writetoLCD( 1, 0, dataseq);
+
+ dataseq[0] = 0x2 ; //return cursor home
+ writetoLCD( 1, 0, dataseq);
+ memset(dataseq, ' ', LCD_BUFF_LEN);
+ memset(dataseq1, ' ', LCD_BUFF_LEN);
+
+ toggle_step = 5;
+
+ if(toggle <= toggle_step)
+ {
+ sprintf((char*)&dataseq[0], "ADDRESS - SELF ");
+ sprintf((char*)&dataseq1[0], "%llX", instance_get_addr());
+ }
+ else if(toggle <= toggle_step*2)
+ {
+ sprintf((char*)&dataseq[0], "RANGING WITH ");
+ if(inst->mode == TAG)
+ {
+ sprintf((char*)&dataseq1[0], "%.3u ANCHORS ", instfindnumactiveuwbinlist(inst));
+ }
+ else if(inst->mode == ANCHOR)
+ {
+ sprintf((char*)&dataseq1[0], "%.3u TAGS ", instfindnumactiveuwbinlist(inst));
+ }
+ }
+ else
+ {
+ sprintf((char*)&dataseq[0], "TX DELAY: %.5u ", inst->txAntennaDelay);
+ sprintf((char*)&dataseq1[0], "RX DELAY: %.5u ", inst->rxAntennaDelay);
+ }
+ toggle++;
+ if(toggle > toggle_step*3)
+ {
+ toggle = 0;
+ }
+
+ writetoLCD( 40, 1, dataseq); //send some data
+ writetoLCD( 16, 1, dataseq1); //send some data
#else
- dataseq[0] = 0x2 ; //return cursor home
- writetoLCD( 1, 0, dataseq);
-
- memset(dataseq, ' ', LCD_BUFF_LEN);
- memset(dataseq1, ' ', LCD_BUFF_LEN);
-
- sprintf((char*)&dataseq[0], "LAST: %4.2f m", range_result);
- writetoLCD( 40, 1, dataseq); //send some data
- sprintf((char*)&dataseq1[0], "%llX", instance_get_uwbaddr(inst->newRangeUWBIndex));
- writetoLCD( 16, 1, dataseq1); //send some data
-#endif
+// dataseq[0] = 0x2 ; //return cursor home
+// writetoLCD( 1, 0, dataseq);
+//
+// memset(dataseq, ' ', LCD_BUFF_LEN);
+// memset(dataseq1, ' ', LCD_BUFF_LEN);
+//
+// sprintf((char*)&dataseq[0], "LAST: %4.2f m", range_result);
+// writetoLCD( 40, 1, dataseq); //send some data
+// sprintf((char*)&dataseq1[0], "%llX", instance_get_uwbaddr(inst->newRangeUWBIndex));
+// writetoLCD( 16, 1, dataseq1); //send some data
#endif
+ }
- aaddr = instancenewrangeancadd();
- taddr = instancenewrangetagadd();
- rng = (int) (range_result*1000);
- rng_raw = (int) (instance_get_idistraw(inst->newRangeUWBIndex)*1000);
+// aaddr = instancenewrangeancadd();
+// taddr = instancenewrangetagadd();
+// rng = (int) (range_result*1000);
+// rng_raw = (int) (instance_get_idistraw(inst->newRangeUWBIndex)*1000);
@@ -582,7 +538,11 @@ int dw_main(void)
if(instance_mode == TAG)
{
-// n = sprintf((char*)&dataseq[0], "RANGE_COMPLETE,%llX,%llX", taddr, aaddr);
+ uint64 aaddr = instancenewrangeancadd();
+ uint64 taddr = instancenewrangetagadd();
+ int n = sprintf((char*)&dataseq[0], "RANGE_COMPLETE,%llX,%llX", taddr, aaddr);
+ send_usbmessage(&dataseq[0], n);
+ usb_run();
}
else
@@ -610,90 +570,90 @@ int dw_main(void)
#endif
}
-#if (USING_LCD == 1)
- if(instanceisranging() == 0) //discovery/initialization mode for anchor and tag
- {
- if(instance_mode != ANCHOR)
- {
- if(instancesleeping())
- {
- dataseq[0] = 0x2 ; //return cursor home
- writetoLCD( 1, 0, dataseq);
- if(toggle)
- {
- toggle = 0;
- memcpy(&dataseq[0], (const uint8 *) " AWAITING ", 16);
- writetoLCD( 40, 1, dataseq); //send some data
- memcpy(&dataseq[0], (const uint8 *) " RESPONSE ", 16);
- writetoLCD( 16, 1, dataseq); //send some data
- }
- else
- {
- toggle = 1;
- memcpy(&dataseq[0], (const uint8 *) " TAG BLINK ", 16);
-
- writetoLCD( 40, 1, dataseq); //send some data
- sprintf((char*)&dataseq[0], "%llX", instance_get_addr());
- writetoLCD( 16, 1, dataseq); //send some data
- }
- }
+ if(enableLCD == TRUE)
+ {
+ toggle_step = 750;
+ memset(dataseq, ' ', LCD_BUFF_LEN);
+ memset(dataseq1, ' ', LCD_BUFF_LEN);
+ uint64 addr = instance_get_addr();
- if(instanceanchorwaiting() == 2)
- {
-// ranging = 1;
- dataseq[0] = 0x2 ; //return cursor home
- writetoLCD( 1, 0, dataseq);
- memcpy(&dataseq[0], (const uint8 *) " RANGING ", 16);
- writetoLCD( 40, 1, dataseq); //send some data
- memcpy(&dataseq[0], (const uint8 *) " STARTED ", 16);
- writetoLCD( 16, 1, dataseq); //send some data
- }
- }
- else //if(instance_mode == ANCHOR)
- {
+ uint8 num_neighbors = instfindnumactiveneighbors(inst);
+ uint8 num_hidden = instfindnumactivehidden(inst);
+ char status[10];
- if(instanceanchorwaiting())
- {
+ if(instance_mode == DISCOVERY)
+ {
+ strcpy(status, "SEARCHING");
+ range_addr = 0x0000;
+ range_result = 0;
+ }
+ else
+ {
+ strcpy(status, "CONNECTED");
+ }
+ dataseq[0] = 0x2 ; //return cursor home
+ writetoLCD( 1, 0, dataseq);
+ //TODO only update the display if something has changed!
+ if(toggle_counter <= toggle_step)
+ {
+ if(toggle == 2)
+ {
+ updateLCD = TRUE;
+ }
+ toggle = 1;
+ }
+ else if(toggle_counter <= toggle_step*2)
+ {
+ if(toggle == 1)
+ {
+ updateLCD = TRUE;
+ }
+ toggle = 2;
+ }
- toggle+=2;
+ toggle_counter++;
+ if(toggle_counter > toggle_step*2)
+ {
+ toggle_counter = 0;
+ }
- if(toggle > 300000)
- {
- dataseq[0] = 0x2 ; //return cursor home
- writetoLCD( 1, 0, dataseq);
- if(toggle & 0x1)
- {
- toggle = 0;
- memcpy(&dataseq[0], (const uint8 *) " AWAITING ", 16);
- writetoLCD( 40, 1, dataseq); //send some data
- memcpy(&dataseq[0], (const uint8 *) " POLL ", 16);
- writetoLCD( 16, 1, dataseq); //send some data
- }
- else
- {
- toggle = 1;
- memcpy(&dataseq[0], (const uint8 *) " DISCOVERY MODE ", 16);
- writetoLCD( 40, 1, dataseq); //send some data
- sprintf((char*)&dataseq[0], "%llX", instance_get_addr());
- writetoLCD( 16, 1, dataseq); //send some data
- }
- }
- }
- else if(instanceanchorwaiting() == 2)
- {
-
- dataseq[0] = 0x2 ; //return cursor home
- writetoLCD( 1, 0, dataseq);
- memcpy(&dataseq[0], (const uint8 *) " RANGING ", 16);
- writetoLCD( 40, 1, dataseq); //send some data
- memcpy(&dataseq[0], (const uint8 *) " STARTED ", 16);
- writetoLCD( 16, 1, dataseq); //send some data
- }
- }
- }
-#endif
+ //TODO only update the display if something has changed!
+ if(updateLCD == TRUE)
+ {
+ if(toggle == 1)
+ {
+ if(inst->addrByteSize == 8)
+ {
+ sprintf((char*)&dataseq[0], "%s ", status);
+ sprintf((char*)&dataseq1[0], "N:%02u H:%02u %05.2fm", num_neighbors, num_hidden, range_result);
+ }
+ else
+ {
+ sprintf((char*)&dataseq[0], "%llX %s", addr, status);
+ sprintf((char*)&dataseq1[0], "N:%02u %04llX:%05.2fm", num_neighbors, range_addr, range_result);
+ }
+ }
+ else //if(toggle == 2)
+ {
+ if(inst->addrByteSize == 8)
+ {
+ sprintf((char*)&dataseq[0], "%llX", addr);
+ sprintf((char*)&dataseq1[0], "N:%02u H:%02u %05.2fm", num_neighbors, num_hidden, range_result);
+ }
+ else
+ {
+ sprintf((char*)&dataseq[0], "%llX %s", addr, status);
+ sprintf((char*)&dataseq1[0], "H:%02u %04llX:%05.2fm", num_hidden, range_addr, range_result);
+ }
+
+ }
+
+ writetoLCD(40, 1, dataseq); //send some data
+ writetoLCD(16, 1, dataseq1); //send some data
+ }
+ }
#ifdef USB_SUPPORT //this is set in the port.h file
// usb_run();
diff --git a/src/application/instance.c b/src/application/instance.c
index 18529f727ed680ec0358e92940e88f6f3b2a4b0d..f2d5c674d79ccdfc0c20a16d41c32e154c76ae0a 100644
--- a/src/application/instance.c
+++ b/src/application/instance.c
@@ -186,23 +186,33 @@ void instanceconfigmessages(instance_data_t *inst)
memcpy(&inst->rng_initmsg.sourceAddr[0], &inst->eui64[0], inst->addrByteSize);
inst->rng_initmsg.messageData[FCODE] = RTLS_DEMO_MSG_RNG_INIT;
- //these bytes not used, zero them out.
- inst->rng_initmsg.messageData[RNG_INIT_TAG_SHORT_ADDR_LO] = 0x00;
- inst->rng_initmsg.messageData[RNG_INIT_TAG_SHORT_ADDR_HI] = 0x00;
- uint16 resp_dly_us, resp_dly;
- // First response delay to send is anchor's response delay.
- resp_dly_us = ANC_TURN_AROUND_TIME_US + inst->frameLengths_us[POLL];
- resp_dly = ((RESP_DLY_UNIT_US << RESP_DLY_UNIT_SHIFT) & RESP_DLY_UNIT_MASK)
- + ((resp_dly_us << RESP_DLY_VAL_SHIFT) & RESP_DLY_VAL_MASK);
- inst->rng_initmsg.messageData[RNG_INIT_ANC_RESP_DLY_LO] = resp_dly & 0xFF;
- inst->rng_initmsg.messageData[RNG_INIT_ANC_RESP_DLY_HI] = (resp_dly >> 8) & 0xFF;
- // Second response delay to send is tag's response delay.
- resp_dly_us = TAG_TURN_AROUND_TIME_US + inst->frameLengths_us[RESP];
- resp_dly = ((RESP_DLY_UNIT_US << RESP_DLY_UNIT_SHIFT) & RESP_DLY_UNIT_MASK)
- + ((resp_dly_us << RESP_DLY_VAL_SHIFT) & RESP_DLY_VAL_MASK);
- inst->rng_initmsg.messageData[RNG_INIT_TAG_RESP_DLY_LO] = resp_dly & 0xFF;
- inst->rng_initmsg.messageData[RNG_INIT_TAG_RESP_DLY_HI] = (resp_dly >> 8) & 0xFF;
+// //////////////////////////////////////////////////////////////////////////////
+// //TODO remove this
+//
+// //these bytes not used, zero them out.
+// inst->rng_initmsg.messageData[RNG_INIT_TAG_SHORT_ADDR_LO] = 0x00;
+// inst->rng_initmsg.messageData[RNG_INIT_TAG_SHORT_ADDR_HI] = 0x00;
+//
+//
+//
+// uint16 resp_dly_us, resp_dly;
+// // First response delay to send is anchor's response delay.
+// resp_dly_us = ANC_TURN_AROUND_TIME_US + inst->frameLengths_us[POLL];
+// resp_dly = ((RESP_DLY_UNIT_US << RESP_DLY_UNIT_SHIFT) & RESP_DLY_UNIT_MASK)
+// + ((resp_dly_us << RESP_DLY_VAL_SHIFT) & RESP_DLY_VAL_MASK);
+// inst->rng_initmsg.messageData[RNG_INIT_ANC_RESP_DLY_LO] = resp_dly & 0xFF; //TODO remove
+// inst->rng_initmsg.messageData[RNG_INIT_ANC_RESP_DLY_HI] = (resp_dly >> 8) & 0xFF; //TODO remove
+// // Second response delay to send is tag's response delay.
+// resp_dly_us = TAG_TURN_AROUND_TIME_US + inst->frameLengths_us[RESP];
+// resp_dly = ((RESP_DLY_UNIT_US << RESP_DLY_UNIT_SHIFT) & RESP_DLY_UNIT_MASK)
+// + ((resp_dly_us << RESP_DLY_VAL_SHIFT) & RESP_DLY_VAL_MASK);
+// inst->rng_initmsg.messageData[RNG_INIT_TAG_RESP_DLY_LO] = resp_dly & 0xFF;
+// inst->rng_initmsg.messageData[RNG_INIT_TAG_RESP_DLY_HI] = (resp_dly >> 8) & 0xFF;
+//
+// ////////////////////////////////////////////////////////////////////////////////////////
+
+
//configure INF message
uint16 broadcast_address = BROADCAST_ADDRESS;
@@ -302,7 +312,7 @@ const char* get_instanceModes_string(enum instanceModes mode)
switch (mode)
{
case DISCOVERY : return "DISCOVERY";
- case CONNECTION_PENDING : return "CONNECTION_PENDING";
+// case CONNECTION_PENDING : return "CONNECTION_PENDING";
case TAG : return "TAG";
case ANCHOR : return "ANCHOR";
case NUM_MODES : return "NUM_MODES";
@@ -424,13 +434,20 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
{
int done = INST_NOT_DONE_YET;
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "TAR");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
+//// uint16 time_now_tim3 = (uint16)portGetTIM3();
+// uint64 time_now_us = portGetTickCntMicro();
+// uint64 deltaT = get_dt64(inst->testTimer, time_now_us);
+// if(deltaT >= 500000)
+// {
+// uint8 debug_msg[200];
+// int n = sprintf((char*)&debug_msg[0], "%llu, %llu, %llu", inst->testTimer, time_now_us, deltaT);
+//// int n = sprintf((char*)&debug_msg[0], "xxx, %llu, %u", inst->testTimer, time_now_tim3);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+// inst->testTimer = time_now_us;
+// }
-// tdma_handler->set_slot(tdma_handler);
if(tdma_handler->slot_transition(tdma_handler))
{
message = 0;
@@ -444,13 +461,39 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
}
inst->lastState = inst->testAppState;
- send_statetousb(inst, tdma_handler);
+// send_statetousb(inst, tdma_handler);
switch (inst->testAppState)
{
case TA_INIT :
{
+ //TODO REMOVE test below
+// //test out how improved timestamping works!!!
+// uint64 tsu1 = portGetTickCntMicro();
+// uint32 tsm1 = portGetTickCnt();
+// int a = 0;
+// while(a < 130)
+// {
+// a = a + 1;
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "%i :", a);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+// }
+// uint64 tsu2 = portGetTickCntMicro();
+// uint32 tsm2 = portGetTickCnt();
+//
+// uint64 dtsu = tsu2 - tsu1;//systick counts down!!!
+// uint32 dtsm = tsm2 - tsm1;
+//
+//
+// int b = a;
+//
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "%llu %lu %i :", dtsu, dtsm, b);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
switch (inst->mode)
{
@@ -466,6 +509,18 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->uwbShortAdd = inst->eui64[0] + (inst->eui64[1] << 8);//TODO use a hashing algorithm
+// RX CB RX: DWT_SIG_RX_OKAY-RTLS_DEMO_MSG_TAG_POLL,4492,xxxx
+// while(TRUE){
+// uint8 debug_msg[100];
+// uint64 my_addr = inst->uwbShortAdd;
+// char *msg = "RTLS_DEMO_MSG_TAG_POLL";
+// int n = sprintf((char*)&debug_msg[0], "RX CB RX: DWT_SIG_RX_OKAY-%s,%llu,xxxx", msg, my_addr);
+//// int n = sprintf((char*)&debug_msg[0], "ADDR,%llu", my_addr);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+// Sleep(100);
+// }
+
#if (USING_64BIT_ADDR==0)
dwt_setaddress16(inst->uwbShortAdd);
memcpy(&inst->uwbList[0][0], &inst->uwbShortAdd, inst->addrByteSize);
@@ -500,6 +555,52 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->canPrintInfo = 1;
inst->wait4ack = 0;
+// uint16 resp_dly_us, resp_dly;
+// uint32 final_reply_delay_us;
+//
+// // First response delay to send is anchor's response delay.
+// resp_dly_us = ANC_TURN_AROUND_TIME_US + inst->frameLengths_us[POLL];
+// resp_dly = ((RESP_DLY_UNIT_US << RESP_DLY_UNIT_SHIFT) & RESP_DLY_UNIT_MASK)
+// + ((resp_dly_us << RESP_DLY_VAL_SHIFT) & RESP_DLY_VAL_MASK);
+// inst->resp_dly[RESP_DLY_ANC] = resp_dly & 0xFFFF; //TODO make sure that we only want the 16 lowest bits
+//
+//
+// // Second response delay to send is tag's response delay.
+// // Get response delays from message and update internal timings accordingly
+// resp_dly_us = TAG_TURN_AROUND_TIME_US + inst->frameLengths_us[RESP];
+// resp_dly = ((RESP_DLY_UNIT_US << RESP_DLY_UNIT_SHIFT) & RESP_DLY_UNIT_MASK)
+// + ((resp_dly_us << RESP_DLY_VAL_SHIFT) & RESP_DLY_VAL_MASK);
+// inst->resp_dly[RESP_DLY_TAG] = resp_dly & 0xFFFF; //TODO make sure that we only want the 16 lowest bits
+//
+//
+// for (int i = 0; i < RESP_DLY_NB; i++)
+// {
+// if (((inst->resp_dly[i] & RESP_DLY_UNIT_MASK) >> RESP_DLY_UNIT_SHIFT) == RESP_DLY_UNIT_MS)
+// {
+// // Remove unit bit and convert to microseconds.
+// inst->resp_dly[i] &= ~RESP_DLY_UNIT_MASK;
+// inst->resp_dly[i] *= 1000;
+// }
+// }
+//
+// // Update delay between poll transmission and response reception.
+// // Use uint64 for resp_dly here to avoid overflows if it is more than 400 ms.
+// inst->txToRxDelayTag_sy = US_TO_SY_INT((uint64)inst->resp_dly[RESP_DLY_ANC] - inst->frameLengths_us[POLL]) - RX_START_UP_SY;
+//
+// // Update delay between poll transmission and final transmission.
+// final_reply_delay_us = inst->resp_dly[RESP_DLY_ANC] + inst->resp_dly[RESP_DLY_TAG];
+// inst->finalReplyDelay = convertmicrosectodevicetimeu(final_reply_delay_us);
+// inst->finalReplyDelay_ms = CEIL_DIV(final_reply_delay_us, 1000);
+
+ // If we are using long response delays, deactivate sleep.
+ if (inst->resp_dly[RESP_DLY_ANC] >= LONG_RESP_DLY_LIMIT_US
+ || inst->resp_dly[RESP_DLY_TAG] >= LONG_RESP_DLY_LIMIT_US)
+ {
+ inst->sleepingEabled = 0;
+ }
+
+
+
}
break;
default:
@@ -627,45 +728,24 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
}
case TA_TXBLINK_WAIT_SEND :
{
- // if (inst->mode == ANCHOR)
- // {
- // send_statetousb(inst);
- // }
-
int flength = (BLINK_FRAME_CRTL_AND_ADDRESS + FRAME_CRC);
// //blink frames with IEEE EUI-64 tag ID
-// inst->blinkmsg.frameCtrl = 0xC5 ;
inst->blinkmsg.seqNum = inst->frameSN++;
- dwt_writetxdata(flength, (uint8 *) (&inst->blinkmsg), 0) ; // write the frame data
- dwt_writetxfctrl(flength, 0, 1);
-
- //using wait for response to do delayed receive
- inst->wait4ack = DWT_RESPONSE_EXPECTED;
+ //using wait for response to do delayed receive
+ inst->wait4ack = DWT_RESPONSE_EXPECTED;
- //TODO fix!!! remove *0
- dwt_setrxtimeout((uint16)inst->fwtoTimeB_sy*0); //units are symbols
- //set the delayed rx on time (the ranging init will be sent after this delay)
- dwt_setrxaftertxdelay((uint32)inst->rnginitW4Rdelay_sy*0); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
+ dwt_setrxtimeout((uint16)inst->fwtoTimeB_sy*0); //units are symbols
+ //set the delayed rx on time (the ranging init will be sent after this delay)
+ dwt_setrxaftertxdelay((uint32)inst->rnginitW4Rdelay_sy*0); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
- int tx_start = dwt_starttx(DWT_START_TX_IMMEDIATE | inst->wait4ack); //always using immediate TX and enable delayed RX
-
- if(tx_start == 0)
- {
-// uint32 deltat;
-// uint8 debug_msg[100];
-// if(inst->blink0range1 == 1){
-// deltat = portGetTickCnt() - inst->range_start;
-// sprintf((char *)&debug_msg, "range time: %u ", (unsigned int)deltat);
-// }
-// else{
-// deltat = portGetTickCnt() - inst->blink_start;
-// sprintf((char *)&debug_msg, "blink time: %u ", (unsigned int)deltat);
-// }
-// send_txmsgtousb((char *)&debug_msg);
-
+ dwt_writetxdata(flength, (uint8 *) (&inst->blinkmsg), 0) ; // write the frame data
+ dwt_writetxfctrl(flength, 0, 1);
+
+ if(dwt_starttx(DWT_START_TX_IMMEDIATE | inst->wait4ack) == 0) //always using immediate TX and enable delayed RX
+ {
uint8 debug_msg[100];
int n = sprintf((char *)&debug_msg, "TX_BLINK,%llX,NULL", instance_get_addr());
send_usbmessage(&debug_msg[0], n);
@@ -673,20 +753,15 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->blink0range1 = 0;
inst->blink_start = portGetTickCnt();
-// send_txmsgtousb("BLINK-SUCCESS");
inst->timeofTx = portGetTickCnt();
tdma_handler->last_blink_time = portGetTickCnt();
}
- else
- {
-// send_txmsgtousb("BLINK-ERROR");
- }
inst->goToSleep = 1; //go to Sleep after this blink
- inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation
- inst->previousState = TA_TXBLINK_WAIT_SEND ;
+ inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation //TODO move this into the if statement above?
+ inst->previousState = TA_TXBLINK_WAIT_SEND ; //TODO move this into the ...
done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
}
@@ -694,12 +769,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
case TA_TXRANGINGINIT_WAIT_SEND :
{
- // if (inst->mode == ANCHOR)
- // {
- // send_statetousb(inst);
- // }
-// send_txmsgtousb("RTLS_DEMO_MSG_RNG_INIT");
-
int psduLength = RANGINGINIT_MSG_LEN;
#if (USING_64BIT_ADDR == 1)
@@ -715,45 +784,73 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
inst->previousState = TA_TXRANGINGINIT_WAIT_SEND ;
+
dwt_writetxdata(psduLength, (uint8 *) &inst->rng_initmsg, 0) ; // write the frame data
- //anchor - we don't use timeout, just wait for next frame
- if(instancesendpacket(psduLength, DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED, inst->delayedReplyTime))
- {
- dwt_setrxaftertxdelay(0);
- inst->testAppState = TA_RXE_WAIT ; // wait to receive a new blink or poll message
- inst->wait4ack = 0; //clear the flag as the TX has failed the TRX is off
- inst->lateTX++;
- uint8 debug_msg[100];
- sprintf((char *)&debug_msg, "RTLS_DEMO_MSG_RNG_INIT -> late tx");
- send_txmsgtousb((char *)&debug_msg);
- usb_run();
- }
- else
- {
+ //anchor - we don't use timeout, just wait for next frame
+ if(instancesendpacket(psduLength, DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED, inst->delayedReplyTime))
+ {
+ dwt_setrxaftertxdelay(0);
+ inst->testAppState = TA_RXE_WAIT ; // wait to receive a new blink or poll message
+ inst->wait4ack = 0; //clear the flag as the TX has failed the TRX is off
+ inst->lateTX++;
+// uint8 debug_msg[100];
+// sprintf((char *)&debug_msg, "RTLS_DEMO_MSG_RNG_INIT -> late tx");
+// send_txmsgtousb((char *)&debug_msg);
+// usb_run();
+ }
+ else
+ {
+
+ inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation
+ inst->previousState = TA_TXRANGINGINIT_WAIT_SEND ;
+ done = INST_DONE_WAIT_FOR_NEXT_EVENT; //no timeout
+
+ uint32 time_now = portGetTickCnt();
+ inst->timeofTx = time_now;
+ tdma_handler->set_discovery_mode(tdma_handler, WAIT_INF_INIT, time_now);
+ //inst->monitor = 1;
+ }
+
+
+
+
+
+// dwt_setrxtimeout((uint16)0);//no timeout (keep RX on until instructed otherwise)
+// dwt_setrxaftertxdelay((uint32)0);//immediately go to rx after tx
- inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation
- inst->previousState = TA_TXRANGINGINIT_WAIT_SEND ;
- done = INST_DONE_WAIT_FOR_NEXT_EVENT; //no timeout
-
- uint32 time_now = portGetTickCnt();
- inst->timeofTx = time_now;
-// tdma_handler->waitForInf = TRUE;
-// tdma_handler->discovery_mode = WAIT_INF_INIT;
- tdma_handler->set_discovery_mode(tdma_handler, WAIT_INF_INIT, time_now);
-// uint8 debug_msg[100];
-// sprintf((char *)&debug_msg, "RTLS_DEMO_MSG_RNG_INIT -> success!!!");
+
+// dwt_writetxdata(psduLength, (uint8 *) &inst->rng_initmsg, 0) ; // write the frame data
+// dwt_writetxfctrl(psduLength, 0, 1);
+// if(dwt_starttx(DWT_START_TX_IMMEDIATE | inst->wait4ack) == 0) //always using immediate TX and enable delayed RX
+// {
+// inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation
+// inst->previousState = TA_TXRANGINGINIT_WAIT_SEND ;
+// done = INST_DONE_WAIT_FOR_NEXT_EVENT; //no timeout
+//
+// uint32 time_now = portGetTickCnt();
+// inst->timeofTx = time_now;
+// tdma_handler->set_discovery_mode(tdma_handler, WAIT_INF_INIT, time_now);
+// }
+// else
+// {
+// inst->testAppState = TA_RXE_WAIT ; // wait to receive a new blink or poll message
+// inst->wait4ack = 0; //clear the flag as the TX has failed the TRX is off
+// uint8 debug_msg[100];
+// sprintf((char *)&debug_msg, "RTLS_DEMO_MSG_RNG_INIT -> TX ERROR");
// send_txmsgtousb((char *)&debug_msg);
// usb_run();
- //inst->monitor = 1;
- }
+// }
+
+
+
+
break;
}
case TA_TXINF_WAIT_SEND :
{
//NOTE: handles INF_SUG, INF_INIT, INF_UPDATE, and INF_REG
-
int psduLength = tdma_handler->infMessageLength; //TODO check all instances of this and dont send messages with zero psduLength! Will cause the UWB to lockup!
inst->inf_msg.seqNum = inst->frameSN++;
@@ -762,47 +859,38 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
tdma_handler->update_inf_tsfs(tdma_handler);
- //would be best to store the results while populating the inf message
- //fcode + framelength + time since frame start + num_neighbors + 3*num_neightbors + 2*framelength
-// int num_neighbors = instfindnumactiveneighbors(inst);
-// int inf_msg_len = 1 + 1 + 4 + 1 + 3*num_neighbors + 2*tdma_handler->uwbListTDMAInfo[0].framelength;
-
-//#if (USING_64BIT_ADDR==1)
-//// psduLength = TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
-// psduLength = inf_msg_len + FRAME_CRTL_AND_ADDRESS_LS + FRAME_CRC;
-//#else
-//// psduLength = TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
-// psduLength = inf_msg_len + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
-//#endif
-
- //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*2); //TODO fix this! remove *2
-
- dwt_writetxdata(psduLength, (uint8 *) &inst->inf_msg, 0) ; // write the frame data
-
//response is not expected
inst->wait4ack = 0;
-// uint32 dt = get_dt32(inst->buildFrameTime, portGetTickCnt());
+ //get the message FCODE
+ uint8 fcode;
+ memcpy(&fcode, &inst->inf_msg.messageData[FCODE], sizeof(uint8));
+ dwt_writetxdata(psduLength, (uint8 *) &inst->inf_msg, 0) ; // write the frame data
dwt_writetxfctrl(psduLength, 0, 1);
- if(dwt_starttx(DWT_START_TX_IMMEDIATE) == 0){
- uint8 debug_msg[100];
- int n = sprintf((char *)&debug_msg, "TX_INF - success!");//,%llX, psdu: %d ", instance_get_addr(), psduLength);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
-
+ if(dwt_starttx(DWT_START_TX_IMMEDIATE | inst->wait4ack) == 0)
+ {
//if we successfully send out INF_INIT, INF_SUG, or INF_UPDATE, switch to INF_REG
- //get the message FCODE
- uint8 fcode;
- memcpy(&fcode, &inst->inf_msg.messageData[FCODE], sizeof(uint8));
-
-
if(fcode == RTLS_DEMO_MSG_INF_SUG)
{
inst->nextState = TA_RXE_WAIT;
+ tdma_handler->set_discovery_mode(tdma_handler, EXIT, portGetTickCnt());
+ inst->mode = ANCHOR;
+
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "TX_INF_SUG,%llX,NULL", instance_get_addr());
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+// //turn on RX after sending INF_SUG
+// //set the delayed rx on time (the response message will be sent after this delay)
+// dwt_setrxaftertxdelay(inst->txToRxDelayTag_sy); //TODO fix this! remove *0 //TODO will this mess up TX callback??
+// dwt_setrxtimeout((uint16)inst->fwtoTime_sy); //TODO fix this! remove *2
+
+ //turn on RX after sending INF_SUG
+ dwt_setrxtimeout((uint16)0);//no timeout (keep RX on until instructed otherwise)
+ dwt_setrxaftertxdelay((uint32)0);//immediately go to rx after tx
}
else
{
@@ -817,108 +905,35 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
fcode = RTLS_DEMO_MSG_INF_REG;
memcpy(&inst->inf_msg.messageData[FCODE], &fcode, sizeof(uint8));
}
+
+ inst->timeofTx = portGetTickCnt();
+ inst->testAppState = TA_TX_WAIT_CONF ; //TODO should only do this if we don't have a problem with the INF send...
+ inst->previousState = TA_TXINF_WAIT_SEND ;
+ done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
+
}
else
{
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "TX_INF - fail!");//,%llX, psdu: %d ", instance_get_addr(), psduLength);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
+ if(fcode == RTLS_DEMO_MSG_INF_SUG)
+ {
+ //TODO think of a way to keep trying to send INF_SUG
+ tdma_handler->set_discovery_mode(tdma_handler, WAIT_SEND_SUG, portGetTickCnt());
+ inst->nextState = TA_RXE_WAIT;
+ }
}
- inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation
- inst->previousState = TA_TXINF_WAIT_SEND ;
- done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
-
- inst->timeofTx = portGetTickCnt();
-
break;
}
-// //TODO fold TA_TXSUG_WAIT_SEND into INF_WAIT_SEND if possible
-// case TA_TXSUG_WAIT_SEND : //TODO need a timeout from this avenue of operations
-// {
-//// send_txmsgtousb("TA_TXSUG_WAIT_SEND");
-//// uint8 debug_msg[100];
-//// int n = sprintf((char *)&debug_msg, "TA_TXSUG_WAIT_SEND!!!");
-//// send_usbmessage(&debug_msg[0], n);
-//// usb_run();
-//
-// int psduLength = 0;
-//
-// inst->inf_msg.seqNum = inst->frameSN++;
-//
-// //TODO include some defines
-// //fcode + framelength + time since frame start + num_neighbors + 3*num_neightbors + 2*framelength
-// int num_neighbors = instfindnumactiveneighbors(inst);
-// int inf_msg_len = 1 + 1 + 4 + 1 + 3*num_neighbors + 2*tdma_handler->uwbListTDMAInfo[0].framelength;
-//
-//#if (USING_64BIT_ADDR==1)
-//// psduLength = TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
-// psduLength = inf_msg_len + FRAME_CRTL_AND_ADDRESS_LS + FRAME_CRC;
-//#else
-//// psduLength = TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
-// psduLength = inf_msg_len + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
-//#endif
-//
-// //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*2); //TODO fix this! remove *2
-//
-// dwt_writetxdata(psduLength, (uint8 *) &inst->inf_msg, 0) ; // write the frame data
-//
-// //response is not expected
-// inst->wait4ack = 0;
-//
-//// uint32 dt = get_dt32(inst->buildFrameTime, portGetTickCnt());
-//
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "TX_INF_SUG, %llX", instance_get_addr());
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-//
-// dwt_writetxfctrl(psduLength, 0, 1);
-// if(dwt_starttx(DWT_START_TX_IMMEDIATE) == 0){
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "TX_INF_SUG, success");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// }
-// else
-// {
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "TX_INF_SUG, fail");//,%llX, psdu: %d ", instance_get_addr(), psduLength);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// }
-//
-//// inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation
-// inst->previousState = TA_TXSUG_WAIT_SEND ;
-//// done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
-//
-//// inst->timeofTx = portGetTickCnt();
-//
-// break;
-// }
case TA_TXPOLL_WAIT_SEND :
{
-// uint8 debug_msg[100];
-// sprintf((char *)&debug_msg, "RTLS_DEMO_MSG_TAG_POLL -> uwb %i ", inst->uwbToRangeWith);
-// send_txmsgtousb((char *)&debug_msg);
-
- //if here, we have already sent RTLS_DEMO_MSG_INF_INIT
-// inst->inf_msg.messageData[FCODE] = RTLS_DEMO_MSG_INF_SUG; //TODO make sure this is right
-
int psduLength = 0;
inst->goToSleep = 1; //go to Sleep after this poll
-// inst->msg[inst->uwbToRangeWith].seqNum = inst->frameSN++;
-// inst->msg[inst->uwbToRangeWith].messageData[FCODE] = RTLS_DEMO_MSG_TAG_POLL; //message function code (specifies if message is a poll, response or other...)
inst->msg.seqNum = inst->frameSN++;
inst->msg.messageData[FCODE] = RTLS_DEMO_MSG_TAG_POLL; //message function code (specifies if message is a poll, response or other...)
- //TODO check where I am setting the destination address
-// instanceconfigframeheader(inst);
+ memcpy(&inst->msg.destAddr[0], &inst->uwbList[inst->uwbToRangeWith], inst->addrByteSize);
#if (USING_64BIT_ADDR==1)
psduLength = TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
@@ -926,79 +941,27 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
psduLength = TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
#endif
-
-
-// for(int d = 1; d < 1000; d++){
-//// psduLength++;
-// uint8 dummy_data = 1;
-// //add some dummy data to the poll to see how it affects timing
-// memcpy(&inst->msg[inst->uwbToRangeWith].messageData[d], &dummy_data, 1);
-//
-// }
-
-
//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*2); //TODO fix this! remove *2
-
-// dwt_writetxdata(psduLength, (uint8 *) &inst->msg[inst->uwbToRangeWith], 0) ; // write the frame data
- dwt_writetxdata(psduLength, (uint8 *) &inst->msg, 0) ; // write the frame data
+ dwt_setrxtimeout((uint16)inst->fwtoTime_sy*0); //TODO fix this! remove *2
//response is expected
inst->wait4ack = DWT_RESPONSE_EXPECTED;
+ 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){
-// uint32 time_now = portGetTickCnt();
uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "TX_POLL,%llX,%llX,%lu", instance_get_addr(), instance_get_uwbaddr(inst->uwbToRangeWith),time_now);
int n = sprintf((char *)&debug_msg, "TX_POLL,%llX,%llX", instance_get_addr(), instance_get_uwbaddr(inst->uwbToRangeWith));
send_usbmessage(&debug_msg[0], n);
usb_run();
-
-// uint8 buffer[SYS_STATUS_LEN];
-// uint64 sys_status_reg;
-// dwt_readfromdevice(SYS_STATUS_ID,0,SYS_STATUS_LEN,buffer);
-//
-// for (int j = SYS_STATUS_LEN-1 ; j >= 0 ; j --)
-// {
-// sys_status_reg = (sys_status_reg << 8) + buffer[j] ;
-// }
-//
-// uint64 sys_state_reg;
-// dwt_readfromdevice(SYS_STATE_ID,0,SYS_STATE_LEN,buffer);
-//
-// for (int j = SYS_STATE_LEN-1 ; j >= 0 ; j --)
-// {
-// sys_state_reg = (sys_state_reg << 8) + buffer[j] ;
-// }
-
-// debug_msg[100];
-// n = sprintf((char *)&debug_msg, "SYS_STATUS: %llu SYS_STATE: %llu", sys_status_reg, sys_state_reg);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
}
- inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation
- inst->previousState = TA_TXPOLL_WAIT_SEND ;
+ inst->testAppState = TA_TX_WAIT_CONF ; //TODO move to if statement above? // wait confirmation
+ inst->previousState = TA_TXPOLL_WAIT_SEND ; //TODO move to ...
done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
-// uint32 deltat;
-//// uint8 debug_msg[100];
-// if(inst->blink0range1 == 1){
-// deltat = portGetTickCnt() - inst->range_start;
-// sprintf((char *)&debug_msg, "range time: %u ", (unsigned int)deltat);
-// }
-// else{
-// deltat = portGetTickCnt() - inst->blink_start;
-// sprintf((char *)&debug_msg, "blink time: %u ", (unsigned int)deltat);
-// }
-// send_txmsgtousb((char *)&debug_msg);
-
-
-// inst->uwbToRangeWith = 255; //let the rx_scheduler reselect the uwb
- inst->blink0range1 = 1;
+ inst->blink0range1 = 1; //TODO check if this is still needed...
inst->range_start = portGetTickCnt();
@@ -1008,27 +971,19 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
}
case TA_TXRESPONSE_WAIT_SEND : //the frame is loaded and sent from the RX callback
{
- // if(inst->mode == ANCHOR)
- // {
- // send_statetousb(inst);
- // }
-
inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
inst->previousState = TA_TXRESPONSE_WAIT_SEND ;
-
+
break;
}
case TA_TXFINAL_WAIT_SEND :
{
dwt_setrxaftertxdelay((uint16)0);
- dwt_setrxtimeout((uint16)0);
-
+ dwt_setrxtimeout((uint16)0);
- int psduLength = 0;
+ int psduLength = 0;
// Embbed into Final message:40-bit respRxTime
// Write Response RX time field of Final message
-// memcpy(&(inst->msg[inst->uwbToRangeWith].messageData[RRXT]), (uint8 *)&inst->anchorRespRxTime, 5);
-// inst->msg[inst->uwbToRangeWith].messageData[FCODE] = RTLS_DEMO_MSG_TAG_FINAL; //message function code (specifies if message is a poll, response or other...)
memcpy(&(inst->msg.messageData[RRXT]), (uint8 *)&inst->anchorRespRxTime, 5);
inst->msg.messageData[FCODE] = RTLS_DEMO_MSG_TAG_FINAL; //message function code (specifies if message is a poll, response or other...)
@@ -1038,46 +993,51 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
psduLength = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
#endif
-// dwt_writetxdata(psduLength, (uint8 *) &inst->msg[inst->uwbToRangeWith], 0) ; // write the frame data
dwt_writetxdata(psduLength, (uint8 *) &inst->msg, 0) ; // write the frame data
+ inst->wait4ack = DWT_RESPONSE_EXPECTED;
-// uint32 response_time = portGetTickCnt() - inst->range_start;
-
-// uint16 reg = dwt_read16bitoffsetreg(PMSC_ID, 0) ;
-// reg |= 0x20 ; // set bit 5 high
-// dwt_write16bitoffsetreg(PMSC_ID, 0, reg) ;
-
- //response is expected
-// inst->wait4ack = DWT_RESPONSE_EXPECTED;
- inst->wait4ack = 0;
-// dwt_writetxfctrl(psduLength, 0, 1); //TODO may need this...
-
-// if(instancesendpacket(psduLength, DWT_START_TX_DELAYED, inst->delayedReplyTime))
- if(instancesendpacket(psduLength, DWT_START_TX_DELAYED | inst->wait4ack, inst->delayedReplyTime))
+ if(instancesendpacket(psduLength, DWT_START_TX_DELAYED | inst->wait4ack, inst->delayedReplyTime))
{
- // initiate the re-transmission
- inst->testAppState = TA_TXE_WAIT ;
- //inst->nextState = TA_TXPOLL_WAIT_SEND ; //TODO should this go to TX_SELECT instead?
-// inst->testAppState = TA_TX_SELECT; //TODO reset next and previous?
- inst->nextState = TA_TX_SELECT; //TODO reset next and previous?
+// // initiate the re-transmission
+// inst->testAppState = TA_TXE_WAIT ;
+// //inst->nextState = TA_TXPOLL_WAIT_SEND ; //TODO should this go to TX_SELECT instead?
+//// inst->testAppState = TA_TX_SELECT; //TODO reset next and previous?
+// inst->nextState = TA_TX_SELECT; //TODO reset next and previous?
+
+ inst->testAppState = TA_RXE_WAIT;
inst->wait4ack = 0; //clear the flag as the TX has failed the TRX is off
inst->lateTX++;
-
uint8 debug_msg[100];
- sprintf((char *)&debug_msg, "RTLS_DEMO_MSG_TAG_FINAL -> late tx");
+ sprintf((char *)&debug_msg, "RTLS_DEMO_MSG_TAG_FINAL->late tx,%X,xxxx",inst->uwbShortAdd);
send_txmsgtousb((char *)&debug_msg);
usb_run();
-// int n = sprintf((char*)&debug_msg[0], "time_now: %lu, replytime: %lu", dwt_readsystimestamphi32(), inst->delayedReplyTime);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
+// uint32 dt = 0;
+//
+// uint32 dwt_time_now = dwt_readsystimestamphi32();
+//
+// if(inst->delayedReplyTime > dwt_time_now)
+// {
+// dt = dwt_getdt(dwt_time_now, inst->delayedReplyTime);
+//
+// }
+// else
+// {
+// dt = dwt_getdt(inst->delayedReplyTime, dwt_time_now);
+// }
+//
+// double dtf = convertdevicetimetosec(dt);
+// int n = sprintf((char*)&debug_msg[0], "time_now: %lu, replytime: %lu, dts: %f", dwt_time_now, inst->delayedReplyTime, dtf);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
}
else
{
-
+
inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
inst->previousState = TA_TXFINAL_WAIT_SEND;
done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
@@ -1095,12 +1055,11 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
}
case TA_TXREPORT_WAIT_SEND :
{
-
//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*2); //TODO fix this! remove *2
- dwt_setrxaftertxdelay((uint16)0);
- dwt_setrxtimeout((uint16)0);
+// dwt_setrxaftertxdelay((uint16)0);
+// dwt_setrxtimeout((uint16)0);
// dwt_writetxfctrl(psduLength, 0, 1);
// if(dwt_starttx(DWT_START_TX_IMMEDIATE) == 0){
@@ -1115,45 +1074,36 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
// done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
//
// inst->timeofTx = portGetTickCnt();
-//
-
int psduLength = 0;
inst->report_msg.seqNum = inst->frameSN++;
#if (USING_64BIT_ADDR==1)
- psduLength = RNG_REPORT_MSG_LEN + FRAME_CRTL_AND_ADDRESS_LS + FRAME_CRC;
+ psduLength = RNG_REPORT_MSG_LEN_LONG + FRAME_CRTL_AND_ADDRESS_LS + FRAME_CRC;
#else
- psduLength = RNG_REPORT_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
+ psduLength = RNG_REPORT_MSG_LEN_SHORT + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
#endif
-
-
//TODO if tof not zero, set tof, else memset 0
// Write calculated TOF into response message
//TODO #define messageData offsets
// memcpy(&inst->msg[inst->uwbToRangeWith].messageData[1], &inst->tof[inst->uwbToRangeWith], 6); //TODO fix number of bytes...
// dwt_writetxdata(psduLength, (uint8 *) &inst->msg[inst->uwbToRangeWith], 0) ; // write the frame data
- memcpy(&inst->report_msg.messageData[1], &inst->tof[inst->uwbToRangeWith], 6); //TODO fix number of bytes...
+ memcpy(&inst->report_msg.messageData[REPORT_TOF], &inst->tof[inst->uwbToRangeWith], 6); //TODO fix number of bytes...
+ memcpy(&inst->report_msg.messageData[REPORT_ADDR], &inst->uwbList[inst->uwbToRangeWith], inst->addrByteSize);
dwt_writetxdata(psduLength, (uint8 *) &inst->report_msg, 0) ; // write the frame data
inst->wait4ack = 0;
if(instancesendpacket(psduLength, DWT_START_RX_IMMEDIATE, 0))
{
- // initiate the re-transmission
- inst->testAppState = TA_TXE_WAIT ;
- inst->nextState = TA_RXE_WAIT; //TODO reset next and previous?
-
- inst->wait4ack = 0; //clear the flag as the TX has failed the TRX is off
- inst->lateTX++;
-
+ inst->testAppState = TA_RXE_WAIT;
}
else
{
// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "TX_RNG_REPORT");
+// int n = sprintf((char *)&debug_msg, "TX_RNG_REPORT,xxxx,%llX",instance_get_addr());
// send_usbmessage(&debug_msg[0], n);
// usb_run();
@@ -1163,7 +1113,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->timeofTx = portGetTickCnt();
inst->monitor = 1;
-
}
break;
@@ -1172,10 +1121,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
{
//TODO look at this state again. it seems to sometimes be problematic
- // if (inst->mode == ANCHOR)
- // {
-// send_statetousb(inst);
- // }
event_data_t* dw_event = instance_getevent(11); //get and clear this event
//NOTE: Can get the ACK before the TX confirm event for the frame requesting the ACK
@@ -1228,7 +1173,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
// inst->gotTO = 1;
// }
// }
-//
+
if(inst->gotTO == 0)
{
break;
@@ -1259,11 +1204,8 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
break;
}
- else if(inst->previousState == TA_TXFINAL_WAIT_SEND) //TAG operations
+ else if(inst->previousState == TA_TXFINAL_WAIT_SEND)
{
-
-// inst->testAppState = TA_TXE_WAIT ;
-// inst->nextState = TA_TX_SELECT;
inst->testAppState = TA_RXE_WAIT;
break;
@@ -1337,7 +1279,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->wait4ack = 0 ; //clear the flag, the next time we want to turn the RX on it might not be auto
}
-
//we are going to use anchor/tag timeout
done = INST_DONE_WAIT_FOR_NEXT_EVENT; //using RX FWTO
@@ -1352,18 +1293,11 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
}
case TA_RX_WAIT_DATA :
{
- // if (inst->mode == ANCHOR)
- // {
- // send_statetousb(inst);
- // }
-
-
// Wait RX data
switch (message)
{
case DWT_SIG_RX_BLINK :
{
-// send_rxmsgtousb("RX process: DWT_SIG_RX_BLINK ");
event_data_t* dw_event = instance_getevent(12); //get and clear this event
// if(inst->mode == ANCHOR)
@@ -1435,14 +1369,14 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
//NOTE: WAIT_RNG_INIT checked in RX callback
// send_rxmsgtousb("RX process: DWT_SIG_RX_OKAY-RTLS_DEMO_MSG_RNG_INIT");
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "BLINK_COMPLETE,%llX,%llX", 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, "BLINK_COMPLETE,%llX,%llX", instance_get_addr(), instance_get_uwbaddr(inst->uwbToRangeWith));
+ send_usbmessage(&debug_msg[0], n);
+ usb_run();
- uint32 final_reply_delay_us;
- uint32 resp_dly[RESP_DLY_NB];
- int i;
+// uint32 final_reply_delay_us;
+// uint32 resp_dly[RESP_DLY_NB];
+// int i;
tdma_handler->build_new_network(tdma_handler);
@@ -1491,41 +1425,48 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
// inst->nextState = TA_TXPOLL_WAIT_SEND ; // send next poll
// inst->nextState = TA_TXINF_WAIT_SEND ; // send next poll
-
- // Get response delays from message and update internal timings accordingly
- resp_dly[RESP_DLY_ANC] = messageData[RNG_INIT_ANC_RESP_DLY_LO]
- + (messageData[RNG_INIT_ANC_RESP_DLY_HI] << 8);
- resp_dly[RESP_DLY_TAG] = messageData[RNG_INIT_TAG_RESP_DLY_LO]
- + (messageData[RNG_INIT_TAG_RESP_DLY_HI] << 8);
-
- for (i = 0; i < RESP_DLY_NB; i++)
- {
- if (((resp_dly[i] & RESP_DLY_UNIT_MASK) >> RESP_DLY_UNIT_SHIFT) == RESP_DLY_UNIT_MS)
- {
- // Remove unit bit and convert to microseconds.
- resp_dly[i] &= ~RESP_DLY_UNIT_MASK;
- resp_dly[i] *= 1000;
- }
- }
-
- // Update delay between poll transmission and response reception.
- // Use uint64 for resp_dly here to avoid overflows if it is more than 400 ms.
- inst->txToRxDelayTag_sy = US_TO_SY_INT((uint64)resp_dly[RESP_DLY_ANC] - inst->frameLengths_us[POLL]) - RX_START_UP_SY;
-
- // Update delay between poll transmission and final transmission.
- final_reply_delay_us = resp_dly[RESP_DLY_ANC] + resp_dly[RESP_DLY_TAG];
- inst->finalReplyDelay = convertmicrosectodevicetimeu(final_reply_delay_us);
- inst->finalReplyDelay_ms = CEIL_DIV(final_reply_delay_us, 1000);
-
- // If we are using long response delays, deactivate sleep.
- if (resp_dly[RESP_DLY_ANC] >= LONG_RESP_DLY_LIMIT_US
- || resp_dly[RESP_DLY_TAG] >= LONG_RESP_DLY_LIMIT_US)
- {
- inst->sleepingEabled = 0;
- }
-
-// memcpy(&inst->msg[inst->uwbToRangeWith].destAddr[0], &srcAddr[0], inst->addrByteSize); //set the anchor address for the reply (set destination address)
- memcpy(&inst->msg.destAddr[0], &srcAddr[0], inst->addrByteSize); //set the anchor address for the reply (set destination address)
+// ////////////////////////////////////////////////////////////////////////////////////////
+// //TODO remove from RNG_INIT message, just set this in INIT for both?
+//
+// // Get response delays from message and update internal timings accordingly
+// resp_dly[RESP_DLY_ANC] = messageData[RNG_INIT_ANC_RESP_DLY_LO]
+// + (messageData[RNG_INIT_ANC_RESP_DLY_HI] << 8);
+// resp_dly[RESP_DLY_TAG] = messageData[RNG_INIT_TAG_RESP_DLY_LO]
+// + (messageData[RNG_INIT_TAG_RESP_DLY_HI] << 8);
+//
+// for (i = 0; i < RESP_DLY_NB; i++)
+// {
+// if (((resp_dly[i] & RESP_DLY_UNIT_MASK) >> RESP_DLY_UNIT_SHIFT) == RESP_DLY_UNIT_MS)
+// {
+// // Remove unit bit and convert to microseconds.
+// resp_dly[i] &= ~RESP_DLY_UNIT_MASK;
+// resp_dly[i] *= 1000;
+// }
+// }
+//
+// // Update delay between poll transmission and response reception.
+// // Use uint64 for resp_dly here to avoid overflows if it is more than 400 ms.
+// inst->txToRxDelayTag_sy = US_TO_SY_INT((uint64)resp_dly[RESP_DLY_ANC] - inst->frameLengths_us[POLL]) - RX_START_UP_SY;
+//
+// // Update delay between poll transmission and final transmission.
+// final_reply_delay_us = resp_dly[RESP_DLY_ANC] + resp_dly[RESP_DLY_TAG];
+// inst->finalReplyDelay = convertmicrosectodevicetimeu(final_reply_delay_us);
+// inst->finalReplyDelay_ms = CEIL_DIV(final_reply_delay_us, 1000);
+//
+// // If we are using long response delays, deactivate sleep.
+// if (resp_dly[RESP_DLY_ANC] >= LONG_RESP_DLY_LIMIT_US
+// || resp_dly[RESP_DLY_TAG] >= LONG_RESP_DLY_LIMIT_US)
+// {
+// inst->sleepingEabled = 0;
+// }
+//
+//// memcpy(&inst->msg[inst->uwbToRangeWith].destAddr[0], &srcAddr[0], inst->addrByteSize); //set the anchor address for the reply (set destination address)
+//
+// //TODO move this to TAG_POLL???
+// memcpy(&inst->msg.destAddr[0], &srcAddr[0], inst->addrByteSize); //set the anchor address for the reply (set destination address)
+//
+//
+// //////////////////////////////////////////////////////////////////////////////////////////////
inst->mode = TAG;
//inst->responseTimeouts = 0; //reset timeout count
@@ -1538,51 +1479,64 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
case RTLS_DEMO_MSG_INF_SUG : //fall through
case RTLS_DEMO_MSG_INF_REG :
{
-// uint32 time_now = portGetTickCnt();
-// uint8 srcIndex = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
-//
-// if(inst->mode == DISCOVERY)
-// {
-// //NOTE: RX callback only accepts INF_UPDATE/INF_SUG/INF_REG for discovery modes WAIT_INF_REG and COLLECT_INF_REG.
-//
-// //1.) sync our frame start time to the local network
-// //2.) collect and combine tdma info so we can construct a SUG packet and send it out
-//
-// if(tdma_handler->discovery_mode == WAIT_INF_REG) //treat INF_UPDATE and INF_SUG the same
-// {
-// //synchronize the frames
-// tdma_handler->frame_sync(tdma_handler, dw_event, messageData, 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
-//// tdma_handler->set_discovery_mode(tdma_handler, COLLECT_INF_REG, time_now);
-// }
-// else if(tdma_handler->discovery_mode == COLLECT_INF_REG)
-// {
-// //synchronize the frames
-// tdma_handler->frame_sync(tdma_handler, dw_event, messageData, srcIndex, FS_AVERAGE);
-//// //collecting tdma info, append to previously stored info
-//// tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, COPY);
-// }
-// }
-// else if(inst->mode == ANCHOR || inst->mode == TAG)
-// {
-// //if we are a TAG or ANCHOR
-// //1.) sync our frame start time to the local network
-// //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);
-//
-// //collecting tdma info, append to previously stored info
-//// bool tdma_modified = tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, CLEAR_LISTED_COPY);
-////
-//// if(tdma_modified)
-//// {
-//// //TODO set some sort of flag that tells us to send INF_UPDATE!!!
-//// tdma_handler->populate_inf_msg(tdma_handler, RTLS_DEMO_MSG_INF_UPDATE);
-//// }
-//
+ uint32 time_now = portGetTickCnt();
+ uint8 srcIndex = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
+
+ //return to dicovery mode if no slots assigned to this UWB
+ if(inst->mode == ANCHOR || inst->mode == TAG)
+ {
+ if(tdma_handler->uwbListTDMAInfo[0].slotsLength == 0)
+ {
+ tdma_handler->set_discovery_mode(tdma_handler, COLLECT_INF_REG, time_now);
+ }
+ }
+
+ if(inst->mode == DISCOVERY)
+ {
+ //NOTE: RX callback only accepts INF_UPDATE/INF_SUG/INF_REG for discovery modes WAIT_INF_REG and COLLECT_INF_REG.
+
+ //1.) sync our frame start time to the local network
+ //2.) collect and combine tdma info so we can construct a SUG packet and send it out
+
+ if(tdma_handler->discovery_mode == WAIT_INF_REG) //treat INF_UPDATE and INF_SUG the same
+ {
+ //synchronize the frames
+ tdma_handler->frame_sync(tdma_handler, dw_event, messageData, 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
+ tdma_handler->set_discovery_mode(tdma_handler, COLLECT_INF_REG, time_now);
+ }
+ else if(tdma_handler->discovery_mode == COLLECT_INF_REG)
+ {
+ //TODO for some reason, it seems like the frame start time is way off with this... fix it!
+
+ //synchronize the frames
+ tdma_handler->frame_sync(tdma_handler, dw_event, messageData, srcIndex, FS_AVERAGE);
+// //collecting tdma info, append to previously stored info
+ tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, COPY);
+ }
+ }
+ else if(inst->mode == ANCHOR || inst->mode == TAG)
+ {
+ //TODO in here (or in RX callback) I have to think about when to adopt the new TDMA and also what INF messages to accept or reject, if a modified TDMA exists but we havent had a chance to TX it in an INF_UPDATE yet.
+
+ //if we are a TAG or ANCHOR
+ //1.) sync our frame start time to the local network
+ //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);
+
+ //collecting tdma info, append to previously stored info
+ bool tdma_modified = tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, CLEAR_LISTED_COPY);
+
+ if(tdma_modified)
+ {
+ //TODO set some sort of flag that tells us to send INF_UPDATE!!!
+ tdma_handler->populate_inf_msg(tdma_handler, RTLS_DEMO_MSG_INF_UPDATE);
+ }
+
// //clear all tdma information
// tdma_handler->tdma_free_all_slots(tdma_handler);
//
@@ -1595,14 +1549,14 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
// tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith], 3);
//
// tdma_handler->populate_inf_msg(tdma_handler, RTLS_DEMO_MSG_INF_UPDATE);
-//
-// //TODO left off here! make sure the logic below is enforced in the RX callback
-// //NOTE i can think of some differences between SUG/UPDATE/REG...
-// //when waiting to send INF_SUG, ignore everything else?
-// //when waiting to send INF_UPDATE, ignore INF_REG?
-//
-//
-// }
+
+ //TODO left off here! make sure the logic below is enforced in the RX callback
+ //NOTE i can think of some differences between SUG/UPDATE/REG...
+ //when waiting to send INF_SUG, ignore everything else?
+ //when waiting to send INF_UPDATE, ignore INF_REG?
+
+
+ }
//wait for next RX
inst->testAppState = TA_RXE_WAIT ;
@@ -1622,38 +1576,28 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
uint32 time_now = portGetTickCnt();
uint8 srcIndex = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
- //clear all tdma information
- tdma_handler->tdma_free_all_slots(tdma_handler);
-
- //build the initial TDMA
- tdma_handler->uwbListTDMAInfo[0].framelength = (uint8)MIN_FRAMELENGTH;
- tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith].framelength = (uint8)MIN_FRAMELENGTH;
+// //clear all tdma information
+// tdma_handler->tdma_free_all_slots(tdma_handler);
+//
+// //build the initial TDMA
+// tdma_handler->uwbListTDMAInfo[0].framelength = (uint8)MIN_FRAMELENGTH;
+// tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith].framelength = (uint8)MIN_FRAMELENGTH;
//synchronise the frames
tdma_handler->frame_sync(tdma_handler, dw_event, messageData, 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);
+ //copy the TDMA network configuration directly
+ tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, CLEAR_ALL_COPY);
+ //copy new TDMA configuration into the INF message that this UWB will send out
+ tdma_handler->populate_inf_msg(tdma_handler, RTLS_DEMO_MSG_INF_UPDATE); //TODO set some sort of flag that tells us to send INF_UPDATE?
//set discovery mode to EXIT
tdma_handler->set_discovery_mode(tdma_handler, EXIT, time_now); //TODO do I really need EXIT?
-// //collecting tdma info, append to previously stored info
-// bool tdma_modified = tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, CLEAR_LISTED_COPY);
+// tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith], 1);
+// tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[0], 2);
+// tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith], 3);
//
-// if(tdma_modified)
-// {
-// //TODO set some sort of flag that tells us to send INF_UPDATE!!!
-// tdma_handler->populate_inf_msg(tdma_handler, RTLS_DEMO_MSG_INF_UPDATE);
-// }
-
-
- tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith], 1);
- tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[0], 2);
- tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith], 3);
-
- tdma_handler->populate_inf_msg(tdma_handler, RTLS_DEMO_MSG_INF_UPDATE);
-
-
+// tdma_handler->populate_inf_msg(tdma_handler, RTLS_DEMO_MSG_INF_UPDATE);
inst->mode = ANCHOR;
@@ -1831,35 +1775,41 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
{
// send_rxmsgtousb("RX process: DWT_SIG_RX_OKAY-RTLS_DEMO_MSG_RNG_REPORT ");
+ uint8 tag_index = instgetuwblistindex(inst, &messageData[REPORT_ADDR], inst->addrByteSize);
+ uint8 anchor_index = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
- inst->tof[inst->uwbToRangeWith] = 0;
+ //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
- //TODO #define messageData offsets
- memcpy(&inst->tof[inst->uwbToRangeWith], &messageData[1], 6);
+ //copy previously calculated ToF
+ //TODO #define messageData offsets
+ memcpy(&inst->tof[anchor_index], &messageData[REPORT_TOF], 6);
- inst->newRangeUWBIndex = inst->uwbToRangeWith;
- inst->newRangeAncAddress = instance_get_uwbaddr(inst->uwbToRangeWith);
- inst->newRangeTagAddress = instance_get_addr();
+ inst->newRangeAncAddress = instance_get_uwbaddr(anchor_index);
+ inst->newRangeTagAddress = instance_get_uwbaddr(tag_index);
- if(inst->tof[inst->newRangeUWBIndex] > 0) //if ToF == 0 - then no new range to report
- {
- if(reportTOF(inst, inst->newRangeUWBIndex)==0)
+ inst->newRangeUWBIndex = anchor_index;
+ if(inst->tof[inst->newRangeUWBIndex] > 0) //if ToF == 0 - then no new range to report
{
- inst->newRange = 1;
- inst->ranging = 1;
- inst->canPrintInfo = 1;
+ if(reportTOF(inst, inst->newRangeUWBIndex)==0)
+ {
+ inst->newRange = 1;
+ inst->ranging = 1;
+ inst->canPrintInfo = 1;
+ }
}
- }
- inst->lastRangeTimeStamp[inst->uwbToRangeWith] = portGetTickCnt();
+ inst->lastRangeTimeStamp[inst->uwbToRangeWith] = portGetTickCnt();
- done = INST_DONE_WAIT_FOR_NEXT_EVENT;
+ uint8 debug_msg[100];
+ int n = sprintf((char *)&debug_msg, "POLL_COMPLETE,%llX,%llX", 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);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
+ }
inst->testAppState = TA_RXE_WAIT ;
@@ -1870,6 +1820,9 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
dwt_setrxaftertxdelay(0);
instancesetantennadelays(); //this will update the antenna delay if it has changed
+ done = INST_DONE_WAIT_FOR_NEXT_EVENT;
+
+
break;
} //RTLS_DEMO_MSG_RNG_REPORT
default:
@@ -1907,10 +1860,12 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
// tdma_handler->discovery_mode = WAIT_INF_REG;
// }
// tdma_handler->waitForRngInit = FALSE;
-// uint8 debug_msg[100];
-// n = sprintf((char *)&debug_msg, "TX_BLINK_TIMEOUT,%llX,NULL", instance_get_addr());
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
+
+
+ uint8 debug_msg[100];
+ n = sprintf((char *)&debug_msg, "TX_BLINK_TIMEOUT,%llX,NULL", instance_get_addr());
+ send_usbmessage(&debug_msg[0], n);
+ usb_run();
}
else if(inst->previousState == TA_TXRANGINGINIT_WAIT_SEND)
{
@@ -1925,46 +1880,18 @@ 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));
-// 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));
+ 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));
-// 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));
+ send_usbmessage(&debug_msg[0], n);
+ usb_run();
}
-
-// uint8 buffer[SYS_STATUS_LEN];
-// uint64 sys_status_reg;
-// dwt_readfromdevice(SYS_STATUS_ID,0,SYS_STATUS_LEN,buffer);
-//
-// for (int j = SYS_STATUS_LEN-1 ; j >= 0 ; j --)
-// {
-// sys_status_reg = (sys_status_reg << 8) + buffer[j] ;
-// }
-//
-// uint64 sys_state_reg;
-// dwt_readfromdevice(SYS_STATE_ID,0,SYS_STATE_LEN,buffer);
-//
-// for (int j = SYS_STATE_LEN-1 ; j >= 0 ; j --)
-// {
-// sys_state_reg = (sys_state_reg << 8) + buffer[j] ;
-// }
-
-// uint8 debug_msg[100];
-// n = sprintf((char *)&debug_msg, "SYS_STATUS: %llu SYS_STATE: %llu", sys_status_reg, sys_state_reg);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
-// uint8 debug_msg[100];
-// n = sprintf((char *)&debug_msg, "inst_processtxrxtimeout(inst) after DWT_SIG_RX_TIMEOUT");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
instance_getevent(17); //get and clear this event
inst_processtxrxtimeout(inst);
@@ -1974,14 +1901,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
}
case DWT_SIG_TX_DONE: //shouldn't happen under normal conditions, but this signal seems to be somewhat buggy
{
- //clear the event?
- //event_data_t* dw_event = instance_getevent(11); //get and clear this event
instance_getevent(11); //get and clear this event
-
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "DWT_SIG_TX_DONE in RX_WAIT_DATA");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
break;
}
case DWT_SIG_TX_AA_DONE: //ignore this event - just process the rx frame that was received before the ACK response
@@ -2007,13 +1927,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
// send_usbmessage(&debug_msg[0], n);
// usb_run();
}
-
-
-// uint8 debug_msg[200];
-// int n = sprintf((char *)&debug_msg, "RX CHECKED: regval %u", regval);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
}
@@ -2023,42 +1936,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
inst->testAppState = TA_TX_SELECT;
}
-
-
-
-// if(inst->mode == ANCHOR)
-// {
-//// send_rxmsgtousb("RX process: default ");
-//
-// uint32 delta_t = portGetTickCnt() - inst->currentStateStartTime;
-// if(delta_t > 20000){
-//
-//
-// uint8 buffer[SYS_STATUS_LEN];
-// uint64 sys_status_reg;
-// dwt_readfromdevice(SYS_STATUS_ID,0,SYS_STATUS_LEN,buffer);
-//
-// for (int j = SYS_STATUS_LEN-1 ; j >= 0 ; j --)
-// {
-// sys_status_reg = (sys_status_reg << 8) + buffer[j] ;
-// }
-//
-// uint64 sys_state_reg;
-// dwt_readfromdevice(SYS_STATE_ID,0,SYS_STATE_LEN,buffer);
-//
-// for (int j = SYS_STATE_LEN-1 ; j >= 0 ; j --)
-// {
-// sys_state_reg = (sys_state_reg << 8) + buffer[j] ;
-// }
-//
-//// uint8 debug_msg[200];
-//// int n = sprintf((char *)&debug_msg, "SYS_STATUS: %llu SYS_STATE: %llu", sys_status_reg, sys_state_reg);
-//// send_usbmessage(&debug_msg[0], n);
-//// usb_run();
-// }
-// }
-
-
if(done == INST_NOT_DONE_YET)
{
done = INST_DONE_WAIT_FOR_NEXT_EVENT;
@@ -2072,10 +1949,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
} // end case TA_RX_WAIT_DATA
default:
{
- // if (inst->mode == ANCHOR)
- // {
- // send_statetousb(inst);
- // }
break;
}
} // end switch on testAppState
@@ -2100,47 +1973,11 @@ int instance_init_s()
inst->mode = DISCOVERY;
inst->testAppState = TA_INIT;
-// inst->framelength = MIN_FRAMELENGTH;
-// inst->maxFramelength = MIN_FRAMELENGTH;
-// while(inst->maxFramelength < UWB_LIST_SIZE + 1)
-// {
-// inst->maxFramelength *= 2;
-// }
-//
-// inst->slotDuration = 30; //TODO use a #define or something?
-// inst->slotAssignments = malloc(sizeof(uint16)*inst->maxFramelength);
-// for(int i = 0; i < inst->maxFramelength; i++)
-// {
-// uint16 blank = 0x0000;
-// memcpy(&inst->slotAssignments[i], &blank, 2);
-// }
-// for(int i = 0; i < UWB_LIST_SIZE; i++)
-// {
-// inst->uwbFramelengths[i] = 0;
-// }
-//
-// //TODO consider using a scheduler class for all of this...
-// inst->discoveryStartTime = portGetTickCnt();
-
-// if(inst->mode == ANCHOR){
-// inst->rx_scheduler = RXScheduler.new(BLINK_FREQUENCY, BLINK_DURATION_MS, RANGE_DURATION_MS, &inst->uwbList, &inst->uwbListLen, UWB_LIST_SIZE, inst->uwbTimeout, inst->time_till_next_reported);//TODO do i need & ???
-// }
-// else if(inst->mode == TAG){
-// inst->tx_scheduler = TXScheduler.new(BLINK_FREQUENCY, BLINK_DURATION_MS, RANGE_DURATION_MS, &inst->uwbList, &inst->uwbListLen, UWB_LIST_SIZE, inst->uwbTimeout);
-// }
-// inst->tdma_handler = TDMAHandler.new(&inst->uwbList, &inst->uwbListLen, &inst->mode);
-// tdma_handler = TDMAHandler.new(&inst->uwbList, &inst->uwbListLen, &inst->mode);
-
// 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(DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_SFDT | DWT_INT_RFTO /*| DWT_INT_RXPTO*/), 1);
- // dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO /*| DWT_INT_RXPTO*/), 1);
// dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_RXOVRR | DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO /*| DWT_INT_RXPTO*/), 1);
dwt_setinterrupt(SYS_MASK_VAL, 1);
-// uint32 mask = dwt_read32bitreg(SYS_MASK_ID);
-
-
//this is platform dependent - only program if DW EVK/EVB
dwt_setleds(3) ; //configure the GPIOs which control the LEDs on EVBs
@@ -2171,9 +2008,12 @@ void instance_init_timings(void)
instance_data_t* inst = instance_get_local_structure_ptr(0);
uint32 pre_len;
int sfd_len;
- static const int data_len_bytes[FRAME_TYPE_NB - 1] = {
- BLINK_FRAME_LEN_BYTES, RNG_INIT_FRAME_LEN_BYTES, POLL_FRAME_LEN_BYTES,
- RESP_FRAME_LEN_BYTES, FINAL_FRAME_LEN_BYTES};
+
+ static const int data_len_bytes[FRAME_TYPE_NB - 1] = {
+ BLINK_FRAME_LEN_BYTES, RNG_INIT_FRAME_LEN_BYTES, POLL_FRAME_LEN_BYTES,
+ RESP_FRAME_LEN_BYTES, FINAL_FRAME_LEN_BYTES};
+
+
int i;
// Margin used for timeouts computation.
const int margin_sy = 500; //50;
@@ -2222,6 +2062,7 @@ void instance_init_timings(void)
pre_len *= 101763;
inst->storedPreLen = pre_len; //store to be used later with inf messages and frame_sync
+ inst->storePreLen_us = CEIL_DIV(pre_len, 100000);
// Second step is data length for all frame types.
for (i = 0; i < FRAME_TYPE_NB - 1; i++)//exclude INF message, since it changes size
@@ -2265,8 +2106,43 @@ void instance_init_timings(void)
// Delay between anchor's response transmission and final reception.
inst->txToRxDelayAnc_sy = US_TO_SY_INT(TAG_TURN_AROUND_TIME_US) - RX_START_UP_SY;
- // No need to init txToRxDelayTag_sy here as it will be set upon reception
- // of ranging init message.
+ uint16 resp_dly_us, resp_dly;
+ uint32 final_reply_delay_us;
+
+ // First response delay to send is anchor's response delay.
+ resp_dly_us = ANC_TURN_AROUND_TIME_US + inst->frameLengths_us[POLL];
+ resp_dly = ((RESP_DLY_UNIT_US << RESP_DLY_UNIT_SHIFT) & RESP_DLY_UNIT_MASK)
+ + ((resp_dly_us << RESP_DLY_VAL_SHIFT) & RESP_DLY_VAL_MASK);
+ inst->resp_dly[RESP_DLY_ANC] = resp_dly & 0xFFFF; //TODO make sure that we only want the 16 lowest bits
+
+
+ // Second response delay to send is tag's response delay.
+ // Get response delays from message and update internal timings accordingly
+ resp_dly_us = TAG_TURN_AROUND_TIME_US + inst->frameLengths_us[RESP];
+ resp_dly = ((RESP_DLY_UNIT_US << RESP_DLY_UNIT_SHIFT) & RESP_DLY_UNIT_MASK)
+ + ((resp_dly_us << RESP_DLY_VAL_SHIFT) & RESP_DLY_VAL_MASK);
+ inst->resp_dly[RESP_DLY_TAG] = resp_dly & 0xFFFF; //TODO make sure that we only want the 16 lowest bits
+
+
+ for (int i = 0; i < RESP_DLY_NB; i++)
+ {
+ if (((inst->resp_dly[i] & RESP_DLY_UNIT_MASK) >> RESP_DLY_UNIT_SHIFT) == RESP_DLY_UNIT_MS)
+ {
+ // Remove unit bit and convert to microseconds.
+ inst->resp_dly[i] &= ~RESP_DLY_UNIT_MASK;
+ inst->resp_dly[i] *= 1000;
+ }
+ }
+
+ // Update delay between poll transmission and response reception.
+ // Use uint64 for resp_dly here to avoid overflows if it is more than 400 ms.
+ inst->txToRxDelayTag_sy = US_TO_SY_INT((uint64)inst->resp_dly[RESP_DLY_ANC] - inst->frameLengths_us[POLL]) - RX_START_UP_SY;
+
+ // Update delay between poll transmission and final transmission.
+ final_reply_delay_us = inst->resp_dly[RESP_DLY_ANC] + inst->resp_dly[RESP_DLY_TAG];
+ inst->finalReplyDelay = convertmicrosectodevicetimeu(final_reply_delay_us);
+ inst->finalReplyDelay_ms = CEIL_DIV(final_reply_delay_us, 1000);
+
// Delay between blink reception and ranging init message transmission.
inst->rnginitReplyDelay = convertmicrosectodevicetimeu(RNG_INIT_REPLY_DLY_MS * 1000);
@@ -2321,16 +2197,21 @@ uint32 instance_getmessageduration_us(int data_length_bytes)
uint64 instance_get_addr(void) //get own address
{
+
+ //TODO should this instead operate on uwbList[0]???
+
instance_data_t* inst = instance_get_local_structure_ptr(0);
- uint64 x = (uint64) inst->eui64[0];
+ uint64 x = 0;
+ x |= (uint64) inst->eui64[0];
x |= (uint64) inst->eui64[1] << 8;
+#if (USING_64BIT_ADDR == 1)
x |= (uint64) inst->eui64[2] << 16;
x |= (uint64) inst->eui64[3] << 24;
x |= (uint64) inst->eui64[4] << 32;
x |= (uint64) inst->eui64[5] << 40;
x |= (uint64) inst->eui64[6] << 48;
x |= (uint64) inst->eui64[7] << 56;
-
+#endif
return (x);
}
@@ -2338,14 +2219,17 @@ uint64 instance_get_addr(void) //get own address
uint64 instance_get_uwbaddr(uint8 uwb_index) //get uwb address by index
{
instance_data_t* inst = instance_get_local_structure_ptr(0);
- uint64 x = (uint64) inst->uwbList[uwb_index][0];
+ uint64 x = 0;
+ x |= (uint64) inst->uwbList[uwb_index][0];
x |= (uint64) inst->uwbList[uwb_index][1] << 8;
+#if (USING_64BIT_ADDR == 1)
x |= (uint64) inst->uwbList[uwb_index][2] << 16;
x |= (uint64) inst->uwbList[uwb_index][3] << 24;
x |= (uint64) inst->uwbList[uwb_index][4] << 32;
x |= (uint64) inst->uwbList[uwb_index][5] << 40;
x |= (uint64) inst->uwbList[uwb_index][6] << 48;
x |= (uint64) inst->uwbList[uwb_index][7] << 56;
+#endif
return (x);
}
@@ -2376,13 +2260,100 @@ uint32 get_dt32(uint32 t1, uint32 t2)
{
return t2 - t1;
}
- else
+ else//TODO maybe rework this... should should instead check if one number is greater than a threshold... somehow spit out error if used incorrectly???
{
//handle timestamp roleover
- return 4294967295 - t1 + t2;
+ return 4294967295 - t1 + t2;
}
}
+//add a duration to a 32 bit timestamp. This function handles number wrapping
+uint32 timestamp_add32(uint32 timestamp, uint32 duration)
+{
+ uint32 to_wrap = 4294967295 - timestamp;
+ if(duration > to_wrap)
+ {
+ return to_wrap + duration;
+ }
+ else
+ {
+ return timestamp + duration;
+ }
+}
+
+//subtract a duration from a 32 bit timestamp. This function handles number wrapping
+uint32 timestamp_subtract32(uint32 timestamp, uint32 duration)
+{
+ if(duration > timestamp)
+ {
+ return 4294967295 - (duration - timestamp);
+ }
+ else
+ {
+ return timestamp - duration;
+ }
+}
+
+//get the time difference between two between two 64-bit unsigned timestamps
+//t1 is the first timestamp
+//t2 is the second timetamp that occured after t1
+uint64 get_dt64(uint64 t1, uint64 t2)
+{
+ if(t2 >= t1)
+ {
+ return t2 - t1;
+ }
+ else//TODO maybe rework this... should should instead check if one number is greater than a threshold... somehow spit out error if used incorrectly???
+ {
+ //handle timestamp roleover
+ return 4294967295999 - t1 + t2;
+ }
+}
+
+//TODO remove
+//get the time difference between two between two 16-bit unsigned timestamps
+//t1 is the first timestamp
+//t2 is the second timetamp that occured after t1
+//uint16 get_dt16(uint16 t1, uint16 t2)
+//{
+// if(t2 >= t1)
+// {
+// return t2 - t1;
+// }
+// else//TODO maybe rework this... should should instead check if one number is greater than a threshold... somehow spit out error if used incorrectly???
+// {
+// //handle timestamp roleover
+// return 0xFFFF - t1 + t2;
+// }
+//}
+
+//add a duration to a 64 bit timestamp. This function handles number wrapping
+uint64 timestamp_add64(uint64 timestamp, uint64 duration)
+{
+ uint32 to_wrap = (uint64)4294967295999 - timestamp;
+ if(duration > to_wrap)
+ {
+ return to_wrap + duration;
+ }
+ else
+ {
+ return timestamp + duration;
+ }
+}
+
+//subtract a duration from a 64 bit timestamp. This function handles number wrapping
+uint64 timestamp_subtract64(uint64 timestamp, uint64 duration)
+{
+ if(duration > timestamp)
+ {
+ return (uint64)4294967295999 - (duration - timestamp);
+ }
+ else
+ {
+ return timestamp - duration;
+ }
+}
+
//TODO implement a hashing function to reduce chance of collisions
//https://stackoverflow.com/questions/31710074/how-to-generate-smaller-unique-number-from-larger-11-bytes-unique-number-gene
//https://en.m.wikipedia.org/wiki/Pearson_hashing
diff --git a/src/application/instance.h b/src/application/instance.h
index 2d604df4d68df9c1691c22d127744ba69bdf261e..0ff87b4e72aa9cd939a1ca07ba810cc84359d628 100644
--- a/src/application/instance.h
+++ b/src/application/instance.h
@@ -68,25 +68,19 @@ typedef struct
// in microseconds.
uint32 frameLengths_us[FRAME_TYPE_NB];
uint32 storedPreLen; //precomputed conversion of preamble and sfd
+ uint64 storePreLen_us; //precomputed conversion of preamble and sfd in microseconds
//message structures used for transmitted messages
#if (USING_64BIT_ADDR == 1)
srd_msg_dlsl rng_initmsg ; // ranging init message (destination long, source long)
-// srd_msg_dlsl msg[UWB_LIST_SIZE] ; // simple 802.15.4 frame structure (used for tx message) - using long addresses
-// srd_ext_msg_dlsl msg[UWB_LIST_SIZE] ; // simple 802.15.4 frame structure (used for tx message) - using long addresses
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_dlsl ranging_msg; //extended frame structure
#else
srd_msg_dlss rng_initmsg ; // ranging init message (destination long, source short)
-// srd_msg_dsss msg[UWB_LIST_SIZE] ; // simple 802.15.4 frame structure (used for tx message) - using short addresses
-// srd_ext_msg_dsss msg[UWB_LIST_SIZE] ; // simple 802.15.4 frame structure (used for tx message) - using short addresses
srd_ext_msg_dsss msg; // simple 802.15.4 frame structure (used for tx message) - using short addresses
-// srd_ext_msg_dlss rng_initmsg ; //extended ranging init message (destination long, source short)
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 ranging_msg; //extended frame structure
#endif
iso_IEEE_EUI64_blink_msg blinkmsg ; // frame structure (used for tx blink message)
@@ -98,6 +92,8 @@ typedef struct
uint8 addrByteSize; // The bytelength used for addresses.
+ uint32 resp_dly[RESP_DLY_NB];
+
//64 bit timestamps
//union of TX timestamps
union {
@@ -178,7 +174,7 @@ typedef struct
uint8 dweventPeek;
uint8 monitor;
uint32 timeofTx;
- uint32 timeofRxCallback;
+ uint64 timeofRxCallback;
uint64 timeofRxCallback_dwtime;
uint8 smartPowerEn;
@@ -191,6 +187,10 @@ typedef struct
uint8 ranging;
+ uint64 testTimer;
+ uint16 timerCounter;
+
+
} instance_data_t ;
//-------------------------------------------------------------------------------------------------------------
@@ -316,6 +316,13 @@ const char* get_discovery_modes_string(enum discovery_modes mode);
instance_data_t* instance_get_local_structure_ptr(unsigned int x);
uint32 get_dt32(uint32 t1, uint32 t2);
+uint32 timestamp_add32(uint32 timestamp, uint32 duration);
+uint32 timestamp_subtract32(uint32 timestamp, uint32 duration);
+uint64 get_dt64(uint64 t1, uint64 t2);
+uint64 timestamp_add64(uint64 timestamp, uint64 duration);
+uint64 timestamp_subtract64(uint64 timestamp, uint64 duration);
+uint16 get_dt16(uint16 t1, uint16 t2);
+
uint16 address64to16(uint8 *address);
//void send_statetousb(instance_data_t *inst);
diff --git a/src/application/instance_common.c b/src/application/instance_common.c
index f3747dfd8f1d9a62f10bd9aeeb22a5c2fe574960..78d5f83abd6e22629f80d35f889a63dca86ed114 100644
--- a/src/application/instance_common.c
+++ b/src/application/instance_common.c
@@ -18,11 +18,6 @@
#include "deca_spi.h"
#include "instance.h"
-//#include "llist.h"
-//#include "uwb_select.h"
-
-#include <inttypes.h>
-
extern void usb_run(void);
extern int usb_init(void);
extern void usb_printconfig(int, uint8*, int);
@@ -218,6 +213,78 @@ int instgetuwblistindex(instance_data_t *inst, uint8 *uwbAddr, uint8 addrByteSiz
{
uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+ char uwbChar[2];
+ memcpy(&uwbChar[0], &uwbAddr[0], 2);
+
+ bool match = FALSE;
+ char test_addr[2] = {0x95, 0x15};
+ if(memcmp(&uwbChar[0], &test_addr[0], 2) == 0)
+ {
+ match = TRUE;
+ }
+ test_addr[0] = 0x50;
+ test_addr[1] = 0x59;
+ if(memcmp(&uwbChar[0], &test_addr[0], 2) == 0)
+ {
+ match = TRUE;
+ }
+ test_addr[0] = 0x36;
+ test_addr[1] = 0x18;
+ if(memcmp(&uwbChar[0], &test_addr[0], 2) == 0)
+ {
+ match = TRUE;
+ }
+ test_addr[0] = 0x39;
+ test_addr[1] = 0x16;
+ if(memcmp(&uwbChar[0], &test_addr[0], 2) == 0)
+ {
+ match = TRUE;
+ }
+ test_addr[0] = 0x27;
+ test_addr[1] = 0x59;
+ if(memcmp(&uwbChar[0], &test_addr[0], 2) == 0)
+ {
+ match = TRUE;
+ }
+ test_addr[0] = 0x20;
+ test_addr[1] = 0x1B;
+ if(memcmp(&uwbChar[0], &test_addr[0], 2) == 0)
+ {
+ match = TRUE;
+ }
+ test_addr[0] = 0x8C;
+ test_addr[1] = 0x11;
+ if(memcmp(&uwbChar[0], &test_addr[0], 2) == 0)
+ {
+ match = TRUE;
+ }
+ test_addr[0] = 0x1D;
+ test_addr[1] = 0x14;
+ if(memcmp(&uwbChar[0], &test_addr[0], 2) == 0)
+ {
+ match = TRUE;
+ }
+ test_addr[0] = 0x8C;
+ test_addr[1] = 0x19;
+ if(memcmp(&uwbChar[0], &test_addr[0], 2) == 0)
+ {
+ match = TRUE;
+ }
+ test_addr[0] = 0xD6;
+ test_addr[1] = 0x0C;
+ if(memcmp(&uwbChar[0], &test_addr[0], 2) == 0)
+ {
+ match = TRUE;
+ }
+
+ if(match == FALSE)
+ {
+// instance_data_t *inst = &instance_data[instance];
+ uint8 debug_msg[100];
+ int n = sprintf((char*)&debug_msg[0], "match not found");
+ send_usbmessage(&debug_msg[0], n);
+ usb_run();
+ }
//add the new UWB to the list, if not already there and there is space
for(uint8 i=0; i<UWB_LIST_SIZE; i++)
@@ -1086,12 +1153,13 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
// usb_run();
// uint8 debug_msg[150];
-// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK, %llX, xxx", instance_get_addr());
+// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK");
// send_usbmessage(&debug_msg[0], n);
// usb_run();
uint32 time_now = portGetTickCnt();
+ uint32 time_now_us = portGetTickCntMicro();
//if we got a frame with a good CRC - RX OK
rxd_event = DWT_SIG_RX_OKAY;
@@ -1107,10 +1175,10 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
{
rxd_event = DWT_SIG_RX_BLINK;
- uint8 debug_msg[150];
- int n = sprintf((char*)&debug_msg[0], "RX CALLBACK: DWT_SIG_RX_BLINK");
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint8 debug_msg[150];
+// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK: DWT_SIG_RX_BLINK");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
}
else
rxd_event = SIG_RX_UNKNOWN;
@@ -1250,10 +1318,80 @@ 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;
+// bool match = FALSE;
+// char test_addr[2] = {0x95, 0x15};
+// if(memcmp(&blink_address, &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x50;
+// test_addr[1] = 0x59;
+// if(memcmp(&blink_address, &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x36;
+// test_addr[1] = 0x18;
+// if(memcmp(&blink_address, &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x39;
+// test_addr[1] = 0x16;
+// if(memcmp(&blink_address, &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x27;
+// test_addr[1] = 0x59;
+// if(memcmp(&blink_address, &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x20;
+// test_addr[1] = 0x1B;
+// if(memcmp(&blink_address, &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x8C;
+// test_addr[1] = 0x11;
+// if(memcmp(&blink_address, &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x1D;
+// test_addr[1] = 0x14;
+// if(memcmp(&blink_address, &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x8C;
+// test_addr[1] = 0x19;
+// if(memcmp(&blink_address, &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0xD6;
+// test_addr[1] = 0x0C;
+// if(memcmp(&blink_address, &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+//
+// if(match == FALSE)
+// {
+// instance_data_t *inst = &instance_data[instance];
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "match not found");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+// }
+
// 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);
-// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK RECEIVED: BLINK %llX, xxxx", instance_get_addr());
+//// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK RECEIVED: BLINK uwb_index: %d, uwbToRangeWith: %d ", uwb_index, instance_data[instance].uwbToRangeWith);
+// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK RECEIVED: BLINK,%llX,xxxx", instance_get_addr());
// send_usbmessage(&debug_msg[0], n);
// usb_run();
}
@@ -1267,11 +1405,105 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
instance_data[instance].uwbTimeout[uwb_index] = 0;
-// uint8 debug_msg[100];
+// bool match = FALSE;
+// char test_addr[2] = {0x95, 0x15};
+// if(memcmp(&dw_event.msgu.frame[srcAddr_index], &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x50;
+// test_addr[1] = 0x59;
+// if(memcmp(&dw_event.msgu.frame[srcAddr_index], &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x36;
+// test_addr[1] = 0x18;
+// if(memcmp(&dw_event.msgu.frame[srcAddr_index], &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x39;
+// test_addr[1] = 0x16;
+// if(memcmp(&dw_event.msgu.frame[srcAddr_index], &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x27;
+// test_addr[1] = 0x59;
+// if(memcmp(&dw_event.msgu.frame[srcAddr_index], &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x20;
+// test_addr[1] = 0x1B;
+// if(memcmp(&dw_event.msgu.frame[srcAddr_index], &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x8C;
+// test_addr[1] = 0x11;
+// if(memcmp(&dw_event.msgu.frame[srcAddr_index], &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x1D;
+// test_addr[1] = 0x14;
+// if(memcmp(&dw_event.msgu.frame[srcAddr_index], &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0x8C;
+// test_addr[1] = 0x19;
+// if(memcmp(&dw_event.msgu.frame[srcAddr_index], &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+// test_addr[0] = 0xD6;
+// test_addr[1] = 0x0C;
+// if(memcmp(&dw_event.msgu.frame[srcAddr_index], &test_addr, 2) == 0)
+// {
+// match = TRUE;
+// }
+//
+// if(match == FALSE)
+// {
+// instance_data_t *inst = &instance_data[instance];
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "match not found");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+// }
+
+
+
+
+
+ uint8 debug_msg[100];
// int n = sprintf((char*)&debug_msg[0], "RX CB RX: DWT_SIG_RX_OKAY-%s uwb_index %d, uwbToRangeWith: %d ", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]), uwb_index, instance_data[instance].uwbToRangeWith);
-// int n = sprintf((char*)&debug_msg[0], "RX CB RX: DWT_SIG_RX_OKAY-%s, %llX, xxxx", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]), instance_get_addr());
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
+ uint16 my_addr = instance_data[instance].uwbShortAdd;
+//// uint64 my_addr = (uint64)instance_data[instance].uwbShortAdd;
+// 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,%X,xxxx", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]), my_addr);
+// 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];
+
+ 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...
}
bool accept_inf = FALSE;
@@ -1382,7 +1614,7 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
//NOTE: these could get messed up if other messages trigger rxcallback before being processed by frame sync...
//should these be folded into the dw_event?
//TODO only do this when we need to (we are accepting and INF message)
- instance_data[instance].timeofRxCallback = time_now;
+ instance_data[instance].timeofRxCallback = time_now_us;
//TODO only do this when we need to (we are accepting and INF message)
uint8 sys_time_arr[5] = {0, 0, 0, 0, 0};
@@ -1393,6 +1625,19 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
instance_data[instance].uwbTimeout[uwb_index] = 0;
place_event = 1;
+
+
+ if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_INF_SUG)
+ {
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "RX CB: DWT_SIG_RX_OKAY-%s, %llX, xxxx", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]), instance_get_addr());
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+ }
+
+
+
+
}
else if(uwb_index != 255 && instance_data[instance].uwbToRangeWith == uwb_index)
{
@@ -1487,9 +1732,9 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
dwt_writetxdata(frameLength, (uint8 *) &instance_data[instance].msg, 0) ; // write the frame data
#if (IMMEDIATE_RESPONSE == 1)
- dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);
+ dwt_starttx(DWT_START_TX_IMMEDIATE | instance_data[instance].wait4ack);
#else
- if(instancesendpacket(frameLength, DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED, instance_data[instance].delayedReplyTime))
+ if(instancesendpacket(frameLength, DWT_START_TX_DELAYED | instance_data[instance].wait4ack, instance_data[instance].delayedReplyTime))
{
dw_event.typePend = DWT_SIG_TX_ERROR ;
dwt_setrxaftertxdelay(0);
@@ -1713,8 +1958,16 @@ int instance_run(void)
while(done == INST_NOT_DONE_YET)
{
+// uint64 time_now_us = portGetTickCntMicro();
+
done = testapprun(&instance_data[instance], &tdma_handler, message) ; // run the communications application
+// uint64 duration = get_dt64(time_now_us, portGetTickCntMicro());
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "TAR, duration, %llu", duration);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
//we've processed message
message = 0;
}
diff --git a/src/application/tdma_handler.c b/src/application/tdma_handler.c
index 152e47b0b40bd171d933aa0462e0850035ab64ac..b589e0963c4a8b05ec0ded6238abc9de76443ae0 100644
--- a/src/application/tdma_handler.c
+++ b/src/application/tdma_handler.c
@@ -12,58 +12,56 @@ static bool slot_transition(struct TDMAHandler *this)
bool transition = FALSE;
instance_data_t *inst = instance_get_local_structure_ptr(0);
- if(inst->mode == TAG || inst->mode == ANCHOR || (inst->mode == DISCOVERY && this->discovery_mode == WAIT_SEND_SUG))
+ if(inst->mode == TAG ||
+ inst->mode == ANCHOR ||
+ (inst->mode == DISCOVERY && (this->discovery_mode == WAIT_SEND_SUG || this->discovery_mode == COLLECT_INF_REG)))
{
- uint32 time_now = portGetTickCnt();
+ uint64 time_now_us = portGetTickCntMicro();
+ uint64 timeSinceSlotStart64 = get_dt64(this->lastSlotStartTime64, time_now_us);
- uint32 timeSinceSlotStart = get_dt32(this->lastSlotStartTime, time_now);
- if(timeSinceSlotStart > this->slotDuration)
+ if(timeSinceSlotStart64 >= this->slotDuration_us) //TODO should i check frame separate from slot?
{
transition = TRUE;
this->infPollSentThisSlot = FALSE;
//we have transitioned into the next slot.
//get the slot number and set the start time appropriately
- uint32 timeSinceFrameStart = get_dt32(this->uwbFrameStartTimes[0], time_now);
- if(timeSinceFrameStart > this->slotDuration*this->uwbListTDMAInfo[0].framelength)
+
+ uint64 timeSinceFrameStart64 = get_dt64(this->uwbFrameStartTimes64[0], time_now_us);
+
+ 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], "NEW FRAME, %llX, xxxx", instance_get_addr());
- send_usbmessage(&debug_msg[0], n);
- usb_run();
-
- struct TDMAInfo *info = &this->uwbListTDMAInfo[0];
- bool assigned0 = this->slot_assigned(info, 0);
- bool assigned1 = this->slot_assigned(info, 1);
- bool assigned2 = this->slot_assigned(info, 2);
- bool assigned3 = this->slot_assigned(info, 3);
-
- n = sprintf((char*)&debug_msg[0], "ass0: %u ass1: %u ass2: %u ass3: %u,", assigned0, assigned1, assigned2, assigned3);
- 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();
+
}
- while(timeSinceFrameStart > this->slotDuration*this->uwbListTDMAInfo[0].framelength)
+ while(timeSinceFrameStart64 >= frameDuration64)
{
- this->uwbFrameStartTimes[0] += this->slotDuration*this->uwbListTDMAInfo[0].framelength;
- timeSinceFrameStart -= this->slotDuration*this->uwbListTDMAInfo[0].framelength;
+ this->uwbFrameStartTimes64[0] = timestamp_add64(this->uwbFrameStartTimes64[0], frameDuration64);
+ timeSinceFrameStart64 -= frameDuration64;
}
- uint8 slot = timeSinceFrameStart/this->slotDuration; //integer division rounded down
- this->lastSlotStartTime = this->uwbFrameStartTimes[0] + this->slotDuration*(uint32)slot;
+ this->lastFST = this->uwbFrameStartTimes64[0];
+ uint8 slot = timeSinceFrameStart64/(this->slotDuration_us); //integer division rounded down
+ this->lastSlotStartTime64 = this->uwbFrameStartTimes64[0] + (uint64)(this->slotDuration_us*slot);
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "NEW SLOT %u, time_now %lu", slot, time_now);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "NEW SLOT %u, time_now %llu", slot, time_now_us);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
// n = sprintf((char*)&debug_msg[0], "&this->slotAssignments[slot]: %u, &inst->uwbShortAdd %u :", this->slotAssignments[slot], inst->uwbShortAdd);
// send_usbmessage(&debug_msg[0], n);
// usb_run();
-
+//
// if(this->slot_assigned(this, slot) == TRUE)
// {
//
@@ -106,10 +104,6 @@ static bool slot_transition(struct TDMAHandler *this)
}
}
-// uint8 debug_msg[100];
-// n = sprintf((char *)&debug_msg, "inst_processtxrxtimeout(inst) after slot transition");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
instance_getevent(17); //get and clear this event
inst_processtxrxtimeout(inst);
}
@@ -122,128 +116,116 @@ static bool slot_transition(struct TDMAHandler *this)
return transition;
}
-//TODO pass in dw_event..
//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)
{
instance_data_t *inst = instance_get_local_structure_ptr(0);
- uint32 time_now = portGetTickCnt();
+ 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 time_now_us = portGetTickCntMicro();
+
uint8 framelength;
- uint32 timeSinceFrameStart;
+ uint32 timeSinceFrameStart64 = 0;
memcpy(&framelength, &messageData[TDMA_FRAMELENGTH], 1);
//timeSinceFrameStart in message
- memcpy(&timeSinceFrameStart, &messageData[TDMA_TSFS], sizeof(timeSinceFrameStart));
-
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "tsfs %lu :", timeSinceFrameStart);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
+ memcpy(&timeSinceFrameStart64, &messageData[TDMA_TSFS], 6);
- //account for the delay between transmission and reception of the INF message
- //TODO make this a function of the data rate as well!
- //TODO experimentally found this number to be +- 1 millisecond for some reason. 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 xmission frequency!
-
//time from message to tx
//assuming zero since we use DWT_START_TX_IMMEDIATE
- //if this is a problem then include the sys_time in the messsage. then the tx_timestamp can be used to figure out this delay
- //NOTE: if this is done, the timestamp in DWT_START_TX_IMMEDIATE includes the antenna delay, so don't do the next step
//tx antenna delay
- //NOTE: this is stored in inst->txAntennaDelay;
- uint32 tx_antenna_delay = convertdevicetimetosec(inst->txAntennaDelay)*1000;
-
+ 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);
-
+ inst->frameLengths_us[INF] = instance_getmessageduration_us(dw_event->rxLength); //TODO should maybe make sure extended framelength cannot overflow a uint32
- //time to propogate
+ //time to propagate
//NOTE: assuming zero since difference for speed of light travel time over 10cm and 100m is negligible for frame sync purposes
//rx antenna delay
- //NOTE: this is stored in inst->rxAntennaDelay
- //NOTE: we won't use this, becaues the antenna delay is captured by the RX timestamp
-
- //time from rx to system process rx
- uint64 delta_t = inst->timeofRxCallback_dwtime - dw_event->timeStamp;
- double rx_process_delay = convertdevicetimetosec(delta_t)*1000;
-
- //time from system process rx to system frame sync
- uint32 fs_process_delay = get_dt32(time_now, inst->timeofRxCallback);
+ //NOTE: we won't use this, because the antenna delay is captured by the RX timestamp
+ //time from rx timestamp to now
+ uint64 rxfs_process_delay = dwt_getdt(dw_event->timeStamp, dwt_time_now);
- //TODO test with different data rates, should set slot length according to data rate!
- //NOTE: this is pretty close but can possibly be improved
- //initial differences are about 1 to 2 ms, but when using FS_AVERAGE, the differences settle to about 20 microseconds
- uint32 txrx_delay = //this->slotStartDelay //timeSinceFrameStart is now updated after the slotStartDelay
- + tx_antenna_delay
- + inst->frameLengths_us[INF]/1000
- + (uint32)rx_process_delay
- + fs_process_delay;
-
- //NOTE: treat each port tick as about a millisecond.
- //TODO handle number wrapping
- this->uwbFrameStartTimes[srcIndex] = time_now - timeSinceFrameStart - txrx_delay;
-
-
- uint32 less = timeSinceFrameStart
- //+ this->slotStartDelay
- + tx_antenna_delay
- + inst->frameLengths_us[INF]/1000
- + (uint32)rx_process_delay
- + fs_process_delay;
+ uint64 txrx_delay = (uint64)(convertdevicetimetosec(tx_antenna_delay + rxfs_process_delay)*1000000.0) + inst->storePreLen_us;
+ uint64 hisTimeSinceFrameStart_us = timeSinceFrameStart64 + txrx_delay;
+ this->uwbFrameStartTimes64[srcIndex] = timestamp_subtract64(time_now_us, hisTimeSinceFrameStart_us); //TODO consider applying the diff!
if(mode == FS_ADOPT)
{
- this->uwbFrameStartTimes[0] = this->uwbFrameStartTimes[srcIndex];
- this->lastSlotStartTime = this->uwbFrameStartTimes[srcIndex] + timeSinceFrameStart; //TODO handle number wrapping...
+ this->uwbFrameStartTimes64[0] = this->uwbFrameStartTimes64[srcIndex];
+ uint8 slot = hisTimeSinceFrameStart_us/this->slotDuration_us; //integer division rounded down
+ this->lastSlotStartTime64 = this->uwbFrameStartTimes64[0] + (uint64)((this->slotDuration_us)*slot);
}
- else if(mode == FS_AVERAGE)
+ 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
{
- uint32 myFramelengthDuration = this->uwbListTDMAInfo[0].framelength*this->slotDuration;
- uint32 myTimeSinceFrameStart = get_dt32(this->uwbFrameStartTimes[0], time_now);
+ uint64 myFramelengthDuration_us = this->uwbListTDMAInfo[0].framelength*this->slotDuration_us;
+ uint64 myTimeSinceFrameStart_us = get_dt64(this->uwbFrameStartTimes64[0], time_now_us);
+
+ //TODO make sure this can't/doesn't happen
+ //myTimeSinceFrameStart is longer than the FramelengthDuration!!!
+// if(myTimeSinceFrameStart > myFramelengthDuration)
+// {
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "myTimeSinceFrameStart > myTimeSinceFrameStart!");//,%llX, psdu: %d ", instance_get_addr(), psduLength);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+// }
if(this->uwbListTDMAInfo[0].framelength <= framelength)
{
- uint32 hisTimeSinceFrameStartMod = (timeSinceFrameStart + txrx_delay)%myFramelengthDuration;
+ uint64 hisTimeSinceFrameStartMod_us = hisTimeSinceFrameStart_us%myFramelengthDuration_us;
- if(myTimeSinceFrameStart > hisTimeSinceFrameStartMod)
+ if(myTimeSinceFrameStart_us > hisTimeSinceFrameStartMod_us)
{
- uint32 diff = myTimeSinceFrameStart - hisTimeSinceFrameStartMod;
- this->uwbFrameStartTimes[0] += diff/2;
- this->lastSlotStartTime += diff/2;
+ uint64 diff_us = myTimeSinceFrameStart_us - hisTimeSinceFrameStartMod_us;
+
+ this->uwbFrameStartTimes64[0] = timestamp_add64(this->uwbFrameStartTimes64[0], diff_us/2);
+ this->lastSlotStartTime64 = timestamp_add64(this->lastSlotStartTime64, diff_us/2);
}
else
{
- uint32 diff = hisTimeSinceFrameStartMod - myTimeSinceFrameStart;
- this->uwbFrameStartTimes[0] -= diff/2;
- this->lastSlotStartTime -= diff/2;
+ uint64 diff_us = hisTimeSinceFrameStartMod_us - myTimeSinceFrameStart_us;
+
+ this->uwbFrameStartTimes64[0] = timestamp_subtract64(this->uwbFrameStartTimes64[0], diff_us/2);
+ this->lastSlotStartTime64 = timestamp_subtract64(this->lastSlotStartTime64, diff_us/2);
}
}
else
{
- uint32 hisFramelengthDuration = framelength*this->slotDuration;
- uint32 myTimeSinceFrameStartMod = (myTimeSinceFrameStart - txrx_delay)%hisFramelengthDuration;
+ uint64 hisFramelengthDuration_us = framelength*this->slotDuration_us;
+ uint64 myTimeSinceFrameStartMod_us = myTimeSinceFrameStart_us%hisFramelengthDuration_us;
+
+
+// if(timeSinceFrameStart > hisFramelengthDuration)
+// {
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "timeSinceFrameStart > hisFramelengthDuration!");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+// }
- if(timeSinceFrameStart > myTimeSinceFrameStartMod)
+ if(hisTimeSinceFrameStart_us > myTimeSinceFrameStartMod_us)
{
- uint32 diff = timeSinceFrameStart - myTimeSinceFrameStartMod;
- this->uwbFrameStartTimes[0] -= diff/2;
- this->lastSlotStartTime -= diff/2;
+ uint64 diff_us = hisTimeSinceFrameStart_us - myTimeSinceFrameStartMod_us;
+
+ this->uwbFrameStartTimes64[0] = timestamp_subtract64(this->uwbFrameStartTimes64[0], diff_us/2);
+ this->lastSlotStartTime64 = timestamp_subtract64(this->lastSlotStartTime64, diff_us/2);
}
else
{
- uint32 diff = myTimeSinceFrameStartMod - timeSinceFrameStart;
- this->uwbFrameStartTimes[0] += diff/2;
- this->lastSlotStartTime += diff/2;
+ uint64 diff_us = myTimeSinceFrameStartMod_us - hisTimeSinceFrameStart_us;
+
+ this->uwbFrameStartTimes64[0] = timestamp_add64(this->uwbFrameStartTimes64[0], diff_us/2);
+ this->lastSlotStartTime64 = timestamp_add64(this->lastSlotStartTime64, diff_us/2);
}
}
}
@@ -259,13 +241,14 @@ static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
instance_data_t *inst = instance_get_local_structure_ptr(0);
- uint32 time_now = portGetTickCnt();
- 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;
+ 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)
@@ -333,11 +316,11 @@ static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
// usb_run();
//get time since slot start and make sure that's greater than delay
- uint32 timeSinceSlotStart = get_dt32(this->lastSlotStartTime, time_now);
+ uint64 timeSinceSlotStart = get_dt64(this->lastSlotStartTime64, time_now_us);
//make sure that we are in slot 0
- if(timeSinceSlotStart <= this->slotStartDelay)
+ if(timeSinceSlotStart <= this->slotStartDelay_us)
{
uwb_index = -1;
}
@@ -345,7 +328,6 @@ static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
{
//TODO figure out how to make sure we send our SUG packet at the right time...
inst->wait4ack = 0;
-// inst->testAppState = TA_TXSUG_WAIT_SEND; //TODO fold into INF_WAIT_SEND if possible
inst->testAppState = TA_TXINF_WAIT_SEND;
inst->uwbToRangeWith = (uint8)255;
return TRUE;
@@ -365,12 +347,12 @@ static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
// usb_run();
//get time since slot start and make sure that's greater than delay
- uint32 timeSinceSlotStart = get_dt32(this->lastSlotStartTime, time_now);
+ 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)
+ if(timeSinceSlotStart <= this->slotStartDelay_us)
{
uwb_index = -1;
}
@@ -457,7 +439,7 @@ static bool check_blink(struct TDMAHandler *this)
{
uint32 time_now = portGetTickCnt();
uint32 timeSinceDiscoveryStart = get_dt32(this->discoveryStartTime, time_now);
- if(timeSinceDiscoveryStart > this->maxFramelength*this->slotDuration )
+ if(timeSinceDiscoveryStart > this->maxFramelength*this->slotDuration_ms )
{
// uint8 debug_msg[100];
// int n = sprintf((char *)&debug_msg, "in RX_WAIT_DATA, portGetTickCnt(): %lu, inst->last_blink_time: %lu, BLINK_PERIOD_MS: %lu", portGetTickCnt(), inst->last_blink_time, (uint32)BLINK_PERIOD_MS);
@@ -486,7 +468,7 @@ static void populate_inf_msg(struct TDMAHandler *this, uint8 inf_msg_type)
int num_neighbors = instfindnumactiveneighbors(inst);
int num_hidden = instfindnumactivehidden(inst);
- uint32 time_now = portGetTickCnt();
+// uint32 time_now = portGetTickCnt();
//fcode
int msgDataIndex = FCODE;
@@ -507,26 +489,26 @@ static void populate_inf_msg(struct TDMAHandler *this, uint8 inf_msg_type)
//number of neighbors
msgDataIndex = TDMA_NUMN;
- memcpy(&inst->inf_msg.messageData[msgDataIndex], &num_neighbors, 1);
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &num_neighbors, sizeof(uint8));
//number of hidden neighbors
msgDataIndex = TDMA_NUMH;
- memcpy(&inst->inf_msg.messageData[msgDataIndex], &num_hidden, 1);
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &num_hidden, sizeof(uint8));
//self framelength
msgDataIndex = TDMA_FRAMELENGTH;
- memcpy(&inst->inf_msg.messageData[msgDataIndex], &this->uwbListTDMAInfo[0].framelength, 1);
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &this->uwbListTDMAInfo[0].framelength, sizeof(uint8));
//self number of slots
msgDataIndex = TDMA_NUMS;
- memcpy(&inst->inf_msg.messageData[msgDataIndex], &this->uwbListTDMAInfo[0].slotsLength, 1);
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &this->uwbListTDMAInfo[0].slotsLength, sizeof(uint8));
msgDataIndex++;
//self slot assignments
for(int s = 0; s < this->uwbListTDMAInfo[0].slotsLength; s++)
{
- memcpy(&inst->inf_msg.messageData[msgDataIndex], &this->uwbListTDMAInfo[0].slots[s], 1);
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &this->uwbListTDMAInfo[0].slots[s], sizeof(uint8));
msgDataIndex++;
}
@@ -542,17 +524,17 @@ static void populate_inf_msg(struct TDMAHandler *this, uint8 inf_msg_type)
msgDataIndex += inst->addrByteSize;
//framelength
- memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->framelength, 1);
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->framelength, sizeof(uint8));
msgDataIndex++;
//number of slots
- memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->slotsLength, 1);
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->slotsLength, sizeof(uint8));
msgDataIndex++;
//slot assignments
for(int s = 0; s < info->slotsLength; s++)
{
- memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->slots[s], 1);
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->slots[s], sizeof(uint8));
msgDataIndex++;
}
}
@@ -570,17 +552,17 @@ static void populate_inf_msg(struct TDMAHandler *this, uint8 inf_msg_type)
msgDataIndex += inst->addrByteSize;
//framelength
- memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->framelength, 1);
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->framelength, sizeof(uint8));
msgDataIndex++;
//number of slots
- memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->slotsLength, 1);
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->slotsLength, sizeof(uint8));
msgDataIndex++;
//slot assignments
for(int s = 0; s < info->slotsLength; s++)
{
- memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->slots[s], 1);
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->slots[s], sizeof(uint8));
msgDataIndex++;
}
}
@@ -598,166 +580,38 @@ static void populate_inf_msg(struct TDMAHandler *this, uint8 inf_msg_type)
static void update_inf_tsfs(struct TDMAHandler *this)
{
+ //TODO check if tsfs is greater than frameDuration, update if so
+
instance_data_t *inst = instance_get_local_structure_ptr(0);
- uint32 time_now = portGetTickCnt();
+ uint64 time_now_us = portGetTickCntMicro();
int msgDataIndex = TDMA_TSFS;
- uint32 timeSinceFrameStart = get_dt32(this->uwbFrameStartTimes[0], time_now); //TODO handle number wrapping
- memcpy(&inst->inf_msg.messageData[msgDataIndex], &timeSinceFrameStart, sizeof(uint32));
-}
+ uint64 timeSinceFrameStart64 = get_dt64(this->uwbFrameStartTimes64[0], time_now_us);
+ uint64 frameDuration = this->slotDuration_us*this->uwbListTDMAInfo[0].framelength;
+ while(timeSinceFrameStart64 > frameDuration)
+ {
+ this->uwbFrameStartTimes64[0] = timestamp_add64(this->uwbFrameStartTimes64[0], frameDuration);
+ timeSinceFrameStart64 -= frameDuration;
+ }
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &timeSinceFrameStart64, 6);
-////TODO
-////while collecting, build/deconflict slotAssignments...
-////when receive a INF_UPDATE or INF_SUG, rebuild/deconflict slotAssignments
-////when timeout, rebuild/deconflict slotAssignments
-//static void rebuild_slot_assignments(struct TDMAHandler *this)
-//{
-// uint8 unassigned = 255;
-// for(int i = 0; i < this->maxFramelength; i++)
-// {
-// memcpy(&this->slotAssignments[i], &unassigned, sizeof(uint8));
-// }
-// this->framelength = MIN_FRAMELENGTH;
-//
-//
-// //dont forget to include my own assignments...
-// for(int i = 0; i < this->mySlotsLength; i++)
-// {
-// bool double_frame = FALSE;
-// uint8 slot = this->mySlots[i];
-//
-// //check if slot is taken
-// if(slot >= this->framelength)
-// {
-// uint8 mod_slot = slot%this->framelength;
-// if(mod_slot == 0)
-// {
-// //slots not allowed to be assigned to the zeroth slot
-// double_frame = TRUE;
-// }
-// else
-// {
-// if(memcmp(&this->slotAssignments[mod_slot], &unassigned, sizeof(uint8)) == 0)
-// {
-// //slot not assigned
-// memcpy(&this->slotAssignments[mod_slot], &i, sizeof(uint8));
-// }
-// else if(memcmp(&this->slotAssignments[mod_slot], &i, sizeof(uint8)) != 0)
-// {
-// //already assigned to another UWB,
-// //double the frame and start over!(?) should I instead consider framelengths and such?
-// double_frame = TRUE;
-// }
-// }
-//
-// }
-// else if(slot_j < this->framelength)
-// {
-// while(slot_j < this->framelength)
-// {
-// if(memcmp(&this->slotAssignments[slot_j], &unassigned, sizeof(uint8)) == 0)
-// {
-// //slot not assigned
-// memcpy(&this->slotAssignments[slot_j], &i, sizeof(uint8));
-// }
-// else if(memcmp(&this->slotAssignments[slot_j], &i, sizeof(uint8)) != 0)
-// {
-// //already assigned to another UWB,
-// //double the frame and start over!(?) should I instead consider framelengths and such?
-// double_frame = TRUE;
-// break;
-// }
-// slot_j += framelength_i;
-// }
-// }
-//
-// if(double_frame == TRUE)
-// {
-// this->framelength *= 2;
-// i = 0;
-// j = 0;
-// }
-//
-//
-// }
-//
-// //getting stuck in here. this->framelength sometimes increases till overflow and gets set to 0
-// for(int i = 0; i < inst->uwbListLen; i++)
-// {
-// if(inst->uwbListType[i] != UWB_LIST_INACTIVE)
-// {
-// uint8 framelength_i = this->uwbFramelengths[i];
-// for(int j = 0; j < this->uwbListSlotsLengths[i]; j++)
-// {
-// //get slot
-// uint8 slot_j;
-// bool double_frame = FALSE;
-// memcpy(&slot_j, &this->uwbListSlots[i][j], 1);
-//
-//
-// //check if slot is taken
-// if(slot_j >= this->framelength)
-// {
-// uint8 mod_slot = slot_j%this->framelength;
-// if(mod_slot == 0)
-// {
-// //slots not allowed to be assigned to the zeroth slot
-// double_frame = TRUE;
-// }
-// else
-// {
-//// if(memcmp(&this->slotAssignments[mod_slot], &zero, sizeof(uint8)) == 0)
-// if(memcmp(&this->slotAssignments[mod_slot], &unassigned, sizeof(uint8)) == 0)
-// {
-// //slot not assigned
-// memcpy(&this->slotAssignments[mod_slot], &i, sizeof(uint8));
-// }
-// else if(memcmp(&this->slotAssignments[mod_slot], &i, sizeof(uint8)) != 0)
-// {
-// //already assigned to another UWB,
-// //double the frame and start over!(?) should I instead consider framelengths and such?
-// double_frame = TRUE;
-// }
-// }
-//
-// }
-// else if(slot_j < this->framelength)
-// {
-// while(slot_j < this->framelength)
-// {
-// if(memcmp(&this->slotAssignments[slot_j], &unassigned, sizeof(uint8)) == 0)
-// {
-// //slot not assigned
-// memcpy(&this->slotAssignments[slot_j], &i, sizeof(uint8));
-// }
-// else if(memcmp(&this->slotAssignments[slot_j], &i, sizeof(uint8)) != 0)
-// {
-// //already assigned to another UWB,
-// //double the frame and start over!(?) should I instead consider framelengths and such?
-// double_frame = TRUE;
-// break;
-// }
-// slot_j += framelength_i;
-// }
-// }
-//
-// if(double_frame == TRUE)
-// {
-// this->framelength *= 2;
-// i = 0;
-// j = 0;
-// }
-//
-// // uint16 uwbShortAdd = address64to16(&inst->uwbList[i][0]);
-// // uint8 index = msgDataIndex + 2*this->uwbListSlots[i][j];
-// // memcpy(&inst->inf_msg.messageData[msgDataIndex + (int)(inst->addrByteSize*this->uwbListSlots[i][j])], &uwbShortAdd, 2);
-// }
-// }
-//
-// }
-//
-//
-//}
+
+ uint64 tsinf = get_dt64(this->lastINFtx, time_now_us);
+ memcpy(&inst->inf_msg.messageData[1], &tsinf, 6);
+ this->lastINFtx = time_now_us;
+
+
+// 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);
+}
//Procedure for processing INF SUG, INF REG, and INF UPDATE
@@ -782,16 +636,26 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
bool tdma_modified = FALSE;
+
+ uint32 t_start = portGetTickCnt();
+
if((mode != CLEAR_ALL_COPY) && //happens when we creat a new network
(mode != CLEAR_LISTED_COPY) && //happens most of the time while processing
(mode != COPY)) //happens when collecting inf messages
{
//only process if valid mode supplied
- return FALSE;
+ return tdma_modified;
}
+ bool safeAssign = FALSE;
+ if(mode == COPY)
+ {
+ safeAssign = TRUE;
+ }
+
+
uint8 inf_msg_type;
- memcpy(&inf_msg_type, &messageData[FCODE], 1);
+ memcpy(&inf_msg_type, &messageData[FCODE], sizeof(uint8));
if((inf_msg_type != RTLS_DEMO_MSG_INF_REG) &&
(inf_msg_type != RTLS_DEMO_MSG_INF_UPDATE) &&
@@ -803,24 +667,19 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
}
instance_data_t *inst = instance_get_local_structure_ptr(0);
-// uint8 srcIndex = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
inst->uwbListType[srcIndex] = UWB_LIST_NEIGHBOR;
- uint32 timeSinceFrameStart;
uint8 numNeighbors;
uint8 numHidden;
uint8 framelength;
uint8 numSlots;
uint8 slot;
struct TDMAInfo *info;
- uint8 address[8] = {0, 0, 0, 0, 0, 0, 0, 0};
-// uint8 uwb_index;
- memcpy(&timeSinceFrameStart, &messageData[TDMA_TSFS], sizeof(timeSinceFrameStart));
- memcpy(&numNeighbors, &messageData[TDMA_NUMN], 1);
- memcpy(&numHidden, &messageData[TDMA_NUMH], 1);
- memcpy(&framelength, &messageData[TDMA_FRAMELENGTH], 1);
- memcpy(&numSlots, &messageData[TDMA_NUMS], 1);
+ memcpy(&numNeighbors, &messageData[TDMA_NUMN], sizeof(uint8));
+ memcpy(&numHidden, &messageData[TDMA_NUMH], sizeof(uint8));
+ memcpy(&framelength, &messageData[TDMA_FRAMELENGTH], sizeof(uint8));
+ memcpy(&numSlots, &messageData[TDMA_NUMS], sizeof(uint8));
int msgDataIndex = TDMA_NUMS + 1;
@@ -830,23 +689,16 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
{
uwbListInMsg[i] = FALSE;
}
+ uwbListInMsg[srcIndex] = TRUE;
if(mode == CLEAR_ALL_COPY)
{
- //clear all TDMA assignments
+ //clear all TDMA assignments and reset framelength to MIN
this->tdma_free_all_slots(this);
tdma_modified = TRUE;
}
-// else if(mode == CLEAR_LISTED_COPY)
-// {
-// //first check for differences and exit if none exist...
-// if(this->check_tdma_diff(this, messageData, srcIndex) == FALSE)
-// {
-// return FALSE;
-// }
-// }
- msgDataIndex = TDMA_NUMS + 1;
+// msgDataIndex = TDMA_NUMS + 1;
//copy slot assignments for source UWB
info = &this->uwbListTDMAInfo[srcIndex];
if(framelength != info->framelength)
@@ -854,31 +706,61 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
tdma_modified = TRUE;
}
+ //check if the tdma has been modified
+ if(tdma_modified == FALSE) //dont look for any more differences if we already know one exists
+ {
+ //frist check if same number of slots
+ if(numSlots == info->slotsLength)
+ {
+ //then check if each incoming slot is already assigned
+ for(int i = 0; i < numSlots; i++)
+ {
+ memcpy(&slot, &messageData[msgDataIndex], sizeof(uint8));
+ msgDataIndex++;
+
+ if(this->slot_assigned(info, slot) == FALSE)
+ {
+ tdma_modified = TRUE;
+ break;
+ }
+ }
+ }
+ else
+ {
+ tdma_modified = TRUE;
+ }
+ }
+
if(mode == CLEAR_LISTED_COPY)
{
this->free_slots(info); //do after cheking framelength because framelength will be reset
}
+
info->framelength = MAX(framelength, info->framelength);
+ msgDataIndex = TDMA_NUMS + 1;
for(int s = 0; s < numSlots; s++)
{
- memcpy(&slot, &messageData[msgDataIndex], 1);
+ memcpy(&slot, &messageData[msgDataIndex], sizeof(uint8));
msgDataIndex++;
- if(this->assign_slot(info, slot) == TRUE)
- {
- tdma_modified = TRUE;
- }
+ this->assign_slot(info, slot, safeAssign);
+
+// if(this->assign_slot(info, slot) == TRUE)
+// {
+// tdma_modified = TRUE; //TODO this is not right, it should somehow compare before and after...
+// }
}
- uwbListInMsg[srcIndex] = TRUE;
+
for(int i = 0; i < numNeighbors; i++)
{
- memcpy(&address, &messageData[msgDataIndex], inst->addrByteSize);
+ uint8 address[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+ memcpy(&address[0], &messageData[msgDataIndex], inst->addrByteSize);
msgDataIndex += inst->addrByteSize;
uint8 uwb_index = instgetuwblistindex(inst, &address[0], inst->addrByteSize);
- if(uwb_index != 0) //0 reserved for self
+ if(uwb_index != 0)
{
if(inst->uwbListType[uwb_index] == UWB_LIST_INACTIVE || inst->uwbListType[uwb_index] == UWB_LIST_TWICE_HIDDEN)
{
@@ -892,12 +774,36 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
- memcpy(&framelength, &messageData[msgDataIndex], 1);
- if(framelength != info->framelength)
+ memcpy(&framelength, &messageData[msgDataIndex], sizeof(uint8));
+ msgDataIndex++;
+ memcpy(&numSlots, &messageData[msgDataIndex], sizeof(uint8));
+ msgDataIndex++;
+ int msgDataIndexSave = msgDataIndex;
+
+ //check if the tdma has been modified
+ if(tdma_modified == FALSE) //dont look for any more differences if we already know one exists
{
- tdma_modified = TRUE;
+ //frist check if same framelength and number of slots
+ if(framelength == info->framelength && numSlots == info->slotsLength)
+ {
+ //then check if each incoming slot is already assigned
+ for(int i = 0; i < numSlots; i++)
+ {
+ memcpy(&slot, &messageData[msgDataIndex], sizeof(uint8));
+ msgDataIndex++;
+
+ if(this->slot_assigned(info, slot) == FALSE)
+ {
+ tdma_modified = TRUE;
+ break;
+ }
+ }
+ }
+ else
+ {
+ tdma_modified = TRUE;
+ }
}
- msgDataIndex++;
if(mode == CLEAR_LISTED_COPY)
{
@@ -906,24 +812,24 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
info->framelength = MAX(framelength, info->framelength);
- memcpy(&numSlots, &messageData[msgDataIndex], 1);
- msgDataIndex++;
-
+ msgDataIndex = msgDataIndexSave;
for(int s = 0; s < numSlots; s++)
{
- memcpy(&slot, &messageData[msgDataIndex], 1);
+ memcpy(&slot, &messageData[msgDataIndex], sizeof(uint8));
msgDataIndex++;
- if(this->assign_slot(info, slot) == TRUE)
- {
- tdma_modified = TRUE;
- }
+ this->assign_slot(info, slot, safeAssign);
+// if(this->assign_slot(info, slot) == TRUE)
+// {
+// tdma_modified = TRUE; //TODO also not right! need to check modification
+// }
}
}
for(int i = 0; i < numHidden; i++)
{
- memcpy(&address, &messageData[msgDataIndex], inst->addrByteSize);
+ uint8 address[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+ memcpy(&address[0], &messageData[msgDataIndex], inst->addrByteSize);
msgDataIndex += inst->addrByteSize;
uint8 uwb_index = instgetuwblistindex(inst, &address[0], inst->addrByteSize);
@@ -938,32 +844,55 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
uwbListInMsg[uwb_index] = TRUE;
info = &this->uwbListTDMAInfo[uwb_index];
- memcpy(&framelength, &messageData[msgDataIndex], 1);
- if(framelength != info->framelength)
+ memcpy(&framelength, &messageData[msgDataIndex], sizeof(uint8));
+ msgDataIndex++;
+ memcpy(&numSlots, &messageData[msgDataIndex], sizeof(uint8));
+ msgDataIndex++;
+ int msgDataIndexSave = msgDataIndex;
+
+ //check if the tdma has been modified
+ if(tdma_modified == FALSE) //dont look for any more differences if we already know one exists
{
- tdma_modified = TRUE;
+ //frist check if same framelength and number of slots
+ if(framelength == info->framelength && numSlots == info->slotsLength)
+ {
+ //then check if each incoming slot is already assigned
+ for(int i = 0; i < numSlots; i++)
+ {
+ memcpy(&slot, &messageData[msgDataIndex], sizeof(uint8));
+ msgDataIndex++;
+
+ if(this->slot_assigned(info, slot) == FALSE)
+ {
+ tdma_modified = TRUE;
+ break;
+ }
+ }
+ }
+ else
+ {
+ tdma_modified = TRUE;
+ }
}
if(mode == CLEAR_LISTED_COPY)
{
this->free_slots(info); //do after checking for difference because will reset framelength as well
}
-
info->framelength = MAX(framelength, info->framelength);
- msgDataIndex++;
-
- memcpy(&numSlots, &messageData[msgDataIndex], 1);
- msgDataIndex++;
+ msgDataIndex = msgDataIndexSave;
+// int msgDataIndex = msgDataIndexSave;
for(int s = 0; s < numSlots; s++)
{
- memcpy(&slot, &messageData[msgDataIndex], 1);
+ memcpy(&slot, &messageData[msgDataIndex], sizeof(uint8));
msgDataIndex++;
- if(this->assign_slot(info, slot) == TRUE) //the only problem i see with this is that it does not give me a good way to isolate which ones were or weren't modified, this is a problem for deconflict logic...
- {
- tdma_modified = TRUE;
- }
+ this->assign_slot(info, slot, safeAssign);
+// if(this->assign_slot(info, slot) == TRUE) //the only problem i see with this is that it does not give me a good way to isolate which ones were or weren't modified, this is a problem for deconflict logic...
+// {
+// tdma_modified = TRUE;
+// }
}
}
@@ -976,7 +905,7 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
{
for(int j = i + 1; j < inst->uwbListLen; j++)
{
- if(uwbListInMsg[i] == FALSE && uwbListInMsg[j] == TRUE)
+ if((uwbListInMsg[i] == FALSE && uwbListInMsg[j] == TRUE) || (uwbListInMsg[i] == TRUE && uwbListInMsg[j] == FALSE))
{
if((inst->uwbListType[i] == UWB_LIST_NEIGHBOR && inst->uwbListType[j] == UWB_LIST_NEIGHBOR) ||
@@ -985,7 +914,10 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
(inst->uwbListType[j] == UWB_LIST_NEIGHBOR && inst->uwbListType[i] == UWB_LIST_HIDDEN))
{
//TODO make sure this is okay. will i need to ensure the assignments from the message are maintained?
- this->deconflict_uwb_pair(this, &this->uwbListTDMAInfo[i], &this->uwbListTDMAInfo[j]);
+ if(this->deconflict_uwb_pair(this, &this->uwbListTDMAInfo[i], &this->uwbListTDMAInfo[j]) == TRUE)
+ {
+ tdma_modified = TRUE;
+ }
}
}
}
@@ -1004,6 +936,12 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
}
}
+ uint32 deltat = get_dt32(t_start, portGetTickCnt());
+
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "process_inf_time,%X,%lu", inst->uwbShortAdd, deltat);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
return tdma_modified;
@@ -1582,9 +1520,9 @@ static bool poll_delay(struct TDMAHandler *this, uint32 time_now_offset, uint32
{
bool delay = FALSE;
- uint32 time_now = portGetTickCnt();
- uint32 timeSinceSlotStart = get_dt32(this->lastSlotStartTime, time_now);
- if(timeSinceSlotStart > this->slotStartDelay)
+ uint64 time_now_us = portGetTickCntMicro();
+ uint64 timeSinceSlotStart = get_dt64(this->lastSlotStartTime64, time_now_us);
+ if(timeSinceSlotStart >= this->slotStartDelay_us)
{
delay = FALSE;
}
@@ -1610,13 +1548,29 @@ static bool slot_assigned(struct TDMAInfo *info, uint8 slot)
return assigned;
}
-static bool assign_slot(struct TDMAInfo *info, uint8 slot)
+static bool assign_slot(struct TDMAInfo *info, uint8 slot, bool safeAssign)
{
//NOTE: deconflicting happens elsewhere
bool retval = FALSE;
- //if not assigned, increase slots size and add slot index to end of array (array is unsorted)
- if(slot_assigned(info, slot) == FALSE)
+ if(safeAssign == TRUE)//when using safeAssign, first check if the slot is assigned
+ {
+ //if not assigned, increase slots size and add slot index to end of array (array is unsorted)
+ if(slot_assigned(info, slot) == FALSE)
+ {
+ uint8 *newSlots = malloc(sizeof(uint8)*(info->slotsLength + 1));
+ memcpy(&newSlots[0], &info->slots[0], sizeof(uint8)*info->slotsLength);
+ memcpy(&newSlots[info->slotsLength], &slot, 1);
+
+ free(info->slots);
+ info->slots = NULL;
+ info->slots = newSlots;
+ info->slotsLength += 1;
+
+ retval = TRUE;
+ }
+ }
+ else
{
uint8 *newSlots = malloc(sizeof(uint8)*(info->slotsLength + 1));
memcpy(&newSlots[0], &info->slots[0], sizeof(uint8)*info->slotsLength);
@@ -1630,6 +1584,7 @@ static bool assign_slot(struct TDMAInfo *info, uint8 slot)
retval = TRUE;
}
+
return retval;
}
@@ -1645,12 +1600,23 @@ static bool assign_slot(struct TDMAInfo *info, uint8 slot)
//4.) Double the Frame (DF)
// applicable if 2.) and 3.) not applicable
// double own framelength and go back to 2.)
+
+//procedure above doesn't seem to alwasy work, even for 3 UWBs... each assigned a slot out of four
+//1,2,3 find no slots to assign and 4 wont change that... will just double the frame forever...
+//actually it should work, because when we have double the framelength, the other uwbs virtually have two slots...
static void find_assign_slot(struct TDMAHandler *this)
{
//NOTE: this assumes that all other TDMAInfo stored in the TDMAHandler are not in conflict with each other
instance_data_t *inst = instance_get_local_structure_ptr(0);
struct TDMAInfo *info = &this->uwbListTDMAInfo[0];
+
+ //if this UWB was somehow reset, recover its prior slot assignment info from the network traffic
+ if(info->slotsLength > 0)
+ {
+ return;
+ }
+
bool assignment_made = FALSE;
//set framelength
@@ -1660,22 +1626,49 @@ static void find_assign_slot(struct TDMAHandler *this)
while(TRUE)
{
//GU
- for(int i = 1; i < info->framelength; i++) //TODO make sure we dont accidentally assign to slot 0
+ for(uint8 i = 1; i < info->framelength; i++) //TODO make sure we don't accidentally assign to slot 0
{
bool assigned = FALSE;
- for(int u = 1; u < inst->uwbListLen; u++)//0 reserved for self
+ for(uint8 u = 1; u < inst->uwbListLen; u++)//0 reserved for self
{
- for(int su = 0; su < this->uwbListTDMAInfo[u].slotsLength; su++)
+ for(uint8 su = 0; su < this->uwbListTDMAInfo[u].slotsLength; su++)
{
uint8 slot_su;
memcpy(&slot_su, &this->uwbListTDMAInfo[u].slots[su], sizeof(uint8));
- uint8 mod_slot_su = slot_su % info->framelength;
- if(slot_su == i || mod_slot_su == i)
+ if(info->framelength > this->uwbListTDMAInfo[u].framelength)
+ {
+ uint8 mod_i = i%this->uwbListTDMAInfo[u].framelength;
+
+ if(slot_su == mod_i)
+ {
+ //slot assigned to this uwb
+ assigned = TRUE;
+ }
+ }
+ else if(info->framelength < this->uwbListTDMAInfo[u].framelength)
+ {
+ uint8 mod_slot_su = slot_su % info->framelength;
+
+ if(mod_slot_su == i)
+ {
+ //slot assigned to this uwb
+ assigned = TRUE;
+ }
+ }
+ else //same framelength
+ {
+ if(slot_su == i)
+ {
+ //slot assigned to this uwb
+ assigned = TRUE;
+ }
+ }
+
+
+ if(assigned == TRUE)
{
- //slot assigned to this uwb
- assigned = TRUE;
break;
}
}
@@ -1689,8 +1682,8 @@ static void find_assign_slot(struct TDMAHandler *this)
//slot not assigned, assign to self
if(assigned == FALSE)
{
- this->assign_slot(info, i);
- assignment_made = TRUE;
+ this->assign_slot(info, i, FALSE);
+ assignment_made = TRUE; //TODO just stop here? or get all unassigned slots?
}
}
@@ -1701,25 +1694,31 @@ static void find_assign_slot(struct TDMAHandler *this)
//RMA
//find UWB with greatest number of slot assignments
- uint8 max_assignments = 0;
+ uint8 max_assignments = 0; //TODO may need uint16?
uint8 max_uwb_index = 255;
- for(int u = 1; u < inst->uwbListLen; u++)//0 reserved for self
+ for(uint8 u = 1; u < inst->uwbListLen; u++)//0 reserved for self
{
- if(this->uwbListTDMAInfo[u].slotsLength > max_assignments)
+ uint8 slotsLength = this->uwbListTDMAInfo[u].slotsLength; //TODO may need uint16?
+ if(info->framelength > this->uwbListTDMAInfo[u].framelength && this->uwbListTDMAInfo[u].slotsLength != 0)
{
- max_assignments = this->uwbListTDMAInfo[u].slotsLength;
+ slotsLength *= info->framelength/this->uwbListTDMAInfo[u].framelength;
+ }
+
+ if(slotsLength > max_assignments)
+ {
+ max_assignments = slotsLength;
max_uwb_index = u;
}
}
- if(max_uwb_index != 255)
+ if(max_uwb_index != 255 && max_assignments > 1)
{
uint8 slot;
memcpy(&slot, &this->uwbListTDMAInfo[max_uwb_index].slots[0], sizeof(uint8));
uint8 mod_slot = slot % info->framelength;
- this->free_slot(&this->uwbListTDMAInfo[max_uwb_index], slot);
- this->assign_slot(info, mod_slot);
+ this->assign_slot(info, mod_slot, TRUE);
+ this->deconflict_uwb_pair(this, info, &this->uwbListTDMAInfo[max_uwb_index]); //TODO consider handling differently (could maybe be more efficient?)
assignment_made = TRUE;
}
@@ -1736,20 +1735,28 @@ static void find_assign_slot(struct TDMAHandler *this)
static void build_new_network(struct TDMAHandler *this)
{
instance_data_t *inst = instance_get_local_structure_ptr(0);
- uint32 time_now = portGetTickCnt();
+// uint32 time_now = portGetTickCnt();
+ uint32 time_now_us = portGetTickCntMicro();
+ bool safeAssign = FALSE;
//clear all tdma information
this->tdma_free_all_slots(this);
//build the initial TDMA
- this->uwbFrameStartTimes[0] = time_now - this->slotDuration;//TODO handle timer wrapping...
- this->lastSlotStartTime = time_now;
+// this->uwbFrameStartTimes_stm32[0] = time_now - this->slotDuration_ms;//TODO handle timer wrapping...
+// this->lastSlotStartTime_stm32 = time_now;
+
+ //build the initial TDMA
+ this->uwbFrameStartTimes64[0] = timestamp_subtract64(time_now_us, this->slotDuration_us);
+ this->lastSlotStartTime64 = time_now_us;
+
+
this->uwbListTDMAInfo[0].framelength = (uint8)MIN_FRAMELENGTH;
this->uwbListTDMAInfo[inst->uwbToRangeWith].framelength = (uint8)MIN_FRAMELENGTH;
- this->assign_slot(&this->uwbListTDMAInfo[0], 1);
- this->assign_slot(&this->uwbListTDMAInfo[inst->uwbToRangeWith], 2);
- this->assign_slot(&this->uwbListTDMAInfo[0], 3);
+ this->assign_slot(&this->uwbListTDMAInfo[0], 1, safeAssign);
+ this->assign_slot(&this->uwbListTDMAInfo[inst->uwbToRangeWith], 2, safeAssign);
+ this->assign_slot(&this->uwbListTDMAInfo[0], 3, safeAssign);
}
@@ -1924,7 +1931,7 @@ static bool deconflict_uwb_pair(struct TDMAHandler *this, struct TDMAInfo *info_
for(int sb = 0; sb < info_b->slotsLength; sb++)
{
uint8 slot_sb;
- memcpy(&slot_sb, &info_a->slots[sb], 1);
+ memcpy(&slot_sb, &info_b->slots[sb], 1);
//check if slot is taken
@@ -2087,12 +2094,12 @@ static bool self_conflict(struct TDMAHandler *this)
for(int sa = 0; sa < info_a->slotsLength; sa++)
{
uint8 slot_sa;
- memcpy(&slot_sa, &info_a->slots[sa], 1);
+ memcpy(&slot_sa, &info_a->slots[sa], sizeof(uint8));
for(int sb = 0; sb < info_b->slotsLength; sb++)
{
uint8 slot_sb;
- memcpy(&slot_sb, &info_a->slots[sb], 1);
+ memcpy(&slot_sb, &info_b->slots[sb], sizeof(uint8));
//check if slot is taken
@@ -2276,29 +2283,37 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
instance_data_t *inst = instance_get_local_structure_ptr(0);
//TODO handle number wrapping
- uint32 tcommon;
- uint32 shortestFrameDuration = this->maxFramelength*this->slotDuration;
+ uint64 time_now_us = portGetTickCntMicro();
+ uint64 tcommon;
+ uint64 shortestFrameDuration = this->maxFramelength*this->slotDuration_us;
+// uint32 tcommon;
+// uint32 shortestFrameDuration = this->maxFramelength*this->slotDuration_ms;
uint8 numNeighbors = instfindnumactiveneighbors(inst);
- uint32 tnext[numNeighbors];
- uint32 latest_tnext;
+// uint32 tnext[numNeighbors];
+// uint32 latest_tnext;
+ uint64 tnext[numNeighbors];
+ uint64 latest_tnext;
uint8 neighborIndices[numNeighbors];
uint8 nidx = 0;
+ uint64 slotDuration_us = this->slotDuration_us;
for(int i = 1; i < inst->uwbListLen; i++)//0 reserved for self
{
if(inst->uwbListType[i] == UWB_LIST_NEIGHBOR)
{
neighborIndices[nidx] = i;
- tnext[nidx] = this->uwbFrameStartTimes[i];
- while(time_now > tnext[nidx]) //TODO handle number wrapping...
+// tnext[nidx] = this->uwbFrameStartTimes_stm32[i];
+ tnext[nidx] = this->uwbFrameStartTimes64[i];
+ while(time_now_us > tnext[nidx]) //TODO handle number wrapping...
{
- tnext[nidx] += this->uwbListTDMAInfo[i].framelength*this->slotDuration;
+ tnext[nidx] += this->uwbListTDMAInfo[i].framelength*slotDuration_us;
}
nidx++;
- if(this->uwbListTDMAInfo[i].framelength*this->slotDuration < shortestFrameDuration)
+// if(this->uwbListTDMAInfo[i].framelength*this->slotDuration_ms < shortestFrameDuration)
+ if(this->uwbListTDMAInfo[i].framelength*slotDuration_us < shortestFrameDuration)
{
- shortestFrameDuration = this->uwbListTDMAInfo[i].framelength*this->slotDuration;
+ shortestFrameDuration = this->uwbListTDMAInfo[i].framelength*slotDuration_us;
}
}
}
@@ -2311,7 +2326,8 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
converged = TRUE;
for(int i = 0; i < nidx; i++)
{
- uint32 frameduration = this->uwbListTDMAInfo[neighborIndices[i]].framelength*this->slotDuration;
+// uint32 frameduration = this->uwbListTDMAInfo[neighborIndices[i]].framelength*this->slotDuration_ms;
+ uint64 frameduration = this->uwbListTDMAInfo[neighborIndices[i]].framelength*slotDuration_us;
while(tnext[i] < tcommon && tcommon - tnext[i] >= frameduration) //include small buffer
{
tnext[i] += frameduration;
@@ -2333,7 +2349,8 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
//expire as the beginning of the common frame start time
- this->discovery_mode_duration = get_dt32(time_now, latest_tnext);
+// this->discovery_mode_duration = get_dt32(time_now, latest_tnext);
+ this->discovery_mode_duration = (uint32)(get_dt64(time_now_us, latest_tnext)/1000); //TODO make duration in us?
this->discovery_mode_expires = TRUE;
break;
}
@@ -2486,21 +2503,18 @@ static void usb_dump_tdma(struct TDMAHandler *this)
static struct TDMAHandler new(){
struct TDMAHandler ret = {};
- ret.slotDuration = 50; //TODO should be a function of the data rate and the max number of UWBs
+ ret.slotDuration_ms = 50; //TODO should be a function of the data rate and the max number of UWBs
+ ret.slotDuration_us = ret.slotDuration_ms*1000;
ret.slot_transition = &slot_transition;
ret.frame_sync = &frame_sync;
ret.tx_select = &tx_select;
ret.check_blink = &check_blink;
- // ret.rebuild_slot_assignments = &rebuild_slot_assignments;
-
ret.populate_inf_msg = &populate_inf_msg;
ret.update_inf_tsfs = &update_inf_tsfs;
ret.process_inf_msg = &process_inf_msg;
ret.check_tdma_diff = &check_tdma_diff;
-// ret.populate_sug_msg = &populate_sug_msg; //TODO remove
-// ret.process_sug_msg = &process_sug_msg; //TODO remove
ret.poll_delay = &poll_delay;
ret.slot_assigned = &slot_assigned;
@@ -2516,14 +2530,13 @@ static struct TDMAHandler new(){
ret.set_discovery_mode = &set_discovery_mode;
ret.check_discovery_mode_expiration = &check_discovery_mode_expiration;
ret.usb_dump_tdma = &usb_dump_tdma;
-// ret.get_largest_framelength = &get_largest_framelength;
ret.deconflict_slot_assignments = &deconflict_slot_assignments;
ret.deconflict_uwb_pair = &deconflict_uwb_pair;
ret.deconflict_slot_pair = &deconflict_slot_pair;
ret.self_conflict = &self_conflict;
- uint32 time_now = portGetTickCnt();
+ uint32 time_now_us = portGetTickCntMicro();
//TODO create a function to clear the frame information!
//and have it called in enter_discovery function!
@@ -2535,11 +2548,6 @@ static struct TDMAHandler new(){
ret.maxFramelength *= 2;
}
-// ret.myTDMAInfo.uwbIndex = 254; //254 reserved to identify self
-// ret.myTDMAInfo.framelength = (uint8)MIN_FRAMELENGTH;
-// ret.myTDMAInfo.slots = NULL;
-// ret.myTDMAInfo.slotsLength = 0;
-
for(int i = 0; i < UWB_LIST_SIZE; i++)
{
ret.uwbListTDMAInfo[i].uwbIndex = i;
@@ -2548,15 +2556,20 @@ static struct TDMAHandler new(){
ret.uwbListTDMAInfo[i].slotsLength = 0;
}
- ret.uwbFrameStartTimes[0] = time_now;
- ret.lastSlotStartTime = time_now;
+ ret.uwbFrameStartTimes64[0] = time_now_us;
+ ret.lastFST = time_now_us;
+ ret.lastSlotStartTime64 = time_now_us;
ret.infPollSentThisSlot = FALSE;
- ret.slotStartDelay = 4;
+ ret.slotStartDelay_us = 4000;
ret.infMessageLength = 0;
ret.enter_discovery_mode(&ret);
- ret.collectInfDuration = ret.maxFramelength*ret.slotDuration;
- ret.waitInfDuration = ret.collectInfDuration;
+// ret.collectInfDuration = ret.maxFramelength*ret.slotDuration;
+ ret.collectInfDuration = 10000;
+ 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 e7a21c563bffd4d50a8257b1041dd1021109311e..917ad3603727bb78df3dc30640d4c01770967b57 100644
--- a/src/application/tdma_handler.h
+++ b/src/application/tdma_handler.h
@@ -35,17 +35,23 @@ struct TDMAHandler
uint8 maxFramelength;
uint8 slotAssingmentSelfIndex;
-// struct TDMAInfo myTDMAInfo;
-
struct TDMAInfo uwbListTDMAInfo[UWB_LIST_SIZE];
- uint32 uwbFrameStartTimes[UWB_LIST_SIZE]; //TODO propagate the use of this
-// uint32 frameStartTime;
- uint32 lastSlotStartTime;
- uint32 slotDuration; //TODO make variable in duration based on UWB_LIST_SIZE
+ //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 infPollSentThisSlot;
- uint32 slotStartDelay; //time between slot start and transmission within that slot
+ //TODO can probably use a smaller data type...
+ uint64 slotStartDelay_us; //time between slot start and transmission within that slot
//discovery variables
DISCOVERY_MODE discovery_mode;
@@ -60,26 +66,15 @@ struct TDMAHandler
uint16 infMessageLength;
-// bool waitForRngInit;
-// bool waitForInf;
-// uint32 waitForInfStart;
-// uint32 waitForRngInitStart;
-
//class functions
bool (*slot_transition)(struct TDMAHandler *this);
-// void (*frame_sync)(struct TDMAHandler *this, uint8 *messageData, uint16 rxLength, 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 (*update_inf_tsfs)(struct TDMAHandler *this);
bool (*tx_select)(struct TDMAHandler *this);
-// bool (*rx_accept)(struct TDMAHandler *this, uint8 uwb_index, uint8 *rxd_event, uint8 *msgu, uint8 fcode_index);
bool (*check_blink)(struct TDMAHandler *this);
-// void (*rebuild_slot_assignments)(struct TDMAHandler *this);
void (*populate_inf_msg)(struct TDMAHandler *this, uint8 inf_msg_type);
-// void (*process_inf_msg)(struct TDMAHandler *this); //TODO implement
bool (*process_inf_msg)(struct TDMAHandler *this, uint8 *messageData, uint8 srcIndex, INF_PROCESS_MODE mode);
bool (*check_tdma_diff)(struct TDMAHandler *this, uint8 *messageData, uint8 *srcAddr);
-// void (*populate_sug_msg)(struct TDMAHandler *this);//TODO remove this
-// void (*process_sug_msg)(struct TDMAHandler *this, uint8 *messageData, uint8 *srcAddr);//TODO remove this
bool (*poll_delay)(struct TDMAHandler *this, uint32 time_now_offset, uint32 offset);
void (*enter_discovery_mode)(struct TDMAHandler *this);
void (*set_discovery_mode)(struct TDMAHandler *this, DISCOVERY_MODE mode, uint32 time_now);
@@ -89,7 +84,7 @@ struct TDMAHandler
//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 (*assign_slot)(struct TDMAInfo *info, uint8 slot, bool safeAssign);
void (*free_slot)(struct TDMAInfo *info, uint8 slot);
void (*free_slots)(struct TDMAInfo *info);
void (*uwblist_free_slots)(struct TDMAHandler *this, uint8 uwb_index);
@@ -97,7 +92,6 @@ struct TDMAHandler
void (*find_assign_slot)(struct TDMAHandler *this);
void (*build_new_network)(struct TDMAHandler *this);
-// void (*uwblist_collect_slot_assignment)(struct TDMAHandler *this, uint8 uwb_index, uint8 slot_index);
//run through all uwb pairs
bool (*deconflict_slot_assignments)(struct TDMAHandler *this); //TODO implement
diff --git a/src/decadriver/deca_device.c b/src/decadriver/deca_device.c
index a146ec920ba3d0b9dc5bcf30f5853ca708f8959e..d8cf0f931bdc8b0b7e0a650d48f688b1fb6d3400 100644
--- a/src/decadriver/deca_device.c
+++ b/src/decadriver/deca_device.c
@@ -855,6 +855,91 @@ void dwt_readsystime(uint8 * timestamp)
dwt_readfromdevice(SYS_TIME_ID, SYS_TIME_OFFSET, SYS_TIME_LEN, timestamp) ;
}
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_getdt()
+ *
+ * @brief This is used get the delta time between two systime timestamps.
+ *
+ * input parameters
+ * @param t1 - the first dwt timestamp
+ * @param t2 - the second dwt timestamp which occurs after t1
+ *
+ * returns difference between the timestamps
+ */
+uint64 dwt_getdt(uint64 t1, uint64 t2)
+{
+ //dwt systime timestamps are 40 bits
+ t1 &= 0x00FFFFFFFFFF;//40 bit mask
+ t2 &= 0x00FFFFFFFFFF;//40 bit mask
+
+ if(t2 >= t1)
+ {
+ return t2 - t1;
+ }
+ else
+ {
+ //handle timestamp roleover
+ return (uint64)0xFFFFFFFFFF - t1 + t2;
+ }
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_timestamp_add()
+ *
+ * @brief this is used to add a duration to a dwt timestamp. This function handles number wrapping
+ *
+ * input parameters
+ * @param timestamp - the dwt timestamp
+ * @param duration - the 40 bit dwt duration
+ *
+ * returns timestamp offset by a duration
+ */
+uint64 dwt_timestamp_add(uint64 timestamp, uint64 duration)
+{
+ //dwt systime timestamps are 40 bits
+ timestamp &= 0x00FFFFFFFFFF;//40 bit mask
+ duration &= 0x00FFFFFFFFFF;//40 bit mask
+
+ uint64 to_wrap = (uint64)0xFFFFFFFFFF - timestamp;
+ if(duration >= to_wrap)
+ {
+ return to_wrap + duration;
+ }
+ else
+ {
+ return timestamp + duration;
+ }
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_timestamp_subtract()
+ *
+ * @brief this function is used to subtract a duration from a dwt timestamp. This function handles number wrapping
+ *
+ * input parameters
+ * @param timestamp - the dwt timestamp
+ * @param duration - the 40 bit dwt duration
+ *
+ * returns timestamp offset by a duration
+ */
+//
+uint64 dwt_timestamp_subtract(uint64 timestamp, uint64 duration)
+{
+ //dwt systime timestamps are 40 bits
+ timestamp &= 0x00FFFFFFFFFF;//40 bit mask
+ duration &= 0x00FFFFFFFFFF;//40 bit mask
+
+ if(duration > timestamp)
+ {
+ return (uint64)0xFFFFFFFFFF - (duration - timestamp);
+ }
+ else
+ {
+ return timestamp - duration;
+ }
+}
+
+
/*! ------------------------------------------------------------------------------------------------------------------
* @fn dwt_writetodevice()
*
@@ -2602,6 +2687,11 @@ int dwt_starttx(uint8 mode)
}
else
{
+// uint8 debug_msg[150];
+// int n = sprintf((char*)&debug_msg[0], "SYS_STATUS: %u", checkTxOK);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
// I am taking DSHP set to Indicate that the TXDLYS was set too late for the specified DX_TIME.
// Remedial Action - (a) cancel delayed send
temp = (uint8)SYS_CTRL_TRXOFF; // This assumes the bit is in the lowest byte
@@ -2665,15 +2755,9 @@ void dwt_forcetrxoff(void)
dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, (uint8)SYS_CTRL_TRXOFF) ; // Disable the radio
// Forcing Transceiver off - so we do not want to see any new events that may have happened
-// dwt_write32bitreg(SYS_STATUS_ID, (SYS_STATUS_ALL_TX | SYS_STATUS_ALL_RX_ERR | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_GOOD));
-// dwt_write32bitreg(SYS_STATUS_ID, (SYS_STATUS_ALL_TX | SYS_STATUS_ALL_RX_ERR | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_GOOD));
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_INT);
-// SYS_MASK_VAL //TODO do something like this for SYS_STATUS above!
dwt_syncrxbufptrs();
-// dwt_write32bitreg(SYS_MASK_ID, mask) ; // Set interrupt mask to what it was
-// uint32 reserved_mask = 0xC0080001;
-// mask &= reserved_mask; //preserve reserved bits
mask |= SYS_MASK_VAL; //set out desired sys_mask values
dwt_write32bitreg(SYS_MASK_ID, mask);
diff --git a/src/decadriver/deca_device_api.h b/src/decadriver/deca_device_api.h
index 74da8857d46f541c4b7ce9b33ddabdbf6a4fe322..4e590516be7d5809852e46cee2e9229c7d1cc94e 100644
--- a/src/decadriver/deca_device_api.h
+++ b/src/decadriver/deca_device_api.h
@@ -39,6 +39,13 @@ typedef unsigned long uint32;
#endif
#endif
+#ifndef uint64
+#ifndef _DECA_UINT64_
+#define _DECA_UINT64_
+typedef unsigned long long uint64;
+#endif
+#endif
+
#ifndef int8
#ifndef _DECA_INT8_
#define _DECA_INT8_
@@ -680,6 +687,46 @@ uint32 dwt_readsystimestamphi32(void);
*/
void dwt_readsystime(uint8 * timestamp);
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_getdt()
+ *
+ * @brief This is used get the delta time between two systime timestamps.
+ *
+ * input parameters
+ * @param t1 - the first dwt timestamp
+ * @param t2 - the second dwt timestamp which occurs after t1
+ *
+ * difference between the timestamps
+ */
+uint64 dwt_getdt(uint64 t1, uint64 t2);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_timestamp_add()
+ *
+ * @brief this is used to add a duration to a dwt timestamp. This function handles number wrapping
+ *
+ * input parameters
+ * @param timestamp - the dwt timestamp
+ * @param duration - the 40 bit dwt duration
+ *
+ * returns timestamp offset by a duration
+ */
+uint64 dwt_timestamp_add(uint64 timestamp, uint64 duration);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_timestamp_subtract()
+ *
+ * @brief this function is used to subtract a duration from a dwt timestamp. This function handles number wrapping
+ *
+ * input parameters
+ * @param timestamp - the dwt timestamp
+ * @param duration - the 40 bit dwt duration
+ *
+ * returns timestamp offset by a duration
+ */
+//
+uint64 dwt_timestamp_subtract(uint64 timestamp, uint64 duration);
+
/*! ------------------------------------------------------------------------------------------------------------------
* @fn dwt_forcetrxoff()
*
diff --git a/src/decadriver/deca_types.h b/src/decadriver/deca_types.h
index 6839bc6a3aa35ba164a29654008da49f11c94d7d..7bbf23d0a49c6391d339ece482f86aec013db81d 100644
--- a/src/decadriver/deca_types.h
+++ b/src/decadriver/deca_types.h
@@ -38,6 +38,13 @@ typedef unsigned long uint32;
#endif
#endif
+#ifndef uint64
+#ifndef _DECA_UINT64_
+#define _DECA_UINT64_
+typedef unsigned long long uint64;
+#endif
+#endif
+
#ifndef int8
#ifndef _DECA_INT8_
#define _DECA_INT8_
diff --git a/src/platform/deca_spi.c b/src/platform/deca_spi.c
index a2b680692b36d03b374b67eb157f6f7fce554d9a..4926a72ff1045787f9b00b99cafc78173e776e82 100644
--- a/src/platform/deca_spi.c
+++ b/src/platform/deca_spi.c
@@ -63,7 +63,7 @@ int writetospi
)
{
- int i=0;
+ uint32 i=0;
decaIrqStatus_t stat ;
@@ -115,7 +115,7 @@ int readfromspi
)
{
- int i=0;
+ uint32 i=0;
decaIrqStatus_t stat ;
@@ -160,7 +160,7 @@ void writetoLCD
const uint8 *bodyBuffer
)
{
- int i = 0;
+ uint32 i = 0;
int sleep = 0;
if(rs_enable)
diff --git a/src/platform/port.c b/src/platform/port.c
index ef5c17bed1bb14c2e790aac662811cdb837c2e75..cd817301ad65208a1a7e7f5a7ad064d981567ca5 100644
--- a/src/platform/port.c
+++ b/src/platform/port.c
@@ -55,6 +55,49 @@ static void SPI1_ConfigRate(uint16_t scalingfactor);
return time32_incr;
}
+/* @fn portGetTickCntMicro
+* @brief function to read a SysTickTimer modified by its count to get a higher resolution timestamp.
+* The resolution is usually one microsecond.
+* */
+unsigned long long portGetTickCntMicro(void)
+{
+ uint64_t major_incr1 = (uint64_t)time32_incr;
+ uint32_t minor_incr1 = SysTick->VAL;
+
+ uint64_t major_incr2 = (uint64_t)time32_incr;
+ uint32_t minor_incr2 = SysTick->VAL;
+
+ if(major_incr1 == major_incr2)
+ {
+ //72 comes from SystemCoreClock / CLOCKS_PER_SEC * 1000
+ return major_incr1*1000 + 1000 - (uint64_t)(minor_incr1/72);
+ }
+ else
+ {
+ //72 comes from SystemCoreClock / CLOCKS_PER_SEC * 1000
+ return major_incr2*1000 + 1000 - (uint64_t)(minor_incr2/72);
+ }
+
+// __asm__ volatile("dmb");
+// //72 comes from SystemCoreClock / CLOCKS_PER_SEC * 1000
+// return time64_incr*1000 + 1000 - (uint64_t)(SysTick->VAL/72);
+//
+//// major = SysTickMajor;
+//// minor = SysTick->VAL;
+//// __asm__ volatile("dmb");
+//// if (major != SysTickMajor)
+//// return SysTickMajor - SysTick->Val;
+//// return major - minor;
+//
+
+}
+
+uint16_t portGetTIM3()
+{
+ return TIM_GetCounter(TIM3);
+}
+
+
/* @fn SysTick_Configuration
* @brief SysTickTimer is configured to be clocked from HCLK (72MHz)
* */
@@ -78,7 +121,7 @@ int SysTick_Configuration(void)
#pragma GCC optimize ("O0")
int usleep(useconds_t usec)
{
- int i,j;
+ useconds_t i,j;
#pragma GCC ivdep
for(i=0;i<usec;i++)
@@ -128,6 +171,32 @@ int peripherals_init (void)
SysTick_Configuration();
NVIC_Configuration();
+
+ //TODO remove
+
+// /* make sure the peripheral is clocked */
+//// RCC_APB1PeriphClockCmd (RCC_APB1Periph_TIM3, ENABLE);
+// RCC_ClocksTypeDef RCC_Clocks;
+// RCC_GetClocksFreq (&RCC_Clocks);
+// uint32_t multiplier;
+// if (RCC_Clocks.PCLK1_Frequency == RCC_Clocks.SYSCLK_Frequency) {
+// multiplier = 1;
+// } else {
+// multiplier = 2;
+// }
+// uint32_t TIM3CLK_Frequency = multiplier * RCC_Clocks.PCLK1_Frequency;
+// uint32_t TIM3COUNTER_Frequency = 1000;//1000000;
+// uint16_t prescaler = (TIM3CLK_Frequency / TIM3COUNTER_Frequency) - 1;
+// uint16_t reload = 0xFFFF;//(25) - 1; // Tevt = 25 us
+// TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+// /* set everything back to default values */
+// TIM_TimeBaseStructInit (&TIM_TimeBaseStructure);
+// /* only changes from the defaults are needed */
+// TIM_TimeBaseStructure.TIM_Period = reload;
+// TIM_TimeBaseStructure.TIM_Prescaler = prescaler;
+// TIM_TimeBaseInit (TIM3, &TIM_TimeBaseStructure);
+//
+// TIM_Cmd(TIM3, ENABLE);
return 0;
}
@@ -282,6 +351,10 @@ int RCC_Configuration(void)
RCC_APB2Periph_GPIOE | RCC_APB2Periph_AFIO,
ENABLE);
+ /* Enable TIM3 clock */ //TODO remove
+// RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
+
+
return 0;
}
diff --git a/src/platform/port.h b/src/platform/port.h
index 200e8579667969ce90785ea31278d1dc83d65d76..44e7c8cd6dd1b4b00fdee47fdc33e8dec045d2f1 100644
--- a/src/platform/port.h
+++ b/src/platform/port.h
@@ -149,6 +149,8 @@ typedef enum
void Sleep(uint32_t Delay);
unsigned long portGetTickCnt(void);
+unsigned long long portGetTickCntMicro(void);
+//uint16_t portGetTIM3(); //TODO remove
#define S1_SWITCH_ON (1)
#define S1_SWITCH_OFF (0)
diff --git a/src/usb/deca_usb.c b/src/usb/deca_usb.c
index 7c082fc2418dc633cd2bb7d936fda4d3911c95b7..efadc011b681b414bdc2e894a8e8c98545d4292a 100644
--- a/src/usb/deca_usb.c
+++ b/src/usb/deca_usb.c
@@ -93,7 +93,7 @@ uint16_t DW_VCP_Ctrl (uint32_t Cmd, uint8_t* Buf, uint32_t Len)
#pragma GCC optimize ("O3")
uint16_t DW_VCP_DataTx (uint8_t* Buf, uint32_t Len)
{
- int i = 0;
+ uint32_t i = 0;
i = APP_Rx_ptr_in ;