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) diff --git a/DW1000_library_pizzo00/src/DW1000.cpp b/DW1000_library_pizzo00/src/DW1000.cpp index 7478a3d..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,57 +981,69 @@ void DW1000Class::tune() /* ########################################################################### * #### Interrupt handling ################################################### * ######################################################################### */ +void IRAM_ATTR DW1000Class::handleInterrupt() { + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(interruptSemaphore, &xHigherPriorityTaskWoken); + if (xHigherPriorityTaskWoken == pdTRUE) + portYIELD_FROM_ISR(); +} -void DW1000Class::handleInterrupt() +void DW1000Class::processInterrupt(void *pvParameter) { - // read current status and handle via callbacks - readSystemEventStatusRegister(); - if (isClockProblem() /* TODO and others */ && _handleError != 0) - { - (*_handleError)(); - } - if (isTransmitDone() && _handleSent != 0) + for(;;) { - (*_handleSent)(); - clearTransmitStatus(); - } - if (isReceiveTimestampAvailable() && _handleReceiveTimestampAvailable != 0) - { - (*_handleReceiveTimestampAvailable)(); - clearReceiveTimestampAvailableStatus(); - } - if (isReceiveFailed() && _handleReceiveFailed != 0) - { - (*_handleReceiveFailed)(); - clearReceiveStatus(); - if (_permanentReceive) + xSemaphoreTake(interruptSemaphore, portMAX_DELAY); + + // 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(); + vTaskDelete(NULL); } /* ########################################################################### @@ -1783,25 +1805,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 fd3b003..ed200d4 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); @@ -472,7 +472,11 @@ class DW1000Class { static boolean _debounceClockEnabled; /* Arduino interrupt handler */ - static void handleInterrupt(); + 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/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 d7fe4f1..586278a 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; @@ -408,22 +412,23 @@ 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) { - 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; + // } } } } @@ -435,7 +440,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); @@ -475,12 +486,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]) @@ -579,9 +589,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: @@ -595,8 +606,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; @@ -613,7 +624,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); } @@ -716,16 +727,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()); @@ -845,17 +854,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() @@ -866,12 +879,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); @@ -888,8 +902,27 @@ 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 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() @@ -900,28 +933,31 @@ 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; + shuffle_array(_networkDeviceIndexes, _networkDevicesNumber); + 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); + // 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) @@ -932,7 +968,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) @@ -941,9 +977,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() @@ -961,7 +997,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]; } @@ -979,7 +1015,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++) @@ -1001,7 +1037,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) @@ -1013,10 +1049,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) @@ -1026,7 +1062,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..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; @@ -179,8 +183,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); 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(); 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 +};