diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 087780eb..bf31c99a 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -172,6 +172,35 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu ReturnValue_t result = returnvalue::OK; switch (actionId) { + case (startracker::ADD_SECONDARY_TM_TO_NORMAL_MODE): { + if (size < 4) { + return HasActionsIF::INVALID_PARAMETERS; + } + DeviceCommandId_t idToAdd; + result = + SerializeAdapter::deSerialize(&idToAdd, data, &size, SerializeIF::Endianness::NETWORK); + if (result != returnvalue::OK) { + return result; + } + addSecondaryTmForNormalMode(idToAdd); + return EXECUTION_FINISHED; + } + case (startracker::RESET_SECONDARY_TM_SET): { + resetSecondaryTmSet(); + return EXECUTION_FINISHED; + } + case (startracker::READ_SECONDARY_TM_SET): { + std::vector dataVec(additionalRequestedTm.size() * 4); + unsigned idx = 0; + size_t serLen = 0; + for (const auto& cmd : additionalRequestedTm) { + SerializeAdapter::serialize(&cmd, dataVec.data() + idx * 4, &serLen, dataVec.size(), + SerializeIF::Endianness::NETWORK); + idx++; + } + actionHelper.reportData(commandedBy, actionId, dataVec.data(), dataVec.size()); + return EXECUTION_FINISHED; + } case (startracker::STOP_IMAGE_LOADER): { strHelper->stopProcess(); return EXECUTION_FINISHED; @@ -914,6 +943,15 @@ ReturnValue_t StarTrackerHandler::statusFieldCheck(const uint8_t* rawFrame) { return returnvalue::OK; } +void StarTrackerHandler::addSecondaryTmForNormalMode(DeviceCommandId_t cmd) { + additionalRequestedTm.emplace(cmd); +} + +void StarTrackerHandler::resetSecondaryTmSet() { + additionalRequestedTm.clear(); + additionalRequestedTm.emplace(startracker::REQ_TEMPERATURE); +} + void StarTrackerHandler::bootBootloader() { if (internalState == InternalState::IDLE) { internalState = InternalState::BOOT_BOOTLOADER; diff --git a/mission/acs/str/StarTrackerHandler.h b/mission/acs/str/StarTrackerHandler.h index ac5812a9..b239559a 100644 --- a/mission/acs/str/StarTrackerHandler.h +++ b/mission/acs/str/StarTrackerHandler.h @@ -357,6 +357,18 @@ class StarTrackerHandler : public DeviceHandlerBase { */ ReturnValue_t executeFlashReadCommand(const uint8_t* commandData, size_t commandDataLen); + /** + * Add a TM request to the list of telemetry which will be polled in the secondary step of + * the device communication. + * @param cmd + */ + void addSecondaryTmForNormalMode(DeviceCommandId_t cmd); + + /** + * Reset the secondary set, which will only contain a TEMPERATURE set request after the reset. + */ + void resetSecondaryTmSet(); + /** * @brief Fills command buffer with data to boot image (works only when star tracker is * in bootloader mode). diff --git a/mission/acs/str/strHelpers.h b/mission/acs/str/strHelpers.h index 8bd63512..31ac0c05 100644 --- a/mission/acs/str/strHelpers.h +++ b/mission/acs/str/strHelpers.h @@ -365,6 +365,9 @@ static constexpr DeviceCommandId_t REQ_BLOB = 90; static constexpr DeviceCommandId_t REQ_BLOBS = 91; static constexpr DeviceCommandId_t REQ_CENTROID = 92; static constexpr DeviceCommandId_t REQ_CENTROIDS = 93; +static constexpr DeviceCommandId_t ADD_SECONDARY_TM_TO_NORMAL_MODE = 94; +static constexpr DeviceCommandId_t RESET_SECONDARY_TM_SET = 95; +static constexpr DeviceCommandId_t READ_SECONDARY_TM_SET = 96; static const DeviceCommandId_t NONE = 0xFFFFFFFF; static const uint32_t VERSION_SET_ID = REQ_VERSION;