mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-31 04:51:35 +08:00
Added muorb aggregator from DSP to Apps direction
This commit is contained in:
Eric Katzfey
committed by
Daniel Agar
Daniel Agar
parent
d945e87e4f
commit
b0580d88e1
@@ -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));
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
};
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user