diff --git a/src/fsfw/datalinklayer/MapPacketExtraction.cpp b/src/fsfw/datalinklayer/MapPacketExtraction.cpp index 839209ac..f2a6c550 100644 --- a/src/fsfw/datalinklayer/MapPacketExtraction.cpp +++ b/src/fsfw/datalinklayer/MapPacketExtraction.cpp @@ -89,12 +89,17 @@ ReturnValue_t MapPacketExtraction::unpackBlockingPackets(TcTransferFrame* frame) if (totalLength > MAX_PACKET_SIZE) return CONTENT_TOO_LARGE; uint8_t* position = frame->getDataField(); while ((totalLength > ccsds::HEADER_LEN)) { - SpacePacketBase packet(position); - uint32_t packetSize = packet.getFullSize(); + SpacePacketReader packet(position, totalLength); + status = packet.checkSize(); + if(status != returnvalue::OK) { + // TODO: Better error handling + status = DATA_CORRUPTED; + } + uint32_t packetSize = packet.getFullPacketLen(); if (packetSize <= totalLength) { - status = sendCompletePacket(packet.getWholeData(), packet.getFullSize()); - totalLength -= packet.getFullSize(); - position += packet.getFullSize(); + status = sendCompletePacket(packet.getFullData(), packet.getFullPacketLen()); + totalLength -= packet.getFullPacketLen(); + position += packet.getFullPacketLen(); status = returnvalue::OK; } else { status = DATA_CORRUPTED; @@ -107,7 +112,7 @@ ReturnValue_t MapPacketExtraction::unpackBlockingPackets(TcTransferFrame* frame) return status; } -ReturnValue_t MapPacketExtraction::sendCompletePacket(uint8_t* data, uint32_t size) { +ReturnValue_t MapPacketExtraction::sendCompletePacket(const uint8_t* data, uint32_t size) { store_address_t store_id; ReturnValue_t status = this->packetStore->addData(&store_id, data, size); if (status == returnvalue::OK) { diff --git a/src/fsfw/datalinklayer/MapPacketExtraction.h b/src/fsfw/datalinklayer/MapPacketExtraction.h index fab7f3c3..3aa92aeb 100644 --- a/src/fsfw/datalinklayer/MapPacketExtraction.h +++ b/src/fsfw/datalinklayer/MapPacketExtraction.h @@ -47,7 +47,7 @@ class MapPacketExtraction : public MapPacketExtractionIF { * @param size Complete total size of the packet. * @return Return Code of the Packet Store or the Message Queue. */ - ReturnValue_t sendCompletePacket(uint8_t* data, uint32_t size); + ReturnValue_t sendCompletePacket(const uint8_t* data, uint32_t size); /** * Helper method to reset the internal buffer. */