From c2088e11fbc6b394542406a641c7e5141526cd49 Mon Sep 17 00:00:00 2001 From: Pizzolato Davide Date: Thu, 19 Feb 2026 10:08:59 +0100 Subject: [PATCH 1/8] Add SAVE_LOGS in m_log --- DW1000_library_pizzo00/src/m_log.cpp | 51 ++++++++++++++++++++++++++++ DW1000_library_pizzo00/src/m_log.h | 6 +++- 2 files changed, 56 insertions(+), 1 deletion(-) diff --git a/DW1000_library_pizzo00/src/m_log.cpp b/DW1000_library_pizzo00/src/m_log.cpp index f62b6d6..52ef190 100644 --- a/DW1000_library_pizzo00/src/m_log.cpp +++ b/DW1000_library_pizzo00/src/m_log.cpp @@ -6,6 +6,15 @@ namespace m_log { + #define SAVE_LOGS false + + #if SAVE_LOGS + ushort logs_idx = 0; + ushort LOGS_QTY = 100; + ushort LOGS_LEN = 251; + char** logs; + #endif + enum class LOG_LEVEL { m_VERBOSE = 0, @@ -20,6 +29,15 @@ namespace m_log void setup() { +#if SAVE_LOGS + logs = (char**)malloc(sizeof(char*) * LOGS_QTY); + for(int i = 0; i < LOGS_QTY; i++) + { + logs[i] = (char*)malloc(sizeof(char) * LOGS_LEN); + strcpy(logs[i], ""); + } +#endif + gloablLogLevels = LOG_LEVEL::m_INFO; logLevels[LOG_DW1000] = LOG_LEVEL::m_VERBOSE; @@ -31,6 +49,11 @@ namespace m_log LOG_LEVEL minLevel = logLevels.count(tag) ? logLevels[tag] : gloablLogLevels; if(level >= minLevel) { +#if SAVE_LOGS + vsnprintf (logs[logs_idx], LOGS_LEN-1, msg, args); + Serial.println(logs[logs_idx]); + logs_idx = (logs_idx + 1) % LOGS_QTY; +#else // https://arduino.stackexchange.com/a/72456 for(const char* i=msg; *i!=0; ++i) { if(*i!='%') { Serial.print(*i); continue; } @@ -45,6 +68,7 @@ namespace m_log } } Serial.println(); +#endif va_end(args); } } @@ -54,4 +78,31 @@ namespace m_log void log_inf(std::string const& tag, const char* msg, ...) { std::va_list args; va_start(args, msg); log(tag, LOG_LEVEL::m_INFO, msg, args); } void log_dbg(std::string const& tag, const char* msg, ...) { std::va_list args; va_start(args, msg); log(tag, LOG_LEVEL::m_DEBUG, msg, args); } void log_vrb(std::string const& tag, const char* msg, ...) { std::va_list args; va_start(args, msg); log(tag, LOG_LEVEL::m_VERBOSE, msg, args); } + + int getLogQty() + { +#if SAVE_LOGS + return LOGS_QTY; +#else + return 0; +#endif + } + + int getCurrentLogIdx() + { +#if SAVE_LOGS + return logs_idx; +#else + return 0; +#endif + } + + char** getLogs() + { +#if SAVE_LOGS + return logs; +#else + return null; +#endif + } } \ No newline at end of file diff --git a/DW1000_library_pizzo00/src/m_log.h b/DW1000_library_pizzo00/src/m_log.h index 6bff200..8ccd29b 100644 --- a/DW1000_library_pizzo00/src/m_log.h +++ b/DW1000_library_pizzo00/src/m_log.h @@ -11,9 +11,13 @@ namespace m_log { void setup(); + int getLogQty(); + int getCurrentLogIdx(); + char** getLogs(); + void log_err(std::string const& tag, const char* msg, ...); void log_war(std::string const& tag, const char* msg, ...); void log_inf(std::string const& tag, const char* msg, ...); void log_dbg(std::string const& tag, const char* msg, ...); void log_vrb(std::string const& tag, const char* msg, ...); -}; \ No newline at end of file +}; From c26315398b5bf5df638c0a052c377d7e33bdd94e Mon Sep 17 00:00:00 2001 From: Pizzolato Davide Date: Thu, 19 Feb 2026 10:11:01 +0100 Subject: [PATCH 2/8] Explicit DW1000Time contructors --- DW1000_library_pizzo00/src/DW1000Time.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/DW1000_library_pizzo00/src/DW1000Time.h b/DW1000_library_pizzo00/src/DW1000Time.h index 46b54fa..644f538 100644 --- a/DW1000_library_pizzo00/src/DW1000Time.h +++ b/DW1000_library_pizzo00/src/DW1000Time.h @@ -65,10 +65,10 @@ class DW1000Time { // constructor DW1000Time(); - DW1000Time(int64_t time); - DW1000Time(byte data[]); + explicit DW1000Time(int64_t time); + explicit DW1000Time(byte data[]); DW1000Time(const DW1000Time& copy); - DW1000Time(float timeUs); + explicit DW1000Time(float timeUs); DW1000Time(int32_t value, float factorUs); ~DW1000Time(); From bab0f9de1e868dba9e84e6cbf841631a74189c29 Mon Sep 17 00:00:00 2001 From: Pizzolato Davide Date: Thu, 19 Feb 2026 10:14:46 +0100 Subject: [PATCH 3/8] Fix DW1000Class::handleInterrupt It causes "Guru Meditation Error: Core 1 panic'ed (Interrupt wdt timeout on CPU1)" --- DW1000_library_pizzo00/src/DW1000.cpp | 91 +++++++++++--------- DW1000_library_pizzo00/src/DW1000.h | 1 + DW1000_library_pizzo00/src/DW1000Ranging.cpp | 2 + 3 files changed, 53 insertions(+), 41 deletions(-) diff --git a/DW1000_library_pizzo00/src/DW1000.cpp b/DW1000_library_pizzo00/src/DW1000.cpp index 7478a3d..ed22726 100644 --- a/DW1000_library_pizzo00/src/DW1000.cpp +++ b/DW1000_library_pizzo00/src/DW1000.cpp @@ -971,57 +971,66 @@ void DW1000Class::tune() /* ########################################################################### * #### Interrupt handling ################################################### * ######################################################################### */ - +volatile bool _interrupt = false; void DW1000Class::handleInterrupt() { - // read current status and handle via callbacks - readSystemEventStatusRegister(); - if (isClockProblem() /* TODO and others */ && _handleError != 0) - { - (*_handleError)(); - } - if (isTransmitDone() && _handleSent != 0) - { - (*_handleSent)(); - clearTransmitStatus(); - } - if (isReceiveTimestampAvailable() && _handleReceiveTimestampAvailable != 0) - { - (*_handleReceiveTimestampAvailable)(); - clearReceiveTimestampAvailableStatus(); - } - if (isReceiveFailed() && _handleReceiveFailed != 0) + _interrupt = true; +} + +void DW1000Class::loop() +{ + if (_interrupt) { - (*_handleReceiveFailed)(); - clearReceiveStatus(); - if (_permanentReceive) + _interrupt = false; + // read current status and handle via callbacks + readSystemEventStatusRegister(); + if (isClockProblem() /* TODO and others */ && _handleError != 0) { - newReceive(); - startReceive(); + (*_handleError)(); } - } - else if (isReceiveTimeout() && _handleReceiveTimeout != 0) - { - (*_handleReceiveTimeout)(); - clearReceiveStatus(); - if (_permanentReceive) + if (isTransmitDone() && _handleSent != 0) { - newReceive(); - startReceive(); + (*_handleSent)(); + clearTransmitStatus(); } - } - else if (isReceiveDone() && _handleReceived != 0) - { - (*_handleReceived)(); - clearReceiveStatus(); - if (_permanentReceive) + if (isReceiveTimestampAvailable() && _handleReceiveTimestampAvailable != 0) + { + (*_handleReceiveTimestampAvailable)(); + clearReceiveTimestampAvailableStatus(); + } + if (isReceiveFailed() && _handleReceiveFailed != 0) + { + (*_handleReceiveFailed)(); + clearReceiveStatus(); + if (_permanentReceive) + { + newReceive(); + startReceive(); + } + } + else if (isReceiveTimeout() && _handleReceiveTimeout != 0) { - newReceive(); - startReceive(); + (*_handleReceiveTimeout)(); + clearReceiveStatus(); + if (_permanentReceive) + { + newReceive(); + startReceive(); + } + } + else if (isReceiveDone() && _handleReceived != 0) + { + (*_handleReceived)(); + clearReceiveStatus(); + if (_permanentReceive) + { + newReceive(); + startReceive(); + } } + // clear all status that is left unhandled + clearAllStatus(); } - // clear all status that is left unhandled - clearAllStatus(); } /* ########################################################################### diff --git a/DW1000_library_pizzo00/src/DW1000.h b/DW1000_library_pizzo00/src/DW1000.h index fd3b003..e8df49f 100644 --- a/DW1000_library_pizzo00/src/DW1000.h +++ b/DW1000_library_pizzo00/src/DW1000.h @@ -473,6 +473,7 @@ class DW1000Class { /* Arduino interrupt handler */ static void handleInterrupt(); + static void loop(); /* Allow MAC frame filtering . */ // TODO auto-acknowledge diff --git a/DW1000_library_pizzo00/src/DW1000Ranging.cpp b/DW1000_library_pizzo00/src/DW1000Ranging.cpp index d7fe4f1..353325c 100644 --- a/DW1000_library_pizzo00/src/DW1000Ranging.cpp +++ b/DW1000_library_pizzo00/src/DW1000Ranging.cpp @@ -331,6 +331,8 @@ uint32_t DEBUGRangeSent; void DW1000RangingClass::loop() { + DW1000.loop(); + // we check if needed to reset! checkForReset(); uint32_t currentTime = millis(); From 6a34bf3e979ed9b5007f343fb1a744496959a03c Mon Sep 17 00:00:00 2001 From: Pizzolato Davide Date: Thu, 19 Feb 2026 10:22:34 +0100 Subject: [PATCH 4/8] Send frames of correct length (Thanks to danielkucera) --- DW1000_library_pizzo00/src/DW1000.cpp | 25 ++---- DW1000_library_pizzo00/src/DW1000.h | 2 +- DW1000_library_pizzo00/src/DW1000Device.h | 2 +- DW1000_library_pizzo00/src/DW1000Ranging.cpp | 94 +++++++++++--------- DW1000_library_pizzo00/src/DW1000Ranging.h | 4 +- 5 files changed, 64 insertions(+), 63 deletions(-) diff --git a/DW1000_library_pizzo00/src/DW1000.cpp b/DW1000_library_pizzo00/src/DW1000.cpp index ed22726..56b969c 100644 --- a/DW1000_library_pizzo00/src/DW1000.cpp +++ b/DW1000_library_pizzo00/src/DW1000.cpp @@ -1792,25 +1792,14 @@ void DW1000Class::getData(byte data[], uint16_t n) readBytes(RX_BUFFER, NO_SUB, data, n); } -void DW1000Class::getData(String &data) +bool DW1000Class::getData(byte data[], uint16_t maxLength, uint16_t &dataLength) { - uint16_t i; - uint16_t n = getDataLength(); // number of bytes w/o the two FCS ones - if (n <= 0) - { // TODO - return; - } - byte *dataBytes = (byte *)malloc(n); - getData(dataBytes, n); - // clear string - data.remove(0); - data = ""; - // append to string - for (i = 0; i < n; i++) - { - data += (char)dataBytes[i]; - } - free(dataBytes); + dataLength = getDataLength(); // number of bytes w/o the two FCS ones + if (dataLength <= 0 || maxLength < dataLength) + return false; + + getData(data, dataLength); + return true; } void DW1000Class::getTransmitTimestamp(DW1000Time &time) diff --git a/DW1000_library_pizzo00/src/DW1000.h b/DW1000_library_pizzo00/src/DW1000.h index e8df49f..bf3a8fd 100644 --- a/DW1000_library_pizzo00/src/DW1000.h +++ b/DW1000_library_pizzo00/src/DW1000.h @@ -245,7 +245,7 @@ class DW1000Class { static void setData(byte data[], uint16_t n); static void setData(const String& data); static void getData(byte data[], uint16_t n); - static void getData(String& data); + static bool getData(byte data[], uint16_t maxLength, uint16_t& dataLength); static uint16_t getDataLength(); static void getTransmitTimestamp(DW1000Time& time); static void getReceiveTimestamp(DW1000Time& time); diff --git a/DW1000_library_pizzo00/src/DW1000Device.h b/DW1000_library_pizzo00/src/DW1000Device.h index 1218a4c..74116eb 100644 --- a/DW1000_library_pizzo00/src/DW1000Device.h +++ b/DW1000_library_pizzo00/src/DW1000Device.h @@ -67,7 +67,7 @@ class DW1000Device DW1000Time timeRangeSent; DW1000Time timeRangeReceived; - bool hasSentPoolAck; + bool hasSentPollAck; DW1000Time timePollAckReceivedMinusPollSent; DW1000Time timeRangeSentMinusPollAckReceived; diff --git a/DW1000_library_pizzo00/src/DW1000Ranging.cpp b/DW1000_library_pizzo00/src/DW1000Ranging.cpp index 353325c..bfdb110 100644 --- a/DW1000_library_pizzo00/src/DW1000Ranging.cpp +++ b/DW1000_library_pizzo00/src/DW1000Ranging.cpp @@ -410,11 +410,11 @@ void DW1000RangingClass::loop() DEBUGtimePollSent = millis(); - // we save the value for all the devices ! + // TODO only for the ones contacted for (uint8_t i = 0; i < _networkDevicesNumber; i++) { _networkDevices[i].timePollSent = timePollSent; - _networkDevices[i].hasSentPoolAck = false; + _networkDevices[i].hasSentPollAck = false; } } else if (messageType == MessageType::RANGE) @@ -437,7 +437,13 @@ void DW1000RangingClass::loop() // we read the datas from the modules: // get message and parse - DW1000.getData(receivedData, LEN_DATA); + u_int16_t dataLength; + receivedData[0] = receivedData[1] = 0; + if(!DW1000.getData(receivedData, LEN_DATA, dataLength)) + { + m_log::log_dbg(LOG_DW1000_MSG, "Error received dataLength %d", dataLength); + return; + } MessageType messageType = detectMessageType(receivedData); @@ -477,12 +483,11 @@ void DW1000RangingClass::loop() bool knownByTheTag = false; - uint8_t numberDevices = receivedData[BLINK_MAC_LEN]; - for (uint8_t i = 0; i < numberDevices; i++) + for (uint8_t i = BLINK_MAC_LEN; i < dataLength; i += 2) { // we check if the tag know us byte shortAddress[2]; - memcpy(shortAddress, receivedData + BLINK_MAC_LEN + 1 + i * 2, 2); + memcpy(shortAddress, receivedData + i, 2); // we test if the short address is our address if (shortAddress[0] == _ownShortAddress[0] && shortAddress[1] == _ownShortAddress[1]) @@ -581,9 +586,10 @@ void DW1000RangingClass::loop() // we receive a POLL which is a broadcast message // we need to grab info about it - uint8_t numberDevices = receivedData[SHORT_MAC_LEN + 1]; + uint8_t freeSlots = receivedData[SHORT_MAC_LEN + 1]; + uint8_t devicesCount = (dataLength - (SHORT_MAC_LEN+2)) / pollDeviceSize; - for (uint8_t i = 0; i < numberDevices; i++) + for (uint8_t i = 0; i < devicesCount; i++) { // we need to test if this value is for us: // we grab the mac address of each devices: @@ -597,8 +603,8 @@ void DW1000RangingClass::loop() myDistantDevice->noteActivity(); // Poll is for us // we grab the replytime which is for us - uint16_t replyTime = getReplyTimeOfIndex(i); - memcpy(&replyTime, receivedData + SHORT_MAC_LEN + 2 + 2 + i * pollDeviceSize, 2); + uint16_t replyTime = getReplyTimeOfIndex(i+freeSlots); + //memcpy(&replyTime, receivedData + SHORT_MAC_LEN + 2 + 2 + i * pollDeviceSize, 2); // on POLL we (re-)start, so no protocol failure _protocolFailed = false; @@ -615,7 +621,7 @@ void DW1000RangingClass::loop() // Remove mydistantdevice, non ci conosce, oppure send ranginginit // removeNetworkDevices(myDistantDevice->getIndex()); - int randomSlot = random(0, pollAckTimeSlots - numberDevices); + int randomSlot = random(0, freeSlots); uint16_t replyTime = getReplyTimeOfIndex(randomSlot); transmitRangingInit(replyTime); } @@ -718,16 +724,14 @@ void DW1000RangingClass::loop() // we test if the short address is our address if (receivedData[6] != _ownShortAddress[0] || receivedData[5] != _ownShortAddress[1]) - { return; - } if (messageType == MessageType::POLL_ACK) { DW1000.getReceiveTimestamp(myDistantDevice->timePollAckReceived); // we note activity for our device: myDistantDevice->noteActivity(); - myDistantDevice->hasSentPoolAck = true; + myDistantDevice->hasSentPollAck = true; // Serial.println(DW1000.getReceivePower()); // Serial.println(DW1000.getFirstPathPower()); @@ -847,17 +851,21 @@ void DW1000RangingClass::transmitInit() DW1000.setDefaults(); } -void DW1000RangingClass::transmit(byte datas[]) +void DW1000RangingClass::transmit(byte datas[], uint16_t dataLength) { - DW1000.setData(datas, LEN_DATA); + if (dataLength <= 0 || LEN_DATA < dataLength) + { + m_log::log_dbg(LOG_DW1000_MSG, "Error try transmit with dataLength %d", dataLength); + return; + } + DW1000.setData(datas, dataLength); DW1000.startTransmit(); } -void DW1000RangingClass::transmit(byte datas[], DW1000Time time) +void DW1000RangingClass::transmit(byte datas[], uint16_t len, DW1000Time time) { DW1000.setDelay(time); - DW1000.setData(datas, LEN_DATA); - DW1000.startTransmit(); + transmit(datas, len); } void DW1000RangingClass::transmitBlink() @@ -868,12 +876,13 @@ void DW1000RangingClass::transmitBlink() transmitInit(); _globalMac.generateBlinkFrame(sentData, _ownShortAddress); - sentData[BLINK_MAC_LEN] = _networkDevicesNumber; - for (uint8_t i = 0; i < _networkDevicesNumber; i++) + uint8_t devicesCount = _networkDevicesNumber < devicePerBlinkTransmit ? _networkDevicesNumber : devicePerBlinkTransmit; + + for (uint8_t i = 0; i < devicesCount; i++) { - memcpy(sentData + BLINK_MAC_LEN + 1 + i * 2, _networkDevices[i].getByteShortAddress(), 2); + memcpy(sentData + BLINK_MAC_LEN + i * 2, _networkDevices[i].getByteShortAddress(), 2); } - transmit(sentData); + transmit(sentData, BLINK_MAC_LEN + devicesCount * 2); byte shortBroadcast[2] = {0xFF, 0xFF}; copyShortAddress(_lastSentToShortAddress, shortBroadcast); @@ -890,8 +899,10 @@ void DW1000RangingClass::transmitRangingInit(u_int16_t delay) copyShortAddress(_lastSentToShortAddress, shortBroadcast); - DW1000Time deltaTime = DW1000Time(delay, DW1000Time::MICROSECONDS); - transmit(sentData, deltaTime); + DW1000Time deltaTime(delay, DW1000Time::MICROSECONDS); + transmit(sentData, SHORT_MAC_LEN+1, deltaTime); +} + } void DW1000RangingClass::transmitPoll() @@ -902,14 +913,15 @@ void DW1000RangingClass::transmitPoll() _timerDelay = _rangeInterval + (uint16_t)(pollAckTimeSlots * 3 * DEFAULT_REPLY_DELAY_TIME / 1000); // TODO meglio fermare il timer forse uint8_t devicesCount = _networkDevicesNumber < devicePerPollTransmit ? _networkDevicesNumber : devicePerPollTransmit; - + byte shortBroadcast[2] = {0xFF, 0xFF}; _globalMac.generateShortMACFrame(sentData, _ownShortAddress, shortBroadcast); sentData[SHORT_MAC_LEN] = static_cast(MessageType::POLL); - // we enter the number of devices - sentData[SHORT_MAC_LEN + 1] = devicesCount; - + uint8_t freeSlots = pollAckTimeSlots - devicesCount; + + // we enter the number of free slots + sentData[SHORT_MAC_LEN + 1] = freeSlots; for (uint8_t i = 0; i < devicesCount; i++) { @@ -920,8 +932,8 @@ void DW1000RangingClass::transmitPoll() memcpy(sentData + SHORT_MAC_LEN + 2 + i * pollDeviceSize, _networkDevices[i].getByteShortAddress(), 2); // we add the replyTime - uint16_t replyTime = _networkDevices[i].getReplyTime(); - memcpy(sentData + SHORT_MAC_LEN + 2 + 2 + i * pollDeviceSize, &replyTime, 2); + // uint16_t replyTime = _networkDevices[i].getReplyTime(); + // memcpy(sentData + SHORT_MAC_LEN + 2 + 2 + i * pollDeviceSize, &replyTime, 2); _addressOfExpectedLastPollAck = _networkDevices[i].getShortAddress(); } @@ -934,7 +946,7 @@ void DW1000RangingClass::transmitPoll() copyShortAddress(_lastSentToShortAddress, shortBroadcast); - transmit(sentData); + transmit(sentData, SHORT_MAC_LEN + 2 + devicesCount * pollDeviceSize); } void DW1000RangingClass::transmitPollAck(DW1000Device *myDistantDevice, u_int16_t delay) @@ -943,9 +955,9 @@ void DW1000RangingClass::transmitPollAck(DW1000Device *myDistantDevice, u_int16_ _globalMac.generateShortMACFrame(sentData, _ownShortAddress, myDistantDevice->getByteShortAddress()); sentData[SHORT_MAC_LEN] = static_cast(MessageType::POLL_ACK); // delay the same amount as ranging tag - DW1000Time deltaTime = DW1000Time(delay, DW1000Time::MICROSECONDS); + DW1000Time deltaTime(delay, DW1000Time::MICROSECONDS); copyShortAddress(_lastSentToShortAddress, myDistantDevice->getByteShortAddress()); - transmit(sentData, deltaTime); + transmit(sentData, SHORT_MAC_LEN+1, deltaTime); } void DW1000RangingClass::transmitRange() @@ -963,7 +975,7 @@ void DW1000RangingClass::transmitRange() DW1000Device *devices[devicePerTransmit]; for (uint8_t i = 0; i < _networkDevicesNumber && devicesCount < devicePerTransmit; i++) { - if (_networkDevices[i].hasSentPoolAck) + if (_networkDevices[i].hasSentPollAck) { devices[devicesCount++] = &_networkDevices[i]; } @@ -981,7 +993,7 @@ void DW1000RangingClass::transmitRange() sentData[SHORT_MAC_LEN + 1] = devicesCount; // delay sending the message and remember expected future sent timestamp - DW1000Time deltaTime = DW1000Time(DEFAULT_REPLY_DELAY_TIME, DW1000Time::MICROSECONDS); + DW1000Time deltaTime(DEFAULT_REPLY_DELAY_TIME, DW1000Time::MICROSECONDS); DW1000Time timeRangeSent = DW1000.setDelay(deltaTime); for (uint8_t i = 0; i < devicesCount; i++) @@ -1003,7 +1015,7 @@ void DW1000RangingClass::transmitRange() copyShortAddress(_lastSentToShortAddress, shortBroadcast); - transmit(sentData); + transmit(sentData, SHORT_MAC_LEN + 2 + rangeDeviceSize * devicesCount); } void DW1000RangingClass::transmitRangeReport(DW1000Device *myDistantDevice, u_int16_t delay) @@ -1015,10 +1027,10 @@ void DW1000RangingClass::transmitRangeReport(DW1000Device *myDistantDevice, u_in float curRange = myDistantDevice->getRange(); float curRXPower = myDistantDevice->getRXPower(); // We add the Range and then the RXPower - memcpy(sentData + 1 + SHORT_MAC_LEN, &curRange, 4); - memcpy(sentData + 5 + SHORT_MAC_LEN, &curRXPower, 4); + memcpy(sentData + SHORT_MAC_LEN + 1, &curRange, 4); + memcpy(sentData + SHORT_MAC_LEN + 5, &curRXPower, 4); copyShortAddress(_lastSentToShortAddress, myDistantDevice->getByteShortAddress()); - transmit(sentData, DW1000Time(delay, DW1000Time::MICROSECONDS)); + transmit(sentData, SHORT_MAC_LEN + 9, DW1000Time(delay, DW1000Time::MICROSECONDS)); } void DW1000RangingClass::transmitRangeFailed(DW1000Device *myDistantDevice) @@ -1028,7 +1040,7 @@ void DW1000RangingClass::transmitRangeFailed(DW1000Device *myDistantDevice) sentData[SHORT_MAC_LEN] = static_cast(MessageType::RANGE_FAILED); copyShortAddress(_lastSentToShortAddress, myDistantDevice->getByteShortAddress()); - transmit(sentData); + transmit(sentData, SHORT_MAC_LEN+1); } void DW1000RangingClass::receiver() diff --git a/DW1000_library_pizzo00/src/DW1000Ranging.h b/DW1000_library_pizzo00/src/DW1000Ranging.h index 98be243..c41a66b 100644 --- a/DW1000_library_pizzo00/src/DW1000Ranging.h +++ b/DW1000_library_pizzo00/src/DW1000Ranging.h @@ -179,8 +179,8 @@ class DW1000RangingClass // ANCHOR ranging protocol static void transmitInit(); - static void transmit(byte datas[]); - static void transmit(byte datas[], DW1000Time time); + static void transmit(byte datas[], uint16_t len); + static void transmit(byte datas[], uint16_t len, DW1000Time time); static void transmitBlink(); static void transmitRangingInit(u_int16_t delay = 0); static void transmitPollAck(DW1000Device *myDistantDevice, u_int16_t delay); From b5e88d304547d1c297f4bd8183f0191190e39ee5 Mon Sep 17 00:00:00 2001 From: Pizzolato Davide Date: Thu, 19 Feb 2026 10:27:18 +0100 Subject: [PATCH 5/8] Randomly select anchors for poll --- DW1000_library_pizzo00/src/DW1000Ranging.cpp | 35 ++++++++++++++++---- DW1000_library_pizzo00/src/DW1000Ranging.h | 8 +++-- 2 files changed, 35 insertions(+), 8 deletions(-) diff --git a/DW1000_library_pizzo00/src/DW1000Ranging.cpp b/DW1000_library_pizzo00/src/DW1000Ranging.cpp index bfdb110..e117dc6 100644 --- a/DW1000_library_pizzo00/src/DW1000Ranging.cpp +++ b/DW1000_library_pizzo00/src/DW1000Ranging.cpp @@ -32,10 +32,12 @@ DW1000RangingClass DW1000Ranging; constexpr short rangeDeviceSize = 12; -constexpr short pollDeviceSize = 4; -constexpr uint8_t devicePerPollTransmit = 4; -constexpr uint8_t pollAckTimeSlots = 6; +constexpr short pollDeviceSize = 2; +constexpr uint8_t devicePerBlinkTransmit = 43; +uint8_t DW1000RangingClass::devicePerPollTransmit; +uint8_t DW1000RangingClass::pollAckTimeSlots; +uint8_t DW1000RangingClass::_networkDeviceIndexes[MAX_DEVICES]; DW1000Device DW1000RangingClass::_networkDevices[MAX_DEVICES]; byte DW1000RangingClass::_ownLongAddress[8]; byte DW1000RangingClass::_ownShortAddress[2]; @@ -76,6 +78,8 @@ void DW1000RangingClass::init(BoardType type, uint16_t shortAddress, const char void DW1000RangingClass::init(BoardType type, const uint8_t *wifiMacAddress, uint16_t shortAddress, bool high_power, const byte mode[], uint8_t myRST, uint8_t mySS, uint8_t myIRQ) { + devicePerPollTransmit = 6; + pollAckTimeSlots = 8; _networkDevicesNumber = 0; _sentAck = false; _receivedAck = false; @@ -903,6 +907,23 @@ void DW1000RangingClass::transmitRangingInit(u_int16_t delay) transmit(sentData, SHORT_MAC_LEN+1, deltaTime); } +void shuffle_array(uint8_t array[], uint8_t length) +{ + // Fisher–Yates shuffle + uint8_t tmp, i, j; + + // init array from 0 to length-1 + for (i = 0; i < length; i++) + array[i] = i; + + for (i = length-1; i > 0; i--) + { + j = rand() % (i + 1); + // swap i/j + tmp = array[i]; + array[i] = array[j]; + array[j] = tmp; + } } void DW1000RangingClass::transmitPoll() @@ -923,19 +944,21 @@ void DW1000RangingClass::transmitPoll() // we enter the number of free slots sentData[SHORT_MAC_LEN + 1] = freeSlots; + shuffle_array(_networkDeviceIndexes, devicesCount); + for (uint8_t i = 0; i < devicesCount; i++) { // each devices have a different reply delay time. - _networkDevices[i].setReplyTime(getReplyTimeOfIndex(i+freeSlots)); + _networkDevices[_networkDeviceIndexes[i]].setReplyTime(getReplyTimeOfIndex(i+freeSlots)); // we write the short address of our device: - memcpy(sentData + SHORT_MAC_LEN + 2 + i * pollDeviceSize, _networkDevices[i].getByteShortAddress(), 2); + memcpy(sentData + SHORT_MAC_LEN + 2 + i * pollDeviceSize, _networkDevices[_networkDeviceIndexes[i]].getByteShortAddress(), 2); // we add the replyTime // uint16_t replyTime = _networkDevices[i].getReplyTime(); // memcpy(sentData + SHORT_MAC_LEN + 2 + 2 + i * pollDeviceSize, &replyTime, 2); - _addressOfExpectedLastPollAck = _networkDevices[i].getShortAddress(); + _addressOfExpectedLastPollAck = _networkDevices[_networkDeviceIndexes[i]].getShortAddress(); } // if (_networkDevicesNumber > 0) diff --git a/DW1000_library_pizzo00/src/DW1000Ranging.h b/DW1000_library_pizzo00/src/DW1000Ranging.h index c41a66b..6e7c782 100644 --- a/DW1000_library_pizzo00/src/DW1000Ranging.h +++ b/DW1000_library_pizzo00/src/DW1000Ranging.h @@ -92,6 +92,11 @@ class DW1000RangingClass static void attachNewDevice(void (*handleNewDevice)(DW1000Device *)) { _handleNewDevice = handleNewDevice; }; static void attachInactiveDevice(void (*handleInactiveDevice)(DW1000Device *)) { _handleInactiveDevice = handleInactiveDevice; }; static void attachRemovedDeviceMaxReached(void (*handleRemovedDeviceMaxReached)(DW1000Device *)) { _handleRemovedDeviceMaxReached = handleRemovedDeviceMaxReached; }; + + // Millis between one range and another + static uint16_t _rangeInterval; + static uint8_t devicePerPollTransmit; + static uint8_t pollAckTimeSlots; private: // Initialization @@ -127,6 +132,7 @@ class DW1000RangingClass private: // Other devices in the network static DW1000Device _networkDevices[MAX_DEVICES]; + static uint8_t _networkDeviceIndexes[MAX_DEVICES]; static volatile uint8_t _networkDevicesNumber; static byte _ownLongAddress[8]; static byte _ownShortAddress[2]; @@ -162,8 +168,6 @@ class DW1000RangingClass static uint32_t _resetPeriod; // Timer Tick delay static uint16_t _timerDelay; - // Millis between one range and another - static uint16_t _rangeInterval; // Ranging counter (per second) static uint32_t _rangingCountPeriod; From ec396c0f0ff22800a6301f5bfe391c4e553c7c49 Mon Sep 17 00:00:00 2001 From: Pizzolato Davide Date: Thu, 19 Feb 2026 10:27:54 +0100 Subject: [PATCH 6/8] Unnecessary assignment removed --- DW1000_library_pizzo00/src/DW1000Ranging.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/DW1000_library_pizzo00/src/DW1000Ranging.cpp b/DW1000_library_pizzo00/src/DW1000Ranging.cpp index e117dc6..44e1d14 100644 --- a/DW1000_library_pizzo00/src/DW1000Ranging.cpp +++ b/DW1000_library_pizzo00/src/DW1000Ranging.cpp @@ -423,13 +423,14 @@ void DW1000RangingClass::loop() } else if (messageType == MessageType::RANGE) { - DW1000Time timeRangeSent; - DW1000.getTransmitTimestamp(timeRangeSent); - // we save the value for all the devices ! - for (uint8_t i = 0; i < _networkDevicesNumber; i++) - { - _networkDevices[i].timeRangeSent = timeRangeSent; - } + // Removed, message already sent + // DW1000Time timeRangeSent; + // DW1000.getTransmitTimestamp(timeRangeSent); + // // we save the value for all the devices ! + // for (uint8_t i = 0; i < _networkDevicesNumber; i++) + // { + // _networkDevices[i].timeRangeSent = timeRangeSent; + // } } } } From 055576845603ac1b2dc77e74f4fb01c95a134417 Mon Sep 17 00:00:00 2001 From: Pizzolato Davide Date: Thu, 19 Feb 2026 10:30:45 +0100 Subject: [PATCH 7/8] Updated README --- DW1000_library_pizzo00/README.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/DW1000_library_pizzo00/README.md b/DW1000_library_pizzo00/README.md index dfe356b..3f22867 100644 --- a/DW1000_library_pizzo00/README.md +++ b/DW1000_library_pizzo00/README.md @@ -14,18 +14,20 @@ Project state **Development:** -Currently (2024) **under development**. +Currently (2026) **under development**. **Improvements:** - Added informations about the known anchors on blink message so only the unkown will respond - After a poll an anchor that is not on the poll can respond with a ranging_init on some free time slots (to avoid waiting until a blink) -- I have increased (by reducing the transmitted data) the maximum number of anchor per tag to 6, the tag can still "know" more than 6 anchor and query only the 6 with the best signal +- I have increased (by reducing the transmitted data) the maximum number of anchor per tag to 6 - The system is working with multiple tags, the limit is the occupation of the channel so the number of tags supported depends on the update frequency - Tag would not wait for the last poll ack to arrive before sending the range anymore (so if the last anchor is offline you had to wait for it to be remove for inactivity). Now it wait for the last one or use a timeout, so the range is always sent. - Range report to the tag can be opt-out using a flag - Removed long address - Add a minimal log library instead of Serial.print +- Send frames of correct length (Thanks to danielkucera) +- Randomly select anchors for poll (if more anchors in sight than needed) **TODOs:** * Create a attachCustomPackageHandler for maintenance operation throught uwb (like changing the esp ip remotely) From 996490461ec0eefab75aeafdd75646efb69c0bc2 Mon Sep 17 00:00:00 2001 From: Pizzolato Davide Date: Mon, 23 Feb 2026 10:22:44 +0100 Subject: [PATCH 8/8] Fix DW1000Class::handleInterrupt with dedicated task (Thanks to thire66) --- DW1000_library_pizzo00/src/DW1000.cpp | 51 ++++++++++++-------- DW1000_library_pizzo00/src/DW1000.h | 7 ++- DW1000_library_pizzo00/src/DW1000Ranging.cpp | 4 +- 3 files changed, 38 insertions(+), 24 deletions(-) diff --git a/DW1000_library_pizzo00/src/DW1000.cpp b/DW1000_library_pizzo00/src/DW1000.cpp index 56b969c..093f9a8 100644 --- a/DW1000_library_pizzo00/src/DW1000.cpp +++ b/DW1000_library_pizzo00/src/DW1000.cpp @@ -166,25 +166,35 @@ void DW1000Class::reselect(uint8_t ss) digitalWrite(_ss, HIGH); } +TaskHandle_t DW1000Class::xHandleUwbInterrupt; +SemaphoreHandle_t DW1000Class::interruptSemaphore = NULL; + void DW1000Class::begin(uint8_t irq, uint8_t rst) { - // generous initial init/wake-up-idle delay - delay(5); - // Configure the IRQ pin as INPUT. Required for correct interrupt setting for ESP8266 + // Generous initial init/wake-up-idle delay + vTaskDelay(pdMS_TO_TICKS(5)); + + // Configure the IRQ pin as INPUT pinMode(irq, INPUT); - // start SPI + + // Start SPI SPI.begin(); - // #ifndef ESP8266 - // SPI.usingInterrupt(digitalPinToInterrupt(irq)); // not every board support this, e.g. ESP8266 - // #endif - // pin and basic member setup + + // Pin and basic member setup _rst = rst; _irq = irq; _deviceMode = IDLE_MODE; - // attach interrupt - // attachInterrupt(_irq, DW1000Class::handleInterrupt, CHANGE); // todo interrupt for ESP8266 - // TODO throw error if pin is not a interrupt pin - attachInterrupt(digitalPinToInterrupt(_irq), DW1000Class::handleInterrupt, RISING); // todo interrupt for ESP8266 + + // Attach interrupt for ESP32 + interruptSemaphore = xSemaphoreCreateCounting(100, 0); + attachInterrupt(digitalPinToInterrupt(_irq), DW1000Class::handleInterrupt, RISING); + vTaskDelay(pdMS_TO_TICKS(5)); + if (xHandleUwbInterrupt != NULL) + { + vTaskDelete(xHandleUwbInterrupt); + xHandleUwbInterrupt = NULL; + } + xTaskCreate(&processInterrupt, "UWB-Interrupt", 4 * 1024, NULL, 2, &xHandleUwbInterrupt); } void DW1000Class::manageLDE() @@ -971,17 +981,19 @@ void DW1000Class::tune() /* ########################################################################### * #### Interrupt handling ################################################### * ######################################################################### */ -volatile bool _interrupt = false; -void DW1000Class::handleInterrupt() -{ - _interrupt = true; +void IRAM_ATTR DW1000Class::handleInterrupt() { + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(interruptSemaphore, &xHigherPriorityTaskWoken); + if (xHigherPriorityTaskWoken == pdTRUE) + portYIELD_FROM_ISR(); } -void DW1000Class::loop() +void DW1000Class::processInterrupt(void *pvParameter) { - if (_interrupt) + for(;;) { - _interrupt = false; + xSemaphoreTake(interruptSemaphore, portMAX_DELAY); + // read current status and handle via callbacks readSystemEventStatusRegister(); if (isClockProblem() /* TODO and others */ && _handleError != 0) @@ -1031,6 +1043,7 @@ void DW1000Class::loop() // clear all status that is left unhandled clearAllStatus(); } + vTaskDelete(NULL); } /* ########################################################################### diff --git a/DW1000_library_pizzo00/src/DW1000.h b/DW1000_library_pizzo00/src/DW1000.h index bf3a8fd..ed200d4 100644 --- a/DW1000_library_pizzo00/src/DW1000.h +++ b/DW1000_library_pizzo00/src/DW1000.h @@ -472,8 +472,11 @@ class DW1000Class { static boolean _debounceClockEnabled; /* Arduino interrupt handler */ - static void handleInterrupt(); - static void loop(); + static TaskHandle_t xHandleUwbInterrupt; + static SemaphoreHandle_t interruptSemaphore; + static void processInterrupt(void *pvParameter); + + static IRAM_ATTR void handleInterrupt(); /* Allow MAC frame filtering . */ // TODO auto-acknowledge diff --git a/DW1000_library_pizzo00/src/DW1000Ranging.cpp b/DW1000_library_pizzo00/src/DW1000Ranging.cpp index 44e1d14..586278a 100644 --- a/DW1000_library_pizzo00/src/DW1000Ranging.cpp +++ b/DW1000_library_pizzo00/src/DW1000Ranging.cpp @@ -335,8 +335,6 @@ uint32_t DEBUGRangeSent; void DW1000RangingClass::loop() { - DW1000.loop(); - // we check if needed to reset! checkForReset(); uint32_t currentTime = millis(); @@ -945,7 +943,7 @@ void DW1000RangingClass::transmitPoll() // we enter the number of free slots sentData[SHORT_MAC_LEN + 1] = freeSlots; - shuffle_array(_networkDeviceIndexes, devicesCount); + shuffle_array(_networkDeviceIndexes, _networkDevicesNumber); for (uint8_t i = 0; i < devicesCount; i++) {