From 077913400fa2bca51ba8de5ddaf83578adad6547 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Mon, 29 Nov 2021 11:04:56 +0100 Subject: [PATCH] star tracker image helper --- bsp_q7s/core/ObjectFactory.cpp | 2 + bsp_q7s/devices/CMakeLists.txt | 1 + bsp_q7s/devices/PlocUpdater.cpp | 4 +- bsp_q7s/devices/StarTrackerImageHelper.cpp | 291 ++++++++++++++++++ bsp_q7s/devices/StarTrackerImageHelper.h | 150 +++++++++ common/config/commonClassIds.h | 1 + common/config/commonObjects.h | 3 +- common/config/commonSubsystemIds.h | 1 + linux/fsfwconfig/OBSWConfig.h.in | 3 +- .../pollingSequenceFactory.cpp | 1 + mission/devices/StarTrackerHandler.cpp | 173 +++++------ mission/devices/StarTrackerHandler.h | 27 +- .../StarTrackerDefinitions.h | 89 ++++-- 13 files changed, 632 insertions(+), 114 deletions(-) create mode 100644 bsp_q7s/devices/StarTrackerImageHelper.cpp create mode 100644 bsp_q7s/devices/StarTrackerImageHelper.h diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 83b59d08..44e4484b 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -16,6 +16,7 @@ #include "bsp_q7s/devices/PlocSupervisorHandler.h" #include "bsp_q7s/devices/PlocUpdater.h" #include "bsp_q7s/devices/PlocMemoryDumper.h" +#include "bsp_q7s/devices/StarTrackerImageHelper.h" #include "bsp_q7s/callbacks/rwSpiCallback.h" #include "bsp_q7s/callbacks/gnssCallback.h" @@ -173,6 +174,7 @@ void ObjectFactory::produce(void* args) { starTrackerCookie->setNoFixedSizeReply(); StarTrackerHandler* starTrackerHandler = new StarTrackerHandler(objects::START_TRACKER, objects::UART_COM_IF, starTrackerCookie); starTrackerHandler->setStartUpImmediately(); + new StarTrackerImageHelper(objects::STR_IMG_HELPER); #endif /* OBSW_ADD_STAR_TRACKER == 1 */ #endif /* TE7020 == 0 */ diff --git a/bsp_q7s/devices/CMakeLists.txt b/bsp_q7s/devices/CMakeLists.txt index 6347b5f8..ecc6bdc1 100644 --- a/bsp_q7s/devices/CMakeLists.txt +++ b/bsp_q7s/devices/CMakeLists.txt @@ -2,4 +2,5 @@ target_sources(${TARGET_NAME} PRIVATE PlocSupervisorHandler.cpp PlocUpdater.cpp PlocMemoryDumper.cpp + StarTrackerImageHelper.cpp ) \ No newline at end of file diff --git a/bsp_q7s/devices/PlocUpdater.cpp b/bsp_q7s/devices/PlocUpdater.cpp index 5a94495b..4771e14c 100644 --- a/bsp_q7s/devices/PlocUpdater.cpp +++ b/bsp_q7s/devices/PlocUpdater.cpp @@ -169,13 +169,13 @@ ReturnValue_t PlocUpdater::getImageLocation(const uint8_t* data, size_t size) { // Check if file is stored on SD card and if associated SD card is mounted if (std::string(reinterpret_cast(data), SD_PREFIX_LENGTH) == std::string(SdCardManager::SD_0_MOUNT_POINT)) { if (!isSdCardMounted(sd::SLOT_0)) { - sif::warning << "PlocUpdater::prepareNvm0AUpdate: SD card 0 not mounted" << std::endl; + sif::warning << "PlocUpdater::getImageLocation: SD card 0 not mounted" << std::endl; return SD_NOT_MOUNTED; } } else if (std::string(reinterpret_cast(data), SD_PREFIX_LENGTH) == std::string(SdCardManager::SD_1_MOUNT_POINT)) { if (!isSdCardMounted(sd::SLOT_0)) { - sif::warning << "PlocUpdater::prepareNvm0AUpdate: SD card 1 not mounted" << std::endl; + sif::warning << "PlocUpdater::getImageLocation: SD card 1 not mounted" << std::endl; return SD_NOT_MOUNTED; } } diff --git a/bsp_q7s/devices/StarTrackerImageHelper.cpp b/bsp_q7s/devices/StarTrackerImageHelper.cpp new file mode 100644 index 00000000..c225e3b9 --- /dev/null +++ b/bsp_q7s/devices/StarTrackerImageHelper.cpp @@ -0,0 +1,291 @@ +#include "fsfw/ipc/QueueFactory.h" +#include "StarTrackerImageHelper.h" + +#include +#include +#include + +StarTrackerImageHelper::StarTrackerImageHelper(object_id_t objectId) : + SystemObject(objectId), commandActionHelper(this), actionHelper(this, nullptr) { + commandQueue = QueueFactory::instance()->createMessageQueue(QUEUE_SIZE); +} + +StarTrackerImageHelper::~StarTrackerImageHelper() { +} + +ReturnValue_t StarTrackerImageHelper::initialize() { + sdcMan = SdCardManager::instance(); + if (sdcMan == nullptr) { + sif::warning << "StarTrackerImageHelper::initialize: Invaldi SD Card Manager" << std::endl; + } + ReturnValue_t result = SystemObject::initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = commandActionHelper.initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = actionHelper.initialize(commandQueue); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t StarTrackerImageHelper::performOperation(uint8_t operationCode) { + readCommandQueue(); + doStateMachine(); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t StarTrackerImageHelper::executeAction(ActionId_t actionId, + MessageQueueId_t commandedBy, const uint8_t* data, size_t size) { + ReturnValue_t result = EXECUTION_FINISHED; + + if (state != State::IDLE) { + return IS_BUSY; + } + + if (size > MAX_STR_IMAGE_PATH) { + return NAME_TOO_LONG; + } + + switch (actionId) { + case UPLOAD_IMAGE: + result = prepareUploadCommand(data, size); + if (result == RETURN_OK) { + result = EXECUTION_FINISHED; + } + break; + case DOWNLOAD_IMAGE: + break; + default: + return INVALID_ACTION_ID; + } + + return result; +} + +MessageQueueId_t StarTrackerImageHelper::getCommandQueue() const { + return commandQueue->getId(); +} + +MessageQueueIF* StarTrackerImageHelper::getCommandQueuePtr() { + return commandQueue; +} + +void StarTrackerImageHelper::readCommandQueue() { + CommandMessage message; + ReturnValue_t result; + + for (result = commandQueue->receiveMessage(&message); result == HasReturnvaluesIF::RETURN_OK; + result = commandQueue->receiveMessage(&message)) { + if (result != RETURN_OK) { + continue; + } + result = actionHelper.handleActionMessage(&message); + if (result == HasReturnvaluesIF::RETURN_OK) { + continue; + } + + result = commandActionHelper.handleReply(&message); + if (result == HasReturnvaluesIF::RETURN_OK) { + continue; + } + + sif::debug << "StarTrackerImageHelper::readCommandQueue: Received inalid message" + << std::endl; + } +} + +void StarTrackerImageHelper::doStateMachine() { + switch (state) { + case State::IDLE: + break; + case State::SEND_NEXT_UPLOAD_CMD: + commandImageUpload(); + break; + case State::COMMAND_EXECUTING: + case State::UPLOAD_LAST: + break; + default: + sif::debug << "StarTrackerImageHelper::doStateMachine: Invalid state" << std::endl; + break; + } +} + +ReturnValue_t StarTrackerImageHelper::getImageLocation(const uint8_t* data, size_t size) { + + // Check if file is stored on SD card and if associated SD card is mounted + if (std::string(reinterpret_cast(data), SD_PREFIX_LENGTH) == std::string(SdCardManager::SD_0_MOUNT_POINT)) { + if (!isSdCardMounted(sd::SLOT_0)) { + sif::warning << "StarTrackerImageHelper::getImageLocation: SD card 0 not mounted" << std::endl; + return SD_NOT_MOUNTED; + } + } + else if (std::string(reinterpret_cast(data), SD_PREFIX_LENGTH) == std::string(SdCardManager::SD_1_MOUNT_POINT)) { + if (!isSdCardMounted(sd::SLOT_0)) { + sif::warning << "StarTrackerImageHelper::getImageLocation: SD card 1 not mounted" << std::endl; + return SD_NOT_MOUNTED; + } + } + else { + // Specified path in RAM filesystem + } + + imageFile = std::string(reinterpret_cast(data), size); + + // Check if file exists + if(not std::filesystem::exists(imageFile)) { + return FILE_NOT_EXISTS; + } + return RETURN_OK; +} + +bool StarTrackerImageHelper::isSdCardMounted(sd::SdCard sdCard) { + SdCardManager::SdStatePair active; + ReturnValue_t result = sdcMan->getSdCardActiveStatus(active); + if (result != RETURN_OK) { + sif::debug << "StarTrackerImageHelper::isSdCardMounted: Failed to get SD card active state"; + return false; + } + if (sdCard == sd::SLOT_0) { + if (active.first == sd::MOUNTED) { + return true; + } + else { + return false; + } + } + else if (sdCard == sd::SLOT_1) { + if (active.second == sd::MOUNTED) { + return true; + } + else { + return false; + } + } + else { + sif::debug << "StarTrackerImageHelper::isSdCardMounted: Unknown SD card specified" << std::endl; + } + return false; +} + +ReturnValue_t StarTrackerImageHelper::prepareUploadCommand(const uint8_t* data, size_t size) { + ReturnValue_t result = RETURN_OK; + result = getImageLocation(data, size); + if (result != RETURN_OK) { + return result; + } + imageSize = std::filesystem::file_size(imageFile); + remainingCommands = imageSize / SIZE_IMAGE_PART ; + if (imageSize % SIZE_IMAGE_PART) { + remainingCommands++; + } + commandsSent = 0; + state = State::SEND_NEXT_UPLOAD_CMD; + return result; +} + +void StarTrackerImageHelper::stepSuccessfulReceived(ActionId_t actionId, + uint8_t step) { +} + +void StarTrackerImageHelper::stepFailedReceived(ActionId_t actionId, uint8_t step, + ReturnValue_t returnCode) { +} + +void StarTrackerImageHelper::dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size) { + +} + +void StarTrackerImageHelper::completionSuccessfulReceived(ActionId_t actionId) { + switch (pendingCommand) { + case (StarTracker::UPLOAD_IMAGE): + if (state == State::UPLOAD_LAST) { + triggerEvent(IMAGE_UPLOAD_FINISHED); + pendingCommand = StarTracker::NONE; + state = State::IDLE; + } + else { + state = State::SEND_NEXT_UPLOAD_CMD; + } + break; + default: + sif::debug << "StarTrackerImageHelper::completionSuccessfulReceived: Invalid pending command" + << std::endl; + state = State::IDLE; + break; + } +} + +void StarTrackerImageHelper::completionFailedReceived(ActionId_t actionId, + ReturnValue_t returnCode) { + switch(pendingCommand) { + case(StarTracker::UPLOAD_IMAGE): { + triggerEvent(IMAGE_UPLOAD_FAILED); + break; + } + default: + sif::debug << "StarTrackerImageHelper::completionFailedReceived: Invalid pending command " + << std::endl; + break; + } + state = State::IDLE; +} + +void StarTrackerImageHelper::commandImageUpload() { + ReturnValue_t result = RETURN_OK; + uint16_t dataLen = 0; + uint8_t tmpCommandBuffer[UPLOAD_COMMAND_SIZE] = {0}; + uint32_t position = commandsSent * SIZE_IMAGE_PART; + + if (not std::filesystem::exists(imageFile)) { + triggerEvent(IMAGE_FILE_NOT_EXISTS, commandsSent); + state = State::IDLE; + return; + } + + std::ifstream file(imageFile, std::ifstream::binary); + file.seekg(position, file.beg); + + if (remainingCommands == 1) { + dataLen = imageSize - file.tellg(); + } + else { + dataLen = SIZE_IMAGE_PART; + } + + size_t size = 0; + size_t maxSize = sizeof(position); + uint8_t* commandBufferPtr = tmpCommandBuffer; + uint8_t** buffer = &commandBufferPtr; + SerializeAdapter::serialize(&position, buffer, &size, maxSize, + SerializeIF::Endianness::BIG); + file.read(reinterpret_cast(tmpCommandBuffer), dataLen); + file.close(); + + result = commandActionHelper.commandAction(objects::START_TRACKER, + StarTracker::UPLOAD_IMAGE, tmpCommandBuffer - size , UPLOAD_COMMAND_SIZE); + + if (result != RETURN_OK) { + sif::warning << "StarTrackerImageHelper::commandImageUpload: Failed to send image " + << "upload command" << std::endl; + triggerEvent(ACTION_COMMANDING_FAILED, result, StarTracker::UPLOAD_IMAGE); + state = State::IDLE; + return; + } + + remainingCommands--; + commandsSent++; + + if (remainingCommands == 0) { + state = State::UPLOAD_LAST; + } + else { + state = State::COMMAND_EXECUTING; + } + pendingCommand = StarTracker::UPLOAD_IMAGE; +} diff --git a/bsp_q7s/devices/StarTrackerImageHelper.h b/bsp_q7s/devices/StarTrackerImageHelper.h new file mode 100644 index 00000000..59dc7b83 --- /dev/null +++ b/bsp_q7s/devices/StarTrackerImageHelper.h @@ -0,0 +1,150 @@ +#ifndef MISSION_DEVICES_STARTRACKERIMAGEHELPER_H_ +#define MISSION_DEVICES_STARTRACKERIMAGEHELPER_H_ + +#include "OBSWConfig.h" +#include "mission/devices/devicedefinitions/StarTrackerDefinitions.h" +#include "fsfw/action/CommandActionHelper.h" +#include "fsfw/action/ActionHelper.h" +#include "fsfw/action/HasActionsIF.h" +#include "fsfw/action/CommandsActionsIF.h" +#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/tasks/ExecutableObjectIF.h" +#include "fsfw/objectmanager/SystemObject.h" +#include "bsp_q7s/memory/SdCardManager.h" +#include "linux/fsfwconfig/objects/systemObjectList.h" + + +/** + * @brief An object of this class helps to download and upload images from/to the star tracker. + * + * @details The star tracker can only receive upload image commands with maximum 1024 bytes of data. + * Thus this class is used to raed the image from the file system and split the upload + * procedure into multiple steps. + * The same applies to downloading images from the star tracker (max. 1024 bytes in image + * download reply). + * + * @author J. Meier + */ +class StarTrackerImageHelper : public SystemObject, + public HasActionsIF, + public ExecutableObjectIF, + public HasReturnvaluesIF, + public CommandsActionsIF { +public: + + static const ActionId_t UPLOAD_IMAGE = 0; + static const ActionId_t DOWNLOAD_IMAGE = 1; + + StarTrackerImageHelper(object_id_t objectId); + virtual ~StarTrackerImageHelper(); + + ReturnValue_t performOperation(uint8_t operationCode = 0) override; + ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, + const uint8_t* data, size_t size); + MessageQueueId_t getCommandQueue() const; + ReturnValue_t initialize() override; + MessageQueueIF* getCommandQueuePtr() override; + void stepSuccessfulReceived(ActionId_t actionId, uint8_t step) override; + void stepFailedReceived(ActionId_t actionId, uint8_t step, ReturnValue_t returnCode) override; + void dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size) override; + void completionSuccessfulReceived(ActionId_t actionId) override; + void completionFailedReceived(ActionId_t actionId, ReturnValue_t returnCode) override; + +private: + + static const uint8_t INTERFACE_ID = CLASS_ID::STR_IMG_HELPER; + + //! [EXPORT] : [COMMENT] Image helper is already executing a command + static const ReturnValue_t IMAGE_HELPER_BUSY = MAKE_RETURN_CODE(0xA0); + //! [EXPORT] : [COMMENT] Invalid path to image location + static const ReturnValue_t NAME_TOO_LONG = MAKE_RETURN_CODE(0xA1); + //! [EXPORT] : [COMMENT] SD card with image not mounted + static const ReturnValue_t SD_NOT_MOUNTED = MAKE_RETURN_CODE(0xA2); + //! [EXPORT] : [COMMENT] Specified image does not exist + static const ReturnValue_t FILE_NOT_EXISTS = MAKE_RETURN_CODE(0xA3); + + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::STR_IMAGE_HELPER; + + //! [EXPORT] : [COMMENT] Try to read image file to upload but the file does not exist. + //! P1: Refers to the upload step the reading fails + static const Event IMAGE_FILE_NOT_EXISTS = MAKE_EVENT(0, severity::LOW); + //! [EXPORT] : [COMMENT] Failed to send command to star tracker handler + //! P1: Return value of CommandActionHelper::commandAction + //! P2: Action ID of command to send + static const Event ACTION_COMMANDING_FAILED = MAKE_EVENT(1, severity::LOW); + //! [EXPORT] : [COMMENT] Star tracker handler replies with completion failure message to upload image command + //! P1: Upload step of the failed command execution + static const Event IMAGE_UPLOAD_FAILED = MAKE_EVENT(2, severity::LOW); + //! [EXPORT] : [COMMENT] Image upload was successful + static const Event IMAGE_UPLOAD_FINISHED = MAKE_EVENT(3, severity::LOW); + + + static const uint32_t QUEUE_SIZE = config::STR_IMG_HELPER_QUEUE_SIZE; + static const size_t MAX_STR_IMAGE_PATH = 50; + static const size_t SD_PREFIX_LENGTH = 8; + // Size of one image part which can be sent per action request + static const size_t SIZE_IMAGE_PART = 1024; + // Position (uint32_t) + image data (1024 bytes) + static const size_t UPLOAD_COMMAND_SIZE = 1028; + + MessageQueueIF* commandQueue = nullptr; + + SdCardManager* sdcMan = nullptr; + + CommandActionHelper commandActionHelper; + + ActionHelper actionHelper; + + enum class State: uint8_t { + IDLE, + SEND_NEXT_UPLOAD_CMD, + UPLOAD_LAST, + COMMAND_EXECUTING + }; + + State state = State::IDLE; + + ActionId_t pendingCommand = StarTracker::NONE; + + uint32_t commandsSent = 0; + uint32_t remainingCommands = 0; + + // Path and name of active image (either upload or download image) + std::string imageFile; + // In case of upload command this variable stores the size of the image to upload + std::uintmax_t imageSize; + + void readCommandQueue(); + void doStateMachine(); + + /** + * @brief Extracts the path and name form the received command. + * + * @param data Pointer to received command + * @param size Size of the received command + * + * @details This string defines the image to upload (must be previously written to the SD card). + * In case of the download image command, this string defines the location and name of + * the image file to create. + */ + ReturnValue_t getImageLocation(const uint8_t* data, size_t size); + + /** + * @brief Prepares properties for the upload image command and changes the state to initiate + * the execution of the upload image command. + */ + ReturnValue_t prepareUploadCommand(const uint8_t* data, size_t size); + + /** + * @brief Reads part of image from file and sends upload image command to star tracker + * handler. + */ + void commandImageUpload(); + + /** + * @brief Checks whether the SD card to read from is mounted or not. + */ + bool isSdCardMounted(sd::SdCard sdCard); +}; + +#endif /* MISSION_DEVICES_STARTRACKERIMAGEHELPER_H_ */ diff --git a/common/config/commonClassIds.h b/common/config/commonClassIds.h index 00b5ca25..b2885009 100644 --- a/common/config/commonClassIds.h +++ b/common/config/commonClassIds.h @@ -19,6 +19,7 @@ enum commonClassIds: uint8_t { CCSDS_IP_CORE_BRIDGE, //IPCI PTME, //PTME PLOC_UPDATER, //PLUD + STR_IMG_HELPER, //STRIMGH GOM_SPACE_HANDLER, //GOMS PLOC_MEMORY_DUMPER, //PLMEMDUMP PDEC_HANDLER, //PDEC diff --git a/common/config/commonObjects.h b/common/config/commonObjects.h index 8cf45e9a..19445dc1 100644 --- a/common/config/commonObjects.h +++ b/common/config/commonObjects.h @@ -90,7 +90,8 @@ enum commonObjects: uint32_t { START_TRACKER = 0x44130001, PLOC_UPDATER = 0x44330000, - PLOC_MEMORY_DUMPER = 0x44330001 + PLOC_MEMORY_DUMPER = 0x44330001, + STR_IMG_HELPER = 0x44330002 }; } diff --git a/common/config/commonSubsystemIds.h b/common/config/commonSubsystemIds.h index 05fda61c..a6a0466d 100644 --- a/common/config/commonSubsystemIds.h +++ b/common/config/commonSubsystemIds.h @@ -18,6 +18,7 @@ enum: uint8_t { PLOC_UPDATER = 117, PLOC_MEMORY_DUMPER = 118, PDEC_HANDLER = 119, + STR_IMAGE_HELPER = 120, COMMON_SUBSYSTEM_ID_END }; } diff --git a/linux/fsfwconfig/OBSWConfig.h.in b/linux/fsfwconfig/OBSWConfig.h.in index 663ef00b..9e2a103e 100644 --- a/linux/fsfwconfig/OBSWConfig.h.in +++ b/linux/fsfwconfig/OBSWConfig.h.in @@ -110,7 +110,7 @@ debugging. */ #define OBSW_DEBUG_STARTRACKER 0 #define OBSW_DEBUG_PLOC_MPSOC 0 #define OBSW_DEBUG_PLOC_SUPERVISOR 0 -#define OBSW_DEBUG_PDEC_HANDLER 0 +#define OBSW_DEBUG_PDEC_HANDLER 1 /*******************************************************************/ /** Hardcoded */ @@ -132,6 +132,7 @@ namespace config { /* Add mission configuration flags here */ static constexpr uint32_t OBSW_FILESYSTEM_HANDLER_QUEUE_SIZE = 50; static constexpr uint32_t PLOC_UPDATER_QUEUE_SIZE = 50; +static constexpr uint32_t STR_IMG_HELPER_QUEUE_SIZE = 50; static constexpr uint8_t LIVE_TM = 0; diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 73a27116..b32ceb5f 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -588,6 +588,7 @@ ReturnValue_t pst::pstUart(FixedTimeslotTaskIF *thisSequence) { thisSequence->addSlot(objects::PLOC_UPDATER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); thisSequence->addSlot(objects::PLOC_MEMORY_DUMPER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::STR_IMG_HELPER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); #if OBSW_ADD_PLOC_SUPERVISOR == 1 thisSequence->addSlot(objects::PLOC_SUPERVISOR_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); diff --git a/mission/devices/StarTrackerHandler.cpp b/mission/devices/StarTrackerHandler.cpp index f8c465e7..0b2048c0 100644 --- a/mission/devices/StarTrackerHandler.cpp +++ b/mission/devices/StarTrackerHandler.cpp @@ -55,7 +55,7 @@ ReturnValue_t StarTrackerHandler::buildTransitionDeviceCommand(DeviceCommandId_t ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t * commandData, size_t commandDataLen) { - + ReturnValue_t result = RETURN_OK; switch (deviceCommand) { case (StarTracker::PING_REQUEST): { preparePingRequest(); @@ -77,6 +77,10 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi prepareInterfaceRequest(); return RETURN_OK; } + case (StarTracker::UPLOAD_IMAGE): { + result = prepareImageUploadCommand(commandData, commandDataLen); + return result; + } case (StarTracker::REQ_POWER): { preparePowerRequest(); return RETURN_OK; @@ -114,6 +118,8 @@ void StarTrackerHandler::fillCommandAndReplyMap() { StarTracker::MAX_FRAME_SIZE * 2 + 2); this->insertInCommandAndReplyMap(StarTracker::REQ_TIME, 3, &timeSet, StarTracker::MAX_FRAME_SIZE * 2 + 2); + this->insertInCommandAndReplyMap(StarTracker::UPLOAD_IMAGE, 3, nullptr, + StarTracker::MAX_FRAME_SIZE * 2 + 2); this->insertInCommandAndReplyMap(StarTracker::REQ_POWER, 3, &powerSet, StarTracker::MAX_FRAME_SIZE * 2 + 2); this->insertInCommandAndReplyMap(StarTracker::REQ_INTERFACE, 3, &interfaceSet, @@ -221,6 +227,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con result = handleInterfaceTm(); break; } + case (StarTracker::UPLOAD_IMAGE): { + result = handleUploadImageReply(); + break; + } case (StarTracker::REQ_POWER): { result = handlePowerTm(); break; @@ -344,6 +354,10 @@ ReturnValue_t StarTrackerHandler::scanForActionReply(DeviceCommandId_t *foundId) *foundId = StarTracker::BOOT; break; } + case (StarTracker::ID::UPLOAD_IMAGE): { + *foundId = StarTracker::UPLOAD_IMAGE; + break; + } default: sif::debug << "StarTrackerHandler::scanForSetParameterReply: Unknown parameter reply id" << std::endl; @@ -457,6 +471,28 @@ void StarTrackerHandler::prepareInterfaceRequest() { rawPacketLen = encLength; } +ReturnValue_t StarTrackerHandler::prepareImageUploadCommand(const uint8_t* commandData, + size_t commandDataLen) { + if (commandDataLen != UPLOAD_COMMAND_LEN) { + return INVALID_UPLOAD_COMMAND; + } + uint32_t length = 0; + uint32_t position = deserializeUint32(commandData); + if (position > MAX_POSITION) { + return MAX_POSITION; + } + rememberUploadPosition = position; + struct UploadActionRequest uploadRequest; + uploadRequest.position = position; + std::memcpy(uploadRequest.data, commandData + 4, commandDataLen - 4); + arc_pack_upload_action_req(&uploadRequest, commandBuffer, &length); + uint32_t encLength = 0; + arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength); + rawPacket = encBuffer; + rawPacketLen = encLength; + return RETURN_OK; +} + void StarTrackerHandler::preparePowerRequest() { uint32_t length = 0; arc_tm_pack_power_req(commandBuffer, &length); @@ -556,11 +592,27 @@ ReturnValue_t StarTrackerHandler::handlePingReply() { sif::info << "Ping id: 0x"<< std::hex << pingId << std::endl; #endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */ if (status != StarTracker::STATUS_OK || pingId != PING_ID) { + sif::warning << "StarTrackerHandler::handlePingReply: Ping failed" << std::endl; result = PING_FAILED; } return result; } +ReturnValue_t StarTrackerHandler::handleUploadImageReply() { + ReturnValue_t result = RETURN_OK; + result = handleActionReply(); + if (result != RETURN_OK) { + return result; + } + uint32_t position = deserializeUint32(decodedFrame + ACTION_DATA_OFFSET); + if (position != rememberUploadPosition) { + sif::warning << "StarTrackerHandler::handleUploadImageReply: Invalid position" + << std::endl; + return UPLOAD_IMAGE_FAILED; + } + return result; +} + ReturnValue_t StarTrackerHandler::handleTimeTm() { ReturnValue_t result = RETURN_OK; result = timeSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT); @@ -595,7 +647,6 @@ ReturnValue_t StarTrackerHandler::handleTimeTm() { ReturnValue_t StarTrackerHandler::handleVersionTm() { ReturnValue_t result = RETURN_OK; - PoolReadGuard rg(&versionSet); uint32_t offset = TM_DATA_FIELD_OFFSET; uint8_t status = 0; uint32_t ticks = 0; @@ -607,6 +658,10 @@ ReturnValue_t StarTrackerHandler::handleVersionTm() { result = VERSION_REQ_FAILED; return result; } + result = versionSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT); + if (result != RETURN_OK) { + return result; + } versionSet.ticks = ticks; versionSet.time = time; versionSet.program = *(decodedFrame + offset); @@ -614,24 +669,18 @@ ReturnValue_t StarTrackerHandler::handleVersionTm() { versionSet.major = *(decodedFrame + offset); offset += 1; versionSet.minor = *(decodedFrame + offset); + result = versionSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT); + if (result != RETURN_OK) { + return result; + } #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 - sif::info << "StarTrackerHandler::handleVersionTm: Ticks: " - << versionSet.ticks << std::endl; - sif::info << "StarTrackerHandler::handleVersionTm: Unix Time: " - << versionSet.time << " us" << std::endl; - sif::info << "StarTrackerHandler::handleVersionTm: Program: " - << static_cast(versionSet.program.value) << std::endl; - sif::info << "StarTrackerHandler::handleVersionTm: Major: " - << static_cast(versionSet.major.value) << std::endl; - sif::info << "StarTrackerHandler::handleVersionTm: Minor: " - << static_cast(versionSet.minor.value) << std::endl; + versionSet.printSet(); #endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */ return result; } ReturnValue_t StarTrackerHandler::handleInterfaceTm() { ReturnValue_t result = RETURN_OK; - PoolReadGuard rg(&interfaceSet); uint32_t offset = TM_DATA_FIELD_OFFSET; uint8_t status = 0; uint32_t ticks = 0; @@ -643,59 +692,28 @@ ReturnValue_t StarTrackerHandler::handleInterfaceTm() { result = INTERFACE_REQ_FAILED; return result; } + result = interfaceSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT); + if (result != RETURN_OK) { + return result; + } interfaceSet.ticks = ticks; interfaceSet.time = time; size_t size = sizeof(uint32_t); interfaceSet.frameCount = deserializeUint32(decodedFrame + offset); offset += size; interfaceSet.checksumerrorCount = deserializeUint32(decodedFrame + offset); - offset += size; - interfaceSet.setParamCount = deserializeUint32(decodedFrame + offset); - offset += size; - interfaceSet.setParamReplyCount = deserializeUint32(decodedFrame + offset); - offset += size; - interfaceSet.paramRequestCount = deserializeUint32(decodedFrame + offset); - offset += size; - interfaceSet.paramReplyCount = deserializeUint32(decodedFrame + offset); - offset += size; - interfaceSet.tmReplyCount = deserializeUint32(decodedFrame + offset); - offset += size; - interfaceSet.actionReqCount = deserializeUint32(decodedFrame + offset); - offset += size; - interfaceSet.actionReplyCount = deserializeUint32(decodedFrame + offset); - + result = interfaceSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT); + if (result != RETURN_OK) { + return result; + } #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 - sif::info << "StarTrackerHandler::handleInterfaceTm: Ticks: " - << interfaceSet.ticks << std::endl; - sif::info << "StarTrackerHandler::handleInterfaceTm: Time: " - << interfaceSet.time << " us" << std::endl; - sif::info << "StarTrackerHandler::handleInterfaceTm: Frame Count: " - << interfaceSet.frameCount << std::endl; - sif::info << "StarTrackerHandler::handleInterfaceTm: Checksum Error Count: " - << interfaceSet.checksumerrorCount << std::endl; - sif::info << "StarTrackerHandler::handleInterfaceTm: Set Param Count: " - << interfaceSet.setParamCount << std::endl; - sif::info << "StarTrackerHandler::handleInterfaceTm: Set Param Reply Count: " - << interfaceSet.setParamReplyCount << std::endl; - sif::info << "StarTrackerHandler::handleInterfaceTm: Param Request Count: " - << interfaceSet.paramRequestCount << std::endl; - sif::info << "StarTrackerHandler::handleInterfaceTm: Param Reply Count: " - << interfaceSet.paramReplyCount << std::endl; - sif::info << "StarTrackerHandler::handleInterfaceTm: Req TM Count: " - << interfaceSet.reqTmCount << std::endl; - sif::info << "StarTrackerHandler::handleInterfaceTm: Telemetry Reply Count: " - << interfaceSet.tmReplyCount << std::endl; - sif::info << "StarTrackerHandler::handleInterfaceTm: Action Request Count: " - << interfaceSet.actionReqCount << std::endl; - sif::info << "StarTrackerHandler::handleInterfaceTm: Action Reply Count: " - << interfaceSet.actionReplyCount << std::endl; + interfaceSet.printSet(); #endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */ return result; } ReturnValue_t StarTrackerHandler::handlePowerTm() { ReturnValue_t result = RETURN_OK; - PoolReadGuard rg(&powerSet); uint32_t offset = TM_DATA_FIELD_OFFSET; uint8_t status = 0; uint32_t ticks = 0; @@ -707,6 +725,10 @@ ReturnValue_t StarTrackerHandler::handlePowerTm() { result = POWER_REQ_FAILED; return result; } + result = powerSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT); + if (result != RETURN_OK) { + return result; + } powerSet.ticks= ticks; powerSet.time= time; float value = 0; @@ -757,43 +779,12 @@ ReturnValue_t StarTrackerHandler::handlePowerTm() { offset += 4; std::memcpy(&value, decodedFrame + offset, sizeof(value)); powerSet.cmvResVoltage = value; + result = powerSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT); + if (result != RETURN_OK) { + return result; + } #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 - sif::info << "StarTrackerHandler::handlePowerTm: Ticks: " - << powerSet.ticks << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: Time: " - << powerSet.time << " us" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: MCU Current: " - << powerSet.mcuCurrent << " A" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: MCU Voltage: " - << powerSet.mcuVoltage << " V" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: FPGA Core current: " - << powerSet.fpgaCoreCurrent << " A" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: FPGA Core voltage: " - << powerSet.fpgaCoreVoltage << " V" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: FPGA 18 current: " - << powerSet.fpga18Current << " A" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: FPGA 18 voltage: " - << powerSet.fpga18Voltage << " V" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: FPGA 25 current: " - << powerSet.fpga25Current << " A" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: FPGA 25 voltage: " - << powerSet.fpga25Voltage << " V" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: CMV 21 current: " - << powerSet.cmv21Current << " A" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: CMV 21 voltage: " - << powerSet.cmv21Voltage << " V" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: CMV Pix current: " - << powerSet.cmvPixCurrent << " A" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: CMV Pix voltage: " - << powerSet.cmvPixVoltage << " V" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: CMV 33 current: " - << powerSet.cmv33Current << " A" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: CMV 33 voltage: " - << powerSet.cmv33Voltage << " V" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: CMV Res current: " - << powerSet.cmvResCurrent << " A" << std::endl; - sif::info << "StarTrackerHandler::handlePowerTm: CMV Res voltage: " - << powerSet.cmvResVoltage << " V" << std::endl; + powerSet.printSet(); #endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */ return result; } @@ -905,7 +896,7 @@ ReturnValue_t StarTrackerHandler::handleTemperatureTm() { float temperature = 0; std::memcpy(&temperature, decodedFrame + offset, sizeof(temperature)); temperatureSet.mcuTemperature = temperature; - offset += sizeof(temperature); + offset += sizeof(float); std::memcpy(&temperature, decodedFrame + offset, sizeof(temperature)); temperatureSet.cmosTemperature = temperature; result = temperatureSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT); @@ -924,7 +915,7 @@ void StarTrackerHandler::getTmHeaderData(uint8_t* status, uint32_t* ticks, uint6 *time = deserializeUint64(decodedFrame + TIME_OFFSET); } -uint32_t StarTrackerHandler::deserializeUint32(uint8_t* buffer) { +uint32_t StarTrackerHandler::deserializeUint32(const uint8_t* buffer) { uint32_t word = 0; word = *(buffer + 3) << 24 | *(buffer + 2) << 16 diff --git a/mission/devices/StarTrackerHandler.h b/mission/devices/StarTrackerHandler.h index 67b957de..e082cf46 100644 --- a/mission/devices/StarTrackerHandler.h +++ b/mission/devices/StarTrackerHandler.h @@ -78,13 +78,26 @@ private: static const ReturnValue_t SET_PARAM_FAILED = MAKE_RETURN_CODE(0xA6); //! [EXPORT] : [COMMENT] Status of reply to action command signals error static const ReturnValue_t ACTION_FAILED = MAKE_RETURN_CODE(0xA7); + //! [EXPORT] : [COMMENT] Received upload image command with invalid length + static const ReturnValue_t UPLOAD_TOO_SHORT = MAKE_RETURN_CODE(0xA8); + //! [EXPORT] : [COMMENT] Received upload image command with invalid position field + static const ReturnValue_t UPLOAD_INVALID_POSITION = MAKE_RETURN_CODE(0xA8); + //! [EXPORT] : [COMMENT] Position value in upload image reply not matching sent position + static const ReturnValue_t UPLOAD_IMAGE_FAILED = MAKE_RETURN_CODE(0xA9); + //! [EXPORT] : [COMMENT] Received upload image command with invalid length + static const ReturnValue_t INVALID_UPLOAD_COMMAND = MAKE_RETURN_CODE(0xA9); + // position (uint32) + 1024 image data + static const size_t UPLOAD_COMMAND_LEN = 1028; + // Max valid position value in upload image command + static const uint16_t MAX_POSITION= 3071; static const uint8_t STATUS_OFFSET = 2; static const uint8_t TICKS_OFFSET = 3; static const uint8_t TIME_OFFSET = 7; static const uint8_t TM_DATA_FIELD_OFFSET = 15; static const uint8_t PARAMETER_ID_OFFSET = 1; static const uint8_t ACTION_ID_OFFSET = 1; + static const uint8_t ACTION_DATA_OFFSET = 3; // Ping request will reply ping with this ID (data field) static const uint32_t PING_ID = 0x55; @@ -115,6 +128,8 @@ private: InternalState internalState = InternalState::TEMPERATURE_REQUEST; + uint32_t rememberUploadPosition = 0; + /** * @brief This function initializes the serial link ip protocol struct slipInfo. */ @@ -150,6 +165,11 @@ private: */ void prepareInterfaceRequest(); + /** + * @brief Fills the command buffer with data to upload part of an image. + */ + ReturnValue_t prepareImageUploadCommand(const uint8_t* commandData, size_t commandDataLen); + /** * @brief Fills the command buffer with data to request the power telemetry packet. */ @@ -190,6 +210,11 @@ private: ReturnValue_t handlePingReply(); + /** + * @brief Handles reply to upload image command + */ + ReturnValue_t handleUploadImageReply(); + /** * @brief Fills the time set with the data of the time request reply. */ @@ -234,7 +259,7 @@ private: * * @note Deserialization will be performed in little endian byte order */ - uint32_t deserializeUint32(uint8_t* buffer); + uint32_t deserializeUint32(const uint8_t* buffer); /** * @brief This function deserializes 8 bytes into a 64 bit unsigned integer. diff --git a/mission/devices/devicedefinitions/StarTrackerDefinitions.h b/mission/devices/devicedefinitions/StarTrackerDefinitions.h index 8546f843..ef9182ff 100644 --- a/mission/devices/devicedefinitions/StarTrackerDefinitions.h +++ b/mission/devices/devicedefinitions/StarTrackerDefinitions.h @@ -91,10 +91,12 @@ static const DeviceCommandId_t REQ_VERSION = 2; static const DeviceCommandId_t REQ_INTERFACE = 3; static const DeviceCommandId_t REQ_TIME = 4; static const DeviceCommandId_t REBOOT = 7; +static const DeviceCommandId_t UPLOAD_IMAGE = 10; static const DeviceCommandId_t REQ_POWER = 11; static const DeviceCommandId_t SUBSCRIBE_TO_TM = 18; static const DeviceCommandId_t REQ_SOLUTION = 24; static const DeviceCommandId_t REQ_TEMPERATURE = 25; +static const DeviceCommandId_t NONE = 0xFFFFFFFF; static const uint32_t VERSION_SET_ID = REQ_VERSION; static const uint32_t INTERFACE_SET_ID = REQ_INTERFACE; @@ -108,7 +110,7 @@ static const size_t MAX_FRAME_SIZE = 1200; static const uint8_t TEMPERATURE_SET_ENTRIES = 4; static const uint8_t VERSION_SET_ENTRIES = 5; -static const uint8_t INTERFACE_SET_ENTRIES = 12; +static const uint8_t INTERFACE_SET_ENTRIES = 4; static const uint8_t POWER_SET_ENTRIES = 18; static const uint8_t TIME_SET_ENTRIES = 4; static const uint8_t SOLUTION_SET_ENTRIES = 23; @@ -120,6 +122,7 @@ namespace ID { static const uint8_t VERSION = 2; static const uint8_t INTERFACE = 3; static const uint8_t REBOOT = 7; + static const uint8_t UPLOAD_IMAGE = 10; static const uint8_t POWER = 11; static const uint8_t SUBSCRIBE = 18; static const uint8_t SOLUTION = 24; @@ -161,7 +164,7 @@ public: sif::info << "TemperatureSet::printSet: MCU Temperature: " << this->mcuTemperature << " °C" << std::endl; sif::info << "TemperatureSet::printSet: CMOS Temperature: " - << this->mcuTemperature << " °C" << std::endl; + << this->cmosTemperature << " °C" << std::endl; } }; @@ -192,6 +195,20 @@ public: PoolIds::MAJOR, this); lp_var_t minor = lp_var_t(sid.objectId, PoolIds::MINOR, this); + + void printSet() { + PoolReadGuard rg(this); + sif::info << "VersionSet::printSet: Ticks: " + << this->ticks << std::endl; + sif::info << "VersionSet::printSet: Unix Time: " + << this->time << " us" << std::endl; + sif::info << "VersionSet::printSet: Program: " + << static_cast(this->program.value) << std::endl; + sif::info << "VersionSet::printSet: Major: " + << static_cast(this->major.value) << std::endl; + sif::info << "VersionSet::printSet: Minor: " + << static_cast(this->minor.value) << std::endl; + } }; /** @@ -219,22 +236,18 @@ public: PoolIds::FRAME_COUNT, this); lp_var_t checksumerrorCount = lp_var_t(sid.objectId, PoolIds::CHECKSUM_ERROR_COUNT, this); - lp_var_t setParamCount = lp_var_t(sid.objectId, - PoolIds::SET_PARAM_COUNT, this); - lp_var_t setParamReplyCount = lp_var_t(sid.objectId, - PoolIds::SET_PARAM_REPLY_COUNT, this); - lp_var_t paramRequestCount = lp_var_t(sid.objectId, - PoolIds::PARAM_REQUEST_COUNT, this); - lp_var_t paramReplyCount = lp_var_t(sid.objectId, - PoolIds::PARAM_REPLY_COUNT, this); - lp_var_t reqTmCount = lp_var_t(sid.objectId, - PoolIds::REQ_TM_COUNT, this); - lp_var_t tmReplyCount = lp_var_t(sid.objectId, - PoolIds::TM_REPLY_COUNT, this); - lp_var_t actionReqCount = lp_var_t(sid.objectId, - PoolIds::ACTION_REQ_COUNT, this); - lp_var_t actionReplyCount = lp_var_t(sid.objectId, - PoolIds::ACTION_REPLY_COUNT, this); + + void printSet() { + PoolReadGuard rg(this); + sif::info << "InterfaceSet::printSet: Ticks: " + << this->ticks << std::endl; + sif::info << "InterfaceSet::printSet: Time: " + << this->time << " us" << std::endl; + sif::info << "InterfaceSet::printSet: Frame Count: " + << this->frameCount << std::endl; + sif::info << "InterfaceSet::printSet: Checksum Error Count: " + << this->checksumerrorCount << std::endl; + } }; /** @@ -290,6 +303,46 @@ public: PoolIds::CMV_RES_CURRENT, this); lp_var_t cmvResVoltage = lp_var_t(sid.objectId, PoolIds::CMV_RES_VOLTAGE, this); + + void printSet() { + PoolReadGuard rg(this); + sif::info << "PowerSet::printSet: Ticks: " + << this->ticks << std::endl; + sif::info << "PowerSet::printSet: Time: " + << this->time << " us" << std::endl; + sif::info << "PowerSet::printSet: MCU Current: " + << this->mcuCurrent << " A" << std::endl; + sif::info << "PowerSet::printSet: MCU Voltage: " + << this->mcuVoltage << " V" << std::endl; + sif::info << "PowerSet::printSet: FPGA Core current: " + << this->fpgaCoreCurrent << " A" << std::endl; + sif::info << "PowerSet::printSet: FPGA Core voltage: " + << this->fpgaCoreVoltage << " V" << std::endl; + sif::info << "PowerSet::printSet: FPGA 18 current: " + << this->fpga18Current << " A" << std::endl; + sif::info << "PowerSet::printSet: FPGA 18 voltage: " + << this->fpga18Voltage << " V" << std::endl; + sif::info << "PowerSet::printSet: FPGA 25 current: " + << this->fpga25Current << " A" << std::endl; + sif::info << "PowerSet::printSet: FPGA 25 voltage: " + << this->fpga25Voltage << " V" << std::endl; + sif::info << "PowerSet::printSet: CMV 21 current: " + << this->cmv21Current << " A" << std::endl; + sif::info << "PowerSet::printSet: CMV 21 voltage: " + << this->cmv21Voltage << " V" << std::endl; + sif::info << "PowerSet::printSet: CMV Pix current: " + << this->cmvPixCurrent << " A" << std::endl; + sif::info << "PowerSet::printSet: CMV Pix voltage: " + << this->cmvPixVoltage << " V" << std::endl; + sif::info << "PowerSet::printSet: CMV 33 current: " + << this->cmv33Current << " A" << std::endl; + sif::info << "PowerSet::printSet: CMV 33 voltage: " + << this->cmv33Voltage << " V" << std::endl; + sif::info << "PowerSet::printSet: CMV Res current: " + << this->cmvResCurrent << " A" << std::endl; + sif::info << "PowerSet::printSet: CMV Res voltage: " + << this->cmvResVoltage << " V" << std::endl; + } }; /**