init

Dependencies:   aconno_I2C Lis2dh12 WatchdogTimer

Files at this revision

API Documentation at this revision

Comitter:
pathfindr
Date:
Mon Feb 17 23:24:52 2020 +0000
Parent:
57:066dfbe8b4b9
Commit message:
usb

Changed in this revision

DW1000/DW1000.cpp Show annotated file Show diff for this revision Revisions of this file
DW1000/DW1000.h Show annotated file Show diff for this revision Revisions of this file
DW1000/DW1000Utils.h Show annotated file Show diff for this revision Revisions of this file
LED.cpp Show annotated file Show diff for this revision Revisions of this file
LED.h Show annotated file Show diff for this revision Revisions of this file
Lis2dh12.lib Show annotated file Show diff for this revision Revisions of this file
MM2WayRanging.cpp Show annotated file Show diff for this revision Revisions of this file
MM2WayRanging.h Show annotated file Show diff for this revision Revisions of this file
aconno_I2C.lib Show annotated file Show diff for this revision Revisions of this file
board.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
main.h Show annotated file Show diff for this revision Revisions of this file
states.h Show diff for this revision Revisions of this file
diff -r 066dfbe8b4b9 -r 8d4a354816b1 DW1000/DW1000.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DW1000/DW1000.cpp	Mon Feb 17 23:24:52 2020 +0000
@@ -0,0 +1,446 @@
+// Adapted from Matthias Grob & Manuel Stalder - ETH Zürich - 2015
+#include "DW1000.h"
+#include "NRFuart.h"
+ 
+// Change this depending on whether damaged or heatlhy DWM1000 modules are used.
+const bool DWM1000_DAMAGED = false;
+//const bool DWM1000_DAMAGED = false;
+ 
+/*DW1000::DW1000(PinName MOSI, PinName MISO, PinName SCLK, PinName CS, PinName IRQ, PinName RESET) : irq(IRQ), spi(MOSI, MISO, SCLK), cs(CS), reset(RESET) {
+    irq.rise(this, &DW1000::ISR);
+    */
+    
+DW1000::DW1000(PinName MOSI, PinName MISO, PinName SCLK, PinName CS, PinName IRQ, PinName RESET) : irq(IRQ), spi(MOSI, MISO, SCLK), cs(CS), reset(RESET) {
+    //irq.rise(this, &DW1000::ISR);
+ 
+    //setCallbacks(NULL, NULL);
+ 
+    select();
+    deselect();                         // Chip must be deselected first
+    resetAll();                         // we do a soft reset of the DW1000 everytime the driver starts
+ 
+    // Configuration TODO: make method for that
+    // User Manual "2.5.5 Default Configurations that should be modified" p. 22
+    //Those values are for the standard mode (6.8Mbps, 5, 16Mhz, 32 Symbols) and are INCOMPLETE!
+//    writeRegister16(DW1000_AGC_CTRL, 0x04, 0x8870);
+//    writeRegister32(DW1000_AGC_CTRL, 0x0C, 0x2502A907);
+//    writeRegister32(DW1000_DRX_CONF, 0x08, 0x311A002D);
+//    writeRegister8 (DW1000_LDE_CTRL, 0x0806, 0xD);
+//    writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x1607);
+//    writeRegister32(DW1000_TX_POWER, 0, 0x0E082848);
+//    writeRegister32(DW1000_RF_CONF, 0x0C, 0x001E3FE0);
+//    writeRegister8 (DW1000_TX_CAL, 0x0B, 0xC0);
+//    writeRegister8 (DW1000_FS_CTRL, 0x0B, 0xA6);
+ 
+ 
+    //Those values are for the 110kbps mode (5, 16MHz, 1024 Symbols) and are quite complete
+    writeRegister16(DW1000_AGC_CTRL, 0x04, 0x8870);             //AGC_TUNE1 for 16MHz PRF
+    writeRegister32(DW1000_AGC_CTRL, 0x0C, 0x2502A907);         //AGC_TUNE2 (Universal)
+    writeRegister16(DW1000_AGC_CTRL, 0x12, 0x0055);             //AGC_TUNE3 (Universal)
+ 
+    writeRegister16(DW1000_DRX_CONF, 0x02, 0x000A);             //DRX_TUNE0b for 110kbps
+    writeRegister16(DW1000_DRX_CONF, 0x04, 0x0087);             //DRX_TUNE1a for 16MHz PRF
+    writeRegister16(DW1000_DRX_CONF, 0x06, 0x0064);             //DRX_TUNE1b for 110kbps & > 1024 symbols
+    writeRegister32(DW1000_DRX_CONF, 0x08, 0x351A009A);         //PAC size for 1024 symbols preamble & 16MHz PRF
+    //writeRegister32(DW1000_DRX_CONF, 0x08, 0x371A011D);               //PAC size for 2048 symbols preamble
+ 
+    writeRegister8 (DW1000_LDE_CTRL, 0x0806, 0xD);              //LDE_CFG1
+    writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x1607);           //LDE_CFG2 for 16MHz PRF
+ 
+    writeRegister32(DW1000_TX_POWER, 0, 0x28282828);            //Power for channel 5
+ 
+    writeRegister8(DW1000_RF_CONF, 0x0B, 0xD8);                 //RF_RXCTRLH for channel 5
+    writeRegister32(DW1000_RF_CONF, 0x0C, 0x001E3FE0);          //RF_TXCTRL for channel 5
+ 
+    writeRegister8 (DW1000_TX_CAL, 0x0B, 0xC0);                 //TC_PGDELAY for channel 5
+ 
+    writeRegister32 (DW1000_FS_CTRL, 0x07, 0x0800041D);         //FS_PLLCFG for channel 5
+    writeRegister8 (DW1000_FS_CTRL, 0x0B, 0xA6);                //FS_PLLTUNE for channel 5
+ 
+    loadLDE();                          // important everytime DW1000 initialises/awakes otherwise the LDE algorithm must be turned off or there's receiving malfunction see User Manual LDELOAD on p22 & p158
+    
+    // 110kbps CAUTION: a lot of other registers have to be set for an optimized operation on 110kbps
+    writeRegister16(DW1000_TX_FCTRL, 1, 0x0800 | 0x0100 | 0x0080); // use 1024 symbols preamble (0x0800) (previously 2048 - 0x2800), 16MHz pulse repetition frequency (0x0100), 110kbps bit rate (0x0080) see p.69 of DW1000 User Manual
+    writeRegister8(DW1000_SYS_CFG, 2, 0x44);    // enable special receiving option for 110kbps (disable smartTxPower)!! (0x44) see p.64 of DW1000 User Manual [DO NOT enable 1024 byte frames (0x03) becuase it generates disturbance of ranging don't know why...]
+ 
+    writeRegister16(DW1000_TX_ANTD, 0, 16384); // set TX and RX Antenna delay to neutral because we calibrate afterwards
+    writeRegister16(DW1000_LDE_CTRL, 0x1804, 16384); // = 2^14 a quarter of the range of the 16-Bit register which corresponds to zero calibration in a round trip (TX1+RX2+TX2+RX1)
+ 
+    writeRegister8(DW1000_SYS_CFG, 3, 0x20);    // enable auto reenabling receiver after error
+ 
+    //irq.enable_irq();
+}
+ 
+/*
+void DW1000::setCallbacks(void (*callbackRX)(void), void (*callbackTX)(void)) {
+    bool RX = false;
+    bool TX = false;
+    if (callbackRX) {
+        this->callbackRX.attach(callbackRX);
+        RX = true;
+    }
+    if (callbackTX) {
+        this->callbackTX.attach(callbackTX);
+        TX = true;
+    }
+    setInterrupt(RX, TX);
+}
+*/
+ 
+uint32_t DW1000::getDeviceID() {
+    uint32_t result;
+    readRegister(DW1000_DEV_ID, 0, (uint8_t*)&result, 4);
+    return result;
+}
+ 
+uint64_t DW1000::getEUI() {
+    uint64_t result;
+    readRegister(DW1000_EUI, 0, (uint8_t*)&result, 8);
+    return result;
+}
+ 
+void DW1000::setEUI(uint64_t EUI) {
+    writeRegister(DW1000_EUI, 0, (uint8_t*)&EUI, 8);
+}
+ 
+float DW1000::getVoltage() {
+    uint8_t buffer[7] = {0x80, 0x0A, 0x0F, 0x01, 0x00};             // algorithm form User Manual p57
+    writeRegister(DW1000_RF_CONF, 0x11, buffer, 2);
+    writeRegister(DW1000_RF_CONF, 0x12, &buffer[2], 1);
+    writeRegister(DW1000_TX_CAL, 0x00, &buffer[3], 1);
+    writeRegister(DW1000_TX_CAL, 0x00, &buffer[4], 1);
+    readRegister(DW1000_TX_CAL, 0x03, &buffer[5], 2);               // get the 8-Bit readings for Voltage and Temperature
+    float Voltage = buffer[5] * 0.0057 + 2.3;
+    //float Temperature = buffer[6] * 1.13 - 113.0;                 // TODO: getTemperature was always ~35 degree with better formula/calibration
+    return Voltage;
+}
+ 
+uint64_t DW1000::getStatus() {
+    return readRegister40(DW1000_SYS_STATUS, 0);
+}
+ 
+bool DW1000::hasReceivedFrame() {
+    uint64_t status = getStatus();
+    return status & 0x4000;
+}
+ 
+void DW1000::clearReceivedFlag() {
+    writeRegister16(DW1000_SYS_STATUS, 0, 0x6F00);              // clearing of receiving status bits
+}
+ 
+bool DW1000::hasTransmissionStarted() {
+    uint64_t status = getStatus();
+    return status & 0x10;
+}
+ 
+bool DW1000::hasSentPreamble() {
+    uint64_t status = getStatus();
+    return status & 0x20;
+}
+ 
+bool DW1000::hasSentPHYHeader() {
+    uint64_t status = getStatus();
+    return status & 0x40;
+}
+ 
+bool DW1000::hasSentFrame() {
+    uint64_t status = getStatus();
+    return status & 0x80;
+}
+ 
+void DW1000::clearSentFlag() {
+    writeRegister8(DW1000_SYS_STATUS, 0, 0xF8);                 // clearing of sending status bits
+}
+ 
+uint64_t DW1000::getSYSTimestamp() {
+    return readRegister40(DW1000_SYS_TIME, 0);
+}
+ 
+uint64_t DW1000::getRXTimestamp() {
+    return readRegister40(DW1000_RX_TIME, 0);
+}
+ 
+uint64_t DW1000::getTXTimestamp() {
+    return readRegister40(DW1000_TX_TIME, 0);
+}
+ 
+float DW1000::getSYSTimestampUS() {
+    return getSYSTimestamp() * TIMEUNITS_TO_US;
+}
+ 
+float DW1000::getRXTimestampUS() {
+    return getRXTimestamp() * TIMEUNITS_TO_US;
+}
+ 
+float DW1000::getTXTimestampUS() {
+    return getTXTimestamp() * TIMEUNITS_TO_US;
+}
+ 
+uint16_t DW1000::getStdNoise() {
+    return readRegister16(DW1000_RX_FQUAL, 0x00);
+}
+ 
+uint16_t DW1000::getPACC() {
+    uint32_t v = readRegister32(DW1000_RX_FINFO, 0x00);
+    v >>= 20;
+    return static_cast<uint16_t>(v);
+}
+ 
+uint16_t DW1000::getFPINDEX() {
+    return readRegister16(DW1000_RX_TIME, 0x05);
+}
+ 
+uint16_t DW1000::getFPAMPL1() {
+    return readRegister16(DW1000_RX_TIME, 0x07);
+}
+ 
+uint16_t DW1000::getFPAMPL2() {
+    return readRegister16(DW1000_RX_FQUAL, 0x02);
+}
+ 
+uint16_t DW1000::getFPAMPL3() {
+    return readRegister16(DW1000_RX_FQUAL, 0x04);
+}
+ 
+uint16_t DW1000::getCIRPWR() {
+    return readRegister16(DW1000_RX_FQUAL, 0x06);
+}
+ 
+uint8_t DW1000::getPRF()
+{
+    uint32_t prf_mask = static_cast<uint32_t>(0x1 << 19 | 0x1 << 18);
+    uint32_t prf = readRegister32(DW1000_CHAN_CTRL, 0x00);
+    prf >>= 18;
+    return static_cast<uint8_t>(prf & prf_mask);
+}
+ 
+void DW1000::sendString(char* message) {
+    sendFrame((uint8_t*)message, strlen(message)+1);
+}
+ 
+void DW1000::receiveString(char* message) {
+    readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)message, getFramelength());  // get data from buffer
+}
+ 
+void DW1000::sendFrame(uint8_t* message, uint16_t length) {
+    //if (length >= 1021) length = 1021;                            // check for maximim length a frame can have with 1024 Byte frames [not used, see constructor]
+    if (length >= 125) length = 125;                                // check for maximim length a frame can have with 127 Byte frames
+ 
+    Timer timer;
+    timer.start();
+    writeRegister(DW1000_TX_BUFFER, 0, message, length);            // fill buffer
+    
+    uint8_t backup = readRegister8(DW1000_TX_FCTRL, 1);             // put length of frame
+    length += 2;                                                    // including 2 CRC Bytes
+    length = ((backup & 0xFC) << 8) | (length & 0x03FF);
+    writeRegister16(DW1000_TX_FCTRL, 0, length);
+    
+    stopTRX();                                                      // stop receiving
+    writeRegister8(DW1000_SYS_CTRL, 0, 0x02);                       // trigger sending process by setting the TXSTRT bit
+}
+ 
+void DW1000::sendDelayedFrame(uint8_t* message, uint16_t length, uint64_t TxTimestamp) {
+    clearSentFlag();                                                // This is necessary, otherwise we pick up the transmission time of the previous send
+ 
+    if (TxTimestamp > CONST_2POWER40) {
+        TxTimestamp -= CONST_2POWER40;
+    }
+ 
+    //if (length >= 1021) length = 1021;                            // check for maximim length a frame can have with 1024 Byte frames [not used, see constructor]
+    if (length >= 125) length = 125;                                // check for maximim length a frame can have with 127 Byte frames
+    writeRegister(DW1000_TX_BUFFER, 0, message, length);            // fill buffer
+ 
+    uint8_t backup = readRegister8(DW1000_TX_FCTRL, 1);             // put length of frame
+    length += 2;                                                    // including 2 CRC Bytes
+    length = ((backup & 0xFC) << 8) | (length & 0x03FF);
+    writeRegister16(DW1000_TX_FCTRL, 0, length);
+ 
+    writeRegister40(DW1000_DX_TIME, 0, TxTimestamp);                //write the timestamp on which to send the message
+ 
+    stopTRX();                                                      // stop receiving
+    writeRegister8(DW1000_SYS_CTRL, 0, 0x02 | 0x04);                // trigger sending process by setting the TXSTRT and TXDLYS bit
+}
+ 
+void DW1000::startRX() {
+    writeRegister8(DW1000_SYS_CTRL, 0x01, 0x01);                    // start listening for preamble by setting the RXENAB bit
+    wait_us(16);                                                    // According to page 81 in the user manual (RXENAB bit)
+}
+ 
+void DW1000::stopTRX() {
+    writeRegister8(DW1000_SYS_CTRL, 0, 0x40);                       // disable tranceiver go back to idle mode by setting the TRXOFF bit
+}
+ 
+// PRIVATE Methods ------------------------------------------------------------------------------------
+void DW1000::loadLDE() {                                            // initialise LDE algorithm LDELOAD User Manual p22
+    writeRegister16(DW1000_PMSC, 0, 0x0301);                        // set clock to XTAL so OTP is reliable
+    writeRegister16(DW1000_OTP_IF, 0x06, 0x8000);                   // set LDELOAD bit in OTP
+    wait_us(150);
+    writeRegister16(DW1000_PMSC, 0, 0x0200);                        // recover to PLL clock
+}
+ 
+void DW1000::resetRX() {    
+    writeRegister8(DW1000_PMSC, 3, 0xE0);   // set RX reset
+    writeRegister8(DW1000_PMSC, 3, 0xF0);   // clear RX reset
+}
+ 
+void DW1000::hardwareReset(PinName reset_pin) {
+    DigitalInOut reset(reset_pin);
+    hardwareReset(reset);
+}
+ 
+void DW1000::hardwareReset(DigitalInOut& reset) {
+    if (reset.is_connected()) {
+        // DWM1000 RESET logic.
+        if (DWM1000_DAMAGED) {
+            /*
+            // The following code works for damaged DWM1000 modules.
+            // IMPORTANT: This will damage healthy DWM1000 modules!
+            reset.output();
+            reset = 1;
+            wait_ms(100);
+            reset = 0;
+            wait_ms(100);
+            reset = 1;
+            wait_ms(100);
+            */
+        } else {
+            // The following code works for healthy DWM1000 modules
+            reset.output();
+            reset = 0;
+            wait_ms(100);
+            reset.input();
+        }
+    }
+}
+ 
+void DW1000::softwareReset() {
+    stopTRX();
+    clearReceivedFlag();
+    clearSentFlag();
+}
+ 
+void DW1000::resetAll() {
+    hardwareReset(reset);
+ 
+    writeRegister8(DW1000_PMSC, 0, 0x01);   // set clock to XTAL
+    writeRegister8(DW1000_PMSC, 3, 0x00);   // set All reset
+    wait_us(10);                            // wait for PLL to lock
+    writeRegister8(DW1000_PMSC, 3, 0xF0);   // clear All reset
+}
+ 
+ 
+void DW1000::setInterrupt(bool RX, bool TX) {
+    writeRegister16(DW1000_SYS_MASK, 0, RX*0x4000 | TX*0x0080);  // RX good frame 0x4000, TX done 0x0080
+}
+ 
+/*
+void DW1000::ISR() {
+    uint64_t status = getStatus();
+    if (status & 0x4000) {                                          // a frame was received
+        callbackRX.call();
+        writeRegister16(DW1000_SYS_STATUS, 0, 0x6F00);              // clearing of receiving status bits
+    }
+    if (status & 0x80) {                                            // sending complete
+        callbackTX.call();
+        writeRegister8(DW1000_SYS_STATUS, 0, 0xF8);                 // clearing of sending status bits
+    }
+}
+*/
+ 
+uint16_t DW1000::getFramelength() {
+    uint16_t framelength = readRegister16(DW1000_RX_FINFO, 0);      // get framelength
+    framelength = (framelength & 0x03FF) - 2;                       // take only the right bits and subtract the 2 CRC Bytes
+    return framelength;
+}
+ 
+// SPI Interface ------------------------------------------------------------------------------------
+uint8_t DW1000::readRegister8(uint8_t reg, uint16_t subaddress) {
+    uint8_t result;
+    readRegister(reg, subaddress, &result, 1);
+    return result;
+}
+ 
+uint16_t DW1000::readRegister16(uint8_t reg, uint16_t subaddress) {
+    uint16_t result;
+    readRegister(reg, subaddress, (uint8_t*)&result, 2);
+    return result;
+}
+ 
+uint32_t DW1000::readRegister32(uint8_t reg, uint16_t subaddress) {
+    uint32_t result;
+    readRegister(reg, subaddress, (uint8_t*)&result, 4);
+    return result;
+}
+ 
+uint64_t DW1000::readRegister40(uint8_t reg, uint16_t subaddress) {
+    uint64_t result;
+    readRegister(reg, subaddress, (uint8_t*)&result, 5);
+    result &= 0xFFFFFFFFFF;                                 // only 40-Bit
+    return result;
+}
+ 
+void DW1000::writeRegister8(uint8_t reg, uint16_t subaddress, uint8_t buffer) {
+    writeRegister(reg, subaddress, &buffer, 1);
+}
+ 
+void DW1000::writeRegister16(uint8_t reg, uint16_t subaddress, uint16_t buffer) {
+    writeRegister(reg, subaddress, (uint8_t*)&buffer, 2);
+}
+ 
+void DW1000::writeRegister32(uint8_t reg, uint16_t subaddress, uint32_t buffer) {
+    writeRegister(reg, subaddress, (uint8_t*)&buffer, 4);
+}
+ 
+void DW1000::writeRegister40(uint8_t reg, uint16_t subaddress, uint64_t buffer) {
+    writeRegister(reg, subaddress, (uint8_t*)&buffer, 5);
+}
+ 
+void DW1000::readRegister(uint8_t reg, uint16_t subaddress, uint8_t *buffer, int length) {
+    setupTransaction(reg, subaddress, false);
+    for(int i=0; i<length; i++)                             // get data
+        buffer[i] = spi.write(0x00);
+    deselect();
+}
+ 
+void DW1000::writeRegister(uint8_t reg, uint16_t subaddress, uint8_t *buffer, int length) {
+    setupTransaction(reg, subaddress, true);
+    for(int i=0; i<length; i++)                             // put data
+        spi.write(buffer[i]);
+    deselect();
+}
+ 
+void DW1000::setupTransaction(uint8_t reg, uint16_t subaddress, bool write) {
+    reg |=  (write * DW1000_WRITE_FLAG);                                        // set read/write flag
+    select();
+    if (subaddress > 0) {                                                       // there's a subadress, we need to set flag and send second header byte
+        spi.write(reg | DW1000_SUBADDRESS_FLAG);
+        if (subaddress > 0x7F) {                                                // sub address too long, we need to set flag and send third header byte
+            spi.write((uint8_t)(subaddress & 0x7F) | DW1000_2_SUBADDRESS_FLAG); // and 
+            spi.write((uint8_t)(subaddress >> 7));
+        } else {
+            spi.write((uint8_t)subaddress);
+        }
+    } else {
+        spi.write(reg);                                                         // say which register address we want to access
+    }
+}
+ 
+void DW1000::select() {     // always called to start an SPI transmission
+    /*
+    if (irq != NULL) {
+        //irq->disable_irq();
+        irq.disable_irq();
+    }
+    */
+    cs = 0;                 // set Cable Select pin low to start transmission
+}
+ 
+void DW1000::deselect() {   // always called to end an SPI transmission
+    cs = 1;                 // set Cable Select pin high to stop transmission
+    /*
+    if (irq != NULL) {
+        //irq->enable_irq();
+        irq.enable_irq();
+    }
+    */
+}
\ No newline at end of file
diff -r 066dfbe8b4b9 -r 8d4a354816b1 DW1000/DW1000.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DW1000/DW1000.h	Mon Feb 17 23:24:52 2020 +0000
@@ -0,0 +1,151 @@
+// Adapted from Matthias Grob & Manuel Stalder - ETH Zürich - 2015
+ 
+#ifndef DW1000_H
+#define DW1000_H
+ 
+#include "mbed.h"
+ 
+// register addresses
+//      Mnemonic                    Address Bytes Description
+#define DW1000_DEV_ID               0x00 //     4 Device Identifier – includes device type and revision information
+#define DW1000_EUI                  0x01 //     8 Extended Unique Identifier
+#define DW1000_PANADR               0x03 //     4 PAN Identifier and Short Address
+#define DW1000_SYS_CFG              0x04 //     4 System Configuration bitmap
+#define DW1000_SYS_TIME             0x06 //     5 System Time Counter (40-bit)
+#define DW1000_TX_FCTRL             0x08 //     5 Transmit Frame Control
+#define DW1000_TX_BUFFER            0x09 //  1024 Transmit Data Buffer
+#define DW1000_DX_TIME              0x0A //     5 Delayed Send or Receive Time (40-bit)
+#define DW1000_RX_FWTO              0x0C //     2 Receive Frame Wait Timeout Period
+#define DW1000_SYS_CTRL             0x0D //     4 System Control Register
+#define DW1000_SYS_MASK             0x0E //     4 System Event Mask Register
+#define DW1000_SYS_STATUS           0x0F //     5 System Event Status Register
+#define DW1000_RX_FINFO             0x10 //     4 RX Frame Information                (in double buffer set)
+#define DW1000_RX_BUFFER            0x11 //  1024 Receive Data Buffer                 (in double buffer set)
+#define DW1000_RX_FQUAL             0x12 //     8 Rx Frame Quality information        (in double buffer set)
+#define DW1000_RX_TTCKI             0x13 //     4 Receiver Time Tracking Interval     (in double buffer set)
+#define DW1000_RX_TTCKO             0x14 //     5 Receiver Time Tracking Offset       (in double buffer set)
+#define DW1000_RX_TIME              0x15 //    14 Receive Message Time of Arrival     (in double buffer set)
+#define DW1000_TX_TIME              0x17 //    10 Transmit Message Time of Sending    (in double buffer set)
+#define DW1000_TX_ANTD              0x18 //     2 16-bit Delay from Transmit to Antenna
+#define DW1000_SYS_STATE            0x19 //     5 System State information
+#define DW1000_ACK_RESP_T           0x1A //     4 Acknowledgement Time and Response Time
+#define DW1000_RX_SNIFF             0x1D //     4 Pulsed Preamble Reception Configuration
+#define DW1000_TX_POWER             0x1E //     4 TX Power Control
+#define DW1000_CHAN_CTRL            0x1F //     4 Channel Control
+#define DW1000_USR_SFD              0x21 //    41 User-specified short/long TX/RX SFD sequences
+#define DW1000_AGC_CTRL             0x23 //    32 Automatic Gain Control configuration
+#define DW1000_EXT_SYNC             0x24 //    12 External synchronisation control.
+#define DW1000_ACC_MEM              0x25 //  4064 Read access to accumulator data
+#define DW1000_GPIO_CTRL            0x26 //    44 Peripheral register bus 1 access - GPIO control
+#define DW1000_DRX_CONF             0x27 //    44 Digital Receiver configuration
+#define DW1000_RF_CONF              0x28 //    58 Analog RF Configuration
+#define DW1000_TX_CAL               0x2A //    52 Transmitter calibration block
+#define DW1000_FS_CTRL              0x2B //    21 Frequency synthesiser control block
+#define DW1000_AON                  0x2C //    12 Always-On register set
+#define DW1000_OTP_IF               0x2D //    18 One Time Programmable Memory Interface
+#define DW1000_LDE_CTRL             0x2E //     - Leading edge detection control block
+#define DW1000_DIG_DIAG             0x2F //    41 Digital Diagnostics Interface
+#define DW1000_PMSC                 0x36 //    48 Power Management System Control Block
+ 
+#define DW1000_WRITE_FLAG           0x80 // First Bit of the address has to be 1 to indicate we want to write
+#define DW1000_SUBADDRESS_FLAG      0x40 // if we have a sub address second Bit has to be 1
+#define DW1000_2_SUBADDRESS_FLAG    0x80 // if we have a long sub adress (more than 7 Bit) we set this Bit in the first part
+ 
+ 
+ 
+//#define SYS_STATUS_ALL_RX_ERR  (SYS_STATUS_RXPHE | SYS_STATUS_RXFCE | SYS_STATUS_RXRFSL | SYS_STATUS_RXSFDTO | SYS_STATUS_AFFREJ | SYS_STATUS_LDEERR)
+                                
+ 
+class DW1000
+{
+    public:
+        const static float TIMEUNITS_TO_US = (1/(128*499.2f));  // conversion between the decawave timeunits (ca 15.65ps) to microseconds.
+        const static float US_TO_TIMEUNITS = (128*499.2f);      // conversion between microseconds to the decawave timeunits (ca 15.65ps).
+        const static uint64_t CONST_2POWER40 = 1099511627776;  // Time register in DW1000 is 40 bit so this is needed to detect overflows.
+ 
+        DW1000(PinName MOSI, PinName MISO, PinName SCLK, PinName CS, PinName IRQ, PinName RESET);              // constructor, uses SPI class             // constructor, uses SPI class
+ 
+        void setCallbacks(void (*callbackRX)(void), void (*callbackTX)(void));                  // setter for callback functions, automatically enables interrupt, if NULL is passed the coresponding interrupt gets disabled
+        template<typename T>
+        void setCallbacks(T* tptr, void (T::*mptrRX)(void), void (T::*mptrTX)(void))
+        {      // overloaded setter to treat member function pointers of objects
+            callbackRX.attach(tptr, mptrRX);                                                    // possible client code: dw.setCallbacks(this, &A::callbackRX, &A::callbackTX);
+            callbackTX.attach(tptr, mptrTX);                                                    // concept seen in line 100 of http://developer.mbed.org/users/mbed_official/code/mbed/docs/4fc01daae5a5/InterruptIn_8h_source.html
+            setInterrupt(true,true);
+        }
+ 
+        // Device API
+        uint32_t getDeviceID();                                                                 // gets the Device ID which should be 0xDECA0130 (good for testing SPI!)
+        uint64_t getEUI();                                                                      // gets 64 bit Extended Unique Identifier according to IEEE standard
+        void setEUI(uint64_t EUI);                                                              // sets 64 bit Extended Unique Identifier according to IEEE standard
+        float getVoltage();                                                                     // gets the current chip voltage measurement form the A/D converter
+        uint64_t getStatus();                                                                   // get the 40 bit device status
+        bool hasTransmissionStarted();                                                          // check if frame transmission has started
+        bool hasSentPreamble();                                                                 // check if preamble has been sent
+        bool hasSentPHYHeader();                                                                // check if PHY header has been sent
+        bool hasSentFrame();                                                                    // check if frame has been sent completely
+        bool hasReceivedFrame();
+        void clearReceivedFlag();
+        void clearSentFlag();
+        uint64_t getSYSTimestamp();
+        uint64_t getRXTimestamp();
+        uint64_t getTXTimestamp();
+        float getSYSTimestampUS();
+        float getRXTimestampUS();
+        float getTXTimestampUS();
+ 
+        uint16_t getStdNoise();
+        uint16_t getPACC();
+        uint16_t getFPINDEX();
+        uint16_t getFPAMPL1();
+        uint16_t getFPAMPL2();
+        uint16_t getFPAMPL3();
+        uint16_t getCIRPWR();
+        uint8_t getPRF();
+        
+        void sendString(char* message);                                                         // to send String with arbitrary length
+        void receiveString(char* message);                                                      // to receive char string (length of the buffer must be 1021 to be safe)
+        void sendFrame(uint8_t* message, uint16_t length);                                      // send a raw frame (length in bytes)
+        void sendDelayedFrame(uint8_t* message, uint16_t length, uint64_t TxTimestamp);
+        uint16_t getFramelength();                                                              // to get the framelength of the received frame from the PHY header
+        void startRX();                                                                         // start listening for frames
+        void stopTRX();                                                                         // disable tranceiver go back to idle mode
+ 
+        static void hardwareReset(PinName reset_pin);
+        static void hardwareReset(DigitalInOut& reset_pin);
+        void softwareReset();
+ 
+        uint8_t readRegister8(uint8_t reg, uint16_t subaddress);                                // expressive methods to read or write the number of bits written in the name
+        uint16_t readRegister16(uint8_t reg, uint16_t subaddress);
+        uint32_t readRegister32(uint8_t reg, uint16_t subaddress);
+        uint64_t readRegister40(uint8_t reg, uint16_t subaddress);
+        void writeRegister8(uint8_t reg, uint16_t subaddress, uint8_t buffer);
+        void writeRegister16(uint8_t reg, uint16_t subaddress, uint16_t buffer);
+        void writeRegister32(uint8_t reg, uint16_t subaddress, uint32_t buffer);
+        void writeRegister40(uint8_t reg, uint16_t subaddress, uint64_t buffer);
+        void readRegister(uint8_t reg, uint16_t subaddress, uint8_t *buffer, int length);       // reads the selected part of a slave register into the buffer memory
+        void writeRegister(uint8_t reg, uint16_t subaddress, uint8_t *buffer, int length);      // writes the buffer memory to the selected slave register
+ 
+    private:
+        void loadLDE();                                                                         // load the leading edge detection algorithm to RAM, [IMPORTANT because receiving malfunction may occur] see User Manual LDELOAD on p22 & p158
+        void resetRX();                                                                         // soft reset only the tranciever part of DW1000
+        void resetAll();                                                                        // soft reset the entire DW1000 (some registers stay as they were see User Manual)
+ 
+        // Interrupt
+        InterruptIn irq;
+        FunctionPointer callbackRX;                                                             // function pointer to callback which is called when successfull RX took place
+        FunctionPointer callbackTX;                                                             // function pointer to callback which is called when successfull TX took place
+        void setInterrupt(bool RX, bool TX);                                                    // set Interrupt for received a good frame (CRC ok) or transmission done
+        void ISR();                                                                             // interrupt handling method (also calls according callback methods)
+        
+        // SPI Inteface
+        SPI spi;                                                                                // SPI Bus
+        DigitalOut cs;                                                                          // Slave selector for SPI-Bus (here explicitly needed to start and end SPI transactions also usable to wake up DW1000)
+        DigitalInOut reset;
+ 
+        void setupTransaction(uint8_t reg, uint16_t subaddress, bool write);                    // sets up an SPI read or write transaction with correct register address and offset
+        void select();                                                                          // selects the only slave for a transaction
+        void deselect();   
+};
+ 
+#endif
\ No newline at end of file
diff -r 066dfbe8b4b9 -r 8d4a354816b1 DW1000/DW1000Utils.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DW1000/DW1000Utils.h	Mon Feb 17 23:24:52 2020 +0000
@@ -0,0 +1,181 @@
+#pragma once
+ 
+#include <mbed.h>
+#include "DW1000.h"
+ 
+class DW1000Utils
+{
+public:
+    // Recommended settings:
+    // Data rate 110 kbps - Preamble length 2048 or 4096.
+    // Data rate 850 kbps - Preamble length 256, 512 or 1024.
+    // Data rate 6.8 Mbps - Preamble length 64.
+ 
+    const static uint32_t PREAMBLE_LENGTH_64 = (1 << 18);
+    const static uint32_t PREAMBLE_LENGTH_128 = (1 << 18) | (1 << 20);
+    const static uint32_t PREAMBLE_LENGTH_256 = (1 << 18) | (1 << 21);
+    const static uint32_t PREAMBLE_LENGTH_512 = (1 << 18) | (1 << 20) | (1 << 21);
+    const static uint32_t PREAMBLE_LENGTH_1024 = (1 << 19);
+    const static uint32_t PREAMBLE_LENGTH_2048 = (1 << 19) | (1 << 21);
+    const static uint32_t PREAMBLE_LENGTH_4096 = (1 << 18) | (1 << 19);
+ 
+    enum PrfSetting
+    {
+        PRF_16_MHz = 1,
+        PRF_64_MHz,
+    };
+ 
+    enum DataRateSetting
+    {
+        RATE_110_kbps = 1,
+        RATE_850_kbps,
+        RATE_6_8_Mbps,
+    };
+ 
+    // Set pulse repetition frequency
+    static void setPulseRepetitionFrequency(DW1000* dw_ptr, PrfSetting prf_setting)
+    {
+        // Transmit PRF setting (see page 75 of user manual)
+        uint32_t prf_value;
+        if (prf_setting == PRF_16_MHz)
+        {
+            prf_value = (1 << 16);
+        }
+        else
+        {
+            prf_value = (1 << 17);
+        }
+        uint32_t prf_mask = (1 << 16) | (1 << 17);
+        uint32_t tx_ctrl = dw_ptr->readRegister32(DW1000_TX_FCTRL, 0x00);
+        tx_ctrl &= ~prf_mask;
+        tx_ctrl |= (prf_value & prf_mask);
+        dw_ptr->writeRegister32(DW1000_TX_FCTRL, 0x00, tx_ctrl);
+ 
+        // Receive PRF setting (see page 109 and of user manual)
+        if (prf_setting == PRF_16_MHz)
+        {
+            prf_value = (1 << 18);
+        }
+        else
+        {
+            prf_value = (1 << 19);
+        }
+        prf_mask = (1 << 18) | (1 << 19);
+        uint32_t chan_ctrl = dw_ptr->readRegister32(DW1000_CHAN_CTRL, 0x00);
+        chan_ctrl &= ~prf_mask;
+        chan_ctrl |= (prf_value & prf_mask);
+        dw_ptr->writeRegister32(DW1000_CHAN_CTRL, 0x00, chan_ctrl);
+    }
+ 
+    // Set preamble length (see page 76 of user manual)
+    static void setPreambleLength(DW1000* dw_ptr, uint32_t preamble_setting)
+    {
+        uint32_t preamble_mask = (1 << 18) | (1 << 19) | (1 << 20) | (1 << 21);
+        uint32_t tx_ctrl = dw_ptr->readRegister32(DW1000_TX_FCTRL, 0x00);
+        tx_ctrl &= ~preamble_mask;
+        tx_ctrl |= (preamble_setting & preamble_mask);
+        dw_ptr->writeRegister32(DW1000_TX_FCTRL, 0x00, tx_ctrl);
+    }
+ 
+    // Set data rate
+    static void setDataRate(DW1000* dw_ptr, DataRateSetting rate_setting)
+    {
+        // Transmit data rate (see page 73 of user manual)
+        uint32_t rate_value;
+        if (rate_setting == RATE_110_kbps)
+        {
+            rate_value = 0;
+        }
+        else if (rate_setting == RATE_850_kbps)
+        {
+            rate_value = (1 << 13);
+        }
+        else
+        {
+            rate_value = (1 << 14);
+        }
+        uint32_t rate_mask = (1 << 13) | (1 << 14);
+        uint32_t tx_ctrl = dw_ptr->readRegister32(DW1000_TX_FCTRL, 0x00);
+        tx_ctrl &= ~rate_mask;
+        tx_ctrl |= (rate_value & rate_mask);
+        dw_ptr->writeRegister32(DW1000_TX_FCTRL, 0x00, tx_ctrl);
+ 
+        // Receive data rate (see page 72 of user manual)
+        if (rate_setting == RATE_110_kbps)
+        {
+            rate_value = (1 << 18);
+            rate_value = (1 << 22);
+        }
+        else if (rate_setting == RATE_850_kbps)
+        {
+            rate_value = (1 << 18);
+        }
+        else
+        {
+            rate_value = 0;
+        }
+        rate_mask = (1 << 18) | (1 << 22);
+        uint32_t sys_cfg = dw_ptr->readRegister32(DW1000_SYS_CFG, 0x00);
+        sys_cfg &= ~rate_mask;
+        sys_cfg |= (rate_value & rate_mask);
+        dw_ptr->writeRegister32(DW1000_SYS_CFG, 0x00, sys_cfg);
+ 
+        if (rate_setting == RATE_110_kbps)
+        {
+            dw_ptr->writeRegister16(DW1000_DRX_CONF, 0x02, 0x000A);     // DRX_TUNE0b for 110 kbps
+            dw_ptr->writeRegister16(DW1000_DRX_CONF, 0x06, 0x0064);     // DRX_TUNE1b for 110 kbps & > 1024 symbols
+        }
+        else if (rate_setting == RATE_850_kbps)
+        {
+            dw_ptr->writeRegister16(DW1000_DRX_CONF, 0x02, 0x0001);     // DRX_TUNE0b for 850 kbps
+            dw_ptr->writeRegister16(DW1000_DRX_CONF, 0x06, 0x0020);     // DRX_TUNE1b for 850 kbps & 128 - 1024 symbols
+        }
+        else
+        {
+            dw_ptr->writeRegister16(DW1000_DRX_CONF, 0x02, 0x0001);     // DRX_TUNE0b for 6.8 Mbps
+            dw_ptr->writeRegister16(DW1000_DRX_CONF, 0x06, 0x0010);     // DRX_TUNE1b for 6.8 Mbps & 64 symbols
+        }
+    }
+ 
+    // Improved settings for direct path detection in non-line-of-sight environments.
+    // See DecaWave Application Note APS006.
+    static void setNLOSSettings(DW1000* dw_ptr, DataRateSetting rate_setting = RATE_850_kbps, PrfSetting prf_setting = PRF_16_MHz, uint32_t preamble_setting = PREAMBLE_LENGTH_1024)
+    {
+        setDataRate(dw_ptr, rate_setting);
+        setPulseRepetitionFrequency(dw_ptr, prf_setting);
+        setPreambleLength(dw_ptr, preamble_setting);
+ 
+        // Setting for Noise Threshold Multiplier 1
+        dw_ptr->writeRegister8(DW1000_LDE_CTRL, 0x0806, 0x07);                  // LDE_CFG1
+        // Setting for Noise Threshold Multiplier 2
+        if (prf_setting == PRF_16_MHz)
+        {
+            dw_ptr->writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x0003);           // LDE_CFG2 for 16 MHz PRF
+        }
+        else
+        {
+            dw_ptr->writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x1603);           // LDE_CFG2 for 64 MHz PRF
+        }
+    }
+ 
+    // Default settings for line-of-sight environments
+    static void setLOSSettings(DW1000* dw_ptr, DataRateSetting rate_setting = RATE_850_kbps, PrfSetting prf_setting = PRF_16_MHz, uint32_t preamble_setting = PREAMBLE_LENGTH_1024)
+    {
+        setDataRate(dw_ptr, rate_setting);
+        setPulseRepetitionFrequency(dw_ptr, prf_setting);
+        setPreambleLength(dw_ptr, preamble_setting);
+ 
+        // Setting for Noise Threshold Multiplier 1
+            dw_ptr->writeRegister8(DW1000_LDE_CTRL, 0x0806, 0x0c);              // LDE_CFG1
+//        dw_ptr->writeRegister8(DW1000_LDE_CTRL, 0x0806, 0x0d);                // LDE_CFG1
+        // Setting for Noise Threshold Multiplier 2
+        if (prf_setting == PRF_16_MHz)
+        {
+            dw_ptr->writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x1607);           // LDE_CFG2 for 16 MHz PRF
+        }
+        else
+        {
+            dw_ptr->writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x0607);           // LDE_CFG2 for 64 MHz PRF
+        }
+    }
+};
\ No newline at end of file
diff -r 066dfbe8b4b9 -r 8d4a354816b1 LED.cpp
--- a/LED.cpp	Thu Feb 13 00:57:06 2020 +0000
+++ b/LED.cpp	Mon Feb 17 23:24:52 2020 +0000
@@ -4,6 +4,7 @@
 // LED
 //------------------------------------------------------------------------------ 
 DigitalOut led1(PN_LED);
+DigitalOut led2(PN_LED_BLUE);
 
 void LED1on(long milliseconds = 0) {
     led1 = 0;
@@ -42,4 +43,42 @@
 }
 void LED1off() {
     led1 = 1;
+}
+
+
+
+
+
+void LED2on(long milliseconds = 0) {
+    led2 = 0;
+    if (milliseconds > 0) {
+        ThisThread::sleep_for(milliseconds);
+        led2 = 1;
+    }
+}
+void LED2blink(int count = 2, long milliseconds = 100) {
+    for (int i = 0; i < (count*2); i++) {
+        led2 = !led2;
+        if (milliseconds > 0) {
+            ThisThread::sleep_for(milliseconds);
+        } else { 
+            ThisThread::sleep_for(100); //default if 0 provided
+        }
+    }
+    led2 = 1;
+}
+void LED2errorCode(int pattern, int count) {
+    for (int i = 0; i < count; i++) {
+        for (int p = 0; p < pattern; p++) {
+            led2 = 0;
+            ThisThread::sleep_for(200);
+            led2 = 1;
+            ThisThread::sleep_for(300);
+        }
+        ThisThread::sleep_for(1000);
+    }
+    led2 = 1;
+}
+void LED2off() {
+    led2 = 1;
 }
\ No newline at end of file
diff -r 066dfbe8b4b9 -r 8d4a354816b1 LED.h
--- a/LED.h	Thu Feb 13 00:57:06 2020 +0000
+++ b/LED.h	Mon Feb 17 23:24:52 2020 +0000
@@ -4,10 +4,18 @@
 #include "main.h"
 
 extern DigitalOut led1;
+extern DigitalOut led2;
+
 extern void LED1on(long milliseconds);
 extern void LED1blink(int count, long milliseconds);
 extern void LED1blinkRTC(int count);
 extern void LED1errorCode(int pattern, int count);
 extern void LED1off(void);
 
+extern void LED2on(long milliseconds);
+extern void LED2blink(int count, long milliseconds);
+extern void LED2blinkRTC(int count);
+extern void LED2errorCode(int pattern, int count);
+extern void LED2off(void);
+
 #endif
\ No newline at end of file
diff -r 066dfbe8b4b9 -r 8d4a354816b1 Lis2dh12.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Lis2dh12.lib	Mon Feb 17 23:24:52 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/Pathfindr/code/Lis2dh12/#9a41168aed47
diff -r 066dfbe8b4b9 -r 8d4a354816b1 MM2WayRanging.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MM2WayRanging.cpp	Mon Feb 17 23:24:52 2020 +0000
@@ -0,0 +1,188 @@
+#include "MM2WayRanging.h"
+#include "NRFuart.h"
+
+
+MM2WayRanging::MM2WayRanging(DW1000& DW) : dw(DW) {
+    isBeacon = true;
+    overflow = false;
+    address = 0;
+    
+    LocalTimer.start();
+    dw.startRX();   
+}
+
+
+bool MM2WayRanging::waitForFrameRX(float time_before) {
+    bool frameReceived = false;
+    while(!frameReceived && (LocalTimer.read() < time_before + 0.02f)) {
+        frameReceived = dw.hasReceivedFrame();
+    }; // wait for succeeding or timeout
+    if (frameReceived) {
+        //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Frame Received\n");debug_exe();
+        callbackRX();
+        dw.clearReceivedFlag();
+    } else {
+        //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Receive Timeout\n");debug_exe();
+    }
+    return frameReceived;
+}
+
+
+bool MM2WayRanging::waitForFrameTX(float time_before) {
+    bool frameSent = false;
+    while(!frameSent && (LocalTimer.read() < time_before + 0.02f)) {
+        frameSent = dw.hasSentFrame();
+    }; // wait for succeeding or timeout
+    if (frameSent) {
+        //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Frame Sent\n");debug_exe();
+        callbackTX();
+        dw.clearSentFlag();
+    } else {
+        //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Send Timeout\n");debug_exe();
+    }
+    return frameSent;
+}
+
+ 
+
+void MM2WayRanging::callbackRX() {
+    dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&receivedFrame, dw.getFramelength());
+    
+    //MESSAGES ADDRESSED FOR ALL DETECTORS
+    if (receivedFrame.destination == 0) {  
+        switch (receivedFrame.type) {
+            case BEACON_READY:
+                anchor_to_beacon_Send(receivedFrame.source);
+                //TODO - WE COULD ALSO TIME THIS FRAME AND THEN DIVIDE BY THREE TO GET MORE ACCURACY?
+                break;
+            default : break;
+        }
+    }
+    
+    //MESSAGES ADDRESSED FOR ME
+    if (receivedFrame.destination == address) 
+        switch (receivedFrame.type) {
+            case ANCHOR_TO_BEACON_PING:
+                RxTimestamp = dw.getRXTimestamp();
+                beacon_to_anchor_response_Send(receivedFrame.source, RxTimestamp);
+                break;
+            case BEACON_TO_ANCHOR_RESPONSE:
+                rangingRxTimestamp[receivedFrame.destination] = dw.getRXTimestamp();
+                //Calulate time/distance
+                rangingTOF[receivedFrame.source] = (rangingRxTimestamp[receivedFrame.source] - rangingTxTimestamp[receivedFrame.source]); //TODO need to remove ANSWER_DELAY_TIMEUNITS from this
+                rangingDistance[receivedFrame.source] = (rangingTOF[receivedFrame.source] * 300 * TIMEUNITS_TO_US / 4); //TODO should this be divide by 2?
+                debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Distance(%d): %d\n",receivedFrame.source, rangingDistance[receivedFrame.source]);debug_exe();
+                break;
+            default : break;
+        }
+ 
+    //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Received From: %d\n",receivedFrame.destination);debug_exe();
+    dw.startRX();
+}
+
+ 
+
+void MM2WayRanging::callbackTX() {
+    //dw.readRegister(DW1000_TX_BUFFER, 0, (uint8_t*)&sentFrame, dw.getFramelength());
+    switch (rangingFrame.type) {
+        case BEACON_READY:
+            //No Need to do anything
+            break;
+        case ANCHOR_TO_BEACON_PING:
+            rangingTxTimestamp[rangingFrame.destination] = dw.getTXTimestamp();
+            break;
+        case BEACON_TO_ANCHOR_RESPONSE:
+            //No Need to do anything
+            break;
+        default: break;
+    }
+}
+
+
+
+bool MM2WayRanging::beacon_requestRanging() {
+    float time_before = LocalTimer.read();
+    beacon_ready_Send();
+    bool sendSuccess = waitForFrameTX(time_before);
+    if (sendSuccess) {
+        //WAIT FOR FIRST RANGING FROM AN ANCHOR - TIMEOUT AFTER NO RANGING FOR CERTAIN TIME
+        float lastRangingTime = LocalTimer.read();
+        while((LocalTimer.read() - lastRangingTime) < 0.02f) {
+            if (waitForFrameRX(lastRangingTime)) {
+                lastRangingTime = LocalTimer.read();
+            }
+        }
+    } else {
+        //Send Fail   
+    }
+    //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Range End - %d\n",destination);debug_exe();
+}
+
+
+
+void MM2WayRanging::anchor_standbyToRange() {
+    float time_before = LocalTimer.read();
+    waitForFrameRX(time_before);
+}
+
+
+void MM2WayRanging::beacon_ready_Send() {
+    rangingFrame.source = address;
+    rangingFrame.destination = 0;
+    rangingFrame.type = BEACON_READY;
+    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
+}
+ 
+void MM2WayRanging::anchor_to_beacon_Send(uint8_t destination) {
+    rangingFrame.source = address;
+    rangingFrame.destination = destination;
+    rangingFrame.type = ANCHOR_TO_BEACON_PING;
+    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
+}
+
+void MM2WayRanging::beacon_to_anchor_response_Send(uint8_t destination, uint64_t rxTimestamp) {
+    rangingFrame.source = address;
+    rangingFrame.destination = destination;
+    rangingFrame.type = BEACON_TO_ANCHOR_RESPONSE;
+    if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40) {
+        dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS - MMRANGING_2POWER40);
+    } else {
+        dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
+    }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+/*
+void MM2WayRanging::correctReceiverTimestamps(uint8_t source){
+    if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){
+        receiverTimestamps[source][1] += MMRANGING_2POWER40;
+        receiverTimestamps[source][2] += MMRANGING_2POWER40;
+    }
+    if(receiverTimestamps[source][1] > receiverTimestamps[source][2]){
+        receiverTimestamps[source][2] += MMRANGING_2POWER40;
+    }
+}
+ 
+void MM2WayRanging::correctSenderTimestamps(uint8_t source){
+    if (senderTimestamps[source][0] > senderTimestamps[source][1]) {
+        senderTimestamps[source][1] += MMRANGING_2POWER40;
+        senderTimestamps[source][2] += MMRANGING_2POWER40;
+        overflow = true;
+    } else if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
+        senderTimestamps[source][2] += MMRANGING_2POWER40;
+        overflow = true;
+    } else { 
+        overflow = false;
+    }
+}
+*/
\ No newline at end of file
diff -r 066dfbe8b4b9 -r 8d4a354816b1 MM2WayRanging.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MM2WayRanging.h	Mon Feb 17 23:24:52 2020 +0000
@@ -0,0 +1,68 @@
+#ifndef MM2WAYRANGING_H
+#define MM2WAYRANGING_H
+ 
+#include "mbed.h"
+#include "DW1000.h"
+
+
+#define TIMEUNITS_TO_US       (1/(128*499.2))               // conversion between the decawave timeunits (ca 15.65ps) to microseconds.
+#define US_TO_TIMEUNITS       (128*499.2)                   // conversion between microseconds to the decawave timeunits (ca 15.65ps).
+#define MMRANGING_2POWER40    1099511627776                 // decimal value of 2^40 to correct timeroverflow between timestamps
+ 
+//Predefined delay for the critical answers in the ranging algorithm
+//HAS TO BE BIGGER THAN THE PROCESSING TIME OF THE FRAME ON THE NODE
+#define ANSWER_DELAY_US             2500                                    //2500 works for 110kbps, 900 for 6.8Mbps
+#define ANSWER_DELAY_TIMEUNITS      ANSWER_DELAY_US * (128*499.2)
+
+class MM2WayRanging {
+ 
+public:
+    MM2WayRanging(DW1000& DW);
+ 
+    bool beacon_requestRanging();
+    void anchor_standbyToRange();
+ 
+    bool isBeacon;
+    uint8_t address;                // Identifies the nodes as source and destination in rangingframes
+    bool overflow;                  // TRUE if counter overflows while ranging
+ 
+private:
+    DW1000& dw;
+    Timer LocalTimer;
+ 
+    bool waitForFrameTX(float time_before);
+    bool waitForFrameRX(float time_before);
+    void callbackRX();
+    void callbackTX();
+    
+    void beacon_ready_Send();
+    void anchor_to_beacon_Send(uint8_t destination);
+    void beacon_to_anchor_response_Send(uint8_t destination, uint64_t rxTimestamp);
+ 
+    //void correctReceiverTimestamps(uint8_t source);
+    //void correctSenderTimestamps(uint8_t source);
+ 
+    enum FrameType{
+        BEACON_READY=1,
+        ANCHOR_TO_BEACON_PING,
+        BEACON_TO_ANCHOR_RESPONSE
+    };
+ 
+    //the packed attribute makes sure the types only use their respective size in memory (8 bit for uint8_t), otherwise they would always use 32 bit
+    //IT IS A GCC SPECIFIC DIRECTIVE
+    struct __attribute__((packed, aligned(1))) RangingFrame {
+        uint8_t source;
+        uint8_t destination;
+        uint8_t type;
+    };
+ 
+    RangingFrame rangingFrame;                  // buffer in class for sending a frame (not made locally because then we can recall in the interrupt what was sent)
+    RangingFrame receivedFrame;
+    
+    uint64_t RxTimestamp;
+    uint64_t rangingTxTimestamp[5];
+    uint64_t rangingRxTimestamp[5];
+    uint64_t rangingTOF[5];
+    float rangingDistance[5];
+};
+#endif
\ No newline at end of file
diff -r 066dfbe8b4b9 -r 8d4a354816b1 aconno_I2C.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/aconno_I2C.lib	Mon Feb 17 23:24:52 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/jurica238814/code/aconno_I2C/#b2f0c302ba6d
diff -r 066dfbe8b4b9 -r 8d4a354816b1 board.h
--- a/board.h	Thu Feb 13 00:57:06 2020 +0000
+++ b/board.h	Mon Feb 17 23:24:52 2020 +0000
@@ -9,11 +9,9 @@
 #define PN_LED                  P0_30   // Green
 #define PN_LED_BLUE             P0_31   // Blue
 
-#define PN_SPI_MOSI             P0_23
-#define PN_SPI_MISO             P0_19
-#define PN_SPI_CLK              P0_24
-#define PN_SPI_CS0              P0_22
-#define PN_SPI_CS1              P0_17
+#define PN_SPI_MOSI             P0_20
+#define PN_SPI_MISO             P0_18
+#define PN_SPI_CLK              P0_16
 #define PN_UART_RX              P0_11
 #define PN_UART_TX              P0_5
 #define PN_UART_CTS             P0_30
@@ -24,6 +22,13 @@
 #define PN_I2C_SDA              P0_29
 #define PN_I2C_SCL              P0_28
 
+#define DW1000_RST              P0_24
+#define DW1000_IRQ              P0_19
+#define DW1000_RDY              P0_25
+#define DW1000_CS               P0_17
+
+
+
 
 //MEMORY FOR DISK
 //#define BD_PAGE_SIZE    4096
diff -r 066dfbe8b4b9 -r 8d4a354816b1 main.cpp
--- a/main.cpp	Thu Feb 13 00:57:06 2020 +0000
+++ b/main.cpp	Mon Feb 17 23:24:52 2020 +0000
@@ -1,4 +1,4 @@
- #include "main.h"
+#include "main.h"
 // MUST USE MBED 5.10.4   2fd0c5cfbd
 
 /*
@@ -26,12 +26,16 @@
 float            RET_temperature                                    = 0.0;
 float            RET_humidity                                       = 0.0;
 bool             RET_motionState                                    = false;
+int8_t           RET_accX                                           = 0;
+int8_t           RET_accY                                           = 0;
+int8_t           RET_accZ                                           = 0;
+time_t           RET_motionLastDetected                             = 0;
+time_t           RET_InMotionSeconds                                = 0;
+time_t           RET_NoMotionSeconds                                = 0;
 bool             RET_impactState                                    = false;
 
 //STATE
 bool             RET_busy                                           = false;
-uint8_t          RET_state                                          = STATE_SETUP;
-uint8_t          RET_state_prev                                     = RET_state;
 bool             RET_asleep                                         = false;
 bool             RET_coldBoot                                       = true;
 
@@ -73,9 +77,14 @@
 time_t           EVENT_settingsScan_time                            = 0;
 
 bool             EVENT_uwbScan                                      = false;
-time_t           EVENT_uwbScan_interval                        = 60;
+time_t           EVENT_uwbScan_interval                             = 60;
 time_t           EVENT_uwbScan_time                                 = 0;
 
+bool             EVENT_resetACCInt                                  = false;
+time_t           EVENT_resetACCInt_interval                         = DEFAULT_INTERVAL_RESETACCINT;
+time_t           EVENT_resetACCInt_time                             = 0;
+
+
 //BLE
 uint16_t         RET_setting_beacon_interval_ms                     = DEFAULT_BEACON_INTERVAL_MS;
 uint16_t         RET_setting_beacon_interval_ms_active              = DEFAULT_BEACON_INTERVAL_MS;
@@ -108,6 +117,8 @@
 //GPIO
 //------------------------------------------------------------------------------ 
 InterruptIn button(PN_IN_BUTTON);
+InterruptIn accInt1(PN_ACC_INT1);
+//InterruptIn DW_irq(DW1000_IRQ);
 
 //------------------------------------------------------------------------------
 //PERIPHERALS
@@ -117,6 +128,10 @@
 LowPowerTicker RTCticker;
 LowPowerTimer LPtimer;
 
+I2C i2c(PN_I2C_SDA,PN_I2C_SCL);
+SPI spi(PN_SPI_MOSI,PN_SPI_MISO,PN_SPI_CLK);
+Lis2dh12 acc(&i2c, 0x32);
+
 //------------------------------------------------------------------------------
 //SEMAPHORES
 //------------------------------------------------------------------------------ 
@@ -200,7 +215,7 @@
 
 
 //------------------------------------------------------------------------------
-// USER BUTTON HANDLING
+// INTERUPTS
 //------------------------------------------------------------------------------ 
 void buttonPress() {
     RET_buttonPressTime = LPtimer.read_ms();
@@ -220,7 +235,10 @@
     RET_buttonReleaseTime_prev = RET_buttonReleaseTime;
 }
 
-
+void accInt1ISR() {
+    //led2 = !led2;
+    RET_motionLastDetected = RET_RTCunixtime;
+}
 
 //------------------------------------------------------------------------------
 // RTC TICKER
@@ -253,9 +271,25 @@
         if (EVENT_uwbScan_time > 0 && RET_RTCunixtime >= EVENT_uwbScan_time) {
             RET_haveEventsToRun = true; EVENT_uwbScan = true;
         }
+        if (EVENT_resetACCInt_time > 0 && RET_RTCunixtime >= EVENT_resetACCInt_time) {
+            RET_haveEventsToRun = true; EVENT_resetACCInt = true;
+        }
     }
     
-    if (RET_haveEventsToRun) {
+    //CHECK FOR MOTION OPT 
+    //Had motion in last 5 seconds?
+    if (RET_motionLastDetected > 0 && (RET_RTCunixtime-RET_motionLastDetected) < 5) {
+        EVENT_motionClear_time = (RET_RTCunixtime + EVENT_motionClear_interval);
+        if (RET_motionState == 0) {
+            RET_motionState = 1;
+            RET_BLEpacketUpdate = true;
+            RET_haveEventsToRun = true;
+        }
+    }
+    led1 = !RET_motionState;
+    
+    //WAKE UP MAIN LOOP AND RUN ANY EVENTS
+    if (RET_haveEventsToRun && RET_asleep) {
         mainthread.release();
     }
 }
@@ -430,7 +464,7 @@
 // BLE SETTINGS SCAN
 //------------------------------------------------------------------------------ 
 void settingsScan() {
-    LED1on(0);
+    //LED1on(0);
     bleStopAdvertising();
     LowPowerTimer bleScan_t;
     bleScan_t.start();
@@ -452,17 +486,62 @@
     }
     bleScan_t.stop();
     myble.gap().stopScan();
-    LED1off();
+    //LED1off();
+}
+
+//------------------------------------------------------------------------------
+// UWB SCAN
+//------------------------------------------------------------------------------ 
+struct __attribute__((packed, aligned(1))) DistancesFrame {
+        uint8_t source;
+        uint8_t destination;
+        uint8_t type;
+        float dist[4];
+    };
+    
+DW1000 dw(PN_SPI_MOSI, PN_SPI_MISO, PN_SPI_CLK, DW1000_CS, DW1000_IRQ, DW1000_RST);   // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ)
+//DW1000 dw(&spi,DW_irq,DW1000_CS,DW1000_RST);
+MM2WayRanging node(dw);
+
+void uwbScan() {
+    
+}
+
+//------------------------------------------------------------------------------
+// ACC
+//------------------------------------------------------------------------------ 
+void acc_configForMotionInt() {
+    acc.setMode(LOW_POWER);
+    acc.enableAxes(X_axis);
+    acc.enableAxes(Y_axis);
+    acc.enableAxes(Z_axis);    
+    acc.setODR(ODR_1Hz);
+    acc.setScale(_8g);
+    acc.int1Setup(0b01000000);       // IntActivity 1 driven to INT1 pad
+    //acc.setCTRL_REG2(0b11001011);  //High pass filter active
+    acc.setCTRL_REG2(0b00000000);    //High pass filter off
+    acc.int1Latch(0b00000000);       //Dont latch
+    acc.int1Threshold(7);
+    acc.int1Duration(0x00);  
+    acc.int1Config(0b01111111);     // INT2_CFG Enable XHigh, YHigh and ZHigh, triggers INT1 on any motion
+    acc.clearIntFlag(); //reset int1 
+}
+
+void acc_updateOrientation() {
+    //Divide by 16 to get range of +- 16 on each axis
+    RET_accX = (acc.readXAxis() / 16);
+    RET_accY = (acc.readYAxis() / 16);
+    RET_accZ = (acc.readZAxis() / 16);
 }
 
 //------------------------------------------------------------------------------
 // STATE ENGINE
 //------------------------------------------------------------------------------ 
 void mainStateEngine() {
-    if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "MainStateEngine");debug_exe();}
+    if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "MainStateEngine");debug_exe();}
     
     if (EVENT_buttonPress) {
-        if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_ButtonPress");debug_exe();}
+        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_ButtonPress");debug_exe();}
         EVENT_buttonClear_time = (RET_RTCunixtime + EVENT_buttonClear_interval);
         RET_setting_beacon_interval_ms_active = DEFAULT_BEACON_INTERVAL_FAST_MS;
         RET_BLEpacketUpdate = true;
@@ -470,60 +549,68 @@
     }
     
     if (EVENT_motionClear) {
-        if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_MotionClear");debug_exe();}
+        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_MotionClear");debug_exe();}
         EVENT_motionClear_time = 0;
+        EVENT_motionClear = false;
         RET_motionState = false;
         RET_impactState = false;
+        acc_updateOrientation(); //update orientation data
         RET_BLEpacketUpdate = true;
-        EVENT_motionClear = false;
     }
     
     if (EVENT_buttonClear) {
-        if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_ButtonClear");debug_exe();}
+        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_ButtonClear");debug_exe();}
         EVENT_buttonClear_time = 0;
+        EVENT_buttonClear = false;
         RET_buttonPressed = false;
         RET_buttonPressCount = 0;
         RET_setting_beacon_interval_ms_active = RET_setting_beacon_interval_ms; //reset back to normal broadcast rate
         RET_BLEpacketUpdate = true;
-        EVENT_buttonClear = false;
     }
     
     if (EVENT_battery) {
-        if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_Battery");debug_exe();}
+        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_Battery");debug_exe();}
         EVENT_battery_time = (RET_RTCunixtime + EVENT_battery_interval);
+        EVENT_battery = false;
         updateBatteryV();
         RET_BLEpacketUpdate = true;
-        EVENT_battery = false;
     }
     
     if (EVENT_temperature) {
-        if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_Temperature");debug_exe();}
+        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_Temperature");debug_exe();}
         EVENT_temperature_time = (RET_RTCunixtime + EVENT_temperature_interval);
-        RET_BLEpacketUpdate = true;
         EVENT_temperature = false;
         //get temperature TODO - only update ble if different to last
+        RET_BLEpacketUpdate = true;
     }
     
     if (EVENT_humidity) {
-        if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_Humidity");debug_exe();}
+        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_Humidity");debug_exe();}
         EVENT_humidity_time = (RET_RTCunixtime + EVENT_humidity_interval);
-        RET_BLEpacketUpdate = true;
         EVENT_humidity = false;
         //get humidity TODO - only update ble if different to last
+        RET_BLEpacketUpdate = true;
     }
     
     if (EVENT_settingsScan) {
-        if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_SettingsScan");debug_exe();}
+        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_SettingsScan");debug_exe();}
         EVENT_settingsScan_time = (RET_RTCunixtime + EVENT_settingsScan_interval);
         EVENT_settingsScan = false;
         settingsScan();
     }
     
     if (EVENT_uwbScan) {
-        if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_uwbScan");debug_exe();}
+        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_uwbScan");debug_exe();}
         EVENT_uwbScan_time = (RET_RTCunixtime + EVENT_uwbScan_interval);
         EVENT_uwbScan = false;
     }
+    
+    /*if (EVENT_resetACCInt) {
+        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_resetACCInt1");debug_exe();}
+        EVENT_resetACCInt_time = 0;
+        EVENT_resetACCInt = false;
+        acc.clearIntFlag();
+    }*/
         
     //BLE START OR UPDATE
     if (RET_bleBroadcasting == false) {
@@ -535,13 +622,15 @@
     }
     if (RET_BLEpacketUpdate == true) {
         if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "BLE Update only");debug_exe();}
-        bleUpdateAndAdvertise();
-    }    
+        bleUpdateAndAdvertise(); //this function also sets RET_BLEpacketUpdate = false
+    }
     
     //END
     RET_haveEventsToRun = false;
 }
 
+
+
 //------------------------------------------------------------------------------
 // MAIN
 //------------------------------------------------------------------------------ 
@@ -554,13 +643,32 @@
     LPtimer.start();
     button.fall(&buttonPress);
     button.rise(&buttonRelease);
+    accInt1.rise(&accInt1ISR);
     //read_app_data_from_flash(&app_data);
+    acc_configForMotionInt();
     firstRun();
+    debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer),"Ready\n");debug_exe();
     
     //MAIN LOOP
     while(true) {
         RET_asleep = false;
-        LED1on(20);
+        //LED2on(30);
+        
+        
+        while(1) {
+            debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer),"UWB SCAN START\n");debug_exe();
+            uwbScan();
+            debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer),"UWB SCAN END\n");debug_exe();
+            nrf_configureForSleep();
+            ThisThread::sleep_for(1000);
+        }
+        
+        
+        while(1) {
+            acc_updateOrientation();
+            debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer),"X:%d    Y:%d   D:%d\n",RET_accX,RET_accY,RET_accZ);debug_exe();
+            ThisThread::sleep_for(20);
+        }
         
         //STATE ENGINE
         mainStateEngine();
@@ -573,6 +681,7 @@
         
         //DEBUGGING OFF TIMER
         if(RET_debug){
+            debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer),"TIME:%d\n",RET_RTCunixtime);debug_exe();
             if (RET_RTCunixtime > RET_debug_offat) {
                 debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer),"DEBUGGING OFF\n");debug_exe();
                 RET_debug = false;
@@ -580,13 +689,13 @@
         }
                 
         //PRE-SLEEP ACTIONS
-        //if (GLOBAL_needToConfigureLis3dh) { lis3dh_configureForSleep(RET_setting_motion_g,RET_setting_impact_g); }
+        acc_configForMotionInt();
         watchdogKick();
         //SLEEP
         nrf_configureForSleep();
         NRFuart_uninit();
         RET_asleep = true;
-        LED1off();
+        //LED1off();
         mainthread.wait(DEFAULT_SLEEP_FRAME);
     }
 }
\ No newline at end of file
diff -r 066dfbe8b4b9 -r 8d4a354816b1 main.h
--- a/main.h	Thu Feb 13 00:57:06 2020 +0000
+++ b/main.h	Mon Feb 17 23:24:52 2020 +0000
@@ -24,7 +24,7 @@
 #define USE_NRF_TEMP_SENSOR                 false
 
 //DEFAULT VELUES
-#define DEFAULT_SLEEP_FRAME                 120000
+#define DEFAULT_SLEEP_FRAME                 360000 // 6 mins
 #define DEFAULT_MOTION_G                    7
 #define DEFAULT_MOTION_START_SECONDS        120
 #define DEFAULT_MOTION_STOP_SECONDS         120
@@ -41,16 +41,15 @@
 #define TENDAYSINSECONDS                    864000
 
 //DEFAULT EVENT TIMES IN SECONDS
-#define DEFAULT_INTERVAL_MOTIONCLEAR        120
+#define DEFAULT_INTERVAL_MOTIONCLEAR        60
+#define DEFAULT_INTERVAL_RESETACCINT        10
 #define DEFAULT_INTERVAL_BUTTONCLEAR        20
 #define DEFAULT_INTERVAL_BATTERY            ONEDAYINSECONDS
 #define DEFAULT_INTERVAL_TEMPERATURE        60
 #define DEFAULT_INTERVAL_HUMIDITY           120
-#define DEFAULT_INTERVAL_SETTINGSSCAN       60
-
+#define DEFAULT_INTERVAL_SETTINGSSCAN       0
 
 //DEFINES
-#define ACTIVITY_BUFFERSIZE                 100
 #define DEBUG_BUFFERSIZE                    200
 
 //------------------------------------------------------------------------------
@@ -66,13 +65,16 @@
 //------------------------------------------------------------------------------
 #include "WatchdogTimer.h"
 #include "acd_nrf52_saadc.h"
+#include "DW1000.h"
+#include "MM2WayRanging.h"
+#include "Lis2dh12.h"
+#include "Lis2dh12_regs.h"
 
 //------------------------------------------------------------------------------
 //Application headers
 //------------------------------------------------------------------------------
 #include "NRFuart.h"
 #include "app_data.h"
-#include "states.h"
 #include "LED.h"
 
 //BLE SERVICE IDS
@@ -91,10 +93,8 @@
 
 extern bool GLOBAL_accel_healthy;
 extern bool GLOBAL_requireSoftReset;
-extern bool GLOBAL_motionFlagTriggered;
 extern bool GLOBAL_debugLED;
 extern bool GLOBAL_needToConfigureLis3dh;
-extern bool GLOBAL_LEDSequenceinProgress;
 extern time_t GLOBAL_RTCunixtime;
 extern char GLOBAL_debug_buffer[DEBUG_BUFFERSIZE];
 //FUNCS
diff -r 066dfbe8b4b9 -r 8d4a354816b1 states.h
--- a/states.h	Thu Feb 13 00:57:06 2020 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,7 +0,0 @@
-#define STATE_SETUP              0
-#define STATE_NORMAL             1
-#define STATE_LAUNCH             2
-#define STATE_ALARM              4
-#define STATE_BUTTONPRESS1       81
-#define STATE_BUTTONPRESS3       82
-#define STATE_SCORCHEDEARTH      90
\ No newline at end of file