Compare commits

..

6 Commits

5 changed files with 110 additions and 148 deletions

View File

@@ -467,7 +467,7 @@ ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceComm
info.expectedReplies = 0;
info.isExecuting = false;
info.sendReplyTo = NO_COMMANDER;
info.useAlternativeReplyId = alternativeReplyId;
info.useAlternativeReplyId = useAlternativeReply;
info.alternativeReplyId = alternativeReplyId;
auto resultPair = deviceCommandMap.emplace(deviceCommand, info);
if (resultPair.second) {
@@ -1601,3 +1601,14 @@ void DeviceHandlerBase::disableCommandsAndReplies() {
}
}
}
ReturnValue_t DeviceHandlerBase::finishAction(bool success, DeviceCommandId_t action,
ReturnValue_t result) {
auto commandIter = deviceCommandMap.find(action);
if (commandIter == deviceCommandMap.end()) {
return MessageQueueIF::NO_QUEUE;
}
commandIter->second.isExecuting = false;
actionHelper.finish(success, commandIter->second.sendReplyTo, action, result);
return returnvalue::OK;
}

View File

@@ -399,6 +399,8 @@ class DeviceHandlerBase : public DeviceHandlerIF,
*/
virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) = 0;
MessageQueueId_t getCommanderQueueId(DeviceCommandId_t replyId) const;
ReturnValue_t finishAction(bool success, DeviceCommandId_t action, ReturnValue_t result);
/**
* Helper function to get pending command. This is useful for devices
* like SPI sensors to identify the last sent command.

View File

@@ -3,159 +3,95 @@
#include <fsfw/serviceinterface/ServiceInterface.h>
#include <cstdio>
#include <fstream>
#include <iostream>
DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf,
BufPair decodedBuf, UserHandler handler, void* args)
BufPair decodedBuf)
: decodeRingBuf(decodeRingBuf),
decoder(decoder),
encodedBuf(encodedBuf),
decodedBuf(decodedBuf),
handler(handler),
ctx(args) {
if (handler == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "DleParser::DleParser: Invalid user handler" << std::endl;
#else
sif::printError("DleParser::DleParser: Invalid user handler\n");
#endif
}
decodedBuf(decodedBuf) {
}
ReturnValue_t DleParser::passData(uint8_t* data, size_t len) {
if (data == nullptr or len == 0 or handler == nullptr) {
ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) {
if (data == nullptr or len == 0) {
return returnvalue::FAILED;
}
size_t copyIntoRingBufFromHere = 0;
size_t copyAmount = len;
size_t startIdx = 0;
ReturnValue_t result = returnvalue::OK;
bool startFoundInThisPacket = false;
for (size_t idx = 0; idx < len; idx++) {
if (data[idx] == DleEncoder::STX_CHAR) {
if (not startFound and not startFoundInThisPacket) {
startIdx = idx;
copyIntoRingBufFromHere = idx;
copyAmount = len - idx;
return decodeRingBuf.writeData(data, len);
}
ReturnValue_t DleParser::parseRingBuf(size_t& readSize) {
ctx.setType(DleParser::ContextType::NONE);
size_t availableData = decodeRingBuf.getAvailableReadData();
if(availableData == 0) {
return NO_PACKET_FOUND;
}
if (availableData > encodedBuf.second) {
ErrorInfo info;
info.len = decodeRingBuf.getAvailableReadData();
setErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info);
return returnvalue::FAILED;
}
ReturnValue_t result = decodeRingBuf.readData(encodedBuf.first, availableData);
if (result != returnvalue::OK) {
ErrorInfo info;
info.res = result;
setErrorContext(ErrorTypes::RING_BUF_ERROR, info);
return result;
}
bool stxFound = false;
size_t stxIdx = 0;
for (size_t vectorIdx = 0; vectorIdx < availableData; vectorIdx++) {
// handle STX char
if (encodedBuf.first[vectorIdx] == DleEncoder::STX_CHAR) {
if (not stxFound) {
stxFound = true;
stxIdx = vectorIdx;
} else {
// Maybe print warning, should not happen
decodeRingBuf.clear();
// might be lost packet, so we should advance the read pointer
// without skipping the STX
readSize = vectorIdx;
ErrorInfo info;
info.len = idx;
prepareErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info);
handler(ctx);
copyIntoRingBufFromHere = idx;
copyAmount = len - idx;
setErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info);
return POSSIBLE_PACKET_LOSS;
}
startFound = true;
startFoundInThisPacket = true;
} else if (data[idx] == DleEncoder::ETX_CHAR) {
if (startFoundInThisPacket) {
size_t readLen = 0;
}
// handle ETX char
if (encodedBuf.first[vectorIdx] == DleEncoder::ETX_CHAR) {
if (stxFound) {
// This is propably a packet, so we decode it.
size_t decodedLen = 0;
result = decoder.decode(data + startIdx, idx + 1 - startIdx, &readLen, decodedBuf.first,
decodedBuf.second, &decodedLen);
size_t dummy = 0;
ReturnValue_t result =
decoder.decode(&encodedBuf.first[stxIdx], availableData - stxIdx, &dummy,
decodedBuf.first, decodedBuf.second, &decodedLen);
if (result == returnvalue::OK) {
ctx.setType(ContextType::PACKET_FOUND);
ctx.decodedPacket.first = decodedBuf.first;
ctx.decodedPacket.second = decodedLen;
this->handler(ctx);
} else if (result == DleEncoder::STREAM_TOO_SHORT) {
readSize = ++vectorIdx;
return returnvalue::OK;
} else {
// invalid packet, skip.
readSize = ++vectorIdx;
ErrorInfo info;
info.res = result;
prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info);
handler(ctx);
} else {
ErrorInfo info;
info.res = result;
prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info);
handler(ctx);
}
decodeRingBuf.clear();
if ((idx + 1) < len) {
copyIntoRingBufFromHere = idx + 1;
copyAmount = len - idx - 1;
} else {
copyAmount = 0;
}
} else if (startFound) {
// ETX found but STX was found in another mini packet. Reconstruct the full packet
// to decode it
result = decodeRingBuf.writeData(data, idx + 1);
if (result != returnvalue::OK) {
ErrorInfo info;
info.res = result;
prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info);
handler(ctx);
}
size_t fullEncodedLen = decodeRingBuf.getAvailableReadData();
if (fullEncodedLen > encodedBuf.second) {
ErrorInfo info;
info.len = fullEncodedLen;
prepareErrorContext(ErrorTypes::ENCODED_BUF_TOO_SMALL, info);
handler(ctx);
decodeRingBuf.clear();
} else {
size_t decodedLen = 0;
size_t readLen = 0;
decodeRingBuf.readData(encodedBuf.first, fullEncodedLen, true);
result = decoder.decode(encodedBuf.first, fullEncodedLen, &readLen, decodedBuf.first,
decodedBuf.second, &decodedLen);
if (result == returnvalue::OK) {
if (this->handler != nullptr) {
ctx.setType(ContextType::PACKET_FOUND);
ctx.decodedPacket.first = decodedBuf.first;
ctx.decodedPacket.second = decodedLen;
this->handler(ctx);
}
} else if (result == DleEncoder::STREAM_TOO_SHORT) {
ErrorInfo info;
info.res = result;
prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info);
handler(ctx);
} else {
ErrorInfo info;
info.res = result;
prepareErrorContext(ErrorTypes::DECODE_ERROR, info);
handler(ctx);
}
decodeRingBuf.clear();
startFound = false;
startFoundInThisPacket = false;
if ((idx + 1) < len) {
copyIntoRingBufFromHere = idx + 1;
copyAmount = len - idx - 1;
} else {
copyAmount = 0;
}
setErrorContext(ErrorTypes::DECODE_ERROR, info);
return POSSIBLE_PACKET_LOSS;
}
} else {
// End data without preceeding STX
// might be lost packet, so we should advance the read pointer
readSize = ++vectorIdx;
ErrorInfo info;
info.len = idx + 1;
prepareErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info);
handler(ctx);
decodeRingBuf.clear();
if ((idx + 1) < len) {
copyIntoRingBufFromHere = idx + 1;
copyAmount = len - idx - 1;
} else {
copyAmount = 0;
}
info.len = 0;
setErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info);
return POSSIBLE_PACKET_LOSS;
}
startFoundInThisPacket = false;
startFound = false;
}
}
if (copyAmount > 0) {
result = decodeRingBuf.writeData(data + copyIntoRingBufFromHere, copyAmount);
if (result != returnvalue::OK) {
ErrorInfo info;
info.res = result;
prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info);
handler(ctx);
}
}
return returnvalue::OK;
return NO_PACKET_FOUND;
}
void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args) {
@@ -169,8 +105,12 @@ void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* arg
#endif
}
void DleParser::defaultErrorHandler(ErrorTypes err, ErrorInfo ctx) {
switch (err) {
void DleParser::defaultErrorHandler() {
if(ctx.getType() != DleParser::ContextType::ERROR) {
errorPrinter("No error");
return;
}
switch (ctx.error.first) {
case (ErrorTypes::NONE): {
errorPrinter("No error");
break;
@@ -186,8 +126,8 @@ void DleParser::defaultErrorHandler(ErrorTypes err, ErrorInfo ctx) {
case (ErrorTypes::ENCODED_BUF_TOO_SMALL):
case (ErrorTypes::DECODING_BUF_TOO_SMALL): {
char opt[64];
snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", ctx.len);
if (err == ErrorTypes::ENCODED_BUF_TOO_SMALL) {
snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", ctx.decodedPacket.second);
if (ctx.error.first == ErrorTypes::ENCODED_BUF_TOO_SMALL) {
errorPrinter("Encoded buf too small", opt);
} else {
errorPrinter("Decoding buf too small", opt);
@@ -218,13 +158,20 @@ void DleParser::errorPrinter(const char* str, const char* opt) {
#endif
}
void DleParser::prepareErrorContext(ErrorTypes err, ErrorInfo info) {
void DleParser::setErrorContext(ErrorTypes err, ErrorInfo info) {
ctx.setType(ContextType::ERROR);
ctx.error.first = err;
ctx.error.second = info;
}
ReturnValue_t DleParser::confirmBytesRead(size_t bytesRead) {
return decodeRingBuf.deleteData(bytesRead);
}
const DleParser::Context& DleParser::getContext() {
return ctx;
}
void DleParser::reset() {
startFound = false;
decodeRingBuf.clear();
}

View File

@@ -18,9 +18,11 @@
*/
class DleParser {
public:
static constexpr ReturnValue_t NO_PACKET_FOUND = returnvalue::makeCode(1, 1);
static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS = returnvalue::makeCode(1, 2);
using BufPair = std::pair<uint8_t*, size_t>;
enum class ContextType { PACKET_FOUND, ERROR };
enum class ContextType { NONE, PACKET_FOUND, ERROR };
enum class ErrorTypes {
NONE,
@@ -41,7 +43,7 @@ class DleParser {
struct Context {
public:
Context(void* args) : userArgs(args) { setType(ContextType::PACKET_FOUND); }
Context() { setType(ContextType::PACKET_FOUND); }
void setType(ContextType type) {
this->type = type;
@@ -58,14 +60,11 @@ class DleParser {
BufPair decodedPacket = {};
ErrorPair error;
void* userArgs;
private:
ContextType type;
};
using UserHandler = void (*)(const Context& ctx);
/**
* Base class constructor
* @param decodeRingBuf Ring buffer used to store multiple packets to allow detecting DLE packets
@@ -79,7 +78,7 @@ class DleParser {
* @param args Arbitrary user argument
*/
DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf,
BufPair decodedBuf, UserHandler handler, void* args);
BufPair decodedBuf);
/**
* This function allows to pass new data into the parser. It then scans for DLE packets
@@ -88,8 +87,13 @@ class DleParser {
* @param len
* @return
*/
ReturnValue_t passData(uint8_t* data, size_t len);
ReturnValue_t passData(const uint8_t* data, size_t len);
ReturnValue_t parseRingBuf(size_t& bytesRead);
ReturnValue_t confirmBytesRead(size_t bytesRead);
const Context& getContext();
/**
* Example found packet handler
* function call
@@ -104,11 +108,11 @@ class DleParser {
* - For buffer length errors, will be set to the detected packet length which is too large
* - For decode or ring buffer errors, will be set to the result returned from the failed call
*/
static void defaultErrorHandler(ErrorTypes err, ErrorInfo ctx);
void defaultErrorHandler();
static void errorPrinter(const char* str, const char* opt = nullptr);
void prepareErrorContext(ErrorTypes err, ErrorInfo ctx);
void setErrorContext(ErrorTypes err, ErrorInfo ctx);
/**
* Resets the parser by resetting the internal states and clearing the decoding ring buffer
*/
@@ -119,7 +123,5 @@ class DleParser {
DleEncoder& decoder;
BufPair encodedBuf;
BufPair decodedBuf;
UserHandler handler = nullptr;
Context ctx;
bool startFound = false;
};

View File

@@ -15,8 +15,8 @@ Service5EventReporting::Service5EventReporting(PsbParams params, size_t maxNumbe
maxNumberReportsPerCycle(maxNumberReportsPerCycle) {
auto mqArgs = MqArgs(getObjectId(), static_cast<void*>(this));
psbParams.name = "PUS 5 Event Reporting";
eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth,
MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
eventQueue = QueueFactory::instance()->createMessageQueue(
messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
}
Service5EventReporting::~Service5EventReporting() {