From 92b5e2d950b534f48899a710cc8e6f5cab9fa038 Mon Sep 17 00:00:00 2001
From: David <stierint@hotmail.com>
Date: Wed, 19 Aug 2020 22:08:28 -0400
Subject: [PATCH] two uwbs working with TDMA
---
DecaRanging.coproj | 44 +-
src/application/application_definitions.h | 488 ++++
src/application/comm_scheduler.c | 82 +-
src/application/comm_scheduler.h | 50 +-
src/application/dw_main.c | 88 +-
src/application/instance.c | 1682 ++++++++++----
src/application/instance.h | 447 +---
src/application/instance_common.c | 1107 +++++----
src/application/rx_scheduler.c | 2 +-
src/application/tdma_handler.c | 2566 +++++++++++++++++++++
src/application/tdma_handler.h | 119 +
src/application/tdma_scheduler.c | 214 ++
src/application/tdma_scheduler.h | 62 +
src/application/tx_scheduler.c | 74 +-
src/application/tx_scheduler.h | 53 +-
src/decadriver/deca_device.c | 96 +-
src/liblist/inc/llist.h | 267 ---
src/liblist/src/Complex.c | 9 -
src/liblist/src/Complex.h | 7 -
src/liblist/src/llist.c | 676 ------
src/usb/deca_usb.c | 4 +-
21 files changed, 5483 insertions(+), 2654 deletions(-)
create mode 100644 src/application/application_definitions.h
create mode 100644 src/application/tdma_handler.c
create mode 100644 src/application/tdma_handler.h
create mode 100644 src/application/tdma_scheduler.c
create mode 100644 src/application/tdma_scheduler.h
delete mode 100644 src/liblist/inc/llist.h
delete mode 100644 src/liblist/src/Complex.c
delete mode 100644 src/liblist/src/Complex.h
delete mode 100644 src/liblist/src/llist.c
diff --git a/DecaRanging.coproj b/DecaRanging.coproj
index 0e90857..983056d 100644
--- a/DecaRanging.coproj
+++ b/DecaRanging.coproj
@@ -4,7 +4,7 @@
<Device manufacturerId="9" manufacturerName="ST" chipId="334" chipName="STM32F105RC" boardId="" boardName=""/>
<BuildOption>
<Compile>
- <Option name="OptimizationLevel" value="0"/>
+ <Option name="OptimizationLevel" value="4"/>
<Option name="UseFPU" value="0"/>
<Option name="UserEditCompiler" value="-ffunction-sections; -fdata-sections; -c; -fmessage-length=0;"/>
<Includepaths>
@@ -89,15 +89,14 @@
<Components path="./"/>
<Files>
<File name="Libraries/STM32_USB_Device_Library/Core/inc/usbd_ioreq.h" path="Libraries/STM32_USB_Device_Library/Core/inc/usbd_ioreq.h" type="1"/>
- <File name="src/application/uwb_select.h" path="src/application/uwb_select.h" type="1"/>
- <File name="src/liblist/inc/llist.h" path="src/liblist/inc/llist.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_fsmc.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_fsmc.h" type="1"/>
<File name="Libraries/STM32_USB_Device_Library/Core/inc/usbd_usr.h" path="Libraries/STM32_USB_Device_Library/Core/inc/usbd_usr.h" type="1"/>
<File name="Libraries/STM32_USB_Device_Library/Core" path="" type="2"/>
<File name="src/usb/usbd_desc.c" path="src/usb/usbd_desc.c" type="1"/>
<File name="src" path="" type="2"/>
- <File name="Libraries/CMSIS" path="" type="2"/>
<File name="Libraries/STM32_USB_Device_Library/Class/cdc" path="" type="2"/>
+ <File name="Libraries/CMSIS" path="" type="2"/>
+ <File name="src/application/comm_scheduler.c" path="src/application/comm_scheduler.c" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_dac.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_dac.h" type="1"/>
<File name="Libraries/STM32_USB_OTG_Driver/inc/usb_hcd_int.h" path="Libraries/STM32_USB_OTG_Driver/inc/usb_hcd_int.h" type="1"/>
<File name="Libraries/STM32_USB_Device_Library/Class/cdc/src" path="" type="2"/>
@@ -106,24 +105,23 @@
<File name="src/platform/deca_spi.h" path="src/platform/deca_spi.h" type="1"/>
<File name="Libraries/STM32_USB_Device_Library/Core/src/usbd_ioreq.c" path="Libraries/STM32_USB_Device_Library/Core/src/usbd_ioreq.c" type="1"/>
<File name="Libraries/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c" path="Libraries/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c" type="1"/>
- <File name="Libraries/STM32_USB_Device_Library/Class/cdc/inc/usbd_cdc_core.h" path="Libraries/STM32_USB_Device_Library/Class/cdc/inc/usbd_cdc_core.h" type="1"/>
<File name="src/usb/usbd_desc.h" path="src/usb/usbd_desc.h" type="1"/>
+ <File name="Libraries/STM32_USB_Device_Library/Class/cdc/inc/usbd_cdc_core.h" path="Libraries/STM32_USB_Device_Library/Class/cdc/inc/usbd_cdc_core.h" type="1"/>
<File name="src/platform/deca_spi.c" path="src/platform/deca_spi.c" type="1"/>
<File name="src/usb" path="" type="2"/>
<File name="src/decadriver/deca_version.h" path="src/decadriver/deca_version.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_flash.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_flash.c" type="1"/>
- <File name="src/liblist/inc" path="" type="2"/>
- <File name="src/application/instance.c" path="src/application/instance.c" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_wwdg.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_wwdg.c" type="1"/>
+ <File name="src/application/instance.c" path="src/application/instance.c" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_dbgmcu.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_dbgmcu.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_sdio.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_sdio.c" type="1"/>
<File name="src/decadriver/deca_device_api.h" path="src/decadriver/deca_device_api.h" type="1"/>
<File name="Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x" path="" type="2"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_iwdg.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_iwdg.c" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_bkp.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_bkp.h" type="1"/>
+ <File name="src/application/tdma_handler.h" path="src/application/tdma_handler.h" type="1"/>
<File name="src/compiler/compiler.h" path="src/compiler/compiler.h" type="1"/>
<File name="Libraries/STM32_USB_OTG_Driver" path="" type="2"/>
- <File name="src/application/uwb_select.c" path="src/application/uwb_select.c" type="1"/>
<File name="src/platform/deca_mutex.c" path="src/platform/deca_mutex.c" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_crc.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_crc.c" type="1"/>
<File name="src/platform/port.h" path="src/platform/port.h" type="1"/>
@@ -132,32 +130,32 @@
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_rcc.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_rcc.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_can.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_can.c" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_sdio.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_sdio.h" type="1"/>
- <File name="src/application/rx_scheduler.h" path="src/application/rx_scheduler.h" type="1"/>
+ <File name="src/application/tdma_handler.c" path="src/application/tdma_handler.c" type="1"/>
<File name="Libraries/STM32_USB_OTG_Driver/src/usb_hcd_int.c" path="Libraries/STM32_USB_OTG_Driver/src/usb_hcd_int.c" type="1"/>
<File name="src/platform/port.c" path="src/platform/port.c" type="1"/>
- <File name="Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.h" path="Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_wwdg.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_wwdg.h" type="1"/>
+ <File name="Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.h" path="Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.h" type="1"/>
<File name="src/decadriver/deca_device.c" path="src/decadriver/deca_device.c" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_exti.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_exti.c" type="1"/>
+ <File name="Libraries/STM32_USB_Device_Library/Core/inc/usbd_req.h" path="Libraries/STM32_USB_Device_Library/Core/inc/usbd_req.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_adc.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_adc.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_rtc.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_rtc.c" type="1"/>
- <File name="Libraries/STM32_USB_Device_Library/Core/inc/usbd_req.h" path="Libraries/STM32_USB_Device_Library/Core/inc/usbd_req.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_fsmc.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_fsmc.c" type="1"/>
<File name="src/usb/usb_conf.h" path="src/usb/usb_conf.h" type="1"/>
<File name="Libraries/STM32_USB_OTG_Driver/inc/usb_otg.h" path="Libraries/STM32_USB_OTG_Driver/inc/usb_otg.h" type="1"/>
- <File name="Libraries/STM32_USB_OTG_Driver/llist.c" path="src/liblist/src/llist.c" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_rcc.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_rcc.c" type="1"/>
<File name="Libraries/STM32_USB_Device_Library/Core/src/usbd_req.c" path="Libraries/STM32_USB_Device_Library/Core/src/usbd_req.c" type="1"/>
- <File name="src/application/rx_scheduler.c" path="src/application/rx_scheduler.c" type="1"/>
<File name="src/usb/usbd_conf.h" path="src/usb/usbd_conf.h" type="1"/>
+ <File name="src/application/application_definitions.h" path="src/application/application_definitions.h" type="1"/>
<File name="src/usb/deca_usb.c" path="src/usb/deca_usb.c" type="1"/>
<File name="src/platform/deca_range_tables.c" path="src/platform/deca_range_tables.c" type="1"/>
+ <File name="src/application/lib.h" path="src/application/lib.h" type="1"/>
<File name="Libraries/STM32_USB_OTG_Driver/src" path="" type="2"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_dma.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_dma.c" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_pwr.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_pwr.c" type="1"/>
<File name="src/application/instance_common.c" path="src/application/instance_common.c" type="1"/>
- <File name="src/decadriver/deca_params_init.c" path="src/decadriver/deca_params_init.c" type="1"/>
<File name="Libraries/STM32_USB_Device_Library/Core/inc" path="" type="2"/>
+ <File name="src/decadriver/deca_params_init.c" path="src/decadriver/deca_params_init.c" type="1"/>
<File name="src/application" path="" type="2"/>
<File name="src/application/instance_calib.c" path="src/application/instance_calib.c" type="1"/>
<File name="Libraries/STM32_USB_OTG_Driver/inc/usb_dcd.h" path="Libraries/STM32_USB_OTG_Driver/inc/usb_dcd.h" type="1"/>
@@ -167,26 +165,24 @@
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/misc.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/misc.c" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_cec.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_cec.h" type="1"/>
<File name="Libraries/STM32_USB_OTG_Driver/src/usb_hcd.c" path="Libraries/STM32_USB_OTG_Driver/src/usb_hcd.c" type="1"/>
- <File name="src/liblist/src" path="" type="2"/>
<File name="src/application/main.c" path="src/main.c" type="1"/>
<File name="src/platform/deca_sleep.c" path="src/platform/deca_sleep.c" type="1"/>
- <File name="Libraries/CMSIS/CM3/CoreSupport" path="" type="2"/>
<File name="Libraries/STM32_USB_OTG_Driver/inc/usb_defines.h" path="Libraries/STM32_USB_OTG_Driver/inc/usb_defines.h" type="1"/>
+ <File name="Libraries/CMSIS/CM3/CoreSupport" path="" type="2"/>
<File name="Libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c" path="Libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c" type="1"/>
<File name="src/compiler" path="" type="2"/>
<File name="src/decadriver/deca_param_types.h" path="src/decadriver/deca_param_types.h" type="1"/>
<File name="src/usb/deca_usb_bsp_evk1000.c" path="src/usb/deca_usb_bsp_evk1000.c" type="1"/>
<File name="Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/startup/startup_stm32f10x_cl.S" path="Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/startup/startup_stm32f10x_cl.S" type="1"/>
- <File name="src/application/tx_scheduler.h" path="src/application/tx_scheduler.h" type="1"/>
- <File name="src/platform/stm32f10x_conf.h" path="src/platform/stm32f10x_conf.h" type="1"/>
<File name="Libraries/STM32_USB_OTG_Driver/inc/usb_bsp.h" path="Libraries/STM32_USB_OTG_Driver/inc/usb_bsp.h" type="1"/>
+ <File name="src/platform/stm32f10x_conf.h" path="src/platform/stm32f10x_conf.h" type="1"/>
<File name="Libraries/CMSIS/CM3" path="" type="2"/>
<File name="Libraries" path="" type="2"/>
<File name="src/decadriver/deca_types.h" path="src/decadriver/deca_types.h" type="1"/>
<File name="src/usb/deca_usb.h" path="src/usb/deca_usb.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc" path="" type="2"/>
- <File name="Libraries/STM32_USB_Device_Library/Core/inc/usbd_def.h" path="Libraries/STM32_USB_Device_Library/Core/inc/usbd_def.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_spi.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_spi.h" type="1"/>
+ <File name="Libraries/STM32_USB_Device_Library/Core/inc/usbd_def.h" path="Libraries/STM32_USB_Device_Library/Core/inc/usbd_def.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_iwdg.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_iwdg.h" type="1"/>
<File name="src/platform/stm32f10x_it.c" path="src/platform/stm32f10x_it.c" type="1"/>
<File name="Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c" path="Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c" type="1"/>
@@ -198,15 +194,15 @@
<File name="Libraries/STM32_USB_Device_Library/Core/src/usbd_core.c" path="Libraries/STM32_USB_Device_Library/Core/src/usbd_core.c" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_exti.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_exti.h" type="1"/>
<File name="Libraries/STM32_USB_OTG_Driver/inc/usb_regs.h" path="Libraries/STM32_USB_OTG_Driver/inc/usb_regs.h" type="1"/>
- <File name="src/liblist/src/llist.c" path="src/liblist/src/llist.c" type="1"/>
<File name="Libraries/STM32_USB_Device_Library" path="" type="2"/>
<File name="Libraries/STM32_USB_Device_Library/Class" path="" type="2"/>
<File name="Libraries/CMSIS/CM3/CoreSupport/core_cm3.c" path="Libraries/CMSIS/CM3/CoreSupport/core_cm3.c" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_cec.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_cec.c" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver" path="" type="2"/>
+ <File name="src/application/comm_scheduler.h" path="src/application/comm_scheduler.h" type="1"/>
<File name="Libraries/STM32_USB_OTG_Driver/inc/usb_hcd.h" path="Libraries/STM32_USB_OTG_Driver/inc/usb_hcd.h" type="1"/>
- <File name="Libraries/STM32_USB_Device_Library/Core/inc/usbd_core.h" path="Libraries/STM32_USB_Device_Library/Core/inc/usbd_core.h" type="1"/>
<File name="Libraries/STM32_USB_OTG_Driver/inc" path="" type="2"/>
+ <File name="Libraries/STM32_USB_Device_Library/Core/inc/usbd_core.h" path="Libraries/STM32_USB_Device_Library/Core/inc/usbd_core.h" type="1"/>
<File name="Libraries/CMSIS/CM3/DeviceSupport" path="" type="2"/>
<File name="Libraries/STM32_USB_OTG_Driver/inc/usb_dcd_int.h" path="Libraries/STM32_USB_OTG_Driver/inc/usb_dcd_int.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_dac.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_dac.c" type="1"/>
@@ -216,8 +212,8 @@
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_usart.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_usart.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_tim.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_tim.h" type="1"/>
<File name="Libraries/STM32_USB_Device_Library/Class/cdc/inc" path="" type="2"/>
- <File name="Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/startup" path="" type="2"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/misc.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/misc.h" type="1"/>
+ <File name="Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/startup" path="" type="2"/>
<File name="Libraries/STM32_USB_OTG_Driver/inc/usb_core.h" path="Libraries/STM32_USB_OTG_Driver/inc/usb_core.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_bkp.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_bkp.c" type="1"/>
<File name="src/application/dw_main.c" path="src/application/dw_main.c" type="1"/>
@@ -225,9 +221,8 @@
<File name="src/platform" path="" type="2"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_tim.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_tim.c" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_i2c.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_i2c.h" type="1"/>
- <File name="Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/stm32f10x.h" path="Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/stm32f10x.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src" path="" type="2"/>
- <File name="src/liblist" path="" type="2"/>
+ <File name="Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/stm32f10x.h" path="Libraries/CMSIS/CM3/DeviceSupport/ST/STM32F10x/stm32f10x.h" type="1"/>
<File name="Libraries/CMSIS/CM3/DeviceSupport/ST" path="" type="2"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_dbgmcu.c" path="Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_dbgmcu.c" type="1"/>
<File name="src/decadriver/deca_regs.h" path="src/decadriver/deca_regs.h" type="1"/>
@@ -235,6 +230,5 @@
<File name="src/usb/usbd_usr.c" path="src/usb/usbd_usr.c" type="1"/>
<File name="src/application/instance.h" path="src/application/instance.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_gpio.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_gpio.h" type="1"/>
- <File name="src/application/tx_scheduler.c" path="src/application/tx_scheduler.c" type="1"/>
</Files>
</Project>
\ No newline at end of file
diff --git a/src/application/application_definitions.h b/src/application/application_definitions.h
new file mode 100644
index 0000000..da3d693
--- /dev/null
+++ b/src/application/application_definitions.h
@@ -0,0 +1,488 @@
+#ifndef APPLICATION_DEFINITIONS_H_
+#define APPLICATION_DEFINITIONS_H_
+
+#include "port.h"
+#include "deca_types.h"
+#include "deca_device_api.h"
+
+/******************************************************************************************************************
+********************* NOTES on Decaranging EVK1000 application features/options ***********************************************************
+*******************************************************************************************************************/
+#define DEEP_SLEEP (0) //To enable deep-sleep set this to 1
+//DEEP_SLEEP mode can be used, for example, by a Tag instance to put the DW1000 into low-power deep-sleep mode
+
+#define CORRECT_RANGE_BIAS (1) // Compensate for small bias due to uneven accumulator growth at close up high power
+
+
+
+/******************************************************************************************************************
+*******************************************************************************************************************
+*******************************************************************************************************************/
+
+#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_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
+
+#define SET_TXRX_DELAY (0) //when set to 1 - the DW1000 RX and TX delays are set to the TX_DELAY and RX_DELAY defines
+#define TX_ANT_DELAY 0
+#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
+#define DWT_SIG_TX_DONE 1 // Frame has been sent
+#define DWT_SIG_RX_OKAY 2 // Frame Received with Good CRC
+#define DWT_SIG_RX_ERROR 3 // Frame Received but CRC is wrong
+#define DWT_SIG_RX_TIMEOUT 4 // Timeout on receive has elapsed
+#define DWT_SIG_TX_AA_DONE 6 // ACK frame has been sent (as a result of auto-ACK)
+#define DWT_SIG_RX_BLINK 7 // Received ISO EUI 64 blink message
+#define DWT_SIG_RX_PHR_ERROR 8 // Error found in PHY Header
+#define DWT_SIG_RX_SYNCLOSS 9 // Un-recoverable error in Reed Solomon Decoder
+#define DWT_SIG_RX_SFDTIMEOUT 10 // Saw preamble but got no SFD within configured time
+#define DWT_SIG_RX_PTOTIMEOUT 11 // Got preamble detection timeout (no preamble detected)
+
+#define DWT_SIG_TX_PENDING 12 // TX is pending
+#define DWT_SIG_TX_ERROR 13 // TX failed
+#define DWT_SIG_RX_PENDING 14 // RX has been re-enabled
+#define DWT_SIG_DW_IDLE 15 // DW radio is in IDLE (no TX or RX pending)
+
+#define SIG_RX_UNKNOWN 99 // Received an unknown frame
+
+// Existing frames type in ranging process.
+enum
+{
+ BLINK = 0,
+ RNG_INIT,
+ POLL,
+ RESP,
+ FINAL,
+ INF,
+ FRAME_TYPE_NB //TODO add in RNG_REPORT? would be useful in calculating how long a frame slot should be... if so, dont forget to update the init_timings function...
+};
+
+//TWO WAY RANGING function codes
+#define RTLS_DEMO_MSG_RNG_INIT (0x20) // Ranging initiation message
+#define RTLS_DEMO_MSG_TAG_POLL (0x21) // Tag poll message
+#define RTLS_DEMO_MSG_ANCH_RESP (0x10) // Anchor response to poll
+#define RTLS_DEMO_MSG_TAG_FINAL (0x29) // Tag final massage back to Anchor (0x29 because of 5 byte timestamps needed for PC app)
+#define RTLS_DEMO_MSG_INF_REG (0x13) // TDMA coordination info packet
+#define RTLS_DEMO_MSG_INF_INIT (0x14) // TDMA coordination info packet
+#define RTLS_DEMO_MSG_INF_SUG (0x15) // TDMA coordination info packet
+#define RTLS_DEMO_MSG_INF_UPDATE (0x16) // TDMA coordination info packet
+#define RTLS_DEMO_MSG_RNG_REPORT (0x11) // Report of calculated range back to the Tag
+
+//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 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 MAX_MAC_MSG_DATA_LEN (TAG_FINAL_MSG_LEN) //max message len of the above
+
+#define BROADCAST_ADDRESS 0xFFFF //address for broadcasting messages to all UWBs the network
+
+#define STANDARD_FRAME_SIZE 127
+#define EXTENDED_FRAME_SIZE 1023
+
+#define ADDR_BYTE_SIZE_L (8)
+#define ADDR_BYTE_SIZE_S (2)
+
+#define FRAME_CONTROL_BYTES 2
+#define FRAME_SEQ_NUM_BYTES 1
+#define FRAME_PANID 2
+#define FRAME_CRC 2
+#define FRAME_SOURCE_ADDRESS_S (ADDR_BYTE_SIZE_S)
+#define FRAME_DEST_ADDRESS_S (ADDR_BYTE_SIZE_S)
+#define FRAME_SOURCE_ADDRESS_L (ADDR_BYTE_SIZE_L)
+#define FRAME_DEST_ADDRESS_L (ADDR_BYTE_SIZE_L)
+#define FRAME_CTRLP (FRAME_CONTROL_BYTES + FRAME_SEQ_NUM_BYTES + FRAME_PANID) //5
+#define FRAME_CRTL_AND_ADDRESS_L (FRAME_DEST_ADDRESS_L + FRAME_SOURCE_ADDRESS_L + FRAME_CTRLP) //21 bytes for 64-bit addresses)
+#define FRAME_CRTL_AND_ADDRESS_S (FRAME_DEST_ADDRESS_S + FRAME_SOURCE_ADDRESS_S + FRAME_CTRLP) //9 bytes for 16-bit addresses)
+#define FRAME_CRTL_AND_ADDRESS_LS (FRAME_DEST_ADDRESS_L + FRAME_SOURCE_ADDRESS_S + FRAME_CTRLP) //15 bytes for 1 16-bit address and 1 64-bit address)
+//#define MAX_USER_PAYLOAD_STRING_LL (STANDARD_FRAME_SIZE-FRAME_CRTL_AND_ADDRESS_L-TAG_FINAL_MSG_LEN-FRAME_CRC) //127 - 21 - 16 - 2 = 88
+//#define MAX_USER_PAYLOAD_STRING_SS (STANDARD_FRAME_SIZE-FRAME_CRTL_AND_ADDRESS_S-TAG_FINAL_MSG_LEN-FRAME_CRC) //127 - 9 - 16 - 2 = 100
+//#define MAX_USER_PAYLOAD_STRING_LS (STANDARD_FRAME_SIZE-FRAME_CRTL_AND_ADDRESS_LS-TAG_FINAL_MSG_LEN-FRAME_CRC) //127 - 15 - 16 - 2 = 94
+#define MAX_USER_PAYLOAD_STRING_LL (STANDARD_FRAME_SIZE-FRAME_CRTL_AND_ADDRESS_L-FRAME_CRC) //127 - 21 - 2 = 104
+#define MAX_USER_PAYLOAD_STRING_SS (STANDARD_FRAME_SIZE-FRAME_CRTL_AND_ADDRESS_S-FRAME_CRC) //127 - 9 - 2 = 116
+#define MAX_USER_PAYLOAD_STRING_LS (STANDARD_FRAME_SIZE-FRAME_CRTL_AND_ADDRESS_LS-FRAME_CRC) //127 - 15 - 2 = 110
+#define MAX_EXTENDED_USER_PAYLOAD_STRING_LL (EXTENDED_FRAME_SIZE-FRAME_CRTL_AND_ADDRESS_L-FRAME_CRC) //1023 - 21 - 2 = 100
+#define MAX_EXTENDED_USER_PAYLOAD_STRING_SS (EXTENDED_FRAME_SIZE-FRAME_CRTL_AND_ADDRESS_S-FRAME_CRC) //1023 - 9 - 2 = 112
+#define MAX_EXTENDED_USER_PAYLOAD_STRING_LS (EXTENDED_FRAME_SIZE-FRAME_CRTL_AND_ADDRESS_LS-FRAME_CRC) //1023 - 15 - 2 = 1006
+
+//NOTE: the user payload assumes that there are only 88 "free" bytes to be used for the user message (it does not scale according to the addressing modes)
+#define MAX_USER_PAYLOAD_STRING MAX_USER_PAYLOAD_STRING_LL
+
+// Total frame lengths.
+#if (USING_64BIT_ADDR == 1)
+ #define RNG_INIT_FRAME_LEN_BYTES (RANGINGINIT_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC)
+ #define POLL_FRAME_LEN_BYTES (TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC)
+ #define RESP_FRAME_LEN_BYTES (ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC)
+ #define FINAL_FRAME_LEN_BYTES (TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC)
+#else
+ #define RNG_INIT_FRAME_LEN_BYTES (RANGINGINIT_MSG_LEN + FRAME_CRTL_AND_ADDRESS_LS + FRAME_CRC)
+ #define POLL_FRAME_LEN_BYTES (TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC)
+ #define RESP_FRAME_LEN_BYTES (ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC)
+ #define FINAL_FRAME_LEN_BYTES (TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC)
+#endif
+
+#define BLINK_FRAME_CONTROL_BYTES (1)
+#define BLINK_FRAME_SEQ_NUM_BYTES (1)
+#define BLINK_FRAME_CRC (FRAME_CRC)
+#define BLINK_FRAME_SOURCE_ADDRESS (ADDR_BYTE_SIZE_L)
+#define BLINK_FRAME_CTRLP (BLINK_FRAME_CONTROL_BYTES + BLINK_FRAME_SEQ_NUM_BYTES) //2
+#define BLINK_FRAME_CRTL_AND_ADDRESS (BLINK_FRAME_SOURCE_ADDRESS + BLINK_FRAME_CTRLP) //10 bytes
+#define BLINK_FRAME_LEN_BYTES (BLINK_FRAME_CRTL_AND_ADDRESS + BLINK_FRAME_CRC)
+
+#define UWB_LIST_SIZE (10) //maximum number of UWBs to range with (valid options are 0 through 253)//TODO check what works based on data sizes needed for INF packets and 16 or 64 bit addres
+#define UWB_COMM_TIMEOUT 3000 //ms //TODO maybe make this a function of other defines?
+
+//UWB_LIST entry types
+#define UWB_LIST_SELF 0 //entry for self in list
+#define UWB_LIST_NEIGHBOR 1 //uwb in list that is active, in range, and are slotted to range with
+#define UWB_LIST_HIDDEN 2 //uwb in list that is active, out of range, and a neighbor is slotted to range with
+#define UWB_LIST_TWICE_HIDDEN 3 //uwb in list that is active, out of range, and a hidden neighbor is slotted to range with
+#define UWB_LIST_INACTIVE 4 //uwb in list that is not active (could have previously been neighbor, hidden, or twice hidden)
+
+#define BLINK_SLEEP_DELAY 0 //ms //how long the tag should sleep after blinking
+#define POLL_SLEEP_DELAY 25 //ms //how long the tag should sleep after ranging
+
+
+
+#define IMMEDIATE_RESPONSE (1)
+
+// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+// NOTE: the maximum RX timeout is ~ 65ms
+// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+
+#define INST_DONE_WAIT_FOR_NEXT_EVENT 1 //this signifies that the current event has been processed and instance is ready for next one
+#define INST_DONE_WAIT_FOR_NEXT_EVENT_TO 2 //this signifies that the current event has been processed and that instance is waiting for next one with a timeout
+ //which will trigger if no event coming in specified time
+#define INST_NOT_DONE_YET 0 //this signifies that the instance is still processing the current event
+
+// Function code byte offset (valid for all message types).
+#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
+
+
+// Final message byte offsets.
+#define PTXT 1
+#define RRXT 6
+#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)
+
+// Ranging init message byte offsets. Composed of tag short address, anchor
+// response delay and tag response delay.
+#define RNG_INIT_TAG_SHORT_ADDR_LO 1
+#define RNG_INIT_TAG_SHORT_ADDR_HI 2
+#define RNG_INIT_ANC_RESP_DLY_LO 3
+#define RNG_INIT_ANC_RESP_DLY_HI 4
+#define RNG_INIT_TAG_RESP_DLY_LO 5
+#define RNG_INIT_TAG_RESP_DLY_HI 6
+
+// Response delay values coded in ranging init message.
+// This is a bitfield composed of:
+// - bits 0 to 14: value
+// - bit 15: unit
+#define RESP_DLY_VAL_SHIFT 0
+#define RESP_DLY_VAL_MASK 0x7FFF
+#define RESP_DLY_UNIT_SHIFT 15
+#define RESP_DLY_UNIT_MASK 0x8000
+
+// Response time possible units: microseconds or milliseconds.
+#define RESP_DLY_UNIT_US 0
+#define RESP_DLY_UNIT_MS 1
+
+// Response delays types present in ranging init message.
+enum
+{
+ RESP_DLY_ANC = 0,
+ RESP_DLY_TAG,
+ RESP_DLY_NB
+};
+
+// Convert microseconds to symbols, float version.
+// param x value in microseconds
+// return value in symbols.
+#define US_TO_SY(x) ((x) / 1.0256)
+
+// Convert microseconds to symbols, integer version.
+// param x value in microseconds
+// return value in symbols.
+// /!\ Due to the the multiplication by 10000, be careful about potential
+// values and type for x to avoid overflows.
+#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 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
+// immediate response, cannot be less than 170 us when not.
+#define ANC_TURN_AROUND_TIME_US RX_TO_TX_TIME_US
+#if (IMMEDIATE_RESPONSE == 1) && (ANC_TURN_AROUND_TIME_US != RX_TO_TX_TIME_US)
+ #error "When using immediate response, anchor turn-around time has to be equal to RX to TX time!"
+#endif
+// Default tag turn-around time: cannot be less than 300 us. Defined as 500 us
+// so that the tag is not transmitting more than one frame by millisecond (for
+// power management purpose).
+#define TAG_TURN_AROUND_TIME_US 1500 //TODO tune again!!! (300)
+
+// "Long" response delays value. Over this limit, special processes must be
+// applied.
+#define LONG_RESP_DLY_LIMIT_US 25000
+
+// Delay between blink reception and ranging init message. This is the same for
+// all modes.
+#define RNG_INIT_REPLY_DLY_MS (20)
+
+#define MAX(a,b) \
+ ({ __typeof__ (a) _a = (a); \
+ __typeof__ (b) _b = (b); \
+ _a > _b ? _a : _b; })
+
+
+#define BLINK_DURATION_MS RNG_INIT_REPLY_DLY_MS + 1
+#define RANGE_DURATION_MS MAX(18, POLL_SLEEP_DELAY) //to increase time between ranging, modify POLL_SLEEP_DELAY
+//#define BLINK_FREQUENCY 0.0001
+#define BLINK_PERIOD_MS 1000 //time to wait between sending blink messages
+
+#define RX_CHECK_ON_PERIOD 200 //TODO modify
+
+// Reception start-up time, in symbols.
+#define RX_START_UP_SY 16
+
+//TDMA defines
+#define MIN_FRAMELENGTH 4 //minimum size by TDMA E-ASAP
+
+typedef uint64_t uint64 ;
+
+typedef int64_t int64 ;
+
+
+typedef enum instanceModes{DISCOVERY, CONNECTION_PENDING, TAG, ANCHOR, NUM_MODES} INST_MODE;
+
+typedef enum discovery_modes
+{
+ WAIT_INF_REG,
+ COLLECT_INF_REG,
+ WAIT_INF_INIT,
+ WAIT_RNG_INIT,
+ WAIT_SEND_SUG,
+ SEND_SUG,
+ EXIT
+}
+DISCOVERY_MODE;
+
+
+//Listener = in this mode, the instance only receives frames, does not respond
+//Tag = Exchanges DecaRanging messages (Poll-Response-Final) with Anchor and enabling Anchor to calculate the range between the two instances
+//Anchor = see above
+
+typedef enum inst_states
+{
+ TA_INIT, //0
+ TA_TXE_WAIT, //1
+ TA_TXINF_WAIT_SEND, //2
+ TA_TXPOLL_WAIT_SEND, //3
+ TA_TXFINAL_WAIT_SEND, //4
+ TA_TXRESPONSE_WAIT_SEND, //5
+ TA_TX_WAIT_CONF, //6
+ TA_RXE_WAIT, //7
+ TA_RX_WAIT_DATA, //8
+ TA_SLEEP_DONE, //9
+ TA_TXBLINK_WAIT_SEND, //10
+ TA_TXRANGINGINIT_WAIT_SEND, //11
+ TA_TX_SELECT, //12
+ TA_MODE_SELECT, //13
+ TA_TXREPORT_WAIT_SEND, //14
+ TA_TXSUG_WAIT_SEND //15
+} INST_STATES;
+
+
+// This file defines data and functions for access to Parameters in the Device
+//message structure for Poll, Response and Final message
+
+typedef struct
+{
+ uint8 frameCtrl[2]; // frame control bytes 00-01
+ uint8 seqNum; // sequence_number 02
+ uint8 panID[2]; // PAN ID 03-04
+ uint8 destAddr[ADDR_BYTE_SIZE_L]; // 05-12 using 64 bit addresses
+ uint8 sourceAddr[ADDR_BYTE_SIZE_L]; // 13-20 using 64 bit addresses
+ uint8 messageData[MAX_USER_PAYLOAD_STRING_LL] ; // 21-124 (application data and any user payload)
+ uint8 fcs[2] ; // 125-126 we allow space for the CRC as it is logically part of the message. However ScenSor TX calculates and adds these bytes.
+} srd_msg_dlsl ;
+
+typedef struct
+{
+ uint8 frameCtrl[2]; // frame control bytes 00-01
+ uint8 seqNum; // sequence_number 02
+ uint8 panID[2]; // PAN ID 03-04
+ uint8 destAddr[ADDR_BYTE_SIZE_S]; // 05-06
+ uint8 sourceAddr[ADDR_BYTE_SIZE_S]; // 07-08
+ uint8 messageData[MAX_USER_PAYLOAD_STRING_SS] ; // 09-124 (application data and any user payload)
+ uint8 fcs[2] ; // 125-126 we allow space for the CRC as it is logically part of the message. However ScenSor TX calculates and adds these bytes.
+} srd_msg_dsss ;
+
+typedef struct
+{
+ uint8 frameCtrl[2]; // frame control bytes 00-01
+ uint8 seqNum; // sequence_number 02
+ uint8 panID[2]; // PAN ID 03-04
+ uint8 destAddr[ADDR_BYTE_SIZE_L]; // 05-12 using 64 bit addresses
+ uint8 sourceAddr[ADDR_BYTE_SIZE_S]; // 13-14
+ uint8 messageData[MAX_USER_PAYLOAD_STRING_LS] ; // 15-124 (application data and any user payload)
+ uint8 fcs[2] ; // 125-126 we allow space for the CRC as it is logically part of the message. However ScenSor TX calculates and adds these bytes.
+} srd_msg_dlss ;
+
+typedef struct
+{
+ uint8 frameCtrl[2]; // frame control bytes 00-01
+ uint8 seqNum; // sequence_number 02
+ uint8 panID[2]; // PAN ID 03-04
+ uint8 destAddr[ADDR_BYTE_SIZE_S]; // 05-06
+ uint8 sourceAddr[ADDR_BYTE_SIZE_L]; // 07-14 using 64 bit addresses
+ uint8 messageData[MAX_USER_PAYLOAD_STRING_LS] ; // 15-124 (application data and any user payload)
+ uint8 fcs[2] ; // 125-126 we allow space for the CRC as it is logically part of the message. However ScenSor TX calculates and adds these bytes.
+} srd_msg_dssl ;
+
+///// extended message definitions
+
+typedef struct
+{
+ uint8 frameCtrl[2]; // frame control bytes 00-01
+ uint8 seqNum; // sequence_number 02
+ uint8 panID[2]; // PAN ID 03-04
+ uint8 destAddr[ADDR_BYTE_SIZE_L]; // 05-12 using 64 bit addresses
+ uint8 sourceAddr[ADDR_BYTE_SIZE_L]; // 13-20 using 64 bit addresses
+ uint8 messageData[MAX_EXTENDED_USER_PAYLOAD_STRING_LL] ; // 21-1020 (application data and any user payload)
+ uint8 fcs[2] ; // 1021-1022 we allow space for the CRC as it is logically part of the message. However ScenSor TX calculates and adds these bytes.
+} srd_ext_msg_dlsl ;
+
+typedef struct
+{
+ uint8 frameCtrl[2]; // frame control bytes 00-01
+ uint8 seqNum; // sequence_number 02
+ uint8 panID[2]; // PAN ID 03-04
+ uint8 destAddr[ADDR_BYTE_SIZE_S]; // 05-06
+ uint8 sourceAddr[ADDR_BYTE_SIZE_S]; // 07-08
+ uint8 messageData[MAX_EXTENDED_USER_PAYLOAD_STRING_SS] ; // 09-1020 (application data and any user payload)
+ uint8 fcs[2] ; // 1021-1022 we allow space for the CRC as it is logically part of the message. However ScenSor TX calculates and adds these bytes.
+} srd_ext_msg_dsss ;
+
+typedef struct
+{
+ uint8 frameCtrl[2]; // frame control bytes 00-01
+ uint8 seqNum; // sequence_number 02
+ uint8 panID[2]; // PAN ID 03-04
+ uint8 destAddr[ADDR_BYTE_SIZE_L]; // 05-12 using 64 bit addresses
+ uint8 sourceAddr[ADDR_BYTE_SIZE_S]; // 13-14
+ uint8 messageData[MAX_EXTENDED_USER_PAYLOAD_STRING_LS] ; // 15-1020 (application data and any user payload)
+ uint8 fcs[2] ; // 1021-1022 we allow space for the CRC as it is logically part of the message. However ScenSor TX calculates and adds these bytes.
+} srd_ext_msg_dlss ;
+
+typedef struct
+{
+ uint8 frameCtrl[2]; // frame control bytes 00-01
+ uint8 seqNum; // sequence_number 02
+ uint8 panID[2]; // PAN ID 03-04
+ uint8 destAddr[ADDR_BYTE_SIZE_S]; // 05-06
+ uint8 sourceAddr[ADDR_BYTE_SIZE_L]; // 07-14 using 64 bit addresses
+ uint8 messageData[MAX_EXTENDED_USER_PAYLOAD_STRING_LS] ; // 15-1020 (application data and any user payload)
+ uint8 fcs[2] ; // 1021-1022 we allow space for the CRC as it is logically part of the message. However ScenSor TX calculates and adds these bytes.
+} srd_ext_msg_dssl ;
+
+
+
+//12 octets for Minimum IEEE ID blink
+typedef struct
+{
+ uint8 frameCtrl; // frame control bytes 00
+ uint8 seqNum; // sequence_number 01
+ uint8 tagID[BLINK_FRAME_SOURCE_ADDRESS]; // 02-09 64 bit address
+ uint8 fcs[2] ; // 10-11 we allow space for the CRC as it is logically part of the message. However ScenSor TX calculates and adds these bytes.
+} iso_IEEE_EUI64_blink_msg ;
+
+typedef struct
+{
+ uint8 channelNumber ; // valid range is 1 to 11
+ uint8 preambleCode ; // 00 = use NS code, 1 to 24 selects code
+ uint8 pulseRepFreq ; // NOMINAL_4M, NOMINAL_16M, or NOMINAL_64M
+ uint8 dataRate ; // DATA_RATE_1 (110K), DATA_RATE_2 (850K), DATA_RATE_3 (6M81)
+ uint8 preambleLen ; // values expected are 64, (128), (256), (512), 1024, (2048), and 4096
+ uint8 pacSize ;
+ uint8 nsSFD ;
+ uint16 sfdTO; //!< SFD timeout value (in symbols) e.g. preamble length (128) + SFD(8) - PAC + some margin ~ 135us... DWT_SFDTOC_DEF; //default value
+} instanceConfig_t ;
+
+
+/******************************************************************************************************************
+*******************************************************************************************************************
+*******************************************************************************************************************/
+
+#define MAX_EVENT_NUMBER (8)
+//NOTE: Accumulators don't need to be stored as part of the event structure as when reading them only one RX event can happen...
+//the receiver is singly buffered and will stop after a frame is received
+
+typedef struct
+{
+ uint8 type; // event type
+ uint8 typeSave; // holds the event type - does not clear (used to show what event has been processed)
+ uint8 typePend; // set if there is a pending event (i.e. DW is not in IDLE (TX/RX pending)
+ uint16 rxLength ;
+
+ uint64 timeStamp ; // last timestamp (Tx or Rx)
+
+ uint32 timeStamp32l ; // last tx/rx timestamp - low 32 bits
+ uint32 timeStamp32h ; // last tx/rx timestamp - high 32 bits
+
+ union {
+ //holds received frame (after a good RX frame event)
+ uint8 frame[EXTENDED_FRAME_SIZE];
+ srd_ext_msg_dlsl rxmsg_ll ; //64 bit addresses
+ srd_ext_msg_dssl rxmsg_sl ;
+ srd_ext_msg_dlss rxmsg_ls ;
+ srd_ext_msg_dsss rxmsg_ss ; //16 bit addresses
+ iso_IEEE_EUI64_blink_msg rxblinkmsg;
+ }msgu;
+
+}event_data_t ;
+
+#define RTD_MED_SZ 8 // buffer size for mean of 8
+
+typedef struct {
+ uint8 PGdelay;
+
+ //TX POWER
+ //31:24 BOOST_0.125ms_PWR
+ //23:16 BOOST_0.25ms_PWR-TX_SHR_PWR
+ //15:8 BOOST_0.5ms_PWR-TX_PHR_PWR
+ //7:0 DEFAULT_PWR-TX_DATA_PWR
+ uint32 txPwr[2]; //
+}tx_struct;
+
+
+
+
+
+#endif
diff --git a/src/application/comm_scheduler.c b/src/application/comm_scheduler.c
index f70aa1c..36b207f 100644
--- a/src/application/comm_scheduler.c
+++ b/src/application/comm_scheduler.c
@@ -1,46 +1,46 @@
-#include "comm_scheduler.h"
-#include "port.h"
-
-//private mehtods
-//int node_compare(llist_node first, llist_node second)
-//{
-// timing_node *first_node = (timing_node *)first;
-// timing_node *second_node = (timing_node *)second;
+//#include "comm_scheduler.h"
+//#include "port.h"
//
-// if(first_node->time == second_node->time)
-// {
-// return 0;
-// }
-// else if(first_node->time > second_node->time)
-// {
-// return 1;
-// }
-// else
-// {
-// return -1;
-// }
-// return 0;
-//}
+////private mehtods
+////int node_compare(llist_node first, llist_node second)
+////{
+//// timing_node *first_node = (timing_node *)first;
+//// timing_node *second_node = (timing_node *)second;
+////
+//// if(first_node->time == second_node->time)
+//// {
+//// return 0;
+//// }
+//// else if(first_node->time > second_node->time)
+//// {
+//// return 1;
+//// }
+//// else
+//// {
+//// return -1;
+//// }
+//// return 0;
+////}
+////
+////
+////bool node_equal(llist_node first, llist_node second)
+////{
+//// if(first == second){return true;}
+////
+//// return false;
+////}
//
//
-//bool node_equal(llist_node first, llist_node second)
-//{
-// if(first == second){return true;}
+////class methods
//
-// return false;
+//static struct CommScheduler new(float blink_frequency, uint32 blink_duration, uint32 range_duration){
+// return (struct CommScheduler){
+//// .list = llist_create(node_compare, node_equal, 0),
+// .blink_frequency = blink_frequency,
+// .blink_duration = blink_duration,
+// .range_duration = range_duration,
+// .blink_period = (int)(1.0/blink_frequency)
+// };
//}
-
-
-//class methods
-
-static struct CommScheduler new(float blink_frequency, uint32 blink_duration, uint32 range_duration){
- return (struct CommScheduler){
-// .list = llist_create(node_compare, node_equal, 0),
- .blink_frequency = blink_frequency,
- .blink_duration = blink_duration,
- .range_duration = range_duration,
- .blink_period = (int)(1.0/blink_frequency)
- };
-}
-
-const struct CommSchedulerClass CommScheduler={.new=&new};
+//
+//const struct CommSchedulerClass CommScheduler={.new=&new};
diff --git a/src/application/comm_scheduler.h b/src/application/comm_scheduler.h
index d7b9c3e..d2bc91d 100644
--- a/src/application/comm_scheduler.h
+++ b/src/application/comm_scheduler.h
@@ -1,25 +1,25 @@
-#include "llist.h"
-#include "deca_types.h"
-
-typedef struct
-{
- uint32 time; //time the node is scheduled to be used.
- uint32 duration; //expected time the node event will take before completion
-
-} timing_node ;
-
-struct CommScheduler
-{
- llist list;
-
- uint32 blink_frequency;
- uint32 blink_period;
- uint32 blink_duration;
- uint32 range_duration;
-};
-
-extern const struct CommSchedulerClass
-{
- struct CommScheduler (*new)(float blink_frequency, uint32 blink_duration, uint32 range_duration);
-
-} CommScheduler;
+//#include "llist.h"
+//#include "deca_types.h"
+//
+//typedef struct
+//{
+// uint32 time; //time the node is scheduled to be used.
+// uint32 duration; //expected time the node event will take before completion
+//
+//} timing_node ;
+//
+//struct CommScheduler
+//{
+// llist list;
+//
+// uint32 blink_frequency;
+// uint32 blink_period;
+// uint32 blink_duration;
+// uint32 range_duration;
+//};
+//
+//extern const struct CommSchedulerClass
+//{
+// struct CommScheduler (*new)(float blink_frequency, uint32 blink_duration, uint32 range_duration);
+//
+//} CommScheduler;
diff --git a/src/application/dw_main.c b/src/application/dw_main.c
index 9929873..336fdba 100644
--- a/src/application/dw_main.c
+++ b/src/application/dw_main.c
@@ -36,7 +36,7 @@ extern void send_usbmessage(uint8*, int);
#define SWS1_CH5_MODE 0x40 //channel 5 mode (switch S1-7)
int dr_mode = 0;
-int instance_mode = ANCHOR;
+int instance_mode = DISCOVERY;
uint8 s1switch = 0;
int chan, tagaddr, ancaddr, prf;
@@ -191,6 +191,8 @@ uint32 inittestapplication(uint8 s1switch)
result = instance_init() ;
if (0 > result) return(-1) ; // Some failure has occurred
+ result = tdma_init_s();
+
port_set_dw1000_fastrate();
devID = instancereaddeviceid() ;
@@ -202,19 +204,19 @@ uint32 inittestapplication(uint8 s1switch)
if(s1switch & SWS1_ANC_MODE)
{
- instance_mode = ANCHOR;
+// instance_mode = ANCHOR;
led_on(LED_PC6);
}
else
{
- instance_mode = TAG;
+// instance_mode = TAG;
led_on(LED_PC7);
}
- instance_init_s(instance_mode);
- dr_mode = decarangingmode(s1switch);
+ instance_init_s();
+ dr_mode = decarangingmode(s1switch);
chan = chConfig[dr_mode].channelNumber ;
prf = (chConfig[dr_mode].pulseRepFreq == DWT_PRF_16M)? 16 : 64 ;
@@ -329,6 +331,8 @@ int dw_main(void)
// enable the USB functionality
usb_init();
Sleep(1000);
+ usb_run();
+ Sleep(10000);
#endif
s1switch = port_is_boot1_on(0) << 1 // is_switch_on(TA_SW1_2) << 2
@@ -449,32 +453,32 @@ int dw_main(void)
#endif
if(s1switch & SWS1_ANC_MODE)
{
- instance_mode = ANCHOR;
+// instance_mode = ANCHOR;
led_on(LED_PC6);
}
else
{
- instance_mode = TAG;
+// instance_mode = TAG;
led_on(LED_PC7);
}
#if (USING_LCD == 1)
- if(instance_mode == TAG)
- {
- memcpy(&dataseq[0], (const uint8 *) " TAG BLINK ", 16);
+// 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
- }
+// }
+// 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);
@@ -494,12 +498,13 @@ int dw_main(void)
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;
- ranging = 1;
+// ranging = 1;
//send the new range information to LCD and/or USB
range_result = instance_get_idist(inst->newRangeUWBIndex);
// uint8 debug_msg[200];
@@ -570,28 +575,43 @@ int dw_main(void)
rng = (int) (range_result*1000);
rng_raw = (int) (instance_get_idistraw(inst->newRangeUWBIndex)*1000);
- if(instance_mode == TAG)
- {
- n = sprintf((char*)&dataseq[0], "t %llX %llX %08X %08X", aaddr, taddr, rng, rng_raw);
-
- }
- else
- {
-// n = sprintf((char*)&dataseq[0], "a %llX %llX %08X %08X", aaddr, taddr, rng, rng_raw);
- n = sprintf((char*)&dataseq[0], "RANGE_COMPLETE,%llX,%llX", taddr, aaddr);
- }
+
+
+// n = sprintf((char*)&dataseq[0], "RANGE_COMPLETE,%llX,%llX", taddr, aaddr);
+// n = sprintf((char*)&dataseq[0], "RANGE_COMPLETE,%llX,%llX", taddr, aaddr);
+
+ if(instance_mode == TAG)
+ {
+// n = sprintf((char*)&dataseq[0], "RANGE_COMPLETE,%llX,%llX", taddr, aaddr);
+
+ }
+ else
+ {
+// n = sprintf((char*)&dataseq[0], "RANGE_COMPLETE,%llX,%llX", taddr, aaddr);
+ }
+
+// if(instance_mode == TAG)
+// {
+// n = sprintf((char*)&dataseq[0], "t %llX %llX %08X %08X", aaddr, taddr, rng, rng_raw);
+//
+// }
+// else
+// {
+//// n = sprintf((char*)&dataseq[0], "a %llX %llX %08X %08X", aaddr, taddr, rng, rng_raw);
+// n = sprintf((char*)&dataseq[0], "RANGE_COMPLETE,%llX,%llX", taddr, aaddr);
+// }
#ifdef USB_SUPPORT //this is set in the port.h file
- if(instance_mode == ANCHOR)
+ if(instance_mode == TAG)
{
- send_usbmessage(&dataseq[0], n);
+// send_usbmessage(&dataseq[0], n);
}
// send_usbmessage(&dataseq[0], n);
#endif
}
#if (USING_LCD == 1)
- if(ranging == 0) //discovery/initialization mode for anchor and tag
+ if(instanceisranging() == 0) //discovery/initialization mode for anchor and tag
{
if(instance_mode != ANCHOR)
{
@@ -620,7 +640,7 @@ int dw_main(void)
if(instanceanchorwaiting() == 2)
{
- ranging = 1;
+// ranging = 1;
dataseq[0] = 0x2 ; //return cursor home
writetoLCD( 1, 0, dataseq);
memcpy(&dataseq[0], (const uint8 *) " RANGING ", 16);
@@ -676,7 +696,7 @@ int dw_main(void)
#endif
#ifdef USB_SUPPORT //this is set in the port.h file
- usb_run();
+// usb_run();
#endif
if(canSleep)__WFI();
diff --git a/src/application/instance.c b/src/application/instance.c
index 0c1c67c..18529f7 100644
--- a/src/application/instance.c
+++ b/src/application/instance.c
@@ -20,7 +20,7 @@
#include "lib.h"
#include "instance.h"
-#include "llist.h"
+//#include "llist.h"
@@ -59,6 +59,8 @@ int usbtxdebugdataprev_size = 0;
// Functions
// -------------------------------------------------------------------------------------------------------------------
+
+
// -------------------------------------------------------------------------------------------------------------------
//
// function to construct the message/frame header bytes
@@ -67,24 +69,166 @@ int usbtxdebugdataprev_size = 0;
//
void instanceconfigframeheader(instance_data_t *inst)
{
- inst->msg[inst->uwbToRangeWith].panID[0] = (inst->panID) & 0xff;
- inst->msg[inst->uwbToRangeWith].panID[1] = inst->panID >> 8;
- //set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
- inst->msg[inst->uwbToRangeWith].frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/;
+// //configure ranging message(s)
+// for(int i = 0; i < UWB_LIST_SIZE; i++)//TODO only have one msg
+// {
+// inst->msg[i].panID[0] = (inst->panID) & 0xff;
+// inst->msg[i].panID[1] = inst->panID >> 8;
+//
+// //set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
+// inst->msg[i].frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/;
+//#if (USING_64BIT_ADDR==1)
+// //source/dest addressing modes and frame version
+// inst->msg[i].frameCtrl[1] = 0xC /*dest extended address (64bits)*/ | 0xC0 /*src extended address (64bits)*/;
+//#else
+// inst->msg[i].frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0x80 /*src short address (16bits)*/;
+//#endif
+// }
+
+ //configure ranging message
+ inst->msg.panID[0] = (inst->panID) & 0xff;
+ inst->msg.panID[1] = inst->panID >> 8;
+
+ //set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
+ inst->msg.frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/;
+#if (USING_64BIT_ADDR==1)
+ //source/dest addressing modes and frame version
+ inst->msg.frameCtrl[1] = 0xC /*dest extended address (64bits)*/ | 0xC0 /*src extended address (64bits)*/;
+#else
+ inst->msg.frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0x80 /*src short address (16bits)*/;
+#endif
+
+
+ //configure RNG_INIT message
+ inst->rng_initmsg.frameCtrl[0] = 0x41;
+
+#if (USING_64BIT_ADDR == 1)
+ inst->rng_initmsg.frameCtrl[1] = 0xCC;
+#else
+ inst->rng_initmsg.frameCtrl[1] = 0x8C;
+#endif
+ inst->rng_initmsg.panID[0] = (inst->panID) & 0xff;
+ inst->rng_initmsg.panID[1] = inst->panID >> 8;
+
+
+ //configure INF message
+ inst->inf_msg.panID[0] = (inst->panID) & 0xff;
+ inst->inf_msg.panID[1] = inst->panID >> 8;
+
+ //set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
+ inst->inf_msg.frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/;
#if (USING_64BIT_ADDR==1)
- //source/dest addressing modes and frame version
- inst->msg[inst->uwbToRangeWith].frameCtrl[1] = 0xC /*dest extended address (64bits)*/ | 0xC0 /*src extended address (64bits)*/;
+ //source/dest addressing modes and frame version
+ inst->inf_msg.frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0xC0 /*src extended address (64bits)*/;
#else
- inst->msg[inst->uwbToRangeWith].frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0x80 /*src short address (16bits)*/;
+ inst->inf_msg.frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0x80 /*src short address (16bits)*/;
#endif
+
+
+ //configure RNG_REPORT
+
+ inst->report_msg.panID[0] = (inst->panID) & 0xff;
+ inst->report_msg.panID[1] = inst->panID >> 8;
+
+ //set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
+ inst->report_msg.frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/;
+#if (USING_64BIT_ADDR==1)
+ //source/dest addressing modes and frame version
+ inst->report_msg.frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0xC0 /*src extended address (64bits)*/;
+#else
+ inst->report_msg.frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0x80 /*src short address (16bits)*/;
+#endif
+
+
+ //configure BLINK
+ //blink frames with IEEE EUI-64 tag ID
+ inst->blinkmsg.frameCtrl = 0xC5 ;
+
+
+
+
+////////////////////// OLD definition below //////////////////////
+// inst->msg[inst->uwbToRangeWith].panID[0] = (inst->panID) & 0xff;
+// inst->msg[inst->uwbToRangeWith].panID[1] = inst->panID >> 8;
+//
+// //set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
+// inst->msg[inst->uwbToRangeWith].frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/;
+//#if (USING_64BIT_ADDR==1)
+// //source/dest addressing modes and frame version
+// inst->msg[inst->uwbToRangeWith].frameCtrl[1] = 0xC /*dest extended address (64bits)*/ | 0xC0 /*src extended address (64bits)*/;
+//#else
+// inst->msg[inst->uwbToRangeWith].frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0x80 /*src short address (16bits)*/;
+//#endif
+}
+
+
+// -------------------------------------------------------------------------------------------------------------------
+//
+// function to construct the fixed portions of the message definitions
+//
+// -------------------------------------------------------------------------------------------------------------------
+//
+void instanceconfigmessages(instance_data_t *inst)
+{
+ //initialize ranging message(s)
+ //set source address into the message structure
+// for(int i=0; i<UWB_LIST_SIZE; i++) //TODO only have 1 msg? or one Tag msg, one Anchor msg?
+// {
+// memcpy(&inst->msg[i].sourceAddr[0], &inst->eui64[0], inst->addrByteSize);
+// }
+
+ memcpy(&inst->msg.sourceAddr[0], &inst->eui64[0], inst->addrByteSize);
+
+
+ //initialize RNG_INIT message
+ //set source address into the message structure
+ 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;
+
+ //configure INF message
+ uint16 broadcast_address = BROADCAST_ADDRESS;
+ memcpy(&inst->inf_msg.sourceAddr[0], &inst->eui64[0], inst->addrByteSize);
+ memcpy(&inst->inf_msg.destAddr[0], &broadcast_address, 2);
+ inst->inf_msg.messageData[FCODE] = 0; //message function code (specifies if message is a poll, response or other...)
+
+ //configure RNG_REPORT
+ memcpy(&inst->report_msg.sourceAddr[0], &inst->eui64[0], inst->addrByteSize);
+ memcpy(&inst->report_msg.destAddr[0], &broadcast_address, 2);
+ inst->report_msg.messageData[FCODE] = RTLS_DEMO_MSG_RNG_REPORT; //message function code (specifies if message is a poll, response or other...)
+
+
+ //configure BLINK message
+ memcpy(&inst->blinkmsg.tagID[0], &inst->eui64[0], ADDR_BYTE_SIZE_L);
+
}
+
// -------------------------------------------------------------------------------------------------------------------
//
// Turn on the receiver with/without delay
//
+// -------------------------------------------------------------------------------------------------------------------
+//
void instancerxon(instance_data_t *inst, int delayed, uint64 delayedReceiveTime)
{
if (delayed)
@@ -96,12 +240,12 @@ void instancerxon(instance_data_t *inst, int delayed, uint64 delayedReceiveTime)
// inst->lateRX -= dwt_rxenable(delayed) ; //- as when fails -1 is returned // turn receiver on, immediate/delayed
- uint32 time_now = portGetTickCnt();
+// uint32 time_now = portGetTickCnt();
int dwt_rx_enable_return = dwt_rxenable(delayed);
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "RX_ENABLE time_now: %lu, rx_enable: %i", time_now, dwt_rx_enable_return);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "RX_ENABLE time_now: %lu, rx_enable: %i", time_now, dwt_rx_enable_return);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
inst->lateRX -= dwt_rx_enable_return;
} // end instancerxon()
@@ -141,9 +285,13 @@ const char* get_inst_states_string(enum inst_states state)
case TA_RXE_WAIT : return "TA_RXE_WAIT";
case TA_RX_WAIT_DATA : return "TA_RX_WAIT_DATA";
case TA_SLEEP_DONE : return "TA_SLEEP_DONE";
+ case TA_TXINF_WAIT_SEND : return "TA_TXINF_WAIT_SEND";
case TA_TXBLINK_WAIT_SEND : return "TA_TXBLINK_WAIT_SEND";
case TA_TXRANGINGINIT_WAIT_SEND : return "TA_TXRANGINGINIT_WAIT_SEND";
case TA_TX_SELECT : return "TA_TX_SELECT";
+ case TA_MODE_SELECT : return "TA_MODE_SELECT";
+ case TA_TXREPORT_WAIT_SEND : return "TA_TXREPORT_WAIT_SEND";
+ case TA_TXSUG_WAIT_SEND : return "TA_TXSUG_WAIT_SEND";
default: return "NONE";
}
}
@@ -153,15 +301,32 @@ const char* get_instanceModes_string(enum instanceModes mode)
{
switch (mode)
{
- case LISTENER : return "LISTENER";
+ case DISCOVERY : return "DISCOVERY";
+ case CONNECTION_PENDING : return "CONNECTION_PENDING";
case TAG : return "TAG";
- case ANCHOR : return "ANCHOR";
- case TAG_TDOA : return "TAG_TDOA";
+ case ANCHOR : return "ANCHOR";
case NUM_MODES : return "NUM_MODES";
default: return "NONE";
}
}
+//debug helper function to print the mode
+const char* get_discovery_modes_string(enum discovery_modes mode)
+{
+ switch (mode)
+ {
+ case WAIT_INF_REG : return "WAIT_INF_REG";
+ case COLLECT_INF_REG : return "COLLECT_INF_REG";
+ case WAIT_INF_INIT : return "WAIT_INF_INIT";
+ case WAIT_RNG_INIT : return "WAIT_RNG_INIT";
+ case WAIT_SEND_SUG : return "WAIT_SEND_SUG";
+ case SEND_SUG : return "SEND_SUG";
+ case EXIT : return "EXIT";
+ default: return "NONE";
+ }
+}
+
+
char* get_msg_fcode_string(int fcode)
{
if (fcode == (int)RTLS_DEMO_MSG_RNG_INIT)
@@ -180,13 +345,34 @@ char* get_msg_fcode_string(int fcode)
{
return "RTLS_DEMO_MSG_TAG_FINAL";
}
+ else if(fcode == (int)RTLS_DEMO_MSG_INF_REG)
+ {
+ return "RTLS_DEMO_MSG_INF_REG";
+ }
+ else if(fcode == (int)RTLS_DEMO_MSG_INF_INIT)
+ {
+ return "RTLS_DEMO_MSG_INF_INIT";
+ }
+ else if(fcode == (int)RTLS_DEMO_MSG_INF_SUG)
+ {
+ return "RTLS_DEMO_MSG_INF_SUG";
+ }
+ else if(fcode == (int)RTLS_DEMO_MSG_INF_UPDATE)
+ {
+ return "RTLS_DEMO_MSG_INF_UPDATE";
+ }
+ else if(fcode == (int)RTLS_DEMO_MSG_RNG_REPORT)
+ {
+ return "RTLS_DEMO_MSG_RNG_REPORT";
+ }
else
{
return "NONE";
}
}
-void send_statetousb(instance_data_t *inst)
+//void send_statetousb(instance_data_t *inst)
+void send_statetousb(instance_data_t *inst, struct TDMAHandler *tdma_handler)
{
int usbdebugdata_size = sprintf((char*)&usbdebugdata[0], "%s , %s , %s , %s", get_inst_states_string(inst->testAppState), get_inst_states_string(inst->previousState), get_inst_states_string(inst->nextState), get_instanceModes_string(inst->mode));
@@ -197,6 +383,13 @@ void send_statetousb(instance_data_t *inst)
usb_run();
usbdebugdataprev_size = usbdebugdata_size;
memcpy(usbdebugdataprev, usbdebugdata, usbdebugdata_size);
+
+ int num_neighbors = instfindnumactiveneighbors(inst);
+ uint8 debug_msg[150];
+ int n = sprintf((char*)&debug_msg[0], "mode: %s, num_neighbors %d, discovery_mode %s", get_instanceModes_string(inst->mode), num_neighbors, get_discovery_modes_string(tdma_handler->discovery_mode));
+ send_usbmessage(&debug_msg[0], n);
+ usb_run();
+
}
}
@@ -223,66 +416,64 @@ void send_txmsgtousb(char *data)
// -------------------------------------------------------------------------------------------------------------------
//
-// the main instance state machine (all the instance modes Tag, Anchor or Listener use the same statemachine....)
+// the main instance state machine (all the instance modes Tag or Anchor use the same state machine)
//
// -------------------------------------------------------------------------------------------------------------------
//
-int testapprun(instance_data_t *inst, int message)
+int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int message)
{
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();
+// tdma_handler->set_slot(tdma_handler);
+ if(tdma_handler->slot_transition(tdma_handler))
+ {
+ message = 0;
+ }
+
+ tdma_handler->check_discovery_mode_expiration(tdma_handler);
+
//NOTE: temporary debug code
if(inst->testAppState != inst->lastState){
inst->currentStateStartTime = portGetTickCnt();
}
inst->lastState = inst->testAppState;
- send_statetousb(inst);
-
+ send_statetousb(inst, tdma_handler);
switch (inst->testAppState)
{
case TA_INIT :
{
- // if (inst->mode == ANCHOR)
- // {
- // send_statetousb(inst);
- // }
switch (inst->mode)
{
- case TAG:
+ case DISCOVERY:
{
int mode = 0;
+ dwt_forcetrxoff();
- dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN); //allow data, ACK frames;
- inst->frameFilteringEnabled = 1 ;
- dwt_setpanid(inst->panID);
- dwt_seteui(inst->eui64);
+ dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN | DWT_FF_RSVD_EN);
+ inst->frameFilteringEnabled = 1 ;
+ dwt_seteui(inst->eui64);
+ dwt_setpanid(inst->panID);
+
+ inst->uwbShortAdd = inst->eui64[0] + (inst->eui64[1] << 8);//TODO use a hashing algorithm
- inst->uwbShortAdd = inst->eui64[0] + (inst->eui64[1] << 8);
-
-
#if (USING_64BIT_ADDR==0)
- dwt_setaddress16(inst->uwbShortAdd);
+ dwt_setaddress16(inst->uwbShortAdd);
+ memcpy(&inst->uwbList[0][0], &inst->uwbShortAdd, inst->addrByteSize);
+#else
+ memcpy(&inst->uwbList[0][0], &inst->eui64, inst->addrByteSize);
#endif
-
- //set source address into the message structure
- for(int i=0; i<UWB_LIST_SIZE; i++)
- {
- memcpy(&inst->msg[i].sourceAddr[0], &inst->eui64[0], inst->addrByteSize);
- }
-
- inst->testAppState = TA_TXBLINK_WAIT_SEND;
- inst->tx_scheduler.last_blink_time = portGetTickCnt();
- memcpy(&inst->blinkmsg.tagID[0], &inst->eui64[0], ADDR_BYTE_SIZE_L);
+ inst->uwbListLen = 1;
+ inst->uwbListType[0] = UWB_LIST_SELF;
mode = (DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV);
@@ -295,77 +486,49 @@ int testapprun(instance_data_t *inst, int message)
dwt_configuresleep(mode, DWT_WAKE_WK|DWT_WAKE_CS|DWT_SLP_EN); //configure the on wake parameters (upload the IC config settings)
#endif
- }
- break;
- case ANCHOR:
- {
- dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN | DWT_FF_RSVD_EN);
- inst->frameFilteringEnabled = 1 ;
- dwt_seteui(inst->eui64);
- dwt_setpanid(inst->panID);
-
- inst->uwbShortAdd = inst->eui64[0] + (inst->eui64[1] << 8);
-
-#if (USING_64BIT_ADDR==0)
- dwt_setaddress16(inst->uwbShortAdd);
-#endif
+ instanceconfigframeheader(inst);
+ instanceconfigmessages(inst);
- //set source address into the message structure
- for(int i=0; i<UWB_LIST_SIZE; i++)
- {
- memcpy(&inst->msg[i].sourceAddr[0], &inst->eui64[0], inst->addrByteSize);
- }
- //set source address into the message structure
- memcpy(&inst->rng_initmsg.sourceAddr[0], &inst->eui64[0], inst->addrByteSize);
+ // First time listening we don't do a delayed RX
+ dwt_setrxaftertxdelay(0);
+ //change to next state - wait to receive a message
+ tdma_handler->discoveryStartTime = portGetTickCnt();
+ tdma_handler->last_blink_time = portGetTickCnt();
+ inst->testAppState = TA_RXE_WAIT ;
- // First time anchor listens we don't do a delayed RX
- dwt_setrxaftertxdelay(0);
- //change to next state - wait to receive a message
- inst->testAppState = TA_RXE_WAIT ;
+ dwt_setrxtimeout(0);
+ inst->canPrintInfo = 1;
+ inst->wait4ack = 0;
- dwt_setrxtimeout(0);
- inst->canPrintInfo = 1;
}
break;
- case LISTENER:
- {
- dwt_enableframefilter(DWT_FF_NOTYPE_EN); //disable frame filtering
- inst->frameFilteringEnabled = 0 ;
- // First time anchor listens we don't do a delayed RX
- dwt_setrxaftertxdelay(0);
- //change to next state - wait to receive a message
- inst->testAppState = TA_RXE_WAIT ;
-
- dwt_setrxtimeout(0);
- }
- break ; // end case TA_INIT
default:
break;
}
break; // end case TA_INIT
}
- case TA_TX_SELECT:
+ case TA_MODE_SELECT :
+ {
+ //TODO maybe this will be used to transition between DISCOVERY/ANCHOR/TAG modes?
+ break;
+ }
+ case TA_TX_SELECT :
{
// send_statetousb(inst);
// select whether to blink or send out a range poll message.
// select a uwb from the list if sending out a range poll message
- int uwb_index = inst->tx_scheduler.tx_select(&inst->tx_scheduler);
+// int uwb_index = inst->tx_scheduler.tx_select(&inst->tx_scheduler);
+
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "selected index %i ", uwb_index);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
- if(uwb_index < 0){
- //do nothing
- //TODO would be best to sleep until next possible poll and then select again...
- done = INST_DONE_WAIT_FOR_NEXT_EVENT; //TODO make sure that this is the right thing to do.
- }else if(uwb_index > 244){
- inst->testAppState = TA_TXBLINK_WAIT_SEND;
- inst->uwbToRangeWith = (uint8)255;
- }else{
- inst->testAppState = TA_TXPOLL_WAIT_SEND;
- inst->uwbToRangeWith = (uint8)uwb_index;
+ //select a TX action, return if TRUE if we should move on to another state
+ if(tdma_handler->tx_select(tdma_handler) == TRUE)
+ {
+ dwt_forcetrxoff();
+ }
+ else
+ {
+ done = INST_DONE_WAIT_FOR_NEXT_EVENT;
}
break; // end case TA_TX_SELECT
@@ -377,7 +540,7 @@ int testapprun(instance_data_t *inst, int message)
// send_statetousb(inst);
// }
event_data_t* dw_event = instance_getevent(10); //clear the event from the queue
- // waiting for timout from application to wakup IC
+ // waiting for timeout from application to wakeup IC
if (dw_event->type != DWT_SIG_RX_TIMEOUT)
{
// if no pause and no wake-up timeout continue waiting for the sleep to be done.
@@ -406,13 +569,16 @@ int testapprun(instance_data_t *inst, int message)
//MP bug - TX antenna delay needs reprogramming as it is not preserved after DEEP SLEEP
dwt_settxantennadelay(inst->txAntennaDelay) ;
}
+ instancesetantennadelays();
#endif
- instancesetantennadelays(); //this will update the antenna delay if it has changed
+ //TODO should this be here? or only if DEEP_SLEEP is enabled (above) putting only above for now
+// instancesetantennadelays(); //this will update the antenna delay if it has changed
break;
}
case TA_TXE_WAIT : //either go to sleep or proceed to TX a message
{
+
// if (inst->mode == ANCHOR)
// {
// send_statetousb(inst);
@@ -429,7 +595,7 @@ int testapprun(instance_data_t *inst, int message)
done = INST_DONE_WAIT_FOR_NEXT_EVENT_TO; //don't sleep here but kick off the TagTimeoutTimer (instancetimer)
inst->testAppState = TA_SLEEP_DONE;
- inst->canPrintInfo = 1;
+// inst->canPrintInfo = 1;
#if (DEEP_SLEEP == 1)
if (inst->sleepingEabled)
@@ -439,16 +605,16 @@ int testapprun(instance_data_t *inst, int message)
}
#endif
//DW1000 gone to sleep - report the received range
- if(inst->newRangeUWBIndex != 255)
- {
- if(inst->tof[inst->newRangeUWBIndex] > 0) //if ToF == 0 - then no new range to report
- {
- if(reportTOF(inst, inst->newRangeUWBIndex)==0)
- {
- inst->newRange = 1;
- }
- }
- }
+// if(inst->newRangeUWBIndex != 255)
+// {
+// if(inst->tof[inst->newRangeUWBIndex] > 0) //if ToF == 0 - then no new range to report
+// {
+// if(reportTOF(inst, inst->newRangeUWBIndex)==0)
+// {
+// inst->newRange = 1;
+// }
+// }
+// }
//inst->deviceissleeping = 1; //this is to stop polling device status register (as it will wake it up)
}
@@ -468,8 +634,8 @@ int testapprun(instance_data_t *inst, int message)
int flength = (BLINK_FRAME_CRTL_AND_ADDRESS + FRAME_CRC);
- //blink frames with IEEE EUI-64 tag ID
- inst->blinkmsg.frameCtrl = 0xC5 ;
+// //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
@@ -478,11 +644,12 @@ int testapprun(instance_data_t *inst, int message)
//using wait for response to do delayed receive
inst->wait4ack = DWT_RESPONSE_EXPECTED;
- dwt_setrxtimeout((uint16)inst->fwtoTimeB_sy); //units are symbols
+ //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); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
+ 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 dealyed RX
+ int tx_start = dwt_starttx(DWT_START_TX_IMMEDIATE | inst->wait4ack); //always using immediate TX and enable delayed RX
if(tx_start == 0)
{
@@ -506,8 +673,10 @@ int testapprun(instance_data_t *inst, int message)
inst->blink0range1 = 0;
inst->blink_start = portGetTickCnt();
- // send_txmsgtousb("BLINK-SUCCESS");
+// send_txmsgtousb("BLINK-SUCCESS");
inst->timeofTx = portGetTickCnt();
+ tdma_handler->last_blink_time = portGetTickCnt();
+
}
else
{
@@ -531,39 +700,13 @@ int testapprun(instance_data_t *inst, int message)
// }
// send_txmsgtousb("RTLS_DEMO_MSG_RNG_INIT");
- uint16 resp_dly_us, resp_dly;
-
int psduLength = RANGINGINIT_MSG_LEN;
- 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;
-
- // 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;
-
- inst->rng_initmsg.frameCtrl[0] = 0x41;
-
#if (USING_64BIT_ADDR == 1)
- inst->rng_initmsg.frameCtrl[1] = 0xCC;
psduLength += FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
#else
- inst->rng_initmsg.frameCtrl[1] = 0x8C;
psduLength += FRAME_CRTL_AND_ADDRESS_LS + FRAME_CRC;
#endif
- inst->rng_initmsg.panID[0] = (inst->panID) & 0xff;
- inst->rng_initmsg.panID[1] = inst->panID >> 8;
inst->rng_initmsg.seqNum = inst->frameSN++;
@@ -581,52 +724,225 @@ int testapprun(instance_data_t *inst, int message)
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
- //CONFIGURE FIXED PARTS OF RESPONSE MESSAGE FRAME (these won't change)
- //program option octet and parameters (not used currently)
- inst->msg[inst->uwbToRangeWith].messageData[RES_R1] = 0x2; // "activity"
- inst->msg[inst->uwbToRangeWith].messageData[RES_R2] = 0x0; //
- inst->msg[inst->uwbToRangeWith].messageData[RES_R3] = 0x0;
- inst->msg[inst->uwbToRangeWith].messageData[FCODE] = RTLS_DEMO_MSG_ANCH_RESP; //message function code (specifies if message is a poll, response or other...)
-
- instanceconfigframeheader(inst);
- inst->timeofTx = portGetTickCnt();
-// ints->
+ 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!!!");
+// 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++;
+
+ //update time since frame start!
+ 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());
+
+
+ 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 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;
+ }
+ else
+ {
+ inst->nextState = TA_TXPOLL_WAIT_SEND;
+ }
+
+
+ if(fcode == RTLS_DEMO_MSG_INF_INIT ||
+ fcode == RTLS_DEMO_MSG_INF_SUG ||
+ fcode == RTLS_DEMO_MSG_INF_UPDATE)
+ {
+ fcode = RTLS_DEMO_MSG_INF_REG;
+ memcpy(&inst->inf_msg.messageData[FCODE], &fcode, sizeof(uint8));
+ }
+ }
+ 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();
+ }
+
+ 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);
- int psduLength = 0;
+
+ //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[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);
+// instanceconfigframeheader(inst);
#if (USING_64BIT_ADDR==1)
psduLength = TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
#else
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[inst->uwbToRangeWith], 0) ; // write the frame data
+ dwt_writetxdata(psduLength, (uint8 *) &inst->msg, 0) ; // write the frame data
//response is expected
inst->wait4ack = DWT_RESPONSE_EXPECTED;
@@ -640,27 +956,27 @@ int testapprun(instance_data_t *inst, int message)
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 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();
+// 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();
}
@@ -704,14 +1020,17 @@ int testapprun(instance_data_t *inst, int message)
}
case TA_TXFINAL_WAIT_SEND :
{
+ dwt_setrxaftertxdelay((uint16)0);
+ dwt_setrxtimeout((uint16)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...)
-
- instanceconfigframeheader(inst);
+// 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...)
#if (USING_64BIT_ADDR==1)
psduLength = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
@@ -719,16 +1038,23 @@ int testapprun(instance_data_t *inst, int message)
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[inst->uwbToRangeWith], 0) ; // write the frame data
+ dwt_writetxdata(psduLength, (uint8 *) &inst->msg, 0) ; // write the frame data
- uint32 response_time = portGetTickCnt() - inst->range_start;
+// 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) ;
- if(instancesendpacket(psduLength, DWT_START_TX_DELAYED, inst->delayedReplyTime))
- {
+ //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))
+ {
// initiate the re-transmission
inst->testAppState = TA_TXE_WAIT ;
//inst->nextState = TA_TXPOLL_WAIT_SEND ; //TODO should this go to TX_SELECT instead?
@@ -738,40 +1064,114 @@ int testapprun(instance_data_t *inst, int 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_TAG_FINAL -> late tx");
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();
+
}
else
{
- inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
+ 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)
inst->timeofTx = portGetTickCnt();
inst->monitor = 1;
- uint8 debug_msg[100];
- sprintf((char *)&debug_msg, "RTLS_DEMO_MSG_TAG_FINAL -> success!");
- send_txmsgtousb((char *)&debug_msg);
- usb_run();
+// uint8 debug_msg[100];
+// sprintf((char *)&debug_msg, "RTLS_DEMO_MSG_TAG_FINAL -> success!");
+// send_txmsgtousb((char *)&debug_msg);
+// usb_run();
}
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "time_now: %lu, replytime: %lu", dwt_readsystimestamphi32(), inst->delayedReplyTime);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
+ break;
+ }
+ 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_writetxfctrl(psduLength, 0, 1);
+// if(dwt_starttx(DWT_START_TX_IMMEDIATE) == 0){
+// // uint8 debug_msg[100];
+// // int n = sprintf((char *)&debug_msg, "TX_INF,%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_TXINF_WAIT_SEND ;
+// done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
+//
+// inst->timeofTx = portGetTickCnt();
+//
+ int psduLength = 0;
- break;
- }
+ inst->report_msg.seqNum = inst->frameSN++;
+
+#if (USING_64BIT_ADDR==1)
+ psduLength = RNG_REPORT_MSG_LEN + FRAME_CRTL_AND_ADDRESS_LS + FRAME_CRC;
+#else
+ psduLength = RNG_REPORT_MSG_LEN + 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...
+ 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++;
+
+ }
+ else
+ {
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "TX_RNG_REPORT");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
+ inst->previousState = TA_TXREPORT_WAIT_SEND;
+ done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
+
+ inst->timeofTx = portGetTickCnt();
+ inst->monitor = 1;
+
+ }
+
+ break;
+ }
case TA_TX_WAIT_CONF :
{
+ //TODO look at this state again. it seems to sometimes be problematic
+
// if (inst->mode == ANCHOR)
// {
// send_statetousb(inst);
@@ -786,54 +1186,74 @@ int testapprun(instance_data_t *inst, int message)
if(dw_event->type == DWT_SIG_RX_TIMEOUT) //got RX timeout - i.e. did not get the response (e.g. ACK)
{
//we need to wait for SIG_TX_DONE and then process the timeout and re-send the frame if needed
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "DWT_SIG_RX_TIMEOUT");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
inst->gotTO = 1;
}
done = INST_DONE_WAIT_FOR_NEXT_EVENT;
-
//TODO maybe move out of this if statement???
//sometimes the DW1000 tx callback (TXFRS) fails to trigger and the the SYS_STATE register
//reads IDLE for for PMSC, RX, and TX so we need another way to timeout since RX FWTO won't be triggered.
- uint32 dt = get_dt32(inst->timeofTx, portGetTickCnt());
-
- if( inst->previousState == TA_TXFINAL_WAIT_SEND ||
- inst->previousState == TA_TXPOLL_WAIT_SEND ||
- inst->previousState == TA_TXRESPONSE_WAIT_SEND)
- {
- //NOTE timeout duration found experimentally, may need to be changed if the delays in instance.h are modified
- if(dt > 20)
- {
- inst->gotTO = 1;
- }
- }
- else if(inst->previousState == TA_TXBLINK_WAIT_SEND){ //TODO put this back above???
- if(dt > BLINK_SLEEP_DELAY + 20)
- {
- inst->gotTO = 1;
- }
- }
- else if(inst->previousState == TA_TXRANGINGINIT_WAIT_SEND)
+// uint32 dt = get_dt32(inst->timeofTx, portGetTickCnt());
+
+// if( inst->previousState == TA_TXFINAL_WAIT_SEND ||
+// inst->previousState == TA_TXPOLL_WAIT_SEND ||
+// inst->previousState == TA_TXRESPONSE_WAIT_SEND ||
+// inst->previousState == TA_TXINF_WAIT_SEND)
+// {
+// //NOTE timeout duration found experimentally, may need to be changed if the delays in instance.h are modified
+// if(dt > 200)
+// {
+// inst->gotTO = 1;
+// //inst_processtxrxtimeout(inst);
+// }
+// }
+// else if(inst->previousState == TA_TXBLINK_WAIT_SEND){ //TODO put this back above???
+// if(dt > BLINK_SLEEP_DELAY + 20)
+// {
+// inst->gotTO = 1;
+// //inst_processtxrxtimeout(inst);
+//
+// }
+// }
+// else if(inst->previousState == TA_TXRANGINGINIT_WAIT_SEND)
+// {
+// if(dt > RNG_INIT_REPLY_DLY_MS + 20)
+// {
+// //inst_processtxrxtimeout(inst);
+// inst->gotTO = 1;
+// }
+// }
+//
+ if(inst->gotTO == 0)
{
- if(dt > RNG_INIT_REPLY_DLY_MS + 20)
- {
- inst->gotTO = 1;
- }
+ break;
}
-
- break;
}
done = INST_NOT_DONE_YET;
+
if (inst->gotTO) //timeout
{
- uint8 debug_msg[100];
- int n = sprintf((char *)&debug_msg, "inst->gotTO");
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint32 mask = dwt_read32bitreg(SYS_MASK_ID);
+
+
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "inst->gotTO, SYS_MASK: %lu", mask);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
// send_txmsgtousb("(2nd) got TO in TA_TX_WAIT_CONF");
- inst_processtxrxtimeout(inst);
+
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "inst_processtxrxtimeout(inst) after inst->gotTO");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+ inst_processtxrxtimeout(inst);
inst->gotTO = 0;
inst->wait4ack = 0 ; //clear this
@@ -842,34 +1262,19 @@ int testapprun(instance_data_t *inst, int message)
else if(inst->previousState == TA_TXFINAL_WAIT_SEND) //TAG operations
{
- //TODO is this in the wrong spot?
- //add cooldown node
- uint32 time_now = portGetTickCnt();
- tx_timing_node *node = malloc(sizeof(tx_timing_node));
- node->index = inst->uwbToRangeWith;
- uint32 time_remaining = 0;
- if(time_now < inst->tx_scheduler.expected_tx_end_time){
- time_remaining = inst->tx_scheduler.expected_tx_end_time - time_now;
- }
- node->duration = (inst->uwbNumActive[inst->uwbToRangeWith]-1)*(uint32)RANGE_DURATION_MS + time_remaining+20;
-
-// node->duration = (inst->uwbNumActive[inst->uwbToRangeWith]-1)*(int)RANGE_DURATION_MS + inst->tx_scheduler.time_reject_select - time_now;
- node->time = time_now;
-
- inst->tx_scheduler.add_node(&inst->tx_scheduler, node);
-
-// uint8 debug_msg[100];
-// int n = sprintf((char *)&debug_msg, "tx_scheduler add_node duration %lu", node->duration);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
+// inst->testAppState = TA_TXE_WAIT ;
+// inst->nextState = TA_TX_SELECT;
+ inst->testAppState = TA_RXE_WAIT;
- inst->testAppState = TA_TXE_WAIT ;
- inst->nextState = TA_TX_SELECT;
-// inst->uwbToRangeWith = instfindfirstactiveuwbinlist(inst, inst->uwbToRangeWith + 1);
-
break;
}
- else
+ else if(inst->previousState == TA_TXINF_WAIT_SEND)
+ {
+ inst->testAppState = inst->nextState;
+
+ break;
+ }
+ else
{
inst->txu.txTimeStamp = dw_event->timeStamp;
@@ -888,11 +1293,20 @@ int testapprun(instance_data_t *inst, int message)
tagCalculatedFinalTxTime = tagCalculatedFinalTxTime + inst->txAntennaDelay;
tagCalculatedFinalTxTime &= MASK_40BIT;
+// // Write Calculated TX time field of Final message
+// memcpy(&(inst->msg[inst->uwbToRangeWith].messageData[FTXT]), (uint8 *)&tagCalculatedFinalTxTime, 5);
+// // Write Poll TX time field of Final message
+// memcpy(&(inst->msg[inst->uwbToRangeWith].messageData[PTXT]), (uint8 *)&inst->txu.tagPollTxTime, 5);
// Write Calculated TX time field of Final message
- memcpy(&(inst->msg[inst->uwbToRangeWith].messageData[FTXT]), (uint8 *)&tagCalculatedFinalTxTime, 5);
- // Write Poll TX time field of Final message
- memcpy(&(inst->msg[inst->uwbToRangeWith].messageData[PTXT]), (uint8 *)&inst->txu.tagPollTxTime, 5);
+ memcpy(&(inst->msg.messageData[FTXT]), (uint8 *)&tagCalculatedFinalTxTime, 5);
+ // Write Poll TX time field of Final message
+ memcpy(&(inst->msg.messageData[PTXT]), (uint8 *)&inst->txu.tagPollTxTime, 5);
+
}
+// else if(inst->previousState == TA_TXRANGINGINIT_WAIT_SEND)
+// {
+// inst->mode = ANCHOR;
+// }
inst->testAppState = TA_RXE_WAIT ; // After sending, tag expects response/report, anchor waits to receive a final/new poll
@@ -911,10 +1325,10 @@ int testapprun(instance_data_t *inst, int message)
{
//turn RX on
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "instancerxon called from case TA_RXE_WAIT :");
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "instancerxon called from case TA_RXE_WAIT :");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
instancerxon(inst, 0, 0) ; // turn RX on, without delay
}
@@ -923,11 +1337,9 @@ int testapprun(instance_data_t *inst, int message)
inst->wait4ack = 0 ; //clear the flag, the next time we want to turn the RX on it might not be auto
}
- if (inst->mode != LISTENER)
- {
- //we are going to use anchor/tag timeout
- done = INST_DONE_WAIT_FOR_NEXT_EVENT; //using RX FWTO
- }
+
+ //we are going to use anchor/tag timeout
+ done = INST_DONE_WAIT_FOR_NEXT_EVENT; //using RX FWTO
inst->testAppState = TA_RX_WAIT_DATA; // let this state handle it
inst->rxCheckOnTime = portGetTickCnt() + RX_CHECK_ON_PERIOD;
@@ -945,6 +1357,7 @@ int testapprun(instance_data_t *inst, int message)
// send_statetousb(inst);
// }
+
// Wait RX data
switch (message)
{
@@ -953,8 +1366,11 @@ int testapprun(instance_data_t *inst, int message)
// send_rxmsgtousb("RX process: DWT_SIG_RX_BLINK ");
event_data_t* dw_event = instance_getevent(12); //get and clear this event
- if((inst->mode == LISTENER) || (inst->mode == ANCHOR))
- {
+// if(inst->mode == ANCHOR)
+ if(inst->mode == DISCOVERY)
+ {
+ //inst->mode = ANCHOR; //TODO instead switch to anchor after sending out the ANCH_RESP
+
inst->canPrintInfo = 1;
//if using longer reply delay time (e.g. if interworking with a PC application)
@@ -1016,25 +1432,72 @@ int testapprun(instance_data_t *inst, int message)
{
case RTLS_DEMO_MSG_RNG_INIT:
{
+ //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;
- inst->testAppState = TA_TXE_WAIT;
- inst->nextState = TA_TXPOLL_WAIT_SEND ; // send next poll
+ tdma_handler->build_new_network(tdma_handler);
+
+ //build the initial TDMA
+// tdma_handler->uwbFrameStartTimes[0] = portGetTickCnt() - tdma_handler->slotDuration;//TODO handle timer wrapping...
+// tdma_handler->lastSlotStartTime = portGetTickCnt();
+// tdma_handler->uwbListTDMAInfo[0].framelength = (uint8)MIN_FRAMELENGTH;
+//
+//
+// //todo: make this a handler function
+// //new
+// tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[0], 1);
+// tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith], 2);
+//// tdma_handler->uwblist_assign_slot(tdma_handler, 0, 2);
+// tdma_handler->assign_slot(&tdma_handler->uwbListTDMAInfo[0], 3);
+
+ //TODO build slotAssignment array
+
+// bool assigned0 = tdma_handler->slot_assigned(tdma_handler, 0);
+// bool assigned1 = tdma_handler->slot_assigned(tdma_handler, 1);
+// bool assigned2 = tdma_handler->slot_assigned(tdma_handler, 2);
+// bool assigned3 = tdma_handler->slot_assigned(tdma_handler, 3);
+//
+// uint8 debug_msg[100];
+// int 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();
+
+
+// tdma_handler->usb_dump_tdma(tdma_handler);
+
+// inst->inf_msg.messageData[FCODE] = RTLS_DEMO_MSG_INF_INIT;
+ tdma_handler->populate_inf_msg(tdma_handler, RTLS_DEMO_MSG_INF_INIT);
+
+ uint32 time_now = portGetTickCnt();
+ tdma_handler->set_discovery_mode(tdma_handler, EXIT, time_now);
+
+ inst->buildFrameTime = time_now;
+// n = sprintf((char *)&debug_msg, "BUILD FRAME,time_now: %lu, framestart: %lu, slotduration: %lu", portGetTickCnt(), tdma_handler->frameStartTime, tdma_handler->slotDuration);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+// inst->testAppState = TA_TXE_WAIT;
+ inst->testAppState = TA_TX_SELECT;
+// inst->testAppState = TA_TXINF_WAIT_SEND;
+// 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)
@@ -1061,28 +1524,183 @@ int testapprun(instance_data_t *inst, int message)
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[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)
-// inst->mode = TAG ;
+ inst->mode = TAG;
//inst->responseTimeouts = 0; //reset timeout count
inst->goToSleep = 0; //don't go to sleep - start ranging instead and then sleep after 1 range is done or poll times out
inst->instanceTimerTimeSaved = inst->instanceTimerTime = portGetTickCnt(); //set timer base
break;
} //RTLS_DEMO_MSG_RNG_INIT
- case RTLS_DEMO_MSG_TAG_POLL:
+ case RTLS_DEMO_MSG_INF_UPDATE : //fall through
+ case RTLS_DEMO_MSG_INF_SUG : //fall through
+ case RTLS_DEMO_MSG_INF_REG :
{
- if(inst->mode == LISTENER) //don't process any ranging messages when in Listener mode
- {
- //only enable receiver when not using double buffering
- inst->testAppState = TA_RXE_WAIT ; // wait for next frame
- break;
- }
+// 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);
+//// }
+//
+// //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;
+//
+// 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);
+//
+// //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 ;
+
+ break;
+ }
+ case RTLS_DEMO_MSG_INF_INIT :
+ {
+ //NOTE: discovery mode WAIT_INF_INIT checked in RX callback
+
+ //process the INF packet
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "process RTLS_DEMO_MSG_INF :");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ 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;
+
+
+ //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);
+ //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);
+//
+// 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);
+
+
+
+ inst->mode = ANCHOR;
+ //stay in RX wait for next frame...
+ inst->testAppState = TA_RXE_WAIT ; // wait for next frame
+
+ break;
+ }//RTLS_DEMO_MSG_INF
+// case RTLS_DEMO_MSG_INF_SUG :
+// {
+// //TODO: process differently depending on if we are or aren't in discovery
+//
+// //process the INF_SUG packet and return to RX
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "process RTLS_DEMO_MSG_INF_SUG :");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+//
+//// uint8 srcIndex = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
+////
+//// //synchronise the frames
+//// tdma_handler->frame_sync(tdma_handler, messageData, dw_event->rxLength, srcIndex, FS_ADOPT);
+//// //initialize collection of tdma info, clear any previously stored info
+//// tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, CLEAR_ALL_COPY);
+//// //set discovery mode to EXIT
+//// tdma_handler->set_discovery_mode(tdma_handler, EXIT, time_now); //TODO do I really need EXIT?
+//
+//
+// uint8 srcIndex = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
+//
+// //TODO implement
+//// tdma_handler->process_sug_msg(tdma_handler, messageData, &srcAddr);
+// tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, CLEAR_LISTED_COPY);
+//
+// //TODO add frame sync!
+//
+// //stay in RX wait for next frame...
+// inst->testAppState = TA_RXE_WAIT ; // wait for next frame
+//
+// break;
+// }//RTLS_DEMO_MSG_INF_SUG
+ case RTLS_DEMO_MSG_TAG_POLL:
+ {
if(dw_event->typePend == DWT_SIG_TX_PENDING)
{
inst->canPrintInfo = 0;
- inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
+ inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
inst->previousState = TA_TXRESPONSE_WAIT_SEND ;
}
else
@@ -1096,53 +1714,31 @@ int testapprun(instance_data_t *inst, int message)
case RTLS_DEMO_MSG_ANCH_RESP:
{
// send_rxmsgtousb("RX process: DWT_SIG_RX_OKAY-RTLS_DEMO_MSG_ANCH_RESP ");
- if(inst->mode == LISTENER) //don't process any ranging messages when in Listener mode
- {
- inst->testAppState = TA_RXE_WAIT ; // wait for next frame
- break;
- }
inst->anchorRespRxTime = dw_event->timeStamp ; //Response's Rx time
- uint32 dt = portGetTickCnt() - inst->timeofTx;
- char debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "time till ANCH_RESP %lu", dt);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint32 dt = portGetTickCnt() - inst->timeofTx;
+// char debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "time till ANCH_RESP %lu", dt);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
inst->testAppState = TA_TXFINAL_WAIT_SEND ; // send our response / the final
- inst->canPrintInfo = 2;
-
- inst->tof[inst->uwbToRangeWith] = 0;
- //copy previously calculated ToF
- memcpy(&inst->tof[inst->uwbToRangeWith], &messageData[TOFR], 6);
- memcpy(&inst->uwbNumActive[inst->uwbToRangeWith], &messageData[NTAG], 1);
-
- uint32 time_till = 0;
- memcpy(&time_till, &messageData[TIME_TILL], sizeof(uint32));
- if(time_till != 0){
-
- tx_timing_node *node = malloc(sizeof(tx_timing_node));
- node->index = inst->uwbToRangeWith;
- node->duration = 200; //TODO cleanup, set to blink window duration
-// node->time = portGetTickCnt() - inst->tof[inst->uwbToRangeWith] + time_till; //TODO correctly implement (convert tof timescale?)
- node->time = portGetTickCnt() + time_till;
-
- inst->tx_scheduler.add_node(&inst->tx_scheduler, node);
-
-
-
- }
-
-
- inst->newRangeUWBIndex = inst->uwbToRangeWith;
- inst->newRangeAncAddress = instance_get_uwbaddr(inst->uwbToRangeWith);
- inst->newRangeTagAddress = instance_get_addr();
+// inst->canPrintInfo = 2;
+//
+// inst->tof[inst->uwbToRangeWith] = 0;
+// //copy previously calculated ToF
+// memcpy(&inst->tof[inst->uwbToRangeWith], &messageData[TOFR], 6);
+// memcpy(&inst->uwbNumActive[inst->uwbToRangeWith], &messageData[NTAG], 1);
+//
+// inst->newRangeUWBIndex = inst->uwbToRangeWith;
+// inst->newRangeAncAddress = instance_get_uwbaddr(inst->uwbToRangeWith);
+// inst->newRangeTagAddress = instance_get_addr();
break;
- } //RTLS_DEMO_MSG_ANCH_RESP
- case RTLS_DEMO_MSG_TAG_FINAL:
+ } //RTLS_DEMO_MSG_ANCH_RESP
+ case RTLS_DEMO_MSG_TAG_FINAL :
{
// send_rxmsgtousb("RX process: DWT_SIG_RX_OKAY-RTLS_DEMO_MSG_TAG_FINAL ");
int64 Rb, Da, Ra, Db ;
@@ -1155,11 +1751,6 @@ int testapprun(instance_data_t *inst, int message)
double RbyDb = 0;
double RayDa = 0;
- if(inst->mode == LISTENER) //don't process any ranging messages when in Listener mode
- {
- inst->testAppState = TA_RXE_WAIT ; // wait for next frame
- break;
- }
// time of arrival of Final message
tagFinalRxTime = dw_event->timeStamp ; //Final's Rx time
@@ -1195,49 +1786,78 @@ int testapprun(instance_data_t *inst, int message)
// send_usbmessage(&debug_msg[0], n);
// usb_run();
- if(reportTOF(inst, inst->newRangeUWBIndex) == 0)
- {
- inst->newRange = 1;
- }
+ if(inst->tof[inst->newRangeUWBIndex] > 0) //if ToF == 0 - then no new range to report
+ {
+ if(reportTOF(inst, inst->newRangeUWBIndex)==0)
+ {
+ inst->newRange = 1;
+ inst->ranging = 1;
+ inst->canPrintInfo = 1;
+ }
+ }
+
+ inst->lastRangeTimeStamp[inst->uwbToRangeWith] = portGetTickCnt();
- //TODO update or remove?
- // inst->newRangeTagAddress = (uint16) srcAddr[0] + ((uint16) srcAddr[1] << 8);
- // inst->newRangeAncAddress = (uint16) inst->eui64[0] + ((uint16) inst->eui64[1] << 8);
inst->newRangeTagAddress = instance_get_uwbaddr(inst->uwbToRangeWith);
inst->newRangeAncAddress = instance_get_addr();
- 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();
-
- //get num active
- rx_timing_node *node = malloc(sizeof(rx_timing_node));
- node->index = inst->uwbToRangeWith;
- uint32 num_active = 0;
- for(int i = 0; i < inst->uwbListLen; i++)
+
+// char debug_msg[100];
+// n = sprintf((char*)&debug_msg[0], "TAG_FINAL %i", *node->index);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ inst->testAppState = TA_TXREPORT_WAIT_SEND;
+
+ //TODO this changes! we need to send RNG_REPORT
+// inst->testAppState = TA_RXE_WAIT ;
+// inst->previousState = TA_INIT;
+// inst->nextState = TA_INIT;
+// inst->uwbToRangeWith = 255;
+//
+// dwt_setrxaftertxdelay(0);
+// instancesetantennadelays(); //this will update the antenna delay if it has changed
+
+
+ //char debug_msg[100];
+// n = sprintf((char*)&debug_msg[0], "TAG_FINAL %d", inst->uwbToRangeWith);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+
+ break;
+ } //RTLS_DEMO_MSG_TAG_FINAL
+ case RTLS_DEMO_MSG_RNG_REPORT:
+ {
+// send_rxmsgtousb("RX process: DWT_SIG_RX_OKAY-RTLS_DEMO_MSG_RNG_REPORT ");
+
+
+ inst->tof[inst->uwbToRangeWith] = 0;
+
+ //copy previously calculated ToF
+ //TODO #define messageData offsets
+ memcpy(&inst->tof[inst->uwbToRangeWith], &messageData[1], 6);
+
+ inst->newRangeUWBIndex = inst->uwbToRangeWith;
+ inst->newRangeAncAddress = instance_get_uwbaddr(inst->uwbToRangeWith);
+ inst->newRangeTagAddress = instance_get_addr();
+
+ if(inst->tof[inst->newRangeUWBIndex] > 0) //if ToF == 0 - then no new range to report
{
- if(inst->uwbTimeout[i] == 0)
+ if(reportTOF(inst, inst->newRangeUWBIndex)==0)
{
- num_active++;
+ inst->newRange = 1;
+ inst->ranging = 1;
+ inst->canPrintInfo = 1;
}
}
- uint32 time_now = portGetTickCnt();
- uint32 time_remaining = 0;
-// if(time_now < inst->rx_scheduler.time_reject_all){
-// time_remaining = inst->rx_scheduler.time_reject_all - time_now;
-// }
- if(time_now < inst->rx_scheduler.accepted_node_end_time){
- time_remaining = inst->rx_scheduler.accepted_node_end_time - time_now;
- }
- node->duration = (int)RANGE_DURATION_MS*(num_active-1) + time_remaining;
- node->time = time_now;
- node->type = RX_REJECT;
- inst->rx_scheduler.add_node(&inst->rx_scheduler, node);
+ inst->lastRangeTimeStamp[inst->uwbToRangeWith] = portGetTickCnt();
-// char debug_msg[100];
-// n = sprintf((char*)&debug_msg[0], "TAG_FINAL %i", *node->index);
+ 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();
@@ -1250,15 +1870,8 @@ int testapprun(instance_data_t *inst, int message)
dwt_setrxaftertxdelay(0);
instancesetantennadelays(); //this will update the antenna delay if it has changed
-
- //char debug_msg[100];
-// n = sprintf((char*)&debug_msg[0], "TAG_FINAL %d", inst->uwbToRangeWith);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
-
- break;
- } //RTLS_DEMO_MSG_TAG_FINAL
+ break;
+ } //RTLS_DEMO_MSG_RNG_REPORT
default:
{
// if(inst->mode == ANCHOR)
@@ -1272,63 +1885,85 @@ int testapprun(instance_data_t *inst, int message)
}
} //end switch (fcode)
- if((inst->goToSleep == 0) && (inst->mode == LISTENER) /*|| (inst->mode == ANCHOR)*/)//update received data, and go back to receiving frames
- {
- inst->testAppState = TA_RXE_WAIT ; // wait for next frame
-
- dwt_setrxaftertxdelay(0);
- }
break ;
} //end of DWT_SIG_RX_OKAY
case DWT_SIG_RX_TIMEOUT :
{
+ if(tdma_handler->discovery_mode == WAIT_RNG_INIT || tdma_handler->discovery_mode == WAIT_INF_INIT)
+ {
+// tdma_handler->discovery_mode = WAIT_INF_REG;
+ uint32 time_now = portGetTickCnt();
+ tdma_handler->set_discovery_mode(tdma_handler, WAIT_INF_REG, time_now);
+ }
+
+
// send_txmsgtousb("RX process: DWT_SIG_RX_TIMEOUT ");
int n;
if(inst->previousState == TA_TXBLINK_WAIT_SEND)
{
- 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();
+// if(tdma_handler->discovery_mode == WAIT_RNG_INIT)
+// {
+// 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();
+ }
+ else if(inst->previousState == TA_TXRANGINGINIT_WAIT_SEND)
+ {
+// if(tdma_handler->discovery_mode == WAIT_INF_INIT)
+// {
+// tdma_handler->discovery_mode = WAIT_INF_REG;
+// }
+// tdma_handler->waitForInf = FALSE;
+ }
+ else if(inst->previousState == TA_TXINF_WAIT_SEND)
+ {
}
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 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, "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
@@ -1351,7 +1986,7 @@ int testapprun(instance_data_t *inst, int message)
}
case DWT_SIG_TX_AA_DONE: //ignore this event - just process the rx frame that was received before the ACK response
case 0:
- default :
+ default:
{
//TODO maybe also check the events to see if nothing is upcoming that will handle this for us!
//check if RX is on every so often. Turn it on if it isn't.
@@ -1367,52 +2002,63 @@ int testapprun(instance_data_t *inst, int message)
if(regval == 0){//RX IDLE
dwt_forcetrxoff();
instancerxon(inst, 0, 0);
- uint8 debug_msg[200];
- int n = sprintf((char *)&debug_msg, "RX IDLE, RESET: regval %u", regval);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint8 debug_msg[200];
+// int n = sprintf((char *)&debug_msg, "RX IDLE, RESET: regval %u", regval);
+// 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();
+// uint8 debug_msg[200];
+// int n = sprintf((char *)&debug_msg, "RX CHECKED: regval %u", regval);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
}
- if(inst->mode == ANCHOR)
- {
-// send_rxmsgtousb("RX process: default ");
-
- uint32 delta_t = portGetTickCnt() - inst->currentStateStartTime;
- if(delta_t > 20000){
+ //check if it's time to BLINK
+ if(tdma_handler->check_blink(tdma_handler) == TRUE)
+ {
+ inst->testAppState = TA_TX_SELECT;
+ }
- 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);
+// 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();
+// }
+// }
- 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;
@@ -1447,40 +2093,43 @@ int testapprun(instance_data_t *inst, int message)
// function to initialise instance structures
//
// Returns 0 on success and -1 on error
-int instance_init_s(int mode)
+int instance_init_s()
{
instance_data_t* inst = instance_get_local_structure_ptr(0);
-
- inst->mode = mode; // assume anchor,
+ inst->mode = DISCOVERY;
inst->testAppState = TA_INIT;
-// inst->blink_duration = BLINK_DURATION_MS;
-// inst->range_duration = RANGE_DURATION_MS;
+// inst->framelength = MIN_FRAMELENGTH;
+// inst->maxFramelength = MIN_FRAMELENGTH;
+// while(inst->maxFramelength < UWB_LIST_SIZE + 1)
+// {
+// inst->maxFramelength *= 2;
+// }
//
-// //TODO move and tie to other #define variables
-// float_t blink_frequency = 0.0001;
+// 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, inst->range_duration, &inst->uwbList, &inst->uwbListLen, UWB_LIST_SIZE, inst->uwbTimeout);
+// 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, inst->blink_duration, inst->range_duration, &inst->uwbList, &inst->uwbListLen, UWB_LIST_SIZE, inst->uwbTimeout);
+// inst->tx_scheduler = TXScheduler.new(BLINK_FREQUENCY, BLINK_DURATION_MS, RANGE_DURATION_MS, &inst->uwbList, &inst->uwbListLen, UWB_LIST_SIZE, inst->uwbTimeout);
// }
-
-// inst->blink_duration = BLINK_DURATION_MS;
-// inst->range_duration = RANGE_DURATION_MS;
-
- //TODO move and tie to other #define variables
- //float_t blink_frequency = 0.0001;
-
- 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
@@ -1508,6 +2157,11 @@ int instance_init_s(int mode)
return 0 ;
}
+
+
+
+
+
extern uint8 dwnsSFDlen[];
// Pre-compute frame lengths, timeouts and delays needed in ranging process.
@@ -1517,7 +2171,7 @@ 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] = {
+ 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;
@@ -1566,32 +2220,38 @@ void instance_init_timings(void)
pre_len *= 99359;
else
pre_len *= 101763;
+
+ inst->storedPreLen = pre_len; //store to be used later with inf messages and frame_sync
+
// Second step is data length for all frame types.
- for (i = 0; i < FRAME_TYPE_NB; i++)
+ for (i = 0; i < FRAME_TYPE_NB - 1; i++)//exclude INF message, since it changes size
{
- // Compute the number of symbols for the given length.
- inst->frameLengths_us[i] = data_len_bytes[i] * 8
- + CEIL_DIV(data_len_bytes[i] * 8, 330) * 48;
- // Convert from symbols to time and add PHY header length.
- if(inst->configData.dataRate == DWT_BR_110K)
- {
- inst->frameLengths_us[i] *= 820513;
- inst->frameLengths_us[i] += 17230800;
- }
- else if (inst->configData.dataRate == DWT_BR_850K)
- {
- inst->frameLengths_us[i] *= 102564;
- inst->frameLengths_us[i] += 2153900;
- }
- else
- {
- inst->frameLengths_us[i] *= 12821;
- inst->frameLengths_us[i] += 2153900;
- }
- // Last step: add preamble length and convert to microseconds.
- inst->frameLengths_us[i] += pre_len;
- inst->frameLengths_us[i] = CEIL_DIV(inst->frameLengths_us[i], 100000);
+ inst->frameLengths_us[i] = instance_getmessageduration_us(data_len_bytes[i]);
+
+// // Compute the number of symbols for the given length.
+// inst->frameLengths_us[i] = data_len_bytes[i] * 8
+// + CEIL_DIV(data_len_bytes[i] * 8, 330) * 48;
+// // Convert from symbols to time and add PHY header length.
+// if(inst->configData.dataRate == DWT_BR_110K)
+// {
+// inst->frameLengths_us[i] *= 820513;
+// inst->frameLengths_us[i] += 17230800;
+// }
+// else if (inst->configData.dataRate == DWT_BR_850K)
+// {
+// inst->frameLengths_us[i] *= 102564;
+// inst->frameLengths_us[i] += 2153900;
+// }
+// else
+// {
+// inst->frameLengths_us[i] *= 12821;
+// inst->frameLengths_us[i] += 2153900;
+// }
+// // Last step: add preamble length and convert to microseconds.
+// inst->frameLengths_us[i] += pre_len;
+// inst->frameLengths_us[i] = CEIL_DIV(inst->frameLengths_us[i], 100000);
}
+
// Final frame wait timeout time.
inst->fwtoTime_sy = US_TO_SY_INT(inst->frameLengths_us[FINAL])
+ RX_START_UP_SY + margin_sy;
@@ -1629,6 +2289,36 @@ void instance_init_timings(void)
inst->smartPowerEn = 0;
}
+uint32 instance_getmessageduration_us(int data_length_bytes)
+{
+ instance_data_t* inst = instance_get_local_structure_ptr(0);
+
+ // Compute the number of symbols for the given length.
+ uint32 framelength_us = data_length_bytes * 8
+ + CEIL_DIV(data_length_bytes * 8, 330) * 48;
+ // Convert from symbols to time and add PHY header length.
+ if(inst->configData.dataRate == DWT_BR_110K)
+ {
+ framelength_us *= 820513;
+ framelength_us += 17230800;
+ }
+ else if (inst->configData.dataRate == DWT_BR_850K)
+ {
+ framelength_us *= 102564;
+ framelength_us += 2153900;
+ }
+ else
+ {
+ framelength_us *= 12821;
+ framelength_us += 2153900;
+ }
+ // Last step: add preamble length and convert to microseconds.
+ framelength_us += inst->storedPreLen;
+ framelength_us = CEIL_DIV(framelength_us, 100000);
+
+ return framelength_us;
+}
+
uint64 instance_get_addr(void) //get own address
{
instance_data_t* inst = instance_get_local_structure_ptr(0);
@@ -1693,6 +2383,14 @@ uint32 get_dt32(uint32 t1, uint32 t2)
}
}
+//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
+uint16 address64to16(uint8 *address)
+{
+ return address[0] + (address[1] << 8);
+}
+
#endif
diff --git a/src/application/instance.h b/src/application/instance.h
index 08da86a..2d604df 100644
--- a/src/application/instance.h
+++ b/src/application/instance.h
@@ -17,394 +17,12 @@
extern "C" {
#endif
+
+#include "application_definitions.h"
#include "port.h"
#include "deca_types.h"
#include "deca_device_api.h"
-#include "tx_scheduler.h"
-#include "rx_scheduler.h"
-
-
-
-/******************************************************************************************************************
-********************* NOTES on Decaranging EVK1000 application features/options ***********************************************************
-*******************************************************************************************************************/
-#define DEEP_SLEEP (0) //To enable deep-sleep set this to 1
-//DEEP_SLEEP mode can be used, for example, by a Tag instance to put the DW1000 into low-power deep-sleep mode
-
-#define CORRECT_RANGE_BIAS (1) // Compensate for small bias due to uneven accumulator growth at close up high power
-
-
-
-/******************************************************************************************************************
-*******************************************************************************************************************
-*******************************************************************************************************************/
-
-#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_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
-
-#define SET_TXRX_DELAY (0) //when set to 1 - the DW1000 RX and TX delays are set to the TX_DELAY and RX_DELAY defines
-#define TX_ANT_DELAY 0
-#define RX_ANT_DELAY 0
-
-#define USING_64BIT_ADDR (1) //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
-#define DWT_SIG_TX_DONE 1 // Frame has been sent
-#define DWT_SIG_RX_OKAY 2 // Frame Received with Good CRC
-#define DWT_SIG_RX_ERROR 3 // Frame Received but CRC is wrong
-#define DWT_SIG_RX_TIMEOUT 4 // Timeout on receive has elapsed
-#define DWT_SIG_TX_AA_DONE 6 // ACK frame has been sent (as a result of auto-ACK)
-#define DWT_SIG_RX_BLINK 7 // Received ISO EUI 64 blink message
-#define DWT_SIG_RX_PHR_ERROR 8 // Error found in PHY Header
-#define DWT_SIG_RX_SYNCLOSS 9 // Un-recoverable error in Reed Solomon Decoder
-#define DWT_SIG_RX_SFDTIMEOUT 10 // Saw preamble but got no SFD within configured time
-#define DWT_SIG_RX_PTOTIMEOUT 11 // Got preamble detection timeout (no preamble detected)
-
-#define DWT_SIG_TX_PENDING 12 // TX is pending
-#define DWT_SIG_TX_ERROR 13 // TX failed
-#define DWT_SIG_RX_PENDING 14 // RX has been re-enabled
-#define DWT_SIG_DW_IDLE 15 // DW radio is in IDLE (no TX or RX pending)
-
-#define SIG_RX_UNKNOWN 99 // Received an unknown frame
-
-// Existing frames type in ranging process.
-enum
-{
- BLINK = 0,
- RNG_INIT,
- POLL,
- RESP,
- FINAL,
- FRAME_TYPE_NB
-};
-
-//TWO WAY RANGING function codes
-#define RTLS_DEMO_MSG_RNG_INIT (0x20) // Ranging initiation message
-#define RTLS_DEMO_MSG_TAG_POLL (0x21) // Tag poll message
-#define RTLS_DEMO_MSG_ANCH_RESP (0x10) // Anchor response to poll
-#define RTLS_DEMO_MSG_TAG_FINAL (0x29) // Tag final massage back to Anchor (0x29 because of 5 byte timestamps needed for PC app)
-
-//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 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 MAX_MAC_MSG_DATA_LEN (TAG_FINAL_MSG_LEN) //max message len of the above
-
-#define STANDARD_FRAME_SIZE 127
-
-#define ADDR_BYTE_SIZE_L (8)
-#define ADDR_BYTE_SIZE_S (2)
-
-#define FRAME_CONTROL_BYTES 2
-#define FRAME_SEQ_NUM_BYTES 1
-#define FRAME_PANID 2
-#define FRAME_CRC 2
-#define FRAME_SOURCE_ADDRESS_S (ADDR_BYTE_SIZE_S)
-#define FRAME_DEST_ADDRESS_S (ADDR_BYTE_SIZE_S)
-#define FRAME_SOURCE_ADDRESS_L (ADDR_BYTE_SIZE_L)
-#define FRAME_DEST_ADDRESS_L (ADDR_BYTE_SIZE_L)
-#define FRAME_CTRLP (FRAME_CONTROL_BYTES + FRAME_SEQ_NUM_BYTES + FRAME_PANID) //5
-#define FRAME_CRTL_AND_ADDRESS_L (FRAME_DEST_ADDRESS_L + FRAME_SOURCE_ADDRESS_L + FRAME_CTRLP) //21 bytes for 64-bit addresses)
-#define FRAME_CRTL_AND_ADDRESS_S (FRAME_DEST_ADDRESS_S + FRAME_SOURCE_ADDRESS_S + FRAME_CTRLP) //9 bytes for 16-bit addresses)
-#define FRAME_CRTL_AND_ADDRESS_LS (FRAME_DEST_ADDRESS_L + FRAME_SOURCE_ADDRESS_S + FRAME_CTRLP) //15 bytes for 1 16-bit address and 1 64-bit address)
-//#define MAX_USER_PAYLOAD_STRING_LL (STANDARD_FRAME_SIZE-FRAME_CRTL_AND_ADDRESS_L-TAG_FINAL_MSG_LEN-FRAME_CRC) //127 - 21 - 16 - 2 = 88
-//#define MAX_USER_PAYLOAD_STRING_SS (STANDARD_FRAME_SIZE-FRAME_CRTL_AND_ADDRESS_S-TAG_FINAL_MSG_LEN-FRAME_CRC) //127 - 9 - 16 - 2 = 100
-//#define MAX_USER_PAYLOAD_STRING_LS (STANDARD_FRAME_SIZE-FRAME_CRTL_AND_ADDRESS_LS-TAG_FINAL_MSG_LEN-FRAME_CRC) //127 - 15 - 16 - 2 = 94
-#define MAX_USER_PAYLOAD_STRING_LL (STANDARD_FRAME_SIZE-FRAME_CRTL_AND_ADDRESS_L-FRAME_CRC) //127 - 21 - 2 = 104
-#define MAX_USER_PAYLOAD_STRING_SS (STANDARD_FRAME_SIZE-FRAME_CRTL_AND_ADDRESS_S-FRAME_CRC) //127 - 9 - 2 = 116
-#define MAX_USER_PAYLOAD_STRING_LS (STANDARD_FRAME_SIZE-FRAME_CRTL_AND_ADDRESS_LS-FRAME_CRC) //127 - 15 - 2 = 110
-
-//NOTE: the user payload assumes that there are only 88 "free" bytes to be used for the user message (it does not scale according to the addressing modes)
-#define MAX_USER_PAYLOAD_STRING MAX_USER_PAYLOAD_STRING_LL
-
-// Total frame lengths.
-#if (USING_64BIT_ADDR == 1)
- #define RNG_INIT_FRAME_LEN_BYTES (RANGINGINIT_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC)
- #define POLL_FRAME_LEN_BYTES (TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC)
- #define RESP_FRAME_LEN_BYTES (ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC)
- #define FINAL_FRAME_LEN_BYTES (TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC)
-#else
- #define RNG_INIT_FRAME_LEN_BYTES (RANGINGINIT_MSG_LEN + FRAME_CRTL_AND_ADDRESS_LS + FRAME_CRC)
- #define POLL_FRAME_LEN_BYTES (TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC)
- #define RESP_FRAME_LEN_BYTES (ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC)
- #define FINAL_FRAME_LEN_BYTES (TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC)
-#endif
-
-#define BLINK_FRAME_CONTROL_BYTES (1)
-#define BLINK_FRAME_SEQ_NUM_BYTES (1)
-#define BLINK_FRAME_CRC (FRAME_CRC)
-#define BLINK_FRAME_SOURCE_ADDRESS (ADDR_BYTE_SIZE_L)
-#define BLINK_FRAME_CTRLP (BLINK_FRAME_CONTROL_BYTES + BLINK_FRAME_SEQ_NUM_BYTES) //2
-#define BLINK_FRAME_CRTL_AND_ADDRESS (BLINK_FRAME_SOURCE_ADDRESS + BLINK_FRAME_CTRLP) //10 bytes
-#define BLINK_FRAME_LEN_BYTES (BLINK_FRAME_CRTL_AND_ADDRESS + BLINK_FRAME_CRC)
-
-#define UWB_LIST_SIZE (25) //maximum number of UWBs to range with (valid options are 0 through 244)
-#define UWB_COMM_TIMEOUT 3000 //ms //TODO maybe make this a function of other defines?
-
-#define BLINK_SLEEP_DELAY 0 //ms //how long the tag should sleep after blinking
-#define POLL_SLEEP_DELAY 25 //ms //how long the tag should sleep after ranging
-
-
-
-#define IMMEDIATE_RESPONSE (1)
-
-// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-// NOTE: the maximum RX timeout is ~ 65ms
-// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-
-#define INST_DONE_WAIT_FOR_NEXT_EVENT 1 //this signifies that the current event has been processed and instance is ready for next one
-#define INST_DONE_WAIT_FOR_NEXT_EVENT_TO 2 //this signifies that the current event has been processed and that instance is waiting for next one with a timeout
- //which will trigger if no event coming in specified time
-#define INST_NOT_DONE_YET 0 //this signifies that the instance is still processing the current event
-
-// Function code byte offset (valid for all message types).
-#define FCODE 0 // Function code is 1st byte of messageData
-// Final message byte offsets.
-#define PTXT 1
-#define RRXT 6
-#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)
-
-// Ranging init message byte offsets. Composed of tag short address, anchor
-// response delay and tag response delay.
-#define RNG_INIT_TAG_SHORT_ADDR_LO 1
-#define RNG_INIT_TAG_SHORT_ADDR_HI 2
-#define RNG_INIT_ANC_RESP_DLY_LO 3
-#define RNG_INIT_ANC_RESP_DLY_HI 4
-#define RNG_INIT_TAG_RESP_DLY_LO 5
-#define RNG_INIT_TAG_RESP_DLY_HI 6
-
-// Response delay values coded in ranging init message.
-// This is a bitfield composed of:
-// - bits 0 to 14: value
-// - bit 15: unit
-#define RESP_DLY_VAL_SHIFT 0
-#define RESP_DLY_VAL_MASK 0x7FFF
-#define RESP_DLY_UNIT_SHIFT 15
-#define RESP_DLY_UNIT_MASK 0x8000
-
-// Response time possible units: microseconds or milliseconds.
-#define RESP_DLY_UNIT_US 0
-#define RESP_DLY_UNIT_MS 1
-
-// Response delays types present in ranging init message.
-enum
-{
- RESP_DLY_ANC = 0,
- RESP_DLY_TAG,
- RESP_DLY_NB
-};
-
-// Convert microseconds to symbols, float version.
-// param x value in microseconds
-// return value in symbols.
-#define US_TO_SY(x) ((x) / 1.0256)
-
-// Convert microseconds to symbols, integer version.
-// param x value in microseconds
-// return value in symbols.
-// /!\ Due to the the multiplication by 10000, be careful about potential
-// values and type for x to avoid overflows.
-#define US_TO_SY_INT(x) (((x) * 10000) / 10256)
-
-// Minimum delay between reception and following transmission.
-#define RX_TO_TX_TIME_US 300//150 //TODO tune again
-#define RXTOTXTIME ((int)(150.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
-// immediate response, cannot be less than 170 us when not.
-#define ANC_TURN_AROUND_TIME_US RX_TO_TX_TIME_US
-#if (IMMEDIATE_RESPONSE == 1) && (ANC_TURN_AROUND_TIME_US != RX_TO_TX_TIME_US)
- #error "When using immediate response, anchor turn-around time has to be equal to RX to TX time!"
-#endif
-// Default tag turn-around time: cannot be less than 300 us. Defined as 500 us
-// so that the tag is not transmitting more than one frame by millisecond (for
-// power management purpose).
-#define TAG_TURN_AROUND_TIME_US 2000//300 //TODO fix again!!!
-
-// "Long" response delays value. Over this limit, special processes must be
-// applied.
-#define LONG_RESP_DLY_LIMIT_US 25000
-
-// Delay between blink reception and ranging init message. This is the same for
-// all modes.
-#define RNG_INIT_REPLY_DLY_MS (20)
-
-#define MAX(a,b) \
- ({ __typeof__ (a) _a = (a); \
- __typeof__ (b) _b = (b); \
- _a > _b ? _a : _b; })
-
-
-#define BLINK_DURATION_MS RNG_INIT_REPLY_DLY_MS + 1
-#define RANGE_DURATION_MS MAX(18, POLL_SLEEP_DELAY) //to increase time between ranging, modify POLL_SLEEP_DELAY
-#define BLINK_FREQUENCY 0.0001
-
-#define RX_CHECK_ON_PERIOD 200; //TODO modify
-
-// Reception start-up time, in symbols.
-#define RX_START_UP_SY 16
-
-typedef uint64_t uint64 ;
-
-typedef int64_t int64 ;
-
-
-typedef enum instanceModes{LISTENER, TAG, ANCHOR, TAG_TDOA, NUM_MODES} INST_MODE;
-
-//Listener = in this mode, the instance only receives frames, does not respond
-//Tag = Exchanges DecaRanging messages (Poll-Response-Final) with Anchor and enabling Anchor to calculate the range between the two instances
-//Anchor = see above
-
-typedef enum inst_states
-{
- TA_INIT, //0
-
- TA_TXE_WAIT, //1
- TA_TXPOLL_WAIT_SEND, //2
- TA_TXFINAL_WAIT_SEND, //3
- TA_TXRESPONSE_WAIT_SEND, //4
- TA_TX_WAIT_CONF, //6
-
- TA_RXE_WAIT, //7
- TA_RX_WAIT_DATA, //8
-
- TA_SLEEP_DONE, //9
- TA_TXBLINK_WAIT_SEND, //10
- TA_TXRANGINGINIT_WAIT_SEND, //11
- TA_TX_SELECT //12
-} INST_STATES;
-
-
-// This file defines data and functions for access to Parameters in the Device
-//message structure for Poll, Response and Final message
-
-typedef struct
-{
- uint8 frameCtrl[2]; // frame control bytes 00-01
- uint8 seqNum; // sequence_number 02
- uint8 panID[2]; // PAN ID 03-04
- uint8 destAddr[ADDR_BYTE_SIZE_L]; // 05-12 using 64 bit addresses
- uint8 sourceAddr[ADDR_BYTE_SIZE_L]; // 13-20 using 64 bit addresses
- uint8 messageData[MAX_USER_PAYLOAD_STRING_LL] ; // 21-124 (application data and any user payload)
- uint8 fcs[2] ; // 125-126 we allow space for the CRC as it is logically part of the message. However ScenSor TX calculates and adds these bytes.
-} srd_msg_dlsl ;
-
-typedef struct
-{
- uint8 frameCtrl[2]; // frame control bytes 00-01
- uint8 seqNum; // sequence_number 02
- uint8 panID[2]; // PAN ID 03-04
- uint8 destAddr[ADDR_BYTE_SIZE_S]; // 05-06
- uint8 sourceAddr[ADDR_BYTE_SIZE_S]; // 07-08
- uint8 messageData[MAX_USER_PAYLOAD_STRING_SS] ; // 09-124 (application data and any user payload)
- uint8 fcs[2] ; // 125-126 we allow space for the CRC as it is logically part of the message. However ScenSor TX calculates and adds these bytes.
-} srd_msg_dsss ;
-
-typedef struct
-{
- uint8 frameCtrl[2]; // frame control bytes 00-01
- uint8 seqNum; // sequence_number 02
- uint8 panID[2]; // PAN ID 03-04
- uint8 destAddr[ADDR_BYTE_SIZE_L]; // 05-12 using 64 bit addresses
- uint8 sourceAddr[ADDR_BYTE_SIZE_S]; // 13-14
- uint8 messageData[MAX_USER_PAYLOAD_STRING_LS] ; // 15-124 (application data and any user payload)
- uint8 fcs[2] ; // 125-126 we allow space for the CRC as it is logically part of the message. However ScenSor TX calculates and adds these bytes.
-} srd_msg_dlss ;
-
-typedef struct
-{
- uint8 frameCtrl[2]; // frame control bytes 00-01
- uint8 seqNum; // sequence_number 02
- uint8 panID[2]; // PAN ID 03-04
- uint8 destAddr[ADDR_BYTE_SIZE_S]; // 05-06
- uint8 sourceAddr[ADDR_BYTE_SIZE_L]; // 07-14 using 64 bit addresses
- uint8 messageData[MAX_USER_PAYLOAD_STRING_LS] ; // 15-124 (application data and any user payload)
- uint8 fcs[2] ; // 125-126 we allow space for the CRC as it is logically part of the message. However ScenSor TX calculates and adds these bytes.
-} srd_msg_dssl ;
-
-//12 octets for Minimum IEEE ID blink
-typedef struct
-{
- uint8 frameCtrl; // frame control bytes 00
- uint8 seqNum; // sequence_number 01
- uint8 tagID[BLINK_FRAME_SOURCE_ADDRESS]; // 02-09 64 bit address
- uint8 fcs[2] ; // 10-11 we allow space for the CRC as it is logically part of the message. However ScenSor TX calculates and adds these bytes.
-} iso_IEEE_EUI64_blink_msg ;
-
-typedef struct
-{
- uint8 channelNumber ; // valid range is 1 to 11
- uint8 preambleCode ; // 00 = use NS code, 1 to 24 selects code
- uint8 pulseRepFreq ; // NOMINAL_4M, NOMINAL_16M, or NOMINAL_64M
- uint8 dataRate ; // DATA_RATE_1 (110K), DATA_RATE_2 (850K), DATA_RATE_3 (6M81)
- uint8 preambleLen ; // values expected are 64, (128), (256), (512), 1024, (2048), and 4096
- uint8 pacSize ;
- uint8 nsSFD ;
- uint16 sfdTO; //!< SFD timeout value (in symbols) e.g. preamble length (128) + SFD(8) - PAC + some margin ~ 135us... DWT_SFDTOC_DEF; //default value
-} instanceConfig_t ;
-
-
-/******************************************************************************************************************
-*******************************************************************************************************************
-*******************************************************************************************************************/
-
-#define MAX_EVENT_NUMBER (8)
-//NOTE: Accumulators don't need to be stored as part of the event structure as when reading them only one RX event can happen...
-//the receiver is singly buffered and will stop after a frame is received
-
-typedef struct
-{
- uint8 type; // event type
- uint8 typeSave; // holds the event type - does not clear (used to show what event has been processed)
- uint8 typePend; // set if there is a pending event (i.e. DW is not in IDLE (TX/RX pending)
- uint16 rxLength ;
-
- uint64 timeStamp ; // last timestamp (Tx or Rx)
-
- uint32 timeStamp32l ; // last tx/rx timestamp - low 32 bits
- uint32 timeStamp32h ; // last tx/rx timestamp - high 32 bits
-
- union {
- //holds received frame (after a good RX frame event)
- uint8 frame[STANDARD_FRAME_SIZE];
- srd_msg_dlsl rxmsg_ll ; //64 bit addresses
- srd_msg_dssl rxmsg_sl ;
- srd_msg_dlss rxmsg_ls ;
- srd_msg_dsss rxmsg_ss ; //16 bit addresses
- iso_IEEE_EUI64_blink_msg rxblinkmsg;
- }msgu;
-
-}event_data_t ;
-
-#define RTD_MED_SZ 8 // buffer size for mean of 8
-
-typedef struct {
- uint8 PGdelay;
-
- //TX POWER
- //31:24 BOOST_0.125ms_PWR
- //23:16 BOOST_0.25ms_PWR-TX_SHR_PWR
- //15:8 BOOST_0.5ms_PWR-TX_PHR_PWR
- //7:0 DEFAULT_PWR-TX_DATA_PWR
- uint32 txPwr[2]; //
-}tx_struct;
+#include "tdma_handler.h"
typedef struct
@@ -449,14 +67,26 @@ typedef struct
// Pre-computed frame lengths for frames involved in the ranging process,
// in microseconds.
uint32 frameLengths_us[FRAME_TYPE_NB];
+ uint32 storedPreLen; //precomputed conversion of preamble and sfd
//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_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_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)
@@ -467,9 +97,6 @@ typedef struct
uint16 panID ; // panid used in the frames
uint8 addrByteSize; // The bytelength used for addresses.
- // uint8 relpyAddress[8] ; // address of the anchor the tag is ranging with
-
-
//64 bit timestamps
//union of TX timestamps
@@ -524,15 +151,17 @@ typedef struct
uint8 uwbToRangeWith; //it is the index of the uwbList array which contains the address of the UWB we are ranging with
uint8 uwbListLen ;
- uint8 uwbList[UWB_LIST_SIZE][8];
- uint8 uwbNumActive[UWB_LIST_SIZE]; //number of TAGs each tracked ANCHOR is actively ranging with.
+ uint8 uwbList[UWB_LIST_SIZE][8]; //index 0 reserved for self, rest for other tracked uwbs
+ uint8 uwbListType[UWB_LIST_SIZE]; //UWB_LIST_SELF, UWB_LIST_NEIGHBOR, UWB_LIST_HIDDEN, UWB_LIST_INACTIVE
+
+ uint8 uwbNumActive[UWB_LIST_SIZE]; //number of TAGs each tracked ANCHOR is actively ranging with. //TODO remove?
- // keep track of when final messages so we can drop uwbs that we havent communicated with in a while
- uint32 lastCommTimeStamp[UWB_LIST_SIZE] ;
+ // keep track of when final messages so we can drop uwbs that we haven't communicated with in a while
+ uint32 lastCommTimeStamp[UWB_LIST_SIZE];
uint8 uwbTimeout[UWB_LIST_SIZE] ;
- struct TXScheduler tx_scheduler; //scheduler used by TAG to decide when to blink and when to range with an ANCHOR (and which ANCHOR)
- struct RXScheduler rx_scheduler; //scheduler used by ANCHOR to decide which RX messages to accept and respond to
+ uint32 lastRangeTimeStamp[UWB_LIST_SIZE];
+
uint8 time_till_next_reported[UWB_LIST_SIZE]; //used to keep track of whether we reported the RX_ACCEPT node. 0 if no, 1 if yes.
uint32 blink_start;
@@ -548,7 +177,9 @@ typedef struct
uint8 dweventIdxIn;
uint8 dweventPeek;
uint8 monitor;
- uint32 timeofTx ;
+ uint32 timeofTx;
+ uint32 timeofRxCallback;
+ uint64 timeofRxCallback_dwtime;
uint8 smartPowerEn;
uint32 currentStateStartTime;
@@ -556,6 +187,9 @@ typedef struct
uint32 rxCheckOnTime;
+ uint32 buildFrameTime;
+
+ uint8 ranging;
} instance_data_t ;
@@ -575,6 +209,9 @@ int instaddactivateuwbinlist(instance_data_t *inst, uint8 *uwbAddr);
int instcheckactiveuwbinlist(instance_data_t *inst, uint8 *uwbAddr);
int instfindfirstactiveuwbinlist(instance_data_t *inst, uint8 startindex);
int instfindnumactiveuwbinlist(instance_data_t *inst);
+int instfindnumactiveneighbors(instance_data_t *inst);
+int instfindnumactivehidden(instance_data_t *inst);
+int instgetuwblistindex(instance_data_t *inst, uint8 *uwbAddr, uint8 addrByteSize);
void instance_readaccumulatordata(void);
@@ -588,7 +225,8 @@ void setupmacframedata(instance_data_t *inst, int fcode);
// Call init, then call config, then call run. call close when finished
// initialise the instance (application) structures and DW1000 device
int instance_init(void);
-int instance_init_s(int mode);
+int instance_init_s();
+int tdma_init_s();
// configure the instance and DW1000 device
void instance_config(instanceConfig_t *config) ;
@@ -600,7 +238,7 @@ int instancesendpacket(uint16 length, uint8 txmode, uint32 dtime);
// called (periodically or from and interrupt) to process any outstanding TX/RX events and to drive the ranging application
int instance_run(void) ; // returns indication of status report change
-int testapprun(instance_data_t *inst, int message);
+int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int message);
// calls the DW1000 interrupt handler
#define instance_process_irq(x) dwt_isr() //call device interrupt handler
@@ -618,6 +256,8 @@ void instancesetreplydelay(int datalength);
// /!\ This function assumes that there is no user payload in the frame.
void instance_init_timings(void);
+uint32 instance_getmessageduration_us(int data_length_bytes);
+
// set/get the instance roles e.g. Tag/Anchor/Listener
int instancegetrole(void) ;
// get the DW1000 device ID (e.g. 0xDECA0130 for MP)
@@ -635,6 +275,7 @@ uint64 instance_get_uwbaddr(uint8 uwb_index); //get uwb address (8 bytes)
uint64 instancenewrangeancadd(void);
uint64 instancenewrangetagadd(void);
+int instanceisranging(void);
int instancenewrange(void);
int instancesleeping(void);
int instanceanchorwaiting(void);
@@ -669,14 +310,16 @@ uint16 instancerxantdly(void);
int instance_starttxtest(int framePeriod);
-// coid instance_
-
+const char* get_instanceModes_string(enum instanceModes mode);
+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);
+uint16 address64to16(uint8 *address);
-void send_statetousb(instance_data_t *inst);
+//void send_statetousb(instance_data_t *inst);
+void send_statetousb(instance_data_t *inst, struct TDMAHandler *tdma_handle);
void send_rxmsgtousb(char *data);
void send_txmsgtousb(char *data);
char* get_msg_fcode_string(int fcode);
diff --git a/src/application/instance_common.c b/src/application/instance_common.c
index f12e46a..f3747df 100644
--- a/src/application/instance_common.c
+++ b/src/application/instance_common.c
@@ -18,7 +18,7 @@
#include "deca_spi.h"
#include "instance.h"
-#include "llist.h"
+//#include "llist.h"
//#include "uwb_select.h"
#include <inttypes.h>
@@ -36,6 +36,7 @@ extern void send_usbmessage(uint8*, int);
// -------------------------------------------------------------------------------------------------------------------
static instance_data_t instance_data[NUM_INST] ;
+static struct TDMAHandler tdma_handler;
extern const uint16 rfDelays[2];
extern const tx_struct txSpectrumConfig[8];
@@ -58,6 +59,32 @@ instance_data_t* instance_get_local_structure_ptr(unsigned int x)
return &instance_data[x];
}
+// -------------------------------------------------------------------------------------------------------------------
+// Functions
+// -------------------------------------------------------------------------------------------------------------------
+/* @fn instance_get_local_structure_ptr
+ * @brief function to return the pointer to local instance data structure
+ * */
+struct TDMAHandler* tdma_get_local_structure_ptr()
+{
+
+ return &tdma_handler;
+}
+
+// -------------------------------------------------------------------------------------------------------------------
+// function to initialise tdma structures
+//
+// Returns 0 on success and -1 on error
+int tdma_init_s()
+{
+
+ tdma_handler = TDMAHandler.new();
+
+ return 0 ;
+}
+
+
+
// -------------------------------------------------------------------------------------------------------------------
// convert microseconds to device time
uint64 convertmicrosectodevicetimeu (double microsecu)
@@ -187,18 +214,19 @@ int reportTOF(instance_data_t *inst, uint8 uwb_index)
//
// -------------------------------------------------------------------------------------------------------------------
// return index of UWB
-int instgetuwblistindex(instance_data_t *inst, uint8 *uwbAddr)
+int instgetuwblistindex(instance_data_t *inst, uint8 *uwbAddr, uint8 addrByteSize)
{
uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+
//add the new UWB to the list, if not already there and there is space
for(uint8 i=0; i<UWB_LIST_SIZE; i++)
{
- if(memcmp(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) != 0)
+ if(memcmp(&inst->uwbList[i][0], &uwbAddr[0], addrByteSize) != 0)
{
- if(memcmp(&inst->uwbList[i][0], &blank[0], inst->addrByteSize) == 0) //blank entry
+ if(memcmp(&inst->uwbList[i][0], &blank[0], addrByteSize) == 0) //blank entry
{
- memcpy(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) ;
+ memcpy(&inst->uwbList[i][0], &uwbAddr[0], addrByteSize) ;
inst->uwbListLen = i + 1 ;
inst->uwbTimeout[i] = 1;
return i;
@@ -227,7 +255,7 @@ int instaddactivateuwbinlist(instance_data_t *inst, uint8 *uwbAddr)
uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
//add the new UWB to the list, if not already there and there is space
- for(i=0; i<UWB_LIST_SIZE; i++)
+ for(i=1; i<UWB_LIST_SIZE; i++)//0 reserved for self. timeout status not used for self
{
if(memcmp(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) != 0)
{
@@ -273,7 +301,7 @@ int instcheckactiveuwbinlist(instance_data_t *inst, uint8 *uwbAddr)
uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
//add the new UWB to the list, if not already there and there is space
- for(i=0; i<UWB_LIST_SIZE; i++)
+ for(i=1; i<UWB_LIST_SIZE; i++)//0 reserved for self, timeout not used for self
{
if(memcmp(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) == 0)
{
@@ -332,7 +360,7 @@ int instfindnumactiveuwbinlist(instance_data_t *inst)
{
uint8 num = 0;
- for(int i=0; i<inst->uwbListLen; i++)
+ for(int i=1; i<inst->uwbListLen; i++) //0 reserved for self, timeout status not applicable
{
if(!inst->uwbTimeout[i])
{
@@ -343,6 +371,47 @@ int instfindnumactiveuwbinlist(instance_data_t *inst)
return num;
}
+// -------------------------------------------------------------------------------------------------------------------
+//
+// function to find the number of neighbor UWBs in our list that are not in a timeout status
+//
+// -------------------------------------------------------------------------------------------------------------------
+int instfindnumactiveneighbors(instance_data_t *inst)
+{
+ uint8 num = 0;
+
+ for(int i=1; i<inst->uwbListLen; i++)// 0 reserved for self, cant be neighbor
+ {
+ if(inst->uwbListType[i] == UWB_LIST_NEIGHBOR)
+ {
+ num++;
+ }
+ }
+
+ return num;
+}
+
+// -------------------------------------------------------------------------------------------------------------------
+//
+// function to find the number of hidden neighbor UWBs in our list that are not in a timeout status
+//
+// -------------------------------------------------------------------------------------------------------------------
+int instfindnumactivehidden(instance_data_t *inst)
+{
+ uint8 num = 0;
+
+ for(int i=1; i<inst->uwbListLen; i++)//0 reserved for self, cannot be hidden
+ {
+ if(inst->uwbListType[i] == UWB_LIST_HIDDEN)
+ {
+ num++;
+ }
+ }
+
+ return num;
+}
+
+
// -------------------------------------------------------------------------------------------------------------------
#if (NUM_INST != 1)
@@ -360,10 +429,12 @@ void instclearuwbList(void)
for(int i=0; i<UWB_LIST_SIZE; i++)
{
instance_data[instance].lastCommTimeStamp[i] = 0;
+ instance_data[instance].lastRangeTimeStamp[i] = 0;
instance_data[instance].uwbTimeout[i] = 0;
instance_data[instance].time_till_next_reported[i] = 0;
memcpy(&instance_data[instance].uwbList[i][0], &blank[0], 8);
+ instance_data[instance].uwbListType[i] = UWB_LIST_INACTIVE;
}
}
@@ -375,6 +446,16 @@ int instancegetrole(void)
return instance_data[0].mode;
}
+int instanceisranging(void)
+{
+ if(instance_data[0].ranging == 1)
+ {
+ return 1;
+ }
+
+ return 0;
+}
+
int instancenewrange(void)
{
if(instance_data[0].newRange)
@@ -449,10 +530,11 @@ void instanceclearcounts(void)
int instance_init(void)
{
int instance = 0 ;
+
int result;
- instance_data[instance].mode = ANCHOR; // assume listener,
-
+ instance_data[instance].mode = DISCOVERY;
+ instance_data[instance].ranging = 0;
instance_data[instance].goToSleep = 0;
instance_data[instance].tofIndex = 0;
@@ -500,7 +582,7 @@ int instance_init(void)
instance_data[instance].clockOffset = 0;
instance_data[instance].monitor = 0;
- dwt_setdblrxbuffmode(1);
+// dwt_setdblrxbuffmode(1);
return 0 ;
}
@@ -647,8 +729,9 @@ void instance_config(instanceConfig_t *config)
instance_data[instance].configData.txPreambLength = config->preambleLen ;
instance_data[instance].configData.rxPAC = config->pacSize ;
instance_data[instance].configData.nsSFD = config->nsSFD ;
- instance_data[instance].configData.phrMode = DWT_PHRMODE_STD ;
- instance_data[instance].configData.sfdTO = config->sfdTO;
+// instance_data[instance].configData.phrMode = DWT_PHRMODE_STD ;
+ instance_data[instance].configData.phrMode = DWT_PHRMODE_EXT ;
+ instance_data[instance].configData.sfdTO = config->sfdTO;
//configure the channel parameters
dwt_configure(&instance_data[instance].configData) ;
@@ -665,15 +748,10 @@ void instance_config(instanceConfig_t *config)
//MUST change the SPI to < 3MHz as the dwt_otpread will change to XTAL clock
port_set_dw1000_slowrate(); //reduce SPI to < 3MHz
-//#if (SET_TXRX_DELAY == 0)
dwt_otpread(ANTDLY_ADDRESS, &antennaDelay, 1);
instance_data[instance].txAntennaDelay = ((antennaDelay >> (16*(config->pulseRepFreq - DWT_PRF_16M))) & 0xFFFF) >> 1;
instance_data[instance].rxAntennaDelay = instance_data[instance].txAntennaDelay ;
-//#else
-// instance_data[instance].txAntennaDelay = (uint16)TX_ANT_DELAY;
-// instance_data[instance].rxAntennaDelay = (uint16)RX_ANT_DELAY;
-//#endif
//read any data from the OTP for the TX power
dwt_otpread(TXCFG_ADDRESS, otpPower, 12);
@@ -694,8 +772,6 @@ void instance_config(instanceConfig_t *config)
instance_data[instance].rxAntennaDelay = (uint16)RX_ANT_DELAY;
#endif
-
-
// -------------------------------------------------------------------------------------------------------------------
// set the antenna delay, we assume that the RX is the same as TX.
dwt_setrxantennadelay(instance_data[instance].txAntennaDelay);
@@ -794,16 +870,64 @@ int instance_get_rxl(void) //get number of late Tx frames
void inst_processtxrxtimeout(instance_data_t *inst)
{
+
+
// send_statetousb(inst);
- // uint8 debug_msg[100];
- // int n = sprintf((char*)&debug_msg[0], "inst_processrxtimeout(), uwbToRangeWith: %i, uwbListlen: %i", inst->uwbToRangeWith, inst->uwbListLen);
- // send_usbmessage(&debug_msg[0], n);
- // usb_run();
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "inst_processrxtimeout(), uwbToRangeWith: %i, uwbListlen: %i", inst->uwbToRangeWith, inst->uwbListLen);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
- if(inst->mode == ANCHOR) //we did not receive the final/ACK - wait for next poll
+// tdma_handler.waitForInf = FALSE;
+// tdma_handler.waitForRngInit = FALSE;
+
+
+ if(inst->mode == DISCOVERY)
+ {
+ inst->wait4ack = 0;
+ inst->uwbToRangeWith = 255;
+// uint32 maxFrameDuration = tdma_handler.maxFramelength*tdma_handler.slotDuration;
+
+ if(tdma_handler.discovery_mode == WAIT_RNG_INIT || tdma_handler.discovery_mode == WAIT_INF_INIT)
+ {
+// tdma_handler.discovery_mode = WAIT_INF_REG;
+ uint32 time_now = portGetTickCnt();
+ tdma_handler.set_discovery_mode(&tdma_handler, WAIT_INF_REG, time_now);
+ }
+
+// if(inst->previousState == TA_TXBLINK_WAIT_SEND)
+// {
+// tdma_handler.waitForRngInit = FALSE;
+// }
+// else if(inst->previousState == TA_TXRANGINGINIT_WAIT_SEND)
+// {
+// tdma_handler.waitForInf = FALSE;
+// }
+
+ //TODO should I keep/remove this...?
+ if(tdma_handler.check_blink(&tdma_handler) == TRUE)
+ {
+ inst->testAppState = TA_TX_SELECT;
+ }
+ else
+ {
+ inst->testAppState = TA_RXE_WAIT;
+ }
+// uint32 maxFrameDuration = tdma_handler.maxFramelength*tdma_handler.slotDuration;
+// if(portGetTickCnt() - tdma_handler.discoveryStartTime > maxFrameDuration) //TODO handle number wrapping!
+// {
+// inst->testAppState = TA_TX_SELECT ;
+// }
+// else
+// {
+// inst->testAppState = TA_RXE_WAIT;
+// }
+
+ dwt_setrxtimeout(0);
+ }
+ else if(inst->mode == ANCHOR) //we did not receive the final/ACK - wait for next poll
{
- //TODO should I set uwbToRangeWith == 255???
inst->wait4ack = 0;
inst->uwbToRangeWith = 255;
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
@@ -828,6 +952,7 @@ void inst_processtxrxtimeout(instance_data_t *inst)
// }
}
+ inst->previousState = TA_INIT;
//timeout - disable the radio (if using SW timeout the rx will not be off)
dwt_forcetrxoff() ;
}
@@ -849,11 +974,11 @@ void instance_txcallback(const dwt_cb_data_t *txd)
dw_event.rxLength = 0;
dw_event.typeSave = dw_event.type = DWT_SIG_TX_DONE ;
- // uint32 dt = portGetTickCnt() - instance_data[instance].timeofTx;
- // char debug_msg[100];
- // int n = sprintf((char*)&debug_msg[0], "TX CALLBACK: DWT_SIG_TX_DONE. dt: %lu ", dt);
- // send_usbmessage((uint8*)&debug_msg[0], n);
- // usb_run();
+// uint32 dt = portGetTickCnt() - instance_data[instance].timeofTx;
+// char debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "TX CALLBACK: DWT_SIG_TX_DONE. dt: %lu ", dt);
+// send_usbmessage((uint8*)&debug_msg[0], n);
+// usb_run();
instance_putevent(dw_event);
@@ -869,14 +994,16 @@ void instance_rxtimeoutcallback(const dwt_cb_data_t *rxd)
{
event_data_t dw_event;
- int instance = 0;
- uint32 response_time = portGetTickCnt() - instance_data[instance].range_start;
+// int instance = 0;
+// uint32 response_time = portGetTickCnt() - instance_data[instance].range_start;
+
+// uint8 debug_msg[200];
+// int n = 0;
+// n = sprintf((char*)&debug_msg[0], "RX TIMEOUT CALLBACK");// duration: %lu", response_time);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
- uint8 debug_msg[200];
- int n = 0;
- n = sprintf((char*)&debug_msg[0], "RX TIMEOUT CALLBACK duration: %lu", response_time);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
dw_event.typeSave = dw_event.type = DWT_SIG_RX_TIMEOUT;
dw_event.rxLength = 0;
@@ -889,11 +1016,11 @@ void instance_rxtimeoutcallback(const dwt_cb_data_t *rxd)
void instance_rxerrorcallback(const dwt_cb_data_t *rxd)
{
- uint8 debug_msg[200];
- int n = 0;
- n = sprintf((char*)&debug_msg[0], "RX ERROR CALLBACK");
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint8 debug_msg[200];
+// int n = 0;
+// n = sprintf((char*)&debug_msg[0], "RX ERROR CALLBACK");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
int instance = 0;
@@ -905,10 +1032,20 @@ void instance_rxerrorcallback(const dwt_cb_data_t *rxd)
}
+// tdma_handler.waitForInf = FALSE;
+// tdma_handler.waitForRngInit = FALSE;
+ if(tdma_handler.discovery_mode == WAIT_RNG_INIT || tdma_handler.discovery_mode == WAIT_INF_INIT)
+ {
+// tdma_handler.discovery_mode = WAIT_INF_REG;
+ uint32 time_now = portGetTickCnt();
+ tdma_handler.set_discovery_mode(&tdma_handler, WAIT_INF_REG, time_now);
+ }
+
+
event_data_t dw_event;
//re-enable the receiver
//for ranging application rx error frame is same as TO - as we are not going to get the expected frame
- if(instance_data[instance].mode == TAG) //TODO I think frame filtering error events are causing this!!! maybe just turn rx back on like the tag!
+ if(instance_data[instance].mode == TAG || instance_data[instance].mode == DISCOVERY)
{
dw_event.type = DWT_SIG_RX_TIMEOUT;
dw_event.typeSave = 0x40 | DWT_SIG_RX_TIMEOUT;
@@ -918,10 +1055,10 @@ void instance_rxerrorcallback(const dwt_cb_data_t *rxd)
}
else
{
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "instancerxon called from case instance_rxerrorcallback :");
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "instancerxon called from case instance_rxerrorcallback :");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
instance_data[instance].uwbToRangeWith = 255;
@@ -930,335 +1067,9 @@ void instance_rxerrorcallback(const dwt_cb_data_t *rxd)
}
-//void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
-//{
-// uint8 debug_msg[200];
-// int n = 0;
-//
-// int instance = 0;
-// uint8 rxTimeStamp[5] = {0, 0, 0, 0, 0};
-// uint8 srcAddr_index = 0;
-// uint8 rxd_event = 0;
-// uint8 fcode_index = 0;
-// event_data_t dw_event;
-//
-// //if we got a frame with a good CRC - RX OK
-// rxd_event = DWT_SIG_RX_OKAY;
-//
-// dw_event.rxLength = rxd->datalength;
-//
-// //need to process the frame control bytes to figure out what type of frame we have received
-// switch(rxd->fctrl[0])
-// {
-// //blink type frame
-// case 0xC5:
-// if(rxd->datalength == 12)
-// {
-// rxd_event = DWT_SIG_RX_BLINK;
-// 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;
-//
-// break;
-//
-// //ACK type frame - not supported in this SW - set as unknown (re-enable RX)
-// case 0x02:
-// rxd_event = SIG_RX_UNKNOWN;
-// break;
-//
-// //data type frames (with/without ACK request) - assume PIDC is on.
-// case 0x41:
-// case 0x61:
-// //read the frame
-// if(rxd->datalength > STANDARD_FRAME_SIZE)
-// rxd_event = SIG_RX_UNKNOWN;
-//
-//
-// //need to check the destination/source address mode
-// if((rxd->fctrl[1] & 0xCC) == 0x88) //dest & src short (16 bits)
-// {
-// fcode_index = FRAME_CRTL_AND_ADDRESS_S; //function code is in first byte after source address
-// srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_S;
-// }
-// else if((rxd->fctrl[1] & 0xCC) == 0xCC) //dest & src long (64 bits)
-// {
-// fcode_index = FRAME_CRTL_AND_ADDRESS_L; //function code is in first byte after source address
-// srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_L;
-// }
-// else //using one short/one long
-// {
-// fcode_index = FRAME_CRTL_AND_ADDRESS_LS; //function code is in first byte after source address
-//
-// if(((rxd->fctrl[1] & 0xCC) == 0x8C)) //source short
-// {
-// srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_L;
-// }
-// else
-// {
-// srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_S;
-// }
-//
-// }
-//
-// break;
-//
-// //any other frame types are not supported by this application
-// default:
-// rxd_event = SIG_RX_UNKNOWN;
-// break;
-// }
-//
-//
-// //read rx timestamp
-// if((rxd_event == DWT_SIG_RX_BLINK) || (rxd_event == DWT_SIG_RX_OKAY))
-// {
-// dwt_readrxtimestamp(rxTimeStamp) ;
-// dw_event.timeStamp32l = (uint32)rxTimeStamp[0] + ((uint32)rxTimeStamp[1] << 8) + ((uint32)rxTimeStamp[2] << 16) + ((uint32)rxTimeStamp[3] << 24);
-// dw_event.timeStamp = rxTimeStamp[4];
-// dw_event.timeStamp <<= 32;
-// dw_event.timeStamp += dw_event.timeStamp32l;
-// dw_event.timeStamp32h = ((uint32)rxTimeStamp[4] << 24) + (dw_event.timeStamp32l >> 8);
-//
-// dwt_readrxdata((uint8 *)&dw_event.msgu.frame[0], rxd->datalength, 0); // Read Data Frame
-// }
-//
-// dw_event.typeSave = dw_event.type = rxd_event;
-//
-// //----------------------------------------------------------------------------------------------
-// //TWR - here we check if we need to respond to a TWR Poll or Response Messages
-// //----------------------------------------------------------------------------------------------
-//
-// //dont process unkown signals or non-blinks that aren't addressed to this UWB
-// if(rxd_event == DWT_SIG_RX_OKAY)
-// {
-// if((dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_INIT ) ||
-// (dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_POLL ) ||
-// (dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_ANCH_RESP) ||
-// (dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_FINAL))
-// {
-// uint8 destAddr_index = FRAME_CTRLP;
-//
-// if(memcmp(&instance_data[instance].eui64[0], &dw_event.msgu.frame[destAddr_index], instance_data[instance].addrByteSize) != 0)
-// {
-// // n = sprintf((char*)&debug_msg[0], "RX Message not addressed to me");
-// // send_usbmessage(&debug_msg[0], n);
-// // usb_run();
-// rxd_event = SIG_RX_UNKNOWN;
-// }
-// }
-// else
-// {
-// rxd_event = SIG_RX_UNKNOWN;
-// }
-// }
-//
-//
-// //ANCHOR RX
-// //first check if the address is already tracked.
-// //if not, add it.
-// //then get the index for that address
-//
-// //next check if the ANCHOR is busy with a blink or ranging exchange.
-// //if BLINK message and busy, add an RX_ACCEPT node to the rx_scheduler
-// //if not busy with a blink/range exchange, check if we can accept the BLINK or POLL
-// //if we can accept, set to corresponding index, set to 255
-//
-//
-// int uwb_index = 255;
-// if(rxd_event == DWT_SIG_RX_BLINK)
-// {
-// uwb_index = instcheckactiveuwbinlist(&instance_data[instance], &dw_event.msgu.rxblinkmsg.tagID[0]);
-//
-// // n = sprintf((char*)&debug_msg[0], "RX CALLBACK RECEIVED: BLINK <- uwb %d", uwb_index);
-// // send_usbmessage(&debug_msg[0], n);
-// // usb_run();
-// }
-// else if(rxd_event == DWT_SIG_RX_OKAY)
-// {
-//
-// uwb_index = instcheckactiveuwbinlist(&instance_data[instance], &dw_event.msgu.frame[srcAddr_index]);
-//
-//
-// n = sprintf((char*)&debug_msg[0], "RX CB RX: DWT_SIG_RX_OKAY-%s <- uwb %d ", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]), uwb_index);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-//
-// }
-//
-// // select a uwb to range with if certain messages are received when not currently ranging with another uwb
-// if (instance_data[instance].uwbToRangeWith == 255 && rxd_event != SIG_RX_UNKNOWN)
-// {
-// if(uwb_index == 255) //uwb not yet in list, or timed out
-// {
-// if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_INIT && instance_data[instance].mode == TAG)
-// {
-// //only process range init from anchors not already in our list, or those that have timed out
-// if(instaddactivateuwbinlist(&instance_data[instance], &dw_event.msgu.frame[srcAddr_index]))
-// {
-// uwb_index = instance_data[instance].uwbToRangeWith;
-// }
-// else
-// {
-// uwb_index = 255;
-// instance_data[instance].uwbToRangeWith = 255;
-// }
-// }
-// else if(rxd_event == DWT_SIG_RX_BLINK && instance_data[instance].mode == ANCHOR)
-// {
-// //TODO consider adding to anchor list once a RTLS_DEMO_MSG_TAG_POLL has been received...
-// //only process blinks from tags not already in our list, or those that have timed out
-// if(instaddactivateuwbinlist(&instance_data[instance], &dw_event.msgu.rxblinkmsg.tagID[0]))
-// {
-// uwb_index = instance_data[instance].uwbToRangeWith;
-// }
-// else
-// {
-// uwb_index = 255;
-// instance_data[instance].uwbToRangeWith = 255;
-// }
-// }
-// }
-// else //uwb_index != 255, uwb already in list and not timed out
-// {
-// if (dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_POLL && instance_data[instance].mode == ANCHOR)
-// {
-// instance_data[instance].uwbToRangeWith = uwb_index;
-// }
-// }
-// }
-//
-// int event_placed = 0;
-// if(uwb_index != 255 && instance_data[instance].uwbToRangeWith == uwb_index)
-// {
-// //update the timestamp for the most recent communication
-// instance_data[instance].lastCommTimeStamp[uwb_index] = portGetTickCnt();
-//
-// if(rxd_event == DWT_SIG_RX_OKAY)
-// {
-// //process RTLS_DEMO_MSG_TAG_POLL immediately.
-// if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_POLL)
-// {
-// uint16 frameLength = 0;
-//
-// instance_data[instance].tagPollRxTime = dw_event.timeStamp ; //Poll's Rx time
-//
-//#if (IMMEDIATE_RESPONSE == 0)
-// instance_data[instance].delayedReplyTime = (instance_data[instance].tagPollRxTime + instance_data[instance].responseReplyDelay) >> 8 ; // time we should send the response
-//#else
-// instance_data[instance].delayedReplyTime = 0;
-//#endif
-//
-//#if (USING_64BIT_ADDR == 1)
-// frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
-//#else
-// frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
-//#endif
-//
-// memcpy(&instance_data[instance].msg[uwb_index].destAddr[0], &dw_event.msgu.frame[srcAddr_index], instance_data[instance].addrByteSize); //remember who to send the reply to (set destination address)
-//
-// // Write calculated TOF into response message
-// memcpy(&(instance_data[instance].msg[uwb_index].messageData[TOFR]), &instance_data[instance].tof[uwb_index], 5);
-//
-// // TODO include the number of active TAGS that the ANCHOR is ranging with
-// uint8 num_active = 0;
-// for(int i = 0; i < instance_data[instance].uwbListLen; i++){
-// if(!instance_data[instance].uwbTimeout[i]){
-// num_active++;
-// }
-// }
-// memcpy(&(instance_data[instance].msg[uwb_index].messageData[NTAG]), &num_active, 1);
-//
-//
-// instance_data[instance].tof[uwb_index] = 0; //clear ToF ..
-//
-// instance_data[instance].msg[uwb_index].seqNum = instance_data[instance].frameSN++;
-//
-// //set the delayed rx on time (the final message will be sent after this delay)
-//// dwt_setrxaftertxdelay((uint32)instance_data[instance].txToRxDelayAnc_sy); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
-// dwt_setrxaftertxdelay(0); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
-//
-// //response is expected
-// instance_data[instance].wait4ack = DWT_RESPONSE_EXPECTED;
-//
-// dwt_writetxfctrl(frameLength, 0, 1);
-// dwt_writetxdata(frameLength, (uint8 *) &instance_data[instance].msg[uwb_index], 0) ; // write the frame data
-//
-// //report out which message is being sent!
-// send_txmsgtousb(get_msg_fcode_string((int)instance_data[instance].msg[uwb_index].messageData[FCODE]));
-//
-//
-//#if (IMMEDIATE_RESPONSE == 1)
-// dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);
-//#else
-// if(instancesendpacket(frameLength, DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED, instance_data[instance].delayedReplyTime))
-// {
-// dw_event.typePend = DWT_SIG_TX_ERROR ;
-// dwt_setrxaftertxdelay(0);
-// instance_data[instance].wait4ack = 0; //clear the flag as the TX has failed the TRX is off
-// instance_data[instance].lateTX++;
-// }
-// else
-//#endif
-// {
-// dw_event.typePend = DWT_SIG_TX_PENDING ; // exit this interrupt and notify the application/instance that TX is in progress.
-// instance_data[instance].timeofTx = portGetTickCnt();
-// }
-// }
-//
-// instance_putevent(dw_event);
-// event_placed = 1;
-//
-//#if (DEEP_SLEEP == 1)
-// if (instance_data[instance].sleepingEabled)
-// {
-// instance_data[instance].rxmsgcount++;
-// }
-//#endif
-// }
-// else if (rxd_event == DWT_SIG_RX_BLINK)
-// {
-// // n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: BLINK");
-// // send_usbmessage(&debug_msg[0], n);
-// // usb_run();
-//
-// instance_putevent(dw_event);
-// event_placed = 1;
-//
-//#if (DEEP_SLEEP == 1)
-// if (instance_data[instance].sleepingEabled)
-// {
-// instance_data[instance].rxmsgcount++;
-// }
-//#endif
-// }
-// }
-//
-// // TODO figure out a better way to do this, I'd like to keep it where it was if possible
-// // Toggle the Host side Receive Buffer Pointer
-// dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_HRBT_OFFSET, 1);
-//
-//
-// if(!event_placed)
-// {
-// // instance_rxerrorcallback(rxd);
-//
-// instancerxon(&instance_data[instance], 0, 0); //immediately reenable RX
-// }
-//}
void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
{
-// uint8 debug_msg[150];
-// int n = 0;
-
-
- uint8 debug_msg[150];
- int n = sprintf((char*)&debug_msg[0], "RX CALLBACK");
- send_usbmessage(&debug_msg[0], n);
- usb_run();
int instance = 0;
uint8 rxTimeStamp[5] = {0, 0, 0, 0, 0};
@@ -1267,6 +1078,21 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
uint8 fcode_index = 0;
event_data_t dw_event;
+
+// int num_neighbors = instfindnumactiveneighbors(&instance_data[instance]);
+// uint8 debug_msg[150];
+// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK, mode: %s, num_neighbors %d, discovery_mode %s", get_instanceModes_string(instance_data[instance].mode), num_neighbors, get_discovery_modes_string(tdma_handler.discovery_mode));
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+// uint8 debug_msg[150];
+// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK, %llX, xxx", instance_get_addr());
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+
+ uint32 time_now = portGetTickCnt();
+
//if we got a frame with a good CRC - RX OK
rxd_event = DWT_SIG_RX_OKAY;
@@ -1281,10 +1107,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;
@@ -1300,9 +1126,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
case 0x41:
case 0x61:
//read the frame
- if(rxd->datalength > STANDARD_FRAME_SIZE)
+ if(rxd->datalength > EXTENDED_FRAME_SIZE)
rxd_event = SIG_RX_UNKNOWN;
-
//need to check the destination/source address mode
if((rxd->fctrl[1] & 0xCC) == 0x88) //dest & src short (16 bits)
@@ -1357,11 +1182,10 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
//TWR - here we check if we need to respond to a TWR Poll or Response Messages
//----------------------------------------------------------------------------------------------
- //don't process unknown signals or non-blinks that aren't addressed to this UWB
+ //don't process unknown signals or non-broadcast messages that aren't addressed to this UWB
if(rxd_event == DWT_SIG_RX_OKAY)
{
-
if((dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_INIT ) ||
(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_POLL ) ||
(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_ANCH_RESP) ||
@@ -1369,24 +1193,41 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
{
uint8 destAddr_index = FRAME_CTRLP;
- //TODO make work with short address
- if(memcmp(&instance_data[instance].eui64[0], &dw_event.msgu.frame[destAddr_index], instance_data[instance].addrByteSize) != 0)
- {
- n = sprintf((char*)&debug_msg[0], "RX Message not addressed to me");
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+//#if (USING_64BIT_ADDR==0)
+// if(memcmp(&instance_data[instance].uwbShortAdd, &dw_event.msgu.frame[destAddr_index], instance_data[instance].addrByteSize) != 0)
+//#else
+// if(memcmp(&instance_data[instance].eui64[0], &dw_event.msgu.frame[destAddr_index], instance_data[instance].addrByteSize) != 0)
+//#endif
+ if(memcmp(&instance_data[instance].uwbList[0][0], &dw_event.msgu.frame[destAddr_index], instance_data[instance].addrByteSize) != 0)
+ {
+// uint8 debug_msg[150];
+// int n = sprintf((char*)&debug_msg[0], "RX Message not addressed to me");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
rxd_event = SIG_RX_UNKNOWN;
}
}
- else
+
+
+ else if(dw_event.msgu.frame[fcode_index] != RTLS_DEMO_MSG_INF_REG &&
+ dw_event.msgu.frame[fcode_index] != RTLS_DEMO_MSG_INF_INIT &&
+ dw_event.msgu.frame[fcode_index] != RTLS_DEMO_MSG_INF_SUG &&
+ dw_event.msgu.frame[fcode_index] != RTLS_DEMO_MSG_INF_UPDATE &&
+ dw_event.msgu.frame[fcode_index] != RTLS_DEMO_MSG_RNG_REPORT)
{
rxd_event = SIG_RX_UNKNOWN;
+// uint8 debug_msg[150];
+// int n = sprintf((char*)&debug_msg[0], "not INIT/POLL/RESP/FINAL");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
}
}
-// bool debug = false;
-// if(rxd_event == DWT_SIG_RX_BLINK){
-// debug = true;
+// if(rxd_event == SIG_RX_UNKNOWN){
+// uint8 debug_msg[150];
+// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK: SIG_RX_UNKNOWN");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
// }
//ANCHOR RX
@@ -1396,121 +1237,164 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
uint8 uwb_index = 255;
if(rxd_event == DWT_SIG_RX_BLINK)
{
- uwb_index = instgetuwblistindex(&instance_data[instance], &dw_event.msgu.rxblinkmsg.tagID[0]);
+ uint8 blink_address[8] = {0,0,0,0,0,0,0,0};
+#if (USING_64BIT_ADDR==0)
+ memcpy(&blink_address, &dw_event.msgu.rxblinkmsg.tagID[0], instance_data[instance].addrByteSize);
+#else
+ uint16 blink_address_short = address64to16(&dw_event.msgu.rxblinkmsg.tagID[0]);
+ memcpy(&blink_address, &blink_address_short, instance_data[instance].addrByteSize);
+#endif
+ //must be a neighbor
+// uwb_index = instgetuwblistindex(&instance_data[instance], &dw_event.msgu.rxblinkmsg.tagID[0], instance_data[instance].addrByteSize);
+ uwb_index = instgetuwblistindex(&instance_data[instance], &blink_address[0], instance_data[instance].addrByteSize);
+// instance_data[instance].uwbListType[uwb_index] = UWB_LIST_NEIGHBOR;
- 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);
- 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());
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
}
else if(rxd_event == DWT_SIG_RX_OKAY)
{
- uwb_index = instgetuwblistindex(&instance_data[instance], &dw_event.msgu.frame[srcAddr_index]);
-
- 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);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
- }
+ //must be a neighbor
+ uwb_index = instgetuwblistindex(&instance_data[instance], &dw_event.msgu.frame[srcAddr_index], instance_data[instance].addrByteSize);
+ //TODO maybe do this somewhere else...
+ instance_data[instance].uwbListType[uwb_index] = UWB_LIST_NEIGHBOR;
+ instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
+ instance_data[instance].uwbTimeout[uwb_index] = 0;
- if(instance_data[instance].mode == ANCHOR)
- {
- //TODO this was the missing ingredient, figure out the best way to implement
- if(instance_data[instance].rx_scheduler.past_accepted_end_Time(&instance_data[instance].rx_scheduler)){
- instance_data[instance].uwbToRangeWith = 255;
- }
+// 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();
}
- //next check if the ANCHOR is busy with a blink or ranging exchange.
- //if BLINK message and busy, add an RX_ACCEPT node to the rx_scheduler
- //if not busy with a blink/range exchange, check if we can accept the BLINK or POLL
- //if we can accept, set to corresponding index
+ bool accept_inf = FALSE;
+ //check if the incoming message indicates that we should range with the source UWB or just accept and process the message
if(rxd_event != SIG_RX_UNKNOWN)
{
-
- if(instance_data[instance].uwbToRangeWith == 255) //not busy
+ if(instance_data[instance].uwbToRangeWith == 255)
{
- if(instance_data[instance].mode == TAG)
+ if(rxd_event == DWT_SIG_RX_OKAY)
{
- if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_INIT)
+ if(instance_data[instance].mode == ANCHOR)
{
- instance_data[instance].uwbTimeout[uwb_index] = 0;
- instance_data[instance].lastCommTimeStamp[uwb_index] = portGetTickCnt();
- instance_data[instance].uwbToRangeWith = uwb_index;
- instance_data[instance].tx_scheduler.last_select_index = uwb_index;
+ if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_POLL)
+ {
+ //TODO would be good to check if the UWB sending this message was supposed to be ranging in this timeslot
+ instance_data[instance].uwbToRangeWith = uwb_index;
+ }
+ else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_INF_REG || dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_INF_SUG || dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_INF_UPDATE)//only anchor if already received INF_INIT or collected regular INF messages and joined with SUG message
+ {
+ accept_inf = TRUE;
+ }
}
- }
- else if(instance_data[instance].mode == ANCHOR)
- {
- if((rxd_event == DWT_SIG_RX_BLINK && instance_data[instance].uwbTimeout[uwb_index] == 1) || (dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_POLL))
+ else if (instance_data[instance].mode == DISCOVERY)
{
- RX_MESSAGE_TYPE message_type = RX_POLL;
- if(rxd_event == DWT_SIG_RX_BLINK)
+ //TODO use switch case?
+ if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_INIT)
{
- message_type = RX_BLINK;
+ if(tdma_handler.discovery_mode == WAIT_RNG_INIT)
+ {
+ //only accept RNG_INIT if no other active neighbors exist (and we are waiting for RNG_INIT)
+ int num_active = instfindnumactiveneighbors(&instance_data[instance]);
+ if(num_active <= 1)
+ {
+ instance_data[instance].uwbToRangeWith = uwb_index;
+ }
+ }
}
-
- if(instance_data[instance].rx_scheduler.rx_accept(&instance_data[instance].rx_scheduler, uwb_index, message_type))
+ else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_INF_INIT)
{
- instance_data[instance].uwbToRangeWith = uwb_index;
+ //only accept if we are waiting for INF_INIT
+ if(tdma_handler.discovery_mode == WAIT_INF_INIT)
+ {
+ instance_data[instance].uwbToRangeWith = uwb_index;
+ accept_inf = TRUE;
+ }
}
- else if(rxd_event == DWT_SIG_RX_BLINK)
+ else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_INF_REG ||
+ dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_INF_UPDATE ||
+ dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_INF_SUG)
{
- //add an RX_ACCEPT node to the rx_scheduler
- rx_timing_node *node = malloc(sizeof(rx_timing_node));
- node->index = uwb_index;
- uint32 half_blink_accept_window = 100; //TODO cleanup (should be a function of the max range...)
- node->duration = half_blink_accept_window*2;
- node->time = portGetTickCnt() + instance_data[instance].rx_scheduler.blink_period - half_blink_accept_window;
- node->type = RX_ACCEPT;
-
- instance_data[instance].rx_scheduler.add_node(&instance_data[instance].rx_scheduler, node);
-
- uint8 debug_msg[100];
- int n = sprintf((char *)&debug_msg, "RX_ACCEPT-add_node (uwbToRangeWith == 255), time_now %lu, index %u", portGetTickCnt(), node->index);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+ if(tdma_handler.discovery_mode == WAIT_INF_REG || tdma_handler.discovery_mode == COLLECT_INF_REG)
+ {
+ accept_inf = TRUE;
+ }
}
}
}
- }
- else //busy
- {
- //note: blinks are auto filtered by DW1000 for TAGs
- //only process blinks for inactive UWBs
- if(rxd_event == DWT_SIG_RX_BLINK && instance_data[instance].uwbTimeout[uwb_index] == 1)
+ else if(rxd_event == DWT_SIG_RX_BLINK)
{
- //add an RX_ACCEPT node to the rx_scheduler
- rx_timing_node *node = malloc(sizeof(rx_timing_node));
- node->index = uwb_index;
- uint32 half_blink_accept_window = 100; //TODO cleanup (should be a function of the max range)
- node->duration = half_blink_accept_window*2;
- node->time = portGetTickCnt() + instance_data[instance].rx_scheduler.blink_period - half_blink_accept_window;
- node->type = RX_ACCEPT;
-
- instance_data[instance].rx_scheduler.add_node(&instance_data[instance].rx_scheduler, node);
-
- uint8 debug_msg[100];
- int n = sprintf((char *)&debug_msg, "RX_ACCEPT-add_node, time_now %lu, index %u", portGetTickCnt(), node->index);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+ //only accept BLINK if in DISCOVERY mode and no other active neighbors exist
+ //and not waiting for inf or rng_init
+ if(instance_data[instance].mode == DISCOVERY)
+ {
+ if(tdma_handler.discovery_mode == WAIT_INF_REG)
+ {
+ int num_neighbors = instfindnumactiveneighbors(&instance_data[instance]);
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "num_neighbors %d", num_neighbors);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ if(num_neighbors <= 1)
+ {
+ instance_data[instance].uwbToRangeWith = uwb_index;
+ }
+ }
+ }
}
}
}
-// else{
-// n = sprintf((char*)&debug_msg[0], "SIG_RX_UNKNOWN");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// }
- n = sprintf((char*)&debug_msg[0], "uwb_index %d, uwbToRangeWith %d", uwb_index, instance_data[instance].uwbToRangeWith);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
- int event_placed = 0;
- if(uwb_index != 255 && instance_data[instance].uwbToRangeWith == uwb_index)
+// n = sprintf((char*)&debug_msg[0], "uwb_index %d, uwbToRangeWith %d", uwb_index, instance_data[instance].uwbToRangeWith);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+
+ int place_event = 0;
+
+
+ if(rxd_event == DWT_SIG_RX_OKAY && uwb_index != 255)
+ {
+ //always accept.
+ if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_REPORT)
+ {
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: RNG_REPORT <- uwb_index %d", uwb_index);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+ instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
+ instance_data[instance].uwbTimeout[uwb_index] = 0;
+ place_event = 1;
+ }
+ }
+
+ if(accept_inf == TRUE)
+ {
+ //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;
+
+ //TODO only do this when we need to (we are accepting and INF message)
+ uint8 sys_time_arr[5] = {0, 0, 0, 0, 0};
+ dwt_readsystime(sys_time_arr);
+ instance_data[instance].timeofRxCallback_dwtime = (uint64)sys_time_arr[0] + ((uint64)sys_time_arr[1] << 8) + ((uint64)sys_time_arr[2] << 16) + ((uint64)sys_time_arr[3] << 24) + ((uint64)sys_time_arr[4] << 32);
+
+
+ instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
+ instance_data[instance].uwbTimeout[uwb_index] = 0;
+ place_event = 1;
+ }
+ else if(uwb_index != 255 && instance_data[instance].uwbToRangeWith == uwb_index)
{
if(rxd_event == DWT_SIG_RX_OKAY)
@@ -1518,7 +1402,12 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
//process RTLS_DEMO_MSG_TAG_POLL immediately.
if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_POLL)
{
- instance_data[instance].lastCommTimeStamp[uwb_index] = portGetTickCnt();
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "RX_POLL");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
instance_data[instance].uwbTimeout[uwb_index] = 0;
uint16 frameLength = 0;
@@ -1537,40 +1426,54 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
#endif
- memcpy(&instance_data[instance].msg[uwb_index].destAddr[0], &dw_event.msgu.frame[srcAddr_index], instance_data[instance].addrByteSize); //remember who to send the reply to (set destination address)
-
- //if tof not zero, set tof, else memset 0
- //also include next TX_ACCEPT node time (offset from now)
- // Write calculated TOF into response message
- memcpy(&instance_data[instance].msg[uwb_index].messageData[TOFR], &instance_data[instance].tof[uwb_index], 6); //TODO fix number of bytes...
-
- uint8 num_active = 0;
- for(int i = 0; i < instance_data[instance].uwbListLen; i++){
- if(!instance_data[instance].uwbTimeout[i]){
- num_active++;
- }
- }
- memcpy(&instance_data[instance].msg[uwb_index].messageData[NTAG], &num_active, 1);
-
- //get time till the next RX_ACCEPT
- uint32 time_till = 0;
- if(instance_data[instance].time_till_next_reported[uwb_index] == 0){
- time_till = instance_data[instance].rx_scheduler.get_time_till_next_rx_accept(&instance_data[instance].rx_scheduler);
- instance_data[instance].time_till_next_reported[uwb_index] = 1;
- }
- memcpy(&instance_data[instance].msg[uwb_index].messageData[TIME_TILL], &time_till, 4);
-
+// instance_data[instance].msg[uwb_index].messageData[FCODE] = RTLS_DEMO_MSG_ANCH_RESP; //message function code (specifies if message is a poll, response or other...)
+// //program option octet and parameters (not used currently)
+// instance_data[instance].msg[uwb_index].messageData[RES_R1] = 0x2; // "activity"
+// instance_data[instance].msg[uwb_index].messageData[RES_R2] = 0x0; //
+// instance_data[instance].msg[uwb_index].messageData[RES_R3] = 0x0;
+//
+//
+// memcpy(&instance_data[instance].msg[uwb_index].destAddr[0], &dw_event.msgu.frame[srcAddr_index], instance_data[instance].addrByteSize); //remember who to send the reply to (set destination address)
+//
+// //if tof not zero, set tof, else memset 0
+// //also include next TX_ACCEPT node time (offset from now)
+// // Write calculated TOF into response message
+// memcpy(&instance_data[instance].msg[uwb_index].messageData[TOFR], &instance_data[instance].tof[uwb_index], 6); //TODO fix number of bytes...
+//
+// uint8 num_active = 0;
+// for(int i = 0; i < instance_data[instance].uwbListLen; i++){
+// if(!instance_data[instance].uwbTimeout[i]){
+// num_active++;
+// }
+// }
+// memcpy(&instance_data[instance].msg[uwb_index].messageData[NTAG], &num_active, 1);
+//
+// //get time till the next RX_ACCEPT
+//// uint32 time_till = 0;
+//// if(instance_data[instance].time_till_next_reported[uwb_index] == 0){
+//// time_till = instance_data[instance].rx_scheduler.get_time_till_next_rx_accept(&instance_data[instance].rx_scheduler);
+//// instance_data[instance].time_till_next_reported[uwb_index] = 1;
+//// }
+//// memcpy(&instance_data[instance].msg[uwb_index].messageData[TIME_TILL], &time_till, 4);
+//
+//
+//// uint8 debug_msg[100];
+//// uint64 mtof = instance_data[instance].msg[uwb_index].messageData[TOFR];
+//// int n = sprintf((char *)&debug_msg, "TOF in resp %llu", mtof);
+//// send_usbmessage(&debug_msg[0], n);
+//// usb_run();
+//
+//
+//// instance_data[instance].tof[uwb_index] = 0; //clear ToF ..
+////
+// instance_data[instance].msg[uwb_index].seqNum = instance_data[instance].frameSN++;
-// uint8 debug_msg[100];
-// uint64 mtof = instance_data[instance].msg[uwb_index].messageData[TOFR];
-// int n = sprintf((char *)&debug_msg, "TOF in resp %llu", mtof);
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
+ instance_data[instance].msg.messageData[FCODE] = RTLS_DEMO_MSG_ANCH_RESP; //message function code (specifies if message is a poll, response or other...)
+ memcpy(&instance_data[instance].msg.destAddr[0], &dw_event.msgu.frame[srcAddr_index], instance_data[instance].addrByteSize); //remember who to send the reply to (set destination address)
+
+ instance_data[instance].msg.seqNum = instance_data[instance].frameSN++;
-// instance_data[instance].tof[uwb_index] = 0; //clear ToF ..
-//
- instance_data[instance].msg[uwb_index].seqNum = instance_data[instance].frameSN++;
//set the delayed rx on time (the final message will be sent after this delay)
// dwt_setrxaftertxdelay((uint32)instance_data[instance].txToRxDelayAnc_sy); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
@@ -1580,7 +1483,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
instance_data[instance].wait4ack = DWT_RESPONSE_EXPECTED;
dwt_writetxfctrl(frameLength, 0, 1);
- dwt_writetxdata(frameLength, (uint8 *) &instance_data[instance].msg[uwb_index], 0) ; // write the frame data
+// dwt_writetxdata(frameLength, (uint8 *) &instance_data[instance].msg[uwb_index], 0) ; // write the frame data
+ 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);
@@ -1596,7 +1500,7 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
#endif
{
dw_event.typePend = DWT_SIG_TX_PENDING ; // exit this interrupt and notify the application/instance that TX is in progress.
- instance_data[instance].timeofTx = portGetTickCnt();
+ instance_data[instance].timeofTx = time_now;
}
//report out which message is being sent!
// send_txmsgtousb(get_msg_fcode_string((int)instance_data[instance].msg[uwb_index].messageData[FCODE]));
@@ -1623,73 +1527,67 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
instance_data[instance].tof[uwb_index] = 0; //clear ToF ..
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "RX TAG_POLL ACCEPTED ANCH_RESP sent <- uwb %d", uwb_index);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint8 debug_msg[100];
+// n = sprintf((char*)&debug_msg[0], "RX TAG_POLL ACCEPTED ANCH_RESP sent <- uwb %d", uwb_index);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
}
else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_ANCH_RESP)
{
// uint32 response_time = portGetTickCnt() - instance_data[instance].range_start;
- instance_data[instance].lastCommTimeStamp[uwb_index] = portGetTickCnt();
+ instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
instance_data[instance].uwbTimeout[uwb_index] = 0;
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: ANCH_RESP <- uwb_index %d", uwb_index);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: ANCH_RESP <- uwb_index %d", uwb_index);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
}
else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_INIT)
{
- instance_data[instance].lastCommTimeStamp[uwb_index] = portGetTickCnt();
+ instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
instance_data[instance].uwbTimeout[uwb_index] = 0;
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: RNG_INIT <- uwb_index %d", uwb_index);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: RNG_INIT <- uwb_index %d", uwb_index);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
}
else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_FINAL)
{
- instance_data[instance].lastCommTimeStamp[uwb_index] = portGetTickCnt();
+ instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
instance_data[instance].uwbTimeout[uwb_index] = 0;
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: TAG_FINAL <- uwb_index %d", uwb_index);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: TAG_FINAL <- uwb_index %d", uwb_index);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
}
-
- instance_putevent(dw_event);
- event_placed = 1;
+// else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_REPORT)
+// {
+// instance_data[instance].lastCommTimeStamp[uwb_index] = portGetTickCnt();
+// instance_data[instance].uwbTimeout[uwb_index] = 0;
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: RNG_REPORT <- uwb_index %d", uwb_index);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+// }
-#if (DEEP_SLEEP == 1)
- if (instance_data[instance].sleepingEabled)
- {
- instance_data[instance].rxmsgcount++;
- }
-#endif
+
+ place_event = 1;
}
else if (rxd_event == DWT_SIG_RX_BLINK)
{
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: BLINK <- uwb %d", uwb_index);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
-
- instance_putevent(dw_event);
- event_placed = 1;
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: BLINK <- uwb %d", uwb_index);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
-#if (DEEP_SLEEP == 1)
- if (instance_data[instance].sleepingEabled)
- {
- instance_data[instance].rxmsgcount++;
- }
-#endif
+ place_event = 1;
}
}
// TODO figure out a better way to do this, I'd like to keep it where it was if possible
// doing it here because it needs to be toggled before toggling the rx buffer pointer
// Toggle the Host side Receive Buffer Pointer
- dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_HRBT_OFFSET, 1);
+// dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_HRBT_OFFSET, 1);
// uint8 buff = dwt_read8bitoffsetreg(SYS_STATUS_ID, 3);
// uint8 debug_msg[100];
@@ -1697,20 +1595,34 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
// send_usbmessage(&debug_msg[0], n);
// usb_run();
+ if(place_event)
+ {
+ instance_putevent(dw_event);
- if(!event_placed)
+#if (DEEP_SLEEP == 1)
+ if (instance_data[instance].sleepingEabled)
+ {
+ instance_data[instance].rxmsgcount++;
+ }
+#endif
+ }
+ else
{
// instance_rxerrorcallback(rxd);
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "instancerxon called from !event_placed :");
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "instancerxon called from !event_placed :");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
instancerxon(&instance_data[instance], 0, 0); //immediately reenable RX
}
}
+
+
+
+
int instance_peekevent(void)
{
int instance = 0;
@@ -1801,32 +1713,67 @@ int instance_run(void)
while(done == INST_NOT_DONE_YET)
{
- //int state = instance_data[instance].testAppState;
- done = testapprun(&instance_data[instance], message) ; // run the communications application
+ done = testapprun(&instance_data[instance], &tdma_handler, message) ; // run the communications application
//we've processed message
message = 0;
}
//check if lastCommTimeStamp has expired for any of the uwbs in our list
- for(int i=0; i < instance_data[instance].uwbListLen; i++)
+ for(int i=1; i < instance_data[instance].uwbListLen; i++)//0 reserved for self, timeout not applicable
{
- if(instance_data[instance].lastCommTimeStamp[i] + UWB_COMM_TIMEOUT < portGetTickCnt())
+ //TODO could also have a timer to put into DISCOVERY if we haven't spoken to anyone in a while
+
+ if(instance_data[instance].lastCommTimeStamp[i] + UWB_COMM_TIMEOUT < portGetTickCnt()) //TODO handle number wrapping
{
- if(instance_data[instance].uwbTimeout[i] == 0)
+// if(instance_data[instance].uwbTimeout[i] == 0)
+ if(instance_data[instance].uwbListType[i] == UWB_LIST_NEIGHBOR) //what about hidden?
{
+ instance_data[instance].uwbListType[i] = UWB_LIST_INACTIVE; //TODO release TDMA slots as well
instance_data[instance].uwbTimeout[i] = 1;
+
+
+ //NEW
+ //if no more active neighbors exist, transition to DISCOVERY
+ uint8 numNeighbors = instfindnumactiveneighbors(&instance_data[instance]);
+ if(numNeighbors <= 0)
+ {
+ tdma_handler.tdma_free_all_slots(&tdma_handler);
+
+
+
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "inst_processtxrxtimeout(inst) after free_all_slots");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ instance_data[instance].mode = DISCOVERY; //TODO clear the TDMA slot information!
+ tdma_handler.discoveryStartTime = portGetTickCnt();
+ tdma_handler.enter_discovery_mode(&tdma_handler);
+ inst_processtxrxtimeout(&instance_data[instance]);
+
+ instance_data[instance].canPrintInfo = 0;
+ instance_data[instance].ranging = 0;
+ }
+ else
+ {
+ //TODO check if able to reconfigure tdma assignements. shrink frame size, reassign emptied slots, etc
+ tdma_handler.uwblist_free_slots(&tdma_handler, i);
+ }
+
+
// uint8 debug_msg[100];
// int n = sprintf((char*)&debug_msg[0], "TIMEOUT: uwb %i", i);
// send_usbmessage(&debug_msg[0], n);
// usb_run();
}
+
if(instance_data[instance].uwbToRangeWith == i)
{
instance_data[instance].uwbToRangeWith = 255;
//NOTE this might need to be changed for TAG operations
- if(instance_data[instance].mode == ANCHOR)
+ if(instance_data[instance].mode == ANCHOR || instance_data[instance].mode == DISCOVERY) //TODO maybe send to TA_MODE_SELECT?
{
instance_data[instance].testAppState = TA_RXE_WAIT;
}
@@ -1843,23 +1790,21 @@ int instance_run(void)
}
-
-
-
if(done == INST_DONE_WAIT_FOR_NEXT_EVENT_TO) //we are in RX and need to timeout (Tag needs to send another poll if no Rx frame)
{
- if(instance_data[instance].mode == TAG) //Tag (is either in RX or sleeping)
- {
- if(instance_data[instance].previousState == TA_TXBLINK_WAIT_SEND)
+ if(instance_data[instance].mode == DISCOVERY)
+ {
+ if(instance_data[instance].previousState == TA_TXBLINK_WAIT_SEND)
{
instance_data[instance].instanceTimerTime += instance_data[instance].tagBlinkSleepTime_ms; //set timeout time
- instance_data[instance].instanceTimerEnabled = 1; //start timer
- }
- else
- {
- instance_data[instance].instanceTimerTime = instance_data[instance].instanceTimerTimeSaved + instance_data[instance].tagSleepTime_ms; //set timeout time
- instance_data[instance].instanceTimerEnabled = 1; //start timer
+ instance_data[instance].instanceTimerEnabled = 1; //start timer
}
+ }
+
+ if(instance_data[instance].mode == TAG) //Tag (is either in RX or sleeping)
+ {
+ instance_data[instance].instanceTimerTime = instance_data[instance].instanceTimerTimeSaved + instance_data[instance].tagSleepTime_ms; //set timeout time
+ instance_data[instance].instanceTimerEnabled = 1; //start timer
}
}
@@ -1868,11 +1813,11 @@ int instance_run(void)
{
if(instance_data[instance].instanceTimerTime < portGetTickCnt())
{
- uint8 debug_msg[200];
- int n = 0;
- n = sprintf((char*)&debug_msg[0], "instance timer expired");
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint8 debug_msg[200];
+// int n = 0;
+// n = sprintf((char*)&debug_msg[0], "instance timer expired");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
event_data_t dw_event;
instance_data[instance].instanceTimerEnabled = 0;
diff --git a/src/application/rx_scheduler.c b/src/application/rx_scheduler.c
index a997630..8c347ee 100644
--- a/src/application/rx_scheduler.c
+++ b/src/application/rx_scheduler.c
@@ -63,7 +63,7 @@ static bool rx_accept(struct RXScheduler *this, uint8 index, RX_MESSAGE_TYPE mes
uint8 debug_msg[500];
int n = 0;
int n_char = 0;
- bool debug = true;
+ bool debug = false;
if(debug){
n = sprintf((char*)&debug_msg[0], "rx_accept, time_now %lu, index %u \n", time_now, index);
n_char = n;
diff --git a/src/application/tdma_handler.c b/src/application/tdma_handler.c
new file mode 100644
index 0000000..152e47b
--- /dev/null
+++ b/src/application/tdma_handler.c
@@ -0,0 +1,2566 @@
+#include "tdma_handler.h"
+#include "port.h"
+#include "instance.h"
+
+
+extern void usb_run(void);
+extern void send_usbmessage(uint8*, int);
+
+//class methods
+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))
+ {
+ uint32 time_now = portGetTickCnt();
+
+ uint32 timeSinceSlotStart = get_dt32(this->lastSlotStartTime, time_now);
+ if(timeSinceSlotStart > this->slotDuration)
+ {
+ 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)
+ {
+ 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();
+ }
+
+ while(timeSinceFrameStart > this->slotDuration*this->uwbListTDMAInfo[0].framelength)
+ {
+ this->uwbFrameStartTimes[0] += this->slotDuration*this->uwbListTDMAInfo[0].framelength;
+ timeSinceFrameStart -= this->slotDuration*this->uwbListTDMAInfo[0].framelength;
+ }
+
+ uint8 slot = timeSinceFrameStart/this->slotDuration; //integer division rounded down
+ this->lastSlotStartTime = this->uwbFrameStartTimes[0] + this->slotDuration*(uint32)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();
+
+
+// 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)
+// {
+//
+// }
+
+// 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);
+//
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "NEW SLOT %u, ass0: %u, ass1: %u, ass2: %u, ass3: %u,", slot, assigned0, assigned1, assigned2, assigned3);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+
+// if(memcmp(&this->slotAssignments[slot], &inst->uwbShortAdd, 2) == 0)
+ if(inst->mode == DISCOVERY)
+ {
+ if(slot == 0)
+ {
+ inst->testAppState = TA_TX_SELECT;
+ }
+ }
+ else
+ {
+ if(this->slot_assigned(&this->uwbListTDMAInfo[0], slot) == TRUE) //TODO left off here, for some reason it fails when I try this...
+ {
+
+ inst->mode = TAG;
+ inst->testAppState = TA_TX_SELECT;
+ //go to TX select, select the oldest uwb, send INF, then send POLL
+ }
+ else
+ {
+ //go to RX
+ inst->mode = ANCHOR;
+ inst->testAppState = TA_RXE_WAIT;
+ }
+ }
+
+// 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);
+ }
+ }
+ else if(inst->mode == DISCOVERY)
+ {
+ this->infPollSentThisSlot = FALSE;
+ }
+
+ 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 framelength;
+ uint32 timeSinceFrameStart;
+
+ 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();
+
+
+ //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;
+
+
+ //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);
+
+
+ //time to propogate
+ //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);
+
+
+ //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;
+
+
+ if(mode == FS_ADOPT)
+ {
+ this->uwbFrameStartTimes[0] = this->uwbFrameStartTimes[srcIndex];
+ this->lastSlotStartTime = this->uwbFrameStartTimes[srcIndex] + timeSinceFrameStart; //TODO handle number wrapping...
+ }
+ else if(mode == FS_AVERAGE)
+ {
+ uint32 myFramelengthDuration = this->uwbListTDMAInfo[0].framelength*this->slotDuration;
+ uint32 myTimeSinceFrameStart = get_dt32(this->uwbFrameStartTimes[0], time_now);
+
+ if(this->uwbListTDMAInfo[0].framelength <= framelength)
+ {
+ uint32 hisTimeSinceFrameStartMod = (timeSinceFrameStart + txrx_delay)%myFramelengthDuration;
+
+ if(myTimeSinceFrameStart > hisTimeSinceFrameStartMod)
+ {
+ uint32 diff = myTimeSinceFrameStart - hisTimeSinceFrameStartMod;
+ this->uwbFrameStartTimes[0] += diff/2;
+ this->lastSlotStartTime += diff/2;
+ }
+ else
+ {
+ uint32 diff = hisTimeSinceFrameStartMod - myTimeSinceFrameStart;
+ this->uwbFrameStartTimes[0] -= diff/2;
+ this->lastSlotStartTime -= diff/2;
+ }
+ }
+ else
+ {
+ uint32 hisFramelengthDuration = framelength*this->slotDuration;
+ uint32 myTimeSinceFrameStartMod = (myTimeSinceFrameStart - txrx_delay)%hisFramelengthDuration;
+
+ if(timeSinceFrameStart > myTimeSinceFrameStartMod)
+ {
+ uint32 diff = timeSinceFrameStart - myTimeSinceFrameStartMod;
+ this->uwbFrameStartTimes[0] -= diff/2;
+ this->lastSlotStartTime -= diff/2;
+ }
+ else
+ {
+ uint32 diff = myTimeSinceFrameStartMod - timeSinceFrameStart;
+ this->uwbFrameStartTimes[0] += diff/2;
+ this->lastSlotStartTime += diff/2;
+ }
+ }
+ }
+}
+
+
+static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
+{
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "tx_select,");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ 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;
+
+ //DISCOVERY pauses for BLINK_DELAY <-added
+// if(this->waitForInf == TRUE || this->waitForRngInit == TRUE)
+// {
+// return FALSE;
+// }
+
+ int uwb_index = 255;
+
+ if(inst->mode == DISCOVERY)
+ {
+ if(this->discovery_mode == WAIT_INF_REG)
+ {
+ //start blinking if enough time has passed since entering DISCOVERY mode
+ uint32 timeSinceDiscoverStart = get_dt32(this->discoveryStartTime, time_now);
+
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "discovery wait inf reg");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ // uint8 debug_msg[100];
+ // int n = sprintf((char *)&debug_msg, "timeSinceDiscoverStart %lu, maxFrameDuration %lu ", timeSinceDiscoverStart, maxFrameDuration);
+ // send_usbmessage(&debug_msg[0], n);
+ // usb_run();
+
+ if(timeSinceDiscoverStart > this->waitInfDuration)
+ {
+ //enforce blink period
+ uint32 timeSinceLastBlink = get_dt32(this->last_blink_time, time_now);
+
+ // uint8 debug_msg[100];
+ // int n = sprintf((char *)&debug_msg, "timeSinceLastBlink %lu, BLINK_PERIOD_MS %lu ", timeSinceLastBlink, (uint32)BLINK_PERIOD_MS);
+ // send_usbmessage(&debug_msg[0], n);
+ // usb_run();
+
+ if(timeSinceLastBlink > (uint32)BLINK_PERIOD_MS + (uint32)(rand() % 100))
+ {
+ //time to blink
+ uwb_index = 255;
+ this->set_discovery_mode(this, WAIT_RNG_INIT, time_now);
+ }
+ else
+ {
+ //not time to blink yet, keep waiting for RNG_INIT
+ inst->wait4ack = 0;
+ inst->testAppState = TA_RXE_WAIT;
+ return TRUE;
+ }
+ }
+ else
+ {
+ //shouldn't be in this mode, should be listening for INF messages
+ inst->wait4ack = 0;
+ inst->testAppState = TA_RXE_WAIT;
+ return TRUE;
+ }
+ }
+// else if(this->discovery_mode == WAIT_SEND_SUG)//TODO left off here
+ else if(this->discovery_mode == SEND_SUG)//TODO left off here
+ {
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "discovery send sug");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ //get time since slot start and make sure that's greater than delay
+ uint32 timeSinceSlotStart = get_dt32(this->lastSlotStartTime, time_now);
+
+ //make sure that we are in slot 0
+
+ if(timeSinceSlotStart <= this->slotStartDelay)
+ {
+ uwb_index = -1;
+ }
+ else
+ {
+ //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;
+ }
+ }
+ else
+ {
+ return FALSE;
+ }
+ }
+ else if(inst->mode == TAG)
+ {
+
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "tag");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ //get time since slot start and make sure that's greater than delay
+ uint32 timeSinceSlotStart = get_dt32(this->lastSlotStartTime, time_now);
+
+
+ //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)
+ {
+ uwb_index = -1;
+ }
+ else
+ {
+ //check which neighbor UWB hasn't been ranged with
+ uint32 timeSinceOldestRange = 0;
+ for(int i = 1; i < inst->uwbListLen; i++)//0 reserved for self
+ {
+ if(inst->uwbListType[i] == UWB_LIST_NEIGHBOR)
+ {
+ uint32 timeSinceRange = get_dt32(inst->lastRangeTimeStamp[i], time_now);
+ if(timeSinceRange > timeSinceOldestRange)
+ {
+ timeSinceOldestRange = timeSinceRange;
+ uwb_index = i;
+ }
+ }
+ }
+
+ if(uwb_index == 255 && inst->uwbListLen > 0)
+ {
+ uwb_index = 1;
+ }
+ }
+
+ }
+ else
+ {
+ //ANCHOR shouldn't be in this mode, should be listening for INF and POLL
+ inst->testAppState = TA_RXE_WAIT;
+ return TRUE;
+ }
+
+// debug_msg[100];
+// n = sprintf((char *)&debug_msg, "selected index %i ", uwb_index);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+
+ if(uwb_index < 1 || this->infPollSentThisSlot == TRUE){
+ //do nothing
+ //TODO would be best to sleep until next possible poll and then select again...
+// done = INST_DONE_WAIT_FOR_NEXT_EVENT; //TODO make sure that this is the right thing to do.
+
+// debug_msg[100];
+// n = sprintf((char *)&debug_msg, "1");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ return FALSE;
+
+ }else if(uwb_index > 254){
+ inst->testAppState = TA_TXBLINK_WAIT_SEND;
+ inst->uwbToRangeWith = (uint8)255;
+
+// debug_msg[100];
+// n = sprintf((char *)&debug_msg, "2");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ }else{
+
+// debug_msg[100];
+// n = sprintf((char *)&debug_msg, "3");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ this->infPollSentThisSlot = TRUE;
+ inst->testAppState = TA_TXINF_WAIT_SEND;
+ inst->uwbToRangeWith = (uint8)uwb_index;
+ }
+
+ return TRUE;
+}
+
+
+static bool check_blink(struct TDMAHandler *this)
+{
+ instance_data_t *inst = instance_get_local_structure_ptr(0);
+ bool retval = FALSE;
+
+ if(inst->mode == DISCOVERY && this->discovery_mode == WAIT_INF_REG)
+ {
+ uint32 time_now = portGetTickCnt();
+ uint32 timeSinceDiscoveryStart = get_dt32(this->discoveryStartTime, time_now);
+ if(timeSinceDiscoveryStart > this->maxFramelength*this->slotDuration )
+ {
+// 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);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ uint32 timeSinceBlink = get_dt32(this->last_blink_time, time_now);
+ if(timeSinceBlink > (uint32)BLINK_PERIOD_MS + (uint32)(rand()%100)){
+// uint8 debug_msg[100];
+// int n = sprintf((char *)&debug_msg, "in RX_WAIT_DATA, siwtch to TA_TX_SELECT");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+// this->discovery_mode = WAIT_RNG_INIT;
+ retval = TRUE;
+ }
+ }
+ }
+
+ return retval;
+}
+
+static void populate_inf_msg(struct TDMAHandler *this, uint8 inf_msg_type)
+{
+ instance_data_t *inst = instance_get_local_structure_ptr(0);
+
+ int num_neighbors = instfindnumactiveneighbors(inst);
+ int num_hidden = instfindnumactivehidden(inst);
+ uint32 time_now = portGetTickCnt();
+
+ //fcode
+ int msgDataIndex = FCODE;
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &inf_msg_type, sizeof(uint8));
+
+
+ //time since frame start
+ //populated directly before being sent
+// 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));
+
+
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "TX MSG_INF timeSinceFrameStart: %lu", timeSinceFrameStart);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ //number of neighbors
+ msgDataIndex = TDMA_NUMN;
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &num_neighbors, 1);
+
+ //number of hidden neighbors
+ msgDataIndex = TDMA_NUMH;
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &num_hidden, 1);
+
+
+ //self framelength
+ msgDataIndex = TDMA_FRAMELENGTH;
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &this->uwbListTDMAInfo[0].framelength, 1);
+
+ //self number of slots
+ msgDataIndex = TDMA_NUMS;
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &this->uwbListTDMAInfo[0].slotsLength, 1);
+ 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);
+ msgDataIndex++;
+ }
+
+ //neighbor address, framelength, number of slots, and slot assignments
+ for(int i = 1; i < inst->uwbListLen; i++) //slot 0 reserved for self
+ {
+ if(inst->uwbListType[i] == UWB_LIST_NEIGHBOR)
+ {
+ struct TDMAInfo *info = &this->uwbListTDMAInfo[i];
+
+ //address
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &inst->uwbList[i][0], inst->addrByteSize);
+ msgDataIndex += inst->addrByteSize;
+
+ //framelength
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->framelength, 1);
+ msgDataIndex++;
+
+ //number of slots
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->slotsLength, 1);
+ msgDataIndex++;
+
+ //slot assignments
+ for(int s = 0; s < info->slotsLength; s++)
+ {
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->slots[s], 1);
+ msgDataIndex++;
+ }
+ }
+ }
+
+ //hidden address, framelength, number of slots, and slot assignments
+ for(int i = 1; i < inst->uwbListLen; i++) //slot 0 reserved for self
+ {
+ if(inst->uwbListType[i] == UWB_LIST_HIDDEN)
+ {
+ struct TDMAInfo *info = &this->uwbListTDMAInfo[i];
+
+ //address
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &inst->uwbList[i][0], inst->addrByteSize);
+ msgDataIndex += inst->addrByteSize;
+
+ //framelength
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->framelength, 1);
+ msgDataIndex++;
+
+ //number of slots
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->slotsLength, 1);
+ msgDataIndex++;
+
+ //slot assignments
+ for(int s = 0; s < info->slotsLength; s++)
+ {
+ memcpy(&inst->inf_msg.messageData[msgDataIndex], &info->slots[s], 1);
+ msgDataIndex++;
+ }
+ }
+ }
+
+
+#if (USING_64BIT_ADDR==1)
+ this->infMessageLength = msgDataIndex + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
+#else
+ this->infMessageLength = msgDataIndex + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
+#endif
+
+}
+
+
+static void update_inf_tsfs(struct TDMAHandler *this)
+{
+ instance_data_t *inst = instance_get_local_structure_ptr(0);
+ uint32 time_now = portGetTickCnt();
+ 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));
+}
+
+
+////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);
+// }
+// }
+//
+// }
+//
+//
+//}
+
+
+//Procedure for processing INF SUG, INF REG, and INF UPDATE
+//1. Check for differences with locally stored TDMA assignment information
+// (a) exit if none exist
+//2. Drop all slot assignments for self, neighbor, hidden, and twice hidden nodes that appear
+// in the INF message
+//3. Copy all assignments for self, neighbors, hidden, and twice hidden nodes that appear
+// in the INF message
+//4. Check for conflicts with slot assignments for nodes not contained in the INF message.
+// (a) if conflicts exist and self node is one of the conflicts, release all slot assignments
+// from self and follow the procedure for a new node (Section 1.1), skipping the
+// collect INF REG step.
+// (b) if conflicts exist and node is not one of the conflicts, deconflict according to 1.2
+//5. Send INF UPDATE message at beginning of allocated slot (handled elsewhere)
+
+//process types... 1.) clear all and copy 2.) clear mentioned, copy 3.) copy
+//returns TRUE if a change was made to the TDMA assingments, FALSE if invalid message FCODE or process mode or if no TDMA changes made
+static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8 srcIndex, INF_PROCESS_MODE mode)
+{
+ //NOTE: this function does not handle TDMA deconflict
+
+ bool tdma_modified = FALSE;
+
+ 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;
+ }
+
+ uint8 inf_msg_type;
+ memcpy(&inf_msg_type, &messageData[FCODE], 1);
+
+ if((inf_msg_type != RTLS_DEMO_MSG_INF_REG) &&
+ (inf_msg_type != RTLS_DEMO_MSG_INF_UPDATE) &&
+ (inf_msg_type != RTLS_DEMO_MSG_INF_INIT) &&
+ (inf_msg_type != RTLS_DEMO_MSG_INF_SUG))
+ {
+ //only process INF messages
+ return FALSE;
+ }
+
+ 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);
+
+ int msgDataIndex = TDMA_NUMS + 1;
+
+ //TODO have deconflict mode???
+ bool uwbListInMsg[UWB_LIST_SIZE];
+ for(int i = 0; i < inst->uwbListLen; i++)
+ {
+ uwbListInMsg[i] = FALSE;
+ }
+
+ if(mode == CLEAR_ALL_COPY)
+ {
+ //clear all TDMA assignments
+ 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;
+ //copy slot assignments for source UWB
+ info = &this->uwbListTDMAInfo[srcIndex];
+ if(framelength != info->framelength)
+ {
+ 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);
+
+ for(int s = 0; s < numSlots; s++)
+ {
+ memcpy(&slot, &messageData[msgDataIndex], 1);
+ msgDataIndex++;
+
+ if(this->assign_slot(info, slot) == TRUE)
+ {
+ tdma_modified = TRUE;
+ }
+ }
+ uwbListInMsg[srcIndex] = TRUE;
+
+ for(int i = 0; i < numNeighbors; i++)
+ {
+ memcpy(&address, &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(inst->uwbListType[uwb_index] == UWB_LIST_INACTIVE || inst->uwbListType[uwb_index] == UWB_LIST_TWICE_HIDDEN)
+ {
+ inst->uwbListType[uwb_index] = UWB_LIST_HIDDEN;
+ }
+ }
+
+ info = &this->uwbListTDMAInfo[uwb_index];
+ uwbListInMsg[uwb_index] = TRUE;
+
+
+
+
+ memcpy(&framelength, &messageData[msgDataIndex], 1);
+ if(framelength != info->framelength)
+ {
+ tdma_modified = TRUE;
+ }
+ msgDataIndex++;
+
+ if(mode == CLEAR_LISTED_COPY)
+ {
+ this->free_slots(info); //do after checking framelength because framelength reset
+ }
+ info->framelength = MAX(framelength, info->framelength);
+
+
+ memcpy(&numSlots, &messageData[msgDataIndex], 1);
+ msgDataIndex++;
+
+ for(int s = 0; s < numSlots; s++)
+ {
+ memcpy(&slot, &messageData[msgDataIndex], 1);
+ msgDataIndex++;
+
+ if(this->assign_slot(info, slot) == TRUE)
+ {
+ tdma_modified = TRUE;
+ }
+ }
+ }
+
+ for(int i = 0; i < numHidden; i++)
+ {
+ memcpy(&address, &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(inst->uwbListType[uwb_index] == UWB_LIST_INACTIVE)
+ {
+ inst->uwbListType[uwb_index] = UWB_LIST_TWICE_HIDDEN;
+ }
+ }
+
+ uwbListInMsg[uwb_index] = TRUE;
+ info = &this->uwbListTDMAInfo[uwb_index];
+
+ memcpy(&framelength, &messageData[msgDataIndex], 1);
+ if(framelength != info->framelength)
+ {
+ 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++;
+
+ for(int s = 0; s < numSlots; s++)
+ {
+ memcpy(&slot, &messageData[msgDataIndex], 1);
+ 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;
+ }
+ }
+ }
+
+
+ //handle deconflict???
+ if(mode == CLEAR_LISTED_COPY)
+ {
+ //deconflict uncopied against copied. (excluding self)
+ for(int i = 1; i < inst->uwbListLen; i++)
+ {
+ for(int j = i + 1; j < inst->uwbListLen; j++)
+ {
+ if(uwbListInMsg[i] == FALSE && uwbListInMsg[j] == TRUE)
+ {
+
+ if((inst->uwbListType[i] == UWB_LIST_NEIGHBOR && inst->uwbListType[j] == UWB_LIST_NEIGHBOR) ||
+ (inst->uwbListType[i] == UWB_LIST_NEIGHBOR && inst->uwbListType[j] == UWB_LIST_HIDDEN) ||
+ (inst->uwbListType[j] == UWB_LIST_NEIGHBOR && inst->uwbListType[i] == UWB_LIST_NEIGHBOR) ||
+ (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]);
+ }
+ }
+ }
+ }
+
+ //check if self has any conflicts
+ if(this->self_conflict(this))
+ {
+ //if so, release all assignments from self
+ this->free_slots(&this->uwbListTDMAInfo[0]);
+
+ //find self a new slot assignment
+ this->find_assign_slot(this);
+
+ tdma_modified = TRUE;
+ }
+ }
+
+
+ return tdma_modified;
+
+ //handle TDMA frame sync
+
+ //TODO keep this but turn back on, dw_event not declared...
+
+// //account for the delay between transmission and reception of the INF message
+// //TODO make this a function of the data rate as well!
+// //NOTE experimentally found this number to be +- 1 millisecond for some reason. //TODO figure out why
+// //experimentally found formula is 0.079667x + 0.85611
+// uint32 txrx_delay = (uint32)(0.079667*(float)(dw_event->rxLength) + 0.85611); //TODO make this function of message length!
+//
+//
+// tdma_handler->uwbFrameStartTimes[srcIndex] = time_now - (timeSinceFrameStart + txrx_delay);
+//
+//
+// uint32 myFramelengthDuration = tdma_handler->framelength*tdma_handler->slotDuration;
+// uint32 myTimeSinceFrameStart = get_dt32(tdma_handler->frameStartTime, time_now);
+//
+// //NEW
+// if(tdma_handler->framelength <= tdma_handler->uwbFramelengths[srcIndex])
+// {
+// uint32 timeSinceFrameStartMod = (timeSinceFrameStart + txrx_delay)%myFramelengthDuration;
+//
+// if(myTimeSinceFrameStart > timeSinceFrameStartMod)
+// {
+// uint32 diff = myTimeSinceFrameStart - timeSinceFrameStartMod;
+// tdma_handler->frameStartTime += diff/2;
+// tdma_handler->lastSlotStartTime += diff/2;
+// }
+// else
+// {
+// uint32 diff = timeSinceFrameStartMod - myTimeSinceFrameStart;
+// tdma_handler->frameStartTime -= diff/2;
+// tdma_handler->lastSlotStartTime -= diff/2;
+//
+// }
+// }
+// else
+// {
+// uint32 hisFramelengthDuration = tdma_handler->uwbFramelengths[srcIndex]*tdma_handler->slotDuration;
+// uint32 myTimeSinceFrameStartMod = (myTimeSinceFrameStart - txrx_delay)%hisFramelengthDuration;
+//
+// if(timeSinceFrameStart > myTimeSinceFrameStartMod)
+// {
+// uint32 diff = timeSinceFrameStart - myTimeSinceFrameStartMod;
+// tdma_handler->frameStartTime -= diff/2;
+// tdma_handler->lastSlotStartTime -= diff/2;
+// }
+// else
+// {
+// uint32 diff = myTimeSinceFrameStartMod - timeSinceFrameStart;
+// tdma_handler->frameStartTime += diff/2;
+// tdma_handler->lastSlotStartTime += diff/2;
+// }
+// }
+
+
+
+
+}
+
+
+static bool check_tdma_diff(struct TDMAHandler *this, uint8 *messageData, uint8 *srcIndex)
+{
+ return TRUE;
+
+// struct TDMAInfo info = &this->uwbListTDMAInfo[srcIndex];
+//
+// if(info->framelength != framelength || info->slotsLength != numSlots)
+// {
+// return TRUE;
+// }
+//
+// for(int s = 0; s < numSlots; s++)
+// {
+// memcpy(&slot, &messageData[msgDataIndex], 1);
+// msgDataIndex++;
+//
+// if(this->slot_assigned(info, slot) == FALSE)
+// {
+// return TRUE;
+// }
+// }
+//
+// for(int i = 0; i < numNeighbors; i++)
+// {
+// memcpy(&address, &messageData[msgDataIndex], inst->addrByteSize);
+// msgDataIndex += inst->addrByteSize;
+//
+// uint8 uwb_index = instgetuwblistindex(inst, &address[0], inst->addrByteSize);
+// info = &this->uwbListTDMAInfo[uwb_index];
+//
+// memcpy(&framelength, &messageData[msgDataIndex], 1);
+// msgDataIndex++;
+//
+// memcpy(&numSlots, &messageData[msgDataIndex], 1);
+// msgDataIndex++;
+//
+// if(info->framelength != framelength || info->slotsLength != numSlots)
+// {
+// return TRUE;
+// }
+//
+// for(int s = 0; s < numSlots; s++)
+// {
+// memcpy(&slot, &messageData[msgDataIndex], 1);
+// msgDataIndex++;
+//
+// if(this->slot_assigned(info, slot) == FALSE)
+// {
+// return TRUE;
+// }
+// }
+// }
+//
+// for(int i = 0; i < numHidden; i++)
+// {
+// memcpy(&address, &messageData[msgDataIndex], inst->addrByteSize);
+// msgDataIndex += inst->addrByteSize;
+//
+// uint8 uwb_index = instgetuwblistindex(inst, &address[0], inst->addrByteSize);
+//
+// info = &this->uwbListTDMAInfo[uwb_index];
+//
+// memcpy(&framelength, &messageData[msgDataIndex], 1);
+// msgDataIndex++;
+//
+// memcpy(&numSlots, &messageData[msgDataIndex], 1);
+// msgDataIndex++;
+//
+// if(info->framelength != framelength || info->slotsLength != numSlots)
+// {
+// return TRUE;
+// }
+//
+// for(int s = 0; s < numSlots; s++)
+// {
+// memcpy(&slot, &messageData[msgDataIndex], 1);
+// msgDataIndex++;
+//
+// if(this->slot_assigned(info, slot) == FALSE)
+// {
+// return TRUE;
+// }
+// }
+// }
+//
+//
+// return FALSE;
+}
+
+
+
+//TODO add conflict detection/resolution!!!
+//static void populate_sug_msg(struct TDMAHandler *this)
+//{
+// instance_data_t *inst = instance_get_local_structure_ptr(0);
+//
+// //if I'm here, i should clear my own assignment information
+// if(this->mySlots != NULL && this->mySlotsLength != 0)
+// {
+// this->mySlotsLength = 0;
+// free(this->mySlots);
+// this->mySlots = NULL;
+// }
+// uint8 unassigned = 255;
+// for(int i = 0; i < this->maxFramelength; i++)
+// {
+// memcpy(&this->slotAssignments[i], &unassigned, sizeof(uint8));
+// }
+// this->framelength = MIN_FRAMELENGTH;
+//
+//
+//
+//
+// inst->inf_msg.messageData[FCODE] = RTLS_DEMO_MSG_INF_SUG;
+// int msgDataIndex = FCODE + 1;
+//
+// int num_neighbors = instfindnumactiveneighbors(inst);
+// uint32 time_now = portGetTickCnt();
+//
+//
+// //TODO build framelength after creating slot assignments...
+//// //framelength
+//// memcpy(&inst->inf_msg.messageData[msgDataIndex], &this->framelength, 1);
+// msgDataIndex++;
+//
+// uint32 timeSinceFrameStart = get_dt32(this->frameStartTime, time_now); //TODO handle number wrapping
+// memcpy(&inst->inf_msg.messageData[msgDataIndex], &timeSinceFrameStart, sizeof(uint32));
+// msgDataIndex += sizeof(timeSinceFrameStart);
+//
+//// uint8 debug_msg[100];
+//// int n = sprintf((char*)&debug_msg[0], "TX MSG_INF timeSinceFrameStart: %lu", timeSinceFrameStart);
+//// send_usbmessage(&debug_msg[0], n);
+//// usb_run();
+//
+//
+// //number of neighbors
+// memcpy(&inst->inf_msg.messageData[msgDataIndex], &num_neighbors, 1);
+// msgDataIndex++;
+//
+// //neighbor addresses
+// for(int i = 0; i < inst->uwbListLen; i++)
+// {
+// if(inst->uwbListType[i] == UWB_LIST_NEIGHBOR)
+// {
+// memcpy(&inst->inf_msg.messageData[msgDataIndex], &inst->uwbList[i][0], inst->addrByteSize);
+// msgDataIndex += inst->addrByteSize;
+// }
+// }
+//
+// //neighbor framelength
+// for(int i = 0; i < inst->uwbListLen; i++)
+// {
+// if(inst->uwbListType[i] == UWB_LIST_NEIGHBOR)
+// {
+// memcpy(&inst->inf_msg.messageData[msgDataIndex], &this->uwbFramelengths[i], 1);
+// msgDataIndex++;
+// }
+// }
+//
+// //addresses in each TDMA slot
+//// for(int i = 0; i < this->framelength; i++)
+//// {
+//// memcpy(&inst->inf_msg.messageData[msgDataIndex], &this->slotAssignments[i], 2);
+////
+//// msgDataIndex += 2;
+//// }
+//
+// //TODO check for conflicts at some point
+//
+// //use the slotAssignements array to combine the collected information
+// //note: try to keep framelength as small as possible
+// //note: if we have to increase the framelength, start over!
+//
+//// uint8 mfl = this->framelength;
+////
+//// uint8 debug_msg[100];
+//// int n = sprintf((char*)&debug_msg[0], "this->framelength: %u", this->framelength);
+//// send_usbmessage(&debug_msg[0], n);
+//// usb_run();
+//
+// //getting stuck in here... somehow this->framelength is getting 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);
+// }
+// }
+//
+// }
+//
+// //if we are here, then we should've successfully combined the INF message information
+//
+//
+//
+// bool assigned = FALSE;
+// while(assigned == FALSE)
+// {
+// //first try GU (Get Unassigned) slot
+// for(int i = 1; i < this->framelength; i++)//start at i = 1 because 0 is a reserved slot
+// {
+//// if(memcmp(&this->slotAssignments[i], &zero, sizeof(uint8)) == 0)
+// if(memcmp(&this->slotAssignments[i], &unassigned, sizeof(uint8)) == 0)
+// {
+// //empty slot, assign
+// this->assign_slot(&this->myTDMAInfo, i);
+// assigned = TRUE;
+// }
+// }
+//
+// //no open slots for GU
+// //next try RMA (Release Multiple Assigned) slot
+// if(assigned == FALSE)
+// {
+// uint8 most_slots = 1;
+// uint8 most_slots_index = 255;
+// uint8 release_slot = 255;
+// uint8 assign_slot = 255;
+// for(int i = 0; i < inst->uwbListLen; i++)
+// {
+// //TODO do I only need to consider neighbors or also hidden?
+// if(inst->uwbListType[i] != UWB_LIST_INACTIVE)
+// {
+// uint8 uwbSlotsLen;
+// memcpy(&uwbSlotsLen, &this->uwbListSlotsLengths[i], sizeof(uint8));
+// if(uwbSlotsLen > most_slots)
+// {
+// bool slot_found = FALSE;
+// //check if the slot to be released can be assigned within our framelength
+// for(int j = 0; j < uwbSlotsLen; j++)
+// {
+// uint8 slot;
+// memcpy(&slot, &this->uwbListSlots[i][j], sizeof(uint8));
+// uint8 mod_slot = slot%this->framelength;
+//
+// if(mod_slot != 0 && memcmp(&this->slotAssignments[mod_slot], &i, sizeof(uint8)) == 0)
+// {
+// release_slot = slot;
+// assign_slot = mod_slot;
+// slot_found = TRUE;
+// break;
+// }
+// }
+//
+// if(slot_found == TRUE)
+// {
+// most_slots = uwbSlotsLen;
+// most_slots_index = i;
+// }
+// }
+// }
+// }
+//
+// if(most_slots_index != 255) //what about self? 254?
+// {
+// this->assign_slot(&this->myTDMAInfo, assign_slot);
+// this->uwblist_free_slot(this, most_slots_index, release_slot);
+// memcpy(&this->slotAssignments[assign_slot], &this->slotAssingmentSelfIndex, sizeof(uint8));
+//
+// assigned = TRUE;
+// }
+// }
+//
+// //no slots released via RMA
+// //DF (Double Frame), and assign via GU
+// if(assigned == FALSE)
+// {
+// memcpy(&this->slotAssignments[(int)this->framelength], &this->slotAssingmentSelfIndex, sizeof(uint8));
+//
+// for(int i = 1; i < this->framelength; i++)
+// {
+// memcpy(&this->slotAssignments[i + (int)this->framelength], &this->slotAssignments[i], sizeof(uint8));
+// }
+// this->framelength *= 2;
+//
+// assigned = TRUE;
+// }
+// }
+//
+// for(int i = 0; i < this->framelength; i++)
+// {
+// //TODO handle the case where we are assigned to the slot
+// uint8 slot_index;
+// memcpy(&slot_index, &this->slotAssignments[i], sizeof(uint8));
+// memcpy(&inst->inf_msg.messageData[msgDataIndex], &inst->uwbList[slot_index], sizeof(inst->addrByteSize));
+// msgDataIndex += sizeof(inst->addrByteSize);
+// }
+//
+// memcpy(&inst->inf_msg.messageData[FCODE + 1], &this->framelength, 1);
+//
+// dwt_setrxtimeout((uint16)0);
+//// this->usb_dump_tdma(this);
+//
+//
+//}
+
+
+//void process_sug_msg(struct TDMAHandler *this, uint8 *messageData, uint8 *srcAddr)
+//{
+// //how to compare and integrate changes???
+// //first add any addresses to my list
+// instance_data_t *inst = instance_get_local_structure_ptr(0);
+//
+// uint8 srcIndex = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
+// inst->uwbListType[srcIndex] = UWB_LIST_NEIGHBOR;
+// uint8 neighborAddressN[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+// uint8 neighborFramelengthN;
+// uint8 slotAddress[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+// uint8 blankAddress[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+// uint32 timeSinceFrameStart;
+//
+//// if(tdma_handler->discovery_mode == WAIT_INF_REG)
+//// {
+//// tdma_handler->tdma_free_all_slots(tdma_handler);
+//// }
+//
+// int msgDataIndex = FCODE + 1;
+// memcpy(&tdma_handler->uwbFramelengths[srcIndex], &messageData[msgDataIndex], 1);
+// msgDataIndex++;
+//
+// //TODO use range to help sync frame start time
+// memcpy(&timeSinceFrameStart, &messageData[msgDataIndex], sizeof(timeSinceFrameStart));
+// msgDataIndex += sizeof(timeSinceFrameStart);
+//
+//
+// memcpy(&srcNumNeighbors, &messageData[msgDataIndex], 1);
+// msgDataIndex++;
+//
+// int fl_index_offset = (int)(inst->addrByteSize*srcNumNeighbors);
+// for(int n = 0; n < srcNumNeighbors; n++)
+// {
+// int framelength_index = msgDataIndex + fl_index_offset;
+// memcpy(&neighborAddressN, &messageData[msgDataIndex], inst->addrByteSize);
+// memcpy(&neighborFramelengthN, &messageData[framelength_index], sizeof(neighborFramelengthN));
+//
+// msgDataIndex += (int)inst->addrByteSize;
+//
+// uint8 uwb_index = instgetuwblistindex(inst, &neighborAddressN[0], inst->addrByteSize);
+// if(inst->uwbListType[uwb_index] != UWB_LIST_NEIGHBOR)
+// {
+// inst->uwbListType[uwb_index] = UWB_LIST_HIDDEN;
+// }
+//
+// tdma_handler->uwbFramelengths[uwb_index] = neighborFramelengthN;
+// }
+// msgDataIndex += sizeof(uint8)*srcNumNeighbors;
+//
+//
+// //now process the slot assignments
+// for(int i = 0; i < tdma_handler->uwbFramelengths[srcIndex]-1; i++)//zeroth slot not included in INF
+// {
+// uint8 slot = i + 1;
+// memcpy(&slotAddress, &messageData[msgDataIndex], inst->addrByteSize);
+// //see who the address belongs to
+//#if (USING_64BIT_ADDR==0)
+// if(memcmp(&inst->uwbShortAdd, &slotAddress, inst->addrByteSize) == 0)
+//#else
+// if(memcmp(&inst->eui64[0], &slotAddress, inst->addrByteSize) == 0)
+//#endif
+// {
+// //assignment is self
+// }
+// else
+// {
+// //assignment in UWB list if not unassigned
+// if(memcmp(&inst->eui64[0], &blankAddress, inst->addrByteSize) != 0)
+// {
+// //find in our list (or add as hidden)
+// uint8 uwb_index = instgetuwblistindex(inst, &slotAddress[0], inst->addrByteSize);
+// if(inst->uwbListType[uwb_index] != UWB_LIST_NEIGHBOR)
+// {
+// inst->uwbListType[uwb_index] = UWB_LIST_HIDDEN;
+// }
+//
+// //what to do with the information???
+// //TODO left off here!!!
+//// tdma_handler->uwblist_assign_slot(tdma_handler, uwb_index, slot);
+// }
+//
+//
+//
+//
+// }
+//
+// msgDataIndex += (int)inst->addrByteSize;
+// }
+//
+//
+// //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 message length!
+//
+//
+// tdma_handler->uwbFrameStartTimes[srcIndex] = time_now - (timeSinceFrameStart + txrx_delay);
+//
+//
+// uint32 myFramelengthDuration = tdma_handler->framelength*tdma_handler->slotDuration;
+// uint32 myTimeSinceFrameStart = get_dt32(tdma_handler->frameStartTime, time_now);
+//
+// //NEW
+// if(tdma_handler->framelength <= tdma_handler->uwbFramelengths[srcIndex])
+// {
+// uint32 timeSinceFrameStartMod = (timeSinceFrameStart + txrx_delay)%myFramelengthDuration;
+//
+// if(myTimeSinceFrameStart > timeSinceFrameStartMod)
+// {
+// uint32 diff = myTimeSinceFrameStart - timeSinceFrameStartMod;
+// tdma_handler->frameStartTime += diff/2;
+// tdma_handler->lastSlotStartTime += diff/2;
+// }
+// else
+// {
+// uint32 diff = timeSinceFrameStartMod - myTimeSinceFrameStart;
+// tdma_handler->frameStartTime -= diff/2;
+// tdma_handler->lastSlotStartTime -= diff/2;
+//
+// }
+// }
+// else
+// {
+// uint32 hisFramelengthDuration = tdma_handler->uwbFramelengths[srcIndex]*tdma_handler->slotDuration;
+// uint32 myTimeSinceFrameStartMod = (myTimeSinceFrameStart - txrx_delay)%hisFramelengthDuration;
+//
+// if(timeSinceFrameStart > myTimeSinceFrameStartMod)
+// {
+// uint32 diff = timeSinceFrameStart - myTimeSinceFrameStartMod;
+// tdma_handler->frameStartTime -= diff/2;
+// tdma_handler->lastSlotStartTime -= diff/2;
+// }
+// else
+// {
+// uint32 diff = myTimeSinceFrameStartMod - timeSinceFrameStart;
+// tdma_handler->frameStartTime += diff/2;
+// tdma_handler->lastSlotStartTime += diff/2;
+// }
+// }
+//
+//
+//
+//
+//}
+
+
+
+static bool poll_delay(struct TDMAHandler *this, uint32 time_now_offset, uint32 offset)
+{
+ bool delay = FALSE;
+
+ uint32 time_now = portGetTickCnt();
+ uint32 timeSinceSlotStart = get_dt32(this->lastSlotStartTime, time_now);
+ if(timeSinceSlotStart > this->slotStartDelay)
+ {
+ delay = FALSE;
+ }
+
+ return delay;
+}
+
+static bool slot_assigned(struct TDMAInfo *info, uint8 slot)
+{
+ bool assigned = FALSE;
+ if(info->slots != NULL && info->slotsLength != 0)
+ {
+ for(int i = 0; i < info->slotsLength; i++)
+ {
+ if(memcmp(&info->slots[i], &slot, 1) == 0)
+ {
+ assigned = TRUE;
+ break;
+ }
+ }
+ }
+
+ return assigned;
+}
+
+static bool assign_slot(struct TDMAInfo *info, uint8 slot)
+{
+ //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)
+ {
+ 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;
+ }
+
+ return retval;
+}
+
+//finding and assigning a slot works according to the following:
+//1.) Set framelength to 4
+//2.) Get Unassigned Slots (GU)
+// applicable if one or more open slots exist
+// assign self all unassigned slots (except for 0th slot); exit
+//3.) Release Multiple Assigned Slots (RMA)
+// applicable if 2.) not applicable
+// applicable if one or more nodes has multiple slot assignments
+// release one slot from node with greatest number of slot assignments and assign to self; exit
+//4.) Double the Frame (DF)
+// applicable if 2.) and 3.) not applicable
+// double own framelength and go back to 2.)
+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];
+ bool assignment_made = FALSE;
+
+ //set framelength
+ info->framelength = 4;
+
+ //come here after DF
+ while(TRUE)
+ {
+ //GU
+ for(int i = 1; i < info->framelength; i++) //TODO make sure we dont accidentally assign to slot 0
+ {
+ bool assigned = FALSE;
+
+ for(int u = 1; u < inst->uwbListLen; u++)//0 reserved for self
+ {
+ for(int 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)
+ {
+ //slot assigned to this uwb
+ assigned = TRUE;
+ break;
+ }
+ }
+
+ if(assigned == TRUE)
+ {
+ break;
+ }
+ }
+
+ //slot not assigned, assign to self
+ if(assigned == FALSE)
+ {
+ this->assign_slot(info, i);
+ assignment_made = TRUE;
+ }
+ }
+
+ if(assignment_made == TRUE)
+ {
+ break;
+ }
+
+ //RMA
+ //find UWB with greatest number of slot assignments
+ uint8 max_assignments = 0;
+ uint8 max_uwb_index = 255;
+ for(int u = 1; u < inst->uwbListLen; u++)//0 reserved for self
+ {
+ if(this->uwbListTDMAInfo[u].slotsLength > max_assignments)
+ {
+ max_assignments = this->uwbListTDMAInfo[u].slotsLength;
+ max_uwb_index = u;
+ }
+ }
+
+ if(max_uwb_index != 255)
+ {
+ 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);
+ assignment_made = TRUE;
+ }
+
+ if(assignment_made == TRUE)
+ {
+ break;
+ }
+
+ //DF
+ info->framelength *= 2;
+ }
+}
+
+static void build_new_network(struct TDMAHandler *this)
+{
+ instance_data_t *inst = instance_get_local_structure_ptr(0);
+ uint32 time_now = portGetTickCnt();
+
+ //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->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);
+}
+
+
+static bool deconflict_slot_assignments(struct TDMAHandler *this)
+{
+ instance_data_t *inst = instance_get_local_structure_ptr(0);
+ bool conflict = FALSE;
+
+ while(TRUE)
+ {
+ bool conflict_this_iter = FALSE;
+ //first deconflict slots in neighbor, hidden, and twice hidden
+ for(int i = 1; i < inst->uwbListLen; i++)//0 reserved for self
+ {
+ if(inst->uwbListType[i] != UWB_LIST_INACTIVE)
+ {
+ for(int j = i+1; j < inst->uwbListLen; j++)
+ {
+ if(inst->uwbListType[j] != UWB_LIST_INACTIVE && j != i)
+ {
+ //first check if their list type requires deconflicting
+ if((inst->uwbListType[i] == UWB_LIST_NEIGHBOR && inst->uwbListType[j] == UWB_LIST_TWICE_HIDDEN) ||
+ (inst->uwbListType[j] == UWB_LIST_NEIGHBOR && inst->uwbListType[i] == UWB_LIST_TWICE_HIDDEN) ||
+ (inst->uwbListType[i] == UWB_LIST_TWICE_HIDDEN && inst->uwbListType[j] == UWB_LIST_TWICE_HIDDEN) ||
+ (inst->uwbListType[i] == UWB_LIST_HIDDEN && inst->uwbListType[j] == UWB_LIST_HIDDEN))
+ {
+ continue;
+ }
+
+ if(this->deconflict_uwb_pair(this, &this->uwbListTDMAInfo[i],&this->uwbListTDMAInfo[j]))
+ {
+ conflict = TRUE;
+ conflict_this_iter = TRUE;
+ break;
+ }
+ }
+ }
+ }
+
+ if(conflict_this_iter)
+ {
+ break;
+ }
+ }
+
+ if(conflict_this_iter)
+ {
+ continue;
+ }
+
+
+ //next deconflict slots between self and neighbor, hidden, and twice hidden
+ for(int i = 1; i < inst->uwbListLen; i++)//0 reserved for self
+ {
+ if(inst->uwbListType[i] != UWB_LIST_INACTIVE)
+ {
+ if(this->deconflict_uwb_pair(this, &this->uwbListTDMAInfo[0], &this->uwbListTDMAInfo[i]))
+ {
+ conflict = TRUE;
+ conflict_this_iter = TRUE;
+ }
+ }
+ if(conflict_this_iter)
+ {
+ break;
+ }
+ }
+
+ if(conflict_this_iter)
+ {
+ continue;
+ }
+
+ break; //no conflicts found this iteration, break out of while loop
+ }
+
+ return conflict;
+}
+
+
+//bool indexed_deconflict_slot_assignments(struct TDMAHandler *this, bool selfDeconflict, bool *uwbListDeconflict)
+//{
+// instance_data_t *inst = instance_get_local_structure_ptr(0);
+// bool conflict = FALSE;
+//
+// while(TRUE)
+// {
+// bool conflict_this_iter = FALSE;
+// //first deconflict slots in neighbor, hidden, and twice hidden
+// for(int i = 0; i < inst->uwbListLen; i++)
+// {
+// if(inst->uwbListType[i] != UWB_LIST_INACTIVE)
+// {
+// for(int j = i+1; j < inst->uwbListLen; j++)
+// {
+// if(inst->uwbListType[j] != UWB_LIST_INACTIVE && j != i)
+// {
+// //first check if their list type requires deconflicting
+// if((inst->uwbListType[i] == UWB_LIST_NEIGHBOR && inst->uwbListType[j] == UWB_LIST_TWICE_HIDDEN) ||
+// (inst->uwbListType[j] == UWB_LIST_NEIGHBOR && inst->uwbListType[i] == UWB_LIST_TWICE_HIDDEN) ||
+// (inst->uwbListType[i] == UWB_LIST_TWICE_HIDDEN && inst->uwbListType[j] == UWB_LIST_TWICE_HIDDEN) ||
+// (inst->uwbListType[i] == UWB_LIST_HIDDEN && inst->uwbListType[j] == UWB_LIST_HIDDEN))
+// {
+// continue;
+// }
+//
+// if(this->deconflict_uwb_pair(this, &this->uwbListTDMAInfo[i], &this->uwbListTDMAInfo[j]))
+// {
+// conflict = TRUE;
+// conflict_this_iter = TRUE;
+// break;
+// }
+// }
+// }
+// }
+//
+// if(conflict_this_iter)
+// {
+// break;
+// }
+// }
+//
+// if(conflict_this_iter)
+// {
+// continue;
+// }
+//
+//
+// //next deconflict slots between self and neighbor, hidden, and twice hidden
+// for(int i = 0; i < inst->uwbListLen; i++)
+// {
+// if(inst->uwbListType[i] != UWB_LIST_INACTIVE)
+// {
+// if(this->deconflict_uwb_pair(this, &this->myTDMAInfo, &this->uwbListTDMAInfo[i]))
+// {
+// conflict = TRUE;
+// conflict_this_iter = TRUE;
+// }
+// }
+// if(conflict_this_iter)
+// {
+// break;
+// }
+// }
+//
+// if(conflict_this_iter)
+// {
+// continue;
+// }
+//
+// break; //no conflicts found this iteration, break out of while loop
+// }
+//
+// return conflict;
+//}
+
+
+//return true if a conflict was found
+static bool deconflict_uwb_pair(struct TDMAHandler *this, struct TDMAInfo *info_a, struct TDMAInfo *info_b)
+{
+ bool conflict = FALSE;
+
+ while(TRUE)
+ {
+ bool conflict_this_iter = false;
+
+ for(int sa = 0; sa < info_a->slotsLength; sa++)
+ {
+ uint8 slot_sa;
+ memcpy(&slot_sa, &info_a->slots[sa], 1);
+
+ for(int sb = 0; sb < info_b->slotsLength; sb++)
+ {
+ uint8 slot_sb;
+ memcpy(&slot_sb, &info_a->slots[sb], 1);
+
+
+ //check if slot is taken
+ if(slot_sa >= info_b->framelength)
+ {
+ uint8 mod_slot_sa = slot_sa%info_b->framelength;
+
+ if(mod_slot_sa == slot_sb)
+ {
+ //slot already assigned, deconflict!
+ this->deconflict_slot_pair(this, info_a, info_b, sa, sb);
+ conflict = TRUE;
+ conflict_this_iter = TRUE;
+ break;
+ }
+ }
+ else if(slot_sb >= info_a->framelength)
+ {
+ uint8 mod_slot_sb = slot_sb%info_a->framelength;
+ if(mod_slot_sb == slot_sa)
+ {
+ //slot already assigned, deconflict!
+ this->deconflict_slot_pair(this, info_a, info_b, sa, sb);
+ conflict = TRUE;
+ conflict_this_iter = TRUE;
+ break;
+ }
+ }
+ else
+ {
+ if(slot_sa == slot_sb)
+ {
+ //slot already assigned, deconflict!
+ this->deconflict_slot_pair(this, info_a, info_b, sa, sb);
+ conflict = TRUE;
+ conflict_this_iter = TRUE;
+ break;
+ }
+ }
+ }
+
+ if(conflict_this_iter)
+ {
+ break;
+ }
+ }
+
+ if(conflict_this_iter)
+ {
+ continue;
+ }
+
+ break; //no conflicts found this iterations, break while loop
+ }
+
+ return conflict;
+}
+
+
+//TODO assign reference in class struct!!! same with others written today
+static void deconflict_slot_pair(struct TDMAHandler *this, struct TDMAInfo *info_a, struct TDMAInfo *info_b, uint8 slot_idx_a, uint8 slot_idx_b)
+{
+ //procedure for deconflicting slots (PDS)
+ //1.) delete a conflicting slot
+ // applicable if all but node with fewest slots has more than one slot assignment
+ // release conflicting slot from all but node with fewest slots
+ //2.) divide the assignment
+ // applicable if multiple conflicting slots between to nodes
+ // release lowest slot from first, greatest from second
+ //3.) double the frame and divide the assignment
+ // applicable if single conflict between two nodes and neither has another slot assignment
+ // double the framelength of one or both and assign one slot assignment to the first and other to the second
+ // make the change
+ //4.) if any of 1-3.) is applied, check for conflicts again, and start again at 1.)
+
+ //logic for 1.) and 2.) not explicitly programmed.
+ //Should be taken care of by repeatedly checking for conflicts and executing the code block below
+ if(info_a->slotsLength > 1 || info_b->slotsLength > 1)
+ {
+ if(info_a->slotsLength >= info_b->slotsLength)
+ {
+ //release slot from uwb_a
+ uint8 slot_a;
+ memcpy(&slot_a, &info_a->slots[slot_idx_a], sizeof(uint8));
+ this->free_slot(info_a, slot_a);
+ return;
+ }
+ else
+ {
+ //release slot from uwb_b
+ uint8 slot_b;
+ memcpy(&slot_b, &info_b->slots[slot_idx_b], sizeof(uint8));
+ this->free_slot(info_b, slot_b);
+ return;
+ }
+ }
+
+ //double the frame and divide the assignment
+ if(info_a->framelength == info_b->framelength)
+ {
+ uint8 slot_b;
+ memcpy(&slot_b, &info_b->slots[slot_idx_b], sizeof(uint8));
+ slot_b += info_b->framelength;
+ memcpy(&info_b->slots[slot_idx_b], &slot_b, sizeof(uint8));
+ info_a->framelength *= 2;
+ info_b->framelength *= 2;
+ }
+ else if(info_a->framelength > info_b->framelength)
+ {
+ uint8 slot_a;
+ memcpy(&slot_a, &info_a->slots[slot_idx_a], sizeof(uint8));
+ uint8 mod_a = slot_a % (2*info_b->framelength);
+
+ uint8 slot_b;
+ memcpy(&slot_b, &info_b->slots[slot_idx_b], sizeof(uint8));
+
+ if(mod_a == slot_b)
+ {
+ slot_b += info_b->framelength;
+ memcpy(&info_b->slots[slot_idx_b], &slot_b, sizeof(uint8));
+ }
+
+ info_b->framelength *= 2;
+ }
+ else if(info_a->framelength < info_b->framelength)
+ {
+ uint8 slot_b;
+ memcpy(&slot_b, &info_b->slots[slot_idx_b], sizeof(uint8));
+ uint8 mod_b = slot_b % (2*info_a->framelength);
+
+ uint8 slot_a;
+ memcpy(&slot_a, &info_a->slots[slot_idx_a], sizeof(uint8));
+
+ if(mod_b == slot_a)
+ {
+ slot_a += info_a->framelength;
+ memcpy(&info_a->slots[slot_idx_a], &slot_a, sizeof(uint8));
+ }
+
+ info_a->framelength *= 2;
+ }
+
+ //re-checking for conflicts handled in calling function
+}
+
+//check if this uwb has any TDMA conflicts with others in the uwbList
+static bool self_conflict(struct TDMAHandler *this)
+{
+ instance_data_t *inst = instance_get_local_structure_ptr(0);
+ struct TDMAInfo *info_a = &this->uwbListTDMAInfo[0];
+
+ for(int b = 1; b < inst->uwbListLen; b++)
+ {
+ if(inst->uwbListType[b] == UWB_LIST_NEIGHBOR ||
+ inst->uwbListType[b] == UWB_LIST_HIDDEN ||
+ inst->uwbListType[b] == UWB_LIST_TWICE_HIDDEN)
+ {
+ struct TDMAInfo *info_b = &this->uwbListTDMAInfo[b];
+
+ for(int sa = 0; sa < info_a->slotsLength; sa++)
+ {
+ uint8 slot_sa;
+ memcpy(&slot_sa, &info_a->slots[sa], 1);
+
+ for(int sb = 0; sb < info_b->slotsLength; sb++)
+ {
+ uint8 slot_sb;
+ memcpy(&slot_sb, &info_a->slots[sb], 1);
+
+
+ //check if slot is taken
+ if(slot_sa >= info_b->framelength)
+ {
+ uint8 mod_slot_sa = slot_sa%info_b->framelength;
+
+ if(mod_slot_sa == slot_sb)
+ {
+ return TRUE;
+ }
+ }
+ else if(slot_sb >= info_a->framelength)
+ {
+ uint8 mod_slot_sb = slot_sb%info_a->framelength;
+ if(mod_slot_sb == slot_sa)
+ {
+ return TRUE;
+ }
+ }
+ else
+ {
+ if(slot_sa == slot_sb)
+ {
+ return TRUE;
+ }
+ }
+ }
+ }
+ }
+ }
+
+ return FALSE;
+}
+
+static void free_slot(struct TDMAInfo *info, uint8 slot)
+{
+ bool assigned = TRUE;
+
+ while(assigned == TRUE) //duplicate assignments shouldn't exist, but will make sure to remove any just in case
+ {
+ uint8 slot_index = 255;
+ assigned = FALSE;
+ if(info->slots != NULL && info->slotsLength != 0)
+ {
+ for(int i = 0; i < info->slotsLength; i++)
+ {
+ if(memcmp(&info->slots[i], &slot, sizeof(uint8)) == 0)
+ {
+ assigned = TRUE;
+ slot_index = i;
+ break;
+ }
+ }
+ }
+
+ //if assigned, remove from array
+ if(assigned == TRUE)
+ {
+ memcpy(&info->slots[slot_index], &info->slots[slot_index + 1], sizeof(uint8)*(info->slotsLength - slot_index - 1));
+ info->slotsLength -= 1;
+ if(info->slotsLength <= 0)
+ {
+ info->slotsLength = 0;
+ free(info->slots);
+ info->slots = NULL;
+ }
+ }
+ }
+
+
+ return;
+}
+
+static void free_slots(struct TDMAInfo *info)
+{
+ if(info->slots != NULL)
+ {
+ free(info->slots);
+ info->slots = NULL;
+ }
+
+ info->slotsLength = 0;
+ info->framelength = MIN_FRAMELENGTH;
+}
+
+static void uwblist_free_slots(struct TDMAHandler *this, uint8 uwb_index)
+{
+ instance_data_t *inst = instance_get_local_structure_ptr(0);
+ if(uwb_index >= inst->uwbListLen)
+ {
+ //out of bounds!
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "uwblist_free_slots: uwb_index %u out of bounds", uwb_index);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+ return;
+ }
+
+ this->free_slots(&this->uwbListTDMAInfo[uwb_index]);
+
+ return;
+}
+
+static void tdma_free_all_slots(struct TDMAHandler *this)
+{
+ for(int i = 0; i < (int)UWB_LIST_SIZE; i++)
+ {
+ if(this->uwbListTDMAInfo[i].slots != NULL)
+ {
+ free(this->uwbListTDMAInfo[i].slots);
+ this->uwbListTDMAInfo[i].slots = NULL;
+ }
+
+ this->uwbListTDMAInfo[i].slotsLength = 0;
+ this->uwbListTDMAInfo[i].framelength = MIN_FRAMELENGTH;
+ }
+
+ return;
+}
+
+
+//TODO
+static void enter_discovery_mode(struct TDMAHandler *this)
+{
+// instance_data_t *inst = instance_get_local_structure_ptr(0);
+// inst->inf_msg.messageData[FCODE] = RTLS_DEMO_MSG_INF_INIT;
+
+ uint32 time_now = portGetTickCnt();
+ this->discoveryStartTime = time_now;
+ this->last_blink_time = time_now;
+ this->set_discovery_mode(this, WAIT_INF_REG, time_now);
+ this->collectInfStartTime = time_now;
+
+ this->tdma_free_all_slots(this);
+}
+
+static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discovery_mode, uint32 time_now)
+{
+ this->discovery_mode_start_time = time_now;
+ this->discovery_mode = discovery_mode;
+
+
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "set_discovery_mode: %s", get_discovery_modes_string(discovery_mode));
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+
+
+ switch (discovery_mode)//TODO make sure all modes are captured here...
+ {
+ case WAIT_INF_REG:
+ {
+ this->discovery_mode_duration = 0;
+ this->discovery_mode_expires = FALSE;
+ break;
+ }
+ case COLLECT_INF_REG:
+ {
+ this->collectInfStartTime = time_now;
+ this->discovery_mode_duration = this->collectInfDuration;
+ this->discovery_mode_expires = TRUE;
+ break;
+ }
+ case WAIT_INF_INIT:
+ {
+ this->discovery_mode_duration = 400;//TODO use a smart timeout value
+ this->discovery_mode_expires = TRUE;
+ break;
+ }
+ case WAIT_RNG_INIT:
+ {
+ this->discovery_mode_duration = 400;//TODO use a smart timeout value
+ this->discovery_mode_expires = TRUE;
+ break;
+ }
+ case WAIT_SEND_SUG:
+ {
+ //find common frame start time among neighbors
+ instance_data_t *inst = instance_get_local_structure_ptr(0);
+
+ //TODO handle number wrapping
+ uint32 tcommon;
+ uint32 shortestFrameDuration = this->maxFramelength*this->slotDuration;
+ uint8 numNeighbors = instfindnumactiveneighbors(inst);
+ uint32 tnext[numNeighbors];
+ uint32 latest_tnext;
+ uint8 neighborIndices[numNeighbors];
+ uint8 nidx = 0;
+
+ 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->uwbListTDMAInfo[i].framelength*this->slotDuration;
+ }
+ nidx++;
+
+ if(this->uwbListTDMAInfo[i].framelength*this->slotDuration < shortestFrameDuration)
+ {
+ shortestFrameDuration = this->uwbListTDMAInfo[i].framelength*this->slotDuration;
+ }
+ }
+ }
+
+ tcommon = tnext[0];
+ latest_tnext = tcommon;
+ bool converged = FALSE;
+ while(converged == FALSE)
+ {
+ converged = TRUE;
+ for(int i = 0; i < nidx; i++)
+ {
+ uint32 frameduration = this->uwbListTDMAInfo[neighborIndices[i]].framelength*this->slotDuration;
+ while(tnext[i] < tcommon && tcommon - tnext[i] >= frameduration) //include small buffer
+ {
+ tnext[i] += frameduration;
+ }
+
+
+ if(tnext[i] > tcommon && tnext[i] - tcommon >= shortestFrameDuration) //TODO maybe include a small buffer to account for small timing errors?
+ {
+ tcommon = tnext[i];
+ converged = FALSE;
+ }
+
+ if(tnext[i] > latest_tnext)
+ {
+ latest_tnext = tnext[i];
+ }
+ }
+ }
+
+
+ //expire as the beginning of the common frame start time
+ this->discovery_mode_duration = get_dt32(time_now, latest_tnext);
+ this->discovery_mode_expires = TRUE;
+ break;
+ }
+ case EXIT:
+ {
+ this->discovery_mode_duration = 0;
+ this->discovery_mode_expires = FALSE;
+ break;
+ }
+ default:
+ {
+ break;
+ }
+
+ }
+
+
+}
+
+static void check_discovery_mode_expiration(struct TDMAHandler *this)
+{
+ instance_data_t *inst = instance_get_local_structure_ptr(0);
+
+ if(inst->mode == DISCOVERY)
+ {
+ if(this->discovery_mode_expires == TRUE)
+ {
+ uint32 time_now = portGetTickCnt();
+ uint32 timeSinceModeStart = get_dt32(this->discovery_mode_start_time, time_now);
+ if(timeSinceModeStart > this->discovery_mode_duration)
+ {
+ //discovery mode expired
+ DISCOVERY_MODE new_mode = WAIT_INF_REG;
+ if(this->discovery_mode == COLLECT_INF_REG)
+ {
+ new_mode = WAIT_SEND_SUG;
+// inst->testAppState = TA_TX_SELECT;
+ inst->testAppState = TA_RXE_WAIT; //still collect RNG_REPORT messages while we wait to send our SUG message
+
+ //deconflict gathered tdma info
+ this->deconflict_slot_assignments(this);
+ //assign self slot
+ this->find_assign_slot(this);
+ //construct SUG packet
+ this->populate_inf_msg(this, RTLS_DEMO_MSG_INF_SUG);
+
+ //TODO how to actually send SUG message? TA_TX_SELECT?
+ //could go directly to TA_TXSUG_WAIT_SEND and delay until start of next frame...
+ //that could remove the need for extra logic in TA_TX_SELECT...
+ }
+ else if(this->discovery_mode == WAIT_SEND_SUG)
+ {
+ inst->testAppState = TA_TX_SELECT;
+ new_mode = SEND_SUG;
+ }
+
+ this->set_discovery_mode(this, new_mode, time_now);
+ }
+ }
+ }
+}
+
+static void usb_dump_tdma(struct TDMAHandler *this)
+{
+ instance_data_t *inst = instance_get_local_structure_ptr(0);
+
+ uint8 debug_msg[20000];
+ int n = sprintf((char*)&debug_msg[0], "TDMA Handler Dump \n");
+ int n_char = n;
+
+// n = sprintf((char*)&debug_msg[n_char], "framelength: %d, maxFramelength: %d \n", this->myTDMAInfo.framelength, this->maxFramelength);
+// n_char += n;
+
+// n = sprintf((char*)&debug_msg[n_char], "mySlots[%u]: ", this->myTDMAInfo.slotsLength);
+// n_char += n;
+// for(int i = 0; i < this->myTDMAInfo.slotsLength; i++)
+// {
+// uint8 slot;
+// memcpy(&slot, &this->myTDMAInfo.slots[i], sizeof(uint8));
+// n = sprintf((char*)&debug_msg[n_char], " %u ", slot);
+// n_char += n;
+// }
+// n = sprintf((char*)&debug_msg[n_char], "\n");
+// n_char += n;
+
+ for(int u = 0; u < inst->uwbListLen; u++)
+ {
+ n = sprintf((char*)&debug_msg[n_char], "UWB %i framelength: %u \n", u, this->uwbListTDMAInfo[u].framelength);
+ n_char += n;
+
+ n = sprintf((char*)&debug_msg[n_char], "UWB %i Slots[%u]: \n", u, this->uwbListTDMAInfo[u].slotsLength);
+ n_char += n;
+ for(int i = 0; i < this->uwbListTDMAInfo[u].slotsLength; i++)
+ {
+ uint8 slot;
+ memcpy(&slot, &this->uwbListTDMAInfo[u].slots[i], sizeof(uint8));
+ n = sprintf((char*)&debug_msg[n_char], " %u ", slot);
+ n_char += n;
+ }
+ n = sprintf((char*)&debug_msg[n_char], "\n");
+ n_char += n;
+ }
+
+
+
+ int div = 100;
+ int idx = 0;
+
+ for(int i = 0; i + div < n_char; i += div)
+ {
+ idx = i;
+ send_usbmessage(&debug_msg[i], div);
+ usb_run();
+ Sleep(10);
+ }
+ if(idx + 1 < n_char)
+ {
+ send_usbmessage(&debug_msg[idx], n_char - idx);
+ usb_run();
+ }
+
+}
+
+//uint8 get_largest_framelength(struct TDMAHandler *this)
+//{
+// instance_data_t *inst = instance_get_local_structure_ptr(0);
+//
+// uint8 max_framelength = MIN_FRAMELENGTH;
+// if(this->myTDMAInfo.framelength > max_framelength)
+// {
+// max_framelength = this->myTDMAInfo.framelength;
+// }
+//
+// for(int i = 0; i < inst->uwbListLen; i++)
+// {
+// if(inst->uwbListType[i] == UWB_LIST_NEIGHBOR)
+// {
+// if(this->uwbFramelengths[i] > max_framelength)
+// {
+// max_framelength = this->uwbFramelengths[i];
+// }
+// }
+//
+// }
+//
+// return max_framelength;
+//}
+
+
+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.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;
+ ret.assign_slot = &assign_slot;
+ ret.find_assign_slot = &find_assign_slot;
+ ret.build_new_network = &build_new_network;
+
+ ret.free_slot = &free_slot;
+ ret.free_slots = &free_slots;
+ ret.tdma_free_all_slots = &tdma_free_all_slots;
+ ret.uwblist_free_slots = &uwblist_free_slots;
+ ret.enter_discovery_mode = &enter_discovery_mode;
+ 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();
+
+ //TODO create a function to clear the frame information!
+ //and have it called in enter_discovery function!
+
+
+ ret.maxFramelength = (uint8)MIN_FRAMELENGTH;
+ while(ret.maxFramelength < (int)UWB_LIST_SIZE + 1)
+ {
+ 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;
+ ret.uwbListTDMAInfo[i].framelength = (uint8)MIN_FRAMELENGTH;
+ ret.uwbListTDMAInfo[i].slots = NULL;
+ ret.uwbListTDMAInfo[i].slotsLength = 0;
+ }
+
+ ret.uwbFrameStartTimes[0] = time_now;
+ ret.lastSlotStartTime = time_now;
+ ret.infPollSentThisSlot = FALSE;
+ ret.slotStartDelay = 4;
+ ret.infMessageLength = 0;
+
+ ret.enter_discovery_mode(&ret);
+ ret.collectInfDuration = ret.maxFramelength*ret.slotDuration;
+ ret.waitInfDuration = ret.collectInfDuration;
+
+ return ret;
+}
+
+const struct TDMAHandlerClass TDMAHandler={.new=&new};
+
+
diff --git a/src/application/tdma_handler.h b/src/application/tdma_handler.h
new file mode 100644
index 0000000..e7a21c5
--- /dev/null
+++ b/src/application/tdma_handler.h
@@ -0,0 +1,119 @@
+#ifndef TDMA_HANDLER_H_
+#define TDMA_HANDLER_H_
+
+//#include "llist.h"
+#include "deca_types.h"
+#include "application_definitions.h"
+
+struct TDMAInfo
+{
+ uint8 uwbIndex; //TODO remove if unused
+ uint8 framelength;
+ uint8 slotsLength;
+ uint8 *slots;
+};
+
+typedef enum inf_process_mode
+{
+ CLEAR_ALL_COPY,
+ CLEAR_LISTED_COPY,
+ COPY
+}
+INF_PROCESS_MODE;
+
+typedef enum frame_sync_mode
+{
+ FS_ADOPT,
+ FS_AVERAGE
+}
+FRAME_SYNC_MODE;
+
+struct TDMAHandler
+{
+
+ //TDMA class variables
+ 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
+ bool infPollSentThisSlot;
+
+ uint32 slotStartDelay; //time between slot start and transmission within that slot
+
+ //discovery variables
+ DISCOVERY_MODE discovery_mode;
+ uint32 last_blink_time; //timestamp of most recent blink
+ uint32 discoveryStartTime; //time that we started listening for other UWBs
+ uint32 discovery_mode_start_time;
+ uint32 discovery_mode_duration;
+ bool discovery_mode_expires;
+ uint32 collectInfStartTime;
+ uint32 collectInfDuration;
+ uint32 waitInfDuration;
+
+ 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);
+ void (*check_discovery_mode_expiration)(struct TDMAHandler *this);
+ void (*usb_dump_tdma)(struct TDMAHandler *this);
+
+ //TODO left off here. updating messageData as well as
+ //TODO revisit anything that works with messageData!!!
+ bool (*slot_assigned)(struct TDMAInfo *info, uint8 slot);
+ bool (*assign_slot)(struct TDMAInfo *info, uint8 slot);
+ void (*free_slot)(struct TDMAInfo *info, uint8 slot);
+ void (*free_slots)(struct TDMAInfo *info);
+ void (*uwblist_free_slots)(struct TDMAHandler *this, uint8 uwb_index);
+ void (*tdma_free_all_slots)(struct TDMAHandler *this);
+ 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
+ //run through each slot of two uwbs
+ bool (*deconflict_uwb_pair)(struct TDMAHandler *this, struct TDMAInfo *info_a, struct TDMAInfo *info_b);
+ //deconflict two specific slots
+ void (*deconflict_slot_pair)(struct TDMAHandler *this, struct TDMAInfo *info_a, struct TDMAInfo *info_b, uint8 slot_idx_a, uint8 slot_idx_b);
+ bool (*self_conflict)(struct TDMAHandler *this);
+};
+
+
+extern const struct TDMAHandlerClass
+{
+ struct TDMAHandler (*new)();
+
+} TDMAHandler;
+
+
+#endif
diff --git a/src/application/tdma_scheduler.c b/src/application/tdma_scheduler.c
new file mode 100644
index 0000000..3fc6704
--- /dev/null
+++ b/src/application/tdma_scheduler.c
@@ -0,0 +1,214 @@
+#include "tdma_handler.h"
+#include "port.h"
+
+
+extern void usb_run(void);
+extern void send_usbmessage(uint8*, int);
+
+//private methods
+int tdma_node_compare(llist_node first, llist_node second)
+{
+ tdma_timing_node *first_node = (tdma_timing_node *)first;
+ tdma_timing_node *second_node = (tdma_timing_node *)second;
+
+ if(first_node->time == second_node->time)
+ {
+ return 0;
+ }
+ else if(first_node->time > second_node->time)
+ {
+ return 1;
+ }
+ else
+ {
+ return -1;
+ }
+ return 0;
+}
+
+
+bool tdma_node_equal(llist_node first, llist_node second)
+{
+ if(first == second){return true;}
+
+ return false;
+}
+
+
+//class methods
+static void set_slot(struct TDMAHandler *this)
+{
+// 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;
+//// time_now += offset;
+// }
+// uint32 time_now_offset = time_now + offset;
+//
+//// if(time_now_offset < this->time_reject_select + offset){
+//// return -1;
+//// }
+//
+//// uint8 debug_msg[100];
+//// int n = sprintf((char*)&debug_msg[0], "[0] %u [1] %u", this->uwb_timeout_list[0], this->uwb_timeout_list[1]);
+//// send_usbmessage(&debug_msg[0], n);
+//// usb_run();
+//
+// //start by removing expired nodes
+// while(true){
+//
+// if(llist_size(this->list) == 0){break;}
+//
+// tdma_timing_node *node = llist_get_head(this->list);
+//
+// if(time_now_offset > node->time + node->duration + offset){ //expired
+// llist_delete_node(this->list, node, true, NULL);
+// }
+// else{
+// break;
+// }
+// }
+//
+// //check if it's time to blink
+// if(time_now_offset > this->last_blink_time + this->blink_period + offset){
+// this->last_blink_time = time_now;
+// return 255;
+// }
+//
+// //don't range if it will carry over into blink time
+// if(time_now_offset + this->range_duration > this->last_blink_time + this->blink_period + offset){
+// return -1;
+// }
+//
+// //TODO could have a set flag for this like uwb_active...
+// int cooldown_arr[*this->uwb_list_length];
+// memset(cooldown_arr, 0, sizeof(cooldown_arr));
+//
+// tx_timing_node *node = llist_get_head(this->list);
+// //dont select if it starts in or ends in a cooldown period...
+// while(node != NULL){
+// if(cooldown_arr[node->index] == 0)
+// {
+// if((time_now_offset > node->time + offset && time_now_offset < node->time + node->duration + offset) ||
+// (time_now_offset + this->range_duration > node->time + offset && time_now_offset + this->range_duration < node->time + node->duration + offset) )
+// {
+// cooldown_arr[node->index] = 1;
+// }
+// }
+// node = llist_get_next(this->list, node);
+// }
+//
+//
+// uint8 uwb_list_len = *this->uwb_list_length;
+// uint8 num_checked = 0;
+// uint8 index = (this->last_select_index + 1)%uwb_list_len;
+// while(num_checked < uwb_list_len){
+// if(cooldown_arr[index] == 0 && this->uwb_timeout_list[index] == 0){
+// this->last_select_index = index;
+// this->time_reject_select = time_now + this->range_duration;
+// this->expected_tx_end_time = time_now + this->range_duration;
+//
+//
+// //add cooldown to prevent polling same tag too rapidly (can block reception of blinks from other tags)
+// tx_timing_node *node = malloc(sizeof(tx_timing_node));
+// node->index = index;
+// node->duration = this->poll_frequency_period;
+// node->time = time_now;
+//
+// this->add_node(this, node);
+//
+// return index;
+// }
+//
+// index = (index + 1)%uwb_list_len;
+// num_checked++;
+// }
+
+ return;
+}
+
+
+static bool add_node(struct TDMAHandler *this, tdma_timing_node *node) //TODO handle unsuccessful add
+{
+// char debug_msg[100];
+// n = sprintf((char*)&debug_msg[0], "node added, index %i", *node->index);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
+ if(llist_is_empty(this->list))
+ {
+ return llist_add_node(this->list, node, ADD_NODE_REAR);
+ }
+ else{
+ uint32 offset = 0;
+
+ //force number wrapping in case the timestamp is getting close to wrapping
+ if(node->time > 4000000000){
+ offset = 1000000000;
+ }
+
+ tdma_timing_node *pos_node = llist_get_head(this->list);
+ while(pos_node != NULL){
+
+ if(node->time + offset < pos_node->time + offset){
+ return llist_insert_node(this->list, node, pos_node, ADD_NODE_BEFORE);
+ }
+
+ pos_node = llist_get_next(this->list, node);
+ }
+
+ return llist_add_node(this->list, node, ADD_NODE_REAR);
+ }
+}
+
+static struct TDMAHandler new(instance_data_t *inst){
+ struct TDMAHandler ret = {};
+// ret.blink_period = (int)(1.0/(int)BLINK_PERIOD_MS);
+// ret.uwb_list = uwb_list;
+// ret.uwb_list_length = uwb_list_length;
+// ret.uwb_max_list_length = uwb_max_list_length;
+// ret.uwb_timeout_list = uwb_timeout_list; //might need uwbliststate pointer here
+
+ ret.list = llist_create(tdma_node_compare, tdma_node_equal, 0);
+
+ ret.add_node = &add_node;
+ ret.set_slot = &set_slot;
+
+ uint32 time_now = portGetTickCnt();
+
+ ret.last_blink_time = time_now;
+ ret.framelength = MIN_FRAMELENGTH;
+ ret.maxFramelength = min_framelength;
+ while(ret.maxFramelength < ret.uwb_max_list_length + 1)
+ {
+ ret.maxFramelength *= 2;
+ }
+ ret.slotDuration = 30; //TODO use a #define or something?
+
+ ret.slotAssignments = malloc(sizeof(uint16)*ret.maxFramelength);
+ for(int i = 0; i < ret.maxFramelength; i++)
+ {
+ uint16 blank = 0x0000;
+ memcpy(&ret.slotAssignments[i], &blank, 2);
+ }
+
+ ret.uwbFramelengths = malloc(sizeof(uint8)*ret.maxFramelength);
+ for(int i = 0; i < ret.uwb_max_list_length; i++)
+ {
+ ret.uwbFramelengths[i] = 0;
+ }
+
+ ret.frameStartTime = time_now;
+ ret.lastSlotStartTime = time_now;
+ ret.discoveryStartTime = time_now;
+ ret.last_blink_time = time_now;
+
+ return ret;
+}
+
+const struct TDMAHandlerClass TDMASHandler={.new=&new};
+
+
diff --git a/src/application/tdma_scheduler.h b/src/application/tdma_scheduler.h
new file mode 100644
index 0000000..37c968b
--- /dev/null
+++ b/src/application/tdma_scheduler.h
@@ -0,0 +1,62 @@
+#ifndef TDMA_HANDLER_H_
+#define TDMA_HANDLER_H_
+
+#include "llist.h"
+#include "deca_types.h"
+#include "instance.h"
+
+typedef struct
+{
+ uint32 time; //time the node is scheduled to be used.
+ uint32 duration; //expected time the node event will take before completion
+ uint8 index; //the node index
+
+} tdma_timing_node;
+
+
+struct TDMASHandler
+{
+
+ llist list; //list of tdma_timing_nodes
+
+ //instance variables
+ instance_data_t *inst;
+// uint8 uwb_max_list_length;
+// uint8 *uwb_list_length;
+
+// uint8 (*uwb_list)[][8]; //pointer to uwbList
+// uint8 *uwb_timeout_list; //pointer to uwbTimeout
+ //might need uwbliststate pointer here
+
+
+ //TDMA class variables
+ uint8 framelength;
+ uint8 maxFramelength;
+ uint16 *slotAssignments; //TODO may need two slot assignments!
+ uint8 *uwbFramelengths; //[UWB_LIST_SIZE]
+ uint32 frameStartTime; //TODO how to sync
+ uint32 lastSlotStartTime;
+ uint32 slotDuration; //TODO 30 ms?
+ uint32 discoveryStartTime; //time that we started listening for other UWBs
+
+ //discovery variables
+// uint32 blink_period;
+ uint32 last_blink_time; //timestamp of most recent blink
+
+
+ //class functions
+ void (*set_slot)(struct TDMAHandler *this);
+ bool (*add_node)(struct TDMAHandler *this, tdma_timing_node *node);
+
+};
+
+
+extern const struct TDMAHandlerClass
+{
+// struct TDMAScheduler (*new)(float blink_frequency, uint32 blink_duration, uint32 range_duration, uint8 (*uwb_list)[][8], uint8 *uwb_list_length, uint8 uwb_max_list_length, uint8 uwb_timeout_list[]);
+ struct TDMAHandler (*new)(instance_data_t *inst);
+
+} TDMAHandler;
+
+
+#endif
diff --git a/src/application/tx_scheduler.c b/src/application/tx_scheduler.c
index 9ec5cf3..12ad530 100644
--- a/src/application/tx_scheduler.c
+++ b/src/application/tx_scheduler.c
@@ -1,4 +1,4 @@
-#include "tx_scheduler.h"
+#include "tmda_scheduler.h"
#include "port.h"
@@ -6,10 +6,10 @@ extern void usb_run(void);
extern void send_usbmessage(uint8*, int);
//private methods
-int tx_node_compare(llist_node first, llist_node second)
+int tdma_node_compare(llist_node first, llist_node second)
{
- tx_timing_node *first_node = (tx_timing_node *)first;
- tx_timing_node *second_node = (tx_timing_node *)second;
+ tdma_timing_node *first_node = (tx_timing_node *)first;
+ tdma_timing_node *second_node = (tx_timing_node *)second;
if(first_node->time == second_node->time)
{
@@ -27,7 +27,7 @@ int tx_node_compare(llist_node first, llist_node second)
}
-bool tx_node_equal(llist_node first, llist_node second)
+bool tdma_node_equal(llist_node first, llist_node second)
{
if(first == second){return true;}
@@ -36,10 +36,7 @@ bool tx_node_equal(llist_node first, llist_node second)
//class methods
-
-//return index of uwb to initiate comms with
-//index 0-244 for uwb, 255 for blink, -1 for none
-static int tx_select(struct TXScheduler *this)
+static int set_slot(struct TDMAScheduler *this)
{
uint32 time_now = portGetTickCnt();
uint32 offset = 0;
@@ -65,7 +62,7 @@ static int tx_select(struct TXScheduler *this)
if(llist_size(this->list) == 0){break;}
- tx_timing_node *node = llist_get_head(this->list);
+ tdma_timing_node *node = llist_get_head(this->list);
if(time_now_offset > node->time + node->duration + offset){ //expired
llist_delete_node(this->list, node, true, NULL);
@@ -133,7 +130,8 @@ static int tx_select(struct TXScheduler *this)
return -1;
}
-static bool add_node(struct TXScheduler *this, tx_timing_node *node) //TODO handle unsuccessful add
+
+static bool add_node(struct TDMAScheduler *this, tdma_timing_node *node) //TODO handle unsuccessful add
{
// char debug_msg[100];
// n = sprintf((char*)&debug_msg[0], "node added, index %i", *node->index);
@@ -166,23 +164,51 @@ static bool add_node(struct TXScheduler *this, tx_timing_node *node) //TODO hand
}
}
-static struct TXScheduler new(float blink_frequency, uint32 blink_duration, uint32 range_duration, uint8 (*uwb_list)[][8], uint8 *uwb_list_length, uint8 uwb_max_list_length, uint8 uwb_timeout_list[]){
- struct TXScheduler ret = {.uwb_list = uwb_list, .uwb_list_length = uwb_list_length, .uwb_max_list_length=uwb_max_list_length, .uwb_timeout_list = uwb_timeout_list};
- ret.list = llist_create(tx_node_compare, tx_node_equal, 0);
- ret.blink_frequency = blink_frequency;
- ret.blink_duration = blink_duration;
- ret.range_duration = range_duration;
+static struct TDMAScheduler new(float blink_frequency, uint8 (*uwb_list)[][8], uint8 *uwb_list_length, uint8 uwb_max_list_length, uint8 min_framelength /*uint8 uwb_timeout_list[]*/){
+ struct TDMAScheduler ret = {};
ret.blink_period = (int)(1.0/blink_frequency);
- ret.tx_select = &tx_select;
+ ret.uwb_list = uwb_list;
+ ret.uwb_list_length = uwb_list_length;
+ ret.uwb_max_list_length = uwb_max_list_length;
+// ret.uwb_timeout_list = uwb_timeout_list; //might need uwbliststate pointer here
+
+ ret.list = llist_create(tx_node_compare, tx_node_equal, 0);
+
ret.add_node = &add_node;
- ret.last_blink_time = 0;
- ret.last_select_index = 0;
- ret.time_reject_select = 0;
- ret.poll_frequency = .05;
- ret.poll_frequency_period = (uint32)(1.0/ret.poll_frequency);
+ ret.set_slot = &set_slot;
+
+ time_now = portGetTickCnt();
+
+ ret.last_blink_time = time_now;
+ ret.framelength = min_framelength;
+ ret.maxFramelength = min_framelength;
+ while(ret.maxFramelength < ret.uwb_max_list_length + 1)
+ {
+ ret.maxFramelength *= 2;
+ }
+ ret.slotDuration = 30; //TODO use a #define or something?
+
+ ret.slotAssignments = malloc(sizeof(uint16)*ret.maxFramelength);
+ for(int i = 0; i < inst->maxFramelength; i++)
+ {
+ uint16 blank = 0x0000;
+ memcpy(&ret.slotAssignments[i], &blank, 2);
+ }
+
+ ret.uwbFramelengths = malloc(sizeof(uint8)*ret.maxFramelength);
+ for(int i = 0; i < ret.uwb_max_list_length; i++)
+ {
+ ret.uwbFramelengths[i] = 0;
+ }
+
+ ret.frameStartTime = time_now;
+ ret.lastSlotStartTime = time_now;
+ ret.discoveryStartTime = time_now;
+ ret.last_blink_time = time_now;
+
return ret;
}
-const struct TXSchedulerClass TXScheduler={.new=&new};
+const struct TDMASchedulerClass TDMAScheduler={.new=&new};
diff --git a/src/application/tx_scheduler.h b/src/application/tx_scheduler.h
index 9d32a3a..2864637 100644
--- a/src/application/tx_scheduler.h
+++ b/src/application/tx_scheduler.h
@@ -1,5 +1,9 @@
+#ifndef TDMA_SCHEDULER_H_
+#define TDMA_SCHEDULER_H_
+
#include "llist.h"
#include "deca_types.h"
+#include "instance.h"
typedef struct
{
@@ -7,49 +11,50 @@ typedef struct
uint32 duration; //expected time the node event will take before completion
uint8 index; //the node index
-} tx_timing_node;
+} tdma_timing_node;
-struct TXScheduler
+struct TDMAScheduler
{
- llist list; //list of tx_timing_nodes
-
-
-
-
+ llist list; //list of tdma_timing_nodes
+ //instance variables
uint8 uwb_max_list_length;
uint8 *uwb_list_length;
uint8 (*uwb_list)[][8]; //pointer to uwbList
- uint8 *uwb_timeout_list; //pointer to uwbTimeout
+// uint8 *uwb_timeout_list; //pointer to uwbTimeout
+ //might need uwbliststate pointer here
- float blink_frequency;
- uint32 blink_period;
- uint32 blink_duration;
- uint32 range_duration;
+ //TDMA class variables
+ uint8 framelength;
+ uint8 maxFramelength;
+ uint16 *slotAssignments; //TODO may need two slot assignments!
+ uint8 *uwbFramelengths; //[UWB_LIST_SIZE]
+ uint32 frameStartTime; //TODO how to sync
+ uint32 lastSlotStartTime;
+ uint32 slotDuration; //TODO 30 ms?
+ uint32 discoveryStartTime; //time that we started listening for other UWBs
- float poll_frequency;
- uint32 poll_frequency_period;
+ //discovery variables
+ uint32 blink_period;
+ uint32 last_blink_time; //timestamp of most recent blink
- uint32 last_blink_time; //timestamp of most recent blink
-// uint32 accepted_node_end_time;
- uint8 last_select_index; //index of most recently selected uwb
- uint32 expected_tx_end_time;
- uint32 time_reject_select;
- int (*tx_select)(struct TXScheduler *this);
- bool (*add_node)(struct TXScheduler *this, tx_timing_node *node);
+ //class functions
+ void (*set_slot)(struct TDMAScheduler *this);
+ bool (*add_node)(struct TDMAScheduler *this, tdma_timing_node *node);
};
-extern const struct TXSchedulerClass
+extern const struct TDMASchedulerClass
{
- struct TXScheduler (*new)(float blink_frequency, uint32 blink_duration, uint32 range_duration, uint8 (*uwb_list)[][8], uint8 *uwb_list_length, uint8 uwb_max_list_length, uint8 uwb_timeout_list[]);
+ struct TDMAScheduler (*new)(float blink_frequency, uint32 blink_duration, uint32 range_duration, uint8 (*uwb_list)[][8], uint8 *uwb_list_length, uint8 uwb_max_list_length, uint8 uwb_timeout_list[]);
-} TXScheduler;
+} TDMAScheduler;
+#endif
diff --git a/src/decadriver/deca_device.c b/src/decadriver/deca_device.c
index eb85741..a146ec9 100644
--- a/src/decadriver/deca_device.c
+++ b/src/decadriver/deca_device.c
@@ -2128,9 +2128,17 @@ uint8 dwt_checkirq(void)
*/
void dwt_isr(void)
{
+
+
uint32 status = dw1000local.cbData.status = dwt_read32bitreg(SYS_STATUS_ID); // Read status register low 32bits
- uint8 status_high = dwt_read8bitoffsetreg(SYS_STATUS_ID, 4);
- uint32 mask = dwt_read32bitreg(SYS_MASK_ID);
+// uint8 status_high = dwt_read8bitoffsetreg(SYS_STATUS_ID, 4);
+// uint32 mask = dwt_read32bitreg(SYS_MASK_ID);
+
+// uint8 debug_msg[50];
+// int n = sprintf((char*)&debug_msg[0], "dwt_isr SYS_STATUS: %lu", status);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+
uint8 callback_triggered = 0;
@@ -2186,7 +2194,7 @@ void dwt_isr(void)
if (dw1000local.dblbuffon)
{
// Toggle the Host side Receive Buffer Pointer
-// dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_HRBT_OFFSET, 1);
+ dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_HRBT_OFFSET, 1);
}
}
@@ -2220,19 +2228,19 @@ void dwt_isr(void)
{
callback_triggered = 1;
- uint32 and_status = status & SYS_STATUS_ALL_RX_TO;
- uint8 debug_msg[100];
- int n = 0;
- n = sprintf((char*)&debug_msg[0], "rx_timeout!!! and_status: %lu", and_status);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint32 and_status = status & SYS_STATUS_ALL_RX_TO;
+// uint8 debug_msg[100];
+// int n = 0;
+// n = sprintf((char*)&debug_msg[0], "rx_timeout!!! and_status: %lu", and_status);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
- if(and_status & SYS_STATUS_AFFREJ){
- uint32 sys_cfg_reg = dwt_read32bitreg(SYS_CFG_ID);
- n = sprintf((char*)&debug_msg[0], "rx_timeout!!! sys_cfg_reg: %lu", sys_cfg_reg);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
- }
+// if(and_status & SYS_STATUS_AFFREJ){
+// uint32 sys_cfg_reg = dwt_read32bitreg(SYS_CFG_ID);
+// n = sprintf((char*)&debug_msg[0], "rx_timeout!!! sys_cfg_reg: %lu", sys_cfg_reg);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+// }
@@ -2258,17 +2266,17 @@ void dwt_isr(void)
{
callback_triggered = 1;
- uint32 and_status = status & SYS_STATUS_ALL_RX_ERR;
- uint8 debug_msg[100];
- int n = 0;
- n = sprintf((char*)&debug_msg[0], "rx_error!!! and_status: %lu", and_status);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
-
- n = sprintf((char*)&debug_msg[0], "rx_error!!! sys_status: %lu", status);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
-
+// uint32 and_status = status & SYS_STATUS_ALL_RX_ERR;
+// uint8 debug_msg[100];
+// int n = 0;
+// n = sprintf((char*)&debug_msg[0], "rx_error!!! and_status: %lu", and_status);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+//
+// n = sprintf((char*)&debug_msg[0], "rx_error!!! sys_status: %lu", status);
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+//
// if(and_status & SYS_STATUS_AFFREJ){
// uint32 sys_cfg_reg = dwt_read32bitreg(SYS_CFG_ID);
// n = sprintf((char*)&debug_msg[0], "rx_error!!! sys_cfg_reg: %lu", sys_cfg_reg);
@@ -2300,10 +2308,10 @@ void dwt_isr(void)
uint32 SYS_STATUS_ALL_INT = 1073217534; //TODO cleanup
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_INT);
- uint8 debug_msg[50];
- int n = sprintf((char*)&debug_msg[0], "unsubscribed callback");
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+// uint8 debug_msg[50];
+// int n = sprintf((char*)&debug_msg[0], "unsubscribed callback");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
}
@@ -2712,23 +2720,23 @@ void dwt_forcetrxoff(void)
uint32 new_mask = dwt_read32bitreg(SYS_MASK_ID);
new_mask &= SYS_MASK_VAL;
- uint8 mismatch = 0;
+// uint8 mismatch = 0;
// while(new_mask != SYS_MASK_VAL){
- if(new_mask != SYS_MASK_VAL){
-// dwt_write32bitreg(SYS_MASK_ID, mask);
-// new_mask = dwt_read32bitreg(SYS_MASK_ID);
-// new_mask &= SYS_MASK_VAL;
-
- mismatch = 1;
- }
+// if(new_mask != SYS_MASK_VAL){
+//// dwt_write32bitreg(SYS_MASK_ID, mask);
+//// new_mask = dwt_read32bitreg(SYS_MASK_ID);
+//// new_mask &= SYS_MASK_VAL;
+//
+// mismatch = 1;
+// }
- if(mismatch == 1){
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "mismatch occurred!");
- send_usbmessage(&debug_msg[0], n);
- usb_run();
- }
+// if(mismatch == 1){
+// uint8 debug_msg[100];
+// int n = sprintf((char*)&debug_msg[0], "mismatch occurred!");
+// send_usbmessage(&debug_msg[0], n);
+// usb_run();
+// }
// if(new_mask != mask){
// uint8 mismatch = 1;
diff --git a/src/liblist/inc/llist.h b/src/liblist/inc/llist.h
deleted file mode 100644
index f5163b0..0000000
--- a/src/liblist/inc/llist.h
+++ /dev/null
@@ -1,267 +0,0 @@
-/*
- * Copyright [2013] [Ramon Fried] <ramon.fried at gmail dot com>
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
- */
-
-#ifndef LLIST_H_
-#define LLIST_H_
-
-#include <stdbool.h>
-
-/*
- * E_LLIST
- * This is the return values most of the llist API return,
- * don't forget to check for success :)
- */
-typedef enum {
- LLIST_SUCCESS = 0x00, /**< Operating success */
- LLIST_NODE_NOT_FOUND, /**< Error: No matching node found*/
- LLIST_EQUAL_MISSING, /**< Error: Equal function is missing*/
- LLIST_COMPERATOR_MISSING, /**< Error: Comparator function is missing*/
- LLIST_NULL_ARGUMENT, /**< Error: NULL argument*/
- LLIST_MALLOC_ERROR, /**< Error: Memory allocation error*/
- LLIST_NOT_IMPLEMENTED, /**< Error: Implementation missing*/
- LLIST_MULTITHREAD_ISSUE, /**< Error: Multithreading issue*/
- LLIST_ERROR /**< Error: Generic error*/
-} E_LLIST;
-
-#define ADD_NODE_FRONT (1 << 0)
-#define ADD_NODE_REAR ~ADD_NODE_FRONT
-
-#define ADD_NODE_BEFORE (1 << 0)
-#define ADD_NODE_AFTER ~ADD_NODE_BEFORE
-
-#define SORT_LIST_ASCENDING (1 << 0)
-#define SORT_LIST_DESCENDING ~SORT_LIST_ASCENDING
-
-#define FLAG_MT_SUPPORT (1 << 0)
-
-typedef void *llist;
-typedef void *llist_node;
-
-// function prototypes
-typedef void (*node_func)(llist_node node);
-
-// function prototypes with user arguments
-typedef void (*node_func_arg)(llist_node node, void *arg);
-
-/**
-* @brief Compares two nodes in a list
-* @param[in] first llist_node
-* @param[in] second llist_node
-* @return an integer less than, equal to, or greater than zero if first,
-* respectively, to be less than, to match, or be greater than second.
-*/
-typedef int (*comperator)(llist_node first, llist_node second);
-
-/**
-* @brief Check if two nodes are equal
-* @param[in] first llist_node
-* @param[in] second llist_node
-* @return true if the nodes are equal, false otherwise
-*/
-typedef bool (*equal)(llist_node, llist_node);
-
-#define LLIST_INITALIZER {0, NULL, NULL, NULL, NULL}
-
-/**
- * @brief Create a list
- * @param[in] compare_func a function used to compare elements in the list
- * @param[in] equal_func a function used to check if two elements are equal
- * @param[in] flags used to identify whether we create a thread safe linked-list
- * @return new list if success, NULL on error
- */
-llist llist_create(comperator compare_func, equal equal_func,
- unsigned int flags);
-
-/**
- * @brief Destroys a list
- * @warning Call this function only if the list was created with llist_create
- * Static initializer created list cannot be destroyed using this function
- * @param[in] list The list to destroy
- * @param[in] destroy_nodes true if the nodes should be destroyed, false if not
- * @param[in] destructor alternative destructor, if the previous param is true,
- * if NULL is provided standard library c free() will be used
- */
-void llist_destroy(llist list, bool destroy_nodes, node_func destructor);
-
-/**
- * @brief Add a node to a list
- * @param[in] list the list to operator upon
- * @param[in] node the node to add
- * @param[in] flags flags
- * @return int LLIST_SUCCESS if success
- */
-int llist_add_node(llist list, llist_node node, int flags);
-
-/**
- * @brief Insert a node at a specific location
- * @param[in] list the list to operator upon
- * @param[in] new_node the node to add
- * @param[in] pos_node a position reference node
- * @param[in] flags flags
- * @return int LLIST_SUCCESS if success
- */
-int llist_insert_node(llist list, llist_node new_node, llist_node pos_node,
- int flags);
-
-/**
- * @brief Delete a node from a list
- * @param[in] list the list to operator upon
- * @param[in] node the node to delete
- * @param[in] destroy_node Should we run a destructor
- * @param[in] destructor function, if NULL is provided, free() will be used
- * @return int LLIST_SUCCESS if success
- */
-int llist_delete_node(llist list, llist_node node, bool destroy_node,
- node_func destructor);
-
-/**
- * @brief Finds a node in a list
- * @param[in] list the list to operator upon
- * @param[in] data the data to find
- * @param[out] found a pointer for found node.
- * this pointer can be used only if
- * llist_find_node returned LLIST_SUCCESS
- * @return LLIST_SUCCESS if success
- */
-int llist_find_node(llist list, void *data, llist_node *found);
-
-/**
- * @brief operate on each element of the list
- * @param[in] list the list to operator upon
- * @param[in] func the function to perform
- * @return int LLIST_SUCCESS if success
- */
-int llist_for_each(llist list, node_func func);
-
-/**
- * @brief operate on each element of the list
- * @param[in] list the list to operator upon
- * @param[in] func the function to perform
- * @param[in] arg passed to func
- * @return int LLIST_SUCCESS if success
- */
-int llist_for_each_arg(llist list, node_func_arg func, void *arg);
-/**
- * @brief sort a lists
- * @param[in] list the list to operator upon
- * @param[in] flags
- * @return int LLIST_SUCCESS if success
- */
-int llist_sort(llist list, int flags);
-
-/**
- * @brief Returns the head node of the list
- * @param[in] list the list to operate on
- * @return the head node, NULL on error
- */
-llist_node llist_get_head(llist list);
-
-/**
- * @brief Returns the next node of the list
- * @param[in] list the list to operate on
- * @param[in] node the node to start from
- * @return the next node, NULL on error
- */
-llist_node llist_get_next(llist list, llist_node node);
-
-/**
- * @brief Returns the tail node of the list
- * @param[in] list the list to operate on
- * @return the tail node, NULL on error
- */
-llist_node llist_get_tail(llist list);
-
-/**
- * @brief push a node to the head of the list
- * @param[in] list the list to operate on
- * @param[in] node the node to push
- * @return int LLIST_SUCCESS if success
- */
-int llist_push(llist list, llist_node node);
-
-/**
- * @brief peek at the head of the list
- * @param[in] list the list to operate on
- * @return llist_node the head node
- */
-llist_node llist_peek(llist list);
-
-/**
- * @brief pop the head of the list
- * @param[in] list the list to operate on
- * @return llist_node the head node
- */
-llist_node llist_pop(llist list);
-
-/**
- * @brief return the number of elements in the list
- * @param[in] list the list to operate on
- * @return int number of elements in the list or -1 if error
- */
-int llist_size(llist list);
-
-/**
- * @brief concatenate the second list to the first list
- * @param[in] first the list to operate on
- * @param[in] second the list to operate on.
- * @warning The nodes from the second list will be deleted and concatenated to the first list
- * Remember to call llist_destroy() on the second list (if it was created by llist_create())
- * @return int LLIST_SUCCESS if success
- */
-int llist_concat(llist first, llist second);
-
-/**
- * @brief merge the second list to the first list
- * @param[in] first the list to operate on
- * @param[in] second the list to operate on
- * @warning The nodes from the second list will be deleted and merged to the first list
- * Remember to call llist_destroy() on the second list (if it was created by llist_create())
- * @return int LLIST_SUCCESS if success
- */
-int llist_merge(llist first, llist second);
-
-/**
- * @brief get the maximum node in a given list
- * @param[in] list the list to operate upon
- * @param[out] max maximum node
- * @return int LLIST_SUCCESS if success
- */
-int llist_get_max(llist list, llist_node *max);
-
-/**
- * @brief get the minimum node in a given list
- * @param[in] list the list to operate upon
- * @param[out] min minumum node
- * @return int LLIST_SUCCESS if success
- */
-int llist_get_min(llist list, llist_node *min);
-
-/**
- * @brief Reverse a list
- * @param[in] list the list to operate upon
- * @return int LLIST_SUCCESS if success
- */
-int llist_reverse(llist list);
-
-/**
- * @brief check if list is empty
- * @param[in] list the list to operate upon
- * @return bool True if list is empty
- */
-bool llist_is_empty(llist list);
-
-#endif /* LLIST_H_ */
diff --git a/src/liblist/src/Complex.c b/src/liblist/src/Complex.c
deleted file mode 100644
index 45bbec4..0000000
--- a/src/liblist/src/Complex.c
+++ /dev/null
@@ -1,9 +0,0 @@
-#include "Complex.h"
-
-static double abss(struct Complex *this) {
- return this->re*this->re+this->im*this->im;
-}
-static struct Complex new(double real, double imag) {
- return (struct Complex){.re=real, .im=imag, .abss=&abss};
-}
-const struct ComplexClass Complex={.new=&new};
diff --git a/src/liblist/src/Complex.h b/src/liblist/src/Complex.h
deleted file mode 100644
index 802285d..0000000
--- a/src/liblist/src/Complex.h
+++ /dev/null
@@ -1,7 +0,0 @@
-struct Complex {
- double re, im;
- double (*abss)(struct Complex *this);
-};
-extern const struct ComplexClass {
- struct Complex (*new)(double real, double imag);
-} Complex;
diff --git a/src/liblist/src/llist.c b/src/liblist/src/llist.c
deleted file mode 100644
index fd2382f..0000000
--- a/src/liblist/src/llist.c
+++ /dev/null
@@ -1,676 +0,0 @@
-/*
- * Copyright [2013] [Ramon Fried] <ramon.fried at gmail dot com>
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
- */
-
-#include "../inc/llist.h"
-#include <stdlib.h>
-#include <stdio.h>
-#include <assert.h>
-#include <pthread.h>
-
-#define LOG_FUNC_ENTRANCE() printf("%lu: In %s\n", time(NULL), __PRETTY_FUNCTION__);
-
-typedef struct __list_node {
- llist_node node;
- struct __list_node *next;
-} _list_node;
-
-typedef struct {
- unsigned int count;
- comperator comp_func;
- equal equal_func;
- _list_node *head;
- _list_node *tail;
-
-} _llist;
-
-
-/* Helper functions - not to be exported */
-static _list_node *listsort(_list_node *list, _list_node **updated_tail,
- comperator cmp, int flags);
-
-llist llist_create(comperator compare_func, equal equal_func, unsigned int flags)
-{
- _llist *new_list;
-// int rc = 0;
-
- new_list = malloc(sizeof(_llist));
-
- if (new_list == NULL)
- return NULL;
-
-// These can be NULL, I don't care...
- new_list->equal_func = equal_func;
- new_list->comp_func = compare_func;
-
-// Reset the list
- new_list->count = 0;
- new_list->head = NULL;
- new_list->tail = NULL;
-
- return new_list;
-}
-
-void llist_destroy(llist list, bool destroy_nodes, node_func destructor)
-{
- _list_node *iterator;
- _list_node *next;
-
- if (list == NULL)
- return;
-
- // Delete the data contained in the nodes
- iterator = ((_llist *) list)->head;
-
- while (iterator != NULL) {
-
- if (destroy_nodes) {
- if (destructor)
- destructor(iterator->node);
- else
- free(iterator->node);
- }
-
- next = iterator->next;
- free(iterator); // Delete's the container
- iterator = next;
- }
-
- //release the list
- free(list);
-}
-
-int llist_size(llist list)
-{
- if (list == NULL)
- return 0;
-
- //read only critical section
- unsigned int retval = ((_llist *) list)->count;
-
- return retval;
-}
-
-int llist_add_node(llist list, llist_node node, int flags)
-{
-
- if (list == NULL)
- return LLIST_NULL_ARGUMENT;
-
- _list_node *node_wrapper = NULL;
-
- node_wrapper = malloc(sizeof(_list_node));
- if (node_wrapper == NULL)
- return LLIST_MALLOC_ERROR;
-
- node_wrapper->node = node;
- ((_llist *) list)->count++;
-
- if (((_llist *) list)->head == NULL) { // Adding the first node, update head and tail to point to that node
- node_wrapper->next = NULL;
- ((_llist *) list)->head =
- ((_llist *) list)->tail = node_wrapper;
- } else if (flags & ADD_NODE_FRONT) {
- node_wrapper->next = ((_llist *) list)->head;
- ((_llist *) list)->head = node_wrapper;
- } else { // add node in the rear
- node_wrapper->next = NULL;
- ((_llist *) list)->tail->next = node_wrapper;
- ((_llist *) list)->tail = node_wrapper;
- }
-
-
- return LLIST_SUCCESS;
-}
-
-int llist_delete_node(llist list, llist_node node,
- bool destroy_node, node_func destructor)
-{
- _list_node *iterator;
- _list_node *tail;
- _list_node *temp;
- equal actual_equal;
-
- if ((list == NULL) || (node == NULL))
- return LLIST_NULL_ARGUMENT;
-
- actual_equal = ((_llist *) list)->equal_func;
-
- if (actual_equal == NULL)
- return LLIST_EQUAL_MISSING;
-
- iterator = ((_llist *) list)->head;
- tail = ((_llist *) list)->tail;
-
-
- if (iterator == NULL) {
- return LLIST_NODE_NOT_FOUND;
- }
-
- // is it the first node ?
- if (actual_equal(iterator->node, node)) {
- if (destroy_node) {
- if (destructor)
- destructor(iterator->node);
- else
- free(iterator->node);
- }
-
- ((_llist *) list)->head = ((_llist *) list)->head->next;
- free(iterator);
- ((_llist *) list)->count--;
-
- if (((_llist *) list)->count == 0) {
- /*
- * if we deleted the last node, we need to reset the tail also
- * There's no need to check it somewhere else, because the last node must be the head (and tail)
- */
- ((_llist *) list)->tail = NULL;
- ((_llist *) list)->head = NULL;
- }
- //assert ( ( ( _llist * ) list )->count >= 0 );
- return LLIST_SUCCESS;
- }else if(actual_equal(tail->node, node)){ //is it the last node?
- while(iterator->next != tail){
- iterator = iterator->next;
- }
- //iterator now second to last.
- ((_llist *) list)->tail = iterator;
- iterator->next = NULL;
- if (destroy_node) {
- if (destructor)
- destructor(tail->node);
- else
- free(tail->node);
- }
-
- free(tail);
- ((_llist *) list)->count--;
-
- return LLIST_SUCCESS;
- } else {
- while (iterator->next != NULL) {
- if (actual_equal(iterator->next->node, node)) {
- // found it
- temp = iterator->next;
- iterator->next = temp->next;
- if (destroy_node) {
- if (destructor)
- destructor(temp->node);
- else
- free(temp->node);
- }
- free(temp);
-
- ((_llist *) list)->count--;
- //assert ( ( ( _llist * ) list )->count >= 0 );
-
- return LLIST_SUCCESS;
- }
-
- iterator = iterator->next;
-
- }
- }
-
- if (iterator->next == NULL) {
- return LLIST_NODE_NOT_FOUND;
- }
-
- //assert ( 1 == 2 );
- // this assert always failed. we assume that the function never gets here...
-
- return LLIST_ERROR;
-}
-
-int llist_for_each(llist list, node_func func)
-{
- _list_node *iterator;
-
- if ((list == NULL) || (func == NULL))
- return LLIST_NULL_ARGUMENT;
-
- iterator = ((_llist *) list)->head;
-
- while (iterator != NULL) {
- func(iterator->node);
- iterator = iterator->next;
- }
-
- return LLIST_SUCCESS;
-}
-
-int llist_for_each_arg(llist list, node_func_arg func, void *arg)
-{
- _list_node *iterator;
-
- if ((list == NULL) || (func == NULL))
- return LLIST_NULL_ARGUMENT;
-
- iterator = ((_llist *) list)->head;
-
- while (iterator != NULL) {
- func(iterator->node, arg);
- iterator = iterator->next;
- }
-
- return LLIST_SUCCESS;
-}
-
-int llist_insert_node(llist list, llist_node new_node, llist_node pos_node,
- int flags)
-{
- _list_node *iterator;
- _list_node *node_wrapper = NULL;
-
- if ((list == NULL) || (new_node == NULL) || (pos_node == NULL))
- return LLIST_NULL_ARGUMENT;
-
- node_wrapper = malloc(sizeof(_list_node));
-
- if (node_wrapper == NULL) {
- return LLIST_MALLOC_ERROR;
- }
-
- node_wrapper->node = new_node;
-
- ((_llist *) list)->count++;
-
- iterator = ((_llist *) list)->head;
-
- if (iterator->node == pos_node) {
- // it's the first node
-
- if (flags & ADD_NODE_BEFORE) {
- node_wrapper->next = iterator;
- ((_llist *) list)->head = node_wrapper;
- } else {
- node_wrapper->next = iterator->next;
- iterator->next = node_wrapper;
- }
-
- //TODO update tail?
-
- return LLIST_SUCCESS;
- }
-
- while (iterator->next != NULL) {
- if (iterator->next->node == pos_node) {
- if (flags & ADD_NODE_BEFORE) {
- node_wrapper->next = iterator->next;
- iterator->next = node_wrapper;
- } else {
- iterator = iterator->next;
- // now we stand on the pos node
- node_wrapper->next = iterator->next;
- iterator->next = node_wrapper;
- }
- return LLIST_SUCCESS;
- }
-
- iterator = iterator->next;
- }
-
-
-// assert(1 == 2);
- // this assert will always fail. we assume that the function never gets here...
- return LLIST_ERROR;
-
-}
-
-int llist_find_node(llist list, void *data, llist_node *found)
-{
- _list_node *iterator;
- equal actual_equal;
-
- if (list == NULL)
- return LLIST_NULL_ARGUMENT;
-
- actual_equal = ((_llist *) list)->equal_func;
-
- if (actual_equal == NULL) {
- return LLIST_EQUAL_MISSING;
- }
-
-// read_lock(list);
-
- iterator = ((_llist *) list)->head;
- while (iterator != NULL) {
- if (actual_equal(iterator->node, data)) {
- *found = iterator->node;
-// unlock(list);
- return LLIST_SUCCESS;
- }
- iterator = iterator->next;
- }
-
-// unlock(list);
-
- // Didn't find the node
- return LLIST_NODE_NOT_FOUND;
-}
-
-llist_node llist_get_head(llist list)
-{
-
- if (list != NULL) {
- if (((_llist *) list)->head) { // there's at least one node
- return ((_llist *) list)->head->node;
- }
- }
-
- return NULL;
-}
-
- llist_node llist_get_next(llist list, llist_node node)
- {
- _list_node *iterator;
- equal actual_equal;
-
- if ((list == NULL) || (node == NULL))
- return NULL;
-
- iterator = ((_llist *) list)->head;
- actual_equal = ((_llist *) list)->equal_func;
-
- if (actual_equal == NULL)
- return NULL;
-
- while(iterator->next != NULL)
- {
- if (actual_equal(iterator->node, node)) {
- return iterator->next->node;
- }
-
- iterator = iterator->next;
- }
-
- return NULL;
- }
-
-llist_node llist_get_tail(llist list)
-{
- if (list != NULL)
- return NULL;
-
- if (((_llist *) list)->tail) { // there's at least one node
- return ((_llist *) list)->tail->node;
- }
-
- return NULL;
-}
-
-int llist_push(llist list, llist_node node)
-{
- return llist_add_node(list, node, ADD_NODE_FRONT);
-}
-
-llist_node llist_peek(llist list)
-{
- return llist_get_head(list);
-}
-
-llist_node llist_pop(llist list)
-{
- llist_node tempnode = NULL;
- _list_node *tempwrapper;
-
-// write_lock(list);
-
- if (((_llist *) list)->count) { // There exists at least one node
- tempwrapper = ((_llist *) list)->head;
- tempnode = tempwrapper->node;
- ((_llist *) list)->head = ((_llist *) list)->head->next;
- ((_llist *) list)->count--;
- free(tempwrapper);
-
- if (((_llist *) list)->count == 0) // We've deleted the last node
- ((_llist *) list)->tail = NULL;
- }
-
-// unlock(list);
-
- return tempnode;
-}
-
-int llist_concat(llist first, llist second)
-{
- _list_node *end_node;
-
- if ((first == NULL) || (second == NULL))
- return LLIST_NULL_ARGUMENT;
-
-// write_lock(first);
-// write_lock(second);
-
- end_node = ((_llist *) first)->tail;
-
- ((_llist *) first)->count += ((_llist *) second)->count;
-
- if (end_node != NULL) { // if the first list is not empty
- end_node->next = ((_llist *) second)->head;
- } else { // It's empty
- ((_llist *) first)->head = ((_llist *) first)->tail =
- ((_llist *) second)->head;
- }
-
- // Delete the nodes from the second list. (not really deletes them, only loses their reference.
- ((_llist *) second)->count = 0;
- ((_llist *) second)->head = ((_llist *) second)->tail = NULL;
-
-// unlock(first);
-// unlock(second);
-
- return LLIST_SUCCESS;
-}
-
-int llist_reverse(llist list)
-{
- if (list == NULL)
- return LLIST_NULL_ARGUMENT;
-
-// write_lock(list);
-
- _list_node *iterator = ((_llist *) list)->head;
- _list_node *nextnode = NULL;
- _list_node *temp = NULL;
-
- /*
- * Swap our Head & Tail pointers
- */
- ((_llist *) list)->head = ((_llist *) list)->tail;
- ((_llist *) list)->tail = iterator;
-
- /*
- * Swap the internals
- */
- while (iterator) {
- nextnode = iterator->next;
- iterator->next = temp;
- temp = iterator;
- iterator = nextnode;
- }
-
-// unlock(list);
-
- return LLIST_SUCCESS;
-}
-
-int llist_sort(llist list, int flags)
-{
-
- comperator cmp;
-
- if (list == NULL)
- return LLIST_NULL_ARGUMENT;
-
- _llist *thelist = (_llist *) list;
-
- cmp = thelist->comp_func;
- if (cmp == NULL)
- return LLIST_COMPERATOR_MISSING;
-
-// write_lock(list);
- thelist->head = listsort(thelist->head, &thelist->tail, cmp, flags);
-// unlock(list);
- /*
- * TODO: update list tail.
- */
- return LLIST_SUCCESS;
-}
-
-static _list_node *listsort(_list_node *list, _list_node **updated_tail,
- comperator cmp, int flags)
-{
- _list_node *p, *q, *e, *tail;
- int insize, nmerges, psize, qsize, i;
- int direction = (flags & SORT_LIST_ASCENDING) ? 1 : -1;
-
- insize = 1;
-
- while (1) {
- p = list;
- list = NULL;
- tail = NULL;
-
- nmerges = 0; /* count number of merges we do in this pass */
-
- while (p) {
- nmerges++; /* there exists a merge to be done */
- /* step `insize' places along from p */
- q = p;
- psize = 0;
- for (i = 0; i < insize; i++) {
- psize++;
- q = q->next;
- if (!q) {
- break;
- }
- }
-
- /* if q hasn't fallen off end, we have two lists to merge */
- qsize = insize;
-
- /* now we have two lists; merge them */
- while (psize > 0 || (qsize > 0 && q)) {
-
- /* decide whether next element of merge comes from p or q */
- if (psize == 0) {
- /* p is empty; e must come from q. */
- e = q;
- q = q->next;
- qsize--;
- } else if (qsize == 0 || !q) {
- /* q is empty; e must come from p. */
- e = p;
- p = p->next;
- psize--;
- } else if ((direction * cmp(p->node, q->node)) <= 0) {
- /* First element of p is lower (or same);
- * e must come from p. */
- e = p;
- p = p->next;
- psize--;
- } else {
- /* First element of q is lower; e must come from q. */
- e = q;
- q = q->next;
- qsize--;
- }
-
- /* add the next element to the merged list */
- if (tail) {
- tail->next = e;
- } else {
- list = e;
- }
-
- tail = e;
- }
-
- /* now p has stepped `insize' places along, and q has too */
- p = q;
- }
-
- tail->next = NULL;
-
- /* If we have done only one merge, we're finished. */
- if (nmerges <= 1) { /* allow for nmerges==0, the empty list case */
- break;
- }
- /* Otherwise repeat, merging lists twice the size */
- insize *= 2;
- }
-
- *updated_tail = tail;
- return list;
-}
-
-static int llist_get_min_max(llist list, llist_node *output, bool max)
-{
- comperator cmp;
-
- if (list == NULL)
- return LLIST_NULL_ARGUMENT;
-
- cmp = ((_llist *) list)->comp_func;
-
- if (cmp == NULL)
- return LLIST_COMPERATOR_MISSING;
-
- _list_node *iterator = ((_llist *) list)->head;
- *output = iterator->node;
- iterator = iterator->next;
- while (iterator) {
- if (max) { // Find maximum
- if (cmp(iterator->node, *output) > 0) {
- *output = iterator->node;
- }
- } else { // Find minimum
- if (cmp(iterator->node, *output) < 0) {
- *output = iterator->node;
- }
- }
- iterator = iterator->next;
- }
-
- return LLIST_SUCCESS;
-}
-
-int llist_get_max(llist list, llist_node *max)
-{
- return llist_get_min_max(list, max, true);
-}
-
-int llist_get_min(llist list, llist_node *min)
-{
- return llist_get_min_max(list, min, false);
-}
-
-bool llist_is_empty(llist list)
-{
- return (!llist_size(list));
-}
-
-/*
- * TODO: Implement the below functions
- */
-
-int llist_merge(llist first, llist second)
-{
- assert(1 == 0); // Fail, function not implemented yet.
- return LLIST_NOT_IMPLEMENTED;
-}
diff --git a/src/usb/deca_usb.c b/src/usb/deca_usb.c
index efb9c48..7c082fc 100644
--- a/src/usb/deca_usb.c
+++ b/src/usb/deca_usb.c
@@ -57,7 +57,7 @@ int local_have_data = 0;
int version_size;
uint8* version;
int s1configswitch;
-extern int ranging;
+//extern int ranging;
extern uint32_t APP_Rx_length;
extern uint32 inittestapplication(uint8 s1switch);
@@ -383,7 +383,7 @@ int process_usbmessage(void)
//enable DW1000 IRQ
port_EnableEXT_IRQ(); //enable IRQ before starting
- ranging = 0;
+// ranging = 0;
}
//d (from "deca")
--
GitLab