79 lines
2.7 KiB
C++
79 lines
2.7 KiB
C++
#include "fsfw/tmtcpacket/pus/tc/TcPacketStoredPus.h"
|
|
|
|
#include <cstring>
|
|
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
|
|
|
TcPacketStoredPus::TcPacketStoredPus(uint16_t apid, uint8_t service, uint8_t subservice,
|
|
uint8_t sequenceCount, const uint8_t* data, size_t size,
|
|
uint8_t ack)
|
|
: TcPacketPus(nullptr) {
|
|
this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
|
|
if (not this->checkAndSetStore()) {
|
|
return;
|
|
}
|
|
uint8_t* pData = nullptr;
|
|
ReturnValue_t returnValue =
|
|
this->STORE->getFreeElement(&this->storeAddress, (TC_PACKET_MIN_SIZE + size), &pData);
|
|
if (returnValue != this->STORE->RETURN_OK) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::warning << "TcPacketStoredBase: Could not get free element from store!" << std::endl;
|
|
#endif
|
|
return;
|
|
}
|
|
this->setData(pData, TC_PACKET_MIN_SIZE + size);
|
|
#if FSFW_USE_PUS_C_TELECOMMANDS == 1
|
|
pus::PusVersion pusVersion = pus::PusVersion::PUS_C_VERSION;
|
|
#else
|
|
pus::PusVersion pusVersion = pus::PusVersion::PUS_A_VERSION;
|
|
#endif
|
|
initializeTcPacket(apid, sequenceCount, ack, service, subservice, pusVersion);
|
|
std::memcpy(&tcData->appData, data, size);
|
|
this->setPacketDataLength(size + sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
|
|
this->setErrorControl();
|
|
}
|
|
|
|
TcPacketStoredPus::TcPacketStoredPus() : TcPacketStoredBase(), TcPacketPus(nullptr) {}
|
|
|
|
TcPacketStoredPus::TcPacketStoredPus(store_address_t setAddress) : TcPacketPus(nullptr) {
|
|
TcPacketStoredBase::setStoreAddress(setAddress, this);
|
|
}
|
|
|
|
TcPacketStoredPus::TcPacketStoredPus(const uint8_t* data, size_t size) : TcPacketPus(data) {
|
|
if (this->getFullSize() != size) {
|
|
return;
|
|
}
|
|
if (this->checkAndSetStore()) {
|
|
ReturnValue_t status = STORE->addData(&storeAddress, data, size);
|
|
if (status != HasReturnvaluesIF::RETURN_OK) {
|
|
this->setData(nullptr, size);
|
|
}
|
|
const uint8_t* storePtr = nullptr;
|
|
// Repoint base data pointer to the data in the store.
|
|
STORE->getData(storeAddress, &storePtr, &size);
|
|
this->setData(const_cast<uint8_t*>(storePtr), size);
|
|
}
|
|
}
|
|
|
|
ReturnValue_t TcPacketStoredPus::deletePacket() {
|
|
ReturnValue_t result = this->STORE->deleteData(this->storeAddress);
|
|
this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
|
|
// To circumvent size checks
|
|
this->setData(nullptr, -1);
|
|
return result;
|
|
}
|
|
|
|
TcPacketPusBase* TcPacketStoredPus::getPacketBase() { return this; }
|
|
|
|
bool TcPacketStoredPus::isSizeCorrect() {
|
|
const uint8_t* temp_data = nullptr;
|
|
size_t temp_size;
|
|
ReturnValue_t status = this->STORE->getData(this->storeAddress, &temp_data, &temp_size);
|
|
if (status == StorageManagerIF::RETURN_OK) {
|
|
if (this->getFullSize() == temp_size) {
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|