diff --git a/bsp_q7s/InitMission.cpp b/bsp_q7s/InitMission.cpp index c776ead0..8c512c83 100644 --- a/bsp_q7s/InitMission.cpp +++ b/bsp_q7s/InitMission.cpp @@ -135,6 +135,8 @@ void initmission::initTasks() { initmission::printAddObjectError("PUS_17", objects::PUS_SERVICE_17_TEST); } +#if TE0720 == 0 + //TODO: Add handling of missed deadlines /* Polling Sequence Table Default */ FixedTimeslotTaskIF * pollingSequenceTableTaskDefault = factory->createFixedTimeslotTask( @@ -145,7 +147,6 @@ void initmission::initTasks() { sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl; } -#if TE0720 == 0 FixedTimeslotTaskIF* gomSpacePstTask = factory-> createFixedTimeslotTask("GS_PST_TASK", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE*8, 3.0, missedDeadlineFunc); @@ -153,6 +154,15 @@ void initmission::initTasks() { if(result != HasReturnvaluesIF::RETURN_OK) { sif::error << "InitMission::initTasks: GomSpace PST initialization failed!" << std::endl; } + +#else + FixedTimeslotTaskIF * pollingSequenceTableTE0720 = factory->createFixedTimeslotTask( + "PST_TASK_TE0720", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0, + missedDeadlineFunc); + result = pst::pollingSequenceTE0720(pollingSequenceTableTE0720); + if (result != HasReturnvaluesIF::RETURN_OK) { + sif::error << "InitMission::initTasks: Creating TE0720 PST failed!" << std::endl; + } #endif PeriodicTaskIF* testTask = factory->createPeriodicTask( @@ -177,8 +187,10 @@ void initmission::initTasks() { #if TE0720 == 0 gomSpacePstTask->startTask(); -#endif pollingSequenceTableTaskDefault->startTask(); +#else + pollingSequenceTableTE0720->startTask(); +#endif pusVerification->startTask(); pusEvents->startTask(); diff --git a/bsp_q7s/ObjectFactory.cpp b/bsp_q7s/ObjectFactory.cpp index fe852662..55373525 100644 --- a/bsp_q7s/ObjectFactory.cpp +++ b/bsp_q7s/ObjectFactory.cpp @@ -351,11 +351,11 @@ void ObjectFactory::produce(){ new LibgpiodTest(objects::LIBGPIOD_TEST, objects::GPIO_IF, gpioCookie); #elif TE0720 == 1 /* Configuration for MIO0 on TE0720-03-1CFA */ - GpiodRegular gpioConfigForDummyHeater(std::string("gpiochip0"), 0, - std::string("Heater0"), gpio::OUT, 0); - heaterGpiosCookie->addGpio(gpioIds::HEATER_0, gpioConfigForDummyHeater); - new HeaterHandler(objects::HEATER_HANDLER, objects::GPIO_IF, heaterGpiosCookie, - objects::PCDU_HANDLER, pcduSwitches::TCS_BOARD_8V_HEATER_IN); +// GpiodRegular* gpio = new GpiodRegular(std::string("gpiochip0"), 0, std::string("Heater0"), +// gpio::OUT, 0); +// heaterGpiosCookie->addGpio(gpioIds::HEATER_0, gpio); +// new HeaterHandler(objects::HEATER_HANDLER, objects::GPIO_IF, heaterGpiosCookie, +// objects::PCDU_HANDLER, pcduSwitches::TCS_BOARD_8V_HEATER_IN); UartCookie* plocUartCookie = new UartCookie(std::string("/dev/ttyPS1"), 115200, PLOC::MAX_REPLY_SIZE); diff --git a/bsp_q7s/devices/HeaterHandler.cpp b/bsp_q7s/devices/HeaterHandler.cpp index 9a44c57b..3f53af2f 100644 --- a/bsp_q7s/devices/HeaterHandler.cpp +++ b/bsp_q7s/devices/HeaterHandler.cpp @@ -58,8 +58,9 @@ ReturnValue_t HeaterHandler::initialize() { if(mainLineSwitcherObjectId != objects::NO_OBJECT) { mainLineSwitcher = objectManager->get(mainLineSwitcherObjectId); if (mainLineSwitcher == nullptr) { - sif::error << "HeaterHandler::initialize: Main line switcher failed to fetch object" - << "from object ID." << std::endl; + sif::error + << "HeaterHandler::initialize: Failed to get main line switcher. Make sure " + << "main line switcher object is initialized." << std::endl; return ObjectManagerIF::CHILD_INIT_FAILED; } } diff --git a/fsfwconfig/OBSWConfig.h b/fsfwconfig/OBSWConfig.h index 73c05edb..3f5ebfca 100644 --- a/fsfwconfig/OBSWConfig.h +++ b/fsfwconfig/OBSWConfig.h @@ -19,7 +19,7 @@ debugging. */ #define OBSW_ADD_TEST_CODE 1 #define TEST_LIBGPIOD 0 -#define TE0720 0 +#define TE0720 1 #define P60DOCK_DEBUG 0 #define PDU1_DEBUG 0 diff --git a/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 86e3629e..51427e90 100644 --- a/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -5,13 +5,13 @@ #include #include #include +#include ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence) { /* Length of a communication cycle */ uint32_t length = thisSequence->getPeriodMs(); - thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0, @@ -229,3 +229,25 @@ ReturnValue_t pst::pollingSequenceAcsTest(FixedTimeslotTaskIF *thisSequence) { } return HasReturnvaluesIF::RETURN_OK; } + +ReturnValue_t pst::pollingSequenceTE0720(FixedTimeslotTaskIF *thisSequence) { + uint32_t length = thisSequence->getPeriodMs(); + + thisSequence->addSlot(objects::PLOC_HANDLER, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.2, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.4, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.6, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.8, + DeviceHandlerIF::GET_READ); + + if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) { + sif::error << "Initialization of TE0720 PST failed" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_OK; +} + diff --git a/fsfwconfig/pollingsequence/pollingSequenceFactory.h b/fsfwconfig/pollingsequence/pollingSequenceFactory.h index 09dd7242..73cdc0bb 100644 --- a/fsfwconfig/pollingsequence/pollingSequenceFactory.h +++ b/fsfwconfig/pollingsequence/pollingSequenceFactory.h @@ -35,6 +35,11 @@ ReturnValue_t pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence); ReturnValue_t gomspacePstInit(FixedTimeslotTaskIF *thisSequence); ReturnValue_t pollingSequenceAcsTest(FixedTimeslotTaskIF* thisSequence); + +/** + * @brief This polling sequence will be created when the software is compiled for the TE0720. + */ +ReturnValue_t pollingSequenceTE0720(FixedTimeslotTaskIF* thisSequence); } diff --git a/mission/devices/PlocHandler.cpp b/mission/devices/PlocHandler.cpp index fca51b71..7300a610 100644 --- a/mission/devices/PlocHandler.cpp +++ b/mission/devices/PlocHandler.cpp @@ -41,24 +41,10 @@ ReturnValue_t PlocHandler::buildCommandFromCommand( size_t commandDataLen) { switch(deviceCommand) { case(PLOC::TC_MEM_WRITE): { - const uint32_t memoryAddress = *(commandData) << 24 | *(commandData + 1) << 16 - | *(commandData + 2) << 8 | *(commandData + 3); - const uint32_t memoryData = *(commandData + 4) << 24 | *(commandData + 5) << 16 - | *(commandData + 6) << 8 | *(commandData + 7); - PLOC::TcMemWrite tcMemWrite(memoryAddress, memoryData); - rawPacket = tcMemWrite.getWholeData(); - rawPacketLen = tcMemWrite.getFullSize(); - rememberCommandId = PLOC::TC_MEM_WRITE; - return RETURN_OK; + return prepareTcMemWriteCommand(commandData, commandDataLen); } case(PLOC::TC_MEM_READ): { - const uint32_t memoryAddress = *(commandData) << 24 | *(commandData + 1) << 16 - | *(commandData + 2) << 8 | *(commandData + 3); - PLOC::TcMemRead tcMemRead(memoryAddress); - rawPacket = tcMemRead.getWholeData(); - rawPacketLen = tcMemRead.getFullSize(); - rememberCommandId = PLOC::TC_MEM_READ; - return RETURN_OK; + return prepareTcMemReadCommand(commandData, commandDataLen); } default: sif::debug << "PlocHandler::buildCommandFromCommand: Command not implemented" << std::endl; @@ -67,11 +53,43 @@ ReturnValue_t PlocHandler::buildCommandFromCommand( return HasReturnvaluesIF::RETURN_FAILED; } +ReturnValue_t PlocHandler::prepareTcMemWriteCommand(const uint8_t * commandData, + size_t commandDataLen) { + const uint32_t memoryAddress = *(commandData) << 24 | *(commandData + 1) << 16 + | *(commandData + 2) << 8 | *(commandData + 3); + const uint32_t memoryData = *(commandData + 4) << 24 | *(commandData + 5) << 16 + | *(commandData + 6) << 8 | *(commandData + 7); + PLOC::TcMemWrite tcMemWrite(memoryAddress, memoryData); + if (tcMemWrite.getFullSize() > PLOC::MAX_COMMAND_SIZE) { + sif::debug << "PlocHandler::prepareTcMemWriteCommand: Command too big" << std::endl; + return RETURN_FAILED; + } + memcpy(commandBuffer, tcMemWrite.getWholeData(), tcMemWrite.getFullSize()); + rawPacket = commandBuffer; + rawPacketLen = tcMemWrite.getFullSize(); + rememberCommandId = PLOC::TC_MEM_WRITE; + return RETURN_OK; +} + +ReturnValue_t PlocHandler::prepareTcMemReadCommand(const uint8_t * commandData, + size_t commandDataLen) { + const uint32_t memoryAddress = *(commandData) << 24 | *(commandData + 1) << 16 + | *(commandData + 2) << 8 | *(commandData + 3); + PLOC::TcMemRead tcMemRead(memoryAddress); + if (tcMemRead.getFullSize() > PLOC::MAX_COMMAND_SIZE) { + sif::debug << "PlocHandler::prepareTcMemReadCommand: Command too big" << std::endl; + return RETURN_FAILED; + } + memcpy(commandBuffer, tcMemRead.getWholeData(), tcMemRead.getFullSize()); + rawPacket = commandBuffer; + rawPacketLen = tcMemRead.getFullSize(); + rememberCommandId = PLOC::TC_MEM_READ; + return RETURN_OK; +} + void PlocHandler::fillCommandAndReplyMap() { this->insertInCommandAndReplyMap(PLOC::TC_MEM_WRITE, 1, nullptr, PLOC::SIZE_ACK_REPORT); this->insertInCommandAndReplyMap(PLOC::TC_MEM_READ, 1, nullptr, PLOC::SIZE_ACK_REPORT); - this->insertInReplyMap(PLOC::ACK_SUCCESS, 1); - this->insertInReplyMap(PLOC::ACK_FAILURE, 1); } ReturnValue_t PlocHandler::scanForReply(const uint8_t *start, @@ -79,31 +97,34 @@ ReturnValue_t PlocHandler::scanForReply(const uint8_t *start, ReturnValue_t result = RETURN_OK; - *foundId = (*(start) << 8 | *(start + 1)) & APID_MASK; + uint16_t apid = (*(start) << 8 | *(start + 1)) & APID_MASK; - switch(*foundId) { - case(PLOC::ACK_SUCCESS): + switch(apid) { + case(PLOC::APID_ACK_SUCCESS): *foundLen = PLOC::SIZE_ACK_REPORT; + *foundId = rememberCommandId; break; - case(PLOC::ACK_FAILURE): + case(PLOC::APID_ACK_FAILURE): *foundLen = PLOC::SIZE_ACK_REPORT; - break; + *foundId = rememberCommandId; + break; default: sif::debug << "PlocHandler::scanForReply: Reply has invalid apid" << std::endl; - result = IGNORE_REPLY_DATA; + return IGNORE_REPLY_DATA; break; } - result = verifyPacket(start, foundLen); - return result; } -ReturnValue_t PlocHandler::verifyPacket(const uint8_t* start, size_t* foundLen) { +ReturnValue_t PlocHandler::verifyPacket(const uint8_t* start, size_t foundLen) { - uint16_t receivedCrc = *(start + *foundLen - 2) << 8 | *(start + *foundLen - 1); + uint16_t receivedCrc = *(start + foundLen - 2) << 8 | *(start + foundLen - 1); - if (receivedCrc != CRC::crc16ccitt(start, *foundLen, 0)) { + uint16_t recalculatedCrc = CRC::crc16ccitt(start, foundLen - 2, 0); + + if (receivedCrc != recalculatedCrc) { + sif::debug << "PlocHandler::verifyPacket: CRC failure of ACK reply" << std::endl; return CRC_FAILURE; } @@ -113,55 +134,68 @@ ReturnValue_t PlocHandler::verifyPacket(const uint8_t* start, size_t* foundLen) ReturnValue_t PlocHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { + uint16_t apid = (*(packet) << 8 | *(packet + 1)) & APID_MASK; + + if (apid == PLOC::APID_ACK_FAILURE) { + sif::error << "PlocHandler::interpretDeviceReply: Received ACK failure reply" << std::endl; + triggerEvent(ACK_FAILURE, id); + replyRawData(packet, PLOC::SIZE_ACK_REPORT, defaultRawReceiver); + return RETURN_OK; + } + + if (verifyPacket(packet, PLOC::SIZE_ACK_REPORT) == CRC_FAILURE) { + sif::error << "PlocHandler::interpretDeviceReply: CRC failure in Ack reply" << std::endl; + replyRawData(packet, PLOC::SIZE_ACK_REPORT, defaultRawReceiver); + triggerEvent(CRC_FAILURE_IN_ACK_REPLY, id); + return RETURN_OK; + } + switch (id) { - case (PLOC::ACK_SUCCESS): - receiveTm(); - receiveExecutionReport(); - break; - case (PLOC::ACK_FAILURE): - //TODO: Interpretation of status field in ack reply. - sif::error << "PlocHandler::interpretDeviceReply: Received ack failure reply" << std::endl; - triggerEvent(ACK_FAILURE, rememberCommandId); - break; - default: { - sif::debug << "PlocHandler::interpretDeviceReply: Unknown device reply id" << std::endl; - return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; - } - } - - return RETURN_OK; -} - -void PlocHandler::receiveTm() { - switch (rememberCommandId) { case (PLOC::TC_MEM_WRITE): + receiveExecutionReport(); break; case (PLOC::TC_MEM_READ): receiveTmMemoryReadReport(); + receiveExecutionReport(); break; default: - sif::debug << "PlocHandler::receiveTm: Rembered unknown command id" << std::endl; - break; - } + sif::debug << "PlocHandler::interpretDeviceReply: Unknown device reply id" << std::endl; + return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; + } + + rememberCommandId = PLOC::NONE; + return RETURN_OK; } -void PlocHandler::receiveExecutionReport() { +ReturnValue_t PlocHandler::receiveExecutionReport() { size_t receivedDataLen = 0; uint8_t *receivedData = nullptr; - communicationInterface->requestReceiveMessage(comCookie, PLOC::SIZE_EXE_REPORT); - communicationInterface->readReceivedMessage(comCookie, &receivedData, &receivedDataLen); + ReturnValue_t result = RETURN_OK; - if(!verifyPacket(receivedData, &receivedDataLen)) { + result = communicationInterface->requestReceiveMessage(comCookie, PLOC::SIZE_EXE_REPORT); + if (result != RETURN_OK) { + sif::error << "PlocHandler::receiveExecutionReport: Failed to request execution report" + << std::endl; + return result; + } + result = communicationInterface->readReceivedMessage(comCookie, &receivedData, &receivedDataLen); + if (result != RETURN_OK) { + sif::error << "PlocHandler::receiveExecutionReport: Failed to read execution report" + << std::endl; + return result; + } + + if(verifyPacket(receivedData, receivedDataLen) == CRC_FAILURE) { replyRawData(receivedData, receivedDataLen, defaultRawReceiver); triggerEvent(EXE_RPT_INVALID_CRC); sif::error << "PlocHandler::receiveExecutionReport: Execution report has invalid crc" << std::endl; - return; + return result; } - handleExecutionReport(receivedData, receivedDataLen); + result = handleExecutionReport(receivedData, receivedDataLen); } void PlocHandler::handleExecutionReport(const uint8_t* receivedData, size_t receivedDataLen) { @@ -177,10 +211,10 @@ void PlocHandler::handleExecutionReport(const uint8_t* receivedData, size_t rece sif::error << "PlocHandler::handleExecutionReport: Received execution failure report" << std::endl; triggerEvent(EXE_FAILURE, rememberCommandId); - return; + return EXE_REPLY_CRC_FAILURE; } } - return; + return RETURN_OK; } void PlocHandler::receiveTmMemoryReadReport() { @@ -211,7 +245,7 @@ void PlocHandler::receiveTmMemoryReadReport() { return; } - if(!verifyPacket(receivedData, &receivedDataLen)) { + if(verifyPacket(receivedData, receivedDataLen) == CRC_FAILURE) { replyRawData(receivedData, receivedDataLen, defaultRawReceiver); triggerEvent(TM_READ_RPT_INVALID_CRC); sif::error << "PlocHandler::receiveTmReadReport: TM read report has invalid crc" diff --git a/mission/devices/PlocHandler.h b/mission/devices/PlocHandler.h index 8e3a6aa4..0c0bf7d8 100644 --- a/mission/devices/PlocHandler.h +++ b/mission/devices/PlocHandler.h @@ -44,12 +44,14 @@ private: static const ReturnValue_t TC_ACK_FAILURE = MAKE_RETURN_CODE(0xA0); static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xA1); + static const ReturnValue_t EXE_REPLY_CRC_FAILURE = MAKE_RETURN_CODE(0xA1); static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_HANDLER; static const Event REQUESTING_TM_READ_REPORT_FAILED = MAKE_EVENT(0, severity::LOW); static const Event TM_READ_RPT_INVALID_CRC = MAKE_EVENT(1, severity::LOW); static const Event ACK_FAILURE = MAKE_EVENT(2, severity::LOW); + static const Event CRC_FAILURE_IN_ACK_REPLY = MAKE_EVENT(2, severity::LOW); static const Event EXE_FAILURE = MAKE_EVENT(3, severity::LOW); static const Event EXE_RPT_INVALID_CRC = MAKE_EVENT(4, severity::LOW); @@ -57,10 +59,27 @@ private: DeviceCommandId_t rememberCommandId = PLOC::NONE; + uint8_t commandBuffer[PLOC::MAX_COMMAND_SIZE]; + /** - * @brief This function checks wheter a telemetry packet is expected or not. + * @brief This function fills the commandBuffer to initiate the write memory command. + * + * @param commandData Pointer to action command data. + * @param commanDataLen Size of command data in bytes. + * + * @return RETURN_OK if successful, else RETURN_FAILURE. */ - void receiveTm(); + ReturnValue_t prepareTcMemWriteCommand(const uint8_t * commandData, size_t commandDataLen); + + /** + * @brief This function fills the commandBuffer to initiate the write reads command. + * + * @param commandData Pointer to action command data. + * @param commanDataLen Size of command data in bytes. + * + * @return RETURN_OK if successful, else RETURN_FAILURE. + */ + ReturnValue_t prepareTcMemReadCommand(const uint8_t * commandData, size_t commandDataLen); /** * @brief This function checks the crc of the received PLOC reply. @@ -70,18 +89,21 @@ private: * * @return RETURN_OK if CRC is ok, otherwise CRC_FAILURE. */ - ReturnValue_t verifyPacket(const uint8_t* start, size_t* foundLen); + ReturnValue_t verifyPacket(const uint8_t* start, size_t foundLen); /** * @brief This function reads and handles the execution reply. + * @param RETURN_OK if reading and handling of reply successful, otherwise failure value. */ - void receiveExecutionReport(); + ReturnValue_t receiveExecutionReport(); /** * @brief This function handles the data of a execution report. * * @param receivedData Pointer to the received data * @param receivedDataLen Size in bytes of the received data + * + * @return RETURN_OK if successful, otherwise an error code. */ void handleExecutionReport(const uint8_t* receivedData, size_t receivedDataLen); @@ -92,7 +114,7 @@ private: * @details In case of a valid packet the received memory content will be forwarded to the * commanding object via an action message. */ - void receiveTmMemoryReadReport(); + ReturnValue_t receiveTmMemoryReadReport(); /** * @brief This function handles action message replies in case the telemetry has been diff --git a/mission/devices/devicedefinitions/PlocDefinitions.h b/mission/devices/devicedefinitions/PlocDefinitions.h index dbd157e5..74dd3c09 100644 --- a/mission/devices/devicedefinitions/PlocDefinitions.h +++ b/mission/devices/devicedefinitions/PlocDefinitions.h @@ -3,15 +3,13 @@ #include #include +#include namespace PLOC { static const DeviceCommandId_t NONE = 0x0; static const DeviceCommandId_t TC_MEM_WRITE = 0x714; static const DeviceCommandId_t TC_MEM_READ = 0x715; - static const DeviceCommandId_t ACK_SUCCESS = 0x400; - static const DeviceCommandId_t ACK_FAILURE = 0x401; - static const DeviceCommandId_t TM_READ_REPORT = 0x404; static const uint16_t SIZE_ACK_REPORT = 14; static const uint16_t SIZE_EXE_REPORT = 14; @@ -23,6 +21,8 @@ namespace PLOC { static const uint16_t APID_TC_MEM_WRITE = 0x714; static const uint16_t APID_TC_MEM_READ = 0x715; static const uint16_t APID_TM_READ_REPORT = 0x404; + static const uint16_t APID_ACK_SUCCESS = 0x400; + static const uint16_t APID_ACK_FAILURE = 0x401; static const uint16_t APID_EXE_SUCCESS = 0x402; static const uint16_t APID_EXE_FAILURE = 0x403; @@ -34,6 +34,7 @@ namespace PLOC { static const uint16_t LENGTH_TC_MEM_READ = 8; static const size_t MAX_REPLY_SIZE = SIZE_TM_MEM_READ_REPORT; + static const size_t MAX_COMMAND_SIZE = 18; /** * @brief This class helps to build the memory read command for the PLOC. @@ -44,6 +45,7 @@ namespace PLOC { */ class TcMemRead : public SpacePacket { public: + /** * @brief Constructor * @@ -63,18 +65,29 @@ namespace PLOC { */ void fillPacketDataField(const uint32_t* memAddrPtr) { /* Add memAddr to packet data field */ - memcpy(this->localData.fields.buffer, memAddrPtr, sizeof(*memAddrPtr)); + size_t serializedSize = 0; + uint8_t* memoryAddressPos = this->localData.fields.buffer; + SerializeAdapter::serialize(memAddrPtr, &memoryAddressPos, &serializedSize, + sizeof(*memAddrPtr), SerializeIF::Endianness::BIG); + /* Add memLen to packet data field */ this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD] = 0; this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD + 1] = 1; + + /* Calculate crc */ uint16_t crc = CRC::crc16ccitt(this->localData.byteStream, sizeof(CCSDSPrimaryHeader) + LENGTH_TC_MEM_READ - CRC_SIZE, 0); + /* Add crc to packet data field of space packet */ + serializedSize = 0; + uint8_t* crcPos = this->localData.fields.buffer + CRC_OFFSET; + SerializeAdapter::serialize(&crc, &crcPos, &serializedSize, + sizeof(crc), SerializeIF::Endianness::BIG); memcpy(this->localData.fields.buffer + CRC_OFFSET, &crc, sizeof(crc)); } - static const uint8_t CRC_OFFSET = 12; - static const uint8_t OFFSET_MEM_LEN_FIELD = 10; + static const uint8_t OFFSET_MEM_LEN_FIELD = 4; + static const uint8_t CRC_OFFSET = 6; }; @@ -107,23 +120,39 @@ namespace PLOC { * @param memoryDataPtr Pointer to the memoryData to write */ void fillPacketDataField(const uint32_t* memAddrPtr, const uint32_t* memoryDataPtr) { + /* Add memAddr to packet data field */ - memcpy(this->localData.fields.buffer, memAddrPtr, sizeof(*memAddrPtr)); + size_t serializedSize = 0; + uint8_t* memoryAddressPos = this->localData.fields.buffer; + SerializeAdapter::serialize(memAddrPtr, &memoryAddressPos, &serializedSize, + sizeof(*memAddrPtr), SerializeIF::Endianness::BIG); + /* Add memLen to packet data field */ this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD] = 0; this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD + 1] = 1; + /* Add memData to packet data field */ - memcpy(this->localData.fields.buffer + OFFSET_MEM_DATA_FIELD, memAddrPtr, - sizeof(*memAddrPtr)); + serializedSize = 0; + uint8_t* memoryDataPos = this->localData.fields.buffer + OFFSET_MEM_DATA_FIELD; + SerializeAdapter::serialize(memoryDataPtr, &memoryDataPos, &serializedSize, + sizeof(*memoryDataPtr), SerializeIF::Endianness::BIG); + + /* Calculate crc */ uint16_t crc = CRC::crc16ccitt(this->localData.byteStream, sizeof(CCSDSPrimaryHeader) + LENGTH_TC_MEM_WRITE - CRC_SIZE, 0); + + serializedSize = 0; + uint8_t* crcPos = this->localData.fields.buffer + CRC_OFFSET; /* Add crc to packet data field of space packet */ + SerializeAdapter::serialize(&crc, &crcPos, &serializedSize, + sizeof(crc), SerializeIF::Endianness::BIG); memcpy(this->localData.fields.buffer + CRC_OFFSET, &crc, sizeof(crc)); } - static const uint8_t OFFSET_MEM_LEN_FIELD = 10; - static const uint8_t OFFSET_MEM_DATA_FIELD = 12; - static const uint8_t CRC_OFFSET = 16; + /** Offsets from base address of packet data field */ + static const uint8_t OFFSET_MEM_LEN_FIELD = 4; + static const uint8_t OFFSET_MEM_DATA_FIELD = 6; + static const uint8_t CRC_OFFSET = 10; }; }