diff --git a/src/fsfw/ipc/MessageQueueSenderIF.h b/src/fsfw/ipc/MessageQueueSenderIF.h index a98862f64..83c7223ac 100644 --- a/src/fsfw/ipc/MessageQueueSenderIF.h +++ b/src/fsfw/ipc/MessageQueueSenderIF.h @@ -1,14 +1,14 @@ #ifndef FSFW_IPC_MESSAGEQUEUESENDERIF_H_ #define FSFW_IPC_MESSAGEQUEUESENDERIF_H_ -#include "../objectmanager/ObjectManagerIF.h" #include "MessageQueueIF.h" #include "MessageQueueMessageIF.h" +#include "fsfw/objectmanager/ObjectManagerIF.h" class MessageQueueSenderIF { public: - virtual ~MessageQueueSenderIF() {} - + virtual ~MessageQueueSenderIF() = default; + MessageQueueSenderIF() = delete; /** * Allows sending messages without actually "owning" a message queue. * Not sure whether this is actually a good idea. @@ -16,9 +16,6 @@ class MessageQueueSenderIF { static ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message, MessageQueueId_t sentFrom = MessageQueueIF::NO_QUEUE, bool ignoreFault = false); - - private: - MessageQueueSenderIF() {} }; #endif /* FSFW_IPC_MESSAGEQUEUESENDERIF_H_ */ diff --git a/src/fsfw/pus/Service5EventReporting.cpp b/src/fsfw/pus/Service5EventReporting.cpp index 4d0dcf1d9..d88e58b86 100644 --- a/src/fsfw/pus/Service5EventReporting.cpp +++ b/src/fsfw/pus/Service5EventReporting.cpp @@ -99,4 +99,5 @@ ReturnValue_t Service5EventReporting::initialize() { return result; } initializeTmHelpers(sendHelper, storeHelper); + return result; } diff --git a/src/fsfw/pus/Service9TimeManagement.cpp b/src/fsfw/pus/Service9TimeManagement.cpp index 99b447c13..4abbd5d2e 100644 --- a/src/fsfw/pus/Service9TimeManagement.cpp +++ b/src/fsfw/pus/Service9TimeManagement.cpp @@ -25,9 +25,7 @@ ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) { ReturnValue_t Service9TimeManagement::setTime() { Clock::TimeOfDay_t timeToSet; - size_t userDataLen = 0; - const uint8_t* timeData = currentPacket.getUserData(userDataLen); - TimePacket timePacket(timeData, userDataLen); + TimePacket timePacket(currentPacket.getUserData(), currentPacket.getUserDataLen()); ReturnValue_t result = CCSDSTime::convertFromCcsds(&timeToSet, timePacket.getTime(), timePacket.getTimeSize()); if (result != RETURN_OK) { diff --git a/src/fsfw/returnvalues/FwClassIds.h b/src/fsfw/returnvalues/FwClassIds.h index 50efd9c58..1dfe1cc84 100644 --- a/src/fsfw/returnvalues/FwClassIds.h +++ b/src/fsfw/returnvalues/FwClassIds.h @@ -36,7 +36,8 @@ enum : uint8_t { TRIPLE_REDUNDACY_CHECK, // TRC PACKET_CHECK, // TCC PACKET_DISTRIBUTION, // TCD - ACCEPTS_TELECOMMANDS_IF, // PUS + ACCEPTS_TELECOMMANDS_IF, // ATC + PUS_IF, // PUS DEVICE_SERVICE_BASE, // DSB COMMAND_SERVICE_BASE, // CSB TM_STORE_BACKEND_IF, // TMB diff --git a/src/fsfw/tcdistribution/PusPacketChecker.h b/src/fsfw/tcdistribution/PusPacketChecker.h index fe3413785..431ee5a8f 100644 --- a/src/fsfw/tcdistribution/PusPacketChecker.h +++ b/src/fsfw/tcdistribution/PusPacketChecker.h @@ -5,7 +5,7 @@ #include "CcsdsPacketCheckerBase.h" #include "fsfw/FSFW.h" #include "fsfw/returnvalues/HasReturnvaluesIF.h" -#include "fsfw/tmtcpacket/pus/definitions.h" +#include "fsfw/tmtcpacket/pus/defs.h" #include "fsfw/tmtcservices/PusVerificationReport.h" class PacketCheckIF : public ReadablePacketIF, public PusIF {}; diff --git a/src/fsfw/timemanager/CCSDSTime.cpp b/src/fsfw/timemanager/CCSDSTime.cpp index 1f84dd03d..4d5de9b36 100644 --- a/src/fsfw/timemanager/CCSDSTime.cpp +++ b/src/fsfw/timemanager/CCSDSTime.cpp @@ -504,7 +504,7 @@ ReturnValue_t CCSDSTime::convertFromCDS(timeval* to, const uint8_t* from, size_t } else if ((pField & 0b11) == 0b10) { expectedLength += 4; } - if (foundLength != NULL) { + if (foundLength != nullptr) { *foundLength = expectedLength; } if (expectedLength > maxLength) { diff --git a/src/fsfw/timemanager/CMakeLists.txt b/src/fsfw/timemanager/CMakeLists.txt index c4f773959..7e38cc6ac 100644 --- a/src/fsfw/timemanager/CMakeLists.txt +++ b/src/fsfw/timemanager/CMakeLists.txt @@ -1,3 +1,4 @@ target_sources( - ${LIB_FSFW_NAME} PRIVATE CCSDSTime.cpp Countdown.cpp Stopwatch.cpp - TimeMessage.cpp TimeStamper.cpp ClockCommon.cpp) + ${LIB_FSFW_NAME} + PRIVATE CCSDSTime.cpp Countdown.cpp Stopwatch.cpp TimeMessage.cpp + CdsShortTimeStamper.cpp ClockCommon.cpp) diff --git a/src/fsfw/timemanager/CcsdsTimeStampReader.cpp b/src/fsfw/timemanager/CcsdsTimeStampReader.cpp new file mode 100644 index 000000000..d23d0387a --- /dev/null +++ b/src/fsfw/timemanager/CcsdsTimeStampReader.cpp @@ -0,0 +1,14 @@ +#include "CcsdsTimeStampReader.h" + +#include "CCSDSTime.h" + +ReturnValue_t CcsdsTimestampReader::readTimeStamp(const uint8_t* buffer, uint8_t maxSize) { + ReturnValue_t result = CCSDSTime::convertFromCcsds(&time, buffer, ×tampLen, maxSize); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return result; +} + +timeval& CcsdsTimestampReader::getTime() { return time; } +size_t CcsdsTimestampReader::getTimestampLen() { return timestampLen; } diff --git a/src/fsfw/timemanager/CcsdsTimeStampReader.h b/src/fsfw/timemanager/CcsdsTimeStampReader.h new file mode 100644 index 000000000..f323c87e7 --- /dev/null +++ b/src/fsfw/timemanager/CcsdsTimeStampReader.h @@ -0,0 +1,19 @@ +#ifndef FSFW_TIMEMANAGER_CCSDSTIMESTAMPREADER_H +#define FSFW_TIMEMANAGER_CCSDSTIMESTAMPREADER_H + +#include + +#include "TimeReaderIF.h" + +class CcsdsTimestampReader : public TimeReaderIF { + public: + ReturnValue_t readTimeStamp(const uint8_t* buffer, uint8_t maxSize) override; + size_t getTimestampLen() override; + timeval& getTime() override; + + private: + timeval time{}; + size_t timestampLen = 0; +}; + +#endif // FSFW_TIMEMANAGER_CCSDSTIMESTAMPREADER_H diff --git a/src/fsfw/timemanager/TimeStamper.cpp b/src/fsfw/timemanager/CdsShortTimeStamper.cpp similarity index 99% rename from src/fsfw/timemanager/TimeStamper.cpp rename to src/fsfw/timemanager/CdsShortTimeStamper.cpp index 7096d95fb..af0344e32 100644 --- a/src/fsfw/timemanager/TimeStamper.cpp +++ b/src/fsfw/timemanager/CdsShortTimeStamper.cpp @@ -1,6 +1,7 @@ +#include "fsfw/timemanager/CdsShortTimeStamper.h" + #include -#include "fsfw/timemanager/CdsShortTimeStamper.h" #include "fsfw/timemanager/Clock.h" CdsShortTimeStamper::CdsShortTimeStamper(object_id_t objectId) : SystemObject(objectId) {} @@ -27,9 +28,12 @@ ReturnValue_t CdsShortTimeStamper::serialize(uint8_t **buffer, size_t *size, siz *size += getSerializedSize(); return result; } + size_t CdsShortTimeStamper::getSerializedSize() const { return getTimestampSize(); } + ReturnValue_t CdsShortTimeStamper::deSerialize(const uint8_t **buffer, size_t *size, SerializeIF::Endianness streamEndianness) { return HasReturnvaluesIF::RETURN_FAILED; } + size_t CdsShortTimeStamper::getTimestampSize() const { return TIMESTAMP_LEN; } diff --git a/src/fsfw/timemanager/CdsShortTimeStamper.h b/src/fsfw/timemanager/CdsShortTimeStamper.h index 1e9abfa85..d392c1a68 100644 --- a/src/fsfw/timemanager/CdsShortTimeStamper.h +++ b/src/fsfw/timemanager/CdsShortTimeStamper.h @@ -1,9 +1,9 @@ #ifndef FSFW_TIMEMANAGER_TIMESTAMPER_H_ #define FSFW_TIMEMANAGER_TIMESTAMPER_H_ -#include "../objectmanager/SystemObject.h" #include "CCSDSTime.h" #include "TimeStamperIF.h" +#include "fsfw/objectmanager/SystemObject.h" /** * @brief Time stamper which can be used to add any timestamp to a @@ -34,10 +34,10 @@ class CdsShortTimeStamper : public TimeStamperIF, public SystemObject { ReturnValue_t addTimeStamp(uint8_t *buffer, uint8_t maxSize) override; ReturnValue_t serialize(uint8_t **buffer, size_t *size, size_t maxSize, Endianness streamEndianness) const override; - size_t getSerializedSize() const override; + [[nodiscard]] size_t getSerializedSize() const override; ReturnValue_t deSerialize(const uint8_t **buffer, size_t *size, Endianness streamEndianness) override; - size_t getTimestampSize() const override; + [[nodiscard]] size_t getTimestampSize() const override; }; #endif /* FSFW_TIMEMANAGER_TIMESTAMPER_H_ */ diff --git a/src/fsfw/timemanager/TimeReaderIF.h b/src/fsfw/timemanager/TimeReaderIF.h new file mode 100644 index 000000000..388f7d71c --- /dev/null +++ b/src/fsfw/timemanager/TimeReaderIF.h @@ -0,0 +1,19 @@ +#ifndef FSFW_TIMEMANAGER_TIMEREADERIF_H +#define FSFW_TIMEMANAGER_TIMEREADERIF_H + +#include + +#include "fsfw/returnvalues/HasReturnvaluesIF.h" + +class TimeReaderIF { + public: + virtual ~TimeReaderIF() = default; + virtual ReturnValue_t readTimeStamp(const uint8_t* buffer, uint8_t maxSize) = 0; + // Would be nice to have this, but the clock backend needs to be redesigned + // virtual ReturnValue_t readTimestampLen(const uint8_t* buffer, uint8_t maxSize, size_t& + // timestampLen) = 0; + virtual size_t getTimestampLen() = 0; + virtual timeval& getTime() = 0; +}; + +#endif // FSFW_TIMEMANAGER_TIMEREADERIF_H diff --git a/src/fsfw/timemanager/TimeStamperIF.h b/src/fsfw/timemanager/TimeStamperIF.h index 404dd10aa..2f0e80218 100644 --- a/src/fsfw/timemanager/TimeStamperIF.h +++ b/src/fsfw/timemanager/TimeStamperIF.h @@ -24,6 +24,7 @@ class TimeStamperIF : public SerializeIF { [[nodiscard]] virtual size_t getTimestampSize() const = 0; virtual ReturnValue_t addTimeStamp(uint8_t* buffer, uint8_t maxSize) = 0; + ~TimeStamperIF() override = default; }; diff --git a/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.cpp b/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.cpp index a0ad53282..c528d0a7d 100644 --- a/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.cpp +++ b/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.cpp @@ -1,7 +1,5 @@ #include "SpacePacketReader.h" -#include - #include "fsfw/serialize/SerializeIF.h" #include "fsfw/serviceinterface/ServiceInterface.h" @@ -9,8 +7,8 @@ SpacePacketReader::SpacePacketReader(const uint8_t* setAddress, size_t maxSize_) setInternalFields(setAddress, maxSize_); } -ReturnValue_t SpacePacketReader::checkLength() const { - if (getFullPacketLen() > maxSize) { +ReturnValue_t SpacePacketReader::checkSize() const { + if (getFullPacketLen() > bufSize) { return SerializeIF::STREAM_TOO_SHORT; } return HasReturnvaluesIF::RETURN_OK; @@ -22,9 +20,8 @@ inline uint16_t SpacePacketReader::getPacketIdRaw() const { return ccsds::getPac const uint8_t* SpacePacketReader::getPacketData() { return packetDataField; } -ReturnValue_t SpacePacketReader::setData(uint8_t* pData, size_t maxSize_, void* args) { - setInternalFields(pData, maxSize_); - return HasReturnvaluesIF::RETURN_OK; +ReturnValue_t SpacePacketReader::setData(uint8_t* data, size_t maxSize_, void* args) { + return setInternalFields(data, maxSize_); } uint16_t SpacePacketReader::getPacketSeqCtrlRaw() const { @@ -32,11 +29,21 @@ uint16_t SpacePacketReader::getPacketSeqCtrlRaw() const { } uint16_t SpacePacketReader::getPacketDataLen() const { return ccsds::getPacketLen(*spHeader); } -void SpacePacketReader::setInternalFields(const uint8_t* data, size_t maxSize_) { - maxSize = maxSize_; + +ReturnValue_t SpacePacketReader::setInternalFields(const uint8_t* data, size_t maxSize_) { + bufSize = maxSize_; spHeader = reinterpret_cast(data); packetDataField = data + ccsds::HEADER_LEN; + return checkSize(); } + const uint8_t* SpacePacketReader::getFullData() { return reinterpret_cast(spHeader); -}; +} +size_t SpacePacketReader::getBufSize() const { return bufSize; } + +bool SpacePacketReader::isNull() const { return spHeader == nullptr; } + +ReturnValue_t SpacePacketReader::setReadOnlyData(const uint8_t* data, size_t maxSize) { + return setData(const_cast(data), maxSize, nullptr); +} diff --git a/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.h b/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.h index 431191464..b35b813b0 100644 --- a/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.h +++ b/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.h @@ -22,17 +22,21 @@ * This class is the basic data handler for any CCSDS Space Packet * compatible Telecommand and Telemetry packet. * It does not contain the packet data itself but a pointer to the - * data must be set on instantiation. An invalid pointer may cause - * damage, as no getter method checks data validity. Anyway, a NULL - * check can be performed by making use of the getWholeData method. - * Remark: All bit numbers in this documentation are counted from - * the most significant bit (from left). + * data must be set on instantiation or with the @setData or @setReadOnlyData call. + * The @isNull and @checkSize methods can be used to check the validity of the data pointed to. + * + * This is a zero-copy reader class. It does not contain the packet data itself but a pointer to + * the data. Calling any accessor methods without pointing the object to valid data first will + * cause undefined behaviour. * @ingroup tmtcpackets */ class SpacePacketReader : public SpacePacketIF, public ReadablePacketIF, public RedirectableDataPointerIF { public: + /** + * Initialize an empty space packet reader which points to no data + */ SpacePacketReader() = default; /** * This is the default constructor. @@ -45,6 +49,24 @@ class SpacePacketReader : public SpacePacketIF, */ ~SpacePacketReader() override; + /** + * Check whether any data is set for the reader object + * @return + */ + [[nodiscard]] bool isNull() const; + /** + * Get size of the buffer. This is the size which is passed to the constructor or to the + * @setData call. It is not the content of the CCSDS data length field and it is not necessarily + * equal to the full packet length of the space packet. + * @return + */ + [[nodiscard]] size_t getBufSize() const; + + /** + * CCSDS header always has 6 bytes + * @return + */ + static constexpr size_t getHeaderLen() { return ccsds::HEADER_LEN; } [[nodiscard]] uint16_t getPacketIdRaw() const override; [[nodiscard]] uint16_t getPacketSeqCtrlRaw() const override; [[nodiscard]] uint16_t getPacketDataLen() const override; @@ -52,16 +74,11 @@ class SpacePacketReader : public SpacePacketIF, const uint8_t* getFullData() override; // Helper methods: - [[nodiscard]] ReturnValue_t checkLength() const; + [[nodiscard]] ReturnValue_t checkSize() const; const uint8_t* getPacketData(); - /** - * With this method, the packet data pointer can be redirected to another - * location. - * @param p_Data A pointer to another raw Space Packet. - */ - ReturnValue_t setData(uint8_t* p_Data, size_t maxSize, void* args) override; + ReturnValue_t setReadOnlyData(const uint8_t* data, size_t maxSize); protected: /** @@ -71,9 +88,15 @@ class SpacePacketReader : public SpacePacketIF, */ const ccsds::PrimaryHeader* spHeader{}; const uint8_t* packetDataField{}; - size_t maxSize = 0; + size_t bufSize = 0; + /** + * With this method, the packet data pointer can be redirected to another + * location. + * @param data A pointer to another raw Space Packet. + */ + ReturnValue_t setData(uint8_t* data, size_t maxSize, void* args) override; - void setInternalFields(const uint8_t* data, size_t maxSize); + ReturnValue_t setInternalFields(const uint8_t* data, size_t maxSize); }; #endif /* FSFW_TMTCPACKET_SPACEPACKETBASE_H_ */ diff --git a/src/fsfw/tmtcpacket/pus/CreatorDataIF.h b/src/fsfw/tmtcpacket/pus/CreatorDataIF.h index 7d5d1d18e..f66ca5678 100644 --- a/src/fsfw/tmtcpacket/pus/CreatorDataIF.h +++ b/src/fsfw/tmtcpacket/pus/CreatorDataIF.h @@ -1,7 +1,7 @@ #ifndef FSFW_TMTCPACKET_CREATORDATAIF_H #define FSFW_TMTCPACKET_CREATORDATAIF_H -#include "definitions.h" +#include "defs.h" class CreatorDataIF { public: diff --git a/src/fsfw/tmtcpacket/pus/PusIF.h b/src/fsfw/tmtcpacket/pus/PusIF.h index eb0e35f86..8a15f01e9 100644 --- a/src/fsfw/tmtcpacket/pus/PusIF.h +++ b/src/fsfw/tmtcpacket/pus/PusIF.h @@ -3,10 +3,15 @@ #include +#include "fsfw/returnvalues/FwClassIds.h" #include "fsfw/tmtcpacket/ccsds/SpacePacketIF.h" class PusIF : public SpacePacketIF { public: + static constexpr uint8_t INTERFACE_ID = CLASS_ID::PUS_IF; + static constexpr ReturnValue_t INVALID_CRC_16 = + HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 0); + ~PusIF() override = default; /** * This command returns the TC Packet PUS Version Number. diff --git a/src/fsfw/tmtcpacket/pus/RawUserDataReaderIF.h b/src/fsfw/tmtcpacket/pus/RawUserDataReaderIF.h index 170ac824c..e4e515f8c 100644 --- a/src/fsfw/tmtcpacket/pus/RawUserDataReaderIF.h +++ b/src/fsfw/tmtcpacket/pus/RawUserDataReaderIF.h @@ -6,7 +6,8 @@ class RawUserDataReaderIF { public: - virtual const uint8_t* getUserData(size_t& userDataLen) = 0; + virtual const uint8_t* getUserData() const = 0; + virtual size_t getUserDataLen() const = 0; }; #endif // FSFW_TMTCPACKET_RAWDATAIF_H diff --git a/src/fsfw/tmtcpacket/pus/definitions.h b/src/fsfw/tmtcpacket/pus/defs.h similarity index 74% rename from src/fsfw/tmtcpacket/pus/definitions.h rename to src/fsfw/tmtcpacket/pus/defs.h index 6a5c10eb3..027ff37ce 100644 --- a/src/fsfw/tmtcpacket/pus/definitions.h +++ b/src/fsfw/tmtcpacket/pus/defs.h @@ -7,6 +7,8 @@ namespace ecss { +using PusChecksumT = uint16_t; + //! Version numbers according to ECSS-E-ST-70-41C p.439 enum PusVersion : uint8_t { PUS_A = 1, PUS_C = 2 }; @@ -36,6 +38,18 @@ struct DataWrapper { } }; +/** + * This struct defines the data structure of a Space Packet when accessed + * via a pointer. + * @ingroup tmtcpackets + */ +struct PusPointers { + const uint8_t* spHeaderStart; + const uint8_t* secHeaderStart; + const uint8_t* userDataStart; + const uint8_t* crcStart; +}; + } // namespace ecss #endif /* FSFW_SRC_FSFW_TMTCPACKET_PUS_TM_DEFINITIONS_H_ */ diff --git a/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp b/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp index 8f8cc8e62..bd824d24a 100644 --- a/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp +++ b/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp @@ -14,7 +14,7 @@ PusTcCreator::PusTcCreator(SpacePacketParams spParams, PusTcParams pusParams) ReturnValue_t PusTcCreator::serialize(uint8_t **buffer, size_t *size, size_t maxSize, SerializeIF::Endianness streamEndianness) const { size_t userDataLen = pusParams.dataWrapper.getLength(); - if (*size + PusTcIF::MIN_LEN + userDataLen > maxSize) { + if (*size + PusTcIF::MIN_SIZE + userDataLen > maxSize) { return SerializeIF::BUFFER_TOO_SHORT; } ReturnValue_t result = spCreator.serialize(buffer, size, maxSize, streamEndianness); @@ -57,7 +57,8 @@ ReturnValue_t PusTcCreator::serialize(uint8_t **buffer, size_t *size, size_t max } void PusTcCreator::updateSpLengthField() { - spCreator.setDataLen(ecss::PusTcDataFieldHeader::MIN_LEN + pusParams.dataWrapper.getLength() + 1); + spCreator.setDataLen(ecss::PusTcDataFieldHeader::MIN_SIZE + pusParams.dataWrapper.getLength() + + 1); } size_t PusTcCreator::getSerializedSize() const { return spCreator.getFullPacketLen(); } @@ -82,4 +83,9 @@ uint8_t PusTcCreator::getService() const { return pusParams.service; } uint8_t PusTcCreator::getSubService() const { return pusParams.subservice; } uint16_t PusTcCreator::getSourceId() const { return pusParams.sourceId; } + ecss::DataWrapper &PusTcCreator::getDataWrapper() { return pusParams.dataWrapper; } + +PusTcParams &PusTcCreator::getPusParams() { return pusParams; } + +SpacePacketParams &PusTcCreator::getSpParams() { return spCreator.getParams(); } diff --git a/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.h b/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.h index 956a2505b..1f6e8acb7 100644 --- a/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.h +++ b/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.h @@ -5,7 +5,7 @@ #include "fsfw/tmtcpacket/ccsds/SpacePacketCreator.h" #include "fsfw/tmtcpacket/ccsds/SpacePacketIF.h" #include "fsfw/tmtcpacket/pus/CreatorDataIF.h" -#include "fsfw/tmtcpacket/pus/definitions.h" +#include "fsfw/tmtcpacket/pus/defs.h" #include "fsfw/tmtcpacket/pus/tc/PusTcIF.h" struct PusTcParams { @@ -24,6 +24,8 @@ class PusTcCreator : public PusTcIF, public SerializeIF, public CreatorDataIF { PusTcCreator(SpacePacketParams spParams, PusTcParams pusParams); void updateSpLengthField(); + PusTcParams &getPusParams(); + SpacePacketParams &getSpParams(); ReturnValue_t serialize(uint8_t **buffer, size_t *size, size_t maxSize, Endianness streamEndianness) const override; [[nodiscard]] size_t getSerializedSize() const override; diff --git a/src/fsfw/tmtcpacket/pus/tc/PusTcIF.h b/src/fsfw/tmtcpacket/pus/tc/PusTcIF.h index 61b29660a..43e9aab0e 100644 --- a/src/fsfw/tmtcpacket/pus/tc/PusTcIF.h +++ b/src/fsfw/tmtcpacket/pus/tc/PusTcIF.h @@ -31,7 +31,7 @@ static constexpr uint8_t ACK_ALL = ACK_ACCEPTANCE | ACK_START | ACK_STEP | ACK_C */ struct PusTcDataFieldHeader { // Version and ACK byte, Service Byte, Subservice Byte, 2 byte Source ID - static constexpr size_t MIN_LEN = 5; + static constexpr size_t MIN_SIZE = 5; uint8_t pusVersion; uint8_t ackFlags; uint8_t serviceType; @@ -44,8 +44,8 @@ struct PusTcDataFieldHeader { class PusTcIF : public PusIF { public: ~PusTcIF() override = default; - static const size_t MIN_LEN = - (sizeof(ccsds::PrimaryHeader) + ecss::PusTcDataFieldHeader::MIN_LEN + 2); + static const size_t MIN_SIZE = + (sizeof(ccsds::PrimaryHeader) + ecss::PusTcDataFieldHeader::MIN_SIZE + 2); /** * This is a getter for the packet's Ack field, which are the lowest four diff --git a/src/fsfw/tmtcpacket/pus/tc/PusTcReader.cpp b/src/fsfw/tmtcpacket/pus/tc/PusTcReader.cpp index bf4ec1812..b9e5e00f1 100644 --- a/src/fsfw/tmtcpacket/pus/tc/PusTcReader.cpp +++ b/src/fsfw/tmtcpacket/pus/tc/PusTcReader.cpp @@ -12,20 +12,30 @@ PusTcReader::PusTcReader(const uint8_t* data, size_t size) { setReadOnlyData(dat PusTcReader::~PusTcReader() = default; ReturnValue_t PusTcReader::parseData() { - ReturnValue_t result = spReader.checkLength(); + if (pointers.spHeaderStart == nullptr or spReader.isNull()) { + return HasReturnvaluesIF::RETURN_FAILED; + } + ReturnValue_t result = spReader.checkSize(); if (result != HasReturnvaluesIF::RETURN_OK) { return result; } - if (size < PusTcIF::MIN_LEN) { + if (spReader.getBufSize() < PusTcIF::MIN_SIZE) { return SerializeIF::STREAM_TOO_SHORT; } + + size_t currentOffset = SpacePacketReader::getHeaderLen(); + pointers.secHeaderStart = pointers.spHeaderStart + currentOffset; // Might become variable sized field in the future - size_t secHeaderLen = ecss::PusTcDataFieldHeader::MIN_LEN; - pointers.secHeaderStart = pointers.spHeaderStart + ccsds::HEADER_LEN; // TODO: No support for spare bytes yet - pointers.userDataStart = pointers.secHeaderStart + secHeaderLen; - appDataSize = size - (ccsds::HEADER_LEN + secHeaderLen); + currentOffset += ecss::PusTcDataFieldHeader::MIN_SIZE; + pointers.userDataStart = pointers.spHeaderStart + currentOffset; + appDataSize = spReader.getFullPacketLen() - currentOffset - sizeof(ecss::PusChecksumT); pointers.crcStart = pointers.userDataStart + appDataSize; + uint16_t crc16 = CRC::crc16ccitt(spReader.getFullData(), getFullPacketLen()); + if (crc16 != 0) { + // Checksum failure + return PusIF::INVALID_CRC_16; + } return HasReturnvaluesIF::RETURN_OK; } @@ -51,21 +61,16 @@ uint8_t PusTcReader::getPusVersion() const { return spReader.getVersion(); } const uint8_t* PusTcReader::getFullData() { return pointers.spHeaderStart; } ReturnValue_t PusTcReader::setData(uint8_t* pData, size_t size_, void* args) { - size = size_; pointers.spHeaderStart = pData; - spReader.setData(pData, size_, args); - return HasReturnvaluesIF::RETURN_OK; + return spReader.setReadOnlyData(pData, size_); } ReturnValue_t PusTcReader::setReadOnlyData(const uint8_t* data, size_t size_) { - setData(const_cast(data), size_, nullptr); - return HasReturnvaluesIF::RETURN_OK; + return setData(const_cast(data), size_, nullptr); } -const uint8_t* PusTcReader::getUserData(size_t& userDataLen) { - userDataLen = appDataSize; - return pointers.userDataStart; -} +const uint8_t* PusTcReader::getUserData() const { return pointers.userDataStart; } +size_t PusTcReader::getUserDataLen() const { return appDataSize; } /* void PusTcReader::print() { diff --git a/src/fsfw/tmtcpacket/pus/tc/PusTcReader.h b/src/fsfw/tmtcpacket/pus/tc/PusTcReader.h index 0cf184965..e79f8b61d 100644 --- a/src/fsfw/tmtcpacket/pus/tc/PusTcReader.h +++ b/src/fsfw/tmtcpacket/pus/tc/PusTcReader.h @@ -7,16 +7,17 @@ #include "fsfw/tmtcpacket/RedirectableDataPointerIF.h" #include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h" #include "fsfw/tmtcpacket/pus/RawUserDataReaderIF.h" +#include "fsfw/tmtcpacket/pus/defs.h" /** - * This class is the basic data handler for any ECSS PUS Telecommand packet. + * This class is the basic reader class to read ECSS PUS C Telecommand packets. * - * In addition to #SpacePacketBase, the class provides methods to handle - * the standardized entries of the PUS TC Packet Data Field Header. - * It does not contain the packet data itself but a pointer to the - * data must be set on instantiation. An invalid pointer may cause - * damage, as no getter method checks data validity. Anyway, a NULL - * check can be performed by making use of the getWholeData method. + * 1. Implements the @SpacePacketIF to provide accessor methods for the contained space packet. + * 2. Implements the @PusTcIF to provide accessor methods for generic PUS C fields + * + * This is a zero-copy reader class. It does not contain the packet data itself but a pointer to + * the data. Calling any accessor methods without pointing the object to valid data first will + * cause undefined behaviour. * @ingroup tmtcpackets */ class PusTcReader : public PusTcIF, @@ -56,7 +57,8 @@ class PusTcReader : public PusTcIF, const uint8_t* getFullData() override; ReturnValue_t setReadOnlyData(const uint8_t* data, size_t size); - const uint8_t* getUserData(size_t& userDataLen) override; + [[nodiscard]] const uint8_t* getUserData() const override; + size_t getUserDataLen() const override; protected: /** @@ -70,20 +72,7 @@ class PusTcReader : public PusTcIF, ReturnValue_t setData(uint8_t* pData, size_t size, void* args) override; SpacePacketReader spReader; - /** - * This struct defines the data structure of a Space Packet when accessed - * via a pointer. - * @ingroup tmtcpackets - */ - struct PusTcPointers { - const uint8_t* spHeaderStart; - const uint8_t* secHeaderStart; - const uint8_t* userDataStart; - const uint8_t* crcStart; - }; - - PusTcPointers pointers{}; - size_t size = 0; + ecss::PusPointers pointers{}; size_t appDataSize = 0; }; diff --git a/src/fsfw/tmtcpacket/pus/tc/TcPacketPus.h b/src/fsfw/tmtcpacket/pus/tc/TcPacketPus.h index fe9766733..8af6125ac 100644 --- a/src/fsfw/tmtcpacket/pus/tc/TcPacketPus.h +++ b/src/fsfw/tmtcpacket/pus/tc/TcPacketPus.h @@ -3,7 +3,7 @@ #include -#include "../definitions.h" +#include "../defs.h" #include "PusTcReader.h" #include "fsfw/FSFW.h" #include "fsfw/tmtcpacket/ccsds/defs.h" diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp b/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp index e80eddd1a..927ec0723 100644 --- a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp +++ b/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp @@ -7,20 +7,20 @@ PusTmCreator::PusTmCreator(SpacePacketParams initSpParams, PusTmParams initPusParams, TimeStamperIF* timeStamper) - : pusParams(initPusParams), spCreator(std::move(initSpParams)){}; + : pusParams(initPusParams), spCreator(std::move(initSpParams)) {} PusTmCreator::PusTmCreator(TimeStamperIF* timeStamper_) { pusParams.secHeader.timeStamper = timeStamper_; -}; +} PusTmCreator::PusTmCreator() = default; -uint16_t PusTmCreator::getPacketIdRaw() const { return 0; } -uint16_t PusTmCreator::getPacketSeqCtrlRaw() const { return 0; } -uint16_t PusTmCreator::getPacketDataLen() const { return 0; } -uint8_t PusTmCreator::getPusVersion() const { return 0; } -uint8_t PusTmCreator::getService() const { return 0; } -uint8_t PusTmCreator::getSubService() const { return 0; } +uint16_t PusTmCreator::getPacketIdRaw() const { return spCreator.getPacketIdRaw(); } +uint16_t PusTmCreator::getPacketSeqCtrlRaw() const { return spCreator.getPacketSeqCtrlRaw(); } +uint16_t PusTmCreator::getPacketDataLen() const { return spCreator.getPacketDataLen(); } +uint8_t PusTmCreator::getPusVersion() const { return pusParams.secHeader.pusVersion; } +uint8_t PusTmCreator::getService() const { return pusParams.secHeader.service; } +uint8_t PusTmCreator::getSubService() const { return pusParams.secHeader.subservice; } PusTmParams& PusTmCreator::getParams() { return pusParams; } void PusTmCreator::setTimeStamper(TimeStamperIF* timeStamper_) { pusParams.secHeader.timeStamper = timeStamper_; @@ -89,11 +89,11 @@ TimeStamperIF* PusTmCreator::getTimestamper() { return pusParams.secHeader.timeS SpacePacketParams& PusTmCreator::getSpParams() { return spCreator.getParams(); } void PusTmCreator::updateSpLengthField() { - size_t headerLen = PusTmIF::MIN_TM_SIZE; + size_t headerLen = PusTmIF::MIN_SIZE; if (pusParams.secHeader.timeStamper != nullptr) { headerLen += pusParams.secHeader.timeStamper->getSerializedSize(); } spCreator.setDataLen(headerLen + pusParams.dataWrapper.getLength() + 1); } -void PusTmCreator::setApid(uint16_t apid) { spCreator.setApid(apid); }; +void PusTmCreator::setApid(uint16_t apid) { spCreator.setApid(apid); } diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.h b/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.h index 5269bd0f7..9ac79d160 100644 --- a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.h +++ b/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.h @@ -50,7 +50,7 @@ class PusTmCreator : public SerializeIF, public PusTmIF, public CreatorDataIF { [[nodiscard]] size_t getSerializedSize() const override; ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, Endianness streamEndianness) override; - TimeStamperIF* getTimestamper() override; + TimeStamperIF* getTimestamper(); private: ecss::DataWrapper& getDataWrapper() override; diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmIF.h b/src/fsfw/tmtcpacket/pus/tm/PusTmIF.h index 94d41467b..356cf20b4 100644 --- a/src/fsfw/tmtcpacket/pus/tm/PusTmIF.h +++ b/src/fsfw/tmtcpacket/pus/tm/PusTmIF.h @@ -6,7 +6,7 @@ #include "fsfw/timemanager/TimeStamperIF.h" #include "fsfw/tmtcpacket/pus/PusIF.h" -#include "fsfw/tmtcpacket/pus/definitions.h" +#include "fsfw/tmtcpacket/pus/defs.h" class PusTmIF : public PusIF { public: @@ -19,12 +19,11 @@ class PusTmIF : public PusIF { /** * 2 bytes for CRC16 */ - static constexpr size_t MIN_TM_SIZE = sizeof(ccsds::PrimaryHeader) + MIN_SEC_HEADER_LEN + 2; + static constexpr size_t MIN_SIZE = sizeof(ccsds::PrimaryHeader) + MIN_SEC_HEADER_LEN + 2; virtual uint8_t getScTimeRefStatus() = 0; virtual uint16_t getMessageTypeCounter() = 0; virtual uint16_t getDestId() = 0; - virtual TimeStamperIF* getTimestamper() = 0; }; #endif // FSFW_TMTCPACKET_PUSTMIF_H diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmMinimal.cpp b/src/fsfw/tmtcpacket/pus/tm/PusTmMinimal.cpp index f36cfc2a8..a389551c7 100644 --- a/src/fsfw/tmtcpacket/pus/tm/PusTmMinimal.cpp +++ b/src/fsfw/tmtcpacket/pus/tm/PusTmMinimal.cpp @@ -42,8 +42,8 @@ uint16_t PusTmMinimal::getDestId() { return 0; } void PusTmMinimal::setApid(uint16_t apid) { /* TODO: Implement. Maybe provide low level function to do this */ } -const uint8_t* PusTmMinimal::getUserData(size_t& userDataLen_) { - userDataLen_ = userDataLen; +size_t PusTmMinimal::getUserDataLen() const { return userDataLen; } + +const uint8_t* PusTmMinimal::getUserData() const { return reinterpret_cast(&tmData->rest); } -TimeStamperIF* PusTmMinimal::getTimestamper() { return nullptr; } diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmMinimal.h b/src/fsfw/tmtcpacket/pus/tm/PusTmMinimal.h index 06ed4aff4..7bfae4f26 100644 --- a/src/fsfw/tmtcpacket/pus/tm/PusTmMinimal.h +++ b/src/fsfw/tmtcpacket/pus/tm/PusTmMinimal.h @@ -73,8 +73,8 @@ class PusTmMinimal : public PusTmIF, public RawUserDataReaderIF, public Redirect uint8_t getScTimeRefStatus() override; uint16_t getMessageTypeCounter() override; uint16_t getDestId() override; - const uint8_t* getUserData(size_t& userDataLen) override; - TimeStamperIF* getTimestamper() override; + const uint8_t* getUserData() const override; + size_t getUserDataLen() const override; protected: /** diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmReader.cpp b/src/fsfw/tmtcpacket/pus/tm/PusTmReader.cpp index d010512ad..cb30de053 100644 --- a/src/fsfw/tmtcpacket/pus/tm/PusTmReader.cpp +++ b/src/fsfw/tmtcpacket/pus/tm/PusTmReader.cpp @@ -1,3 +1,83 @@ -// -// Created by rmueller on 7/19/22. -// +#include "PusTmReader.h" + +#include "fsfw/globalfunctions/CRC.h" + +PusTmReader::PusTmReader(TimeReaderIF *timeReader) : timeReader(timeReader) {} + +PusTmReader::PusTmReader(const uint8_t *data, size_t size) { setReadOnlyData(data, size); } + +PusTmReader::PusTmReader(TimeReaderIF *timeReader, const uint8_t *data, size_t size) + : PusTmReader(timeReader) { + setReadOnlyData(data, size); +} + +ReturnValue_t PusTmReader::parseData() { + // Time reader is required to read the time stamp length at run-time + if (pointers.spHeaderStart == nullptr or spReader.isNull() or timeReader == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + ReturnValue_t result = spReader.checkSize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + if (spReader.getBufSize() < PusTmIF::MIN_SIZE) { + return SerializeIF::STREAM_TOO_SHORT; + } + + size_t currentOffset = SpacePacketReader::getHeaderLen(); + pointers.secHeaderStart = pointers.spHeaderStart + currentOffset; + currentOffset += PusTmIF::MIN_SEC_HEADER_LEN; + size_t minTimestampLen = spReader.getBufSize() - currentOffset; + result = timeReader->readTimeStamp(pointers.secHeaderStart + PusTmIF::MIN_SEC_HEADER_LEN, + minTimestampLen); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + size_t timestampLen = timeReader->getTimestampLen(); + if (currentOffset + timestampLen > spReader.getBufSize()) { + return SerializeIF::STREAM_TOO_SHORT; + } + currentOffset += timestampLen; + pointers.userDataStart = pointers.spHeaderStart + currentOffset; + sourceDataLen = spReader.getFullPacketLen() - currentOffset - sizeof(ecss::PusChecksumT); + currentOffset += sourceDataLen; + pointers.crcStart = pointers.spHeaderStart + currentOffset; + uint16_t crc16 = CRC::crc16ccitt(spReader.getFullData(), getFullPacketLen()); + if (crc16 != 0) { + // Checksum failure + return PusIF::INVALID_CRC_16; + } + return HasReturnvaluesIF::RETURN_OK; +} +const uint8_t *PusTmReader::getFullData() { return spReader.getFullData(); } + +ReturnValue_t PusTmReader::setReadOnlyData(const uint8_t *data, size_t size) { + return setData(const_cast(data), size, nullptr); +} + +ReturnValue_t PusTmReader::setData(uint8_t *dataPtr, size_t size, void *args) { + pointers.spHeaderStart = dataPtr; + return spReader.setReadOnlyData(dataPtr, size); +} + +uint16_t PusTmReader::getPacketIdRaw() const { return spReader.getPacketIdRaw(); } +uint16_t PusTmReader::getPacketSeqCtrlRaw() const { return spReader.getPacketSeqCtrlRaw(); } +uint16_t PusTmReader::getPacketDataLen() const { return spReader.getPacketDataLen(); } +uint8_t PusTmReader::getPusVersion() const { return (pointers.secHeaderStart[0] >> 4) & 0b1111; } +uint8_t PusTmReader::getScTimeRefStatus() { return pointers.secHeaderStart[0] & 0b1111; } +uint8_t PusTmReader::getService() const { return pointers.secHeaderStart[1]; } +uint8_t PusTmReader::getSubService() const { return pointers.secHeaderStart[2]; } +const uint8_t *PusTmReader::getUserData() const { return pointers.userDataStart; } +size_t PusTmReader::getUserDataLen() const { return sourceDataLen; } + +uint16_t PusTmReader::getMessageTypeCounter() { + return (pointers.secHeaderStart[3] << 8) | pointers.secHeaderStart[4]; +} + +uint16_t PusTmReader::getDestId() { + return (pointers.secHeaderStart[5] << 8) | pointers.secHeaderStart[6]; +} + +void PusTmReader::setTimeReader(TimeReaderIF *timeReader_) { timeReader = timeReader_; } + +TimeReaderIF *PusTmReader::getTimeReader() { return timeReader; } diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmReader.h b/src/fsfw/tmtcpacket/pus/tm/PusTmReader.h index 47eff2936..6eda4e517 100644 --- a/src/fsfw/tmtcpacket/pus/tm/PusTmReader.h +++ b/src/fsfw/tmtcpacket/pus/tm/PusTmReader.h @@ -1,7 +1,47 @@ #ifndef FSFW_TMTCPACKET_PUSTMREADER_H #define FSFW_TMTCPACKET_PUSTMREADER_H +#include "fsfw/timemanager/TimeReaderIF.h" +#include "fsfw/tmtcpacket/ReadablePacketIF.h" +#include "fsfw/tmtcpacket/RedirectableDataPointerIF.h" +#include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h" +#include "fsfw/tmtcpacket/pus/RawUserDataReaderIF.h" #include "fsfw/tmtcpacket/pus/tm/PusTmIF.h" -class PusTmReader : public PusTmIF {}; +class PusTmReader : public PusTmIF, + public RawUserDataReaderIF, + public ReadablePacketIF, + public RedirectableDataPointerIF { + public: + explicit PusTmReader(TimeReaderIF* timeReader); + PusTmReader(const uint8_t* data, size_t size); + PusTmReader(TimeReaderIF* timeReader, const uint8_t* data, size_t size); + + ReturnValue_t parseData(); + const uint8_t* getFullData() override; + + void setTimeReader(TimeReaderIF* timeReader); + TimeReaderIF* getTimeReader(); + ReturnValue_t setReadOnlyData(const uint8_t* data, size_t size); + [[nodiscard]] uint16_t getPacketIdRaw() const override; + [[nodiscard]] uint16_t getPacketSeqCtrlRaw() const override; + [[nodiscard]] uint16_t getPacketDataLen() const override; + [[nodiscard]] uint8_t getPusVersion() const override; + [[nodiscard]] uint8_t getService() const override; + [[nodiscard]] uint8_t getSubService() const override; + const uint8_t* getUserData() const override; + size_t getUserDataLen() const override; + uint8_t getScTimeRefStatus() override; + uint16_t getMessageTypeCounter() override; + uint16_t getDestId() override; + + private: + ReturnValue_t setData(uint8_t* dataPtr, size_t size, void* args) override; + + SpacePacketReader spReader{}; + size_t sourceDataLen = 0; + TimeReaderIF* timeReader{}; + ecss::PusPointers pointers{}; +}; + #endif // FSFW_TMTCPACKET_PUSTMREADER_H diff --git a/src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.cpp b/src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.cpp index 7ae6de82d..5c35c5402 100644 --- a/src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.cpp +++ b/src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.cpp @@ -2,7 +2,7 @@ #include -#include "../definitions.h" +#include "../defs.h" #include "TmPacketBase.h" #include "fsfw/globalfunctions/CRC.h" #include "fsfw/globalfunctions/arrayprinter.h" diff --git a/src/fsfw/tmtcservices/CommandingServiceBase.cpp b/src/fsfw/tmtcservices/CommandingServiceBase.cpp index 8aa5dbcea..d8ac88e78 100644 --- a/src/fsfw/tmtcservices/CommandingServiceBase.cpp +++ b/src/fsfw/tmtcservices/CommandingServiceBase.cpp @@ -251,8 +251,18 @@ void CommandingServiceBase::handleRequestQueue() { result = tcStore->getData(message.getStorageId(), &dataPtr, &dataLen); if (result != HasReturnvaluesIF::RETURN_OK) { // TODO: Warning? + continue; + } + result = tcReader.setReadOnlyData(dataPtr, dataLen); + if (result != HasReturnvaluesIF::RETURN_OK) { + // TODO: Warning? + continue; + } + result = tcReader.parseData(); + if (result != HasReturnvaluesIF::RETURN_OK) { + // TODO: Warning? + continue; } - tcReader.setReadOnlyData(dataPtr, dataLen); if ((tcReader.getSubService() == 0) or (isValidSubservice(tcReader.getSubService()) != RETURN_OK)) { @@ -260,10 +270,8 @@ void CommandingServiceBase::handleRequestQueue() { continue; } - size_t appDataLen = 0; - const uint8_t* appData = tcReader.getUserData(appDataLen); - result = - getMessageQueueAndObject(tcReader.getSubService(), appData, appDataLen, &queue, &objectId); + result = getMessageQueueAndObject(tcReader.getSubService(), tcReader.getUserData(), + tcReader.getUserDataLen(), &queue, &objectId); if (result != HasReturnvaluesIF::RETURN_OK) { rejectPacket(tc_verification::START_FAILURE, address, &tcReader, result); continue; @@ -332,10 +340,9 @@ void CommandingServiceBase::startExecution(store_address_t storeId, PusTcReader* return; } iter->second.subservice = storedPacket->getSubService(); - size_t appDataLen = 0; - const uint8_t* appData = storedPacket->getUserData(appDataLen); - result = prepareCommand(&command, iter->second.subservice, appData, appDataLen, - &iter->second.state, iter->second.objectId); + result = + prepareCommand(&command, iter->second.subservice, storedPacket->getUserData(), + storedPacket->getUserDataLen(), &iter->second.state, iter->second.objectId); ReturnValue_t sendResult = RETURN_OK; switch (result) { @@ -401,7 +408,16 @@ void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter& iter) { size_t dataLen = 0; ReturnValue_t result = tcStore->getData(address, &dataPtr, &dataLen); if (result == HasReturnvaluesIF::RETURN_OK) { - tcReader.setReadOnlyData(dataPtr, dataLen); + result = tcReader.setReadOnlyData(dataPtr, dataLen); + if (result != HasReturnvaluesIF::RETURN_OK) { + // TODO: Warning? + return; + } + result = tcReader.parseData(); + if (result != HasReturnvaluesIF::RETURN_OK) { + // TODO: Warning? + return; + } startExecution(address, &tcReader, iter); } else { // TODO: Warning? diff --git a/src/fsfw/tmtcservices/PusServiceBase.cpp b/src/fsfw/tmtcservices/PusServiceBase.cpp index e924ba67c..5b985ad67 100644 --- a/src/fsfw/tmtcservices/PusServiceBase.cpp +++ b/src/fsfw/tmtcservices/PusServiceBase.cpp @@ -47,39 +47,51 @@ void PusServiceBase::handleRequestQueue() { // << std::endl; #endif // } - - if (status == RETURN_OK) { - const uint8_t* dataPtr; - size_t dataLen = 0; - result = ipcStore->getData(message.getStorageId(), &dataPtr, &dataLen); - if (result != HasReturnvaluesIF::RETURN_OK) { - // TODO: Warning? - } - - currentPacket.setReadOnlyData(dataPtr, dataLen); - - result = this->handleRequest(currentPacket.getSubService()); - if (result == RETURN_OK) { - this->verifyReporter.sendSuccessReport(tc_verification::COMPLETION_SUCCESS, - &this->currentPacket); - } else { - this->verifyReporter.sendFailureReport(tc_verification::COMPLETION_FAILURE, - &this->currentPacket, result, 0, errorParameter1, - errorParameter2); - } - ipcStore->deleteData(message.getStorageId()); - errorParameter1 = 0; - errorParameter2 = 0; - } else if (status == MessageQueueIF::EMPTY) { + if (status == MessageQueueIF::EMPTY) { status = RETURN_OK; break; - } else { + } else if (status != HasReturnvaluesIF::RETURN_OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "PusServiceBase::performOperation: Service " << this->serviceId << ": Error receiving packet. Code: " << std::hex << status << std::dec << std::endl; +#else + sif::printError( + "PusServiceBase::performOperation: Service %d. Error receiving packet. Code: %04x\n", + serviceId, status); #endif + break; } + const uint8_t* dataPtr; + size_t dataLen = 0; + result = ipcStore->getData(message.getStorageId(), &dataPtr, &dataLen); + if (result != HasReturnvaluesIF::RETURN_OK) { + // TODO: Warning? + continue; + } + + result = currentPacket.setReadOnlyData(dataPtr, dataLen); + if (result != HasReturnvaluesIF::RETURN_OK) { + // TODO: Warning? + continue; + } + result = currentPacket.parseData(); + if (result != HasReturnvaluesIF::RETURN_OK) { + // TODO: Warning? + continue; + } + result = this->handleRequest(currentPacket.getSubService()); + if (result == RETURN_OK) { + this->verifyReporter.sendSuccessReport(tc_verification::COMPLETION_SUCCESS, + &this->currentPacket); + } else { + this->verifyReporter.sendFailureReport(tc_verification::COMPLETION_FAILURE, + &this->currentPacket, result, 0, errorParameter1, + errorParameter2); + } + ipcStore->deleteData(message.getStorageId()); + errorParameter1 = 0; + errorParameter2 = 0; } }