Added muorb aggregator from DSP to Apps direction

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 <px4_platform_common/log.h>
+#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<uint8_t *>(&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<uint8_t *>(data));
+	}
+}

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 <string>
+#include <string.h>
+#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);
+};
+
+}

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
 	)

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<std::string, int> 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<uint8_t *>(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;
 }
 

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<std::string, int>           _SlpiSubscriberCache;
 	static pthread_mutex_t                      _tx_mutex;
 	static pthread_mutex_t                      _rx_mutex;

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)
+target_include_directories(modules__muorb__slpi PRIVATE ../aggregator)
+target_include_directories(modules__muorb__slpi PRIVATE ${PX4_BINARY_DIR}/platforms/common/uORB)

src/modules/muorb/slpi/uORBProtobufChannel.cpp

@@ -43,6 +43,7 @@
 #include <px4_platform_common/log.h>
 #include <lib/parameters/param.h>
 #include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
+#include <qurt.h>
 
 #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<std::string, int> 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"); }

src/modules/muorb/slpi/uORBProtobufChannel.hpp

@@ -40,6 +40,7 @@
 #include <pthread.h>
 
 #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<std::string, int>           _AppsSubscriberCache;
 	static pthread_mutex_t                      _tx_mutex;
 	static pthread_mutex_t                      _rx_mutex;