WIP: somethings wrong.. #19
@ -21,10 +21,10 @@ public:
|
|||||||
return (availableWriteSpace(n) == 0);
|
return (availableWriteSpace(n) == 0);
|
||||||
}
|
}
|
||||||
bool isEmpty(uint8_t n = 0) {
|
bool isEmpty(uint8_t n = 0) {
|
||||||
return (availableReadData(n) == 0);
|
return (getAvailableReadData(n) == 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
size_t availableReadData(uint8_t n = 0) const {
|
size_t getAvailableReadData(uint8_t n = 0) const {
|
||||||
return ((write + size) - read[n]) % size;
|
return ((write + size) - read[n]) % size;
|
||||||
}
|
}
|
||||||
size_t availableWriteSpace(uint8_t n = 0) const {
|
size_t availableWriteSpace(uint8_t n = 0) const {
|
||||||
@ -36,7 +36,7 @@ public:
|
|||||||
return overwriteOld;
|
return overwriteOld;
|
||||||
}
|
}
|
||||||
|
|
||||||
size_t maxSize() const {
|
size_t getMaxSize() const {
|
||||||
return size - 1;
|
return size - 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -73,7 +73,7 @@ protected:
|
|||||||
}
|
}
|
||||||
|
|
||||||
ReturnValue_t readData(uint32_t amount, uint8_t n = 0) {
|
ReturnValue_t readData(uint32_t amount, uint8_t n = 0) {
|
||||||
if (availableReadData(n) >= amount) {
|
if (getAvailableReadData(n) >= amount) {
|
||||||
incrementRead(amount, n);
|
incrementRead(amount, n);
|
||||||
return HasReturnvaluesIF::RETURN_OK;
|
return HasReturnvaluesIF::RETURN_OK;
|
||||||
} else {
|
} else {
|
||||||
|
@ -71,7 +71,7 @@ ReturnValue_t SimpleRingBuffer::writeData(const uint8_t* data,
|
|||||||
|
|
||||||
ReturnValue_t SimpleRingBuffer::readData(uint8_t* data, size_t amount,
|
ReturnValue_t SimpleRingBuffer::readData(uint8_t* data, size_t amount,
|
||||||
bool incrementReadPtr, bool readRemaining, size_t* trueAmount) {
|
bool incrementReadPtr, bool readRemaining, size_t* trueAmount) {
|
||||||
size_t availableData = availableReadData(READ_PTR);
|
size_t availableData = getAvailableReadData(READ_PTR);
|
||||||
size_t amountTillWrap = readTillWrap(READ_PTR);
|
size_t amountTillWrap = readTillWrap(READ_PTR);
|
||||||
if (availableData < amount) {
|
if (availableData < amount) {
|
||||||
if (readRemaining) {
|
if (readRemaining) {
|
||||||
@ -110,7 +110,7 @@ void SimpleRingBuffer::moveExcessBytesToStart() {
|
|||||||
|
|
||||||
ReturnValue_t SimpleRingBuffer::deleteData(size_t amount,
|
ReturnValue_t SimpleRingBuffer::deleteData(size_t amount,
|
||||||
bool deleteRemaining, size_t* trueAmount) {
|
bool deleteRemaining, size_t* trueAmount) {
|
||||||
size_t availableData = availableReadData(READ_PTR);
|
size_t availableData = getAvailableReadData(READ_PTR);
|
||||||
if (availableData < amount) {
|
if (availableData < amount) {
|
||||||
if (deleteRemaining) {
|
if (deleteRemaining) {
|
||||||
amount = availableData;
|
amount = availableData;
|
||||||
|
@ -13,7 +13,7 @@ ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream,
|
|||||||
size_t encodedIndex = 0, sourceIndex = 0;
|
size_t encodedIndex = 0, sourceIndex = 0;
|
||||||
uint8_t nextByte;
|
uint8_t nextByte;
|
||||||
if (addStxEtx) {
|
if (addStxEtx) {
|
||||||
destStream[0] = STX;
|
destStream[0] = STX_CHAR;
|
||||||
++encodedIndex;
|
++encodedIndex;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -21,12 +21,12 @@ ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream,
|
|||||||
{
|
{
|
||||||
nextByte = sourceStream[sourceIndex];
|
nextByte = sourceStream[sourceIndex];
|
||||||
// STX, ETX and CR characters in the stream need to be escaped with DLE
|
// STX, ETX and CR characters in the stream need to be escaped with DLE
|
||||||
if (nextByte == STX or nextByte == ETX or nextByte == CARRIAGE_RETURN) {
|
if (nextByte == STX_CHAR or nextByte == ETX_CHAR or nextByte == CARRIAGE_RETURN) {
|
||||||
if (encodedIndex + 1 >= maxDestLen) {
|
if (encodedIndex + 1 >= maxDestLen) {
|
||||||
return STREAM_TOO_SHORT;
|
return STREAM_TOO_SHORT;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
destStream[encodedIndex] = DLE;
|
destStream[encodedIndex] = DLE_CHAR;
|
||||||
++encodedIndex;
|
++encodedIndex;
|
||||||
/* Escaped byte will be actual byte + 0x40. This prevents
|
/* Escaped byte will be actual byte + 0x40. This prevents
|
||||||
* STX, ETX, and carriage return characters from appearing
|
* STX, ETX, and carriage return characters from appearing
|
||||||
@ -39,14 +39,14 @@ ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
// DLE characters are simply escaped with DLE.
|
// DLE characters are simply escaped with DLE.
|
||||||
else if (nextByte == DLE) {
|
else if (nextByte == DLE_CHAR) {
|
||||||
if (encodedIndex + 1 >= maxDestLen) {
|
if (encodedIndex + 1 >= maxDestLen) {
|
||||||
return STREAM_TOO_SHORT;
|
return STREAM_TOO_SHORT;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
destStream[encodedIndex] = DLE;
|
destStream[encodedIndex] = DLE_CHAR;
|
||||||
++encodedIndex;
|
++encodedIndex;
|
||||||
destStream[encodedIndex] = DLE;
|
destStream[encodedIndex] = DLE_CHAR;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
@ -58,7 +58,7 @@ ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream,
|
|||||||
|
|
||||||
if (sourceIndex == sourceLen and encodedIndex < maxDestLen) {
|
if (sourceIndex == sourceLen and encodedIndex < maxDestLen) {
|
||||||
if (addStxEtx) {
|
if (addStxEtx) {
|
||||||
destStream[encodedIndex] = ETX;
|
destStream[encodedIndex] = ETX_CHAR;
|
||||||
++encodedIndex;
|
++encodedIndex;
|
||||||
}
|
}
|
||||||
*encodedLen = encodedIndex;
|
*encodedLen = encodedIndex;
|
||||||
@ -74,19 +74,19 @@ ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream,
|
|||||||
size_t maxDestStreamlen, size_t *decodedLen) {
|
size_t maxDestStreamlen, size_t *decodedLen) {
|
||||||
size_t encodedIndex = 0, decodedIndex = 0;
|
size_t encodedIndex = 0, decodedIndex = 0;
|
||||||
uint8_t nextByte;
|
uint8_t nextByte;
|
||||||
if (*sourceStream != STX) {
|
if (*sourceStream != STX_CHAR) {
|
||||||
return DECODING_ERROR;
|
return DECODING_ERROR;
|
||||||
}
|
}
|
||||||
++encodedIndex;
|
++encodedIndex;
|
||||||
|
|
||||||
while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen)
|
while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen)
|
||||||
&& (sourceStream[encodedIndex] != ETX)
|
&& (sourceStream[encodedIndex] != ETX_CHAR)
|
||||||
&& (sourceStream[encodedIndex] != STX)) {
|
&& (sourceStream[encodedIndex] != STX_CHAR)) {
|
||||||
if (sourceStream[encodedIndex] == DLE) {
|
if (sourceStream[encodedIndex] == DLE_CHAR) {
|
||||||
nextByte = sourceStream[encodedIndex + 1];
|
nextByte = sourceStream[encodedIndex + 1];
|
||||||
// The next byte is a DLE character that was escaped by another
|
// The next byte is a DLE character that was escaped by another
|
||||||
// DLE character, so we can write it to the destination stream.
|
// DLE character, so we can write it to the destination stream.
|
||||||
if (nextByte == DLE) {
|
if (nextByte == DLE_CHAR) {
|
||||||
destStream[decodedIndex] = nextByte;
|
destStream[decodedIndex] = nextByte;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
@ -111,7 +111,7 @@ ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream,
|
|||||||
++decodedIndex;
|
++decodedIndex;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (sourceStream[encodedIndex] != ETX) {
|
if (sourceStream[encodedIndex] != ETX_CHAR) {
|
||||||
return DECODING_ERROR;
|
return DECODING_ERROR;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
|
@ -35,12 +35,12 @@ public:
|
|||||||
static constexpr ReturnValue_t DECODING_ERROR = MAKE_RETURN_CODE(0x02);
|
static constexpr ReturnValue_t DECODING_ERROR = MAKE_RETURN_CODE(0x02);
|
||||||
|
|
||||||
//! Start Of Text character. First character is encoded stream
|
//! Start Of Text character. First character is encoded stream
|
||||||
static constexpr uint8_t STX = 0x02;
|
static constexpr uint8_t STX_CHAR = 0x02;
|
||||||
//! End Of Text character. Last character in encoded stream
|
//! End Of Text character. Last character in encoded stream
|
||||||
static constexpr uint8_t ETX = 0x03;
|
static constexpr uint8_t ETX_CHAR = 0x03;
|
||||||
//! Data Link Escape character. Used to escape STX, ETX and DLE occurences
|
//! Data Link Escape character. Used to escape STX, ETX and DLE occurences
|
||||||
//! in the source stream.
|
//! in the source stream.
|
||||||
static constexpr uint8_t DLE = 0x10;
|
static constexpr uint8_t DLE_CHAR = 0x10;
|
||||||
static constexpr uint8_t CARRIAGE_RETURN = 0x0D;
|
static constexpr uint8_t CARRIAGE_RETURN = 0x0D;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -66,6 +66,7 @@ enum {
|
|||||||
HOUSEKEEPING_MANAGER, //HKM 60
|
HOUSEKEEPING_MANAGER, //HKM 60
|
||||||
DLE_ENCODER, //DLEE 61
|
DLE_ENCODER, //DLEE 61
|
||||||
PUS_PARSER, //PUSP 62
|
PUS_PARSER, //PUSP 62
|
||||||
|
SERIAL_ANALYZER, //SERA 63
|
||||||
FW_CLASS_ID_COUNT //is actually count + 1 !
|
FW_CLASS_ID_COUNT //is actually count + 1 !
|
||||||
|
|
||||||
};
|
};
|
||||||
|
@ -1,114 +0,0 @@
|
|||||||
#include "../tmtcservices/PusParser.h"
|
|
||||||
#include "../serviceinterface/ServiceInterfaceStream.h"
|
|
||||||
|
|
||||||
PusParser::PusParser(uint16_t maxExpectedPusPackets,
|
|
||||||
bool storeSplitPackets): indexSizePairFIFO(maxExpectedPusPackets) {
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t PusParser::parsePusPackets(const uint8_t *frame,
|
|
||||||
size_t frameSize) {
|
|
||||||
if(frame == nullptr or frameSize < 5) {
|
|
||||||
sif::error << "PusParser::parsePusPackets: Frame invalid!" << std::endl;
|
|
||||||
return HasReturnvaluesIF::RETURN_FAILED;
|
|
||||||
}
|
|
||||||
|
|
||||||
if(indexSizePairFIFO.full()) {
|
|
||||||
sif::error << "PusParser::parsePusPackets: FIFO is full!" << std::endl;
|
|
||||||
return HasReturnvaluesIF::RETURN_FAILED;
|
|
||||||
}
|
|
||||||
|
|
||||||
size_t lengthField = frame[4] << 8 | frame[5];
|
|
||||||
|
|
||||||
if(lengthField == 0) {
|
|
||||||
return NO_PACKET_FOUND;
|
|
||||||
}
|
|
||||||
|
|
||||||
size_t packetSize = lengthField + 7;
|
|
||||||
// sif::debug << frameSize << std::endl;
|
|
||||||
// Size of a pus packet is the value in the packet length field plus 7.
|
|
||||||
if(packetSize > frameSize) {
|
|
||||||
if(storeSplitPackets) {
|
|
||||||
indexSizePairFIFO.insert(indexSizePair(0, frameSize));
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
sif::debug << "TcSerialPollingTask::readNextPacket: Next packet "
|
|
||||||
"larger than remaining frame," << std::endl;
|
|
||||||
sif::debug << "Throwing away packet. Detected packet size: "
|
|
||||||
<< packetSize << std::endl;
|
|
||||||
}
|
|
||||||
return SPLIT_PACKET;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
indexSizePairFIFO.insert(indexSizePair(0, packetSize));
|
|
||||||
if(packetSize == frameSize) {
|
|
||||||
return HasReturnvaluesIF::RETURN_OK;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// packet size is smaller than frame size, parse for more packets.
|
|
||||||
return readMultiplePackets(frame, frameSize, packetSize);
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t PusParser::readMultiplePackets(const uint8_t *frame,
|
|
||||||
size_t frameSize, size_t startIndex) {
|
|
||||||
while (startIndex < frameSize) {
|
|
||||||
ReturnValue_t result = readNextPacket(frame, frameSize, startIndex);
|
|
||||||
if(result != HasReturnvaluesIF::RETURN_OK) {
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return HasReturnvaluesIF::RETURN_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
DynamicFIFO<PusParser::indexSizePair>* PusParser::fifo(){
|
|
||||||
return &indexSizePairFIFO;
|
|
||||||
}
|
|
||||||
|
|
||||||
PusParser::indexSizePair PusParser::getNextFifoPair() {
|
|
||||||
indexSizePair nextIndexSizePair;
|
|
||||||
indexSizePairFIFO.retrieve(&nextIndexSizePair);
|
|
||||||
return nextIndexSizePair;
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t PusParser::readNextPacket(const uint8_t *frame,
|
|
||||||
size_t frameSize, size_t& currentIndex) {
|
|
||||||
// sif::debug << startIndex << std::endl;
|
|
||||||
if(currentIndex + 5 > frameSize) {
|
|
||||||
currentIndex = frameSize;
|
|
||||||
return HasReturnvaluesIF::RETURN_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t lengthField = frame[currentIndex + 4] << 8 |
|
|
||||||
frame[currentIndex + 5];
|
|
||||||
if(lengthField == 0) {
|
|
||||||
// It is assumed that no packet follows.
|
|
||||||
currentIndex = frameSize;
|
|
||||||
return HasReturnvaluesIF::RETURN_OK;
|
|
||||||
}
|
|
||||||
size_t nextPacketSize = lengthField + 7;
|
|
||||||
size_t remainingSize = frameSize - currentIndex;
|
|
||||||
if(nextPacketSize > remainingSize)
|
|
||||||
{
|
|
||||||
if(storeSplitPackets) {
|
|
||||||
indexSizePairFIFO.insert(indexSizePair(currentIndex, remainingSize));
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
sif::debug << "TcSerialPollingTask::readNextPacket: Next packet "
|
|
||||||
"larger than remaining frame," << std::endl;
|
|
||||||
sif::debug << "Throwing away packet. Detected packet size: "
|
|
||||||
<< nextPacketSize << std::endl;
|
|
||||||
}
|
|
||||||
return SPLIT_PACKET;
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t result = indexSizePairFIFO.insert(indexSizePair(currentIndex,
|
|
||||||
nextPacketSize));
|
|
||||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
||||||
// FIFO full.
|
|
||||||
sif::debug << "PusParser: Issue inserting into start index size "
|
|
||||||
"FIFO, it is full!" << std::endl;
|
|
||||||
}
|
|
||||||
currentIndex += nextPacketSize;
|
|
||||||
|
|
||||||
return result;
|
|
||||||
}
|
|
@ -1,82 +0,0 @@
|
|||||||
#ifndef FRAMEWORK_TMTCSERVICES_PUSPARSER_H_
|
|
||||||
#define FRAMEWORK_TMTCSERVICES_PUSPARSER_H_
|
|
||||||
|
|
||||||
#include "../container/DynamicFIFO.h"
|
|
||||||
#include <utility>
|
|
||||||
#include <cstdint>
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief This small helper class scans a given buffer for PUS packets.
|
|
||||||
* Can be used if PUS packets are serialized in a tightly packed frame.
|
|
||||||
* @details
|
|
||||||
* The parser uses the payload length field of PUS packets to find
|
|
||||||
* the respective PUS packet sizes.
|
|
||||||
*
|
|
||||||
* The parser parses a buffer by taking a pointer and the maximum size to scan.
|
|
||||||
* If PUS packets are found, they are stored in a FIFO which stores pairs
|
|
||||||
* consisting of the index in the buffer and the respective packet sizes.
|
|
||||||
*
|
|
||||||
* If the parser detects split packets (which means that the size of the
|
|
||||||
* next packet is larger than the remaining size to scan), it can either
|
|
||||||
* store that split packet or throw away the packet.
|
|
||||||
*/
|
|
||||||
class PusParser {
|
|
||||||
public:
|
|
||||||
//! The first entry is the index inside the buffer while the second index
|
|
||||||
//! is the size of the PUS packet starting at that index.
|
|
||||||
using indexSizePair = std::pair<size_t, size_t>;
|
|
||||||
|
|
||||||
static constexpr uint8_t INTERFACE_ID = CLASS_ID::PUS_PARSER;
|
|
||||||
static constexpr ReturnValue_t NO_PACKET_FOUND = MAKE_RETURN_CODE(0x00);
|
|
||||||
static constexpr ReturnValue_t SPLIT_PACKET = MAKE_RETURN_CODE(0x01);
|
|
||||||
/**
|
|
||||||
* Parser constructor.
|
|
||||||
* @param maxExpectedPusPackets
|
|
||||||
* Maximum expected number of PUS packets. A good estimate is to divide
|
|
||||||
* the frame size by the minimum size of a PUS packet (12 bytes)
|
|
||||||
* @param storeSplitPackets
|
|
||||||
* Specifies whether split packets are also stored inside the FIFO,
|
|
||||||
* with the size being the remaining frame size.
|
|
||||||
*/
|
|
||||||
PusParser(uint16_t maxExpectedPusPackets, bool storeSplitPackets);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Parse a given frame for PUS packets
|
|
||||||
* @param frame
|
|
||||||
* @param frameSize
|
|
||||||
* @return -@c NO_PACKET_FOUND if no packet was found.
|
|
||||||
*/
|
|
||||||
ReturnValue_t parsePusPackets(const uint8_t* frame, size_t frameSize);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Accessor function to get a reference to the internal FIFO which
|
|
||||||
* stores pairs of indexi and packet sizes. This FIFO is filled
|
|
||||||
* by the parsePusPackets() function.
|
|
||||||
* @return
|
|
||||||
*/
|
|
||||||
DynamicFIFO<indexSizePair>* fifo();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Retrieve the next index and packet size pair from the FIFO.
|
|
||||||
* This also removed it from the FIFO. Please note that if the FIFO
|
|
||||||
* is empty, an empty pair will be returned.
|
|
||||||
* @return
|
|
||||||
*/
|
|
||||||
indexSizePair getNextFifoPair();
|
|
||||||
|
|
||||||
private:
|
|
||||||
/** A FIFO is used to store information about multiple PUS packets
|
|
||||||
* inside the receive buffer. The maximum number of entries is defined
|
|
||||||
* by the first constructor argument.
|
|
||||||
*/
|
|
||||||
DynamicFIFO<indexSizePair> indexSizePairFIFO;
|
|
||||||
|
|
||||||
bool storeSplitPackets = false;
|
|
||||||
|
|
||||||
ReturnValue_t readMultiplePackets(const uint8_t *frame, size_t frameSize,
|
|
||||||
size_t startIndex);
|
|
||||||
ReturnValue_t readNextPacket(const uint8_t *frame,
|
|
||||||
size_t frameSize, size_t& startIndex);
|
|
||||||
};
|
|
||||||
|
|
||||||
#endif /* FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ */
|
|
Loading…
Reference in New Issue
Block a user