From 7bf4352ed1a4d1deac8c346b5fc0a6f63ae89552 Mon Sep 17 00:00:00 2001 From: Gabriel Lessard Date: Thu, 2 Dec 2021 09:15:37 -0500 Subject: [PATCH 1/4] Fix unnecessary log --- src/message-handler/src/MessageDispatcher.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/message-handler/src/MessageDispatcher.cpp b/src/message-handler/src/MessageDispatcher.cpp index 9c46fa34..6ebaf522 100644 --- a/src/message-handler/src/MessageDispatcher.cpp +++ b/src/message-handler/src/MessageDispatcher.cpp @@ -110,6 +110,10 @@ bool MessageDispatcher::forwardMessage(const MessageDTO& message, bool networkFo // Fixme: change to unicast queue once socket creation delay is fixed return m_broadcastOutputQueue.push(message); } + else if (!networkForwarding) { + m_logger.log(LogLevel::Debug, "No need to forward undestined message"); + return true; + } m_logger.log(LogLevel::Error, "Failed to forward message"); return false; } From 7900534a93de83983b95822f747de7bb4b657b06 Mon Sep 17 00:00:00 2001 From: Gabriel Lessard Date: Sat, 4 Dec 2021 16:51:27 -0500 Subject: [PATCH 2/4] Fix for udp sending --- src/network/src/esp/src/NetworkBroadcast.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/network/src/esp/src/NetworkBroadcast.cpp b/src/network/src/esp/src/NetworkBroadcast.cpp index 8535846c..df0eaf53 100644 --- a/src/network/src/esp/src/NetworkBroadcast.cpp +++ b/src/network/src/esp/src/NetworkBroadcast.cpp @@ -43,8 +43,13 @@ bool NetworkBroadcast::send(const uint8_t* data, uint16_t length) { broadcast.sin_len = sizeof(broadcast); // Use lwip_sendto with udp since it is a connection-less protocol. - return lwip_sendto(m_socket, data, length, 0, (sockaddr*)&broadcast, sizeof(broadcast)) == - length; + int ret = lwip_sendto(m_socket, data, length, 0, (sockaddr*)&broadcast, sizeof(broadcast)); + // If buffers were full, will return -1. Check for successful queueing + while (ret == -1) { + Task::delay(1); + ret = lwip_sendto(m_socket, data, length, 0, (sockaddr*)&broadcast, sizeof(broadcast)); + } + return ret == length; } bool NetworkBroadcast::receive(uint8_t* data, uint16_t length) { From 25a408c25f7dc6e57d593128c732fd0b390599eb Mon Sep 17 00:00:00 2001 From: Gabriel Lessard Date: Sat, 4 Dec 2021 16:52:34 -0500 Subject: [PATCH 3/4] Reduce quality of service and improved stability --- src/bsp/src/esp/include/SpiStm.h | 5 +-- src/bsp/src/esp/src/SpiStm.cpp | 68 +++++++++++--------------------- 2 files changed, 24 insertions(+), 49 deletions(-) diff --git a/src/bsp/src/esp/include/SpiStm.h b/src/bsp/src/esp/include/SpiStm.h index 089801d7..d4699909 100644 --- a/src/bsp/src/esp/include/SpiStm.h +++ b/src/bsp/src/esp/include/SpiStm.h @@ -23,13 +23,12 @@ class SpiStm : public ISpiStm { BaseTask m_driverTask; ILogger& m_logger; - enum class transmitState { SENDING_HEADER, SENDING_PAYLOAD, ERROR } m_txState; + enum class transmitState { SENDING_HEADER, SENDING_PAYLOAD } m_txState; enum class receiveState { RECEIVING_HEADER, PARSING_HEADER, RECEIVING_PAYLOAD, - VALIDATE_CRC, - ERROR + VALIDATE_CRC } m_rxState; spi_slave_transaction_t m_transaction; diff --git a/src/bsp/src/esp/src/SpiStm.cpp b/src/bsp/src/esp/src/SpiStm.cpp index b5fd84c0..a6b58f73 100644 --- a/src/bsp/src/esp/src/SpiStm.cpp +++ b/src/bsp/src/esp/src/SpiStm.cpp @@ -73,15 +73,14 @@ bool SpiStm::send(const uint8_t* buffer, uint16_t length) { // Wait for transmission to be over. Will be notified when ACK received or upon error m_sendingTaskHandle = xTaskGetCurrentTaskHandle(); - ulTaskNotifyTake(pdTRUE, portMAX_DELAY); - if (m_txState == transmitState::ERROR) { - m_logger.log(LogLevel::Error, "Error occurred..."); - return false; + m_hasSentPayload = false; + while(!m_hasSentPayload) { + ulTaskNotifyTake(pdTRUE, 20); } m_logger.log(LogLevel::Debug, "Payload sent!"); m_sendingTaskHandle = nullptr; - return m_crcOK; + return true; } bool SpiStm::isConnected() const { @@ -93,11 +92,11 @@ bool SpiStm::isConnected() const { void SpiStm::execute() { uint32_t txLengthBytes = 0; - uint32_t rxLengthBytes = 0; + uint32_t rxLengthBytes = StmHeader::sizeBytes; switch (m_rxState) { case receiveState::RECEIVING_HEADER: - rxLengthBytes = StmHeader::sizeBytes; + // Default state, nothing to do here break; case receiveState::PARSING_HEADER: m_inboundHeader = (StmHeader::Header*)m_inboundMessage.m_data.data(); @@ -108,7 +107,8 @@ void SpiStm::execute() { m_logger.log(LogLevel::Debug, "Bytes were: | %d | %d | %d | %d |", m_inboundMessage.m_data[0], m_inboundMessage.m_data[1], m_inboundMessage.m_data[2], m_inboundMessage.m_data[3]); - m_rxState = receiveState::ERROR; + m_inboundMessage.m_sizeBytes = 0; + m_rxState = receiveState::RECEIVING_HEADER; m_isConnected = false; break; } @@ -133,17 +133,8 @@ void SpiStm::execute() { m_logger.log(LogLevel::Debug, "Receiving payload"); m_rxState = receiveState::RECEIVING_PAYLOAD; } else { - rxLengthBytes = StmHeader::sizeBytes; m_rxState = receiveState::RECEIVING_HEADER; } - // Payload has been sent. Check crc and notify sending task - if (m_hasSentPayload) { - m_hasSentPayload = false; - m_crcOK = !m_inboundHeader->systemState.stmSystemState.failedCrc; - if (m_sendingTaskHandle != nullptr) { - xTaskNotifyGive(m_sendingTaskHandle); - } - } break; case receiveState::RECEIVING_PAYLOAD: rxLengthBytes = m_inboundHeader->txSizeBytes; @@ -170,16 +161,6 @@ void SpiStm::execute() { m_inboundMessage.m_sizeBytes = 0; m_inboundMessage.m_payloadSizeBytes = 0; m_rxState = receiveState::RECEIVING_HEADER; - rxLengthBytes = StmHeader::sizeBytes; - break; - case receiveState::ERROR: - m_logger.log(LogLevel::Debug, "Error within Spi driver STM - RX"); - m_inboundMessage.m_sizeBytes = 0; - CircularBuff_clear(&m_circularBuf); - if (m_receivingTaskHandle != nullptr) { - xTaskNotifyGive(m_receivingTaskHandle); - } - m_rxState = receiveState::RECEIVING_HEADER; break; } @@ -192,32 +173,24 @@ void SpiStm::execute() { case transmitState::SENDING_PAYLOAD: m_transaction.tx_buffer = m_outboundMessage.m_data.data(); txLengthBytes = m_outboundMessage.m_sizeBytes; - m_hasSentPayload = false; m_crcOK = false; break; - case transmitState::ERROR: - // Notify sending task that error occurred - if (m_sendingTaskHandle != nullptr) { - xTaskNotifyGive(m_sendingTaskHandle); - } - m_logger.log(LogLevel::Error, "Error within Spi driver STM - TX"); - break; } // The length field of m_transaction is in bits m_transaction.length = BYTES_TO_BITS(std::max(rxLengthBytes, txLengthBytes)); // Rx buffer should always be the inbound message buffer. m_transaction.rx_buffer = m_inboundMessage.m_data.data(); - - if (m_txState != transmitState::ERROR && m_rxState != receiveState::ERROR) { - if (m_outboundHeader.payloadSizeBytes != 0) { - notifyMaster(); - } - // This call is blocking, so the rate of the of the loop is inferred byt the rate of the - // loop of the master driver in HiveMind. The loop needs no delay and shouldn't - // have one as it will only increase latency, which could lead to instability. - spi_slave_transmit(STM_SPI, &m_transaction, portMAX_DELAY); + // Only notify master when payload needs to be sent + if (m_outboundHeader.payloadSizeBytes != 0) { + notifyMaster(); } + m_inboundMessage.m_data.fill(0); + // This call is blocking, so the rate of the of the loop is inferred byt the rate of the + // loop of the master driver in HiveMind. The loop needs no delay and shouldn't + // have one as it will only increase latency, which could lead to instability. + spi_slave_transmit(STM_SPI, &m_transaction, portMAX_DELAY); + } void SpiStm::updateOutboundHeader() { @@ -237,6 +210,8 @@ void SpiStm::notifyMaster() { void IRAM_ATTR SpiStm::transactionCallback(void* context, spi_slave_transaction_t* transaction) { auto* instance = static_cast(context); if (transaction->length != transaction->trans_len) { + instance->m_txState = transmitState::SENDING_HEADER; + instance->m_rxState = receiveState::RECEIVING_HEADER; // Transaction timed out before it could finish. return; } @@ -252,14 +227,15 @@ void IRAM_ATTR SpiStm::transactionCallback(void* context, spi_slave_transaction_ // This should never be called. The state machine should never be in any other state during // the ISR. instance->m_logger.log(LogLevel::Error, "Interrupted called on invalid state"); - instance->m_txState = transmitState::ERROR; break; } - + BaseType_t yield; if (instance->m_txState == transmitState::SENDING_PAYLOAD) { // TODO: confirm reception with ack instance->m_txState = transmitState::SENDING_HEADER; instance->m_outboundMessage.m_sizeBytes = 0; instance->m_outboundMessage.m_payloadSizeBytes = 0; instance->m_hasSentPayload = true; + vTaskNotifyGiveFromISR(instance->m_sendingTaskHandle, &yield); } + portYIELD_FROM_ISR(); } From a952bfd2698164aa24581440943151303c92c526 Mon Sep 17 00:00:00 2001 From: Gabriel Lessard Date: Sat, 4 Dec 2021 17:04:18 -0500 Subject: [PATCH 4/4] Format --- src/bsp/src/esp/src/SpiStm.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/bsp/src/esp/src/SpiStm.cpp b/src/bsp/src/esp/src/SpiStm.cpp index a6b58f73..12462887 100644 --- a/src/bsp/src/esp/src/SpiStm.cpp +++ b/src/bsp/src/esp/src/SpiStm.cpp @@ -74,7 +74,7 @@ bool SpiStm::send(const uint8_t* buffer, uint16_t length) { // Wait for transmission to be over. Will be notified when ACK received or upon error m_sendingTaskHandle = xTaskGetCurrentTaskHandle(); m_hasSentPayload = false; - while(!m_hasSentPayload) { + while (!m_hasSentPayload) { ulTaskNotifyTake(pdTRUE, 20); } m_logger.log(LogLevel::Debug, "Payload sent!"); @@ -190,7 +190,6 @@ void SpiStm::execute() { // loop of the master driver in HiveMind. The loop needs no delay and shouldn't // have one as it will only increase latency, which could lead to instability. spi_slave_transmit(STM_SPI, &m_transaction, portMAX_DELAY); - } void SpiStm::updateOutboundHeader() {