diff --git a/src/modules/muorb/aggregator/mUORBAggregator.cpp b/src/modules/muorb/aggregator/mUORBAggregator.cpp new file mode 100644 index 0000000000..5a401bb4e9 --- /dev/null +++ b/src/modules/muorb/aggregator/mUORBAggregator.cpp @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * Copyright (C) 2022 ModalAI, Inc. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "mUORBAggregator.hpp" + +const bool mUORB::Aggregator::debugFlag = false; + +bool mUORB::Aggregator::NewRecordOverflows(const char *messageName, int32_t length) +{ + if (! messageName) { return false; } + + uint32_t messageNameLength = strlen(messageName); + uint32_t newMessageRecordTotalLength = headerSize + messageNameLength + length; + return ((bufferWriteIndex + newMessageRecordTotalLength) > bufferSize); +} + +void mUORB::Aggregator::MoveToNextBuffer() +{ + bufferWriteIndex = 0; + bufferId++; + bufferId %= numBuffers; +} + +void mUORB::Aggregator::AddRecordToBuffer(const char *messageName, int32_t length, const uint8_t *data) +{ + if (! messageName) { return; } + + uint32_t messageNameLength = strlen(messageName); + memcpy(&buffer[bufferId][bufferWriteIndex], (uint8_t *) &syncFlag, syncFlagSize); + bufferWriteIndex += syncFlagSize; + memcpy(&buffer[bufferId][bufferWriteIndex], (uint8_t *) &messageNameLength, topicNameLengthSize); + bufferWriteIndex += topicNameLengthSize; + memcpy(&buffer[bufferId][bufferWriteIndex], (uint8_t *) &length, dataLengthSize); + bufferWriteIndex += dataLengthSize; + memcpy(&buffer[bufferId][bufferWriteIndex], (uint8_t *) messageName, messageNameLength); + bufferWriteIndex += messageNameLength; + memcpy(&buffer[bufferId][bufferWriteIndex], data, length); + bufferWriteIndex += length; +} + +int16_t mUORB::Aggregator::SendData() +{ + int16_t rc = 0; + + if (sendFunc) { + if (aggregationEnabled) { + if (bufferWriteIndex) { + rc = sendFunc(topicName.c_str(), buffer[bufferId], bufferWriteIndex); + MoveToNextBuffer(); + } + } + } + + return rc; +} + +int16_t mUORB::Aggregator::ProcessTransmitTopic(const char *topic, const uint8_t *data, uint32_t length_in_bytes) +{ + int16_t rc = 0; + + if (sendFunc) { + if (aggregationEnabled) { + if (NewRecordOverflows(topic, length_in_bytes)) { + rc = SendData(); + } + + AddRecordToBuffer(topic, length_in_bytes, data); + + } else if (topic) { + rc = sendFunc(topic, data, length_in_bytes); + } + } + + return rc; +} + +void mUORB::Aggregator::ProcessReceivedTopic(const char *topic, const uint8_t *data, uint32_t length_in_bytes) +{ + if (isAggregate(topic)) { + if (debugFlag) { PX4_INFO("Parsing aggregate buffer of length %u", length_in_bytes); } + + uint32_t current_index = 0; + const uint32_t name_buffer_length = 80; + char name_buffer[name_buffer_length]; + + while ((current_index + headerSize) < length_in_bytes) { + uint32_t sync_flag = *((uint32_t *) &data[current_index]); + + if (sync_flag != syncFlag) { + PX4_ERR("Expected sync flag but got 0x%X", sync_flag); + break; + } + + current_index += syncFlagSize; + + uint32_t name_length = *((uint32_t *) &data[current_index]); + + // Make sure name plus a terminating null can fit into our buffer + if (name_length > (name_buffer_length - 1)) { + PX4_ERR("Name length too long %u", name_length); + break; + } + + current_index += topicNameLengthSize; + + uint32_t data_length = *((uint32_t *) &data[current_index]); + current_index += dataLengthSize; + + int32_t payload_size = name_length + data_length; + int32_t remaining_bytes = length_in_bytes - current_index; + + if (payload_size > remaining_bytes) { + PX4_ERR("Payload too big %u. Remaining bytes %d", payload_size, remaining_bytes); + break; + } + + memcpy(name_buffer, &data[current_index], name_length); + name_buffer[name_length] = 0; + + current_index += name_length; + + if (debugFlag) { PX4_INFO("Parsed topic: %s, name length %u, data length: %u", name_buffer, name_length, data_length); } + + _RxHandler->process_received_message(name_buffer, + data_length, + const_cast(&data[current_index])); + current_index += data_length; + } + + } else { + if (debugFlag) { PX4_INFO("Got non-aggregate buffer for topic %s", topic); } + + // It isn't an aggregated buffer so just process normally + _RxHandler->process_received_message(topic, + length_in_bytes, + const_cast(data)); + } +} diff --git a/src/modules/muorb/aggregator/mUORBAggregator.hpp b/src/modules/muorb/aggregator/mUORBAggregator.hpp new file mode 100644 index 0000000000..63bcf5845c --- /dev/null +++ b/src/modules/muorb/aggregator/mUORBAggregator.hpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (C) 2022 ModalAI, Inc. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include "uORB/uORBCommunicator.hpp" + +namespace mUORB +{ + +class Aggregator +{ +public: + typedef int (*sendFuncPtr)(const char *, const uint8_t *, int); + + void RegisterSendHandler(sendFuncPtr func) { sendFunc = func; } + + void RegisterHandler(uORBCommunicator::IChannelRxHandler *handler) { _RxHandler = handler; } + + int16_t ProcessTransmitTopic(const char *topic, const uint8_t *data, uint32_t length_in_bytes); + + void ProcessReceivedTopic(const char *topic, const uint8_t *data, uint32_t length_in_bytes); + + int16_t SendData(); + +private: + static const bool debugFlag; + + const std::string topicName = "aggregation"; + + // Master flag to enable aggregation + const bool aggregationEnabled = true; + + const uint32_t syncFlag = 0x5A01FF00; + const uint32_t syncFlagSize = 4; + const uint32_t topicNameLengthSize = 4; + const uint32_t dataLengthSize = 4; + const uint32_t headerSize = syncFlagSize + topicNameLengthSize + dataLengthSize; + static const uint32_t numBuffers = 2; + static const uint32_t bufferSize = 2048; + + uint32_t bufferId; + uint32_t bufferWriteIndex; + uint8_t buffer[numBuffers][bufferSize]; + + uORBCommunicator::IChannelRxHandler *_RxHandler; + + sendFuncPtr sendFunc; + + bool isAggregate(const char *name) { return (strcmp(name, topicName.c_str()) == 0); } + + bool NewRecordOverflows(const char *messageName, int32_t length); + + void MoveToNextBuffer(); + + void AddRecordToBuffer(const char *messageName, int32_t length, const uint8_t *data); +}; + +} diff --git a/src/modules/muorb/apps/CMakeLists.txt b/src/modules/muorb/apps/CMakeLists.txt index 010c8d3212..55573cde35 100644 --- a/src/modules/muorb/apps/CMakeLists.txt +++ b/src/modules/muorb/apps/CMakeLists.txt @@ -38,8 +38,10 @@ px4_add_module( -Wno-cast-align # TODO: fix and enable INCLUDES ../test + ../aggregator SRCS uORBAppsProtobufChannel.cpp muorb_main.cpp ../test/MUORBTest.cpp + ../aggregator/mUORBAggregator.cpp ) diff --git a/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp b/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp index b57bcf81e5..9634165479 100644 --- a/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp +++ b/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp @@ -41,6 +41,7 @@ bool uORB::AppsProtobufChannel::test_flag = false; // Initialize the static members uORB::AppsProtobufChannel *uORB::AppsProtobufChannel::_InstancePtr = nullptr; uORBCommunicator::IChannelRxHandler *uORB::AppsProtobufChannel::_RxHandler = nullptr; +mUORB::Aggregator uORB::AppsProtobufChannel::_Aggregator; std::map uORB::AppsProtobufChannel::_SlpiSubscriberCache; pthread_mutex_t uORB::AppsProtobufChannel::_tx_mutex = PTHREAD_MUTEX_INITIALIZER; pthread_mutex_t uORB::AppsProtobufChannel::_rx_mutex = PTHREAD_MUTEX_INITIALIZER; @@ -81,9 +82,7 @@ void uORB::AppsProtobufChannel::ReceiveCallback(const char *topic, return; } else if (_RxHandler) { - _RxHandler->process_received_message(topic, - length_in_bytes, - const_cast(data)); + _Aggregator.ProcessReceivedTopic(topic, data, length_in_bytes); } else { PX4_ERR("Couldn't handle topic %s in receive callback", topic); @@ -283,6 +282,7 @@ int16_t uORB::AppsProtobufChannel::remove_subscription(const char *messageName) int16_t uORB::AppsProtobufChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) { _RxHandler = handler; + _Aggregator.RegisterHandler(handler); return 0; } diff --git a/src/modules/muorb/apps/uORBAppsProtobufChannel.hpp b/src/modules/muorb/apps/uORBAppsProtobufChannel.hpp index 5ab26891e1..7d280a460b 100644 --- a/src/modules/muorb/apps/uORBAppsProtobufChannel.hpp +++ b/src/modules/muorb/apps/uORBAppsProtobufChannel.hpp @@ -42,6 +42,7 @@ #include "MUORBTest.hpp" #include "uORB/uORBCommunicator.hpp" +#include "mUORBAggregator.hpp" namespace uORB { @@ -161,6 +162,7 @@ private: */ static uORB::AppsProtobufChannel *_InstancePtr; static uORBCommunicator::IChannelRxHandler *_RxHandler; + static mUORB::Aggregator _Aggregator; static std::map _SlpiSubscriberCache; static pthread_mutex_t _tx_mutex; static pthread_mutex_t _rx_mutex; diff --git a/src/modules/muorb/slpi/CMakeLists.txt b/src/modules/muorb/slpi/CMakeLists.txt index ab76a64e8c..fdbf9147cf 100644 --- a/src/modules/muorb/slpi/CMakeLists.txt +++ b/src/modules/muorb/slpi/CMakeLists.txt @@ -34,6 +34,8 @@ px4_add_library(modules__muorb__slpi uORBProtobufChannel.cpp ../test/MUORBTest.cpp + ../aggregator/mUORBAggregator.cpp ) target_include_directories(modules__muorb__slpi PRIVATE ../test) -target_include_directories(modules__muorb__slpi PRIVATE ${PX4_BINARY_DIR}/platforms/common/uORB) \ No newline at end of file +target_include_directories(modules__muorb__slpi PRIVATE ../aggregator) +target_include_directories(modules__muorb__slpi PRIVATE ${PX4_BINARY_DIR}/platforms/common/uORB) diff --git a/src/modules/muorb/slpi/uORBProtobufChannel.cpp b/src/modules/muorb/slpi/uORBProtobufChannel.cpp index 30434a85ac..fa1dcdbe22 100644 --- a/src/modules/muorb/slpi/uORBProtobufChannel.cpp +++ b/src/modules/muorb/slpi/uORBProtobufChannel.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include "hrt_work.h" @@ -54,6 +55,7 @@ fc_func_ptrs muorb_func_ptrs; // static initialization. uORB::ProtobufChannel uORB::ProtobufChannel::_Instance; uORBCommunicator::IChannelRxHandler *uORB::ProtobufChannel::_RxHandler; +mUORB::Aggregator uORB::ProtobufChannel::_Aggregator; std::map uORB::ProtobufChannel::_AppsSubscriberCache; pthread_mutex_t uORB::ProtobufChannel::_rx_mutex = PTHREAD_MUTEX_INITIALIZER; pthread_mutex_t uORB::ProtobufChannel::_tx_mutex = PTHREAD_MUTEX_INITIALIZER; @@ -62,6 +64,30 @@ bool uORB::ProtobufChannel::_debug = false; bool _px4_muorb_debug = false; static bool px4muorb_orb_initialized = false; +// Thread for aggregator checking +qurt_thread_t aggregator_tid; +qurt_thread_attr_t aggregator_attr; +// 1 is highest priority, 255 is lowest. Set it low. +const uint32_t aggregator_thread_priority = 240; +const uint32_t aggregator_stack_size = 8096; +char aggregator_stack[aggregator_stack_size]; + +static void aggregator_thread_func(void *ptr) +{ + PX4_INFO("muorb aggregator thread running"); + + uORB::ProtobufChannel *muorb = uORB::ProtobufChannel::GetInstance(); + + while (true) { + // Check for timeout. Send buffer if timeout happened. + muorb->SendAggregateData(); + + qurt_timer_sleep(2000); + } + + qurt_thread_exit(QURT_EOK); +} + int16_t uORB::ProtobufChannel::topic_advertised(const char *messageName) { if (_debug) { PX4_INFO("Advertising %s on remote side", messageName); } @@ -111,6 +137,7 @@ int16_t uORB::ProtobufChannel::remove_subscription(const char *messageName) int16_t uORB::ProtobufChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) { _RxHandler = handler; + _Aggregator.RegisterHandler(handler); return 0; } @@ -141,8 +168,17 @@ int16_t uORB::ProtobufChannel::send_message(const char *messageName, int32_t len PX4_INFO("Sending message for topic %s", messageName); } + int16_t rc = 0; pthread_mutex_lock(&_tx_mutex); - int16_t rc = muorb_func_ptrs.topic_data_func_ptr(messageName, data, length); + + if (is_not_slpi_log) { + rc = _Aggregator.ProcessTransmitTopic(messageName, data, length); + + } else { + // SLPI logs don't go through the aggregator + rc = muorb_func_ptrs.topic_data_func_ptr(messageName, data, length); + } + pthread_mutex_unlock(&_tx_mutex); return rc; } @@ -239,6 +275,8 @@ int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) px4::WorkQueueManagerStart(); + uORB::ProtobufChannel::GetInstance()->RegisterSendHandler(muorb_func_ptrs.topic_data_func_ptr); + // Configure the SPI driver function pointers device::SPI::configure_callbacks(muorb_func_ptrs._config_spi_bus_func_t, muorb_func_ptrs._spi_transfer_func_t); @@ -257,6 +295,13 @@ int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) PX4_ERR("slpi failed in %s", __FUNCTION__); } + // Setup the thread to monitor for topic aggregator timeouts + qurt_thread_attr_init(&aggregator_attr); + qurt_thread_attr_set_stack_addr(&aggregator_attr, aggregator_stack); + qurt_thread_attr_set_stack_size(&aggregator_attr, aggregator_stack_size); + qurt_thread_attr_set_priority(&aggregator_attr, aggregator_thread_priority); + (void) qurt_thread_create(&aggregator_tid, &aggregator_attr, aggregator_thread_func, NULL); + px4muorb_orb_initialized = true; if (_px4_muorb_debug) { PX4_INFO("px4muorb_orb_initialize called"); } diff --git a/src/modules/muorb/slpi/uORBProtobufChannel.hpp b/src/modules/muorb/slpi/uORBProtobufChannel.hpp index 1e5e3ea847..7bc35aa088 100644 --- a/src/modules/muorb/slpi/uORBProtobufChannel.hpp +++ b/src/modules/muorb/slpi/uORBProtobufChannel.hpp @@ -40,6 +40,7 @@ #include #include "uORB/uORBCommunicator.hpp" +#include "mUORBAggregator.hpp" namespace uORB { @@ -125,6 +126,11 @@ public: return _RxHandler; } + void RegisterSendHandler(mUORB::Aggregator::sendFuncPtr func) + { + _Aggregator.RegisterSendHandler(func); + } + void AddRemoteSubscriber(const std::string &messageName) { pthread_mutex_lock(&_rx_mutex); @@ -145,12 +151,20 @@ public: bool DebugEnabled() { return _debug; } + void SendAggregateData() + { + pthread_mutex_lock(&_tx_mutex); + _Aggregator.SendData(); + pthread_mutex_unlock(&_tx_mutex); + } + private: /** * Data Members */ static uORB::ProtobufChannel _Instance; static uORBCommunicator::IChannelRxHandler *_RxHandler; + static mUORB::Aggregator _Aggregator; static std::map _AppsSubscriberCache; static pthread_mutex_t _tx_mutex; static pthread_mutex_t _rx_mutex;