├── documentation ├── DecaDuinoTWR_client.png ├── DecaDuinoTWR_server.png └── src │ ├── DecaDuinoTWR_client.odp │ └── DecaDuinoTWR_server.odp ├── LICENSE ├── .gitignore ├── examples ├── DecaDuinoJSON │ ├── send_test.json │ └── DecaDuinoJSON.ino ├── SetAntennaDelayInEEPROM │ └── SetAntennaDelayInEEPROM.ino ├── GetAntennaDelayFromEEPROM │ └── GetAntennaDelayFromEEPROM.ino ├── DecaDuinoSender │ └── DecaDuinoSender.ino ├── testDecaDuino │ └── testDecaDuino.ino ├── DecaDuinoVoltageSender │ └── DecaDuinoVoltageSender.ino ├── DecaDuinoReceiverSniffer │ └── DecaDuinoReceiverSniffer.ino ├── DecaDuinoVoltageReceiver │ └── DecaDuinoVoltageReceiver.ino ├── twoMsgTwrDecaDuino_B │ └── twoMsgTwrDecaDuino_B.ino ├── DecaDuinoVoltageSenderWithOptionnalRxPeriod │ └── DecaDuinoVoltageSenderWithOptionnalRxPeriod.ino ├── DecaDuinoTWR_server │ └── DecaDuinoTWR_server.ino ├── twoMsgTwrDecaDuino_A │ └── twoMsgTwrDecaDuino_A.ino ├── DecaDuinoSDSTWR_server │ └── DecaDuinoSDSTWR_server.ino ├── DecaDuinoTWR_client │ └── DecaDuinoTWR_client.ino ├── DecaDuinoBeacon │ └── DecaDuinoBeacon.ino ├── DecaDuinoSDSTWR_client │ └── DecaDuinoSDSTWR_client.ino ├── sdsTwrDecaDuino_A_withLedStrip │ └── sdsTwrDecaDuino_A_withLedStrip.ino ├── PsdsTwrDecaDuino_B │ └── PsdsTwrDecaDuino_B.ino ├── DecaDuinoChat │ └── DecaDuinoChat.ino ├── PsdsTwrDecaDuino_A │ └── PsdsTwrDecaDuino_A.ino ├── DecaDuinoAllRangingProtocols │ └── DecaDuinoAllRangingProtocols.ino └── DecaDuinoAllRangingProtocols2 │ └── DecaDuinoAllRangingProtocols2.ino ├── Makefile └── README.md /documentation/DecaDuinoTWR_client.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/irit-rmess/DecaDuino/HEAD/documentation/DecaDuinoTWR_client.png -------------------------------------------------------------------------------- /documentation/DecaDuinoTWR_server.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/irit-rmess/DecaDuino/HEAD/documentation/DecaDuinoTWR_server.png -------------------------------------------------------------------------------- /documentation/src/DecaDuinoTWR_client.odp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/irit-rmess/DecaDuino/HEAD/documentation/src/DecaDuinoTWR_client.odp -------------------------------------------------------------------------------- /documentation/src/DecaDuinoTWR_server.odp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/irit-rmess/DecaDuino/HEAD/documentation/src/DecaDuinoTWR_server.odp -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Using DecaDuino is subject to licensing: 2 | 3 | - GPL_V3: See http://www.gnu.org/copyleft/gpl.html 4 | - Commercial: Please contact Adrien van den Bossche for Commercial Licensing 5 | 6 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.elf 2 | *.hex 3 | /.cproject 4 | /.project 5 | /.settings/language.settings.xml 6 | /.settings/org.eclipse.cdt.codan.core.prefs 7 | .settings/* 8 | examples/*/build/* 9 | examples/DecaDuinoChat/Makefile 10 | -------------------------------------------------------------------------------- /examples/DecaDuinoJSON/send_test.json: -------------------------------------------------------------------------------- 1 | { 2 | "message_type": "DecaDuino_tx", 3 | "message": { 4 | "phyPayload": "SGVsbG8gV29ybGQK", 5 | "txInfo": { 6 | "nodeID": "8f", 7 | "frequency": 3993600000, 8 | "preambleLength": 2048, 9 | "modulation": "UWB", 10 | "UWBModulationInfo": { 11 | "bandwidth": 499200, 12 | "dataRate": 110000 13 | } 14 | }, 15 | "token": 38150 16 | } 17 | } 18 | -------------------------------------------------------------------------------- /examples/SetAntennaDelayInEEPROM/SetAntennaDelayInEEPROM.ino: -------------------------------------------------------------------------------- 1 | #ifndef TEENSYDUINO 2 | #error this sketch will only work on teensyduino. However, there is an eeprom WITHIN THE DW1000 that can be used for the same purpose (and which is maybe already populated from the factory) 3 | #ifndef 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #define ANTENNA_DELAY_VALUE_TO_SET_IN_EEPROM DWM1000_DEFAULT_ANTENNA_DELAY_VALUE 10 | 11 | DecaDuino decaduino; 12 | 13 | void setup() { 14 | 15 | uint8_t buf[2]; 16 | 17 | pinMode(13, OUTPUT); 18 | SPI.setSCK(14); 19 | if ( !decaduino.init() ) { 20 | Serial.println("decaduino init failed"); 21 | while(1) { 22 | digitalWrite(13, HIGH); 23 | delay(50); 24 | digitalWrite(13, LOW); 25 | delay(50); 26 | } 27 | } 28 | 29 | // Stores antenna delay at the end of EEPROM. The two last bytes are used for DecaWiNo label, 30 | // so use n-2 and n-3 to store the antenna delay (16bit value) 31 | decaduino.encodeUint16(ANTENNA_DELAY_VALUE_TO_SET_IN_EEPROM, buf); 32 | EEPROM.write(EEPROM.length()-4, buf[0]); 33 | EEPROM.write(EEPROM.length()-3, buf[1]); 34 | } 35 | 36 | void loop() { 37 | 38 | } 39 | 40 | 41 | -------------------------------------------------------------------------------- /examples/GetAntennaDelayFromEEPROM/GetAntennaDelayFromEEPROM.ino: -------------------------------------------------------------------------------- 1 | #ifndef TEENSYDUINO 2 | #error this sketch will only work on teensyduino. However, there is an eeprom WITHIN THE DW1000 that can be used for the same purpose (and which is maybe already populated from the factory) 3 | #ifndef 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | 11 | #define ANTENNA_DELAY_VALUE_TO_SET_IN_EEPROM DWM1000_DEFAULT_ANTENNA_DELAY_VALUE 12 | 13 | DecaDuino decaduino; 14 | 15 | void setup() { 16 | 17 | uint8_t buf[2]; 18 | uint16_t antennaDelay; 19 | 20 | pinMode(13, OUTPUT); 21 | SPI.setSCK(14); 22 | if ( !decaduino.init() ) { 23 | Serial.println("decaduino init failed"); 24 | while(1) { 25 | digitalWrite(13, HIGH); 26 | delay(50); 27 | digitalWrite(13, LOW); 28 | delay(50); 29 | } 30 | } 31 | 32 | // Gets antenna delay from the end of EEPROM. The two last bytes are used for DecaWiNo label, 33 | // so use n-2 and n-3 to store the antenna delay (16bit value) 34 | buf[0] = EEPROM.read(EEPROM.length()-4); 35 | buf[1] = EEPROM.read(EEPROM.length()-3); 36 | antennaDelay = decaduino.decodeUint16(buf); 37 | 38 | delay(1000); // 39 | 40 | if ( antennaDelay == 0xffff ) { 41 | Serial.println("Unvalid antenna delay value found in EEPROM. Using default value."); 42 | } else decaduino.setAntennaDelay(antennaDelay); 43 | } 44 | 45 | void loop() { 46 | 47 | } 48 | 49 | 50 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # some environment variables can be set: 2 | # 3 | # FQBN Fully Qualified Board Name; if not set in the environment 4 | # it will be assigned a value in this makefile 5 | # 6 | # CLI_ARGS Passed as arguments to arduino-cli for compilation 7 | # it will be assigned the default value of an empty string 8 | # 9 | # LIBRARIES arduino libraries path 10 | # 11 | # 12 | FQBN ?= RMESS:nRF5:DWM1001-DEV 13 | LIBRARIES ?= 14 | CLI_ARGS ?= 15 | 16 | SRC := $(wildcard *.cpp) 17 | HDRS := $(wildcard *.h) 18 | 19 | ifneq ($(strip $(LIBRARIES)),) 20 | LIBARGS = --libraries $(LIBRARIES) 21 | else 22 | LIBARGS = 23 | endif 24 | 25 | OUT_PATH = 26 | 27 | $(info FQBN is [${FQBN}]) 28 | $(info CLI_ARGS is [${CLI_ARGS}]) 29 | $(info LIBRAIRIES is [${LIBRARIES}]) 30 | $(info SRC is [${SRC}]) 31 | $(info HDRS is [${HDRS}]) 32 | 33 | all: 34 | .PHONY: all 35 | 36 | clean: 37 | @echo "---> Cleaning the examples directory" 38 | @for f in $(shell ls examples/); do echo examples/$${f}/ ; rm -rf examples/$${f}/build examples/$${f}/*.elf ; done 39 | 40 | .SECONDEXPANSION: # required for secondary expansions of $$* 41 | % : examples/$$*/$$*.elf 42 | 43 | 44 | .SECONDARY: 45 | %.elf : $$*.ino $(SRC) $(HDRS) 46 | $(eval OUT_PATH := $(shell dirname $@)) 47 | $(eval BASENAME := $(shell basename $@)) 48 | arduino-cli compile -b $(FQBN) $(CLI_ARGS) $(LIBARGS) -e $*.ino 49 | mv $(OUT_PATH)/build/$(subst :,.,$(FQBN))/*.elf $@ 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /examples/DecaDuinoSender/DecaDuinoSender.ino: -------------------------------------------------------------------------------- 1 | // DecaDuinoSender 2 | // This sketch shows how to use the DecaDuino library to send messages over the UWB radio 3 | // by Adrien van den Bossche 4 | // This sketch is a part of the DecaDuino Project - please refer to the DecaDuino LICENCE file for licensing details 5 | 6 | #include 7 | #include 8 | 9 | #define MAX_FRAME_LEN 120 10 | uint8_t txData[MAX_FRAME_LEN]; 11 | uint16_t txLen; 12 | #ifdef ARDUINO_DWM1001_DEV 13 | DecaDuino decaduino(SS1, DW_IRQ); 14 | #define LED_ON LOW 15 | #define LED_OFF HIGH 16 | #else 17 | DecaDuino decaduino; 18 | #define LED_ON HIGH 19 | #define LED_OFF LOW 20 | #endif 21 | 22 | void setup() 23 | { 24 | pinMode(LED_BUILTIN, OUTPUT); 25 | Serial.begin(115200); // Init Serial port 26 | #ifndef ARDUINO_DWM1001_DEV 27 | SPI.setSCK(14); // Set SPI clock pin (pin 14 on DecaWiNo board) 28 | #endif 29 | 30 | // Init DecaDuino and blink if initialisation fails 31 | if ( !decaduino.init() ) { 32 | Serial.println("decaduino init failed"); 33 | while(1) { digitalWrite(LED_BUILTIN, LED_ON); delay(50); digitalWrite(LED_BUILTIN, LED_OFF); delay(50); } 34 | } 35 | } 36 | 37 | 38 | void loop() 39 | { 40 | decaduino.engine(); 41 | 42 | // make dummy data, send it and wait the end of the transmission. 43 | digitalWrite(LED_BUILTIN, LED_ON); 44 | for (int i=0; i 2 | #include 3 | 4 | int i; 5 | int rxFrames; 6 | #ifdef UWB_MODULE_DWM1001 7 | DecaDuino decaduino(SS1, DW_IRQ); 8 | #elif defined(TEENSYDUINO) 9 | DecaDuino decaduino; 10 | #endif 11 | uint8_t txData[128]; 12 | uint8_t rxData[128]; 13 | uint16_t rxLen; 14 | #define FRAME_LEN 64 15 | 16 | #define SENDER 17 | // ou 18 | //#define RECEIVER 19 | 20 | void setup() { 21 | Serial.begin(115200); 22 | 23 | pinMode(LED_BUILTIN, OUTPUT); 24 | #ifdef TEENSYDUINO 25 | SPI.setSCK(14); 26 | delay(250); // delay for wino serial setup 27 | #endif 28 | if ( !decaduino.init() ) { 29 | while(1){ 30 | Serial.println("decaduino init failed"); 31 | digitalWrite(LED_BUILTIN, HIGH); 32 | delay(100); 33 | digitalWrite(LED_BUILTIN, LOW); 34 | delay(100); 35 | } 36 | } 37 | 38 | #ifdef RECEIVER 39 | rxFrames = 0; 40 | decaduino.setRxBuffer(rxData, &rxLen); 41 | decaduino.plmeRxEnableRequest(); 42 | 43 | #endif 44 | } 45 | 46 | void loop() { 47 | int i; 48 | decaduino.engine(); // it is required to periodically call this function 49 | 50 | #ifdef RECEIVER 51 | if ( decaduino.rxFrameAvailable() ) { 52 | digitalWrite(LED_BUILTIN, HIGH); 53 | Serial.print("["); Serial.print(++rxFrames); Serial.print("] "); 54 | Serial.print(rxLen); 55 | Serial.print("bytes received: "); 56 | for (i=0; i 4 | // This sketch is a part of the DecaDuino Project - please refer to the DecaDuino LICENCE file for licensing details 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #define MAX_FRAME_LEN 120 11 | uint8_t txData[MAX_FRAME_LEN]; 12 | uint16_t txLen; 13 | #ifdef UWB_MODULE_DWM1001 14 | DecaDuino decaduino(SS1, DW_IRQ); 15 | #define LED_RED LED_RED_TOP 16 | #ifdef ARDUINO_DWM1001_DEV 17 | #define LED_ON LOW 18 | #define LED_OFF HIGH 19 | #else 20 | #define LED_ON HIGH 21 | #define LED_OFF LOW 22 | #endif 23 | #else 24 | DecaDuino decaduino; 25 | #define LED_ON HIGH 26 | #define LED_OFF LOW 27 | #endif 28 | uint16_t nodeId; 29 | 30 | void setup() 31 | { 32 | pinMode(LED_BUILTIN, OUTPUT); 33 | pinMode(LED_RED, OUTPUT); 34 | Serial.begin(115200); // Init Serial port 35 | #ifndef UWB_MODULE_DWM1001 36 | SPI.setSCK(14); // Set SPI clock pin (pin 14 on DecaWiNo board) 37 | #endif 38 | 39 | // Init DecaDuino and blink if initialisation fails 40 | if ( !decaduino.init() ) { 41 | Serial.println("decaduino init failed"); 42 | while(1) { digitalWrite(LED_BUILTIN, LED_ON); delay(50); digitalWrite(LED_BUILTIN, LED_OFF); delay(50); } 43 | } 44 | 45 | pinMode(1, INPUT); 46 | nodeId = toNodeID(getDwMacAddrUint64()); 47 | } 48 | 49 | 50 | void loop() 51 | { 52 | decaduino.engine(); 53 | digitalWrite(LED_RED, LED_ON); 54 | float voltage = decaduino.getVoltage(); 55 | float temperature = decaduino.getTemperature(); 56 | txData[0] = 'B'; txData[1] = 'A'; txData[2] = 'T'; 57 | decaduino.encodeUint16(nodeId, &txData[3]); 58 | txData[5] = digitalRead(1); 59 | decaduino.encodeFloat(voltage, &txData[6]); 60 | decaduino.encodeFloat(temperature, &txData[10]); 61 | decaduino.pdDataRequest(txData, 14); 62 | while ( !decaduino.hasTxSucceeded() ) { decaduino.engine(); } ; 63 | digitalWrite(LED_RED, LED_OFF); 64 | // wait 2 second 65 | delay(2000); 66 | } 67 | -------------------------------------------------------------------------------- /examples/DecaDuinoReceiverSniffer/DecaDuinoReceiverSniffer.ino: -------------------------------------------------------------------------------- 1 | // DecaDuinoReceiver 2 | // This sketch shows how to use the DecaDuino library to receive messages over the UWB radio. 3 | // The sketch prints the received bytes in HEX; it can be used as a frame sniffer. 4 | // by Adrien van den Bossche 5 | // This sketch is a part of the DecaDuino Project - please refer to the DecaDuino LICENCE file for licensing details 6 | 7 | #include 8 | #include 9 | 10 | #define MAX_FRAME_LEN 120 11 | uint8_t rxData[MAX_FRAME_LEN]; 12 | uint16_t rxLen; 13 | int rxFrames; 14 | #ifdef ARDUINO_DWM1001_DEV 15 | DecaDuino decaduino(SS1, DW_IRQ); 16 | #define LED_ON LOW 17 | #define LED_OFF HIGH 18 | #else 19 | DecaDuino decaduino; 20 | #define LED_ON HIGH 21 | #define LED_OFF LOW 22 | #endif 23 | 24 | 25 | void setup() 26 | { 27 | pinMode(LED_BUILTIN, OUTPUT); // Internal LED (pin LED_BUILTIN on DecaWiNo board) 28 | Serial.begin(115200); // Init Serial port 29 | #ifndef ARDUINO_DWM1001_DEV 30 | SPI.setSCK(14); // Set SPI clock pin (pin 14 on DecaWiNo board) 31 | #endif 32 | 33 | // Init DecaDuino and blink if initialisation fails 34 | if ( !decaduino.init() ) { 35 | Serial.println("decaduino init failed"); 36 | while(1) { digitalWrite(LED_BUILTIN, LED_ON); delay(50); digitalWrite(LED_BUILTIN, LED_OFF); delay(50); } 37 | } 38 | // Set RX buffer and enable RX 39 | decaduino.setRxBuffer(rxData, &rxLen); 40 | decaduino.plmeRxEnableRequest(); 41 | rxFrames = 0; 42 | } 43 | 44 | 45 | void loop() 46 | { 47 | 48 | decaduino.engine(); 49 | 50 | // If a message has been received, print it and re-enable receiver 51 | if ( decaduino.rxFrameAvailable() ) { 52 | digitalWrite(LED_BUILTIN, LED_ON); 53 | Serial.print("#"); Serial.print(++rxFrames); Serial.print(" "); 54 | Serial.print(rxLen); 55 | Serial.print("bytes received: |"); 56 | for (int i=0; i 4 | // This sketch is a part of the DecaDuino Project - please refer to the DecaDuino LICENCE file for licensing details 5 | 6 | #include 7 | #include 8 | 9 | #define MAX_FRAME_LEN 120 10 | uint8_t rxData[MAX_FRAME_LEN]; 11 | uint16_t rxLen; 12 | int rxFrames; 13 | #ifdef UWB_MODULE_DWM1001 14 | DecaDuino decaduino(SS1, DW_IRQ); 15 | #ifdef ARDUINO_DWM1001_DEV 16 | #define LED_ON LOW 17 | #define LED_OFF HIGH 18 | #else 19 | #define LED_ON HIGH 20 | #define LED_OFF LOW 21 | #endif 22 | #else 23 | DecaDuino decaduino; 24 | #define LED_ON HIGH 25 | #define LED_OFF LOW 26 | #endif 27 | 28 | 29 | void setup() 30 | { 31 | pinMode(LED_BUILTIN, OUTPUT); // Internal LED (pin LED_BUILTIN on DecaWiNo board) 32 | pinMode(LED_GREEN, OUTPUT); // Internal LED (pin LED_BUILTIN on DecaWiNo board) 33 | Serial.begin(115200); // Init Serial port 34 | #ifndef UWB_MODULE_DWM1001 35 | SPI.setSCK(14); // Set SPI clock pin (pin 14 on DecaWiNo board) 36 | #endif 37 | 38 | // Init DecaDuino and blink if initialisation fails 39 | if ( !decaduino.init() ) { 40 | Serial.println("decaduino init failed"); 41 | while(1) { digitalWrite(LED_BUILTIN, LED_ON); delay(50); digitalWrite(LED_BUILTIN, LED_OFF); delay(50); } 42 | } 43 | // Set RX buffer and enable RX 44 | decaduino.setRxBuffer(rxData, &rxLen); 45 | decaduino.plmeRxEnableRequest(); 46 | rxFrames = 0; 47 | } 48 | 49 | 50 | void loop() 51 | { 52 | decaduino.engine(); 53 | // If a message has been received, print it and re-enable receiver 54 | if ( decaduino.rxFrameAvailable() ) 55 | { 56 | char str[16]; 57 | str[15] = 0; 58 | digitalWrite(LED_GREEN, LED_ON); 59 | 60 | if ( (rxData[0]=='B') && (rxData[1]=='A') && (rxData[2]=='T') ) 61 | { 62 | char str[256]; 63 | int inCharge; 64 | float voltage = decaduino.decodeFloat(&rxData[6]); 65 | float temperature = decaduino.decodeFloat(&rxData[10]); 66 | uint16_t nodeId = decaduino.decodeUint16(&rxData[3]); 67 | 68 | if ( rxData[5] ) inCharge = 0; 69 | else inCharge = 1; 70 | sprintf(str,"{\"id\":%d,\"volt\":%.2f,\"temp\":%.1f,\"ichrg\":%d}", nodeId, voltage, temperature, inCharge); 71 | Serial.print(str); 72 | Serial.println(""); 73 | 74 | } 75 | decaduino.plmeRxEnableRequest(); // Always renable RX after a frame reception 76 | digitalWrite(LED_GREEN, LED_OFF); 77 | } 78 | } 79 | -------------------------------------------------------------------------------- /examples/twoMsgTwrDecaDuino_B/twoMsgTwrDecaDuino_B.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define FRAME_LEN 64 5 | 6 | #define TRANSMITION_DELAY 1000000000000/15.65*0.1*5E-3 // 0.1ms 7 | 8 | #define TWR_ENGINE_STATE_INIT 1 9 | #define TWR_ENGINE_STATE_RX_ON 2 10 | #define TWR_ENGINE_STATE_WAIT_START 3 11 | #define TWR_ENGINE_STATE_MEMORISE_T2 4 12 | #define TWR_ENGINE_STATE_SEND_ACK 5 13 | #define TWR_ENGINE_STATE_WAIT_SENT 6 14 | #define TWR_ENGINE_STATE_MEMORISE_T3 7 15 | #define TWR_ENGINE_STATE_WAIT_BEFORE_SEND_DATA_REPLY 8 16 | #define TWR_ENGINE_STATE_SEND_DATA_REPLY 9 17 | 18 | #define TWR_MSG_TYPE_UNKNOWN 0 19 | #define TWR_MSG_TYPE_START 1 20 | #define TWR_MSG_TYPE_ACK 2 21 | #define TWR_MSG_TYPE_DATA_REPLY 3 22 | 23 | int i; 24 | int rxFrames; 25 | 26 | uint64_t t2, t3, t3_predicted; 27 | 28 | #ifdef ARDUINO_DWM1001_DEV 29 | DecaDuino decaduino(SS1, DW_IRQ); 30 | #elif defined(TEENSYDUINO) 31 | DecaDuino decaduino; 32 | #endif 33 | uint8_t txData[128]; 34 | uint8_t rxData[128]; 35 | uint16_t rxLen; 36 | int state; 37 | 38 | 39 | void setup() { 40 | 41 | pinMode(13, OUTPUT); 42 | #ifdef TEENSYDUINO 43 | SPI.setSCK(14); 44 | #endif 45 | if ( !decaduino.init() ) { 46 | Serial.println("decaduino init failed"); 47 | while(1) { 48 | digitalWrite(13, HIGH); 49 | delay(50); 50 | digitalWrite(13, LOW); 51 | delay(50); 52 | } 53 | } 54 | 55 | // Set RX buffer 56 | decaduino.setRxBuffer(rxData, &rxLen); 57 | state = TWR_ENGINE_STATE_INIT; 58 | } 59 | 60 | void loop() { 61 | decaduino.engine(); 62 | 63 | switch (state) { 64 | 65 | case TWR_ENGINE_STATE_INIT: 66 | //decaduino.plmeRxDisableRequest(); 67 | state = TWR_ENGINE_STATE_RX_ON; 68 | break; 69 | 70 | case TWR_ENGINE_STATE_RX_ON: 71 | decaduino.plmeRxEnableRequest(); 72 | state = TWR_ENGINE_STATE_WAIT_START; 73 | break; 74 | 75 | case TWR_ENGINE_STATE_WAIT_START: 76 | if ( decaduino.rxFrameAvailable() ) { 77 | if ( rxData[0] == TWR_MSG_TYPE_START ) { 78 | state = TWR_ENGINE_STATE_MEMORISE_T2; 79 | } else state = TWR_ENGINE_STATE_RX_ON; 80 | } 81 | break; 82 | 83 | case TWR_ENGINE_STATE_MEMORISE_T2: 84 | t2 = decaduino.getLastRxTimestamp(); 85 | state = TWR_ENGINE_STATE_SEND_ACK; 86 | break; 87 | 88 | case TWR_ENGINE_STATE_SEND_ACK: 89 | txData[0] = TWR_MSG_TYPE_ACK; 90 | t3_predicted = decaduino.alignDelayedTransmission(TRANSMITION_DELAY); 91 | decaduino.encodeUint64(t2, &txData[1]); 92 | decaduino.encodeUint64(t3_predicted, &txData[9]); 93 | decaduino.pdDataRequest(txData, 17, true, t3_predicted); 94 | state = TWR_ENGINE_STATE_WAIT_SENT; 95 | break; 96 | 97 | case TWR_ENGINE_STATE_WAIT_SENT: 98 | if ( decaduino.hasTxSucceeded() ) { 99 | t3 = decaduino.getLastTxTimestamp(); 100 | Serial.print("t3-t3_predicted="); 101 | Serial.print((int)(t3-t3_predicted)); 102 | Serial.println(); 103 | state = TWR_ENGINE_STATE_INIT; 104 | } 105 | break; 106 | 107 | default: 108 | state = TWR_ENGINE_STATE_INIT; 109 | break; 110 | } 111 | } 112 | 113 | 114 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DecaDuino 2 | 3 | DecaDuino is an Arduino library which provides a driver for the DecaWave DW1000 4 | transceiver, and modules based on this transceiver, such as DecaWave DWM1000. 5 | Since the DecaWave DW1000/DWM1000 is based on a Ultra Wide Band (UWB) Physical 6 | layer, DecaDuino can be used as an open framework for wireless Time-of-Flight 7 | (ToF) ranging systems. 8 | 9 | For more details on the DecaDuino library, get the latest version of the 10 | documentation here: https://www.irit.fr/~Adrien.Van-Den-Bossche/decaduino/) 11 | 12 | ## Installation 13 | You have to put this library in your Arduino libraries directory. 14 | 15 | ### dependencies 16 | You need to have the library printfToSerial in your Arduino libraries directory (https://github.com/quent8192/printfToSerial). 17 | 18 | ### DecaWino (Teensy) 19 | 20 | Instructions here: https://wino.cc/tutorials/teensywino-installing-software-development-tools/ 21 | 22 | ### DWM1001-DEV (nRF5x) 23 | 24 | Instructions here: https://github.com/irit-rmess/arduino-nRF5 25 | (more details in french https://octeus.iut-blagnac.fr/enseignants/octeus2/tp/micro-tp-cha%C3%AEne-de-compilation/) 26 | 27 | ## Start using DecaDuino with simple example sketchs 28 | All example sketches are available as usual in the Arduino IDE menu: 29 | ``` 30 | File > Examples > DecaDuino 31 | ``` 32 | 33 | ### DecaDuinoSender 34 | 35 | This sketch shows how to use the DecaDuino library to send messages over the UWB 36 | radio. 37 | 38 | ### DecaDuinoReceiverSniffer 39 | 40 | This sketch shows how to use the DecaDuino library to receive messages over the 41 | UWB radio. The received bytes are printed in HEX; this sketch can be used as a 42 | frame sniffer to dump received messages. 43 | 44 | ### DecaDuinoChat 45 | 46 | This sketch shows how to use the DecaDuino library to send and receive ascii 47 | messages via the Serial port over the UWB radio. Commands are available to 48 | change the radio's configuration: channel, datarate and preamble length. 49 | 50 | ### DecaDuinoJSON 51 | 52 | This sketch shows how to use the DecaDuino library to control the UWB radio 53 | with JSON over serial. The JSON format used is based on LoraServer's Gateway 54 | Bridge JSON format (https://www.loraserver.io/lora-gateway-bridge/integrate/payload-types/json/). 55 | 56 | ## Use DecaDuino to implement ranging protocols 57 | 58 | DecaDuino can be used to implement Time-of-Flight (ToF) ranging protocols 59 | by timestamping frames at transmission and reception. The DecaDuino library 60 | comes with a set of sketches that implement popular ranging protocols. 61 | 62 | ### Two-Way Ranging (TWR) protocol 63 | 64 | The two example sketchs DecaDuinoTWR_client and DecaDuinoTWR_server propose a 65 | simple implementation of the TWR protocol without addressing fields. 66 | 67 | Flash each example sketch on two nodes (client and server) and get the distance 68 | between the two nodes. The documentation directory contains the client and 69 | server state machine diagram. 70 | 71 | ### Symmetric Double-Sided Two-Way Ranging (SDS-TWR) protocol 72 | 73 | The two example sketchs DecaDuinoSDSTWR_client and DecaDuinoSDSTWR_server 74 | propose a simple implementation of the SDS-TWR protocol without addressing 75 | fields. The SDS-TWR protocol implies more messages but is theorically better 76 | than the TWR protocol since the clock skew effect is compensated by the 77 | symetry of the exchanges. 78 | 79 | Flash each example sketch on two nodes (client and server) and get the distance 80 | between the two nodes. 81 | 82 | -------------------------------------------------------------------------------- /examples/DecaDuinoVoltageSenderWithOptionnalRxPeriod/DecaDuinoVoltageSenderWithOptionnalRxPeriod.ino: -------------------------------------------------------------------------------- 1 | // DecaDuinoVoltageSenderWithOptionnalRxPeriod 2 | // This sketch shows how to use the DecaDuino library to send messages containing voltage and temperature over the UWB radio 3 | // with an optionnal short RX window after the message sending 4 | // by Adrien van den Bossche 5 | // This sketch is a part of the DecaDuino Project - please refer to the DecaDuino LICENCE file for licensing details 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #define RX_ENABLE 12 | #define RX_ENABLE_DURATION 100 13 | #define SENDING_PERIOD 2000 14 | 15 | #define MAX_FRAME_LEN 120 16 | uint8_t txData[MAX_FRAME_LEN]; 17 | uint16_t txLen; 18 | uint8_t rxData[MAX_FRAME_LEN]; 19 | uint16_t rxLen; 20 | int rxFrames; 21 | #ifdef UWB_MODULE_DWM1001 22 | DecaDuino decaduino(SS1, DW_IRQ); 23 | #ifdef ARDUINO_DWM1001_DEV 24 | #define LED_RED LED_RED_TOP 25 | #define LED_ON LOW 26 | #define LED_OFF HIGH 27 | #else 28 | #define LED_ON HIGH 29 | #define LED_OFF LOW 30 | #endif 31 | #else 32 | DecaDuino decaduino; 33 | #define LED_ON HIGH 34 | #define LED_OFF LOW 35 | #endif 36 | uint16_t nodeId; 37 | uint32_t timeToSend = 0; 38 | uint32_t timeToStopRx = 0; 39 | 40 | void setup() 41 | { 42 | pinMode(LED_BUILTIN, OUTPUT); 43 | pinMode(LED_RED, OUTPUT); 44 | pinMode(LED_GREEN, OUTPUT); // Internal LED (pin LED_BUILTIN on DecaWiNo board) 45 | 46 | Serial.begin(115200); // Init Serial port 47 | #ifndef UWB_MODULE_DWM1001 48 | SPI.setSCK(14); // Set SPI clock pin (pin 14 on DecaWiNo board) 49 | #endif 50 | 51 | // Init DecaDuino and blink if initialisation fails 52 | if ( !decaduino.init() ) { 53 | Serial.println("decaduino init failed"); 54 | while(1) { digitalWrite(LED_BUILTIN, LED_ON); delay(50); digitalWrite(LED_BUILTIN, LED_OFF); delay(50); } 55 | } 56 | 57 | pinMode(1, INPUT); 58 | nodeId = toNodeID(getDwMacAddrUint64()); 59 | 60 | #ifdef RX_ENABLE 61 | // Set RX buffer and enable RX 62 | decaduino.setRxBuffer(rxData, &rxLen); 63 | decaduino.plmeRxEnableRequest(); 64 | rxFrames = 0; 65 | #endif 66 | 67 | } 68 | 69 | 70 | void loop() 71 | { 72 | decaduino.engine(); 73 | if ( millis() >= timeToSend ) 74 | { 75 | digitalWrite(LED_RED, LED_ON); 76 | float voltage = decaduino.getVoltage(); 77 | float temperature = decaduino.getTemperature(); 78 | txData[0] = 'B'; txData[1] = 'A'; txData[2] = 'T'; 79 | decaduino.encodeUint16(nodeId, &txData[3]); 80 | txData[5] = digitalRead(1); 81 | decaduino.encodeFloat(voltage, &txData[6]); 82 | decaduino.encodeFloat(temperature, &txData[10]); 83 | #ifdef RX_ENABLE 84 | decaduino.plmeRxDisableRequest(); 85 | #ifdef RX_ENABLE_DURATION 86 | timeToStopRx = timeToSend+RX_ENABLE_DURATION; 87 | #endif 88 | #endif 89 | decaduino.pdDataRequest(txData, 14); 90 | while ( !decaduino.hasTxSucceeded() ) { decaduino.engine(); }; 91 | #ifdef RX_ENABLE 92 | decaduino.plmeRxEnableRequest(); 93 | #endif 94 | digitalWrite(LED_RED, LED_OFF); 95 | timeToSend += SENDING_PERIOD; 96 | } 97 | 98 | #ifdef RX_ENABLE_DURATION 99 | if ( millis() >= timeToStopRx ) 100 | { 101 | decaduino.plmeRxDisableRequest(); 102 | timeToStopRx += SENDING_PERIOD; 103 | } 104 | #endif 105 | 106 | #ifdef RX_ENABLE 107 | // If a message has been received, drop it and re-enable receiver 108 | if ( decaduino.rxFrameAvailable() ) 109 | { 110 | digitalWrite(LED_GREEN, LED_ON); 111 | delay(20); 112 | decaduino.plmeRxEnableRequest(); // Always renable RX after a frame reception 113 | digitalWrite(LED_GREEN, LED_OFF); 114 | } 115 | #endif 116 | } 117 | -------------------------------------------------------------------------------- /examples/DecaDuinoTWR_server/DecaDuinoTWR_server.ino: -------------------------------------------------------------------------------- 1 | // DecaDuinoTWR_server 2 | // A simple implementation of the TWR protocol, server side 3 | // Contributors: Adrien van den Bossche, Réjane Dalcé, Ibrahim Fofana, Robert Try, Thierry Val 4 | // This sketch is a part of the DecaDuino Project - please refer to the DecaDuino LICENCE file for licensing details 5 | 6 | #include 7 | #include 8 | 9 | // Timeout parameters 10 | #define TIMEOUT_WAIT_ACK_SENT 5 //ms 11 | #define TIMEOUT_WAIT_DATA_REPLY_SENT 5 //ms 12 | #define ACK_DATA_REPLY_INTERFRAME 10 //ms 13 | 14 | // TWR server states state machine enumeration: see state diagram on documentation for more details 15 | enum { TWR_ENGINE_STATE_INIT, TWR_ENGINE_STATE_WAIT_START, TWR_ENGINE_STATE_MEMORISE_T2, 16 | TWR_ENGINE_STATE_SEND_ACK, TWR_ENGINE_STATE_WAIT_ACK_SENT, TWR_ENGINE_STATE_MEMORISE_T3, 17 | TWR_ENGINE_STATE_SEND_DATA_REPLY, TWR_ENGINE_STATE_WAIT_DATA_REPLY_SENT }; 18 | 19 | // Message types of the TWR protocol 20 | #define TWR_MSG_TYPE_UNKNOWN 0 21 | #define TWR_MSG_TYPE_START 1 22 | #define TWR_MSG_TYPE_ACK 2 23 | #define TWR_MSG_TYPE_DATA_REPLY 3 24 | 25 | uint64_t t2, t3; 26 | 27 | #ifdef ARDUINO_DWM1001_DEV 28 | DecaDuino decaduino(SS1, DW_IRQ); 29 | #else 30 | DecaDuino decaduino; 31 | #endif 32 | 33 | uint8_t txData[128]; 34 | uint8_t rxData[128]; 35 | uint16_t rxLen; 36 | int state; 37 | uint32_t timeout; 38 | 39 | 40 | void setup() 41 | { 42 | pinMode(13, OUTPUT); // Internal LED (pin 13 on DecaWiNo board) 43 | Serial.begin(115200); // Init Serial port 44 | #ifndef ARDUINO_DWM1001_DEV 45 | SPI.setSCK(14); // Set SPI clock pin (pin 14 on DecaWiNo board) 46 | #endif 47 | // Init DecaDuino and blink if initialisation fails 48 | if ( !decaduino.init() ) { 49 | Serial.println("decaduino init failed"); 50 | while(1) { digitalWrite(13, HIGH); delay(50); digitalWrite(13, LOW); delay(50); } 51 | } 52 | 53 | // Set RX buffer 54 | decaduino.setRxBuffer(rxData, &rxLen); 55 | state = TWR_ENGINE_STATE_INIT; 56 | } 57 | 58 | 59 | void loop() 60 | { 61 | decaduino.engine(); 62 | 63 | switch (state) { 64 | 65 | case TWR_ENGINE_STATE_INIT: 66 | decaduino.plmeRxEnableRequest(); 67 | state = TWR_ENGINE_STATE_WAIT_START; 68 | break; 69 | 70 | case TWR_ENGINE_STATE_WAIT_START: 71 | if ( decaduino.rxFrameAvailable() ) { 72 | if ( rxData[0] == TWR_MSG_TYPE_START ) { 73 | state = TWR_ENGINE_STATE_MEMORISE_T2; 74 | } else { 75 | state = TWR_ENGINE_STATE_INIT; 76 | } 77 | } 78 | break; 79 | 80 | case TWR_ENGINE_STATE_MEMORISE_T2: 81 | t2 = decaduino.getLastRxTimestamp(); 82 | state = TWR_ENGINE_STATE_SEND_ACK; 83 | break; 84 | 85 | case TWR_ENGINE_STATE_SEND_ACK: 86 | txData[0] = TWR_MSG_TYPE_ACK; 87 | decaduino.pdDataRequest(txData, 1); 88 | timeout = millis() + TIMEOUT_WAIT_ACK_SENT; 89 | state = TWR_ENGINE_STATE_WAIT_ACK_SENT; 90 | break; 91 | 92 | case TWR_ENGINE_STATE_WAIT_ACK_SENT: 93 | if ( millis() > timeout ) { 94 | state = TWR_ENGINE_STATE_INIT; 95 | } else { 96 | if ( decaduino.hasTxSucceeded() ) { 97 | state = TWR_ENGINE_STATE_MEMORISE_T3; 98 | } 99 | } 100 | break; 101 | 102 | case TWR_ENGINE_STATE_MEMORISE_T3: 103 | t3 = decaduino.getLastTxTimestamp(); 104 | state = TWR_ENGINE_STATE_SEND_DATA_REPLY; 105 | break; 106 | 107 | case TWR_ENGINE_STATE_SEND_DATA_REPLY: 108 | delay(ACK_DATA_REPLY_INTERFRAME); 109 | txData[0] = TWR_MSG_TYPE_DATA_REPLY; 110 | decaduino.encodeUint40(t2, &txData[1]); 111 | decaduino.encodeUint40(t3, &txData[6]); 112 | decaduino.pdDataRequest(txData, 11); 113 | timeout = millis() + TIMEOUT_WAIT_DATA_REPLY_SENT; 114 | state = TWR_ENGINE_STATE_WAIT_DATA_REPLY_SENT; 115 | break; 116 | 117 | case TWR_ENGINE_STATE_WAIT_DATA_REPLY_SENT: 118 | if ( (millis()>timeout) || (decaduino.hasTxSucceeded()) ) { 119 | state = TWR_ENGINE_STATE_INIT; 120 | } 121 | break; 122 | 123 | default: 124 | state = TWR_ENGINE_STATE_INIT; 125 | break; 126 | } 127 | } 128 | 129 | 130 | -------------------------------------------------------------------------------- /examples/twoMsgTwrDecaDuino_A/twoMsgTwrDecaDuino_A.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define AIR_SPEED_OF_LIGHT 229702547.0 5 | #define DW1000_TIMEBASE 15.65E-12 6 | #define COEFF AIR_SPEED_OF_LIGHT*DW1000_TIMEBASE 7 | #define TRANSMITION_DELAY 1000000000000/15.65*0.1*5E-3 // 0.1ms 8 | 9 | #define TIMEOUT 20 10 | 11 | #define TWR_ENGINE_STATE_INIT 1 12 | #define TWR_ENGINE_STATE_WAIT_NEW_CYCLE 2 13 | #define TWR_ENGINE_STATE_SEND_START 3 14 | #define TWR_ENGINE_STATE_WAIT_SEND_START 4 15 | #define TWR_ENGINE_STATE_MEMORISE_T1 5 16 | #define TWR_ENGINE_STATE_WATCHDOG_FOR_ACK 6 17 | #define TWR_ENGINE_STATE_RX_ON_FOR_ACK 7 18 | #define TWR_ENGINE_STATE_WAIT_ACK 8 19 | #define TWR_ENGINE_STATE_MEMORISE_T4 9 20 | #define TWR_ENGINE_STATE_WATCHDOG_FOR_DATA_REPLY 10 21 | #define TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY 11 22 | #define TWR_ENGINE_STATE_WAIT_DATA_REPLY 12 23 | #define TWR_ENGINE_STATE_EXTRACT_T2_T3 13 24 | 25 | #define TWR_MSG_TYPE_UNKNOWN 0 26 | #define TWR_MSG_TYPE_START 1 27 | #define TWR_MSG_TYPE_ACK 2 28 | #define TWR_MSG_TYPE_DATA_REPLY 3 29 | 30 | int i; 31 | int rxFrames; 32 | 33 | uint64_t t1_predicted, t1, t2, t3_predicted, t3, t4; 34 | int32_t tof; 35 | 36 | #ifdef ARDUINO_DWM1001_DEV 37 | DecaDuino decaduino(SS1, DW_IRQ); 38 | #elif defined(TEENSYDUINO) 39 | DecaDuino decaduino; 40 | #endif 41 | uint8_t txData[128]; 42 | uint8_t rxData[128]; 43 | uint16_t rxLen; 44 | int state; 45 | int timeout; 46 | 47 | 48 | void setup() { 49 | 50 | pinMode(13, OUTPUT); 51 | #ifdef TEENSYDUINO 52 | SPI.setSCK(14); 53 | #endif 54 | if ( !decaduino.init() ) { 55 | Serial.println("decaduino init failed"); 56 | while(1) { 57 | digitalWrite(13, HIGH); 58 | delay(50); 59 | digitalWrite(13, LOW); 60 | delay(50); 61 | } 62 | } 63 | 64 | // Set RX buffer 65 | decaduino.setRxBuffer(rxData, &rxLen); 66 | state = TWR_ENGINE_STATE_INIT; 67 | } 68 | 69 | void loop() { 70 | decaduino.engine(); 71 | 72 | float distance; 73 | 74 | switch (state) { 75 | 76 | case TWR_ENGINE_STATE_INIT: 77 | decaduino.plmeRxDisableRequest(); 78 | state = TWR_ENGINE_STATE_WAIT_NEW_CYCLE; 79 | break; 80 | 81 | case TWR_ENGINE_STATE_WAIT_NEW_CYCLE: 82 | delay(100); 83 | Serial.println("New TWR"); 84 | state = TWR_ENGINE_STATE_SEND_START; 85 | break; 86 | 87 | case TWR_ENGINE_STATE_SEND_START: 88 | txData[0] = TWR_MSG_TYPE_START; 89 | t1_predicted = decaduino.alignDelayedTransmission(TRANSMITION_DELAY); 90 | decaduino.pdDataRequest(txData, 1, true, t1_predicted); 91 | state = TWR_ENGINE_STATE_WAIT_SEND_START; 92 | break; 93 | 94 | case TWR_ENGINE_STATE_WAIT_SEND_START: 95 | if ( decaduino.hasTxSucceeded() ) 96 | state = TWR_ENGINE_STATE_MEMORISE_T1; 97 | break; 98 | 99 | case TWR_ENGINE_STATE_MEMORISE_T1: 100 | t1 = decaduino.getLastTxTimestamp(); 101 | state = TWR_ENGINE_STATE_WATCHDOG_FOR_ACK; 102 | break; 103 | 104 | case TWR_ENGINE_STATE_WATCHDOG_FOR_ACK: 105 | timeout = millis() + TIMEOUT; 106 | state = TWR_ENGINE_STATE_RX_ON_FOR_ACK; 107 | break; 108 | 109 | case TWR_ENGINE_STATE_RX_ON_FOR_ACK: 110 | decaduino.plmeRxEnableRequest(); 111 | state = TWR_ENGINE_STATE_WAIT_ACK; 112 | break; 113 | 114 | case TWR_ENGINE_STATE_WAIT_ACK: 115 | if ( millis() > timeout ) { 116 | state = TWR_ENGINE_STATE_INIT; 117 | } else { 118 | if ( decaduino.rxFrameAvailable() ) { 119 | if ( rxData[0] == TWR_MSG_TYPE_ACK ) { 120 | state = TWR_ENGINE_STATE_MEMORISE_T4; 121 | } else state = TWR_ENGINE_STATE_RX_ON_FOR_ACK; 122 | } 123 | } 124 | break; 125 | 126 | case TWR_ENGINE_STATE_MEMORISE_T4: 127 | t4 = decaduino.getLastRxTimestamp(); 128 | state = TWR_ENGINE_STATE_EXTRACT_T2_T3; 129 | break; 130 | 131 | case TWR_ENGINE_STATE_EXTRACT_T2_T3: 132 | t2 = decaduino.decodeUint64(&rxData[1]); 133 | t3_predicted = decaduino.decodeUint64(&rxData[9]); 134 | tof = (t4 - t1_predicted - (t3_predicted - t2))/2; 135 | distance = tof*COEFF; 136 | Serial.print("t1-t1_predicted="); 137 | Serial.print((int)(t1-t1_predicted)); 138 | Serial.print("\tToF="); 139 | Serial.print(tof, HEX); 140 | Serial.print("\td="); 141 | Serial.print(distance); 142 | Serial.println(); 143 | state = TWR_ENGINE_STATE_INIT; 144 | break; 145 | 146 | default: 147 | state = TWR_ENGINE_STATE_INIT; 148 | break; 149 | } 150 | } 151 | 152 | 153 | -------------------------------------------------------------------------------- /examples/DecaDuinoSDSTWR_server/DecaDuinoSDSTWR_server.ino: -------------------------------------------------------------------------------- 1 | // DecaDuinoSDSTWR_server 2 | // A simple implementation of the TWR protocol, server side 3 | // Contributors: Adrien van den Bossche, Réjane Dalcé, Ibrahim Fofana, Robert Try, Thierry Val 4 | // This sketch is a part of the DecaDuino Project - please refer to the DecaDuino LICENCE file for licensing details 5 | 6 | #include 7 | #include 8 | 9 | // Timeout parameters 10 | #define TIMEOUT_WAIT_ACK_REQ_SENT 5 //ms 11 | #define TIMEOUT_WAIT_ACK 10 //ms 12 | #define TIMEOUT_WAIT_DATA_REPLY_SENT 5 //ms 13 | 14 | // SDS-TWR server states state machine enumeration: see state diagram on documentation for more details 15 | enum { SDSTWR_ENGINE_STATE_INIT, SDSTWR_ENGINE_STATE_WAIT_START, SDSTWR_ENGINE_STATE_MEMORISE_T2, 16 | SDSTWR_ENGINE_STATE_SEND_ACK_REQ, SDSTWR_ENGINE_STATE_WAIT_ACK_REQ_SENT, SDSTWR_ENGINE_STATE_MEMORISE_T3, 17 | SDSTWR_ENGINE_STATE_WAIT_ACK, SDSTWR_ENGINE_STATE_MEMORISE_T6, SDSTWR_ENGINE_STATE_SEND_DATA_REPLY, 18 | SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY_SENT}; 19 | 20 | // Message types of the SDS-TWR protocol 21 | #define SDSTWR_MSG_TYPE_UNKNOWN 10 22 | #define SDSTWR_MSG_TYPE_START 11 23 | #define SDSTWR_MSG_TYPE_ACK_REQ 12 24 | #define SDSTWR_MSG_TYPE_ACK 13 25 | #define SDSTWR_MSG_TYPE_DATA_REPLY 14 26 | 27 | uint64_t t2, t3, t6; 28 | 29 | #ifdef ARDUINO_DWM1001_DEV 30 | DecaDuino decaduino(SS1, DW_IRQ); 31 | #elif defined(TEENSYDUINO) 32 | DecaDuino decaduino; 33 | #endif 34 | uint8_t txData[128]; 35 | uint8_t rxData[128]; 36 | uint16_t rxLen; 37 | int state; 38 | uint32_t timeout; 39 | 40 | 41 | void setup() 42 | { 43 | pinMode(13, OUTPUT); // Internal LED (pin 13 on DecaWiNo board) 44 | Serial.begin(115200); // Init Serial port 45 | #ifdef TEENSYDUINO 46 | SPI.setSCK(14); // Set SPI clock pin (pin 14 on DecaWiNo board) 47 | #endif 48 | // Init DecaDuino and blink if initialisation fails 49 | if ( !decaduino.init() ) { 50 | Serial.println("decaduino init failed"); 51 | while(1) { digitalWrite(13, HIGH); delay(50); digitalWrite(13, LOW); delay(50); } 52 | } 53 | 54 | // Set RX buffer 55 | decaduino.setRxBuffer(rxData, &rxLen); 56 | state = SDSTWR_ENGINE_STATE_INIT; 57 | } 58 | 59 | 60 | void loop() 61 | { 62 | decaduino.engine(); 63 | 64 | switch (state) { 65 | 66 | case SDSTWR_ENGINE_STATE_INIT : 67 | decaduino.plmeRxEnableRequest(); 68 | state = SDSTWR_ENGINE_STATE_WAIT_START; 69 | break; 70 | 71 | case SDSTWR_ENGINE_STATE_WAIT_START : 72 | if (decaduino.rxFrameAvailable()){ 73 | if ( rxData[0] == SDSTWR_MSG_TYPE_START){ 74 | state = SDSTWR_ENGINE_STATE_MEMORISE_T2; 75 | } else { 76 | decaduino.plmeRxEnableRequest(); 77 | state = SDSTWR_ENGINE_STATE_WAIT_START; 78 | } 79 | } 80 | break; 81 | 82 | case SDSTWR_ENGINE_STATE_MEMORISE_T2 : 83 | t2 = decaduino.getLastRxTimestamp(); 84 | state = SDSTWR_ENGINE_STATE_SEND_ACK_REQ; 85 | break; 86 | 87 | case SDSTWR_ENGINE_STATE_SEND_ACK_REQ : 88 | txData[0]= SDSTWR_MSG_TYPE_ACK_REQ; 89 | decaduino.pdDataRequest(txData, 1); 90 | timeout = millis() + TIMEOUT_WAIT_ACK_REQ_SENT; 91 | state = SDSTWR_ENGINE_STATE_WAIT_ACK_REQ_SENT; 92 | break; 93 | 94 | case SDSTWR_ENGINE_STATE_WAIT_ACK_REQ_SENT: 95 | if ( millis() > timeout ) { 96 | state = SDSTWR_ENGINE_STATE_INIT; 97 | } else { 98 | if ( decaduino.hasTxSucceeded() ) { 99 | state = SDSTWR_ENGINE_STATE_MEMORISE_T3; 100 | } 101 | } 102 | break; 103 | 104 | case SDSTWR_ENGINE_STATE_MEMORISE_T3 : 105 | t3 = decaduino.getLastTxTimestamp(); 106 | timeout = millis() + TIMEOUT_WAIT_ACK; 107 | decaduino.plmeRxEnableRequest(); 108 | state = SDSTWR_ENGINE_STATE_WAIT_ACK; 109 | break; 110 | 111 | case SDSTWR_ENGINE_STATE_WAIT_ACK : 112 | if ( millis() > timeout) { 113 | state = SDSTWR_ENGINE_STATE_INIT; 114 | } 115 | else { 116 | if (decaduino.rxFrameAvailable()){ 117 | if (rxData[0] == SDSTWR_MSG_TYPE_ACK) { 118 | state = SDSTWR_ENGINE_STATE_MEMORISE_T6; 119 | } else { 120 | decaduino.plmeRxEnableRequest(); 121 | state = SDSTWR_ENGINE_STATE_WAIT_ACK; 122 | } 123 | } 124 | } 125 | break; 126 | 127 | case SDSTWR_ENGINE_STATE_MEMORISE_T6 : 128 | t6 = decaduino.getLastRxTimestamp(); 129 | state = SDSTWR_ENGINE_STATE_SEND_DATA_REPLY; 130 | break; 131 | 132 | case SDSTWR_ENGINE_STATE_SEND_DATA_REPLY : 133 | txData[0] = SDSTWR_MSG_TYPE_DATA_REPLY; 134 | decaduino.encodeUint40(t2, &txData[1]); 135 | decaduino.encodeUint40(t3, &txData[6]); 136 | decaduino.encodeUint40(t6, &txData[11]); 137 | decaduino.pdDataRequest(txData, 16); 138 | timeout = millis() + TIMEOUT_WAIT_DATA_REPLY_SENT; 139 | state = SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY_SENT; 140 | break; 141 | 142 | case SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY_SENT: 143 | if ( (millis()>timeout) || (decaduino.hasTxSucceeded()) ) { 144 | state = SDSTWR_ENGINE_STATE_INIT; 145 | } 146 | break; 147 | 148 | default : 149 | state = SDSTWR_ENGINE_STATE_INIT; 150 | break; 151 | } 152 | } 153 | -------------------------------------------------------------------------------- /examples/DecaDuinoTWR_client/DecaDuinoTWR_client.ino: -------------------------------------------------------------------------------- 1 | // DecaDuinoTWR_client 2 | // A simple implementation of the TWR protocol, client side 3 | // Contributors: Adrien van den Bossche, Réjane Dalcé, Ibrahim Fofana, Robert Try, Thierry Val 4 | // This sketch is a part of the DecaDuino Project - please refer to the DecaDuino LICENCE file for licensing details 5 | // This sketch implements the skew correction published in "Nezo Ibrahim Fofana, Adrien van den Bossche, Réjane 6 | // Dalcé, Thierry Val, "An Original Correction Method for Indoor Ultra Wide Band Ranging-based Localisation System" 7 | // https://arxiv.org/pdf/1603.06736.pdf 8 | 9 | #include 10 | #include 11 | 12 | // Timeout parameters 13 | #define TIMEOUT_WAIT_START_SENT 5 //ms 14 | #define TIMEOUT_WAIT_ACK 10 //ms 15 | #define TIMEOUT_WAIT_DATA_REPLY 20 //ms 16 | 17 | // Ranging period parameter 18 | #define RANGING_PERIOD 500 //ms 19 | 20 | // TWR client states state machine enumeration: see state diagram on documentation for more details 21 | enum { TWR_ENGINE_STATE_INIT, TWR_ENGINE_STATE_WAIT_START_SENT, TWR_ENGINE_STATE_MEMORISE_T1, 22 | TWR_ENGINE_STATE_WAIT_ACK, TWR_ENGINE_STATE_MEMORISE_T4, TWR_ENGINE_STATE_WAIT_DATA_REPLY, 23 | TWR_ENGINE_STATE_EXTRACT_T2_T3 }; 24 | 25 | // Message types of the TWR protocol 26 | #define TWR_MSG_TYPE_UNKNOWN 0 27 | #define TWR_MSG_TYPE_START 1 28 | #define TWR_MSG_TYPE_ACK 2 29 | #define TWR_MSG_TYPE_DATA_REPLY 3 30 | 31 | uint64_t t1, t2, t3, t4; 32 | uint64_t mask = 0xFFFFFFFFFF; 33 | int32_t tof; 34 | 35 | #ifdef ARDUINO_DWM1001_DEV 36 | DecaDuino decaduino(SS1, DW_IRQ); 37 | #else 38 | DecaDuino decaduino; 39 | #endif 40 | uint8_t txData[128]; 41 | uint8_t rxData[128]; 42 | uint16_t rxLen; 43 | int state; 44 | uint32_t timeout; 45 | 46 | 47 | void setup() 48 | { 49 | pinMode(13, OUTPUT); // Internal LED (pin 13 on DecaWiNo board) 50 | Serial.begin(115200); // Init Serial port 51 | #ifndef ARDUINO_DWM1001_DEV 52 | SPI.setSCK(14); // Set SPI clock pin (pin 14 on DecaWiNo board) 53 | #endif 54 | // Init DecaDuino and blink if initialisation fails 55 | if ( !decaduino.init() ) { 56 | Serial.println("decaduino init failed"); 57 | while(1) { digitalWrite(13, HIGH); delay(50); digitalWrite(13, LOW); delay(50); } 58 | } 59 | 60 | // Set RX buffer 61 | decaduino.setRxBuffer(rxData, &rxLen); 62 | state = TWR_ENGINE_STATE_INIT; 63 | 64 | // Print top table colomns 65 | Serial.println("ToF\td\tToF_sk\td_sk"); 66 | } 67 | 68 | void loop() 69 | { 70 | decaduino.engine(); 71 | 72 | float distance; 73 | 74 | switch (state) { 75 | 76 | case TWR_ENGINE_STATE_INIT: 77 | delay(RANGING_PERIOD); // Wait to avoid medium flooding between two rangings or if a ranging fails 78 | decaduino.plmeRxDisableRequest(); 79 | Serial.println("New TWR"); 80 | txData[0] = TWR_MSG_TYPE_START; 81 | decaduino.pdDataRequest(txData, 1); 82 | timeout = millis() + TIMEOUT_WAIT_START_SENT; 83 | state = TWR_ENGINE_STATE_WAIT_START_SENT; 84 | break; 85 | 86 | case TWR_ENGINE_STATE_WAIT_START_SENT: 87 | if ( millis() > timeout ) { 88 | state = TWR_ENGINE_STATE_INIT; 89 | } else { 90 | if ( decaduino.hasTxSucceeded() ) { 91 | state = TWR_ENGINE_STATE_MEMORISE_T1; 92 | } 93 | } 94 | break; 95 | 96 | case TWR_ENGINE_STATE_MEMORISE_T1: 97 | t1 = decaduino.getLastTxTimestamp(); 98 | timeout = millis() + TIMEOUT_WAIT_ACK; 99 | decaduino.plmeRxEnableRequest(); 100 | state = TWR_ENGINE_STATE_WAIT_ACK; 101 | break; 102 | 103 | case TWR_ENGINE_STATE_WAIT_ACK: 104 | if ( millis() > timeout ) { 105 | state = TWR_ENGINE_STATE_INIT; 106 | } else { 107 | if ( decaduino.rxFrameAvailable() ) { 108 | if ( rxData[0] == TWR_MSG_TYPE_ACK ) { 109 | state = TWR_ENGINE_STATE_MEMORISE_T4; 110 | } else { 111 | decaduino.plmeRxEnableRequest(); 112 | state = TWR_ENGINE_STATE_WAIT_ACK; 113 | } 114 | } 115 | } 116 | break; 117 | 118 | case TWR_ENGINE_STATE_MEMORISE_T4: 119 | t4 = decaduino.getLastRxTimestamp(); 120 | timeout = millis() + TIMEOUT_WAIT_DATA_REPLY; 121 | decaduino.plmeRxEnableRequest(); 122 | state = TWR_ENGINE_STATE_WAIT_DATA_REPLY; 123 | break; 124 | 125 | case TWR_ENGINE_STATE_WAIT_DATA_REPLY: 126 | if ( millis() > timeout ) { 127 | state = TWR_ENGINE_STATE_INIT; 128 | } else { 129 | if ( decaduino.rxFrameAvailable() ) { 130 | if ( rxData[0] == TWR_MSG_TYPE_DATA_REPLY ) { 131 | state = TWR_ENGINE_STATE_EXTRACT_T2_T3; 132 | } else { 133 | decaduino.plmeRxEnableRequest(); 134 | state = TWR_ENGINE_STATE_WAIT_DATA_REPLY; 135 | } 136 | } 137 | } 138 | break; 139 | 140 | case TWR_ENGINE_STATE_EXTRACT_T2_T3: 141 | t2 = decaduino.decodeUint40(&rxData[1]); 142 | t3 = decaduino.decodeUint40(&rxData[6]); 143 | tof = (((t4 - t1) & mask) - ((t3 - t2) & mask))/2; 144 | distance = tof*RANGING_UNIT; 145 | Serial.print(tof); 146 | Serial.print("\t"); 147 | Serial.print(distance); 148 | tof = (((t4 - t1) & mask) - (1+1.0E-6*decaduino.getLastRxSkew())*((t3 - t2) & mask))/2; 149 | distance = tof*RANGING_UNIT; 150 | Serial.print("\t"); 151 | Serial.print(tof); 152 | Serial.print("\t"); 153 | Serial.println(distance); 154 | state = TWR_ENGINE_STATE_INIT; 155 | break; 156 | 157 | default: 158 | state = TWR_ENGINE_STATE_INIT; 159 | break; 160 | } 161 | } 162 | 163 | 164 | -------------------------------------------------------------------------------- /examples/DecaDuinoBeacon/DecaDuinoBeacon.ino: -------------------------------------------------------------------------------- 1 | // DecaDuinoBeacon 2 | // by Adrien van den Bossche 3 | // This sketch is a part of the DecaDuino Project - please refer to the DecaDuino LICENCE file for licensing details 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #define BEACON_PERIOD 2000 10 | #define MAX_FRAME_LEN 15 11 | uint8_t rxData[MAX_FRAME_LEN]; 12 | uint16_t rxLen; 13 | uint8_t txData[MAX_FRAME_LEN]; 14 | uint16_t txLen; 15 | int rxFrames; 16 | #ifdef ARDUINO_DWM1001_DEV 17 | DecaDuino decaduino(SS1, DW_IRQ); 18 | #define LED_ON LOW 19 | #define LED_OFF HIGH 20 | #else 21 | DecaDuino decaduino; 22 | #define LED_ON HIGH 23 | #define LED_OFF LOW 24 | #endif 25 | 26 | uint32_t beacon_send_time = BEACON_PERIOD; 27 | uint64_t dwMacAddr; 28 | uint16_t myAddress = 0; 29 | uint16_t myNseq = 0; 30 | 31 | 32 | void setup() 33 | { 34 | pinMode(LED_BUILTIN, OUTPUT); // Internal LED (pin LED_BUILTIN on DecaWiNo board) 35 | Serial.begin(115200); // Init Serial port 36 | #ifndef ARDUINO_DWM1001_DEV 37 | SPI.setSCK(14); // Set SPI clock pin (pin 14 on DecaWiNo board) 38 | #endif 39 | 40 | // Init DecaDuino and blink if initialisation fails 41 | if ( !decaduino.init() ) { 42 | Serial.println("decaduino init failed"); 43 | while(1) { digitalWrite(LED_BUILTIN, LED_ON); delay(50); digitalWrite(LED_BUILTIN, LED_OFF); delay(50); } 44 | } 45 | // Set RX buffer and enable RX 46 | decaduino.setRxBuffer(rxData, &rxLen); 47 | decaduino.plmeRxEnableRequest(); 48 | rxFrames = 0; 49 | 50 | dwMacAddr = getDwMacAddrUint64(); 51 | 52 | decaduino.printUint64(getDwMacAddrUint64()); Serial.println(); Serial.flush(); 53 | 54 | myAddress = toNodeID(getDwMacAddrUint64()); 55 | if (myAddress == 0){ 56 | Serial.print("NRF chip ID not registered ind DWM100x_id.cpp : "); 57 | decaduino.printUint64(getDwMacAddrUint64()); Serial.println(); Serial.flush(); 58 | while(1) { digitalWrite(LED_BUILTIN, LED_ON); delay(200); digitalWrite(LED_BUILTIN, LED_OFF); delay(200); } 59 | } 60 | randomSeed(dwMacAddr); 61 | } 62 | 63 | #define SERIAL_BUFFER_LENGTH 2048 64 | char serialBuffer[SERIAL_BUFFER_LENGTH]; 65 | int serialBufferWriteIndex = 0; 66 | int serialBufferReadIndex = 0; 67 | 68 | 69 | void pushSerialBuffer(char* str) 70 | { 71 | int len = strlen(str); 72 | 73 | for (int i=0; i beacon_send_time ) 118 | { 119 | beacon_send_time += random(BEACON_PERIOD/2, BEACON_PERIOD*1.5); 120 | digitalWrite(LED_BUILTIN, LED_ON); 121 | decaduino.plmeRxDisableRequest(); // Always disable RX before sending a frame 122 | // make dummy data, send it and wait the end of the transmission. 123 | for (int i=0; i 8 | #include 9 | 10 | // Timeout parameters 11 | #define TIMEOUT_WAIT_START_SENT 5 //ms 12 | #define TIMEOUT_WAIT_ACK_REQ 10 //ms 13 | #define TIMEOUT_WAIT_ACK_SENT 5 //ms 14 | #define TIMEOUT_WAIT_DATA_REPLY 10 //ms 15 | 16 | // Ranging period parameter 17 | #define RANGING_PERIOD 500 //ms 18 | 19 | // SDS-TWR client states state machine enumeration 20 | enum { SDSTWR_ENGINE_STATE_INIT, SDSTWR_ENGINE_STATE_WAIT_START_SENT, SDSTWR_ENGINE_STATE_MEMORISE_T1, 21 | SDSTWR_ENGINE_STATE_WAIT_ACK_REQ, SDSTWR_ENGINE_STATE_MEMORISE_T4, SDSTWR_ENGINE_STATE_SEND_ACK, 22 | SDSTWR_ENGINE_STATE_WAIT_ACK_SENT, SDSTWR_ENGINE_STATE_MEMORISE_T5, SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY, 23 | SDSTWR_ENGINE_STATE_EXTRACT_T2_T3_T6 }; 24 | 25 | // Message types of the SDS-TWR protocol 26 | #define SDSTWR_MSG_TYPE_UNKNOWN 10 27 | #define SDSTWR_MSG_TYPE_START 11 28 | #define SDSTWR_MSG_TYPE_ACK_REQ 12 29 | #define SDSTWR_MSG_TYPE_ACK 13 30 | #define SDSTWR_MSG_TYPE_DATA_REPLY 14 31 | 32 | uint64_t t1, t2, t3, t4, t5, t6; 33 | uint64_t mask = 0xFFFFFFFFFF; 34 | int32_t tof; 35 | 36 | #ifdef ARDUINO_DWM1001_DEV 37 | DecaDuino decaduino(SS1, DW_IRQ); 38 | #elif defined(TEENSYDUINO) 39 | DecaDuino decaduino; 40 | #endif 41 | 42 | uint8_t txData[128]; 43 | uint8_t rxData[128]; 44 | uint16_t rxLen; 45 | int state; 46 | uint32_t timeout; 47 | 48 | 49 | void setup() 50 | { 51 | pinMode(13, OUTPUT); // Internal LED (pin 13 on DecaWiNo board) 52 | Serial.begin(115200); // Init Serial port 53 | #ifdef TEENSYDUINO 54 | SPI.setSCK(14); // Set SPI clock pin (pin 14 on DecaWiNo board) 55 | #endif 56 | // Init DecaDuino and blink if initialisation fails 57 | if ( !decaduino.init() ) { 58 | Serial.println("decaduino init failed"); 59 | while(1) { digitalWrite(13, HIGH); delay(50); digitalWrite(13, LOW); delay(50); } 60 | } 61 | 62 | // Set RX buffer 63 | decaduino.setRxBuffer(rxData, &rxLen); 64 | state = SDSTWR_ENGINE_STATE_INIT; 65 | 66 | // Print top table colomns 67 | Serial.println("ToF\td"); 68 | } 69 | 70 | 71 | void loop() 72 | { 73 | decaduino.engine(); 74 | 75 | float distance; 76 | 77 | switch (state) { 78 | 79 | case SDSTWR_ENGINE_STATE_INIT: 80 | delay(RANGING_PERIOD); // Wait to avoid medium flooding between two rangings or if a ranging fails 81 | decaduino.plmeRxDisableRequest(); 82 | Serial.println("New SDS-TWR"); 83 | txData[0] = SDSTWR_MSG_TYPE_START; 84 | decaduino.pdDataRequest(txData, 1); 85 | timeout = millis() + TIMEOUT_WAIT_START_SENT; 86 | state = SDSTWR_ENGINE_STATE_WAIT_START_SENT; 87 | break; 88 | 89 | case SDSTWR_ENGINE_STATE_WAIT_START_SENT: 90 | if ( millis() > timeout ) { 91 | state = SDSTWR_ENGINE_STATE_INIT; 92 | } else { 93 | if (decaduino.hasTxSucceeded()) { 94 | state = SDSTWR_ENGINE_STATE_MEMORISE_T1; 95 | } 96 | } 97 | break; 98 | 99 | case SDSTWR_ENGINE_STATE_MEMORISE_T1 : 100 | t1 = decaduino.getLastTxTimestamp(); 101 | timeout = millis() + TIMEOUT_WAIT_ACK_REQ; 102 | decaduino.plmeRxEnableRequest(); 103 | state = SDSTWR_ENGINE_STATE_WAIT_ACK_REQ; 104 | break; 105 | 106 | case SDSTWR_ENGINE_STATE_WAIT_ACK_REQ : 107 | if ( millis() > timeout) { 108 | state = SDSTWR_ENGINE_STATE_INIT; 109 | } 110 | else { 111 | if (decaduino.rxFrameAvailable()){ 112 | if (rxData[0] == SDSTWR_MSG_TYPE_ACK_REQ) { 113 | state = SDSTWR_ENGINE_STATE_MEMORISE_T4; 114 | } else { 115 | decaduino.plmeRxEnableRequest(); 116 | state = SDSTWR_ENGINE_STATE_WAIT_ACK_REQ; 117 | } 118 | } 119 | } 120 | break; 121 | 122 | case SDSTWR_ENGINE_STATE_MEMORISE_T4 : 123 | t4 = decaduino.getLastRxTimestamp(); 124 | state = SDSTWR_ENGINE_STATE_SEND_ACK; 125 | break; 126 | 127 | case SDSTWR_ENGINE_STATE_SEND_ACK : 128 | txData[0]= SDSTWR_MSG_TYPE_ACK; 129 | decaduino.pdDataRequest(txData, 1); 130 | timeout = millis() + TIMEOUT_WAIT_ACK_SENT; 131 | state = SDSTWR_ENGINE_STATE_WAIT_ACK_SENT; 132 | break; 133 | 134 | case SDSTWR_ENGINE_STATE_WAIT_ACK_SENT : 135 | if ( millis() > timeout ) { 136 | state = SDSTWR_ENGINE_STATE_INIT; 137 | } else { 138 | if (decaduino.hasTxSucceeded()){ 139 | state = SDSTWR_ENGINE_STATE_MEMORISE_T5; 140 | } 141 | } 142 | break; 143 | 144 | case SDSTWR_ENGINE_STATE_MEMORISE_T5 : 145 | t5 = decaduino.getLastTxTimestamp(); 146 | timeout = millis() + TIMEOUT_WAIT_DATA_REPLY; 147 | decaduino.plmeRxEnableRequest(); 148 | state = SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY; 149 | break; 150 | 151 | case SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY : 152 | if ( millis() > timeout) { 153 | state = SDSTWR_ENGINE_STATE_INIT; 154 | } 155 | else { 156 | if (decaduino.rxFrameAvailable()){ 157 | if (rxData[0] == SDSTWR_MSG_TYPE_DATA_REPLY) { 158 | state = SDSTWR_ENGINE_STATE_EXTRACT_T2_T3_T6; 159 | } else { 160 | decaduino.plmeRxEnableRequest(); 161 | state = SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY; 162 | } 163 | } 164 | } 165 | break; 166 | 167 | case SDSTWR_ENGINE_STATE_EXTRACT_T2_T3_T6 : 168 | t2 = decaduino.decodeUint40(&rxData[1]); 169 | t3 = decaduino.decodeUint40(&rxData[6]); 170 | t6 = decaduino.decodeUint40(&rxData[11]); 171 | tof = (((((t4 - t1) & mask) - ((t3 - t2) & mask)) & mask) + ((((t6 - t3) & mask) - ((t5 - t4) & mask)) & mask))/4; 172 | distance = tof*RANGING_UNIT; 173 | Serial.print(tof); 174 | Serial.print("\t"); 175 | Serial.println(distance); 176 | state = SDSTWR_ENGINE_STATE_INIT; 177 | break; 178 | 179 | default: 180 | state = SDSTWR_ENGINE_STATE_INIT; 181 | break; 182 | } 183 | } 184 | -------------------------------------------------------------------------------- /examples/sdsTwrDecaDuino_A_withLedStrip/sdsTwrDecaDuino_A_withLedStrip.ino: -------------------------------------------------------------------------------- 1 | //#define ENABLE_CALIBRATION_FROM_EEPROM 2 | 3 | #include 4 | #include 5 | #ifdef ENABLE_CALIBRATION_FROM_EEPROM 6 | #include 7 | uint16_t antennaDelay; 8 | #endif 9 | 10 | #include "FastLED.h" 11 | 12 | #define LED_DATA_PIN 8 13 | #define NUM_LEDS 240 14 | 15 | #define X_CORRECTION 1.0 16 | #define Y_CORRECTION 0.0 17 | 18 | #define INTERLED_DISTANCE 0.975/59 // 1.65cm between leds 19 | 20 | #define COEFF RANGING_UNIT 21 | 22 | #define TIMEOUT 10 23 | 24 | #define TWR_ENGINE_STATE_INIT 1 25 | #define TWR_ENGINE_STATE_WAIT_START_SENT 2 26 | #define TWR_ENGINE_STATE_MEMORISE_T1 3 27 | #define TWR_ENGINE_STATE_WAIT_ACK_REQ 4 28 | #define TWR_ENGINE_STATE_MEMORISE_T4 5 29 | #define TWR_ENGINE_STATE_SEND_ACK 6 30 | #define TWR_ENGINE_STATE_WAIT_ACK_SENT 7 31 | #define TWR_ENGINE_STATE_MEMORISE_T5 8 32 | #define TWR_ENGINE_STATE_WAIT_DATA_REPLY 9 33 | #define TWR_ENGINE_STATE_EXTRACT_T2_T3_T6 10 34 | 35 | #define TWR_MSG_TYPE_UNKNOWN 0 36 | #define TWR_MSG_TYPE_START 1 37 | #define TWR_MSG_TYPE_ACK_REQ 2 38 | #define TWR_MSG_TYPE_ACK 3 39 | #define TWR_MSG_TYPE_DATA_REPLY 4 40 | 41 | int rxFrames; 42 | 43 | uint64_t t1, t2, t3, t4, t5, t6; 44 | uint64_t mask = 0xFFFFFFFFFF; 45 | int32_t tof; 46 | 47 | #ifdef ARDUINO_DWM1001_DEV 48 | DecaDuino decaduino(SS1, DW_IRQ); 49 | #elif defined(TEENSYDUINO) 50 | DecaDuino decaduino; 51 | #endif 52 | uint8_t txData[128]; 53 | uint8_t rxData[128]; 54 | uint16_t rxLen; 55 | int state; 56 | uint32_t timeout; 57 | 58 | CRGB leds[NUM_LEDS]; 59 | 60 | void dist2led(float dist) { 61 | 62 | int ledIndex = dist/(1.0*INTERLED_DISTANCE); 63 | 64 | Serial.printf("dist=%f, ledIndex=%d\n", dist, ledIndex); 65 | 66 | for (int i=0; i 60 ) { 72 | leds[NUM_LEDS-1] = CRGB::Red; 73 | } else { 74 | leds[ledIndex] = CRGB::White; 75 | } 76 | 77 | FastLED.show(); 78 | } 79 | 80 | void setup() { 81 | 82 | uint8_t buf[2]; 83 | 84 | FastLED.addLeds(leds, NUM_LEDS); 85 | 86 | pinMode(13, OUTPUT); 87 | #ifdef TEENSYDUINO 88 | SPI.setSCK(14); 89 | #endif 90 | if (!decaduino.init()){ 91 | Serial.print("decaduino init failled"); 92 | while(1){ 93 | digitalWrite(13, HIGH); 94 | delay(50); 95 | digitalWrite(13, LOW); 96 | delay(50); 97 | } 98 | } 99 | 100 | #ifdef ENABLE_CALIBRATION_FROM_EEPROM 101 | 102 | // Gets antenna delay from the end of EEPROM. The two last bytes are used for DecaWiNo label, 103 | // so use n-2 and n-3 to store the antenna delay (16bit value) 104 | buf[0] = EEPROM.read(EEPROM.length()-4); 105 | buf[1] = EEPROM.read(EEPROM.length()-3); 106 | antennaDelay = decaduino.decodeUint16(buf); 107 | 108 | if ( antennaDelay == 0xffff ) { 109 | Serial.println("Unvalid antenna delay value found in EEPROM. Using default value."); 110 | } else decaduino.setAntennaDelay(antennaDelay); 111 | 112 | #endif 113 | 114 | decaduino.setRxBuffer(rxData, &rxLen); 115 | state=TWR_ENGINE_STATE_INIT; 116 | 117 | FastLED.setBrightness(32); 118 | } 119 | 120 | void loop() { 121 | decaduino.engine(); 122 | 123 | float distance; 124 | 125 | switch (state) { 126 | 127 | case TWR_ENGINE_STATE_INIT : 128 | decaduino.plmeRxDisableRequest(); 129 | delay(100); 130 | Serial.println("New SDS_TWR"); 131 | txData[0] = TWR_MSG_TYPE_START; 132 | decaduino.pdDataRequest(txData, 1); 133 | state = TWR_ENGINE_STATE_WAIT_START_SENT; 134 | break; 135 | 136 | case TWR_ENGINE_STATE_WAIT_START_SENT : 137 | if (decaduino.hasTxSucceeded()) { 138 | state = TWR_ENGINE_STATE_MEMORISE_T1; 139 | } 140 | break; 141 | 142 | case TWR_ENGINE_STATE_MEMORISE_T1 : 143 | t1 = decaduino.getLastTxTimestamp(); 144 | timeout = millis() + TIMEOUT; 145 | decaduino.plmeRxEnableRequest(); 146 | state = TWR_ENGINE_STATE_WAIT_ACK_REQ; 147 | break; 148 | 149 | case TWR_ENGINE_STATE_WAIT_ACK_REQ : 150 | if ( millis() > timeout) { 151 | state = TWR_ENGINE_STATE_INIT; 152 | } 153 | else { 154 | if (decaduino.rxFrameAvailable()){ 155 | if (rxData[0] == TWR_MSG_TYPE_ACK_REQ) { 156 | state = TWR_ENGINE_STATE_MEMORISE_T4; 157 | } else { 158 | decaduino.plmeRxEnableRequest(); 159 | state = TWR_ENGINE_STATE_WAIT_ACK_REQ; 160 | } 161 | } 162 | } 163 | break; 164 | 165 | case TWR_ENGINE_STATE_MEMORISE_T4 : 166 | t4 = decaduino.getLastRxTimestamp(); 167 | state = TWR_ENGINE_STATE_SEND_ACK; 168 | break; 169 | 170 | case TWR_ENGINE_STATE_SEND_ACK : 171 | txData[0]= TWR_MSG_TYPE_ACK; 172 | decaduino.pdDataRequest(txData, 1); 173 | state = TWR_ENGINE_STATE_WAIT_ACK_SENT; 174 | break; 175 | 176 | case TWR_ENGINE_STATE_WAIT_ACK_SENT : 177 | if (decaduino.hasTxSucceeded()) { 178 | state = TWR_ENGINE_STATE_MEMORISE_T5; 179 | } 180 | break; 181 | 182 | case TWR_ENGINE_STATE_MEMORISE_T5 : 183 | t5 = decaduino.getLastTxTimestamp(); 184 | timeout = millis() + TIMEOUT; 185 | decaduino.plmeRxEnableRequest(); 186 | state = TWR_ENGINE_STATE_WAIT_DATA_REPLY; 187 | break; 188 | 189 | case TWR_ENGINE_STATE_WAIT_DATA_REPLY : 190 | if ( millis() > timeout) { 191 | state = TWR_ENGINE_STATE_INIT; 192 | } 193 | else { 194 | if (decaduino.rxFrameAvailable()){ 195 | if (rxData[0] == TWR_MSG_TYPE_DATA_REPLY) { 196 | state = TWR_ENGINE_STATE_EXTRACT_T2_T3_T6; 197 | } else { 198 | decaduino.plmeRxEnableRequest(); 199 | state = TWR_ENGINE_STATE_WAIT_DATA_REPLY; 200 | } 201 | } 202 | } 203 | break; 204 | 205 | case TWR_ENGINE_STATE_EXTRACT_T2_T3_T6 : 206 | t2 = decaduino.decodeUint64(&rxData[1]); 207 | t3 = decaduino.decodeUint64(&rxData[9]); 208 | t6 = decaduino.decodeUint64(&rxData[17]); 209 | tof = (((((t4 - t1) & mask) - ((t3 - t2) & mask)) & mask) + ((((t6 - t3) & mask) - ((t5 - t4) & mask)) & mask))/4; 210 | distance = tof*COEFF*X_CORRECTION + Y_CORRECTION; 211 | Serial.print("ToF="); 212 | Serial.print(tof); 213 | Serial.print(" d="); 214 | Serial.print(distance); 215 | Serial.println(); 216 | dist2led(distance); 217 | state = TWR_ENGINE_STATE_INIT; 218 | break; 219 | 220 | default: 221 | state = TWR_ENGINE_STATE_INIT; 222 | break; 223 | } 224 | } 225 | -------------------------------------------------------------------------------- /examples/PsdsTwrDecaDuino_B/PsdsTwrDecaDuino_B.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define TIMEOUT 10 5 | 6 | #define TWR_ENGINE_STATE_INIT 1 7 | #define TWR_ENGINE_STATE_RX_ON 2 8 | #define TWR_ENGINE_STATE_WAIT_START 3 9 | #define TWR_ENGINE_STATE_MEMORISE_T2 4 10 | #define TWR_ENGINE_STATE_SEND_ACK_REQ 5 11 | #define TWR_ENGINE_STATE_WAIT_SENT 6 12 | #define TWR_ENGINE_STATE_MEMORISE_T3 7 13 | #define TWR_ENGINE_STATE_WATCHDOG_FOR_ACK 8 14 | #define TWR_ENGINE_STATE_RX_ON_FOR_ACK 9 15 | #define TWR_ENGINE_STATE_WAIT_ACK 10 16 | #define TWR_ENGINE_STATE_MEMORISE_T6 11 17 | #define TWR_ENGINE_STATE_SEND_DATA_REPLY 12 18 | #define TWR_ENGINE_STATE_ON_FOR_DATA_REQUEST 13 19 | #define TWR_ENGINE_WAIT_DATAREQ 14 20 | 21 | #define TWR_MSG_TYPE_UNKNOW 0 22 | #define TWR_MSG_TYPE_START 1 23 | #define TWR_MSG_TYPE_ACK_REQ 2 24 | #define TWR_MSG_TYPE_ACK 3 25 | #define TWR_MSG_TYPE_DATA_REPLY 4 26 | #define TWR_MSG_TYPE_DATAREQ 5 27 | 28 | int i; 29 | int rxFrames; 30 | int seqnum; 31 | 32 | uint64_t t2, t3, t6; 33 | 34 | #ifdef ARDUINO_DWM1001_DEV 35 | DecaDuino decaduino(SS1, DW_IRQ); 36 | #elif defined(TEENSYDUINO) 37 | DecaDuino decaduino; 38 | #endif 39 | uint8_t txData[128]; 40 | uint8_t rxData[128]; 41 | uint16_t rxLen; 42 | int state; 43 | int timeout; 44 | 45 | //uint32_t addrPANID=0xDECA0022; 46 | //uint32_t addrPANID=0xDECA0023; 47 | uint32_t addrPANID=0xDECA0024; 48 | int dtim; 49 | 50 | void setup() { 51 | 52 | pinMode(13, OUTPUT); 53 | #ifdef TEENSYDUINO 54 | SPI.setSCK(14); 55 | #endif 56 | if (!decaduino.init(addrPANID)){ 57 | Serial.print("decaduino init failled"); 58 | while(1){ 59 | digitalWrite(13, HIGH); 60 | delay(50); 61 | digitalWrite(13, LOW); 62 | delay(50); 63 | } 64 | } 65 | decaduino.setRxBuffer(rxData, &rxLen); 66 | state=TWR_ENGINE_STATE_INIT; 67 | seqnum=0; 68 | } 69 | 70 | void loop() { 71 | decaduino.engine(); 72 | 73 | switch (state) { 74 | 75 | case TWR_ENGINE_STATE_INIT : 76 | state = TWR_ENGINE_STATE_RX_ON; 77 | break; 78 | 79 | case TWR_ENGINE_STATE_RX_ON : 80 | decaduino.plmeRxEnableRequest(); 81 | state = TWR_ENGINE_STATE_WAIT_START; 82 | break; 83 | 84 | case TWR_ENGINE_STATE_WAIT_START : 85 | if (decaduino.rxFrameAvailable()){ 86 | if ( rxData[9] == TWR_MSG_TYPE_START){ 87 | state = TWR_ENGINE_STATE_MEMORISE_T2; 88 | //selon position dans liste, attendre 89 | // 90 | switch((uint16_t)addrPANID){ 91 | case 0x23: dtim=10; 92 | break; 93 | case 0x22: dtim=0; 94 | break; 95 | case 0x24: dtim=20; 96 | break; 97 | 98 | } 99 | delay(dtim); 100 | Serial.println("received START"); 101 | } else state = TWR_ENGINE_STATE_RX_ON; 102 | } 103 | break; 104 | 105 | case TWR_ENGINE_STATE_MEMORISE_T2 : 106 | t2 = decaduino.getLastTxTimestamp(); 107 | state = TWR_ENGINE_STATE_SEND_ACK_REQ; 108 | break; 109 | 110 | case TWR_ENGINE_STATE_SEND_ACK_REQ : 111 | txData[0] =0x41; 112 | txData[1] =0x88; 113 | txData[2] =seqnum; 114 | txData[3] =0xCA; 115 | txData[4] =0xDE; 116 | txData[5] =0x25; 117 | txData[6] =0x00; 118 | txData[7] =(uint16_t)addrPANID;//0x23; 119 | txData[8] =0x00; 120 | txData[9]= TWR_MSG_TYPE_ACK_REQ; 121 | decaduino.pdDataRequest(txData, 10); 122 | state = TWR_ENGINE_STATE_WAIT_SENT; 123 | seqnum++; 124 | break; 125 | 126 | case TWR_ENGINE_STATE_WAIT_SENT: 127 | if (decaduino.hasTxSucceeded()){ 128 | state = TWR_ENGINE_STATE_MEMORISE_T3; 129 | Serial.println("_________________got T3!"); 130 | } 131 | break; 132 | 133 | case TWR_ENGINE_STATE_MEMORISE_T3 : 134 | t3 = decaduino.getLastTxTimestamp(); 135 | //state = TWR_ENGINE_STATE_WATCHDOG_FOR_ACK; 136 | state = TWR_ENGINE_STATE_ON_FOR_DATA_REQUEST; 137 | break; 138 | case TWR_ENGINE_STATE_ON_FOR_DATA_REQUEST : 139 | decaduino.plmeRxEnableRequest(); 140 | state=TWR_ENGINE_WAIT_DATAREQ; 141 | break; 142 | case TWR_ENGINE_WAIT_DATAREQ : 143 | if (decaduino.rxFrameAvailable()){ 144 | if (rxData[9] == TWR_MSG_TYPE_DATAREQ) { 145 | t6 = decaduino.getLastTxTimestamp(); 146 | Serial.println("Received DATAREQ____sent datareply"); 147 | //time to reply! 148 | switch((uint16_t)addrPANID){ 149 | case 0x23: dtim=20; 150 | break; 151 | case 0x22: dtim=10; 152 | break; 153 | case 0x24: dtim=30; 154 | break; 155 | 156 | } 157 | delay(dtim); 158 | txData[0] =0x41; 159 | txData[1] =0x88; 160 | txData[2] =seqnum; 161 | txData[3] =0xCA; 162 | txData[4] =0xDE; 163 | txData[5] =0x25; 164 | txData[6] =0x00; 165 | txData[7] =(uint16_t)addrPANID;//0x23; 166 | txData[8] =0x00; 167 | txData[9] = TWR_MSG_TYPE_DATA_REPLY; 168 | decaduino.encodeUint64(t2, &txData[10]); 169 | decaduino.encodeUint64(t3, &txData[19]); 170 | decaduino.encodeUint64(t6, &txData[27]); 171 | decaduino.pdDataRequest(txData, 35); 172 | state = TWR_ENGINE_STATE_INIT; 173 | seqnum++; 174 | } else state = TWR_ENGINE_STATE_RX_ON_FOR_ACK; 175 | } 176 | break; 177 | // case TWR_ENGINE_STATE_WATCHDOG_FOR_ACK : 178 | // timeout = millis() + TIMEOUT; 179 | // state = TWR_ENGINE_STATE_RX_ON_FOR_ACK; 180 | // break; 181 | // 182 | // case TWR_ENGINE_STATE_RX_ON_FOR_ACK : 183 | // decaduino.plmeRxEnableRequest(); 184 | // state = TWR_ENGINE_STATE_WAIT_ACK; 185 | // break; 186 | // 187 | // case TWR_ENGINE_STATE_WAIT_ACK : 188 | // if ( millis() > timeout) { 189 | // state = TWR_ENGINE_STATE_INIT;} 190 | // else { 191 | // if (decaduino.rxFrameAvailable()){ 192 | // if (rxData[0] == TWR_MSG_TYPE_ACK) { 193 | // state = TWR_ENGINE_STATE_MEMORISE_T6; 194 | // } else state = TWR_ENGINE_STATE_RX_ON_FOR_ACK; 195 | // } 196 | // } 197 | // break; 198 | // 199 | // case TWR_ENGINE_STATE_MEMORISE_T6 : 200 | // t6 = decaduino.getLastTxTimestamp(); 201 | // state = TWR_ENGINE_STATE_SEND_DATA_REPLY; 202 | // break; 203 | // 204 | // case TWR_ENGINE_STATE_SEND_DATA_REPLY : 205 | // txData[0] = TWR_MSG_TYPE_DATA_REPLY; 206 | // decaduino.encodeUint64(t2, &txData[1]); 207 | // decaduino.encodeUint64(t3, &txData[9]); 208 | // decaduino.encodeUint64(t6, &txData[17]); 209 | // decaduino.pdDataRequest(txData, 25); 210 | // state = TWR_ENGINE_STATE_INIT; 211 | // break; 212 | 213 | default : 214 | state = TWR_ENGINE_STATE_INIT; 215 | break; 216 | } 217 | } 218 | -------------------------------------------------------------------------------- /examples/DecaDuinoChat/DecaDuinoChat.ino: -------------------------------------------------------------------------------- 1 | // DecaDuinoChat 2 | // This sketch shows how to use the DecaDuino library to send and receive ascii messages via the Serial port over 3 | // the UWB radio 4 | // by Adrien van den Bossche 5 | // This sketch is a part of the DecaDuino Project - please refer to the DecaDuino LICENCE file for licensing details 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #define MAX_FRAME_LEN 120 13 | uint8_t rxData[MAX_FRAME_LEN]; 14 | uint16_t rxLen; 15 | int rxFrames; 16 | #ifdef ARDUINO_DWM1001_DEV 17 | DecaDuino decaduino(SS1, DW_IRQ); 18 | #define LED_ON LOW 19 | #define LED_OFF HIGH 20 | #else 21 | DecaDuino decaduino; 22 | #define LED_ON HIGH 23 | #define LED_OFF LOW 24 | #endif 25 | 26 | uint32_t dev_id; 27 | uint64_t euid; 28 | 29 | void promptCmd() 30 | { 31 | char string[29]; 32 | // Depends on how the standard library is implemented for platform 33 | // Works with DecaDuino and DWM1001-DEV 34 | sprintf(string, "%08lX-%08lX%08lX > ", 35 | dev_id, 36 | (uint32_t)(euid >> 32), (uint32_t)(euid & 0xFFFFFFFF) 37 | ); 38 | Serial.print(string); 39 | } 40 | 41 | void printConfig() 42 | { 43 | Serial.println("Current configuration"); 44 | Serial.println("---------------------"); 45 | Serial.print(" * channel = "); 46 | Serial.println(decaduino.getChannel()); 47 | Serial.print(" * datarate = "); 48 | Serial.println(DW1000_DATARATE[decaduino.getDataRate()]); 49 | Serial.print(" * preamble length = "); 50 | Serial.println(decaduino.getPreambleLength()); 51 | } 52 | 53 | void printHelp() 54 | { 55 | Serial.println("Available commands"); 56 | Serial.println("------------------"); 57 | Serial.println(" * config -> prints the current configuration"); 58 | Serial.println(" * help -> prints this message"); 59 | Serial.println(" * channel -> prompts and sets the channel"); 60 | Serial.println(" * datarate -> prompts and sets the datarate"); 61 | Serial.println(" * preamble -> prompts and sets the preamble length"); 62 | } 63 | 64 | char readChar() 65 | { 66 | while (Serial.available() == 0); 67 | return Serial.read(); 68 | } 69 | 70 | // ASCII Keycode 71 | #define BACKSPACE 8 72 | #define DEL 127 73 | void handleChar(char c, char * buffer, int * index) 74 | { 75 | if ((c == BACKSPACE || c == DEL) && *index > 0) 76 | { 77 | buffer[--(*index)] = 0; 78 | Serial.write(BACKSPACE); 79 | Serial.print(' '); 80 | Serial.write(BACKSPACE); 81 | } 82 | else if (isalnum(c) || isspace(c) || ispunct(c)) 83 | { 84 | buffer[(*index)++] = c; 85 | Serial.write(c); 86 | } 87 | } 88 | 89 | int promptInt() 90 | { 91 | char buffer[10]; 92 | int index = 0; 93 | Serial.print("Please enter a number: "); 94 | char c = readChar(); 95 | while (c != '\r' && c != '\n') 96 | { 97 | handleChar(c, buffer, &index); 98 | c = readChar(); 99 | } 100 | buffer[index] = 0; 101 | char *end; 102 | long l = strtol(buffer, &end, 10); 103 | if (*end != '\0') 104 | { 105 | Serial.println("Error: Incorrect input"); 106 | return -1; 107 | } 108 | Serial.println(); 109 | return (int) l; 110 | } 111 | 112 | void setChannel() 113 | { 114 | Serial.println("Channel configuration"); 115 | Serial.println("---------------------"); 116 | Serial.println(" * 1, f = 3494.4MHz, bw = 499.2MHz"); 117 | Serial.println(" * 2, f = 3993.6MHz, bw = 499.2MHz"); 118 | Serial.println(" * 3, f = 4492.8MHz, bw = 499.2MHz"); 119 | Serial.println(" * 4, f = 3993.6MHz, bw = 1331.2MHz"); 120 | Serial.println(" * 5, f = 6489.6MHz, bw = 499.2MHz"); 121 | Serial.println(" * 7, f = 6489.6MHz, bw = 1081.6MHz"); 122 | int i = promptInt(); 123 | if (i != -1) 124 | { 125 | decaduino.setChannel(i); 126 | } 127 | else 128 | { 129 | Serial.println("Error: Can't set Channel"); 130 | } 131 | } 132 | 133 | void setDataRate() 134 | { 135 | Serial.println("DataRate configuration"); 136 | Serial.println("----------------------"); 137 | Serial.println(" * 1, 110 Kbps"); 138 | Serial.println(" * 2, 850 Kbps"); 139 | Serial.println(" * 3, 6,8 Mbps"); 140 | int i = promptInt(); 141 | if (i != -1) 142 | { 143 | decaduino.setDataRate((dw1000_datarate_t) i); 144 | } 145 | else 146 | { 147 | Serial.println("Error: Can't set DataRate"); 148 | } 149 | } 150 | 151 | void setPreamble() 152 | { 153 | Serial.println("Preamble Length configuration"); 154 | Serial.println("-----------------------------"); 155 | Serial.println(" * 64"); 156 | Serial.println(" * 128"); 157 | Serial.println(" * 256"); 158 | Serial.println(" * 512"); 159 | Serial.println(" * 1024"); 160 | Serial.println(" * 1536"); 161 | Serial.println(" * 2048"); 162 | Serial.println(" * 4096"); 163 | int i = promptInt(); 164 | if (i != -1) 165 | { 166 | decaduino.setPreambleLength(i); 167 | } 168 | else 169 | { 170 | Serial.println("Error: Can't set Preamble"); 171 | } 172 | } 173 | 174 | void handleCmd(char * cmdline) 175 | { 176 | if (!strcmp("config", cmdline)) printConfig(); 177 | else if (!strcmp("help", cmdline)) printHelp(); 178 | else if (!strcmp("channel", cmdline)) setChannel(); 179 | else if (!strcmp("datarate", cmdline)) setDataRate(); 180 | else if (!strcmp("preamble", cmdline)) setPreamble(); 181 | else Serial.println("Error: command not recognized"); 182 | } 183 | 184 | void sendData(char * data, int size) 185 | { 186 | // LED ON, disable RX, send, wait sent complete, re-enable RX and LED OFF 187 | digitalWrite(LED_BUILTIN, LED_ON); 188 | decaduino.plmeRxDisableRequest(); // Always disable RX before request frame sending 189 | Serial.println("Sending... "); 190 | decaduino.pdDataRequest((uint8_t *)data, size); 191 | #ifdef ARDUINO_DWM1001_DEV 192 | digitalWrite(LED_BLUE, LED_ON); 193 | #endif 194 | while ( !decaduino.hasTxSucceeded() )decaduino.engine(); 195 | #ifdef ARDUINO_DWM1001_DEV 196 | digitalWrite(LED_BLUE, LED_OFF); 197 | #endif 198 | Serial.println("done!"); 199 | decaduino.plmeRxEnableRequest(); 200 | digitalWrite(LED_BUILTIN, LED_OFF); 201 | } 202 | 203 | void setup() 204 | { 205 | pinMode(LED_BUILTIN, OUTPUT); 206 | digitalWrite(LED_BUILTIN, LED_OFF); 207 | #ifdef ARDUINO_DWM1001_DEV 208 | pinMode(LED_GREEN, OUTPUT); 209 | pinMode(LED_BLUE, OUTPUT); 210 | digitalWrite(LED_GREEN, LED_OFF); 211 | digitalWrite(LED_BLUE, LED_OFF); 212 | #endif 213 | 214 | Serial.begin(115200); // Init Serial port 215 | 216 | #ifndef ARDUINO_DWM1001_DEV 217 | SPI.setSCK(14); // Set SPI clock pin (pin 14 on DecaWiNo board) 218 | #endif 219 | 220 | // Init DecaDuino and blink if initialisation fails 221 | if ( !decaduino.init() ) { 222 | Serial.println("decaduino init failed"); 223 | while(1) { digitalWrite(LED_BUILTIN, LED_ON); delay(50); digitalWrite(LED_BUILTIN, LED_OFF); delay(50); } 224 | } 225 | 226 | dev_id = decaduino.getDevID(); 227 | euid = decaduino.getEuid(); 228 | printConfig(); 229 | 230 | // Set RX buffer and enable RX 231 | decaduino.setRxBuffer(rxData, &rxLen); 232 | decaduino.plmeRxEnableRequest(); 233 | rxFrames = 0; 234 | Serial.println("DecaDuino chat ready!"); 235 | promptCmd(); 236 | } 237 | 238 | void loop() 239 | { 240 | decaduino.engine(); 241 | 242 | static char cmdline[MAX_FRAME_LEN]; 243 | static int index = 0; 244 | // This is the sender part **************************************************************** 245 | // Get chars on Serial and prepare buffer 246 | while ( ( Serial.available() > 0 ) && ( index < MAX_FRAME_LEN ) ) { 247 | char c = readChar(); 248 | if ( (c=='\n')||(c=='\r')||(index==MAX_FRAME_LEN) ) { 249 | Serial.println(); 250 | cmdline[index] = 0; 251 | if (index > 0) 252 | { 253 | if (cmdline[0] == '/') handleCmd((char *)(cmdline + 1)); 254 | else sendData(cmdline, index); 255 | } 256 | index = 0; 257 | promptCmd(); 258 | } 259 | else 260 | { 261 | handleChar(c, cmdline, &index); 262 | } 263 | } 264 | 265 | // This is the receiver part ************************************************************** 266 | if ( decaduino.rxFrameAvailable() ) { 267 | // LED ON, print received chars, re-enable RX and LED OFF 268 | digitalWrite(LED_BUILTIN, LED_ON); 269 | #ifdef ARDUINO_DWM1001_DEV 270 | digitalWrite(LED_GREEN, LED_ON); 271 | #endif 272 | Serial.println(); 273 | Serial.print("["); Serial.print(++rxFrames); Serial.print("] "); 274 | Serial.print(rxLen); 275 | Serial.print("bytes received: "); 276 | rxData[rxLen] = 0; 277 | Serial.println((char*)rxData); 278 | decaduino.plmeRxEnableRequest(); // Always renable RX after a frame reception 279 | digitalWrite(LED_BUILTIN, LED_OFF); 280 | #ifdef ARDUINO_DWM1001_DEV 281 | digitalWrite(LED_GREEN, LED_ON); 282 | #endif 283 | } 284 | } 285 | 286 | 287 | -------------------------------------------------------------------------------- /examples/DecaDuinoJSON/DecaDuinoJSON.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #define DEBUG_MESSAGE(...) if (debug) printf(__VA_ARGS__) 8 | 9 | #define MAX_MESSAGE_LEN 120 10 | #ifdef ARDUINO_DWM1001_DEV 11 | DecaDuino decaduino(SS1, DW_IRQ); 12 | #elif defined(TEENSYDUINO) 13 | #include 14 | #include 15 | DecaDuino decaduino; 16 | WiNoIO wino; 17 | #endif 18 | 19 | char txBuffer[MAX_MESSAGE_LEN]; 20 | int txBufferLen = 0; 21 | char rxBuffer[MAX_MESSAGE_LEN]; 22 | uint16_t rxBufferLen = 0; 23 | 24 | int debug = false; //true; 25 | int current_power = 0; 26 | unsigned long current_frequency = 0; 27 | int current_bandwidth = 0; 28 | int current_dataRate = 0; 29 | int current_preambleLength = 0; 30 | int tx_token = 0; 31 | char stored_nodeID[64]; 32 | 33 | uint32_t statsTimeout; 34 | int stats_rxPacketsReceived = 0; 35 | int stats_rxPacketsReceivedOK = 0; 36 | int stats_txPacketsReceived = 0; 37 | int stats_txPacketsEmitted = 0; 38 | #define STATS_PERIOD 60000 //ms 39 | 40 | char json_buffer[2048]; 41 | int json_buffer_len = 0; 42 | #define JSON_TIMEOUT 200 //ms 43 | 44 | int parseCommand ( char json[] ) 45 | { 46 | char json_buffer_temp[2048]; 47 | StaticJsonDocument<2048> jsonDoc; 48 | memcpy(json_buffer_temp, json, strlen(json)); 49 | DeserializationError error = deserializeJson(jsonDoc, json_buffer_temp); 50 | JsonObject root = jsonDoc.as(); 51 | 52 | if (error) { 53 | DEBUG_MESSAGE("parseObject() failed\r\n"); 54 | return false; 55 | } 56 | 57 | const char* message_type = root["message_type"]; 58 | if (strcmp(message_type, "DecaDuino_tx")) { 59 | return false; 60 | } 61 | 62 | int token = root["message"]["token"]; 63 | const char* nodeID = root["message"]["txInfo"]["nodeID"]; 64 | int immediately = root["message"]["txInfo"]["immediately"]; 65 | unsigned long frequency = root["message"]["txInfo"]["frequency"]; 66 | int power = root["message"]["txInfo"]["power"]; 67 | int preambleLength = root["message"]["txInfo"]["preambleLength"]; 68 | const char* modulation = root["message"]["txInfo"]["modulation"]; 69 | int bandwidth = root["message"]["txInfo"]["UWBModulationInfo"]["bandwidth"]; 70 | int dataRate = root["message"]["txInfo"]["UWBModulationInfo"]["dataRate"]; 71 | const char* phyPayload = root["message"]["phyPayload"]; 72 | unsigned char phyPayloadBytes[MAX_MESSAGE_LEN]; 73 | unsigned int payload_length = decode_base64((unsigned char*)phyPayload, phyPayloadBytes); 74 | 75 | if ( debug ) 76 | { 77 | // Print values for debug 78 | Serial.println("Decoded JSON data:"); 79 | Serial.print(" token: "); 80 | Serial.println(token); 81 | Serial.print(" nodeID: "); 82 | Serial.println(nodeID); 83 | Serial.print(" immediately: "); 84 | Serial.println(immediately); 85 | Serial.print(" frequency: "); 86 | Serial.println(frequency); 87 | Serial.print(" power: "); 88 | Serial.println(power); 89 | Serial.print(" preambleLength: "); 90 | Serial.println(preambleLength); 91 | Serial.print(" modulation: "); 92 | Serial.println(modulation); 93 | Serial.print(" bandwidth: "); 94 | Serial.println(bandwidth); 95 | Serial.print(" dataRate: "); 96 | Serial.println(dataRate); 97 | Serial.print(" phyPayload: "); 98 | Serial.println(phyPayload); 99 | Serial.print(" phyPayloadBytes: |"); 100 | for (unsigned int i=0; i statsTimeout ) send_stats(); 350 | if ( millis() > json_timeout ) reset_json_buffer(); 351 | 352 | // Get chars from Serial 353 | while ( Serial.available() > 0 ) 354 | { 355 | char c = Serial.read(); 356 | 357 | if ( json_buffer_len == 0 ) 358 | { 359 | if (c == '\r' || c == '\t' || c == ' ' || c == '\n' ) 360 | { 361 | continue; 362 | } 363 | else 364 | { 365 | // First char: set timeout 366 | json_timeout = millis() + JSON_TIMEOUT; 367 | } 368 | } 369 | 370 | if (c != '\r' && c != '\t') 371 | { 372 | json_buffer[json_buffer_len++] = c; 373 | } 374 | 375 | if (json_buffer_len != 0) 376 | { 377 | // Always put a NULL char at the end of the string for the parser 378 | json_buffer[json_buffer_len] = '\0'; 379 | if ( isJsonComplete(json_buffer) ) 380 | { 381 | if ( parseCommand(json_buffer) == true ) reset_json_buffer(); 382 | } 383 | } 384 | } 385 | 386 | // Send frame if length != 0 387 | if ( txBufferLen != 0 ) 388 | { 389 | if ( debug ) 390 | { 391 | printf("Sending frame: |"); 392 | for ( int i=0; i 2 | #include 3 | 4 | #define AIR_SPEED_OF_LIGHT 299792458 5 | #define DW1000_TIMEBASE 15.65E-12 6 | #define COEFF AIR_SPEED_OF_LIGHT*DW1000_TIMEBASE 7 | 8 | //#define VERBOSE 1 9 | 10 | #define TIMEOUT 40 11 | 12 | #define TWR_ENGINE_STATE_INIT 1 13 | #define TWR_ENGINE_STATE_WAIT_NEW_CYCLE 2 14 | #define TWR_ENGINE_STATE_SEND_START 3 15 | #define TWR_ENGINE_STATE_WAIT_START_SENT 4 16 | #define TWR_ENGINE_STATE_MEMORISE_T1 5 17 | #define TWR_ENGINE_STATE_WATCHDOG_FOR_ACK_REQ 6 18 | #define TWR_ENGINE_STATE_RX_ON_FOR_ACK_REQ 7 19 | #define TWR_ENGINE_STATE_WAIT_ACK_REQ 8 20 | #define TWR_ENGINE_STATE_MEMORISE_T4 9 21 | #define TWR_ENGINE_STATE_SEND_ACK 10 22 | #define TWR_ENGINE_STATE_WAIT_ACK_SENT 11 23 | #define TWR_ENGINE_STATE_MEMORISE_T5 12 24 | #define TWR_ENGINE_STATE_WATCHDOG_FOR_DATA_REPLY 13 25 | #define TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY 14 26 | #define TWR_ENGINE_STATE_WAIT_DATA_REPLY 15 27 | #define TWR_ENGINE_STATE_EXTRACT_T2_T3_T6 16 28 | 29 | #define TWR_ENGINE_STATE_ON_FOR_DATA_REQUEST 17 30 | #define TWR_ENGINE_WAIT_DATAREQ 18 31 | 32 | #define TWR_MSG_TYPE_UNKNOW 0 33 | #define TWR_MSG_TYPE_START 1 34 | #define TWR_MSG_TYPE_ACK_REQ 2 35 | #define TWR_MSG_TYPE_ACK 3 36 | #define TWR_MSG_TYPE_DATA_REPLY 4 37 | 38 | #define TWR_MSG_TYPE_DATAREQ 5 39 | 40 | #define COMPUTE 50 41 | 42 | int seqnum; 43 | 44 | int rxFrames; 45 | 46 | uint64_t t1, t2, t3, t4, t5, t6; 47 | uint64_t tof; 48 | 49 | uint64_t _Delai; 50 | 51 | #ifdef ARDUINO_DWM1001_DEV 52 | DecaDuino decaduino(SS1, DW_IRQ); 53 | #elif defined(TEENSYDUINO) 54 | DecaDuino decaduino; 55 | #endif 56 | uint8_t txData[128]; 57 | uint8_t rxData[128]; 58 | uint16_t rxLen; 59 | int state; 60 | int timeout; 61 | 62 | int ons; 63 | 64 | /*storage for timestamps*/ 65 | struct Ranging{ 66 | uint16_t anchor; 67 | uint64_t t1; 68 | uint64_t t2; 69 | uint64_t t3; 70 | uint64_t t4; 71 | uint64_t t5; 72 | uint64_t t6; 73 | double distance; 74 | int valid; 75 | double offsetPPM_t4; 76 | double offsetPPM_drep; 77 | }; 78 | 79 | uint16_t hi16=0; 80 | double tmp=0; 81 | 82 | uint64_t test=0x0000000F24586219; 83 | 84 | Ranging results[3]; 85 | uint32_t addrPANID=0xDECA0025; 86 | int mindex; 87 | 88 | void setup() { 89 | 90 | pinMode(13, OUTPUT); 91 | 92 | #ifdef TEENSYDUINO 93 | SPI.setSCK(14); 94 | #endif 95 | if (!decaduino.init(addrPANID)){ 96 | Serial.print("decaduino init failled"); 97 | while(1){ 98 | digitalWrite(13, HIGH); 99 | delay(50); 100 | digitalWrite(13, LOW); 101 | delay(50); 102 | } 103 | } 104 | decaduino.setRxBuffer(rxData, &rxLen); 105 | state=TWR_ENGINE_STATE_INIT; 106 | ons=0; 107 | results[0].anchor=0x0022; 108 | results[1].anchor=0x0023; 109 | results[2].anchor=0x0024; 110 | results[0].valid=0; 111 | results[1].valid=0; 112 | results[2].valid=0; 113 | seqnum=0; 114 | //decaduino.print(test); 115 | mindex=0; 116 | } 117 | 118 | void loop() { 119 | 120 | decaduino.engine(); 121 | 122 | switch (state) { 123 | 124 | case TWR_ENGINE_STATE_INIT : 125 | decaduino.plmeRxDisableRequest(); 126 | state = TWR_ENGINE_STATE_WAIT_NEW_CYCLE; 127 | break; 128 | 129 | case TWR_ENGINE_STATE_WAIT_NEW_CYCLE : 130 | delay(100); 131 | //Serial.println("New PDS_TWR"); 132 | 133 | state = TWR_ENGINE_STATE_SEND_START; 134 | results[0].valid=0; 135 | results[1].valid=0; 136 | results[2].valid=0; 137 | break; 138 | 139 | case TWR_ENGINE_STATE_SEND_START : 140 | txData[0] =0x41; 141 | txData[1] =0x88; 142 | txData[2] =seqnum; 143 | txData[3] =0xCA; 144 | txData[4] =0xDE; 145 | txData[5] =0xFF; 146 | txData[6] =0xFF; 147 | txData[7] =0x25; 148 | txData[8] =0x00; 149 | txData[9] = TWR_MSG_TYPE_START; 150 | decaduino.pdDataRequest(txData, 10); 151 | state = TWR_ENGINE_STATE_WAIT_START_SENT; 152 | seqnum++; 153 | break; 154 | 155 | case TWR_ENGINE_STATE_WAIT_START_SENT : 156 | if (decaduino.hasTxSucceeded()) 157 | state = TWR_ENGINE_STATE_MEMORISE_T1; 158 | break; 159 | 160 | case TWR_ENGINE_STATE_MEMORISE_T1 : 161 | t1 = decaduino.getLastTxTimestamp(); 162 | state = TWR_ENGINE_STATE_WATCHDOG_FOR_ACK_REQ; 163 | break; 164 | 165 | case TWR_ENGINE_STATE_WATCHDOG_FOR_ACK_REQ : 166 | timeout = millis() + TIMEOUT*2; 167 | state = TWR_ENGINE_STATE_RX_ON_FOR_ACK_REQ; 168 | break; 169 | 170 | case TWR_ENGINE_STATE_RX_ON_FOR_ACK_REQ : 171 | decaduino.plmeRxEnableRequest(); 172 | state = TWR_ENGINE_STATE_WAIT_ACK_REQ; 173 | break; 174 | 175 | case TWR_ENGINE_STATE_WAIT_ACK_REQ : 176 | if (( millis() > timeout)||(ons>2)) {//too late 177 | //Serial.println("_______________TIMEOUT!!!!!!"); 178 | if(ons==0){//je n'ai aucune réponse 179 | state = TWR_ENGINE_STATE_INIT; 180 | } else { state=TWR_ENGINE_STATE_SEND_ACK;} 181 | } 182 | else { //encore du temps 183 | if (decaduino.rxFrameAvailable()){ 184 | if (rxData[9] == TWR_MSG_TYPE_ACK_REQ) { 185 | state = TWR_ENGINE_STATE_MEMORISE_T4; 186 | 187 | 188 | ons++; 189 | } else state = TWR_ENGINE_STATE_RX_ON_FOR_ACK_REQ; 190 | } 191 | } 192 | break; 193 | 194 | case TWR_ENGINE_STATE_MEMORISE_T4 : 195 | t4 = decaduino.getLastRxTimestamp(); 196 | switch(rxData[7]){ 197 | case 0x23: results[1].t4=t4; 198 | results[1].offsetPPM_t4=decaduino.getLastRxSkew(); 199 | // Serial.println("rx ACK+REQ: 0x23 "); 200 | break; 201 | case 0x22: results[0].t4=t4; 202 | results[0].offsetPPM_t4=decaduino.getLastRxSkew(); 203 | //Serial.println("rx ACK+REQ: 0x22 "); 204 | break; 205 | case 0x24: results[2].t4=t4; 206 | results[2].offsetPPM_t4=decaduino.getLastRxSkew(); 207 | //Serial.println("rx ACK+REQ: 0x24 "); 208 | break; 209 | 210 | } 211 | if ( millis() > timeout){ 212 | state = TWR_ENGINE_STATE_SEND_ACK; 213 | }else{//still got time 214 | state=TWR_ENGINE_STATE_RX_ON_FOR_ACK_REQ; 215 | } 216 | break; 217 | 218 | case TWR_ENGINE_STATE_SEND_ACK : 219 | decaduino.plmeRxDisableRequest(); 220 | txData[0] =0x41; 221 | txData[1] =0x88; 222 | txData[2] =seqnum; 223 | txData[3] =0xCA; 224 | txData[4] =0xDE; 225 | txData[5] =0xFF; 226 | txData[6] =0xFF; 227 | txData[7] =0x25; 228 | txData[8] =0x00; 229 | txData[9]= TWR_MSG_TYPE_DATAREQ; 230 | decaduino.pdDataRequest(txData, 10); 231 | state = TWR_ENGINE_STATE_WAIT_ACK_SENT; 232 | //Serial.println("ready to send datareq"); 233 | seqnum++; 234 | break; 235 | 236 | case TWR_ENGINE_STATE_WAIT_ACK_SENT : 237 | if (decaduino.hasTxSucceeded()){ 238 | state = TWR_ENGINE_STATE_MEMORISE_T5; 239 | //Serial.println("_________________got t5!"); 240 | ons=0; 241 | }//else Serial.println("tx issue"); 242 | break; 243 | 244 | case TWR_ENGINE_STATE_MEMORISE_T5 : 245 | t5 = decaduino.getLastTxTimestamp(); 246 | state = TWR_ENGINE_STATE_WATCHDOG_FOR_DATA_REPLY; 247 | break; 248 | 249 | case TWR_ENGINE_STATE_WATCHDOG_FOR_DATA_REPLY : 250 | timeout = millis() + TIMEOUT; 251 | state = TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY; 252 | break; 253 | 254 | case TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY : 255 | decaduino.plmeRxEnableRequest(); 256 | state = TWR_ENGINE_STATE_WAIT_DATA_REPLY; 257 | break; 258 | 259 | case TWR_ENGINE_STATE_WAIT_DATA_REPLY : 260 | if (( millis() > timeout)||(ons>2)) { 261 | if(ons!=0) 262 | state=COMPUTE; 263 | else 264 | state = TWR_ENGINE_STATE_INIT;} 265 | else { 266 | if (decaduino.rxFrameAvailable()){ 267 | if (rxData[9] == TWR_MSG_TYPE_DATA_REPLY) { 268 | state = TWR_ENGINE_STATE_EXTRACT_T2_T3_T6; 269 | ons++; 270 | //Serial.println("received DATA_REPLY"); 271 | } else state = TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY; 272 | } 273 | } 274 | break; 275 | 276 | case TWR_ENGINE_STATE_EXTRACT_T2_T3_T6 : 277 | t2 = decaduino.decodeUint64(&rxData[10]); 278 | t3 = decaduino.decodeUint64(&rxData[19]); 279 | t6 = decaduino.decodeUint64(&rxData[27]); 280 | 281 | switch(rxData[7]){ 282 | case 0x23: results[1].t2=t2; 283 | results[1].t3=t3; 284 | results[1].t6=t6; 285 | results[1].valid=1; 286 | results[1].offsetPPM_drep=decaduino.getLastRxSkew(); 287 | 288 | break; 289 | case 0x22: results[0].t2=t2; 290 | results[0].t3=t3; 291 | results[0].t6=t6; 292 | results[0].valid=1; 293 | results[0].offsetPPM_drep=decaduino.getLastRxSkew(); 294 | break; 295 | case 0x24: results[2].t2=t2; 296 | results[2].t3=t3; 297 | results[2].t6=t6; 298 | results[2].valid=1; 299 | results[2].offsetPPM_drep=decaduino.getLastRxSkew(); 300 | break; 301 | 302 | } 303 | 304 | if ( millis() > timeout) { 305 | if(ons!=0){ 306 | state=COMPUTE; 307 | }else state = TWR_ENGINE_STATE_INIT;} 308 | else { 309 | state = TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY; 310 | } 311 | break; 312 | 313 | case COMPUTE: 314 | Serial.print("index="); 315 | Serial.println(mindex); 316 | if(results[0].valid==1){ 317 | Serial.print("set 0|"); 318 | #ifdef VERBOSE 319 | Serial.print("t1="); 320 | decaduino.print(t1); 321 | 322 | Serial.print("t2="); 323 | decaduino.print(results[0].t2); 324 | 325 | Serial.print("t3="); 326 | decaduino.print(results[0].t3); 327 | 328 | Serial.print("t4="); 329 | decaduino.print(results[0].t4); 330 | 331 | Serial.print("t5="); 332 | decaduino.print(t5); 333 | 334 | Serial.print("t6="); 335 | decaduino.print(results[0].t6); 336 | Serial.println(); 337 | #endif 338 | tof = (2*results[0].t4 - t1 - 2*results[0].t3 + results[0].t2 + results[0].t6 - t5)/4; 339 | Serial.print("ToF="); 340 | printf("%"PRIu64"",tof); 341 | hi16=(uint16_t)(tof >> 32); 342 | if(hi16 & 0x80!=0){ 343 | // Serial.println("TOF is negative"); 344 | tof |=0xFFFFFF0000000000; 345 | tof=~tof; 346 | tof++; 347 | tmp=-1*tof; 348 | Serial.print("|d="); 349 | Serial.print(-1*(double)(tof*COEFF)); 350 | Serial.print("|"); 351 | //Serial.println(tmp); 352 | }else{ 353 | //Serial.println("TOF is positive"); 354 | Serial.print("|d="); 355 | Serial.print((double)(tof*COEFF)); 356 | Serial.print("|"); 357 | } 358 | Serial.print("clkOffset(ppm)_t4="); 359 | Serial.print(results[0].offsetPPM_t4); 360 | Serial.print("|"); 361 | Serial.print("clkOffset(ppm)_drep="); 362 | Serial.print(results[0].offsetPPM_drep); 363 | Serial.print("|"); 364 | _Delai=results[1].t4-results[0].t4; 365 | Serial.print("D="); 366 | printf("%"PRIu64"",_Delai); 367 | Serial.println(); 368 | } 369 | 370 | if(results[1].valid==1){ 371 | Serial.print("set 1|"); 372 | #ifdef VERBOSE 373 | Serial.print("t1="); 374 | decaduino.print(t1); 375 | 376 | Serial.print("t2="); 377 | decaduino.print(results[1].t2); 378 | 379 | Serial.print("t3="); 380 | decaduino.print(results[1].t3); 381 | 382 | Serial.print("t4="); 383 | decaduino.print(results[1].t4); 384 | 385 | Serial.print("t5="); 386 | decaduino.print(t5); 387 | 388 | Serial.print("t6="); 389 | decaduino.print(results[1].t6); 390 | Serial.println(); 391 | #endif 392 | tof = (2*results[1].t4 - t1 - 2*results[1].t3 + results[1].t2 + results[1].t6 - t5)/4; 393 | Serial.print("ToF="); 394 | printf("%"PRIu64"",tof); 395 | 396 | hi16=(uint16_t)(tof >> 32); 397 | if(hi16 & 0x80!=0){ 398 | //Serial.println("TOF is negative"); 399 | //erial.println("TOF is negative"); 400 | tof |=0xFFFFFF0000000000; 401 | tof=~tof; 402 | tof++; 403 | Serial.print("|d="); 404 | Serial.print(-1*(double)(tof*COEFF)); 405 | Serial.print("|"); 406 | //Serial.println(tmp); 407 | }else{ 408 | // Serial.println("TOF is positive"); 409 | 410 | Serial.print("|d="); 411 | Serial.print((double)(tof*COEFF)); 412 | Serial.print("|"); 413 | } 414 | Serial.print("clkOffset(ppm)="); 415 | Serial.print(results[1].offsetPPM_t4); 416 | Serial.print("|"); 417 | Serial.print("clkOffset(ppm)_drep="); 418 | Serial.print(results[1].offsetPPM_drep); 419 | Serial.print("|"); 420 | _Delai=results[1].t4-results[0].t4; 421 | Serial.print("D="); 422 | printf("%"PRIu64"",_Delai); 423 | Serial.println(); 424 | } 425 | 426 | if(results[2].valid==1){ 427 | Serial.print("set 2|"); 428 | #ifdef VERBOSE 429 | Serial.print("t1="); 430 | decaduino.print(t1); 431 | 432 | Serial.print("t2="); 433 | decaduino.print(results[2].t2); 434 | 435 | Serial.print("t3="); 436 | decaduino.print(results[2].t3); 437 | 438 | Serial.print("t4="); 439 | decaduino.print(results[2].t4); 440 | 441 | Serial.print("t5="); 442 | decaduino.print(t5); 443 | 444 | Serial.print("t6="); 445 | decaduino.print(results[2].t6); 446 | Serial.println(); 447 | #endif 448 | tof = (2*results[2].t4 - t1 - 2*results[2].t3 + results[2].t2 + results[2].t6 - t5)/4; 449 | Serial.print("ToF="); 450 | printf("%"PRIu64"",tof); 451 | 452 | hi16=(uint16_t)(tof >> 32); 453 | if(hi16 & 0x80!=0){ 454 | //Serial.println("TOF is negative"); 455 | //Serial.println("TOF is negative"); 456 | tof |=0xFFFFFF0000000000; 457 | tof=~tof; 458 | tof++; 459 | Serial.print("|d="); 460 | Serial.print(-1*(double)(tof*COEFF)); 461 | Serial.print("|"); 462 | //Serial.println(tmp); 463 | }else{ 464 | //Serial.println("TOF is positive"); 465 | 466 | Serial.print("|d="); 467 | Serial.print((double)(tof*COEFF)); 468 | Serial.print("|"); 469 | } 470 | Serial.print("clkOffset(ppm)="); 471 | Serial.print(results[2].offsetPPM_t4); 472 | Serial.print("|"); 473 | Serial.print("clkOffset(ppm)_drep="); 474 | Serial.print(results[2].offsetPPM_drep); 475 | Serial.print("|"); 476 | _Delai=results[1].t4-results[0].t4; 477 | Serial.print("D="); 478 | printf("%"PRIu64"",_Delai); 479 | Serial.println(); 480 | } 481 | 482 | mindex=mindex+1; 483 | state = TWR_ENGINE_STATE_INIT; 484 | break; 485 | 486 | default: 487 | state = TWR_ENGINE_STATE_INIT; 488 | break; 489 | } 490 | } 491 | -------------------------------------------------------------------------------- /examples/DecaDuinoAllRangingProtocols/DecaDuinoAllRangingProtocols.ino: -------------------------------------------------------------------------------- 1 | //#define ENABLE_CALIBRATION_FROM_EEPROM 2 | //#define ENABLE_LABEL_FROM_EEPROM 3 | 4 | #include 5 | #include 6 | #if defined(ENABLE_CALIBRATION_FROM_EEPROM) || defined(ENABLE_LABEL_FROM_EEPROM) 7 | #include 8 | uint16_t antennaDelay; 9 | #endif 10 | 11 | uint16_t label, label2; 12 | int role, protocol; 13 | int inter_cycle_delay; 14 | int rxA_enable_between_cycles; 15 | 16 | #define EXPERIMENTATION_DURATION 8 // seconds 17 | 18 | #define COEFF RANGING_UNIT 19 | 20 | #define TIMEOUT 10 21 | 22 | #define RGB_RED_PIN 5 23 | #define RGB_GREEN_PIN 4 24 | #define RGB_BLUE_PIN 3 25 | 26 | 27 | // TWR_ENGINE_STATE for node 'A' 28 | #define TWR_ENGINE_STATE_INIT 1 29 | #define TWR_ENGINE_STATE_WAIT_NEW_CYCLE 2 30 | #define TWR_ENGINE_STATE_SEND_START 3 31 | #define TWR_ENGINE_STATE_WAIT_SEND_START 4 32 | #define TWR_ENGINE_STATE_MEMORISE_T1 5 33 | #define TWR_ENGINE_STATE_WATCHDOG_FOR_ACK 6 34 | #define TWR_ENGINE_STATE_RX_ON_FOR_ACK 7 35 | #define TWR_ENGINE_STATE_WAIT_ACK 8 36 | #define TWR_ENGINE_STATE_MEMORISE_T4 9 37 | #define TWR_ENGINE_STATE_WATCHDOG_FOR_DATA_REPLY 10 38 | #define TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY 11 39 | #define TWR_ENGINE_STATE_WAIT_DATA_REPLY 12 40 | #define TWR_ENGINE_STATE_EXTRACT_T2_T3 13 41 | 42 | // TWR_ENGINE_STATE for node 'B' 43 | #define TWR_ENGINE_STATE_INIT 1 44 | #define TWR_ENGINE_STATE_RX_ON 2 45 | #define TWR_ENGINE_STATE_WAIT_START 3 46 | #define TWR_ENGINE_STATE_MEMORISE_T2 4 47 | #define TWR_ENGINE_STATE_SEND_ACK 5 48 | #define TWR_ENGINE_STATE_WAIT_SENT 6 49 | #define TWR_ENGINE_STATE_MEMORISE_T3 7 50 | #define TWR_ENGINE_STATE_WAIT_BEFORE_SEND_DATA_REPLY 8 51 | #define TWR_ENGINE_STATE_SEND_DATA_REPLY 9 52 | 53 | // SDSTWR_ENGINE_STATE for node 'A' 54 | #define SDSTWR_ENGINE_STATE_INIT 1 55 | #define SDSTWR_ENGINE_STATE_WAIT_NEW_CYCLE 2 56 | #define SDSTWR_ENGINE_STATE_SEND_START 3 57 | #define SDSTWR_ENGINE_STATE_WAIT_START_SENT 4 58 | #define SDSTWR_ENGINE_STATE_MEMORISE_T1 5 59 | #define SDSTWR_ENGINE_STATE_WATCHDOG_FOR_ACK_REQ 6 60 | #define SDSTWR_ENGINE_STATE_RX_ON_FOR_ACK_REQ 7 61 | #define SDSTWR_ENGINE_STATE_WAIT_ACK_REQ 8 62 | #define SDSTWR_ENGINE_STATE_MEMORISE_T4 9 63 | #define SDSTWR_ENGINE_STATE_SEND_ACK 10 64 | #define SDSTWR_ENGINE_STATE_WAIT_ACK_SENT 11 65 | #define SDSTWR_ENGINE_STATE_MEMORISE_T5 12 66 | #define SDSTWR_ENGINE_STATE_WATCHDOG_FOR_DATA_REPLY 13 67 | #define SDSTWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY 14 68 | #define SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY 15 69 | #define SDSTWR_ENGINE_STATE_EXTRACT_T2_T3_T6 16 70 | 71 | // SDSTWR_ENGINE_STATE for node 'B' 72 | #define SDSTWR_ENGINE_STATE_INIT 1 73 | #define SDSTWR_ENGINE_STATE_RX_ON 2 74 | #define SDSTWR_ENGINE_STATE_WAIT_START 3 75 | #define SDSTWR_ENGINE_STATE_MEMORISE_T2 4 76 | #define SDSTWR_ENGINE_STATE_SEND_ACK_REQ 5 77 | #define SDSTWR_ENGINE_STATE_WAIT_SENT 6 78 | #define SDSTWR_ENGINE_STATE_MEMORISE_T3 7 79 | #define SDSTWR_ENGINE_STATE_WATCHDOG_FOR_ACK 8 80 | #define SDSTWR_ENGINE_STATE_RX_ON_FOR_ACK 9 81 | #define SDSTWR_ENGINE_STATE_WAIT_ACK 10 82 | #define SDSTWR_ENGINE_STATE_MEMORISE_T6 11 83 | #define SDSTWR_ENGINE_STATE_SEND_DATA_REPLY 12 84 | 85 | // TWR messages 86 | #define TWR_MSG_TYPE_UNKNOWN 0 87 | #define TWR_MSG_TYPE_START 52 88 | #define TWR_MSG_TYPE_ACK 53 89 | #define TWR_MSG_TYPE_DATA_REPLY 54 90 | 91 | // SDSTWR messages 92 | #define SDSTWR_MSG_TYPE_UNKNOW 0 93 | #define SDSTWR_MSG_TYPE_START 55 94 | #define SDSTWR_MSG_TYPE_ACK_REQ 56 95 | #define SDSTWR_MSG_TYPE_ACK 57 96 | #define SDSTWR_MSG_TYPE_DATA_REPLY 58 97 | 98 | int i; 99 | int rxFrames; 100 | 101 | uint64_t t1, t2, t3, t4, t5, t6; 102 | int64_t tof; 103 | int64_t tof_skew; 104 | 105 | DecaDuino decaduino; 106 | uint8_t txData[128]; 107 | uint8_t rxData[128]; 108 | uint16_t rxLen; 109 | int state; 110 | uint32_t timeout; 111 | 112 | void setup() { 113 | 114 | uint8_t buf[2]; 115 | 116 | pinMode(13, OUTPUT); 117 | pinMode(RGB_RED_PIN, OUTPUT); 118 | pinMode(RGB_GREEN_PIN, OUTPUT); 119 | pinMode(RGB_BLUE_PIN, OUTPUT); 120 | digitalWrite(RGB_RED_PIN, HIGH); 121 | digitalWrite(RGB_GREEN_PIN, LOW); 122 | digitalWrite(RGB_BLUE_PIN, HIGH); 123 | 124 | while(Serial.available()==0); 125 | Serial.setTimeout(86400); 126 | label=Serial.parseInt(); 127 | label2=Serial.parseInt(); 128 | protocol=Serial.parseInt(); 129 | role=Serial.parseInt(); 130 | inter_cycle_delay=Serial.parseInt(); 131 | rxA_enable_between_cycles=Serial.parseInt(); 132 | 133 | Serial.printf("My label is 0x%04X, my role is %d and I range with 0x%04X using protocol %d, with inter_cycle_delay=%d and rxA_enable_between_cycles=%d\n", label, role, label2, protocol, inter_cycle_delay, rxA_enable_between_cycles); 134 | 135 | SPI.setSCK(14); 136 | if ( !decaduino.init() ) { 137 | Serial.print("decaduino init failed"); 138 | while(1) { 139 | digitalWrite(13, HIGH); 140 | delay(50); 141 | digitalWrite(13, LOW); 142 | delay(50); 143 | } 144 | } 145 | 146 | #ifdef ENABLE_LABEL_FROM_EEPROM 147 | 148 | // Gets DecaWiNo label from the end of EEPROM (the two last bytes) 149 | buf[0] = EEPROM.read(EEPROM.length()-2); 150 | buf[1] = EEPROM.read(EEPROM.length()-1); 151 | label = decaduino.decodeUint16(buf); 152 | 153 | if ( label == 0xffff ) { 154 | Serial.println("Unvalid label value found in EEPROM. Using default value (0x0000)."); 155 | label = 0; 156 | } else Serial.printf("My label is 0x%04X\n", label); 157 | 158 | #endif 159 | 160 | #ifdef ENABLE_CALIBRATION_FROM_EEPROM 161 | 162 | // Gets antenna delay from the end of EEPROM. The two last bytes are used for DecaWiNo label, 163 | // so use n-2 and n-3 to store the antenna delay (16bit value) 164 | buf[0] = EEPROM.read(EEPROM.length()-4); 165 | buf[1] = EEPROM.read(EEPROM.length()-3); 166 | antennaDelay = decaduino.decodeUint16(buf); 167 | 168 | if ( antennaDelay == 0xffff ) { 169 | Serial.println("Unvalid antenna delay value found in EEPROM. Using default value."); 170 | } else decaduino.setAntennaDelay(antennaDelay); 171 | 172 | #endif 173 | 174 | decaduino.setRxBuffer(rxData, &rxLen); 175 | 176 | switch (protocol) { 177 | case 1: state=TWR_ENGINE_STATE_INIT; break; 178 | case 2: state=SDSTWR_ENGINE_STATE_INIT; break; 179 | default: break; 180 | } 181 | 182 | Serial.println("RUN"); 183 | } 184 | 185 | 186 | void loop() { 187 | 188 | decaduino.engine(); 189 | 190 | if ( millis() > EXPERIMENTATION_DURATION*1000 ) { 191 | Serial.println("END"); 192 | digitalWrite(RGB_RED_PIN, HIGH); 193 | digitalWrite(RGB_GREEN_PIN, LOW); 194 | digitalWrite(RGB_BLUE_PIN, HIGH); 195 | while (1); 196 | } 197 | 198 | switch (protocol) { 199 | case 1: twrEngine(); break; 200 | case 2: sdstwrEngine(); break; 201 | default: break; 202 | } 203 | } 204 | 205 | 206 | void twrEngine ( void ) { 207 | 208 | switch (role) { 209 | 210 | case 1: 211 | switch (state) { 212 | 213 | case TWR_ENGINE_STATE_INIT: 214 | digitalWrite(RGB_RED_PIN, LOW); 215 | digitalWrite(RGB_GREEN_PIN, HIGH); 216 | digitalWrite(RGB_BLUE_PIN, HIGH); 217 | decaduino.plmeRxDisableRequest(); 218 | state = TWR_ENGINE_STATE_WAIT_NEW_CYCLE; 219 | break; 220 | 221 | case TWR_ENGINE_STATE_WAIT_NEW_CYCLE: 222 | if ( rxA_enable_between_cycles == 1 ) { 223 | decaduino.plmeRxEnableRequest(); 224 | } 225 | delay(inter_cycle_delay); 226 | decaduino.plmeRxDisableRequest(); 227 | Serial.println("New TWR"); 228 | state = TWR_ENGINE_STATE_SEND_START; 229 | break; 230 | 231 | case TWR_ENGINE_STATE_SEND_START: 232 | txData[0] = TWR_MSG_TYPE_START; 233 | decaduino.pdDataRequest(txData, 1); 234 | state = TWR_ENGINE_STATE_WAIT_SEND_START; 235 | break; 236 | 237 | case TWR_ENGINE_STATE_WAIT_SEND_START: 238 | if ( decaduino.hasTxSucceeded() ) 239 | state = TWR_ENGINE_STATE_MEMORISE_T1; 240 | break; 241 | 242 | case TWR_ENGINE_STATE_MEMORISE_T1: 243 | t1 = decaduino.lastTxTimestamp; 244 | state = TWR_ENGINE_STATE_WATCHDOG_FOR_ACK; 245 | break; 246 | 247 | case TWR_ENGINE_STATE_WATCHDOG_FOR_ACK: 248 | timeout = millis() + TIMEOUT; 249 | state = TWR_ENGINE_STATE_RX_ON_FOR_ACK; 250 | break; 251 | 252 | case TWR_ENGINE_STATE_RX_ON_FOR_ACK: 253 | decaduino.plmeRxEnableRequest(); 254 | state = TWR_ENGINE_STATE_WAIT_ACK; 255 | break; 256 | 257 | case TWR_ENGINE_STATE_WAIT_ACK: 258 | if ( millis() > timeout ) { 259 | Serial.println("ACK not received"); 260 | state = TWR_ENGINE_STATE_INIT; 261 | } else { 262 | if ( decaduino.rxFrameAvailable() ) { 263 | if ( rxData[0] == TWR_MSG_TYPE_ACK ) { 264 | state = TWR_ENGINE_STATE_MEMORISE_T4; 265 | } else state = TWR_ENGINE_STATE_RX_ON_FOR_ACK; 266 | } 267 | } 268 | break; 269 | 270 | case TWR_ENGINE_STATE_MEMORISE_T4: 271 | t4 = decaduino.lastRxTimestamp; 272 | state = TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY; 273 | break; 274 | 275 | case TWR_ENGINE_STATE_WATCHDOG_FOR_DATA_REPLY: 276 | timeout = millis() + TIMEOUT; 277 | state = TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY; 278 | break; 279 | 280 | case TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY: 281 | decaduino.plmeRxEnableRequest(); 282 | state = TWR_ENGINE_STATE_WAIT_DATA_REPLY; 283 | break; 284 | 285 | case TWR_ENGINE_STATE_WAIT_DATA_REPLY: 286 | if ( millis() > timeout ) { 287 | Serial.println("DATA_REPLY not received"); 288 | state = TWR_ENGINE_STATE_INIT; 289 | } else { 290 | if ( decaduino.rxFrameAvailable() ) { 291 | if ( rxData[0] == TWR_MSG_TYPE_DATA_REPLY ) { 292 | state = TWR_ENGINE_STATE_EXTRACT_T2_T3; 293 | } else state = TWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY; 294 | } 295 | } 296 | break; 297 | 298 | case TWR_ENGINE_STATE_EXTRACT_T2_T3: 299 | t2 = decaduino.decodeUint64(&rxData[1]); 300 | t3 = decaduino.decodeUint64(&rxData[9]); 301 | tof = (t4 - t1 - (t3 - t2))/2; 302 | tof_skew = (t4 - t1 - (1+1.0E-6*decaduino.getLastRxSkew())*(t3 - t2))/2; 303 | //Serial.printf("New timestamps,0x%04X,0x%04X,%d,%d,%f,%lld,%lld,%lld,%lld\n",label, label2, protocol, role, decaduino.getLastRxSkew(), t1, t2, t3, t4); 304 | Serial.printf("New timestamps_hex,0x%04X,0x%04X,%d,%d,%f,",label, label2, protocol, role, decaduino.getLastRxSkew()); 305 | decaduino.printUint64(t1); 306 | Serial.print(","); 307 | decaduino.printUint64(t2); 308 | Serial.print(","); 309 | decaduino.printUint64(t3); 310 | Serial.print(","); 311 | decaduino.printUint64(t4); 312 | Serial.printf(",%d,%d,%f,%d",rxA_enable_between_cycles, inter_cycle_delay, decaduino.getNLOSIndication(),decaduino.getTemperatureRaw()); 313 | Serial.println(); 314 | Serial.printf("New ranging,0x%04X,0x%04X,%f,-1,%d,%d,-1,%f,%f,%d\n",label, label2, tof*COEFF, /*tof, */protocol, role, /*tof_skew, */decaduino.getLastRxSkew(), decaduino.getNLOSIndication(),decaduino.getTemperatureRaw()); 315 | state = TWR_ENGINE_STATE_INIT; 316 | break; 317 | 318 | default: 319 | state = TWR_ENGINE_STATE_INIT; 320 | break; 321 | } 322 | break; 323 | 324 | case 2: 325 | switch (state) { 326 | 327 | case TWR_ENGINE_STATE_INIT: 328 | digitalWrite(RGB_RED_PIN, HIGH); 329 | digitalWrite(RGB_GREEN_PIN, HIGH); 330 | digitalWrite(RGB_BLUE_PIN, LOW); 331 | //decaduino.plmeRxDisableRequest(); 332 | state = TWR_ENGINE_STATE_RX_ON; 333 | break; 334 | 335 | case TWR_ENGINE_STATE_RX_ON: 336 | decaduino.plmeRxEnableRequest(); 337 | state = TWR_ENGINE_STATE_WAIT_START; 338 | break; 339 | 340 | case TWR_ENGINE_STATE_WAIT_START: 341 | if ( decaduino.rxFrameAvailable() ) { 342 | if ( rxData[0] == TWR_MSG_TYPE_START ) { 343 | state = TWR_ENGINE_STATE_MEMORISE_T2; 344 | } else state = TWR_ENGINE_STATE_RX_ON; 345 | } 346 | break; 347 | 348 | case TWR_ENGINE_STATE_MEMORISE_T2: 349 | t2 = decaduino.lastRxTimestamp; 350 | state = TWR_ENGINE_STATE_SEND_ACK; 351 | break; 352 | 353 | case TWR_ENGINE_STATE_SEND_ACK: 354 | txData[0] = TWR_MSG_TYPE_ACK; 355 | decaduino.pdDataRequest(txData, 1); 356 | state = TWR_ENGINE_STATE_WAIT_SENT; 357 | break; 358 | 359 | case TWR_ENGINE_STATE_WAIT_SENT: 360 | if ( decaduino.hasTxSucceeded() ) 361 | state = TWR_ENGINE_STATE_MEMORISE_T3; 362 | break; 363 | 364 | case TWR_ENGINE_STATE_MEMORISE_T3: 365 | t3 = decaduino.lastTxTimestamp; 366 | state = TWR_ENGINE_STATE_WAIT_BEFORE_SEND_DATA_REPLY; 367 | break; 368 | 369 | case TWR_ENGINE_STATE_WAIT_BEFORE_SEND_DATA_REPLY: 370 | delay(1); 371 | state = TWR_ENGINE_STATE_SEND_DATA_REPLY; 372 | break; 373 | 374 | case TWR_ENGINE_STATE_SEND_DATA_REPLY: 375 | txData[0] = TWR_MSG_TYPE_DATA_REPLY; 376 | decaduino.encodeUint64(t2, &txData[1]); 377 | decaduino.encodeUint64(t3, &txData[9]); 378 | decaduino.pdDataRequest(txData, 17); 379 | state = TWR_ENGINE_STATE_INIT; 380 | break; 381 | 382 | default: 383 | state = TWR_ENGINE_STATE_INIT; 384 | break; 385 | } 386 | break; 387 | } 388 | } 389 | 390 | 391 | void sdstwrEngine ( void ) { 392 | 393 | switch (role) { 394 | 395 | case 1: 396 | switch (state) { 397 | 398 | case SDSTWR_ENGINE_STATE_INIT : 399 | digitalWrite(RGB_RED_PIN, LOW); 400 | digitalWrite(RGB_GREEN_PIN, HIGH); 401 | digitalWrite(RGB_BLUE_PIN, HIGH); 402 | decaduino.plmeRxDisableRequest(); 403 | state = SDSTWR_ENGINE_STATE_WAIT_NEW_CYCLE; 404 | break; 405 | 406 | case SDSTWR_ENGINE_STATE_WAIT_NEW_CYCLE : 407 | if ( rxA_enable_between_cycles == 1 ) { 408 | decaduino.plmeRxEnableRequest(); 409 | } 410 | delay(inter_cycle_delay); 411 | decaduino.plmeRxDisableRequest(); 412 | Serial.println("New SDS_TWR"); 413 | state = SDSTWR_ENGINE_STATE_SEND_START; 414 | break; 415 | 416 | case SDSTWR_ENGINE_STATE_SEND_START : 417 | txData[0] = SDSTWR_MSG_TYPE_START; 418 | decaduino.pdDataRequest(txData, 1); 419 | state = SDSTWR_ENGINE_STATE_WAIT_START_SENT; 420 | break; 421 | 422 | case SDSTWR_ENGINE_STATE_WAIT_START_SENT : 423 | if (decaduino.hasTxSucceeded()) 424 | state = SDSTWR_ENGINE_STATE_MEMORISE_T1; 425 | break; 426 | 427 | case SDSTWR_ENGINE_STATE_MEMORISE_T1 : 428 | t1 = decaduino.lastTxTimestamp; 429 | state = SDSTWR_ENGINE_STATE_WATCHDOG_FOR_ACK_REQ; 430 | break; 431 | 432 | case SDSTWR_ENGINE_STATE_WATCHDOG_FOR_ACK_REQ : 433 | timeout = millis() + TIMEOUT; 434 | state = SDSTWR_ENGINE_STATE_RX_ON_FOR_ACK_REQ; 435 | break; 436 | 437 | case SDSTWR_ENGINE_STATE_RX_ON_FOR_ACK_REQ : 438 | decaduino.plmeRxEnableRequest(); 439 | state = SDSTWR_ENGINE_STATE_WAIT_ACK_REQ; 440 | break; 441 | 442 | case SDSTWR_ENGINE_STATE_WAIT_ACK_REQ : 443 | if ( millis() > timeout) { 444 | Serial.println("ACK not received"); 445 | state = SDSTWR_ENGINE_STATE_INIT; 446 | } 447 | else { 448 | if (decaduino.rxFrameAvailable()){ 449 | if (rxData[0] == SDSTWR_MSG_TYPE_ACK_REQ) { 450 | state = SDSTWR_ENGINE_STATE_MEMORISE_T4; 451 | } else state = SDSTWR_ENGINE_STATE_RX_ON_FOR_ACK_REQ; 452 | } 453 | } 454 | break; 455 | 456 | case SDSTWR_ENGINE_STATE_MEMORISE_T4 : 457 | t4 = decaduino.lastRxTimestamp; 458 | state = SDSTWR_ENGINE_STATE_SEND_ACK; 459 | break; 460 | 461 | case SDSTWR_ENGINE_STATE_SEND_ACK : 462 | txData[0]= SDSTWR_MSG_TYPE_ACK; 463 | decaduino.pdDataRequest(txData, 1); 464 | state = SDSTWR_ENGINE_STATE_WAIT_ACK_SENT; 465 | break; 466 | 467 | case SDSTWR_ENGINE_STATE_WAIT_ACK_SENT : 468 | if (decaduino.hasTxSucceeded()){ 469 | state = SDSTWR_ENGINE_STATE_MEMORISE_T5;} 470 | break; 471 | 472 | case SDSTWR_ENGINE_STATE_MEMORISE_T5 : 473 | t5 = decaduino.lastTxTimestamp; 474 | state = SDSTWR_ENGINE_STATE_WATCHDOG_FOR_DATA_REPLY; 475 | break; 476 | 477 | case SDSTWR_ENGINE_STATE_WATCHDOG_FOR_DATA_REPLY : 478 | timeout = millis() + TIMEOUT; 479 | state = SDSTWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY; 480 | break; 481 | 482 | case SDSTWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY : 483 | decaduino.plmeRxEnableRequest(); 484 | state = SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY; 485 | break; 486 | 487 | case SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY : 488 | if ( millis() > timeout) { 489 | state = SDSTWR_ENGINE_STATE_INIT; 490 | Serial.println("DATA_REPLY not received"); 491 | } 492 | else { 493 | if (decaduino.rxFrameAvailable()){ 494 | if (rxData[0] == SDSTWR_MSG_TYPE_DATA_REPLY) { 495 | state = SDSTWR_ENGINE_STATE_EXTRACT_T2_T3_T6; 496 | } else state = SDSTWR_ENGINE_STATE_RX_ON_FOR_DATA_REPLY; 497 | } 498 | } 499 | break; 500 | 501 | case SDSTWR_ENGINE_STATE_EXTRACT_T2_T3_T6 : 502 | t2 = decaduino.decodeUint64(&rxData[1]); 503 | t3 = decaduino.decodeUint64(&rxData[9]); 504 | t6 = decaduino.decodeUint64(&rxData[17]); 505 | tof = (2*t4 - t1 - 2*t3 + t2 + t6 - t5)/4; 506 | tof_skew = ( (t4-t1) - (t5-t4) - (1+1.0E-6*decaduino.getLastRxSkew())*((t3-t2)+(t6-t3)) )/4; 507 | //tof = (2*t4 - t1 - 2*t3 + t2 + t6 - t5)/4; 508 | //tof_skew = (t4 - t1 - (1+1.0E-6*decaduino.getLastRxSkew())*(t3 - t2))/2; 509 | // Serial.printf("New ranging,0x%04X,0x%04X,%f,%ld,%d,%d\n",label, label2, tof*COEFF, (int32_t)tof, protocol, role, (int32_t)tof_skew, decaduino.getLastRxSkew()); 510 | //Serial.printf("New timestamps,0x%04X,0x%04X,%d,%d,%f,%lld,%lld,%lld,%lld,%lld,%lld\n",label, label2, protocol, role, decaduino.getLastRxSkew(), t1, t2, t3, t4, t5, t6); 511 | Serial.printf("New timestamps_hex,0x%04X,0x%04X,%d,%d,%f,",label, label2, protocol, role, decaduino.getLastRxSkew()); 512 | decaduino.printUint64(t1); 513 | Serial.print(","); 514 | decaduino.printUint64(t2); 515 | Serial.print(","); 516 | decaduino.printUint64(t3); 517 | Serial.print(","); 518 | decaduino.printUint64(t4); 519 | Serial.print(","); 520 | decaduino.printUint64(t5); 521 | Serial.print(","); 522 | decaduino.printUint64(t6); 523 | Serial.printf(",%d,%d,%f,%d",rxA_enable_between_cycles, inter_cycle_delay,decaduino.getNLOSIndication(),decaduino.getTemperatureRaw()); 524 | Serial.println(); 525 | Serial.printf("New ranging,0x%04X,0x%04X,%f,-1,%d,%d,-1,%f,%f,%d\n",label, label2, tof*COEFF, /*tof, */protocol, role, /*tof_skew, */decaduino.getLastRxSkew(), decaduino.getNLOSIndication(),decaduino.getTemperatureRaw()); 526 | //Serial.printf("New ranging,0x%04X,0x%04X,%f,%ld,%d,%d\n",label, label2, tof*COEFF, (int32_t)tof, protocol, role, tof*COEFF); 527 | state = SDSTWR_ENGINE_STATE_INIT; 528 | break; 529 | 530 | default: 531 | state = SDSTWR_ENGINE_STATE_INIT; 532 | break; 533 | } 534 | break; 535 | 536 | case 2: 537 | switch (state) { 538 | 539 | case SDSTWR_ENGINE_STATE_INIT : 540 | digitalWrite(RGB_RED_PIN, HIGH); 541 | digitalWrite(RGB_GREEN_PIN, HIGH); 542 | digitalWrite(RGB_BLUE_PIN, LOW); 543 | state = SDSTWR_ENGINE_STATE_RX_ON; 544 | break; 545 | 546 | case SDSTWR_ENGINE_STATE_RX_ON : 547 | decaduino.plmeRxEnableRequest(); 548 | state = SDSTWR_ENGINE_STATE_WAIT_START; 549 | break; 550 | 551 | case SDSTWR_ENGINE_STATE_WAIT_START : 552 | if (decaduino.rxFrameAvailable()){ 553 | if ( rxData[0] == SDSTWR_MSG_TYPE_START){ 554 | state = SDSTWR_ENGINE_STATE_MEMORISE_T2; 555 | } else state = SDSTWR_ENGINE_STATE_RX_ON; 556 | } 557 | break; 558 | 559 | case SDSTWR_ENGINE_STATE_MEMORISE_T2 : 560 | t2 = decaduino.lastRxTimestamp; 561 | state = SDSTWR_ENGINE_STATE_SEND_ACK_REQ; 562 | break; 563 | 564 | case SDSTWR_ENGINE_STATE_SEND_ACK_REQ : 565 | txData[0]= SDSTWR_MSG_TYPE_ACK_REQ; 566 | decaduino.pdDataRequest(txData, 1); 567 | state = SDSTWR_ENGINE_STATE_WAIT_SENT; 568 | break; 569 | 570 | case SDSTWR_ENGINE_STATE_WAIT_SENT: 571 | if (decaduino.hasTxSucceeded()){ 572 | state = SDSTWR_ENGINE_STATE_MEMORISE_T3; } 573 | break; 574 | 575 | case SDSTWR_ENGINE_STATE_MEMORISE_T3 : 576 | t3 = decaduino.lastTxTimestamp; 577 | state = SDSTWR_ENGINE_STATE_WATCHDOG_FOR_ACK; 578 | break; 579 | 580 | case SDSTWR_ENGINE_STATE_WATCHDOG_FOR_ACK : 581 | timeout = millis() + TIMEOUT; 582 | state = SDSTWR_ENGINE_STATE_RX_ON_FOR_ACK; 583 | break; 584 | 585 | case SDSTWR_ENGINE_STATE_RX_ON_FOR_ACK : 586 | decaduino.plmeRxEnableRequest(); 587 | state = SDSTWR_ENGINE_STATE_WAIT_ACK; 588 | break; 589 | 590 | case SDSTWR_ENGINE_STATE_WAIT_ACK : 591 | if ( millis() > timeout) { 592 | state = SDSTWR_ENGINE_STATE_INIT; 593 | Serial.println("ACK not received"); 594 | } 595 | else { 596 | if (decaduino.rxFrameAvailable()){ 597 | if (rxData[0] == SDSTWR_MSG_TYPE_ACK) { 598 | state = SDSTWR_ENGINE_STATE_MEMORISE_T6; 599 | } else state = SDSTWR_ENGINE_STATE_RX_ON_FOR_ACK; 600 | } 601 | } 602 | break; 603 | 604 | case SDSTWR_ENGINE_STATE_MEMORISE_T6 : 605 | t6 = decaduino.lastRxTimestamp; 606 | state = SDSTWR_ENGINE_STATE_SEND_DATA_REPLY; 607 | break; 608 | 609 | case SDSTWR_ENGINE_STATE_SEND_DATA_REPLY : 610 | txData[0] = SDSTWR_MSG_TYPE_DATA_REPLY; 611 | decaduino.encodeUint64(t2, &txData[1]); 612 | decaduino.encodeUint64(t3, &txData[9]); 613 | decaduino.encodeUint64(t6, &txData[17]); 614 | decaduino.pdDataRequest(txData, 25); 615 | state = SDSTWR_ENGINE_STATE_INIT; 616 | break; 617 | 618 | default : 619 | state = SDSTWR_ENGINE_STATE_INIT; 620 | break; 621 | } 622 | break; 623 | 624 | default: 625 | break; 626 | } 627 | } 628 | 629 | -------------------------------------------------------------------------------- /examples/DecaDuinoAllRangingProtocols2/DecaDuinoAllRangingProtocols2.ino: -------------------------------------------------------------------------------- 1 | //#define ENABLE_CALIBRATION_FROM_EEPROM 2 | //#define ENABLE_LABEL_FROM_EEPROM 3 | 4 | #include 5 | #include 6 | #include 7 | #if defined(ENABLE_CALIBRATION_FROM_EEPROM) || defined(ENABLE_LABEL_FROM_EEPROM) 8 | #include 9 | uint16_t antennaDelay; 10 | #endif 11 | 12 | #define RANGING_DEBUG 1 // 1 to print debugging messages else 0 13 | 14 | bool rangingDebug = RANGING_DEBUG; 15 | 16 | uint16_t label, label2; 17 | uint16_t broadcast = 0xFFFF; 18 | 19 | uint8_t protocol; 20 | int inter_cycle_delay; 21 | int rxA_enable_between_cycles; 22 | 23 | uint8_t sqn = 0; 24 | uint8_t sqn_ack; 25 | bool isAvailable = false; 26 | 27 | uint8_t src[2]; 28 | uint8_t dst[2]; 29 | 30 | // Node specifications 31 | uint8_t node_status; 32 | float node_x; 33 | float node_y; 34 | float node_z; 35 | float node_eAR; 36 | uint8_t node_protocols_available; 37 | 38 | typedef struct Neighb Neighb; 39 | 40 | struct Neighb { 41 | uint16_t neighb_label; 42 | uint8_t Status; 43 | float x; 44 | float y; 45 | float z; 46 | float eAR; 47 | uint8_t protocols_available; 48 | }; 49 | 50 | uint32_t printNeighbTimeout; 51 | uint32_t rangeNeighbTimeout; 52 | 53 | #define NEIGHB_POS_PERIOD 5 // seconds 54 | #define PRINT_NEIGHB_PERIOD 5 // seconds 55 | #define RANGE_NEIGHB_PERIOD 10 // seconds 56 | 57 | #define MAX_NEIGHB 100 58 | 59 | Neighb neighb_table[MAX_NEIGHB]; 60 | 61 | #define EXPERIMENTATION_DURATION 500 // seconds 62 | 63 | #define COEFF RANGING_UNIT 64 | 65 | #define TIMEOUT 50 66 | #define TIMEOUT2 40 67 | 68 | #define RGB_RED_PIN 5 69 | #define RGB_GREEN_PIN 4 70 | #define RGB_BLUE_PIN 3 71 | 72 | #define NEIGHB_POS 0 73 | #define RANGING_MSG 1 74 | 75 | #define TWR_PROTOCOL 0 76 | #define SDSTWR_PROTOCOL 1 77 | 78 | #define RANGING_ENGINE_STATE_IDLE 1 79 | #define TWR_ENGINE_STATE_IDLE 1 80 | #define SDSTWR_ENGINE_STATE_IDLE 1 81 | 82 | // RANGING_ENGINE_STATE 83 | #define RANGING_ENGINE_STATE_SEND_RANGING_REQUEST 2 84 | #define RANGING_ENGINE_STATE_WAIT_RANGING_REQUEST_SENT 3 85 | 86 | // TWR_ENGINE_STATE for node 'CLIENT' 87 | #define TWR_ENGINE_STATE_SEND_START 2 88 | #define TWR_ENGINE_STATE_MEMORISE_T1 3 89 | #define TWR_ENGINE_STATE_WAIT_ACK 4 90 | #define TWR_ENGINE_STATE_WAIT_DATA_REPLY 5 91 | #define TWR_ENGINE_STATE_EXTRACT_T2_T3 6 92 | #define TWR_ENGINE_STATE_SEND_ACK_CLIENT 7 93 | #define TWR_ENGINE_STATE_WAIT_ACK_SENT_CLIENT 8 94 | 95 | // TWR_ENGINE_STATE for node 'SERVER' 96 | #define TWR_ENGINE_STATE_INIT_SERVER 9 97 | #define TWR_ENGINE_STATE_WAIT_START 10 98 | #define TWR_ENGINE_STATE_SEND_ACK_SERVER 11 99 | #define TWR_ENGINE_STATE_WAIT_ACK_SENT_SERVER 12 100 | #define TWR_ENGINE_STATE_MEMORISE_T3 13 101 | #define TWR_ENGINE_STATE_SEND_DATA_REPLY 14 102 | #define TWR_ENGINE_STATE_WAIT_DATA_REPLY_SENT 15 103 | #define TWR_ENGINE_STATE_WAIT_ACK_SERVER 16 104 | 105 | // SDSTWR_ENGINE_STATE for node 'CLIENT' 106 | #define SDSTWR_ENGINE_STATE_SEND_START 2 107 | #define SDSTWR_ENGINE_STATE_MEMORISE_T1 3 108 | #define SDSTWR_ENGINE_STATE_WAIT_ACK_CLIENT 4 109 | #define SDSTWR_ENGINE_STATE_SEND_ACK_CLIENT 5 110 | #define SDSTWR_ENGINE_STATE_MEMORISE_T5 6 111 | #define SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY 7 112 | #define SDSTWR_ENGINE_STATE_EXTRACT_T2_T3_T6 8 113 | 114 | // SDSTWR_ENGINE_STATE for node 'SERVER' 115 | #define SDSTWR_ENGINE_STATE_INIT_SERVER 9 116 | #define SDSTWR_ENGINE_STATE_WAIT_START 10 117 | #define SDSTWR_ENGINE_STATE_SEND_ACK_SERVER 11 118 | #define SDSTWR_ENGINE_STATE_MEMORISE_T3 12 119 | #define SDSTWR_ENGINE_STATE_WAIT_ACK_SERVER 13 120 | #define SDSTWR_ENGINE_STATE_SEND_DATA_REPLY 14 121 | #define SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY_SENT 15 122 | 123 | // TWR messages 124 | #define TWR_MSG_TYPE_UNKNOWN 0 125 | #define TWR_MSG_TYPE_START 52 126 | #define TWR_MSG_TYPE_ACK 53 127 | #define TWR_MSG_TYPE_DATA_REPLY 54 128 | 129 | // SDSTWR messages 130 | #define SDSTWR_MSG_TYPE_UNKNOW 0 131 | #define SDSTWR_MSG_TYPE_START 55 132 | #define SDSTWR_MSG_TYPE_ACK_REQ 56 133 | #define SDSTWR_MSG_TYPE_ACK 57 134 | #define SDSTWR_MSG_TYPE_DATA_REPLY 58 135 | 136 | #define RX_ON 1 // Waiting for a specific frame 137 | #define RX_DONE 0 // Correct frame received 138 | #define TX_ON 1 // A frame needs to be sent 139 | #define TX_SENDING 0 // pdDataRequest 140 | #define TX_END_ON 1 // Waiting for the frame to be sent 141 | #define TX_SUCCEEDED 0 // Frame has been sent 142 | 143 | int tx = 0; 144 | int txEnd = 0; 145 | int rx = RX_ON; 146 | 147 | int i; 148 | int rxFrames; 149 | 150 | uint64_t t1, t2, t3, t4, t5, t6; 151 | uint64_t mask = 0xFFFFFFFFFF; 152 | int32_t tof; 153 | int32_t tof_skew; 154 | 155 | DecaDuino decaduino; 156 | uint8_t txData[128]; 157 | uint8_t rxData[128]; 158 | uint16_t rxLen; 159 | 160 | int state_ranging; 161 | int state_twr; 162 | int state_sdstwr; 163 | 164 | uint32_t timeout; 165 | uint32_t neighb_timeout; 166 | 167 | int nbData; 168 | int msg_type; 169 | bool ranging = false; 170 | bool rangingReq = false; 171 | 172 | 173 | void macEngine() { 174 | 175 | if ( tx == TX_ON ) { 176 | // A frame need to be sent 177 | rx = RX_DONE; 178 | decaduino.plmeRxDisableRequest(); 179 | decaduino.pdDataRequest(txData, nbData); 180 | tx = TX_SENDING; 181 | txEnd = TX_END_ON; 182 | } 183 | 184 | if ( txEnd == TX_END_ON ) { 185 | // Checks if the frame has been sent 186 | if ( decaduino.hasTxSucceeded()) { 187 | txEnd = TX_SUCCEEDED; 188 | if ( txData[5] == NEIGHB_POS ) { 189 | rx = RX_ON; 190 | } 191 | decaduino.plmeRxEnableRequest(); 192 | } 193 | } 194 | 195 | if ( rx == RX_ON ) { 196 | if ( decaduino.rxFrameAvailable()) { 197 | dst[0] = rxData[2]; 198 | dst[1] = rxData[3]; 199 | if ( decaduino.decodeUint16(dst) == label ) { 200 | src[0] = rxData[0]; 201 | src[1] = rxData[1]; 202 | msg_type = rxData[7]; 203 | if (( !rangingReq ) && ( !ranging ) && ( rxData[5] == RANGING_MSG )) { 204 | protocol = rxData[6]; 205 | rangingResponse(); 206 | rx = RX_DONE; 207 | } else if ( ranging ) { 208 | if ( decaduino.decodeUint16(src) == label2 ) { 209 | rx = RX_DONE; 210 | } 211 | } 212 | } else if (( !rangingReq ) && ( !ranging ) && ( rxData[5] == NEIGHB_POS )) { 213 | src[0] = rxData[0]; 214 | src[1] = rxData[1]; 215 | neighbSave(); 216 | } 217 | decaduino.plmeRxEnableRequest(); 218 | } 219 | } 220 | } 221 | 222 | 223 | void rangingEngine() { 224 | 225 | if ( rangingReq) { 226 | switch (state_ranging) { 227 | 228 | case RANGING_ENGINE_STATE_IDLE: 229 | break; 230 | 231 | case RANGING_ENGINE_STATE_SEND_RANGING_REQUEST: 232 | decaduino.encodeUint16(label, &txData[0]); 233 | decaduino.encodeUint16(label2, &txData[2]); 234 | txData[4] = sqn; 235 | txData[5] = RANGING_MSG; 236 | txData[6] = protocol; 237 | nbData = 7; 238 | tx = TX_ON; 239 | state_ranging = RANGING_ENGINE_STATE_WAIT_RANGING_REQUEST_SENT; 240 | break; 241 | 242 | case RANGING_ENGINE_STATE_WAIT_RANGING_REQUEST_SENT: 243 | if (( txEnd == TX_SUCCEEDED )) { 244 | if ( rangingDebug ) { 245 | Serial.println("RANGING REQUEST SENT"); 246 | } 247 | rangingReq = false; 248 | state_ranging = RANGING_ENGINE_STATE_IDLE; 249 | if ( protocol == TWR_PROTOCOL ) { 250 | ranging = true; 251 | state_twr = TWR_ENGINE_STATE_SEND_START; 252 | } else if ( protocol == SDSTWR_PROTOCOL ) { 253 | ranging = true; 254 | state_sdstwr = SDSTWR_ENGINE_STATE_SEND_START; 255 | } 256 | } 257 | break; 258 | 259 | default: 260 | state_ranging = RANGING_ENGINE_STATE_IDLE; 261 | break; 262 | } 263 | } 264 | } 265 | 266 | 267 | void twrEngine() { 268 | 269 | switch (state_twr) { 270 | 271 | case TWR_ENGINE_STATE_IDLE: 272 | ranging = false; 273 | rx = RX_ON; 274 | break; 275 | 276 | case TWR_ENGINE_STATE_SEND_START: 277 | if ( rangingDebug ) { 278 | Serial.println("SEND START"); 279 | } 280 | decaduino.encodeUint16(label, &txData[0]); 281 | decaduino.encodeUint16(label2, &txData[2]); 282 | txData[4] = sqn; 283 | txData[5] = RANGING_MSG; 284 | txData[6] = TWR_PROTOCOL; 285 | txData[7] = TWR_MSG_TYPE_START; 286 | nbData = 8; 287 | tx = TX_ON; 288 | state_twr = TWR_ENGINE_STATE_MEMORISE_T1; 289 | break; 290 | 291 | case TWR_ENGINE_STATE_MEMORISE_T1: 292 | if (( txEnd == TX_SUCCEEDED )) { 293 | if ( rangingDebug ) { 294 | Serial.println("START SENT AND MEMO T1"); 295 | } 296 | t1 = decaduino.lastTxTimestamp; 297 | timeout = millis() + TIMEOUT; 298 | rx = RX_ON; 299 | state_twr = TWR_ENGINE_STATE_WAIT_ACK; 300 | } 301 | break; 302 | 303 | case TWR_ENGINE_STATE_WAIT_ACK: 304 | if ( millis() > timeout ) { 305 | //state_twr = TWR_ENGINE_STATE_SEND_START; 306 | state_twr = TWR_ENGINE_STATE_IDLE; // abort ranging if ACK has not been received 307 | } else { 308 | if (( rx == RX_DONE ) && ( msg_type == TWR_MSG_TYPE_ACK )) { 309 | if ( rangingDebug ) { 310 | Serial.println("ACK RECEIVED & MEMO T4"); 311 | } 312 | sqn++; 313 | t4 = decaduino.lastRxTimestamp; 314 | timeout = millis() + TIMEOUT2; 315 | rx = RX_ON; 316 | state_twr = TWR_ENGINE_STATE_WAIT_DATA_REPLY; 317 | } 318 | } 319 | break; 320 | 321 | case TWR_ENGINE_STATE_WAIT_DATA_REPLY: 322 | if ( millis() > timeout ) { 323 | //state_twr = TWR_ENGINE_STATE_SEND_START; 324 | state_twr = TWR_ENGINE_STATE_IDLE; // abort ranging if DATA_REPLY has not been received 325 | } else { 326 | if (( rx == RX_DONE ) && ( msg_type == TWR_MSG_TYPE_DATA_REPLY )) { 327 | if ( rangingDebug ) { 328 | Serial.println("DATA REPLY RECEIVED"); 329 | } 330 | sqn_ack = rxData[4] + 1; 331 | state_twr = TWR_ENGINE_STATE_EXTRACT_T2_T3; 332 | } 333 | } 334 | break; 335 | 336 | case TWR_ENGINE_STATE_EXTRACT_T2_T3: 337 | if ( rangingDebug ) { 338 | Serial.println("EXTRACT T2 T3"); 339 | } 340 | t2 = decaduino.decodeUint64(&rxData[8]); 341 | t3 = decaduino.decodeUint64(&rxData[16]); 342 | tof_skew = ((( t4 - t1 ) & mask ) - ( 1 + 1.0E-6*decaduino.getLastRxSkew()) * (( t3 - t2 ) & mask ))/2; 343 | Serial.print("ToF="); 344 | Serial.print(tof_skew, HEX); 345 | Serial.print(" d="); 346 | Serial.print(tof_skew*COEFF); 347 | Serial.println(); 348 | if (tof_skew*COEFF < 0) { 349 | Serial.println("ERROR NEGATIVE VALUE FOR DISTANCE"); 350 | } 351 | state_twr = TWR_ENGINE_STATE_SEND_ACK_CLIENT; 352 | break; 353 | 354 | case TWR_ENGINE_STATE_SEND_ACK_CLIENT: 355 | if ( rangingDebug ) { 356 | Serial.println("SEND ACK TO SERVER"); 357 | } 358 | decaduino.encodeUint16(label, &txData[0]); 359 | decaduino.encodeUint16(label2, &txData[2]); 360 | txData[4] = sqn_ack; 361 | txData[5] = RANGING_MSG; 362 | txData[6] = TWR_PROTOCOL; 363 | txData[7] = TWR_MSG_TYPE_ACK; 364 | tx = TX_ON; 365 | state_twr = TWR_ENGINE_STATE_WAIT_ACK_SENT_CLIENT; 366 | break; 367 | 368 | case TWR_ENGINE_STATE_WAIT_ACK_SENT_CLIENT: 369 | if ( txEnd == TX_SUCCEEDED ) { 370 | if ( rangingDebug ) { 371 | Serial.println("ACK SENT"); 372 | } 373 | state_twr = TWR_ENGINE_STATE_IDLE; 374 | } 375 | break; 376 | 377 | 378 | case TWR_ENGINE_STATE_INIT_SERVER: 379 | if ( rangingDebug ) { 380 | Serial.println("TWR SERVER"); 381 | } 382 | rx = RX_ON; 383 | state_twr = TWR_ENGINE_STATE_WAIT_START; 384 | break; 385 | 386 | case TWR_ENGINE_STATE_WAIT_START: 387 | if (( rx == RX_DONE ) && ( msg_type == TWR_MSG_TYPE_START )) { 388 | if ( rangingDebug ) { 389 | Serial.println("START RECEIVED & MEMO T2"); 390 | } 391 | sqn_ack = rxData[4] + 1; 392 | t2 = decaduino.lastRxTimestamp; 393 | state_twr = TWR_ENGINE_STATE_SEND_ACK_SERVER; 394 | } 395 | break; 396 | 397 | case TWR_ENGINE_STATE_SEND_ACK_SERVER: 398 | if ( rangingDebug ) { 399 | Serial.println("SEND ACK TO CLIENT"); 400 | } 401 | decaduino.encodeUint16(label, &txData[0]); 402 | decaduino.encodeUint16(label2, &txData[2]); 403 | txData[4] = sqn_ack; 404 | txData[5] = RANGING_MSG; 405 | txData[6] = TWR_PROTOCOL; 406 | txData[7] = TWR_MSG_TYPE_ACK; 407 | nbData = 8; 408 | tx = TX_ON; 409 | state_twr = TWR_ENGINE_STATE_MEMORISE_T3; 410 | break; 411 | 412 | case TWR_ENGINE_STATE_MEMORISE_T3: 413 | if ( txEnd == TX_SUCCEEDED) { 414 | if ( rangingDebug ) { 415 | Serial.println("ACK SENT AND MEMO T3"); 416 | } 417 | t3 = decaduino.lastTxTimestamp; 418 | state_twr = TWR_ENGINE_STATE_SEND_DATA_REPLY; 419 | } 420 | break; 421 | 422 | case TWR_ENGINE_STATE_SEND_DATA_REPLY: 423 | if ( rangingDebug ) { 424 | Serial.println("SEND DATA REPLY"); 425 | } 426 | decaduino.encodeUint16(label, &txData[0]); 427 | decaduino.encodeUint16(label2, &txData[2]); 428 | txData[4] = sqn; 429 | txData[5] = RANGING_MSG; 430 | txData[6] = TWR_PROTOCOL; 431 | txData[7] = TWR_MSG_TYPE_DATA_REPLY; 432 | decaduino.encodeUint64(t2, &txData[8]); 433 | decaduino.encodeUint64(t3, &txData[16]); 434 | nbData = 24; 435 | tx = TX_ON; 436 | state_twr = TWR_ENGINE_STATE_WAIT_DATA_REPLY_SENT; 437 | break; 438 | 439 | case TWR_ENGINE_STATE_WAIT_DATA_REPLY_SENT: 440 | if (( txEnd == TX_SUCCEEDED )) { 441 | if ( rangingDebug ) { 442 | Serial.println("DATA REPLY SENT"); 443 | } 444 | timeout = millis() + TIMEOUT; 445 | rx = RX_ON; 446 | state_twr = TWR_ENGINE_STATE_WAIT_ACK_SERVER; 447 | } 448 | break; 449 | 450 | case TWR_ENGINE_STATE_WAIT_ACK_SERVER: 451 | if ( millis() > timeout ) { 452 | state_twr = TWR_ENGINE_STATE_INIT_SERVER; 453 | } else { 454 | if (( rx == RX_DONE ) && ( msg_type == TWR_MSG_TYPE_ACK )) { 455 | if ( rangingDebug ) { 456 | Serial.println("ACK RECEIVED FROM CLIENT"); 457 | } 458 | sqn++; 459 | state_twr = TWR_ENGINE_STATE_IDLE; 460 | } 461 | } 462 | break; 463 | 464 | default: 465 | state_twr = TWR_ENGINE_STATE_IDLE; 466 | break; 467 | } 468 | } 469 | 470 | 471 | void sdstwrEngine() { 472 | 473 | switch (state_sdstwr) { 474 | 475 | case SDSTWR_ENGINE_STATE_IDLE: 476 | ranging = false; 477 | rx = RX_ON; 478 | break; 479 | 480 | case SDSTWR_ENGINE_STATE_SEND_START: 481 | if ( rangingDebug ) { 482 | Serial.println("SEND START"); 483 | } 484 | decaduino.encodeUint16(label, &txData[0]); 485 | decaduino.encodeUint16(label2, &txData[2]); 486 | txData[4] = sqn; 487 | txData[5] = RANGING_MSG; 488 | txData[6] = SDSTWR_PROTOCOL; 489 | txData[7] = SDSTWR_MSG_TYPE_START; 490 | nbData = 8; 491 | tx = TX_ON; 492 | state_sdstwr = SDSTWR_ENGINE_STATE_MEMORISE_T1; 493 | break; 494 | 495 | case SDSTWR_ENGINE_STATE_MEMORISE_T1: 496 | if (( txEnd == TX_SUCCEEDED )) { 497 | if ( rangingDebug ) { 498 | Serial.println("START SENT AND MEMO T1"); 499 | } 500 | t1 = decaduino.lastTxTimestamp; 501 | timeout = millis() + TIMEOUT; 502 | rx = RX_ON; 503 | state_sdstwr = SDSTWR_ENGINE_STATE_WAIT_ACK_CLIENT; 504 | } 505 | break; 506 | 507 | case SDSTWR_ENGINE_STATE_WAIT_ACK_CLIENT: 508 | if ( millis() > timeout ) { 509 | // state_sdstwr = SDSTWR_ENGINE_STATE_SEND_START; 510 | state_sdstwr = SDSTWR_ENGINE_STATE_IDLE; // abort ranging if ACK has not been received 511 | } else { 512 | if (( rx == RX_DONE ) && ( msg_type == SDSTWR_MSG_TYPE_ACK )) { 513 | if ( rangingDebug ) { 514 | Serial.println("ACK RECEIVED & MEMO T4"); 515 | } 516 | sqn++; 517 | t4 = decaduino.lastRxTimestamp; 518 | state_sdstwr = SDSTWR_ENGINE_STATE_SEND_ACK_CLIENT; 519 | } 520 | } 521 | break; 522 | 523 | case SDSTWR_ENGINE_STATE_SEND_ACK_CLIENT: 524 | if ( rangingDebug ) { 525 | Serial.println("SEND ACK TO SERVER"); 526 | } 527 | decaduino.encodeUint16(label, &txData[0]); 528 | decaduino.encodeUint16(label2, &txData[2]); 529 | txData[4] = sqn_ack; 530 | txData[5] = RANGING_MSG; 531 | txData[6] = SDSTWR_PROTOCOL; 532 | txData[7] = SDSTWR_MSG_TYPE_ACK; 533 | nbData = 8; 534 | tx = TX_ON; 535 | state_sdstwr = SDSTWR_ENGINE_STATE_MEMORISE_T5; 536 | break; 537 | 538 | case SDSTWR_ENGINE_STATE_MEMORISE_T5: 539 | if (( txEnd == TX_SUCCEEDED )) { 540 | if ( rangingDebug ) { 541 | Serial.println("ACK SENT AND MEMO T5"); 542 | } 543 | t5 = decaduino.lastTxTimestamp; 544 | timeout = millis() + TIMEOUT; 545 | rx = RX_ON; 546 | state_sdstwr = SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY; 547 | } 548 | break; 549 | 550 | case SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY: 551 | if ( millis() > timeout ) { 552 | // state_sdstwr = SDSTWR_ENGINE_STATE_SEND_START; 553 | state_sdstwr = SDSTWR_ENGINE_STATE_IDLE; // abort ranging if DATA_REPLY has not been received 554 | } else { 555 | if (( rx == RX_DONE ) && ( msg_type == SDSTWR_MSG_TYPE_DATA_REPLY )) { 556 | if ( rangingDebug ) { 557 | Serial.println("DATA REPLY RECEIVED"); 558 | } 559 | sqn_ack = rxData[4] + 1; 560 | state_sdstwr = SDSTWR_ENGINE_STATE_EXTRACT_T2_T3_T6; 561 | } 562 | } 563 | break; 564 | 565 | case SDSTWR_ENGINE_STATE_EXTRACT_T2_T3_T6: 566 | if ( rangingDebug ) { 567 | Serial.println("EXTRACT T2 T3 T6"); 568 | } 569 | t2 = decaduino.decodeUint64(&rxData[8]); 570 | t3 = decaduino.decodeUint64(&rxData[16]); 571 | t6 = decaduino.decodeUint64(&rxData[24]); 572 | tof = ((((( t4 - t1 ) & mask ) - (( t3 - t2 ) & mask )) & mask ) + (((( t6 - t3 ) & mask ) - (( t5 - t4 ) & mask )) & mask ))/4; 573 | Serial.print("ToF="); 574 | Serial.print(tof, HEX); 575 | Serial.print(" d="); 576 | Serial.print(tof * COEFF); 577 | Serial.println(); 578 | if ( tof * COEFF < 0 ) { 579 | Serial.println("ERROR NEGATIVE VALUE FOR DISTANCE"); 580 | } 581 | state_sdstwr = SDSTWR_ENGINE_STATE_IDLE; 582 | break; 583 | 584 | 585 | case SDSTWR_ENGINE_STATE_INIT_SERVER: 586 | if ( rangingDebug ) { 587 | Serial.println("SDSTWR SERVER"); 588 | } 589 | rx = RX_ON; 590 | state_sdstwr = SDSTWR_ENGINE_STATE_WAIT_START; 591 | break; 592 | 593 | case SDSTWR_ENGINE_STATE_WAIT_START: 594 | if (( rx == RX_DONE ) && ( msg_type == SDSTWR_MSG_TYPE_START )) { 595 | if ( rangingDebug ) { 596 | Serial.println("START RECEIVED & MEMO T2"); 597 | } 598 | sqn_ack = rxData[4] + 1; 599 | t2 = decaduino.lastRxTimestamp; 600 | state_sdstwr = SDSTWR_ENGINE_STATE_SEND_ACK_SERVER; 601 | } 602 | break; 603 | 604 | case SDSTWR_ENGINE_STATE_SEND_ACK_SERVER: 605 | if ( rangingDebug ) { 606 | Serial.println("SEND ACK TO CLIENT"); 607 | } 608 | decaduino.encodeUint16(label, &txData[0]); 609 | decaduino.encodeUint16(label2, &txData[2]); 610 | txData[4] = sqn_ack; 611 | txData[5] = RANGING_MSG; 612 | txData[6] = SDSTWR_PROTOCOL; 613 | txData[7] = SDSTWR_MSG_TYPE_ACK; 614 | nbData = 8; 615 | tx = TX_ON; 616 | state_sdstwr = SDSTWR_ENGINE_STATE_MEMORISE_T3; 617 | break; 618 | 619 | case SDSTWR_ENGINE_STATE_MEMORISE_T3: 620 | if ( txEnd == TX_SUCCEEDED ) { 621 | if ( rangingDebug ) { 622 | Serial.println("ACK SENT AND MEMO T3"); 623 | } 624 | t3 = decaduino.lastTxTimestamp; 625 | timeout = millis() + TIMEOUT; 626 | rx = RX_ON; 627 | state_sdstwr = TWR_ENGINE_STATE_WAIT_ACK_SERVER; 628 | } 629 | break; 630 | 631 | case TWR_ENGINE_STATE_WAIT_ACK_SERVER: 632 | if ( millis() > timeout ) { 633 | state_sdstwr = SDSTWR_ENGINE_STATE_INIT_SERVER; 634 | } else { 635 | if (( rx == RX_DONE ) && ( msg_type == SDSTWR_MSG_TYPE_ACK )) { 636 | if ( rangingDebug ) { 637 | Serial.println("ACK RECEIVED & MEMO T6"); 638 | } 639 | t6 = decaduino.lastRxTimestamp; 640 | state_sdstwr = SDSTWR_ENGINE_STATE_SEND_DATA_REPLY; 641 | } 642 | } 643 | break; 644 | 645 | case SDSTWR_ENGINE_STATE_SEND_DATA_REPLY: 646 | if ( rangingDebug ) { 647 | Serial.println("SEND DATA REPLY"); 648 | } 649 | decaduino.encodeUint16(label, &txData[0]); 650 | decaduino.encodeUint16(label2, &txData[2]); 651 | txData[4] = sqn; 652 | txData[5] = RANGING_MSG; 653 | txData[6] = SDSTWR_PROTOCOL; 654 | txData[7] = SDSTWR_MSG_TYPE_DATA_REPLY; 655 | decaduino.encodeUint64(t2, &txData[8]); 656 | decaduino.encodeUint64(t3, &txData[16]); 657 | decaduino.encodeUint64(t6, &txData[24]); 658 | nbData = 32; 659 | tx = TX_ON; 660 | state_sdstwr = SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY_SENT; 661 | break; 662 | 663 | case SDSTWR_ENGINE_STATE_WAIT_DATA_REPLY_SENT: 664 | if (( txEnd == TX_SUCCEEDED )) { 665 | if ( rangingDebug ) { 666 | Serial.println("DATA REPLY SENT"); 667 | } 668 | timeout = millis() + TIMEOUT; 669 | sqn++; 670 | state_sdstwr = SDSTWR_ENGINE_STATE_IDLE; 671 | } 672 | break; 673 | 674 | default: 675 | state_sdstwr = SDSTWR_ENGINE_STATE_IDLE; 676 | break; 677 | } 678 | } 679 | 680 | 681 | void rangingRequest(int index) { 682 | if ( neighb_table[index].Status%2 == 1 ) { 683 | if ( protocol == TWR_PROTOCOL ) { 684 | if (( neighb_table[index].protocols_available%2 == 1 ) && ( node_protocols_available%2 == 1 )) { 685 | Serial.println("TWR RANGING REQUEST"); 686 | rangingReq = true; 687 | state_ranging = RANGING_ENGINE_STATE_SEND_RANGING_REQUEST; 688 | } else { 689 | Serial.println("PROTOCOL IS NOT AVAILABLE"); 690 | } 691 | } else if ( protocol == SDSTWR_PROTOCOL ) { 692 | if ((( neighb_table[index].protocols_available%4 == 2 ) || ( neighb_table[index].protocols_available%4 == 3 )) && (( node_protocols_available%4 == 2 ) || ( node_protocols_available%4 == 3 ))) { 693 | Serial.println("SDSTWR RANGING REQUEST"); 694 | rangingReq = true; 695 | state_ranging = RANGING_ENGINE_STATE_SEND_RANGING_REQUEST; 696 | } else { 697 | Serial.println("PROTOCOL IS NOT AVAILABLE"); 698 | } 699 | } 700 | } else { 701 | Serial.println("NODE IS NOT AVAILABLE"); 702 | } 703 | } 704 | 705 | 706 | void rangingResponse() { 707 | 708 | if ( rangingDebug ) { 709 | Serial.println("RANGING RESPONSE"); 710 | } 711 | label2 = decaduino.decodeUint16(src); 712 | if ( protocol == TWR_PROTOCOL ) { 713 | state_twr = TWR_ENGINE_STATE_INIT_SERVER; 714 | } else if ( protocol == SDSTWR_PROTOCOL ) { 715 | state_sdstwr = TWR_ENGINE_STATE_INIT_SERVER; 716 | } 717 | ranging = true; 718 | } 719 | 720 | 721 | void sendNeighbPosition() { 722 | 723 | decaduino.encodeUint16(label, &txData[0]); 724 | decaduino.encodeUint16(broadcast, &txData[2]); 725 | txData[4] = sqn; 726 | txData[5] = NEIGHB_POS; 727 | txData[6] = node_status; 728 | if (( node_status%8 == 4 ) || ( node_status%8 == 5 )) { 729 | // 3D Position 730 | decaduino.encodeUint32(node_x, &txData[7]); 731 | decaduino.encodeUint32(node_y, &txData[11]); 732 | decaduino.encodeUint32(node_z, &txData[15]); 733 | if (( node_status%32 >= 8 ) && ( node_status%32 <= 23 )) { 734 | // eAR announced 735 | decaduino.encodeUint32(node_eAR, &txData[19]); 736 | txData[23] = node_protocols_available; 737 | nbData = 24; 738 | } else { 739 | txData[19] = node_protocols_available; 740 | nbData = 20; 741 | } 742 | } else if (( node_status%8 == 2 ) || ( node_status%8 == 3 )) { 743 | // 2D Position 744 | decaduino.encodeUint32(node_x, &txData[7]); 745 | decaduino.encodeUint32(node_y, &txData[11]); 746 | if (( node_status%32 >= 8 ) && ( node_status%32 <= 23 )) { 747 | // eAR announced 748 | decaduino.encodeUint32(node_eAR, &txData[15]); 749 | txData[19] = node_protocols_available; 750 | nbData = 20; 751 | } else { 752 | txData[15] = node_protocols_available; 753 | nbData = 16; 754 | } 755 | } else { 756 | // No position announced 757 | if (( node_status%32 >= 8 ) && ( node_status%32 <= 23 )) { 758 | // eAR announced 759 | decaduino.encodeUint32(node_eAR, &txData[7]); 760 | txData[11] = node_protocols_available; 761 | nbData = 12; 762 | } else { 763 | txData[7] = node_protocols_available; 764 | nbData = 8; 765 | } 766 | } 767 | if ( rangingDebug ) { 768 | Serial.println("SEND NEIGHBOUR POSITION"); 769 | Serial.printf("Source : 0x%04X", label); 770 | Serial.println(); 771 | Serial.printf("Destination : 0x%04X", broadcast); 772 | Serial.println(); 773 | Serial.print("sqn : "); 774 | Serial.println(sqn); 775 | Serial.print("Node status : 0b"); 776 | Serial.println(node_status, BIN); 777 | if (( node_status%8 == 4 ) || ( node_status%8 == 5 )) { 778 | Serial.print("X : "); 779 | Serial.println(node_x); 780 | Serial.print("Y : "); 781 | Serial.println(node_y); 782 | Serial.print("Z : "); 783 | Serial.println(node_z); 784 | } else if (( node_status%8 == 2 ) || ( node_status%8 == 3 )) { 785 | Serial.print("X : "); 786 | Serial.println(node_x); 787 | Serial.print("Y : "); 788 | Serial.println(node_y); 789 | } 790 | if (( node_status%32 >= 8 ) && ( node_status%32 <= 23 )) { 791 | Serial.print("eAR value : "); 792 | Serial.println(node_eAR); 793 | } 794 | Serial.print("Available protocols : 0b"); 795 | Serial.println(node_protocols_available, BIN); 796 | Serial.println(); 797 | } 798 | tx = TX_ON; 799 | } 800 | 801 | 802 | void initNeighbTable() { 803 | 804 | int i; 805 | for (i = 0; i 3); 840 | } 841 | 842 | 843 | //return index of the last node in table 844 | int indexLastNode() { 845 | 846 | int i; 847 | for (i = 0; i= 8 ) && ( neighb_table[index].Status%32 <= 23 )) { 866 | // eAR announced 867 | neighb_table[index].eAR = decaduino.decodeUint32(&rxData[19]); 868 | neighb_table[index].protocols_available = rxData[23]; 869 | } else { 870 | neighb_table[index].protocols_available = rxData[19]; 871 | } 872 | } else if (( neighb_table[index].Status%8 == 2 ) || ( neighb_table[index].Status%8 == 3 )) { 873 | // 2D Position 874 | neighb_table[index].x = decaduino.decodeUint32(&rxData[7]); 875 | neighb_table[index].y = decaduino.decodeUint32(&rxData[11]); 876 | if (( neighb_table[index].Status%32 >= 8 ) && ( neighb_table[index].Status%32 <= 23 )) { 877 | // eAR announced 878 | neighb_table[index].eAR = decaduino.decodeUint32(&rxData[15]); 879 | neighb_table[index].protocols_available = rxData[19]; 880 | } else { 881 | neighb_table[index].protocols_available = rxData[15]; 882 | } 883 | } else { 884 | // No position 885 | if (( neighb_table[index].Status%32 >= 8 ) && ( neighb_table[index].Status%32 <= 23 )) { 886 | // eAR announced 887 | neighb_table[index].eAR = decaduino.decodeUint32(&rxData[7]); 888 | neighb_table[index].protocols_available = rxData[11]; 889 | } else { 890 | neighb_table[index].protocols_available = rxData[7]; 891 | } 892 | } 893 | if ( rangingDebug ) { 894 | Serial.print("NEIGHB AT INDEX "); 895 | Serial.println(index); 896 | Serial.printf("label : 0x%04X", neighb_table[index].neighb_label); 897 | Serial.println(); 898 | Serial.print("status : 0b"); 899 | Serial.println(neighb_table[index].Status, BIN); 900 | if (( neighb_table[index].Status%8 == 4 ) || ( neighb_table[index].Status%8 == 5 )) { 901 | // 3D Position 902 | Serial.print("X : "); 903 | Serial.println(neighb_table[index].x); 904 | Serial.print("Y : "); 905 | Serial.println(neighb_table[index].y); 906 | Serial.print("Z : "); 907 | Serial.println(neighb_table[index].z); 908 | } else if (( neighb_table[index].Status%8 == 2 ) || ( neighb_table[index].Status%8 == 3 )) { 909 | // 2D Position 910 | Serial.print("X : "); 911 | Serial.println(neighb_table[index].x); 912 | Serial.print("Y : "); 913 | Serial.println(neighb_table[index].y); 914 | } 915 | if (( neighb_table[index].Status%32 >= 8 ) && ( neighb_table[index].Status%32 <= 23 )) { 916 | // eAR announced 917 | Serial.print("eAR value : "); 918 | Serial.println(neighb_table[index].eAR); 919 | } 920 | Serial.print("Available protocols : 0b"); 921 | Serial.println(neighb_table[index].protocols_available, BIN); 922 | Serial.println(); 923 | } 924 | } 925 | 926 | 927 | void neighbSave() { 928 | 929 | if ( rangingDebug ) { 930 | Serial.println("SAVE NEIGHB"); 931 | } 932 | int index = nodeInNeighbTable(decaduino.decodeUint16(src)); 933 | // node in table 934 | if ( index != -1) { 935 | // update 936 | if ( rangingDebug ) { 937 | Serial.print("UPDATE "); 938 | } 939 | neighbUpdate(index); 940 | } 941 | else { 942 | // add 943 | if ( rangingDebug ) { 944 | Serial.print("ADD "); 945 | } 946 | neighbUpdate(indexLastNode()); 947 | } 948 | } 949 | 950 | 951 | bool nodePositionIsKnown() { 952 | 953 | return (( node_status%8 >= 2 ) && ( node_status%8 <= 5 )); 954 | } 955 | 956 | 957 | bool neighbPositionIsKnown(int index) { 958 | return (( neighb_table[index].Status%8 >= 2 ) && ( neighb_table[index].Status%8 <= 5 )); 959 | } 960 | 961 | 962 | float calcPosition(int index) { 963 | 964 | if ((( node_status%8 == 4 ) || ( node_status%8 == 5 )) && (( neighb_table[index].Status%8 == 4 ) || ( neighb_table[index].Status%8 == 5 ))) { 965 | // calc 3D Position 966 | if ( rangingDebug ) { 967 | Serial.println("3D distance"); 968 | } 969 | return calc3DPosition(index); 970 | } else { 971 | // calc 2D Position 972 | if ( rangingDebug ) { 973 | Serial.println("2D distance"); 974 | } 975 | return calc2DPosition(index); 976 | } 977 | } 978 | 979 | 980 | float square(float a) { 981 | return (a*a); 982 | } 983 | 984 | 985 | float calc3DPosition(int index) { 986 | return ( sqrt(square(node_x - neighb_table[index].x) + square(node_y - neighb_table[index].y) + square(node_z - neighb_table[index].z))); 987 | } 988 | 989 | 990 | float calc2DPosition(int index) { 991 | return ( sqrt(square(node_x - neighb_table[index].x) + square(node_y - neighb_table[index].y))); 992 | } 993 | 994 | 995 | void checkSerialAvailable() { 996 | 997 | if (isAvailable) { 998 | 999 | while (Serial.available() > 1) { 1000 | // if the positions of both nodes are known, only the label is needed 1001 | label2 = Serial.parseInt(); 1002 | if ( rangingDebug ) { 1003 | Serial.print("Destination label = "); 1004 | Serial.println(label2); 1005 | } 1006 | int index = nodeInNeighbTable(label2); 1007 | if ( index != -1 ) { 1008 | if ( nodePositionIsKnown() && neighbPositionIsKnown(index) ) { 1009 | float distance = calcPosition(index); 1010 | Serial.print("Calculated distance = "); 1011 | Serial.println(distance); 1012 | } else { 1013 | // else protocol number is also needed 1014 | Serial.println("Specify the protocol number : 0 for TWR and 1 for SDSTWR"); 1015 | protocol = Serial.parseInt(); 1016 | if ( rangingDebug ) { 1017 | if ( protocol == 0 ) { 1018 | Serial.println("Protocol is TWR"); 1019 | } else if ( protocol == 1 ) { 1020 | Serial.println("Protocol is SDSTWR"); 1021 | } 1022 | } 1023 | if (( protocol == TWR_PROTOCOL ) || ( protocol == SDSTWR_PROTOCOL )) { 1024 | rangingRequest(index); 1025 | } else { 1026 | Serial.println("Specified protocol number is wrong, choose between 0 for TWR and 1 for SDSTWR"); 1027 | } 1028 | } 1029 | } else { 1030 | Serial.println("NODE IS NOT IN TABLE"); 1031 | } 1032 | if ( rangingDebug ) { 1033 | Serial.println(); 1034 | } 1035 | } 1036 | } 1037 | } 1038 | 1039 | 1040 | void neighbPosition() { 1041 | 1042 | if (( !rangingReq ) && ( !ranging ) && ( millis() > neighb_timeout )) { 1043 | // NEIGHB_POSITION can't be send during a ranging phase 1044 | sendNeighbPosition(); 1045 | sqn++; 1046 | neighb_timeout = millis()+NEIGHB_POS_PERIOD*1000+random(-2000, 2000); 1047 | } 1048 | } 1049 | 1050 | 1051 | 1052 | void setup() { 1053 | 1054 | uint8_t buf[2]; 1055 | 1056 | pinMode(13, OUTPUT); 1057 | pinMode(RGB_RED_PIN, OUTPUT); 1058 | pinMode(RGB_GREEN_PIN, OUTPUT); 1059 | pinMode(RGB_BLUE_PIN, OUTPUT); 1060 | digitalWrite(RGB_RED_PIN, HIGH); 1061 | digitalWrite(RGB_GREEN_PIN, LOW); 1062 | digitalWrite(RGB_BLUE_PIN, HIGH); 1063 | 1064 | randomSeed(analogRead(A20)); 1065 | 1066 | while(Serial.available()==0); 1067 | Serial.setTimeout(86400); 1068 | Serial.println("