eive/fsfw
eive/fsfw
15
1
Fork 0
forked from fsfw/fsfw
Code Issues Pull Requests Releases 1 Wiki Activity

some bugfixes

Browse Source
This commit is contained in:
Robin Müller 2022-10-04 20:51:58 +02:00
parent cfc00d0260
commit 49747fc8a4
No known key found for this signature in database
GPG Key ID: 71B58F8A3CDFA9AC
3 changed files with 45 additions and 175 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

196
src/fsfw/globalfunctions/DleParser.cpp
View File

@ -7,176 +7,35 @@
#include <iostream> #include <iostream>
DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf, DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf,
BufPair decodedBuf, UserHandler handler, void* args) BufPair decodedBuf)
: decodeRingBuf(decodeRingBuf), : decodeRingBuf(decodeRingBuf),
decoder(decoder), decoder(decoder),
encodedBuf(encodedBuf), encodedBuf(encodedBuf),
decodedBuf(decodedBuf), 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
}
} }
ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) { ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) {
if (data == nullptr or len == 0 or handler == nullptr) { if (data == nullptr or len == 0) {
return returnvalue::FAILED; return returnvalue::FAILED;
} }
return decodeRingBuf.writeData(data, len); return decodeRingBuf.writeData(data, len);
// std::string filename = std::string("/mnt/sd0/scex/transfer") + std::to_string(COUNTER++);
// std::ofstream of(filename);
// of.write(reinterpret_cast<const char*>(data), len);
// 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;
// } else {
// // Maybe print warning, should not happen
// decodeRingBuf.clear();
// ErrorInfo info;
// info.len = idx;
// prepareErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info);
// handler(ctx);
// copyIntoRingBufFromHere = idx;
// copyAmount = len - idx;
// }
// startFound = true;
// startFoundInThisPacket = true;
// } else if (data[idx] == DleEncoder::ETX_CHAR) {
// if (startFoundInThisPacket) {
// size_t readLen = 0;
// size_t decodedLen = 0;
// result = decoder.decode(data + startIdx, idx + 1 - startIdx, &readLen, 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) {
// 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;
// }
// }
// } else {
// // End data without preceeding STX
// 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;
// }
// }
// 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);
// }
// }
} }
ReturnValue_t DleParser::parseRingBuf(size_t& readSize) { ReturnValue_t DleParser::parseRingBuf(size_t& readSize) {
ctx.setType(DleParser::ContextType::NONE);
size_t availableData = decodeRingBuf.getAvailableReadData(); size_t availableData = decodeRingBuf.getAvailableReadData();
if (availableData > encodedBuf.second) { if (availableData > encodedBuf.second) {
ErrorInfo info; ErrorInfo info;
info.len = decodeRingBuf.getAvailableReadData(); info.len = decodeRingBuf.getAvailableReadData();
prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); setErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info);
handler(ctx); return returnvalue::FAILED;
} }
ReturnValue_t result = decodeRingBuf.readData(encodedBuf.first, availableData); ReturnValue_t result = decodeRingBuf.readData(encodedBuf.first, availableData);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
ErrorInfo info; ErrorInfo info;
info.res = result; info.res = result;
prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); setErrorContext(ErrorTypes::RING_BUF_ERROR, info);
return result;
} }
bool stxFound = false; bool stxFound = false;
size_t stxIdx = 0; size_t stxIdx = 0;
@ -190,6 +49,8 @@ ReturnValue_t DleParser::parseRingBuf(size_t& readSize) {
// might be lost packet, so we should advance the read pointer // might be lost packet, so we should advance the read pointer
// without skipping the STX // without skipping the STX
readSize = vectorIdx; readSize = vectorIdx;
ErrorInfo info;
setErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info);
return POSSIBLE_PACKET_LOSS; return POSSIBLE_PACKET_LOSS;
} }
} }
@ -199,31 +60,37 @@ ReturnValue_t DleParser::parseRingBuf(size_t& readSize) {
// This is propably a packet, so we decode it. // This is propably a packet, so we decode it.
size_t decodedLen = 0; size_t decodedLen = 0;
size_t dummy = 0; size_t dummy = 0;
readSize = vectorIdx;
ReturnValue_t result = decoder.decode(&encodedBuf.first[stxIdx], availableData - stxIdx, ReturnValue_t result =
&dummy, decodedBuf.first, decodedBuf.second, &decodedLen); decoder.decode(&encodedBuf.first[stxIdx], availableData - stxIdx, &dummy,
decodedBuf.first, decodedBuf.second, &decodedLen);
if (result == returnvalue::OK) { if (result == returnvalue::OK) {
ctx.setType(ContextType::PACKET_FOUND); ctx.setType(ContextType::PACKET_FOUND);
ctx.decodedPacket.first = decodedBuf.first; ctx.decodedPacket.first = decodedBuf.first;
ctx.decodedPacket.second = decodedLen; ctx.decodedPacket.second = decodedLen;
this->handler(ctx); readSize = vectorIdx + 1;
return returnvalue::OK; return returnvalue::OK;
} else { } else {
// invalid packet, skip. // invalid packet, skip.
readSize = ++vectorIdx; readSize = ++vectorIdx;
ErrorInfo info;
info.res = result;
setErrorContext(ErrorTypes::DECODE_ERROR, info);
return POSSIBLE_PACKET_LOSS; return POSSIBLE_PACKET_LOSS;
} }
} else { } else {
// might be lost packet, so we should advance the read pointer // might be lost packet, so we should advance the read pointer
readSize = ++vectorIdx; readSize = ++vectorIdx;
ErrorInfo info;
info.len = 0;
setErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info);
return POSSIBLE_PACKET_LOSS; return POSSIBLE_PACKET_LOSS;
} }
} }
} }
return returnvalue::OK; return NO_PACKET_FOUND;
} }
void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args) { void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args) {
#if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
@ -235,8 +102,12 @@ void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* arg
#endif #endif
} }
void DleParser::defaultErrorHandler(ErrorTypes err, ErrorInfo ctx) { void DleParser::defaultErrorHandler() {
switch (err) { if(ctx.getType() != DleParser::ContextType::ERROR) {
errorPrinter("No error");
return;
}
switch (ctx.error.first) {
case (ErrorTypes::NONE): { case (ErrorTypes::NONE): {
errorPrinter("No error"); errorPrinter("No error");
break; break;
@ -252,8 +123,8 @@ void DleParser::defaultErrorHandler(ErrorTypes err, ErrorInfo ctx) {
case (ErrorTypes::ENCODED_BUF_TOO_SMALL): case (ErrorTypes::ENCODED_BUF_TOO_SMALL):
case (ErrorTypes::DECODING_BUF_TOO_SMALL): { case (ErrorTypes::DECODING_BUF_TOO_SMALL): {
char opt[64]; char opt[64];
snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", ctx.len); snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", ctx.decodedPacket.second);
if (err == ErrorTypes::ENCODED_BUF_TOO_SMALL) { if (ctx.error.first == ErrorTypes::ENCODED_BUF_TOO_SMALL) {
errorPrinter("Encoded buf too small", opt); errorPrinter("Encoded buf too small", opt);
} else { } else {
errorPrinter("Decoding buf too small", opt); errorPrinter("Decoding buf too small", opt);
@ -284,7 +155,7 @@ void DleParser::errorPrinter(const char* str, const char* opt) {
#endif #endif
} }
void DleParser::prepareErrorContext(ErrorTypes err, ErrorInfo info) { void DleParser::setErrorContext(ErrorTypes err, ErrorInfo info) {
ctx.setType(ContextType::ERROR); ctx.setType(ContextType::ERROR);
ctx.error.first = err; ctx.error.first = err;
ctx.error.second = info; ctx.error.second = info;
@ -294,7 +165,10 @@ ReturnValue_t DleParser::confirmBytesRead(size_t bytesRead) {
return decodeRingBuf.deleteData(bytesRead); return decodeRingBuf.deleteData(bytesRead);
} }
const DleParser::Context& DleParser::getContext() {
return ctx;
}
void DleParser::reset() { void DleParser::reset() {
startFound = false;
decodeRingBuf.clear(); decodeRingBuf.clear();
} }

16
src/fsfw/globalfunctions/DleParser.h
View File

@ -22,7 +22,7 @@ class DleParser {
static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS = returnvalue::makeCode(1, 2); static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS = returnvalue::makeCode(1, 2);
using BufPair = std::pair<uint8_t*, size_t>; using BufPair = std::pair<uint8_t*, size_t>;
enum class ContextType { PACKET_FOUND, ERROR }; enum class ContextType { NONE, PACKET_FOUND, ERROR };
enum class ErrorTypes { enum class ErrorTypes {
NONE, NONE,
@ -43,7 +43,7 @@ class DleParser {
struct Context { struct Context {
public: public:
Context(void* args) : userArgs(args) { setType(ContextType::PACKET_FOUND); } Context() { setType(ContextType::PACKET_FOUND); }
void setType(ContextType type) { void setType(ContextType type) {
this->type = type; this->type = type;
@ -60,14 +60,11 @@ class DleParser {
BufPair decodedPacket = {}; BufPair decodedPacket = {};
ErrorPair error; ErrorPair error;
void* userArgs;
private: private:
ContextType type; ContextType type;
}; };
using UserHandler = void (*)(const Context& ctx);
/** /**
* Base class constructor * Base class constructor
* @param decodeRingBuf Ring buffer used to store multiple packets to allow detecting DLE packets * @param decodeRingBuf Ring buffer used to store multiple packets to allow detecting DLE packets
@ -81,7 +78,7 @@ class DleParser {
* @param args Arbitrary user argument * @param args Arbitrary user argument
*/ */
DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf, 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 * This function allows to pass new data into the parser. It then scans for DLE packets
@ -96,6 +93,7 @@ class DleParser {
ReturnValue_t confirmBytesRead(size_t bytesRead); ReturnValue_t confirmBytesRead(size_t bytesRead);
const Context& getContext();
/** /**
* Example found packet handler * Example found packet handler
* function call * function call
@ -110,11 +108,11 @@ class DleParser {
* - For buffer length errors, will be set to the detected packet length which is too large * - 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 * - 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); 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 * Resets the parser by resetting the internal states and clearing the decoding ring buffer
*/ */
@ -125,7 +123,5 @@ class DleParser {
DleEncoder& decoder; DleEncoder& decoder;
BufPair encodedBuf; BufPair encodedBuf;
BufPair decodedBuf; BufPair decodedBuf;
UserHandler handler = nullptr;
Context ctx; Context ctx;
bool startFound = false;
}; };

4
src/fsfw/pus/Service5EventReporting.cpp
View File

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