From 5657234be9b545362a0b0c1fc6e3d66166187611 Mon Sep 17 00:00:00 2001
From: David <stierint@hotmail.com>
Date: Tue, 18 Feb 2020 13:56:57 -0600
Subject: [PATCH] Removed printf (too slow). Implemented short addresses.
Renamed Variables
---
DecaRanging.coproj | 21 +-
Libraries/CMSIS/CM3/CoreSupport/core_cm3.h | 1 +
semihosting/semihosting.c | 72 --
semihosting/semihosting.h | 25 -
semihosting/sh_cmd.s | 77 --
src/application/dw_main.c | 100 +-
src/application/instance.c | 1248 ++++++++------------
src/application/instance.h | 68 +-
src/application/instance_common.c | 618 ++++------
src/decadriver/deca_device.c | 11 +-
src/platform/port.c | 1 +
stdio/printf.c | 559 ---------
12 files changed, 775 insertions(+), 2026 deletions(-)
delete mode 100644 semihosting/semihosting.c
delete mode 100644 semihosting/semihosting.h
delete mode 100644 semihosting/sh_cmd.s
delete mode 100644 stdio/printf.c
diff --git a/DecaRanging.coproj b/DecaRanging.coproj
index daa3c74..4f99c98 100644
--- a/DecaRanging.coproj
+++ b/DecaRanging.coproj
@@ -68,28 +68,25 @@
<DebugOption>
<Option name="org.coocox.codebugger.gdbjtag.core.adapter" value="ST-Link"/>
<Option name="org.coocox.codebugger.gdbjtag.core.debugMode" value="SWD"/>
- <Option name="org.coocox.codebugger.gdbjtag.core.clockDiv" value="2M"/>
+ <Option name="org.coocox.codebugger.gdbjtag.core.clockDiv" value="1M"/>
<Option name="org.coocox.codebugger.gdbjtag.corerunToMain" value="1"/>
<Option name="org.coocox.codebugger.gdbjtag.core.jlinkgdbserver" value=""/>
<Option name="org.coocox.codebugger.gdbjtag.core.userDefineGDBScript" value=""/>
<Option name="org.coocox.codebugger.gdbjtag.core.targetEndianess" value="0"/>
<Option name="org.coocox.codebugger.gdbjtag.core.jlinkResetMode" value="Type 0: Normal"/>
<Option name="org.coocox.codebugger.gdbjtag.core.resetMode" value="SYSRESETREQ"/>
- <Option name="org.coocox.codebugger.gdbjtag.core.ifSemihost" value="1"/>
+ <Option name="org.coocox.codebugger.gdbjtag.core.ifSemihost" value="0"/>
<Option name="org.coocox.codebugger.gdbjtag.core.ifCacheRom" value="1"/>
<Option name="org.coocox.codebugger.gdbjtag.core.ipAddress" value="127.0.0.1"/>
<Option name="org.coocox.codebugger.gdbjtag.core.portNumber" value="2009"/>
<Option name="org.coocox.codebugger.gdbjtag.core.autoDownload" value="1"/>
<Option name="org.coocox.codebugger.gdbjtag.core.verify" value="1"/>
<Option name="org.coocox.codebugger.gdbjtag.core.downloadFuction" value="Erase Effected"/>
- <Option name="org.coocox.codebugger.gdbjtag.core.defaultAlgorithm" value="./stm32f10x_cl_256.elf"/>
+ <Option name="org.coocox.codebugger.gdbjtag.core.defaultAlgorithm" value="STM32F10x_CL_256.elf"/>
</DebugOption>
<ExcludeFile/>
</Target>
- <Components path="./">
- <Component id="31" name="Retarget printf" path="" type="2"/>
- <Component id="33" name="Semihosting" path="" type="2"/>
- </Components>
+ <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="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_fsmc.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_fsmc.h" type="1"/>
@@ -121,7 +118,6 @@
<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="stdio/printf.c" path="stdio/printf.c" 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/platform/deca_mutex.c" path="src/platform/deca_mutex.c" type="1"/>
@@ -130,7 +126,6 @@
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_dma.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_dma.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_crc.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_crc.h" type="1"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_rcc.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_rcc.h" type="1"/>
- <File name="stdio" path="" type="2"/>
<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="Libraries/STM32_USB_OTG_Driver/src/usb_hcd_int.c" path="Libraries/STM32_USB_OTG_Driver/src/usb_hcd_int.c" type="1"/>
@@ -139,9 +134,9 @@
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_wwdg.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_wwdg.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/STM32F10x_StdPeriph_Driver/inc/stm32f10x_adc.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_adc.h" 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="Libraries/STM32_USB_OTG_Driver/inc/usb_otg.h" path="Libraries/STM32_USB_OTG_Driver/inc/usb_otg.h" type="1"/>
<File name="src/usb/usb_conf.h" path="src/usb/usb_conf.h" type="1"/>
@@ -174,12 +169,10 @@
<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="semihosting/sh_cmd.s" path="semihosting/sh_cmd.s" 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="semihosting/semihosting.c" path="semihosting/semihosting.c" type="1"/>
<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"/>
@@ -205,7 +198,6 @@
<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="semihosting" 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"/>
<File name="Libraries/CMSIS/CM3/CoreSupport/core_cm3.h" path="Libraries/CMSIS/CM3/CoreSupport/core_cm3.h" type="1"/>
@@ -229,7 +221,6 @@
<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"/>
<File name="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_pwr.h" path="Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_pwr.h" type="1"/>
- <File name="semihosting/semihosting.h" path="semihosting/semihosting.h" type="1"/>
<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"/>
diff --git a/Libraries/CMSIS/CM3/CoreSupport/core_cm3.h b/Libraries/CMSIS/CM3/CoreSupport/core_cm3.h
index 2b6b51a..7354dcb 100644
--- a/Libraries/CMSIS/CM3/CoreSupport/core_cm3.h
+++ b/Libraries/CMSIS/CM3/CoreSupport/core_cm3.h
@@ -1697,6 +1697,7 @@ static __INLINE uint32_t SysTick_Config(uint32_t ticks)
SysTick->LOAD = (ticks & SysTick_LOAD_RELOAD_Msk) - 1; /* set reload register */
NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1); /* set Priority for Cortex-M0 System Interrupts */
+ // NVIC_SetPriority (SysTick_IRQn, 1); /* set Priority for Cortex-M0 System Interrupts */
SysTick->VAL = 0; /* Load the SysTick Counter Value */
SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
SysTick_CTRL_TICKINT_Msk |
diff --git a/semihosting/semihosting.c b/semihosting/semihosting.c
deleted file mode 100644
index cb16156..0000000
--- a/semihosting/semihosting.c
+++ /dev/null
@@ -1,72 +0,0 @@
-/**
- ******************************************************************************
- * @file semihosting.c
- * @author Coocox
- * @version V1.0
- * @date 09/10/2011
- * @brief Semihosting LowLayer GetChar/SendChar Implement.
- *
- *******************************************************************************
- */
-
-#include <sys/types.h>
-#include <sys/stat.h>
-#include "semihosting.h"
-
-static char g_buf[16];
-static char g_buf_len = 0;
-
-/**************************************************************************//**
- * @brief Transmit a char on semihosting mode.
- *
- * @param ch is the char that to send.
- *
- * @return Character to write.
- *****************************************************************************/
-void SH_SendChar(int ch) {
- g_buf[g_buf_len++] = ch;
- g_buf[g_buf_len] = '\0';
- if (g_buf_len + 1 >= sizeof(g_buf) || ch == '\n' || ch == '\0') {
- g_buf_len = 0;
- /* Send the char */
- if (SH_DoCommand(0x04, (int) g_buf, NULL) != 0) {
- return;
- }
- }
-}
-
-/**************************************************************************//**
- * @brief Transmit a null-terminated string on semihosting mode.
- *
- * @param str is the string that to send.
- *
- * @return Character to write.
- *****************************************************************************/
-void SH_SendString(const char *str)
-{
- int j;
- if (SH_DoCommand(0x04, (int)str, NULL) != 0) {
- return;
- }
-}
-
-/**************************************************************************//**
- * @brief Read a char on semihosting mode.
- *
- * @param None.
- *
- * @return Character that have read.
- *****************************************************************************/
-char SH_GetChar() {
- int nRet;
-
- while (SH_DoCommand(0x101, 0, &nRet) != 0) {
- if (nRet != 0) {
- SH_DoCommand(0x07, 0, &nRet);
- return (char) nRet;
- }
- }
-
- return 0;
-}
-
diff --git a/semihosting/semihosting.h b/semihosting/semihosting.h
deleted file mode 100644
index caa55d1..0000000
--- a/semihosting/semihosting.h
+++ /dev/null
@@ -1,25 +0,0 @@
-/**
- ******************************************************************************
- * @file semihosting.h
- * @author Coocox
- * @version V1.0
- * @date 09/10/2011
- * @brief Semihosting Low Layer GetChar/SendChar API.
- *
- *******************************************************************************
- */
-#ifndef __SIMIHOSTTING_IMPL
-#define __SIMIHOSTTING_IMPL
-
-/********************************************************************************************************//**
- * Semihosting functions prototype
-************************************************************************************************************/
-extern int SH_DoCommand(int n32In_R0, int n32In_R1, int *pn32Out_R0);
-void SH_SendChar(int ch);
-void SH_SendString(const char *str);
-char SH_GetChar(void);
-
-
-#endif
-
-
diff --git a/semihosting/sh_cmd.s b/semihosting/sh_cmd.s
deleted file mode 100644
index f82b129..0000000
--- a/semihosting/sh_cmd.s
+++ /dev/null
@@ -1,77 +0,0 @@
-/**
- ******************************************************************************
- * @file semihost_cmd.s
- * @author Coocox
- * @version V1.0
- * @date 09/10/2011
- * @brief Semihost command support.
- *
- *******************************************************************************
- */
-
-
-.text
-.global SH_DoCommand
-//.global HardFault_Handler
-.code 16
-.syntax unified
-.type SH_DoCommand, function
-//.type HardFault_Handler, function
-
-
-/**************************************************************************//**
- * @brief prototype: int SH_DoCommand(int n32In_R0, int n32In_R1, int *pn32Out_R0)
- *
- * @param n32In_R0 R0
- * @param n32In_R1 R1
- * @param pn32Out_R0 R2
- *
- * @retval None
- *****************************************************************************/
-SH_DoCommand:
- BKPT 0xAB /* Wait ICE or HardFault */
- /* ICE will step over BKPT directly */
- /* HardFault will step BKPT and the next line */
-
- B SH_ICE
-
-SH_HardFault: /* Captured by HardFault */
- MOVS R0, #0 /* Set return value to 0 */
- BX LR /* Return */
-
-SH_ICE: /* Captured by ICE */
- /* Save return value */
- CMP R2, #0
- BEQ SH_End
- STR R0, [R2] /* Save the return value to *pn32Out_R0 */
-SH_End:
- MOVS R0, #1 /* Set return value to 1 */
- BX LR /* Return */
-
-/**************************************************************************//**
- * @brief HardFault Handler
- *
- * @param None.
- *
- * Skip the semihost command in free run mode.
- *
- * @retval None
- *****************************************************************************/
-HardFault_Handler:
- LDR R0, [R13, #24] /* Get previous PC */
- LDRH R1, [R0] /* Get instruction */
- LDR R2, =0xBEAB /* The sepcial BKPT instruction */
- CMP R1, R2 /* Test if the instruction at previous PC is BKPT */
- BNE HardFault_Handler_Ret /* Not BKPT */
-
- ADDS R0, #4 /* Skip BKPT and next line */
- STR R0, [R13, #24] /* Save previous PC */
-
- BX LR
-
-HardFault_Handler_Ret:
- B .
-
-
-.end
-
diff --git a/src/application/dw_main.c b/src/application/dw_main.c
index 6e1774c..42a3a8f 100644
--- a/src/application/dw_main.c
+++ b/src/application/dw_main.c
@@ -295,7 +295,7 @@ int dw_main(void)
int i = 0;
int toggle = 1;
double range_result = 0;
- double avg_result = 0;
+// double avg_result = 0;
int canSleep;
led_off(LED_ALL); //turn off all the LEDs
@@ -464,33 +464,12 @@ int dw_main(void)
instance_data_t* inst = instance_get_local_structure_ptr(0);
canSleep = instance_run(); //run the state machine!!!
- //if delayed TX scheduled but did not happen after expected time then it has failed... (has to be < slot period)
- //if anchor just go into RX and wait for next message from tags/anchors
- //if tag handle as a timeout
- if((inst->monitor == 1) && ((portGetTickCnt() - inst->timeofTx) > inst->finalReplyDelay_ms))
- {
- inst->wait4ack = 0;
-
- if(instance_mode == TAG)
- {
- inst_processrxtimeout(inst);
- }
- else //if(instance_mode == ANCHOR)
- {
- dwt_forcetrxoff(); //this will clear all events
- //enable the RX
- inst->testAppState = TA_RXE_WAIT ;
- }
- inst->monitor = 0;
- }
-
if(instancenewrange())
{
- int n, l = 0, /*txl = 0, rxl = 0,*/ aaddr, taddr, txa, rxa, rng, rng_raw;
+ int n, /* txl = 0, rxl = 0,*/ aaddr, taddr, txa, rxa, rng, rng_raw;
ranging = 1;
//send the new range information to LCD and/or USB
- range_result = instance_get_idist();
- avg_result = instance_get_adist();
+ range_result = instance_get_idist(inst->newRangeUWBIndex);
//set_rangeresult(range_result);
dataseq[0] = 0x2 ; //return cursor home
writetoLCD( 1, 0, dataseq);
@@ -499,45 +478,34 @@ int dw_main(void)
memset(dataseq1, ' ', LCD_BUFF_LEN);
sprintf((char*)&dataseq[1], "LAST: %4.2f m", range_result);
writetoLCD( 40, 1, dataseq); //send some data
-
- sprintf((char*)&dataseq1[1], "AVG8: %4.2f m", avg_result);
+ sprintf((char*)&dataseq1[0], "%llX", instance_get_uwbaddr(inst->newRangeUWBIndex));
+ // sprintf((char*)&dataseq1[1], " ");
writetoLCD( 16, 1, dataseq1); //send some data
- l = instance_get_lcount();
- //txl = instance_get_txl();
- //rxl = instance_get_rxl();
aaddr = instancenewrangeancadd();
taddr = instancenewrangetagadd();
txa = instancetxantdly();
rxa = instancerxantdly();
rng = (int) (range_result*1000);
- rng_raw = (int) (instance_get_idistraw()*1000);
+ rng_raw = (int) (instance_get_idistraw(inst->newRangeUWBIndex)*1000); //TODO change this to use actual tag
if(instance_mode == TAG)
{
//n = sprintf((char*)&dataseq[0], "ia%04x t%04x %04x %04x %04x %04x %04x %02x %02x t", aaddr, taddr, rng, rng_raw, l, txa, rxa, txl, rxl);
- n = sprintf((char*)&dataseq[0], "ia%04x t%04x %08x %08x %04x %04x %04x t", aaddr, taddr, rng, rng_raw, l, txa, rxa);
+ // n = sprintf((char*)&dataseq[0], "ia%04x t%04x %08x %08x %04x %04x %04x t", aaddr, taddr, rng, rng_raw, l, txa, rxa);
+ n = sprintf((char*)&dataseq[0], "ia%04x t%04x %08x %08x %04x %04x t", aaddr, taddr, rng, rng_raw, txa, rxa);
+
}
else
{
//n = sprintf((char*)&dataseq[0], "ia%04x t%04x %04x %04x %04x %04x %04x %02x %02x a", aaddr, taddr, rng, rng_raw, l, txa, rxa, txl, rxl);
//n = sprintf((char*)&dataseq[0], "ia%04x t%04x %08x %08x %04x %04x %04x %2.2f a", aaddr, taddr, rng, rng_raw, l, txa, rxa, instance_data[0].clockOffset);
- n = sprintf((char*)&dataseq[0], "ia%04x t%04x %08x %08x %04x %04x %04x a", aaddr, taddr, rng, rng_raw, l, txa, rxa);
- //n = sprintf((char*)&dataseq[0], "TEST");
+ // n = sprintf((char*)&dataseq[0], "ia%04x t%04x %08x %08x %04x %04x %04x a", aaddr, taddr, rng, rng_raw, l, txa, rxa);
+ n = sprintf((char*)&dataseq[0], "ia%04x t%04x %08x %08x %04x %04x a", aaddr, taddr, rng, rng_raw, txa, rxa);
+
}
-
-// ranging = 0;
-// resetinstanceanchorwaiting();
-// dataseq[0] = 0x2 ; //return cursor home
-// writetoLCD( 1, 0, dataseq);
-//
-// memcpy(&dataseq[0], (const uint8 *) " RESET ", 16);
-// writetoLCD( 40, 1, dataseq); //send some data
-// memcpy(&dataseq[0], (const uint8 *) " RANGING ", 16);
-// writetoLCD( 16, 1, dataseq); //send some data
-
#ifdef USB_SUPPORT //this is set in the port.h file
send_usbmessage(&dataseq[0], n);
@@ -550,14 +518,6 @@ int dw_main(void)
if(ranging == 0) //discovery/initialization mode for anchor and tag
{
- // dataseq[0] = 0x2 ; //return cursor home
- // writetoLCD( 1, 0, dataseq);
- // memcpy(&dataseq[0], (const uint8 *) " RANGING ", 16);
- // writetoLCD( 40, 1, dataseq); //send some data
- // memcpy(&dataseq[0], (const uint8 *) " == 0 ", 16);
- // writetoLCD( 16, 1, dataseq); //send some data
- //Sleep(1000);
-
if(instance_mode != ANCHOR)
{
if(instancesleeping())
@@ -588,34 +548,18 @@ int dw_main(void)
ranging = 1;
dataseq[0] = 0x2 ; //return cursor home
writetoLCD( 1, 0, dataseq);
- memcpy(&dataseq[0], (const uint8 *) " RANGING WITH", 16);
+ memcpy(&dataseq[0], (const uint8 *) " RANGING ", 16);
writetoLCD( 40, 1, dataseq); //send some data
- sprintf((char*)&dataseq[0], "%016llX", instance_get_anchaddr());
+ memcpy(&dataseq[0], (const uint8 *) " STARTED ", 16);
writetoLCD( 16, 1, dataseq); //send some data
}
}
else //if(instance_mode == ANCHOR)
{
- // dataseq[0] = 0x2 ; //return cursor home
- // writetoLCD( 1, 0, dataseq);
- // memcpy(&dataseq[0], (const uint8 *) " AM AN ", 16);
- // writetoLCD( 40, 1, dataseq); //send some data
- // memcpy(&dataseq[0], (const uint8 *) " ANCHOR ", 16);
- // writetoLCD( 16, 1, dataseq); //send some data
- //Sleep(1000);
if(instanceanchorwaiting())
{
- // dataseq[0] = 0x2 ; //return cursor home
- // writetoLCD( 1, 0, dataseq);
- // memcpy(&dataseq[0], (const uint8 *) " ANCHOR WAITING ", 16);
- // writetoLCD( 40, 1, dataseq); //send some data
- // memcpy(&dataseq[0], (const uint8 *) " == 1 ", 16);
- // writetoLCD( 16, 1, dataseq); //send some data
- //Sleep(1000);
-
-
toggle+=2;
@@ -639,33 +583,23 @@ int dw_main(void)
sprintf((char*)&dataseq[0], "%llX", instance_get_addr());
writetoLCD( 16, 1, dataseq); //send some data
}
- // Sleep(1000);
}
}
else if(instanceanchorwaiting() == 2)
{
- // dataseq[0] = 0x2 ; //return cursor home
- // writetoLCD( 1, 0, dataseq);
- // memcpy(&dataseq[0], (const uint8 *) " ANCHOR WAITING ", 16);
- // writetoLCD( 40, 1, dataseq); //send some data
- // memcpy(&dataseq[0], (const uint8 *) " == 2 ", 16);
- // writetoLCD( 16, 1, dataseq); //send some data
- //Sleep(1000);
dataseq[0] = 0x2 ; //return cursor home
writetoLCD( 1, 0, dataseq);
- memcpy(&dataseq[0], (const uint8 *) " RANGING WITH", 16);
+ memcpy(&dataseq[0], (const uint8 *) " RANGING ", 16);
writetoLCD( 40, 1, dataseq); //send some data
- sprintf((char*)&dataseq[0], "%llX", instance_get_tagaddr());
+ memcpy(&dataseq[0], (const uint8 *) " STARTED ", 16);
writetoLCD( 16, 1, dataseq); //send some data
- //Sleep(1000);
}
}
}
#ifdef USB_SUPPORT //this is set in the port.h file
- // int n = sprintf((char*)&dataseq[0], "Main Loop");
- // send_usbmessage(&dataseq[0], n);
+
usb_run();
#endif
diff --git a/src/application/instance.c b/src/application/instance.c
index d098d0a..7a28461 100644
--- a/src/application/instance.c
+++ b/src/application/instance.c
@@ -20,7 +20,6 @@
#include "lib.h"
#include "instance.h"
-#include <stdio.h>
@@ -67,16 +66,16 @@ int usbtxdebugdataprev_size = 0;
//
void instanceconfigframeheader(instance_data_t *inst)
{
- inst->msg[inst->tagToRangeWith].panID[0] = (inst->panID) & 0xff;
- inst->msg[inst->tagToRangeWith].panID[1] = inst->panID >> 8;
+ 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->tagToRangeWith].frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/;
+ 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->tagToRangeWith].frameCtrl[1] = 0xC /*dest extended address (64bits)*/ | 0xC0 /*src extended address (64bits)*/;
+ inst->msg[inst->uwbToRangeWith].frameCtrl[1] = 0xC /*dest extended address (64bits)*/ | 0xC0 /*src extended address (64bits)*/;
#else
- inst->msg[inst->tagToRangeWith].frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0x80 /*src short address (16bits)*/;
+ inst->msg[inst->uwbToRangeWith].frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0x80 /*src short address (16bits)*/;
#endif
}
@@ -183,19 +182,10 @@ void send_statetousb(instance_data_t *inst)
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));
- // memcpy(usbdebugdata, usbdebugdata, usbdebugdata_size);
- // if (strncmp(usbdebugdataprev, usbdebugdata, usbdebugdata_size) != 0 || usbdebugdata_size != usbdebugdataprev_size)
if (memcmp(usbdebugdataprev, usbdebugdata, usbdebugdata_size) != 0 || usbdebugdata_size != usbdebugdataprev_size)
{
- // send_usbmessage("start", 5);
-
- // usb_run();
send_usbmessage(&usbdebugdata[0], usbdebugdata_size);
usb_run();
- // send_usbmessage(&usbdebugdataprev[0], usbdebugdataprev_size);
- // usb_run();
-
- // usbrxdebugdataprev[0] = 0x2 ; //return cursor home
usbdebugdataprev_size = usbdebugdata_size;
memcpy(usbdebugdataprev, usbdebugdata, usbdebugdata_size);
}
@@ -203,24 +193,12 @@ void send_statetousb(instance_data_t *inst)
void send_rxmsgtousb(char *data)
{
- // usbrxdebugdata[0] = 0x2 ; //return cursor home
int usbrxdebugdata_size = sprintf((char*)&usbrxdebugdata[0], "%s", data);
- //memcpy(usbrxdebugdata, usbrxdebugdata, usbrxdebugdata_size);
- // if (strncmp(usbrxdebugdataprev, usbrxdebugdata, usbrxdebugdata_size) != 0 || usbrxdebugdata_size != usbrxdebugdataprev_size)
if (memcmp(usbrxdebugdataprev, usbrxdebugdata, usbrxdebugdata_size) != 0 || usbrxdebugdata_size != usbrxdebugdataprev_size)
{
- // n = sprintf((char*)&usbtmpmsg[0], "prev: %s curr: %s", (char*)&usbrxdebugdata, (char*)&usbrxdebugdataprev);
- // send_usbmessage(&usbtmpmsg[0], n);
-
- // send_usbmessage("start", 5);
- // usb_run();
send_usbmessage(&usbrxdebugdata[0], usbrxdebugdata_size);
usb_run();
- // send_usbmessage(&usbrxdebugdataprev[0], usbrxdebugdataprev_size);
- // usb_run();
-
- // usbrxdebugdataprev[0] = 0x2 ; //return cursor home
usbrxdebugdataprev_size = usbrxdebugdata_size;
memcpy(usbrxdebugdataprev, usbrxdebugdata, usbrxdebugdata_size);
}
@@ -230,15 +208,8 @@ void send_txmsgtousb(char *data)
{
int usbtxdebugdata_size = sprintf((char*)&usbtxdebugdata[0], "TX message: %s", data);
- // memcpy(usbtxdebugdata, usbtxdebugdata, usbtxdebugdata_size);
- // if (strncmp(usbtxdebugdataprev, usbtxdebugdata, usbtxdebugdata_size) != 0 || usbtxdebugdata_size != usbtxdebugdataprev_size)
- // {
- send_usbmessage(&usbtxdebugdata[0], usbtxdebugdata_size);
- usb_run();
-
- // usbtxdebugdataprev_size = usbtxdebugdata_size;
- // memcpy(usbtxdebugdataprev, usbtxdebugdata, usbtxdebugdata_size);
- // }
+ send_usbmessage(&usbtxdebugdata[0], usbtxdebugdata_size);
+ usb_run();
}
// -------------------------------------------------------------------------------------------------------------------
@@ -251,51 +222,41 @@ int testapprun(instance_data_t *inst, int message)
{
int done = INST_NOT_DONE_YET;
- printf("Test 123");
-
- // uint8 debug_msg[100];
- // int n = sprintf((char*)&debug_msg[0], "Test app run");
- // send_usbmessage(&debug_msg[0], n);
- // usb_run();
-
- // uint8 debug_msg2[100];
- // int n = sprintf((char*)&debug_msg2[0], "TTRW %i", inst->tagToRangeWith);
- // send_usbmessage(&debug_msg2[0], n);
- // usb_run();
-
switch (inst->testAppState)
{
case TA_INIT :
+ {
// if (inst->mode == ANCHOR)
// {
- send_statetousb(inst);
+ // send_statetousb(inst);
// }
- // printf("TA_INIT") ;
+
switch (inst->mode)
{
case TAG:
{
- Sleep(5000);
int mode = 0;
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);
+
+ inst->uwbShortAdd = inst->eui64[0] + (inst->eui64[1] << 8);
+
+
#if (USING_64BIT_ADDR==0)
- //the short address is assigned by the anchor
-#else
+ dwt_setaddress16(inst->uwbShortAdd);
+#endif
+
//set source address into the message structure
- for(int i=0; i<TAG_LIST_SIZE; i++)
+ for(int i=0; i<UWB_LIST_SIZE; i++)
{
- memcpy(&inst->msg[i].sourceAddr[0], inst->eui64, ADDR_BYTE_SIZE_L);
+ memcpy(&inst->msg[i].sourceAddr[0], &inst->eui64[0], inst->addrByteSize);
}
-#endif
- //change to next state - send a Poll message to 1st anchor in the list
- //inst->mode = TAG_TDOA ;
inst->testAppState = TA_TXBLINK_WAIT_SEND;
- memcpy(inst->blinkmsg.tagID, inst->eui64, ADDR_BYTE_SIZE_L);
+ memcpy(&inst->blinkmsg.tagID[0], &inst->eui64[0], ADDR_BYTE_SIZE_L);
mode = (DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV);
@@ -318,28 +279,19 @@ int testapprun(instance_data_t *inst, int message)
dwt_seteui(inst->eui64);
dwt_setpanid(inst->panID);
+ inst->uwbShortAdd = inst->eui64[0] + (inst->eui64[1] << 8);
#if (USING_64BIT_ADDR==0)
- {
- uint16 addr = inst->eui64[0] + (inst->eui64[1] << 8);
- dwt_setaddress16(addr);
- //set source address into the message structure
- for(int i=0; i<TAG_LIST_SIZE; i++)
- {
- memcpy(&inst->msg[i].sourceAddr[0], inst->eui64, ADDR_BYTE_SIZE_S);
- }
- //set source address into the message structure
- memcpy(&inst->rng_initmsg.sourceAddr[0], inst->eui64, ADDR_BYTE_SIZE_S);
- }
-#else
+ dwt_setaddress16(inst->uwbShortAdd);
+#endif
+
//set source address into the message structure
- for(int i=0; i<TAG_LIST_SIZE; i++)
+ for(int i=0; i<UWB_LIST_SIZE; i++)
{
- memcpy(&inst->msg[i].sourceAddr[0], inst->eui64, ADDR_BYTE_SIZE_L);
+ 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, ADDR_BYTE_SIZE_L);
-#endif
+ memcpy(&inst->rng_initmsg.sourceAddr[0], &inst->eui64[0], inst->addrByteSize);
// First time anchor listens we don't do a delayed RX
dwt_setrxaftertxdelay(0);
@@ -360,54 +312,43 @@ int testapprun(instance_data_t *inst, int 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:
- {
- // uint8 debug_msg[100];
- // int n = sprintf((char*)&debug_msg[0], "TA_TX_SELECT");
- // send_usbmessage(&debug_msg[0], n);
- // usb_run();
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "TA_TX_SELECT: inst->tagListLen: %i, inst->tagToRangeWith: %i", inst->tagListLen, inst->tagToRangeWith);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+ {
// select whether to blink or send out a range poll message.
- // select a tag from the list if sending out a range poll message
- if(inst->tagListLen == 0) //no known anchors yet, send out blink
+ // select a uwb from the list if sending out a range poll message
+ if(inst->uwbListLen == 0) //no known anchors yet, send out blink
{
inst->testAppState = TA_TXBLINK_WAIT_SEND;
- inst->tagToRangeWith = 255;
+ inst->uwbToRangeWith = 255;
}
else
{
- //for now, blink after attempting to poll each tag in the list
- if((inst->tagToRangeWith >= inst->tagListLen) || (inst->tagTimeout[inst->tagToRangeWith] == 1))
+ //for now, blink after attempting to poll each uwb in the list
+ if((inst->uwbToRangeWith >= inst->uwbListLen) || (inst->uwbTimeout[inst->uwbToRangeWith] == 1))
{
inst->testAppState = TA_TXBLINK_WAIT_SEND;
- inst->tagToRangeWith = 255;
+ inst->uwbToRangeWith = 255;
}
else
{
- // inst->tagToRangeWith = inst->tagToRangeWith + 1;
inst->testAppState = TA_TXPOLL_WAIT_SEND;
}
}
- send_statetousb(inst);
break; // end case TA_TX_SELECT
- }
+ }
case TA_SLEEP_DONE :
{
// if (inst->mode == ANCHOR)
// {
- send_statetousb(inst);
+ // 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
@@ -440,20 +381,19 @@ int testapprun(instance_data_t *inst, int message)
dwt_settxantennadelay(inst->txAntennaDelay) ;
}
#endif
-
instancesetantennadelays(); //this will update the antenna delay if it has changed
- }
- break;
+ break;
+ }
case TA_TXE_WAIT : //either go to sleep or proceed to TX a message
+ {
// if (inst->mode == ANCHOR)
// {
- send_statetousb(inst);
+ // send_statetousb(inst);
// }
- // printf("TA_TXE_WAIT") ;
//if we are scheduled to go to sleep before next sending then sleep first.
if(((inst->nextState == TA_TXPOLL_WAIT_SEND)
- || (inst->nextState == TA_TXBLINK_WAIT_SEND) || (inst->nextState == TA_TX_SELECT)) //add TA_TX_SELECT???
+ || (inst->nextState == TA_TXBLINK_WAIT_SEND) || (inst->nextState == TA_TX_SELECT))
&& (inst->goToSleep) //go to sleep before sending the next poll
)
{
@@ -462,10 +402,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;
- // if(inst->mode == TAG_TDOA) //once we start ranging we want to display the new range
- // {
inst->canPrintInfo = 1;
- // }
#if (DEEP_SLEEP == 1)
if (inst->sleepingEabled)
@@ -475,15 +412,18 @@ int testapprun(instance_data_t *inst, int message)
}
#endif
//DW1000 gone to sleep - report the received range
- if(inst->tof[inst->tagToRangeWith] > 0) //if ToF == 0 - then no new range to report
+ if(inst->newRangeUWBIndex != 255)
{
- if(reportTOF(inst)==0)
+ if(inst->tof[inst->newRangeUWBIndex] > 0) //if ToF == 0 - then no new range to report
{
- inst->newRange = 1;
+ 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)
-
}
else //proceed to configuration and transmission of a frame
{
@@ -491,12 +431,12 @@ int testapprun(instance_data_t *inst, int message)
inst->nextState = 0; //clear
}
break ; // end case TA_TXE_WAIT
-
+ }
case TA_TXBLINK_WAIT_SEND :
{
// if (inst->mode == ANCHOR)
// {
- send_statetousb(inst);
+ // send_statetousb(inst);
// }
int flength = (BLINK_FRAME_CRTL_AND_ADDRESS + FRAME_CRC);
@@ -519,13 +459,14 @@ int testapprun(instance_data_t *inst, int message)
if(tx_stat == 0)
{
- send_txmsgtousb("BLINK-SUCCESS");
- }
- else
- {
- send_txmsgtousb("BLINK-ERROR");
+ // send_txmsgtousb("BLINK-SUCCESS");
+ inst->timeofTx = portGetTickCnt();
}
-
+ // else
+ // {
+ // send_txmsgtousb("BLINK-ERROR");
+ // }
+
inst->goToSleep = 1; //go to Sleep after this blink
inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation
@@ -536,290 +477,289 @@ int testapprun(instance_data_t *inst, int message)
break ; // end case TA_TXBLINK_WAIT_SEND
case TA_TXRANGINGINIT_WAIT_SEND :
- {
- if (inst->mode == ANCHOR)
- {
- send_statetousb(inst);
- }
- send_txmsgtousb("RTLS_DEMO_MSG_RNG_INIT");
-
- // TODO should only send RTLS_DEMO_MSG_RNG_INIT if handshake not already complete
-
- uint16 resp_dly_us, resp_dly;
-
- int psduLength = RANGINGINIT_MSG_LEN;
-
- //tell Tag what it's address will be for the ranging exchange
- inst->rng_initmsg.messageData[FCODE] = RTLS_DEMO_MSG_RNG_INIT;
- inst->rng_initmsg.messageData[RNG_INIT_TAG_SHORT_ADDR_LO] = inst->tagShortAdd & 0xFF;
- inst->rng_initmsg.messageData[RNG_INIT_TAG_SHORT_ADDR_HI] = (inst->tagShortAdd >> 8) & 0xFF;
-
- // 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++;
+ {
+ // if (inst->mode == ANCHOR)
+ // {
+ // send_statetousb(inst);
+ // }
+ // send_txmsgtousb("RTLS_DEMO_MSG_RNG_INIT");
- inst->wait4ack = DWT_RESPONSE_EXPECTED;
+ uint16 resp_dly_us, resp_dly;
- inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
- inst->previousState = TA_TXRANGINGINIT_WAIT_SEND ;
+ int psduLength = RANGINGINIT_MSG_LEN;
- dwt_writetxdata(psduLength, (uint8 *) &inst->rng_initmsg, 0) ; // write the frame data
+ 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;
- //anchor - we don't use timeout, just wait for next frame
- if(instancesendpacket(psduLength, DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED, inst->delayedReplyTime))
- {
- dwt_setrxaftertxdelay(0);
- inst->testAppState = TA_RXE_WAIT ; // wait to receive a new blink or poll message
- inst->wait4ack = 0; //clear the flag as the TX has failed the TRX is off
- inst->lateTX++;
- }
- 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->tagToRangeWith].messageData[RES_R1] = 0x2; // "activity"
- inst->msg[inst->tagToRangeWith].messageData[RES_R2] = 0x0; //
- inst->msg[inst->tagToRangeWith].messageData[RES_R3] = 0x0;
- //set the destination address in the TWR response message
-#if 0
#if (USING_64BIT_ADDR == 1)
- memcpy(&inst->msg[inst->tagToRangeWith].destAddr[0], &inst->rng_initmsg.destAddr[0], ADDR_BYTE_SIZE_L); //remember who to send the reply to (set destination address)
+ inst->rng_initmsg.frameCtrl[1] = 0xCC;
+ psduLength += FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
#else
- memcpy(&inst->msg[inst->tagToRangeWith].destAddr[0], &inst->tagShortAdd, ADDR_BYTE_SIZE_S); //remember who to send the reply to (set destination address)
-#endif
+ 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->msg[inst->tagToRangeWith].messageData[FCODE] = RTLS_DEMO_MSG_ANCH_RESP; //message function code (specifies if message is a poll, response or other...)
+ inst->rng_initmsg.seqNum = inst->frameSN++;
- instanceconfigframeheader(inst);
- //inst->timeofTx = portGetTickCnt();
- //inst->monitor = 1;
- }
+ inst->wait4ack = DWT_RESPONSE_EXPECTED;
- }
- break;
+ inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
+ inst->previousState = TA_TXRANGINGINIT_WAIT_SEND ;
- case TA_TXPOLL_WAIT_SEND :
+ dwt_writetxdata(psduLength, (uint8 *) &inst->rng_initmsg, 0) ; // write the frame data
+
+ //anchor - we don't use timeout, just wait for next frame
+ if(instancesendpacket(psduLength, DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED, inst->delayedReplyTime))
+ {
+ dwt_setrxaftertxdelay(0);
+ inst->testAppState = TA_RXE_WAIT ; // wait to receive a new blink or poll message
+ inst->wait4ack = 0; //clear the flag as the TX has failed the TRX is off
+ inst->lateTX++;
+ }
+ else
{
- send_statetousb(inst);
- // send_txmsgtousb("RTLS_DEMO_MSG_TAG_POLL");
- int psduLength = 0;
- //NOTE the anchor address is set after receiving the ranging initialisation message
- inst->goToSleep = 1; //go to Sleep after this poll
+ inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation
+ inst->previousState = TA_TXRANGINGINIT_WAIT_SEND ;
+ done = INST_DONE_WAIT_FOR_NEXT_EVENT; //no timeout
- inst->msg[inst->tagToRangeWith].seqNum = inst->frameSN++;
- inst->msg[inst->tagToRangeWith].messageData[FCODE] = RTLS_DEMO_MSG_TAG_POLL; //message function code (specifies if message is a poll, response or other...)
+ //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);
-#if (USING_64BIT_ADDR==1)
+ inst->timeofTx = portGetTickCnt();
+ //inst->monitor = 1;
+ }
- psduLength = TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
+
+ 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;
+
+ 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...)
+
+ 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;
+ psduLength = TAG_POLL_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);
- dwt_setrxtimeout((uint16)inst->fwtoTime_sy);
+ //set the delayed rx on time (the response message will be sent after this delay)
+ dwt_setrxaftertxdelay(inst->txToRxDelayTag_sy);
+ dwt_setrxtimeout((uint16)inst->fwtoTime_sy);
- dwt_writetxdata(psduLength, (uint8 *) &inst->msg[inst->tagToRangeWith], 0) ; // write the frame data
+ dwt_writetxdata(psduLength, (uint8 *) &inst->msg[inst->uwbToRangeWith], 0) ; // write the frame data
- //response is expected
- inst->wait4ack = DWT_RESPONSE_EXPECTED;
+ //response is expected
+ inst->wait4ack = DWT_RESPONSE_EXPECTED;
- dwt_writetxfctrl(psduLength, 0, 1);
- dwt_starttx(DWT_START_TX_IMMEDIATE | inst->wait4ack);
+ dwt_writetxfctrl(psduLength, 0, 1);
+ dwt_starttx(DWT_START_TX_IMMEDIATE | inst->wait4ack);
- inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation
- inst->previousState = TA_TXPOLL_WAIT_SEND ;
- done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
+ inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation
+ inst->previousState = TA_TXPOLL_WAIT_SEND ;
+ done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
- }
+ inst->timeofTx = portGetTickCnt();
+
break;
-
+ }
case TA_TXRESPONSE_WAIT_SEND : //the frame is loaded and sent from the RX callback
- {
- send_statetousb(inst);
- //printf("TA_TXRESPONSE\n") ;
- inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
- inst->previousState = TA_TXRESPONSE_WAIT_SEND ;
- }
+ {
+ // if(inst->mode == ANCHOR)
+ // {
+ // send_statetousb(inst);
+ // }
+
+ inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
+ inst->previousState = TA_TXRESPONSE_WAIT_SEND ;
+
break;
-
+ }
case TA_TXFINAL_WAIT_SEND :
- {
- send_statetousb(inst);
- // send_txmsgtousb("RTLS_DEMO_MSG_TAG_FINAL");
- int psduLength = 0;
- // Embbed into Final message:40-bit respRxTime
- // Write Response RX time field of Final message
- memcpy(&(inst->msg[inst->tagToRangeWith].messageData[RRXT]), (uint8 *)&inst->anchorRespRxTime, 5);
+ {
+ 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->tagToRangeWith].messageData[FCODE] = RTLS_DEMO_MSG_TAG_FINAL; //message function code (specifies if message is a poll, response or other...)
+ 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);
+ instanceconfigframeheader(inst);
+
#if (USING_64BIT_ADDR==1)
- psduLength = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
+ psduLength = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
#else
- psduLength = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
+ psduLength = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
#endif
- dwt_writetxdata(psduLength, (uint8 *) &inst->msg[inst->tagToRangeWith], 0) ; // write the frame data
+ dwt_writetxdata(psduLength, (uint8 *) &inst->msg[inst->uwbToRangeWith], 0) ; // write the frame data
- if(instancesendpacket(psduLength, DWT_START_TX_DELAYED, inst->delayedReplyTime))
- {
- // initiate the re-transmission
- inst->testAppState = TA_TXE_WAIT ;
- inst->nextState = TA_TXPOLL_WAIT_SEND ;
-
- inst->wait4ack = 0; //clear the flag as the TX has failed the TRX is off
- inst->lateTX++;
+ if(instancesendpacket(psduLength, DWT_START_TX_DELAYED, inst->delayedReplyTime))
+ {
+ // initiate the re-transmission
+ inst->testAppState = TA_TXE_WAIT ;
+ inst->nextState = TA_TXPOLL_WAIT_SEND ;
+
+ inst->wait4ack = 0; //clear the flag as the TX has failed the TRX is off
+ inst->lateTX++;
- break; //exit this switch case...
- }
- else
- {
-
- inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
- inst->previousState = TA_TXFINAL_WAIT_SEND;
- done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
- // inst->responseTimeouts = 0; //reset response timeout count
- inst->timeofTx = portGetTickCnt();
- inst->monitor = 1;
- send_txmsgtousb("RTLS_DEMO_MSG_TAG_FINAL");
- }
+ break; //exit this switch case...
}
- break;
-
+ else
+ {
+
+ inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
+ inst->previousState = TA_TXFINAL_WAIT_SEND;
+ done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
+
+ inst->timeofTx = portGetTickCnt();
+ inst->monitor = 1;
+ // uint8 debug_msg[100];
+ // sprintf((char *)&debug_msg, "RTLS_DEMO_MSG_TAG_FINAL -> tag %i ", inst->uwbToRangeWith);
+ // send_txmsgtousb((char *)&debug_msg);
+
+ break;
+ }
+ }
case TA_TX_WAIT_CONF :
- //printf("TA_TX_WAIT_CONF %d m%d states %08x %08x\n", inst->previousState, message, dwt_read32bitreg(0x19), dwt_read32bitreg(0x0f)) ;
+ {
+ // if (inst->mode == ANCHOR)
+ // {
+ // send_statetousb(inst);
+ // }
+ event_data_t* dw_event = instance_getevent(11); //get and clear this event
+ //NOTE: Can get the ACK before the TX confirm event for the frame requesting the ACK
+ //this happens because if polling the ISR the RX event will be processed 1st and then the TX event
+ //thus the reception of the ACK will be processed before the TX confirmation of the frame that requested it.
+ if(dw_event->type != DWT_SIG_TX_DONE) //wait for TX done confirmation
{
- // if (inst->mode == ANCHOR)
- // {
- send_statetousb(inst);
- // }
- event_data_t* dw_event = instance_getevent(11); //get and clear this event
-
- //NOTE: Can get the ACK before the TX confirm event for the frame requesting the ACK
- //this happens because if polling the ISR the RX event will be processed 1st and then the TX event
- //thus the reception of the ACK will be processed before the TX confirmation of the frame that requested it.
- if(dw_event->type != DWT_SIG_TX_DONE) //wait for TX done confirmation
+ if(dw_event->type == DWT_SIG_RX_TIMEOUT) //got RX timeout - i.e. did not get the response (e.g. ACK)
{
- if(dw_event->type == DWT_SIG_RX_TIMEOUT) //got RX timeout - i.e. did not get the response (e.g. ACK)
- {
-
- // printf("RX timeout in TA_TX_WAIT_CONF (%d)\n", inst->previousState);
- //we need to wait for SIG_TX_DONE and then process the timeout and re-send the frame if needed
- inst->gotTO = 1;
- }
-
- send_rxmsgtousb("RX process: (1st) got TO in TA_TX_WAIT_CONF");
- done = INST_DONE_WAIT_FOR_NEXT_EVENT;
- break;
-
+ //we need to wait for SIG_TX_DONE and then process the timeout and re-send the frame if needed
+ inst->gotTO = 1;
}
- done = INST_NOT_DONE_YET;
+ done = INST_DONE_WAIT_FOR_NEXT_EVENT;
+ //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 ptc = portGetTickCnt();
-
- if(inst->previousState == TA_TXFINAL_WAIT_SEND) //TAG operations
- {
- inst->testAppState = TA_TXE_WAIT ;
- //inst->nextState = TA_TXPOLL_WAIT_SEND ;
- //inst->nextState = TA_TXBLINK_WAIT_SEND ;
- inst->nextState = TA_TX_SELECT;
- uint8 idx = inst->tagToRangeWith + 1;
- inst->tagToRangeWith = instfindfirstawaketaginlist(inst, idx);
-
- break;
- }
- else if (inst->gotTO) //timeout
+ if(inst->previousState == TA_TXBLINK_WAIT_SEND ||
+ inst->previousState == TA_TXFINAL_WAIT_SEND ||
+ inst->previousState == TA_TXPOLL_WAIT_SEND ||
+ inst->previousState == TA_TXRESPONSE_WAIT_SEND)
{
- // printf("got TO in TA_TX_WAIT_CONF\n");
- // send_rxmsgtousb("RX process: (2nd) got TO in TA_TX_WAIT_CONF");
- inst_processrxtimeout(inst);
- inst->gotTO = 0;
- inst->wait4ack = 0 ; //clear this
- break;
+ //NOTE timeout duration found experimentally, may need to be changed if the delays in instance.h are modified
+ if(ptc - (uint32)inst->timeofTx > 10) {
+ inst_processtxrxtimeout(inst);
+ }
}
- else
+ else if(inst->previousState == TA_TXRANGINGINIT_WAIT_SEND)
{
- inst->txu.txTimeStamp = dw_event->timeStamp;
-
- if(inst->previousState == TA_TXPOLL_WAIT_SEND) //TAG operations
+ //NOTE timeout duration found experimentally, may need to be changed if the delays in instance.h are modified
+ if(ptc - (uint32)inst->timeofTx > 180)
{
- uint64 tagCalculatedFinalTxTime ;
- // Embed into Final message: 40-bit pollTXTime, 40-bit respRxTime, 40-bit finalTxTime
-
- tagCalculatedFinalTxTime = (inst->txu.txTimeStamp + inst->finalReplyDelay) & MASK_TXDTS; // time we should send the response
- inst->delayedReplyTime = tagCalculatedFinalTxTime >> 8;
-
- // Calculate Time Final message will be sent and write this field of Final message
- // Sending time will be delayedReplyTime, snapped to ~125MHz or ~250MHz boundary by
- // zeroing its low 9 bits, and then having the TX antenna delay added
- // getting antenna delay from the device and add it to the Calculated TX Time
- tagCalculatedFinalTxTime = tagCalculatedFinalTxTime + inst->txAntennaDelay;
- tagCalculatedFinalTxTime &= MASK_40BIT;
-
- // Write Calculated TX time field of Final message
- memcpy(&(inst->msg[inst->tagToRangeWith].messageData[FTXT]), (uint8 *)&tagCalculatedFinalTxTime, 5);
- // Write Poll TX time field of Final message
- memcpy(&(inst->msg[inst->tagToRangeWith].messageData[PTXT]), (uint8 *)&inst->txu.tagPollTxTime, 5);
+ inst_processtxrxtimeout(inst);
}
-
- inst->testAppState = TA_RXE_WAIT ; // After sending, tag expects response/report, anchor waits to receive a final/new poll
- //fall into the next case (turn on the RX)
- message = 0;
}
-
+
+ break;
}
- //break ; // end case TA_TX_WAIT_CONF
+ done = INST_NOT_DONE_YET;
+ if(inst->previousState == TA_TXFINAL_WAIT_SEND) //TAG operations
+ {
+ inst->testAppState = TA_TXE_WAIT ;
+ inst->nextState = TA_TX_SELECT;
+ inst->uwbToRangeWith = instfindfirstactiveuwbinlist(inst, inst->uwbToRangeWith + 1);
+
+ break;
+ }
+ else if (inst->gotTO) //timeout
+ {
+ // send_txmsgtousb("(2nd) got TO in TA_TX_WAIT_CONF");
+ inst_processtxrxtimeout(inst);
+ inst->gotTO = 0;
+ inst->wait4ack = 0 ; //clear this
+ break;
+ }
+ else
+ {
+ inst->txu.txTimeStamp = dw_event->timeStamp;
+
+ if(inst->previousState == TA_TXPOLL_WAIT_SEND) //TAG operations
+ {
+ uint64 tagCalculatedFinalTxTime ;
+ // Embed into Final message: 40-bit pollTXTime, 40-bit respRxTime, 40-bit finalTxTime
+
+ tagCalculatedFinalTxTime = (inst->txu.txTimeStamp + inst->finalReplyDelay) & MASK_TXDTS; // time we should send the response
+ inst->delayedReplyTime = tagCalculatedFinalTxTime >> 8;
+
+ // Calculate Time Final message will be sent and write this field of Final message
+ // Sending time will be delayedReplyTime, snapped to ~125MHz or ~250MHz boundary by
+ // zeroing its low 9 bits, and then having the TX antenna delay added
+ // getting antenna delay from the device and add it to the Calculated TX Time
+ 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);
+ }
+ inst->testAppState = TA_RXE_WAIT ; // After sending, tag expects response/report, anchor waits to receive a final/new poll
+
+ //fall into the next case (turn on the RX)
+ message = 0;
+ }
+ }// end case TA_TX_WAIT_CONF
case TA_RXE_WAIT :
- // printf("TA_RXE_WAIT") ;
{
- if (inst->mode == ANCHOR)
- {
- send_statetousb(inst);
- }
+ // if (inst->mode == ANCHOR)
+ // {
+ // send_statetousb(inst);
+ // }
if(inst->wait4ack == 0) //if this is set the RX will turn on automatically after TX
{
//turn RX on
- instancerxon(inst, 0, 0) ; // turn RX on, with/without delay
+ instancerxon(inst, 0, 0) ; // turn RX on, without delay
}
else
{
@@ -835,91 +775,51 @@ int testapprun(instance_data_t *inst, int message)
inst->testAppState = TA_RX_WAIT_DATA; // let this state handle it
// end case TA_RXE_WAIT, don't break, but fall through into the TA_RX_WAIT_DATA state to process it immediately.
- if(message == 0) break;
- }
-
- case TA_RX_WAIT_DATA :
- if (inst->mode == ANCHOR)
+ if(message == 0)
{
- send_statetousb(inst);
+ break;
}
- // Wait RX data
- //printf("TA_RX_WAIT_DATA %d", message) ;
+ }
+ case TA_RX_WAIT_DATA :
+ {
+ // if (inst->mode == ANCHOR)
+ // {
+ // send_statetousb(inst);
+ // }
+ // Wait RX data
switch (message)
{
case DWT_SIG_RX_BLINK :
{
- send_rxmsgtousb("RX process: DWT_SIG_RX_BLINK ");
+ // send_rxmsgtousb("RX process: DWT_SIG_RX_BLINK ");
event_data_t* dw_event = instance_getevent(12); //get and clear this event
- //printf("we got blink message from %08X\n", ( tagaddr& 0xFFFF));
+
if((inst->mode == LISTENER) || (inst->mode == ANCHOR))
{
inst->canPrintInfo = 1;
- // if(inst->tagToRangeWith == 255)
- // {
- // instaddtagtolist(inst, &(dw_event->msgu.rxblinkmsg.tagID[0]));
- // // int n = sprintf((char*)&debug_msg[0], "BLINK!!! tagToRangeWith after %i", inst->tagToRangeWith);
- // // send_usbmessage(&debug_msg[0], n);
- // // usb_run();
- // //initiate ranging message this is a Blink from the Tag we would like to range to
- // //NOTE not sure if this check is needed... but leave it for now
- // if(memcmp(&inst->tagList[inst->tagToRangeWith][0], &(dw_event->msgu.rxblinkmsg.tagID[0]), BLINK_FRAME_SOURCE_ADDRESS) == 0)
- // {
- // inst->tagShortAdd = (dwt_getpartid() & 0xFF);
- // inst->tagShortAdd = (inst->tagShortAdd << 8) + dw_event->msgu.rxblinkmsg.tagID[0] ; //TODO need to figure this out...
-
- // //if using longer reply delay time (e.g. if interworking with a PC application)
- // inst->delayedReplyTime = (dw_event->timeStamp + inst->rnginitReplyDelay) >> 8 ; // time we should send the blink response
-
- // //set destination address
- // memcpy(&inst->rng_initmsg.destAddr[0], &(dw_event->msgu.rxblinkmsg.tagID[0]), BLINK_FRAME_SOURCE_ADDRESS); //remember who to send the reply to
-
- // inst->testAppState = TA_TXE_WAIT;
- // inst->nextState = TA_TXRANGINGINIT_WAIT_SEND ;
-
- // break;
- // }
-
- // //else stay in RX
- // }
-
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "memcmp tagList & tagID: %i %i", inst->tagList[inst->tagToRangeWith][0], dw_event->msgu.rxblinkmsg.tagID[0]);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+ //if using longer reply delay time (e.g. if interworking with a PC application)
+ inst->delayedReplyTime = (dw_event->timeStamp + inst->rnginitReplyDelay) >> 8 ; // time we should send the blink response
- if(memcmp(&inst->tagList[inst->tagToRangeWith][0], &(dw_event->msgu.rxblinkmsg.tagID[0]), BLINK_FRAME_SOURCE_ADDRESS) == 0)
- {
- inst->tagShortAdd = (dwt_getpartid() & 0xFF);
- inst->tagShortAdd = (inst->tagShortAdd << 8) + dw_event->msgu.rxblinkmsg.tagID[0] ; //TODO need to figure this out...
+ //set destination address
+ memcpy(&inst->rng_initmsg.destAddr[0], &(dw_event->msgu.rxblinkmsg.tagID[0]), BLINK_FRAME_SOURCE_ADDRESS); //remember who to send the reply to
- //if using longer reply delay time (e.g. if interworking with a PC application)
- inst->delayedReplyTime = (dw_event->timeStamp + inst->rnginitReplyDelay) >> 8 ; // time we should send the blink response
-
- //set destination address
- memcpy(&inst->rng_initmsg.destAddr[0], &(dw_event->msgu.rxblinkmsg.tagID[0]), BLINK_FRAME_SOURCE_ADDRESS); //remember who to send the reply to
+ inst->testAppState = TA_TXE_WAIT;
+ inst->nextState = TA_TXRANGINGINIT_WAIT_SEND ;
- inst->testAppState = TA_TXE_WAIT;
- inst->nextState = TA_TXRANGINGINIT_WAIT_SEND ;
-
- break;
- }
- //else stay in RX
}
- //else //not initiating ranging - continue to receive
+ else //not initiating ranging - continue to receive
{
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
done = INST_NOT_DONE_YET;
}
+ break;
}
- break;
-
- //if we have received a DWT_SIG_RX_OKAY event - this means that the message is IEEE data type - need to check frame control to know which addressing mode is used
case DWT_SIG_RX_OKAY :
{
+ //if we have received a DWT_SIG_RX_OKAY event - this means that the message is IEEE data type - need to check frame control to know which addressing mode is used
event_data_t* dw_event = instance_getevent(15); //get and clear this event
uint8 srcAddr[8] = {0,0,0,0,0,0,0,0};
@@ -930,394 +830,269 @@ int testapprun(instance_data_t *inst, int message)
// 16 or 64 bit addresses
switch(dw_event->msgu.frame[1])
{
- case 0xCC: //
+ case 0xCC:
memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ll.sourceAddr[0]), ADDR_BYTE_SIZE_L);
fn_code = dw_event->msgu.rxmsg_ll.messageData[FCODE];
messageData = &dw_event->msgu.rxmsg_ll.messageData[0];
break;
- case 0xC8: //
+ case 0xC8:
memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_sl.sourceAddr[0]), ADDR_BYTE_SIZE_L);
fn_code = dw_event->msgu.rxmsg_sl.messageData[FCODE];
messageData = &dw_event->msgu.rxmsg_sl.messageData[0];
break;
- case 0x8C: //
+ case 0x8C:
memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ls.sourceAddr[0]), ADDR_BYTE_SIZE_S);
fn_code = dw_event->msgu.rxmsg_ls.messageData[FCODE];
messageData = &dw_event->msgu.rxmsg_ls.messageData[0];
break;
- case 0x88: //
+ case 0x88:
memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ss.sourceAddr[0]), ADDR_BYTE_SIZE_S);
fn_code = dw_event->msgu.rxmsg_ss.messageData[FCODE];
messageData = &dw_event->msgu.rxmsg_ss.messageData[0];
break;
}
+
+ fcode = fn_code;
+ switch(fcode)
{
- if(inst->mode == ANCHOR)
+ case RTLS_DEMO_MSG_RNG_INIT:
{
-#if (USING_64BIT_ADDR==1)
+ // send_rxmsgtousb("RX process: DWT_SIG_RX_OKAY-RTLS_DEMO_MSG_RNG_INIT");
- if(memcmp(&inst->tagList[inst->tagToRangeWith][0], &srcAddr[0], BLINK_FRAME_SOURCE_ADDRESS) == 0) //if the Tag's address does not match (ignore the message)
-#else
- //if using 16-bit addresses the ranging messages from tag are using the short address tag was given in the ranging init message
- if(inst->tagShortAdd == (srcAddr[0] + (srcAddr[1] << 8)))
-#endif
- //only process messages from the associated tag
- {
- fcode = fn_code;
- }
-// #if (USING_64BIT_ADDR==1)
-// if(instchecktaginlist(&inst, &srcAddr[0]) != 255) //if the Tag's address does not match (ignore the message)
-// #else
-// //if using 16-bit addresses the ranging messages from tag are using the short address tag was given in the ranging init message
-// if(inst->tagShortAdd == (srcAddr[0] + (srcAddr[1] << 8)))
-// #endif
-// //only process messages from an associated tag
-// {
-// fcode = fn_code;
-// }
-// else
-// {
-// uint8 debug_msg[100];
-// int n = sprintf((char*)&debug_msg[0], "Tag not in list, RX message not processed");
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-// }
- }
- else // LISTENER or TAG
- {
- fcode = fn_code;
- }
+ uint32 final_reply_delay_us;
+ uint32 resp_dly[RESP_DLY_NB];
+ int i;
- switch(fcode)
- {
- case RTLS_DEMO_MSG_RNG_INIT: //TAG operations
+ inst->testAppState = TA_TXE_WAIT;
+ inst->nextState = TA_TXPOLL_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++)
{
- send_rxmsgtousb("RX process: DWT_SIG_RX_OKAY-RTLS_DEMO_MSG_RNG_INIT");
- // send_rxmsgtousb("RX message fcode: RTLS_DEMO_MSG_RNG_INIT");
-
- //shouldn't use different tag modes. should only accept if not in list already
- //NOTE this is the poll message sent by the tag to initiate the handshake
-
- 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
-
- inst->tagShortAdd = messageData[RNG_INIT_TAG_SHORT_ADDR_LO]
- + (messageData[RNG_INIT_TAG_SHORT_ADDR_HI] << 8) ;
-
- // Get response delays from message and update internal timings accordingly
- resp_dly[RESP_DLY_ANC] = messageData[RNG_INIT_ANC_RESP_DLY_LO]
- + (messageData[RNG_INIT_ANC_RESP_DLY_HI] << 8);
- resp_dly[RESP_DLY_TAG] = messageData[RNG_INIT_TAG_RESP_DLY_LO]
- + (messageData[RNG_INIT_TAG_RESP_DLY_HI] << 8);
- for (i = 0; i < RESP_DLY_NB; i++)
- {
- if (((resp_dly[i] & RESP_DLY_UNIT_MASK) >> RESP_DLY_UNIT_SHIFT) == RESP_DLY_UNIT_MS)
- {
- // Remove unit bit and convert to microseconds.
- resp_dly[i] &= ~RESP_DLY_UNIT_MASK;
- resp_dly[i] *= 1000;
- }
- }
- // Update delay between poll transmission and response reception.
- // Use uint64 for resp_dly here to avoid overflows if it is more than 400 ms.
- inst->txToRxDelayTag_sy = US_TO_SY_INT((uint64)resp_dly[RESP_DLY_ANC] - inst->frameLengths_us[POLL]) - RX_START_UP_SY;
- // Update delay between poll transmission and final transmission.
- final_reply_delay_us = resp_dly[RESP_DLY_ANC] + resp_dly[RESP_DLY_TAG];
- inst->finalReplyDelay = convertmicrosectodevicetimeu(final_reply_delay_us);
- inst->finalReplyDelay_ms = CEIL_DIV(final_reply_delay_us, 1000);
- // If we are using long response delays, deactivate sleep.
- if (resp_dly[RESP_DLY_ANC] >= LONG_RESP_DLY_LIMIT_US
- || resp_dly[RESP_DLY_TAG] >= LONG_RESP_DLY_LIMIT_US)
+ if (((resp_dly[i] & RESP_DLY_UNIT_MASK) >> RESP_DLY_UNIT_SHIFT) == RESP_DLY_UNIT_MS)
{
- inst->sleepingEabled = 0;
+ // Remove unit bit and convert to microseconds.
+ resp_dly[i] &= ~RESP_DLY_UNIT_MASK;
+ resp_dly[i] *= 1000;
}
-
-#if (USING_64BIT_ADDR == 1)
- memcpy(&inst->msg[inst->tagToRangeWith].destAddr[0], &srcAddr[0], ADDR_BYTE_SIZE_L); //set the anchor address for the reply (set destination address)
-#else
- memcpy(&inst->msg[inst->tagToRangeWith].destAddr[0], &srcAddr[0], ADDR_BYTE_SIZE_S); //set anchor address for the reply (set destination address)
- inst->msg[inst->tagToRangeWith].sourceAddr[0] = messageData[RES_R1]; //set tag short address
- inst->msg[inst->tagToRangeWith].sourceAddr[1] = messageData[RES_R2];
- dwt_setaddress16(inst->tagShortAdd); //NOTE tagShortAdd might need to be reworked for TAG...
-#endif
-
- //NOTE this might be a problem...
- memcpy(&inst->relpyAddress[0], &srcAddr[0], ADDR_BYTE_SIZE_L); //remember who to send the reply to (set destination address)
-
- 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
-
-
-
-// //NOTE this is the poll message sent by the tag to initiate the handshake
-// if(inst->mode == TAG_TDOA) //only start ranging with someone if not ranging already
-// {
-// 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
-
-// inst->tagShortAdd = messageData[RNG_INIT_TAG_SHORT_ADDR_LO]
-// + (messageData[RNG_INIT_TAG_SHORT_ADDR_HI] << 8) ;
-
-// // Get response delays from message and update internal timings accordingly
-// resp_dly[RESP_DLY_ANC] = messageData[RNG_INIT_ANC_RESP_DLY_LO]
-// + (messageData[RNG_INIT_ANC_RESP_DLY_HI] << 8);
-// resp_dly[RESP_DLY_TAG] = messageData[RNG_INIT_TAG_RESP_DLY_LO]
-// + (messageData[RNG_INIT_TAG_RESP_DLY_HI] << 8);
-// for (i = 0; i < RESP_DLY_NB; i++)
-// {
-// if (((resp_dly[i] & RESP_DLY_UNIT_MASK) >> RESP_DLY_UNIT_SHIFT) == RESP_DLY_UNIT_MS)
-// {
-// // Remove unit bit and convert to microseconds.
-// resp_dly[i] &= ~RESP_DLY_UNIT_MASK;
-// resp_dly[i] *= 1000;
-// }
-// }
-// // Update delay between poll transmission and response reception.
-// // Use uint64 for resp_dly here to avoid overflows if it is more than 400 ms.
-// inst->txToRxDelayTag_sy = US_TO_SY_INT((uint64)resp_dly[RESP_DLY_ANC] - inst->frameLengths_us[POLL]) - RX_START_UP_SY;
-// // Update delay between poll transmission and final transmission.
-// final_reply_delay_us = resp_dly[RESP_DLY_ANC] + resp_dly[RESP_DLY_TAG];
-// inst->finalReplyDelay = convertmicrosectodevicetimeu(final_reply_delay_us);
-// inst->finalReplyDelay_ms = CEIL_DIV(final_reply_delay_us, 1000);
-// // If we are using long response delays, deactivate sleep.
-// if (resp_dly[RESP_DLY_ANC] >= LONG_RESP_DLY_LIMIT_US
-// || resp_dly[RESP_DLY_TAG] >= LONG_RESP_DLY_LIMIT_US)
-// {
-// inst->sleepingEabled = 0;
-// }
-
-// #if (USING_64BIT_ADDR == 1)
-// memcpy(&inst->msg.destAddr[0], &srcAddr[0], ADDR_BYTE_SIZE_L); //set the anchor address for the reply (set destination address)
-// #else
-// memcpy(&inst->msg.destAddr[0], &srcAddr[0], ADDR_BYTE_SIZE_S); //set anchor address for the reply (set destination address)
-// inst->msg.sourceAddr[0] = messageData[RES_R1]; //set tag short address
-// inst->msg.sourceAddr[1] = messageData[RES_R2];
-// dwt_setaddress16(inst->tagShortAdd);
-// #endif
-
-// memcpy(&inst->relpyAddress[0], &srcAddr[0], ADDR_BYTE_SIZE_L); //remember who to send the reply to (set destination address)
-
-// 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
-// }
- //printf("GOT RTLS_DEMO_MSG_RNG_INIT - start ranging - \n");
- //else we ignore this message if already associated... (not TAG_TDOA)
}
- break; //RTLS_DEMO_MSG_RNG_INIT
-
- case RTLS_DEMO_MSG_TAG_POLL:
+
+ // Update delay between poll transmission and response reception.
+ // Use uint64 for resp_dly here to avoid overflows if it is more than 400 ms.
+ inst->txToRxDelayTag_sy = US_TO_SY_INT((uint64)resp_dly[RESP_DLY_ANC] - inst->frameLengths_us[POLL]) - RX_START_UP_SY;
+
+ // Update delay between poll transmission and final transmission.
+ final_reply_delay_us = resp_dly[RESP_DLY_ANC] + resp_dly[RESP_DLY_TAG];
+ inst->finalReplyDelay = convertmicrosectodevicetimeu(final_reply_delay_us);
+ inst->finalReplyDelay_ms = CEIL_DIV(final_reply_delay_us, 1000);
+
+ // If we are using long response delays, deactivate sleep.
+ if (resp_dly[RESP_DLY_ANC] >= LONG_RESP_DLY_LIMIT_US
+ || resp_dly[RESP_DLY_TAG] >= LONG_RESP_DLY_LIMIT_US)
{
- // send_rxmsgtousb("RX process: DWT_SIG_RX_OKAY-RTLS_DEMO_MSG_TAG_POLL ");
- // send_rxmsgtousb("RX message fcode: RTLS_DEMO_MSG_TAG_POLL");
- 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;
- }
-
+ inst->sleepingEabled = 0;
+ }
- if (!inst->frameFilteringEnabled) //NOTE this might cause problems... might need to move somewhere else
- {
- // if we missed the ACK to the ranging init message we may not have turned frame filtering on
- dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN); //we are starting ranging - enable the filter....
-
- inst->frameFilteringEnabled = 1 ;
- }
+ memcpy(&inst->msg[inst->uwbToRangeWith].destAddr[0], &srcAddr[0], inst->addrByteSize); //set the anchor address for the reply (set destination address)
- if(dw_event->typePend == DWT_SIG_TX_PENDING)
- {
- inst->canPrintInfo = 0;
- inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
- inst->previousState = TA_TXRESPONSE_WAIT_SEND ;
- }
- else
- {
- //stay in RX wait for next frame...
- inst->testAppState = TA_RXE_WAIT ; // wait for next frame
- }
+ 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:
+ {
+ 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;
}
- break; //RTLS_DEMO_MSG_TAG_POLL
- case RTLS_DEMO_MSG_ANCH_RESP:
+ if (!inst->frameFilteringEnabled) //NOTE this might cause problems... might need to move somewhere else
{
- // send_rxmsgtousb("RX process: DWT_SIG_RX_OKAY-RTLS_DEMO_MSG_ANCH_RESP ");
- // send_rxmsgtousb("RX message fcode: 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
-
- inst->testAppState = TA_TXFINAL_WAIT_SEND ; // send our response / the final
-
- inst->canPrintInfo = 2;
-
- inst->tof[inst->tagToRangeWith] = 0;
- //copy previously calculated ToF
- memcpy(&inst->tof[inst->tagToRangeWith], &(messageData[TOFR]), 5);
+ // if we missed the ACK to the ranging init message we may not have turned frame filtering on
+ dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN); //we are starting ranging - enable the filter....
+
+ inst->frameFilteringEnabled = 1 ;
+ }
- inst->newRangeAncAddress = srcAddr[0] + ((uint16) srcAddr[1] << 8);
- inst->newRangeTagAddress = inst->eui64[0] + ((uint16) inst->eui64[1] << 8);
+ if(dw_event->typePend == DWT_SIG_TX_PENDING)
+ {
+ inst->canPrintInfo = 0;
+ inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
+ inst->previousState = TA_TXRESPONSE_WAIT_SEND ;
+ }
+ else
+ {
+ //stay in RX wait for next frame...
+ inst->testAppState = TA_RXE_WAIT ; // wait for next frame
}
- break; //RTLS_DEMO_MSG_ANCH_RESP
- case RTLS_DEMO_MSG_TAG_FINAL:
+ break;
+ }//RTLS_DEMO_MSG_TAG_POLL
+ 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
{
- send_rxmsgtousb("RX process: DWT_SIG_RX_OKAY-RTLS_DEMO_MSG_TAG_FINAL ");
- int64 Rb, Da, Ra, Db ;
- uint64 tagFinalTxTime = 0;
- uint64 tagFinalRxTime = 0;
- uint64 tagPollTxTime = 0;
- uint64 anchorRespRxTime = 0;
-
- double RaRbxDaDb = 0;
- 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;
- }
+ inst->testAppState = TA_RXE_WAIT ; // wait for next frame
+ break;
+ }
- // time of arrival of Final message
- tagFinalRxTime = dw_event->timeStamp ; //Final's Rx time
+ inst->anchorRespRxTime = dw_event->timeStamp ; //Response's Rx time
- inst->delayedReplyTime = 0 ;
+ inst->testAppState = TA_TXFINAL_WAIT_SEND ; // send our response / the final
- // times measured at Tag extracted from the message buffer
- // extract 40bit times
- memcpy(&tagPollTxTime, &(messageData[PTXT]), 5);
- memcpy(&anchorRespRxTime, &(messageData[RRXT]), 5);
- memcpy(&tagFinalTxTime, &(messageData[FTXT]), 5);
+ inst->canPrintInfo = 2;
- // poll response round trip delay time is calculated as
- // (anchorRespRxTime - tagPollTxTime) - (anchorRespTxTime - tagPollRxTime)
- Ra = (int64)((anchorRespRxTime - tagPollTxTime) & MASK_40BIT);
- Db = (int64)((inst->txu.anchorRespTxTime - inst->tagPollRxTime) & MASK_40BIT);
+ inst->tof[inst->uwbToRangeWith] = 0;
+ //copy previously calculated ToF
+ memcpy(&inst->tof[inst->uwbToRangeWith], &(messageData[TOFR]), 5);
- // response final round trip delay time is calculated as
- // (tagFinalRxTime - anchorRespTxTime) - (tagFinalTxTime - anchorRespRxTime)
- Rb = (int64)((tagFinalRxTime - inst->txu.anchorRespTxTime) & MASK_40BIT);
- Da = (int64)((tagFinalTxTime - anchorRespRxTime) & MASK_40BIT);
+ inst->newRangeUWBIndex = inst->uwbToRangeWith;
+ //TODO remove or update below?
+ inst->newRangeAncAddress = (uint16) srcAddr[0] + ((uint16) srcAddr[1] << 8);
+ inst->newRangeTagAddress = (uint16) inst->eui64[0] + ((uint16) inst->eui64[1] << 8);
+
+ break;
+ } //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 ;
+ uint64 tagFinalTxTime = 0;
+ uint64 tagFinalRxTime = 0;
+ uint64 tagPollTxTime = 0;
+ uint64 anchorRespRxTime = 0;
+
+ double RaRbxDaDb = 0;
+ 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;
+ }
- RaRbxDaDb = (((double)Ra))*(((double)Rb))
- - (((double)Da))*(((double)Db));
+ // time of arrival of Final message
+ tagFinalRxTime = dw_event->timeStamp ; //Final's Rx time
- RbyDb = ((double)Rb + (double)Db);
+ inst->delayedReplyTime = 0 ;
- RayDa = ((double)Ra + (double)Da);
+ // times measured at Tag extracted from the message buffer
+ // extract 40bit times
+ memcpy(&tagPollTxTime, &(messageData[PTXT]), 5);
+ memcpy(&anchorRespRxTime, &(messageData[RRXT]), 5);
+ memcpy(&tagFinalTxTime, &(messageData[FTXT]), 5);
- //time-of-flight
- inst->tof[inst->tagToRangeWith] = (int64) ( RaRbxDaDb/(RbyDb + RayDa) );
+ // poll response round trip delay time is calculated as
+ // (anchorRespRxTime - tagPollTxTime) - (anchorRespTxTime - tagPollRxTime)
+ Ra = (int64)((anchorRespRxTime - tagPollTxTime) & MASK_40BIT);
+ Db = (int64)((inst->txu.anchorRespTxTime - inst->tagPollRxTime) & MASK_40BIT);
- if(reportTOF(inst) == 0)
- {
- inst->newRange = 1;
- }
- inst->newRangeTagAddress = srcAddr[0] + ((uint16) srcAddr[1] << 8);
- inst->newRangeAncAddress = inst->eui64[0] + ((uint16) inst->eui64[1] << 8);
- //inst->lastReportTime = time_ms;
+ // response final round trip delay time is calculated as
+ // (tagFinalRxTime - anchorRespTxTime) - (tagFinalTxTime - anchorRespRxTime)
+ Rb = (int64)((tagFinalRxTime - inst->txu.anchorRespTxTime) & MASK_40BIT);
+ Da = (int64)((tagFinalTxTime - anchorRespRxTime) & MASK_40BIT);
- // inst->testAppState = TA_RXE_WAIT ; // wait for next frame
- // ranging complete with one of the tags, lets go back to listening for blinks/polls!
-
- //keep track of when the tag was last ranged to
- // inst->finalTimeStamp[inst->tagToRangeWith] = tagFinalRxTime;
-
- inst->testAppState = TA_RXE_WAIT ;
- inst->previousState = TA_INIT;
- inst->nextState = TA_INIT;
- inst->prevTagToRangeWith = inst->tagToRangeWith;
- inst->tagToRangeWith = 255;
- inst->frameFilteringEnabled = 0;
- dwt_enableframefilter(DWT_FF_NOTYPE_EN);
-
- dwt_setrxaftertxdelay(0);
+ RaRbxDaDb = (((double)Ra))*(((double)Rb)) - (((double)Da))*(((double)Db));
+ RbyDb = ((double)Rb + (double)Db);
+ RayDa = ((double)Ra + (double)Da);
- instancesetantennadelays(); //this will update the antenna delay if it has changed
+ //time-of-flight
+ inst->tof[inst->uwbToRangeWith] = (int64) ( RaRbxDaDb/(RbyDb + RayDa) );
+ inst->newRangeUWBIndex = inst->uwbToRangeWith;
+ if(reportTOF(inst, inst->newRangeUWBIndex) == 0)
+ {
+ inst->newRange = 1;
}
- break; //RTLS_DEMO_MSG_TAG_FINAL
+ //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->testAppState = TA_RXE_WAIT ;
+ inst->previousState = TA_INIT;
+ inst->nextState = TA_INIT;
+ inst->uwbToRangeWith = 255;
+ inst->frameFilteringEnabled = 0;
+
+ dwt_enableframefilter(DWT_FF_NOTYPE_EN);
+ dwt_setrxaftertxdelay(0);
+ instancesetantennadelays(); //this will update the antenna delay if it has changed
- default:
- {
- if(inst->mode == ANCHOR)
- {
- send_rxmsgtousb("RX process: DWT_SIG_RX_OKAY-default ");
- }
- //send_rxmsgtousb("RX message fcode: default");
- inst->testAppState = TA_RXE_WAIT ; // wait for next frame
- dwt_setrxaftertxdelay(0);
-
- }
- break;
- } //end switch (fcode)
- } //end else
+
+ break;
+ } //RTLS_DEMO_MSG_TAG_FINAL
+ default:
+ {
+ // if(inst->mode == ANCHOR)
+ // {
+ // send_rxmsgtousb("RX process: DWT_SIG_RX_OKAY-default ");
+ // }
+ inst->testAppState = TA_RXE_WAIT ; // wait for next frame
+ dwt_setrxaftertxdelay(0);
+
+ break;
+ }
+ } //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
-
+
+ break ;
+ } //end of DWT_SIG_RX_OKAY
case DWT_SIG_RX_TIMEOUT :
- // if(inst->mode == ANCHOR)
- // {
- send_rxmsgtousb("RX process: DWT_SIG_RX_TIMEOUT ");
- // }
+ {
+ // send_txmsgtousb("RX process: DWT_SIG_RX_TIMEOUT ");
+
instance_getevent(17); //get and clear this event
- //printf("PD_DATA_TIMEOUT %d\n", inst->previousState) ;
- inst_processrxtimeout(inst);
+ inst_processtxrxtimeout(inst);
message = 0; //clear the message as we have processed the event
- break ;
-
+
+ break;
+ }
case DWT_SIG_TX_AA_DONE: //ignore this event - just process the rx frame that was received before the ACK response
case 0:
default :
{
- if(inst->mode == ANCHOR)
- {
- send_rxmsgtousb("RX process: default ");
- }
- //if(DWT_SIG_TX_AA_DONE == message) printf("Got SIG_TX_AA_DONE in RX wait - ignore\n");
- if(done == INST_NOT_DONE_YET) done = INST_DONE_WAIT_FOR_NEXT_EVENT;
- // inst->testAppState = TA_RXE_WAIT;
+ // if(inst->mode == ANCHOR)
+ // {
+ // send_rxmsgtousb("RX process: default ");
+ // }
+ if(done == INST_NOT_DONE_YET)
+ {
+ done = INST_DONE_WAIT_FOR_NEXT_EVENT;
+ }
+
+ break;
}
- break;
+ } // end of switch on message
- }
- break ; // end case TA_RX_WAIT_DATA
+ break;
+ } // end case TA_RX_WAIT_DATA
default:
+ {
// if (inst->mode == ANCHOR)
// {
- send_statetousb(inst);
+ // send_statetousb(inst);
// }
- //printf("\nERROR - invalid state %d - what is going on??\n", inst->testAppState) ;
break;
+ }
} // end switch on testAppState
return done;
@@ -1350,9 +1125,13 @@ int instance_init_s(int mode)
dwt_setcallbacks(instance_txcallback, instance_rxgoodcallback, instance_rxtimeoutcallback, instance_rxerrorcallback);
- //inst->anchorListIndex = 0 ;
- inst->tagToRangeWith = 255;
- inst->prevTagToRangeWith = 255;
+#if (USING_64BIT_ADDR==0)
+ inst->addrByteSize = ADDR_BYTE_SIZE_S;
+#else
+ inst->addrByteSize = ADDR_BYTE_SIZE_L;
+#endif
+
+ inst->uwbToRangeWith = 255;
return 0 ;
}
@@ -1493,36 +1272,21 @@ uint64 instance_get_addr(void) //get own address
return (x);
}
-uint64 instance_get_tagaddr(void) //get own address
+uint64 instance_get_uwbaddr(uint8 uwb_index) //get uwb address by index
{
instance_data_t* inst = instance_get_local_structure_ptr(0);
- uint64 x = (uint64) inst->tagList[0][0];
- x |= (uint64) inst->tagList[0][1] << 8;
- x |= (uint64) inst->tagList[0][2] << 16;
- x |= (uint64) inst->tagList[0][3] << 24;
- x |= (uint64) inst->tagList[0][4] << 32;
- x |= (uint64) inst->tagList[0][5] << 40;
- x |= (uint64) inst->tagList[0][6] << 48;
- x |= (uint64) inst->tagList[0][7] << 56;
-
+ uint64 x = (uint64) inst->uwbList[uwb_index][0];
+ x |= (uint64) inst->uwbList[uwb_index][1] << 8;
+ x |= (uint64) inst->uwbList[uwb_index][2] << 16;
+ x |= (uint64) inst->uwbList[uwb_index][3] << 24;
+ x |= (uint64) inst->uwbList[uwb_index][4] << 32;
+ x |= (uint64) inst->uwbList[uwb_index][5] << 40;
+ x |= (uint64) inst->uwbList[uwb_index][6] << 48;
+ x |= (uint64) inst->uwbList[uwb_index][7] << 56;
return (x);
}
-uint64 instance_get_anchaddr(void) //get anchor address (that sent the ToF)
-{
- instance_data_t* inst = instance_get_local_structure_ptr(0);
- uint64 x = (uint64) inst->relpyAddress[0];
- x |= (uint64) inst->relpyAddress[1] << 8;
- x |= (uint64) inst->relpyAddress[2] << 16;
- x |= (uint64) inst->relpyAddress[3] << 24;
- x |= (uint64) inst->relpyAddress[4] << 32;
- x |= (uint64) inst->relpyAddress[5] << 40;
- x |= (uint64) inst->relpyAddress[6] << 48;
- x |= (uint64) inst->relpyAddress[7] << 56;
- return (x);
-}
-
void instance_readaccumulatordata(void)
{
#if DECA_SUPPORT_SOUNDING==1
diff --git a/src/application/instance.h b/src/application/instance.h
index 366abe6..b587d81 100644
--- a/src/application/instance.h
+++ b/src/application/instance.h
@@ -134,13 +134,11 @@ enum
#define BLINK_FRAME_CRTL_AND_ADDRESS (BLINK_FRAME_SOURCE_ADDRESS + BLINK_FRAME_CTRLP) //10 bytes
#define BLINK_FRAME_LEN_BYTES (BLINK_FRAME_CRTL_AND_ADDRESS + BLINK_FRAME_CRC)
-#define TAG_LIST_SIZE (10) //anchor will range with 1st Tag it gets blink from
-#define TAG_COMM_TIMEOUT 3000 //ms
+#define UWB_LIST_SIZE (10) //maximum number of UWBs to range with
+#define UWB_COMM_TIMEOUT 3000 //ms
#define BLINK_SLEEP_DELAY 1000 //ms
#define POLL_SLEEP_DELAY 500 //ms
-//#define POLL_SLEEP_DELAY 50 //ms //NOTE 200 gives 5 Hz range period
-
#define IMMEDIATE_RESPONSE (1)
@@ -381,7 +379,7 @@ typedef struct {
typedef struct
{
- INST_MODE mode; //instance mode (tag or anchor)
+ INST_MODE mode; //instance mode (tag or anchor)
INST_STATES testAppState ; //state machine - current state
INST_STATES nextState ; //state machine - next state
INST_STATES previousState ; //state machine - previous state
@@ -424,20 +422,23 @@ typedef struct
//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[TAG_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
#else
srd_msg_dlss rng_initmsg ; // ranging init message (destination long, source short)
- srd_msg_dsss msg[TAG_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
#endif
iso_IEEE_EUI64_blink_msg blinkmsg ; // frame structure (used for tx blink message)
//Tag function address/message configuration
uint8 eui64[8]; // devices EUI 64-bit address
- uint16 tagShortAdd ; // Tag's short address (16-bit) used when USING_64BIT_ADDR == 0
+ uint16 uwbShortAdd; // UWB's short address (16-bit) used when USING_64BIT_ADDR == 0
uint8 frameSN; // modulo 256 frame sequence number - it is incremented for each new frame transmittion
uint16 panID ; // panid used in the frames
- uint8 relpyAddress[8] ; // address of the anchor the tag is ranging with
+ 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
@@ -460,9 +461,7 @@ typedef struct
//diagnostic counters/data, results and logging
-
- // int32 tof32 ;
- int64 tof[TAG_LIST_SIZE] ;
+ int64 tof[UWB_LIST_SIZE] ;
double clockOffset ;
//counts for debug
@@ -478,24 +477,27 @@ typedef struct
int tofIndex ;
int tofCount ;
- double idistance ; // instantaneous distance
+ uint8 newRangeUWBIndex;
int newRange;
int newRangeAncAddress; //last 4 bytes of anchor address
int newRangeTagAddress; //last 4 bytes of tag address
+ double idistance[UWB_LIST_SIZE];
+ double idistanceraw[UWB_LIST_SIZE];
+
//if set to 1 then it means that DW1000 is in DEEP_SLEEP
//so the ranging has finished and micro can output on USB/LCD
//if sending data to LCD during ranging this limits the speed of ranging
uint8 canPrintInfo ;
- uint8 tagToRangeWith; //it is the index of the tagList array which contains the address of the Tag we are ranging with
- uint8 tagListLen ;
- uint8 prevTagToRangeWith ;
- uint8 tagList[TAG_LIST_SIZE][8];
+ uint8 uwbToRangeWith; //it is the index of the uwbList array which contains the address of the UWB we are ranging with
+ uint8 uwbListLen ;
+ // uint8 prevTagToRangeWith ;
+ uint8 uwbList[UWB_LIST_SIZE][8];
- // keep track of when final messages so we can drop tags that we havent communicated with in a while
- uint32 lastCommTimeStamp[TAG_LIST_SIZE] ;
- uint8 tagTimeout[TAG_LIST_SIZE] ;
+ // keep track of when final messages so we can drop uwbs that we havent communicated with in a while
+ uint32 lastCommTimeStamp[UWB_LIST_SIZE] ;
+ uint8 uwbTimeout[UWB_LIST_SIZE] ;
//event queue - used to store DW1000 events as they are processed by the dw_isr/callback functions
@@ -516,14 +518,14 @@ typedef struct
//-------------------------------------------------------------------------------------------------------------
// function to calculate and report the Time of Flight to the GUI/display
-int reportTOF(instance_data_t *inst);
+int reportTOF(instance_data_t *inst, uint8 uwb_index);
// clear the status/ranging data
void instanceclearcounts(void) ;
-void instcleartaglist(void);
-void instsettagtorangewith(int tagID);
-int instaddwaketagilist(instance_data_t *inst, uint8 *tagAddr);
-int instcheckawaketaginlist(instance_data_t *inst, uint8 *tagAddr);
-int instfindfirstawaketaginlist(instance_data_t *inst, uint8 startindex);
+void instclearuwblist(void);
+// void instsetuwbtorangewith(int uwbID);
+int instaddactivateuwbinlist(instance_data_t *inst, uint8 *uwbAddr);
+int instcheckactiveuwbinlist(instance_data_t *inst, uint8 *uwbAddr);
+int instfindfirstactiveuwbinlist(instance_data_t *inst, uint8 startindex);
void instance_readaccumulatordata(void);
//-------------------------------------------------------------------------------------------------------------
@@ -542,7 +544,7 @@ int instance_init_s(int mode);
void instance_config(instanceConfig_t *config) ;
void instancerxon(instance_data_t *inst, int delayed, uint64 delayedReceiveTime);
-void inst_processrxtimeout(instance_data_t *inst);
+void inst_processtxrxtimeout(instance_data_t *inst);
int instancesendpacket(uint16 length, uint8 txmode, uint32 dtime);
@@ -572,14 +574,14 @@ int instancegetrole(void) ;
uint32 instancereaddeviceid(void) ; // Return Device ID reg, enables validation of physical device presence
void instancerxon(instance_data_t *inst, int delayed, uint64 delayedReceiveTime);
-double instance_get_adist(void);
-double instance_get_idist(void);
-double instance_get_idistraw(void);
-int instance_get_lcount(void);
+// double instance_get_adist(void);
+double instance_get_idist(uint8 uwb_index);
+double instance_get_idistraw(uint8 uwb_index);
+// int instance_get_lcount(void);
uint64 instance_get_addr(void); //get own address (8 bytes)
-uint64 instance_get_tagaddr(void); //get tag address (8 bytes)
-uint64 instance_get_anchaddr(void); //get anchor address (that sent the ToF)
+uint64 instance_get_uwbaddr(uint8 uwb_index); //get uwb address (8 bytes)
+// uint64 instance_get_anchaddr(void); //get anchor address (that sent the ToF)
int instancenewrangeancadd(void);
int instancenewrangetagadd(void);
diff --git a/src/application/instance_common.c b/src/application/instance_common.c
index 69915eb..e45b0ff 100644
--- a/src/application/instance_common.c
+++ b/src/application/instance_common.c
@@ -14,9 +14,10 @@
#include "port.h"
#include "deca_device_api.h"
#include "deca_spi.h"
-
#include "instance.h"
+#include <inttypes.h>
+
extern void usb_run(void);
extern int usb_init(void);
extern void usb_printconfig(int, uint8*, int);
@@ -29,18 +30,11 @@ extern void send_usbmessage(uint8*, int);
// -------------------------------------------------------------------------------------------------------------------
-double inst_idist = 0;
-double inst_idistraw = 0;
-double inst_adist = 0;
-double inst_ldist = 0;
-
static instance_data_t instance_data[NUM_INST] ;
extern const uint16 rfDelays[2];
extern const tx_struct txSpectrumConfig[8];
-
-
extern double dwt_getrangebias(uint8 chan, float range, uint8 prf);
// -------------------------------------------------------------------------------------------------------------------
@@ -113,16 +107,21 @@ double convertdevicetimetosec8(uint8* dt)
}
-int reportTOF(instance_data_t *inst)
+int reportTOF(instance_data_t *inst, uint8 uwb_index)
{
+ //not TOF to report if not a uwb that we are tracking
+ if(uwb_index > UWB_LIST_SIZE)
+ {
+ return -1;
+ }
+
double distance ;
double distance_to_correct ;
double tof ;
- double ltave;
int64 tofi ;
// check for negative results and accept them making them proper negative integers
- tofi = inst->tof[inst->tagToRangeWith] ; // make it signed
+ tofi = inst->tof[uwb_index] ; // make it signed
if (tofi > 0x007FFFFFFFFF) // MP counter is 40 bits, close up TOF may be negative
{
tofi -= 0x010000000000 ; // subtract fill 40 bit range to make it negative
@@ -130,7 +129,7 @@ int reportTOF(instance_data_t *inst)
// convert to seconds (as floating point)
tof = convertdevicetimetosec(tofi); //this is divided by 4 to get single time of flight
- inst_idistraw = distance = tof * SPEED_OF_LIGHT;
+ inst->idistanceraw[uwb_index] = distance = tof * SPEED_OF_LIGHT;
#if (CORRECT_RANGE_BIAS == 1)
//for the 6.81Mb data rate we assume gating gain of 6dB is used,
@@ -157,82 +156,53 @@ int reportTOF(instance_data_t *inst)
if ((distance < 0) || (distance > 20000.000)) // discount any items with error
{
- inst_idist = 0;
+ inst->idistance[uwb_index] = 0;
return -1;
}
- inst_idist = distance;
-
- inst->longTermRangeSum+= distance ;
- inst->longTermRangeCount++ ; // for computing a long term average
- ltave = inst->longTermRangeSum / inst->longTermRangeCount ;
-
- inst_ldist = ltave ;
-
- inst->adist[inst->tofIndex++] = distance;
-
- if(inst->tofIndex == RTD_MED_SZ) inst->tofIndex = 0;
-
- if(inst->tofCount == RTD_MED_SZ)
- {
- int i;
- double avg;
-
- avg = 0;
- for(i = 0; i < inst->tofCount; i++)
- {
- avg += inst->adist[i];
- }
- avg /= inst->tofCount;
-
- inst_adist = avg ;
-
- }
- else
- inst->tofCount++;
+ inst->idistance[uwb_index] = distance;
return 0;
}// end of reportTOF
// -------------------------------------------------------------------------------------------------------------------
//
-// function to select the destination address (e.g. the address of the next anchor to poll)
+// function to add new UWBs to the uwb list or remove timeout status from UWBs already in uwb list
//
// -------------------------------------------------------------------------------------------------------------------
-// return 1 if tag added to list or already in list but woke up from timeout status
-// return 0 if tag not added list or woke up from timeout status
-int instaddwaketagilist(instance_data_t *inst, uint8 *tagAddr)
+// return 1 if UWB added to list or removed from timeout status
+// return 0 if UWB not added list or removed from timeout status
+int instaddactivateuwbinlist(instance_data_t *inst, uint8 *uwbAddr)
{
uint8 i;
uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
- //add the new Tag to the list, if not already there and there is space
- for(i=0; i<TAG_LIST_SIZE; i++)
+ //add the new UWB to the list, if not already there and there is space
+ for(i=0; i<UWB_LIST_SIZE; i++)
{
- if(memcmp(&inst->tagList[i][0], &tagAddr[0], 8) != 0)
+ if(memcmp(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) != 0)
{
- if(memcmp(&inst->tagList[i][0], &blank[0], 8) == 0) //blank entry
+ if(memcmp(&inst->uwbList[i][0], &blank[0], inst->addrByteSize) == 0) //blank entry
{
- memcpy(&inst->tagList[i][0], &tagAddr[0], 8) ;
- inst->tagListLen = i + 1 ;
- inst->tagToRangeWith = i;
- inst->tagTimeout[i] = 0;
+ memcpy(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) ;
+ inst->uwbListLen = i + 1 ;
+ inst->uwbToRangeWith = i;
+ inst->uwbTimeout[i] = 0;
return 1;
- // break;
}
}
else
{
- if(inst->tagTimeout[i])
+ if(inst->uwbTimeout[i])
{
- //tag has timed out, wake it up
- inst->tagToRangeWith = i;
- inst->tagTimeout[i] = 0;
+ //uwb has timed out, wake it up
+ inst->uwbToRangeWith = i;
+ inst->uwbTimeout[i] = 0;
return 1;
}
else
{
- //we already have this Tag in the list and it has not timed out
+ //we already have this uwb in the list and it has not timed out
break;
}
}
@@ -243,34 +213,34 @@ int instaddwaketagilist(instance_data_t *inst, uint8 *tagAddr)
// -------------------------------------------------------------------------------------------------------------------
//
-// function to check if a tag is already in our list and not in a timeout status
+// function to check if a UWB is already in our list and not in a timeout status
//
// -------------------------------------------------------------------------------------------------------------------
-// return index if tag in list and not timed out
-// return 255 if tag not in list or is but has timed out
-int instcheckawaketaginlist(instance_data_t *inst, uint8 *tagAddr)
+// return index if UWB in list and not timed out
+// return 255 if UWB not in list or is but has timed out
+int instcheckactiveuwbinlist(instance_data_t *inst, uint8 *uwbAddr)
{
uint8 i;
uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
- //add the new Tag to the list, if not already there and there is space
- for(i=0; i<TAG_LIST_SIZE; i++)
+ //add the new UWB to the list, if not already there and there is space
+ for(i=0; i<UWB_LIST_SIZE; i++)
{
- if(memcmp(&inst->tagList[i][0], &tagAddr[0], 8) == 0)
+ if(memcmp(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) == 0)
{
- if(inst->tagTimeout[i])
+ if(inst->uwbTimeout[i])
{
- //tag in list, but timed out
+ //UWB in list, but timed out
break;
}
else
{
- return i; //we already have this Tag in the list
+ return i; //we already have this UWB in the list
}
}
else
{
- if(memcmp(&inst->tagList[i][0], &blank[0], 8) == 0) //blank entry
+ if(memcmp(&inst->uwbList[i][0], &blank[0], 8) == 0) //blank entry
{
break;
}
@@ -282,18 +252,18 @@ int instcheckawaketaginlist(instance_data_t *inst, uint8 *tagAddr)
// -------------------------------------------------------------------------------------------------------------------
//
-// function to find the first tag in our list that is not in a timeout status, starting with the given index
+// function to find the first UWB in our list that is not in a timeout status, starting with the given index
//
// -------------------------------------------------------------------------------------------------------------------
-// return index for first tag in list that is not timed out
-// return 255 if all tags in list (from the starting index) are timed out
-int instfindfirstawaketaginlist(instance_data_t *inst, uint8 startindex)
+// return index for first UWB in list that is not timed out
+// return 255 if all UWBs in list (from the starting index) are timed out
+int instfindfirstactiveuwbinlist(instance_data_t *inst, uint8 startindex)
{
uint8 i;
- for(i=startindex; i<inst->tagListLen; i++)
+ for(i=startindex; i<inst->uwbListLen; i++)
{
- if(!inst->tagTimeout[i])
+ if(!inst->uwbTimeout[i])
{
return i;
}
@@ -308,22 +278,21 @@ int instfindfirstawaketaginlist(instance_data_t *inst, uint8 startindex)
#error These functions assume one instance only
#else
-void instcleartaglist(void)
+void instclearuwbList(void)
{
int instance = 0 ;
uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
- instance_data[instance].tagListLen = 0 ;
- instance_data[instance].tagToRangeWith = 255;
- instance_data[instance].prevTagToRangeWith = 255;
- for(int i=0; i<TAG_LIST_SIZE; i++)
+ instance_data[instance].uwbListLen = 0 ;
+ instance_data[instance].uwbToRangeWith = 255;
+
+ for(int i=0; i<UWB_LIST_SIZE; i++)
{
instance_data[instance].lastCommTimeStamp[i] = 0;
- instance_data[instance].tagTimeout[i] = 0;
+ instance_data[instance].uwbTimeout[i] = 0;
+
+ memcpy(&instance_data[instance].uwbList[i][0], &blank[0], 8);
}
-
-
- memcpy(&instance_data[instance].tagList[0][0], &blank[0], 8);
}
@@ -395,7 +364,7 @@ void instanceclearcounts(void)
instance_data[instance].longTermRangeSum = 0;
instance_data[instance].longTermRangeCount = 0;
- instcleartaglist();
+ instclearuwbList();
} // end instanceclearcounts()
@@ -416,10 +385,13 @@ int instance_init(void)
instance_data[instance].tofIndex = 0;
instance_data[instance].tofCount = 0;
- for(uint8 i=0; i<TAG_LIST_SIZE; i++)
+ for(uint8 i=0; i<UWB_LIST_SIZE; i++)
{
instance_data[instance].tof[i] = 0;
+ instance_data[instance].idistance[i] = 0;
+ instance_data[instance].idistanceraw[i] = 0;
}
+ instance_data[instance].newRangeUWBIndex = 0;
// Reset the IC (might be needed if not getting here from POWER ON)
@@ -690,31 +662,17 @@ void instancesettagsleepdelay(int sleepdelay, int blinksleepdelay) //sleep in ms
instance_data[instance].tagBlinkSleepTime_ms = blinksleepdelay ;
}
-// -------------------------------------------------------------------------------------------------------------------
-double instance_get_ldist(void) //get long term average range
-{
- double x = inst_ldist;
-
- return (x);
-}
-int instance_get_lcount(void) //get count of ranges used for calculation of lt avg
+double instance_get_idist(uint8 uwb_index) //get instantaneous range
{
- int x = instance_data[0].longTermRangeCount;
+ double x = instance_data[0].idistance[uwb_index];
return (x);
}
-double instance_get_idist(void) //get instantaneous range
+double instance_get_idistraw(uint8 uwb_index) //get instantaneous range
{
- double x = inst_idist;
-
- return (x);
-}
-
-double instance_get_idistraw(void) //get instantaneous range
-{
- double x = inst_idistraw;
+ double x = instance_data[0].idistanceraw[uwb_index];
return (x);
}
@@ -747,23 +705,14 @@ int instance_get_rxl(void) //get number of late Tx frames
return (x);
}
-double instance_get_adist(void) //get average range
+void inst_processtxrxtimeout(instance_data_t *inst)
{
- double x = inst_adist;
-
- return (x);
-}
+ // send_statetousb(inst);
-void inst_processrxtimeout(instance_data_t *inst)
-{
- send_statetousb(inst);
-
-#ifdef USB_SUPPORT //this is set in the port.h file
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "inst_processrxtimeout(), tagToRangeWith: %i, taglistlen: %i", inst->tagToRangeWith, inst->tagListLen);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
-#endif
+ // 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
{
@@ -777,36 +726,13 @@ void inst_processrxtimeout(instance_data_t *inst)
inst->testAppState = TA_TX_SELECT ;
if(inst->previousState == TA_TXBLINK_WAIT_SEND)
{
- inst->tagToRangeWith = instfindfirstawaketaginlist(inst, 0);
- // inst->tagToRangeWith = 0;
+ inst->uwbToRangeWith = instfindfirstactiveuwbinlist(inst, 0);
}
else if(inst->previousState == TA_TXPOLL_WAIT_SEND)
{
- uint8 idx = inst->tagToRangeWith + 1;
- inst->tagToRangeWith = instfindfirstawaketaginlist(inst, idx);
- // inst->tagToRangeWith = inst->tagToRangeWith + 1;
+ uint8 idx = inst->uwbToRangeWith + 1;
+ inst->uwbToRangeWith = instfindfirstactiveuwbinlist(inst, idx);
}
-
-
- // inst->testAppState = TA_TXE_WAIT ;
-
- // if(inst->previousState == TA_TXBLINK_WAIT_SEND)
- // {
- // inst->nextState = TA_TXBLINK_WAIT_SEND ;
- // }
- // else
- // {
- // inst->nextState = TA_TXPOLL_WAIT_SEND ;
- // }
- // if(inst->mode == TAG)
- // {
- // inst->nextState = TA_TXPOLL_WAIT_SEND ;
- // }
- // else //TAG_TDOA
- // {
- // inst->nextState = TA_TXBLINK_WAIT_SEND ;
- // }
-
}
//timeout - disable the radio (if using SW timeout the rx will not be off)
@@ -830,6 +756,12 @@ 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();
+
instance_putevent(dw_event);
#if (DEEP_SLEEP == 1)
@@ -859,7 +791,6 @@ void instance_rxerrorcallback(const dwt_cb_data_t *rxd)
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) || (instance_data[instance].mode == TAG_TDOA))
if(instance_data[instance].mode == TAG)
{
dw_event.type = DWT_SIG_RX_TIMEOUT;
@@ -877,10 +808,8 @@ void instance_rxerrorcallback(const dwt_cb_data_t *rxd)
void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
{
- uint8 debug_msg[100];
- // int n = sprintf((char*)&debug_msg[0], "RX CALLBACK");
- // send_usbmessage(&debug_msg[0], n);
- // usb_run();
+// uint8 debug_msg[200];
+// int n = 0;
int instance = 0;
uint8 rxTimeStamp[5] = {0, 0, 0, 0, 0};
@@ -888,9 +817,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
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;
@@ -903,13 +831,13 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
if(rxd->datalength == 12)
{
rxd_event = DWT_SIG_RX_BLINK;
- int n = sprintf((char*)&debug_msg[0], "RX CALLBACK: DWT_SIG_RX_BLINK");
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+ // 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;
-
+
break;
//ACK type frame - not supported in this SW - set as unknown (re-enable RX)
@@ -924,6 +852,7 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
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)
{
@@ -947,7 +876,9 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
{
srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_S;
}
+
}
+
break;
//any other frame types are not supported by this application
@@ -976,323 +907,194 @@ 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
//----------------------------------------------------------------------------------------------
-
- //TODO need some way to drop tags from list after a certain period of time
+ //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;
+ }
+ }
- int tag_index = 255;
+
+ int uwb_index = 255;
if(rxd_event == DWT_SIG_RX_BLINK)
{
- int n = sprintf((char*)&debug_msg[0], "RX CALLBACK RECEIVED: BLINK");
- send_usbmessage(&debug_msg[0], n);
- usb_run();
-
- tag_index = instcheckawaketaginlist(&instance_data[instance], &dw_event.msgu.rxblinkmsg.tagID[0]);
+
+ 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
+ else if(rxd_event == DWT_SIG_RX_OKAY)
{
- int n = sprintf((char*)&debug_msg[0], "RX CALLBACK RECEIVED: DWT_SIG_RX_OKAY-%s", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]));
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+
+ uwb_index = instcheckactiveuwbinlist(&instance_data[instance], &dw_event.msgu.frame[srcAddr_index]);
+
- tag_index = instcheckawaketaginlist(&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();
+
}
- // instcheckawaketaginlist(&instance_data[instance], &dw_event.msgu.frame[srcAddr_index]);
- // int n = sprintf((char*)&debug_msg[0], "RX callback: tag_index: %i, tagToRangeWith: %i", tag_index, instance_data[instance].tagToRangeWith);
- // send_usbmessage(&debug_msg[0], n);
- // usb_run();
- // select a tag to range with if certain messages are received when not currently ranging with another tag
- if (instance_data[instance].tagToRangeWith == 255)
+ // 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(instance_data[instance].mode == ANCHOR)
- // {
- // if(rxd_event == DWT_SIG_RX_BLINK)
- // {
-
- // }
- // else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_POLL)
- // {
-
- // }
- // }
- // else if(instance_data[instance].mode == TAG)
- // {
- // if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_INIT)
- // {
- // if(instaddwaketagilist(&instance_data[instance], &dw_event.msgu.frame[srcAddr_index]))
- // {
- // tag_index = instance_data[instance].tagToRangeWith;
- // }
- // else
- // {
- // tag_index = 255;
- // instance_data[instance].tagToRangeWith == 255;
- // }
- // }
- // }
-
-
- if(tag_index == 255) //tag not yet in list, or timed out
+ 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(instaddwaketagilist(&instance_data[instance], &dw_event.msgu.frame[srcAddr_index]))
+ if(instaddactivateuwbinlist(&instance_data[instance], &dw_event.msgu.frame[srcAddr_index]))
{
- tag_index = instance_data[instance].tagToRangeWith;
+ uwb_index = instance_data[instance].uwbToRangeWith;
}
else
{
- tag_index = 255;
- instance_data[instance].tagToRangeWith = 255;
+ uwb_index = 255;
+ instance_data[instance].uwbToRangeWith = 255;
}
}
else if(rxd_event == DWT_SIG_RX_BLINK && instance_data[instance].mode == ANCHOR)
{
//only process blinks from tags not already in our list, or those that have timed out
- if(instaddwaketagilist(&instance_data[instance], &dw_event.msgu.rxblinkmsg.tagID[0]))
+ if(instaddactivateuwbinlist(&instance_data[instance], &dw_event.msgu.rxblinkmsg.tagID[0]))
{
- tag_index = instance_data[instance].tagToRangeWith;
+ uwb_index = instance_data[instance].uwbToRangeWith;
}
else
{
- tag_index = 255;
- instance_data[instance].tagToRangeWith = 255;
+ uwb_index = 255;
+ instance_data[instance].uwbToRangeWith = 255;
}
}
}
- else //tag_index != 255 tag already in list and not timed out
+ 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].tagToRangeWith = tag_index;
+ instance_data[instance].uwbToRangeWith = uwb_index;
}
}
}
-
- int n = sprintf((char*)&debug_msg[0], "RX callback: tag_index: %i, tagToRangeWith: %i", tag_index, instance_data[instance].tagToRangeWith);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
-
-
-
int event_placed = 0;
- if(tag_index != 255 && instance_data[instance].tagToRangeWith == tag_index)
+ if(uwb_index != 255 && instance_data[instance].uwbToRangeWith == uwb_index)
{
//update the timestamp for the most recent communication
- instance_data[instance].lastCommTimeStamp[tag_index] = portGetTickCnt();
+ instance_data[instance].lastCommTimeStamp[uwb_index] = portGetTickCnt();
if(rxd_event == DWT_SIG_RX_OKAY)
{
- // int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: DWT_SIG_RX_OKAY-%s", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]));
- // send_usbmessage(&debug_msg[0], n);
- // usb_run();
-
- if(instance_data[instance].tagListLen > 0) //NOTE shouldn't need this check if here.
- {
- //process RTLS_DEMO_MSG_TAG_POLL immediately.
- if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_POLL)
- {
- uint16 frameLength = 0;
+ //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
+ 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
+ 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;
+ instance_data[instance].delayedReplyTime = 0;
#endif
#if (USING_64BIT_ADDR == 1)
- frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
- memcpy(&instance_data[instance].msg[tag_index].destAddr[0], &dw_event.msgu.frame[srcAddr_index], ADDR_BYTE_SIZE_L); //remember who to send the reply to (set destination address)
+ 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;
- memcpy(&instance_data[instance].msg[tag_index].destAddr[0], &dw_event.msgu.frame[srcAddr_index], ADDR_BYTE_SIZE_S); //remember who to send the reply to (set destination address)
+ frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
#endif
- // Write calculated TOF into response message
- memcpy(&(instance_data[instance].msg[tag_index].messageData[TOFR]), &instance_data[instance].tof[tag_index], 5);
-
- instance_data[instance].tof[tag_index] = 0; //clear ToF ..
+ 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);
- instance_data[instance].msg[tag_index].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)
+ //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)
- //response is expected
- instance_data[instance].wait4ack = DWT_RESPONSE_EXPECTED;
+ //response is expected
+ instance_data[instance].wait4ack = DWT_RESPONSE_EXPECTED;
- dwt_writetxfctrl(frameLength, 0, 1);
- dwt_writetxdata(frameLength, (uint8 *) &instance_data[instance].msg[tag_index], 0) ; // write the frame data
+ 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_rxmsgtousb("RX message: DWT_SIG_RX_OKAY-RTLS_DEMO_MSG_TAG_POLL ");
- send_txmsgtousb(get_msg_fcode_string((int)instance_data[instance].msg[tag_index].messageData[FCODE]));
-
+ //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);
+ 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
+ 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.
- }
+ {
+ 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)
{
- int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: BLINK");
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+ // 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
}
}
- //NOTE maybe don't want to do this. maybe just process actual error signals... or dont process errors at all.
- //treat as error
if(!event_placed)
{
- instance_rxerrorcallback(rxd);
- //instancerxon(&instance_data[instance], 0, 0); //immediate enable
+ // instance_rxerrorcallback(rxd);
+ instancerxon(&instance_data[instance], 0, 0); //immediately reenable RX
}
-
-
-// //Process good frames
-// // only process DWT_SIG_RX_OKAY events for the selected tag in our list
-// if(rxd_event == DWT_SIG_RX_OKAY & tag_index != 255 & instance_data[instance].tagToRangeWith == tag_index)
-// // if(rxd_event == DWT_SIG_RX_OKAY)
-// {
-// int n = sprintf((char*)&debug_msg[0], "RX CALLBACK: DWT_SIG_RX_OKAY-%s", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]));
-// send_usbmessage(&debug_msg[0], n);
-// usb_run();
-
-// //check if this is a TWR message (and also which one)
-// if(instance_data[instance].tagListLen > 0)
-// {
-// switch(dw_event.msgu.frame[fcode_index])
-// {
-
-// case RTLS_DEMO_MSG_TAG_POLL:
-// {
-// // if(tag_index != 255 & tag_index == instance_data[instance].tagToRangeWith)
-// // {
-// 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;
-// memcpy(&instance_data[instance].msg.destAddr[0], &dw_event.msgu.frame[srcAddr_index], ADDR_BYTE_SIZE_L); //remember who to send the reply to (set destination address)
-// #else
-// frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
-// memcpy(&instance_data[instance].msg.destAddr[0], &dw_event.msgu.frame[srcAddr_index], ADDR_BYTE_SIZE_S); //remember who to send the reply to (set destination address)
-// #endif
-// // Write calculated TOF into response message
-// memcpy(&(instance_data[instance].msg.messageData[TOFR]), &instance_data[instance].tof, 5);
-
-// instance_data[instance].tof = 0; //clear ToF ..
-
-// instance_data[instance].msg.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)
-
-// //response is expected
-// instance_data[instance].wait4ack = DWT_RESPONSE_EXPECTED;
-
-// dwt_writetxfctrl(frameLength, 0, 1);
-// dwt_writetxdata(frameLength, (uint8 *) &instance_data[instance].msg, 0) ; // write the frame data
-
-// //report out which message is being sent!
-// send_rxmsgtousb("RX message: DWT_SIG_RX_OKAY-RTLS_DEMO_MSG_TAG_POLL ");
-// send_txmsgtousb(get_msg_fcode_string((int)instance_data[instance].msg.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.
-// }
-// // }
-// }
-// break;
-
-// case RTLS_DEMO_MSG_ANCH_RESP:
-// default: //process rx frame in the application state machine
-// break;
-// }
-// }
-
-// instance_putevent(dw_event);
-
-// #if (DEEP_SLEEP == 1)
-// if (instance_data[instance].sleepingEabled)
-// instance_data[instance].rxmsgcount++;
-// #endif
-// }
-// else if (rxd_event == DWT_SIG_RX_BLINK)// & tag_index == 255) //only accept blinks from tags that aren't already in our taglist
-// {
-
-// instance_putevent(dw_event);
-
-// #if (DEEP_SLEEP == 1)
-// if (instance_data[instance].sleepingEabled)
-// instance_data[instance].rxmsgcount++;
-// #endif
-// }
-// else
-// //if (rxd_event == SIG_RX_UNKNOWN) //need to re-enable the rx
-// {
-// instance_rxerrorcallback(rxd);
-// }
-
-
-
}
@@ -1329,8 +1131,6 @@ event_data_t* instance_getevent(int x)
int instance = 0;
int indexOut = instance_data[instance].dweventIdxOut;
- instance_data_t idt = instance_data[instance];
-
if(instance_data[instance].dwevent[indexOut].type == 0) //exit with "no event"
{
dw_event_g.type = 0;
@@ -1386,42 +1186,32 @@ int instance_run(void)
int done = INST_NOT_DONE_YET;
int message = instance_peekevent(); //get any of the received events from ISR
- // send_rxmsgtousb("Instance Run 1");
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], message) ; // run the communications application
- if(done == INST_DONE_WAIT_FOR_NEXT_EVENT)
- {
- // uint8 debug_msg[100];
- // int n = sprintf((char*)&debug_msg[0], "done == INST_DONE_WAIT_FOR_NEXT_EVENT");
- // send_usbmessage(&debug_msg[0], n);
- // usb_run();
- // send_rxmsgtousb("done == INST_DONE_WAIT_FOR_NEXT_EVENT");
-
- }
//we've processed message
message = 0;
}
- //check if lastCommTimeStamp has expired for any of the tags in our list
- for(int i=0; i < instance_data[instance].tagListLen; i++)
+ //check if lastCommTimeStamp has expired for any of the uwbs in our list
+ for(int i=0; i < instance_data[instance].uwbListLen; i++)
{
- if(instance_data[instance].lastCommTimeStamp[i] + TAG_COMM_TIMEOUT < portGetTickCnt())
+ if(instance_data[instance].lastCommTimeStamp[i] + UWB_COMM_TIMEOUT < portGetTickCnt())
{
- if(instance_data[instance].tagTimeout[i] == 0)
+ if(instance_data[instance].uwbTimeout[i] == 0)
{
- instance_data[instance].tagTimeout[i] = 1;
+ instance_data[instance].uwbTimeout[i] = 1;
- uint8 debug_msg[100];
- int n = sprintf((char*)&debug_msg[0], "TIMEOUT: tag %i", i);
- send_usbmessage(&debug_msg[0], n);
- usb_run();
+ // 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].tagToRangeWith == i)
+ if(instance_data[instance].uwbToRangeWith == i)
{
- instance_data[instance].tagToRangeWith = 255;
+ instance_data[instance].uwbToRangeWith = 255;
//NOTE this might need to be changed for TAG operations
if(instance_data[instance].mode == ANCHOR)
{
diff --git a/src/decadriver/deca_device.c b/src/decadriver/deca_device.c
index 1a79588..45d985e 100644
--- a/src/decadriver/deca_device.c
+++ b/src/decadriver/deca_device.c
@@ -584,8 +584,8 @@ void dwt_settxantennadelay(uint16 txDelay)
* standard PHR mode allows up to 127 bytes
* if > 127 is programmed, DWT_PHRMODE_EXT needs to be set in the phrMode configuration
* see dwt_configure function
- * @param txFrameBytes - Pointer to the user’s buffer containing the data to send.
- * @param txBufferOffset - This specifies an offset in the DW1000’s TX Buffer at which to start writing data.
+ * @param txFrameBytes - Pointer to the user�s buffer containing the data to send.
+ * @param txBufferOffset - This specifies an offset in the DW1000�s TX Buffer at which to start writing data.
*
* output parameters
*
@@ -2625,7 +2625,7 @@ void dwt_syncrxbufptrs(void)
* @param enable - 1 to enable SNIFF mode, 0 to disable. When 0, all other parameters are not taken into account.
* @param timeOn - duration of receiver ON phase, expressed in multiples of PAC size. The counter automatically adds 1 PAC
* size to the value set. Min value that can be set is 1 (i.e. an ON time of 2 PAC size), max value is 15.
- * @param timeOff - duration of receiver OFF phase, expressed in multiples of 128/125 µs (~1 µs). Max value is 255.
+ * @param timeOff - duration of receiver OFF phase, expressed in multiples of 128/125 �s (~1 �s). Max value is 255.
*
* output parameters
*
@@ -2705,9 +2705,9 @@ void dwt_setlowpowerlistening(int enable)
* @brief Set duration of "short sleep" phase when in low-power listening mode.
*
* input parameters:
- * @param snooze_time - "short sleep" phase duration, expressed in multiples of 512/19.2 µs (~26.7 µs). The counter
+ * @param snooze_time - "short sleep" phase duration, expressed in multiples of 512/19.2 �s (~26.7 �s). The counter
* automatically adds 1 to the value set. The smallest working value that should be set is 1,
- * i.e. giving a snooze time of 2 units (or ~53 µs).
+ * i.e. giving a snooze time of 2 units (or ~53 �s).
*
* output parameters
*
@@ -3237,7 +3237,6 @@ uint8 dwt_readwakeupvbat(void)
return dwt_read8bitoffsetreg(TX_CAL_ID, TC_SARL_SAR_LVBAT_OFFSET);
}
-
/* ===============================================================================================
List of expected (known) device ID handled by this software
===============================================================================================
diff --git a/src/platform/port.c b/src/platform/port.c
index 42de344..71f5bae 100644
--- a/src/platform/port.c
+++ b/src/platform/port.c
@@ -66,6 +66,7 @@ int SysTick_Configuration(void)
while (1);
}
NVIC_SetPriority (SysTick_IRQn, 5);
+ // NVIC_SetPriority (SysTick_IRQn, 1);
return 0;
}
diff --git a/stdio/printf.c b/stdio/printf.c
deleted file mode 100644
index 8ec2e4c..0000000
--- a/stdio/printf.c
+++ /dev/null
@@ -1,559 +0,0 @@
-/**************************************************************************//*****
- * @file printf.c
- * @brief Implementation of several stdio.h methods, such as printf(),
- * sprintf() and so on. This reduces the memory footprint of the
- * binary when using those methods, compared to the libc implementation.
- ********************************************************************************/
-#include <stdio.h>
-#include <stdarg.h>
-
-extern SH_SendChar(int c);
-
-/**
- * @brief Transmit a char, if you want to use printf(),
- * you need implement this function
- *
- * @param pStr Storage string.
- * @param c Character to write.
- */
-void PrintChar(char c)
-{
- SH_SendChar(c);
- /* Send a char like:
- while(Transfer not completed);
- Transmit a char;
- */
-}
-
-/** Maximum string size allowed (in bytes). */
-#define MAX_STRING_SIZE 100
-
-
-/** Required for proper compilation. */
-struct _reent r = {0, (FILE *) 0, (FILE *) 1, (FILE *) 0};
-//struct _reent *_impure_ptr = &r;
-
-/**
- * @brief Writes a character inside the given string. Returns 1.
- *
- * @param pStr Storage string.
- * @param c Character to write.
- */
-signed int PutChar(char *pStr, char c)
-{
- *pStr = c;
- return 1;
-}
-
-
-/**
- * @brief Writes a string inside the given string.
- *
- * @param pStr Storage string.
- * @param pSource Source string.
- * @return The size of the written
- */
-signed int PutString(char *pStr, const char *pSource)
-{
- signed int num = 0;
-
- while (*pSource != 0) {
-
- *pStr++ = *pSource++;
- num++;
- }
-
- return num;
-}
-
-
-/**
- * @brief Writes an unsigned int inside the given string, using the provided fill &
- * width parameters.
- *
- * @param pStr Storage string.
- * @param fill Fill character.
- * @param width Minimum integer width.
- * @param value Integer value.
- */
-signed int PutUnsignedInt(
- char *pStr,
- char fill,
- signed int width,
- unsigned int value)
-{
- signed int num = 0;
-
- /* Take current digit into account when calculating width */
- width--;
-
- /* Recursively write upper digits */
- if ((value / 10) > 0) {
-
- num = PutUnsignedInt(pStr, fill, width, value / 10);
- pStr += num;
- }
-
- /* Write filler characters */
- else {
-
- while (width > 0) {
-
- PutChar(pStr, fill);
- pStr++;
- num++;
- width--;
- }
- }
-
- /* Write lower digit */
- num += PutChar(pStr, (value % 10) + '0');
-
- return num;
-}
-
-
-/**
- * @brief Writes a signed int inside the given string, using the provided fill & width
- * parameters.
- *
- * @param pStr Storage string.
- * @param fill Fill character.
- * @param width Minimum integer width.
- * @param value Signed integer value.
- */
-signed int PutSignedInt(
- char *pStr,
- char fill,
- signed int width,
- signed int value)
-{
- signed int num = 0;
- unsigned int absolute;
-
- /* Compute absolute value */
- if (value < 0) {
-
- absolute = -value;
- }
- else {
-
- absolute = value;
- }
-
- /* Take current digit into account when calculating width */
- width--;
-
- /* Recursively write upper digits */
- if ((absolute / 10) > 0) {
-
- if (value < 0) {
-
- num = PutSignedInt(pStr, fill, width, -(absolute / 10));
- }
- else {
-
- num = PutSignedInt(pStr, fill, width, absolute / 10);
- }
- pStr += num;
- }
- else {
-
- /* Reserve space for sign */
- if (value < 0) {
-
- width--;
- }
-
- /* Write filler characters */
- while (width > 0) {
-
- PutChar(pStr, fill);
- pStr++;
- num++;
- width--;
- }
-
- /* Write sign */
- if (value < 0) {
-
- num += PutChar(pStr, '-');
- pStr++;
- }
- }
-
- /* Write lower digit */
- num += PutChar(pStr, (absolute % 10) + '0');
-
- return num;
-}
-
-
-/**
- * @brief Writes an hexadecimal value into a string, using the given fill, width &
- * capital parameters.
- *
- * @param pStr Storage string.
- * @param fill Fill character.
- * @param width Minimum integer width.
- * @param maj Indicates if the letters must be printed in lower- or upper-case.
- * @param value Hexadecimal value.
- *
- * @return The number of char written
- */
-signed int PutHexa(
- char *pStr,
- char fill,
- signed int width,
- unsigned char maj,
- unsigned int value)
-{
- signed int num = 0;
-
- /* Decrement width */
- width--;
-
- /* Recursively output upper digits */
- if ((value >> 4) > 0) {
-
- num += PutHexa(pStr, fill, width, maj, value >> 4);
- pStr += num;
- }
- /* Write filler chars */
- else {
-
- while (width > 0) {
-
- PutChar(pStr, fill);
- pStr++;
- num++;
- width--;
- }
- }
-
- /* Write current digit */
- if ((value & 0xF) < 10) {
-
- PutChar(pStr, (value & 0xF) + '0');
- }
- else if (maj) {
-
- PutChar(pStr, (value & 0xF) - 10 + 'A');
- }
- else {
-
- PutChar(pStr, (value & 0xF) - 10 + 'a');
- }
- num++;
-
- return num;
-}
-
-
-
-/* Global Functions ----------------------------------------------------------- */
-
-
-/**
- * @brief Stores the result of a formatted string into another string. Format
- * arguments are given in a va_list instance.
- *
- * @param pStr Destination string.
- * @param length Length of Destination string.
- * @param pFormat Format string.
- * @param ap Argument list.
- *
- * @return The number of characters written.
- */
-signed int vsnprintf(char *pStr, size_t length, const char *pFormat, va_list ap)
-{
- char fill;
- unsigned char width;
- signed int num = 0;
- signed int size = 0;
-
- /* Clear the string */
- if (pStr) {
-
- *pStr = 0;
- }
-
- /* Phase string */
- while (*pFormat != 0 && size < length) {
-
- /* Normal character */
- if (*pFormat != '%') {
-
- *pStr++ = *pFormat++;
- size++;
- }
- /* Escaped '%' */
- else if (*(pFormat+1) == '%') {
-
- *pStr++ = '%';
- pFormat += 2;
- size++;
- }
- /* Token delimiter */
- else {
-
- fill = ' ';
- width = 0;
- pFormat++;
-
- /* Parse filler */
- if (*pFormat == '0') {
-
- fill = '0';
- pFormat++;
- }
-
- /* Parse width */
- while ((*pFormat >= '0') && (*pFormat <= '9')) {
-
- width = (width*10) + *pFormat-'0';
- pFormat++;
- }
-
- /* Check if there is enough space */
- if (size + width > length) {
-
- width = length - size;
- }
-
- /* Parse type */
- switch (*pFormat) {
- case 'd':
- case 'i': num = PutSignedInt(pStr, fill, width, va_arg(ap, signed int)); break;
- case 'u': num = PutUnsignedInt(pStr, fill, width, va_arg(ap, unsigned int)); break;
- case 'x': num = PutHexa(pStr, fill, width, 0, va_arg(ap, unsigned int)); break;
- case 'X': num = PutHexa(pStr, fill, width, 1, va_arg(ap, unsigned int)); break;
- case 's': num = PutString(pStr, va_arg(ap, char *)); break;
- case 'c': num = PutChar(pStr, va_arg(ap, unsigned int)); break;
- default:
- return EOF;
- }
-
- pFormat++;
- pStr += num;
- size += num;
- }
- }
-
- /* NULL-terminated (final \0 is not counted) */
- if (size < length) {
-
- *pStr = 0;
- }
- else {
-
- *(--pStr) = 0;
- size--;
- }
-
- return size;
-}
-
-
-/**
- * @brief Stores the result of a formatted string into another string. Format
- * arguments are given in a va_list instance.
- *
- * @param pStr Destination string.
- * @param length Length of Destination string.
- * @param pFormat Format string.
- * @param ... Other arguments
- *
- * @return The number of characters written.
- */
-signed int snprintf(char *pString, size_t length, const char *pFormat, ...)
-{
- va_list ap;
- signed int rc;
-
- va_start(ap, pFormat);
- rc = vsnprintf(pString, length, pFormat, ap);
- va_end(ap);
-
- return rc;
-}
-
-
-/**
- * @brief Stores the result of a formatted string into another string. Format
- * arguments are given in a va_list instance.
- *
- * @param pString Destination string.
- * @param length Length of Destination string.
- * @param pFormat Format string.
- * @param ap Argument list.
- *
- * @return The number of characters written.
- */
-signed int vsprintf(char *pString, const char *pFormat, va_list ap)
-{
- return vsnprintf(pString, MAX_STRING_SIZE, pFormat, ap);
-}
-
-/**
- * @brief Outputs a formatted string on the given stream. Format arguments are given
- * in a va_list instance.
- *
- * @param pStream Output stream.
- * @param pFormat Format string
- * @param ap Argument list.
- */
-signed int vfprintf(FILE *pStream, const char *pFormat, va_list ap)
-{
- char pStr[MAX_STRING_SIZE];
- char pError[] = "stdio.c: increase MAX_STRING_SIZE\n\r";
-
- /* Write formatted string in buffer */
- if (vsprintf(pStr, pFormat, ap) >= MAX_STRING_SIZE) {
-
- fputs(pError, stderr);
- while (1); /* Increase MAX_STRING_SIZE */
- }
-
- /* Display string */
- return fputs(pStr, pStream);
-}
-
-
-/**
- * @brief Outputs a formatted string on the DBGU stream. Format arguments are given
- * in a va_list instance.
- *
- * @param pFormat Format string.
- * @param ap Argument list.
- */
-signed int vprintf(const char *pFormat, va_list ap)
-{
- return vfprintf(stdout, pFormat, ap);
-}
-
-
-/**
- * @brief Outputs a formatted string on the given stream, using a variable
- * number of arguments.
- *
- * @param pStream Output stream.
- * @param pFormat Format string.
- */
-signed int fprintf(FILE *pStream, const char *pFormat, ...)
-{
- va_list ap;
- signed int result;
-
- /* Forward call to vfprintf */
- va_start(ap, pFormat);
- result = vfprintf(pStream, pFormat, ap);
- va_end(ap);
-
- return result;
-}
-
-
-/**
- * @brief Outputs a formatted string on the DBGU stream, using a variable number of
- * arguments.
- *
- * @param pFormat Format string.
- */
-signed int printf(const char *pFormat, ...)
-{
- va_list ap;
- signed int result;
-
- /* Forward call to vprintf */
- va_start(ap, pFormat);
- result = vprintf(pFormat, ap);
- va_end(ap);
-
- return result;
-}
-
-
-/**
- * @brief Writes a formatted string inside another string.
- *
- * @param pStr torage string.
- * @param pFormat Format string.
- */
-signed int sprintf(char *pStr, const char *pFormat, ...)
-{
- va_list ap;
- signed int result;
-
- // Forward call to vsprintf
- va_start(ap, pFormat);
- result = vsprintf(pStr, pFormat, ap);
- va_end(ap);
-
- return result;
-}
-
-
-/**
- * @brief Outputs a string on stdout.
- *
- * @param pStr String to output.
- */
-signed int puts(const char *pStr)
-{
- return fputs(pStr, stdout);
-}
-
-
-/**
- * @brief Implementation of fputc using the DBGU as the standard output. Required
- * for printf().
- *
- * @param c Character to write.
- * @param pStream Output stream.
- * @param The character written if successful, or -1 if the output stream is
- * not stdout or stderr.
- */
-signed int fputc(signed int c, FILE *pStream)
-{
- if ((pStream == stdout) || (pStream == stderr)) {
-
- PrintChar(c);
-
- return c;
- }
- else {
-
- return EOF;
- }
-}
-
-
-/**
- * @brief Implementation of fputs using the DBGU as the standard output. Required
- * for printf().
- *
- * @param pStr String to write.
- * @param pStream Output stream.
- *
- * @return Number of characters written if successful, or -1 if the output
- * stream is not stdout or stderr.
- */
-signed int fputs(const char *pStr, FILE *pStream)
-{
- signed int num = 0;
-
- while (*pStr != 0) {
-
- if (fputc(*pStr, pStream) == -1) {
-
- return -1;
- }
- num++;
- pStr++;
- }
-
- return num;
-}
-
-/* --------------------------------- End Of File ------------------------------ */
--
GitLab