From b8d010cd39a4a1e08bf04b6c7bbb4d78c7dd0ef6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 15 Apr 2023 13:01:41 +0200 Subject: [PATCH 001/506] CHANGELOG + FS helpers --- CHANGELOG.md | 14 +++ bsp_q7s/core/CoreController.cpp | 201 +++++++++++++++++++++++++------- bsp_q7s/core/CoreController.h | 7 ++ mission/sysDefs.h | 199 ++++++++++++++++++++++++++++++- 4 files changed, 379 insertions(+), 42 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6c3e4e50..fde054ed 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,20 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Added + +- Added `mv`, `cp` and `rm` action helpers for the core controller for common filesystem operations. +- Extended directory listing helpers. There is now a directory listing helper which dumps the + directory listing as an action data reply immediately. For smaller directory listings, this + allows a listing without requiring a separate file downlink (which also has not been implemented + yet) + +## Changed + +- The directory listing action commands now allow compressing of either the target output file + for the directory listing into file action command, or compression in the helper which dumps + the directory listing directly. + # [v1.45.0] 2023-04-14 - q7s-package: v2.5.0 diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 49145f9f..d9d30119 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -233,6 +233,67 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ case (LIST_DIRECTORY_INTO_FILE): { return actionListDirectoryIntoFile(actionId, commandedBy, data, size); } + case (LIST_DIRECTORY_DUMP_DIRECTLY): { + return actionListDirectoryDumpDirectly(actionId, commandedBy, data, size); + } + case (CP_HELPER): { + CpHelperParser parser(data, size); + ReturnValue_t result = parser.parse(); + if (result != returnvalue::OK) { + return result; + } + std::ostringstream oss("cp ", std::ostringstream::ate); + if (parser.isRecursiveOptSet()) { + oss << "-r "; + } + auto &sourceTgt = parser.destTgtPair(); + oss << sourceTgt.sourceName << " " << sourceTgt.targetName; + int ret = std::system(oss.str().c_str()); + if (ret != 0) { + return returnvalue::FAILED; + } + return EXECUTION_FINISHED; + } + case (MV_HELPER): { + MvHelperParser parser(data, size); + ReturnValue_t result = parser.parse(); + if (result != returnvalue::OK) { + return result; + } + std::ostringstream oss("mv ", std::ostringstream::ate); + auto &sourceTgt = parser.destTgtPair(); + oss << sourceTgt.sourceName << " " << sourceTgt.targetName; + int ret = std::system(oss.str().c_str()); + if (ret != 0) { + return returnvalue::FAILED; + } + return EXECUTION_FINISHED; + } + case (RM_HELPER): { + RmHelperParser parser(data, size); + ReturnValue_t result = parser.parse(); + if (result != returnvalue::OK) { + return result; + } + std::ostringstream oss("rm ", std::ostringstream::ate); + if (parser.isRecursiveOptSet() or parser.isForceOptSet()) { + oss << "-"; + } + if (parser.isRecursiveOptSet()) { + oss << "r"; + } + if (parser.isForceOptSet()) { + oss << "f"; + } + size_t removeTargetSize = 0; + const char *removeTgt = parser.getRemoveTarget(removeTargetSize); + oss << " " << removeTgt; + int ret = std::system(oss.str().c_str()); + if (ret != 0) { + return returnvalue::FAILED; + } + return EXECUTION_FINISHED; + } case (SWITCH_REBOOT_FILE_HANDLING): { if (size < 1) { return HasActionsIF::INVALID_PARAMETERS; @@ -911,57 +972,115 @@ ReturnValue_t CoreController::initVersionFile() { return returnvalue::OK; } -ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId, - MessageQueueId_t commandedBy, - const uint8_t *data, size_t size) { - // TODO: Packet definition for clean deserialization - // 2 bytes for a and R flag, at least 5 bytes for minimum valid path /tmp with - // null termination, at least 7 bytes for minimum target file name /tmp/a with - // null termination. - if (size < 14) { - return HasActionsIF::INVALID_PARAMETERS; +ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionId, + MessageQueueId_t commandedBy, + const uint8_t *data, size_t size) { + core::ListDirectoryCmdBase parser(data, size); + ReturnValue_t result = parser.parse(); + if (result != returnvalue::OK) { + return result; } - // We could also make -l optional, but I can't think of a reason why to not use -l.. - // This flag specifies to run ls with -a - bool aFlag = data[0]; - data += 1; - // This flag specifies to run ls with -R - bool RFlag = data[1]; - data += 1; - - size_t remainingSize = size - 2; - // One larger for null termination, which prevents undefined behaviour if the sent - // strings are not 0 terminated properly - std::vector repoAndTargetFileBuffer(remainingSize + 1, 0); - std::memcpy(repoAndTargetFileBuffer.data(), data, remainingSize); - const char *currentCharPtr = reinterpret_cast(repoAndTargetFileBuffer.data()); - // Full target file name - std::string repoName(currentCharPtr); - size_t repoLength = repoName.length(); - // The other string needs to be at least one letter plus NULL termination to be valid at all - // The first string also needs to be NULL terminated, but the termination is not included - // in the string length, so this is subtracted from the remaining size as well - if (repoLength > remainingSize - 3) { - return HasActionsIF::INVALID_PARAMETERS; - } - // The file length will not include the NULL termination, so we skip it - currentCharPtr += repoLength + 1; - std::string targetFileName(currentCharPtr); - std::ostringstream oss; + std::ostringstream oss("ls -l", std::ostringstream::ate); oss << "ls -l"; - if (aFlag) { + if (parser.aFlagSet()) { oss << "a"; } - if (RFlag) { + if (parser.rFlagSet()) { oss << "R"; } + size_t repoNameLen = 0; + const char *repoName = parser.getRepoName(repoNameLen); + + oss << " " << repoName << " > " << LIST_DIR_DUMP_WORK_FILE; + int ret = std::system(oss.str().c_str()); + if (ret != 0) { + utility::handleSystemError(result, "CoreController::actionListDirectoryDumpDirectly"); + return returnvalue::FAILED; + } + if (parser.compressionOptionSet()) { + std::string compressedName = LIST_DIR_DUMP_WORK_FILE + std::string(".tar.xz"); + oss.str(""); + oss << "tar -cJvf /tmp/dir_listing_compressed.tmp " << LIST_DIR_DUMP_WORK_FILE; + ret = std::system(oss.str().c_str()); + if (ret != 0) { + utility::handleSystemError(result, "CoreController::actionListDirectoryDumpDirectly"); + return returnvalue::FAILED; + } + oss.str(""); + // Overwrite the old work file with the compressed archive. + oss << "mv /tmp/dir_listing_compressed.tmp " << LIST_DIR_DUMP_WORK_FILE; + ret = std::system(oss.str().c_str()); + if (ret != 0) { + utility::handleSystemError(result, "CoreController::actionListDirectoryDumpDirectly"); + return returnvalue::FAILED; + } + } + std::array dirListingBuf{}; + std::ifstream ifile("/tmp/dir_listing.tmp", std::ios::binary); + std::error_code e; + size_t totalFileSize = std::filesystem::file_size(LIST_DIR_DUMP_WORK_FILE, e); + size_t dumpedBytes = 0; + size_t nextDumpLen = 0; + while (dumpedBytes < totalFileSize) { + nextDumpLen = 1024; + if (totalFileSize - dumpedBytes < 1024) { + nextDumpLen = totalFileSize - dumpedBytes; + } + ifile.read(reinterpret_cast(dirListingBuf.data()), nextDumpLen); + result = actionHelper.reportData(commandedBy, actionId, dirListingBuf.data(), nextDumpLen); + if (result != returnvalue::OK) { + // Remove work file when we are done + std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); + return result; + } + } + // Remove work file when we are done + std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); + return returnvalue::OK; +} + +ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId, + MessageQueueId_t commandedBy, + const uint8_t *data, size_t size) { + core::ListDirectoryIntoFile parser(data, size); + ReturnValue_t result = parser.parse(); + if (result != returnvalue::OK) { + return result; + } + + std::ostringstream oss("ls -l", std::ostringstream::ate); + oss << "ls -l"; + if (parser.aFlagSet()) { + oss << "a"; + } + if (parser.rFlagSet()) { + oss << "R"; + } + + size_t repoNameLen = 0; + const char *repoName = parser.getRepoName(repoNameLen); + size_t targetFileNameLen = 0; + const char *targetFileName = parser.getTargetName(targetFileNameLen); oss << " " << repoName << " > " << targetFileName; - int result = std::system(oss.str().c_str()); - if (result != 0) { + sif::info << "Executing list directory request, command: " << oss.str() << std::endl; + int ret = std::system(oss.str().c_str()); + if (ret != 0) { utility::handleSystemError(result, "CoreController::actionListDirectoryIntoFile"); - actionHelper.finish(false, commandedBy, actionId); + return returnvalue::FAILED; + } + + if (parser.compressionOptionSet()) { + std::string compressedName = targetFileName + std::string(".tar.xz"); + oss.str(""); + oss << "tar -cJvf " << compressedName << " " << targetFileName; + sif::info << "Compressing directory listing: " << oss.str() << std::endl; + ret = std::system(oss.str().c_str()); + if (ret != 0) { + utility::handleSystemError(result, "CoreController::actionListDirectoryIntoFile"); + return returnvalue::FAILED; + } } return returnvalue::OK; } diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index c1f5e40d..d44907e8 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -66,6 +66,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe static constexpr char CHIP_0_COPY_1_MOUNT_DIR[] = "/tmp/mntupdate-xdi-qspi0-gold-rootfs"; static constexpr char CHIP_1_COPY_0_MOUNT_DIR[] = "/tmp/mntupdate-xdi-qspi1-nom-rootfs"; static constexpr char CHIP_1_COPY_1_MOUNT_DIR[] = "/tmp/mntupdate-xdi-qspi1-gold-rootfs"; + static constexpr char LIST_DIR_DUMP_WORK_FILE[] = "/tmp/dir_listing.tmp"; static constexpr dur_millis_t INIT_SD_CARD_CHECK_TIMEOUT = 5000; static constexpr dur_millis_t DEFAULT_SD_CARD_CHECK_TIMEOUT = 60000; @@ -250,6 +251,12 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe ReturnValue_t actionListDirectoryIntoFile(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size); + ReturnValue_t actionListDirectoryDumpDirectly(ActionId_t actionId, MessageQueueId_t commandedBy, + const uint8_t* data, size_t size); + + ReturnValue_t actionListDirectoryCommonCommandCreator(const uint8_t* data, size_t size, + std::ostringstream& oss); + ReturnValue_t actionXscReboot(const uint8_t* data, size_t size); ReturnValue_t actionReboot(const uint8_t* data, size_t size); diff --git a/mission/sysDefs.h b/mission/sysDefs.h index 424b5752..3ec3f7e0 100644 --- a/mission/sysDefs.h +++ b/mission/sysDefs.h @@ -32,7 +32,6 @@ static constexpr char VERSION_FILE_NAME[] = "version.txt"; static constexpr char REBOOT_FILE_NAME[] = "reboot.txt"; static constexpr char TIME_FILE_NAME[] = "time_backup.txt"; -static constexpr ActionId_t LIST_DIRECTORY_INTO_FILE = 0; static constexpr ActionId_t ANNOUNCE_VERSION = 1; static constexpr ActionId_t ANNOUNCE_CURRENT_IMAGE = 2; static constexpr ActionId_t ANNOUNCE_BOOT_COUNTS = 3; @@ -59,6 +58,12 @@ static constexpr ActionId_t EXECUTE_SHELL_CMD_BLOCKING = 40; static constexpr ActionId_t EXECUTE_SHELL_CMD_NON_BLOCKING = 41; static constexpr ActionId_t SYSTEMCTL_CMD_EXECUTOR = 42; +static constexpr ActionId_t LIST_DIRECTORY_INTO_FILE = 50; +static constexpr ActionId_t LIST_DIRECTORY_DUMP_DIRECTLY = 51; +static constexpr ActionId_t CP_HELPER = 52; +static constexpr ActionId_t MV_HELPER = 53; +static constexpr ActionId_t RM_HELPER = 54; + static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::CORE; static constexpr Event ALLOC_FAILURE = event::makeEvent(SUBSYSTEM_ID, 0, severity::MEDIUM); @@ -96,6 +101,198 @@ static constexpr Event I2C_REBOOT = event::makeEvent(SUBSYSTEM_ID, 11, severity: //! [EXPORT] : [COMMENT] PDEC recovery through reset was not possible, performing full reboot. static constexpr Event PDEC_REBOOT = event::makeEvent(SUBSYSTEM_ID, 12, severity::HIGH); +class ListDirectoryCmdBase { + public: // TODO: Packet definition for clean deserialization + // 3 bytes for a and R flag, at least 5 bytes for minimum valid path /tmp with + // null termination + static constexpr size_t MIN_DATA_LEN = 8; + + ListDirectoryCmdBase(const uint8_t* data, size_t maxSize) : data(data), maxSize(maxSize) {} + virtual ~ListDirectoryCmdBase() = default; + + virtual ReturnValue_t parse() { + if (maxSize < MIN_DATA_LEN) { + return SerializeIF::STREAM_TOO_SHORT; + } + aFlag = data[0]; + rFlag = data[1]; + compressOption = data[2]; + + repoNameLen = strnlen(reinterpret_cast(data + 3), maxSize - 3); + // Last byte MUST be null terminated! + if (repoNameLen >= maxSize - 3) { + return HasActionsIF::INVALID_PARAMETERS; + } + repoName = reinterpret_cast(data + 3); + return returnvalue::OK; + } + + bool aFlagSet() const { return this->aFlag; } + bool rFlagSet() const { return this->rFlag; } + + bool compressionOptionSet() const { return this->compressOption; } + + const char* getRepoName(size_t& repoNameLen) const { + return this->repoName; + repoNameLen = this->repoNameLen; + } + + size_t getBaseSize() { + // Include NULL termination + if (repoName != nullptr) { + return 3 + repoNameLen + 1; + } + return 0; + } + + protected: + const uint8_t* data; + size_t maxSize; + + bool aFlag = false; + bool rFlag = false; + bool compressOption = false; + const char* repoName = nullptr; + size_t repoNameLen = 0; +}; + +class ListDirectoryIntoFile : public ListDirectoryCmdBase { + public: + // TODO: Packet definition for clean deserialization + // 3 bytes for a and R flag, at least 5 bytes for minimum valid path /tmp with + // null termination, at least 7 bytes for minimum target file name /tmp/a with + // null termination. + static constexpr size_t MIN_DATA_LEN = 15; + + ListDirectoryIntoFile(const uint8_t* data, size_t maxSize) + : ListDirectoryCmdBase(data, maxSize) {} + ReturnValue_t parse() override { + if (maxSize < MIN_DATA_LEN) { + return SerializeIF::STREAM_TOO_SHORT; + } + ReturnValue_t result = ListDirectoryCmdBase::parse(); + if (result != returnvalue::OK) { + return result; + } + + targetNameLen = + strnlen(reinterpret_cast(data + getBaseSize()), maxSize - getBaseSize()); + if (targetNameLen >= maxSize - getBaseSize()) { + // Again: String MUST be null terminated. + return HasActionsIF::INVALID_PARAMETERS; + } + targetName = reinterpret_cast(data + getBaseSize()); + return result; + } + const char* getTargetName(size_t& targetNameLen) const { + return this->targetName; + targetNameLen = this->targetNameLen; + } + + private: + const char* targetName = nullptr; + size_t targetNameLen = 0; +}; + +struct SourceTargetPair { + const char* sourceName = nullptr; + size_t sourceNameSize = 0; + const char* targetName = nullptr; + size_t targetNameSize = 0; +}; + +static ReturnValue_t parseDestTargetString(const uint8_t* data, size_t maxLen, + SourceTargetPair& destTgt) { + if (maxLen < 4) { + return SerializeIF::STREAM_TOO_SHORT; + } + destTgt.sourceNameSize = strnlen(reinterpret_cast(data), maxLen); + if (destTgt.sourceNameSize >= maxLen) { + return HasActionsIF::INVALID_PARAMETERS; + } + destTgt.sourceName = reinterpret_cast(data); + size_t remainingLen = maxLen - destTgt.sourceNameSize + 1; + if (remainingLen == 0) { + return HasActionsIF::INVALID_PARAMETERS; + } + destTgt.targetNameSize = + strnlen(reinterpret_cast(data + destTgt.sourceNameSize + 1), remainingLen); + if (destTgt.targetNameSize >= remainingLen) { + return HasActionsIF::INVALID_PARAMETERS; + } + destTgt.targetName = reinterpret_cast(data + destTgt.targetNameSize + 1); + return returnvalue::OK; +} + +class CpHelperParser { + public: + CpHelperParser(const uint8_t* data, size_t maxLen) : data(data), maxLen(maxLen) {} + + ReturnValue_t parse() { + if (maxLen < 1) { + return SerializeIF::STREAM_TOO_SHORT; + } + recursiveOpt = data[0]; + return parseDestTargetString(data + 1, maxLen - 1, destTgt); + } + const SourceTargetPair& destTgtPair() const { return destTgt; } + bool isRecursiveOptSet() const { return recursiveOpt; } + + private: + const uint8_t* data; + size_t maxLen; + bool recursiveOpt = false; + SourceTargetPair destTgt; +}; + +class MvHelperParser { + public: + MvHelperParser(const uint8_t* data, size_t maxLen) : data(data), maxLen(maxLen) {} + + ReturnValue_t parse() { return parseDestTargetString(data, maxLen, destTgt); } + const SourceTargetPair& destTgtPair() const { return destTgt; } + + private: + const uint8_t* data; + size_t maxLen; + SourceTargetPair destTgt; +}; + +class RmHelperParser { + public: + RmHelperParser(const uint8_t* data, size_t maxLen) : data(data), maxLen(maxLen) {} + + ReturnValue_t parse() { + if (maxLen < 2) { + return SerializeIF::STREAM_TOO_SHORT; + } + recursiveOpt = data[0]; + forceOpt = data[1]; + removeTargetSize = strnlen(reinterpret_cast(data + 2), maxLen - 2); + // Must be null-terminated + if (removeTargetSize >= maxLen - 2) { + return HasActionsIF::INVALID_PARAMETERS; + } + removeTarget = reinterpret_cast(data + 2); + return returnvalue::OK; + } + bool isRecursiveOptSet() const { return recursiveOpt; } + bool isForceOptSet() const { return forceOpt; } + + const char* getRemoveTarget(size_t& removeTargetSize) { + removeTargetSize = this->removeTargetSize; + return removeTarget; + } + + private: + const uint8_t* data; + size_t maxLen; + bool recursiveOpt = false; + bool forceOpt = false; + const char* removeTarget = nullptr; + size_t removeTargetSize = 0; +}; + } // namespace core #endif /* MISSION_SYSDEFS_H_ */ From d439aedee740bddf8a64873f5a649d441b7041d5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 15 Apr 2023 13:03:44 +0200 Subject: [PATCH 002/506] bugfix --- bsp_q7s/core/CoreController.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index d9d30119..4e30e2d7 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -982,7 +982,6 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI } std::ostringstream oss("ls -l", std::ostringstream::ate); - oss << "ls -l"; if (parser.aFlagSet()) { oss << "a"; } @@ -1051,7 +1050,6 @@ ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId, } std::ostringstream oss("ls -l", std::ostringstream::ate); - oss << "ls -l"; if (parser.aFlagSet()) { oss << "a"; } From 143002de486a47d38bb341ed7dc9d1569ddaa7ed Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 15 Apr 2023 13:06:16 +0200 Subject: [PATCH 003/506] some improvements --- bsp_q7s/core/CoreController.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 4e30e2d7..08889d70 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -993,6 +993,7 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI const char *repoName = parser.getRepoName(repoNameLen); oss << " " << repoName << " > " << LIST_DIR_DUMP_WORK_FILE; + sif::info << "Executing " << oss.str() << " for direct dump" << std::endl; int ret = std::system(oss.str().c_str()); if (ret != 0) { utility::handleSystemError(result, "CoreController::actionListDirectoryDumpDirectly"); @@ -1017,7 +1018,10 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI } } std::array dirListingBuf{}; - std::ifstream ifile("/tmp/dir_listing.tmp", std::ios::binary); + std::ifstream ifile(LIST_DIR_DUMP_WORK_FILE, std::ios::binary); + if (ifile.bad()) { + return returnvalue::FAILED; + } std::error_code e; size_t totalFileSize = std::filesystem::file_size(LIST_DIR_DUMP_WORK_FILE, e); size_t dumpedBytes = 0; From 0185691dba780776e2de65fcd503f1f20042690a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 15 Apr 2023 20:51:46 +0200 Subject: [PATCH 004/506] add more context info for dumped dir listing --- bsp_q7s/core/CoreController.cpp | 17 ++++++++++++++--- tmtc | 2 +- 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 08889d70..a2e1d5c4 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -1018,26 +1018,37 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI } } std::array dirListingBuf{}; + // First four bytes reserved for segment index. + std::memcpy(dirListingBuf.data() + sizeof(uint32_t), repoName, repoNameLen); + // Null termination for ground parser. + dirListingBuf[sizeof(uint32_t) + repoNameLen] = '\0'; std::ifstream ifile(LIST_DIR_DUMP_WORK_FILE, std::ios::binary); if (ifile.bad()) { return returnvalue::FAILED; } std::error_code e; size_t totalFileSize = std::filesystem::file_size(LIST_DIR_DUMP_WORK_FILE, e); + uint32_t segmentIdx = 0; size_t dumpedBytes = 0; size_t nextDumpLen = 0; + size_t dummy = 0; + size_t maxDumpLen = dirListingBuf.size() - repoNameLen - sizeof(uint32_t); while (dumpedBytes < totalFileSize) { - nextDumpLen = 1024; - if (totalFileSize - dumpedBytes < 1024) { + nextDumpLen = maxDumpLen; + if (totalFileSize - dumpedBytes < maxDumpLen) { nextDumpLen = totalFileSize - dumpedBytes; } - ifile.read(reinterpret_cast(dirListingBuf.data()), nextDumpLen); + SerializeAdapter::serialize(&segmentIdx, dirListingBuf.data(), &dummy, dirListingBuf.size(), + SerializeIF::Endianness::NETWORK); + ifile.read(reinterpret_cast(dirListingBuf.data() + sizeof(uint32_t) + repoNameLen + 1), + nextDumpLen); result = actionHelper.reportData(commandedBy, actionId, dirListingBuf.data(), nextDumpLen); if (result != returnvalue::OK) { // Remove work file when we are done std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); return result; } + segmentIdx++; } // Remove work file when we are done std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); diff --git a/tmtc b/tmtc index d00e4247..37bb164c 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit d00e4247f66eb2f010f1fe53ee7f59b7fb992481 +Subproject commit 37bb164cc461abd51668038e02c5f76db70e7ca6 From 7bf880a29f6bb965d0326bdc0fb7c7179f8e7ed4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 15 Apr 2023 22:17:50 +0200 Subject: [PATCH 005/506] use gzip directly --- bsp_q7s/core/CoreController.cpp | 29 +++++++++++++++++++++++------ mission/sysDefs.h | 5 +++-- tmtc | 2 +- 3 files changed, 27 insertions(+), 9 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index a2e1d5c4..659e1f97 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -248,6 +248,7 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ } auto &sourceTgt = parser.destTgtPair(); oss << sourceTgt.sourceName << " " << sourceTgt.targetName; + sif::info << "CoreController: Performing copy command: " << oss.str() << std::endl; int ret = std::system(oss.str().c_str()); if (ret != 0) { return returnvalue::FAILED; @@ -263,6 +264,7 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ std::ostringstream oss("mv ", std::ostringstream::ate); auto &sourceTgt = parser.destTgtPair(); oss << sourceTgt.sourceName << " " << sourceTgt.targetName; + sif::info << "CoreController: Performing move command: " << oss.str() << std::endl; int ret = std::system(oss.str().c_str()); if (ret != 0) { return returnvalue::FAILED; @@ -288,6 +290,21 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ size_t removeTargetSize = 0; const char *removeTgt = parser.getRemoveTarget(removeTargetSize); oss << " " << removeTgt; + sif::info << "CoreController: Performing remove command: " << oss.str() << std::endl; + int ret = std::system(oss.str().c_str()); + if (ret != 0) { + return returnvalue::FAILED; + } + return EXECUTION_FINISHED; + } + case (MKDIR_HELPER): { + if (size < 1) { + return HasActionsIF::INVALID_PARAMETERS; + } + std::string createdDir = std::string(reinterpret_cast(data), size); + std::ostringstream oss("mkdir ", std::ostringstream::ate); + oss << createdDir; + sif::info << "CoreController: Performing directory creation: " << oss.str() << std::endl; int ret = std::system(oss.str().c_str()); if (ret != 0) { return returnvalue::FAILED; @@ -1000,17 +1017,17 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI return returnvalue::FAILED; } if (parser.compressionOptionSet()) { - std::string compressedName = LIST_DIR_DUMP_WORK_FILE + std::string(".tar.xz"); + std::string compressedName = LIST_DIR_DUMP_WORK_FILE + std::string(".gz"); oss.str(""); - oss << "tar -cJvf /tmp/dir_listing_compressed.tmp " << LIST_DIR_DUMP_WORK_FILE; + oss << "gzip " << LIST_DIR_DUMP_WORK_FILE; ret = std::system(oss.str().c_str()); if (ret != 0) { utility::handleSystemError(result, "CoreController::actionListDirectoryDumpDirectly"); return returnvalue::FAILED; } oss.str(""); - // Overwrite the old work file with the compressed archive. - oss << "mv /tmp/dir_listing_compressed.tmp " << LIST_DIR_DUMP_WORK_FILE; + // Overwrite the work file with the compressed archive. + oss << "mv " << compressedName << " " << LIST_DIR_DUMP_WORK_FILE; ret = std::system(oss.str().c_str()); if (ret != 0) { utility::handleSystemError(result, "CoreController::actionListDirectoryDumpDirectly"); @@ -1085,9 +1102,9 @@ ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId, } if (parser.compressionOptionSet()) { - std::string compressedName = targetFileName + std::string(".tar.xz"); + std::string compressedName = targetFileName + std::string(".gz"); oss.str(""); - oss << "tar -cJvf " << compressedName << " " << targetFileName; + oss << "gzip -c " << compressedName << " > " << targetFileName; sif::info << "Compressing directory listing: " << oss.str() << std::endl; ret = std::system(oss.str().c_str()); if (ret != 0) { diff --git a/mission/sysDefs.h b/mission/sysDefs.h index 3ec3f7e0..cc654e90 100644 --- a/mission/sysDefs.h +++ b/mission/sysDefs.h @@ -63,6 +63,7 @@ static constexpr ActionId_t LIST_DIRECTORY_DUMP_DIRECTLY = 51; static constexpr ActionId_t CP_HELPER = 52; static constexpr ActionId_t MV_HELPER = 53; static constexpr ActionId_t RM_HELPER = 54; +static constexpr ActionId_t MKDIR_HELPER = 55; static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::CORE; @@ -211,7 +212,7 @@ static ReturnValue_t parseDestTargetString(const uint8_t* data, size_t maxLen, return HasActionsIF::INVALID_PARAMETERS; } destTgt.sourceName = reinterpret_cast(data); - size_t remainingLen = maxLen - destTgt.sourceNameSize + 1; + size_t remainingLen = maxLen - destTgt.sourceNameSize - 1; if (remainingLen == 0) { return HasActionsIF::INVALID_PARAMETERS; } @@ -220,7 +221,7 @@ static ReturnValue_t parseDestTargetString(const uint8_t* data, size_t maxLen, if (destTgt.targetNameSize >= remainingLen) { return HasActionsIF::INVALID_PARAMETERS; } - destTgt.targetName = reinterpret_cast(data + destTgt.targetNameSize + 1); + destTgt.targetName = reinterpret_cast(data + destTgt.sourceNameSize + 1); return returnvalue::OK; } diff --git a/tmtc b/tmtc index 37bb164c..77e90328 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 37bb164cc461abd51668038e02c5f76db70e7ca6 +Subproject commit 77e90328a1e1ffd0d79bc4c26932ad48adbf2920 From f2c71d962af022537e012131a91ea656e802977a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 15 Apr 2023 23:00:34 +0200 Subject: [PATCH 006/506] done soon --- bsp_q7s/core/CoreController.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 659e1f97..763a6856 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -1066,10 +1066,11 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI return result; } segmentIdx++; + dumpedBytes += nextDumpLen; } // Remove work file when we are done std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); - return returnvalue::OK; + return EXECUTION_FINISHED; } ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId, @@ -1112,7 +1113,7 @@ ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId, return returnvalue::FAILED; } } - return returnvalue::OK; + return EXECUTION_FINISHED; } ReturnValue_t CoreController::initBootCopyFile() { @@ -2198,6 +2199,8 @@ ReturnValue_t CoreController::executeSwUpdate(SwUpdateSources sourceDir, const u if (not exists(archivePath, e)) { return HasFileSystemIF::FILE_DOES_NOT_EXIST; } + // TODO: Decompressing without limiting memory usage with xz is actually a bit risky.. + // But has not been an issue so far. ostringstream cmd("tar -xJf", ios::app); cmd << " " << archivePath << " -C " << prefixPath; int result = system(cmd.str().c_str()); From 767a0eda3041b4580e0fdd66eca5a8d502c9bbd3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 15 Apr 2023 23:11:00 +0200 Subject: [PATCH 007/506] hopefully last bugfix --- bsp_q7s/core/CoreController.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 763a6856..46abcc5c 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -1010,7 +1010,11 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI const char *repoName = parser.getRepoName(repoNameLen); oss << " " << repoName << " > " << LIST_DIR_DUMP_WORK_FILE; - sif::info << "Executing " << oss.str() << " for direct dump" << std::endl; + sif::info << "Executing " << oss.str() << " for direct dump"; + if (parser.compressionOptionSet()) { + sif::info << " with compression"; + } + sif::info << std::endl; int ret = std::system(oss.str().c_str()); if (ret != 0) { utility::handleSystemError(result, "CoreController::actionListDirectoryDumpDirectly"); @@ -1102,10 +1106,11 @@ ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId, return returnvalue::FAILED; } + // Compression will add a .gz ending. I don't have any issue with this, it makes it explicit + // that this is a compressed file. if (parser.compressionOptionSet()) { - std::string compressedName = targetFileName + std::string(".gz"); oss.str(""); - oss << "gzip -c " << compressedName << " > " << targetFileName; + oss << "gzip " << targetFileName; sif::info << "Compressing directory listing: " << oss.str() << std::endl; ret = std::system(oss.str().c_str()); if (ret != 0) { From 016fab105eb883044dfc720b8caa24a1b9c9e61a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 15 Apr 2023 23:13:04 +0200 Subject: [PATCH 008/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 77e90328..bb463aa0 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 77e90328a1e1ffd0d79bc4c26932ad48adbf2920 +Subproject commit bb463aa05cf90809edf852d348ab1f7593ece74e From f4fedd20c984f51b7d086af9d106ec61f41ee957 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 16 Apr 2023 02:56:46 +0200 Subject: [PATCH 009/506] added more bugfixes for list dir dump --- bsp_q7s/core/CoreController.cpp | 24 ++++++++++++++++-------- mission/sysDefs.h | 4 ++-- tmtc | 2 +- 3 files changed, 19 insertions(+), 11 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 46abcc5c..5c4c4d62 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -1039,10 +1039,9 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI } } std::array dirListingBuf{}; - // First four bytes reserved for segment index. - std::memcpy(dirListingBuf.data() + sizeof(uint32_t), repoName, repoNameLen); - // Null termination for ground parser. - dirListingBuf[sizeof(uint32_t) + repoNameLen] = '\0'; + dirListingBuf[8] = parser.compressionOptionSet(); + // First four bytes reserved for segment index. One byte for compression option information + std::strcpy(reinterpret_cast(dirListingBuf.data() + 2 * sizeof(uint32_t) + 1), repoName); std::ifstream ifile(LIST_DIR_DUMP_WORK_FILE, std::ios::binary); if (ifile.bad()) { return returnvalue::FAILED; @@ -1053,17 +1052,26 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI size_t dumpedBytes = 0; size_t nextDumpLen = 0; size_t dummy = 0; - size_t maxDumpLen = dirListingBuf.size() - repoNameLen - sizeof(uint32_t); + size_t maxDumpLen = dirListingBuf.size() - 2 * sizeof(uint32_t) - 1 - repoNameLen - 1; + size_t listingDataOffset = 2 * sizeof(uint32_t) + 1 + repoNameLen + 1; + uint32_t chunks = totalFileSize / maxDumpLen; + if (totalFileSize % maxDumpLen != 0) { + chunks++; + } + SerializeAdapter::serialize(&chunks, dirListingBuf.data() + sizeof(uint32_t), &dummy, + dirListingBuf.size() - sizeof(uint32_t), + SerializeIF::Endianness::NETWORK); while (dumpedBytes < totalFileSize) { + ifile.seekg(dumpedBytes, std::ios::beg); nextDumpLen = maxDumpLen; if (totalFileSize - dumpedBytes < maxDumpLen) { nextDumpLen = totalFileSize - dumpedBytes; } SerializeAdapter::serialize(&segmentIdx, dirListingBuf.data(), &dummy, dirListingBuf.size(), SerializeIF::Endianness::NETWORK); - ifile.read(reinterpret_cast(dirListingBuf.data() + sizeof(uint32_t) + repoNameLen + 1), - nextDumpLen); - result = actionHelper.reportData(commandedBy, actionId, dirListingBuf.data(), nextDumpLen); + ifile.read(reinterpret_cast(dirListingBuf.data() + listingDataOffset), nextDumpLen); + result = actionHelper.reportData(commandedBy, actionId, dirListingBuf.data(), + listingDataOffset + nextDumpLen); if (result != returnvalue::OK) { // Remove work file when we are done std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); diff --git a/mission/sysDefs.h b/mission/sysDefs.h index cc654e90..e8303524 100644 --- a/mission/sysDefs.h +++ b/mission/sysDefs.h @@ -134,8 +134,8 @@ class ListDirectoryCmdBase { bool compressionOptionSet() const { return this->compressOption; } const char* getRepoName(size_t& repoNameLen) const { - return this->repoName; repoNameLen = this->repoNameLen; + return this->repoName; } size_t getBaseSize() { @@ -186,8 +186,8 @@ class ListDirectoryIntoFile : public ListDirectoryCmdBase { return result; } const char* getTargetName(size_t& targetNameLen) const { - return this->targetName; targetNameLen = this->targetNameLen; + return this->targetName; } private: diff --git a/tmtc b/tmtc index bb463aa0..d623e83b 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit bb463aa05cf90809edf852d348ab1f7593ece74e +Subproject commit d623e83be8536f8887f1da00a1f0c4be191db1e8 From 611a2c0b4542b45f84aa7c3099f89b96912e1cec Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 16 Apr 2023 11:41:30 +0200 Subject: [PATCH 010/506] update CHANGELOG and CMakeLists.txt --- CHANGELOG.md | 6 ++++++ CMakeLists.txt | 4 ++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index fde054ed..72ad3573 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,12 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v2.0.0] 2023-04-16 + +- q7s-package: v2.5.0 +- eive-tmtc: v3.0.0 +- `wire` library is now on version v10.7 as well. + ## Added - Added `mv`, `cp` and `rm` action helpers for the core controller for common filesystem operations. diff --git a/CMakeLists.txt b/CMakeLists.txt index 2d96ca08..1f52a56f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,8 +9,8 @@ # ############################################################################## cmake_minimum_required(VERSION 3.13) -set(OBSW_VERSION_MAJOR 1) -set(OBSW_VERSION_MINOR 45) +set(OBSW_VERSION_MAJOR 2) +set(OBSW_VERSION_MINOR 0) set(OBSW_VERSION_REVISION 0) # set(CMAKE_VERBOSE TRUE) From 8f5982fd7233c5898f2dc3f404efdb709ca62ee9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 16 Apr 2023 11:48:24 +0200 Subject: [PATCH 011/506] added additional note --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 72ad3573..4726e89b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,8 @@ will consitute of a breaking change warranting a new major release: # [v2.0.0] 2023-04-16 +This is the version which will fly on the satellite for the initial launch phase. + - q7s-package: v2.5.0 - eive-tmtc: v3.0.0 - `wire` library is now on version v10.7 as well. From babea226ab6e78143bcd450a9f222699a75dfbcc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 16 Apr 2023 11:54:27 +0200 Subject: [PATCH 012/506] thats an annoying patch release --- CHANGELOG.md | 4 ++++ tmtc | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4726e89b..8568b9b7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v2.0.1] 2023-04-16 + +- eive-tmtc: v3.1.0 + # [v2.0.0] 2023-04-16 This is the version which will fly on the satellite for the initial launch phase. diff --git a/tmtc b/tmtc index d623e83b..3bb0a08e 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit d623e83be8536f8887f1da00a1f0c4be191db1e8 +Subproject commit 3bb0a08e951133cc230baccf59f476169b790993 From 07e8f95a3114b2bd2c2df323478c048c5db3f054 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 16 Apr 2023 12:05:41 +0200 Subject: [PATCH 013/506] of course something else was missing --- CHANGELOG.md | 4 ++++ CMakeLists.txt | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8568b9b7..69acb077 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v2.0.2] 2023-04-16 + +- Bump patch version to 2. + # [v2.0.1] 2023-04-16 - eive-tmtc: v3.1.0 diff --git a/CMakeLists.txt b/CMakeLists.txt index 1f52a56f..204ff55b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 2) set(OBSW_VERSION_MINOR 0) -set(OBSW_VERSION_REVISION 0) +set(OBSW_VERSION_REVISION 2) # set(CMAKE_VERBOSE TRUE) From 304aabc336971c10952540627f517a8ca52577db Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 16 Apr 2023 12:10:29 +0200 Subject: [PATCH 014/506] bump sagittactl to avoid CMake warning --- thirdparty/sagittactl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/thirdparty/sagittactl b/thirdparty/sagittactl index 2c066bfb..20f32a2f 160000 --- a/thirdparty/sagittactl +++ b/thirdparty/sagittactl @@ -1 +1 @@ -Subproject commit 2c066bfbe9ef8d6b00e142b8f48d45988a469618 +Subproject commit 20f32a2f291d00869c693066de02c0257dec7be4 From 07572ab3a029780493145f793df474cc19127465 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 17 Apr 2023 09:20:09 +0200 Subject: [PATCH 015/506] fixed shadowing --- mission/controller/acs/control/SafeCtrl.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index ce50f276..f0ebd5a0 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -95,7 +95,6 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate acsParameters->safeModeControllerParameters.k_orthoNonMekf); // sum of all torques - double cmdTorque[3] = {0, 0, 0}; VectorOperations::add(cmdParallel, cmdOrtho, cmdTorque, 3); // calculate magnetic moment to command From e05d9d4b2a14d5c132eddddcbc243b6a9b528c1f Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 17 Apr 2023 09:29:50 +0200 Subject: [PATCH 016/506] prevent low pass filter if no sensor data is available --- mission/controller/acs/SensorProcessing.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index c1030c7b..4cc15a16 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -150,7 +150,8 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const } timeOfSavedMagFieldEst = timeOfMgmMeasurement; - if (mgmDataProcessed->mgmVecTotDerivative.isValid()) { + if (VectorOperations::norm(mgmVecTotDerivative, 3) != 0 and + mgmDataProcessed->mgmVecTotDerivative.isValid()) { lowPassFilter(mgmVecTotDerivative, mgmDataProcessed->mgmVecTotDerivative.value, mgmParameters->mgmDerivativeFilterWeight); } @@ -533,7 +534,7 @@ void SensorProcessing::processGyr( } } - if (gyrDataProcessed->gyrVecTot.isValid()) { + if (VectorOperations::norm(gyrVecTot, 3) != 0 and gyrDataProcessed->gyrVecTot.isValid()) { lowPassFilter(gyrVecTot, gyrDataProcessed->gyrVecTot.value, gyrParameters->gyrFilterWeight); } From 9097a3f3c7be6f338e1facf19da5173fe9efa752 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 17 Apr 2023 09:35:23 +0200 Subject: [PATCH 017/506] changelog --- CHANGELOG.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 69acb077..9d318d21 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,15 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- Fixed shadowing within the `SafeCtrl`, which prevented actuator commands to be calculated during + eclipse phase. + +## Changed + +- Low-pass filters can no longer be executed if no actual data is available. + # [v2.0.2] 2023-04-16 - Bump patch version to 2. From bdd2b23ec3287beecccadc9f81785a28607fd034 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Apr 2023 10:41:10 +0200 Subject: [PATCH 018/506] test cold case camera --- dummies/TemperatureSensorInserter.cpp | 12 +++++++++++- dummies/TemperatureSensorInserter.h | 1 + 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/dummies/TemperatureSensorInserter.cpp b/dummies/TemperatureSensorInserter.cpp index cfba0ef9..62deacf4 100644 --- a/dummies/TemperatureSensorInserter.cpp +++ b/dummies/TemperatureSensorInserter.cpp @@ -15,7 +15,7 @@ TemperatureSensorInserter::TemperatureSensorInserter(object_id_t objectId, tmp1075DummyMap(std::move(tempTmpSensorDummies_)) {} ReturnValue_t TemperatureSensorInserter::initialize() { - testCase = TestCase::NONE; + testCase = TestCase::COLD_CAMERA; return returnvalue::OK; } @@ -96,6 +96,16 @@ ReturnValue_t TemperatureSensorInserter::performOperation(uint8_t opCode) { } break; } + case (TestCase::COLD_CAMERA): { + if (cycles == 15) { + sif::debug << "Setting cold CAM temperature" << std::endl; + max31865DummyMap[objects::RTD_2_IC5_4K_CAMERA]->setTemperature(-40, true); + } + if (cycles == 30) { + sif::debug << "Setting CAM temperature back to normal" << std::endl; + max31865DummyMap[objects::RTD_2_IC5_4K_CAMERA]->setTemperature(0, true); + } + } } cycles++; return returnvalue::OK; diff --git a/dummies/TemperatureSensorInserter.h b/dummies/TemperatureSensorInserter.h index 675bcd91..eb6cc1ba 100644 --- a/dummies/TemperatureSensorInserter.h +++ b/dummies/TemperatureSensorInserter.h @@ -31,6 +31,7 @@ class TemperatureSensorInserter : public ExecutableObjectIF, public SystemObject COLD_MGT = 3, COLD_STR = 4, COLD_STR_CONSECUTIVE = 5, + COLD_CAMERA = 6, }; int iteration = 0; uint32_t cycles = 0; From 701ecbd182dfc9dc65a6ff13de8be35e8c51bd0a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Apr 2023 10:45:01 +0200 Subject: [PATCH 019/506] disable test --- dummies/TemperatureSensorInserter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dummies/TemperatureSensorInserter.cpp b/dummies/TemperatureSensorInserter.cpp index 62deacf4..14a005aa 100644 --- a/dummies/TemperatureSensorInserter.cpp +++ b/dummies/TemperatureSensorInserter.cpp @@ -15,7 +15,7 @@ TemperatureSensorInserter::TemperatureSensorInserter(object_id_t objectId, tmp1075DummyMap(std::move(tempTmpSensorDummies_)) {} ReturnValue_t TemperatureSensorInserter::initialize() { - testCase = TestCase::COLD_CAMERA; + testCase = TestCase::NONE; return returnvalue::OK; } From ab588b4844893adacd301886ea348097071b19c9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Apr 2023 11:35:10 +0200 Subject: [PATCH 020/506] fixes and tweaks TCS and EM build --- CHANGELOG.md | 8 ++++++ .../fsfwconfig/events/translateEvents.cpp | 7 ++++-- .../fsfwconfig/objects/translateObjects.cpp | 2 +- dummies/RwDummy.cpp | 3 +++ dummies/RwDummy.h | 3 +++ generators/bsp_hosted_events.csv | 1 + generators/bsp_q7s_events.csv | 1 + generators/events/translateEvents.cpp | 7 ++++-- generators/objects/translateObjects.cpp | 2 +- linux/fsfwconfig/events/translateEvents.cpp | 7 ++++-- linux/fsfwconfig/objects/translateObjects.cpp | 2 +- linux/ipcore/PdecHandler.cpp | 3 +-- mission/controller/ThermalController.cpp | 25 ++++++++++++++++--- mission/controller/ThermalController.h | 1 + mission/controller/tcsDefs.h | 1 + mission/system/EiveSystem.cpp | 2 ++ tmtc | 2 +- 17 files changed, 62 insertions(+), 15 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9d318d21..ff47b4df 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,10 +16,18 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v2.0.3] 2023-04-17 + ## Fixed - Fixed shadowing within the `SafeCtrl`, which prevented actuator commands to be calculated during eclipse phase. +- EM build idle mode fixes for RW dummy. + +## Added + +- Add `MGT_OVERHEATING` event and fallback to system SAFE mode if the MGT is overheating for + whatever reason. ## Changed diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index 4c1fdb55..f7850a99 100644 --- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp +++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 290 translations. + * @brief Auto-generated event translation file. Contains 291 translations. * @details - * Generated on: 2023-04-14 20:23:17 + * Generated on: 2023-04-17 11:34:19 */ #include "translateEvents.h" @@ -280,6 +280,7 @@ const char *OBC_OVERHEATING_STRING = "OBC_OVERHEATING"; const char *CAMERA_OVERHEATING_STRING = "CAMERA_OVERHEATING"; const char *PCDU_SYSTEM_OVERHEATING_STRING = "PCDU_SYSTEM_OVERHEATING"; const char *HEATER_NOT_OFF_FOR_OFF_MODE_STRING = "HEATER_NOT_OFF_FOR_OFF_MODE"; +const char *MGT_OVERHEATING_STRING = "MGT_OVERHEATING"; const char *TX_TIMER_EXPIRED_STRING = "TX_TIMER_EXPIRED"; const char *BIT_LOCK_TX_ON_STRING = "BIT_LOCK_TX_ON"; const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION"; @@ -848,6 +849,8 @@ const char *translateEvents(Event event) { return PCDU_SYSTEM_OVERHEATING_STRING; case (14107): return HEATER_NOT_OFF_FOR_OFF_MODE_STRING; + case (14108): + return MGT_OVERHEATING_STRING; case (14201): return TX_TIMER_EXPIRED_STRING; case (14202): diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index b89eba11..16235835 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 171 translations. - * Generated on: 2023-04-14 20:23:17 + * Generated on: 2023-04-17 11:34:19 */ #include "translateObjects.h" diff --git a/dummies/RwDummy.cpp b/dummies/RwDummy.cpp index 0f97f24a..60652ef7 100644 --- a/dummies/RwDummy.cpp +++ b/dummies/RwDummy.cpp @@ -37,6 +37,9 @@ uint32_t RwDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return ReturnValue_t RwDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(rws::RW_SPEED, &rwSpeed); + localDataPoolMap.emplace(rws::RAMP_TIME, &rampTime); + localDataPoolMap.emplace(rws::TEMPERATURE_C, new PoolEntry({0})); localDataPoolMap.emplace(rws::CURR_SPEED, new PoolEntry({0})); diff --git a/dummies/RwDummy.h b/dummies/RwDummy.h index e5738420..5673717a 100644 --- a/dummies/RwDummy.h +++ b/dummies/RwDummy.h @@ -15,6 +15,9 @@ class RwDummy : public DeviceHandlerBase { virtual ~RwDummy(); protected: + PoolEntry rwSpeed = PoolEntry({0}); + PoolEntry rampTime = PoolEntry({10}); + void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv index 54233239..2d5f5693 100644 --- a/generators/bsp_hosted_events.csv +++ b/generators/bsp_hosted_events.csv @@ -274,6 +274,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14105;0x3719;CAMERA_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14106;0x371a;PCDU_SYSTEM_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14107;0x371b;HEATER_NOT_OFF_FOR_OFF_MODE;MEDIUM;No description;mission/controller/tcsDefs.h +14108;0x371c;MGT_OVERHEATING;MEDIUM;No description;mission/controller/tcsDefs.h 14201;0x3779;TX_TIMER_EXPIRED;INFO;The transmit timer to protect the Syrlinks expired P1: The current timer value;mission/system/com/ComSubsystem.h 14202;0x377a;BIT_LOCK_TX_ON;INFO;Transmitter will be turned on due to detection of bitlock;mission/system/com/ComSubsystem.h 14300;0x37dc;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/persistentTmStoreDefs.h diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 54233239..2d5f5693 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -274,6 +274,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14105;0x3719;CAMERA_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14106;0x371a;PCDU_SYSTEM_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14107;0x371b;HEATER_NOT_OFF_FOR_OFF_MODE;MEDIUM;No description;mission/controller/tcsDefs.h +14108;0x371c;MGT_OVERHEATING;MEDIUM;No description;mission/controller/tcsDefs.h 14201;0x3779;TX_TIMER_EXPIRED;INFO;The transmit timer to protect the Syrlinks expired P1: The current timer value;mission/system/com/ComSubsystem.h 14202;0x377a;BIT_LOCK_TX_ON;INFO;Transmitter will be turned on due to detection of bitlock;mission/system/com/ComSubsystem.h 14300;0x37dc;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/persistentTmStoreDefs.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index 4c1fdb55..f7850a99 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 290 translations. + * @brief Auto-generated event translation file. Contains 291 translations. * @details - * Generated on: 2023-04-14 20:23:17 + * Generated on: 2023-04-17 11:34:19 */ #include "translateEvents.h" @@ -280,6 +280,7 @@ const char *OBC_OVERHEATING_STRING = "OBC_OVERHEATING"; const char *CAMERA_OVERHEATING_STRING = "CAMERA_OVERHEATING"; const char *PCDU_SYSTEM_OVERHEATING_STRING = "PCDU_SYSTEM_OVERHEATING"; const char *HEATER_NOT_OFF_FOR_OFF_MODE_STRING = "HEATER_NOT_OFF_FOR_OFF_MODE"; +const char *MGT_OVERHEATING_STRING = "MGT_OVERHEATING"; const char *TX_TIMER_EXPIRED_STRING = "TX_TIMER_EXPIRED"; const char *BIT_LOCK_TX_ON_STRING = "BIT_LOCK_TX_ON"; const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION"; @@ -848,6 +849,8 @@ const char *translateEvents(Event event) { return PCDU_SYSTEM_OVERHEATING_STRING; case (14107): return HEATER_NOT_OFF_FOR_OFF_MODE_STRING; + case (14108): + return MGT_OVERHEATING_STRING; case (14201): return TX_TIMER_EXPIRED_STRING; case (14202): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index bb0ef28a..1c252ec5 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-04-14 20:23:17 + * Generated on: 2023-04-17 11:34:19 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index 4c1fdb55..f7850a99 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 290 translations. + * @brief Auto-generated event translation file. Contains 291 translations. * @details - * Generated on: 2023-04-14 20:23:17 + * Generated on: 2023-04-17 11:34:19 */ #include "translateEvents.h" @@ -280,6 +280,7 @@ const char *OBC_OVERHEATING_STRING = "OBC_OVERHEATING"; const char *CAMERA_OVERHEATING_STRING = "CAMERA_OVERHEATING"; const char *PCDU_SYSTEM_OVERHEATING_STRING = "PCDU_SYSTEM_OVERHEATING"; const char *HEATER_NOT_OFF_FOR_OFF_MODE_STRING = "HEATER_NOT_OFF_FOR_OFF_MODE"; +const char *MGT_OVERHEATING_STRING = "MGT_OVERHEATING"; const char *TX_TIMER_EXPIRED_STRING = "TX_TIMER_EXPIRED"; const char *BIT_LOCK_TX_ON_STRING = "BIT_LOCK_TX_ON"; const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION"; @@ -848,6 +849,8 @@ const char *translateEvents(Event event) { return PCDU_SYSTEM_OVERHEATING_STRING; case (14107): return HEATER_NOT_OFF_FOR_OFF_MODE_STRING; + case (14108): + return MGT_OVERHEATING_STRING; case (14201): return TX_TIMER_EXPIRED_STRING; case (14202): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index bb0ef28a..1c252ec5 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-04-14 20:23:17 + * Generated on: 2023-04-17 11:34:19 */ #include "translateObjects.h" diff --git a/linux/ipcore/PdecHandler.cpp b/linux/ipcore/PdecHandler.cpp index 7506dd11..cc074ddd 100644 --- a/linux/ipcore/PdecHandler.cpp +++ b/linux/ipcore/PdecHandler.cpp @@ -435,8 +435,7 @@ ReturnValue_t PdecHandler::releasePdec() { } ReturnValue_t PdecHandler::pdecToReset() { - ReturnValue_t result = returnvalue::OK; - result = gpioComIF->pullLow(pdecReset); + ReturnValue_t result = gpioComIF->pullLow(pdecReset); if (result != returnvalue::OK) { sif::error << "PdecHandler::pdecToReset: Failed to pull PDEC reset line" " to low" diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 82efb85c..d916e767 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -20,6 +20,8 @@ #define LOWER_SYRLINKS_UPPER_LIMITS 0 #define LOWER_EBAND_UPPER_LIMITS 0 #define LOWER_PLOC_UPPER_LIMITS 0 +#define LOWER_MGT_UPPER_LIMITS 0 +#define LOWER_RW_UPPER_LIMITS 0 ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater, const std::atomic_bool& tcsBoardShortUnavailable) @@ -111,7 +113,7 @@ void ThermalController::performControlOperation() { break; } - if (cycles == 50) { + if (cycles == 40) { bool changedLimits = false; #if LOWER_SYRLINKS_UPPER_LIMITS == 1 changedLimits = true; @@ -130,9 +132,21 @@ void ThermalController::performControlOperation() { hpaLimits.cutOffLimit = 0; hpaLimits.opUpperLimit = 0; hpaLimits.nopUpperLimit = 0; +#endif +#if LOWER_MGT_UPPER_LIMITS == 1 + changedLimits = true; + mgtLimits.cutOffLimit = 0; + mgtLimits.opUpperLimit = 0; + mgtLimits.nopUpperLimit = 0; +#endif +#if LOWER_RW_UPPER_LIMITS == 1 + changedLimits = true; + rwLimits.cutOffLimit = 0; + rwLimits.opUpperLimit = 0; + rwLimits.nopUpperLimit = 0; #endif if (changedLimits) { - sif::debug << "ThermalController: changing limits" << std::endl; // TODO: rausschmeissen + sif::debug << "ThermalController: changing limits" << std::endl; } } @@ -1055,7 +1069,12 @@ void ThermalController::ctrlMgt() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, mgtLimits); ctrlComponentTemperature(htrCtx); - // TODO: trigger special event + if (componentAboveUpperLimit and not mgtTooHotFlag) { + triggerEvent(tcsCtrl::MGT_OVERHEATING, tempFloatToU32()); + mgtTooHotFlag = true; + } else if (not componentAboveUpperLimit) { + mgtTooHotFlag = false; + } } void ThermalController::ctrlRw() { diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 7aa05b1e..4eb41c67 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -267,6 +267,7 @@ class ThermalController : public ExtendedControllerBase { bool pcduSystemTooHotFlag = false; bool syrlinksTooHotFlag = false; bool obcTooHotFlag = false; + bool mgtTooHotFlag = false; bool strTooHotFlag = false; bool rwTooHotFlag = false; diff --git a/mission/controller/tcsDefs.h b/mission/controller/tcsDefs.h index 03aa8ffe..bef79f1e 100644 --- a/mission/controller/tcsDefs.h +++ b/mission/controller/tcsDefs.h @@ -17,6 +17,7 @@ static constexpr Event OBC_OVERHEATING = MAKE_EVENT(4, severity::HIGH); static constexpr Event CAMERA_OVERHEATING = MAKE_EVENT(5, severity::HIGH); static constexpr Event PCDU_SYSTEM_OVERHEATING = MAKE_EVENT(6, severity::HIGH); static constexpr Event HEATER_NOT_OFF_FOR_OFF_MODE = MAKE_EVENT(7, severity::MEDIUM); +static constexpr Event MGT_OVERHEATING = MAKE_EVENT(8, severity::MEDIUM); enum SetId : uint32_t { SENSOR_TEMPERATURES = 0, diff --git a/mission/system/EiveSystem.cpp b/mission/system/EiveSystem.cpp index eb1c4101..53bf7a32 100644 --- a/mission/system/EiveSystem.cpp +++ b/mission/system/EiveSystem.cpp @@ -114,6 +114,7 @@ ReturnValue_t EiveSystem::initialize() { manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::PCDU_SYSTEM_OVERHEATING)); manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::OBC_OVERHEATING)); + manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::MGT_OVERHEATING)); manager->subscribeToEvent(eventQueue->getId(), event::getEventId(pdec::INVALID_TC_FRAME)); return Subsystem::initialize(); } @@ -132,6 +133,7 @@ void EiveSystem::handleEventMessages() { break; } case tcsCtrl::OBC_OVERHEATING: + case tcsCtrl::MGT_OVERHEATING: case tcsCtrl::PCDU_SYSTEM_OVERHEATING: { if (isInTransition) { performSafeRecovery = true; diff --git a/tmtc b/tmtc index 3bb0a08e..7eaf6557 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 3bb0a08e951133cc230baccf59f476169b790993 +Subproject commit 7eaf6557ebb886861c819fda0319641855bbb650 From f846a18b33bcc5e898185d74086344f4ef5d53bd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Apr 2023 11:36:17 +0200 Subject: [PATCH 021/506] update defines --- mission/controller/ThermalController.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index d916e767..69535ab4 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -17,11 +17,11 @@ #include // Enabling this should trigger a special event which in turn should trigger a system reaction. -#define LOWER_SYRLINKS_UPPER_LIMITS 0 -#define LOWER_EBAND_UPPER_LIMITS 0 -#define LOWER_PLOC_UPPER_LIMITS 0 -#define LOWER_MGT_UPPER_LIMITS 0 -#define LOWER_RW_UPPER_LIMITS 0 +#define LOWER_SYRLINKS_UPPER_LIMITS 0 +#define LOWER_EBAND_UPPER_LIMITS 0 +#define LOWER_PLOC_UPPER_LIMITS 0 +#define LOWER_MGT_UPPER_LIMITS 0 +#define LOWER_RW_UPPER_LIMITS 0 ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater, const std::atomic_bool& tcsBoardShortUnavailable) From 00da51b6a294430fff0240ccb50d43dbd2c57901 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Apr 2023 11:36:34 +0200 Subject: [PATCH 022/506] okay its an afmt thing --- mission/controller/ThermalController.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 69535ab4..d916e767 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -17,11 +17,11 @@ #include // Enabling this should trigger a special event which in turn should trigger a system reaction. -#define LOWER_SYRLINKS_UPPER_LIMITS 0 -#define LOWER_EBAND_UPPER_LIMITS 0 -#define LOWER_PLOC_UPPER_LIMITS 0 -#define LOWER_MGT_UPPER_LIMITS 0 -#define LOWER_RW_UPPER_LIMITS 0 +#define LOWER_SYRLINKS_UPPER_LIMITS 0 +#define LOWER_EBAND_UPPER_LIMITS 0 +#define LOWER_PLOC_UPPER_LIMITS 0 +#define LOWER_MGT_UPPER_LIMITS 0 +#define LOWER_RW_UPPER_LIMITS 0 ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater, const std::atomic_bool& tcsBoardShortUnavailable) From 2658cabc9d5828647bb76695773c1df3e788565f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Apr 2023 11:41:21 +0200 Subject: [PATCH 023/506] remove TODO --- mission/controller/ThermalController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index d916e767..7133dd88 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1259,7 +1259,7 @@ void ThermalController::ctrlObcIfBoard() { triggerEvent(tcsCtrl::OBC_OVERHEATING, tempFloatToU32()); obcTooHotFlag = true; } else if (not componentAboveUpperLimit) { - obcTooHotFlag = false; // TODO: !! + obcTooHotFlag = false; } } From 468fb096c97fb5180dc034925fff02019a75a017 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Apr 2023 11:44:51 +0200 Subject: [PATCH 024/506] high event severity --- mission/controller/tcsDefs.h | 2 +- tmtc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/tcsDefs.h b/mission/controller/tcsDefs.h index bef79f1e..e2efd98c 100644 --- a/mission/controller/tcsDefs.h +++ b/mission/controller/tcsDefs.h @@ -17,7 +17,7 @@ static constexpr Event OBC_OVERHEATING = MAKE_EVENT(4, severity::HIGH); static constexpr Event CAMERA_OVERHEATING = MAKE_EVENT(5, severity::HIGH); static constexpr Event PCDU_SYSTEM_OVERHEATING = MAKE_EVENT(6, severity::HIGH); static constexpr Event HEATER_NOT_OFF_FOR_OFF_MODE = MAKE_EVENT(7, severity::MEDIUM); -static constexpr Event MGT_OVERHEATING = MAKE_EVENT(8, severity::MEDIUM); +static constexpr Event MGT_OVERHEATING = MAKE_EVENT(8, severity::HIGH); enum SetId : uint32_t { SENSOR_TEMPERATURES = 0, diff --git a/tmtc b/tmtc index 7eaf6557..0c6a9677 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 7eaf6557ebb886861c819fda0319641855bbb650 +Subproject commit 0c6a9677e1b9b86456efff2e11ba31f99e15ac2a From a146c34140da4c2e3418bf2a0cfae5b655d03457 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Apr 2023 11:55:08 +0200 Subject: [PATCH 025/506] CI/CD: Use 8 jobs --- automation/Jenkinsfile | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/automation/Jenkinsfile b/automation/Jenkinsfile index 167c28cc..12c4bdf8 100644 --- a/automation/Jenkinsfile +++ b/automation/Jenkinsfile @@ -22,7 +22,7 @@ pipeline { steps { dir(BUILDDIR_Q7S) { sh 'cmake -DTGT_BSP="arm/q7s" -DCMAKE_BUILD_TYPE=Debug ..' - sh 'cmake --build . -j6' + sh 'cmake --build . -j8' } } } @@ -30,7 +30,7 @@ pipeline { steps { dir(BUILDDIR_Q7S_EM) { sh 'cmake -DTGT_BSP="arm/q7s" -DEIVE_Q7S_EM=ON -DCMAKE_BUILD_TYPE=Debug ..' - sh 'cmake --build . -j6' + sh 'cmake --build . -j8' } } } @@ -38,7 +38,7 @@ pipeline { steps { dir(BUILDDIR_LINUX) { sh 'cmake ..' - sh 'cmake --build . -j6' + sh 'cmake --build . -j8' sh './eive-unittest' } } From 74ee2919832a6d5b4ad707bb4d2b302359566c15 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Apr 2023 11:58:13 +0200 Subject: [PATCH 026/506] add tmtc release version --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index ff47b4df..756b0709 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,8 @@ will consitute of a breaking change warranting a new major release: # [v2.0.3] 2023-04-17 +- eive-tmtc: v3.1.1 + ## Fixed - Fixed shadowing within the `SafeCtrl`, which prevented actuator commands to be calculated during From 0eb6b7cccb5c77d23bb9f8ae1c7646e6ea0402e7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Apr 2023 11:58:44 +0200 Subject: [PATCH 027/506] bump patch revision to 3 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 204ff55b..8f632d2d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 2) set(OBSW_VERSION_MINOR 0) -set(OBSW_VERSION_REVISION 2) +set(OBSW_VERSION_REVISION 3) # set(CMAKE_VERBOSE TRUE) From 432b400835324357ba8409e81d02800f5e024c6f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 18 Apr 2023 10:28:01 +0200 Subject: [PATCH 028/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 0c6a9677..5fbd19bb 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 0c6a9677e1b9b86456efff2e11ba31f99e15ac2a +Subproject commit 5fbd19bb6cca0790373a809d78f2307adca9d0c8 From befa84a74b7a3d7b3363492bda0c3cf0d689a376 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 19 Apr 2023 18:09:57 +0200 Subject: [PATCH 029/506] the hottest fix --- mission/acs/GyrAdis1650XHandler.cpp | 1 + mission/acs/GyrL3gCustomHandler.cpp | 1 + mission/acs/MgmLis3CustomHandler.cpp | 1 + mission/acs/MgmRm3100CustomHandler.cpp | 1 + mission/acs/SusHandler.cpp | 1 + 5 files changed, 5 insertions(+) diff --git a/mission/acs/GyrAdis1650XHandler.cpp b/mission/acs/GyrAdis1650XHandler.cpp index b9e0b049..c61187ce 100644 --- a/mission/acs/GyrAdis1650XHandler.cpp +++ b/mission/acs/GyrAdis1650XHandler.cpp @@ -37,6 +37,7 @@ void GyrAdis1650XHandler::doStartUp() { void GyrAdis1650XHandler::doShutDown() { if (internalState != InternalState::SHUTDOWN) { commandExecuted = false; + PoolReadGuard pg(&primaryDataset); primaryDataset.setValidity(false, true); internalState = InternalState::SHUTDOWN; } diff --git a/mission/acs/GyrL3gCustomHandler.cpp b/mission/acs/GyrL3gCustomHandler.cpp index 934fba99..ebc6ba42 100644 --- a/mission/acs/GyrL3gCustomHandler.cpp +++ b/mission/acs/GyrL3gCustomHandler.cpp @@ -33,6 +33,7 @@ void GyrL3gCustomHandler::doStartUp() { void GyrL3gCustomHandler::doShutDown() { if (internalState != InternalState::SHUTDOWN) { internalState = InternalState::SHUTDOWN; + PoolReadGuard pg(&dataset); dataset.setValidity(false, true); commandExecuted = false; } diff --git a/mission/acs/MgmLis3CustomHandler.cpp b/mission/acs/MgmLis3CustomHandler.cpp index d1dd5eff..4b2e992d 100644 --- a/mission/acs/MgmLis3CustomHandler.cpp +++ b/mission/acs/MgmLis3CustomHandler.cpp @@ -29,6 +29,7 @@ void MgmLis3CustomHandler::doStartUp() { void MgmLis3CustomHandler::doShutDown() { if (internalState != InternalState::SHUTDOWN) { + PoolReadGuard pg(&dataset); dataset.setValidity(false, true); internalState = InternalState::SHUTDOWN; commandExecuted = false; diff --git a/mission/acs/MgmRm3100CustomHandler.cpp b/mission/acs/MgmRm3100CustomHandler.cpp index e57effda..24c94938 100644 --- a/mission/acs/MgmRm3100CustomHandler.cpp +++ b/mission/acs/MgmRm3100CustomHandler.cpp @@ -33,6 +33,7 @@ void MgmRm3100CustomHandler::doStartUp() { void MgmRm3100CustomHandler::doShutDown() { if (internalState != InternalState::SHUTDOWN) { commandExecuted = false; + PoolReadGuard pg(&primaryDataset); primaryDataset.setValidity(false, true); internalState = InternalState::SHUTDOWN; } diff --git a/mission/acs/SusHandler.cpp b/mission/acs/SusHandler.cpp index 0032a11d..c347020f 100644 --- a/mission/acs/SusHandler.cpp +++ b/mission/acs/SusHandler.cpp @@ -29,6 +29,7 @@ void SusHandler::doStartUp() { void SusHandler::doShutDown() { if (internalState != InternalState::SHUTDOWN) { + PoolReadGuard pg(&dataset); dataset.setValidity(false, true); internalState = InternalState::SHUTDOWN; commandExecuted = false; From f46a705900a67c981a4afefa267a2c7fd38896a1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 19 Apr 2023 18:12:32 +0200 Subject: [PATCH 030/506] changelog --- CHANGELOG.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 756b0709..2731eb8e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,13 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v2.0.4] 2023-04-19 + +## Fixed + +- The dual lane assembly device handlers did not properly set their datasets + to invalid on off transitions + # [v2.0.3] 2023-04-17 - eive-tmtc: v3.1.1 From 1756b5edcc4293892d2aef7a30d5b58c8c40db33 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 19 Apr 2023 18:14:28 +0200 Subject: [PATCH 031/506] patch revision --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8f632d2d..358dd8ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 2) set(OBSW_VERSION_MINOR 0) -set(OBSW_VERSION_REVISION 3) +set(OBSW_VERSION_REVISION 4) # set(CMAKE_VERBOSE TRUE) From a75f9553be53ab922311b31d5140e1776a7c02e4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 18:38:41 +0200 Subject: [PATCH 032/506] okay lets see if this works --- CHANGELOG.md | 13 +++++++++++-- mission/system/acs/DualLaneAssemblyBase.cpp | 9 +++++---- 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2731eb8e..3d543d0f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,12 +16,21 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v2.0.5] to be released + +- The dual lane assembly transition failed handler started new transitions towards the current mode + instead of the target mode. This means that if the dual lane assembly never reached the initial + submode (e.g. mode normal and submode dual side), it will transition back to the current mode, + which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent + recovery handling becomes stuck in the custom recovery sequence when swichting power back on. +- The dual lane custom recovery handling was adapted to always perform proper power switch handling + irrespective of current or target modes. + # [v2.0.4] 2023-04-19 ## Fixed -- The dual lane assembly device handlers did not properly set their datasets - to invalid on off transitions +- The dual lane assembly datasets were not marked invalid properly on OFF transitions. # [v2.0.3] 2023-04-17 diff --git a/mission/system/acs/DualLaneAssemblyBase.cpp b/mission/system/acs/DualLaneAssemblyBase.cpp index 7d22fbd1..dc97908f 100644 --- a/mission/system/acs/DualLaneAssemblyBase.cpp +++ b/mission/system/acs/DualLaneAssemblyBase.cpp @@ -183,11 +183,11 @@ void DualLaneAssemblyBase::handleModeTransitionFailed(ReturnValue_t result) { // transition to dual mode. if (not tryingOtherSide) { triggerEvent(CANT_KEEP_MODE, mode, submode); - startTransition(mode, nextSubmode); + startTransition(targetMode, nextSubmode); tryingOtherSide = true; } else { - triggerEvent(transitionOtherSideFailedEvent, mode, targetSubmode); - startTransition(mode, Submodes::DUAL_MODE); + triggerEvent(transitionOtherSideFailedEvent, targetMode, targetSubmode); + startTransition(targetMode, Submodes::DUAL_MODE); } } @@ -205,7 +205,8 @@ bool DualLaneAssemblyBase::checkAndHandleRecovery() { opCode = pwrStateMachine.fsm(); if (opCode == OpCodes::TO_OFF_DONE or opCode == OpCodes::TIMEOUT_OCCURED) { customRecoveryStates = RecoveryCustomStates::POWER_SWITCHING_ON; - pwrStateMachine.start(targetMode, targetSubmode); + // Command power back on in any case. + pwrStateMachine.start(HasModesIF::MODE_ON, targetSubmode); } } if (customRecoveryStates == POWER_SWITCHING_ON) { From 045d1499468c95a4864402c8579b3946798c777a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 18:50:49 +0200 Subject: [PATCH 033/506] bump revision --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 358dd8ab..ace83e22 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 2) set(OBSW_VERSION_MINOR 0) -set(OBSW_VERSION_REVISION 4) +set(OBSW_VERSION_REVISION 5) # set(CMAKE_VERBOSE TRUE) From 8ade378a69ca8f96a4f0e5ab0de1858e82f968df Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 18 Apr 2023 10:28:01 +0200 Subject: [PATCH 034/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 0c6a9677..5fbd19bb 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 0c6a9677e1b9b86456efff2e11ba31f99e15ac2a +Subproject commit 5fbd19bb6cca0790373a809d78f2307adca9d0c8 From 05b0efe851a0f4137f975f44a71eb850ffd62bff Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 19 Apr 2023 18:14:28 +0200 Subject: [PATCH 035/506] patch revision --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8f632d2d..358dd8ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 2) set(OBSW_VERSION_MINOR 0) -set(OBSW_VERSION_REVISION 3) +set(OBSW_VERSION_REVISION 4) # set(CMAKE_VERBOSE TRUE) From 3e9933c7ee24b8d986b2b31961376a6fc3550c0b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 26 Apr 2023 13:15:42 +0200 Subject: [PATCH 036/506] this is annoying --- bsp_hosted/ObjectFactory.cpp | 2 +- mission/genericFactory.cpp | 5 +++-- mission/genericFactory.h | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 3d618cdb..bdbd577b 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -64,7 +64,7 @@ void ObjectFactory::produce(void* args) { PersistentTmStores persistentStores; auto sdcMan = new DummySdCardManager("/tmp"); ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel, *sdcMan, &ipcStore, - &tmStore, persistentStores); + &tmStore, persistentStores, 120); auto* dummyGpioIF = new DummyGpioIF(); auto* dummySwitcher = new DummyPowerSwitcher(objects::PCDU_HANDLER, 18, 0); diff --git a/mission/genericFactory.cpp b/mission/genericFactory.cpp index 06f1220e..2dd00ee6 100644 --- a/mission/genericFactory.cpp +++ b/mission/genericFactory.cpp @@ -96,9 +96,10 @@ std::atomic_bool tcs::TCS_BOARD_SHORTLY_UNAVAILABLE = false; void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFunnel** pusFunnel, CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan, StorageManagerIF** ipcStore, StorageManagerIF** tmStore, - PersistentTmStores& stores) { + PersistentTmStores& stores, + uint32_t eventManagerQueueDepth) { // Framework objects - new EventManager(objects::EVENT_MANAGER, 160); + new EventManager(objects::EVENT_MANAGER, eventManagerQueueDepth); auto healthTable = new HealthTable(objects::HEALTH_TABLE); if (healthTable_ != nullptr) { *healthTable_ = healthTable; diff --git a/mission/genericFactory.h b/mission/genericFactory.h index 6cd2068d..a3a52704 100644 --- a/mission/genericFactory.h +++ b/mission/genericFactory.h @@ -45,7 +45,7 @@ namespace ObjectFactory { void produceGenericObjects(HealthTableIF** healthTable, PusTmFunnel** pusFunnel, CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan, StorageManagerIF** ipcStore, StorageManagerIF** tmStore, - PersistentTmStores& stores); + PersistentTmStores& stores, uint32_t eventManagerQueueDepth); void createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF& pwrSwitcher, HeaterHandler*& heaterHandler); From e31a95d1c47ae5064b33f8ef6e06c2c775081e39 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 26 Apr 2023 16:23:50 +0200 Subject: [PATCH 037/506] host build requires dedicated live TM task.. --- bsp_hosted/scheduling.cpp | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/bsp_hosted/scheduling.cpp b/bsp_hosted/scheduling.cpp index 0a5f1c35..5b9177f3 100644 --- a/bsp_hosted/scheduling.cpp +++ b/bsp_hosted/scheduling.cpp @@ -59,19 +59,15 @@ void scheduling::initTasks() { "DIST", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); ReturnValue_t result = tmtcDistributor->addComponent(objects::CCSDS_PACKET_DISTRIBUTOR); if (result != returnvalue::OK) { - sif::error << "adding CCSDS distributor failed" << std::endl; + sif::error << "Adding CCSDS distributor failed" << std::endl; } result = tmtcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR); if (result != returnvalue::OK) { - sif::error << "adding PUS distributor failed" << std::endl; - } - result = tmtcDistributor->addComponent(objects::TM_FUNNEL); - if (result != returnvalue::OK) { - sif::error << "adding TM funnel failed" << std::endl; + sif::error << "Adding PUS distributor failed" << std::endl; } result = tmtcDistributor->addComponent(objects::CFDP_DISTRIBUTOR); if (result != returnvalue::OK) { - sif::error << "adding CFDP distributor failed" << std::endl; + sif::error << "Adding CFDP distributor failed" << std::endl; } result = tmtcDistributor->addComponent(objects::UDP_TMTC_SERVER); if (result != returnvalue::OK) { @@ -94,6 +90,13 @@ void scheduling::initTasks() { if (result != returnvalue::OK) { sif::error << "Add component UDP Polling failed" << std::endl; } + // All the TM store tasks run in permanent loops, frequency does not matter + PeriodicTaskIF* liveTmTask = factory->createPeriodicTask( + "LIVE_TM", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr, &RR_SCHEDULING); + result = liveTmTask->addComponent(objects::LIVE_TM_TASK); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("LIVE_TM", objects::LIVE_TM_TASK); + } PeriodicTaskIF* pusHighPrio = factory->createPeriodicTask( "PUS_HIGH_PRIO", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); @@ -149,7 +152,7 @@ void scheduling::initTasks() { "THERMAL_CTL_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); result = thermalTask->addComponent(objects::CORE_CONTROLLER); if (result != returnvalue::OK) { - scheduling::printAddObjectError("Core controller dummy", objects::CORE_CONTROLLER); + scheduling::printAddObjectError("CORE_CTRL", objects::CORE_CONTROLLER); } result = thermalTask->addComponent(objects::THERMAL_CONTROLLER); if (result != returnvalue::OK) { @@ -217,6 +220,7 @@ void scheduling::initTasks() { tmtcDistributor->startTask(); udpPollingTask->startTask(); tcpPollingTask->startTask(); + liveTmTask->startTask(); pusHighPrio->startTask(); pusMedPrio->startTask(); From b7383294c9d44c7bb85cea6b706899adc9deed19 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 26 Apr 2023 16:41:40 +0200 Subject: [PATCH 038/506] add back tm funnel handler for hosted build --- bsp_hosted/ObjectFactory.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index bdbd577b..d792b57b 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -66,6 +66,7 @@ void ObjectFactory::produce(void* args) { ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel, *sdcMan, &ipcStore, &tmStore, persistentStores, 120); + new TmFunnelHandler(objects::LIVE_TM_TASK, *pusFunnel, *cfdpFunnel); auto* dummyGpioIF = new DummyGpioIF(); auto* dummySwitcher = new DummyPowerSwitcher(objects::PCDU_HANDLER, 18, 0); std::vector switcherList; From aa63bd1fa57f703d900a39384f80e51148ab5569 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 26 Apr 2023 17:00:04 +0200 Subject: [PATCH 039/506] EM adaptions --- bsp_q7s/em/emObjectFactory.cpp | 3 +++ dummies/helperFactory.cpp | 4 +++- dummies/helperFactory.h | 1 + 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 05a3fee0..96f541da 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -57,6 +57,9 @@ void ObjectFactory::produce(void* args) { #if OBSW_ADD_GOMSPACE_PCDU == 1 dummyCfg.addPowerDummies = false; #endif +#if OBSW_ADD_BPX_BATTERY_HANDLER == 1 + dummyCfg.addBpxBattDummy = false; +#endif #if OBSW_ADD_ACS_BOARD == 1 dummyCfg.addAcsBoardDummies = false; #endif diff --git a/dummies/helperFactory.cpp b/dummies/helperFactory.cpp index 98d2ecfa..ff18370a 100644 --- a/dummies/helperFactory.cpp +++ b/dummies/helperFactory.cpp @@ -45,7 +45,9 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpioIF) { new ComIFDummy(objects::DUMMY_COM_IF); auto* comCookieDummy = new ComCookieDummy(); - new BpxDummy(objects::BPX_BATT_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + if (cfg.addBpxBattDummy) { + new BpxDummy(objects::BPX_BATT_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + } if (cfg.addCoreCtrlCfg) { new CoreControllerDummy(objects::CORE_CONTROLLER); } diff --git a/dummies/helperFactory.h b/dummies/helperFactory.h index 2181c79c..dc272650 100644 --- a/dummies/helperFactory.h +++ b/dummies/helperFactory.h @@ -9,6 +9,7 @@ namespace dummy { struct DummyCfg { bool addCoreCtrlCfg = true; bool addPowerDummies = true; + bool addBpxBattDummy = true; bool addSyrlinksDummies = true; bool addAcsBoardDummies = true; bool addSusDummies = true; From 7dfc7dc8c179e1ffad6c6a215f964388ad40cc9d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 26 Apr 2023 17:26:32 +0200 Subject: [PATCH 040/506] configurable event manager queue depth --- bsp_q7s/em/emObjectFactory.cpp | 3 ++- bsp_q7s/fmObjectFactory.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 96f541da..0859b520 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -36,7 +36,8 @@ void ObjectFactory::produce(void* args) { PersistentTmStores stores; ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel, - *SdCardManager::instance(), &ipcStore, &tmStore, stores); + *SdCardManager::instance(), &ipcStore, &tmStore, stores, + 200); LinuxLibgpioIF* gpioComIF = nullptr; SerialComIF* uartComIF = nullptr; diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 5cb7c751..927c807e 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -32,7 +32,8 @@ void ObjectFactory::produce(void* args) { PersistentTmStores stores; ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel, - *SdCardManager::instance(), &ipcStore, &tmStore, stores); + *SdCardManager::instance(), &ipcStore, &tmStore, stores, + 200); LinuxLibgpioIF* gpioComIF = nullptr; SerialComIF* uartComIF = nullptr; From 490790bd0cae3899481fd761ec28378c5b53bf7a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 26 Apr 2023 17:36:38 +0200 Subject: [PATCH 041/506] host SW works properly again --- dummies/helperFactory.cpp | 8 ++++---- mission/scheduling.cpp | 10 ++++++---- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/dummies/helperFactory.cpp b/dummies/helperFactory.cpp index ff18370a..e517426a 100644 --- a/dummies/helperFactory.cpp +++ b/dummies/helperFactory.cpp @@ -197,10 +197,10 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio objects::TMP1075_HANDLER_PLPCDU_0, new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_0, objects::DUMMY_COM_IF, comCookieDummy)); // damaged. - // tmpSensorDummies.emplace( - // objects::TMP1075_HANDLER_PLPCDU_1, - // new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_1, objects::DUMMY_COM_IF, - // comCookieDummy)); + // tmpSensorDummies.emplace( + // objects::TMP1075_HANDLER_PLPCDU_1, + // new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_1, objects::DUMMY_COM_IF, + // comCookieDummy)); tmpSensorDummies.emplace( objects::TMP1075_HANDLER_IF_BOARD, new Tmp1075Dummy(objects::TMP1075_HANDLER_IF_BOARD, objects::DUMMY_COM_IF, comCookieDummy)); diff --git a/mission/scheduling.cpp b/mission/scheduling.cpp index f89ce711..27a6417c 100644 --- a/mission/scheduling.cpp +++ b/mission/scheduling.cpp @@ -5,10 +5,12 @@ #include "fsfw/tasks/PeriodicTaskIF.h" void scheduling::scheduleTmpTempSensors(PeriodicTaskIF* tmpTask) { - const std::array tmpIds = { - objects::TMP1075_HANDLER_TCS_0, objects::TMP1075_HANDLER_TCS_1, - objects::TMP1075_HANDLER_PLPCDU_0, objects::TMP1075_HANDLER_PLPCDU_1, - objects::TMP1075_HANDLER_IF_BOARD}; + const std::array tmpIds = {objects::TMP1075_HANDLER_TCS_0, + objects::TMP1075_HANDLER_TCS_1, + objects::TMP1075_HANDLER_PLPCDU_0, + // damaged. + // objects::TMP1075_HANDLER_PLPCDU_1, + objects::TMP1075_HANDLER_IF_BOARD}; for (const auto& tmpId : tmpIds) { tmpTask->addComponent(tmpId, DeviceHandlerIF::PERFORM_OPERATION); tmpTask->addComponent(tmpId, DeviceHandlerIF::SEND_WRITE); From 67c0a3b03ede7d79d985c0400900d8a539fe0363 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 26 Apr 2023 17:38:06 +0200 Subject: [PATCH 042/506] lower live TM handler frequency --- bsp_hosted/scheduling.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bsp_hosted/scheduling.cpp b/bsp_hosted/scheduling.cpp index 5b9177f3..5fd53906 100644 --- a/bsp_hosted/scheduling.cpp +++ b/bsp_hosted/scheduling.cpp @@ -90,9 +90,9 @@ void scheduling::initTasks() { if (result != returnvalue::OK) { sif::error << "Add component UDP Polling failed" << std::endl; } - // All the TM store tasks run in permanent loops, frequency does not matter + PeriodicTaskIF* liveTmTask = factory->createPeriodicTask( - "LIVE_TM", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr, &RR_SCHEDULING); + "LIVE_TM", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.4, nullptr, &RR_SCHEDULING); result = liveTmTask->addComponent(objects::LIVE_TM_TASK); if (result != returnvalue::OK) { scheduling::printAddObjectError("LIVE_TM", objects::LIVE_TM_TASK); From 5747f73dfe55b5b2c6f475db6640d188e0d58cb6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 19:31:50 +0200 Subject: [PATCH 043/506] changelog --- CHANGELOG.md | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3d543d0f..f4efc53a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,18 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v2.1.0] to be released + +## Changed + +- Event Manager queue depth is configurable now. +- Do not construct and schedule broken TMP1075 device anymore. + +## Fixed + +- Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP + funnel. + # [v2.0.5] to be released - The dual lane assembly transition failed handler started new transitions towards the current mode From 2f58ca853440aa2408bbd2524a3fb42c485ac92c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:22:21 +0200 Subject: [PATCH 044/506] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index f4efc53a..cc94360b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,6 +27,11 @@ will consitute of a breaking change warranting a new major release: - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. +- The dual lane assembly transition failed handler started new transitions towards the current mode + instead of the target mode. This means that if the dual lane assembly never reached the initial + submode (e.g. mode normal and submode dual side), it will transition back to the current mode, + which is `MODE_OFF`. Furthermore, this lead to invalid internal states, so the subsequent + recovery handling becomes stuck in the custom recovery sequence when swichting power back on. # [v2.0.5] to be released From fb062bb4e841d1e6695f055486f2a7e531e01b94 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:23:21 +0200 Subject: [PATCH 045/506] changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index cc94360b..c6fb32c9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -30,7 +30,7 @@ will consitute of a breaking change warranting a new major release: - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial submode (e.g. mode normal and submode dual side), it will transition back to the current mode, - which is `MODE_OFF`. Furthermore, this lead to invalid internal states, so the subsequent + which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent recovery handling becomes stuck in the custom recovery sequence when swichting power back on. # [v2.0.5] to be released From 09ffcc88046f512c67f8698c7cd92c03cc1fcad3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:28:05 +0200 Subject: [PATCH 046/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index c6fb32c9..a3c299f0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -32,6 +32,8 @@ will consitute of a breaking change warranting a new major release: submode (e.g. mode normal and submode dual side), it will transition back to the current mode, which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent recovery handling becomes stuck in the custom recovery sequence when swichting power back on. +- The dual lane custom recovery handling was adapted to always perform proper power switch handling + irrespective of current or target modes. # [v2.0.5] to be released From 1cbb7d0af8c3c2924c7c0b0eb9f374b36e772c81 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 19:32:33 +0200 Subject: [PATCH 047/506] changelog --- CHANGELOG.md | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a3c299f0..ba80d0fe 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,8 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v2.2.0] to be released + # [v2.1.0] to be released ## Changed @@ -27,13 +29,6 @@ will consitute of a breaking change warranting a new major release: - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. -- The dual lane assembly transition failed handler started new transitions towards the current mode - instead of the target mode. This means that if the dual lane assembly never reached the initial - submode (e.g. mode normal and submode dual side), it will transition back to the current mode, - which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent - recovery handling becomes stuck in the custom recovery sequence when swichting power back on. -- The dual lane custom recovery handling was adapted to always perform proper power switch handling - irrespective of current or target modes. # [v2.0.5] to be released From f5e20db03f5914f9f609b00ef6c68e8d97fb7ae2 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 19 Apr 2023 15:07:21 +0200 Subject: [PATCH 048/506] added remaining acs modes as system modes --- mission/sysDefs.h | 11 +- mission/system/EiveSystem.cpp | 12 +- mission/system/systemTree.cpp | 254 ++++++++++++++++++++++++++++++---- 3 files changed, 247 insertions(+), 30 deletions(-) diff --git a/mission/sysDefs.h b/mission/sysDefs.h index e8303524..a74cf369 100644 --- a/mission/sysDefs.h +++ b/mission/sysDefs.h @@ -9,8 +9,15 @@ extern std::atomic_uint16_t I2C_FATAL_ERRORS; namespace satsystem { -enum Mode : Mode_t { BOOT = 5, SAFE = acs::AcsMode::SAFE, PTG_IDLE = acs::AcsMode::PTG_IDLE }; - +enum Mode : Mode_t { + BOOT = 5, + SAFE = acs::AcsMode::SAFE, + PTG_IDLE = acs::AcsMode::PTG_IDLE, + PTG_NADIR = acs::AcsMode::PTG_NADIR, + PTG_TARGET = acs::AcsMode::PTG_TARGET, + PTG_TARGET_GS = acs::AcsMode::PTG_TARGET_GS, + PTG_INERTIAL = acs::AcsMode::PTG_INERTIAL, +}; } namespace xsc { diff --git a/mission/system/EiveSystem.cpp b/mission/system/EiveSystem.cpp index 53bf7a32..f17b82cb 100644 --- a/mission/system/EiveSystem.cpp +++ b/mission/system/EiveSystem.cpp @@ -39,18 +39,22 @@ void EiveSystem::announceMode(bool recursive) { modeStr = "POINTING IDLE"; break; } - case (acs::AcsMode::PTG_INERTIAL): { - modeStr = "POINTING INERTIAL"; + case (satsystem::Mode::PTG_NADIR): { + modeStr = "POINTING NADIR"; break; } - case (acs::AcsMode::PTG_TARGET): { + case (satsystem::Mode::PTG_TARGET): { modeStr = "POINTING TARGET"; break; } - case (acs::AcsMode::PTG_TARGET_GS): { + case (satsystem::Mode::PTG_TARGET_GS): { modeStr = "POINTING TARGET GS"; break; } + case (satsystem::Mode::PTG_INERTIAL): { + modeStr = "POINTING INERTIAL"; + break; + } } sif::info << "EIVE system is now in " << modeStr << " mode" << std::endl; return Subsystem::announceMode(recursive); diff --git a/mission/system/systemTree.cpp b/mission/system/systemTree.cpp index 37adbaf5..1b5b6c4b 100644 --- a/mission/system/systemTree.cpp +++ b/mission/system/systemTree.cpp @@ -22,6 +22,10 @@ const auto check = subsystem::checkInsert; void buildBootSequence(Subsystem& ss, ModeListEntry& eh); void buildSafeSequence(Subsystem& ss, ModeListEntry& eh); void buildIdleSequence(Subsystem& ss, ModeListEntry& eh); +void buildPtgNadirSequence(Subsystem& ss, ModeListEntry& eh); +void buildPtgTargetSequence(Subsystem& ss, ModeListEntry& eh); +void buildPtgTargetGsSequence(Subsystem& ss, ModeListEntry& eh); +void buildPtgInertialSequence(Subsystem& ss, ModeListEntry& eh); } // namespace static const auto OFF = HasModesIF::MODE_OFF; @@ -40,6 +44,10 @@ void satsystem::init() { buildBootSequence(EIVE_SYSTEM, entry); buildSafeSequence(EIVE_SYSTEM, entry); buildIdleSequence(EIVE_SYSTEM, entry); + buildPtgNadirSequence(EIVE_SYSTEM, entry); + buildPtgTargetSequence(EIVE_SYSTEM, entry); + buildPtgTargetGsSequence(EIVE_SYSTEM, entry); + buildPtgInertialSequence(EIVE_SYSTEM, entry); EIVE_SYSTEM.setInitialMode(satsystem::Mode::BOOT, 0); } @@ -68,8 +76,89 @@ auto EIVE_TABLE_IDLE_TRANS_0 = auto EIVE_TABLE_IDLE_TRANS_1 = std::make_pair((satsystem::Mode::PTG_IDLE << 24) | 3, FixedArrayList()); +auto EIVE_SEQUENCE_PTG_NADIR = + std::make_pair(satsystem::Mode::PTG_NADIR, FixedArrayList()); +auto EIVE_TABLE_PTG_NADIR_TGT = + std::make_pair((satsystem::Mode::PTG_NADIR << 24) | 1, FixedArrayList()); +auto EIVE_TABLE_PTG_NADIR_TRANS_0 = + std::make_pair((satsystem::Mode::PTG_NADIR << 24) | 2, FixedArrayList()); +auto EIVE_TABLE_PTG_NADIR_TRANS_1 = + std::make_pair((satsystem::Mode::PTG_NADIR << 24) | 3, FixedArrayList()); + +auto EIVE_SEQUENCE_PTG_TARGET = + std::make_pair(satsystem::Mode::PTG_TARGET, FixedArrayList()); +auto EIVE_TABLE_PTG_TARGET_TGT = + std::make_pair((satsystem::Mode::PTG_TARGET << 24) | 1, FixedArrayList()); +auto EIVE_TABLE_PTG_TARGET_TRANS_0 = + std::make_pair((satsystem::Mode::PTG_TARGET << 24) | 2, FixedArrayList()); +auto EIVE_TABLE_PTG_TARGET_TRANS_1 = + std::make_pair((satsystem::Mode::PTG_TARGET << 24) | 3, FixedArrayList()); + +auto EIVE_SEQUENCE_PTG_TARGET_GS = + std::make_pair(satsystem::Mode::PTG_TARGET_GS, FixedArrayList()); +auto EIVE_TABLE_PTG_TARGET_GS_TGT = + std::make_pair((satsystem::Mode::PTG_TARGET_GS << 24) | 1, FixedArrayList()); +auto EIVE_TABLE_PTG_TARGET_GS_TRANS_0 = + std::make_pair((satsystem::Mode::PTG_TARGET_GS << 24) | 2, FixedArrayList()); +auto EIVE_TABLE_PTG_TARGET_GS_TRANS_1 = + std::make_pair((satsystem::Mode::PTG_TARGET_GS << 24) | 3, FixedArrayList()); + +auto EIVE_SEQUENCE_PTG_INERTIAL = + std::make_pair(satsystem::Mode::PTG_INERTIAL, FixedArrayList()); +auto EIVE_TABLE_PTG_INERTIAL_TGT = + std::make_pair((satsystem::Mode::PTG_INERTIAL << 24) | 1, FixedArrayList()); +auto EIVE_TABLE_PTG_INERTIAL_TRANS_0 = + std::make_pair((satsystem::Mode::PTG_INERTIAL << 24) | 2, FixedArrayList()); +auto EIVE_TABLE_PTG_INERTIAL_TRANS_1 = + std::make_pair((satsystem::Mode::PTG_INERTIAL << 24) | 3, FixedArrayList()); + namespace { +void buildBootSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::buildBootSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table, + bool allowAllSubmodes = false) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + if (allowAllSubmodes) { + eh.allowAllSubmodes(); + } + check(table.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::OFF, 0, EIVE_TABLE_BOOT_TGT.second, true); + iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TGT.second); + iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_BOOT_TGT.second); + iht(objects::TCS_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TGT.second); + check(ss.addTable(TableEntry(EIVE_TABLE_BOOT_TGT.first, &EIVE_TABLE_BOOT_TGT.second)), ctxc); + + // Build SAFE transition 0. + iht(objects::TCS_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TRANS_0.second); + iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_BOOT_TRANS_0.second); + iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TRANS_0.second); + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::OFF, 0, EIVE_TABLE_BOOT_TRANS_0.second, true); + check(ss.addTable(TableEntry(EIVE_TABLE_BOOT_TRANS_0.first, &EIVE_TABLE_BOOT_TRANS_0.second)), + ctxc); + + // Build Safe sequence + ihs(EIVE_SEQUENCE_BOOT.second, EIVE_TABLE_BOOT_TGT.first, 0, false); + ihs(EIVE_SEQUENCE_BOOT.second, EIVE_TABLE_BOOT_TRANS_0.first, 0, false); + check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_BOOT.first, &EIVE_SEQUENCE_BOOT.second, + EIVE_SEQUENCE_SAFE.first)), + ctxc); +} + void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { std::string context = "satsystem::buildSafeSequence"; auto ctxc = context.c_str(); @@ -150,18 +239,14 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { ctxc); } -void buildBootSequence(Subsystem& ss, ModeListEntry& eh) { - std::string context = "satsystem::buildBootSequence"; +void buildPtgNadirSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::buildPtgNadirSequence"; auto ctxc = context.c_str(); // Insert Helper Table - auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table, - bool allowAllSubmodes = false) { + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { eh.setObject(obj); eh.setMode(mode); eh.setSubmode(submode); - if (allowAllSubmodes) { - eh.allowAllSubmodes(); - } check(table.insert(eh), ctxc); }; // Insert Helper Sequence @@ -173,25 +258,146 @@ void buildBootSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; - iht(objects::ACS_SUBSYSTEM, acs::AcsMode::OFF, 0, EIVE_TABLE_BOOT_TGT.second, true); - iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TGT.second); - iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_BOOT_TGT.second); - iht(objects::TCS_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TGT.second); - check(ss.addTable(TableEntry(EIVE_TABLE_BOOT_TGT.first, &EIVE_TABLE_BOOT_TGT.second)), ctxc); - - // Build SAFE transition 0. - iht(objects::TCS_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TRANS_0.second); - iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_BOOT_TRANS_0.second); - iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TRANS_0.second); - iht(objects::ACS_SUBSYSTEM, acs::AcsMode::OFF, 0, EIVE_TABLE_BOOT_TRANS_0.second, true); - check(ss.addTable(TableEntry(EIVE_TABLE_BOOT_TRANS_0.first, &EIVE_TABLE_BOOT_TRANS_0.second)), + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_NADIR, 0, EIVE_TABLE_PTG_NADIR_TGT.second); + check(ss.addTable(TableEntry(EIVE_TABLE_PTG_NADIR_TGT.first, &EIVE_TABLE_PTG_NADIR_TGT.second)), ctxc); - // Build Safe sequence - ihs(EIVE_SEQUENCE_BOOT.second, EIVE_TABLE_BOOT_TGT.first, 0, false); - ihs(EIVE_SEQUENCE_BOOT.second, EIVE_TABLE_BOOT_TRANS_0.first, 0, false); - check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_BOOT.first, &EIVE_SEQUENCE_BOOT.second, - EIVE_SEQUENCE_SAFE.first)), + // Build PTG_NADIR transition 0 + iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second); + iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second); + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_NADIR, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second); + check(ss.addTable( + TableEntry(EIVE_TABLE_PTG_NADIR_TRANS_0.first, &EIVE_TABLE_PTG_NADIR_TRANS_0.second)), + ctxc); + + // Build PTG_NADIR sequence + ihs(EIVE_SEQUENCE_PTG_NADIR.second, EIVE_TABLE_PTG_NADIR_TGT.first, 0, false); + ihs(EIVE_SEQUENCE_PTG_NADIR.second, EIVE_TABLE_PTG_NADIR_TRANS_0.first, 0, false); + check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_PTG_NADIR.first, &EIVE_SEQUENCE_PTG_NADIR.second, + EIVE_SEQUENCE_IDLE.first)), ctxc); } + +void buildPtgTargetSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::buildPtgTargetSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(table.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET, 0, EIVE_TABLE_PTG_TARGET_TGT.second); + check(ss.addTable(TableEntry(EIVE_TABLE_PTG_TARGET_TGT.first, &EIVE_TABLE_PTG_TARGET_TGT.second)), + ctxc); + + // Build PTG_TARGET transition 0 + iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second); + iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second); + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second); + check(ss.addTable( + TableEntry(EIVE_TABLE_PTG_TARGET_TRANS_0.first, &EIVE_TABLE_PTG_TARGET_TRANS_0.second)), + ctxc); + + // Build PTG_TARGET sequence + ihs(EIVE_SEQUENCE_PTG_TARGET.second, EIVE_TABLE_PTG_TARGET_TGT.first, 0, false); + ihs(EIVE_SEQUENCE_PTG_TARGET.second, EIVE_TABLE_PTG_TARGET_TRANS_0.first, 0, false); + check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_PTG_TARGET.first, + &EIVE_SEQUENCE_PTG_TARGET.second, EIVE_SEQUENCE_IDLE.first)), + ctxc); +} + +void buildPtgTargetGsSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::buildPtgTargetGsSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(table.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET_GS, 0, EIVE_TABLE_PTG_TARGET_GS_TGT.second); + check(ss.addTable( + TableEntry(EIVE_TABLE_PTG_TARGET_GS_TGT.first, &EIVE_TABLE_PTG_TARGET_GS_TGT.second)), + ctxc); + + // Build PTG_TARGET_GS transition 0 + iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second); + iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second); + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET_GS, 0, + EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second); + check(ss.addTable(TableEntry(EIVE_TABLE_PTG_TARGET_GS_TRANS_0.first, + &EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second)), + ctxc); + + // Build PTG_TARGET_GS sequence + ihs(EIVE_SEQUENCE_PTG_TARGET_GS.second, EIVE_TABLE_PTG_TARGET_GS_TGT.first, 0, false); + ihs(EIVE_SEQUENCE_PTG_TARGET_GS.second, EIVE_TABLE_PTG_TARGET_GS_TRANS_0.first, 0, false); + check( + ss.addSequence(SequenceEntry(EIVE_SEQUENCE_PTG_TARGET_GS.first, + &EIVE_SEQUENCE_PTG_TARGET_GS.second, EIVE_SEQUENCE_IDLE.first)), + ctxc); +} + +void buildPtgInertialSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::buildPtgInertialSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(table.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_INERTIAL, 0, EIVE_TABLE_PTG_INERTIAL_TGT.second); + check(ss.addTable( + TableEntry(EIVE_TABLE_PTG_INERTIAL_TGT.first, &EIVE_TABLE_PTG_INERTIAL_TGT.second)), + ctxc); + + // Build PTG_INERTIAL transition 0 + iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_INERTIAL_TRANS_0.second); + iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_PTG_INERTIAL_TRANS_0.second); + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_INERTIAL, 0, + EIVE_TABLE_PTG_INERTIAL_TRANS_0.second); + check(ss.addTable(TableEntry(EIVE_TABLE_PTG_INERTIAL_TRANS_0.first, + &EIVE_TABLE_PTG_INERTIAL_TRANS_0.second)), + ctxc); + + // Build PTG_INERTIAL sequence + ihs(EIVE_SEQUENCE_PTG_INERTIAL.second, EIVE_TABLE_PTG_INERTIAL_TGT.first, 0, false); + ihs(EIVE_SEQUENCE_PTG_INERTIAL.second, EIVE_TABLE_PTG_INERTIAL_TRANS_0.first, 0, false); + check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_PTG_INERTIAL.first, + &EIVE_SEQUENCE_PTG_INERTIAL.second, EIVE_SEQUENCE_IDLE.first)), + ctxc); +} + } // namespace From 141438bd24e7141c9b123fb406c29a373542211e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 15:04:30 +0200 Subject: [PATCH 049/506] changelog and stop payload tracking --- CHANGELOG.md | 1 + mission/system/systemTree.cpp | 6 ------ 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index ba80d0fe..67e7a6f4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,6 +24,7 @@ will consitute of a breaking change warranting a new major release: - Event Manager queue depth is configurable now. - Do not construct and schedule broken TMP1075 device anymore. +- Do not track payload modes in system mode tables. ## Fixed diff --git a/mission/system/systemTree.cpp b/mission/system/systemTree.cpp index 1b5b6c4b..41bf3216 100644 --- a/mission/system/systemTree.cpp +++ b/mission/system/systemTree.cpp @@ -184,7 +184,6 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { // Do no track submode to allow transitions to DETUMBLE submode. iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TGT.second, true); - iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TGT.second); check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TGT.first, &EIVE_TABLE_SAFE_TGT.second)), ctxc); // Build SAFE transition 0. @@ -226,7 +225,6 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { // Build IDLE transition 0 iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_IDLE_TRANS_0.second); - iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_IDLE_TRANS_0.second); iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TRANS_0.second); check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_0.first, &EIVE_TABLE_IDLE_TRANS_0.second)), ctxc); @@ -264,7 +262,6 @@ void buildPtgNadirSequence(Subsystem& ss, ModeListEntry& eh) { // Build PTG_NADIR transition 0 iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second); - iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second); iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_NADIR, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second); check(ss.addTable( TableEntry(EIVE_TABLE_PTG_NADIR_TRANS_0.first, &EIVE_TABLE_PTG_NADIR_TRANS_0.second)), @@ -303,7 +300,6 @@ void buildPtgTargetSequence(Subsystem& ss, ModeListEntry& eh) { // Build PTG_TARGET transition 0 iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second); - iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second); iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second); check(ss.addTable( TableEntry(EIVE_TABLE_PTG_TARGET_TRANS_0.first, &EIVE_TABLE_PTG_TARGET_TRANS_0.second)), @@ -343,7 +339,6 @@ void buildPtgTargetGsSequence(Subsystem& ss, ModeListEntry& eh) { // Build PTG_TARGET_GS transition 0 iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second); - iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second); iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET_GS, 0, EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second); check(ss.addTable(TableEntry(EIVE_TABLE_PTG_TARGET_GS_TRANS_0.first, @@ -385,7 +380,6 @@ void buildPtgInertialSequence(Subsystem& ss, ModeListEntry& eh) { // Build PTG_INERTIAL transition 0 iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_INERTIAL_TRANS_0.second); - iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_PTG_INERTIAL_TRANS_0.second); iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_INERTIAL, 0, EIVE_TABLE_PTG_INERTIAL_TRANS_0.second); check(ss.addTable(TableEntry(EIVE_TABLE_PTG_INERTIAL_TRANS_0.first, From a352f25029624e625ae0535584570bf64a4c92c0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 15:18:16 +0200 Subject: [PATCH 050/506] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 67e7a6f4..19a867f5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -26,6 +26,10 @@ will consitute of a breaking change warranting a new major release: - Do not construct and schedule broken TMP1075 device anymore. - Do not track payload modes in system mode tables. +## Added + +- Add the remaining system modes derives from the ACS modes. + ## Fixed - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP From 26e48f2f46342b60c6d69d01fb872f4cd558d2c4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 15:18:47 +0200 Subject: [PATCH 051/506] changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 19a867f5..5caaf84b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -28,7 +28,7 @@ will consitute of a breaking change warranting a new major release: ## Added -- Add the remaining system modes derives from the ACS modes. +- Add the remaining system modes derived from the ACS modes. ## Fixed From 670c753a75350a47e4ba2e5c5e32c60f9e970d94 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 15:30:09 +0200 Subject: [PATCH 052/506] hello --- CHANGELOG.md | 3 ++- mission/acs/defs.h | 13 +++++++------ mission/sysDefs.h | 21 +++++++++++++-------- 3 files changed, 22 insertions(+), 15 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5caaf84b..e872810d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -25,10 +25,11 @@ will consitute of a breaking change warranting a new major release: - Event Manager queue depth is configurable now. - Do not construct and schedule broken TMP1075 device anymore. - Do not track payload modes in system mode tables. +- ACS modes derived from system modes. ## Added -- Add the remaining system modes derived from the ACS modes. +- Add the remaining system modes. ## Fixed diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 7748562d..41d09976 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -3,6 +3,7 @@ #include #include +#include namespace acs { @@ -11,12 +12,12 @@ enum class SimpleSensorMode { NORMAL = 0, OFF = 1 }; // These modes are the modes of the ACS controller and of the ACS subsystem. enum AcsMode : Mode_t { OFF = HasModesIF::MODE_OFF, - SAFE = 10, - PTG_IDLE = 11, - PTG_NADIR = 12, - PTG_TARGET = 13, - PTG_TARGET_GS = 14, - PTG_INERTIAL = 15, + SAFE = satsystem::Mode::SAFE, + PTG_IDLE = satsystem::Mode::PTG_IDLE, + PTG_NADIR = satsystem::Mode::PTG_NADIR, + PTG_TARGET = satsystem::Mode::PTG_TARGET, + PTG_TARGET_GS = satsystem::Mode::PTG_TARGET_GS, + PTG_INERTIAL = satsystem::Mode::PTG_INERTIAL, }; enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 }; diff --git a/mission/sysDefs.h b/mission/sysDefs.h index a74cf369..c84c237f 100644 --- a/mission/sysDefs.h +++ b/mission/sysDefs.h @@ -1,9 +1,13 @@ #ifndef MISSION_SYSDEFS_H_ #define MISSION_SYSDEFS_H_ -#include +#include +#include +#include +#include -#include "acs/defs.h" +#include +#include extern std::atomic_uint16_t I2C_FATAL_ERRORS; @@ -11,12 +15,13 @@ namespace satsystem { enum Mode : Mode_t { BOOT = 5, - SAFE = acs::AcsMode::SAFE, - PTG_IDLE = acs::AcsMode::PTG_IDLE, - PTG_NADIR = acs::AcsMode::PTG_NADIR, - PTG_TARGET = acs::AcsMode::PTG_TARGET, - PTG_TARGET_GS = acs::AcsMode::PTG_TARGET_GS, - PTG_INERTIAL = acs::AcsMode::PTG_INERTIAL, + // DO NOT CHANGE THE ORDER starting from here, breaks ACS mode checks. + SAFE = 10, + PTG_IDLE = 11, + PTG_NADIR = 12, + PTG_TARGET = 13, + PTG_TARGET_GS = 14, + PTG_INERTIAL = 15, }; } From 51b0d897a16c12dde02c3565329ba2662a5ca42f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 10:18:46 +0200 Subject: [PATCH 053/506] adapt EM SW: GS PCDU added, but use dummy for ACU --- CMakeLists.txt | 5 ++++- bsp_q7s/OBSWConfig.h.in | 3 +++ bsp_q7s/core/ObjectFactory.cpp | 7 +++++-- bsp_q7s/em/emObjectFactory.cpp | 4 +++- dummies/helperFactory.cpp | 3 ++- dummies/helperFactory.h | 3 +++ 6 files changed, 20 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ace83e22..9589b20f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -146,8 +146,11 @@ set(OBSW_ADD_TMP_DEVICES ${INIT_VAL} CACHE STRING "Add TMP devices") set(OBSW_ADD_GOMSPACE_PCDU - ${INIT_VAL} + 1 CACHE STRING "Add GomSpace PCDU modules") +set(OBSW_ADD_GOMSPACE_ACU + ${INIT_VAL} + CACHE STRING "Add GomSpace ACU submodule") set(OBSW_ADD_RW ${INIT_VAL} CACHE STRING "Add RW modules") diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index 3502a7bb..51ed8828 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -22,6 +22,9 @@ #define OBSW_COMMAND_SAFE_MODE_AT_STARTUP 1 #define OBSW_ADD_GOMSPACE_PCDU @OBSW_ADD_GOMSPACE_PCDU@ +// This define is necessary because the EM setup has the P60 dock module, but no ACU on the P60 +// module because it broke. +#define OBSW_ADD_GOMSPACE_ACU @OBSW_ADD_GOMSPACE_ACU@ #define OBSW_ADD_MGT @OBSW_ADD_MGT@ #define OBSW_ADD_BPX_BATTERY_HANDLER @OBSW_ADD_BPX_BATTERY_HANDLER@ #define OBSW_ADD_STAR_TRACKER @OBSW_ADD_STAR_TRACKER@ diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 964d728b..dd363e3d 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -189,7 +189,6 @@ void ObjectFactory::createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, *i2cComIF = new I2cComIF(objects::I2C_COM_IF); *uartComIF = new SerialComIF(objects::UART_COM_IF); *spiMainComIF = new SpiComIF(objects::SPI_MAIN_COM_IF, q7s::SPI_DEFAULT_DEV, **gpioComIF); - //*spiRWComIF = new SpiComIF(objects::SPI_RW_COM_IF, q7s::SPI_RW_DEV, **gpioComIF); } void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher, @@ -197,7 +196,6 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI CspCookie* p60DockCspCookie = new CspCookie(P60Dock::MAX_REPLY_SIZE, addresses::P60DOCK, 500); CspCookie* pdu1CspCookie = new CspCookie(PDU::MAX_REPLY_SIZE, addresses::PDU1, 500); CspCookie* pdu2CspCookie = new CspCookie(PDU::MAX_REPLY_SIZE, addresses::PDU2, 500); - CspCookie* acuCspCookie = new CspCookie(ACU::MAX_REPLY_SIZE, addresses::ACU, 500); auto p60Fdir = new GomspacePowerFdir(objects::P60DOCK_HANDLER); P60DockHandler* p60dockhandler = new P60DockHandler(objects::P60DOCK_HANDLER, objects::CSP_COM_IF, @@ -211,9 +209,12 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI Pdu2Handler* pdu2handler = new Pdu2Handler(objects::PDU2_HANDLER, objects::CSP_COM_IF, pdu2CspCookie, pdu2Fdir, enableHkSets); +#if OBSW_ADD_GOMSPACE_ACU == 1 + CspCookie* acuCspCookie = new CspCookie(ACU::MAX_REPLY_SIZE, addresses::ACU, 500); auto acuFdir = new GomspacePowerFdir(objects::ACU_HANDLER); ACUHandler* acuhandler = new ACUHandler(objects::ACU_HANDLER, objects::CSP_COM_IF, acuCspCookie, acuFdir, enableHkSets); +#endif auto pcduHandler = new PcduHandler(objects::PCDU_HANDLER, 50); /** @@ -223,7 +224,9 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI p60dockhandler->setModeNormal(); pdu1handler->setModeNormal(); pdu2handler->setModeNormal(); +#if OBSW_ADD_GOMSPACE_ACU == 1 acuhandler->setModeNormal(); +#endif if (pwrSwitcher != nullptr) { *pwrSwitcher = pcduHandler; } diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 0859b520..67dec96d 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -57,6 +57,8 @@ void ObjectFactory::produce(void* args) { #endif #if OBSW_ADD_GOMSPACE_PCDU == 1 dummyCfg.addPowerDummies = false; + // The ACU broke. + dummyCfg.addOnlyAcuDummy = true; #endif #if OBSW_ADD_BPX_BATTERY_HANDLER == 1 dummyCfg.addBpxBattDummy = false; @@ -93,7 +95,7 @@ void ObjectFactory::produce(void* args) { // createRadSensorComponent(gpioComIF); #if OBSW_ADD_ACS_BOARD == 1 - createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher); + createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher, true); #else // Still add all GPIOs for EM. GpioCookie* acsBoardGpios = new GpioCookie(); diff --git a/dummies/helperFactory.cpp b/dummies/helperFactory.cpp index e517426a..73be6940 100644 --- a/dummies/helperFactory.cpp +++ b/dummies/helperFactory.cpp @@ -77,8 +77,9 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio auto* imtqDummy = new ImtqDummy(objects::IMTQ_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); imtqDummy->enableThermalModule(ThermalStateCfg()); imtqDummy->connectModeTreeParent(*imtqAssy); - if (cfg.addPowerDummies) { + if (cfg.addOnlyAcuDummy) { new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + } else if (cfg.addPowerDummies) { new PduDummy(objects::PDU1_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new PduDummy(objects::PDU2_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new P60DockDummy(objects::P60DOCK_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); diff --git a/dummies/helperFactory.h b/dummies/helperFactory.h index dc272650..83456e96 100644 --- a/dummies/helperFactory.h +++ b/dummies/helperFactory.h @@ -6,8 +6,11 @@ class GpioIF; namespace dummy { +// Default values targeted towards EM. struct DummyCfg { bool addCoreCtrlCfg = true; + // Special variant because the ACU broke. Overrides addPowerDummies, only ACU dummy will be added. + bool addOnlyAcuDummy = false; bool addPowerDummies = true; bool addBpxBattDummy = true; bool addSyrlinksDummies = true; From 1870bf6405a99d11bbd68abbcc787fc7d3206ea1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 19:33:02 +0200 Subject: [PATCH 054/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index e872810d..d8173e70 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -26,6 +26,8 @@ will consitute of a breaking change warranting a new major release: - Do not construct and schedule broken TMP1075 device anymore. - Do not track payload modes in system mode tables. - ACS modes derived from system modes. +- Adapt EM configuration to include all GomSpace PCDU devices except the ACU. For the ACU + (which broke), a dummy will still be used. ## Added From 1602d5d3188a7f61e1e46ce21a64008acff6415b Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 10 May 2023 11:24:12 +0200 Subject: [PATCH 055/506] stuff --- bsp_q7s/em/emObjectFactory.cpp | 2 +- cmake/Zynq7020CrossCompileConfig.cmake | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 67dec96d..81771a79 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -140,7 +140,7 @@ void ObjectFactory::produce(void* args) { #endif /* OBSW_ADD_TEST_CODE == 1 */ #if OBSW_ADD_SCEX_DEVICE == 1 createScexComponents(q7s::UART_SCEX_DEV, pwrSwitcher, *SdCardManager::instance(), false, - pcdu::Switches::PDU1_CH5_SOLAR_CELL_EXP_5V); + power::Switches::PDU1_CH5_SOLAR_CELL_EXP_5V); #endif createAcsController(true, enableHkSets); HeaterHandler* heaterHandler; diff --git a/cmake/Zynq7020CrossCompileConfig.cmake b/cmake/Zynq7020CrossCompileConfig.cmake index be6702a1..e6e762ad 100644 --- a/cmake/Zynq7020CrossCompileConfig.cmake +++ b/cmake/Zynq7020CrossCompileConfig.cmake @@ -40,8 +40,8 @@ set(CROSS_COMPILE_OBJCOPY "${CROSS_COMPILE}-objcopy") set(CROSS_COMPILE_SIZE "${CROSS_COMPILE}-size") # At the very least, cross compile gcc and g++ have to be set! -find_program (CMAKE_C_COMPILER ${CROSS_COMPILE_CC} REQUIRED) -find_program (CMAKE_CXX_COMPILER ${CROSS_COMPILE_CXX} REQUIRED) +find_program (CMAKE_C_COMPILER ${CROSS_COMPILE_CC} HINTS $ENV{CROSS_COMPILE_BIN_PATH} NO_DEFAULT_PATH REQUIRED) +find_program (CMAKE_CXX_COMPILER ${CROSS_COMPILE_CXX} HINTS $ENV{CROSS_COMPILE_BIN_PATH} NO_DEFAULT_PATH REQUIRED) # Useful utilities, not strictly necessary find_program(CMAKE_SIZE ${CROSS_COMPILE_SIZE}) find_program(CMAKE_OBJCOPY ${CROSS_COMPILE_OBJCOPY}) From 1412dd4fb6239a307d2abfd8109a49365d07bc48 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 10 May 2023 11:33:34 +0200 Subject: [PATCH 056/506] changelog readme etc. --- CHANGELOG.md | 4 ++++ README.md | 18 ++++++++++++++---- q7s-env-em.sh | 2 +- q7s-env.sh | 2 +- 4 files changed, 20 insertions(+), 6 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d8173e70..822a1ae2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -47,6 +47,10 @@ will consitute of a breaking change warranting a new major release: recovery handling becomes stuck in the custom recovery sequence when swichting power back on. - The dual lane custom recovery handling was adapted to always perform proper power switch handling irrespective of current or target modes. +- The cross compiler binary path now needs to be set explicitely as an environmental variable + named `CROSS_COMPILE_BIN_PATH` when setting up the build system. This prevents CMake from + selecting wrong cross-compilers. CMake will not seach in the system path for cross-compilers + anymore. # [v2.0.4] 2023-04-19 diff --git a/README.md b/README.md index cd350386..6b06283c 100644 --- a/README.md +++ b/README.md @@ -99,11 +99,21 @@ When using Windows, run theses steps in MSYS2. git submodule update --init ``` -3. Ensure that the cross-compiler is working with `arm-linux-gnueabihf-gcc --version` and that +3. Create two system variables to pass the system root path and the cross-compiler path to the + build system. You only need to do this once when setting up the build system. + Example for Unix: + + ```sh + export CROSS_COMPILE_BIN_PATH= + export ZYNQ_7020_SYSROOT= + ``` + +4. Ensure that the cross-compiler is working with + `${CROSS_COMPILE_BIN_PATH}/arm-linux-gnueabihf-gcc --version` and that the sysroot environmental variables have been set like specified in the [root filesystem chapter](#sysroot). -4. Run the CMake configuration to create the build system in a `build-Debug-Q7S` folder. +5. Run the CMake configuration to create the build system in a `build-Debug-Q7S` folder. Add `-G "MinGW Makefiles` in MinGW64 on Windows. ```sh @@ -112,7 +122,7 @@ When using Windows, run theses steps in MSYS2. cmake --build . -j ``` - You can also use provided shell scripts to perform these commands. + Please note that you can also use provided shell scripts to perform these commands. ```sh cp scripts/q7s-env.sh .. cp scripts/q7s-env-em.sh .. @@ -144,7 +154,7 @@ When using Windows, run theses steps in MSYS2. There are also different values for `-DTGT_BSP` to build for the Raspberry Pi or the Beagle Bone Black: `arm/raspberrypi` and `arm/beagleboneblack`. -5. Build the software with +6. Build the software with ```sh cd cmake-build-debug-q7s diff --git a/q7s-env-em.sh b/q7s-env-em.sh index 86737627..969d9ec0 100755 --- a/q7s-env-em.sh +++ b/q7s-env-em.sh @@ -4,7 +4,7 @@ # custom cross-compiler and sysroot path setups # Adapt the following two entries to your need -CROSS_COMPILE_BIN_PATH="/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin" +export CROSS_COMPILE_BIN_PATH="/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin" export ZYNQ_7020_SYSROOT="/opt/xiphos/sdk/ark/sysroots/cortexa9hf-neon-xiphos-linux-gnueabi" export PATH=$PATH:${CROSS_COMPILE_BIN_PATH} diff --git a/q7s-env.sh b/q7s-env.sh index 5cf608a0..4dea9154 100755 --- a/q7s-env.sh +++ b/q7s-env.sh @@ -4,7 +4,7 @@ # custom cross-compiler and sysroot path setups # Adapt the following two entries to your need -CROSS_COMPILE_BIN_PATH="/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin" +export CROSS_COMPILE_BIN_PATH="/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin" export ZYNQ_7020_SYSROOT="/opt/xiphos/sdk/ark/sysroots/cortexa9hf-neon-xiphos-linux-gnueabi" export PATH=$PATH:${CROSS_COMPILE_BIN_PATH} From 6c00404ca788edd840dd290de211f8d7299b524c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 10 May 2023 11:35:15 +0200 Subject: [PATCH 057/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 822a1ae2..ccf76847 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -51,6 +51,7 @@ will consitute of a breaking change warranting a new major release: named `CROSS_COMPILE_BIN_PATH` when setting up the build system. This prevents CMake from selecting wrong cross-compilers. CMake will not seach in the system path for cross-compilers anymore. +- Compile fix if SCEX is compiled for the EM. # [v2.0.4] 2023-04-19 From f3a651602bcc1c12596cb02823bd189a8a20a65d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 10 May 2023 11:47:49 +0200 Subject: [PATCH 058/506] this should be robust --- CHANGELOG.md | 8 ++++---- cmake/Zynq7020CrossCompileConfig.cmake | 4 ++-- q7s-env-em.sh | 2 +- q7s-env.sh | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index ccf76847..6aaae61a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -47,10 +47,10 @@ will consitute of a breaking change warranting a new major release: recovery handling becomes stuck in the custom recovery sequence when swichting power back on. - The dual lane custom recovery handling was adapted to always perform proper power switch handling irrespective of current or target modes. -- The cross compiler binary path now needs to be set explicitely as an environmental variable - named `CROSS_COMPILE_BIN_PATH` when setting up the build system. This prevents CMake from - selecting wrong cross-compilers. CMake will not seach in the system path for cross-compilers - anymore. +- The CMake build generator will now search for the cross-compiler binaries in the environmental + variable named `CROSS_COMPILE_BIN_PATH` first when setting up the build system. This prevents + CMake from selecting wrong cross-compilers if multiple cross-compilers with the same name are used + on the same system. - Compile fix if SCEX is compiled for the EM. # [v2.0.4] 2023-04-19 diff --git a/cmake/Zynq7020CrossCompileConfig.cmake b/cmake/Zynq7020CrossCompileConfig.cmake index e6e762ad..5e269f1a 100644 --- a/cmake/Zynq7020CrossCompileConfig.cmake +++ b/cmake/Zynq7020CrossCompileConfig.cmake @@ -40,8 +40,8 @@ set(CROSS_COMPILE_OBJCOPY "${CROSS_COMPILE}-objcopy") set(CROSS_COMPILE_SIZE "${CROSS_COMPILE}-size") # At the very least, cross compile gcc and g++ have to be set! -find_program (CMAKE_C_COMPILER ${CROSS_COMPILE_CC} HINTS $ENV{CROSS_COMPILE_BIN_PATH} NO_DEFAULT_PATH REQUIRED) -find_program (CMAKE_CXX_COMPILER ${CROSS_COMPILE_CXX} HINTS $ENV{CROSS_COMPILE_BIN_PATH} NO_DEFAULT_PATH REQUIRED) +find_program (CMAKE_C_COMPILER ${CROSS_COMPILE_CC} HINTS $ENV{CROSS_COMPILE_BIN_PATH} REQUIRED) +find_program (CMAKE_CXX_COMPILER ${CROSS_COMPILE_CXX} HINTS $ENV{CROSS_COMPILE_BIN_PATH} REQUIRED) # Useful utilities, not strictly necessary find_program(CMAKE_SIZE ${CROSS_COMPILE_SIZE}) find_program(CMAKE_OBJCOPY ${CROSS_COMPILE_OBJCOPY}) diff --git a/q7s-env-em.sh b/q7s-env-em.sh index 969d9ec0..698b6b23 100755 --- a/q7s-env-em.sh +++ b/q7s-env-em.sh @@ -7,7 +7,7 @@ export CROSS_COMPILE_BIN_PATH="/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin" export ZYNQ_7020_SYSROOT="/opt/xiphos/sdk/ark/sysroots/cortexa9hf-neon-xiphos-linux-gnueabi" -export PATH=$PATH:${CROSS_COMPILE_BIN_PATH} +export PATH=${CROSS_COMPILE_BIN_PATH}:$PATH export CROSS_COMPILE="arm-linux-gnueabihf" export EIVE_Q7S_EM=1 diff --git a/q7s-env.sh b/q7s-env.sh index 4dea9154..4b752e17 100755 --- a/q7s-env.sh +++ b/q7s-env.sh @@ -7,7 +7,7 @@ export CROSS_COMPILE_BIN_PATH="/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin" export ZYNQ_7020_SYSROOT="/opt/xiphos/sdk/ark/sysroots/cortexa9hf-neon-xiphos-linux-gnueabi" -export PATH=$PATH:${CROSS_COMPILE_BIN_PATH} +export PATH=${CROSS_COMPILE_BIN_PATH}:$PATH export CROSS_COMPILE="arm-linux-gnueabihf" # export EIVE_Q7S_EM=1 From 2e6a93c4796c329b5eecf0dee95273b1b292180f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 15:05:44 +0200 Subject: [PATCH 059/506] ACS board and init payload --- CHANGELOG.md | 11 ++++++++- CMakeLists.txt | 2 +- bsp_q7s/core/ObjectFactory.cpp | 44 ++++++++++++++++++---------------- bsp_q7s/core/ObjectFactory.h | 5 +++- bsp_q7s/em/emObjectFactory.cpp | 11 ++++++++- bsp_q7s/fmObjectFactory.cpp | 4 +++- dummies/helperFactory.cpp | 8 ++++--- dummies/helperFactory.h | 1 + 8 files changed, 57 insertions(+), 29 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6aaae61a..21ab5ab5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -40,6 +40,8 @@ will consitute of a breaking change warranting a new major release: # [v2.0.5] to be released +## Fixed + - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial submode (e.g. mode normal and submode dual side), it will transition back to the current mode, @@ -47,11 +49,18 @@ will consitute of a breaking change warranting a new major release: recovery handling becomes stuck in the custom recovery sequence when swichting power back on. - The dual lane custom recovery handling was adapted to always perform proper power switch handling irrespective of current or target modes. +- Compile fix if SCEX is compiled for the EM. +- Set up Rad Sensor chip select even for EM to avoid SPI bus issues. +- Correct ADIS Gyroscope type configuration for the EM, where the 16507 type is used instead of the + 16505 type. + +## Changed + - The CMake build generator will now search for the cross-compiler binaries in the environmental variable named `CROSS_COMPILE_BIN_PATH` first when setting up the build system. This prevents CMake from selecting wrong cross-compilers if multiple cross-compilers with the same name are used on the same system. -- Compile fix if SCEX is compiled for the EM. +- Add ACS board for EM by default now. # [v2.0.4] 2023-04-19 diff --git a/CMakeLists.txt b/CMakeLists.txt index 9589b20f..c2a9482e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,7 +98,7 @@ set(OBSW_ADD_THERMAL_TEMP_INSERTER ${OBSW_Q7S_EM} CACHE STRING "Add thermal sensor temperature inserter") set(OBSW_ADD_ACS_BOARD - ${INIT_VAL} + 1 CACHE STRING "Add ACS board module") set(OBSW_ADD_GPS_CTRL ${INIT_VAL} diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index dd363e3d..12e9177f 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -240,20 +240,7 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI ReturnValue_t ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& stackHandler) { - using namespace gpio; - if (gpioComIF == nullptr) { - return returnvalue::FAILED; - } - GpioCookie* gpioCookieRadSensor = new GpioCookie; - std::stringstream consumer; - consumer << "0x" << std::hex << objects::RAD_SENSOR; - GpiodRegularByLineName* gpio = new GpiodRegularByLineName( - q7s::gpioNames::RAD_SENSOR_CHIP_SELECT, consumer.str(), Direction::OUT, Levels::HIGH); - gpioCookieRadSensor->addGpio(gpioIds::CS_RAD_SENSOR, gpio); - gpio = new GpiodRegularByLineName(q7s::gpioNames::ENABLE_RADFET, consumer.str(), Direction::OUT, - Levels::LOW); - gpioCookieRadSensor->addGpio(gpioIds::ENABLE_RADFET, gpio); - gpioChecker(gpioComIF->addGpios(gpioCookieRadSensor), "RAD sensor"); + createRadSensorChipSelect(gpioComIF); SpiCookie* spiCookieRadSensor = new SpiCookie(addresses::RAD_SENSOR, gpioIds::CS_RAD_SENSOR, RAD_SENSOR::READ_SIZE, @@ -367,7 +354,7 @@ void ObjectFactory::createAcsBoardGpios(GpioCookie& cookie) { void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, PowerSwitchIF& pwrSwitcher, - bool enableHkSets) { + bool enableHkSets, adis1650x::Type adisType) { using namespace gpio; GpioCookie* gpioCookieAcsBoard = new GpioCookie(); createAcsBoardGpios(*gpioCookieAcsBoard); @@ -453,9 +440,8 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, adis1650x::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - auto adisHandler = - new GyrAdis1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK, - spiCookie, adis1650x::Type::ADIS16505); + auto adisHandler = new GyrAdis1650XHandler(objects::GYRO_0_ADIS_HANDLER, + objects::ACS_BOARD_POLLING_TASK, spiCookie, adisType); fdir = new AcsBoardFdir(objects::GYRO_0_ADIS_HANDLER); adisHandler->setCustomFdir(fdir); assemblyChildren[4] = adisHandler; @@ -488,9 +474,8 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, adis1650x::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - adisHandler = - new GyrAdis1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK, - spiCookie, adis1650x::Type::ADIS16505); + adisHandler = new GyrAdis1650XHandler(objects::GYRO_2_ADIS_HANDLER, + objects::ACS_BOARD_POLLING_TASK, spiCookie, adisType); fdir = new AcsBoardFdir(objects::GYRO_2_ADIS_HANDLER); adisHandler->setCustomFdir(fdir); assemblyChildren[6] = adisHandler; @@ -1022,3 +1007,20 @@ void ObjectFactory::testAcsBrdAss(AcsBoardAssembly* acsAss) { sif::warning << "Sending mode command failed" << std::endl; } } + +void ObjectFactory::createRadSensorChipSelect(LinuxLibgpioIF* gpioIF) { + using namespace gpio; + if (gpioIF == nullptr) { + return; + } + GpioCookie* gpioCookieRadSensor = new GpioCookie; + std::stringstream consumer; + consumer << "0x" << std::hex << objects::RAD_SENSOR; + GpiodRegularByLineName* gpio = new GpiodRegularByLineName( + q7s::gpioNames::RAD_SENSOR_CHIP_SELECT, consumer.str(), Direction::OUT, Levels::HIGH); + gpioCookieRadSensor->addGpio(gpioIds::CS_RAD_SENSOR, gpio); + gpio = new GpiodRegularByLineName(q7s::gpioNames::ENABLE_RADFET, consumer.str(), Direction::OUT, + Levels::LOW); + gpioCookieRadSensor->addGpio(gpioIds::ENABLE_RADFET, gpio); + gpioChecker(gpioIF->addGpios(gpioCookieRadSensor), "RAD sensor"); +} diff --git a/bsp_q7s/core/ObjectFactory.h b/bsp_q7s/core/ObjectFactory.h index b5685389..df65e1ae 100644 --- a/bsp_q7s/core/ObjectFactory.h +++ b/bsp_q7s/core/ObjectFactory.h @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -58,10 +59,12 @@ void createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher void createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF, PowerSwitchIF* pwrSwitcher, Stack5VHandler& stackHandler); void createTmpComponents(); +void createRadSensorChipSelect(LinuxLibgpioIF* gpioIF); ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& handler); void createAcsBoardGpios(GpioCookie& cookie); void createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, - PowerSwitchIF& pwrSwitcher, bool enableHkSets); + PowerSwitchIF& pwrSwitcher, bool enableHkSets, + adis1650x::Type adisType); void createHeaterComponents(GpioIF* gpioIF, PowerSwitchIF* pwrSwitcher, HealthTableIF* healthTable, HeaterHandler*& heaterHandler); void createImtqComponents(PowerSwitchIF* pwrSwitcher, bool enableHkSets); diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 81771a79..0311a468 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -55,6 +55,10 @@ void ObjectFactory::produce(void* args) { #if OBSW_ADD_SYRLINKS == 1 dummyCfg.addSyrlinksDummies = false; #endif +#if OBSW_ADD_PLOC_SUPERVISOR == 1 || OBSW_ADD_PLOC_MPSOC == 1 + dummyCfg.addPlocDummies = false; + dummyCfg.addCamSwitcherDummy = false; +#endif #if OBSW_ADD_GOMSPACE_PCDU == 1 dummyCfg.addPowerDummies = false; // The ACU broke. @@ -93,9 +97,12 @@ void ObjectFactory::produce(void* args) { // TODO: Careful! Switching this on somehow messes with the communication with the ProASIC // and will cause xsc_boot_copy commands to always boot to 0 0 // createRadSensorComponent(gpioComIF); + // Still initialize chip select to avoid SPI bus issues. + createRadSensorChipSelect(gpioComIF); #if OBSW_ADD_ACS_BOARD == 1 - createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher, true); + createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher, true, + adis1650x::Type::ADIS16507); #else // Still add all GPIOs for EM. GpioCookie* acsBoardGpios = new GpioCookie(); @@ -123,6 +130,8 @@ void ObjectFactory::produce(void* args) { createStrComponents(pwrSwitcher); #endif /* OBSW_ADD_STAR_TRACKER == 1 */ + createPayloadComponents(gpioComIF, *pwrSwitcher); + #if OBSW_ADD_CCSDS_IP_CORES == 1 CcsdsIpCoreHandler* ipCoreHandler = nullptr; CcsdsComponentArgs ccsdsArgs(*gpioComIF, *ipcStore, *tmStore, stores, *pusFunnel, *cfdpFunnel, diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 927c807e..df0bbc91 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -59,7 +60,8 @@ void ObjectFactory::produce(void* args) { #endif #if OBSW_ADD_ACS_BOARD == 1 - createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher, true); + createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher, true, + adis1650x::Type::ADIS16505); #endif HeaterHandler* heaterHandler; createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler); diff --git a/dummies/helperFactory.cpp b/dummies/helperFactory.cpp index 73be6940..2b03ab7f 100644 --- a/dummies/helperFactory.cpp +++ b/dummies/helperFactory.cpp @@ -217,9 +217,11 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio tmp.second->connectModeTreeParent(satsystem::tcs::SUBSYSTEM); } } - auto* camSwitcher = - new CamSwitcher(objects::CAM_SWITCHER, pwrSwitcher, power::Switches::PDU2_CH8_PAYLOAD_CAMERA); - camSwitcher->connectModeTreeParent(satsystem::payload::SUBSYSTEM); + if (cfg.addCamSwitcherDummy) { + auto* camSwitcher = new CamSwitcher(objects::CAM_SWITCHER, pwrSwitcher, + power::Switches::PDU2_CH8_PAYLOAD_CAMERA); + camSwitcher->connectModeTreeParent(satsystem::payload::SUBSYSTEM); + } auto* scexDummy = new ScexDummy(objects::SCEX, objects::DUMMY_COM_IF, comCookieDummy); scexDummy->connectModeTreeParent(satsystem::payload::SUBSYSTEM); auto* plPcduDummy = diff --git a/dummies/helperFactory.h b/dummies/helperFactory.h index 83456e96..e3809404 100644 --- a/dummies/helperFactory.h +++ b/dummies/helperFactory.h @@ -19,6 +19,7 @@ struct DummyCfg { bool addTempSensorDummies = true; bool addRtdComIFDummy = true; bool addPlocDummies = true; + bool addCamSwitcherDummy = true; }; void createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitch, GpioIF* gpioIF); From d4b7411de1b53f6d91452f569ef63ae1566d2278 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 19:34:37 +0200 Subject: [PATCH 060/506] I guess no patch version --- CHANGELOG.md | 25 ++++++++++--------------- 1 file changed, 10 insertions(+), 15 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 21ab5ab5..f4d6e5b9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -28,6 +28,11 @@ will consitute of a breaking change warranting a new major release: - ACS modes derived from system modes. - Adapt EM configuration to include all GomSpace PCDU devices except the ACU. For the ACU (which broke), a dummy will still be used. +- The CMake build generator will now search for the cross-compiler binaries in the environmental + variable named `CROSS_COMPILE_BIN_PATH` first when setting up the build system. This prevents + CMake from selecting wrong cross-compilers if multiple cross-compilers with the same name are used + on the same system. +- Add ACS board for EM by default now. ## Added @@ -35,12 +40,14 @@ will consitute of a breaking change warranting a new major release: ## Fixed +- Compile fix if SCEX is compiled for the EM. +- Set up Rad Sensor chip select even for EM to avoid SPI bus issues. +- Correct ADIS Gyroscope type configuration for the EM, where the 16507 type is used instead of the + 16505 type. - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. -# [v2.0.5] to be released - -## Fixed +# [v2.0.5] 2023-05-11 - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial @@ -49,18 +56,6 @@ will consitute of a breaking change warranting a new major release: recovery handling becomes stuck in the custom recovery sequence when swichting power back on. - The dual lane custom recovery handling was adapted to always perform proper power switch handling irrespective of current or target modes. -- Compile fix if SCEX is compiled for the EM. -- Set up Rad Sensor chip select even for EM to avoid SPI bus issues. -- Correct ADIS Gyroscope type configuration for the EM, where the 16507 type is used instead of the - 16505 type. - -## Changed - -- The CMake build generator will now search for the cross-compiler binaries in the environmental - variable named `CROSS_COMPILE_BIN_PATH` first when setting up the build system. This prevents - CMake from selecting wrong cross-compilers if multiple cross-compilers with the same name are used - on the same system. -- Add ACS board for EM by default now. # [v2.0.4] 2023-04-19 From ef039c47b829f49bf42596f13965e0063326c1a5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 19:35:57 +0200 Subject: [PATCH 061/506] update changelog again --- CHANGELOG.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index f4d6e5b9..dc320982 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,12 +22,12 @@ will consitute of a breaking change warranting a new major release: ## Changed +- Adapt EM configuration to include all GomSpace PCDU devices except the ACU. For the ACU + (which broke), a dummy will still be used. - Event Manager queue depth is configurable now. - Do not construct and schedule broken TMP1075 device anymore. - Do not track payload modes in system mode tables. - ACS modes derived from system modes. -- Adapt EM configuration to include all GomSpace PCDU devices except the ACU. For the ACU - (which broke), a dummy will still be used. - The CMake build generator will now search for the cross-compiler binaries in the environmental variable named `CROSS_COMPILE_BIN_PATH` first when setting up the build system. This prevents CMake from selecting wrong cross-compilers if multiple cross-compilers with the same name are used From 4f6f12217c5cb714fea63ed383fdfbaa15ce166f Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 5 May 2023 10:58:57 +0200 Subject: [PATCH 062/506] fixed variances of ADIS sensors --- mission/controller/acs/AcsParameters.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 9600ca7e..badc47af 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -781,9 +781,9 @@ class AcsParameters : public HasParametersIF { /* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is * assumed to be equal for the same class of sensors */ - float gyr02variance[3] = {pow(3.0e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms - pow(3.0e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms - pow(4.3e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms + float gyr02variance[3] = {pow(4.6e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms + pow(4.6e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms + pow(6.1e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; uint8_t preferAdis = false; float gyrFilterWeight = 0.6; From e78d458f06b780b17d0bea8fe17fb1c3690504fa Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 5 May 2023 11:00:46 +0200 Subject: [PATCH 063/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index dc320982..5449853f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -46,6 +46,7 @@ will consitute of a breaking change warranting a new major release: 16505 type. - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. +- The variance for the ADIS GYRs now represents the used `-3` version and not the `-1` version # [v2.0.5] 2023-05-11 From 1166c66c8c049661be67a89529d6cebf3eff935e Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 19 Apr 2023 15:31:45 +0200 Subject: [PATCH 064/506] added config values for testing --- fsfw | 2 +- mission/acs/gyroAdisHelpers.h | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 5eb9ee8b..258f0d33 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 5eb9ee8bc1e6f8640cbea46febd11e4b92241881 +Subproject commit 258f0d331329d67e13eec9d7f4053fd269e3f9b6 diff --git a/mission/acs/gyroAdisHelpers.h b/mission/acs/gyroAdisHelpers.h index 09f0aa1e..c8a28369 100644 --- a/mission/acs/gyroAdisHelpers.h +++ b/mission/acs/gyroAdisHelpers.h @@ -92,6 +92,9 @@ static constexpr size_t SENSOR_READOUT_SIZE = 20 + 2; static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA; static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG; +static constexpr uint16_t FILT_CTRL = 0x0000; +static constexpr uint16_t DEC_RATE = 0x0013; + enum GlobCmds : uint8_t { FACTORY_CALIBRATION = 0b0000'0010, SENSOR_SELF_TEST = 0b0000'0100, From e04313b9f30d1873a92ff56455a10e42d086801e Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 27 Apr 2023 09:35:38 +0200 Subject: [PATCH 065/506] make Robin happy --- mission/controller/AcsController.cpp | 12 +++++++++--- mission/controller/acs/control/Detumble.cpp | 6 ++++-- mission/controller/acs/control/Detumble.h | 5 +++-- mission/controller/acs/control/SafeCtrl.cpp | 7 ++++--- mission/controller/acs/control/SafeCtrl.h | 6 +++--- 5 files changed, 23 insertions(+), 13 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 0b49de04..c6089aa6 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -169,7 +169,7 @@ void AcsController::performSafe() { guidance.getTargetParamsSafe(sunTargetDir); double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; - uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy( + acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), acsParameters.safeModeControllerParameters.useMekf, @@ -205,6 +205,9 @@ void AcsController::performSafe() { case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; + default: + sif::error << "AcsController: Invalid safe mode strategy for performSafe" << std::endl; + break; } actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, @@ -259,7 +262,7 @@ void AcsController::performDetumble() { triggerEvent(acs::MEKF_RECOVERY); mekfInvalidFlag = false; } - uint8_t safeCtrlStrat = detumble.detumbleStrategy( + acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy( mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.isValid(), acsParameters.detumbleParameter.useFullDetumbleLaw); @@ -279,6 +282,9 @@ void AcsController::performDetumble() { case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; + default: + sif::error << "AcsController: Invalid safe mode strategy for performDetumble" << std::endl; + break; } actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, @@ -477,7 +483,7 @@ void AcsController::performPointingCtrl() { enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; default: - sif::error << "AcsController: Invalid mode for performPointingCtrl"; + sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl; break; } diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 9ca20862..8f422ec1 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -7,8 +7,10 @@ Detumble::Detumble() {} Detumble::~Detumble() {} -uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, - const bool magFieldRateValid, const bool useFullDetumbleLaw) { +acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid, + const bool satRotRateValid, + const bool magFieldRateValid, + const bool useFullDetumbleLaw) { if (not magFieldValid) { return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (satRotRateValid and useFullDetumbleLaw) { diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 4424896e..9fca77e6 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -11,8 +11,9 @@ class Detumble { Detumble(); virtual ~Detumble(); - uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, - const bool magFieldRateValid, const bool useFullDetumbleLaw); + acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, + const bool magFieldRateValid, + const bool useFullDetumbleLaw); void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB, double gain); diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index f0ebd5a0..43677ccf 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -9,9 +9,10 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter SafeCtrl::~SafeCtrl() {} -uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, - const bool satRotRateValid, const bool sunDirValid, - const uint8_t mekfEnabled, const uint8_t dampingEnabled) { +acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, + const bool satRotRateValid, const bool sunDirValid, + const uint8_t mekfEnabled, + const uint8_t dampingEnabled) { if (not magFieldValid) { return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 91625360..12f9ddb0 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -12,9 +12,9 @@ class SafeCtrl { SafeCtrl(AcsParameters *acsParameters_); virtual ~SafeCtrl(); - uint8_t safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, - const bool satRotRateValid, const bool sunDirValid, - const uint8_t mekfEnabled, const uint8_t dampingEnabled); + acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, + const bool satRotRateValid, const bool sunDirValid, + const uint8_t mekfEnabled, const uint8_t dampingEnabled); void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, const double *quatBI, const double *sunDirRefB, double *magMomB, From 0ff7e0f97aaa04f3bb54465f40e0d3f793784971 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 27 Apr 2023 10:19:07 +0200 Subject: [PATCH 066/506] RW cmd fix and cleanup --- mission/controller/AcsController.cpp | 7 +++--- mission/controller/acs/ActuatorCmd.cpp | 34 ++++++++++++++------------ mission/controller/acs/ActuatorCmd.h | 21 ++++++++-------- 3 files changed, 32 insertions(+), 30 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index c6089aa6..bd3bea5a 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -489,10 +489,9 @@ void AcsController::performPointingCtrl() { actuatorCmd.cmdSpeedToRws( sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws, - cmdSpeedRws, acsParameters.onBoardParams.sampleTime, - acsParameters.rwHandlingParameters.maxRwSpeed, - acsParameters.rwHandlingParameters.inertiaWheel); + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel, + acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws); if (enableAntiStiction) { ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); } diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index d2fe2d65..17d195e8 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -5,8 +5,6 @@ #include #include -#include - #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" @@ -25,24 +23,30 @@ void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) { } } -void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, - int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed, - double sampleTime, int32_t maxRwSpeed, double inertiaWheel) { - using namespace Math; - - // Calculating the commanded speed in RPM for every reaction wheel +void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, + const int32_t speedRw2, const int32_t speedRw3, + const double sampleTime, const double inertiaWheel, + const int32_t maxRwSpeed, const double *rwTorque, + int32_t *rwCmdSpeed) { + // concentrate RW speed values (in 0.1 [RPM]) in vector int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3}; + + // calculate required RW speed as sum of current RW speed and RW speed delta + // delta w_rw = delta t / I_RW * torque_RW [rad/s] double deltaSpeed[4] = {0, 0, 0, 0}; - double radToRpm = 60 / (2 * PI); // factor for conversion to RPM - // W_RW = Torque_RW / I_RW * delta t [rad/s] - double factor = sampleTime / inertiaWheel * radToRpm; - int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; + const double factor = sampleTime / inertiaWheel * RAD_PER_SEC_TO_RPM * 10; VectorOperations::mulScalar(rwTorque, factor, deltaSpeed, 4); + + // convert double to int32 + int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; for (int i = 0; i < 4; i++) { deltaSpeedInt[i] = std::round(deltaSpeed[i]); } + + // sum of current RW speed and RW speed delta VectorOperations::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4); - VectorOperations::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4); + + // crop values which would exceed the maximum possible RPM for (uint8_t i = 0; i < 4; i++) { if (rwCmdSpeed[i] > maxRwSpeed) { rwCmdSpeed[i] = maxRwSpeed; @@ -54,11 +58,11 @@ void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t spee void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, const double *inverseAlignment, double maxDipol) { - // Convert to actuator frame + // convert to actuator frame double dipolMomentActuatorDouble[3] = {0, 0, 0}; MatrixOperations::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3, 1); - // Scaling along largest element if dipol exceeds maximum + // scaling along largest element if dipole exceeds maximum uint8_t maxIdx = 0; VectorOperations::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx); double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]); diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 5d5d47f3..2c9624a9 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -1,9 +1,8 @@ #ifndef ACTUATORCMD_H_ #define ACTUATORCMD_H_ -#include "MultiplicativeKalmanFilter.h" -#include "SensorProcessing.h" -#include "SensorValues.h" +#include + class ActuatorCmd { public: @@ -19,17 +18,16 @@ class ActuatorCmd { void scalingTorqueRws(double *rwTrq, double maxTorque); /* - * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction - * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given - * as Input to the RWs - * @param: rwTrqIn given torque from pointing controller - * rwTrqNS Nullspace torque + * @brief: cmdSpeedToRws() Calculates the RPM for the reaction wheel configuration, + * given the required torque calculated by the controller. Will also scale down the RPM of the + * wheels if they exceed the maximum possible RPM + * @param: rwTrq given torque from pointing controller * rwCmdSpeed output revolutions per minute for every * reaction wheel */ - void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3, - const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime, - int32_t maxRwSpeed, double inertiaWheel); + void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, const double sampleTime, const double inertiaWheel, + const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed); /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques @@ -42,6 +40,7 @@ class ActuatorCmd { protected: private: + static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI); }; #endif /* ACTUATORCMD_H_ */ From 6705ede2fc451c97004db0cef7c2ae29712e591e Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 27 Apr 2023 10:33:43 +0200 Subject: [PATCH 067/506] englando too hard --- mission/controller/AcsController.cpp | 27 +++++++++++------------- mission/controller/AcsController.h | 2 +- mission/controller/acs/AcsParameters.cpp | 2 +- mission/controller/acs/AcsParameters.h | 2 +- mission/controller/acs/ActuatorCmd.cpp | 27 +++++++++++------------- mission/controller/acs/ActuatorCmd.h | 11 +++++----- 6 files changed, 33 insertions(+), 38 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index bd3bea5a..b4523f09 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -210,9 +210,8 @@ void AcsController::performSafe() { break; } - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, - *acsParameters.magnetorquerParameter.inverseAlignment, - acsParameters.magnetorquerParameter.dipolMax); + actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); // detumble check and switch if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf && @@ -234,8 +233,8 @@ void AcsController::performSafe() { } updateCtrlValData(errAng, safeCtrlStrat); - updateActuatorCmdData(cmdDipolMtqs); - commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + updateActuatorCmdData(cmdDipoleMtqs); + commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration); } @@ -287,9 +286,8 @@ void AcsController::performDetumble() { break; } - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, - *acsParameters.magnetorquerParameter.inverseAlignment, - acsParameters.magnetorquerParameter.dipolMax); + actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < @@ -310,8 +308,8 @@ void AcsController::performDetumble() { } disableCtrlValData(); - updateActuatorCmdData(cmdDipolMtqs); - commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + updateActuatorCmdData(cmdDipoleMtqs); + commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration); } @@ -495,13 +493,12 @@ void AcsController::performPointingCtrl() { if (enableAntiStiction) { ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); } - actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, - *acsParameters.magnetorquerParameter.inverseAlignment, - acsParameters.magnetorquerParameter.dipolMax); + actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); - updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs); - commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs); + commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], acsParameters.rwHandlingParameters.rampTime); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 6ec26c57..0c8b94bb 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -69,7 +69,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes bool mekfLost = false; int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; - int16_t cmdDipolMtqs[3] = {0, 0, 0}; + int16_t cmdDipoleMtqs[3] = {0, 0, 0}; #if OBSW_THREAD_TRACING == 1 uint32_t opCounter = 0; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 03500cc3..0e127b20 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -690,7 +690,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment); break; case 0x5: - parameterWrapper->set(magnetorquerParameter.dipolMax); + parameterWrapper->set(magnetorquerParameter.dipoleMax); break; case 0x6: parameterWrapper->set(magnetorquerParameter.torqueDuration); diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 9600ca7e..dac33ec2 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -931,7 +931,7 @@ class AcsParameters : public HasParametersIF { double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}}; double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}; - double dipolMax = 0.2; // [Am^2] + double dipoleMax = 0.2; // [Am^2] uint16_t torqueDuration = 300; // [ms] } magnetorquerParameter; diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 17d195e8..27d566c0 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -5,9 +5,6 @@ #include #include -#include "util/CholeskyDecomposition.h" -#include "util/MathOperations.h" - ActuatorCmd::ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {} @@ -56,24 +53,24 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, } } -void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, - const double *inverseAlignment, double maxDipol) { +void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxDipole, + const double *dipoleMoment, int16_t *dipoleMomentActuator) { // convert to actuator frame - double dipolMomentActuatorDouble[3] = {0, 0, 0}; - MatrixOperations::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3, + double dipoleMomentActuatorDouble[3] = {0, 0, 0}; + MatrixOperations::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3, 3, 1); // scaling along largest element if dipole exceeds maximum uint8_t maxIdx = 0; - VectorOperations::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx); - double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]); - if (maxAbsValue > maxDipol) { - double scalingFactor = maxDipol / maxAbsValue; - VectorOperations::mulScalar(dipolMomentActuatorDouble, scalingFactor, - dipolMomentActuatorDouble, 3); + VectorOperations::maxAbsValue(dipoleMomentActuatorDouble, 3, &maxIdx); + double maxAbsValue = abs(dipoleMomentActuatorDouble[maxIdx]); + if (maxAbsValue > maxDipole) { + double scalingFactor = maxDipole / maxAbsValue; + VectorOperations::mulScalar(dipoleMomentActuatorDouble, scalingFactor, + dipoleMomentActuatorDouble, 3); } // scale dipole from 1 Am^2 to 1e^-4 Am^2 - VectorOperations::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3); + VectorOperations::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble, 3); for (int i = 0; i < 3; i++) { - dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]); + dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]); } } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 2c9624a9..6a1b3dbb 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -30,13 +30,14 @@ class ActuatorCmd { const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed); /* - * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques + * @brief: cmdDipoleMtq() gives the commanded dipole moment for the + * magnetorquer * - * @param: dipolMoment given dipol moment in spacecraft frame - * dipolMomentActuator resulting dipol moment in actuator reference frame + * @param: dipoleMoment given dipole moment in spacecraft frame + * dipoleMomentActuator resulting dipole moment in actuator reference frame */ - void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, - const double *inverseAlignment, double maxDipol); + void cmdDipoleMtq(const double *inverseAlignment, const double maxDipole, + const double *dipoleMoment, int16_t *dipoleMomentActuator); protected: private: From 4e428e635383cec95ec3d783064945c56cb00375 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 27 Apr 2023 10:47:20 +0200 Subject: [PATCH 068/506] fixed antistiction logic --- mission/controller/acs/control/PtgCtrl.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 1b20efb9..98d3a0fa 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -169,15 +169,9 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee if (rwCmdSpeeds[i] != 0) { if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed && rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) { - if (currRwSpeed[i] == 0) { - if (rwCmdSpeeds[i] > 0) { - rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; - } else if (rwCmdSpeeds[i] < 0) { - rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; - } - } else if (currRwSpeed[i] < -acsParameters->rwHandlingParameters.stictionSpeed) { + if (rwCmdSpeeds[i] > currRwSpeed[i]) { rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; - } else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) { + } else if (rwCmdSpeeds[i] < currRwSpeed[i]) { rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; } } From a71be232acaf2e9ce2ebc893f5d5113b9be17242 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 27 Apr 2023 11:21:02 +0200 Subject: [PATCH 069/506] make Robin even happier --- mission/controller/acs/control/PtgCtrl.cpp | 8 ++++---- mission/controller/acs/control/PtgCtrl.h | 10 ++++------ 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 98d3a0fa..21d86e28 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -5,9 +5,7 @@ #include #include #include -#include - -#include "../util/MathOperations.h" +#include PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } @@ -135,7 +133,9 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { - double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; + // concentrate RW speeds as vector and convert to double + double speedRws[4] = {static_cast(*speedRw0), static_cast(*speedRw1), + static_cast(*speedRw2), static_cast(*speedRw3)}; double wheelMomentum[4] = {0, 0, 0, 0}; double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; // conversion to [rad/s] for further calculations diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index fad72e6b..870732c7 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -1,13 +1,10 @@ #ifndef PTGCTRL_H_ #define PTGCTRL_H_ +#include +#include +#include #include -#include -#include - -#include "../AcsParameters.h" -#include "../SensorValues.h" -#include "eive/resultClassIds.h" class PtgCtrl { /* @@ -45,6 +42,7 @@ class PtgCtrl { private: const AcsParameters *acsParameters; + static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI); }; #endif /* ACS_CONTROL_PTGCTRL_H_ */ From 5a9da1a99ca47dae2de07e2b76443b3a00af4647 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 27 Apr 2023 13:52:41 +0200 Subject: [PATCH 070/506] fixed nullspace controller --- mission/controller/acs/AcsParameters.cpp | 87 +++++++++++++--------- mission/controller/acs/AcsParameters.h | 4 +- mission/controller/acs/control/PtgCtrl.cpp | 34 +++++---- mission/controller/acs/control/PtgCtrl.h | 2 +- 4 files changed, 74 insertions(+), 53 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 0e127b20..e9ab50f3 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -315,7 +315,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->setMatrix(rwMatrices.without4); break; case 0x6: - parameterWrapper->setVector(rwMatrices.nullspace); + parameterWrapper->setVector(rwMatrices.nullspaceVector); break; default: return INVALID_IDENTIFIER_ID; @@ -375,15 +375,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(idleModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef); + parameterWrapper->set(idleModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(idleModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(idleModeControllerParameters.desatOn); + parameterWrapper->set(idleModeControllerParameters.deSatGainFactor); break; case 0x8: + parameterWrapper->set(idleModeControllerParameters.desatOn); + break; + case 0x9: parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); break; default: @@ -408,48 +411,51 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); + parameterWrapper->set(targetModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(targetModeControllerParameters.desatOn); + parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); + parameterWrapper->set(targetModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(targetModeControllerParameters.refDirection); + parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(targetModeControllerParameters.refRotRate); + parameterWrapper->setVector(targetModeControllerParameters.refDirection); break; case 0xB: - parameterWrapper->setVector(targetModeControllerParameters.quatRef); + parameterWrapper->setVector(targetModeControllerParameters.refRotRate); break; case 0xC: - parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(targetModeControllerParameters.quatRef); break; case 0xD: - parameterWrapper->set(targetModeControllerParameters.latitudeTgt); + parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); break; case 0xE: - parameterWrapper->set(targetModeControllerParameters.longitudeTgt); + parameterWrapper->set(targetModeControllerParameters.latitudeTgt); break; case 0xF: - parameterWrapper->set(targetModeControllerParameters.altitudeTgt); + parameterWrapper->set(targetModeControllerParameters.longitudeTgt); break; case 0x10: - parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); + parameterWrapper->set(targetModeControllerParameters.altitudeTgt); break; case 0x11: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); + parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); break; case 0x12: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); break; case 0x13: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + break; + case 0x14: parameterWrapper->set(targetModeControllerParameters.blindRotRate); break; default: @@ -474,30 +480,33 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef); + parameterWrapper->set(gsTargetModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(gsTargetModeControllerParameters.desatOn); + parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); + parameterWrapper->set(gsTargetModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); + parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); break; case 0xB: - parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); break; case 0xC: - parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); break; case 0xD: + parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + break; + case 0xE: parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); break; default: @@ -522,21 +531,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef); + parameterWrapper->set(nadirModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(nadirModeControllerParameters.desatOn); + parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); + parameterWrapper->set(nadirModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(nadirModeControllerParameters.refDirection); + parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); break; case 0xA: + parameterWrapper->setVector(nadirModeControllerParameters.refDirection); + break; + case 0xB: parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; case 0xC: @@ -564,21 +576,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef); + parameterWrapper->set(inertialModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(inertialModeControllerParameters.desatOn); + parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); + parameterWrapper->set(inertialModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); + parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); break; case 0xA: + parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); + break; + case 0xB: parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); break; case 0xC: diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index dac33ec2..2e428e4b 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -814,7 +814,7 @@ class AcsParameters : public HasParametersIF { {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}}; double without4[4][3] = { {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}}; - double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000}; + double nullspaceVector[4] = {-1, 1, -1, 1}; } rwMatrices; struct SafeModeControllerParameters { @@ -838,7 +838,9 @@ class AcsParameters : public HasParametersIF { double om = 0.3; double omMax = 1 * M_PI / 180; double qiMin = 0.1; + double gainNullspace = 0.01; + double nullspaceSpeed = 32500; // 0.1 RPM double desatMomentumRef[3] = {0, 0, 0}; double deSatGainFactor = 1000; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 21d86e28..0889d8c1 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -136,23 +136,27 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara // concentrate RW speeds as vector and convert to double double speedRws[4] = {static_cast(*speedRw0), static_cast(*speedRw1), static_cast(*speedRw2), static_cast(*speedRw3)}; - double wheelMomentum[4] = {0, 0, 0, 0}; - double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; - // conversion to [rad/s] for further calculations - VectorOperations::mulScalar(rpmOffset, factor, rpmOffset, 4); - VectorOperations::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); - double diffRwSpeed[4] = {0, 0, 0, 0}; + + // calculate RPM offset utilizing the nullspace + double rpmOffset[4] = {0, 0, 0, 0}; + double rpmOffsetSpeed = pointingLawParameters->nullspaceSpeed / 10 * RPM_TO_RAD_PER_SEC; + VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, rpmOffsetSpeed, + rpmOffset, 4); + + // calculate resulting angular momentum + double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0}; VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); VectorOperations::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel, - wheelMomentum, 4); - double gainNs = pointingLawParameters->gainNullspace; - double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace, - acsParameters->rwMatrices.nullspace, - *nullSpaceMatrix, 4); - MatrixOperations::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); - VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); - VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); + rwAngMomentum, 4); + + // calculate resulting torque + double nullspaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(acsParameters->rwMatrices.nullspaceVector, + acsParameters->rwMatrices.nullspaceVector, *nullspaceMatrix, 4, + 1, 4); + MatrixOperations::multiply(*nullspaceMatrix, rwAngMomentum, rwTrqNs, 4, 4, 1); + VectorOperations::mulScalar(rwTrqNs, -1 * pointingLawParameters->gainNullspace, rwTrqNs, + 4); } void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) { diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 870732c7..bb286d67 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -42,7 +42,7 @@ class PtgCtrl { private: const AcsParameters *acsParameters; - static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI); + static constexpr double RPM_TO_RAD_PER_SEC = (2 * M_PI) / 60; }; #endif /* ACS_CONTROL_PTGCTRL_H_ */ From db63757c0c1f49ea022490f2b158f94f398b03e0 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 27 Apr 2023 15:40:15 +0200 Subject: [PATCH 071/506] deprecated include --- mission/controller/acs/control/PtgCtrl.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 0889d8c1..56aa7615 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -5,7 +5,6 @@ #include #include #include -#include PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } From 722a4208d4b205e5e2afb4fe713f29e03a6e9bce Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:22:21 +0200 Subject: [PATCH 072/506] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index dc320982..acfd8ef3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -46,6 +46,11 @@ will consitute of a breaking change warranting a new major release: 16505 type. - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. +- The dual lane assembly transition failed handler started new transitions towards the current mode + instead of the target mode. This means that if the dual lane assembly never reached the initial + submode (e.g. mode normal and submode dual side), it will transition back to the current mode, + which is `MODE_OFF`. Furthermore, this lead to invalid internal states, so the subsequent + recovery handling becomes stuck in the custom recovery sequence when swichting power back on. # [v2.0.5] 2023-05-11 From 65c59352e9509e34732803f4a1c5245d6c7ab3bf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:23:21 +0200 Subject: [PATCH 073/506] changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index acfd8ef3..c185fab1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -49,7 +49,7 @@ will consitute of a breaking change warranting a new major release: - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial submode (e.g. mode normal and submode dual side), it will transition back to the current mode, - which is `MODE_OFF`. Furthermore, this lead to invalid internal states, so the subsequent + which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent recovery handling becomes stuck in the custom recovery sequence when swichting power back on. # [v2.0.5] 2023-05-11 From db3dc807565b044325fbb845da4605a69f976e89 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:28:05 +0200 Subject: [PATCH 074/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index c185fab1..af09ec1a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -51,6 +51,8 @@ will consitute of a breaking change warranting a new major release: submode (e.g. mode normal and submode dual side), it will transition back to the current mode, which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent recovery handling becomes stuck in the custom recovery sequence when swichting power back on. +- The dual lane custom recovery handling was adapted to always perform proper power switch handling + irrespective of current or target modes. # [v2.0.5] 2023-05-11 From ad32eee70a0059981417255eab28ab8d7c341f15 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 15:34:45 +0200 Subject: [PATCH 075/506] changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index af09ec1a..7d096ddd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -46,6 +46,9 @@ will consitute of a breaking change warranting a new major release: 16505 type. - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. + +# [v2.0.5] to be released + - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial submode (e.g. mode normal and submode dual side), it will transition back to the current mode, From 52acb3373edb1af0b2ad4913ffe0111f0ca55938 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 20:07:54 +0200 Subject: [PATCH 076/506] changelog --- CHANGELOG.md | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7d096ddd..dc320982 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -47,16 +47,6 @@ will consitute of a breaking change warranting a new major release: - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. -# [v2.0.5] to be released - -- The dual lane assembly transition failed handler started new transitions towards the current mode - instead of the target mode. This means that if the dual lane assembly never reached the initial - submode (e.g. mode normal and submode dual side), it will transition back to the current mode, - which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent - recovery handling becomes stuck in the custom recovery sequence when swichting power back on. -- The dual lane custom recovery handling was adapted to always perform proper power switch handling - irrespective of current or target modes. - # [v2.0.5] 2023-05-11 - The dual lane assembly transition failed handler started new transitions towards the current mode From 83a373859d0637ed809b8e466f9de3831229c79d Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 2 May 2023 09:40:48 +0200 Subject: [PATCH 077/506] Robin must be the happiest man alive --- mission/controller/acs/control/PtgCtrl.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 56aa7615..6039d525 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -106,8 +106,11 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP return; } + // concentrate RW speeds as vector and convert to double + double speedRws[4] = {static_cast(*speedRw0), static_cast(*speedRw1), + static_cast(*speedRw2), static_cast(*speedRw3)}; + // calculating momentum of satellite and momentum of reaction wheels - double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; VectorOperations::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel, momentumRwU, 4); From 8bb97f5f44cef56f1aef365faca0b9a508f228cd Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 3 May 2023 09:35:46 +0200 Subject: [PATCH 078/506] further fixes and cleanup for desaturation --- mission/controller/acs/control/PtgCtrl.cpp | 92 +++++++++++++--------- mission/controller/acs/control/PtgCtrl.h | 10 +-- 2 files changed, 60 insertions(+), 42 deletions(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 6039d525..c2c6e2b0 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -95,43 +95,6 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); } -void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, - double *magFieldEst, bool magFieldEstValid, double *satRate, - int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, - int32_t *speedRw3, double *mgtDpDes) { - if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) { - mgtDpDes[0] = 0; - mgtDpDes[1] = 0; - mgtDpDes[2] = 0; - return; - } - - // concentrate RW speeds as vector and convert to double - double speedRws[4] = {static_cast(*speedRw0), static_cast(*speedRw1), - static_cast(*speedRw2), static_cast(*speedRw3)}; - - // calculating momentum of satellite and momentum of reaction wheels - double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; - VectorOperations::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel, - momentumRwU, 4); - MatrixOperations::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU, - momentumRw, 3, 4, 1); - double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; - MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate, - momentumSat, 3, 3, 1); - VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); - // calculating momentum error - double deltaMomentum[3] = {0, 0, 0}; - VectorOperations::subtract(momentumTotal, pointingLawParameters->desatMomentumRef, - deltaMomentum, 3); - // resulting magnetic dipole command - double crossMomentumMagField[3] = {0, 0, 0}; - VectorOperations::cross(deltaMomentum, magFieldEst, crossMomentumMagField); - double normMag = VectorOperations::norm(magFieldEst, 3), factor = 0; - factor = (pointingLawParameters->deSatGainFactor) / normMag; - VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); -} - void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { @@ -161,6 +124,61 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara 4); } +void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, + const double *magFieldB, const bool magFieldBValid, + const double *satRate, const int32_t speedRw0, const int32_t speedRw1, + const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) { + if (not magFieldBValid or not pointingLawParameters->desatOn) { + return; + } + + // concentrate RW speeds as vector and convert to double + double speedRws[4] = {static_cast(speedRw0), static_cast(speedRw1), + static_cast(speedRw2), static_cast(speedRw3)}; + + // convert magFieldB from uT to T + double magFieldBT[3] = {0, 0, 0}; + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); + + // calculate angular momentum of the satellite + double angMomentumSat[3] = {0, 0, 0}; + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate, + angMomentumSat, 3, 3, 1); + + // calculate angular momentum of the reaction wheels with respect to the nullspace RW speed + // relocate RW speed zero to nullspace RW speed + double refSpeedRws[4] = {0, 0, 0, 0}; + VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, + pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); + VectorOperations::subtract(speedRws, refSpeedRws, speedRws, 4); + // convert to rad/s + VectorOperations::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4); + // calculate angular momentum of each RW + double angMomentumRwU[4] = {0, 0, 0, 0}; + VectorOperations::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel, + angMomentumRwU, 4); + // convert RW angular momentum to body RF + double angMomentumRw[3] = {0, 0, 0}; + MatrixOperations::multiply(*(acsParameters->rwMatrices.alignmentMatrix), angMomentumRwU, + angMomentumRw, 3, 4, 1); + + // calculate total angular momentum + double angMomentumTotal[3] = {0, 0, 0}; + VectorOperations::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3); + + // calculating momentum error + double deltaAngMomentum[3] = {0, 0, 0}; + VectorOperations::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef, + deltaAngMomentum, 3); + + // resulting magnetic dipole command + double crossAngMomentumMagField[3] = {0, 0, 0}; + VectorOperations::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField); + double factor = + pointingLawParameters->deSatGainFactor / VectorOperations::norm(magFieldBT, 3); + VectorOperations::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3); +} + void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) { bool rwAvailable[4] = { (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()), diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index bb286d67..f04950f9 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -26,15 +26,15 @@ class PtgCtrl { void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, const double *deltaRate, const double *rwPseudoInv, double *torqueRws); - void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, - double *magFieldEst, bool magFieldEstValid, double *satRate, - int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, - double *mgtDpDes); - void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs); + void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, + const double *magFieldB, const bool magFieldBValid, const double *satRate, + const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, double *mgtDpDes); + /* @brief: Commands the stiction torque in case wheel speed is to low * torqueCommand modified torque after antistiction */ From e3dc39a028c30330feec2da043db5a919cb8369f Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 3 May 2023 09:40:00 +0200 Subject: [PATCH 079/506] fixes for function calls --- mission/controller/AcsController.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index b4523f09..f3ed5cda 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -384,8 +384,8 @@ void AcsController::performPointingCtrl() { ptgCtrl.ptgDesaturation( &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; break; @@ -408,8 +408,8 @@ void AcsController::performPointingCtrl() { ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; @@ -429,8 +429,8 @@ void AcsController::performPointingCtrl() { ptgCtrl.ptgDesaturation( &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; break; @@ -453,8 +453,8 @@ void AcsController::performPointingCtrl() { ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; break; @@ -476,8 +476,8 @@ void AcsController::performPointingCtrl() { ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; default: From 8a65aca7b8186813c28432eff0e73f8766eaeed9 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 3 May 2023 09:43:02 +0200 Subject: [PATCH 080/506] someone seemed to like pointers way too much --- mission/controller/AcsController.cpp | 40 +++++++++++----------- mission/controller/acs/control/PtgCtrl.cpp | 8 ++--- mission/controller/acs/control/PtgCtrl.h | 4 +-- 3 files changed, 26 insertions(+), 26 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index f3ed5cda..557a6105 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -375,10 +375,10 @@ void AcsController::performPointingCtrl() { targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( @@ -399,10 +399,10 @@ void AcsController::performPointingCtrl() { errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( @@ -420,10 +420,10 @@ void AcsController::performPointingCtrl() { targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.gsTargetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( @@ -444,10 +444,10 @@ void AcsController::performPointingCtrl() { errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( @@ -467,10 +467,10 @@ void AcsController::performPointingCtrl() { errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index c2c6e2b0..1ef88ccc 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -96,11 +96,11 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters } void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, - const int32_t *speedRw0, const int32_t *speedRw1, - const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { + const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, double *rwTrqNs) { // concentrate RW speeds as vector and convert to double - double speedRws[4] = {static_cast(*speedRw0), static_cast(*speedRw1), - static_cast(*speedRw2), static_cast(*speedRw3)}; + double speedRws[4] = {static_cast(speedRw0), static_cast(speedRw1), + static_cast(speedRw2), static_cast(speedRw3)}; // calculate RPM offset utilizing the nullspace double rpmOffset[4] = {0, 0, 0, 0}; diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index f04950f9..5f731e6b 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -27,8 +27,8 @@ class PtgCtrl { const double *deltaRate, const double *rwPseudoInv, double *torqueRws); void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, - const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, - const int32_t *speedRw3, double *rwTrqNs); + const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, double *rwTrqNs); void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, const double *magFieldB, const bool magFieldBValid, const double *satRate, From 04b6c7006e963f1b99525ba2438196ce32a8ab5f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:22:21 +0200 Subject: [PATCH 081/506] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index dc320982..acfd8ef3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -46,6 +46,11 @@ will consitute of a breaking change warranting a new major release: 16505 type. - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. +- The dual lane assembly transition failed handler started new transitions towards the current mode + instead of the target mode. This means that if the dual lane assembly never reached the initial + submode (e.g. mode normal and submode dual side), it will transition back to the current mode, + which is `MODE_OFF`. Furthermore, this lead to invalid internal states, so the subsequent + recovery handling becomes stuck in the custom recovery sequence when swichting power back on. # [v2.0.5] 2023-05-11 From 621a6fd4012f7ba73c922013e20fb64411b62d04 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:23:21 +0200 Subject: [PATCH 082/506] changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index acfd8ef3..c185fab1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -49,7 +49,7 @@ will consitute of a breaking change warranting a new major release: - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial submode (e.g. mode normal and submode dual side), it will transition back to the current mode, - which is `MODE_OFF`. Furthermore, this lead to invalid internal states, so the subsequent + which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent recovery handling becomes stuck in the custom recovery sequence when swichting power back on. # [v2.0.5] 2023-05-11 From 97567736fb18cf75987cdd460866a14d93283fda Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:28:05 +0200 Subject: [PATCH 083/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index c185fab1..af09ec1a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -51,6 +51,8 @@ will consitute of a breaking change warranting a new major release: submode (e.g. mode normal and submode dual side), it will transition back to the current mode, which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent recovery handling becomes stuck in the custom recovery sequence when swichting power back on. +- The dual lane custom recovery handling was adapted to always perform proper power switch handling + irrespective of current or target modes. # [v2.0.5] 2023-05-11 From b34767c87009ef2806ec904ad6f1efcea2c4b604 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 15:34:45 +0200 Subject: [PATCH 084/506] changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index af09ec1a..7d096ddd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -46,6 +46,9 @@ will consitute of a breaking change warranting a new major release: 16505 type. - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. + +# [v2.0.5] to be released + - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial submode (e.g. mode normal and submode dual side), it will transition back to the current mode, From 32d2ad8f1d84597839b75a5dbfc41cbedc57c1d7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 20:13:26 +0200 Subject: [PATCH 085/506] changelog --- CHANGELOG.md | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7d096ddd..dc320982 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -47,16 +47,6 @@ will consitute of a breaking change warranting a new major release: - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. -# [v2.0.5] to be released - -- The dual lane assembly transition failed handler started new transitions towards the current mode - instead of the target mode. This means that if the dual lane assembly never reached the initial - submode (e.g. mode normal and submode dual side), it will transition back to the current mode, - which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent - recovery handling becomes stuck in the custom recovery sequence when swichting power back on. -- The dual lane custom recovery handling was adapted to always perform proper power switch handling - irrespective of current or target modes. - # [v2.0.5] 2023-05-11 - The dual lane assembly transition failed handler started new transitions towards the current mode From c8d1ce40baf7688e7c9be06ab0a8f715adddd997 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 2 May 2023 13:42:20 +0200 Subject: [PATCH 086/506] add HK request command --- linux/payload/PlocMpsocHandler.cpp | 47 ++++++++++++++++++++++++++---- linux/payload/PlocMpsocHandler.h | 9 ++---- linux/payload/plocMpscoDefs.h | 14 +++++++++ 3 files changed, 58 insertions(+), 12 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 72f2355f..29656c59 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -247,6 +247,10 @@ ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t device result = prepareTcReplayWriteSequence(commandData, commandDataLen); break; } + case (mpsoc::TC_GET_HK_REPORT): { + result = prepareTcGetHkReport(); + break; + } case (mpsoc::TC_MODE_REPLAY): { result = prepareTcModeReplay(); break; @@ -304,6 +308,7 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() { this->insertInCommandMap(mpsoc::TC_MODE_REPLAY); this->insertInCommandMap(mpsoc::TC_MODE_IDLE); this->insertInCommandMap(mpsoc::TC_CAM_CMD_SEND); + this->insertInCommandMap(mpsoc::TC_GET_HK_REPORT); this->insertInCommandMap(mpsoc::RELEASE_UART_TX); this->insertInCommandMap(mpsoc::SET_UART_TX_TRISTATE); this->insertInCommandMap(mpsoc::TC_CAM_TAKE_PIC); @@ -313,6 +318,7 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() { this->insertInReplyMap(mpsoc::ACK_REPORT, 3, nullptr, mpsoc::SIZE_ACK_REPORT); this->insertInReplyMap(mpsoc::EXE_REPORT, 3, nullptr, mpsoc::SIZE_EXE_REPORT); this->insertInReplyMap(mpsoc::TM_MEMORY_READ_REPORT, 2, nullptr, mpsoc::SIZE_TM_MEM_READ_REPORT); + this->insertInReplyMap(mpsoc::TM_GET_HK_REPORT, 2, nullptr, 0); this->insertInReplyMap(mpsoc::TM_CAM_CMD_RPT, 2, nullptr, mpsoc::SP_MAX_SIZE); } @@ -346,9 +352,15 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain break; case (mpsoc::apid::TM_CAM_CMD_RPT): *foundLen = spacePacket.getFullPacketLen(); - tmCamCmdRpt.rememberSpacePacketSize = *foundLen; + foundPacketLen = *foundLen; *foundId = mpsoc::TM_CAM_CMD_RPT; break; + case (mpsoc::apid::TM_HK_GET_REPORT): { + *foundLen = spacePacket.getFullPacketLen(); + foundPacketLen = *foundLen; + *foundId = mpsoc::TM_GET_HK_REPORT; + break; + } case (mpsoc::apid::EXE_SUCCESS): *foundLen = mpsoc::SIZE_EXE_REPORT; *foundId = mpsoc::EXE_REPORT; @@ -386,6 +398,10 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const result = handleMemoryReadReport(packet); break; } + case (mpsoc::TM_GET_HK_REPORT): { + result = handleGetHkReport(packet); + break; + } case (mpsoc::TM_CAM_CMD_RPT): { result = handleCamCmdRpt(packet); break; @@ -512,6 +528,17 @@ ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOff() { return returnvalue::OK; } +ReturnValue_t PlocMPSoCHandler::prepareTcGetHkReport() { + ReturnValue_t result = returnvalue::OK; + mpsoc::TcGetHkReport tcDownlinkPwrOff(spParams, sequenceCount); + result = tcDownlinkPwrOff.buildPacket(); + if (result != returnvalue::OK) { + return result; + } + finishTcPrep(tcDownlinkPwrOff.getFullPacketLen()); + return returnvalue::OK; +} + ReturnValue_t PlocMPSoCHandler::prepareTcReplayWriteSequence(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; @@ -724,17 +751,25 @@ ReturnValue_t PlocMPSoCHandler::handleMemoryReadReport(const uint8_t* data) { return result; } +ReturnValue_t PlocMPSoCHandler::handleGetHkReport(const uint8_t* data) { + ReturnValue_t result = verifyPacket(data, foundPacketLen); + if (result != returnvalue::OK) { + return result; + } + SpacePacketReader packetReader(data, foundPacketLen); + return returnvalue::OK; +} + ReturnValue_t PlocMPSoCHandler::handleCamCmdRpt(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; - result = verifyPacket(data, tmCamCmdRpt.rememberSpacePacketSize); + ReturnValue_t result = verifyPacket(data, foundPacketLen); if (result == MPSoCReturnValuesIF::CRC_FAILURE) { sif::warning << "PlocMPSoCHandler::handleCamCmdRpt: CRC failure" << std::endl; } - SpacePacketReader packetReader(data, tmCamCmdRpt.rememberSpacePacketSize); + SpacePacketReader packetReader(data, foundPacketLen); const uint8_t* dataFieldPtr = data + mpsoc::SPACE_PACKET_HEADER_SIZE + sizeof(uint16_t); - std::string camCmdRptMsg( - reinterpret_cast(dataFieldPtr), - tmCamCmdRpt.rememberSpacePacketSize - mpsoc::SPACE_PACKET_HEADER_SIZE - sizeof(uint16_t) - 3); + std::string camCmdRptMsg(reinterpret_cast(dataFieldPtr), + foundPacketLen - mpsoc::SPACE_PACKET_HEADER_SIZE - sizeof(uint16_t) - 3); #if OBSW_DEBUG_PLOC_MPSOC == 1 uint8_t ackValue = *(packetReader.getFullData() + packetReader.getFullPacketLen() - 2); sif::info << "PlocMPSoCHandler: CamCmdRpt message: " << camCmdRptMsg << std::endl; diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index 10e6bd5d..e11f05eb 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -138,17 +138,12 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { TmMemReadReport tmMemReadReport; - struct TmCamCmdRpt { - size_t rememberSpacePacketSize = 0; - }; - - TmCamCmdRpt tmCamCmdRpt; - struct TelemetryBuffer { uint16_t length = 0; uint8_t buffer[mpsoc::SP_MAX_SIZE]; }; + size_t foundPacketLen = 0; TelemetryBuffer tmBuffer; enum class PowerState { OFF, BOOTING, SHUTDOWN, ON }; @@ -167,6 +162,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { ReturnValue_t prepareTcReplayStop(); ReturnValue_t prepareTcDownlinkPwrOn(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcDownlinkPwrOff(); + ReturnValue_t prepareTcGetHkReport(); ReturnValue_t prepareTcReplayWriteSequence(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcCamCmdSend(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcModeIdle(); @@ -213,6 +209,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { */ ReturnValue_t handleMemoryReadReport(const uint8_t* data); + ReturnValue_t handleGetHkReport(const uint8_t* data); ReturnValue_t handleCamCmdRpt(const uint8_t* data); /** diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index 975dad32..e6acf4bf 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -37,6 +37,8 @@ static const DeviceCommandId_t TC_CAM_TAKE_PIC = 22; static const DeviceCommandId_t TC_SIMPLEX_SEND_FILE = 23; static const DeviceCommandId_t TC_DOWNLINK_DATA_MODULATE = 24; static const DeviceCommandId_t TC_MODE_SNAPSHOT = 25; +static const DeviceCommandId_t TC_GET_HK_REPORT = 26; +static const DeviceCommandId_t TM_GET_HK_REPORT = 27; // Will reset the sequence count of the OBSW static const DeviceCommandId_t OBSW_RESET_SEQ_COUNT = 50; @@ -45,6 +47,7 @@ static const uint16_t SIZE_ACK_REPORT = 14; static const uint16_t SIZE_EXE_REPORT = 14; static const uint16_t SIZE_TM_MEM_READ_REPORT = 18; static const uint16_t SIZE_TM_CAM_CMD_RPT = 18; +static constexpr size_t SIZE_TM_HK_REPORT = 369; /** * SpacePacket apids of PLOC telecommands and telemetry. @@ -65,6 +68,8 @@ static const uint16_t TC_MODE_IDLE = 0x11E; static const uint16_t TC_MODE_REPLAY = 0x11F; static const uint16_t TC_MODE_SNAPSHOT = 0x120; static const uint16_t TC_DOWNLINK_DATA_MODULATE = 0x121; +static constexpr uint16_t TC_HK_GET_REPORT = 0x123; +static constexpr uint16_t TM_HK_GET_REPORT = 0x408; static const uint16_t TC_DOWNLINK_PWR_OFF = 0x124; static const uint16_t TC_CAM_CMD_SEND = 0x12C; static const uint16_t TC_SIMPLEX_SEND_FILE = 0x130; @@ -561,6 +566,15 @@ class TcDownlinkPwrOn : public TcBase { } }; +/** + * @brief Class to build replay stop space packet. + */ +class TcGetHkReport : public TcBase { + public: + TcGetHkReport(ploc::SpTcParams params, uint16_t sequenceCount) + : TcBase(params, apid::TC_HK_GET_REPORT, sequenceCount) {} +}; + /** * @brief Class to build replay stop space packet. */ From 3a0db9c9ad401e6490e940b3ac1c205b612176fd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 2 May 2023 15:04:14 +0200 Subject: [PATCH 087/506] add new set --- linux/payload/PlocMpsocHandler.cpp | 44 +++++++++++- linux/payload/PlocMpsocHandler.h | 40 ++++++++++- linux/payload/plocMpscoDefs.h | 103 +++++++++++++++++++++++++++++ mission/tcs/Tmp1075Definitions.h | 2 +- 4 files changed, 186 insertions(+), 3 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 29656c59..b0c56667 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -11,6 +11,7 @@ PlocMPSoCHandler::PlocMPSoCHandler(object_id_t objectId, object_id_t uartComIFid CookieIF* comCookie, PlocMPSoCHelper* plocMPSoCHelper, Gpio uartIsolatorSwitch, object_id_t supervisorHandler) : DeviceHandlerBase(objectId, uartComIFid, comCookie), + hkReport(this), plocMPSoCHelper(plocMPSoCHelper), uartIsolatorSwitch(uartIsolatorSwitch), supervisorHandler(supervisorHandler), @@ -425,6 +426,41 @@ uint32_t PlocMPSoCHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) ReturnValue_t PlocMPSoCHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { + localDataPoolMap.emplace(mpsoc::poolid::STATUS, &peStatus); + localDataPoolMap.emplace(mpsoc::poolid::MODE, &peMode); + localDataPoolMap.emplace(mpsoc::poolid::MODE, &peMode); + localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_PWR_ON, &peDownlinkPwrOn); + localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_JESD_SYNC_STATUS, &peDownlinkJesdSyncStatus); + localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_DAC_STATUS, &peDownlinkDacStatus); + localDataPoolMap.emplace(mpsoc::poolid::CAM_STATUS, &peCameraStatus); + localDataPoolMap.emplace(mpsoc::poolid::CAM_SDI_STATUS, &peCameraSdiStatus); + localDataPoolMap.emplace(mpsoc::poolid::CAM_FPGA_TEMP, &peCameraFpgaTemp); + localDataPoolMap.emplace(mpsoc::poolid::CAM_SOC_TEMP, &peCameraSocTemp); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_TEMP, &peSysmonTemp); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCINT, &peSysmonVccInt); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCAUX, &peSysmonVccAux); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCBRAM, &peSysmonVccBram); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCPAUX, &peSysmonVccPaux); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCPINT, &peSysmonVccPint); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCPDRO, &peSysmonVccPdro); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_MB12V, &peSysmonMb12V); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_MB3V3, &peSysmonMb3V3); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_MB1V8, &peSysmonMb1V8); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC12V, &peSysmonVcc12V); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC3V3, &peSysmonVcc3V3); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC3V3VA, &peSysmonVcc3V3VA); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC2V5DDR, &peSysmonVcc2V5DDR); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC1V2DDR, &peSysmonVcc1V2DDR); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC0V9, &peSysmonVcc0V9); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC0V6VTT, &peSysmonVcc0V6VTT); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_SAFE_COTS_CUR, &peSysmonSafeCotsCur); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_NVM4_XO_CUR, &peSysmonNvm4XoCur); + localDataPoolMap.emplace(mpsoc::poolid::SEM_UNCORRECTABLE_ERRS, &peSemUncorrectableErrs); + localDataPoolMap.emplace(mpsoc::poolid::SEM_CORRECTABLE_ERRS, &peSemCorrectableErrs); + localDataPoolMap.emplace(mpsoc::poolid::SEM_STATUS, &peSemStatus); + localDataPoolMap.emplace(mpsoc::poolid::REBOOT_MPSOC_REQUIRED, &peRebootMpsocRequired); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(hkReport.getSid(), false, 10.0)); return returnvalue::OK; } @@ -761,7 +797,6 @@ ReturnValue_t PlocMPSoCHandler::handleGetHkReport(const uint8_t* data) { } ReturnValue_t PlocMPSoCHandler::handleCamCmdRpt(const uint8_t* data) { - ReturnValue_t result = returnvalue::OK; ReturnValue_t result = verifyPacket(data, foundPacketLen); if (result == MPSoCReturnValuesIF::CRC_FAILURE) { sif::warning << "PlocMPSoCHandler::handleCamCmdRpt: CRC failure" << std::endl; @@ -1143,6 +1178,13 @@ void PlocMPSoCHandler::handleActionCommandFailure(ActionId_t actionId) { return; } +LocalPoolDataSetBase* PlocMPSoCHandler::getDataSetHandle(sid_t sid) { + if (sid == hkReport.getSid()) { + return &hkReport; + } + return nullptr; +} + std::string PlocMPSoCHandler::getStatusString(uint16_t status) { switch (status) { case (mpsoc::status_code::UNKNOWN_APID): { diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index e11f05eb..c9850e18 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -77,7 +77,8 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { uint8_t expectedReplies = 1, bool useAlternateId = false, DeviceCommandId_t alternateReplyID = 0) override; size_t getNextReplyLength(DeviceCommandId_t deviceCommand) override; - virtual ReturnValue_t doSendReadHook() override; + ReturnValue_t doSendReadHook() override; + LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override; private: static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_MPSOC_HANDLER; @@ -105,6 +106,8 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { static const uint16_t PACKET_SEQUENCE_COUNT_MASK = 0x3FFF; static const uint8_t STATUS_OFFSET = 10; + mpsoc::HkReport hkReport; + MessageQueueIF* eventQueue = nullptr; MessageQueueIF* commandActionHelperQueue = nullptr; @@ -114,6 +117,41 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { SpacePacketCreator creator; ploc::SpTcParams spParams = ploc::SpTcParams(creator); + PoolEntry peStatus = PoolEntry(); + PoolEntry peMode = PoolEntry(); + PoolEntry peDownlinkPwrOn = PoolEntry(); + PoolEntry peDownlinkReplyActive = PoolEntry(); + PoolEntry peDownlinkJesdSyncStatus = PoolEntry(); + PoolEntry peDownlinkDacStatus = PoolEntry(); + PoolEntry peCameraStatus = PoolEntry(); + PoolEntry peCameraSdiStatus = PoolEntry(); + PoolEntry peCameraFpgaTemp = PoolEntry(); + PoolEntry peCameraSocTemp = PoolEntry(); + PoolEntry peSysmonTemp = PoolEntry(); + PoolEntry peSysmonVccInt = PoolEntry(); + PoolEntry peSysmonVccAux = PoolEntry(); + PoolEntry peSysmonVccBram = PoolEntry(); + PoolEntry peSysmonVccPaux = PoolEntry(); + PoolEntry peSysmonVccPint = PoolEntry(); + PoolEntry peSysmonVccPdro = PoolEntry(); + PoolEntry peSysmonMb12V = PoolEntry(); + PoolEntry peSysmonMb3V3 = PoolEntry(); + PoolEntry peSysmonMb1V8 = PoolEntry(); + PoolEntry peSysmonVcc12V = PoolEntry(); + PoolEntry peSysmonVcc5V = PoolEntry(); + PoolEntry peSysmonVcc3V3 = PoolEntry(); + PoolEntry peSysmonVcc3V3VA = PoolEntry(); + PoolEntry peSysmonVcc2V5DDR = PoolEntry(); + PoolEntry peSysmonVcc1V2DDR = PoolEntry(); + PoolEntry peSysmonVcc0V9 = PoolEntry(); + PoolEntry peSysmonVcc0V6VTT = PoolEntry(); + PoolEntry peSysmonSafeCotsCur = PoolEntry(); + PoolEntry peSysmonNvm4XoCur = PoolEntry(); + PoolEntry peSemUncorrectableErrs = PoolEntry(); + PoolEntry peSemCorrectableErrs = PoolEntry(); + PoolEntry peSemStatus = PoolEntry(); + PoolEntry peRebootMpsocRequired = PoolEntry(); + /** * This variable is used to store the id of the next reply to receive. This is necessary * because the PLOC sends as reply to each command at least one acknowledgment and execution diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index e6acf4bf..1ef72bf5 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -1,6 +1,7 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_PLOCMPSOCDEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_PLOCMPSOCDEFINITIONS_H_ +#include #include #include #include @@ -12,6 +13,47 @@ namespace mpsoc { +static constexpr uint32_t HK_SET_ID = 0; + +namespace poolid { +enum { + STATUS = 0, + MODE = 1, + DOWNLINK_PWR_ON = 2, + DOWNLINK_REPLY_ACTIIVE = 3, + DOWNLINK_JESD_SYNC_STATUS = 4, + DOWNLINK_DAC_STATUS = 5, + CAM_STATUS = 6, + CAM_SDI_STATUS = 7, + CAM_FPGA_TEMP = 8, + CAM_SOC_TEMP = 9, + SYSMON_TEMP = 10, + SYSMON_VCCINT = 11, + SYSMON_VCCAUX = 12, + SYSMON_VCCBRAM = 13, + SYSMON_VCCPAUX = 14, + SYSMON_VCCPINT = 15, + SYSMON_VCCPDRO = 16, + SYSMON_MB12V = 17, + SYSMON_MB3V3 = 18, + SYSMON_MB1V8 = 19, + SYSMON_VCC12V = 20, + SYSMON_VCC5V = 21, + SYSMON_VCC3V3 = 22, + SYSMON_VCC3V3VA = 23, + SYSMON_VCC2V5DDR = 24, + SYSMON_VCC1V2DDR = 25, + SYSMON_VCC0V9 = 26, + SYSMON_VCC0V6VTT = 27, + SYSMON_SAFE_COTS_CUR = 28, + SYSMON_NVM4_XO_CUR = 29, + SEM_UNCORRECTABLE_ERRS = 30, + SEM_CORRECTABLE_ERRS = 31, + SEM_STATUS = 32, + REBOOT_MPSOC_REQUIRED = 33, +}; +} + static const DeviceCommandId_t NONE = 0; static const DeviceCommandId_t TC_MEM_WRITE = 1; static const DeviceCommandId_t TC_MEM_READ = 2; @@ -788,6 +830,67 @@ class TcCamcmdSend : public TcBase { static const uint8_t CARRIAGE_RETURN = 0xD; }; +class HkReport : public StaticLocalDataSet<36> { + public: + HkReport(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, HK_SET_ID) {} + + HkReport(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, HK_SET_ID)) {} + + lp_var_t status = lp_var_t(sid.objectId, mpsoc::poolid::STATUS, this); + lp_var_t mode = lp_var_t(sid.objectId, mpsoc::poolid::MODE, this); + lp_var_t downlinkPwrOn = + lp_var_t(sid.objectId, mpsoc::poolid::DOWNLINK_PWR_ON, this); + lp_var_t downlinkReplyActive = + lp_var_t(sid.objectId, mpsoc::poolid::DOWNLINK_REPLY_ACTIIVE, this); + lp_var_t downlinkJesdSyncStatus = + lp_var_t(sid.objectId, mpsoc::poolid::DOWNLINK_JESD_SYNC_STATUS, this); + lp_var_t downlinkDacStatus = + lp_var_t(sid.objectId, mpsoc::poolid::DOWNLINK_DAC_STATUS, this); + lp_var_t camStatus = lp_var_t(sid.objectId, mpsoc::poolid::CAM_STATUS, this); + lp_var_t camSdiStatus = + lp_var_t(sid.objectId, mpsoc::poolid::CAM_SDI_STATUS, this); + lp_var_t camFpgaTemp = lp_var_t(sid.objectId, mpsoc::poolid::CAM_FPGA_TEMP, this); + lp_var_t camSocTemp = lp_var_t(sid.objectId, mpsoc::poolid::CAM_SOC_TEMP, this); + lp_var_t sysmonTemp = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_TEMP, this); + lp_var_t sysmonVccInt = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCINT, this); + lp_var_t sysmonVccAux = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCAUX, this); + lp_var_t sysmonVccBram = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCBRAM, this); + lp_var_t sysmonVccPaux = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCPAUX, this); + lp_var_t sysmonVccPint = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCPINT, this); + lp_var_t sysmonVccPdro = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCPDRO, this); + lp_var_t sysmonMb12V = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_MB12V, this); + lp_var_t sysmonMb3V3 = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_MB3V3, this); + lp_var_t sysmonMb1V8 = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_MB1V8, this); + lp_var_t sysmonVcc12V = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC12V, this); + lp_var_t sysmonVcc5V = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC5V, this); + lp_var_t sysmonVcc3V3 = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC3V3, this); + lp_var_t sysmonVcc3V3VA = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC3V3VA, this); + lp_var_t sysmonVcc2V5DDR = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC2V5DDR, this); + lp_var_t sysmonVcc1V2DDR = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC1V2DDR, this); + lp_var_t sysmonVcc0V9 = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC0V9, this); + lp_var_t sysmonVcc0V6VTT = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC0V6VTT, this); + lp_var_t sysmonSafeCotsCur = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_SAFE_COTS_CUR, this); + lp_var_t sysmonNvm4XoCur = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_NVM4_XO_CUR, this); + lp_var_t semUncorrectableErrs = + lp_var_t(sid.objectId, mpsoc::poolid::SEM_UNCORRECTABLE_ERRS, this); + lp_var_t semCorrectableErrs = + lp_var_t(sid.objectId, mpsoc::poolid::SEM_CORRECTABLE_ERRS, this); + lp_var_t semStatus = + lp_var_t(sid.objectId, mpsoc::poolid::SEM_CORRECTABLE_ERRS, this); + lp_var_t rebootMpsocRequired = + lp_var_t(sid.objectId, mpsoc::poolid::REBOOT_MPSOC_REQUIRED, this); +}; + } // namespace mpsoc #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_PLOCMPSOCDEFINITIONS_H_ */ diff --git a/mission/tcs/Tmp1075Definitions.h b/mission/tcs/Tmp1075Definitions.h index 946ac82e..345146e6 100644 --- a/mission/tcs/Tmp1075Definitions.h +++ b/mission/tcs/Tmp1075Definitions.h @@ -29,7 +29,7 @@ static const uint8_t MAX_REPLY_LENGTH = GET_TEMP_REPLY_SIZE; enum Tmp1075PoolIds : lp_id_t { TEMPERATURE_C_TMP1075 }; -class Tmp1075Dataset : public StaticLocalDataSet { +class Tmp1075Dataset : public StaticLocalDataSet<2> { public: Tmp1075Dataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, TMP1075_DATA_SET_ID) {} From 531f87cd760199ee2714c7ae686ad3330dec0595 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 13:26:47 +0200 Subject: [PATCH 088/506] bugfxi by marius --- linux/payload/PlocMpsocHandler.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index b0c56667..b9d97e90 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -428,7 +428,6 @@ ReturnValue_t PlocMPSoCHandler::initializeLocalDataPool(localpool::DataPool& loc LocalDataPoolManager& poolManager) { localDataPoolMap.emplace(mpsoc::poolid::STATUS, &peStatus); localDataPoolMap.emplace(mpsoc::poolid::MODE, &peMode); - localDataPoolMap.emplace(mpsoc::poolid::MODE, &peMode); localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_PWR_ON, &peDownlinkPwrOn); localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_JESD_SYNC_STATUS, &peDownlinkJesdSyncStatus); localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_DAC_STATUS, &peDownlinkDacStatus); From 98e69b9a6a097b0ad39e214359f6b028729a5f13 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 13:37:20 +0200 Subject: [PATCH 089/506] mpsoc update --- linux/payload/PlocMpsocHandler.cpp | 55 ++++++++++++++++-------------- 1 file changed, 29 insertions(+), 26 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index b9d97e90..c1cb4bde 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -822,6 +822,16 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator uint8_t enabledReplies = 0; + auto enableThreeReplies = [&](DeviceCommandId_t replyId) { + enabledReplies = 3; + result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, replyId); + if (result != returnvalue::OK) { + sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " + << mpsoc::TM_MEMORY_READ_REPORT << " not in replyMap" << std::endl; + return result; + } + break; + }; switch (command->first) { case mpsoc::TC_MEM_WRITE: case mpsoc::TC_FLASHDELETE: @@ -838,26 +848,16 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator case mpsoc::TC_MODE_SNAPSHOT: enabledReplies = 2; break; + case mpsoc::TC_GET_HK_REPORT: { + enableThreeReplies(mpsoc::TM_GET_HK_REPORT); + break; + } case mpsoc::TC_MEM_READ: { - enabledReplies = 3; - result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, - mpsoc::TM_MEMORY_READ_REPORT); - if (result != returnvalue::OK) { - sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " - << mpsoc::TM_MEMORY_READ_REPORT << " not in replyMap" << std::endl; - return result; - } + enableThreeReplies(mpsoc::TM_MEMORY_READ_REPORT); break; } case mpsoc::TC_CAM_CMD_SEND: { - enabledReplies = 3; - result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, - mpsoc::TM_CAM_CMD_RPT); - if (result != returnvalue::OK) { - sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " - << mpsoc::TM_CAM_CMD_RPT << " not in replyMap" << std::endl; - return result; - } + enableThreeReplies(mpsoc::TM_CAM_CMD_RPT); break; } case mpsoc::OBSW_RESET_SEQ_COUNT: @@ -1065,6 +1065,13 @@ void PlocMPSoCHandler::disableAllReplies() { DeviceCommandId_t commandId = getPendingCommand(); + auto disableCommandWithReply = [](DeviceCommandId_t replyId) { + iter = deviceReplyMap.find(replyId); + info = &(iter->second); + info->delayCycles = 0; + info->active = false; + info->command = deviceCommandMap.end(); + }; /* If the command expects a telemetry packet the appropriate tm reply will be disabled here */ switch (commandId) { case TC_MEM_WRITE: @@ -1082,19 +1089,15 @@ void PlocMPSoCHandler::disableAllReplies() { case TC_MODE_SNAPSHOT: break; case TC_MEM_READ: { - iter = deviceReplyMap.find(TM_MEMORY_READ_REPORT); - info = &(iter->second); - info->delayCycles = 0; - info->active = false; - info->command = deviceCommandMap.end(); + disableCommandWithReply(TM_MEMORY_READ_REPORT); + break; + } + case TC_GET_HK_REPORT: { + disableCommandWithReply(TM_GET_HK_REPORT); break; } case TC_CAM_CMD_SEND: { - iter = deviceReplyMap.find(TM_CAM_CMD_RPT); - info = &(iter->second); - info->delayCycles = 0; - info->active = false; - info->command = deviceCommandMap.end(); + disableCommandWithReply(TM_CAM_CMD_RPT); break; } default: { From 30fba0456b32a832ae6a0cf003bfa8a11277f891 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 20:14:12 +0200 Subject: [PATCH 090/506] MPSoC HK packet --- CHANGELOG.md | 1 + linux/payload/PlocMpsocHandler.cpp | 188 ++++++++++++++++++++++++++++- 2 files changed, 184 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index dc320982..7b0ecbe9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -33,6 +33,7 @@ will consitute of a breaking change warranting a new major release: CMake from selecting wrong cross-compilers if multiple cross-compilers with the same name are used on the same system. - Add ACS board for EM by default now. +- Add support for MPSoC HK packet. ## Added diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index c1cb4bde..e4c91a4a 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -792,6 +792,175 @@ ReturnValue_t PlocMPSoCHandler::handleGetHkReport(const uint8_t* data) { return result; } SpacePacketReader packetReader(data, foundPacketLen); + const uint8_t* dataStart = data + 6; + PoolReadGuard pg(&hkReport); + size_t deserLen = mpsoc::SIZE_TM_HK_REPORT; + SerializeIF::Endianness endianness = SerializeIF::Endianness::NETWORK; + result = SerializeAdapter::deSerialize(&hkReport.status.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.mode.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.downlinkPwrOn.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.downlinkReplyActive.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.downlinkJesdSyncStatus.value, &dataStart, + &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.downlinkDacStatus.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.camStatus.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.camSdiStatus.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.camFpgaTemp.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.sysmonTemp.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccInt.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccAux.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccBram.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccPaux.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccPint.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccPdro.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.sysmonMb12V.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.sysmonMb3V3.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.sysmonMb1V8.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc12V.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.sysmonVcc5V.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc3V3.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc3V3VA.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc2V5DDR.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc1V2DDR.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc0V9.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc0V6VTT.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonSafeCotsCur.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonNvm4XoCur.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.semUncorrectableErrs.value, &dataStart, + &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.semCorrectableErrs.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.semStatus.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + // Skip the weird filename + dataStart += 256; + result = SerializeAdapter::deSerialize(&hkReport.rebootMpsocRequired, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } return returnvalue::OK; } @@ -830,7 +999,7 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator << mpsoc::TM_MEMORY_READ_REPORT << " not in replyMap" << std::endl; return result; } - break; + return returnvalue::OK; }; switch (command->first) { case mpsoc::TC_MEM_WRITE: @@ -849,15 +1018,24 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator enabledReplies = 2; break; case mpsoc::TC_GET_HK_REPORT: { - enableThreeReplies(mpsoc::TM_GET_HK_REPORT); + result = enableThreeReplies(mpsoc::TM_GET_HK_REPORT); + if (result != returnvalue::OK) { + return result; + } break; } case mpsoc::TC_MEM_READ: { - enableThreeReplies(mpsoc::TM_MEMORY_READ_REPORT); + result = enableThreeReplies(mpsoc::TM_MEMORY_READ_REPORT); + if (result != returnvalue::OK) { + return result; + } break; } case mpsoc::TC_CAM_CMD_SEND: { - enableThreeReplies(mpsoc::TM_CAM_CMD_RPT); + result = enableThreeReplies(mpsoc::TM_CAM_CMD_RPT); + if (result != returnvalue::OK) { + return result; + } break; } case mpsoc::OBSW_RESET_SEQ_COUNT: @@ -1065,7 +1243,7 @@ void PlocMPSoCHandler::disableAllReplies() { DeviceCommandId_t commandId = getPendingCommand(); - auto disableCommandWithReply = [](DeviceCommandId_t replyId) { + auto disableCommandWithReply = [&](DeviceCommandId_t replyId) { iter = deviceReplyMap.find(replyId); info = &(iter->second); info->delayCycles = 0; From 758ed22b08551ac599bc636a70fb706ee068a1e9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 14:19:15 +0200 Subject: [PATCH 091/506] request HK regularly now --- linux/payload/PlocMpsocHandler.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index e4c91a4a..2e708496 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -202,8 +202,10 @@ void PlocMPSoCHandler::doShutDown() { #endif } + ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { - return NOTHING_TO_SEND; + *id = mpsoc::TC_GET_HK_REPORT; + return buildCommandFromCommand(*id, nullptr, 0); } ReturnValue_t PlocMPSoCHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { From 2b36195dacf5651c541281576a73d11b50b1f78b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 16:42:24 +0200 Subject: [PATCH 092/506] dynamically enable/disable MPSoC --- linux/payload/PlocMpsocHandler.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 2e708496..cd405fa6 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -160,6 +160,7 @@ void PlocMPSoCHandler::doStartUp() { powerState = PowerState::BOOTING; break; case PowerState::ON: + hkReport.setReportingEnabled(true); setMode(_MODE_TO_ON); uartIsolatorSwitch.pullHigh(); break; @@ -168,11 +169,13 @@ void PlocMPSoCHandler::doStartUp() { } #else powerState = PowerState::ON; + hkReport.setReportingEnabled(true); setMode(_MODE_TO_ON); uartIsolatorSwitch.pullHigh(); #endif /* not MSPOC_JTAG_BOOT == 1 */ #else powerState = PowerState::ON; + hkReport.setReportingEnabled(true); setMode(_MODE_TO_ON); #endif /* XIPHOS_Q7S */ } @@ -188,6 +191,7 @@ void PlocMPSoCHandler::doShutDown() { break; case PowerState::OFF: sequenceCount = 0; + hkReport.setReportingEnabled(false); setMode(_MODE_POWER_DOWN); break; default: @@ -196,6 +200,7 @@ void PlocMPSoCHandler::doShutDown() { #else sequenceCount = 0; uartIsolatorSwitch.pullLow(); + hkReport.setReportingEnabled(false); setMode(_MODE_POWER_DOWN); powerState = PowerState::OFF; #endif From 2c9da6a1e43b72775417b40e280da78c3c52622f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 16:45:14 +0200 Subject: [PATCH 093/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7b0ecbe9..75aa9c06 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -34,6 +34,7 @@ will consitute of a breaking change warranting a new major release: on the same system. - Add ACS board for EM by default now. - Add support for MPSoC HK packet. +- Dynamically enable and disable HK packets for MPSoC on `ON` and `OFF` commands. ## Added From a7ccfae04e228890806d51db5dda4e7e7c3d9743 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:22:21 +0200 Subject: [PATCH 094/506] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 75aa9c06..b627d3a9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -48,6 +48,11 @@ will consitute of a breaking change warranting a new major release: 16505 type. - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. +- The dual lane assembly transition failed handler started new transitions towards the current mode + instead of the target mode. This means that if the dual lane assembly never reached the initial + submode (e.g. mode normal and submode dual side), it will transition back to the current mode, + which is `MODE_OFF`. Furthermore, this lead to invalid internal states, so the subsequent + recovery handling becomes stuck in the custom recovery sequence when swichting power back on. # [v2.0.5] 2023-05-11 From 12749934282bbd7bdacb81989732b304134e043d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:23:21 +0200 Subject: [PATCH 095/506] changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index b627d3a9..f3b1b5da 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -51,7 +51,7 @@ will consitute of a breaking change warranting a new major release: - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial submode (e.g. mode normal and submode dual side), it will transition back to the current mode, - which is `MODE_OFF`. Furthermore, this lead to invalid internal states, so the subsequent + which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent recovery handling becomes stuck in the custom recovery sequence when swichting power back on. # [v2.0.5] 2023-05-11 From c3604085c2d6a4a07220e7b26d91e4c47d9ffe77 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:28:05 +0200 Subject: [PATCH 096/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index f3b1b5da..163ab8fa 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -53,6 +53,8 @@ will consitute of a breaking change warranting a new major release: submode (e.g. mode normal and submode dual side), it will transition back to the current mode, which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent recovery handling becomes stuck in the custom recovery sequence when swichting power back on. +- The dual lane custom recovery handling was adapted to always perform proper power switch handling + irrespective of current or target modes. # [v2.0.5] 2023-05-11 From 09ece30304f11375b9f68c0bb0f88e89a4006f73 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 15:34:45 +0200 Subject: [PATCH 097/506] changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 163ab8fa..ae1b5470 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -48,6 +48,9 @@ will consitute of a breaking change warranting a new major release: 16505 type. - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. + +# [v2.0.5] to be released + - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial submode (e.g. mode normal and submode dual side), it will transition back to the current mode, From fa137033942870cecf78a50e92129b770e0cd7c0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 20:18:39 +0200 Subject: [PATCH 098/506] changelog --- CHANGELOG.md | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index ae1b5470..75aa9c06 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -49,16 +49,6 @@ will consitute of a breaking change warranting a new major release: - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. -# [v2.0.5] to be released - -- The dual lane assembly transition failed handler started new transitions towards the current mode - instead of the target mode. This means that if the dual lane assembly never reached the initial - submode (e.g. mode normal and submode dual side), it will transition back to the current mode, - which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent - recovery handling becomes stuck in the custom recovery sequence when swichting power back on. -- The dual lane custom recovery handling was adapted to always perform proper power switch handling - irrespective of current or target modes. - # [v2.0.5] 2023-05-11 - The dual lane assembly transition failed handler started new transitions towards the current mode From 0a109e552daa513dc5212e33bb67900e46239d4b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 17:46:47 +0200 Subject: [PATCH 099/506] start adding flash read --- linux/payload/PlocMpsocHandler.cpp | 1 - linux/payload/PlocMpsocHelper.cpp | 30 ++++++++++++++++++++++-------- linux/payload/PlocMpsocHelper.h | 11 ++++++++--- 3 files changed, 30 insertions(+), 12 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index cd405fa6..7583be76 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -207,7 +207,6 @@ void PlocMPSoCHandler::doShutDown() { #endif } - ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { *id = mpsoc::TC_GET_HK_REPORT; return buildCommandFromCommand(*id, nullptr, 0); diff --git a/linux/payload/PlocMpsocHelper.cpp b/linux/payload/PlocMpsocHelper.cpp index a22462e2..e0b832a4 100644 --- a/linux/payload/PlocMpsocHelper.cpp +++ b/linux/payload/PlocMpsocHelper.cpp @@ -93,8 +93,8 @@ ReturnValue_t PlocMPSoCHelper::startFlashWrite(std::string obcFile, std::string } #endif - flashWrite.obcFile = obcFile; - flashWrite.mpsocFile = mpsocFile; + flashReadAndWrite.obcFile = obcFile; + flashReadAndWrite.mpsocFile = mpsocFile; internalState = InternalState::FLASH_WRITE; result = resetHelper(); if (result != returnvalue::OK) { @@ -116,12 +116,14 @@ void PlocMPSoCHelper::stopProcess() { terminate = true; } ReturnValue_t PlocMPSoCHelper::performFlashWrite() { ReturnValue_t result = returnvalue::OK; + std::ifstream file(flashReadAndWrite.obcFile, std::ifstream::binary); + if (file.bad()) { + return returnvalue::FAILED; + } result = flashfopen(); if (result != returnvalue::OK) { return result; } - uint8_t tempData[mpsoc::MAX_DATA_SIZE]; - std::ifstream file(flashWrite.obcFile, std::ifstream::binary); // Set position of next character to end of file input stream file.seekg(0, file.end); // tellg returns position of character in input stream @@ -139,7 +141,7 @@ ReturnValue_t PlocMPSoCHelper::performFlashWrite() { } if (file.is_open()) { file.seekg(bytesRead, file.beg); - file.read(reinterpret_cast(tempData), dataLength); + file.read(reinterpret_cast(fileBuf.data()), dataLength); bytesRead += dataLength; remainingSize -= dataLength; } else { @@ -147,7 +149,7 @@ ReturnValue_t PlocMPSoCHelper::performFlashWrite() { } (*sequenceCount)++; mpsoc::TcFlashWrite tc(spParams, *sequenceCount); - result = tc.buildPacket(tempData, dataLength); + result = tc.buildPacket(fileBuf.data(), dataLength); if (result != returnvalue::OK) { return result; } @@ -163,12 +165,24 @@ ReturnValue_t PlocMPSoCHelper::performFlashWrite() { return result; } +ReturnValue_t PlocMPSoCHelper::performFlashRead() { + std::ofstream ofile(flashReadAndWrite.obcFile); + if (ofile.bad()) { + return returnvalue::FAILED; + } + ReturnValue_t result = flashfopen(); + if (result != returnvalue::OK) { + return result; + } + return result; +} + ReturnValue_t PlocMPSoCHelper::flashfopen() { ReturnValue_t result = returnvalue::OK; spParams.buf = commandBuffer; (*sequenceCount)++; mpsoc::FlashFopen flashFopen(spParams, *sequenceCount); - result = flashFopen.createPacket(flashWrite.mpsocFile, mpsoc::FlashFopen::APPEND); + result = flashFopen.createPacket(flashReadAndWrite.mpsocFile, mpsoc::FlashFopen::APPEND); if (result != returnvalue::OK) { return result; } @@ -184,7 +198,7 @@ ReturnValue_t PlocMPSoCHelper::flashfclose() { spParams.buf = commandBuffer; (*sequenceCount)++; mpsoc::FlashFclose flashFclose(spParams, *sequenceCount); - result = flashFclose.createPacket(flashWrite.mpsocFile); + result = flashFclose.createPacket(flashReadAndWrite.mpsocFile); if (result != returnvalue::OK) { return result; } diff --git a/linux/payload/PlocMpsocHelper.h b/linux/payload/PlocMpsocHelper.h index b74c0844..1b74b568 100644 --- a/linux/payload/PlocMpsocHelper.h +++ b/linux/payload/PlocMpsocHelper.h @@ -111,13 +111,16 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { // buffer static const int RETRIES = 10000; - struct FlashWrite { + struct FlashInfo { std::string obcFile; std::string mpsocFile; }; - struct FlashWrite flashWrite; + struct FlashRead : public FlashInfo { + size_t readSize = 0; + }; + struct FlashRead flashReadAndWrite; #if OBSW_THREAD_TRACING == 1 uint32_t opCounter = 0; #endif @@ -134,7 +137,8 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { SpacePacketCreator creator; ploc::SpTcParams spParams = ploc::SpTcParams(creator); - std::array tmBuf; + std::array fileBuf{}; + std::array tmBuf{}; bool terminate = false; @@ -150,6 +154,7 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { ReturnValue_t resetHelper(); ReturnValue_t performFlashWrite(); + ReturnValue_t performFlashRead(); ReturnValue_t flashfopen(); ReturnValue_t flashfclose(); ReturnValue_t handlePacketTransmission(ploc::SpTcBase& tc); From ff47fafdc28b66575097856bab335a3e07cf64b1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 19:37:10 +0200 Subject: [PATCH 100/506] first flash read impl almost done --- linux/payload/PlocMpsocHelper.cpp | 153 +++++++++++++++++++++++++----- linux/payload/PlocMpsocHelper.h | 25 +++-- linux/payload/plocMpscoDefs.h | 63 ++++++++++-- 3 files changed, 199 insertions(+), 42 deletions(-) diff --git a/linux/payload/PlocMpsocHelper.cpp b/linux/payload/PlocMpsocHelper.cpp index e0b832a4..25bfc8d9 100644 --- a/linux/payload/PlocMpsocHelper.cpp +++ b/linux/payload/PlocMpsocHelper.cpp @@ -51,6 +51,16 @@ ReturnValue_t PlocMPSoCHelper::performOperation(uint8_t operationCode) { internalState = InternalState::IDLE; break; } + case InternalState::FLASH_READ: { + result = performFlashRead(); + if (result == returnvalue::OK) { + triggerEvent(MPSOC_FLASH_READ_SUCCESSFUL); + } else { + triggerEvent(MPSOC_FLASH_READ_FAILED); + } + internalState = InternalState::IDLE; + break; + } default: sif::debug << "PlocMPSoCHelper::performOperation: Invalid state" << std::endl; break; @@ -132,29 +142,32 @@ ReturnValue_t PlocMPSoCHelper::performFlashWrite() { size_t bytesRead = 0; while (remainingSize > 0) { if (terminate) { + flashfclose(); return returnvalue::OK; } - if (remainingSize > mpsoc::MAX_DATA_SIZE) { - dataLength = mpsoc::MAX_DATA_SIZE; + if (remainingSize > mpsoc::SP_MAX_DATA_SIZE) { + dataLength = mpsoc::SP_MAX_DATA_SIZE; } else { dataLength = remainingSize; } - if (file.is_open()) { - file.seekg(bytesRead, file.beg); - file.read(reinterpret_cast(fileBuf.data()), dataLength); - bytesRead += dataLength; - remainingSize -= dataLength; - } else { - return FILE_CLOSED_ACCIDENTALLY; + if (file.bad() or not file.is_open()) { + flashfclose(); + return FILE_WRITE_ERROR; } + file.seekg(bytesRead, file.beg); + file.read(reinterpret_cast(fileBuf.data()), dataLength); + bytesRead += dataLength; + remainingSize -= dataLength; (*sequenceCount)++; mpsoc::TcFlashWrite tc(spParams, *sequenceCount); result = tc.buildPacket(fileBuf.data(), dataLength); if (result != returnvalue::OK) { + flashfclose(); return result; } - result = handlePacketTransmission(tc); + result = handlePacketTransmissionNoReply(tc); if (result != returnvalue::OK) { + flashfclose(); return result; } } @@ -166,27 +179,68 @@ ReturnValue_t PlocMPSoCHelper::performFlashWrite() { } ReturnValue_t PlocMPSoCHelper::performFlashRead() { - std::ofstream ofile(flashReadAndWrite.obcFile); + std::error_code e; + std::ofstream ofile(flashReadAndWrite.obcFile, std::ios::trunc | std::ios::binary); if (ofile.bad()) { return returnvalue::FAILED; } ReturnValue_t result = flashfopen(); if (result != returnvalue::OK) { + std::filesystem::remove(flashReadAndWrite.obcFile, e); return result; } + size_t readSoFar = 0; + size_t nextReadSize = mpsoc::MAX_FLASH_READ_DATA_SIZE; + while (readSoFar < flashReadAndWrite.totalReadSize) { + if (terminate) { + std::filesystem::remove(flashReadAndWrite.obcFile, e); + flashfclose(); + return returnvalue::OK; + } + nextReadSize = mpsoc::MAX_FLASH_READ_DATA_SIZE; + if (flashReadAndWrite.totalReadSize - readSoFar < mpsoc::MAX_FLASH_READ_DATA_SIZE) { + nextReadSize = flashReadAndWrite.totalReadSize - readSoFar; + } + if (ofile.bad() or not ofile.is_open()) { + std::filesystem::remove(flashReadAndWrite.obcFile, e); + flashfclose(); + return FILE_READ_ERROR; + } + mpsoc::TcFlashRead flashReadRequest(spParams, *sequenceCount); + result = flashReadRequest.buildPacket(nextReadSize); + if (result != returnvalue::OK) { + std::filesystem::remove(flashReadAndWrite.obcFile, e); + flashfclose(); + return result; + } + result = handlePacketTransmissionFlashRead(flashReadRequest); + if (result != returnvalue::OK) { + std::filesystem::remove(flashReadAndWrite.obcFile, e); + flashfclose(); + return result; + } + result = handleFlashReadReply(ofile, nextReadSize); + if (result != returnvalue::OK) { + std::filesystem::remove(flashReadAndWrite.obcFile, e); + flashfclose(); + return result; + } + (*sequenceCount)++; + readSoFar += nextReadSize; + } return result; } ReturnValue_t PlocMPSoCHelper::flashfopen() { - ReturnValue_t result = returnvalue::OK; spParams.buf = commandBuffer; (*sequenceCount)++; mpsoc::FlashFopen flashFopen(spParams, *sequenceCount); - result = flashFopen.createPacket(flashReadAndWrite.mpsocFile, mpsoc::FlashFopen::APPEND); + ReturnValue_t result = + flashFopen.createPacket(flashReadAndWrite.mpsocFile, mpsoc::FlashFopen::APPEND); if (result != returnvalue::OK) { return result; } - result = handlePacketTransmission(flashFopen); + result = handlePacketTransmissionNoReply(flashFopen); if (result != returnvalue::OK) { return result; } @@ -194,24 +248,18 @@ ReturnValue_t PlocMPSoCHelper::flashfopen() { } ReturnValue_t PlocMPSoCHelper::flashfclose() { - ReturnValue_t result = returnvalue::OK; spParams.buf = commandBuffer; (*sequenceCount)++; mpsoc::FlashFclose flashFclose(spParams, *sequenceCount); - result = flashFclose.createPacket(flashReadAndWrite.mpsocFile); + ReturnValue_t result = flashFclose.createPacket(flashReadAndWrite.mpsocFile); if (result != returnvalue::OK) { return result; } - result = handlePacketTransmission(flashFclose); - if (result != returnvalue::OK) { - return result; - } - return returnvalue::OK; + return handlePacketTransmissionNoReply(flashFclose); } -ReturnValue_t PlocMPSoCHelper::handlePacketTransmission(ploc::SpTcBase& tc) { - ReturnValue_t result = returnvalue::OK; - result = sendCommand(tc); +ReturnValue_t PlocMPSoCHelper::handlePacketTransmissionFlashRead(mpsoc::TcFlashRead& tc) { + ReturnValue_t result = sendCommand(tc); if (result != returnvalue::OK) { return result; } @@ -219,11 +267,24 @@ ReturnValue_t PlocMPSoCHelper::handlePacketTransmission(ploc::SpTcBase& tc) { if (result != returnvalue::OK) { return result; } - result = handleExe(); + result = handleTmReception(ccsds::HEADER_LEN + mpsoc::FLASH_READ_MIN_OVERHEAD + tc.readSize + + mpsoc::CRC_SIZE); if (result != returnvalue::OK) { return result; } - return returnvalue::OK; + return handleExe(); +} + +ReturnValue_t PlocMPSoCHelper::handlePacketTransmissionNoReply(ploc::SpTcBase& tc) { + ReturnValue_t result = sendCommand(tc); + if (result != returnvalue::OK) { + return result; + } + result = handleAck(); + if (result != returnvalue::OK) { + return result; + } + return handleExe(); } ReturnValue_t PlocMPSoCHelper::sendCommand(ploc::SpTcBase& tc) { @@ -323,6 +384,46 @@ ReturnValue_t PlocMPSoCHelper::handleTmReception(size_t remainingBytes) { return result; } +ReturnValue_t PlocMPSoCHelper::handleFlashReadReply(std::ofstream& ofile, size_t expectedReadLen) { + SpTmReader tmPacket(tmBuf.data(), tmBuf.size()); + ReturnValue_t result = checkReceivedTm(tmPacket); + if (result != returnvalue::OK) { + return result; + } + uint16_t apid = tmPacket.getApid(); + if (apid != mpsoc::apid::TM_FLASH_READ_REPORT) { + triggerEvent(MPSOC_FLASH_READ_PACKET_ERROR, FlashReadErrorType::FLASH_READ_APID_ERROR); + sif::warning << "PLOC MPSoC Flash Read: Unexpected APID" << std::endl; + return result; + } + const uint8_t* packetData = tmPacket.getPacketData(); + size_t deserDummy = tmPacket.getPacketDataLen() - mpsoc::CRC_SIZE; + uint32_t receivedReadLen = 0; + std::string receivedShortName = std::string(reinterpret_cast(packetData), 12); + if (receivedShortName != flashReadAndWrite.mpsocFile.substr(0, 12)) { + sif::warning << "PLOC MPSoC Flash Read: Missmatch between request file name and " + "received file name" + << std::endl; + triggerEvent(MPSOC_FLASH_READ_PACKET_ERROR, FlashReadErrorType::FLASH_READ_FILENAME_ERROR); + return returnvalue::FAILED; + } + packetData += 12; + result = SerializeAdapter::deSerialize(&receivedReadLen, &packetData, &deserDummy, + SerializeIF::Endianness::NETWORK); + if (result != returnvalue::OK) { + return result; + } + if (receivedReadLen != expectedReadLen) { + sif::warning << "PLOC MPSoC Flash Read: Missmatch between request read length and " + "received read length" + << std::endl; + triggerEvent(MPSOC_FLASH_READ_PACKET_ERROR, FlashReadErrorType::FLASH_READ_READLEN_ERROR); + return returnvalue::FAILED; + } + ofile.write(reinterpret_cast(packetData), receivedReadLen); + return returnvalue::OK; +} + ReturnValue_t PlocMPSoCHelper::checkReceivedTm(SpTmReader& reader) { ReturnValue_t result = reader.checkSize(); if (result != returnvalue::OK) { diff --git a/linux/payload/PlocMpsocHelper.h b/linux/payload/PlocMpsocHelper.h index 1b74b568..24dd50a1 100644 --- a/linux/payload/PlocMpsocHelper.h +++ b/linux/payload/PlocMpsocHelper.h @@ -29,7 +29,7 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { //! [EXPORT] : [COMMENT] Flash write fails static const Event MPSOC_FLASH_WRITE_FAILED = MAKE_EVENT(0, severity::LOW); //! [EXPORT] : [COMMENT] Flash write successful - static const Event MPSOC_FLASH_WRITE_SUCCESSFUL = MAKE_EVENT(1, severity::LOW); + static const Event MPSOC_FLASH_WRITE_SUCCESSFUL = MAKE_EVENT(1, severity::INFO); //! [EXPORT] : [COMMENT] Communication interface returned failure when trying to send the command //! to the MPSoC //! P1: Return value returned by the communication interface sendMessage function @@ -71,6 +71,15 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { static const Event MPSOC_HELPER_SEQ_CNT_MISMATCH = MAKE_EVENT(11, severity::LOW); static const Event MPSOC_TM_SIZE_ERROR = MAKE_EVENT(12, severity::LOW); static const Event MPSOC_TM_CRC_MISSMATCH = MAKE_EVENT(13, severity::LOW); + static const Event MPSOC_FLASH_READ_PACKET_ERROR = MAKE_EVENT(14, severity::LOW); + static const Event MPSOC_FLASH_READ_FAILED = MAKE_EVENT(15, severity::LOW); + static const Event MPSOC_FLASH_READ_SUCCESSFUL = MAKE_EVENT(16, severity::INFO); + + enum FlashReadErrorType : uint32_t { + FLASH_READ_APID_ERROR = 0, + FLASH_READ_FILENAME_ERROR = 1, + FLASH_READ_READLEN_ERROR = 2 + }; PlocMPSoCHelper(object_id_t objectId); virtual ~PlocMPSoCHelper(); @@ -104,8 +113,10 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { private: static const uint8_t INTERFACE_ID = CLASS_ID::PLOC_MPSOC_HELPER; - //! [EXPORT] : [COMMENT] File accidentally close - static const ReturnValue_t FILE_CLOSED_ACCIDENTALLY = MAKE_RETURN_CODE(0xA0); + //! [EXPORT] : [COMMENT] File error occured for file transfers from OBC to the MPSoC. + static const ReturnValue_t FILE_WRITE_ERROR = MAKE_RETURN_CODE(0xA0); + //! [EXPORT] : [COMMENT] File error occured for file transfers from MPSoC to OBC. + static const ReturnValue_t FILE_READ_ERROR = MAKE_RETURN_CODE(0xA1); // Maximum number of times the communication interface retries polling data from the reply // buffer @@ -117,7 +128,7 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { }; struct FlashRead : public FlashInfo { - size_t readSize = 0; + size_t totalReadSize = 0; }; struct FlashRead flashReadAndWrite; @@ -137,7 +148,7 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { SpacePacketCreator creator; ploc::SpTcParams spParams = ploc::SpTcParams(creator); - std::array fileBuf{}; + std::array fileBuf{}; std::array tmBuf{}; bool terminate = false; @@ -157,7 +168,9 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { ReturnValue_t performFlashRead(); ReturnValue_t flashfopen(); ReturnValue_t flashfclose(); - ReturnValue_t handlePacketTransmission(ploc::SpTcBase& tc); + ReturnValue_t handlePacketTransmissionNoReply(ploc::SpTcBase& tc); + ReturnValue_t handlePacketTransmissionFlashRead(mpsoc::TcFlashRead& tc); + ReturnValue_t handleFlashReadReply(std::ofstream& ofile, size_t expectedReadLen); ReturnValue_t sendCommand(ploc::SpTcBase& tc); ReturnValue_t receive(uint8_t* data, size_t* readBytes, size_t requestBytes); ReturnValue_t handleAck(); diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index 1ef72bf5..4c98e6e1 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -102,7 +102,8 @@ static const uint16_t TC_DOWNLINK_PWR_ON = 0x113; static const uint16_t TC_MEM_WRITE = 0x114; static const uint16_t TC_MEM_READ = 0x115; static const uint16_t TC_CAM_TAKE_PIC = 0x116; -static const uint16_t TC_FLASHWRITE = 0x117; +static constexpr uint16_t TC_FLASHWRITE = 0x117; +static constexpr uint16_t TC_FLASHREAD = 0x118; static const uint16_t TC_FLASHFOPEN = 0x119; static const uint16_t TC_FLASHFCLOSE = 0x11A; static const uint16_t TC_FLASHDELETE = 0x11C; @@ -115,11 +116,12 @@ static constexpr uint16_t TM_HK_GET_REPORT = 0x408; static const uint16_t TC_DOWNLINK_PWR_OFF = 0x124; static const uint16_t TC_CAM_CMD_SEND = 0x12C; static const uint16_t TC_SIMPLEX_SEND_FILE = 0x130; -static const uint16_t TM_MEMORY_READ_REPORT = 0x404; static const uint16_t ACK_SUCCESS = 0x400; static const uint16_t ACK_FAILURE = 0x401; static const uint16_t EXE_SUCCESS = 0x402; static const uint16_t EXE_FAILURE = 0x403; +static const uint16_t TM_MEMORY_READ_REPORT = 0x404; +static const uint16_t TM_FLASH_READ_REPORT = 0x405; static const uint16_t TM_CAM_CMD_RPT = 0x407; } // namespace apid @@ -153,9 +155,13 @@ static const uint16_t LENGTH_TC_MEM_READ = 8; * at sheet README */ static constexpr size_t SP_MAX_SIZE = 1024; -static const size_t MAX_REPLY_SIZE = SP_MAX_SIZE * 3; -static const size_t MAX_COMMAND_SIZE = SP_MAX_SIZE; -static const size_t MAX_DATA_SIZE = 1016; +static constexpr size_t MAX_REPLY_SIZE = SP_MAX_SIZE * 3; +static constexpr size_t MAX_COMMAND_SIZE = SP_MAX_SIZE; +// 1016 bytes. +static constexpr size_t SP_MAX_DATA_SIZE = SP_MAX_SIZE - ccsds::HEADER_LEN - CRC_SIZE; +static constexpr size_t FLASH_READ_MIN_OVERHEAD = 16; +// 1000 bytes. +static const size_t MAX_FLASH_READ_DATA_SIZE = SP_MAX_DATA_SIZE - FLASH_READ_MIN_OVERHEAD; /** * The replay write sequence command has a maximum delay for the execution report which amounts to @@ -421,8 +427,8 @@ class TcFlashWrite : public ploc::SpTcBase { ReturnValue_t buildPacket(const uint8_t* writeData, uint32_t writeLen_) { ReturnValue_t result = returnvalue::OK; writeLen = writeLen_; - if (writeLen > MAX_DATA_SIZE) { - sif::debug << "FlashWrite::createPacket: Command data too big" << std::endl; + if (writeLen > SP_MAX_DATA_SIZE) { + sif::error << "TcFlashWrite: Command data too big" << std::endl; return returnvalue::FAILED; } spParams.setFullPayloadLen(static_cast(writeLen) + 4 + CRC_SIZE); @@ -438,9 +444,9 @@ class TcFlashWrite : public ploc::SpTcBase { } std::memcpy(payloadStart + sizeof(writeLen), writeData, writeLen); updateSpFields(); - auto res = checkSizeAndSerializeHeader(); - if (res != returnvalue::OK) { - return res; + result = checkSizeAndSerializeHeader(); + if (result != returnvalue::OK) { + return result; } return calcAndSetCrc(); } @@ -449,6 +455,43 @@ class TcFlashWrite : public ploc::SpTcBase { uint32_t writeLen = 0; }; +class TcFlashRead : public ploc::SpTcBase { + public: + TcFlashRead(ploc::SpTcParams params, uint16_t sequenceCount) + : ploc::SpTcBase(params, apid::TC_FLASHWRITE, sequenceCount) {} + + ReturnValue_t buildPacket(uint32_t readLen) { + if (readLen > MAX_FLASH_READ_DATA_SIZE) { + sif::error << "TcFlashRead: Read length " << readLen << " too large" << std::endl; + return returnvalue::FAILED; + } + spParams.setFullPayloadLen(readLen + FLASH_READ_MIN_OVERHEAD + CRC_SIZE); + ReturnValue_t result = checkPayloadLen(); + if (result != returnvalue::OK) { + return result; + } + size_t serializedSize = ccsds::HEADER_LEN; + result = SerializeAdapter::serialize(&readLen, payloadStart, &serializedSize, spParams.maxSize, + SerializeIF::Endianness::NETWORK); + if (result != returnvalue::OK) { + return result; + } + updateSpFields(); + result = checkSizeAndSerializeHeader(); + if (result != returnvalue::OK) { + return result; + } + result = calcAndSetCrc(); + if (result != returnvalue::OK) { + return result; + } + readSize = readLen; + return result; + } + + uint32_t readSize = 0; +}; + /** * @brief Class to help creation of flash delete command. */ From 119afe61482dff33763194d15b6135d215160cdf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 19:49:45 +0200 Subject: [PATCH 101/506] now the helper just needs to be driven --- linux/payload/PlocMpsocHelper.cpp | 56 ++++++++++++++++++++----------- linux/payload/PlocMpsocHelper.h | 11 ++++++ 2 files changed, 48 insertions(+), 19 deletions(-) diff --git a/linux/payload/PlocMpsocHelper.cpp b/linux/payload/PlocMpsocHelper.cpp index 25bfc8d9..3eaa833d 100644 --- a/linux/payload/PlocMpsocHelper.cpp +++ b/linux/payload/PlocMpsocHelper.cpp @@ -84,32 +84,22 @@ void PlocMPSoCHelper::setSequenceCount(SourceSequenceCounter* sequenceCount_) { } ReturnValue_t PlocMPSoCHelper::startFlashWrite(std::string obcFile, std::string mpsocFile) { - ReturnValue_t result = returnvalue::OK; -#ifdef XIPHOS_Q7S - result = FilesystemHelper::checkPath(obcFile); + ReturnValue_t result = startFlashReadOrWriteBase(obcFile, mpsocFile); if (result != returnvalue::OK) { return result; } - result = FilesystemHelper::fileExists(mpsocFile); - if (result != returnvalue::OK) { - return result; - } -#endif -#ifdef TE0720_1CFA - if (not std::filesystem::exists(obcFile)) { - sif::warning << "PlocMPSoCHelper::startFlashWrite: File " << obcFile << "does not exist" - << std::endl; - return returnvalue::FAILED; - } -#endif - - flashReadAndWrite.obcFile = obcFile; - flashReadAndWrite.mpsocFile = mpsocFile; internalState = InternalState::FLASH_WRITE; - result = resetHelper(); + return result; +} + +ReturnValue_t PlocMPSoCHelper::startFlashRead(std::string obcFile, std::string mpsocFile, + size_t readFileSize) { + ReturnValue_t result = startFlashReadOrWriteBase(obcFile, mpsocFile); if (result != returnvalue::OK) { return result; } + flashReadAndWrite.totalReadSize = readFileSize; + internalState = InternalState::FLASH_READ; return result; } @@ -424,6 +414,34 @@ ReturnValue_t PlocMPSoCHelper::handleFlashReadReply(std::ofstream& ofile, size_t return returnvalue::OK; } +ReturnValue_t PlocMPSoCHelper::fileCheck(std::string obcFile) { +#ifdef XIPHOS_Q7S + ReturnValue_t result = FilesystemHelper::checkPath(obcFile); + if (result != returnvalue::OK) { + return result; + } +#elif defined(TE0720_1CFA) + if (not std::filesystem::exists(obcFile)) { + sif::warning << "PlocMPSoCHelper::startFlashWrite: File " << obcFile << "does not exist" + << std::endl; + return returnvalue::FAILED; + } +#endif + return returnvalue::OK; +} + +ReturnValue_t PlocMPSoCHelper::startFlashReadOrWriteBase(std::string obcFile, + std::string mpsocFile) { + ReturnValue_t result = fileCheck(obcFile); + if (result != returnvalue::OK) { + return result; + } + + flashReadAndWrite.obcFile = obcFile; + flashReadAndWrite.mpsocFile = mpsocFile; + return resetHelper(); +} + ReturnValue_t PlocMPSoCHelper::checkReceivedTm(SpTmReader& reader) { ReturnValue_t result = reader.checkSize(); if (result != returnvalue::OK) { diff --git a/linux/payload/PlocMpsocHelper.h b/linux/payload/PlocMpsocHelper.h index 24dd50a1..63a40d0c 100644 --- a/linux/payload/PlocMpsocHelper.h +++ b/linux/payload/PlocMpsocHelper.h @@ -6,6 +6,7 @@ #include +#include "OBSWConfig.h" #include "fsfw/devicehandlers/CookieIF.h" #include "fsfw/objectmanager/SystemObject.h" #include "fsfw/osal/linux/BinarySemaphore.h" @@ -99,6 +100,14 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { * @return returnvalue::OK if successful, otherwise error return value */ ReturnValue_t startFlashWrite(std::string obcFile, std::string mpsocFile); + /** + * + * @param obcFile Full target file name on OBC + * @param mpsocFile The file on the MPSoC which should be copied ot the OBC + * @param readFileSize The size of the file on the MPSoC. + * @return + */ + ReturnValue_t startFlashRead(std::string obcFile, std::string mpsocFile, size_t readFileSize); /** * @brief Can be used to interrupt a running data transfer. @@ -175,6 +184,8 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { ReturnValue_t receive(uint8_t* data, size_t* readBytes, size_t requestBytes); ReturnValue_t handleAck(); ReturnValue_t handleExe(); + ReturnValue_t startFlashReadOrWriteBase(std::string obcFile, std::string mpsocFile); + ReturnValue_t fileCheck(std::string obcFile); void handleAckApidFailure(uint16_t apid); void handleExeApidFailure(uint16_t apid); ReturnValue_t handleTmReception(size_t remainingBytes); From e4431d20c4842e1c94c5dcf2475c9202572d02ba Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 19:54:30 +0200 Subject: [PATCH 102/506] driving code almost done as well --- linux/payload/PlocMpsocHandler.cpp | 8 +++++++- linux/payload/plocMpscoDefs.h | 3 ++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 7583be76..2cbc66af 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -124,7 +124,7 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI } switch (actionId) { - case mpsoc::TC_FLASHWRITE: { + case mpsoc::TC_FLASH_WRITE_FULL_FILE: { if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) { return MPSoCReturnValuesIF::FILENAME_TOO_LONG; } @@ -141,6 +141,10 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI plocMPSoCHelperExecuting = true; return EXECUTION_FINISHED; } + case mpsoc::TC_FLASH_READ_FULL_FILE: { + // TODO: Finsh this. + break; + } case (mpsoc::OBSW_RESET_SEQ_COUNT): { sequenceCount = 0; return EXECUTION_FINISHED; @@ -307,6 +311,8 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() { this->insertInCommandMap(mpsoc::TC_MEM_WRITE); this->insertInCommandMap(mpsoc::TC_MEM_READ); this->insertInCommandMap(mpsoc::TC_FLASHDELETE); + insertInCommandMap(mpsoc::TC_FLASH_WRITE_FULL_FILE); + insertInCommandMap(mpsoc::TC_FLASH_READ_FULL_FILE); this->insertInCommandMap(mpsoc::TC_REPLAY_START); this->insertInCommandMap(mpsoc::TC_REPLAY_STOP); this->insertInCommandMap(mpsoc::TC_DOWNLINK_PWR_ON); diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index 4c98e6e1..cbb4f1f8 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -62,7 +62,7 @@ static const DeviceCommandId_t EXE_REPORT = 5; static const DeviceCommandId_t TM_MEMORY_READ_REPORT = 6; static const DeviceCommandId_t TC_FLASHFOPEN = 7; static const DeviceCommandId_t TC_FLASHFCLOSE = 8; -static const DeviceCommandId_t TC_FLASHWRITE = 9; +static const DeviceCommandId_t TC_FLASH_WRITE_FULL_FILE = 9; static const DeviceCommandId_t TC_FLASHDELETE = 10; static const DeviceCommandId_t TC_REPLAY_START = 11; static const DeviceCommandId_t TC_REPLAY_STOP = 12; @@ -81,6 +81,7 @@ static const DeviceCommandId_t TC_DOWNLINK_DATA_MODULATE = 24; static const DeviceCommandId_t TC_MODE_SNAPSHOT = 25; static const DeviceCommandId_t TC_GET_HK_REPORT = 26; static const DeviceCommandId_t TM_GET_HK_REPORT = 27; +static constexpr DeviceCommandId_t TC_FLASH_READ_FULL_FILE = 30; // Will reset the sequence count of the OBSW static const DeviceCommandId_t OBSW_RESET_SEQ_COUNT = 50; From ff175170aaff43495c1920ac8f678eca5e20e2ef Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 20:19:46 +0200 Subject: [PATCH 103/506] start adding dir content report --- linux/payload/plocMpscoDefs.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index cbb4f1f8..7e862ba3 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -81,8 +81,11 @@ static const DeviceCommandId_t TC_DOWNLINK_DATA_MODULATE = 24; static const DeviceCommandId_t TC_MODE_SNAPSHOT = 25; static const DeviceCommandId_t TC_GET_HK_REPORT = 26; static const DeviceCommandId_t TM_GET_HK_REPORT = 27; +static const DeviceCommandId_t TC_FLASH_GET_DIRECTORY_CONTENT = 28; +static const DeviceCommandId_t TM_FLASH_DIRECTORY_CONTENT = 29; static constexpr DeviceCommandId_t TC_FLASH_READ_FULL_FILE = 30; + // Will reset the sequence count of the OBSW static const DeviceCommandId_t OBSW_RESET_SEQ_COUNT = 50; @@ -107,6 +110,7 @@ static constexpr uint16_t TC_FLASHWRITE = 0x117; static constexpr uint16_t TC_FLASHREAD = 0x118; static const uint16_t TC_FLASHFOPEN = 0x119; static const uint16_t TC_FLASHFCLOSE = 0x11A; +static constexpr uint16_t TC_FLASH_GET_DIRECTORY_CONTENT = 0x11B; static const uint16_t TC_FLASHDELETE = 0x11C; static const uint16_t TC_MODE_IDLE = 0x11E; static const uint16_t TC_MODE_REPLAY = 0x11F; @@ -121,8 +125,12 @@ static const uint16_t ACK_SUCCESS = 0x400; static const uint16_t ACK_FAILURE = 0x401; static const uint16_t EXE_SUCCESS = 0x402; static const uint16_t EXE_FAILURE = 0x403; +<<<<<<< HEAD static const uint16_t TM_MEMORY_READ_REPORT = 0x404; static const uint16_t TM_FLASH_READ_REPORT = 0x405; +======= +static constexpr uint16_t TM_FLASH_DIRECTORY_CONTENT = 0x406; +>>>>>>> 98ede40d (start adding dir content report) static const uint16_t TM_CAM_CMD_RPT = 0x407; } // namespace apid From 5b4261104ef83e8db254cbde13d0a35ec77e1bcf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 20:20:47 +0200 Subject: [PATCH 104/506] add some more commands --- linux/payload/plocMpscoDefs.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index 7e862ba3..c3a2bc35 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -112,26 +112,26 @@ static const uint16_t TC_FLASHFOPEN = 0x119; static const uint16_t TC_FLASHFCLOSE = 0x11A; static constexpr uint16_t TC_FLASH_GET_DIRECTORY_CONTENT = 0x11B; static const uint16_t TC_FLASHDELETE = 0x11C; +static constexpr uint16_t TC_FLASH_CREATE_DIR = 0x11D; static const uint16_t TC_MODE_IDLE = 0x11E; static const uint16_t TC_MODE_REPLAY = 0x11F; static const uint16_t TC_MODE_SNAPSHOT = 0x120; static const uint16_t TC_DOWNLINK_DATA_MODULATE = 0x121; static constexpr uint16_t TC_HK_GET_REPORT = 0x123; -static constexpr uint16_t TM_HK_GET_REPORT = 0x408; static const uint16_t TC_DOWNLINK_PWR_OFF = 0x124; static const uint16_t TC_CAM_CMD_SEND = 0x12C; +static constexpr uint16_t TC_FLASH_COPY_FILE = 0x12E; static const uint16_t TC_SIMPLEX_SEND_FILE = 0x130; + static const uint16_t ACK_SUCCESS = 0x400; static const uint16_t ACK_FAILURE = 0x401; static const uint16_t EXE_SUCCESS = 0x402; static const uint16_t EXE_FAILURE = 0x403; -<<<<<<< HEAD static const uint16_t TM_MEMORY_READ_REPORT = 0x404; static const uint16_t TM_FLASH_READ_REPORT = 0x405; -======= static constexpr uint16_t TM_FLASH_DIRECTORY_CONTENT = 0x406; ->>>>>>> 98ede40d (start adding dir content report) static const uint16_t TM_CAM_CMD_RPT = 0x407; +static constexpr uint16_t TM_HK_GET_REPORT = 0x408; } // namespace apid /** Offset from first byte in space packet to first byte of data field */ From 827419ef34ea29ad1e5f1e58fa2a08df26287a2f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 15:24:47 +0200 Subject: [PATCH 105/506] implement dir content report in OBSW --- linux/payload/PlocMpsocHandler.cpp | 83 +++++++++++++++++++++++------- linux/payload/PlocMpsocHandler.h | 3 +- linux/payload/plocMpscoDefs.h | 17 ++++++ 3 files changed, 83 insertions(+), 20 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 2cbc66af..78093740 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -262,6 +262,10 @@ ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t device result = prepareTcGetHkReport(); break; } + case (mpsoc::TC_FLASH_GET_DIRECTORY_CONTENT): { + result = prepareTcGetDirContent(commandData, commandDataLen); + break; + } case (mpsoc::TC_MODE_REPLAY): { result = prepareTcModeReplay(); break; @@ -325,14 +329,16 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() { this->insertInCommandMap(mpsoc::RELEASE_UART_TX); this->insertInCommandMap(mpsoc::SET_UART_TX_TRISTATE); this->insertInCommandMap(mpsoc::TC_CAM_TAKE_PIC); + this->insertInCommandMap(mpsoc::TC_FLASH_GET_DIRECTORY_CONTENT); this->insertInCommandMap(mpsoc::TC_SIMPLEX_SEND_FILE); this->insertInCommandMap(mpsoc::TC_DOWNLINK_DATA_MODULATE); this->insertInCommandMap(mpsoc::TC_MODE_SNAPSHOT); this->insertInReplyMap(mpsoc::ACK_REPORT, 3, nullptr, mpsoc::SIZE_ACK_REPORT); this->insertInReplyMap(mpsoc::EXE_REPORT, 3, nullptr, mpsoc::SIZE_EXE_REPORT); this->insertInReplyMap(mpsoc::TM_MEMORY_READ_REPORT, 2, nullptr, mpsoc::SIZE_TM_MEM_READ_REPORT); - this->insertInReplyMap(mpsoc::TM_GET_HK_REPORT, 2, nullptr, 0); + this->insertInReplyMap(mpsoc::TM_GET_HK_REPORT, 2, nullptr, mpsoc::SIZE_TM_HK_REPORT); this->insertInReplyMap(mpsoc::TM_CAM_CMD_RPT, 2, nullptr, mpsoc::SP_MAX_SIZE); + this->insertInReplyMap(mpsoc::TM_FLASH_DIRECTORY_CONTENT, 2, nullptr, 0); } ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remainingSize, @@ -350,6 +356,11 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain } uint16_t apid = spacePacket.getApid(); + auto handleDedicatedReply = [&](DeviceCommandId_t replyId) { + *foundLen = spacePacket.getFullPacketLen(); + foundPacketLen = *foundLen; + *foundId = replyId; + }; switch (apid) { case (mpsoc::apid::ACK_SUCCESS): *foundLen = mpsoc::SIZE_ACK_REPORT; @@ -364,14 +375,14 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain *foundId = mpsoc::TM_MEMORY_READ_REPORT; break; case (mpsoc::apid::TM_CAM_CMD_RPT): - *foundLen = spacePacket.getFullPacketLen(); - foundPacketLen = *foundLen; - *foundId = mpsoc::TM_CAM_CMD_RPT; + handleDedicatedReply(mpsoc::TM_CAM_CMD_RPT); break; case (mpsoc::apid::TM_HK_GET_REPORT): { - *foundLen = spacePacket.getFullPacketLen(); - foundPacketLen = *foundLen; - *foundId = mpsoc::TM_GET_HK_REPORT; + handleDedicatedReply(mpsoc::TM_GET_HK_REPORT); + break; + } + case (mpsoc::apid::TM_FLASH_DIRECTORY_CONTENT): { + handleDedicatedReply(mpsoc::TM_FLASH_DIRECTORY_CONTENT); break; } case (mpsoc::apid::EXE_SUCCESS): @@ -419,6 +430,18 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const result = handleCamCmdRpt(packet); break; } + case (mpsoc::TM_FLASH_DIRECTORY_CONTENT): { + result = verifyPacket(packet, foundPacketLen); + if (result == MPSoCReturnValuesIF::CRC_FAILURE) { + sif::warning << "PLOC MPSoC: Flash directory content reply invalid CRC" << std::endl; + } + /** Send data to commanding queue */ + handleDeviceTm(packet + mpsoc::DATA_FIELD_OFFSET, + foundPacketLen - mpsoc::DATA_FIELD_OFFSET - mpsoc::CRC_SIZE, + mpsoc::TM_FLASH_DIRECTORY_CONTENT); + nextReplyId = mpsoc::EXE_REPORT; + return result; + } case (mpsoc::EXE_REPORT): { result = handleExecutionReport(packet); break; @@ -432,7 +455,10 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const return result; } -void PlocMPSoCHandler::setNormalDatapoolEntriesInvalid() {} +void PlocMPSoCHandler::setNormalDatapoolEntriesInvalid() { + PoolReadGuard pg(&hkReport); + hkReport.setValidity(false, true); +} uint32_t PlocMPSoCHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } @@ -635,9 +661,8 @@ ReturnValue_t PlocMPSoCHandler::prepareTcCamCmdSend(const uint8_t* commandData, ReturnValue_t PlocMPSoCHandler::prepareTcCamTakePic(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = returnvalue::OK; mpsoc::TcCamTakePic tcCamTakePic(spParams, sequenceCount); - result = tcCamTakePic.buildPacket(commandData, commandDataLen); + ReturnValue_t result = tcCamTakePic.buildPacket(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } @@ -647,9 +672,8 @@ ReturnValue_t PlocMPSoCHandler::prepareTcCamTakePic(const uint8_t* commandData, ReturnValue_t PlocMPSoCHandler::prepareTcSimplexSendFile(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = returnvalue::OK; mpsoc::TcSimplexSendFile tcSimplexSendFile(spParams, sequenceCount); - result = tcSimplexSendFile.buildPacket(commandData, commandDataLen); + ReturnValue_t result = tcSimplexSendFile.buildPacket(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } @@ -657,11 +681,21 @@ ReturnValue_t PlocMPSoCHandler::prepareTcSimplexSendFile(const uint8_t* commandD return returnvalue::OK; } +ReturnValue_t PlocMPSoCHandler::prepareTcGetDirContent(const uint8_t* commandData, + size_t commandDataLen) { + mpsoc::TcGetDirContent tcGetDirContent(spParams, sequenceCount); + ReturnValue_t result = tcGetDirContent.buildPacket(commandData, commandDataLen); + if (result != returnvalue::OK) { + return result; + } + finishTcPrep(tcGetDirContent.getFullPacketLen()); + return returnvalue::OK; +} + ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkDataModulate(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = returnvalue::OK; mpsoc::TcDownlinkDataModulate tcDownlinkDataModulate(spParams, sequenceCount); - result = tcDownlinkDataModulate.buildPacket(commandData, commandDataLen); + ReturnValue_t result = tcDownlinkDataModulate.buildPacket(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } @@ -670,9 +704,8 @@ ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkDataModulate(const uint8_t* com } ReturnValue_t PlocMPSoCHandler::prepareTcModeSnapshot() { - ReturnValue_t result = returnvalue::OK; mpsoc::TcModeSnapshot tcModeSnapshot(spParams, sequenceCount); - result = tcModeSnapshot.buildPacket(); + ReturnValue_t result = tcModeSnapshot.buildPacket(); if (result != returnvalue::OK) { return result; } @@ -792,7 +825,7 @@ ReturnValue_t PlocMPSoCHandler::handleMemoryReadReport(const uint8_t* data) { uint16_t memLen = *(data + mpsoc::MEM_READ_RPT_LEN_OFFSET) << 8 | *(data + mpsoc::MEM_READ_RPT_LEN_OFFSET + 1); /** Send data to commanding queue */ - handleDeviceTM(data + mpsoc::DATA_FIELD_OFFSET, mpsoc::SIZE_MEM_READ_RPT_FIX + memLen * 4, + handleDeviceTm(data + mpsoc::DATA_FIELD_OFFSET, mpsoc::SIZE_MEM_READ_RPT_FIX + memLen * 4, mpsoc::TM_MEMORY_READ_REPORT); nextReplyId = mpsoc::EXE_REPORT; return result; @@ -991,7 +1024,7 @@ ReturnValue_t PlocMPSoCHandler::handleCamCmdRpt(const uint8_t* data) { sif::info << "PlocMPSoCHandler: CamCmdRpt Ack value: 0x" << std::hex << static_cast(ackValue) << std::endl; #endif /* OBSW_DEBUG_PLOC_MPSOC == 1 */ - handleDeviceTM(packetReader.getPacketData() + sizeof(uint16_t), + handleDeviceTm(packetReader.getPacketData() + sizeof(uint16_t), packetReader.getPacketDataLen() - 1, mpsoc::TM_CAM_CMD_RPT); return result; } @@ -1050,6 +1083,13 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator } break; } + case mpsoc::TC_FLASH_GET_DIRECTORY_CONTENT: { + result = enableThreeReplies(mpsoc::TM_FLASH_DIRECTORY_CONTENT); + if (result != returnvalue::OK) { + return result; + } + break; + } case mpsoc::OBSW_RESET_SEQ_COUNT: break; default: @@ -1116,6 +1156,7 @@ void PlocMPSoCHandler::setNextReplyId() { break; } } + size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) { size_t replyLen = 0; @@ -1217,7 +1258,7 @@ void PlocMPSoCHandler::completionFailedReceived(ActionId_t actionId, ReturnValue handleActionCommandFailure(actionId); } -void PlocMPSoCHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, +void PlocMPSoCHandler::handleDeviceTm(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) { ReturnValue_t result = returnvalue::OK; @@ -1286,6 +1327,10 @@ void PlocMPSoCHandler::disableAllReplies() { disableCommandWithReply(TM_GET_HK_REPORT); break; } + case TC_FLASH_GET_DIRECTORY_CONTENT: { + disableCommandWithReply(TM_FLASH_DIRECTORY_CONTENT); + break; + } case TC_CAM_CMD_SEND: { disableCommandWithReply(TM_CAM_CMD_RPT); break; diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index c9850e18..a6a866ef 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -201,6 +201,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { ReturnValue_t prepareTcDownlinkPwrOn(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcDownlinkPwrOff(); ReturnValue_t prepareTcGetHkReport(); + ReturnValue_t prepareTcGetDirContent(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcReplayWriteSequence(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcCamCmdSend(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcModeIdle(); @@ -266,7 +267,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { * @param dataSize Size of telemetry in bytes. * @param replyId Id of the reply. This will be added to the ActionMessage. */ - void handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId); + void handleDeviceTm(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId); /** * @brief In case an acknowledgment failure reply has been received this function disables diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index c3a2bc35..9cdd4515 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -669,6 +669,23 @@ class TcGetHkReport : public TcBase { : TcBase(params, apid::TC_HK_GET_REPORT, sequenceCount) {} }; +class TcGetDirContent : public TcBase { + public: + TcGetDirContent(ploc::SpTcParams params, uint16_t sequenceCount) + : TcBase(params, apid::TC_FLASH_GET_DIRECTORY_CONTENT, sequenceCount) {} + + ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) { + ReturnValue_t result = returnvalue::OK; + spParams.setFullPayloadLen(commandDataLen + sizeof(NULL_TERMINATOR) + CRC_SIZE); + if (result != returnvalue::OK) { + return result; + } + std::memcpy(payloadStart, commandData, commandDataLen); + *(payloadStart + commandDataLen) = NULL_TERMINATOR; + return result; + } +}; + /** * @brief Class to build replay stop space packet. */ From 98ef38f3eb0c7c22839d50e16a29e2b4741f3939 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 15:30:30 +0200 Subject: [PATCH 106/506] i think that should do the job --- linux/payload/PlocMpsocHandler.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 78093740..25996b1a 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -1181,6 +1181,10 @@ size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) { // report is not fixed replyLen = mpsoc::SP_MAX_SIZE; break; + case mpsoc::TM_FLASH_DIRECTORY_CONTENT: + // I think the reply size is not fixed either. + replyLen = mpsoc::SP_MAX_SIZE; + break; default: { replyLen = iter->second.replyLen; break; From 71ef1edb683f02e14e3329501b939b725b7661e8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 20:21:07 +0200 Subject: [PATCH 107/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 75aa9c06..5592997c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -35,6 +35,7 @@ will consitute of a breaking change warranting a new major release: - Add ACS board for EM by default now. - Add support for MPSoC HK packet. - Dynamically enable and disable HK packets for MPSoC on `ON` and `OFF` commands. +- Add support for MPSoC Flash Directory Content Report. ## Added From f72c797f53f6a9d47f2510fd79537b189219d7c6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 20:21:44 +0200 Subject: [PATCH 108/506] this should do the job --- CHANGELOG.md | 2 + common/config/eive/definitions.h | 4 +- linux/payload/PlocMpsocHandler.cpp | 19 ++++++--- linux/payload/PlocMpsocHelper.cpp | 8 ++-- linux/payload/plocMpscoDefs.h | 59 ++++++++++++++++++++------ mission/acs/str/StarTrackerHandler.cpp | 17 ++++---- mission/acs/str/StarTrackerHandler.h | 3 -- 7 files changed, 77 insertions(+), 35 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5592997c..d4fa7a22 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -36,6 +36,8 @@ will consitute of a breaking change warranting a new major release: - Add support for MPSoC HK packet. - Dynamically enable and disable HK packets for MPSoC on `ON` and `OFF` commands. - Add support for MPSoC Flash Directory Content Report. +- Larger allowed path and file sizes for STR and PLOC MPSoC modules. +- More robust MPSoC flash read and write command data handling. ## Added diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h index 8c460f53..78ba52e4 100644 --- a/common/config/eive/definitions.h +++ b/common/config/eive/definitions.h @@ -34,8 +34,8 @@ static constexpr uint32_t STR_IMG_HELPER_QUEUE_SIZE = 50; static constexpr uint8_t LIVE_TM = 0; /* Limits for filename and path checks */ -static constexpr uint32_t MAX_PATH_SIZE = 100; -static constexpr uint32_t MAX_FILENAME_SIZE = 50; +static constexpr uint32_t MAX_PATH_SIZE = 200; +static constexpr uint32_t MAX_FILENAME_SIZE = 100; static constexpr uint32_t SA_DEPL_INIT_BUFFER_SECS = 120; // Burn time for autonomous deployment diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 25996b1a..3462859b 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -125,10 +125,7 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI switch (actionId) { case mpsoc::TC_FLASH_WRITE_FULL_FILE: { - if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) { - return MPSoCReturnValuesIF::FILENAME_TOO_LONG; - } - mpsoc::FlashWritePusCmd flashWritePusCmd; + mpsoc::FlashBasePusCmd flashWritePusCmd; result = flashWritePusCmd.extractFields(data, size); if (result != returnvalue::OK) { return result; @@ -142,7 +139,19 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI return EXECUTION_FINISHED; } case mpsoc::TC_FLASH_READ_FULL_FILE: { - // TODO: Finsh this. + mpsoc::FlashReadPusCmd flashReadPusCmd; + result = flashReadPusCmd.extractFields(data, size); + if (result != returnvalue::OK) { + return result; + } + result = plocMPSoCHelper->startFlashRead(flashReadPusCmd.getObcFile(), + flashReadPusCmd.getMPSoCFile(), + flashReadPusCmd.getReadSize()); + if (result != returnvalue::OK) { + return result; + } + plocMPSoCHelperExecuting = true; + return EXECUTION_FINISHED; break; } case (mpsoc::OBSW_RESET_SEQ_COUNT): { diff --git a/linux/payload/PlocMpsocHelper.cpp b/linux/payload/PlocMpsocHelper.cpp index 3eaa833d..421ac83c 100644 --- a/linux/payload/PlocMpsocHelper.cpp +++ b/linux/payload/PlocMpsocHelper.cpp @@ -84,7 +84,7 @@ void PlocMPSoCHelper::setSequenceCount(SourceSequenceCounter* sequenceCount_) { } ReturnValue_t PlocMPSoCHelper::startFlashWrite(std::string obcFile, std::string mpsocFile) { - ReturnValue_t result = startFlashReadOrWriteBase(obcFile, mpsocFile); + ReturnValue_t result = startFlashReadOrWriteBase(std::move(obcFile), std::move(mpsocFile)); if (result != returnvalue::OK) { return result; } @@ -94,7 +94,7 @@ ReturnValue_t PlocMPSoCHelper::startFlashWrite(std::string obcFile, std::string ReturnValue_t PlocMPSoCHelper::startFlashRead(std::string obcFile, std::string mpsocFile, size_t readFileSize) { - ReturnValue_t result = startFlashReadOrWriteBase(obcFile, mpsocFile); + ReturnValue_t result = startFlashReadOrWriteBase(std::move(obcFile), std::move(mpsocFile)); if (result != returnvalue::OK) { return result; } @@ -437,8 +437,8 @@ ReturnValue_t PlocMPSoCHelper::startFlashReadOrWriteBase(std::string obcFile, return result; } - flashReadAndWrite.obcFile = obcFile; - flashReadAndWrite.mpsocFile = mpsocFile; + flashReadAndWrite.obcFile = std::move(obcFile); + flashReadAndWrite.mpsocFile = std::move(mpsocFile); return resetHelper(); } diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index 9cdd4515..d7bfa028 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -736,36 +736,69 @@ class TcReplayWriteSeq : public TcBase { /** * @brief Helps to extract the fields of the flash write command from the PUS packet. */ -class FlashWritePusCmd : public MPSoCReturnValuesIF { +class FlashBasePusCmd : public MPSoCReturnValuesIF { public: - FlashWritePusCmd(){}; + FlashBasePusCmd() = default; + virtual ~FlashBasePusCmd() = default; - ReturnValue_t extractFields(const uint8_t* commandData, size_t commandDataLen) { + virtual ReturnValue_t extractFields(const uint8_t* commandData, size_t commandDataLen) { if (commandDataLen > (config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE + MAX_FILENAME_SIZE)) { return INVALID_LENGTH; } - obcFile = std::string(reinterpret_cast(commandData)); - if (obcFile.size() > (config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE)) { + size_t fileLen = strnlen(reinterpret_cast(commandData), commandDataLen); + if (fileLen > (config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE)) { return FILENAME_TOO_LONG; } - mpsocFile = std::string( - reinterpret_cast(commandData + obcFile.size() + SIZE_NULL_TERMINATOR)); - if (mpsocFile.size() > MAX_FILENAME_SIZE) { + obcFile = std::string(reinterpret_cast(commandData), fileLen); + fileLen = + strnlen(reinterpret_cast(commandData + obcFile.size() + SIZE_NULL_TERMINATOR), + commandDataLen - obcFile.size() - 1); + if (fileLen > MAX_FILENAME_SIZE) { return MPSOC_FILENAME_TOO_LONG; } + mpsocFile = std::string( + reinterpret_cast(commandData + obcFile.size() + SIZE_NULL_TERMINATOR), + fileLen); return returnvalue::OK; } - std::string getObcFile() { return obcFile; } + const std::string& getObcFile() const { return obcFile; } - std::string getMPSoCFile() { return mpsocFile; } + const std::string& getMPSoCFile() const { return mpsocFile; } + + protected: + size_t getParsedSize() const { + return getObcFile().size() + getMPSoCFile().size() + 2 * SIZE_NULL_TERMINATOR; + } + static const size_t SIZE_NULL_TERMINATOR = 1; private: - static const size_t SIZE_NULL_TERMINATOR = 1; - std::string obcFile = ""; - std::string mpsocFile = ""; + std::string obcFile; + std::string mpsocFile; }; +class FlashReadPusCmd : public FlashBasePusCmd { + public: + FlashReadPusCmd(){}; + + ReturnValue_t extractFields(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t result = FlashBasePusCmd::extractFields(commandData, commandDataLen); + if (result != returnvalue::OK) { + return result; + } + if (commandDataLen < (getParsedSize() + 4)) { + return returnvalue::FAILED; + } + size_t deserDummy = 4; + return SerializeAdapter::deSerialize(&readSize, commandData + getParsedSize(), &deserDummy, + SerializeIF::Endianness::NETWORK); + } + + size_t getReadSize() const { return readSize; } + + private: + size_t readSize = 0; +}; /** * @brief Class to build replay stop space packet. */ diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index d40e5fab..0942164a 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -17,6 +17,7 @@ extern "C" { #include #include "OBSWConfig.h" +#include "eive/definitions.h" std::atomic_bool JCFG_DONE(false); @@ -152,7 +153,7 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu return EXECUTION_FINISHED; } case (startracker::SET_JSON_FILE_NAME): { - if (size > MAX_PATH_SIZE) { + if (size > config::MAX_PATH_SIZE) { return FILE_PATH_TOO_LONG; } paramJsonFile = std::string(reinterpret_cast(data), size); @@ -189,7 +190,7 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu if (result != returnvalue::OK) { return result; } - if (size > MAX_PATH_SIZE + MAX_FILE_NAME) { + if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) { return FILE_PATH_TOO_LONG; } result = strHelper->startImageUpload(std::string(reinterpret_cast(data), size)); @@ -204,7 +205,7 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu if (result != returnvalue::OK) { return result; } - if (size > MAX_PATH_SIZE) { + if (size > config::MAX_PATH_SIZE) { return FILE_PATH_TOO_LONG; } result = @@ -228,14 +229,14 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu return EXECUTION_FINISHED; } case (startracker::CHANGE_IMAGE_DOWNLOAD_FILE): { - if (size > MAX_FILE_NAME) { + if (size > config::MAX_FILENAME_SIZE) { return FILENAME_TOO_LONG; } strHelper->setDownloadImageName(std::string(reinterpret_cast(data), size)); return EXECUTION_FINISHED; } case (startracker::SET_FLASH_READ_FILENAME): { - if (size > MAX_FILE_NAME) { + if (size > config::MAX_FILENAME_SIZE) { return FILENAME_TOO_LONG; } strHelper->setFlashReadFilename(std::string(reinterpret_cast(data), size)); @@ -246,7 +247,7 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu if (result != returnvalue::OK) { return result; } - if (size > MAX_PATH_SIZE + MAX_FILE_NAME) { + if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) { return FILE_PATH_TOO_LONG; } result = @@ -1568,7 +1569,7 @@ ReturnValue_t StarTrackerHandler::executeFlashReadCommand(const uint8_t* command << std::endl; return result; } - if (commandDataLen - sizeof(startRegion) - sizeof(length) > MAX_PATH_SIZE) { + if (commandDataLen - sizeof(startRegion) - sizeof(length) > config::MAX_PATH_SIZE) { sif::warning << "StarTrackerHandler::executeFlashReadCommand: Received command with invalid" << " path and filename" << std::endl; return FILE_PATH_TOO_LONG; @@ -1708,7 +1709,7 @@ ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData bool reinitSet) { // Stopwatch watch; ReturnValue_t result = returnvalue::OK; - if (commandDataLen > MAX_PATH_SIZE) { + if (commandDataLen > config::MAX_PATH_SIZE) { return FILE_PATH_TOO_LONG; } if (reinitSet) { diff --git a/mission/acs/str/StarTrackerHandler.h b/mission/acs/str/StarTrackerHandler.h index a74bff44..7bcc9f2a 100644 --- a/mission/acs/str/StarTrackerHandler.h +++ b/mission/acs/str/StarTrackerHandler.h @@ -144,9 +144,6 @@ class StarTrackerHandler : public DeviceHandlerBase { //! [EXPORT] : [COMMENT] Failed to boot star tracker into bootloader mode static const Event BOOTING_BOOTLOADER_FAILED_EVENT = MAKE_EVENT(2, severity::LOW); - static const size_t MAX_PATH_SIZE = 50; - static const size_t MAX_FILE_NAME = 30; - static const uint8_t STATUS_OFFSET = 2; static const uint8_t PARAMS_OFFSET = 2; static const uint8_t TICKS_OFFSET = 3; From 119b1c8eb9b2212b9fc87b7ff3ade7160ceebf83 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 20:22:10 +0200 Subject: [PATCH 109/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 5fbd19bb..13fd9a7d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 5fbd19bb6cca0790373a809d78f2307adca9d0c8 +Subproject commit 13fd9a7d84645535496e9ff2855118e5d0b59916 From e762cc5fb35eaffe83d33b0ce9b64c104824ea75 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:22:21 +0200 Subject: [PATCH 110/506] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index dc320982..acfd8ef3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -46,6 +46,11 @@ will consitute of a breaking change warranting a new major release: 16505 type. - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. +- The dual lane assembly transition failed handler started new transitions towards the current mode + instead of the target mode. This means that if the dual lane assembly never reached the initial + submode (e.g. mode normal and submode dual side), it will transition back to the current mode, + which is `MODE_OFF`. Furthermore, this lead to invalid internal states, so the subsequent + recovery handling becomes stuck in the custom recovery sequence when swichting power back on. # [v2.0.5] 2023-05-11 From 5728d916efb9a91244b4e3121e927f11beb75267 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:23:21 +0200 Subject: [PATCH 111/506] changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index acfd8ef3..c185fab1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -49,7 +49,7 @@ will consitute of a breaking change warranting a new major release: - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial submode (e.g. mode normal and submode dual side), it will transition back to the current mode, - which is `MODE_OFF`. Furthermore, this lead to invalid internal states, so the subsequent + which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent recovery handling becomes stuck in the custom recovery sequence when swichting power back on. # [v2.0.5] 2023-05-11 From d5a6feb3475d30e4bb5a5485c522101b5ce49731 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:28:05 +0200 Subject: [PATCH 112/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index c185fab1..af09ec1a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -51,6 +51,8 @@ will consitute of a breaking change warranting a new major release: submode (e.g. mode normal and submode dual side), it will transition back to the current mode, which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent recovery handling becomes stuck in the custom recovery sequence when swichting power back on. +- The dual lane custom recovery handling was adapted to always perform proper power switch handling + irrespective of current or target modes. # [v2.0.5] 2023-05-11 From 07dd84ff1e6867f4ba8ce5effe5003685a5085b8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 15:34:45 +0200 Subject: [PATCH 113/506] changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index af09ec1a..7d096ddd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -46,6 +46,9 @@ will consitute of a breaking change warranting a new major release: 16505 type. - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. + +# [v2.0.5] to be released + - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial submode (e.g. mode normal and submode dual side), it will transition back to the current mode, From 9b5fac828b5d5093ef24ecc01e6f5eddc5463ee8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 21:17:06 +0200 Subject: [PATCH 114/506] changelog --- CHANGELOG.md | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7d096ddd..dc320982 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -47,16 +47,6 @@ will consitute of a breaking change warranting a new major release: - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. -# [v2.0.5] to be released - -- The dual lane assembly transition failed handler started new transitions towards the current mode - instead of the target mode. This means that if the dual lane assembly never reached the initial - submode (e.g. mode normal and submode dual side), it will transition back to the current mode, - which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent - recovery handling becomes stuck in the custom recovery sequence when swichting power back on. -- The dual lane custom recovery handling was adapted to always perform proper power switch handling - irrespective of current or target modes. - # [v2.0.5] 2023-05-11 - The dual lane assembly transition failed handler started new transitions towards the current mode From f225d9a7c19f4b2bfa267bab7d893cfd988d638f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 2 May 2023 13:42:20 +0200 Subject: [PATCH 115/506] add HK request command --- linux/payload/PlocMpsocHandler.cpp | 47 ++++++++++++++++++++++++++---- linux/payload/PlocMpsocHandler.h | 9 ++---- linux/payload/plocMpscoDefs.h | 14 +++++++++ 3 files changed, 58 insertions(+), 12 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 72f2355f..29656c59 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -247,6 +247,10 @@ ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t device result = prepareTcReplayWriteSequence(commandData, commandDataLen); break; } + case (mpsoc::TC_GET_HK_REPORT): { + result = prepareTcGetHkReport(); + break; + } case (mpsoc::TC_MODE_REPLAY): { result = prepareTcModeReplay(); break; @@ -304,6 +308,7 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() { this->insertInCommandMap(mpsoc::TC_MODE_REPLAY); this->insertInCommandMap(mpsoc::TC_MODE_IDLE); this->insertInCommandMap(mpsoc::TC_CAM_CMD_SEND); + this->insertInCommandMap(mpsoc::TC_GET_HK_REPORT); this->insertInCommandMap(mpsoc::RELEASE_UART_TX); this->insertInCommandMap(mpsoc::SET_UART_TX_TRISTATE); this->insertInCommandMap(mpsoc::TC_CAM_TAKE_PIC); @@ -313,6 +318,7 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() { this->insertInReplyMap(mpsoc::ACK_REPORT, 3, nullptr, mpsoc::SIZE_ACK_REPORT); this->insertInReplyMap(mpsoc::EXE_REPORT, 3, nullptr, mpsoc::SIZE_EXE_REPORT); this->insertInReplyMap(mpsoc::TM_MEMORY_READ_REPORT, 2, nullptr, mpsoc::SIZE_TM_MEM_READ_REPORT); + this->insertInReplyMap(mpsoc::TM_GET_HK_REPORT, 2, nullptr, 0); this->insertInReplyMap(mpsoc::TM_CAM_CMD_RPT, 2, nullptr, mpsoc::SP_MAX_SIZE); } @@ -346,9 +352,15 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain break; case (mpsoc::apid::TM_CAM_CMD_RPT): *foundLen = spacePacket.getFullPacketLen(); - tmCamCmdRpt.rememberSpacePacketSize = *foundLen; + foundPacketLen = *foundLen; *foundId = mpsoc::TM_CAM_CMD_RPT; break; + case (mpsoc::apid::TM_HK_GET_REPORT): { + *foundLen = spacePacket.getFullPacketLen(); + foundPacketLen = *foundLen; + *foundId = mpsoc::TM_GET_HK_REPORT; + break; + } case (mpsoc::apid::EXE_SUCCESS): *foundLen = mpsoc::SIZE_EXE_REPORT; *foundId = mpsoc::EXE_REPORT; @@ -386,6 +398,10 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const result = handleMemoryReadReport(packet); break; } + case (mpsoc::TM_GET_HK_REPORT): { + result = handleGetHkReport(packet); + break; + } case (mpsoc::TM_CAM_CMD_RPT): { result = handleCamCmdRpt(packet); break; @@ -512,6 +528,17 @@ ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOff() { return returnvalue::OK; } +ReturnValue_t PlocMPSoCHandler::prepareTcGetHkReport() { + ReturnValue_t result = returnvalue::OK; + mpsoc::TcGetHkReport tcDownlinkPwrOff(spParams, sequenceCount); + result = tcDownlinkPwrOff.buildPacket(); + if (result != returnvalue::OK) { + return result; + } + finishTcPrep(tcDownlinkPwrOff.getFullPacketLen()); + return returnvalue::OK; +} + ReturnValue_t PlocMPSoCHandler::prepareTcReplayWriteSequence(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; @@ -724,17 +751,25 @@ ReturnValue_t PlocMPSoCHandler::handleMemoryReadReport(const uint8_t* data) { return result; } +ReturnValue_t PlocMPSoCHandler::handleGetHkReport(const uint8_t* data) { + ReturnValue_t result = verifyPacket(data, foundPacketLen); + if (result != returnvalue::OK) { + return result; + } + SpacePacketReader packetReader(data, foundPacketLen); + return returnvalue::OK; +} + ReturnValue_t PlocMPSoCHandler::handleCamCmdRpt(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; - result = verifyPacket(data, tmCamCmdRpt.rememberSpacePacketSize); + ReturnValue_t result = verifyPacket(data, foundPacketLen); if (result == MPSoCReturnValuesIF::CRC_FAILURE) { sif::warning << "PlocMPSoCHandler::handleCamCmdRpt: CRC failure" << std::endl; } - SpacePacketReader packetReader(data, tmCamCmdRpt.rememberSpacePacketSize); + SpacePacketReader packetReader(data, foundPacketLen); const uint8_t* dataFieldPtr = data + mpsoc::SPACE_PACKET_HEADER_SIZE + sizeof(uint16_t); - std::string camCmdRptMsg( - reinterpret_cast(dataFieldPtr), - tmCamCmdRpt.rememberSpacePacketSize - mpsoc::SPACE_PACKET_HEADER_SIZE - sizeof(uint16_t) - 3); + std::string camCmdRptMsg(reinterpret_cast(dataFieldPtr), + foundPacketLen - mpsoc::SPACE_PACKET_HEADER_SIZE - sizeof(uint16_t) - 3); #if OBSW_DEBUG_PLOC_MPSOC == 1 uint8_t ackValue = *(packetReader.getFullData() + packetReader.getFullPacketLen() - 2); sif::info << "PlocMPSoCHandler: CamCmdRpt message: " << camCmdRptMsg << std::endl; diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index 10e6bd5d..e11f05eb 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -138,17 +138,12 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { TmMemReadReport tmMemReadReport; - struct TmCamCmdRpt { - size_t rememberSpacePacketSize = 0; - }; - - TmCamCmdRpt tmCamCmdRpt; - struct TelemetryBuffer { uint16_t length = 0; uint8_t buffer[mpsoc::SP_MAX_SIZE]; }; + size_t foundPacketLen = 0; TelemetryBuffer tmBuffer; enum class PowerState { OFF, BOOTING, SHUTDOWN, ON }; @@ -167,6 +162,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { ReturnValue_t prepareTcReplayStop(); ReturnValue_t prepareTcDownlinkPwrOn(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcDownlinkPwrOff(); + ReturnValue_t prepareTcGetHkReport(); ReturnValue_t prepareTcReplayWriteSequence(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcCamCmdSend(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcModeIdle(); @@ -213,6 +209,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { */ ReturnValue_t handleMemoryReadReport(const uint8_t* data); + ReturnValue_t handleGetHkReport(const uint8_t* data); ReturnValue_t handleCamCmdRpt(const uint8_t* data); /** diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index 975dad32..e6acf4bf 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -37,6 +37,8 @@ static const DeviceCommandId_t TC_CAM_TAKE_PIC = 22; static const DeviceCommandId_t TC_SIMPLEX_SEND_FILE = 23; static const DeviceCommandId_t TC_DOWNLINK_DATA_MODULATE = 24; static const DeviceCommandId_t TC_MODE_SNAPSHOT = 25; +static const DeviceCommandId_t TC_GET_HK_REPORT = 26; +static const DeviceCommandId_t TM_GET_HK_REPORT = 27; // Will reset the sequence count of the OBSW static const DeviceCommandId_t OBSW_RESET_SEQ_COUNT = 50; @@ -45,6 +47,7 @@ static const uint16_t SIZE_ACK_REPORT = 14; static const uint16_t SIZE_EXE_REPORT = 14; static const uint16_t SIZE_TM_MEM_READ_REPORT = 18; static const uint16_t SIZE_TM_CAM_CMD_RPT = 18; +static constexpr size_t SIZE_TM_HK_REPORT = 369; /** * SpacePacket apids of PLOC telecommands and telemetry. @@ -65,6 +68,8 @@ static const uint16_t TC_MODE_IDLE = 0x11E; static const uint16_t TC_MODE_REPLAY = 0x11F; static const uint16_t TC_MODE_SNAPSHOT = 0x120; static const uint16_t TC_DOWNLINK_DATA_MODULATE = 0x121; +static constexpr uint16_t TC_HK_GET_REPORT = 0x123; +static constexpr uint16_t TM_HK_GET_REPORT = 0x408; static const uint16_t TC_DOWNLINK_PWR_OFF = 0x124; static const uint16_t TC_CAM_CMD_SEND = 0x12C; static const uint16_t TC_SIMPLEX_SEND_FILE = 0x130; @@ -561,6 +566,15 @@ class TcDownlinkPwrOn : public TcBase { } }; +/** + * @brief Class to build replay stop space packet. + */ +class TcGetHkReport : public TcBase { + public: + TcGetHkReport(ploc::SpTcParams params, uint16_t sequenceCount) + : TcBase(params, apid::TC_HK_GET_REPORT, sequenceCount) {} +}; + /** * @brief Class to build replay stop space packet. */ From 0fd18204bfdfa2ca9de02424c6783c2631608e3b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 2 May 2023 15:04:14 +0200 Subject: [PATCH 116/506] add new set --- linux/payload/PlocMpsocHandler.cpp | 44 +++++++++++- linux/payload/PlocMpsocHandler.h | 40 ++++++++++- linux/payload/plocMpscoDefs.h | 103 +++++++++++++++++++++++++++++ mission/tcs/Tmp1075Definitions.h | 2 +- 4 files changed, 186 insertions(+), 3 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 29656c59..b0c56667 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -11,6 +11,7 @@ PlocMPSoCHandler::PlocMPSoCHandler(object_id_t objectId, object_id_t uartComIFid CookieIF* comCookie, PlocMPSoCHelper* plocMPSoCHelper, Gpio uartIsolatorSwitch, object_id_t supervisorHandler) : DeviceHandlerBase(objectId, uartComIFid, comCookie), + hkReport(this), plocMPSoCHelper(plocMPSoCHelper), uartIsolatorSwitch(uartIsolatorSwitch), supervisorHandler(supervisorHandler), @@ -425,6 +426,41 @@ uint32_t PlocMPSoCHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) ReturnValue_t PlocMPSoCHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { + localDataPoolMap.emplace(mpsoc::poolid::STATUS, &peStatus); + localDataPoolMap.emplace(mpsoc::poolid::MODE, &peMode); + localDataPoolMap.emplace(mpsoc::poolid::MODE, &peMode); + localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_PWR_ON, &peDownlinkPwrOn); + localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_JESD_SYNC_STATUS, &peDownlinkJesdSyncStatus); + localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_DAC_STATUS, &peDownlinkDacStatus); + localDataPoolMap.emplace(mpsoc::poolid::CAM_STATUS, &peCameraStatus); + localDataPoolMap.emplace(mpsoc::poolid::CAM_SDI_STATUS, &peCameraSdiStatus); + localDataPoolMap.emplace(mpsoc::poolid::CAM_FPGA_TEMP, &peCameraFpgaTemp); + localDataPoolMap.emplace(mpsoc::poolid::CAM_SOC_TEMP, &peCameraSocTemp); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_TEMP, &peSysmonTemp); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCINT, &peSysmonVccInt); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCAUX, &peSysmonVccAux); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCBRAM, &peSysmonVccBram); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCPAUX, &peSysmonVccPaux); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCPINT, &peSysmonVccPint); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCPDRO, &peSysmonVccPdro); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_MB12V, &peSysmonMb12V); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_MB3V3, &peSysmonMb3V3); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_MB1V8, &peSysmonMb1V8); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC12V, &peSysmonVcc12V); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC3V3, &peSysmonVcc3V3); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC3V3VA, &peSysmonVcc3V3VA); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC2V5DDR, &peSysmonVcc2V5DDR); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC1V2DDR, &peSysmonVcc1V2DDR); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC0V9, &peSysmonVcc0V9); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC0V6VTT, &peSysmonVcc0V6VTT); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_SAFE_COTS_CUR, &peSysmonSafeCotsCur); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_NVM4_XO_CUR, &peSysmonNvm4XoCur); + localDataPoolMap.emplace(mpsoc::poolid::SEM_UNCORRECTABLE_ERRS, &peSemUncorrectableErrs); + localDataPoolMap.emplace(mpsoc::poolid::SEM_CORRECTABLE_ERRS, &peSemCorrectableErrs); + localDataPoolMap.emplace(mpsoc::poolid::SEM_STATUS, &peSemStatus); + localDataPoolMap.emplace(mpsoc::poolid::REBOOT_MPSOC_REQUIRED, &peRebootMpsocRequired); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(hkReport.getSid(), false, 10.0)); return returnvalue::OK; } @@ -761,7 +797,6 @@ ReturnValue_t PlocMPSoCHandler::handleGetHkReport(const uint8_t* data) { } ReturnValue_t PlocMPSoCHandler::handleCamCmdRpt(const uint8_t* data) { - ReturnValue_t result = returnvalue::OK; ReturnValue_t result = verifyPacket(data, foundPacketLen); if (result == MPSoCReturnValuesIF::CRC_FAILURE) { sif::warning << "PlocMPSoCHandler::handleCamCmdRpt: CRC failure" << std::endl; @@ -1143,6 +1178,13 @@ void PlocMPSoCHandler::handleActionCommandFailure(ActionId_t actionId) { return; } +LocalPoolDataSetBase* PlocMPSoCHandler::getDataSetHandle(sid_t sid) { + if (sid == hkReport.getSid()) { + return &hkReport; + } + return nullptr; +} + std::string PlocMPSoCHandler::getStatusString(uint16_t status) { switch (status) { case (mpsoc::status_code::UNKNOWN_APID): { diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index e11f05eb..c9850e18 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -77,7 +77,8 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { uint8_t expectedReplies = 1, bool useAlternateId = false, DeviceCommandId_t alternateReplyID = 0) override; size_t getNextReplyLength(DeviceCommandId_t deviceCommand) override; - virtual ReturnValue_t doSendReadHook() override; + ReturnValue_t doSendReadHook() override; + LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override; private: static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_MPSOC_HANDLER; @@ -105,6 +106,8 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { static const uint16_t PACKET_SEQUENCE_COUNT_MASK = 0x3FFF; static const uint8_t STATUS_OFFSET = 10; + mpsoc::HkReport hkReport; + MessageQueueIF* eventQueue = nullptr; MessageQueueIF* commandActionHelperQueue = nullptr; @@ -114,6 +117,41 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { SpacePacketCreator creator; ploc::SpTcParams spParams = ploc::SpTcParams(creator); + PoolEntry peStatus = PoolEntry(); + PoolEntry peMode = PoolEntry(); + PoolEntry peDownlinkPwrOn = PoolEntry(); + PoolEntry peDownlinkReplyActive = PoolEntry(); + PoolEntry peDownlinkJesdSyncStatus = PoolEntry(); + PoolEntry peDownlinkDacStatus = PoolEntry(); + PoolEntry peCameraStatus = PoolEntry(); + PoolEntry peCameraSdiStatus = PoolEntry(); + PoolEntry peCameraFpgaTemp = PoolEntry(); + PoolEntry peCameraSocTemp = PoolEntry(); + PoolEntry peSysmonTemp = PoolEntry(); + PoolEntry peSysmonVccInt = PoolEntry(); + PoolEntry peSysmonVccAux = PoolEntry(); + PoolEntry peSysmonVccBram = PoolEntry(); + PoolEntry peSysmonVccPaux = PoolEntry(); + PoolEntry peSysmonVccPint = PoolEntry(); + PoolEntry peSysmonVccPdro = PoolEntry(); + PoolEntry peSysmonMb12V = PoolEntry(); + PoolEntry peSysmonMb3V3 = PoolEntry(); + PoolEntry peSysmonMb1V8 = PoolEntry(); + PoolEntry peSysmonVcc12V = PoolEntry(); + PoolEntry peSysmonVcc5V = PoolEntry(); + PoolEntry peSysmonVcc3V3 = PoolEntry(); + PoolEntry peSysmonVcc3V3VA = PoolEntry(); + PoolEntry peSysmonVcc2V5DDR = PoolEntry(); + PoolEntry peSysmonVcc1V2DDR = PoolEntry(); + PoolEntry peSysmonVcc0V9 = PoolEntry(); + PoolEntry peSysmonVcc0V6VTT = PoolEntry(); + PoolEntry peSysmonSafeCotsCur = PoolEntry(); + PoolEntry peSysmonNvm4XoCur = PoolEntry(); + PoolEntry peSemUncorrectableErrs = PoolEntry(); + PoolEntry peSemCorrectableErrs = PoolEntry(); + PoolEntry peSemStatus = PoolEntry(); + PoolEntry peRebootMpsocRequired = PoolEntry(); + /** * This variable is used to store the id of the next reply to receive. This is necessary * because the PLOC sends as reply to each command at least one acknowledgment and execution diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index e6acf4bf..1ef72bf5 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -1,6 +1,7 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_PLOCMPSOCDEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_PLOCMPSOCDEFINITIONS_H_ +#include #include #include #include @@ -12,6 +13,47 @@ namespace mpsoc { +static constexpr uint32_t HK_SET_ID = 0; + +namespace poolid { +enum { + STATUS = 0, + MODE = 1, + DOWNLINK_PWR_ON = 2, + DOWNLINK_REPLY_ACTIIVE = 3, + DOWNLINK_JESD_SYNC_STATUS = 4, + DOWNLINK_DAC_STATUS = 5, + CAM_STATUS = 6, + CAM_SDI_STATUS = 7, + CAM_FPGA_TEMP = 8, + CAM_SOC_TEMP = 9, + SYSMON_TEMP = 10, + SYSMON_VCCINT = 11, + SYSMON_VCCAUX = 12, + SYSMON_VCCBRAM = 13, + SYSMON_VCCPAUX = 14, + SYSMON_VCCPINT = 15, + SYSMON_VCCPDRO = 16, + SYSMON_MB12V = 17, + SYSMON_MB3V3 = 18, + SYSMON_MB1V8 = 19, + SYSMON_VCC12V = 20, + SYSMON_VCC5V = 21, + SYSMON_VCC3V3 = 22, + SYSMON_VCC3V3VA = 23, + SYSMON_VCC2V5DDR = 24, + SYSMON_VCC1V2DDR = 25, + SYSMON_VCC0V9 = 26, + SYSMON_VCC0V6VTT = 27, + SYSMON_SAFE_COTS_CUR = 28, + SYSMON_NVM4_XO_CUR = 29, + SEM_UNCORRECTABLE_ERRS = 30, + SEM_CORRECTABLE_ERRS = 31, + SEM_STATUS = 32, + REBOOT_MPSOC_REQUIRED = 33, +}; +} + static const DeviceCommandId_t NONE = 0; static const DeviceCommandId_t TC_MEM_WRITE = 1; static const DeviceCommandId_t TC_MEM_READ = 2; @@ -788,6 +830,67 @@ class TcCamcmdSend : public TcBase { static const uint8_t CARRIAGE_RETURN = 0xD; }; +class HkReport : public StaticLocalDataSet<36> { + public: + HkReport(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, HK_SET_ID) {} + + HkReport(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, HK_SET_ID)) {} + + lp_var_t status = lp_var_t(sid.objectId, mpsoc::poolid::STATUS, this); + lp_var_t mode = lp_var_t(sid.objectId, mpsoc::poolid::MODE, this); + lp_var_t downlinkPwrOn = + lp_var_t(sid.objectId, mpsoc::poolid::DOWNLINK_PWR_ON, this); + lp_var_t downlinkReplyActive = + lp_var_t(sid.objectId, mpsoc::poolid::DOWNLINK_REPLY_ACTIIVE, this); + lp_var_t downlinkJesdSyncStatus = + lp_var_t(sid.objectId, mpsoc::poolid::DOWNLINK_JESD_SYNC_STATUS, this); + lp_var_t downlinkDacStatus = + lp_var_t(sid.objectId, mpsoc::poolid::DOWNLINK_DAC_STATUS, this); + lp_var_t camStatus = lp_var_t(sid.objectId, mpsoc::poolid::CAM_STATUS, this); + lp_var_t camSdiStatus = + lp_var_t(sid.objectId, mpsoc::poolid::CAM_SDI_STATUS, this); + lp_var_t camFpgaTemp = lp_var_t(sid.objectId, mpsoc::poolid::CAM_FPGA_TEMP, this); + lp_var_t camSocTemp = lp_var_t(sid.objectId, mpsoc::poolid::CAM_SOC_TEMP, this); + lp_var_t sysmonTemp = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_TEMP, this); + lp_var_t sysmonVccInt = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCINT, this); + lp_var_t sysmonVccAux = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCAUX, this); + lp_var_t sysmonVccBram = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCBRAM, this); + lp_var_t sysmonVccPaux = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCPAUX, this); + lp_var_t sysmonVccPint = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCPINT, this); + lp_var_t sysmonVccPdro = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCPDRO, this); + lp_var_t sysmonMb12V = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_MB12V, this); + lp_var_t sysmonMb3V3 = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_MB3V3, this); + lp_var_t sysmonMb1V8 = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_MB1V8, this); + lp_var_t sysmonVcc12V = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC12V, this); + lp_var_t sysmonVcc5V = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC5V, this); + lp_var_t sysmonVcc3V3 = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC3V3, this); + lp_var_t sysmonVcc3V3VA = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC3V3VA, this); + lp_var_t sysmonVcc2V5DDR = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC2V5DDR, this); + lp_var_t sysmonVcc1V2DDR = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC1V2DDR, this); + lp_var_t sysmonVcc0V9 = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC0V9, this); + lp_var_t sysmonVcc0V6VTT = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC0V6VTT, this); + lp_var_t sysmonSafeCotsCur = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_SAFE_COTS_CUR, this); + lp_var_t sysmonNvm4XoCur = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_NVM4_XO_CUR, this); + lp_var_t semUncorrectableErrs = + lp_var_t(sid.objectId, mpsoc::poolid::SEM_UNCORRECTABLE_ERRS, this); + lp_var_t semCorrectableErrs = + lp_var_t(sid.objectId, mpsoc::poolid::SEM_CORRECTABLE_ERRS, this); + lp_var_t semStatus = + lp_var_t(sid.objectId, mpsoc::poolid::SEM_CORRECTABLE_ERRS, this); + lp_var_t rebootMpsocRequired = + lp_var_t(sid.objectId, mpsoc::poolid::REBOOT_MPSOC_REQUIRED, this); +}; + } // namespace mpsoc #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_PLOCMPSOCDEFINITIONS_H_ */ diff --git a/mission/tcs/Tmp1075Definitions.h b/mission/tcs/Tmp1075Definitions.h index 946ac82e..345146e6 100644 --- a/mission/tcs/Tmp1075Definitions.h +++ b/mission/tcs/Tmp1075Definitions.h @@ -29,7 +29,7 @@ static const uint8_t MAX_REPLY_LENGTH = GET_TEMP_REPLY_SIZE; enum Tmp1075PoolIds : lp_id_t { TEMPERATURE_C_TMP1075 }; -class Tmp1075Dataset : public StaticLocalDataSet { +class Tmp1075Dataset : public StaticLocalDataSet<2> { public: Tmp1075Dataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, TMP1075_DATA_SET_ID) {} From d67414e829574bd90d52e6a6d85cea691726a56d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 13:26:47 +0200 Subject: [PATCH 117/506] bugfxi by marius --- linux/payload/PlocMpsocHandler.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index b0c56667..b9d97e90 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -428,7 +428,6 @@ ReturnValue_t PlocMPSoCHandler::initializeLocalDataPool(localpool::DataPool& loc LocalDataPoolManager& poolManager) { localDataPoolMap.emplace(mpsoc::poolid::STATUS, &peStatus); localDataPoolMap.emplace(mpsoc::poolid::MODE, &peMode); - localDataPoolMap.emplace(mpsoc::poolid::MODE, &peMode); localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_PWR_ON, &peDownlinkPwrOn); localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_JESD_SYNC_STATUS, &peDownlinkJesdSyncStatus); localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_DAC_STATUS, &peDownlinkDacStatus); From 66ca156ab30d71ee2e4a80d4fd8891de0218996d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 13:37:20 +0200 Subject: [PATCH 118/506] mpsoc update --- linux/payload/PlocMpsocHandler.cpp | 55 ++++++++++++++++-------------- 1 file changed, 29 insertions(+), 26 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index b9d97e90..c1cb4bde 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -822,6 +822,16 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator uint8_t enabledReplies = 0; + auto enableThreeReplies = [&](DeviceCommandId_t replyId) { + enabledReplies = 3; + result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, replyId); + if (result != returnvalue::OK) { + sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " + << mpsoc::TM_MEMORY_READ_REPORT << " not in replyMap" << std::endl; + return result; + } + break; + }; switch (command->first) { case mpsoc::TC_MEM_WRITE: case mpsoc::TC_FLASHDELETE: @@ -838,26 +848,16 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator case mpsoc::TC_MODE_SNAPSHOT: enabledReplies = 2; break; + case mpsoc::TC_GET_HK_REPORT: { + enableThreeReplies(mpsoc::TM_GET_HK_REPORT); + break; + } case mpsoc::TC_MEM_READ: { - enabledReplies = 3; - result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, - mpsoc::TM_MEMORY_READ_REPORT); - if (result != returnvalue::OK) { - sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " - << mpsoc::TM_MEMORY_READ_REPORT << " not in replyMap" << std::endl; - return result; - } + enableThreeReplies(mpsoc::TM_MEMORY_READ_REPORT); break; } case mpsoc::TC_CAM_CMD_SEND: { - enabledReplies = 3; - result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, - mpsoc::TM_CAM_CMD_RPT); - if (result != returnvalue::OK) { - sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " - << mpsoc::TM_CAM_CMD_RPT << " not in replyMap" << std::endl; - return result; - } + enableThreeReplies(mpsoc::TM_CAM_CMD_RPT); break; } case mpsoc::OBSW_RESET_SEQ_COUNT: @@ -1065,6 +1065,13 @@ void PlocMPSoCHandler::disableAllReplies() { DeviceCommandId_t commandId = getPendingCommand(); + auto disableCommandWithReply = [](DeviceCommandId_t replyId) { + iter = deviceReplyMap.find(replyId); + info = &(iter->second); + info->delayCycles = 0; + info->active = false; + info->command = deviceCommandMap.end(); + }; /* If the command expects a telemetry packet the appropriate tm reply will be disabled here */ switch (commandId) { case TC_MEM_WRITE: @@ -1082,19 +1089,15 @@ void PlocMPSoCHandler::disableAllReplies() { case TC_MODE_SNAPSHOT: break; case TC_MEM_READ: { - iter = deviceReplyMap.find(TM_MEMORY_READ_REPORT); - info = &(iter->second); - info->delayCycles = 0; - info->active = false; - info->command = deviceCommandMap.end(); + disableCommandWithReply(TM_MEMORY_READ_REPORT); + break; + } + case TC_GET_HK_REPORT: { + disableCommandWithReply(TM_GET_HK_REPORT); break; } case TC_CAM_CMD_SEND: { - iter = deviceReplyMap.find(TM_CAM_CMD_RPT); - info = &(iter->second); - info->delayCycles = 0; - info->active = false; - info->command = deviceCommandMap.end(); + disableCommandWithReply(TM_CAM_CMD_RPT); break; } default: { From 4a3af32a655c563846028203273af32300fd91a3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 21:28:46 +0200 Subject: [PATCH 119/506] MPSoC HK packet --- CHANGELOG.md | 1 + linux/payload/PlocMpsocHandler.cpp | 188 ++++++++++++++++++++++++++++- 2 files changed, 184 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index dc320982..7b0ecbe9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -33,6 +33,7 @@ will consitute of a breaking change warranting a new major release: CMake from selecting wrong cross-compilers if multiple cross-compilers with the same name are used on the same system. - Add ACS board for EM by default now. +- Add support for MPSoC HK packet. ## Added diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index c1cb4bde..e4c91a4a 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -792,6 +792,175 @@ ReturnValue_t PlocMPSoCHandler::handleGetHkReport(const uint8_t* data) { return result; } SpacePacketReader packetReader(data, foundPacketLen); + const uint8_t* dataStart = data + 6; + PoolReadGuard pg(&hkReport); + size_t deserLen = mpsoc::SIZE_TM_HK_REPORT; + SerializeIF::Endianness endianness = SerializeIF::Endianness::NETWORK; + result = SerializeAdapter::deSerialize(&hkReport.status.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.mode.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.downlinkPwrOn.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.downlinkReplyActive.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.downlinkJesdSyncStatus.value, &dataStart, + &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.downlinkDacStatus.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.camStatus.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.camSdiStatus.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.camFpgaTemp.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.sysmonTemp.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccInt.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccAux.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccBram.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccPaux.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccPint.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccPdro.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.sysmonMb12V.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.sysmonMb3V3.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.sysmonMb1V8.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc12V.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.sysmonVcc5V.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc3V3.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc3V3VA.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc2V5DDR.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc1V2DDR.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc0V9.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc0V6VTT.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonSafeCotsCur.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonNvm4XoCur.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.semUncorrectableErrs.value, &dataStart, + &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.semCorrectableErrs.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.semStatus.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + // Skip the weird filename + dataStart += 256; + result = SerializeAdapter::deSerialize(&hkReport.rebootMpsocRequired, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } return returnvalue::OK; } @@ -830,7 +999,7 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator << mpsoc::TM_MEMORY_READ_REPORT << " not in replyMap" << std::endl; return result; } - break; + return returnvalue::OK; }; switch (command->first) { case mpsoc::TC_MEM_WRITE: @@ -849,15 +1018,24 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator enabledReplies = 2; break; case mpsoc::TC_GET_HK_REPORT: { - enableThreeReplies(mpsoc::TM_GET_HK_REPORT); + result = enableThreeReplies(mpsoc::TM_GET_HK_REPORT); + if (result != returnvalue::OK) { + return result; + } break; } case mpsoc::TC_MEM_READ: { - enableThreeReplies(mpsoc::TM_MEMORY_READ_REPORT); + result = enableThreeReplies(mpsoc::TM_MEMORY_READ_REPORT); + if (result != returnvalue::OK) { + return result; + } break; } case mpsoc::TC_CAM_CMD_SEND: { - enableThreeReplies(mpsoc::TM_CAM_CMD_RPT); + result = enableThreeReplies(mpsoc::TM_CAM_CMD_RPT); + if (result != returnvalue::OK) { + return result; + } break; } case mpsoc::OBSW_RESET_SEQ_COUNT: @@ -1065,7 +1243,7 @@ void PlocMPSoCHandler::disableAllReplies() { DeviceCommandId_t commandId = getPendingCommand(); - auto disableCommandWithReply = [](DeviceCommandId_t replyId) { + auto disableCommandWithReply = [&](DeviceCommandId_t replyId) { iter = deviceReplyMap.find(replyId); info = &(iter->second); info->delayCycles = 0; From 2e00e10e093d6c5ffbd6d5fcdf6cf53ab75c1ad2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 14:19:15 +0200 Subject: [PATCH 120/506] request HK regularly now --- linux/payload/PlocMpsocHandler.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index e4c91a4a..2e708496 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -202,8 +202,10 @@ void PlocMPSoCHandler::doShutDown() { #endif } + ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { - return NOTHING_TO_SEND; + *id = mpsoc::TC_GET_HK_REPORT; + return buildCommandFromCommand(*id, nullptr, 0); } ReturnValue_t PlocMPSoCHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { From d4fc95ed9f73de645d75d63c8294278a510eac62 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 14:29:02 +0200 Subject: [PATCH 121/506] start adding dir content report --- linux/payload/plocMpscoDefs.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index 1ef72bf5..4df6037a 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -81,6 +81,8 @@ static const DeviceCommandId_t TC_DOWNLINK_DATA_MODULATE = 24; static const DeviceCommandId_t TC_MODE_SNAPSHOT = 25; static const DeviceCommandId_t TC_GET_HK_REPORT = 26; static const DeviceCommandId_t TM_GET_HK_REPORT = 27; +static const DeviceCommandId_t TC_FLASH_GET_DIRECTORY_CONTENT = 28; +static const DeviceCommandId_t TM_FLASH_DIRECTORY_CONTENT = 29; // Will reset the sequence count of the OBSW static const DeviceCommandId_t OBSW_RESET_SEQ_COUNT = 50; @@ -105,6 +107,7 @@ static const uint16_t TC_CAM_TAKE_PIC = 0x116; static const uint16_t TC_FLASHWRITE = 0x117; static const uint16_t TC_FLASHFOPEN = 0x119; static const uint16_t TC_FLASHFCLOSE = 0x11A; +static constexpr uint16_t TC_FLASH_GET_DIRECTORY_CONTENT = 0x11B; static const uint16_t TC_FLASHDELETE = 0x11C; static const uint16_t TC_MODE_IDLE = 0x11E; static const uint16_t TC_MODE_REPLAY = 0x11F; @@ -120,6 +123,7 @@ static const uint16_t ACK_SUCCESS = 0x400; static const uint16_t ACK_FAILURE = 0x401; static const uint16_t EXE_SUCCESS = 0x402; static const uint16_t EXE_FAILURE = 0x403; +static constexpr uint16_t TM_FLASH_DIRECTORY_CONTENT = 0x406; static const uint16_t TM_CAM_CMD_RPT = 0x407; } // namespace apid From 98a858876a58a0d7e04782a4aa6aa2c6d9e7c5fa Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 14:31:47 +0200 Subject: [PATCH 122/506] add some more commands --- linux/payload/PlocMpsocHandler.cpp | 1 - linux/payload/plocMpscoDefs.h | 4 +++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 2e708496..3c605d2d 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -202,7 +202,6 @@ void PlocMPSoCHandler::doShutDown() { #endif } - ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { *id = mpsoc::TC_GET_HK_REPORT; return buildCommandFromCommand(*id, nullptr, 0); diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index 4df6037a..c2c5d570 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -109,15 +109,17 @@ static const uint16_t TC_FLASHFOPEN = 0x119; static const uint16_t TC_FLASHFCLOSE = 0x11A; static constexpr uint16_t TC_FLASH_GET_DIRECTORY_CONTENT = 0x11B; static const uint16_t TC_FLASHDELETE = 0x11C; +static constexpr uint16_t TC_FLASH_CREATE_DIR = 0x11D; static const uint16_t TC_MODE_IDLE = 0x11E; static const uint16_t TC_MODE_REPLAY = 0x11F; static const uint16_t TC_MODE_SNAPSHOT = 0x120; static const uint16_t TC_DOWNLINK_DATA_MODULATE = 0x121; static constexpr uint16_t TC_HK_GET_REPORT = 0x123; -static constexpr uint16_t TM_HK_GET_REPORT = 0x408; static const uint16_t TC_DOWNLINK_PWR_OFF = 0x124; static const uint16_t TC_CAM_CMD_SEND = 0x12C; +static constexpr uint16_t TC_FLASH_COPY_FILE = 0x12E; static const uint16_t TC_SIMPLEX_SEND_FILE = 0x130; +static constexpr uint16_t TM_HK_GET_REPORT = 0x408; static const uint16_t TM_MEMORY_READ_REPORT = 0x404; static const uint16_t ACK_SUCCESS = 0x400; static const uint16_t ACK_FAILURE = 0x401; From 0fb07fd3d527461b836a9584b0962005c5f5c96b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 15:24:47 +0200 Subject: [PATCH 123/506] implement dir content report in OBSW --- linux/payload/PlocMpsocHandler.cpp | 83 +++++++++++++++++++++++------- linux/payload/PlocMpsocHandler.h | 3 +- linux/payload/plocMpscoDefs.h | 17 ++++++ 3 files changed, 83 insertions(+), 20 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 3c605d2d..0caebea3 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -253,6 +253,10 @@ ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t device result = prepareTcGetHkReport(); break; } + case (mpsoc::TC_FLASH_GET_DIRECTORY_CONTENT): { + result = prepareTcGetDirContent(commandData, commandDataLen); + break; + } case (mpsoc::TC_MODE_REPLAY): { result = prepareTcModeReplay(); break; @@ -314,14 +318,16 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() { this->insertInCommandMap(mpsoc::RELEASE_UART_TX); this->insertInCommandMap(mpsoc::SET_UART_TX_TRISTATE); this->insertInCommandMap(mpsoc::TC_CAM_TAKE_PIC); + this->insertInCommandMap(mpsoc::TC_FLASH_GET_DIRECTORY_CONTENT); this->insertInCommandMap(mpsoc::TC_SIMPLEX_SEND_FILE); this->insertInCommandMap(mpsoc::TC_DOWNLINK_DATA_MODULATE); this->insertInCommandMap(mpsoc::TC_MODE_SNAPSHOT); this->insertInReplyMap(mpsoc::ACK_REPORT, 3, nullptr, mpsoc::SIZE_ACK_REPORT); this->insertInReplyMap(mpsoc::EXE_REPORT, 3, nullptr, mpsoc::SIZE_EXE_REPORT); this->insertInReplyMap(mpsoc::TM_MEMORY_READ_REPORT, 2, nullptr, mpsoc::SIZE_TM_MEM_READ_REPORT); - this->insertInReplyMap(mpsoc::TM_GET_HK_REPORT, 2, nullptr, 0); + this->insertInReplyMap(mpsoc::TM_GET_HK_REPORT, 2, nullptr, mpsoc::SIZE_TM_HK_REPORT); this->insertInReplyMap(mpsoc::TM_CAM_CMD_RPT, 2, nullptr, mpsoc::SP_MAX_SIZE); + this->insertInReplyMap(mpsoc::TM_FLASH_DIRECTORY_CONTENT, 2, nullptr, 0); } ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remainingSize, @@ -339,6 +345,11 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain } uint16_t apid = spacePacket.getApid(); + auto handleDedicatedReply = [&](DeviceCommandId_t replyId) { + *foundLen = spacePacket.getFullPacketLen(); + foundPacketLen = *foundLen; + *foundId = replyId; + }; switch (apid) { case (mpsoc::apid::ACK_SUCCESS): *foundLen = mpsoc::SIZE_ACK_REPORT; @@ -353,14 +364,14 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain *foundId = mpsoc::TM_MEMORY_READ_REPORT; break; case (mpsoc::apid::TM_CAM_CMD_RPT): - *foundLen = spacePacket.getFullPacketLen(); - foundPacketLen = *foundLen; - *foundId = mpsoc::TM_CAM_CMD_RPT; + handleDedicatedReply(mpsoc::TM_CAM_CMD_RPT); break; case (mpsoc::apid::TM_HK_GET_REPORT): { - *foundLen = spacePacket.getFullPacketLen(); - foundPacketLen = *foundLen; - *foundId = mpsoc::TM_GET_HK_REPORT; + handleDedicatedReply(mpsoc::TM_GET_HK_REPORT); + break; + } + case (mpsoc::apid::TM_FLASH_DIRECTORY_CONTENT): { + handleDedicatedReply(mpsoc::TM_FLASH_DIRECTORY_CONTENT); break; } case (mpsoc::apid::EXE_SUCCESS): @@ -408,6 +419,18 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const result = handleCamCmdRpt(packet); break; } + case (mpsoc::TM_FLASH_DIRECTORY_CONTENT): { + result = verifyPacket(packet, foundPacketLen); + if (result == MPSoCReturnValuesIF::CRC_FAILURE) { + sif::warning << "PLOC MPSoC: Flash directory content reply invalid CRC" << std::endl; + } + /** Send data to commanding queue */ + handleDeviceTm(packet + mpsoc::DATA_FIELD_OFFSET, + foundPacketLen - mpsoc::DATA_FIELD_OFFSET - mpsoc::CRC_SIZE, + mpsoc::TM_FLASH_DIRECTORY_CONTENT); + nextReplyId = mpsoc::EXE_REPORT; + return result; + } case (mpsoc::EXE_REPORT): { result = handleExecutionReport(packet); break; @@ -421,7 +444,10 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const return result; } -void PlocMPSoCHandler::setNormalDatapoolEntriesInvalid() {} +void PlocMPSoCHandler::setNormalDatapoolEntriesInvalid() { + PoolReadGuard pg(&hkReport); + hkReport.setValidity(false, true); +} uint32_t PlocMPSoCHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } @@ -624,9 +650,8 @@ ReturnValue_t PlocMPSoCHandler::prepareTcCamCmdSend(const uint8_t* commandData, ReturnValue_t PlocMPSoCHandler::prepareTcCamTakePic(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = returnvalue::OK; mpsoc::TcCamTakePic tcCamTakePic(spParams, sequenceCount); - result = tcCamTakePic.buildPacket(commandData, commandDataLen); + ReturnValue_t result = tcCamTakePic.buildPacket(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } @@ -636,9 +661,8 @@ ReturnValue_t PlocMPSoCHandler::prepareTcCamTakePic(const uint8_t* commandData, ReturnValue_t PlocMPSoCHandler::prepareTcSimplexSendFile(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = returnvalue::OK; mpsoc::TcSimplexSendFile tcSimplexSendFile(spParams, sequenceCount); - result = tcSimplexSendFile.buildPacket(commandData, commandDataLen); + ReturnValue_t result = tcSimplexSendFile.buildPacket(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } @@ -646,11 +670,21 @@ ReturnValue_t PlocMPSoCHandler::prepareTcSimplexSendFile(const uint8_t* commandD return returnvalue::OK; } +ReturnValue_t PlocMPSoCHandler::prepareTcGetDirContent(const uint8_t* commandData, + size_t commandDataLen) { + mpsoc::TcGetDirContent tcGetDirContent(spParams, sequenceCount); + ReturnValue_t result = tcGetDirContent.buildPacket(commandData, commandDataLen); + if (result != returnvalue::OK) { + return result; + } + finishTcPrep(tcGetDirContent.getFullPacketLen()); + return returnvalue::OK; +} + ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkDataModulate(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = returnvalue::OK; mpsoc::TcDownlinkDataModulate tcDownlinkDataModulate(spParams, sequenceCount); - result = tcDownlinkDataModulate.buildPacket(commandData, commandDataLen); + ReturnValue_t result = tcDownlinkDataModulate.buildPacket(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } @@ -659,9 +693,8 @@ ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkDataModulate(const uint8_t* com } ReturnValue_t PlocMPSoCHandler::prepareTcModeSnapshot() { - ReturnValue_t result = returnvalue::OK; mpsoc::TcModeSnapshot tcModeSnapshot(spParams, sequenceCount); - result = tcModeSnapshot.buildPacket(); + ReturnValue_t result = tcModeSnapshot.buildPacket(); if (result != returnvalue::OK) { return result; } @@ -781,7 +814,7 @@ ReturnValue_t PlocMPSoCHandler::handleMemoryReadReport(const uint8_t* data) { uint16_t memLen = *(data + mpsoc::MEM_READ_RPT_LEN_OFFSET) << 8 | *(data + mpsoc::MEM_READ_RPT_LEN_OFFSET + 1); /** Send data to commanding queue */ - handleDeviceTM(data + mpsoc::DATA_FIELD_OFFSET, mpsoc::SIZE_MEM_READ_RPT_FIX + memLen * 4, + handleDeviceTm(data + mpsoc::DATA_FIELD_OFFSET, mpsoc::SIZE_MEM_READ_RPT_FIX + memLen * 4, mpsoc::TM_MEMORY_READ_REPORT); nextReplyId = mpsoc::EXE_REPORT; return result; @@ -980,7 +1013,7 @@ ReturnValue_t PlocMPSoCHandler::handleCamCmdRpt(const uint8_t* data) { sif::info << "PlocMPSoCHandler: CamCmdRpt Ack value: 0x" << std::hex << static_cast(ackValue) << std::endl; #endif /* OBSW_DEBUG_PLOC_MPSOC == 1 */ - handleDeviceTM(packetReader.getPacketData() + sizeof(uint16_t), + handleDeviceTm(packetReader.getPacketData() + sizeof(uint16_t), packetReader.getPacketDataLen() - 1, mpsoc::TM_CAM_CMD_RPT); return result; } @@ -1039,6 +1072,13 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator } break; } + case mpsoc::TC_FLASH_GET_DIRECTORY_CONTENT: { + result = enableThreeReplies(mpsoc::TM_FLASH_DIRECTORY_CONTENT); + if (result != returnvalue::OK) { + return result; + } + break; + } case mpsoc::OBSW_RESET_SEQ_COUNT: break; default: @@ -1105,6 +1145,7 @@ void PlocMPSoCHandler::setNextReplyId() { break; } } + size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) { size_t replyLen = 0; @@ -1206,7 +1247,7 @@ void PlocMPSoCHandler::completionFailedReceived(ActionId_t actionId, ReturnValue handleActionCommandFailure(actionId); } -void PlocMPSoCHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, +void PlocMPSoCHandler::handleDeviceTm(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) { ReturnValue_t result = returnvalue::OK; @@ -1275,6 +1316,10 @@ void PlocMPSoCHandler::disableAllReplies() { disableCommandWithReply(TM_GET_HK_REPORT); break; } + case TC_FLASH_GET_DIRECTORY_CONTENT: { + disableCommandWithReply(TM_FLASH_DIRECTORY_CONTENT); + break; + } case TC_CAM_CMD_SEND: { disableCommandWithReply(TM_CAM_CMD_RPT); break; diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index c9850e18..a6a866ef 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -201,6 +201,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { ReturnValue_t prepareTcDownlinkPwrOn(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcDownlinkPwrOff(); ReturnValue_t prepareTcGetHkReport(); + ReturnValue_t prepareTcGetDirContent(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcReplayWriteSequence(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcCamCmdSend(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcModeIdle(); @@ -266,7 +267,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { * @param dataSize Size of telemetry in bytes. * @param replyId Id of the reply. This will be added to the ActionMessage. */ - void handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId); + void handleDeviceTm(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId); /** * @brief In case an acknowledgment failure reply has been received this function disables diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index c2c5d570..5d89360c 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -623,6 +623,23 @@ class TcGetHkReport : public TcBase { : TcBase(params, apid::TC_HK_GET_REPORT, sequenceCount) {} }; +class TcGetDirContent : public TcBase { + public: + TcGetDirContent(ploc::SpTcParams params, uint16_t sequenceCount) + : TcBase(params, apid::TC_FLASH_GET_DIRECTORY_CONTENT, sequenceCount) {} + + ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) { + ReturnValue_t result = returnvalue::OK; + spParams.setFullPayloadLen(commandDataLen + sizeof(NULL_TERMINATOR) + CRC_SIZE); + if (result != returnvalue::OK) { + return result; + } + std::memcpy(payloadStart, commandData, commandDataLen); + *(payloadStart + commandDataLen) = NULL_TERMINATOR; + return result; + } +}; + /** * @brief Class to build replay stop space packet. */ From bacc46a8dce77c6d934022d5391d43984e3d5a55 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 15:30:30 +0200 Subject: [PATCH 124/506] i think that should do the job --- linux/payload/PlocMpsocHandler.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 0caebea3..ec2877a6 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -1170,6 +1170,10 @@ size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) { // report is not fixed replyLen = mpsoc::SP_MAX_SIZE; break; + case mpsoc::TM_FLASH_DIRECTORY_CONTENT: + // I think the reply size is not fixed either. + replyLen = mpsoc::SP_MAX_SIZE; + break; default: { replyLen = iter->second.replyLen; break; From dbdcf0b0d683f97b84ca193ddb03c07fecd97372 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 15:31:54 +0200 Subject: [PATCH 125/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7b0ecbe9..644d7684 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -34,6 +34,7 @@ will consitute of a breaking change warranting a new major release: on the same system. - Add ACS board for EM by default now. - Add support for MPSoC HK packet. +- Add support for MPSoC Flash Directory Content Report. ## Added From 92fd1548cbc1ab025ef266ea516cc3b92dfa3829 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 3 May 2023 16:42:24 +0200 Subject: [PATCH 126/506] dynamically enable/disable MPSoC --- linux/payload/PlocMpsocHandler.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index ec2877a6..96f6c50d 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -160,6 +160,7 @@ void PlocMPSoCHandler::doStartUp() { powerState = PowerState::BOOTING; break; case PowerState::ON: + hkReport.setReportingEnabled(true); setMode(_MODE_TO_ON); uartIsolatorSwitch.pullHigh(); break; @@ -168,11 +169,13 @@ void PlocMPSoCHandler::doStartUp() { } #else powerState = PowerState::ON; + hkReport.setReportingEnabled(true); setMode(_MODE_TO_ON); uartIsolatorSwitch.pullHigh(); #endif /* not MSPOC_JTAG_BOOT == 1 */ #else powerState = PowerState::ON; + hkReport.setReportingEnabled(true); setMode(_MODE_TO_ON); #endif /* XIPHOS_Q7S */ } @@ -188,6 +191,7 @@ void PlocMPSoCHandler::doShutDown() { break; case PowerState::OFF: sequenceCount = 0; + hkReport.setReportingEnabled(false); setMode(_MODE_POWER_DOWN); break; default: @@ -196,6 +200,7 @@ void PlocMPSoCHandler::doShutDown() { #else sequenceCount = 0; uartIsolatorSwitch.pullLow(); + hkReport.setReportingEnabled(false); setMode(_MODE_POWER_DOWN); powerState = PowerState::OFF; #endif From 50d5180076b7076a4664c43b0edfc556bd415467 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 21:29:15 +0200 Subject: [PATCH 127/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 644d7684..0f2f6ed0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -35,6 +35,7 @@ will consitute of a breaking change warranting a new major release: - Add ACS board for EM by default now. - Add support for MPSoC HK packet. - Add support for MPSoC Flash Directory Content Report. +- Dynamically enable and disable HK packets for MPSoC on `ON` and `OFF` commands. ## Added From 7e6c25901bb8d73cb23d74a13fe03d4eafeb6b8d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 09:13:02 +0200 Subject: [PATCH 128/506] Squashed commit of the following: MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit commit c137df64ab43e5b20fcf26c8821982fa71e43cff Merge: a919b3d1 4179e8e1 Author: Robin Mueller Date: Fri Apr 28 15:38:52 2023 +0200 Merge remote-tracking branch 'origin/main' into update_ptme_code commit 4179e8e124b3f9f425ac64b7df8b7e7395982dbf Merge: 26f5eff6 51f9d5e1 Author: Robin Müller Date: Fri Apr 28 15:35:54 2023 +0200 Merge pull request 'This bugfix might be super important' (#621) from possible_bugfix_dual_lane_assy into main Reviewed-on: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/621 Reviewed-by: Marius Eggert Reviewed-by: Steffen Gaisser commit 51f9d5e1fe8e71445ec15b833020c4e30b254e72 Merge: a17f57cb 26f5eff6 Author: Robin Müller Date: Fri Apr 28 15:35:10 2023 +0200 Merge branch 'main' into possible_bugfix_dual_lane_assy commit a17f57cbb587f960366baae7059309d18619cdc4 Author: Robin Mueller Date: Fri Apr 28 15:34:45 2023 +0200 changelog commit 26f5eff6d5957e4898f6d5a84200e77b1bd6250c Merge: 4074e084 8a0f13ba Author: Robin Müller Date: Fri Apr 28 15:32:03 2023 +0200 Merge pull request 'More System Modes' (#612) from more-system-modes into main Reviewed-on: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/612 commit 8a0f13bafbb995b724321a3936a674078d01882f Merge: 14baa356 4074e084 Author: Robin Müller Date: Fri Apr 28 15:31:51 2023 +0200 Merge branch 'main' into more-system-modes commit 14baa3563c3dc6499bac49bb83c08ba98bfad171 Author: Robin Mueller Date: Fri Apr 28 15:30:09 2023 +0200 hello commit 7045b6034afdb819f4978f91f697d7f23806ab89 Author: Robin Mueller Date: Fri Apr 28 15:18:47 2023 +0200 changelog commit 37b9615525637294118266910979236127f8a8ce Author: Robin Mueller Date: Fri Apr 28 15:18:16 2023 +0200 changelog commit 4074e0848012042565047726b8c526c6b5809a2c Merge: 862a4f26 38686ac3 Author: Robin Müller Date: Fri Apr 28 15:12:46 2023 +0200 Merge pull request 'Adaption for EM: Add P60 dock without ACU' (#620) from adaption_em_p60_dock_without_acu into main Reviewed-on: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/620 Reviewed-by: Marius Eggert commit 23796345d966787f6f43c1d89b9555413bfe583a Author: Robin Mueller Date: Fri Apr 28 15:04:30 2023 +0200 changelog and stop payload tracking commit b10275ca43f13ee68be013c7ece3b63acf762130 Author: Robin Mueller Date: Fri Apr 28 14:28:05 2023 +0200 changelog commit 383849c5cb62b1543037ab07c904b2725c91be13 Author: Robin Mueller Date: Fri Apr 28 14:25:14 2023 +0200 that is more robust commit c66cef9129586e8708a8f2679ff86b3c9977c5f5 Author: Robin Mueller Date: Fri Apr 28 14:23:21 2023 +0200 changelog commit 02ea8a7298ff14c8acab55b2f693e8593c277624 Author: Robin Mueller Date: Fri Apr 28 14:22:21 2023 +0200 changelog commit 38686ac3f621e50ea76be023520167841d2e2c19 Merge: 189a3126 74d5d709 Author: Robin Mueller Date: Fri Apr 28 14:04:03 2023 +0200 Merge branch 'possible_bugfix_dual_lane_assy' into adaption_em_p60_dock_without_acu commit 74d5d70973b46c70d1483077e71e2820d3ca4ad6 Author: Robin Mueller Date: Fri Apr 28 13:58:08 2023 +0200 this bugfix might be super important commit 48355e82635b071b9347b62f2bd1da636bf4eae5 Merge: 5691fe8e 862a4f26 Author: Robin Müller Date: Fri Apr 28 10:25:50 2023 +0200 Merge branch 'main' into more-system-modes commit 189a312628e561f54491ec238a2d2d5e2f211b57 Merge: a1279428 862a4f26 Author: Robin Mueller Date: Fri Apr 28 10:23:46 2023 +0200 Merge remote-tracking branch 'origin/main' into adaption_em_p60_dock_without_acu commit a12794281bfc2ebb27a02bbc151696023e69a9f0 Author: Robin Mueller Date: Fri Apr 28 10:20:31 2023 +0200 changelog commit fcaabb4e421b5f792ede490b6dd135ce661c9db5 Merge: 6c326489 4aafca64 Author: Robin Mueller Date: Fri Apr 28 10:20:02 2023 +0200 Merge remote-tracking branch 'origin/main' into adaption_em_p60_dock_without_acu commit 6c326489cbc59f2d70993542f05676472ca0102f Author: Robin Mueller Date: Fri Apr 28 10:18:46 2023 +0200 adapt EM SW: GS PCDU added, but use dummy for ACU commit 862a4f268510e04fc3cfb878b5eed9f0f9d62f05 Merge: 4aafca64 2daca272 Author: Robin Müller Date: Fri Apr 28 09:37:55 2023 +0200 Merge pull request 'Host SW bugfixes' (#618) from try_fix_host_obsw into main Reviewed-on: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/618 Reviewed-by: Marius Eggert commit 5691fe8e72a2e3b008d6130f31be32d9d7aee9b7 Merge: 097be17a 4aafca64 Author: meggert Date: Thu Apr 27 15:40:31 2023 +0200 Merge branch 'main' into more-system-modes commit 2daca272f8f0397125349c0f6c193e22d0a26383 Author: Robin Mueller Date: Thu Apr 27 11:29:18 2023 +0200 changelog commit 03762f962020b55188d445ee3dd429b6e1a32135 Author: Robin Mueller Date: Wed Apr 26 17:38:06 2023 +0200 lower live TM handler frequency commit a296f16e5ce9d803ce5db54d9602e396dad7ebce Author: Robin Mueller Date: Wed Apr 26 17:36:38 2023 +0200 host SW works properly again commit 83f07a6e16cbbf5fc4a9ccb2e7d92c3fdffb172b Author: Robin Mueller Date: Wed Apr 26 17:26:32 2023 +0200 configurable event manager queue depth commit 00dab64628a53d2109fceb5f9a4ffc765a72fab6 Merge: 641b8e84 4aafca64 Author: Robin Mueller Date: Wed Apr 26 17:03:35 2023 +0200 Merge remote-tracking branch 'origin/main' into try_fix_host_obsw commit 4aafca64a67589826f1578865c92fa76141e8531 Merge: f271242d 6901eae8 Author: Robin Müller Date: Wed Apr 26 17:03:01 2023 +0200 Merge pull request 'EM adaptions' (#619) from em_adaptions into main Reviewed-on: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/619 commit 6901eae81689981937e7eb49b98886f8c78b727b Merge: 7d630ebc f271242d Author: Robin Mueller Date: Wed Apr 26 17:02:24 2023 +0200 Merge remote-tracking branch 'origin/main' into em_adaptions commit 7d630ebcf3894a9a9896d1c3f05548d5ef230333 Author: Robin Mueller Date: Wed Apr 26 17:00:04 2023 +0200 EM adaptions commit 641b8e847d2b63ad11f28c4a37434a6161443690 Author: Robin Mueller Date: Wed Apr 26 16:41:40 2023 +0200 add back tm funnel handler for hosted build commit 13142686823bedee1a79ae5f851f124968822a54 Author: Robin Mueller Date: Wed Apr 26 16:23:50 2023 +0200 host build requires dedicated live TM task.. commit 4040304ef09a172f9d2bef42547d6b1effb8ca76 Author: Robin Mueller Date: Wed Apr 26 13:15:42 2023 +0200 this is annoying commit a919b3d1645d5c4a6e20903d973ce96424508cae Merge: e22f2a53 9672d6d6 Author: Robin Mueller Date: Wed Apr 26 11:11:30 2023 +0200 Merge remote-tracking branch 'origin/develop' into update_ptme_code commit 9672d6d6cca3ad7369bb4b06fd91a7f17a0f9db9 Author: Robin Mueller Date: Wed Apr 26 11:11:09 2023 +0200 changelog commit e22f2a53ea7388cc5c75c2024953aab293c8d277 Merge: b076e80b 0eb6b7cc Author: Robin Mueller Date: Wed Apr 26 11:05:58 2023 +0200 Merge remote-tracking branch 'origin/develop' into update_ptme_code commit b076e80b44e7ef2b2417def66ec1f1a684cfb228 Author: Robin Mueller Date: Wed Apr 26 11:04:10 2023 +0200 changelog commit 269aa6f7b0006125e9a516f8ecb6cdc32c91cbe5 Author: Robin Mueller Date: Wed Apr 26 11:03:42 2023 +0200 changelog commit caae2b4ba925747aa0ed1e2112582c6deff39d77 Author: Robin Mueller Date: Wed Apr 26 11:02:24 2023 +0200 update PTME code commit 097be17a2900e64021fcbb457c89d6d4756ab697 Author: meggert Date: Wed Apr 19 15:07:21 2023 +0200 added remaining acs modes as system modes --- CHANGELOG.md | 11 +++++++++ bsp_q7s/boardconfig/busConf.h | 6 ++--- bsp_q7s/core/ObjectFactory.cpp | 28 +++++++---------------- common/config/devices/gpioIds.h | 5 ---- linux/ipcore/PapbVcInterface.cpp | 39 +++++++++++++++----------------- linux/ipcore/PapbVcInterface.h | 11 ++++----- 6 files changed, 43 insertions(+), 57 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index dc320982..45bddbd4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,16 @@ will consitute of a breaking change warranting a new major release: # [v2.2.0] to be released +TODO: New firmware package version. + +## Changed + +- Removed PTME busy/ready signals. Those were not used anyway because register reads are used now. + +## Fixed + +- Important bugfixes for PTME. See `q7s-package` CHANGELOG. + # [v2.1.0] to be released ## Changed @@ -49,6 +59,7 @@ will consitute of a breaking change warranting a new major release: # [v2.0.5] 2023-05-11 + - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial submode (e.g. mode normal and submode dual side), it will transition back to the current mode, diff --git a/bsp_q7s/boardconfig/busConf.h b/bsp_q7s/boardconfig/busConf.h index 4fd15258..304113d2 100644 --- a/bsp_q7s/boardconfig/busConf.h +++ b/bsp_q7s/boardconfig/busConf.h @@ -82,14 +82,12 @@ static constexpr char EN_RW_4[] = "enable_rw_4"; static constexpr char RAD_SENSOR_CHIP_SELECT[] = "rad_sensor_chip_select"; static constexpr char ENABLE_RADFET[] = "enable_radfet"; -static constexpr char PAPB_BUSY_SIGNAL_VC0[] = "papb_busy_signal_vc0"; + static constexpr char PAPB_EMPTY_SIGNAL_VC0[] = "papb_empty_signal_vc0"; -static constexpr char PAPB_BUSY_SIGNAL_VC1[] = "papb_busy_signal_vc1"; static constexpr char PAPB_EMPTY_SIGNAL_VC1[] = "papb_empty_signal_vc1"; -static constexpr char PAPB_BUSY_SIGNAL_VC2[] = "papb_busy_signal_vc2"; static constexpr char PAPB_EMPTY_SIGNAL_VC2[] = "papb_empty_signal_vc2"; -static constexpr char PAPB_BUSY_SIGNAL_VC3[] = "papb_busy_signal_vc3"; static constexpr char PAPB_EMPTY_SIGNAL_VC3[] = "papb_empty_signal_vc3"; + static constexpr char PTME_RESETN[] = "ptme_resetn"; static constexpr char RS485_EN_TX_CLOCK[] = "tx_clock_enable_ltc2872"; diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 12e9177f..24ea22f8 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -729,20 +729,12 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(CcsdsComponentArgs& args) { // GPIO definitions of signals connected to the virtual channel interfaces of the PTME IP Core GpioCookie* gpioCookiePtmeIp = new GpioCookie; GpiodRegularByLineName* gpio = nullptr; - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC0, "PAPB VC0"); - gpioCookiePtmeIp->addGpio(gpioIds::VC0_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC0, "PAPB VC0"); gpioCookiePtmeIp->addGpio(gpioIds::VC0_PAPB_EMPTY, gpio); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC1, "PAPB VC1"); - gpioCookiePtmeIp->addGpio(gpioIds::VC1_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC1, "PAPB VC1"); gpioCookiePtmeIp->addGpio(gpioIds::VC1_PAPB_EMPTY, gpio); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC2, "PAPB VC2"); - gpioCookiePtmeIp->addGpio(gpioIds::VC2_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC2, "PAPB VC2"); gpioCookiePtmeIp->addGpio(gpioIds::VC2_PAPB_EMPTY, gpio); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC3, "PAPB VC3"); - gpioCookiePtmeIp->addGpio(gpioIds::VC3_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC3, "PAPB VC3"); gpioCookiePtmeIp->addGpio(gpioIds::VC3_PAPB_EMPTY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PTME_RESETN, "PTME RESETN", @@ -751,18 +743,14 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(CcsdsComponentArgs& args) { gpioChecker(args.gpioComIF.addGpios(gpioCookiePtmeIp), "PTME PAPB VCs"); // Creating virtual channel interfaces - VirtualChannelIF* vc0 = - new PapbVcInterface(&args.gpioComIF, gpioIds::VC0_PAPB_BUSY, gpioIds::VC0_PAPB_EMPTY, - q7s::UIO_PTME, q7s::uiomapids::PTME_VC0); - VirtualChannelIF* vc1 = - new PapbVcInterface(&args.gpioComIF, gpioIds::VC1_PAPB_BUSY, gpioIds::VC1_PAPB_EMPTY, - q7s::UIO_PTME, q7s::uiomapids::PTME_VC1); - VirtualChannelIF* vc2 = - new PapbVcInterface(&args.gpioComIF, gpioIds::VC2_PAPB_BUSY, gpioIds::VC2_PAPB_EMPTY, - q7s::UIO_PTME, q7s::uiomapids::PTME_VC2); - VirtualChannelIF* vc3 = - new PapbVcInterface(&args.gpioComIF, gpioIds::VC3_PAPB_BUSY, gpioIds::VC3_PAPB_EMPTY, - q7s::UIO_PTME, q7s::uiomapids::PTME_VC3); + VirtualChannelIF* vc0 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC0_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC0); + VirtualChannelIF* vc1 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC1_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC1); + VirtualChannelIF* vc2 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC2_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC2); + VirtualChannelIF* vc3 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC3_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC3); // Creating ptme object and adding virtual channel interfaces Ptme* ptme = new Ptme(objects::PTME); ptme->addVcInterface(ccsds::VC0, vc0); diff --git a/common/config/devices/gpioIds.h b/common/config/devices/gpioIds.h index 640f4ead..573327fa 100644 --- a/common/config/devices/gpioIds.h +++ b/common/config/devices/gpioIds.h @@ -93,15 +93,10 @@ enum gpioId_t { EN_RW_CS, SPI_MUX, - VC0_PAPB_EMPTY, - VC0_PAPB_BUSY, VC1_PAPB_EMPTY, - VC1_PAPB_BUSY, VC2_PAPB_EMPTY, - VC2_PAPB_BUSY, VC3_PAPB_EMPTY, - VC3_PAPB_BUSY, PTME_RESETN, PDEC_RESET, diff --git a/linux/ipcore/PapbVcInterface.cpp b/linux/ipcore/PapbVcInterface.cpp index 60968cc6..7a1a89e4 100644 --- a/linux/ipcore/PapbVcInterface.cpp +++ b/linux/ipcore/PapbVcInterface.cpp @@ -7,13 +7,9 @@ #include "fsfw/serviceinterface/ServiceInterface.h" -PapbVcInterface::PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbBusyId, - gpioId_t papbEmptyId, std::string uioFile, int mapNum) - : gpioComIF(gpioComIF), - papbBusyId(papbBusyId), - papbEmptyId(papbEmptyId), - uioFile(std::move(uioFile)), - mapNum(mapNum) {} +PapbVcInterface::PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbEmptyId, + std::string uioFile, int mapNum) + : gpioComIF(gpioComIF), papbEmptyId(papbEmptyId), uioFile(std::move(uioFile)), mapNum(mapNum) {} PapbVcInterface::~PapbVcInterface() {} @@ -99,7 +95,7 @@ void PapbVcInterface::startPacketTransfer(ByteWidthCfg initWidth) { void PapbVcInterface::completePacketTransfer() { *vcBaseReg = CONFIG_END; } ReturnValue_t PapbVcInterface::pollInterfaceReadiness(uint32_t maxPollRetries, - bool checkReadyState) const { + bool checkReadyForPacketState) const { uint32_t busyIdx = 0; nextDelay.tv_nsec = FIRST_DELAY_PAPB_POLLING_NS; @@ -108,13 +104,16 @@ ReturnValue_t PapbVcInterface::pollInterfaceReadiness(uint32_t maxPollRetries, // Bit 5, see PTME ptme_001_01-0-7-r2 Table 31. uint32_t reg = *vcBaseReg; bool busy = (reg >> 5) & 0b1; - bool ready = (reg >> 6) & 0b1; - if (not busy) { + bool readyForPacket = (reg >> 6) & 0b1; + if (checkReadyForPacketState) { + if (not busy and readyForPacket) { + return returnvalue::OK; + } else if (not busy and not readyForPacket) { + return PAPB_BUSY; + } + } else if (not busy) { return returnvalue::OK; } - if (checkReadyState and not ready) { - return PAPB_BUSY; - } busyIdx++; if (busyIdx >= maxPollRetries) { @@ -131,24 +130,22 @@ ReturnValue_t PapbVcInterface::pollInterfaceReadiness(uint32_t maxPollRetries, return returnvalue::OK; } -void PapbVcInterface::isVcInterfaceBufferEmpty() { +bool PapbVcInterface::isVcInterfaceBufferEmpty() { ReturnValue_t result = returnvalue::OK; gpio::Levels papbEmptyState = gpio::Levels::HIGH; result = gpioComIF->readGpio(papbEmptyId, papbEmptyState); if (result != returnvalue::OK) { - sif::warning << "PapbVcInterface::isVcInterfaceBufferEmpty: Failed to read papb empty signal" - << std::endl; - return; + sif::error << "PapbVcInterface::isVcInterfaceBufferEmpty: Failed to read papb empty signal" + << std::endl; + return true; } if (papbEmptyState == gpio::Levels::HIGH) { - sif::debug << "PapbVcInterface::isVcInterfaceBufferEmpty: Buffer is empty" << std::endl; - } else { - sif::debug << "PapbVcInterface::isVcInterfaceBufferEmpty: Buffer is not empty" << std::endl; + return true; } - return; + return false; } bool PapbVcInterface::isBusy() const { return pollInterfaceReadiness(0, true) == PAPB_BUSY; } diff --git a/linux/ipcore/PapbVcInterface.h b/linux/ipcore/PapbVcInterface.h index e54def5d..71dd143b 100644 --- a/linux/ipcore/PapbVcInterface.h +++ b/linux/ipcore/PapbVcInterface.h @@ -30,8 +30,7 @@ class PapbVcInterface : public VirtualChannelIF { * @param uioFile UIO file providing access to the PAPB bus * @param mapNum Map number of UIO map associated with this virtual channel */ - PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbBusyId, gpioId_t papbEmptyId, - std::string uioFile, int mapNum); + PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbEmptyId, std::string uioFile, int mapNum); virtual ~PapbVcInterface(); bool isBusy() const override; @@ -83,9 +82,6 @@ class PapbVcInterface : public VirtualChannelIF { static constexpr long int MAX_DELAY_PAPB_POLLING_NS = 40; LinuxLibgpioIF* gpioComIF = nullptr; - - /** Pulled to low when virtual channel not ready to receive data */ - gpioId_t papbBusyId = gpio::NO_GPIO; /** High when external buffer memory of virtual channel is empty */ gpioId_t papbEmptyId = gpio::NO_GPIO; @@ -120,13 +116,14 @@ class PapbVcInterface : public VirtualChannelIF { * * @return returnvalue::OK when ready to receive data else PAPB_BUSY. */ - inline ReturnValue_t pollInterfaceReadiness(uint32_t maxPollRetries, bool checkReadyState) const; + inline ReturnValue_t pollInterfaceReadiness(uint32_t maxPollRetries, + bool checkReadyForPacketState) const; /** * @brief This function can be used for debugging to check whether there are packets in * the packet buffer of the virtual channel or not. */ - void isVcInterfaceBufferEmpty(); + bool isVcInterfaceBufferEmpty(); /** * @brief This function sends a complete telemetry transfer frame data field (1105 bytes) From cbf3315e16737a6e8d388146b4c7acfd6426fe33 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 09:32:24 +0200 Subject: [PATCH 129/506] Various fixes and improvements --- bsp_q7s/core/ObjectFactory.cpp | 1 + linux/payload/PlocSupervisorHandler.cpp | 8 ++++++-- linux/payload/PlocSupervisorHandler.h | 9 ++++++++- tmtc | 2 +- 4 files changed, 16 insertions(+), 4 deletions(-) diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 12e9177f..8fcace4e 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -644,6 +644,7 @@ void ObjectFactory::createPayloadComponents(LinuxLibgpioIF* gpioComIF, PowerSwit auto* supvHandler = new PlocSupervisorHandler(objects::PLOC_SUPERVISOR_HANDLER, supervisorCookie, Gpio(gpioIds::ENABLE_SUPV_UART, gpioComIF), power::PDU1_CH6_PLOC_12V, *supvHelper); + supvHandler->setPowerSwitcher(&pwrSwitch); supvHandler->connectModeTreeParent(satsystem::payload::SUBSYSTEM); #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */ static_cast(consumer); diff --git a/linux/payload/PlocSupervisorHandler.cpp b/linux/payload/PlocSupervisorHandler.cpp index 06a4cf07..682b8020 100644 --- a/linux/payload/PlocSupervisorHandler.cpp +++ b/linux/payload/PlocSupervisorHandler.cpp @@ -151,7 +151,7 @@ void PlocSupervisorHandler::doStartUp() { } } } - if (startupState == StartupState::SET_TIME_EXECUTING) { + if (startupState == StartupState::TIME_WAS_SET) { startupState = StartupState::ON; } if (startupState == StartupState::ON) { @@ -176,7 +176,7 @@ ReturnValue_t PlocSupervisorHandler::buildNormalDeviceCommand(DeviceCommandId_t* ReturnValue_t PlocSupervisorHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { if (startupState == StartupState::SET_TIME) { *id = supv::SET_TIME_REF; - startupState = StartupState::SET_TIME_EXECUTING; + startupState = StartupState::WAIT_FOR_TIME_REPLY; return buildCommandFromCommand(*id, nullptr, 0); } return NOTHING_TO_SEND; @@ -1909,6 +1909,10 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionSuccessReport(ExecutionRepor case supv::SET_TIME_REF: { // We could only allow proper bootup when the time was set successfully, but // this makes debugging difficult. + + if (startupState == StartupState::WAIT_FOR_TIME_REPLY) { + startupState = StartupState::TIME_WAS_SET; + } break; } default: diff --git a/linux/payload/PlocSupervisorHandler.h b/linux/payload/PlocSupervisorHandler.h index 78c20205..3e5ac2a0 100644 --- a/linux/payload/PlocSupervisorHandler.h +++ b/linux/payload/PlocSupervisorHandler.h @@ -99,7 +99,14 @@ class PlocSupervisorHandler : public DeviceHandlerBase { static const uint32_t MRAM_DUMP_TIMEOUT = 60000; // 4 s static const uint32_t BOOT_TIMEOUT = 4000; - enum class StartupState : uint8_t { OFF, BOOTING, SET_TIME, SET_TIME_EXECUTING, ON }; + enum class StartupState : uint8_t { + OFF, + BOOTING, + SET_TIME, + WAIT_FOR_TIME_REPLY, + TIME_WAS_SET, + ON + }; static constexpr bool SET_TIME_DURING_BOOT = true; diff --git a/tmtc b/tmtc index 5fbd19bb..f090c3af 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 5fbd19bb6cca0790373a809d78f2307adca9d0c8 +Subproject commit f090c3af66d1a0b760344e80053d6e83895e661a From 0156533385abaa85e743d193677b949f8009184a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 09:35:58 +0200 Subject: [PATCH 130/506] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index dc320982..fea60176 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -46,6 +46,10 @@ will consitute of a breaking change warranting a new major release: 16505 type. - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. +- PLOC Supervisor handler now has a power switcher assigned to make PLOC power switching work + without an additional PCDU power switch command. +- The PLOC Supervisor handler now waits for the replies to the `SET_TIME` command to verify working + communication. # [v2.0.5] 2023-05-11 From 29eb0e736f7d22fa8d2931f07eb25eb9b0f314e4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 09:40:31 +0200 Subject: [PATCH 131/506] never add CAM switcher dummy --- bsp_q7s/em/emObjectFactory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 0311a468..36863b89 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -52,12 +52,12 @@ void ObjectFactory::produce(void* args) { // level components. dummy::DummyCfg dummyCfg; dummyCfg.addCoreCtrlCfg = false; + dummyCfg.addCamSwitcherDummy = false; #if OBSW_ADD_SYRLINKS == 1 dummyCfg.addSyrlinksDummies = false; #endif #if OBSW_ADD_PLOC_SUPERVISOR == 1 || OBSW_ADD_PLOC_MPSOC == 1 dummyCfg.addPlocDummies = false; - dummyCfg.addCamSwitcherDummy = false; #endif #if OBSW_ADD_GOMSPACE_PCDU == 1 dummyCfg.addPowerDummies = false; From 1820dae3d465929d6096cc0bcf52c7c377783b47 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 11:06:26 +0200 Subject: [PATCH 132/506] horrible --- linux/payload/PlocMpsocHandler.cpp | 30 +++++++++++++++++++++++------- linux/payload/plocMpscoDefs.h | 3 --- 2 files changed, 23 insertions(+), 10 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index cd405fa6..61b82f7f 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -209,8 +209,11 @@ void PlocMPSoCHandler::doShutDown() { ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { - *id = mpsoc::TC_GET_HK_REPORT; - return buildCommandFromCommand(*id, nullptr, 0); + if(getPendingCommand() == DeviceHandlerIF::NO_COMMAND_ID) { + *id = mpsoc::TC_GET_HK_REPORT; + return buildCommandFromCommand(*id, nullptr, 0); + } + return NOTHING_TO_SEND; } ReturnValue_t PlocMPSoCHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { @@ -256,6 +259,7 @@ ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t device break; } case (mpsoc::TC_GET_HK_REPORT): { + sif::debug << "getting HK report" << std::endl; result = prepareTcGetHkReport(); break; } @@ -326,13 +330,14 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() { this->insertInReplyMap(mpsoc::ACK_REPORT, 3, nullptr, mpsoc::SIZE_ACK_REPORT); this->insertInReplyMap(mpsoc::EXE_REPORT, 3, nullptr, mpsoc::SIZE_EXE_REPORT); this->insertInReplyMap(mpsoc::TM_MEMORY_READ_REPORT, 2, nullptr, mpsoc::SIZE_TM_MEM_READ_REPORT); - this->insertInReplyMap(mpsoc::TM_GET_HK_REPORT, 2, nullptr, 0); + this->insertInReplyMap(mpsoc::TM_GET_HK_REPORT, 5, nullptr, mpsoc::SIZE_TM_HK_REPORT); this->insertInReplyMap(mpsoc::TM_CAM_CMD_RPT, 2, nullptr, mpsoc::SP_MAX_SIZE); } ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { ReturnValue_t result = returnvalue::OK; + sif::debug << "remaining size: " << remainingSize << std::endl; SpacePacketReader spacePacket; spacePacket.setReadOnlyData(start, remainingSize); @@ -347,6 +352,7 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain switch (apid) { case (mpsoc::apid::ACK_SUCCESS): + sif::debug << "recv ack success" << std::endl; *foundLen = mpsoc::SIZE_ACK_REPORT; *foundId = mpsoc::ACK_REPORT; break; @@ -364,12 +370,14 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain *foundId = mpsoc::TM_CAM_CMD_RPT; break; case (mpsoc::apid::TM_HK_GET_REPORT): { + sif::debug << "recv hk report" << std::endl; *foundLen = spacePacket.getFullPacketLen(); foundPacketLen = *foundLen; *foundId = mpsoc::TM_GET_HK_REPORT; break; } case (mpsoc::apid::EXE_SUCCESS): + sif::debug << "recv exe success" << std::endl; *foundLen = mpsoc::SIZE_EXE_REPORT; *foundId = mpsoc::EXE_REPORT; break; @@ -427,7 +435,9 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const return result; } -void PlocMPSoCHandler::setNormalDatapoolEntriesInvalid() {} +void PlocMPSoCHandler::setNormalDatapoolEntriesInvalid() { + hkReport.setValidity(false, true); +} uint32_t PlocMPSoCHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } @@ -572,12 +582,12 @@ ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOff() { ReturnValue_t PlocMPSoCHandler::prepareTcGetHkReport() { ReturnValue_t result = returnvalue::OK; - mpsoc::TcGetHkReport tcDownlinkPwrOff(spParams, sequenceCount); - result = tcDownlinkPwrOff.buildPacket(); + mpsoc::TcGetHkReport tcGetHkReport(spParams, sequenceCount); + result = tcGetHkReport.buildPacket(); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcDownlinkPwrOff.getFullPacketLen()); + finishTcPrep(tcGetHkReport.getFullPacketLen()); return returnvalue::OK; } @@ -1105,12 +1115,17 @@ void PlocMPSoCHandler::setNextReplyId() { case mpsoc::TC_MEM_READ: nextReplyId = mpsoc::TM_MEMORY_READ_REPORT; break; + case mpsoc::TC_GET_HK_REPORT: { + nextReplyId = mpsoc::TM_GET_HK_REPORT; + break; + } default: /* If no telemetry is expected the next reply is always the execution report */ nextReplyId = mpsoc::EXE_REPORT; break; } } + size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) { size_t replyLen = 0; @@ -1136,6 +1151,7 @@ size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) { replyLen = mpsoc::SP_MAX_SIZE; break; default: { + sif::debug << "reply length " << iter->second.replyLen << std::endl; replyLen = iter->second.replyLen; break; } diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index 1ef72bf5..20d70d66 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -608,9 +608,6 @@ class TcDownlinkPwrOn : public TcBase { } }; -/** - * @brief Class to build replay stop space packet. - */ class TcGetHkReport : public TcBase { public: TcGetHkReport(ploc::SpTcParams params, uint16_t sequenceCount) From 42152ab711ce575ac51dac7e1eb55c3163d238f3 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 12 May 2023 11:07:14 +0200 Subject: [PATCH 133/506] fixed gps altitude fdir --- mission/controller/acs/SensorProcessing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 4cc15a16..b106baaa 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -573,7 +573,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong // Altitude FDIR if (gpsAltitude > gpsParameters->maximumFdirAltitude || - gpsAltitude < gpsParameters->maximumFdirAltitude) { + gpsAltitude < gpsParameters->minimumFdirAltitude) { altitude = gpsParameters->fdirAltitude; } else { altitude = gpsAltitude; From f71a363385b6b63e615c762bc9494d8f1ebac924 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 12 May 2023 11:08:41 +0200 Subject: [PATCH 134/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index fea60176..c33e49f5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -50,6 +50,7 @@ will consitute of a breaking change warranting a new major release: without an additional PCDU power switch command. - The PLOC Supervisor handler now waits for the replies to the `SET_TIME` command to verify working communication. +- Fixed correction for `GPS Altitude` in case the sensor data is out of the expected bonds. # [v2.0.5] 2023-05-11 From ba060be0d68df262f78120a32defbac6cd605977 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 11:44:39 +0200 Subject: [PATCH 135/506] some more minor improvements --- linux/payload/PlocMpsocHandler.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 61b82f7f..1e904cd0 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -259,7 +259,6 @@ ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t device break; } case (mpsoc::TC_GET_HK_REPORT): { - sif::debug << "getting HK report" << std::endl; result = prepareTcGetHkReport(); break; } @@ -581,9 +580,8 @@ ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOff() { } ReturnValue_t PlocMPSoCHandler::prepareTcGetHkReport() { - ReturnValue_t result = returnvalue::OK; mpsoc::TcGetHkReport tcGetHkReport(spParams, sequenceCount); - result = tcGetHkReport.buildPacket(); + ReturnValue_t result = tcGetHkReport.buildPacket(); if (result != returnvalue::OK) { return result; } From 66c9a5eea3ff86c7a89fbd0316c077c55467f175 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 11:55:33 +0200 Subject: [PATCH 136/506] some more fixes --- linux/payload/plocMpscoDefs.h | 12 +----------- tmtc | 2 +- 2 files changed, 2 insertions(+), 12 deletions(-) diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index 80ebefb1..c42e61df 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -621,15 +621,6 @@ class TcGetHkReport : public TcBase { : TcBase(params, apid::TC_HK_GET_REPORT, sequenceCount) {} }; -/** - * @brief Class to build replay stop space packet. - */ -class TcGetHkReport : public TcBase { - public: - TcGetHkReport(ploc::SpTcParams params, uint16_t sequenceCount) - : TcBase(params, apid::TC_HK_GET_REPORT, sequenceCount) {} -}; - class TcGetDirContent : public TcBase { public: TcGetDirContent(ploc::SpTcParams params, uint16_t sequenceCount) @@ -637,12 +628,11 @@ class TcGetDirContent : public TcBase { ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; - spParams.setFullPayloadLen(commandDataLen + sizeof(NULL_TERMINATOR) + CRC_SIZE); + spParams.setFullPayloadLen(commandDataLen + CRC_SIZE); if (result != returnvalue::OK) { return result; } std::memcpy(payloadStart, commandData, commandDataLen); - *(payloadStart + commandDataLen) = NULL_TERMINATOR; return result; } }; diff --git a/tmtc b/tmtc index f090c3af..87e5abe8 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit f090c3af66d1a0b760344e80053d6e83895e661a +Subproject commit 87e5abe8ebb6a33d36445d43bcb6674b313626f1 From 699fc9a861243cd428d29b4b9c9e47da8f5b2358 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 12:14:37 +0200 Subject: [PATCH 137/506] some HK corrections --- linux/payload/PlocMpsocHandler.cpp | 2 ++ linux/payload/PlocMpsocHandler.h | 2 +- tmtc | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 1e904cd0..9c2aee4a 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -445,6 +445,7 @@ ReturnValue_t PlocMPSoCHandler::initializeLocalDataPool(localpool::DataPool& loc localDataPoolMap.emplace(mpsoc::poolid::STATUS, &peStatus); localDataPoolMap.emplace(mpsoc::poolid::MODE, &peMode); localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_PWR_ON, &peDownlinkPwrOn); + localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_REPLY_ACTIIVE, &peDownlinkReplyActive); localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_JESD_SYNC_STATUS, &peDownlinkJesdSyncStatus); localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_DAC_STATUS, &peDownlinkDacStatus); localDataPoolMap.emplace(mpsoc::poolid::CAM_STATUS, &peCameraStatus); @@ -462,6 +463,7 @@ ReturnValue_t PlocMPSoCHandler::initializeLocalDataPool(localpool::DataPool& loc localDataPoolMap.emplace(mpsoc::poolid::SYSMON_MB3V3, &peSysmonMb3V3); localDataPoolMap.emplace(mpsoc::poolid::SYSMON_MB1V8, &peSysmonMb1V8); localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC12V, &peSysmonVcc12V); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC5V, &peSysmonVcc5V); localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC3V3, &peSysmonVcc3V3); localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC3V3VA, &peSysmonVcc3V3VA); localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC2V5DDR, &peSysmonVcc2V5DDR); diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index c9850e18..d814f086 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -117,7 +117,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { SpacePacketCreator creator; ploc::SpTcParams spParams = ploc::SpTcParams(creator); - PoolEntry peStatus = PoolEntry(); + PoolEntry peStatus = PoolEntry(); PoolEntry peMode = PoolEntry(); PoolEntry peDownlinkPwrOn = PoolEntry(); PoolEntry peDownlinkReplyActive = PoolEntry(); diff --git a/tmtc b/tmtc index f090c3af..87e5abe8 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit f090c3af66d1a0b760344e80053d6e83895e661a +Subproject commit 87e5abe8ebb6a33d36445d43bcb6674b313626f1 From 2b999e7fa7feaae8c1d7fc26254a421da47abfcf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 12:37:25 +0200 Subject: [PATCH 138/506] requires some wait cycles --- linux/payload/PlocMpsocHandler.cpp | 68 +++++++++++++++++------------- linux/payload/PlocMpsocHandler.h | 2 + linux/payload/plocMpscoDefs.h | 3 +- 3 files changed, 41 insertions(+), 32 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 9c2aee4a..227180b4 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -152,32 +152,45 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI } void PlocMPSoCHandler::doStartUp() { + if (startupState == StartupState::IDLE) { + startupState = StartupState::HW_INIT; + } + if (startupState == StartupState::HW_INIT) { #ifdef XIPHOS_Q7S #if not OBSW_MPSOC_JTAG_BOOT == 1 - switch (powerState) { - case PowerState::OFF: - commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC); - powerState = PowerState::BOOTING; - break; - case PowerState::ON: - hkReport.setReportingEnabled(true); - setMode(_MODE_TO_ON); - uartIsolatorSwitch.pullHigh(); - break; - default: - break; - } + switch (powerState) { + case PowerState::OFF: + commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC); + powerState = PowerState::BOOTING; + return; + case PowerState::ON: + uartIsolatorSwitch.pullHigh(); + startupState = StartupState::WAIT_CYCLES; + break; + default: + return; + } #else - powerState = PowerState::ON; - hkReport.setReportingEnabled(true); - setMode(_MODE_TO_ON); - uartIsolatorSwitch.pullHigh(); + uartIsolatorSwitch.pullHigh(); + startupState = StartupState::WAIT_CYCLE; #endif /* not MSPOC_JTAG_BOOT == 1 */ #else - powerState = PowerState::ON; - hkReport.setReportingEnabled(true); - setMode(_MODE_TO_ON); + startupState = StartupState::WAIT_CYCLES; + powerState = PowerState::ON; #endif /* XIPHOS_Q7S */ + } + if (startupState == StartupState::WAIT_CYCLES) { + waitCycles++; + if (waitCycles == 2) { + startupState = StartupState::DONE; + waitCycles = 0; + } + } + if (startupState == StartupState::DONE) { + setMode(_MODE_TO_ON); + hkReport.setReportingEnabled(true); + startupState = StartupState::IDLE; + } } void PlocMPSoCHandler::doShutDown() { @@ -205,11 +218,12 @@ void PlocMPSoCHandler::doShutDown() { powerState = PowerState::OFF; #endif #endif + startupState = StartupState::IDLE; } - ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { - if(getPendingCommand() == DeviceHandlerIF::NO_COMMAND_ID) { + // testCycles ++; + if (getPendingCommand() == DeviceHandlerIF::NO_COMMAND_ID) { *id = mpsoc::TC_GET_HK_REPORT; return buildCommandFromCommand(*id, nullptr, 0); } @@ -336,7 +350,6 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() { ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { ReturnValue_t result = returnvalue::OK; - sif::debug << "remaining size: " << remainingSize << std::endl; SpacePacketReader spacePacket; spacePacket.setReadOnlyData(start, remainingSize); @@ -351,7 +364,7 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain switch (apid) { case (mpsoc::apid::ACK_SUCCESS): - sif::debug << "recv ack success" << std::endl; + sif::debug << "recv ack success" << std::endl; *foundLen = mpsoc::SIZE_ACK_REPORT; *foundId = mpsoc::ACK_REPORT; break; @@ -369,14 +382,12 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain *foundId = mpsoc::TM_CAM_CMD_RPT; break; case (mpsoc::apid::TM_HK_GET_REPORT): { - sif::debug << "recv hk report" << std::endl; *foundLen = spacePacket.getFullPacketLen(); foundPacketLen = *foundLen; *foundId = mpsoc::TM_GET_HK_REPORT; break; } case (mpsoc::apid::EXE_SUCCESS): - sif::debug << "recv exe success" << std::endl; *foundLen = mpsoc::SIZE_EXE_REPORT; *foundId = mpsoc::EXE_REPORT; break; @@ -434,9 +445,7 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const return result; } -void PlocMPSoCHandler::setNormalDatapoolEntriesInvalid() { - hkReport.setValidity(false, true); -} +void PlocMPSoCHandler::setNormalDatapoolEntriesInvalid() { hkReport.setValidity(false, true); } uint32_t PlocMPSoCHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } @@ -1151,7 +1160,6 @@ size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) { replyLen = mpsoc::SP_MAX_SIZE; break; default: { - sif::debug << "reply length " << iter->second.replyLen << std::endl; replyLen = iter->second.replyLen; break; } diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index d814f086..2b6548ca 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -183,7 +183,9 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { size_t foundPacketLen = 0; TelemetryBuffer tmBuffer; + uint32_t waitCycles = 0; + enum class StartupState { IDLE, HW_INIT, WAIT_CYCLES, DONE } startupState; enum class PowerState { OFF, BOOTING, SHUTDOWN, ON }; PowerState powerState = PowerState::OFF; diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index 20d70d66..cd17d911 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -882,8 +882,7 @@ class HkReport : public StaticLocalDataSet<36> { lp_var_t(sid.objectId, mpsoc::poolid::SEM_UNCORRECTABLE_ERRS, this); lp_var_t semCorrectableErrs = lp_var_t(sid.objectId, mpsoc::poolid::SEM_CORRECTABLE_ERRS, this); - lp_var_t semStatus = - lp_var_t(sid.objectId, mpsoc::poolid::SEM_CORRECTABLE_ERRS, this); + lp_var_t semStatus = lp_var_t(sid.objectId, mpsoc::poolid::SEM_STATUS, this); lp_var_t rebootMpsocRequired = lp_var_t(sid.objectId, mpsoc::poolid::REBOOT_MPSOC_REQUIRED, this); }; From 9391949369ea5494e128ee317680b627cdfff832 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 12:39:54 +0200 Subject: [PATCH 139/506] speed up payload components a bit --- bsp_q7s/core/scheduling.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index f2cd8229..5866ce8c 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -366,7 +366,7 @@ void scheduling::initTasks() { #endif /* OBSW_ADD_PLOC_SUPERVISOR */ PeriodicTaskIF* plTask = factory->createPeriodicTask( - "PL_TASK", 25, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc, &RR_SCHEDULING); + "PL_TASK", 25, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.5, missedDeadlineFunc, &RR_SCHEDULING); plTask->addComponent(objects::CAM_SWITCHER); scheduling::addMpsocSupvHandlers(plTask); scheduling::scheduleScexDev(plTask); From 90d289f56eaa5cb3bdc412d532942d27e4122a62 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 13:23:17 +0200 Subject: [PATCH 140/506] works now --- linux/payload/PlocMpsocHandler.cpp | 19 +++++++++++++------ linux/payload/PlocMpsocHandler.h | 3 ++- tmtc | 2 +- 3 files changed, 16 insertions(+), 8 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 227180b4..ef92c796 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -172,16 +172,18 @@ void PlocMPSoCHandler::doStartUp() { } #else uartIsolatorSwitch.pullHigh(); - startupState = StartupState::WAIT_CYCLE; + startupState = StartupState::WAIT_CYCLES; #endif /* not MSPOC_JTAG_BOOT == 1 */ #else startupState = StartupState::WAIT_CYCLES; powerState = PowerState::ON; #endif /* XIPHOS_Q7S */ } + // Need to wait, MPSoC still not booted properly, requesting HK without these wait cycles does + // not work, no replies.. if (startupState == StartupState::WAIT_CYCLES) { waitCycles++; - if (waitCycles == 2) { + if (waitCycles >= 10) { startupState = StartupState::DONE; waitCycles = 0; } @@ -222,9 +224,9 @@ void PlocMPSoCHandler::doShutDown() { } ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { - // testCycles ++; - if (getPendingCommand() == DeviceHandlerIF::NO_COMMAND_ID) { + if (not normalCmdPending) { *id = mpsoc::TC_GET_HK_REPORT; + normalCmdPending = true; return buildCommandFromCommand(*id, nullptr, 0); } return NOTHING_TO_SEND; @@ -364,7 +366,6 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain switch (apid) { case (mpsoc::apid::ACK_SUCCESS): - sif::debug << "recv ack success" << std::endl; *foundLen = mpsoc::SIZE_ACK_REPORT; *foundId = mpsoc::ACK_REPORT; break; @@ -447,7 +448,7 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const void PlocMPSoCHandler::setNormalDatapoolEntriesInvalid() { hkReport.setValidity(false, true); } -uint32_t PlocMPSoCHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } +uint32_t PlocMPSoCHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 10000; } ReturnValue_t PlocMPSoCHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { @@ -767,9 +768,15 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { switch (apid) { case (mpsoc::apid::EXE_SUCCESS): { + if (normalCmdPending) { + normalCmdPending = false; + } break; } case (mpsoc::apid::EXE_FAILURE): { + if (normalCmdPending) { + normalCmdPending = false; + } // TODO: Interpretation of status field in execution report sif::warning << "PlocMPSoCHandler::handleExecutionReport: Received execution failure report" << std::endl; diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index 2b6548ca..d201aeaf 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -108,6 +108,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { mpsoc::HkReport hkReport; + bool normalCmdPending = false; MessageQueueIF* eventQueue = nullptr; MessageQueueIF* commandActionHelperQueue = nullptr; @@ -185,7 +186,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { TelemetryBuffer tmBuffer; uint32_t waitCycles = 0; - enum class StartupState { IDLE, HW_INIT, WAIT_CYCLES, DONE } startupState; + enum class StartupState { IDLE, HW_INIT, WAIT_CYCLES, DONE } startupState = StartupState::IDLE; enum class PowerState { OFF, BOOTING, SHUTDOWN, ON }; PowerState powerState = PowerState::OFF; diff --git a/tmtc b/tmtc index 87e5abe8..377e98b5 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 87e5abe8ebb6a33d36445d43bcb6674b313626f1 +Subproject commit 377e98b5c2da12f10cdd12b027548a8075fdcb58 From 9715612e4608c0d49aeb2e81748c15d1cc91c15e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 13:24:14 +0200 Subject: [PATCH 141/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6f435358..a5944e7c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -52,6 +52,8 @@ will consitute of a breaking change warranting a new major release: without an additional PCDU power switch command. - The PLOC Supervisor handler now waits for the replies to the `SET_TIME` command to verify working communication. +- The PLOC MPSoC now waits 10 cycles before going to on. These wait cycles are necessary because + the MPSoC is not ready to process commands without this additional boot time. # [v2.0.5] 2023-05-11 From a3b119fda67b0bd3e29f95330933f470d3c87719 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 13:26:15 +0200 Subject: [PATCH 142/506] update changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3580df9b..189be00e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -35,6 +35,7 @@ will consitute of a breaking change warranting a new major release: - Add ACS board for EM by default now. - Add support for MPSoC HK packet. - Dynamically enable and disable HK packets for MPSoC on `ON` and `OFF` commands. +- Increase frequency of payload handlers from 0.8 seconds to 0.5 seconds. ## Added From 1f9c9e2407afb26ef3f50725039903e1dc8aac75 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 15:52:47 +0200 Subject: [PATCH 143/506] horrible --- linux/payload/PlocMpsocHandler.cpp | 27 ++++++++++++++++++--------- linux/payload/plocMpscoDefs.h | 4 +++- tmtc | 2 +- 3 files changed, 22 insertions(+), 11 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 481c6b09..325e8ed7 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -224,11 +225,11 @@ void PlocMPSoCHandler::doShutDown() { } ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { - if (not normalCmdPending) { - *id = mpsoc::TC_GET_HK_REPORT; - normalCmdPending = true; - return buildCommandFromCommand(*id, nullptr, 0); - } + // if (not normalCmdPending) { + // *id = mpsoc::TC_GET_HK_REPORT; + // normalCmdPending = true; + // return buildCommandFromCommand(*id, nullptr, 0); + // } return NOTHING_TO_SEND; } @@ -352,13 +353,14 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() { this->insertInReplyMap(mpsoc::TM_MEMORY_READ_REPORT, 2, nullptr, mpsoc::SIZE_TM_MEM_READ_REPORT); this->insertInReplyMap(mpsoc::TM_GET_HK_REPORT, 5, nullptr, mpsoc::SIZE_TM_HK_REPORT); this->insertInReplyMap(mpsoc::TM_CAM_CMD_RPT, 2, nullptr, mpsoc::SP_MAX_SIZE); - this->insertInReplyMap(mpsoc::TM_FLASH_DIRECTORY_CONTENT, 2, nullptr, 0); + this->insertInReplyMap(mpsoc::TM_FLASH_DIRECTORY_CONTENT, 2, nullptr, mpsoc::SP_MAX_SIZE); } ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { ReturnValue_t result = returnvalue::OK; + sif::debug << "remainingSize: " << remainingSize << std::endl; SpacePacketReader spacePacket; spacePacket.setReadOnlyData(start, remainingSize); if (spacePacket.isNull()) { @@ -377,6 +379,7 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain }; switch (apid) { case (mpsoc::apid::ACK_SUCCESS): + sif::debug << "got ack" << std::endl; *foundLen = mpsoc::SIZE_ACK_REPORT; *foundId = mpsoc::ACK_REPORT; break; @@ -408,7 +411,8 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain *foundId = mpsoc::EXE_REPORT; break; default: { - sif::debug << "PlocMPSoCHandler::scanForReply: Reply has invalid apid" << std::endl; + sif::debug << "PlocMPSoCHandler::scanForReply: Reply has invalid APID 0x" << std::hex + << std::setfill('0') << std::setw(2) << apid << std::dec << std::endl; *foundLen = remainingSize; return MPSoCReturnValuesIF::INVALID_APID; } @@ -445,6 +449,7 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const break; } case (mpsoc::TM_FLASH_DIRECTORY_CONTENT): { + sif::debug << "received flash dir content packet" << std::endl; result = verifyPacket(packet, foundPacketLen); if (result == MPSoCReturnValuesIF::CRC_FAILURE) { sif::warning << "PLOC MPSoC: Flash directory content reply invalid CRC" << std::endl; @@ -1171,6 +1176,10 @@ void PlocMPSoCHandler::setNextReplyId() { case mpsoc::TC_MEM_READ: nextReplyId = mpsoc::TM_MEMORY_READ_REPORT; break; + case mpsoc::TC_FLASH_GET_DIRECTORY_CONTENT: { + nextReplyId = mpsoc::TM_FLASH_DIRECTORY_CONTENT; + break; + } case mpsoc::TC_GET_HK_REPORT: { nextReplyId = mpsoc::TM_GET_HK_REPORT; break; @@ -1403,12 +1412,12 @@ void PlocMPSoCHandler::disableExeReportReply() { } void PlocMPSoCHandler::printStatus(const uint8_t* data) { - uint16_t status = *(data + STATUS_OFFSET) << 8 | *(data + STATUS_OFFSET + 1); + uint16_t status = (*(data + STATUS_OFFSET) << 8) | *(data + STATUS_OFFSET + 1); sif::info << "Verification report status: " << getStatusString(status) << std::endl; } uint16_t PlocMPSoCHandler::getStatus(const uint8_t* data) { - return *(data + STATUS_OFFSET) << 8 | *(data + STATUS_OFFSET + 1); + return (*(data + STATUS_OFFSET) << 8) | *(data + STATUS_OFFSET + 1); } void PlocMPSoCHandler::handleActionCommandFailure(ActionId_t actionId) { diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index de123719..c5d53e6f 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -628,11 +628,13 @@ class TcGetDirContent : public TcBase { ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; - spParams.setFullPayloadLen(commandDataLen + CRC_SIZE); + // Yeah it needs to be 256.. even if the path is shorter. + spParams.setFullPayloadLen(256 + CRC_SIZE); if (result != returnvalue::OK) { return result; } std::memcpy(payloadStart, commandData, commandDataLen); + payloadStart[255] = '\0'; return result; } }; diff --git a/tmtc b/tmtc index 377e98b5..ef0adef0 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 377e98b5c2da12f10cdd12b027548a8075fdcb58 +Subproject commit ef0adef04aebf8aa0d673e14403b484bd1200d9c From 9887359e31592480b9e080e0606d47f045eb5be4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 16:09:08 +0200 Subject: [PATCH 144/506] finally receiving the content report --- linux/payload/PlocMpsocHandler.cpp | 2 -- tmtc | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 325e8ed7..23f27060 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -379,7 +379,6 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain }; switch (apid) { case (mpsoc::apid::ACK_SUCCESS): - sif::debug << "got ack" << std::endl; *foundLen = mpsoc::SIZE_ACK_REPORT; *foundId = mpsoc::ACK_REPORT; break; @@ -449,7 +448,6 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const break; } case (mpsoc::TM_FLASH_DIRECTORY_CONTENT): { - sif::debug << "received flash dir content packet" << std::endl; result = verifyPacket(packet, foundPacketLen); if (result == MPSoCReturnValuesIF::CRC_FAILURE) { sif::warning << "PLOC MPSoC: Flash directory content reply invalid CRC" << std::endl; diff --git a/tmtc b/tmtc index ef0adef0..e05a54b0 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit ef0adef04aebf8aa0d673e14403b484bd1200d9c +Subproject commit e05a54b076a9b34059bfa5baf783a7f134a91f09 From edda42cb6165f5e5fd2980de5de412058cffb03e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 16:25:59 +0200 Subject: [PATCH 145/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index e05a54b0..04bbe057 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit e05a54b076a9b34059bfa5baf783a7f134a91f09 +Subproject commit 04bbe057e7a0ca24b7d0a2d3c620d422ca15f59a From ac73e6d2c3a1e1198c9f2832ac19f688a2f615c7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 16:27:25 +0200 Subject: [PATCH 146/506] bump tmtc again --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 04bbe057..0c1bfc6f 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 04bbe057e7a0ca24b7d0a2d3c620d422ca15f59a +Subproject commit 0c1bfc6fd32e66fe0da13bebc4eeb3030ead13a9 From 3dc096c42b76bf5f47c7b592080972b8fe270d80 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 16:29:15 +0200 Subject: [PATCH 147/506] add back normal command --- linux/payload/PlocMpsocHandler.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 23f27060..8a00e307 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -225,11 +225,11 @@ void PlocMPSoCHandler::doShutDown() { } ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { - // if (not normalCmdPending) { - // *id = mpsoc::TC_GET_HK_REPORT; - // normalCmdPending = true; - // return buildCommandFromCommand(*id, nullptr, 0); - // } + if (not normalCmdPending) { + *id = mpsoc::TC_GET_HK_REPORT; + normalCmdPending = true; + return buildCommandFromCommand(*id, nullptr, 0); + } return NOTHING_TO_SEND; } From 4887dc9e6b13c2977366526d2e59fb5aa97ba2f6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 16:58:56 +0200 Subject: [PATCH 148/506] somethings still wrong --- linux/payload/PlocMpsocHandler.cpp | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 8a00e307..86589d21 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -360,7 +360,6 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain DeviceCommandId_t* foundId, size_t* foundLen) { ReturnValue_t result = returnvalue::OK; - sif::debug << "remainingSize: " << remainingSize << std::endl; SpacePacketReader spacePacket; spacePacket.setReadOnlyData(start, remainingSize); if (spacePacket.isNull()) { @@ -379,6 +378,7 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain }; switch (apid) { case (mpsoc::apid::ACK_SUCCESS): + sif::debug << "recv ack" << std::endl; *foundLen = mpsoc::SIZE_ACK_REPORT; *foundId = mpsoc::ACK_REPORT; break; @@ -394,6 +394,7 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain handleDedicatedReply(mpsoc::TM_CAM_CMD_RPT); break; case (mpsoc::apid::TM_HK_GET_REPORT): { + sif::debug << "recv hk report" << std::endl; handleDedicatedReply(mpsoc::TM_GET_HK_REPORT); break; } @@ -402,6 +403,7 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain break; } case (mpsoc::apid::EXE_SUCCESS): + sif::debug << "recv exe" << std::endl; *foundLen = mpsoc::SIZE_EXE_REPORT; *foundId = mpsoc::EXE_REPORT; break; @@ -802,17 +804,22 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { uint16_t apid = (*(data) << 8 | *(data + 1)) & APID_MASK; + auto cmdDoneHandler = [&]() { + if (normalCmdPending) { + normalCmdPending = false; + } + auto commandIter = deviceCommandMap.find(getPendingCommand()); + if (commandIter != deviceCommandMap.end()) { + commandIter->second.isExecuting = false; + } + disableAllReplies(); + }; switch (apid) { case (mpsoc::apid::EXE_SUCCESS): { - if (normalCmdPending) { - normalCmdPending = false; - } + cmdDoneHandler(); break; } case (mpsoc::apid::EXE_FAILURE): { - if (normalCmdPending) { - normalCmdPending = false; - } // TODO: Interpretation of status field in execution report sif::warning << "PlocMPSoCHandler::handleExecutionReport: Received execution failure report" << std::endl; @@ -825,8 +832,8 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { } printStatus(data); sendFailureReport(mpsoc::EXE_REPORT, MPSoCReturnValuesIF::RECEIVED_EXE_FAILURE); - disableExeReportReply(); result = IGNORE_REPLY_DATA; + cmdDoneHandler(); break; } default: { From 714e2d07e530f7a06c10c8a7bf7fade2617e47ef Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 17:13:51 +0200 Subject: [PATCH 149/506] and were at DHB debugging again --- linux/payload/PlocMpsocHandler.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 86589d21..484ce47c 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -225,9 +225,9 @@ void PlocMPSoCHandler::doShutDown() { } ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { - if (not normalCmdPending) { + if (not commandIsExecuting(mpsoc::TC_GET_HK_REPORT)) { *id = mpsoc::TC_GET_HK_REPORT; - normalCmdPending = true; + // normalCmdPending = true; return buildCommandFromCommand(*id, nullptr, 0); } return NOTHING_TO_SEND; @@ -424,6 +424,7 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain triggerEvent(MPSOC_HANDLER_SEQUENCE_COUNT_MISMATCH, sequenceCount, recvSeqCnt); sequenceCount = recvSeqCnt; } + sif::debug << "sequence count: " << sequenceCount.get() << std::endl; // This sequence count ping pong does not make any sense but it is how the MPSoC expects it. sequenceCount++; return result; @@ -804,19 +805,22 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { uint16_t apid = (*(data) << 8 | *(data + 1)) & APID_MASK; - auto cmdDoneHandler = [&]() { + auto cmdDoneHandler = [&](bool success) { if (normalCmdPending) { normalCmdPending = false; } auto commandIter = deviceCommandMap.find(getPendingCommand()); if (commandIter != deviceCommandMap.end()) { commandIter->second.isExecuting = false; + if (commandIter->second.sendReplyTo != MessageQueueIF::NO_QUEUE) { + actionHelper.finish(success, commandIter->second.sendReplyTo, getPendingCommand(), result); + } } disableAllReplies(); }; switch (apid) { case (mpsoc::apid::EXE_SUCCESS): { - cmdDoneHandler(); + cmdDoneHandler(true); break; } case (mpsoc::apid::EXE_FAILURE): { @@ -833,7 +837,7 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { printStatus(data); sendFailureReport(mpsoc::EXE_REPORT, MPSoCReturnValuesIF::RECEIVED_EXE_FAILURE); result = IGNORE_REPLY_DATA; - cmdDoneHandler(); + cmdDoneHandler(false); break; } default: { From a718d182fcd1fdedde402db0ef42c9b0e133d435 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 17:39:15 +0200 Subject: [PATCH 150/506] done --- linux/payload/PlocMpsocHandler.cpp | 12 +++++------- linux/payload/PlocMpsocHandler.h | 1 + 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 484ce47c..94fcb978 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -105,6 +105,7 @@ void PlocMPSoCHandler::performOperationHook() { ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size) { ReturnValue_t result = returnvalue::OK; + commandIsPending = true; switch (actionId) { case mpsoc::SET_UART_TX_TRISTATE: { uartIsolatorSwitch.pullLow(); @@ -225,9 +226,9 @@ void PlocMPSoCHandler::doShutDown() { } ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { - if (not commandIsExecuting(mpsoc::TC_GET_HK_REPORT)) { + if (not commandIsPending) { *id = mpsoc::TC_GET_HK_REPORT; - // normalCmdPending = true; + commandIsPending = true; return buildCommandFromCommand(*id, nullptr, 0); } return NOTHING_TO_SEND; @@ -378,7 +379,6 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain }; switch (apid) { case (mpsoc::apid::ACK_SUCCESS): - sif::debug << "recv ack" << std::endl; *foundLen = mpsoc::SIZE_ACK_REPORT; *foundId = mpsoc::ACK_REPORT; break; @@ -394,7 +394,6 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain handleDedicatedReply(mpsoc::TM_CAM_CMD_RPT); break; case (mpsoc::apid::TM_HK_GET_REPORT): { - sif::debug << "recv hk report" << std::endl; handleDedicatedReply(mpsoc::TM_GET_HK_REPORT); break; } @@ -403,7 +402,6 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain break; } case (mpsoc::apid::EXE_SUCCESS): - sif::debug << "recv exe" << std::endl; *foundLen = mpsoc::SIZE_EXE_REPORT; *foundId = mpsoc::EXE_REPORT; break; @@ -419,12 +417,11 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain } } - uint16_t recvSeqCnt = (*(start + 2) << 8 | *(start + 3)) & PACKET_SEQUENCE_COUNT_MASK; + uint16_t recvSeqCnt = ((*(start + 2) << 8) | *(start + 3)) & PACKET_SEQUENCE_COUNT_MASK; if (recvSeqCnt != sequenceCount) { triggerEvent(MPSOC_HANDLER_SEQUENCE_COUNT_MISMATCH, sequenceCount, recvSeqCnt); sequenceCount = recvSeqCnt; } - sif::debug << "sequence count: " << sequenceCount.get() << std::endl; // This sequence count ping pong does not make any sense but it is how the MPSoC expects it. sequenceCount++; return result; @@ -809,6 +806,7 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { if (normalCmdPending) { normalCmdPending = false; } + commandIsPending = false; auto commandIter = deviceCommandMap.find(getPendingCommand()); if (commandIter != deviceCommandMap.end()) { commandIter->second.isExecuting = false; diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index 654cded4..a6af8156 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -169,6 +169,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { // Used to block incoming commands when MPSoC helper class is currently executing a command bool plocMPSoCHelperExecuting = false; + bool commandIsPending = false; struct TmMemReadReport { static const uint8_t FIX_SIZE = 14; From edf792c4fe972364ed71570db312c604b85d9fad Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 May 2023 17:44:41 +0200 Subject: [PATCH 151/506] 8 wait cycles are also okay --- linux/payload/PlocMpsocHandler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 94fcb978..07f7759d 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -185,7 +185,7 @@ void PlocMPSoCHandler::doStartUp() { // not work, no replies.. if (startupState == StartupState::WAIT_CYCLES) { waitCycles++; - if (waitCycles >= 10) { + if (waitCycles >= 8) { startupState = StartupState::DONE; waitCycles = 0; } From 770697d5d06f55dfd0e293c88d45d86e5e59b366 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 10:16:34 +0200 Subject: [PATCH 152/506] forgot some git merge conflicts --- linux/payload/plocMpscoDefs.h | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index 3d74fe0d..27bef077 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -127,10 +127,7 @@ static const uint16_t ACK_FAILURE = 0x401; static const uint16_t EXE_SUCCESS = 0x402; static const uint16_t EXE_FAILURE = 0x403; static const uint16_t TM_MEMORY_READ_REPORT = 0x404; -<<<<<<< HEAD static const uint16_t TM_FLASH_READ_REPORT = 0x405; -======= ->>>>>>> origin/v2.1.0-dev static constexpr uint16_t TM_FLASH_DIRECTORY_CONTENT = 0x406; static const uint16_t TM_CAM_CMD_RPT = 0x407; static constexpr uint16_t TM_HK_GET_REPORT = 0x408; @@ -675,21 +672,13 @@ class TcGetDirContent : public TcBase { ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; -<<<<<<< HEAD - spParams.setFullPayloadLen(commandDataLen + sizeof(NULL_TERMINATOR) + CRC_SIZE); -======= // Yeah it needs to be 256.. even if the path is shorter. spParams.setFullPayloadLen(256 + CRC_SIZE); ->>>>>>> origin/v2.1.0-dev if (result != returnvalue::OK) { return result; } std::memcpy(payloadStart, commandData, commandDataLen); -<<<<<<< HEAD - *(payloadStart + commandDataLen) = NULL_TERMINATOR; -======= payloadStart[255] = '\0'; ->>>>>>> origin/v2.1.0-dev return result; } }; From 9fde16c9125338b7967d5d876b8a2f3a176168ea Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 10:29:15 +0200 Subject: [PATCH 153/506] disable missed deadline printing by default --- bsp_q7s/OBSWConfig.h.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index 51ed8828..1c0a4db8 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -72,7 +72,7 @@ // Can be used to switch device to NORMAL mode immediately #define OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP 0 -#define OBSW_PRINT_MISSED_DEADLINES 1 +#define OBSW_PRINT_MISSED_DEADLINES 0 #define OBSW_MPSOC_JTAG_BOOT 0 #define OBSW_STAR_TRACKER_GROUND_CONFIG @OBSW_STAR_TRACKER_GROUND_CONFIG@ From f099cd7688cfe028a6f3463cb89916e586a73241 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 13:25:53 +0200 Subject: [PATCH 154/506] seems to work fine --- linux/payload/PlocMpsocHandler.cpp | 44 +++++++++++++++++++----------- linux/payload/PlocMpsocHandler.h | 8 ++++-- 2 files changed, 34 insertions(+), 18 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 0c2ce279..a8634f6a 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -79,6 +79,11 @@ ReturnValue_t PlocMPSoCHandler::initialize() { } void PlocMPSoCHandler::performOperationHook() { + if (commandIsPending and cmdCountdown.hasTimedOut()) { + commandIsPending = false; + // TODO: Better returnvalue? + cmdDoneHandler(false, returnvalue::FAILED); + } EventMessage event; for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK; result = eventQueue->receiveMessage(&event)) { @@ -106,6 +111,7 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI const uint8_t* data, size_t size) { ReturnValue_t result = returnvalue::OK; commandIsPending = true; + cmdCountdown.resetTimer(); switch (actionId) { case mpsoc::SET_UART_TX_TRISTATE: { uartIsolatorSwitch.pullLow(); @@ -242,6 +248,7 @@ ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) if (not commandIsPending) { *id = mpsoc::TC_GET_HK_REPORT; commandIsPending = true; + cmdCountdown.resetTimer(); return buildCommandFromCommand(*id, nullptr, 0); } return NOTHING_TO_SEND; @@ -439,6 +446,7 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain } // This sequence count ping pong does not make any sense but it is how the MPSoC expects it. sequenceCount++; + return result; } @@ -817,23 +825,9 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { uint16_t apid = (*(data) << 8 | *(data + 1)) & APID_MASK; - auto cmdDoneHandler = [&](bool success) { - if (normalCmdPending) { - normalCmdPending = false; - } - commandIsPending = false; - auto commandIter = deviceCommandMap.find(getPendingCommand()); - if (commandIter != deviceCommandMap.end()) { - commandIter->second.isExecuting = false; - if (commandIter->second.sendReplyTo != MessageQueueIF::NO_QUEUE) { - actionHelper.finish(success, commandIter->second.sendReplyTo, getPendingCommand(), result); - } - } - disableAllReplies(); - }; switch (apid) { case (mpsoc::apid::EXE_SUCCESS): { - cmdDoneHandler(true); + cmdDoneHandler(true, result); break; } case (mpsoc::apid::EXE_FAILURE): { @@ -850,7 +844,7 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { printStatus(data); sendFailureReport(mpsoc::EXE_REPORT, MPSoCReturnValuesIF::RECEIVED_EXE_FAILURE); result = IGNORE_REPLY_DATA; - cmdDoneHandler(false); + cmdDoneHandler(false, MPSoCReturnValuesIF::RECEIVED_EXE_FAILURE); break; } default: { @@ -1482,6 +1476,24 @@ LocalPoolDataSetBase* PlocMPSoCHandler::getDataSetHandle(sid_t sid) { return nullptr; } +bool PlocMPSoCHandler::dontCheckQueue() { + // The TC and TMs need to be handled strictly sequentially, so while a command is pending, + // more specifically while replies are still expected, do not check the queue.s + return commandIsPending; +} + +void PlocMPSoCHandler::cmdDoneHandler(bool success, ReturnValue_t result) { + commandIsPending = false; + auto commandIter = deviceCommandMap.find(getPendingCommand()); + if (commandIter != deviceCommandMap.end()) { + commandIter->second.isExecuting = false; + if (commandIter->second.sendReplyTo != MessageQueueIF::NO_QUEUE) { + actionHelper.finish(success, commandIter->second.sendReplyTo, getPendingCommand(), result); + } + } + disableAllReplies(); +} + std::string PlocMPSoCHandler::getStatusString(uint16_t status) { switch (status) { case (mpsoc::status_code::UNKNOWN_APID): { diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index a6af8156..2a16d9fe 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -28,7 +28,8 @@ * @note The sequence count in the space packets must be incremented with each received and sent * packet otherwise the MPSoC will reply with an acknowledgment failure report. * - * @author J. Meier + * NOTE: This is not an example for a good device handler, DO NOT USE THIS AS A REFERENCE HANDLER. + * @author J. Meier, R. Mueller */ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { public: @@ -79,6 +80,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { size_t getNextReplyLength(DeviceCommandId_t deviceCommand) override; ReturnValue_t doSendReadHook() override; LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override; + bool dontCheckQueue() override; private: static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_MPSOC_HANDLER; @@ -108,7 +110,6 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { mpsoc::HkReport hkReport; - bool normalCmdPending = false; MessageQueueIF* eventQueue = nullptr; MessageQueueIF* commandActionHelperQueue = nullptr; @@ -177,6 +178,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { }; TmMemReadReport tmMemReadReport; + Countdown cmdCountdown = Countdown(10000); struct TelemetryBuffer { uint16_t length = 0; @@ -301,6 +303,8 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { uint16_t getStatus(const uint8_t* data); + void cmdDoneHandler(bool success, ReturnValue_t result); + void handleActionCommandFailure(ActionId_t actionId); std::string getStatusString(uint16_t status); From ae6b5b491b7d7bb54269f01739e5d0a3d4245f93 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 13:27:24 +0200 Subject: [PATCH 155/506] that should get the job done --- linux/payload/PlocMpsocHandler.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index a8634f6a..dacc423f 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -110,8 +110,6 @@ void PlocMPSoCHandler::performOperationHook() { ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size) { ReturnValue_t result = returnvalue::OK; - commandIsPending = true; - cmdCountdown.resetTimer(); switch (actionId) { case mpsoc::SET_UART_TX_TRISTATE: { uartIsolatorSwitch.pullLow(); @@ -169,6 +167,9 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI default: break; } + // For longer commands, do not set these. + commandIsPending = true; + cmdCountdown.resetTimer(); return DeviceHandlerBase::executeAction(actionId, commandedBy, data, size); } @@ -245,7 +246,7 @@ void PlocMPSoCHandler::doShutDown() { } ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { - if (not commandIsPending) { + if (not commandIsPending and not plocMPSoCHelperExecuting) { *id = mpsoc::TC_GET_HK_REPORT; commandIsPending = true; cmdCountdown.resetTimer(); From 178a2183a2acc2e2fb3d9e5e4edb54201cb21529 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 14:40:14 +0200 Subject: [PATCH 156/506] clean up error handlng code --- bsp_q7s/core/scheduling.cpp | 4 +- linux/payload/PlocMpsocHandler.cpp | 108 +++-------------------------- linux/payload/PlocMpsocHandler.h | 4 -- linux/payload/PlocMpsocHelper.cpp | 30 +++++--- linux/payload/PlocMpsocHelper.h | 2 +- linux/payload/plocMpscoDefs.h | 87 +++++++++++++++++++++++ tmtc | 2 +- 7 files changed, 121 insertions(+), 116 deletions(-) diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index 5866ce8c..71da5bdc 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -345,7 +345,6 @@ void scheduling::initTasks() { } #endif /* OBSW_ADD_STAR_TRACKER == 1 */ - // TODO: Use regular scheduler for this task #if OBSW_ADD_PLOC_MPSOC == 1 PeriodicTaskIF* mpsocHelperTask = factory->createPeriodicTask( "PLOC_MPSOC_HELPER", 0, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); @@ -472,6 +471,9 @@ void scheduling::initTasks() { #if OBSW_ADD_PLOC_SUPERVISOR == 1 supvHelperTask->startTask(); #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */ +#if OBSW_ADD_PLOC_MPSOC == 1 + mpsocHelperTask->startTask(); +#endif plTask->startTask(); #if OBSW_ADD_TEST_CODE == 1 diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index dacc423f..68dccd86 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -136,6 +136,7 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI if (result != returnvalue::OK) { return result; } + plocMPSoCHelper->setSequenceCount(&sequenceCount); result = plocMPSoCHelper->startFlashWrite(flashWritePusCmd.getObcFile(), flashWritePusCmd.getMPSoCFile()); if (result != returnvalue::OK) { @@ -150,6 +151,9 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI if (result != returnvalue::OK) { return result; } + sif::debug << "starting flash read" << std::endl; + sif::debug << "sequence count: " << sequenceCount.get() << std::endl; + plocMPSoCHelper->setSequenceCount(&sequenceCount); result = plocMPSoCHelper->startFlashRead(flashReadPusCmd.getObcFile(), flashReadPusCmd.getMPSoCFile(), flashReadPusCmd.getReadSize()); @@ -158,7 +162,6 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI } plocMPSoCHelperExecuting = true; return EXECUTION_FINISHED; - break; } case (mpsoc::OBSW_RESET_SEQ_COUNT): { sequenceCount = 0; @@ -790,7 +793,7 @@ ReturnValue_t PlocMPSoCHandler::handleAckReport(const uint8_t* data) { sif::debug << "PlocMPSoCHandler::handleAckReport: Received Ack failure report" << std::endl; DeviceCommandId_t commandId = getPendingCommand(); uint16_t status = getStatus(data); - printStatus(data); + sif::warning << "Verification report status: " << mpsoc::getStatusString(status) << std::endl; if (commandId != DeviceHandlerIF::NO_COMMAND_ID) { triggerEvent(ACK_FAILURE, commandId, status); } @@ -836,13 +839,12 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { sif::warning << "PlocMPSoCHandler::handleExecutionReport: Received execution failure report" << std::endl; DeviceCommandId_t commandId = getPendingCommand(); - if (commandId != DeviceHandlerIF::NO_COMMAND_ID) { - uint16_t status = getStatus(data); - triggerEvent(EXE_FAILURE, commandId, status); - } else { + if (commandId == DeviceHandlerIF::NO_COMMAND_ID) { sif::debug << "PlocMPSoCHandler::handleExecutionReport: Unknown command id" << std::endl; } - printStatus(data); + uint16_t status = getStatus(data); + sif::warning << "Verification report status: " << mpsoc::getStatusString(status) << std::endl; + triggerEvent(EXE_FAILURE, commandId, status); sendFailureReport(mpsoc::EXE_REPORT, MPSoCReturnValuesIF::RECEIVED_EXE_FAILURE); result = IGNORE_REPLY_DATA; cmdDoneHandler(false, MPSoCReturnValuesIF::RECEIVED_EXE_FAILURE); @@ -1428,11 +1430,6 @@ void PlocMPSoCHandler::disableExeReportReply() { info->command->second.expectedReplies = 0; } -void PlocMPSoCHandler::printStatus(const uint8_t* data) { - uint16_t status = (*(data + STATUS_OFFSET) << 8) | *(data + STATUS_OFFSET + 1); - sif::info << "Verification report status: " << getStatusString(status) << std::endl; -} - uint16_t PlocMPSoCHandler::getStatus(const uint8_t* data) { return (*(data + STATUS_OFFSET) << 8) | *(data + STATUS_OFFSET + 1); } @@ -1494,90 +1491,3 @@ void PlocMPSoCHandler::cmdDoneHandler(bool success, ReturnValue_t result) { } disableAllReplies(); } - -std::string PlocMPSoCHandler::getStatusString(uint16_t status) { - switch (status) { - case (mpsoc::status_code::UNKNOWN_APID): { - return "Unknown APID"; - break; - } - case (mpsoc::status_code::INCORRECT_LENGTH): { - return "Incorrect length"; - break; - } - case (mpsoc::status_code::INCORRECT_CRC): { - return "Incorrect crc"; - break; - } - case (mpsoc::status_code::INCORRECT_PKT_SEQ_CNT): { - return "Incorrect packet sequence count"; - break; - } - case (mpsoc::status_code::TC_NOT_ALLOWED_IN_MODE): { - return "TC not allowed in this mode"; - break; - } - case (mpsoc::status_code::TC_EXEUTION_DISABLED): { - return "TC execution disabled"; - break; - } - case (mpsoc::status_code::FLASH_MOUNT_FAILED): { - return "Flash mount failed"; - break; - } - case (mpsoc::status_code::FLASH_FILE_ALREADY_CLOSED): { - return "Flash file already closed"; - break; - } - case (mpsoc::status_code::FLASH_FILE_OPEN_FAILED): { - return "Flash file open failed"; - break; - } - case (mpsoc::status_code::FLASH_FILE_NOT_OPEN): { - return "Flash file not open"; - break; - } - case (mpsoc::status_code::FLASH_UNMOUNT_FAILED): { - return "Flash unmount failed"; - break; - } - case (mpsoc::status_code::HEAP_ALLOCATION_FAILED): { - return "Heap allocation failed"; - break; - } - case (mpsoc::status_code::INVALID_PARAMETER): { - return "Invalid parameter"; - break; - } - case (mpsoc::status_code::NOT_INITIALIZED): { - return "Not initialized"; - break; - } - case (mpsoc::status_code::REBOOT_IMMINENT): { - return "Reboot imminent"; - break; - } - case (mpsoc::status_code::CORRUPT_DATA): { - return "Corrupt data"; - break; - } - case (mpsoc::status_code::FLASH_CORRECTABLE_MISMATCH): { - return "Flash correctable mismatch"; - break; - } - case (mpsoc::status_code::FLASH_UNCORRECTABLE_MISMATCH): { - return "Flash uncorrectable mismatch"; - break; - } - case (mpsoc::status_code::DEFAULT_ERROR_CODE): { - return "Default error code"; - break; - } - default: - std::stringstream ss; - ss << "0x" << std::hex << status; - return ss.str(); - break; - } - return ""; -} diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index 2a16d9fe..dc6ebd99 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -297,8 +297,6 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { */ void disableExeReportReply(); - void printStatus(const uint8_t* data); - ReturnValue_t prepareTcModeReplay(); uint16_t getStatus(const uint8_t* data); @@ -306,8 +304,6 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { void cmdDoneHandler(bool success, ReturnValue_t result); void handleActionCommandFailure(ActionId_t actionId); - - std::string getStatusString(uint16_t status); }; #endif /* BSP_Q7S_DEVICES_PLOC_PLOCMPSOCHANDLER_H_ */ diff --git a/linux/payload/PlocMpsocHelper.cpp b/linux/payload/PlocMpsocHelper.cpp index 421ac83c..66e46c18 100644 --- a/linux/payload/PlocMpsocHelper.cpp +++ b/linux/payload/PlocMpsocHelper.cpp @@ -38,6 +38,7 @@ ReturnValue_t PlocMPSoCHelper::performOperation(uint8_t operationCode) { #endif switch (internalState) { case InternalState::IDLE: { + sif::debug << "ploc mpsoc helper idle" << std::endl; semaphore.acquire(); break; } @@ -84,32 +85,35 @@ void PlocMPSoCHelper::setSequenceCount(SourceSequenceCounter* sequenceCount_) { } ReturnValue_t PlocMPSoCHelper::startFlashWrite(std::string obcFile, std::string mpsocFile) { + if (internalState != InternalState::IDLE) { + return returnvalue::FAILED; + } ReturnValue_t result = startFlashReadOrWriteBase(std::move(obcFile), std::move(mpsocFile)); if (result != returnvalue::OK) { return result; } internalState = InternalState::FLASH_WRITE; - return result; + return semaphore.release(); } ReturnValue_t PlocMPSoCHelper::startFlashRead(std::string obcFile, std::string mpsocFile, size_t readFileSize) { + if (internalState != InternalState::IDLE) { + return returnvalue::FAILED; + } ReturnValue_t result = startFlashReadOrWriteBase(std::move(obcFile), std::move(mpsocFile)); if (result != returnvalue::OK) { return result; } flashReadAndWrite.totalReadSize = readFileSize; internalState = InternalState::FLASH_READ; - return result; + return semaphore.release(); } -ReturnValue_t PlocMPSoCHelper::resetHelper() { - ReturnValue_t result = returnvalue::OK; - semaphore.release(); +void PlocMPSoCHelper::resetHelper() { spParams.buf = commandBuffer; terminate = false; - result = uartComIF->flushUartRxBuffer(comCookie); - return result; + uartComIF->flushUartRxBuffer(comCookie); } void PlocMPSoCHelper::stopProcess() { terminate = true; } @@ -169,11 +173,13 @@ ReturnValue_t PlocMPSoCHelper::performFlashWrite() { } ReturnValue_t PlocMPSoCHelper::performFlashRead() { + sif::debug << "performing flash read" << std::endl; std::error_code e; std::ofstream ofile(flashReadAndWrite.obcFile, std::ios::trunc | std::ios::binary); if (ofile.bad()) { return returnvalue::FAILED; } + sif::debug << "Sequence count: " << sequenceCount->get() << std::endl; ReturnValue_t result = flashfopen(); if (result != returnvalue::OK) { std::filesystem::remove(flashReadAndWrite.obcFile, e); @@ -191,11 +197,13 @@ ReturnValue_t PlocMPSoCHelper::performFlashRead() { if (flashReadAndWrite.totalReadSize - readSoFar < mpsoc::MAX_FLASH_READ_DATA_SIZE) { nextReadSize = flashReadAndWrite.totalReadSize - readSoFar; } + sif::debug << "reading " << nextReadSize << " bytes from offset " << readSoFar << std::endl; if (ofile.bad() or not ofile.is_open()) { std::filesystem::remove(flashReadAndWrite.obcFile, e); flashfclose(); return FILE_READ_ERROR; } + (*sequenceCount)++; mpsoc::TcFlashRead flashReadRequest(spParams, *sequenceCount); result = flashReadRequest.buildPacket(nextReadSize); if (result != returnvalue::OK) { @@ -215,9 +223,9 @@ ReturnValue_t PlocMPSoCHelper::performFlashRead() { flashfclose(); return result; } - (*sequenceCount)++; readSoFar += nextReadSize; } + sif::debug << "read file done" << std::endl; return result; } @@ -439,7 +447,8 @@ ReturnValue_t PlocMPSoCHelper::startFlashReadOrWriteBase(std::string obcFile, flashReadAndWrite.obcFile = std::move(obcFile); flashReadAndWrite.mpsocFile = std::move(mpsocFile); - return resetHelper(); + resetHelper(); + return returnvalue::OK; } ReturnValue_t PlocMPSoCHelper::checkReceivedTm(SpTmReader& reader) { @@ -456,12 +465,13 @@ ReturnValue_t PlocMPSoCHelper::checkReceivedTm(SpTmReader& reader) { triggerEvent(MPSOC_TM_CRC_MISSMATCH, *sequenceCount); return result; } - (*sequenceCount)++; uint16_t recvSeqCnt = reader.getSequenceCount(); if (recvSeqCnt != *sequenceCount) { triggerEvent(MPSOC_HELPER_SEQ_CNT_MISMATCH, *sequenceCount, recvSeqCnt); *sequenceCount = recvSeqCnt; } + // This sequence count ping pong does not make any sense but it is how the MPSoC expects it. + (*sequenceCount)++; return returnvalue::OK; } diff --git a/linux/payload/PlocMpsocHelper.h b/linux/payload/PlocMpsocHelper.h index 63a40d0c..2595c811 100644 --- a/linux/payload/PlocMpsocHelper.h +++ b/linux/payload/PlocMpsocHelper.h @@ -172,7 +172,7 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { // Sequence count, must be set by Ploc MPSoC Handler SourceSequenceCounter* sequenceCount = nullptr; - ReturnValue_t resetHelper(); + void resetHelper(); ReturnValue_t performFlashWrite(); ReturnValue_t performFlashRead(); ReturnValue_t flashfopen(); diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h index 27bef077..4bb19e4d 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpscoDefs.h @@ -989,6 +989,93 @@ class HkReport : public StaticLocalDataSet<36> { lp_var_t(sid.objectId, mpsoc::poolid::REBOOT_MPSOC_REQUIRED, this); }; +const char* getStatusString(uint16_t status) { + switch (status) { + case (mpsoc::status_code::UNKNOWN_APID): { + return "Unknown APID"; + break; + } + case (mpsoc::status_code::INCORRECT_LENGTH): { + return "Incorrect length"; + break; + } + case (mpsoc::status_code::INCORRECT_CRC): { + return "Incorrect crc"; + break; + } + case (mpsoc::status_code::INCORRECT_PKT_SEQ_CNT): { + return "Incorrect packet sequence count"; + break; + } + case (mpsoc::status_code::TC_NOT_ALLOWED_IN_MODE): { + return "TC not allowed in this mode"; + break; + } + case (mpsoc::status_code::TC_EXEUTION_DISABLED): { + return "TC execution disabled"; + break; + } + case (mpsoc::status_code::FLASH_MOUNT_FAILED): { + return "Flash mount failed"; + break; + } + case (mpsoc::status_code::FLASH_FILE_ALREADY_CLOSED): { + return "Flash file already closed"; + break; + } + case (mpsoc::status_code::FLASH_FILE_OPEN_FAILED): { + return "Flash file open failed"; + break; + } + case (mpsoc::status_code::FLASH_FILE_NOT_OPEN): { + return "Flash file not open"; + break; + } + case (mpsoc::status_code::FLASH_UNMOUNT_FAILED): { + return "Flash unmount failed"; + break; + } + case (mpsoc::status_code::HEAP_ALLOCATION_FAILED): { + return "Heap allocation failed"; + break; + } + case (mpsoc::status_code::INVALID_PARAMETER): { + return "Invalid parameter"; + break; + } + case (mpsoc::status_code::NOT_INITIALIZED): { + return "Not initialized"; + break; + } + case (mpsoc::status_code::REBOOT_IMMINENT): { + return "Reboot imminent"; + break; + } + case (mpsoc::status_code::CORRUPT_DATA): { + return "Corrupt data"; + break; + } + case (mpsoc::status_code::FLASH_CORRECTABLE_MISMATCH): { + return "Flash correctable mismatch"; + break; + } + case (mpsoc::status_code::FLASH_UNCORRECTABLE_MISMATCH): { + return "Flash uncorrectable mismatch"; + break; + } + case (mpsoc::status_code::DEFAULT_ERROR_CODE): { + return "Default error code"; + break; + } + default: + std::stringstream ss; + ss << "0x" << std::hex << status; + return ss.str(); + break; + } + return ""; +} + } // namespace mpsoc #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_PLOCMPSOCDEFINITIONS_H_ */ diff --git a/tmtc b/tmtc index dd3e4c64..280c7243 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit dd3e4c649b687ea6b9444389439f3f2d5a558ad2 +Subproject commit 280c72439effa1b4290dc500dade2c62a9d6e3f7 From 17df79b0d60a052f07b636abc7b9c9e8286063c5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 15:15:58 +0200 Subject: [PATCH 157/506] this does not make sense --- bsp_q7s/core/ObjectFactory.cpp | 8 +- linux/payload/CMakeLists.txt | 3 +- linux/payload/PlocMpsocHandler.cpp | 132 +++++++++--------- linux/payload/PlocMpsocHandler.h | 17 +-- ...lper.cpp => PlocMpsocSpecialComHelper.cpp} | 89 ++++++------ ...ocHelper.h => PlocMpsocSpecialComHelper.h} | 14 +- linux/payload/plocMpsocHelpers.cpp | 91 ++++++++++++ .../{plocMpscoDefs.h => plocMpsocHelpers.h} | 117 +++------------- 8 files changed, 243 insertions(+), 228 deletions(-) rename linux/payload/{PlocMpsocHelper.cpp => PlocMpsocSpecialComHelper.cpp} (81%) rename linux/payload/{PlocMpsocHelper.h => PlocMpsocSpecialComHelper.h} (95%) create mode 100644 linux/payload/plocMpsocHelpers.cpp rename linux/payload/{plocMpscoDefs.h => plocMpsocHelpers.h} (92%) diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 8fcace4e..17b14b56 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -10,10 +10,10 @@ #include #include #include -#include +#include #include #include -#include +#include #include #include #include @@ -623,8 +623,8 @@ void ObjectFactory::createPayloadComponents(LinuxLibgpioIF* gpioComIF, PowerSwit new SerialCookie(objects::PLOC_MPSOC_HANDLER, q7s::UART_PLOC_MPSOC_DEV, serial::PLOC_MPSOC_BAUD, mpsoc::MAX_REPLY_SIZE, UartModes::NON_CANONICAL); mpsocCookie->setNoFixedSizeReply(); - auto plocMpsocHelper = new PlocMPSoCHelper(objects::PLOC_MPSOC_HELPER); - auto* mpsocHandler = new PlocMPSoCHandler( + auto plocMpsocHelper = new PlocMpsocSpecialComHelper(objects::PLOC_MPSOC_HELPER); + auto* mpsocHandler = new PlocMpsocHandler( objects::PLOC_MPSOC_HANDLER, objects::UART_COM_IF, mpsocCookie, plocMpsocHelper, Gpio(gpioIds::ENABLE_MPSOC_UART, gpioComIF), objects::PLOC_SUPERVISOR_HANDLER); mpsocHandler->connectModeTreeParent(satsystem::payload::SUBSYSTEM); diff --git a/linux/payload/CMakeLists.txt b/linux/payload/CMakeLists.txt index 7b2c4486..e697fac6 100644 --- a/linux/payload/CMakeLists.txt +++ b/linux/payload/CMakeLists.txt @@ -2,7 +2,8 @@ target_sources( ${OBSW_NAME} PUBLIC PlocMemoryDumper.cpp PlocMpsocHandler.cpp - PlocMpsocHelper.cpp + PlocMpsocSpecialComHelper.cpp + plocMpsocHelpers.cpp PlocSupervisorHandler.cpp PlocSupvUartMan.cpp ScexDleParser.cpp diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 68dccd86..a07c7871 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -8,8 +8,8 @@ #include "fsfw/datapool/PoolReadGuard.h" #include "fsfw/globalfunctions/CRC.h" -PlocMPSoCHandler::PlocMPSoCHandler(object_id_t objectId, object_id_t uartComIFid, - CookieIF* comCookie, PlocMPSoCHelper* plocMPSoCHelper, +PlocMpsocHandler::PlocMpsocHandler(object_id_t objectId, object_id_t uartComIFid, + CookieIF* comCookie, PlocMpsocSpecialComHelper* plocMPSoCHelper, Gpio uartIsolatorSwitch, object_id_t supervisorHandler) : DeviceHandlerBase(objectId, uartComIFid, comCookie), hkReport(this), @@ -27,9 +27,9 @@ PlocMPSoCHandler::PlocMPSoCHandler(object_id_t objectId, object_id_t uartComIFid spParams.buf = commandBuffer; } -PlocMPSoCHandler::~PlocMPSoCHandler() {} +PlocMpsocHandler::~PlocMpsocHandler() {} -ReturnValue_t PlocMPSoCHandler::initialize() { +ReturnValue_t PlocMpsocHandler::initialize() { ReturnValue_t result = returnvalue::OK; result = DeviceHandlerBase::initialize(); if (result != returnvalue::OK) { @@ -54,8 +54,8 @@ ReturnValue_t PlocMPSoCHandler::initialize() { return result; } result = manager->subscribeToEventRange( - eventQueue->getId(), event::getEventId(PlocMPSoCHelper::MPSOC_FLASH_WRITE_FAILED), - event::getEventId(PlocMPSoCHelper::MPSOC_FLASH_WRITE_SUCCESSFUL)); + eventQueue->getId(), event::getEventId(PlocMpsocSpecialComHelper::MPSOC_FLASH_WRITE_FAILED), + event::getEventId(PlocMpsocSpecialComHelper::MPSOC_FLASH_WRITE_SUCCESSFUL)); if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "PlocMPSoCHandler::initialize: Failed to subscribe to events from " @@ -78,7 +78,7 @@ ReturnValue_t PlocMPSoCHandler::initialize() { return result; } -void PlocMPSoCHandler::performOperationHook() { +void PlocMpsocHandler::performOperationHook() { if (commandIsPending and cmdCountdown.hasTimedOut()) { commandIsPending = false; // TODO: Better returnvalue? @@ -107,7 +107,7 @@ void PlocMPSoCHandler::performOperationHook() { } } -ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, +ReturnValue_t PlocMpsocHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size) { ReturnValue_t result = returnvalue::OK; switch (actionId) { @@ -176,7 +176,7 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI return DeviceHandlerBase::executeAction(actionId, commandedBy, data, size); } -void PlocMPSoCHandler::doStartUp() { +void PlocMpsocHandler::doStartUp() { if (startupState == StartupState::IDLE) { startupState = StartupState::HW_INIT; } @@ -220,7 +220,7 @@ void PlocMPSoCHandler::doStartUp() { } } -void PlocMPSoCHandler::doShutDown() { +void PlocMpsocHandler::doShutDown() { #ifdef XIPHOS_Q7S #if not OBSW_MPSOC_JTAG_BOOT == 1 switch (powerState) { @@ -248,7 +248,7 @@ void PlocMPSoCHandler::doShutDown() { startupState = StartupState::IDLE; } -ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { +ReturnValue_t PlocMpsocHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { if (not commandIsPending and not plocMPSoCHelperExecuting) { *id = mpsoc::TC_GET_HK_REPORT; commandIsPending = true; @@ -258,11 +258,11 @@ ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) return NOTHING_TO_SEND; } -ReturnValue_t PlocMPSoCHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { +ReturnValue_t PlocMpsocHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { return NOTHING_TO_SEND; } -ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, +ReturnValue_t PlocMpsocHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, size_t commandDataLen) { spParams.buf = commandBuffer; @@ -353,7 +353,7 @@ ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t device return result; } -void PlocMPSoCHandler::fillCommandAndReplyMap() { +void PlocMpsocHandler::fillCommandAndReplyMap() { this->insertInCommandMap(mpsoc::TC_MEM_WRITE); this->insertInCommandMap(mpsoc::TC_MEM_READ); this->insertInCommandMap(mpsoc::TC_FLASHDELETE); @@ -383,7 +383,7 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() { this->insertInReplyMap(mpsoc::TM_FLASH_DIRECTORY_CONTENT, 2, nullptr, mpsoc::SP_MAX_SIZE); } -ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remainingSize, +ReturnValue_t PlocMpsocHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { ReturnValue_t result = returnvalue::OK; @@ -454,7 +454,7 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain return result; } -ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { +ReturnValue_t PlocMpsocHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { ReturnValue_t result = returnvalue::OK; switch (id) { @@ -499,14 +499,14 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const return result; } -void PlocMPSoCHandler::setNormalDatapoolEntriesInvalid() { +void PlocMpsocHandler::setNormalDatapoolEntriesInvalid() { PoolReadGuard pg(&hkReport); hkReport.setValidity(false, true); } -uint32_t PlocMPSoCHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 10000; } +uint32_t PlocMpsocHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 10000; } -ReturnValue_t PlocMPSoCHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, +ReturnValue_t PlocMpsocHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { localDataPoolMap.emplace(mpsoc::poolid::STATUS, &peStatus); localDataPoolMap.emplace(mpsoc::poolid::MODE, &peMode); @@ -547,7 +547,7 @@ ReturnValue_t PlocMPSoCHandler::initializeLocalDataPool(localpool::DataPool& loc return returnvalue::OK; } -void PlocMPSoCHandler::handleEvent(EventMessage* eventMessage) { +void PlocMpsocHandler::handleEvent(EventMessage* eventMessage) { object_id_t objectId = eventMessage->getReporter(); switch (objectId) { case objects::PLOC_MPSOC_HELPER: { @@ -560,7 +560,7 @@ void PlocMPSoCHandler::handleEvent(EventMessage* eventMessage) { } } -ReturnValue_t PlocMPSoCHandler::prepareTcMemWrite(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcMemWrite(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; mpsoc::TcMemWrite tcMemWrite(spParams, sequenceCount); @@ -572,7 +572,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcMemWrite(const uint8_t* commandData, return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcMemRead(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcMemRead(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; mpsoc::TcMemRead tcMemRead(spParams, sequenceCount); @@ -585,7 +585,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcMemRead(const uint8_t* commandData, return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcFlashDelete(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcFlashDelete(const uint8_t* commandData, size_t commandDataLen) { if (commandDataLen > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) { return MPSoCReturnValuesIF::NAME_TOO_LONG; @@ -601,7 +601,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcFlashDelete(const uint8_t* commandData, return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcReplayStart(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcReplayStart(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; mpsoc::TcReplayStart tcReplayStart(spParams, sequenceCount); @@ -613,7 +613,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcReplayStart(const uint8_t* commandData, return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcReplayStop() { +ReturnValue_t PlocMpsocHandler::prepareTcReplayStop() { ReturnValue_t result = returnvalue::OK; mpsoc::TcReplayStop tcReplayStop(spParams, sequenceCount); result = tcReplayStop.buildPacket(); @@ -624,7 +624,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcReplayStop() { return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOn(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcDownlinkPwrOn(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; mpsoc::TcDownlinkPwrOn tcDownlinkPwrOn(spParams, sequenceCount); @@ -636,7 +636,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOn(const uint8_t* commandDat return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOff() { +ReturnValue_t PlocMpsocHandler::prepareTcDownlinkPwrOff() { ReturnValue_t result = returnvalue::OK; mpsoc::TcDownlinkPwrOff tcDownlinkPwrOff(spParams, sequenceCount); result = tcDownlinkPwrOff.buildPacket(); @@ -647,7 +647,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOff() { return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcGetHkReport() { +ReturnValue_t PlocMpsocHandler::prepareTcGetHkReport() { mpsoc::TcGetHkReport tcGetHkReport(spParams, sequenceCount); ReturnValue_t result = tcGetHkReport.buildPacket(); if (result != returnvalue::OK) { @@ -657,7 +657,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcGetHkReport() { return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcReplayWriteSequence(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcReplayWriteSequence(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; mpsoc::TcReplayWriteSeq tcReplayWriteSeq(spParams, sequenceCount); @@ -669,7 +669,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcReplayWriteSequence(const uint8_t* comm return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcModeReplay() { +ReturnValue_t PlocMpsocHandler::prepareTcModeReplay() { ReturnValue_t result = returnvalue::OK; mpsoc::TcModeReplay tcModeReplay(spParams, sequenceCount); result = tcModeReplay.buildPacket(); @@ -680,7 +680,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcModeReplay() { return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcModeIdle() { +ReturnValue_t PlocMpsocHandler::prepareTcModeIdle() { ReturnValue_t result = returnvalue::OK; mpsoc::TcModeIdle tcModeIdle(spParams, sequenceCount); result = tcModeIdle.buildPacket(); @@ -691,7 +691,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcModeIdle() { return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcCamCmdSend(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcCamCmdSend(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; mpsoc::TcCamcmdSend tcCamCmdSend(spParams, sequenceCount); @@ -704,7 +704,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcCamCmdSend(const uint8_t* commandData, return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcCamTakePic(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcCamTakePic(const uint8_t* commandData, size_t commandDataLen) { mpsoc::TcCamTakePic tcCamTakePic(spParams, sequenceCount); ReturnValue_t result = tcCamTakePic.buildPacket(commandData, commandDataLen); @@ -715,7 +715,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcCamTakePic(const uint8_t* commandData, return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcSimplexSendFile(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcSimplexSendFile(const uint8_t* commandData, size_t commandDataLen) { mpsoc::TcSimplexSendFile tcSimplexSendFile(spParams, sequenceCount); ReturnValue_t result = tcSimplexSendFile.buildPacket(commandData, commandDataLen); @@ -726,7 +726,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcSimplexSendFile(const uint8_t* commandD return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcGetDirContent(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcGetDirContent(const uint8_t* commandData, size_t commandDataLen) { mpsoc::TcGetDirContent tcGetDirContent(spParams, sequenceCount); ReturnValue_t result = tcGetDirContent.buildPacket(commandData, commandDataLen); @@ -737,7 +737,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcGetDirContent(const uint8_t* commandDat return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkDataModulate(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcDownlinkDataModulate(const uint8_t* commandData, size_t commandDataLen) { mpsoc::TcDownlinkDataModulate tcDownlinkDataModulate(spParams, sequenceCount); ReturnValue_t result = tcDownlinkDataModulate.buildPacket(commandData, commandDataLen); @@ -748,7 +748,7 @@ ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkDataModulate(const uint8_t* com return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcModeSnapshot() { +ReturnValue_t PlocMpsocHandler::prepareTcModeSnapshot() { mpsoc::TcModeSnapshot tcModeSnapshot(spParams, sequenceCount); ReturnValue_t result = tcModeSnapshot.buildPacket(); if (result != returnvalue::OK) { @@ -758,21 +758,21 @@ ReturnValue_t PlocMPSoCHandler::prepareTcModeSnapshot() { return returnvalue::OK; } -void PlocMPSoCHandler::finishTcPrep(size_t packetLen) { +void PlocMpsocHandler::finishTcPrep(size_t packetLen) { nextReplyId = mpsoc::ACK_REPORT; rawPacket = commandBuffer; rawPacketLen = packetLen; sequenceCount++; } -ReturnValue_t PlocMPSoCHandler::verifyPacket(const uint8_t* start, size_t foundLen) { +ReturnValue_t PlocMpsocHandler::verifyPacket(const uint8_t* start, size_t foundLen) { if (CRC::crc16ccitt(start, foundLen) != 0) { return MPSoCReturnValuesIF::CRC_FAILURE; } return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::handleAckReport(const uint8_t* data) { +ReturnValue_t PlocMpsocHandler::handleAckReport(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, mpsoc::SIZE_ACK_REPORT); @@ -792,7 +792,7 @@ ReturnValue_t PlocMPSoCHandler::handleAckReport(const uint8_t* data) { case mpsoc::apid::ACK_FAILURE: { sif::debug << "PlocMPSoCHandler::handleAckReport: Received Ack failure report" << std::endl; DeviceCommandId_t commandId = getPendingCommand(); - uint16_t status = getStatus(data); + uint16_t status = mpsoc::getStatusFromRawData(data); sif::warning << "Verification report status: " << mpsoc::getStatusString(status) << std::endl; if (commandId != DeviceHandlerIF::NO_COMMAND_ID) { triggerEvent(ACK_FAILURE, commandId, status); @@ -817,7 +817,7 @@ ReturnValue_t PlocMPSoCHandler::handleAckReport(const uint8_t* data) { return result; } -ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { +ReturnValue_t PlocMpsocHandler::handleExecutionReport(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, mpsoc::SIZE_EXE_REPORT); @@ -842,7 +842,7 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { if (commandId == DeviceHandlerIF::NO_COMMAND_ID) { sif::debug << "PlocMPSoCHandler::handleExecutionReport: Unknown command id" << std::endl; } - uint16_t status = getStatus(data); + uint16_t status = mpsoc::getStatusFromRawData(data); sif::warning << "Verification report status: " << mpsoc::getStatusString(status) << std::endl; triggerEvent(EXE_FAILURE, commandId, status); sendFailureReport(mpsoc::EXE_REPORT, MPSoCReturnValuesIF::RECEIVED_EXE_FAILURE); @@ -860,7 +860,7 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { return result; } -ReturnValue_t PlocMPSoCHandler::handleMemoryReadReport(const uint8_t* data) { +ReturnValue_t PlocMpsocHandler::handleMemoryReadReport(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, tmMemReadReport.rememberRequestedSize); if (result == MPSoCReturnValuesIF::CRC_FAILURE) { @@ -876,7 +876,7 @@ ReturnValue_t PlocMPSoCHandler::handleMemoryReadReport(const uint8_t* data) { return result; } -ReturnValue_t PlocMPSoCHandler::handleGetHkReport(const uint8_t* data) { +ReturnValue_t PlocMpsocHandler::handleGetHkReport(const uint8_t* data) { ReturnValue_t result = verifyPacket(data, foundPacketLen); if (result != returnvalue::OK) { return result; @@ -1054,7 +1054,7 @@ ReturnValue_t PlocMPSoCHandler::handleGetHkReport(const uint8_t* data) { return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::handleCamCmdRpt(const uint8_t* data) { +ReturnValue_t PlocMpsocHandler::handleCamCmdRpt(const uint8_t* data) { ReturnValue_t result = verifyPacket(data, foundPacketLen); if (result == MPSoCReturnValuesIF::CRC_FAILURE) { sif::warning << "PlocMPSoCHandler::handleCamCmdRpt: CRC failure" << std::endl; @@ -1074,7 +1074,7 @@ ReturnValue_t PlocMPSoCHandler::handleCamCmdRpt(const uint8_t* data) { return result; } -ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator command, +ReturnValue_t PlocMpsocHandler::enableReplyInReplyMap(DeviceCommandMap::iterator command, uint8_t expectedReplies, bool useAlternateId, DeviceCommandId_t alternateReplyID) { ReturnValue_t result = returnvalue::OK; @@ -1190,7 +1190,7 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator return returnvalue::OK; } -void PlocMPSoCHandler::setNextReplyId() { +void PlocMpsocHandler::setNextReplyId() { switch (getPendingCommand()) { case mpsoc::TC_MEM_READ: nextReplyId = mpsoc::TM_MEMORY_READ_REPORT; @@ -1210,7 +1210,7 @@ void PlocMPSoCHandler::setNextReplyId() { } } -size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) { +size_t PlocMpsocHandler::getNextReplyLength(DeviceCommandId_t commandId) { size_t replyLen = 0; if (nextReplyId == mpsoc::NONE) { @@ -1251,7 +1251,7 @@ size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) { return replyLen; } -ReturnValue_t PlocMPSoCHandler::doSendReadHook() { +ReturnValue_t PlocMpsocHandler::doSendReadHook() { // Prevent DHB from polling UART during commands executed by the mpsoc helper task if (plocMPSoCHelperExecuting) { return returnvalue::FAILED; @@ -1259,11 +1259,11 @@ ReturnValue_t PlocMPSoCHandler::doSendReadHook() { return returnvalue::OK; } -MessageQueueIF* PlocMPSoCHandler::getCommandQueuePtr() { return commandActionHelperQueue; } +MessageQueueIF* PlocMpsocHandler::getCommandQueuePtr() { return commandActionHelperQueue; } -void PlocMPSoCHandler::stepSuccessfulReceived(ActionId_t actionId, uint8_t step) { return; } +void PlocMpsocHandler::stepSuccessfulReceived(ActionId_t actionId, uint8_t step) { return; } -void PlocMPSoCHandler::stepFailedReceived(ActionId_t actionId, uint8_t step, +void PlocMpsocHandler::stepFailedReceived(ActionId_t actionId, uint8_t step, ReturnValue_t returnCode) { switch (actionId) { case supv::START_MPSOC: { @@ -1286,11 +1286,11 @@ void PlocMPSoCHandler::stepFailedReceived(ActionId_t actionId, uint8_t step, } } -void PlocMPSoCHandler::dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size) { +void PlocMpsocHandler::dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size) { return; } -void PlocMPSoCHandler::completionSuccessfulReceived(ActionId_t actionId) { +void PlocMpsocHandler::completionSuccessfulReceived(ActionId_t actionId) { if (actionId != supv::EXE_REPORT) { sif::debug << "PlocMPSoCHandler::completionSuccessfulReceived: Did not expect this action " << "ID" << std::endl; @@ -1311,11 +1311,11 @@ void PlocMPSoCHandler::completionSuccessfulReceived(ActionId_t actionId) { } } -void PlocMPSoCHandler::completionFailedReceived(ActionId_t actionId, ReturnValue_t returnCode) { +void PlocMpsocHandler::completionFailedReceived(ActionId_t actionId, ReturnValue_t returnCode) { handleActionCommandFailure(actionId); } -void PlocMPSoCHandler::handleDeviceTm(const uint8_t* data, size_t dataSize, +void PlocMpsocHandler::handleDeviceTm(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) { ReturnValue_t result = returnvalue::OK; @@ -1341,7 +1341,7 @@ void PlocMPSoCHandler::handleDeviceTm(const uint8_t* data, size_t dataSize, } } -void PlocMPSoCHandler::disableAllReplies() { +void PlocMpsocHandler::disableAllReplies() { using namespace mpsoc; DeviceReplyMap::iterator iter; @@ -1404,7 +1404,7 @@ void PlocMPSoCHandler::disableAllReplies() { nextReplyId = mpsoc::NONE; } -void PlocMPSoCHandler::sendFailureReport(DeviceCommandId_t replyId, ReturnValue_t status) { +void PlocMpsocHandler::sendFailureReport(DeviceCommandId_t replyId, ReturnValue_t status) { DeviceReplyIter iter = deviceReplyMap.find(replyId); if (iter == deviceReplyMap.end()) { sif::debug << "PlocMPSoCHandler::sendFailureReport: Reply not in reply map" << std::endl; @@ -1421,7 +1421,7 @@ void PlocMPSoCHandler::sendFailureReport(DeviceCommandId_t replyId, ReturnValue_ info->isExecuting = false; } -void PlocMPSoCHandler::disableExeReportReply() { +void PlocMpsocHandler::disableExeReportReply() { DeviceReplyIter iter = deviceReplyMap.find(mpsoc::EXE_REPORT); DeviceReplyInfo* info = &(iter->second); info->delayCycles = 0; @@ -1430,11 +1430,7 @@ void PlocMPSoCHandler::disableExeReportReply() { info->command->second.expectedReplies = 0; } -uint16_t PlocMPSoCHandler::getStatus(const uint8_t* data) { - return (*(data + STATUS_OFFSET) << 8) | *(data + STATUS_OFFSET + 1); -} - -void PlocMPSoCHandler::handleActionCommandFailure(ActionId_t actionId) { +void PlocMpsocHandler::handleActionCommandFailure(ActionId_t actionId) { switch (actionId) { case supv::ACK_REPORT: case supv::EXE_REPORT: @@ -1467,20 +1463,20 @@ void PlocMPSoCHandler::handleActionCommandFailure(ActionId_t actionId) { return; } -LocalPoolDataSetBase* PlocMPSoCHandler::getDataSetHandle(sid_t sid) { +LocalPoolDataSetBase* PlocMpsocHandler::getDataSetHandle(sid_t sid) { if (sid == hkReport.getSid()) { return &hkReport; } return nullptr; } -bool PlocMPSoCHandler::dontCheckQueue() { +bool PlocMpsocHandler::dontCheckQueue() { // The TC and TMs need to be handled strictly sequentially, so while a command is pending, // more specifically while replies are still expected, do not check the queue.s return commandIsPending; } -void PlocMPSoCHandler::cmdDoneHandler(bool success, ReturnValue_t result) { +void PlocMpsocHandler::cmdDoneHandler(bool success, ReturnValue_t result) { commandIsPending = false; auto commandIter = deviceCommandMap.find(getPendingCommand()); if (commandIter != deviceCommandMap.end()) { diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index dc6ebd99..a42075d8 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -1,9 +1,9 @@ #ifndef BSP_Q7S_DEVICES_PLOC_PLOCMPSOCHANDLER_H_ #define BSP_Q7S_DEVICES_PLOC_PLOCMPSOCHANDLER_H_ -#include +#include #include -#include +#include #include #include @@ -31,7 +31,7 @@ * NOTE: This is not an example for a good device handler, DO NOT USE THIS AS A REFERENCE HANDLER. * @author J. Meier, R. Mueller */ -class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { +class PlocMpsocHandler : public DeviceHandlerBase, public CommandsActionsIF { public: /** * @brief Constructor @@ -44,10 +44,10 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { * module in the programmable logic * @param supervisorHandler Object ID of the supervisor handler */ - PlocMPSoCHandler(object_id_t objectId, object_id_t uartComIFid, CookieIF* comCookie, - PlocMPSoCHelper* plocMPSoCHelper, Gpio uartIsolatorSwitch, + PlocMpsocHandler(object_id_t objectId, object_id_t uartComIFid, CookieIF* comCookie, + PlocMpsocSpecialComHelper* plocMPSoCHelper, Gpio uartIsolatorSwitch, object_id_t supervisorHandler); - virtual ~PlocMPSoCHandler(); + virtual ~PlocMpsocHandler(); virtual ReturnValue_t initialize() override; ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size) override; @@ -106,7 +106,6 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { static const uint16_t APID_MASK = 0x7FF; static const uint16_t PACKET_SEQUENCE_COUNT_MASK = 0x3FFF; - static const uint8_t STATUS_OFFSET = 10; mpsoc::HkReport hkReport; @@ -163,7 +162,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { SerialComIF* uartComIf = nullptr; - PlocMPSoCHelper* plocMPSoCHelper = nullptr; + PlocMpsocSpecialComHelper* plocMPSoCHelper = nullptr; Gpio uartIsolatorSwitch; object_id_t supervisorHandler = 0; CommandActionHelper commandActionHelper; @@ -299,8 +298,6 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { ReturnValue_t prepareTcModeReplay(); - uint16_t getStatus(const uint8_t* data); - void cmdDoneHandler(bool success, ReturnValue_t result); void handleActionCommandFailure(ActionId_t actionId); diff --git a/linux/payload/PlocMpsocHelper.cpp b/linux/payload/PlocMpsocSpecialComHelper.cpp similarity index 81% rename from linux/payload/PlocMpsocHelper.cpp rename to linux/payload/PlocMpsocSpecialComHelper.cpp index 66e46c18..6bc338c6 100644 --- a/linux/payload/PlocMpsocHelper.cpp +++ b/linux/payload/PlocMpsocSpecialComHelper.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -11,14 +11,15 @@ using namespace ploc; -PlocMPSoCHelper::PlocMPSoCHelper(object_id_t objectId) : SystemObject(objectId) { +PlocMpsocSpecialComHelper::PlocMpsocSpecialComHelper(object_id_t objectId) + : SystemObject(objectId) { spParams.buf = commandBuffer; spParams.maxSize = sizeof(commandBuffer); } -PlocMPSoCHelper::~PlocMPSoCHelper() {} +PlocMpsocSpecialComHelper::~PlocMpsocSpecialComHelper() {} -ReturnValue_t PlocMPSoCHelper::initialize() { +ReturnValue_t PlocMpsocSpecialComHelper::initialize() { #ifdef XIPHOS_Q7S sdcMan = SdCardManager::instance(); if (sdcMan == nullptr) { @@ -29,7 +30,7 @@ ReturnValue_t PlocMPSoCHelper::initialize() { return returnvalue::OK; } -ReturnValue_t PlocMPSoCHelper::performOperation(uint8_t operationCode) { +ReturnValue_t PlocMpsocSpecialComHelper::performOperation(uint8_t operationCode) { ReturnValue_t result = returnvalue::OK; semaphore.acquire(); while (true) { @@ -69,7 +70,7 @@ ReturnValue_t PlocMPSoCHelper::performOperation(uint8_t operationCode) { } } -ReturnValue_t PlocMPSoCHelper::setComIF(DeviceCommunicationIF* communicationInterface_) { +ReturnValue_t PlocMpsocSpecialComHelper::setComIF(DeviceCommunicationIF* communicationInterface_) { uartComIF = dynamic_cast(communicationInterface_); if (uartComIF == nullptr) { sif::warning << "PlocMPSoCHelper::initialize: Invalid uart com if" << std::endl; @@ -78,13 +79,14 @@ ReturnValue_t PlocMPSoCHelper::setComIF(DeviceCommunicationIF* communicationInte return returnvalue::OK; } -void PlocMPSoCHelper::setComCookie(CookieIF* comCookie_) { comCookie = comCookie_; } +void PlocMpsocSpecialComHelper::setComCookie(CookieIF* comCookie_) { comCookie = comCookie_; } -void PlocMPSoCHelper::setSequenceCount(SourceSequenceCounter* sequenceCount_) { +void PlocMpsocSpecialComHelper::setSequenceCount(SourceSequenceCounter* sequenceCount_) { sequenceCount = sequenceCount_; } -ReturnValue_t PlocMPSoCHelper::startFlashWrite(std::string obcFile, std::string mpsocFile) { +ReturnValue_t PlocMpsocSpecialComHelper::startFlashWrite(std::string obcFile, + std::string mpsocFile) { if (internalState != InternalState::IDLE) { return returnvalue::FAILED; } @@ -96,8 +98,8 @@ ReturnValue_t PlocMPSoCHelper::startFlashWrite(std::string obcFile, std::string return semaphore.release(); } -ReturnValue_t PlocMPSoCHelper::startFlashRead(std::string obcFile, std::string mpsocFile, - size_t readFileSize) { +ReturnValue_t PlocMpsocSpecialComHelper::startFlashRead(std::string obcFile, std::string mpsocFile, + size_t readFileSize) { if (internalState != InternalState::IDLE) { return returnvalue::FAILED; } @@ -110,21 +112,21 @@ ReturnValue_t PlocMPSoCHelper::startFlashRead(std::string obcFile, std::string m return semaphore.release(); } -void PlocMPSoCHelper::resetHelper() { +void PlocMpsocSpecialComHelper::resetHelper() { spParams.buf = commandBuffer; terminate = false; uartComIF->flushUartRxBuffer(comCookie); } -void PlocMPSoCHelper::stopProcess() { terminate = true; } +void PlocMpsocSpecialComHelper::stopProcess() { terminate = true; } -ReturnValue_t PlocMPSoCHelper::performFlashWrite() { +ReturnValue_t PlocMpsocSpecialComHelper::performFlashWrite() { ReturnValue_t result = returnvalue::OK; std::ifstream file(flashReadAndWrite.obcFile, std::ifstream::binary); if (file.bad()) { return returnvalue::FAILED; } - result = flashfopen(); + result = flashfopen(mpsoc::FileAccessMode::WRITE); if (result != returnvalue::OK) { return result; } @@ -172,7 +174,7 @@ ReturnValue_t PlocMPSoCHelper::performFlashWrite() { return result; } -ReturnValue_t PlocMPSoCHelper::performFlashRead() { +ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { sif::debug << "performing flash read" << std::endl; std::error_code e; std::ofstream ofile(flashReadAndWrite.obcFile, std::ios::trunc | std::ios::binary); @@ -180,7 +182,7 @@ ReturnValue_t PlocMPSoCHelper::performFlashRead() { return returnvalue::FAILED; } sif::debug << "Sequence count: " << sequenceCount->get() << std::endl; - ReturnValue_t result = flashfopen(); + ReturnValue_t result = flashfopen(mpsoc::FileAccessMode::READ); if (result != returnvalue::OK) { std::filesystem::remove(flashReadAndWrite.obcFile, e); return result; @@ -229,12 +231,11 @@ ReturnValue_t PlocMPSoCHelper::performFlashRead() { return result; } -ReturnValue_t PlocMPSoCHelper::flashfopen() { +ReturnValue_t PlocMpsocSpecialComHelper::flashfopen(mpsoc::FileAccessMode mode) { spParams.buf = commandBuffer; (*sequenceCount)++; mpsoc::FlashFopen flashFopen(spParams, *sequenceCount); - ReturnValue_t result = - flashFopen.createPacket(flashReadAndWrite.mpsocFile, mpsoc::FlashFopen::APPEND); + ReturnValue_t result = flashFopen.createPacket(flashReadAndWrite.mpsocFile, mode); if (result != returnvalue::OK) { return result; } @@ -245,7 +246,7 @@ ReturnValue_t PlocMPSoCHelper::flashfopen() { return returnvalue::OK; } -ReturnValue_t PlocMPSoCHelper::flashfclose() { +ReturnValue_t PlocMpsocSpecialComHelper::flashfclose() { spParams.buf = commandBuffer; (*sequenceCount)++; mpsoc::FlashFclose flashFclose(spParams, *sequenceCount); @@ -256,7 +257,7 @@ ReturnValue_t PlocMPSoCHelper::flashfclose() { return handlePacketTransmissionNoReply(flashFclose); } -ReturnValue_t PlocMPSoCHelper::handlePacketTransmissionFlashRead(mpsoc::TcFlashRead& tc) { +ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionFlashRead(mpsoc::TcFlashRead& tc) { ReturnValue_t result = sendCommand(tc); if (result != returnvalue::OK) { return result; @@ -273,7 +274,7 @@ ReturnValue_t PlocMPSoCHelper::handlePacketTransmissionFlashRead(mpsoc::TcFlashR return handleExe(); } -ReturnValue_t PlocMPSoCHelper::handlePacketTransmissionNoReply(ploc::SpTcBase& tc) { +ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionNoReply(ploc::SpTcBase& tc) { ReturnValue_t result = sendCommand(tc); if (result != returnvalue::OK) { return result; @@ -285,7 +286,7 @@ ReturnValue_t PlocMPSoCHelper::handlePacketTransmissionNoReply(ploc::SpTcBase& t return handleExe(); } -ReturnValue_t PlocMPSoCHelper::sendCommand(ploc::SpTcBase& tc) { +ReturnValue_t PlocMpsocSpecialComHelper::sendCommand(ploc::SpTcBase& tc) { ReturnValue_t result = returnvalue::OK; result = uartComIF->sendMessage(comCookie, tc.getFullPacket(), tc.getFullPacketLen()); if (result != returnvalue::OK) { @@ -296,7 +297,7 @@ ReturnValue_t PlocMPSoCHelper::sendCommand(ploc::SpTcBase& tc) { return result; } -ReturnValue_t PlocMPSoCHelper::handleAck() { +ReturnValue_t PlocMpsocSpecialComHelper::handleAck() { ReturnValue_t result = returnvalue::OK; result = handleTmReception(mpsoc::SIZE_ACK_REPORT); if (result != returnvalue::OK) { @@ -309,17 +310,18 @@ ReturnValue_t PlocMPSoCHelper::handleAck() { } uint16_t apid = tmPacket.getApid(); if (apid != mpsoc::apid::ACK_SUCCESS) { - handleAckApidFailure(apid); + handleAckApidFailure(tmPacket); return returnvalue::FAILED; } return returnvalue::OK; } -void PlocMPSoCHelper::handleAckApidFailure(uint16_t apid) { +void PlocMpsocSpecialComHelper::handleAckApidFailure(const ploc::SpTmReader& reader) { + uint16_t apid = reader.getApid(); if (apid == mpsoc::apid::ACK_FAILURE) { - triggerEvent(MPSOC_ACK_FAILURE_REPORT, static_cast(internalState)); - sif::warning << "PlocMPSoCHelper::handleAckApidFailure: Received acknowledgement failure " - << "report" << std::endl; + uint16_t status = mpsoc::getStatusFromRawData(reader.getFullData()); + sif::warning << "MPSoC ACK Failure: " << mpsoc::getStatusString(status) << std::endl; + triggerEvent(MPSOC_ACK_FAILURE_REPORT, static_cast(internalState), status); } else { triggerEvent(MPSOC_ACK_INVALID_APID, apid, static_cast(internalState)); sif::warning << "PlocMPSoCHelper::handleAckApidFailure: Expected acknowledgement report " @@ -327,7 +329,7 @@ void PlocMPSoCHelper::handleAckApidFailure(uint16_t apid) { } } -ReturnValue_t PlocMPSoCHelper::handleExe() { +ReturnValue_t PlocMpsocSpecialComHelper::handleExe() { ReturnValue_t result = returnvalue::OK; result = handleTmReception(mpsoc::SIZE_EXE_REPORT); @@ -341,17 +343,18 @@ ReturnValue_t PlocMPSoCHelper::handleExe() { } uint16_t apid = tmPacket.getApid(); if (apid != mpsoc::apid::EXE_SUCCESS) { - handleExeApidFailure(apid); + handleExeApidFailure(tmPacket); return returnvalue::FAILED; } return returnvalue::OK; } -void PlocMPSoCHelper::handleExeApidFailure(uint16_t apid) { +void PlocMpsocSpecialComHelper::handleExeApidFailure(const ploc::SpTmReader& reader) { + uint16_t apid = reader.getApid(); if (apid == mpsoc::apid::EXE_FAILURE) { + uint16_t status = mpsoc::getStatusFromRawData(reader.getFullData()); + sif::warning << "MPSoC EXE Failure: " << mpsoc::getStatusString(status) << std::endl; triggerEvent(MPSOC_EXE_FAILURE_REPORT, static_cast(internalState)); - sif::warning << "PlocMPSoCHelper::handleExeApidFailure: Received execution failure " - << "report" << std::endl; } else { triggerEvent(MPSOC_EXE_INVALID_APID, apid, static_cast(internalState)); sif::warning << "PlocMPSoCHelper::handleExeApidFailure: Expected execution report " @@ -359,7 +362,7 @@ void PlocMPSoCHelper::handleExeApidFailure(uint16_t apid) { } } -ReturnValue_t PlocMPSoCHelper::handleTmReception(size_t remainingBytes) { +ReturnValue_t PlocMpsocSpecialComHelper::handleTmReception(size_t remainingBytes) { ReturnValue_t result = returnvalue::OK; size_t readBytes = 0; size_t currentBytes = 0; @@ -382,7 +385,8 @@ ReturnValue_t PlocMPSoCHelper::handleTmReception(size_t remainingBytes) { return result; } -ReturnValue_t PlocMPSoCHelper::handleFlashReadReply(std::ofstream& ofile, size_t expectedReadLen) { +ReturnValue_t PlocMpsocSpecialComHelper::handleFlashReadReply(std::ofstream& ofile, + size_t expectedReadLen) { SpTmReader tmPacket(tmBuf.data(), tmBuf.size()); ReturnValue_t result = checkReceivedTm(tmPacket); if (result != returnvalue::OK) { @@ -422,7 +426,7 @@ ReturnValue_t PlocMPSoCHelper::handleFlashReadReply(std::ofstream& ofile, size_t return returnvalue::OK; } -ReturnValue_t PlocMPSoCHelper::fileCheck(std::string obcFile) { +ReturnValue_t PlocMpsocSpecialComHelper::fileCheck(std::string obcFile) { #ifdef XIPHOS_Q7S ReturnValue_t result = FilesystemHelper::checkPath(obcFile); if (result != returnvalue::OK) { @@ -438,8 +442,8 @@ ReturnValue_t PlocMPSoCHelper::fileCheck(std::string obcFile) { return returnvalue::OK; } -ReturnValue_t PlocMPSoCHelper::startFlashReadOrWriteBase(std::string obcFile, - std::string mpsocFile) { +ReturnValue_t PlocMpsocSpecialComHelper::startFlashReadOrWriteBase(std::string obcFile, + std::string mpsocFile) { ReturnValue_t result = fileCheck(obcFile); if (result != returnvalue::OK) { return result; @@ -451,7 +455,7 @@ ReturnValue_t PlocMPSoCHelper::startFlashReadOrWriteBase(std::string obcFile, return returnvalue::OK; } -ReturnValue_t PlocMPSoCHelper::checkReceivedTm(SpTmReader& reader) { +ReturnValue_t PlocMpsocSpecialComHelper::checkReceivedTm(SpTmReader& reader) { ReturnValue_t result = reader.checkSize(); if (result != returnvalue::OK) { sif::error << "PlocMPSoCHelper::handleTmReception: Size check on received TM failed" @@ -475,7 +479,8 @@ ReturnValue_t PlocMPSoCHelper::checkReceivedTm(SpTmReader& reader) { return returnvalue::OK; } -ReturnValue_t PlocMPSoCHelper::receive(uint8_t* data, size_t* readBytes, size_t requestBytes) { +ReturnValue_t PlocMpsocSpecialComHelper::receive(uint8_t* data, size_t* readBytes, + size_t requestBytes) { ReturnValue_t result = returnvalue::OK; uint8_t* buffer = nullptr; result = uartComIF->requestReceiveMessage(comCookie, requestBytes); diff --git a/linux/payload/PlocMpsocHelper.h b/linux/payload/PlocMpsocSpecialComHelper.h similarity index 95% rename from linux/payload/PlocMpsocHelper.h rename to linux/payload/PlocMpsocSpecialComHelper.h index 2595c811..2a8e03dc 100644 --- a/linux/payload/PlocMpsocHelper.h +++ b/linux/payload/PlocMpsocSpecialComHelper.h @@ -1,7 +1,7 @@ #ifndef BSP_Q7S_DEVICES_PLOCMPSOCHELPER_H_ #define BSP_Q7S_DEVICES_PLOCMPSOCHELPER_H_ -#include +#include #include #include @@ -23,7 +23,7 @@ * MPSoC and OBC. * @author J. Meier */ -class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { +class PlocMpsocSpecialComHelper : public SystemObject, public ExecutableObjectIF { public: static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_MPSOC_HELPER; @@ -82,8 +82,8 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { FLASH_READ_READLEN_ERROR = 2 }; - PlocMPSoCHelper(object_id_t objectId); - virtual ~PlocMPSoCHelper(); + PlocMpsocSpecialComHelper(object_id_t objectId); + virtual ~PlocMpsocSpecialComHelper(); ReturnValue_t initialize() override; ReturnValue_t performOperation(uint8_t operationCode = 0) override; @@ -175,7 +175,7 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { void resetHelper(); ReturnValue_t performFlashWrite(); ReturnValue_t performFlashRead(); - ReturnValue_t flashfopen(); + ReturnValue_t flashfopen(mpsoc::FileAccessMode mode); ReturnValue_t flashfclose(); ReturnValue_t handlePacketTransmissionNoReply(ploc::SpTcBase& tc); ReturnValue_t handlePacketTransmissionFlashRead(mpsoc::TcFlashRead& tc); @@ -186,8 +186,8 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { ReturnValue_t handleExe(); ReturnValue_t startFlashReadOrWriteBase(std::string obcFile, std::string mpsocFile); ReturnValue_t fileCheck(std::string obcFile); - void handleAckApidFailure(uint16_t apid); - void handleExeApidFailure(uint16_t apid); + void handleAckApidFailure(const ploc::SpTmReader& reader); + void handleExeApidFailure(const ploc::SpTmReader& reader); ReturnValue_t handleTmReception(size_t remainingBytes); ReturnValue_t checkReceivedTm(ploc::SpTmReader& reader); }; diff --git a/linux/payload/plocMpsocHelpers.cpp b/linux/payload/plocMpsocHelpers.cpp new file mode 100644 index 00000000..ab6f0907 --- /dev/null +++ b/linux/payload/plocMpsocHelpers.cpp @@ -0,0 +1,91 @@ +#include "plocMpsocHelpers.h" + +uint16_t mpsoc::getStatusFromRawData(const uint8_t* data) { + return (*(data + STATUS_OFFSET) << 8) | *(data + STATUS_OFFSET + 1); +} +std::string mpsoc::getStatusString(uint16_t status) { + switch (status) { + case (mpsoc::status_code::UNKNOWN_APID): { + return "Unknown APID"; + break; + } + case (mpsoc::status_code::INCORRECT_LENGTH): { + return "Incorrect length"; + break; + } + case (mpsoc::status_code::INCORRECT_CRC): { + return "Incorrect crc"; + break; + } + case (mpsoc::status_code::INCORRECT_PKT_SEQ_CNT): { + return "Incorrect packet sequence count"; + break; + } + case (mpsoc::status_code::TC_NOT_ALLOWED_IN_MODE): { + return "TC not allowed in this mode"; + break; + } + case (mpsoc::status_code::TC_EXEUTION_DISABLED): { + return "TC execution disabled"; + break; + } + case (mpsoc::status_code::FLASH_MOUNT_FAILED): { + return "Flash mount failed"; + break; + } + case (mpsoc::status_code::FLASH_FILE_ALREADY_CLOSED): { + return "Flash file already closed"; + break; + } + case (mpsoc::status_code::FLASH_FILE_OPEN_FAILED): { + return "Flash file open failed"; + break; + } + case (mpsoc::status_code::FLASH_FILE_NOT_OPEN): { + return "Flash file not open"; + break; + } + case (mpsoc::status_code::FLASH_UNMOUNT_FAILED): { + return "Flash unmount failed"; + break; + } + case (mpsoc::status_code::HEAP_ALLOCATION_FAILED): { + return "Heap allocation failed"; + break; + } + case (mpsoc::status_code::INVALID_PARAMETER): { + return "Invalid parameter"; + break; + } + case (mpsoc::status_code::NOT_INITIALIZED): { + return "Not initialized"; + break; + } + case (mpsoc::status_code::REBOOT_IMMINENT): { + return "Reboot imminent"; + break; + } + case (mpsoc::status_code::CORRUPT_DATA): { + return "Corrupt data"; + break; + } + case (mpsoc::status_code::FLASH_CORRECTABLE_MISMATCH): { + return "Flash correctable mismatch"; + break; + } + case (mpsoc::status_code::FLASH_UNCORRECTABLE_MISMATCH): { + return "Flash uncorrectable mismatch"; + break; + } + case (mpsoc::status_code::DEFAULT_ERROR_CODE): { + return "Default error code"; + break; + } + default: + std::stringstream ss; + ss << "0x" << std::hex << status; + return ss.str().c_str(); + break; + } + return ""; +} diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpsocHelpers.h similarity index 92% rename from linux/payload/plocMpscoDefs.h rename to linux/payload/plocMpsocHelpers.h index 4bb19e4d..a7173d65 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpsocHelpers.h @@ -13,6 +13,16 @@ namespace mpsoc { +enum FileAccessMode : uint8_t { + OPEN_EXISTING = 0x00, + READ = 0x01, + WRITE = 0x02, + CREATE_NEW = 0x04, + CREATE_ALWAYS = 0x08, + OPEN_ALWAYS = 0x10, + OPEN_APPEND = 0x30 +}; + static constexpr uint32_t HK_SET_ID = 0; namespace poolid { @@ -140,6 +150,8 @@ static const char NULL_TERMINATOR = '\0'; static const uint8_t MIN_SPACE_PACKET_LENGTH = 7; static const uint8_t SPACE_PACKET_HEADER_SIZE = 6; +static const uint8_t STATUS_OFFSET = 10; + static constexpr size_t CRC_SIZE = 2; /** @@ -380,27 +392,24 @@ class FlashFopen : public ploc::SpTcBase { FlashFopen(ploc::SpTcParams params, uint16_t sequenceCount) : ploc::SpTcBase(params, apid::TC_FLASHFOPEN, sequenceCount) {} - static const char APPEND = 'a'; - static const char WRITE = 'w'; - static const char READ = 'r'; - - ReturnValue_t createPacket(std::string filename, char accessMode_) { - accessMode = accessMode_; + ReturnValue_t createPacket(std::string filename, FileAccessMode mode) { + accessMode = mode; size_t nameSize = filename.size(); - spParams.setFullPayloadLen(nameSize + sizeof(NULL_TERMINATOR) + sizeof(accessMode) + CRC_SIZE); + spParams.setFullPayloadLen(256 + sizeof(uint8_t) + CRC_SIZE); ReturnValue_t result = checkPayloadLen(); if (result != returnvalue::OK) { return result; } std::memcpy(payloadStart, filename.c_str(), nameSize); - *(spParams.buf + nameSize) = NULL_TERMINATOR; - std::memcpy(payloadStart + nameSize + sizeof(NULL_TERMINATOR), &accessMode, sizeof(accessMode)); + payloadStart[nameSize] = NULL_TERMINATOR; + payloadStart[255] = NULL_TERMINATOR; + payloadStart[256] = static_cast(accessMode); updateSpFields(); return calcAndSetCrc(); } private: - char accessMode = APPEND; + FileAccessMode accessMode = FileAccessMode::OPEN_EXISTING; }; /** @@ -989,92 +998,8 @@ class HkReport : public StaticLocalDataSet<36> { lp_var_t(sid.objectId, mpsoc::poolid::REBOOT_MPSOC_REQUIRED, this); }; -const char* getStatusString(uint16_t status) { - switch (status) { - case (mpsoc::status_code::UNKNOWN_APID): { - return "Unknown APID"; - break; - } - case (mpsoc::status_code::INCORRECT_LENGTH): { - return "Incorrect length"; - break; - } - case (mpsoc::status_code::INCORRECT_CRC): { - return "Incorrect crc"; - break; - } - case (mpsoc::status_code::INCORRECT_PKT_SEQ_CNT): { - return "Incorrect packet sequence count"; - break; - } - case (mpsoc::status_code::TC_NOT_ALLOWED_IN_MODE): { - return "TC not allowed in this mode"; - break; - } - case (mpsoc::status_code::TC_EXEUTION_DISABLED): { - return "TC execution disabled"; - break; - } - case (mpsoc::status_code::FLASH_MOUNT_FAILED): { - return "Flash mount failed"; - break; - } - case (mpsoc::status_code::FLASH_FILE_ALREADY_CLOSED): { - return "Flash file already closed"; - break; - } - case (mpsoc::status_code::FLASH_FILE_OPEN_FAILED): { - return "Flash file open failed"; - break; - } - case (mpsoc::status_code::FLASH_FILE_NOT_OPEN): { - return "Flash file not open"; - break; - } - case (mpsoc::status_code::FLASH_UNMOUNT_FAILED): { - return "Flash unmount failed"; - break; - } - case (mpsoc::status_code::HEAP_ALLOCATION_FAILED): { - return "Heap allocation failed"; - break; - } - case (mpsoc::status_code::INVALID_PARAMETER): { - return "Invalid parameter"; - break; - } - case (mpsoc::status_code::NOT_INITIALIZED): { - return "Not initialized"; - break; - } - case (mpsoc::status_code::REBOOT_IMMINENT): { - return "Reboot imminent"; - break; - } - case (mpsoc::status_code::CORRUPT_DATA): { - return "Corrupt data"; - break; - } - case (mpsoc::status_code::FLASH_CORRECTABLE_MISMATCH): { - return "Flash correctable mismatch"; - break; - } - case (mpsoc::status_code::FLASH_UNCORRECTABLE_MISMATCH): { - return "Flash uncorrectable mismatch"; - break; - } - case (mpsoc::status_code::DEFAULT_ERROR_CODE): { - return "Default error code"; - break; - } - default: - std::stringstream ss; - ss << "0x" << std::hex << status; - return ss.str(); - break; - } - return ""; -} +uint16_t getStatusFromRawData(const uint8_t* data); +std::string getStatusString(uint16_t status); } // namespace mpsoc From fabc6da562a523e9af978220095e743b33b34c7b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 15:16:44 +0200 Subject: [PATCH 158/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5c151397..7c3924f1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -60,6 +60,8 @@ will consitute of a breaking change warranting a new major release: - The PLOC MPSoC now waits 10 cycles before going to on. These wait cycles are necessary because the MPSoC is not ready to process commands without this additional boot time. - Fixed correction for `GPS Altitude` in case the sensor data is out of the expected bonds. +- PLOC MPSoC special communication is now scheduled, which allows flash read and flash write + commands to work. # [v2.0.5] 2023-05-11 From 5ce0a60184d42420f8200abd3d0a5908d86910e8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 15:19:12 +0200 Subject: [PATCH 159/506] some more cleaning and refacoring --- linux/payload/PlocMpsocHandler.cpp | 32 ++++++++++++------------------ linux/payload/PlocMpsocHandler.h | 2 +- 2 files changed, 14 insertions(+), 20 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index a07c7871..89e9a248 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -13,7 +13,7 @@ PlocMpsocHandler::PlocMpsocHandler(object_id_t objectId, object_id_t uartComIFid Gpio uartIsolatorSwitch, object_id_t supervisorHandler) : DeviceHandlerBase(objectId, uartComIFid, comCookie), hkReport(this), - plocMPSoCHelper(plocMPSoCHelper), + specialComHelper(plocMPSoCHelper), uartIsolatorSwitch(uartIsolatorSwitch), supervisorHandler(supervisorHandler), commandActionHelper(this) { @@ -65,12 +65,12 @@ ReturnValue_t PlocMpsocHandler::initialize() { return ObjectManagerIF::CHILD_INIT_FAILED; } - result = plocMPSoCHelper->setComIF(communicationInterface); + result = specialComHelper->setComIF(communicationInterface); if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } - plocMPSoCHelper->setComCookie(comCookie); - plocMPSoCHelper->setSequenceCount(&sequenceCount); + specialComHelper->setComCookie(comCookie); + specialComHelper->setSequenceCount(&sequenceCount); result = commandActionHelper.initialize(); if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; @@ -136,9 +136,9 @@ ReturnValue_t PlocMpsocHandler::executeAction(ActionId_t actionId, MessageQueueI if (result != returnvalue::OK) { return result; } - plocMPSoCHelper->setSequenceCount(&sequenceCount); - result = plocMPSoCHelper->startFlashWrite(flashWritePusCmd.getObcFile(), - flashWritePusCmd.getMPSoCFile()); + specialComHelper->setSequenceCount(&sequenceCount); + result = specialComHelper->startFlashWrite(flashWritePusCmd.getObcFile(), + flashWritePusCmd.getMPSoCFile()); if (result != returnvalue::OK) { return result; } @@ -151,12 +151,10 @@ ReturnValue_t PlocMpsocHandler::executeAction(ActionId_t actionId, MessageQueueI if (result != returnvalue::OK) { return result; } - sif::debug << "starting flash read" << std::endl; - sif::debug << "sequence count: " << sequenceCount.get() << std::endl; - plocMPSoCHelper->setSequenceCount(&sequenceCount); - result = plocMPSoCHelper->startFlashRead(flashReadPusCmd.getObcFile(), - flashReadPusCmd.getMPSoCFile(), - flashReadPusCmd.getReadSize()); + specialComHelper->setSequenceCount(&sequenceCount); + result = specialComHelper->startFlashRead(flashReadPusCmd.getObcFile(), + flashReadPusCmd.getMPSoCFile(), + flashReadPusCmd.getReadSize()); if (result != returnvalue::OK) { return result; } @@ -790,10 +788,9 @@ ReturnValue_t PlocMpsocHandler::handleAckReport(const uint8_t* data) { switch (apid) { case mpsoc::apid::ACK_FAILURE: { - sif::debug << "PlocMPSoCHandler::handleAckReport: Received Ack failure report" << std::endl; DeviceCommandId_t commandId = getPendingCommand(); uint16_t status = mpsoc::getStatusFromRawData(data); - sif::warning << "Verification report status: " << mpsoc::getStatusString(status) << std::endl; + sif::warning << "MPSoC ACK Failure: " << mpsoc::getStatusString(status) << std::endl; if (commandId != DeviceHandlerIF::NO_COMMAND_ID) { triggerEvent(ACK_FAILURE, commandId, status); } @@ -835,15 +832,12 @@ ReturnValue_t PlocMpsocHandler::handleExecutionReport(const uint8_t* data) { break; } case (mpsoc::apid::EXE_FAILURE): { - // TODO: Interpretation of status field in execution report - sif::warning << "PlocMPSoCHandler::handleExecutionReport: Received execution failure report" - << std::endl; DeviceCommandId_t commandId = getPendingCommand(); if (commandId == DeviceHandlerIF::NO_COMMAND_ID) { sif::debug << "PlocMPSoCHandler::handleExecutionReport: Unknown command id" << std::endl; } uint16_t status = mpsoc::getStatusFromRawData(data); - sif::warning << "Verification report status: " << mpsoc::getStatusString(status) << std::endl; + sif::warning << "MPSoC EXE Failure: " << mpsoc::getStatusString(status) << std::endl; triggerEvent(EXE_FAILURE, commandId, status); sendFailureReport(mpsoc::EXE_REPORT, MPSoCReturnValuesIF::RECEIVED_EXE_FAILURE); result = IGNORE_REPLY_DATA; diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index a42075d8..ea62e484 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -162,7 +162,7 @@ class PlocMpsocHandler : public DeviceHandlerBase, public CommandsActionsIF { SerialComIF* uartComIf = nullptr; - PlocMpsocSpecialComHelper* plocMPSoCHelper = nullptr; + PlocMpsocSpecialComHelper* specialComHelper = nullptr; Gpio uartIsolatorSwitch; object_id_t supervisorHandler = 0; CommandActionHelper commandActionHelper; From c8ed7fe20e53a134290dbebc292e48ef8135bf5e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 15:21:22 +0200 Subject: [PATCH 160/506] new events and retvals --- .../fsfwconfig/events/translateEvents.cpp | 13 ++++++-- .../fsfwconfig/objects/translateObjects.cpp | 2 +- generators/bsp_hosted_events.csv | 33 ++++++++++--------- generators/bsp_q7s_events.csv | 33 ++++++++++--------- generators/bsp_q7s_returnvalues.csv | 7 ++-- generators/events/translateEvents.cpp | 13 ++++++-- generators/objects/translateObjects.cpp | 2 +- linux/fsfwconfig/events/translateEvents.cpp | 13 ++++++-- linux/fsfwconfig/objects/translateObjects.cpp | 2 +- tmtc | 2 +- 10 files changed, 77 insertions(+), 43 deletions(-) diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index f7850a99..63fee50d 100644 --- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp +++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 291 translations. + * @brief Auto-generated event translation file. Contains 294 translations. * @details - * Generated on: 2023-04-17 11:34:19 + * Generated on: 2023-05-15 15:20:19 */ #include "translateEvents.h" @@ -197,6 +197,9 @@ const char *MPSOC_EXE_INVALID_APID_STRING = "MPSOC_EXE_INVALID_APID"; const char *MPSOC_HELPER_SEQ_CNT_MISMATCH_STRING = "MPSOC_HELPER_SEQ_CNT_MISMATCH"; const char *MPSOC_TM_SIZE_ERROR_STRING = "MPSOC_TM_SIZE_ERROR"; const char *MPSOC_TM_CRC_MISSMATCH_STRING = "MPSOC_TM_CRC_MISSMATCH"; +const char *MPSOC_FLASH_READ_PACKET_ERROR_STRING = "MPSOC_FLASH_READ_PACKET_ERROR"; +const char *MPSOC_FLASH_READ_FAILED_STRING = "MPSOC_FLASH_READ_FAILED"; +const char *MPSOC_FLASH_READ_SUCCESSFUL_STRING = "MPSOC_FLASH_READ_SUCCESSFUL"; const char *TRANSITION_BACK_TO_OFF_STRING = "TRANSITION_BACK_TO_OFF"; const char *NEG_V_OUT_OF_BOUNDS_STRING = "NEG_V_OUT_OF_BOUNDS"; const char *U_DRO_OUT_OF_BOUNDS_STRING = "U_DRO_OUT_OF_BOUNDS"; @@ -683,6 +686,12 @@ const char *translateEvents(Event event) { return MPSOC_TM_SIZE_ERROR_STRING; case (12613): return MPSOC_TM_CRC_MISSMATCH_STRING; + case (12614): + return MPSOC_FLASH_READ_PACKET_ERROR_STRING; + case (12615): + return MPSOC_FLASH_READ_FAILED_STRING; + case (12616): + return MPSOC_FLASH_READ_SUCCESSFUL_STRING; case (12700): return TRANSITION_BACK_TO_OFF_STRING; case (12701): diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index 16235835..6ab96892 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 171 translations. - * Generated on: 2023-04-17 11:34:19 + * Generated on: 2023-05-15 15:20:19 */ #include "translateObjects.h" diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv index 2d5f5693..2df4aa32 100644 --- a/generators/bsp_hosted_events.csv +++ b/generators/bsp_hosted_events.csv @@ -177,20 +177,23 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 12515;0x30e3;STR_HELPER_FILE_NOT_EXISTS;LOW;Specified file does not exist P1: Internal state of str helper;linux/acs/StrComHandler.h 12516;0x30e4;STR_HELPER_SENDING_PACKET_FAILED;LOW;No description;linux/acs/StrComHandler.h 12517;0x30e5;STR_HELPER_REQUESTING_MSG_FAILED;LOW;No description;linux/acs/StrComHandler.h -12600;0x3138;MPSOC_FLASH_WRITE_FAILED;LOW;Flash write fails;linux/payload/PlocMpsocHelper.h -12601;0x3139;MPSOC_FLASH_WRITE_SUCCESSFUL;LOW;Flash write successful;linux/payload/PlocMpsocHelper.h -12602;0x313a;MPSOC_SENDING_COMMAND_FAILED;LOW;No description;linux/payload/PlocMpsocHelper.h -12603;0x313b;MPSOC_HELPER_REQUESTING_REPLY_FAILED;LOW;Request receive message of communication interface failed P1: Return value returned by the communication interface requestReceiveMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12604;0x313c;MPSOC_HELPER_READING_REPLY_FAILED;LOW;Reading receive message of communication interface failed P1: Return value returned by the communication interface readingReceivedMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12605;0x313d;MPSOC_MISSING_ACK;LOW;Did not receive acknowledgment report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12606;0x313e;MPSOC_MISSING_EXE;LOW;Did not receive execution report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12607;0x313f;MPSOC_ACK_FAILURE_REPORT;LOW;Received acknowledgment failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12608;0x3140;MPSOC_EXE_FAILURE_REPORT;LOW;Received execution failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12609;0x3141;MPSOC_ACK_INVALID_APID;LOW;Expected acknowledgment report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12610;0x3142;MPSOC_EXE_INVALID_APID;LOW;Expected execution report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12611;0x3143;MPSOC_HELPER_SEQ_CNT_MISMATCH;LOW;Received sequence count does not match expected sequence count P1: Expected sequence count P2: Received sequence count;linux/payload/PlocMpsocHelper.h -12612;0x3144;MPSOC_TM_SIZE_ERROR;LOW;No description;linux/payload/PlocMpsocHelper.h -12613;0x3145;MPSOC_TM_CRC_MISSMATCH;LOW;No description;linux/payload/PlocMpsocHelper.h +12600;0x3138;MPSOC_FLASH_WRITE_FAILED;LOW;Flash write fails;linux/payload/PlocMpsocSpecialComHelper.h +12601;0x3139;MPSOC_FLASH_WRITE_SUCCESSFUL;INFO;Flash write successful;linux/payload/PlocMpsocSpecialComHelper.h +12602;0x313a;MPSOC_SENDING_COMMAND_FAILED;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12603;0x313b;MPSOC_HELPER_REQUESTING_REPLY_FAILED;LOW;Request receive message of communication interface failed P1: Return value returned by the communication interface requestReceiveMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12604;0x313c;MPSOC_HELPER_READING_REPLY_FAILED;LOW;Reading receive message of communication interface failed P1: Return value returned by the communication interface readingReceivedMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12605;0x313d;MPSOC_MISSING_ACK;LOW;Did not receive acknowledgment report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12606;0x313e;MPSOC_MISSING_EXE;LOW;Did not receive execution report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12607;0x313f;MPSOC_ACK_FAILURE_REPORT;LOW;Received acknowledgment failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12608;0x3140;MPSOC_EXE_FAILURE_REPORT;LOW;Received execution failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12609;0x3141;MPSOC_ACK_INVALID_APID;LOW;Expected acknowledgment report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12610;0x3142;MPSOC_EXE_INVALID_APID;LOW;Expected execution report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12611;0x3143;MPSOC_HELPER_SEQ_CNT_MISMATCH;LOW;Received sequence count does not match expected sequence count P1: Expected sequence count P2: Received sequence count;linux/payload/PlocMpsocSpecialComHelper.h +12612;0x3144;MPSOC_TM_SIZE_ERROR;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12613;0x3145;MPSOC_TM_CRC_MISSMATCH;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12614;0x3146;MPSOC_FLASH_READ_PACKET_ERROR;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12615;0x3147;MPSOC_FLASH_READ_FAILED;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12616;0x3148;MPSOC_FLASH_READ_SUCCESSFUL;INFO;No description;linux/payload/PlocMpsocSpecialComHelper.h 12700;0x319c;TRANSITION_BACK_TO_OFF;MEDIUM;Could not transition properly and went back to ALL OFF;mission/payload/PayloadPcduHandler.h 12701;0x319d;NEG_V_OUT_OF_BOUNDS;MEDIUM;P1: 0 -> too low, 1 -> too high P2: Float value;mission/payload/PayloadPcduHandler.h 12702;0x319e;U_DRO_OUT_OF_BOUNDS;MEDIUM;P1: 0 -> too low, 1 -> too high P2: Float value;mission/payload/PayloadPcduHandler.h @@ -274,7 +277,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14105;0x3719;CAMERA_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14106;0x371a;PCDU_SYSTEM_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14107;0x371b;HEATER_NOT_OFF_FOR_OFF_MODE;MEDIUM;No description;mission/controller/tcsDefs.h -14108;0x371c;MGT_OVERHEATING;MEDIUM;No description;mission/controller/tcsDefs.h +14108;0x371c;MGT_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14201;0x3779;TX_TIMER_EXPIRED;INFO;The transmit timer to protect the Syrlinks expired P1: The current timer value;mission/system/com/ComSubsystem.h 14202;0x377a;BIT_LOCK_TX_ON;INFO;Transmitter will be turned on due to detection of bitlock;mission/system/com/ComSubsystem.h 14300;0x37dc;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/persistentTmStoreDefs.h diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 2d5f5693..2df4aa32 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -177,20 +177,23 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 12515;0x30e3;STR_HELPER_FILE_NOT_EXISTS;LOW;Specified file does not exist P1: Internal state of str helper;linux/acs/StrComHandler.h 12516;0x30e4;STR_HELPER_SENDING_PACKET_FAILED;LOW;No description;linux/acs/StrComHandler.h 12517;0x30e5;STR_HELPER_REQUESTING_MSG_FAILED;LOW;No description;linux/acs/StrComHandler.h -12600;0x3138;MPSOC_FLASH_WRITE_FAILED;LOW;Flash write fails;linux/payload/PlocMpsocHelper.h -12601;0x3139;MPSOC_FLASH_WRITE_SUCCESSFUL;LOW;Flash write successful;linux/payload/PlocMpsocHelper.h -12602;0x313a;MPSOC_SENDING_COMMAND_FAILED;LOW;No description;linux/payload/PlocMpsocHelper.h -12603;0x313b;MPSOC_HELPER_REQUESTING_REPLY_FAILED;LOW;Request receive message of communication interface failed P1: Return value returned by the communication interface requestReceiveMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12604;0x313c;MPSOC_HELPER_READING_REPLY_FAILED;LOW;Reading receive message of communication interface failed P1: Return value returned by the communication interface readingReceivedMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12605;0x313d;MPSOC_MISSING_ACK;LOW;Did not receive acknowledgment report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12606;0x313e;MPSOC_MISSING_EXE;LOW;Did not receive execution report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12607;0x313f;MPSOC_ACK_FAILURE_REPORT;LOW;Received acknowledgment failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12608;0x3140;MPSOC_EXE_FAILURE_REPORT;LOW;Received execution failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12609;0x3141;MPSOC_ACK_INVALID_APID;LOW;Expected acknowledgment report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12610;0x3142;MPSOC_EXE_INVALID_APID;LOW;Expected execution report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12611;0x3143;MPSOC_HELPER_SEQ_CNT_MISMATCH;LOW;Received sequence count does not match expected sequence count P1: Expected sequence count P2: Received sequence count;linux/payload/PlocMpsocHelper.h -12612;0x3144;MPSOC_TM_SIZE_ERROR;LOW;No description;linux/payload/PlocMpsocHelper.h -12613;0x3145;MPSOC_TM_CRC_MISSMATCH;LOW;No description;linux/payload/PlocMpsocHelper.h +12600;0x3138;MPSOC_FLASH_WRITE_FAILED;LOW;Flash write fails;linux/payload/PlocMpsocSpecialComHelper.h +12601;0x3139;MPSOC_FLASH_WRITE_SUCCESSFUL;INFO;Flash write successful;linux/payload/PlocMpsocSpecialComHelper.h +12602;0x313a;MPSOC_SENDING_COMMAND_FAILED;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12603;0x313b;MPSOC_HELPER_REQUESTING_REPLY_FAILED;LOW;Request receive message of communication interface failed P1: Return value returned by the communication interface requestReceiveMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12604;0x313c;MPSOC_HELPER_READING_REPLY_FAILED;LOW;Reading receive message of communication interface failed P1: Return value returned by the communication interface readingReceivedMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12605;0x313d;MPSOC_MISSING_ACK;LOW;Did not receive acknowledgment report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12606;0x313e;MPSOC_MISSING_EXE;LOW;Did not receive execution report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12607;0x313f;MPSOC_ACK_FAILURE_REPORT;LOW;Received acknowledgment failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12608;0x3140;MPSOC_EXE_FAILURE_REPORT;LOW;Received execution failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12609;0x3141;MPSOC_ACK_INVALID_APID;LOW;Expected acknowledgment report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12610;0x3142;MPSOC_EXE_INVALID_APID;LOW;Expected execution report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12611;0x3143;MPSOC_HELPER_SEQ_CNT_MISMATCH;LOW;Received sequence count does not match expected sequence count P1: Expected sequence count P2: Received sequence count;linux/payload/PlocMpsocSpecialComHelper.h +12612;0x3144;MPSOC_TM_SIZE_ERROR;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12613;0x3145;MPSOC_TM_CRC_MISSMATCH;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12614;0x3146;MPSOC_FLASH_READ_PACKET_ERROR;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12615;0x3147;MPSOC_FLASH_READ_FAILED;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12616;0x3148;MPSOC_FLASH_READ_SUCCESSFUL;INFO;No description;linux/payload/PlocMpsocSpecialComHelper.h 12700;0x319c;TRANSITION_BACK_TO_OFF;MEDIUM;Could not transition properly and went back to ALL OFF;mission/payload/PayloadPcduHandler.h 12701;0x319d;NEG_V_OUT_OF_BOUNDS;MEDIUM;P1: 0 -> too low, 1 -> too high P2: Float value;mission/payload/PayloadPcduHandler.h 12702;0x319e;U_DRO_OUT_OF_BOUNDS;MEDIUM;P1: 0 -> too low, 1 -> too high P2: Float value;mission/payload/PayloadPcduHandler.h @@ -274,7 +277,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14105;0x3719;CAMERA_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14106;0x371a;PCDU_SYSTEM_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14107;0x371b;HEATER_NOT_OFF_FOR_OFF_MODE;MEDIUM;No description;mission/controller/tcsDefs.h -14108;0x371c;MGT_OVERHEATING;MEDIUM;No description;mission/controller/tcsDefs.h +14108;0x371c;MGT_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14201;0x3779;TX_TIMER_EXPIRED;INFO;The transmit timer to protect the Syrlinks expired P1: The current timer value;mission/system/com/ComSubsystem.h 14202;0x377a;BIT_LOCK_TX_ON;INFO;Transmitter will be turned on due to detection of bitlock;mission/system/com/ComSubsystem.h 14300;0x37dc;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/persistentTmStoreDefs.h diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv index 20b76df6..1bc91860 100644 --- a/generators/bsp_q7s_returnvalues.csv +++ b/generators/bsp_q7s_returnvalues.csv @@ -478,8 +478,8 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x53b6;STRH_StartrackerAlreadyBooted;Star tracker is already in firmware mode;182;STR_HANDLER;mission/acs/str/StarTrackerHandler.h 0x53b7;STRH_StartrackerNotRunningFirmware;Star tracker must be in firmware mode to run this command;183;STR_HANDLER;mission/acs/str/StarTrackerHandler.h 0x53b8;STRH_StartrackerNotRunningBootloader;Star tracker must be in bootloader mode to run this command;184;STR_HANDLER;mission/acs/str/StarTrackerHandler.h -0x54e0;DWLPWRON_InvalidMode;Received command has invalid JESD mode (valid modes are 0 - 5);224;DWLPWRON_CMD;linux/payload/plocMpscoDefs.h -0x54e1;DWLPWRON_InvalidLaneRate;Received command has invalid lane rate (valid lane rate are 0 - 9);225;DWLPWRON_CMD;linux/payload/plocMpscoDefs.h +0x54e0;DWLPWRON_InvalidMode;Received command has invalid JESD mode (valid modes are 0 - 5);224;DWLPWRON_CMD;linux/payload/plocMpsocHelpers.h +0x54e1;DWLPWRON_InvalidLaneRate;Received command has invalid lane rate (valid lane rate are 0 - 9);225;DWLPWRON_CMD;linux/payload/plocMpsocHelpers.h 0x5700;PLSPVhLP_RequestDone;No description;0;PLOC_SUPV_HELPER;linux/payload/PlocSupvUartMan.h 0x5701;PLSPVhLP_NoPacketFound;No description;1;PLOC_SUPV_HELPER;linux/payload/PlocSupvUartMan.h 0x5702;PLSPVhLP_DecodeBufTooSmall;No description;2;PLOC_SUPV_HELPER;linux/payload/PlocSupvUartMan.h @@ -543,7 +543,8 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NvmParameterBase.h 0x64a0;FSHLP_SdNotMounted;SD card specified with path string not mounted;160;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h 0x64a1;FSHLP_FileNotExists;Specified file does not exist on filesystem;161;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h -0x65a0;PLMPHLP_FileClosedAccidentally;File accidentally close;160;PLOC_MPSOC_HELPER;linux/payload/PlocMpsocHelper.h +0x65a0;PLMPHLP_FileWriteError;File error occured for file transfers from OBC to the MPSoC.;160;PLOC_MPSOC_HELPER;linux/payload/PlocMpsocSpecialComHelper.h +0x65a1;PLMPHLP_FileReadError;File error occured for file transfers from MPSoC to OBC.;161;PLOC_MPSOC_HELPER;linux/payload/PlocMpsocSpecialComHelper.h 0x66a0;SADPL_CommandNotSupported;No description;160;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h 0x66a1;SADPL_DeploymentAlreadyExecuting;No description;161;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h 0x66a2;SADPL_MainSwitchTimeoutFailure;No description;162;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index f7850a99..63fee50d 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 291 translations. + * @brief Auto-generated event translation file. Contains 294 translations. * @details - * Generated on: 2023-04-17 11:34:19 + * Generated on: 2023-05-15 15:20:19 */ #include "translateEvents.h" @@ -197,6 +197,9 @@ const char *MPSOC_EXE_INVALID_APID_STRING = "MPSOC_EXE_INVALID_APID"; const char *MPSOC_HELPER_SEQ_CNT_MISMATCH_STRING = "MPSOC_HELPER_SEQ_CNT_MISMATCH"; const char *MPSOC_TM_SIZE_ERROR_STRING = "MPSOC_TM_SIZE_ERROR"; const char *MPSOC_TM_CRC_MISSMATCH_STRING = "MPSOC_TM_CRC_MISSMATCH"; +const char *MPSOC_FLASH_READ_PACKET_ERROR_STRING = "MPSOC_FLASH_READ_PACKET_ERROR"; +const char *MPSOC_FLASH_READ_FAILED_STRING = "MPSOC_FLASH_READ_FAILED"; +const char *MPSOC_FLASH_READ_SUCCESSFUL_STRING = "MPSOC_FLASH_READ_SUCCESSFUL"; const char *TRANSITION_BACK_TO_OFF_STRING = "TRANSITION_BACK_TO_OFF"; const char *NEG_V_OUT_OF_BOUNDS_STRING = "NEG_V_OUT_OF_BOUNDS"; const char *U_DRO_OUT_OF_BOUNDS_STRING = "U_DRO_OUT_OF_BOUNDS"; @@ -683,6 +686,12 @@ const char *translateEvents(Event event) { return MPSOC_TM_SIZE_ERROR_STRING; case (12613): return MPSOC_TM_CRC_MISSMATCH_STRING; + case (12614): + return MPSOC_FLASH_READ_PACKET_ERROR_STRING; + case (12615): + return MPSOC_FLASH_READ_FAILED_STRING; + case (12616): + return MPSOC_FLASH_READ_SUCCESSFUL_STRING; case (12700): return TRANSITION_BACK_TO_OFF_STRING; case (12701): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index 1c252ec5..458e8d67 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-04-17 11:34:19 + * Generated on: 2023-05-15 15:20:19 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index f7850a99..63fee50d 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 291 translations. + * @brief Auto-generated event translation file. Contains 294 translations. * @details - * Generated on: 2023-04-17 11:34:19 + * Generated on: 2023-05-15 15:20:19 */ #include "translateEvents.h" @@ -197,6 +197,9 @@ const char *MPSOC_EXE_INVALID_APID_STRING = "MPSOC_EXE_INVALID_APID"; const char *MPSOC_HELPER_SEQ_CNT_MISMATCH_STRING = "MPSOC_HELPER_SEQ_CNT_MISMATCH"; const char *MPSOC_TM_SIZE_ERROR_STRING = "MPSOC_TM_SIZE_ERROR"; const char *MPSOC_TM_CRC_MISSMATCH_STRING = "MPSOC_TM_CRC_MISSMATCH"; +const char *MPSOC_FLASH_READ_PACKET_ERROR_STRING = "MPSOC_FLASH_READ_PACKET_ERROR"; +const char *MPSOC_FLASH_READ_FAILED_STRING = "MPSOC_FLASH_READ_FAILED"; +const char *MPSOC_FLASH_READ_SUCCESSFUL_STRING = "MPSOC_FLASH_READ_SUCCESSFUL"; const char *TRANSITION_BACK_TO_OFF_STRING = "TRANSITION_BACK_TO_OFF"; const char *NEG_V_OUT_OF_BOUNDS_STRING = "NEG_V_OUT_OF_BOUNDS"; const char *U_DRO_OUT_OF_BOUNDS_STRING = "U_DRO_OUT_OF_BOUNDS"; @@ -683,6 +686,12 @@ const char *translateEvents(Event event) { return MPSOC_TM_SIZE_ERROR_STRING; case (12613): return MPSOC_TM_CRC_MISSMATCH_STRING; + case (12614): + return MPSOC_FLASH_READ_PACKET_ERROR_STRING; + case (12615): + return MPSOC_FLASH_READ_FAILED_STRING; + case (12616): + return MPSOC_FLASH_READ_SUCCESSFUL_STRING; case (12700): return TRANSITION_BACK_TO_OFF_STRING; case (12701): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index 1c252ec5..458e8d67 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-04-17 11:34:19 + * Generated on: 2023-05-15 15:20:19 */ #include "translateObjects.h" diff --git a/tmtc b/tmtc index 280c7243..b8e1c7af 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 280c72439effa1b4290dc500dade2c62a9d6e3f7 +Subproject commit b8e1c7afe91bddfea2b139217320033f3a3b0efb From 78972cd173194b9a05151ea571f24f1448eb9631 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 16:16:17 +0200 Subject: [PATCH 161/506] remove some more printouts --- linux/payload/PlocMpsocSpecialComHelper.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/linux/payload/PlocMpsocSpecialComHelper.cpp b/linux/payload/PlocMpsocSpecialComHelper.cpp index 6bc338c6..d54090a4 100644 --- a/linux/payload/PlocMpsocSpecialComHelper.cpp +++ b/linux/payload/PlocMpsocSpecialComHelper.cpp @@ -181,7 +181,6 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { if (ofile.bad()) { return returnvalue::FAILED; } - sif::debug << "Sequence count: " << sequenceCount->get() << std::endl; ReturnValue_t result = flashfopen(mpsoc::FileAccessMode::READ); if (result != returnvalue::OK) { std::filesystem::remove(flashReadAndWrite.obcFile, e); From 3d7d01d6abc3cce17c9cee2280209aa94fb81d87 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 16:17:08 +0200 Subject: [PATCH 162/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 0c1bfc6f..280c7243 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 0c1bfc6fd32e66fe0da13bebc4eeb3030ead13a9 +Subproject commit 280c72439effa1b4290dc500dade2c62a9d6e3f7 From a615ed2d21cb7543249ecae2ecb3f7fb84dfd138 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 16:18:44 +0200 Subject: [PATCH 163/506] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 5eb9ee8b..4391823f 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 5eb9ee8bc1e6f8640cbea46febd11e4b92241881 +Subproject commit 4391823f01d792bcc078d47b60f7df7624f8cbe4 From 97a7087827e0430085fd5ff398a86a31c6c8dc34 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 16:45:14 +0200 Subject: [PATCH 164/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 280c7243..5f379bf2 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 280c72439effa1b4290dc500dade2c62a9d6e3f7 +Subproject commit 5f379bf2bb76f5cc65e9ca58036a4b239d8638b8 From ab6a0c3e4d4b52dc7e8d0c95d80747630fc41664 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 16:49:16 +0200 Subject: [PATCH 165/506] fix host build --- bsp_hosted/ObjectFactory.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index d792b57b..018d0e56 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -39,7 +39,7 @@ #include "devices/gpioIds.h" #include "fsfw_hal/linux/gpio/Gpio.h" #include "linux/payload/PlocMpsocHandler.h" -#include "linux/payload/PlocMpsocHelper.h" +#include "linux/payload/PlocMpsocSpecialComHelper.h" #include "linux/payload/PlocSupervisorHandler.h" #include "linux/payload/PlocSupvUartMan.h" #include "test/gpio/DummyGpioIF.h" @@ -82,8 +82,8 @@ void ObjectFactory::produce(void* args) { auto mpsocCookie = new UartCookie(objects::PLOC_MPSOC_HANDLER, mpscoDev, uart::PLOC_MPSOC_BAUD, mpsoc::MAX_REPLY_SIZE, UartModes::NON_CANONICAL); mpsocCookie->setNoFixedSizeReply(); - auto plocMpsocHelper = new PlocMPSoCHelper(objects::PLOC_MPSOC_HELPER); - new PlocMPSoCHandler(objects::PLOC_MPSOC_HANDLER, objects::UART_COM_IF, mpsocCookie, + auto plocMpsocHelper = new PlocMpsocSpecialComHelper(objects::PLOC_MPSOC_HELPER); + new PlocMpsocHandler(objects::PLOC_MPSOC_HANDLER, objects::UART_COM_IF, mpsocCookie, plocMpsocHelper, Gpio(gpioIds::ENABLE_MPSOC_UART, dummyGpioIF), objects::PLOC_SUPERVISOR_HANDLER); #endif /* OBSW_ADD_PLOC_MPSOC == 1 */ From 04b9ed450406ad1dbcc31a3ba8f28b5a63d80ea9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 16:53:18 +0200 Subject: [PATCH 166/506] add ACU dummy as well --- dummies/helperFactory.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/dummies/helperFactory.cpp b/dummies/helperFactory.cpp index 2b03ab7f..d7fdafbd 100644 --- a/dummies/helperFactory.cpp +++ b/dummies/helperFactory.cpp @@ -80,6 +80,7 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio if (cfg.addOnlyAcuDummy) { new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); } else if (cfg.addPowerDummies) { + new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new PduDummy(objects::PDU1_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new PduDummy(objects::PDU2_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new P60DockDummy(objects::P60DOCK_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); From b39e448ab555f65d8cf2d70c82c6d30455cfc896 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 16 May 2023 08:19:30 +0200 Subject: [PATCH 167/506] correction for flashfclose cmd --- linux/payload/plocMpsocHelpers.h | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/linux/payload/plocMpsocHelpers.h b/linux/payload/plocMpsocHelpers.h index a7173d65..702df949 100644 --- a/linux/payload/plocMpsocHelpers.h +++ b/linux/payload/plocMpsocHelpers.h @@ -419,18 +419,6 @@ class FlashFclose : public ploc::SpTcBase { public: FlashFclose(ploc::SpTcParams params, uint16_t sequenceCount) : ploc::SpTcBase(params, apid::TC_FLASHFCLOSE, sequenceCount) {} - - ReturnValue_t createPacket(std::string filename) { - size_t nameSize = filename.size(); - spParams.setFullPayloadLen(nameSize + sizeof(NULL_TERMINATOR) + CRC_SIZE); - ReturnValue_t result = checkPayloadLen(); - if (result != returnvalue::OK) { - return result; - } - std::memcpy(payloadStart, filename.c_str(), nameSize); - *(payloadStart + nameSize) = NULL_TERMINATOR; - return calcAndSetCrc(); - } }; /** From 468fa016504496c1adbe5df01ca614cd961d5517 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 16 May 2023 11:08:33 +0200 Subject: [PATCH 168/506] bugfix --- linux/payload/PlocMpsocSpecialComHelper.cpp | 6 ++---- linux/payload/plocMpsocHelpers.h | 20 ++++++++++---------- 2 files changed, 12 insertions(+), 14 deletions(-) diff --git a/linux/payload/PlocMpsocSpecialComHelper.cpp b/linux/payload/PlocMpsocSpecialComHelper.cpp index d54090a4..f8b81603 100644 --- a/linux/payload/PlocMpsocSpecialComHelper.cpp +++ b/linux/payload/PlocMpsocSpecialComHelper.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -249,10 +250,6 @@ ReturnValue_t PlocMpsocSpecialComHelper::flashfclose() { spParams.buf = commandBuffer; (*sequenceCount)++; mpsoc::FlashFclose flashFclose(spParams, *sequenceCount); - ReturnValue_t result = flashFclose.createPacket(flashReadAndWrite.mpsocFile); - if (result != returnvalue::OK) { - return result; - } return handlePacketTransmissionNoReply(flashFclose); } @@ -287,6 +284,7 @@ ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionNoReply(ploc::S ReturnValue_t PlocMpsocSpecialComHelper::sendCommand(ploc::SpTcBase& tc) { ReturnValue_t result = returnvalue::OK; + arrayprinter::print(tc.getFullPacket(), tc.getFullPacketLen()); result = uartComIF->sendMessage(comCookie, tc.getFullPacket(), tc.getFullPacketLen()); if (result != returnvalue::OK) { sif::warning << "PlocMPSoCHelper::sendCommand: Failed to send command" << std::endl; diff --git a/linux/payload/plocMpsocHelpers.h b/linux/payload/plocMpsocHelpers.h index 702df949..c65d20e0 100644 --- a/linux/payload/plocMpsocHelpers.h +++ b/linux/payload/plocMpsocHelpers.h @@ -387,10 +387,10 @@ class TcMemWrite : public TcBase { /** * @brief Class to help creation of flash fopen command. */ -class FlashFopen : public ploc::SpTcBase { +class FlashFopen : public TcBase { public: FlashFopen(ploc::SpTcParams params, uint16_t sequenceCount) - : ploc::SpTcBase(params, apid::TC_FLASHFOPEN, sequenceCount) {} + : TcBase(params, apid::TC_FLASHFOPEN, sequenceCount) {} ReturnValue_t createPacket(std::string filename, FileAccessMode mode) { accessMode = mode; @@ -415,19 +415,19 @@ class FlashFopen : public ploc::SpTcBase { /** * @brief Class to help creation of flash fclose command. */ -class FlashFclose : public ploc::SpTcBase { +class FlashFclose : public TcBase { public: FlashFclose(ploc::SpTcParams params, uint16_t sequenceCount) - : ploc::SpTcBase(params, apid::TC_FLASHFCLOSE, sequenceCount) {} + : TcBase(params, apid::TC_FLASHFCLOSE, sequenceCount) {} }; /** * @brief Class to build flash write space packet. */ -class TcFlashWrite : public ploc::SpTcBase { +class TcFlashWrite : public TcBase { public: TcFlashWrite(ploc::SpTcParams params, uint16_t sequenceCount) - : ploc::SpTcBase(params, apid::TC_FLASHWRITE, sequenceCount) {} + : TcBase(params, apid::TC_FLASHWRITE, sequenceCount) {} ReturnValue_t buildPacket(const uint8_t* writeData, uint32_t writeLen_) { ReturnValue_t result = returnvalue::OK; @@ -460,10 +460,10 @@ class TcFlashWrite : public ploc::SpTcBase { uint32_t writeLen = 0; }; -class TcFlashRead : public ploc::SpTcBase { +class TcFlashRead : public TcBase { public: TcFlashRead(ploc::SpTcParams params, uint16_t sequenceCount) - : ploc::SpTcBase(params, apid::TC_FLASHWRITE, sequenceCount) {} + : TcBase(params, apid::TC_FLASHWRITE, sequenceCount) {} ReturnValue_t buildPacket(uint32_t readLen) { if (readLen > MAX_FLASH_READ_DATA_SIZE) { @@ -500,10 +500,10 @@ class TcFlashRead : public ploc::SpTcBase { /** * @brief Class to help creation of flash delete command. */ -class TcFlashDelete : public ploc::SpTcBase { +class TcFlashDelete : public TcBase { public: TcFlashDelete(ploc::SpTcParams params, uint16_t sequenceCount) - : ploc::SpTcBase(params, apid::TC_FLASHDELETE, sequenceCount) {} + : TcBase(params, apid::TC_FLASHDELETE, sequenceCount) {} ReturnValue_t buildPacket(std::string filename) { size_t nameSize = filename.size(); From 8b467ec69e301690b1087968451bdcd61fb7c8be Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 16 May 2023 11:36:19 +0200 Subject: [PATCH 169/506] dock hk improvements --- mission/power/P60DockHandler.cpp | 4 +++- mission/power/gsDefs.h | 3 ++- tmtc | 2 +- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/mission/power/P60DockHandler.cpp b/mission/power/P60DockHandler.cpp index d643aef1..77fd9aaf 100644 --- a/mission/power/P60DockHandler.cpp +++ b/mission/power/P60DockHandler.cpp @@ -66,7 +66,7 @@ void P60DockHandler::parseHkTableReply(const uint8_t *packet) { } coreHk.battMode = newBattMode; - auxHk.heaterOn = *(packet + 0x57); + auxHk.heaterForBp4PackOn = *(packet + 0x57); auxHk.converter5VStatus = *(packet + 0x58); for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) { @@ -111,6 +111,8 @@ void P60DockHandler::parseHkTableReply(const uint8_t *packet) { } coreHk.setValidity(true, true); auxHk.setValidity(true, true); + // No BP4 pack, no this is always invalid. + auxHk.heaterForBp4PackOn.setValid(false); } ReturnValue_t P60DockHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, diff --git a/mission/power/gsDefs.h b/mission/power/gsDefs.h index d42cabd2..bbe9911f 100644 --- a/mission/power/gsDefs.h +++ b/mission/power/gsDefs.h @@ -260,7 +260,8 @@ class HkTableDataset : public StaticLocalDataSet<32> { lp_var_t resetcause = lp_var_t(sid.objectId, pool::P60DOCK_RESETCAUSE, this); /** Battery heater control only possible on BP4 packs */ - lp_var_t heaterOn = lp_var_t(sid.objectId, pool::P60DOCK_HEATER_ON, this); + lp_var_t heaterForBp4PackOn = + lp_var_t(sid.objectId, pool::P60DOCK_HEATER_ON, this); lp_var_t converter5VStatus = lp_var_t(sid.objectId, pool::P60DOCK_CONV_5V_ENABLE_STATUS, this); diff --git a/tmtc b/tmtc index 0c1bfc6f..280c7243 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 0c1bfc6fd32e66fe0da13bebc4eeb3030ead13a9 +Subproject commit 280c72439effa1b4290dc500dade2c62a9d6e3f7 From 4ba9ebf58f15f1c4db5b87c4c5a08ad3f65b8b7a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 16 May 2023 13:34:06 +0200 Subject: [PATCH 170/506] kenning west alder --- linux/payload/PlocMpsocHandler.cpp | 103 +++++++------------- linux/payload/PlocMpsocHandler.h | 2 +- linux/payload/PlocMpsocSpecialComHelper.cpp | 14 ++- linux/payload/plocMpsocHelpers.h | 68 ++++--------- 4 files changed, 65 insertions(+), 122 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 89e9a248..856f888d 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -562,11 +562,11 @@ ReturnValue_t PlocMpsocHandler::prepareTcMemWrite(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; mpsoc::TcMemWrite tcMemWrite(spParams, sequenceCount); - result = tcMemWrite.buildPacket(commandData, commandDataLen); + result = tcMemWrite.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcMemWrite.getFullPacketLen()); + finishTcPrep(tcMemWrite); return returnvalue::OK; } @@ -574,11 +574,11 @@ ReturnValue_t PlocMpsocHandler::prepareTcMemRead(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; mpsoc::TcMemRead tcMemRead(spParams, sequenceCount); - result = tcMemRead.buildPacket(commandData, commandDataLen); + result = tcMemRead.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcMemRead.getFullPacketLen()); + finishTcPrep(tcMemRead); tmMemReadReport.rememberRequestedSize = tcMemRead.getMemLen() * 4 + TmMemReadReport::FIX_SIZE; return returnvalue::OK; } @@ -590,12 +590,12 @@ ReturnValue_t PlocMpsocHandler::prepareTcFlashDelete(const uint8_t* commandData, } ReturnValue_t result = returnvalue::OK; mpsoc::TcFlashDelete tcFlashDelete(spParams, sequenceCount); - result = tcFlashDelete.buildPacket( - std::string(reinterpret_cast(commandData), commandDataLen)); + std::string filename = std::string(reinterpret_cast(commandData), commandDataLen); + result = tcFlashDelete.setPayload(filename); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcFlashDelete.getFullPacketLen()); + finishTcPrep(tcFlashDelete); return returnvalue::OK; } @@ -603,22 +603,17 @@ ReturnValue_t PlocMpsocHandler::prepareTcReplayStart(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; mpsoc::TcReplayStart tcReplayStart(spParams, sequenceCount); - result = tcReplayStart.buildPacket(commandData, commandDataLen); + result = tcReplayStart.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcReplayStart.getFullPacketLen()); + finishTcPrep(tcReplayStart); return returnvalue::OK; } ReturnValue_t PlocMpsocHandler::prepareTcReplayStop() { - ReturnValue_t result = returnvalue::OK; mpsoc::TcReplayStop tcReplayStop(spParams, sequenceCount); - result = tcReplayStop.buildPacket(); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(tcReplayStop.getFullPacketLen()); + finishTcPrep(tcReplayStop); return returnvalue::OK; } @@ -626,78 +621,49 @@ ReturnValue_t PlocMpsocHandler::prepareTcDownlinkPwrOn(const uint8_t* commandDat size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; mpsoc::TcDownlinkPwrOn tcDownlinkPwrOn(spParams, sequenceCount); - result = tcDownlinkPwrOn.buildPacket(commandData, commandDataLen); + result = tcDownlinkPwrOn.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcDownlinkPwrOn.getFullPacketLen()); + finishTcPrep(tcDownlinkPwrOn); return returnvalue::OK; } ReturnValue_t PlocMpsocHandler::prepareTcDownlinkPwrOff() { - ReturnValue_t result = returnvalue::OK; mpsoc::TcDownlinkPwrOff tcDownlinkPwrOff(spParams, sequenceCount); - result = tcDownlinkPwrOff.buildPacket(); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(tcDownlinkPwrOff.getFullPacketLen()); + finishTcPrep(tcDownlinkPwrOff); return returnvalue::OK; } ReturnValue_t PlocMpsocHandler::prepareTcGetHkReport() { mpsoc::TcGetHkReport tcGetHkReport(spParams, sequenceCount); - ReturnValue_t result = tcGetHkReport.buildPacket(); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(tcGetHkReport.getFullPacketLen()); + finishTcPrep(tcGetHkReport); return returnvalue::OK; } ReturnValue_t PlocMpsocHandler::prepareTcReplayWriteSequence(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = returnvalue::OK; mpsoc::TcReplayWriteSeq tcReplayWriteSeq(spParams, sequenceCount); - result = tcReplayWriteSeq.buildPacket(commandData, commandDataLen); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(tcReplayWriteSeq.getFullPacketLen()); + finishTcPrep(tcReplayWriteSeq); return returnvalue::OK; } ReturnValue_t PlocMpsocHandler::prepareTcModeReplay() { - ReturnValue_t result = returnvalue::OK; mpsoc::TcModeReplay tcModeReplay(spParams, sequenceCount); - result = tcModeReplay.buildPacket(); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(tcModeReplay.getFullPacketLen()); + finishTcPrep(tcModeReplay); return returnvalue::OK; } ReturnValue_t PlocMpsocHandler::prepareTcModeIdle() { - ReturnValue_t result = returnvalue::OK; mpsoc::TcModeIdle tcModeIdle(spParams, sequenceCount); - result = tcModeIdle.buildPacket(); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(tcModeIdle.getFullPacketLen()); + finishTcPrep(tcModeIdle); return returnvalue::OK; } ReturnValue_t PlocMpsocHandler::prepareTcCamCmdSend(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = returnvalue::OK; mpsoc::TcCamcmdSend tcCamCmdSend(spParams, sequenceCount); - result = tcCamCmdSend.buildPacket(commandData, commandDataLen); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(tcCamCmdSend.getFullPacketLen()); + finishTcPrep(tcCamCmdSend); nextReplyId = mpsoc::TM_CAM_CMD_RPT; return returnvalue::OK; } @@ -705,62 +671,59 @@ ReturnValue_t PlocMpsocHandler::prepareTcCamCmdSend(const uint8_t* commandData, ReturnValue_t PlocMpsocHandler::prepareTcCamTakePic(const uint8_t* commandData, size_t commandDataLen) { mpsoc::TcCamTakePic tcCamTakePic(spParams, sequenceCount); - ReturnValue_t result = tcCamTakePic.buildPacket(commandData, commandDataLen); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(tcCamTakePic.getFullPacketLen()); + finishTcPrep(tcCamTakePic); return returnvalue::OK; } ReturnValue_t PlocMpsocHandler::prepareTcSimplexSendFile(const uint8_t* commandData, size_t commandDataLen) { mpsoc::TcSimplexSendFile tcSimplexSendFile(spParams, sequenceCount); - ReturnValue_t result = tcSimplexSendFile.buildPacket(commandData, commandDataLen); + ReturnValue_t result = tcSimplexSendFile.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcSimplexSendFile.getFullPacketLen()); + finishTcPrep(tcSimplexSendFile); return returnvalue::OK; } ReturnValue_t PlocMpsocHandler::prepareTcGetDirContent(const uint8_t* commandData, size_t commandDataLen) { mpsoc::TcGetDirContent tcGetDirContent(spParams, sequenceCount); - ReturnValue_t result = tcGetDirContent.buildPacket(commandData, commandDataLen); + ReturnValue_t result = tcGetDirContent.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcGetDirContent.getFullPacketLen()); + finishTcPrep(tcGetDirContent); return returnvalue::OK; } ReturnValue_t PlocMpsocHandler::prepareTcDownlinkDataModulate(const uint8_t* commandData, size_t commandDataLen) { mpsoc::TcDownlinkDataModulate tcDownlinkDataModulate(spParams, sequenceCount); - ReturnValue_t result = tcDownlinkDataModulate.buildPacket(commandData, commandDataLen); + ReturnValue_t result = tcDownlinkDataModulate.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcDownlinkDataModulate.getFullPacketLen()); + finishTcPrep(tcDownlinkDataModulate); return returnvalue::OK; } ReturnValue_t PlocMpsocHandler::prepareTcModeSnapshot() { mpsoc::TcModeSnapshot tcModeSnapshot(spParams, sequenceCount); - ReturnValue_t result = tcModeSnapshot.buildPacket(); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(tcModeSnapshot.getFullPacketLen()); + finishTcPrep(tcModeSnapshot); return returnvalue::OK; } -void PlocMpsocHandler::finishTcPrep(size_t packetLen) { +ReturnValue_t PlocMpsocHandler::finishTcPrep(mpsoc::TcBase& tcBase) { nextReplyId = mpsoc::ACK_REPORT; + ReturnValue_t result = tcBase.finishPacket(); + if (result != returnvalue::OK) { + return result; + } rawPacket = commandBuffer; - rawPacketLen = packetLen; + rawPacketLen = tcBase.getFullPacketLen(); sequenceCount++; + return returnvalue::OK; } ReturnValue_t PlocMpsocHandler::verifyPacket(const uint8_t* start, size_t foundLen) { diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index ea62e484..e0474a33 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -214,7 +214,7 @@ class PlocMpsocHandler : public DeviceHandlerBase, public CommandsActionsIF { ReturnValue_t prepareTcSimplexSendFile(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcDownlinkDataModulate(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcModeSnapshot(); - void finishTcPrep(size_t packetLen); + ReturnValue_t finishTcPrep(mpsoc::TcBase& tcBase); /** * @brief This function checks the crc of the received PLOC reply. diff --git a/linux/payload/PlocMpsocSpecialComHelper.cpp b/linux/payload/PlocMpsocSpecialComHelper.cpp index f8b81603..78fa430f 100644 --- a/linux/payload/PlocMpsocSpecialComHelper.cpp +++ b/linux/payload/PlocMpsocSpecialComHelper.cpp @@ -207,7 +207,13 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { } (*sequenceCount)++; mpsoc::TcFlashRead flashReadRequest(spParams, *sequenceCount); - result = flashReadRequest.buildPacket(nextReadSize); + result = flashReadRequest.setPayload(nextReadSize); + if (result != returnvalue::OK) { + std::filesystem::remove(flashReadAndWrite.obcFile, e); + flashfclose(); + return result; + } + result = flashReadRequest.finishPacket(); if (result != returnvalue::OK) { std::filesystem::remove(flashReadAndWrite.obcFile, e); flashfclose(); @@ -235,7 +241,11 @@ ReturnValue_t PlocMpsocSpecialComHelper::flashfopen(mpsoc::FileAccessMode mode) spParams.buf = commandBuffer; (*sequenceCount)++; mpsoc::FlashFopen flashFopen(spParams, *sequenceCount); - ReturnValue_t result = flashFopen.createPacket(flashReadAndWrite.mpsocFile, mode); + ReturnValue_t result = flashFopen.setPayload(flashReadAndWrite.mpsocFile, mode); + if (result != returnvalue::OK) { + return result; + } + result = flashFopen.finishPacket(); if (result != returnvalue::OK) { return result; } diff --git a/linux/payload/plocMpsocHelpers.h b/linux/payload/plocMpsocHelpers.h index c65d20e0..c896be3d 100644 --- a/linux/payload/plocMpsocHelpers.h +++ b/linux/payload/plocMpsocHelpers.h @@ -239,47 +239,23 @@ class TcBase : public ploc::SpTcBase, public MPSoCReturnValuesIF { */ TcBase(ploc::SpTcParams params, uint16_t apid, uint16_t sequenceCount) : ploc::SpTcBase(params, apid, 0, sequenceCount) { + payloadStart = spParams.buf + ccsds::HEADER_LEN; spParams.setFullPayloadLen(INIT_LENGTH); } - ReturnValue_t buildPacket() { return buildPacket(nullptr, 0); } - /** - * @brief Function to initialize the space packet - * - * @param commandData Pointer to command specific data - * @param commandDataLen Length of command data - * + * @brief Function to finsh and write the space packet. It is expected that the user has + * set the payload fields in the child class* * @return returnvalue::OK if packet creation was successful, otherwise error return value */ - ReturnValue_t buildPacket(const uint8_t* commandData, size_t commandDataLen) { - payloadStart = spParams.buf + ccsds::HEADER_LEN; - ReturnValue_t res; - if (commandData != nullptr and commandDataLen > 0) { - res = initPacket(commandData, commandDataLen); - if (res != returnvalue::OK) { - return res; - } - } - + ReturnValue_t finishPacket() { updateSpFields(); - res = checkSizeAndSerializeHeader(); + ReturnValue_t res = checkSizeAndSerializeHeader(); if (res != returnvalue::OK) { return res; } return calcAndSetCrc(); } - - protected: - /** - * @brief Must be overwritten by the child class to define the command specific parameters - * - * @param commandData Pointer to received command data - * @param commandDataLen Length of received command data - */ - virtual ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) { - return returnvalue::OK; - } }; /** @@ -297,8 +273,7 @@ class TcMemRead : public TcBase { uint16_t getMemLen() const { return memLen; } - protected: - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; result = lengthCheck(commandDataLen); if (result != returnvalue::OK) { @@ -344,8 +319,7 @@ class TcMemWrite : public TcBase { TcMemWrite(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_MEM_WRITE, sequenceCount) {} - protected: - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; result = lengthCheck(commandDataLen); if (result != returnvalue::OK) { @@ -392,7 +366,7 @@ class FlashFopen : public TcBase { FlashFopen(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_FLASHFOPEN, sequenceCount) {} - ReturnValue_t createPacket(std::string filename, FileAccessMode mode) { + ReturnValue_t setPayload(std::string filename, FileAccessMode mode) { accessMode = mode; size_t nameSize = filename.size(); spParams.setFullPayloadLen(256 + sizeof(uint8_t) + CRC_SIZE); @@ -404,8 +378,7 @@ class FlashFopen : public TcBase { payloadStart[nameSize] = NULL_TERMINATOR; payloadStart[255] = NULL_TERMINATOR; payloadStart[256] = static_cast(accessMode); - updateSpFields(); - return calcAndSetCrc(); + return returnvalue::OK; } private: @@ -465,7 +438,7 @@ class TcFlashRead : public TcBase { TcFlashRead(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_FLASHWRITE, sequenceCount) {} - ReturnValue_t buildPacket(uint32_t readLen) { + ReturnValue_t setPayload(uint32_t readLen) { if (readLen > MAX_FLASH_READ_DATA_SIZE) { sif::error << "TcFlashRead: Read length " << readLen << " too large" << std::endl; return returnvalue::FAILED; @@ -505,7 +478,7 @@ class TcFlashDelete : public TcBase { TcFlashDelete(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_FLASHDELETE, sequenceCount) {} - ReturnValue_t buildPacket(std::string filename) { + ReturnValue_t setPayload(std::string filename) { size_t nameSize = filename.size(); spParams.setFullPayloadLen(nameSize + sizeof(NULL_TERMINATOR) + CRC_SIZE); auto res = checkPayloadLen(); @@ -544,8 +517,7 @@ class TcReplayStart : public TcBase { TcReplayStart(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_REPLAY_START, sequenceCount) {} - protected: - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; spParams.setFullPayloadLen(commandDataLen + CRC_SIZE); result = lengthCheck(commandDataLen); @@ -593,8 +565,7 @@ class TcDownlinkPwrOn : public TcBase { TcDownlinkPwrOn(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_DOWNLINK_PWR_ON, sequenceCount) {} - protected: - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; result = lengthCheck(commandDataLen); if (result != returnvalue::OK) { @@ -667,7 +638,7 @@ class TcGetDirContent : public TcBase { TcGetDirContent(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_FLASH_GET_DIRECTORY_CONTENT, sequenceCount) {} - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; // Yeah it needs to be 256.. even if the path is shorter. spParams.setFullPayloadLen(256 + CRC_SIZE); @@ -700,8 +671,7 @@ class TcReplayWriteSeq : public TcBase { TcReplayWriteSeq(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_REPLAY_WRITE_SEQUENCE, sequenceCount) {} - protected: - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; spParams.setFullPayloadLen(commandDataLen + sizeof(NULL_TERMINATOR) + CRC_SIZE); result = lengthCheck(commandDataLen); @@ -828,7 +798,7 @@ class TcCamTakePic : public TcBase { TcCamTakePic(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_CAM_TAKE_PIC, sequenceCount) {} - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { if (commandDataLen > MAX_DATA_LENGTH) { return INVALID_LENGTH; } @@ -857,7 +827,7 @@ class TcSimplexSendFile : public TcBase { TcSimplexSendFile(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_SIMPLEX_SEND_FILE, sequenceCount) {} - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { if (commandDataLen > MAX_DATA_LENGTH) { return INVALID_LENGTH; } @@ -882,7 +852,7 @@ class TcDownlinkDataModulate : public TcBase { TcDownlinkDataModulate(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_DOWNLINK_DATA_MODULATE, sequenceCount) {} - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { if (commandDataLen > MAX_DATA_LENGTH) { return INVALID_LENGTH; } @@ -901,7 +871,7 @@ class TcCamcmdSend : public TcBase { : TcBase(params, apid::TC_CAM_CMD_SEND, sequenceCount) {} protected: - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { if (commandDataLen > MAX_DATA_LENGTH) { return INVALID_LENGTH; } From 90b7f069dc3824288b2352b3032216c7a71fa534 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 16 May 2023 18:41:04 +0200 Subject: [PATCH 171/506] it is getting annoying again --- linux/payload/PlocMpsocHandler.cpp | 17 +- linux/payload/PlocMpsocSpecialComHelper.cpp | 177 ++++++++++++-------- linux/payload/PlocMpsocSpecialComHelper.h | 15 +- linux/payload/plocMpsocHelpers.h | 8 +- 4 files changed, 136 insertions(+), 81 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 856f888d..4167b68c 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -136,7 +136,6 @@ ReturnValue_t PlocMpsocHandler::executeAction(ActionId_t actionId, MessageQueueI if (result != returnvalue::OK) { return result; } - specialComHelper->setSequenceCount(&sequenceCount); result = specialComHelper->startFlashWrite(flashWritePusCmd.getObcFile(), flashWritePusCmd.getMPSoCFile()); if (result != returnvalue::OK) { @@ -151,7 +150,6 @@ ReturnValue_t PlocMpsocHandler::executeAction(ActionId_t actionId, MessageQueueI if (result != returnvalue::OK) { return result; } - specialComHelper->setSequenceCount(&sequenceCount); result = specialComHelper->startFlashRead(flashReadPusCmd.getObcFile(), flashReadPusCmd.getMPSoCFile(), flashReadPusCmd.getReadSize()); @@ -226,9 +224,8 @@ void PlocMpsocHandler::doShutDown() { uartIsolatorSwitch.pullLow(); commandActionHelper.commandAction(supervisorHandler, supv::SHUTDOWN_MPSOC); powerState = PowerState::SHUTDOWN; - break; + return; case PowerState::OFF: - sequenceCount = 0; hkReport.setReportingEnabled(false); setMode(_MODE_POWER_DOWN); break; @@ -236,13 +233,19 @@ void PlocMpsocHandler::doShutDown() { break; } #else - sequenceCount = 0; uartIsolatorSwitch.pullLow(); - hkReport.setReportingEnabled(false); - setMode(_MODE_POWER_DOWN); powerState = PowerState::OFF; #endif #endif + + if (specialComHelper != nullptr) { + specialComHelper->stopProcess(); + } + hkReport.setReportingEnabled(false); + setMode(_MODE_POWER_DOWN); + commandIsPending = false; + sequenceCount = 0; + plocMPSoCHelperExecuting = false; startupState = StartupState::IDLE; } diff --git a/linux/payload/PlocMpsocSpecialComHelper.cpp b/linux/payload/PlocMpsocSpecialComHelper.cpp index 78fa430f..7b3cf264 100644 --- a/linux/payload/PlocMpsocSpecialComHelper.cpp +++ b/linux/payload/PlocMpsocSpecialComHelper.cpp @@ -1,5 +1,7 @@ #include +#include #include +#include #include #include @@ -40,16 +42,15 @@ ReturnValue_t PlocMpsocSpecialComHelper::performOperation(uint8_t operationCode) #endif switch (internalState) { case InternalState::IDLE: { - sif::debug << "ploc mpsoc helper idle" << std::endl; semaphore.acquire(); break; } case InternalState::FLASH_WRITE: { result = performFlashWrite(); if (result == returnvalue::OK) { - triggerEvent(MPSOC_FLASH_WRITE_SUCCESSFUL); + triggerEvent(MPSOC_FLASH_WRITE_SUCCESSFUL, sequenceCount->get()); } else { - triggerEvent(MPSOC_FLASH_WRITE_FAILED); + triggerEvent(MPSOC_FLASH_WRITE_FAILED, sequenceCount->get()); } internalState = InternalState::IDLE; break; @@ -57,9 +58,9 @@ ReturnValue_t PlocMpsocSpecialComHelper::performOperation(uint8_t operationCode) case InternalState::FLASH_READ: { result = performFlashRead(); if (result == returnvalue::OK) { - triggerEvent(MPSOC_FLASH_READ_SUCCESSFUL); + triggerEvent(MPSOC_FLASH_READ_SUCCESSFUL, sequenceCount->get()); } else { - triggerEvent(MPSOC_FLASH_READ_FAILED); + triggerEvent(MPSOC_FLASH_READ_FAILED, sequenceCount->get()); } internalState = InternalState::IDLE; break; @@ -155,13 +156,18 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashWrite() { file.read(reinterpret_cast(fileBuf.data()), dataLength); bytesRead += dataLength; remainingSize -= dataLength; - (*sequenceCount)++; mpsoc::TcFlashWrite tc(spParams, *sequenceCount); - result = tc.buildPacket(fileBuf.data(), dataLength); + result = tc.setPayload(fileBuf.data(), dataLength); if (result != returnvalue::OK) { flashfclose(); return result; } + result = tc.finishPacket(); + if (result != returnvalue::OK) { + flashfclose(); + return result; + } + (*sequenceCount)++; result = handlePacketTransmissionNoReply(tc); if (result != returnvalue::OK) { flashfclose(); @@ -205,7 +211,6 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { flashfclose(); return FILE_READ_ERROR; } - (*sequenceCount)++; mpsoc::TcFlashRead flashReadRequest(spParams, *sequenceCount); result = flashReadRequest.setPayload(nextReadSize); if (result != returnvalue::OK) { @@ -219,15 +224,11 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { flashfclose(); return result; } - result = handlePacketTransmissionFlashRead(flashReadRequest); - if (result != returnvalue::OK) { - std::filesystem::remove(flashReadAndWrite.obcFile, e); - flashfclose(); - return result; - } - result = handleFlashReadReply(ofile, nextReadSize); + (*sequenceCount)++; + result = handlePacketTransmissionFlashRead(flashReadRequest, ofile, nextReadSize); if (result != returnvalue::OK) { std::filesystem::remove(flashReadAndWrite.obcFile, e); + sif::debug << "flash close" << std::endl; flashfclose(); return result; } @@ -239,7 +240,6 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { ReturnValue_t PlocMpsocSpecialComHelper::flashfopen(mpsoc::FileAccessMode mode) { spParams.buf = commandBuffer; - (*sequenceCount)++; mpsoc::FlashFopen flashFopen(spParams, *sequenceCount); ReturnValue_t result = flashFopen.setPayload(flashReadAndWrite.mpsocFile, mode); if (result != returnvalue::OK) { @@ -249,6 +249,7 @@ ReturnValue_t PlocMpsocSpecialComHelper::flashfopen(mpsoc::FileAccessMode mode) if (result != returnvalue::OK) { return result; } + (*sequenceCount)++; result = handlePacketTransmissionNoReply(flashFopen); if (result != returnvalue::OK) { return result; @@ -258,12 +259,22 @@ ReturnValue_t PlocMpsocSpecialComHelper::flashfopen(mpsoc::FileAccessMode mode) ReturnValue_t PlocMpsocSpecialComHelper::flashfclose() { spParams.buf = commandBuffer; - (*sequenceCount)++; mpsoc::FlashFclose flashFclose(spParams, *sequenceCount); - return handlePacketTransmissionNoReply(flashFclose); + ReturnValue_t result = flashFclose.finishPacket(); + if (result != returnvalue::OK) { + return result; + } + (*sequenceCount)++; + result = handlePacketTransmissionNoReply(flashFclose); + if (result != returnvalue::OK) { + return result; + } + return result; } -ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionFlashRead(mpsoc::TcFlashRead& tc) { +ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionFlashRead(mpsoc::TcFlashRead& tc, + std::ofstream& ofile, + size_t expectedReadLen) { ReturnValue_t result = sendCommand(tc); if (result != returnvalue::OK) { return result; @@ -272,12 +283,29 @@ ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionFlashRead(mpsoc if (result != returnvalue::OK) { return result; } - result = handleTmReception(ccsds::HEADER_LEN + mpsoc::FLASH_READ_MIN_OVERHEAD + tc.readSize + - mpsoc::CRC_SIZE); + // ccsds::HEADER_LEN + mpsoc::FLASH_READ_MIN_OVERHEAD + tc.readSize + mpsoc::CRC_SIZE + result = handleTmReception(); if (result != returnvalue::OK) { return result; } - return handleExe(); + + // We have the nominal case where the flash read report appears first, or the case where we + // get an EXE failure immediately. + if (spReader.getApid() == mpsoc::apid::TM_FLASH_READ_REPORT) { + result = handleFlashReadReply(ofile, expectedReadLen); + if (result != returnvalue::OK) { + return result; + } + return handleExe(); + } else if (spReader.getApid() == mpsoc::apid::EXE_FAILURE) { + handleExeFailure(); + } else { + triggerEvent(MPSOC_EXE_INVALID_APID, spReader.getApid(), static_cast(internalState)); + sif::warning << "PLOC MPSoC: Expected execution report " + << "but received space packet with apid " << std::hex << spReader.getApid() + << std::endl; + } + return returnvalue::FAILED; } ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionNoReply(ploc::SpTcBase& tc) { @@ -294,7 +322,6 @@ ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionNoReply(ploc::S ReturnValue_t PlocMpsocSpecialComHelper::sendCommand(ploc::SpTcBase& tc) { ReturnValue_t result = returnvalue::OK; - arrayprinter::print(tc.getFullPacket(), tc.getFullPacketLen()); result = uartComIF->sendMessage(comCookie, tc.getFullPacket(), tc.getFullPacketLen()); if (result != returnvalue::OK) { sif::warning << "PlocMPSoCHelper::sendCommand: Failed to send command" << std::endl; @@ -306,12 +333,12 @@ ReturnValue_t PlocMpsocSpecialComHelper::sendCommand(ploc::SpTcBase& tc) { ReturnValue_t PlocMpsocSpecialComHelper::handleAck() { ReturnValue_t result = returnvalue::OK; - result = handleTmReception(mpsoc::SIZE_ACK_REPORT); + result = handleTmReception(); if (result != returnvalue::OK) { return result; } SpTmReader tmPacket(tmBuf.data(), tmBuf.size()); - result = checkReceivedTm(tmPacket); + result = checkReceivedTm(); if (result != returnvalue::OK) { return result; } @@ -327,7 +354,7 @@ void PlocMpsocSpecialComHelper::handleAckApidFailure(const ploc::SpTmReader& rea uint16_t apid = reader.getApid(); if (apid == mpsoc::apid::ACK_FAILURE) { uint16_t status = mpsoc::getStatusFromRawData(reader.getFullData()); - sif::warning << "MPSoC ACK Failure: " << mpsoc::getStatusString(status) << std::endl; + sif::warning << "PLOC MPSoC ACK Failure: " << mpsoc::getStatusString(status) << std::endl; triggerEvent(MPSOC_ACK_FAILURE_REPORT, static_cast(internalState), status); } else { triggerEvent(MPSOC_ACK_INVALID_APID, apid, static_cast(internalState)); @@ -339,74 +366,93 @@ void PlocMpsocSpecialComHelper::handleAckApidFailure(const ploc::SpTmReader& rea ReturnValue_t PlocMpsocSpecialComHelper::handleExe() { ReturnValue_t result = returnvalue::OK; - result = handleTmReception(mpsoc::SIZE_EXE_REPORT); + result = handleTmReception(); if (result != returnvalue::OK) { return result; } - ploc::SpTmReader tmPacket(tmBuf.data(), tmBuf.size()); - result = checkReceivedTm(tmPacket); + result = checkReceivedTm(); if (result != returnvalue::OK) { return result; } - uint16_t apid = tmPacket.getApid(); - if (apid != mpsoc::apid::EXE_SUCCESS) { - handleExeApidFailure(tmPacket); + uint16_t apid = spReader.getApid(); + if (apid == mpsoc::apid::EXE_FAILURE) { + handleExeFailure(); return returnvalue::FAILED; + } else if (apid != mpsoc::apid::EXE_SUCCESS) { + triggerEvent(MPSOC_EXE_INVALID_APID, apid, static_cast(internalState)); + sif::warning << "PLOC MPSoC: Expected execution report " + << "but received space packet with apid " << std::hex << apid << std::endl; } return returnvalue::OK; } -void PlocMpsocSpecialComHelper::handleExeApidFailure(const ploc::SpTmReader& reader) { - uint16_t apid = reader.getApid(); - if (apid == mpsoc::apid::EXE_FAILURE) { - uint16_t status = mpsoc::getStatusFromRawData(reader.getFullData()); - sif::warning << "MPSoC EXE Failure: " << mpsoc::getStatusString(status) << std::endl; - triggerEvent(MPSOC_EXE_FAILURE_REPORT, static_cast(internalState)); - } else { - triggerEvent(MPSOC_EXE_INVALID_APID, apid, static_cast(internalState)); - sif::warning << "PlocMPSoCHelper::handleExeApidFailure: Expected execution report " - << "but received space packet with apid " << std::hex << apid << std::endl; - } +void PlocMpsocSpecialComHelper::handleExeFailure() { + uint16_t status = mpsoc::getStatusFromRawData(spReader.getFullData()); + sif::warning << "PLOC MPSoC EXE Failure: " << mpsoc::getStatusString(status) << std::endl; + triggerEvent(MPSOC_EXE_FAILURE_REPORT, static_cast(internalState)); } -ReturnValue_t PlocMpsocSpecialComHelper::handleTmReception(size_t remainingBytes) { +ReturnValue_t PlocMpsocSpecialComHelper::handleTmReception() { ReturnValue_t result = returnvalue::OK; + tmCountdown.resetTimer(); size_t readBytes = 0; size_t currentBytes = 0; - for (int retries = 0; retries < RETRIES; retries++) { - result = receive(tmBuf.data() + readBytes, ¤tBytes, remainingBytes); + uint32_t usleepDelay = 5; + size_t fullPacketLen = 0; + while (true) { + if (tmCountdown.hasTimedOut()) { + triggerEvent(MPSOC_READ_TIMEOUT, tmCountdown.getTimeoutMs()); + return returnvalue::FAILED; + } + result = receive(tmBuf.data() + readBytes, 6, ¤tBytes); if (result != returnvalue::OK) { return result; } + spReader.setReadOnlyData(tmBuf.data(), tmBuf.size()); + fullPacketLen = spReader.getFullPacketLen(); readBytes += currentBytes; - remainingBytes = remainingBytes - currentBytes; - if (remainingBytes == 0) { + if (readBytes == 6) { break; } + usleep(usleepDelay); + if (usleepDelay < 200000) { + usleepDelay *= 4; + } } - if (remainingBytes != 0) { - sif::warning << "PlocMPSoCHelper::handleTmReception: Failed to receive reply" << std::endl; - triggerEvent(MPSOC_MISSING_EXE, remainingBytes, static_cast(internalState)); - return returnvalue::FAILED; + sif::debug << "recvd first 6 bytes" << std::endl; + while (true) { + if (tmCountdown.hasTimedOut()) { + triggerEvent(MPSOC_READ_TIMEOUT, tmCountdown.getTimeoutMs()); + return returnvalue::FAILED; + } + result = receive(tmBuf.data() + readBytes, fullPacketLen - readBytes, ¤tBytes); + readBytes += currentBytes; + if (fullPacketLen == readBytes) { + sif::debug << "recvd full " << fullPacketLen << std::endl; + break; + } + usleep(usleepDelay); + if (usleepDelay < 200000) { + usleepDelay *= 4; + } } return result; } ReturnValue_t PlocMpsocSpecialComHelper::handleFlashReadReply(std::ofstream& ofile, size_t expectedReadLen) { - SpTmReader tmPacket(tmBuf.data(), tmBuf.size()); - ReturnValue_t result = checkReceivedTm(tmPacket); + ReturnValue_t result = checkReceivedTm(); if (result != returnvalue::OK) { return result; } - uint16_t apid = tmPacket.getApid(); + uint16_t apid = spReader.getApid(); if (apid != mpsoc::apid::TM_FLASH_READ_REPORT) { triggerEvent(MPSOC_FLASH_READ_PACKET_ERROR, FlashReadErrorType::FLASH_READ_APID_ERROR); sif::warning << "PLOC MPSoC Flash Read: Unexpected APID" << std::endl; return result; } - const uint8_t* packetData = tmPacket.getPacketData(); - size_t deserDummy = tmPacket.getPacketDataLen() - mpsoc::CRC_SIZE; + const uint8_t* packetData = spReader.getPacketData(); + size_t deserDummy = spReader.getPacketDataLen() - mpsoc::CRC_SIZE; uint32_t receivedReadLen = 0; std::string receivedShortName = std::string(reinterpret_cast(packetData), 12); if (receivedShortName != flashReadAndWrite.mpsocFile.substr(0, 12)) { @@ -462,21 +508,20 @@ ReturnValue_t PlocMpsocSpecialComHelper::startFlashReadOrWriteBase(std::string o return returnvalue::OK; } -ReturnValue_t PlocMpsocSpecialComHelper::checkReceivedTm(SpTmReader& reader) { - ReturnValue_t result = reader.checkSize(); +ReturnValue_t PlocMpsocSpecialComHelper::checkReceivedTm() { + ReturnValue_t result = spReader.checkSize(); if (result != returnvalue::OK) { - sif::error << "PlocMPSoCHelper::handleTmReception: Size check on received TM failed" - << std::endl; + sif::error << "PLOC MPSoC: Size check on received TM failed" << std::endl; triggerEvent(MPSOC_TM_SIZE_ERROR); return result; } - reader.checkCrc(); + spReader.checkCrc(); if (result != returnvalue::OK) { - sif::warning << "PlocMPSoCHelper::handleTmReception: CRC check failed" << std::endl; + sif::warning << "PLOC MPSoC: CRC check failed" << std::endl; triggerEvent(MPSOC_TM_CRC_MISSMATCH, *sequenceCount); return result; } - uint16_t recvSeqCnt = reader.getSequenceCount(); + uint16_t recvSeqCnt = spReader.getSequenceCount(); if (recvSeqCnt != *sequenceCount) { triggerEvent(MPSOC_HELPER_SEQ_CNT_MISMATCH, *sequenceCount, recvSeqCnt); *sequenceCount = recvSeqCnt; @@ -486,8 +531,8 @@ ReturnValue_t PlocMpsocSpecialComHelper::checkReceivedTm(SpTmReader& reader) { return returnvalue::OK; } -ReturnValue_t PlocMpsocSpecialComHelper::receive(uint8_t* data, size_t* readBytes, - size_t requestBytes) { +ReturnValue_t PlocMpsocSpecialComHelper::receive(uint8_t* data, size_t requestBytes, + size_t* readBytes) { ReturnValue_t result = returnvalue::OK; uint8_t* buffer = nullptr; result = uartComIF->requestReceiveMessage(comCookie, requestBytes); diff --git a/linux/payload/PlocMpsocSpecialComHelper.h b/linux/payload/PlocMpsocSpecialComHelper.h index 2a8e03dc..4b829c63 100644 --- a/linux/payload/PlocMpsocSpecialComHelper.h +++ b/linux/payload/PlocMpsocSpecialComHelper.h @@ -75,6 +75,7 @@ class PlocMpsocSpecialComHelper : public SystemObject, public ExecutableObjectIF static const Event MPSOC_FLASH_READ_PACKET_ERROR = MAKE_EVENT(14, severity::LOW); static const Event MPSOC_FLASH_READ_FAILED = MAKE_EVENT(15, severity::LOW); static const Event MPSOC_FLASH_READ_SUCCESSFUL = MAKE_EVENT(16, severity::INFO); + static const Event MPSOC_READ_TIMEOUT = MAKE_EVENT(17, severity::LOW); enum FlashReadErrorType : uint32_t { FLASH_READ_APID_ERROR = 0, @@ -157,6 +158,8 @@ class PlocMpsocSpecialComHelper : public SystemObject, public ExecutableObjectIF SpacePacketCreator creator; ploc::SpTcParams spParams = ploc::SpTcParams(creator); + Countdown tmCountdown = Countdown(5000); + std::array fileBuf{}; std::array tmBuf{}; @@ -171,6 +174,7 @@ class PlocMpsocSpecialComHelper : public SystemObject, public ExecutableObjectIF CookieIF* comCookie = nullptr; // Sequence count, must be set by Ploc MPSoC Handler SourceSequenceCounter* sequenceCount = nullptr; + ploc::SpTmReader spReader; void resetHelper(); ReturnValue_t performFlashWrite(); @@ -178,18 +182,19 @@ class PlocMpsocSpecialComHelper : public SystemObject, public ExecutableObjectIF ReturnValue_t flashfopen(mpsoc::FileAccessMode mode); ReturnValue_t flashfclose(); ReturnValue_t handlePacketTransmissionNoReply(ploc::SpTcBase& tc); - ReturnValue_t handlePacketTransmissionFlashRead(mpsoc::TcFlashRead& tc); + ReturnValue_t handlePacketTransmissionFlashRead(mpsoc::TcFlashRead& tc, std::ofstream& ofile, + size_t expectedReadLen); ReturnValue_t handleFlashReadReply(std::ofstream& ofile, size_t expectedReadLen); ReturnValue_t sendCommand(ploc::SpTcBase& tc); - ReturnValue_t receive(uint8_t* data, size_t* readBytes, size_t requestBytes); + ReturnValue_t receive(uint8_t* data, size_t requestBytes, size_t* readBytes); ReturnValue_t handleAck(); ReturnValue_t handleExe(); ReturnValue_t startFlashReadOrWriteBase(std::string obcFile, std::string mpsocFile); ReturnValue_t fileCheck(std::string obcFile); void handleAckApidFailure(const ploc::SpTmReader& reader); - void handleExeApidFailure(const ploc::SpTmReader& reader); - ReturnValue_t handleTmReception(size_t remainingBytes); - ReturnValue_t checkReceivedTm(ploc::SpTmReader& reader); + void handleExeFailure(); + ReturnValue_t handleTmReception(); + ReturnValue_t checkReceivedTm(); }; #endif /* BSP_Q7S_DEVICES_PLOCMPSOCHELPER_H_ */ diff --git a/linux/payload/plocMpsocHelpers.h b/linux/payload/plocMpsocHelpers.h index c896be3d..820b479a 100644 --- a/linux/payload/plocMpsocHelpers.h +++ b/linux/payload/plocMpsocHelpers.h @@ -229,7 +229,7 @@ class TcBase : public ploc::SpTcBase, public MPSoCReturnValuesIF { virtual ~TcBase() = default; // Initial length field of space packet. Will always be updated when packet is created. - static const uint16_t INIT_LENGTH = 2; + static const uint16_t INIT_LENGTH = CRC_SIZE; /** * @brief Constructor @@ -391,7 +391,9 @@ class FlashFopen : public TcBase { class FlashFclose : public TcBase { public: FlashFclose(ploc::SpTcParams params, uint16_t sequenceCount) - : TcBase(params, apid::TC_FLASHFCLOSE, sequenceCount) {} + : TcBase(params, apid::TC_FLASHFCLOSE, sequenceCount) { + spParams.setFullPayloadLen(CRC_SIZE); + } }; /** @@ -402,7 +404,7 @@ class TcFlashWrite : public TcBase { TcFlashWrite(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_FLASHWRITE, sequenceCount) {} - ReturnValue_t buildPacket(const uint8_t* writeData, uint32_t writeLen_) { + ReturnValue_t setPayload(const uint8_t* writeData, uint32_t writeLen_) { ReturnValue_t result = returnvalue::OK; writeLen = writeLen_; if (writeLen > SP_MAX_DATA_SIZE) { From c3b6b0a7ee4559ea3cb885c6f99560899ef55034 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 16 May 2023 18:46:13 +0200 Subject: [PATCH 172/506] pure chaos --- linux/payload/PlocMpsocHandler.cpp | 20 ++++++++------------ 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 4167b68c..750b64d7 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -219,18 +219,14 @@ void PlocMpsocHandler::doStartUp() { void PlocMpsocHandler::doShutDown() { #ifdef XIPHOS_Q7S #if not OBSW_MPSOC_JTAG_BOOT == 1 - switch (powerState) { - case PowerState::ON: - uartIsolatorSwitch.pullLow(); - commandActionHelper.commandAction(supervisorHandler, supv::SHUTDOWN_MPSOC); - powerState = PowerState::SHUTDOWN; - return; - case PowerState::OFF: - hkReport.setReportingEnabled(false); - setMode(_MODE_POWER_DOWN); - break; - default: - break; + if (powerState == PowerState::ON) { + uartIsolatorSwitch.pullLow(); + commandActionHelper.commandAction(supervisorHandler, supv::SHUTDOWN_MPSOC); + powerState = PowerState::SHUTDOWN; + return; + } else if (powerState == PowerState::SHUTDOWN) { + // Wait till power state is OFF. + return; } #else uartIsolatorSwitch.pullLow(); From 0179c04472874f1ef4aa47c7abbb60cef3d43277 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 17 May 2023 12:59:29 +0200 Subject: [PATCH 173/506] that did not help --- linux/payload/PlocMpsocSpecialComHelper.cpp | 18 ++++++++++++++---- linux/payload/plocMpsocHelpers.h | 7 ++++--- 2 files changed, 18 insertions(+), 7 deletions(-) diff --git a/linux/payload/PlocMpsocSpecialComHelper.cpp b/linux/payload/PlocMpsocSpecialComHelper.cpp index 7b3cf264..20e9acca 100644 --- a/linux/payload/PlocMpsocSpecialComHelper.cpp +++ b/linux/payload/PlocMpsocSpecialComHelper.cpp @@ -198,6 +198,7 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { while (readSoFar < flashReadAndWrite.totalReadSize) { if (terminate) { std::filesystem::remove(flashReadAndWrite.obcFile, e); + // TODO: Might not be needed flashfclose(); return returnvalue::OK; } @@ -208,6 +209,7 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { sif::debug << "reading " << nextReadSize << " bytes from offset " << readSoFar << std::endl; if (ofile.bad() or not ofile.is_open()) { std::filesystem::remove(flashReadAndWrite.obcFile, e); + // TODO: Might not be needed flashfclose(); return FILE_READ_ERROR; } @@ -215,12 +217,14 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { result = flashReadRequest.setPayload(nextReadSize); if (result != returnvalue::OK) { std::filesystem::remove(flashReadAndWrite.obcFile, e); + // TODO: Might not be needed flashfclose(); return result; } result = flashReadRequest.finishPacket(); if (result != returnvalue::OK) { std::filesystem::remove(flashReadAndWrite.obcFile, e); + // TODO: Might not be needed flashfclose(); return result; } @@ -228,7 +232,7 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { result = handlePacketTransmissionFlashRead(flashReadRequest, ofile, nextReadSize); if (result != returnvalue::OK) { std::filesystem::remove(flashReadAndWrite.obcFile, e); - sif::debug << "flash close" << std::endl; + // TODO: Might not be needed flashfclose(); return result; } @@ -283,7 +287,6 @@ ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionFlashRead(mpsoc if (result != returnvalue::OK) { return result; } - // ccsds::HEADER_LEN + mpsoc::FLASH_READ_MIN_OVERHEAD + tc.readSize + mpsoc::CRC_SIZE result = handleTmReception(); if (result != returnvalue::OK) { return result; @@ -296,6 +299,10 @@ ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionFlashRead(mpsoc if (result != returnvalue::OK) { return result; } + result = handleTmReception(); + if (result != returnvalue::OK) { + return result; + } return handleExe(); } else if (spReader.getApid() == mpsoc::apid::EXE_FAILURE) { handleExeFailure(); @@ -322,6 +329,8 @@ ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionNoReply(ploc::S ReturnValue_t PlocMpsocSpecialComHelper::sendCommand(ploc::SpTcBase& tc) { ReturnValue_t result = returnvalue::OK; + sif::debug << "sending TC" << std::endl; + arrayprinter::print(tc.getFullPacket(), tc.getFullPacketLen()); result = uartComIF->sendMessage(comCookie, tc.getFullPacket(), tc.getFullPacketLen()); if (result != returnvalue::OK) { sif::warning << "PlocMPSoCHelper::sendCommand: Failed to send command" << std::endl; @@ -419,7 +428,7 @@ ReturnValue_t PlocMpsocSpecialComHelper::handleTmReception() { usleepDelay *= 4; } } - sif::debug << "recvd first 6 bytes" << std::endl; + // sif::debug << "recvd first 6 bytes" << std::endl; while (true) { if (tmCountdown.hasTimedOut()) { triggerEvent(MPSOC_READ_TIMEOUT, tmCountdown.getTimeoutMs()); @@ -428,7 +437,7 @@ ReturnValue_t PlocMpsocSpecialComHelper::handleTmReception() { result = receive(tmBuf.data() + readBytes, fullPacketLen - readBytes, ¤tBytes); readBytes += currentBytes; if (fullPacketLen == readBytes) { - sif::debug << "recvd full " << fullPacketLen << std::endl; + // sif::debug << "recvd full " << fullPacketLen << std::endl; break; } usleep(usleepDelay); @@ -436,6 +445,7 @@ ReturnValue_t PlocMpsocSpecialComHelper::handleTmReception() { usleepDelay *= 4; } } + arrayprinter::print(tmBuf.data(), readBytes); return result; } diff --git a/linux/payload/plocMpsocHelpers.h b/linux/payload/plocMpsocHelpers.h index 820b479a..4a6e6b5c 100644 --- a/linux/payload/plocMpsocHelpers.h +++ b/linux/payload/plocMpsocHelpers.h @@ -375,8 +375,9 @@ class FlashFopen : public TcBase { return result; } std::memcpy(payloadStart, filename.c_str(), nameSize); - payloadStart[nameSize] = NULL_TERMINATOR; - payloadStart[255] = NULL_TERMINATOR; + // payloadStart[nameSize] = NULL_TERMINATOR; + std::memset(payloadStart + nameSize, 0, 256 - nameSize); + // payloadStart[255] = NULL_TERMINATOR; payloadStart[256] = static_cast(accessMode); return returnvalue::OK; } @@ -445,7 +446,7 @@ class TcFlashRead : public TcBase { sif::error << "TcFlashRead: Read length " << readLen << " too large" << std::endl; return returnvalue::FAILED; } - spParams.setFullPayloadLen(readLen + FLASH_READ_MIN_OVERHEAD + CRC_SIZE); + spParams.setFullPayloadLen(sizeof(uint32_t) + CRC_SIZE); ReturnValue_t result = checkPayloadLen(); if (result != returnvalue::OK) { return result; From f9e8dc6e6016fee7b4514c10a1e4ef59d27ada8d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 17 May 2023 16:33:52 +0200 Subject: [PATCH 174/506] D'OH --- linux/payload/PlocMpsocSpecialComHelper.cpp | 10 +++++----- linux/payload/plocMpsocHelpers.h | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/linux/payload/PlocMpsocSpecialComHelper.cpp b/linux/payload/PlocMpsocSpecialComHelper.cpp index 20e9acca..0d29db14 100644 --- a/linux/payload/PlocMpsocSpecialComHelper.cpp +++ b/linux/payload/PlocMpsocSpecialComHelper.cpp @@ -199,7 +199,7 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { if (terminate) { std::filesystem::remove(flashReadAndWrite.obcFile, e); // TODO: Might not be needed - flashfclose(); + // flashfclose(); return returnvalue::OK; } nextReadSize = mpsoc::MAX_FLASH_READ_DATA_SIZE; @@ -210,7 +210,7 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { if (ofile.bad() or not ofile.is_open()) { std::filesystem::remove(flashReadAndWrite.obcFile, e); // TODO: Might not be needed - flashfclose(); + // flashfclose(); return FILE_READ_ERROR; } mpsoc::TcFlashRead flashReadRequest(spParams, *sequenceCount); @@ -218,14 +218,14 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { if (result != returnvalue::OK) { std::filesystem::remove(flashReadAndWrite.obcFile, e); // TODO: Might not be needed - flashfclose(); + // flashfclose(); return result; } result = flashReadRequest.finishPacket(); if (result != returnvalue::OK) { std::filesystem::remove(flashReadAndWrite.obcFile, e); // TODO: Might not be needed - flashfclose(); + // flashfclose(); return result; } (*sequenceCount)++; @@ -233,7 +233,7 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { if (result != returnvalue::OK) { std::filesystem::remove(flashReadAndWrite.obcFile, e); // TODO: Might not be needed - flashfclose(); + // flashfclose(); return result; } readSoFar += nextReadSize; diff --git a/linux/payload/plocMpsocHelpers.h b/linux/payload/plocMpsocHelpers.h index 4a6e6b5c..9ed5b3f0 100644 --- a/linux/payload/plocMpsocHelpers.h +++ b/linux/payload/plocMpsocHelpers.h @@ -439,7 +439,7 @@ class TcFlashWrite : public TcBase { class TcFlashRead : public TcBase { public: TcFlashRead(ploc::SpTcParams params, uint16_t sequenceCount) - : TcBase(params, apid::TC_FLASHWRITE, sequenceCount) {} + : TcBase(params, apid::TC_FLASHREAD, sequenceCount) {} ReturnValue_t setPayload(uint32_t readLen) { if (readLen > MAX_FLASH_READ_DATA_SIZE) { From e03df2ebca446794009570cbda0d8c87be714ffc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 17 May 2023 17:33:14 +0200 Subject: [PATCH 175/506] almost done --- .../fsfwconfig/events/translateEvents.cpp | 7 ++- .../fsfwconfig/objects/translateObjects.cpp | 2 +- generators/bsp_hosted_events.csv | 1 + generators/bsp_q7s_events.csv | 1 + generators/events/translateEvents.cpp | 7 ++- generators/objects/translateObjects.cpp | 2 +- linux/fsfwconfig/events/translateEvents.cpp | 7 ++- linux/fsfwconfig/objects/translateObjects.cpp | 2 +- linux/payload/PlocMpsocHandler.cpp | 39 ++++++++++------ linux/payload/PlocMpsocHandler.h | 2 +- linux/payload/PlocMpsocSpecialComHelper.cpp | 45 +++++++------------ tmtc | 2 +- 12 files changed, 62 insertions(+), 55 deletions(-) diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index 63fee50d..9a93fe4c 100644 --- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp +++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 294 translations. + * @brief Auto-generated event translation file. Contains 295 translations. * @details - * Generated on: 2023-05-15 15:20:19 + * Generated on: 2023-05-17 17:15:34 */ #include "translateEvents.h" @@ -200,6 +200,7 @@ const char *MPSOC_TM_CRC_MISSMATCH_STRING = "MPSOC_TM_CRC_MISSMATCH"; const char *MPSOC_FLASH_READ_PACKET_ERROR_STRING = "MPSOC_FLASH_READ_PACKET_ERROR"; const char *MPSOC_FLASH_READ_FAILED_STRING = "MPSOC_FLASH_READ_FAILED"; const char *MPSOC_FLASH_READ_SUCCESSFUL_STRING = "MPSOC_FLASH_READ_SUCCESSFUL"; +const char *MPSOC_READ_TIMEOUT_STRING = "MPSOC_READ_TIMEOUT"; const char *TRANSITION_BACK_TO_OFF_STRING = "TRANSITION_BACK_TO_OFF"; const char *NEG_V_OUT_OF_BOUNDS_STRING = "NEG_V_OUT_OF_BOUNDS"; const char *U_DRO_OUT_OF_BOUNDS_STRING = "U_DRO_OUT_OF_BOUNDS"; @@ -692,6 +693,8 @@ const char *translateEvents(Event event) { return MPSOC_FLASH_READ_FAILED_STRING; case (12616): return MPSOC_FLASH_READ_SUCCESSFUL_STRING; + case (12617): + return MPSOC_READ_TIMEOUT_STRING; case (12700): return TRANSITION_BACK_TO_OFF_STRING; case (12701): diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index 6ab96892..34edfa41 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 171 translations. - * Generated on: 2023-05-15 15:20:19 + * Generated on: 2023-05-17 17:15:34 */ #include "translateObjects.h" diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv index 2df4aa32..9dc26e07 100644 --- a/generators/bsp_hosted_events.csv +++ b/generators/bsp_hosted_events.csv @@ -194,6 +194,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 12614;0x3146;MPSOC_FLASH_READ_PACKET_ERROR;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h 12615;0x3147;MPSOC_FLASH_READ_FAILED;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h 12616;0x3148;MPSOC_FLASH_READ_SUCCESSFUL;INFO;No description;linux/payload/PlocMpsocSpecialComHelper.h +12617;0x3149;MPSOC_READ_TIMEOUT;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h 12700;0x319c;TRANSITION_BACK_TO_OFF;MEDIUM;Could not transition properly and went back to ALL OFF;mission/payload/PayloadPcduHandler.h 12701;0x319d;NEG_V_OUT_OF_BOUNDS;MEDIUM;P1: 0 -> too low, 1 -> too high P2: Float value;mission/payload/PayloadPcduHandler.h 12702;0x319e;U_DRO_OUT_OF_BOUNDS;MEDIUM;P1: 0 -> too low, 1 -> too high P2: Float value;mission/payload/PayloadPcduHandler.h diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 2df4aa32..9dc26e07 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -194,6 +194,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 12614;0x3146;MPSOC_FLASH_READ_PACKET_ERROR;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h 12615;0x3147;MPSOC_FLASH_READ_FAILED;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h 12616;0x3148;MPSOC_FLASH_READ_SUCCESSFUL;INFO;No description;linux/payload/PlocMpsocSpecialComHelper.h +12617;0x3149;MPSOC_READ_TIMEOUT;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h 12700;0x319c;TRANSITION_BACK_TO_OFF;MEDIUM;Could not transition properly and went back to ALL OFF;mission/payload/PayloadPcduHandler.h 12701;0x319d;NEG_V_OUT_OF_BOUNDS;MEDIUM;P1: 0 -> too low, 1 -> too high P2: Float value;mission/payload/PayloadPcduHandler.h 12702;0x319e;U_DRO_OUT_OF_BOUNDS;MEDIUM;P1: 0 -> too low, 1 -> too high P2: Float value;mission/payload/PayloadPcduHandler.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index 63fee50d..9a93fe4c 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 294 translations. + * @brief Auto-generated event translation file. Contains 295 translations. * @details - * Generated on: 2023-05-15 15:20:19 + * Generated on: 2023-05-17 17:15:34 */ #include "translateEvents.h" @@ -200,6 +200,7 @@ const char *MPSOC_TM_CRC_MISSMATCH_STRING = "MPSOC_TM_CRC_MISSMATCH"; const char *MPSOC_FLASH_READ_PACKET_ERROR_STRING = "MPSOC_FLASH_READ_PACKET_ERROR"; const char *MPSOC_FLASH_READ_FAILED_STRING = "MPSOC_FLASH_READ_FAILED"; const char *MPSOC_FLASH_READ_SUCCESSFUL_STRING = "MPSOC_FLASH_READ_SUCCESSFUL"; +const char *MPSOC_READ_TIMEOUT_STRING = "MPSOC_READ_TIMEOUT"; const char *TRANSITION_BACK_TO_OFF_STRING = "TRANSITION_BACK_TO_OFF"; const char *NEG_V_OUT_OF_BOUNDS_STRING = "NEG_V_OUT_OF_BOUNDS"; const char *U_DRO_OUT_OF_BOUNDS_STRING = "U_DRO_OUT_OF_BOUNDS"; @@ -692,6 +693,8 @@ const char *translateEvents(Event event) { return MPSOC_FLASH_READ_FAILED_STRING; case (12616): return MPSOC_FLASH_READ_SUCCESSFUL_STRING; + case (12617): + return MPSOC_READ_TIMEOUT_STRING; case (12700): return TRANSITION_BACK_TO_OFF_STRING; case (12701): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index 458e8d67..eb2125b4 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-05-15 15:20:19 + * Generated on: 2023-05-17 17:15:34 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index 63fee50d..9a93fe4c 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 294 translations. + * @brief Auto-generated event translation file. Contains 295 translations. * @details - * Generated on: 2023-05-15 15:20:19 + * Generated on: 2023-05-17 17:15:34 */ #include "translateEvents.h" @@ -200,6 +200,7 @@ const char *MPSOC_TM_CRC_MISSMATCH_STRING = "MPSOC_TM_CRC_MISSMATCH"; const char *MPSOC_FLASH_READ_PACKET_ERROR_STRING = "MPSOC_FLASH_READ_PACKET_ERROR"; const char *MPSOC_FLASH_READ_FAILED_STRING = "MPSOC_FLASH_READ_FAILED"; const char *MPSOC_FLASH_READ_SUCCESSFUL_STRING = "MPSOC_FLASH_READ_SUCCESSFUL"; +const char *MPSOC_READ_TIMEOUT_STRING = "MPSOC_READ_TIMEOUT"; const char *TRANSITION_BACK_TO_OFF_STRING = "TRANSITION_BACK_TO_OFF"; const char *NEG_V_OUT_OF_BOUNDS_STRING = "NEG_V_OUT_OF_BOUNDS"; const char *U_DRO_OUT_OF_BOUNDS_STRING = "U_DRO_OUT_OF_BOUNDS"; @@ -692,6 +693,8 @@ const char *translateEvents(Event event) { return MPSOC_FLASH_READ_FAILED_STRING; case (12616): return MPSOC_FLASH_READ_SUCCESSFUL_STRING; + case (12617): + return MPSOC_READ_TIMEOUT_STRING; case (12700): return TRANSITION_BACK_TO_OFF_STRING; case (12701): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index 458e8d67..eb2125b4 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-05-15 15:20:19 + * Generated on: 2023-05-17 17:15:34 */ #include "translateObjects.h" diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 750b64d7..00b0081e 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -53,15 +53,26 @@ ReturnValue_t PlocMpsocHandler::initialize() { if (result != returnvalue::OK) { return result; } - result = manager->subscribeToEventRange( - eventQueue->getId(), event::getEventId(PlocMpsocSpecialComHelper::MPSOC_FLASH_WRITE_FAILED), + result = manager->subscribeToEvent( + eventQueue->getId(), event::getEventId(PlocMpsocSpecialComHelper::MPSOC_FLASH_WRITE_FAILED)); + if (result != returnvalue::OK) { + return ObjectManagerIF::CHILD_INIT_FAILED; + } + result = manager->subscribeToEvent( + eventQueue->getId(), event::getEventId(PlocMpsocSpecialComHelper::MPSOC_FLASH_WRITE_SUCCESSFUL)); if (result != returnvalue::OK) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "PlocMPSoCHandler::initialize: Failed to subscribe to events from " - " ploc mpsoc helper" - << std::endl; -#endif + return ObjectManagerIF::CHILD_INIT_FAILED; + } + result = manager->subscribeToEvent( + eventQueue->getId(), + event::getEventId(PlocMpsocSpecialComHelper::MPSOC_FLASH_READ_SUCCESSFUL)); + if (result != returnvalue::OK) { + return ObjectManagerIF::CHILD_INIT_FAILED; + } + result = manager->subscribeToEvent( + eventQueue->getId(), event::getEventId(PlocMpsocSpecialComHelper::MPSOC_FLASH_READ_FAILED)); + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } @@ -125,7 +136,7 @@ ReturnValue_t PlocMpsocHandler::executeAction(ActionId_t actionId, MessageQueueI } } - if (plocMPSoCHelperExecuting) { + if (specialComHelperExecuting) { return MPSoCReturnValuesIF::MPSOC_HELPER_EXECUTING; } @@ -141,7 +152,7 @@ ReturnValue_t PlocMpsocHandler::executeAction(ActionId_t actionId, MessageQueueI if (result != returnvalue::OK) { return result; } - plocMPSoCHelperExecuting = true; + specialComHelperExecuting = true; return EXECUTION_FINISHED; } case mpsoc::TC_FLASH_READ_FULL_FILE: { @@ -156,7 +167,7 @@ ReturnValue_t PlocMpsocHandler::executeAction(ActionId_t actionId, MessageQueueI if (result != returnvalue::OK) { return result; } - plocMPSoCHelperExecuting = true; + specialComHelperExecuting = true; return EXECUTION_FINISHED; } case (mpsoc::OBSW_RESET_SEQ_COUNT): { @@ -241,12 +252,12 @@ void PlocMpsocHandler::doShutDown() { setMode(_MODE_POWER_DOWN); commandIsPending = false; sequenceCount = 0; - plocMPSoCHelperExecuting = false; + specialComHelperExecuting = false; startupState = StartupState::IDLE; } ReturnValue_t PlocMpsocHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { - if (not commandIsPending and not plocMPSoCHelperExecuting) { + if (not commandIsPending and not specialComHelperExecuting) { *id = mpsoc::TC_GET_HK_REPORT; commandIsPending = true; cmdCountdown.resetTimer(); @@ -548,7 +559,7 @@ void PlocMpsocHandler::handleEvent(EventMessage* eventMessage) { object_id_t objectId = eventMessage->getReporter(); switch (objectId) { case objects::PLOC_MPSOC_HELPER: { - plocMPSoCHelperExecuting = false; + specialComHelperExecuting = false; break; } default: @@ -1209,7 +1220,7 @@ size_t PlocMpsocHandler::getNextReplyLength(DeviceCommandId_t commandId) { ReturnValue_t PlocMpsocHandler::doSendReadHook() { // Prevent DHB from polling UART during commands executed by the mpsoc helper task - if (plocMPSoCHelperExecuting) { + if (specialComHelperExecuting) { return returnvalue::FAILED; } return returnvalue::OK; diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index e0474a33..a82623b0 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -168,7 +168,7 @@ class PlocMpsocHandler : public DeviceHandlerBase, public CommandsActionsIF { CommandActionHelper commandActionHelper; // Used to block incoming commands when MPSoC helper class is currently executing a command - bool plocMPSoCHelperExecuting = false; + bool specialComHelperExecuting = false; bool commandIsPending = false; struct TmMemReadReport { diff --git a/linux/payload/PlocMpsocSpecialComHelper.cpp b/linux/payload/PlocMpsocSpecialComHelper.cpp index 0d29db14..6ab20802 100644 --- a/linux/payload/PlocMpsocSpecialComHelper.cpp +++ b/linux/payload/PlocMpsocSpecialComHelper.cpp @@ -140,7 +140,6 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashWrite() { size_t bytesRead = 0; while (remainingSize > 0) { if (terminate) { - flashfclose(); return returnvalue::OK; } if (remainingSize > mpsoc::SP_MAX_DATA_SIZE) { @@ -149,7 +148,6 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashWrite() { dataLength = remainingSize; } if (file.bad() or not file.is_open()) { - flashfclose(); return FILE_WRITE_ERROR; } file.seekg(bytesRead, file.beg); @@ -159,18 +157,15 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashWrite() { mpsoc::TcFlashWrite tc(spParams, *sequenceCount); result = tc.setPayload(fileBuf.data(), dataLength); if (result != returnvalue::OK) { - flashfclose(); return result; } result = tc.finishPacket(); if (result != returnvalue::OK) { - flashfclose(); return result; } (*sequenceCount)++; result = handlePacketTransmissionNoReply(tc); if (result != returnvalue::OK) { - flashfclose(); return result; } } @@ -198,8 +193,6 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { while (readSoFar < flashReadAndWrite.totalReadSize) { if (terminate) { std::filesystem::remove(flashReadAndWrite.obcFile, e); - // TODO: Might not be needed - // flashfclose(); return returnvalue::OK; } nextReadSize = mpsoc::MAX_FLASH_READ_DATA_SIZE; @@ -209,35 +202,31 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { sif::debug << "reading " << nextReadSize << " bytes from offset " << readSoFar << std::endl; if (ofile.bad() or not ofile.is_open()) { std::filesystem::remove(flashReadAndWrite.obcFile, e); - // TODO: Might not be needed - // flashfclose(); return FILE_READ_ERROR; } mpsoc::TcFlashRead flashReadRequest(spParams, *sequenceCount); result = flashReadRequest.setPayload(nextReadSize); if (result != returnvalue::OK) { std::filesystem::remove(flashReadAndWrite.obcFile, e); - // TODO: Might not be needed - // flashfclose(); return result; } result = flashReadRequest.finishPacket(); if (result != returnvalue::OK) { std::filesystem::remove(flashReadAndWrite.obcFile, e); - // TODO: Might not be needed - // flashfclose(); return result; } (*sequenceCount)++; result = handlePacketTransmissionFlashRead(flashReadRequest, ofile, nextReadSize); if (result != returnvalue::OK) { std::filesystem::remove(flashReadAndWrite.obcFile, e); - // TODO: Might not be needed - // flashfclose(); return result; } readSoFar += nextReadSize; } + result = flashfclose(); + if (result != returnvalue::OK) { + return result; + } sif::debug << "read file done" << std::endl; return result; } @@ -299,10 +288,6 @@ ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionFlashRead(mpsoc if (result != returnvalue::OK) { return result; } - result = handleTmReception(); - if (result != returnvalue::OK) { - return result; - } return handleExe(); } else if (spReader.getApid() == mpsoc::apid::EXE_FAILURE) { handleExeFailure(); @@ -346,14 +331,13 @@ ReturnValue_t PlocMpsocSpecialComHelper::handleAck() { if (result != returnvalue::OK) { return result; } - SpTmReader tmPacket(tmBuf.data(), tmBuf.size()); result = checkReceivedTm(); if (result != returnvalue::OK) { return result; } - uint16_t apid = tmPacket.getApid(); + uint16_t apid = spReader.getApid(); if (apid != mpsoc::apid::ACK_SUCCESS) { - handleAckApidFailure(tmPacket); + handleAckApidFailure(spReader); return returnvalue::FAILED; } return returnvalue::OK; @@ -464,14 +448,15 @@ ReturnValue_t PlocMpsocSpecialComHelper::handleFlashReadReply(std::ofstream& ofi const uint8_t* packetData = spReader.getPacketData(); size_t deserDummy = spReader.getPacketDataLen() - mpsoc::CRC_SIZE; uint32_t receivedReadLen = 0; - std::string receivedShortName = std::string(reinterpret_cast(packetData), 12); - if (receivedShortName != flashReadAndWrite.mpsocFile.substr(0, 12)) { - sif::warning << "PLOC MPSoC Flash Read: Missmatch between request file name and " - "received file name" - << std::endl; - triggerEvent(MPSOC_FLASH_READ_PACKET_ERROR, FlashReadErrorType::FLASH_READ_FILENAME_ERROR); - return returnvalue::FAILED; - } + // I think this is buggy.. + // std::string receivedShortName = std::string(reinterpret_cast(packetData), 12); + // if (receivedShortName != flashReadAndWrite.mpsocFile.substr(0, 11)) { + // sif::warning << "PLOC MPSoC Flash Read: Missmatch between request file name and " + // "received file name" + // << std::endl; + // triggerEvent(MPSOC_FLASH_READ_PACKET_ERROR, FlashReadErrorType::FLASH_READ_FILENAME_ERROR); + // return returnvalue::FAILED; + // } packetData += 12; result = SerializeAdapter::deSerialize(&receivedReadLen, &packetData, &deserDummy, SerializeIF::Endianness::NETWORK); diff --git a/tmtc b/tmtc index b8e1c7af..6ec0ce20 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit b8e1c7afe91bddfea2b139217320033f3a3b0efb +Subproject commit 6ec0ce20fa98877c9f88acb5fe9129254291344b From 7f115303aed65d350ed8a58c3fa6d8ef83d9aab7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 17 May 2023 18:33:48 +0200 Subject: [PATCH 176/506] everything is working now --- linux/payload/PlocMpsocSpecialComHelper.cpp | 22 +++++++----------- linux/payload/PlocMpsocSpecialComHelper.h | 2 +- linux/payload/plocMpsocHelpers.cpp | 4 ++++ linux/payload/plocMpsocHelpers.h | 25 ++++++++++++--------- 4 files changed, 28 insertions(+), 25 deletions(-) diff --git a/linux/payload/PlocMpsocSpecialComHelper.cpp b/linux/payload/PlocMpsocSpecialComHelper.cpp index 6ab20802..acf88abd 100644 --- a/linux/payload/PlocMpsocSpecialComHelper.cpp +++ b/linux/payload/PlocMpsocSpecialComHelper.cpp @@ -128,7 +128,7 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashWrite() { if (file.bad()) { return returnvalue::FAILED; } - result = flashfopen(mpsoc::FileAccessMode::WRITE); + result = flashfopen(mpsoc::FileAccessModes::WRITE | mpsoc::FileAccessModes::OPEN_ALWAYS); if (result != returnvalue::OK) { return result; } @@ -142,8 +142,9 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashWrite() { if (terminate) { return returnvalue::OK; } - if (remainingSize > mpsoc::SP_MAX_DATA_SIZE) { - dataLength = mpsoc::SP_MAX_DATA_SIZE; + // The minus 4 is necessary for unknown reasons. Maybe some bug in the ILH software? + if (remainingSize > mpsoc::MAX_FLASH_WRITE_DATA_SIZE - 4) { + dataLength = mpsoc::MAX_FLASH_WRITE_DATA_SIZE - 4; } else { dataLength = remainingSize; } @@ -177,13 +178,12 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashWrite() { } ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { - sif::debug << "performing flash read" << std::endl; std::error_code e; std::ofstream ofile(flashReadAndWrite.obcFile, std::ios::trunc | std::ios::binary); if (ofile.bad()) { return returnvalue::FAILED; } - ReturnValue_t result = flashfopen(mpsoc::FileAccessMode::READ); + ReturnValue_t result = flashfopen(mpsoc::FileAccessModes::READ); if (result != returnvalue::OK) { std::filesystem::remove(flashReadAndWrite.obcFile, e); return result; @@ -199,7 +199,6 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { if (flashReadAndWrite.totalReadSize - readSoFar < mpsoc::MAX_FLASH_READ_DATA_SIZE) { nextReadSize = flashReadAndWrite.totalReadSize - readSoFar; } - sif::debug << "reading " << nextReadSize << " bytes from offset " << readSoFar << std::endl; if (ofile.bad() or not ofile.is_open()) { std::filesystem::remove(flashReadAndWrite.obcFile, e); return FILE_READ_ERROR; @@ -227,11 +226,10 @@ ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { if (result != returnvalue::OK) { return result; } - sif::debug << "read file done" << std::endl; return result; } -ReturnValue_t PlocMpsocSpecialComHelper::flashfopen(mpsoc::FileAccessMode mode) { +ReturnValue_t PlocMpsocSpecialComHelper::flashfopen(uint8_t mode) { spParams.buf = commandBuffer; mpsoc::FlashFopen flashFopen(spParams, *sequenceCount); ReturnValue_t result = flashFopen.setPayload(flashReadAndWrite.mpsocFile, mode); @@ -314,8 +312,6 @@ ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionNoReply(ploc::S ReturnValue_t PlocMpsocSpecialComHelper::sendCommand(ploc::SpTcBase& tc) { ReturnValue_t result = returnvalue::OK; - sif::debug << "sending TC" << std::endl; - arrayprinter::print(tc.getFullPacket(), tc.getFullPacketLen()); result = uartComIF->sendMessage(comCookie, tc.getFullPacket(), tc.getFullPacketLen()); if (result != returnvalue::OK) { sif::warning << "PlocMPSoCHelper::sendCommand: Failed to send command" << std::endl; @@ -412,7 +408,6 @@ ReturnValue_t PlocMpsocSpecialComHelper::handleTmReception() { usleepDelay *= 4; } } - // sif::debug << "recvd first 6 bytes" << std::endl; while (true) { if (tmCountdown.hasTimedOut()) { triggerEvent(MPSOC_READ_TIMEOUT, tmCountdown.getTimeoutMs()); @@ -421,7 +416,6 @@ ReturnValue_t PlocMpsocSpecialComHelper::handleTmReception() { result = receive(tmBuf.data() + readBytes, fullPacketLen - readBytes, ¤tBytes); readBytes += currentBytes; if (fullPacketLen == readBytes) { - // sif::debug << "recvd full " << fullPacketLen << std::endl; break; } usleep(usleepDelay); @@ -429,7 +423,7 @@ ReturnValue_t PlocMpsocSpecialComHelper::handleTmReception() { usleepDelay *= 4; } } - arrayprinter::print(tmBuf.data(), readBytes); + // arrayprinter::print(tmBuf.data(), readBytes); return result; } @@ -448,7 +442,7 @@ ReturnValue_t PlocMpsocSpecialComHelper::handleFlashReadReply(std::ofstream& ofi const uint8_t* packetData = spReader.getPacketData(); size_t deserDummy = spReader.getPacketDataLen() - mpsoc::CRC_SIZE; uint32_t receivedReadLen = 0; - // I think this is buggy.. + // I think this is buggy, weird stuff in the short name field. // std::string receivedShortName = std::string(reinterpret_cast(packetData), 12); // if (receivedShortName != flashReadAndWrite.mpsocFile.substr(0, 11)) { // sif::warning << "PLOC MPSoC Flash Read: Missmatch between request file name and " diff --git a/linux/payload/PlocMpsocSpecialComHelper.h b/linux/payload/PlocMpsocSpecialComHelper.h index 4b829c63..bc6bec8c 100644 --- a/linux/payload/PlocMpsocSpecialComHelper.h +++ b/linux/payload/PlocMpsocSpecialComHelper.h @@ -179,7 +179,7 @@ class PlocMpsocSpecialComHelper : public SystemObject, public ExecutableObjectIF void resetHelper(); ReturnValue_t performFlashWrite(); ReturnValue_t performFlashRead(); - ReturnValue_t flashfopen(mpsoc::FileAccessMode mode); + ReturnValue_t flashfopen(uint8_t accessMode); ReturnValue_t flashfclose(); ReturnValue_t handlePacketTransmissionNoReply(ploc::SpTcBase& tc); ReturnValue_t handlePacketTransmissionFlashRead(mpsoc::TcFlashRead& tc, std::ofstream& ofile, diff --git a/linux/payload/plocMpsocHelpers.cpp b/linux/payload/plocMpsocHelpers.cpp index ab6f0907..9e5b11c9 100644 --- a/linux/payload/plocMpsocHelpers.cpp +++ b/linux/payload/plocMpsocHelpers.cpp @@ -33,6 +33,10 @@ std::string mpsoc::getStatusString(uint16_t status) { return "Flash mount failed"; break; } + case (mpsoc::status_code::FLASH_FILE_ALREADY_OPEN): { + return "Flash file already open"; + break; + } case (mpsoc::status_code::FLASH_FILE_ALREADY_CLOSED): { return "Flash file already closed"; break; diff --git a/linux/payload/plocMpsocHelpers.h b/linux/payload/plocMpsocHelpers.h index 9ed5b3f0..6d42e6c6 100644 --- a/linux/payload/plocMpsocHelpers.h +++ b/linux/payload/plocMpsocHelpers.h @@ -13,12 +13,16 @@ namespace mpsoc { -enum FileAccessMode : uint8_t { +enum FileAccessModes : uint8_t { + // Opens a file, fails if the file does not exist. OPEN_EXISTING = 0x00, READ = 0x01, WRITE = 0x02, + // Creates a new file, fails if it already exists. CREATE_NEW = 0x04, + // Creates a new file, existing file is truncated and overwritten. CREATE_ALWAYS = 0x08, + // Opens the file if it is existing. If not, a new file is created. OPEN_ALWAYS = 0x10, OPEN_APPEND = 0x30 }; @@ -182,6 +186,8 @@ static constexpr size_t SP_MAX_DATA_SIZE = SP_MAX_SIZE - ccsds::HEADER_LEN - CRC static constexpr size_t FLASH_READ_MIN_OVERHEAD = 16; // 1000 bytes. static const size_t MAX_FLASH_READ_DATA_SIZE = SP_MAX_DATA_SIZE - FLASH_READ_MIN_OVERHEAD; +// 1012 bytes, 4 bytes for the write length. +static constexpr size_t MAX_FLASH_WRITE_DATA_SIZE = SP_MAX_DATA_SIZE - sizeof(uint32_t); /** * The replay write sequence command has a maximum delay for the execution report which amounts to @@ -204,7 +210,7 @@ static const uint16_t TC_EXEUTION_DISABLED = 0x5E2; static const uint16_t FLASH_MOUNT_FAILED = 0x5E3; static const uint16_t FLASH_FILE_ALREADY_CLOSED = 0x5E4; static const uint16_t FLASH_FILE_OPEN_FAILED = 0x5E5; -static const uint16_t FLASH_FILE_ALREDY_OPEN = 0x5E6; +static const uint16_t FLASH_FILE_ALREADY_OPEN = 0x5E6; static const uint16_t FLASH_FILE_NOT_OPEN = 0x5E7; static const uint16_t FLASH_UNMOUNT_FAILED = 0x5E8; static const uint16_t HEAP_ALLOCATION_FAILED = 0x5E9; @@ -366,7 +372,7 @@ class FlashFopen : public TcBase { FlashFopen(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_FLASHFOPEN, sequenceCount) {} - ReturnValue_t setPayload(std::string filename, FileAccessMode mode) { + ReturnValue_t setPayload(std::string filename, uint8_t mode) { accessMode = mode; size_t nameSize = filename.size(); spParams.setFullPayloadLen(256 + sizeof(uint8_t) + CRC_SIZE); @@ -378,12 +384,12 @@ class FlashFopen : public TcBase { // payloadStart[nameSize] = NULL_TERMINATOR; std::memset(payloadStart + nameSize, 0, 256 - nameSize); // payloadStart[255] = NULL_TERMINATOR; - payloadStart[256] = static_cast(accessMode); + payloadStart[256] = accessMode; return returnvalue::OK; } private: - FileAccessMode accessMode = FileAccessMode::OPEN_EXISTING; + uint8_t accessMode = FileAccessModes::OPEN_EXISTING; }; /** @@ -406,14 +412,13 @@ class TcFlashWrite : public TcBase { : TcBase(params, apid::TC_FLASHWRITE, sequenceCount) {} ReturnValue_t setPayload(const uint8_t* writeData, uint32_t writeLen_) { - ReturnValue_t result = returnvalue::OK; writeLen = writeLen_; - if (writeLen > SP_MAX_DATA_SIZE) { + if (writeLen > MAX_FLASH_WRITE_DATA_SIZE) { sif::error << "TcFlashWrite: Command data too big" << std::endl; return returnvalue::FAILED; } - spParams.setFullPayloadLen(static_cast(writeLen) + 4 + CRC_SIZE); - result = checkPayloadLen(); + spParams.setFullPayloadLen(sizeof(uint32_t) + static_cast(writeLen) + CRC_SIZE); + ReturnValue_t result = checkPayloadLen(); if (result != returnvalue::OK) { return result; } @@ -423,7 +428,7 @@ class TcFlashWrite : public TcBase { if (result != returnvalue::OK) { return result; } - std::memcpy(payloadStart + sizeof(writeLen), writeData, writeLen); + std::memcpy(payloadStart + sizeof(uint32_t), writeData, writeLen); updateSpFields(); result = checkSizeAndSerializeHeader(); if (result != returnvalue::OK) { From b9afeb9c19e5dcd7e0fd34b8c00050a9c5c62bd8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 17 May 2023 18:44:12 +0200 Subject: [PATCH 177/506] fix some regressions --- linux/payload/PlocMpsocHandler.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 00b0081e..cdf59c52 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -654,6 +654,10 @@ ReturnValue_t PlocMpsocHandler::prepareTcGetHkReport() { ReturnValue_t PlocMpsocHandler::prepareTcReplayWriteSequence(const uint8_t* commandData, size_t commandDataLen) { mpsoc::TcReplayWriteSeq tcReplayWriteSeq(spParams, sequenceCount); + ReturnValue_t result = tcReplayWriteSeq.setPayload(commandData, commandDataLen); + if (result != returnvalue::OK) { + return result; + } finishTcPrep(tcReplayWriteSeq); return returnvalue::OK; } @@ -673,6 +677,10 @@ ReturnValue_t PlocMpsocHandler::prepareTcModeIdle() { ReturnValue_t PlocMpsocHandler::prepareTcCamCmdSend(const uint8_t* commandData, size_t commandDataLen) { mpsoc::TcCamcmdSend tcCamCmdSend(spParams, sequenceCount); + ReturnValue_t result = tcCamCmdSend.setPayload(commandData, commandDataLen); + if (result != returnvalue::OK) { + return result; + } finishTcPrep(tcCamCmdSend); nextReplyId = mpsoc::TM_CAM_CMD_RPT; return returnvalue::OK; @@ -681,6 +689,10 @@ ReturnValue_t PlocMpsocHandler::prepareTcCamCmdSend(const uint8_t* commandData, ReturnValue_t PlocMpsocHandler::prepareTcCamTakePic(const uint8_t* commandData, size_t commandDataLen) { mpsoc::TcCamTakePic tcCamTakePic(spParams, sequenceCount); + ReturnValue_t result = tcCamTakePic.setPayload(commandData, commandDataLen); + if (result != returnvalue::OK) { + return result; + } finishTcPrep(tcCamTakePic); return returnvalue::OK; } From d28bc3f74ddbcf8a4f79738fee8144290b857e23 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 17 May 2023 18:51:53 +0200 Subject: [PATCH 178/506] bugfix --- linux/payload/plocMpsocHelpers.h | 1 - 1 file changed, 1 deletion(-) diff --git a/linux/payload/plocMpsocHelpers.h b/linux/payload/plocMpsocHelpers.h index 6d42e6c6..c9b08a28 100644 --- a/linux/payload/plocMpsocHelpers.h +++ b/linux/payload/plocMpsocHelpers.h @@ -878,7 +878,6 @@ class TcCamcmdSend : public TcBase { TcCamcmdSend(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_CAM_CMD_SEND, sequenceCount) {} - protected: ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { if (commandDataLen > MAX_DATA_LENGTH) { return INVALID_LENGTH; From a0e305fe1f923fe3d8a1ca548a3324ea7c9a04bf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 17 May 2023 18:55:53 +0200 Subject: [PATCH 179/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7c3924f1..586c497f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -44,6 +44,7 @@ will consitute of a breaking change warranting a new major release: ## Added - Add the remaining system modes. +- PLOC MPSoC flash read command working. ## Fixed @@ -62,6 +63,7 @@ will consitute of a breaking change warranting a new major release: - Fixed correction for `GPS Altitude` in case the sensor data is out of the expected bonds. - PLOC MPSoC special communication is now scheduled, which allows flash read and flash write commands to work. +- Fixed the MPSoC flash write command. # [v2.0.5] 2023-05-11 From ed74367f01d37a2684d0a5ee9dd9b0b5ede30dbd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 19 May 2023 09:41:44 +0200 Subject: [PATCH 180/506] changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7c3924f1..119205b6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -40,6 +40,9 @@ will consitute of a breaking change warranting a new major release: - Larger allowed path and file sizes for STR and PLOC MPSoC modules. - More robust MPSoC flash read and write command data handling. - Increase frequency of payload handlers from 0.8 seconds to 0.5 seconds. +- Disable missed deadlines per default. Not useful in orbit, and triggers all the time on the EM + build after a number of subsequent runs, without any apparent reason (deadlines are not actually + missed, thread usage displayed is nominal) ## Added From cd514e81250d78a268d73865298eb6e1d1434fca Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 22 May 2023 11:40:33 +0200 Subject: [PATCH 181/506] ze batteri wos add --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c2a9482e..b5ecf173 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,8 +83,8 @@ set(OBSW_ADD_MGT ${INIT_VAL} CACHE STRING "Add MGT module") set(OBSW_ADD_BPX_BATTERY_HANDLER - ${INIT_VAL} - CACHE STRING "Add MGT module") + 1 + CACHE STRING "Add BPX battery module") set(OBSW_ADD_STAR_TRACKER ${INIT_VAL} CACHE STRING "Add Startracker module") From d23082283dd1e53c4c7b04560e95f6d53c304ab8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 22 May 2023 11:42:08 +0200 Subject: [PATCH 182/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index a7ed6681..13c81828 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -48,6 +48,7 @@ will consitute of a breaking change warranting a new major release: - Add the remaining system modes. - PLOC MPSoC flash read command working. +- BPX battery handler is added for EM by default. ## Fixed From 2543cdf5b7b67ea97af31704a2fff7d241312ba2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 22 May 2023 11:44:42 +0200 Subject: [PATCH 183/506] changelog --- CHANGELOG.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8841cd0c..3ab6a184 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,12 @@ will consitute of a breaking change warranting a new major release: # [v2.2.0] to be released +- eive-tmtc: v4.0.0 (to be released) + +## Fixed + +- CFDP low level protocol bugfix. Requires fsfw update and tmtc update. + # [v2.1.0] to be released ## Changed From 9ff154cedce3daf02592243dd51adbc50088883d Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 23 May 2023 14:56:04 +0200 Subject: [PATCH 184/506] added missing parameter --- mission/controller/acs/AcsParameters.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 03500cc3..53f25bb1 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -539,6 +539,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0xA: parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; + case 0xB: + parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); + break; case 0xC: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); break; From 5390e947cae6a4e966f0874715b219afc00dec57 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 23 May 2023 15:02:35 +0200 Subject: [PATCH 185/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index a7ed6681..a85086a2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -67,6 +67,7 @@ will consitute of a breaking change warranting a new major release: - PLOC MPSoC special communication is now scheduled, which allows flash read and flash write commands to work. - Fixed the MPSoC flash write command. +- Added missing ACS parameter. # [v2.0.5] 2023-05-11 From 62ae7ff48296e3d3bd75524171c4371989cd0701 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 23 May 2023 15:04:43 +0200 Subject: [PATCH 186/506] frmt --- mission/controller/acs/AcsParameters.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 53f25bb1..e9ba7c3a 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -540,8 +540,8 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; case 0xB: - parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); - break; + parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); + break; case 0xC: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); break; From 32a4aa48d9936dfa1d50208f41242886572ed243 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 23 May 2023 15:07:23 +0200 Subject: [PATCH 187/506] frmt --- mission/controller/acs/AcsParameters.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index e9ba7c3a..792a72ec 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -540,8 +540,8 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; case 0xB: - parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); - break; + parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); + break; case 0xC: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); break; From 7ac2271eabb6041b636a3430e4800d9124a80d4e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 23 May 2023 19:12:35 +0200 Subject: [PATCH 188/506] buuuug --- bsp_q7s/core/ObjectFactory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 17b14b56..cea90bf2 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -805,7 +805,7 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(CcsdsComponentArgs& args) { // Core task which handles the HK store and takes care of dumping it as TM using a VC directly auto* hkStore = new PersistentSingleTmStoreTask( objects::HK_STORE_AND_TM_TASK, args.ipcStore, *args.stores.hkStore, *vc, - persTmStore::DUMP_HK_STORE_DONE, persTmStore::DUMP_HK_STORE_DONE, *SdCardManager::instance(), + persTmStore::DUMP_HK_STORE_DONE, persTmStore::DUMP_HK_CANCELLED, *SdCardManager::instance(), PTME_LOCKED); hkStore->connectModeTreeParent(satsystem::com::SUBSYSTEM); From 6bfb0d4fb058b3afe20e7d01b5b21e14cb921ede Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 23 May 2023 19:14:03 +0200 Subject: [PATCH 189/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 13c81828..cc74f261 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -68,6 +68,7 @@ will consitute of a breaking change warranting a new major release: - PLOC MPSoC special communication is now scheduled, which allows flash read and flash write commands to work. - Fixed the MPSoC flash write command. +- HK TM store: The HK store dump success event was triggered for cancelled HK dumps. # [v2.0.5] 2023-05-11 From 722247598598a1cdb3a7ee2e8607009ab5c43202 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 23 May 2023 19:18:42 +0200 Subject: [PATCH 190/506] changelog, remove unnecessary logic --- CHANGELOG.md | 3 +++ mission/com/TmStoreTaskBase.cpp | 8 +------- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index f36fc3aa..4073d7fa 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -55,6 +55,9 @@ TODO: New firmware package version. - Disable missed deadlines per default. Not useful in orbit, and triggers all the time on the EM build after a number of subsequent runs, without any apparent reason (deadlines are not actually missed, thread usage displayed is nominal) +- TM store dumpes will not be cancelled anymore if the transmitter is off. The dump can be cancelled + with an OFF command, and the PTME is perfectly capable of dumping without the transmitter being + on. ## Added diff --git a/mission/com/TmStoreTaskBase.cpp b/mission/com/TmStoreTaskBase.cpp index 6598225d..5efb40bf 100644 --- a/mission/com/TmStoreTaskBase.cpp +++ b/mission/com/TmStoreTaskBase.cpp @@ -39,6 +39,7 @@ bool TmStoreTaskBase::handleOneStore(PersistentTmStoreWithTmQueue& store, } // Dump TMs if (store.getState() == PersistentTmStore::State::DUMPING) { + sif::debug << "handling dump" << std::endl; if (handleOneDump(store, dumpContext, dumpPerformed) != returnvalue::OK) { return result; } @@ -94,13 +95,6 @@ void TmStoreTaskBase::cancelDump(DumpContext& ctx, PersistentTmStore& store, boo ReturnValue_t TmStoreTaskBase::handleOneDump(PersistentTmStoreWithTmQueue& store, DumpContext& dumpContext, bool& dumpPerformed) { ReturnValue_t result = returnvalue::OK; - // The PTME might have been reset an transmitter state change, so there is no point in continuing - // the dump. - // TODO: Will be solved in a cleaner way, this is kind of a hack. - if (not channel.isTxOn()) { - cancelDump(dumpContext, store, false); - return returnvalue::FAILED; - } // It is assumed that the PTME will only be locked for a short period (e.g. to change datarate). if (not channel.isBusy() and not ptmeLocked) { performDump(store, dumpContext, dumpPerformed); From 28beb006b2c39792eb4f158916027a473469f857 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 23 May 2023 19:37:01 +0200 Subject: [PATCH 191/506] thats an annoying bug --- CHANGELOG.md | 2 ++ mission/com/TmStoreTaskBase.cpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 13c81828..c7997dac 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -68,6 +68,8 @@ will consitute of a breaking change warranting a new major release: - PLOC MPSoC special communication is now scheduled, which allows flash read and flash write commands to work. - Fixed the MPSoC flash write command. +- When a PUS parsing error occured while parsing a TM store file, the dump completion procedure + was always executed. # [v2.0.5] 2023-05-11 diff --git a/mission/com/TmStoreTaskBase.cpp b/mission/com/TmStoreTaskBase.cpp index 6598225d..0d3da770 100644 --- a/mission/com/TmStoreTaskBase.cpp +++ b/mission/com/TmStoreTaskBase.cpp @@ -140,7 +140,7 @@ ReturnValue_t TmStoreTaskBase::performDump(PersistentTmStoreWithTmQueue& store, ReturnValue_t result = store.getNextDumpPacket(tmReader, fileHasSwapped); if (result != returnvalue::OK) { sif::error << "PersistentTmStore: Getting next dump packet failed" << std::endl; - } else if (fileHasSwapped or result == PersistentTmStore::DUMP_DONE) { + } else if (fileHasSwapped and result == PersistentTmStore::DUMP_DONE) { // This can happen if a file is corrupted and the next file swap completes the dump. dumpDoneHandler(); return returnvalue::OK; From 50327fb6148d25b1e8fb28a692a3cc5e72767b0d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 23 May 2023 19:44:06 +0200 Subject: [PATCH 192/506] get rid of the printouts and of the delay --- mission/com/PersistentLogTmStoreTask.cpp | 2 +- mission/com/PersistentSingleTmStoreTask.cpp | 2 +- mission/com/TmStoreTaskBase.cpp | 1 - 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/mission/com/PersistentLogTmStoreTask.cpp b/mission/com/PersistentLogTmStoreTask.cpp index 77f2bb7d..0d47e50f 100644 --- a/mission/com/PersistentLogTmStoreTask.cpp +++ b/mission/com/PersistentLogTmStoreTask.cpp @@ -48,7 +48,7 @@ ReturnValue_t PersistentLogTmStoreTask::performOperation(uint8_t opCode) { } else { // TODO: Would be best to remove this, but not delaying here can lead to evil issues. // Polling the PAPB of the PTME core too often leads to scheuduling issues. - TaskFactory::delayTask(2); + // TaskFactory::delayTask(2); } } } diff --git a/mission/com/PersistentSingleTmStoreTask.cpp b/mission/com/PersistentSingleTmStoreTask.cpp index 1b77365b..2afc8e31 100644 --- a/mission/com/PersistentSingleTmStoreTask.cpp +++ b/mission/com/PersistentSingleTmStoreTask.cpp @@ -30,7 +30,7 @@ ReturnValue_t PersistentSingleTmStoreTask::performOperation(uint8_t opCode) { } else { // TODO: Would be best to remove this, but not delaying here can lead to evil issues. // Polling the PAPB of the PTME core too often leads to scheuduling issues. - TaskFactory::delayTask(2); + // TaskFactory::delayTask(2); } } } diff --git a/mission/com/TmStoreTaskBase.cpp b/mission/com/TmStoreTaskBase.cpp index f7257f23..2158cf2a 100644 --- a/mission/com/TmStoreTaskBase.cpp +++ b/mission/com/TmStoreTaskBase.cpp @@ -39,7 +39,6 @@ bool TmStoreTaskBase::handleOneStore(PersistentTmStoreWithTmQueue& store, } // Dump TMs if (store.getState() == PersistentTmStore::State::DUMPING) { - sif::debug << "handling dump" << std::endl; if (handleOneDump(store, dumpContext, dumpPerformed) != returnvalue::OK) { return result; } From 303df55a1298a5c027a521c3a9fb8604bd3d266b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 23 May 2023 20:10:52 +0200 Subject: [PATCH 193/506] always dump data into the VC --- mission/com/VirtualChannel.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/mission/com/VirtualChannel.cpp b/mission/com/VirtualChannel.cpp index 8e225674..ea5527a8 100644 --- a/mission/com/VirtualChannel.cpp +++ b/mission/com/VirtualChannel.cpp @@ -11,10 +11,7 @@ ReturnValue_t VirtualChannel::sendNextTm(const uint8_t* data, size_t size) { } ReturnValue_t VirtualChannel::write(const uint8_t* data, size_t size) { - if (txOn) { - return ptme.writeToVc(vcId, data, size); - } - return returnvalue::OK; + return ptme.writeToVc(vcId, data, size); } uint8_t VirtualChannel::getVcid() const { return vcId; } @@ -22,10 +19,6 @@ uint8_t VirtualChannel::getVcid() const { return vcId; } const char* VirtualChannel::getName() const { return vcName.c_str(); } bool VirtualChannel::isBusy() const { - // Data is discarded, so channel is not busy. - if (not txOn) { - return false; - } return ptme.isBusy(vcId); } From 042b8fb3c324ccab5ba56bf9d03980b7b091a456 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 23 May 2023 20:13:02 +0200 Subject: [PATCH 194/506] changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 13c81828..8890ad7d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -43,6 +43,9 @@ will consitute of a breaking change warranting a new major release: - Disable missed deadlines per default. Not useful in orbit, and triggers all the time on the EM build after a number of subsequent runs, without any apparent reason (deadlines are not actually missed, thread usage displayed is nominal) +- Transmitter state is not taken into account anymore for writing into the PTME. The PTME should + be perfectly capable of generating a valid CADU, even when the transmitter is not ON for any + reason. ## Added From 266abad3b3a63efb1b2c45c287e335a13545c9a2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 23 May 2023 20:15:36 +0200 Subject: [PATCH 195/506] no dump cancel on TX OFF --- CHANGELOG.md | 3 +++ mission/com/TmStoreTaskBase.cpp | 7 ------- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 13c81828..b021a184 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -43,6 +43,9 @@ will consitute of a breaking change warranting a new major release: - Disable missed deadlines per default. Not useful in orbit, and triggers all the time on the EM build after a number of subsequent runs, without any apparent reason (deadlines are not actually missed, thread usage displayed is nominal) +- TM store dumpes will not be cancelled anymore if the transmitter is off. The dump can be cancelled + with an OFF command, and the PTME is perfectly capable of dumping without the transmitter being + on. ## Added diff --git a/mission/com/TmStoreTaskBase.cpp b/mission/com/TmStoreTaskBase.cpp index 6598225d..c8abbae8 100644 --- a/mission/com/TmStoreTaskBase.cpp +++ b/mission/com/TmStoreTaskBase.cpp @@ -94,13 +94,6 @@ void TmStoreTaskBase::cancelDump(DumpContext& ctx, PersistentTmStore& store, boo ReturnValue_t TmStoreTaskBase::handleOneDump(PersistentTmStoreWithTmQueue& store, DumpContext& dumpContext, bool& dumpPerformed) { ReturnValue_t result = returnvalue::OK; - // The PTME might have been reset an transmitter state change, so there is no point in continuing - // the dump. - // TODO: Will be solved in a cleaner way, this is kind of a hack. - if (not channel.isTxOn()) { - cancelDump(dumpContext, store, false); - return returnvalue::FAILED; - } // It is assumed that the PTME will only be locked for a short period (e.g. to change datarate). if (not channel.isBusy() and not ptmeLocked) { performDump(store, dumpContext, dumpPerformed); From ee0646caf140373c6f1132718fa8f5d767120cb9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 24 May 2023 11:04:07 +0200 Subject: [PATCH 196/506] enable IMTQ HK sets --- bsp_q7s/em/emObjectFactory.cpp | 2 +- dummies/ImtqDummy.cpp | 40 ++++++++++++++++++++++++++++++++-- dummies/ImtqDummy.h | 26 +++++++++++++++++++++- dummies/helperFactory.cpp | 6 +++-- dummies/helperFactory.h | 2 +- 5 files changed, 69 insertions(+), 7 deletions(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 36863b89..59660b86 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -79,7 +79,7 @@ void ObjectFactory::produce(void* args) { #endif satsystem::EIVE_SYSTEM.setI2cRecoveryParams(pwrSwitcher); - dummy::createDummies(dummyCfg, *pwrSwitcher, gpioComIF); + dummy::createDummies(dummyCfg, *pwrSwitcher, gpioComIF, enableHkSets); new CoreController(objects::CORE_CONTROLLER, enableHkSets); diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index b2f61bb3..8f8d1513 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -2,8 +2,12 @@ #include -ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) - : DeviceHandlerBase(objectId, comif, comCookie) {} +ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, + bool enableHkSets) + : DeviceHandlerBase(objectId, comif, comCookie), + setNoTorque(this), + setWithTorque(this), + enableHkSets(enableHkSets) {} ImtqDummy::~ImtqDummy() = default; @@ -45,5 +49,37 @@ ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataP localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, new PoolEntry({0})); localDataPoolMap.emplace(imtq::DIPOLES_ID, new PoolEntry({0, 0, 0})); localDataPoolMap.emplace(imtq::CURRENT_TORQUE_DURATION, new PoolEntry({0})); + + // ENG HK No Torque + localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::DIGITAL_CURRENT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::ANALOG_CURRENT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::COIL_CURRENTS, &coilCurrentsMilliampsNoTorque); + localDataPoolMap.emplace(imtq::COIL_TEMPERATURES, &coilTempsNoTorque); + localDataPoolMap.emplace(imtq::MCU_TEMPERATURE, new PoolEntry({0})); + + // ENG HK With Torque + localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::DIGITAL_CURRENT_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::ANALOG_CURRENT_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::COIL_CURRENTS_WT, &coilCurrentsMilliampsWithTorque); + localDataPoolMap.emplace(imtq::COIL_TEMPERATURES_WT, &coilTempsWithTorque); + localDataPoolMap.emplace(imtq::MCU_TEMPERATURE_WT, new PoolEntry({0})); + + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(setNoTorque.getSid(), enableHkSets, 30.0)); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(setWithTorque.getSid(), enableHkSets, 30.0)); return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager); } + +LocalPoolDataSetBase *ImtqDummy::getDataSetHandle(sid_t sid) { + if (sid == setNoTorque.getSid()) { + return &setNoTorque; + } else if (sid == setWithTorque.getSid()) { + return &setWithTorque; + } + return nullptr; +} diff --git a/dummies/ImtqDummy.h b/dummies/ImtqDummy.h index 0cfdf518..5a1de175 100644 --- a/dummies/ImtqDummy.h +++ b/dummies/ImtqDummy.h @@ -3,6 +3,8 @@ #include +#include "mission/acs/imtqHelpers.h" + class ImtqDummy : public DeviceHandlerBase { public: static const DeviceCommandId_t SIMPLE_COMMAND = 1; @@ -11,10 +13,31 @@ class ImtqDummy : public DeviceHandlerBase { static const uint8_t SIMPLE_COMMAND_DATA = 1; static const uint8_t PERIODIC_REPLY_DATA = 2; - ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie); + ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, bool enableHkSets); ~ImtqDummy() override; protected: + imtq::HkDatasetNoTorque setNoTorque; + imtq::HkDatasetWithTorque setWithTorque; + bool enableHkSets; + + PoolEntry statusMode = PoolEntry({0}); + PoolEntry statusError = PoolEntry({0}); + PoolEntry statusConfig = PoolEntry({0}); + PoolEntry statusUptime = PoolEntry({0}); + + PoolEntry mgmCalEntry = PoolEntry(3); + PoolEntry dipolesPoolEntry = PoolEntry({0, 0, 0}, false); + PoolEntry torqueDurationEntry = PoolEntry({0}, false); + PoolEntry coilCurrentsMilliampsNoTorque = PoolEntry(3); + PoolEntry coilCurrentsMilliampsWithTorque = PoolEntry(3); + PoolEntry coilTempsNoTorque = PoolEntry(3); + PoolEntry coilTempsWithTorque = PoolEntry(3); + PoolEntry mtmRawNoTorque = PoolEntry(3); + PoolEntry actStatusNoTorque = PoolEntry(1); + PoolEntry mtmRawWithTorque = PoolEntry(3); + PoolEntry actStatusWithTorque = PoolEntry(1); + void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; @@ -28,6 +51,7 @@ class ImtqDummy : public DeviceHandlerBase { uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) override; + LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; }; #endif /* DUMMIES_IMTQDUMMY_H_ */ diff --git a/dummies/helperFactory.cpp b/dummies/helperFactory.cpp index d7fdafbd..90a50160 100644 --- a/dummies/helperFactory.cpp +++ b/dummies/helperFactory.cpp @@ -42,7 +42,8 @@ #include "mission/system/tree/payloadModeTree.h" #include "mission/tcs/defs.h" -void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpioIF) { +void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpioIF, + bool enableHkSets) { new ComIFDummy(objects::DUMMY_COM_IF); auto* comCookieDummy = new ComCookieDummy(); if (cfg.addBpxBattDummy) { @@ -74,7 +75,8 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio } auto* imtqAssy = new ImtqAssembly(objects::IMTQ_ASSY); imtqAssy->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); - auto* imtqDummy = new ImtqDummy(objects::IMTQ_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + auto* imtqDummy = + new ImtqDummy(objects::IMTQ_HANDLER, objects::DUMMY_COM_IF, comCookieDummy, enableHkSets); imtqDummy->enableThermalModule(ThermalStateCfg()); imtqDummy->connectModeTreeParent(*imtqAssy); if (cfg.addOnlyAcuDummy) { diff --git a/dummies/helperFactory.h b/dummies/helperFactory.h index e3809404..467cb172 100644 --- a/dummies/helperFactory.h +++ b/dummies/helperFactory.h @@ -22,6 +22,6 @@ struct DummyCfg { bool addCamSwitcherDummy = true; }; -void createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitch, GpioIF* gpioIF); +void createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitch, GpioIF* gpioIF, bool enableHkSets); } // namespace dummy From 758f2b9d7ab312196a0dda273c3be6ced0edf008 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 24 May 2023 11:15:58 +0200 Subject: [PATCH 197/506] add ACU HK sets --- dummies/AcuDummy.cpp | 49 +++++++++++++++++++++++++++++++++++++-- dummies/AcuDummy.h | 8 ++++++- dummies/helperFactory.cpp | 4 ++-- 3 files changed, 56 insertions(+), 5 deletions(-) diff --git a/dummies/AcuDummy.cpp b/dummies/AcuDummy.cpp index 7c18f6bf..c9844eb1 100644 --- a/dummies/AcuDummy.cpp +++ b/dummies/AcuDummy.cpp @@ -2,8 +2,11 @@ #include -AcuDummy::AcuDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) - : DeviceHandlerBase(objectId, comif, comCookie) {} +AcuDummy::AcuDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, bool enableHkSets) + : DeviceHandlerBase(objectId, comif, comCookie), + coreHk(this), + auxHk(this), + enableHkSets(enableHkSets) {} AcuDummy::~AcuDummy() {} @@ -37,7 +40,49 @@ uint32_t AcuDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return ReturnValue_t AcuDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + using namespace ACU; + localDataPoolMap.emplace(pool::ACU_CURRENT_IN_CHANNELS, new PoolEntry(6)); + localDataPoolMap.emplace(pool::ACU_VOLTAGE_IN_CHANNELS, new PoolEntry(6)); + + localDataPoolMap.emplace(pool::ACU_VCC, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_VBAT, new PoolEntry({0})); + localDataPoolMap.emplace(ACU::pool::ACU_TEMPERATURES, new PoolEntry({10.0, 10.0, 10.0}, true)); + + localDataPoolMap.emplace(pool::ACU_MPPT_MODE, new PoolEntry({0})); + + localDataPoolMap.emplace(pool::ACU_VBOOST_IN_CHANNELS, new PoolEntry(6)); + localDataPoolMap.emplace(pool::ACU_POWER_IN_CHANNELS, new PoolEntry(6)); + + localDataPoolMap.emplace(pool::ACU_DAC_ENABLES, new PoolEntry(3)); + localDataPoolMap.emplace(pool::ACU_DAC_RAW_CHANNELS, new PoolEntry(6)); + + localDataPoolMap.emplace(pool::ACU_BOOTCAUSE, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_BOOTCNT, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_UPTIME, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_RESET_CAUSE, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_MPPT_TIME, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_MPPT_PERIOD, new PoolEntry({0})); + + localDataPoolMap.emplace(pool::ACU_DEVICES, new PoolEntry(8)); + localDataPoolMap.emplace(pool::ACU_DEVICES_STATUS, new PoolEntry(8)); + + localDataPoolMap.emplace(pool::ACU_WDT_CNT_GND, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_WDT_GND_LEFT, new PoolEntry({0})); + + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), enableHkSets, 30.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(auxHk.getSid(), enableHkSets, 6000.0)); return returnvalue::OK; } + +LocalPoolDataSetBase *AcuDummy::getDataSetHandle(sid_t sid) { + if (sid == coreHk.getSid()) { + return &coreHk; + } else if (sid == auxHk.getSid()) { + return &auxHk; + } + return nullptr; +} diff --git a/dummies/AcuDummy.h b/dummies/AcuDummy.h index d5527222..8d855281 100644 --- a/dummies/AcuDummy.h +++ b/dummies/AcuDummy.h @@ -2,6 +2,7 @@ #define DUMMIES_ACUDUMMY_H_ #include +#include class AcuDummy : public DeviceHandlerBase { public: @@ -11,10 +12,14 @@ class AcuDummy : public DeviceHandlerBase { static const uint8_t SIMPLE_COMMAND_DATA = 1; static const uint8_t PERIODIC_REPLY_DATA = 2; - AcuDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie); + AcuDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, bool enableHkSets); virtual ~AcuDummy(); protected: + ACU::CoreHk coreHk; + ACU::AuxHk auxHk; + bool enableHkSets; + void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; @@ -28,6 +33,7 @@ class AcuDummy : public DeviceHandlerBase { uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) override; + LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; }; #endif /* DUMMIES_ACUDUMMY_H_ */ diff --git a/dummies/helperFactory.cpp b/dummies/helperFactory.cpp index 90a50160..16de01f0 100644 --- a/dummies/helperFactory.cpp +++ b/dummies/helperFactory.cpp @@ -80,9 +80,9 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio imtqDummy->enableThermalModule(ThermalStateCfg()); imtqDummy->connectModeTreeParent(*imtqAssy); if (cfg.addOnlyAcuDummy) { - new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy, enableHkSets); } else if (cfg.addPowerDummies) { - new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy, enableHkSets); new PduDummy(objects::PDU1_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new PduDummy(objects::PDU2_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new P60DockDummy(objects::P60DOCK_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); From 9e0989915c6087be44247352933db4106db2a44b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 24 May 2023 11:16:35 +0200 Subject: [PATCH 198/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index cda8e93b..bae37800 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -52,6 +52,7 @@ will consitute of a breaking change warranting a new major release: - Add the remaining system modes. - PLOC MPSoC flash read command working. - BPX battery handler is added for EM by default. +- ACU dummy HK sets. ## Fixed From b899bad0a81c30de37951c3b2ed85011ea756f4a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 24 May 2023 11:17:00 +0200 Subject: [PATCH 199/506] IMTQ HK sets --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index cda8e93b..56943595 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -52,6 +52,7 @@ will consitute of a breaking change warranting a new major release: - Add the remaining system modes. - PLOC MPSoC flash read command working. - BPX battery handler is added for EM by default. +- IMTQ HK sets ## Fixed From 31e24e297ff6baf5d4683bbf67951938fcf6f470 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 24 May 2023 11:53:07 +0200 Subject: [PATCH 200/506] fix host SW --- bsp_hosted/ObjectFactory.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 018d0e56..dd88d552 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -62,6 +62,10 @@ void ObjectFactory::produce(void* args) { StorageManagerIF* tmStore; StorageManagerIF* ipcStore; PersistentTmStores persistentStores; + bool enableHkSets = false; +#if OBSW_ENABLE_PERIODIC_HK == 1 + enableHkSets = true; +#endif auto sdcMan = new DummySdCardManager("/tmp"); ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel, *sdcMan, &ipcStore, &tmStore, persistentStores, 120); @@ -101,7 +105,7 @@ void ObjectFactory::produce(void* args) { #endif dummy::DummyCfg cfg; - dummy::createDummies(cfg, *dummySwitcher, dummyGpioIF); + dummy::createDummies(cfg, *dummySwitcher, dummyGpioIF, enableHkSets); HeaterHandler* heaterHandler = nullptr; // new ThermalController(objects::THERMAL_CONTROLLER); From 276501c504d50e7822dbb44d2163eff957140ad5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 25 May 2023 08:43:52 +0200 Subject: [PATCH 201/506] another tweak for TM stores --- mission/com/TmStoreTaskBase.cpp | 23 ++++++++++------------- mission/com/VirtualChannel.cpp | 4 +--- 2 files changed, 11 insertions(+), 16 deletions(-) diff --git a/mission/com/TmStoreTaskBase.cpp b/mission/com/TmStoreTaskBase.cpp index 2158cf2a..0470dc04 100644 --- a/mission/com/TmStoreTaskBase.cpp +++ b/mission/com/TmStoreTaskBase.cpp @@ -131,25 +131,22 @@ ReturnValue_t TmStoreTaskBase::performDump(PersistentTmStoreWithTmQueue& store, dumpContext.ptmeBusyCounter = 0; tmSinkBusyCd.resetTimer(); ReturnValue_t result = store.getNextDumpPacket(tmReader, fileHasSwapped); - if (result != returnvalue::OK) { - sif::error << "PersistentTmStore: Getting next dump packet failed" << std::endl; - } else if (fileHasSwapped and result == PersistentTmStore::DUMP_DONE) { + if (fileHasSwapped and result == PersistentTmStore::DUMP_DONE) { // This can happen if a file is corrupted and the next file swap completes the dump. dumpDoneHandler(); return returnvalue::OK; + } else if (result != returnvalue::OK) { + sif::error << "PersistentTmStore: Getting next dump packet failed" << std::endl; + return result; } dumpedLen = tmReader.getFullPacketLen(); - // Only write to VC if mode is on, but always confirm the dump. - // If the mode is OFF, it is assumed the PTME is not usable and is not allowed to be written - // (e.g. to confirm a reset or the transmitter is off anyway). - if (mode == MODE_ON) { - result = channel.write(tmReader.getFullData(), dumpedLen); - if (result == DirectTmSinkIF::IS_BUSY) { - sif::warning << "PersistentTmStore: Unexpected VC channel busy" << std::endl; - } else if (result != returnvalue::OK) { - sif::warning << "PersistentTmStore: Unexpected VC channel write failure" << std::endl; - } + result = channel.write(tmReader.getFullData(), dumpedLen); + if (result == DirectTmSinkIF::IS_BUSY) { + sif::warning << "PersistentTmStore: Unexpected VC channel busy" << std::endl; + } else if (result != returnvalue::OK) { + sif::warning << "PersistentTmStore: Unexpected VC channel write failure" << std::endl; } + result = store.confirmDump(tmReader, fileHasSwapped); if ((result == PersistentTmStore::DUMP_DONE or result == returnvalue::OK)) { dumpPerformed = true; diff --git a/mission/com/VirtualChannel.cpp b/mission/com/VirtualChannel.cpp index ea5527a8..ff3749a9 100644 --- a/mission/com/VirtualChannel.cpp +++ b/mission/com/VirtualChannel.cpp @@ -18,9 +18,7 @@ uint8_t VirtualChannel::getVcid() const { return vcId; } const char* VirtualChannel::getName() const { return vcName.c_str(); } -bool VirtualChannel::isBusy() const { - return ptme.isBusy(vcId); -} +bool VirtualChannel::isBusy() const { return ptme.isBusy(vcId); } void VirtualChannel::cancelTransfer() { ptme.cancelTransfer(vcId); } From 8d041c67538a1f1dc85f16591a71d2e88920a04c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 25 May 2023 08:45:20 +0200 Subject: [PATCH 202/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 376a7f64..7e7ba753 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -49,6 +49,8 @@ will consitute of a breaking change warranting a new major release: - Transmitter state is not taken into account anymore for writing into the PTME. The PTME should be perfectly capable of generating a valid CADU, even when the transmitter is not ON for any reason. +- OFF mode is ignores in TM store for determining whether a store will be written. The modes will + only be used to cancel a transfer. ## Added From e1aff4c641ac2ee7df2475d3aba6a5ea963fab36 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 25 May 2023 08:48:03 +0200 Subject: [PATCH 203/506] another small logic fix --- CHANGELOG.md | 1 + mission/tmtc/PersistentTmStore.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7e7ba753..341f1a26 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -82,6 +82,7 @@ will consitute of a breaking change warranting a new major release: - HK TM store: The HK store dump success event was triggered for cancelled HK dumps. - When a PUS parsing error occured while parsing a TM store file, the dump completion procedure was always executed. +- Some smaller logic fixes in the TM store base class # [v2.0.5] 2023-05-11 diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index 7c3738fc..8174f8a0 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -256,6 +256,7 @@ ReturnValue_t PersistentTmStore::getNextDumpPacket(PusTmReader& reader, bool& fi if (state == State::IDLE or dumpParams.pendingPacketDump) { return returnvalue::FAILED; } + fileHasSwapped = false; reader.setReadOnlyData(fileBuf.data() + dumpParams.currentSize, fileBuf.size() - dumpParams.currentSize); // CRC check to fully ensure this is a valid TM @@ -270,7 +271,6 @@ ReturnValue_t PersistentTmStore::getNextDumpPacket(PusTmReader& reader, bool& fi fileHasSwapped = true; return loadNextDumpFile(); } - fileHasSwapped = false; dumpParams.pendingPacketDump = true; return returnvalue::OK; } From db79e34d3bcc3c72fad0dbf2352d530d15702b69 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 25 May 2023 11:20:28 +0200 Subject: [PATCH 204/506] added switcher for IMTQ dummy --- dummies/ImtqDummy.cpp | 14 ++++++++++++-- dummies/ImtqDummy.h | 6 +++++- dummies/helperFactory.cpp | 5 +++-- 3 files changed, 20 insertions(+), 5 deletions(-) diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index 8f8d1513..0c8f9076 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -3,11 +3,12 @@ #include ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, - bool enableHkSets) + power::Switch_t pwrSwitcher, bool enableHkSets) : DeviceHandlerBase(objectId, comif, comCookie), setNoTorque(this), setWithTorque(this), - enableHkSets(enableHkSets) {} + enableHkSets(enableHkSets), + switcher(pwrSwitcher) {} ImtqDummy::~ImtqDummy() = default; @@ -15,6 +16,15 @@ void ImtqDummy::doStartUp() { setMode(MODE_NORMAL); } void ImtqDummy::doShutDown() { setMode(_MODE_POWER_DOWN); } +ReturnValue_t ImtqDummy::getSwitches(const uint8_t **switches, uint8_t *numberOfSwitches) { + if (switcher != power::NO_SWITCH) { + *numberOfSwitches = 1; + *switches = &switcher; + return returnvalue::OK; + } + return DeviceHandlerBase::NO_SWITCH; +} + ReturnValue_t ImtqDummy::buildNormalDeviceCommand(DeviceCommandId_t *id) { return NOTHING_TO_SEND; } ReturnValue_t ImtqDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { diff --git a/dummies/ImtqDummy.h b/dummies/ImtqDummy.h index 5a1de175..990df6e0 100644 --- a/dummies/ImtqDummy.h +++ b/dummies/ImtqDummy.h @@ -13,10 +13,12 @@ class ImtqDummy : public DeviceHandlerBase { static const uint8_t SIMPLE_COMMAND_DATA = 1; static const uint8_t PERIODIC_REPLY_DATA = 2; - ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, bool enableHkSets); + ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, + power::Switch_t pwrSwitcher, bool enableHkSets); ~ImtqDummy() override; protected: + ReturnValue_t getSwitches(const uint8_t **switches, uint8_t *numberOfSwitches) override; imtq::HkDatasetNoTorque setNoTorque; imtq::HkDatasetWithTorque setWithTorque; bool enableHkSets; @@ -38,6 +40,8 @@ class ImtqDummy : public DeviceHandlerBase { PoolEntry mtmRawWithTorque = PoolEntry(3); PoolEntry actStatusWithTorque = PoolEntry(1); + power::Switch_t switcher = power::NO_SWITCH; + void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; diff --git a/dummies/helperFactory.cpp b/dummies/helperFactory.cpp index 16de01f0..157d11d3 100644 --- a/dummies/helperFactory.cpp +++ b/dummies/helperFactory.cpp @@ -75,9 +75,10 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio } auto* imtqAssy = new ImtqAssembly(objects::IMTQ_ASSY); imtqAssy->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); - auto* imtqDummy = - new ImtqDummy(objects::IMTQ_HANDLER, objects::DUMMY_COM_IF, comCookieDummy, enableHkSets); + auto* imtqDummy = new ImtqDummy(objects::IMTQ_HANDLER, objects::DUMMY_COM_IF, comCookieDummy, + power::Switches::PDU1_CH3_MGT_5V, enableHkSets); imtqDummy->enableThermalModule(ThermalStateCfg()); + imtqDummy->setPowerSwitcher(&pwrSwitcher); imtqDummy->connectModeTreeParent(*imtqAssy); if (cfg.addOnlyAcuDummy) { new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy, enableHkSets); From a6787538f2baaa6481ad9cbb1596b014952a16f6 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 25 May 2023 11:21:28 +0200 Subject: [PATCH 205/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 341f1a26..39f6a0a8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -59,6 +59,7 @@ will consitute of a breaking change warranting a new major release: - BPX battery handler is added for EM by default. - ACU dummy HK sets - IMTQ HK sets +- IMTQ dummy now handles power switch ## Fixed From 5afe22bc92042144b1bb54a91066f5a0a05f70b0 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 25 May 2023 15:48:28 +0200 Subject: [PATCH 206/506] heater mode fix --- mission/tcs/HeaterHandler.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/mission/tcs/HeaterHandler.cpp b/mission/tcs/HeaterHandler.cpp index 630dc92c..f422f382 100644 --- a/mission/tcs/HeaterHandler.cpp +++ b/mission/tcs/HeaterHandler.cpp @@ -268,6 +268,8 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) { triggerEvent(GPIO_PULL_HIGH_FAILED, result); } else { triggerEvent(HEATER_WENT_ON, heaterIdx, 0); + EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO, + MODE_ON, MODE_OFF); { MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); heater.switchState = ON; @@ -324,6 +326,8 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switch heaterIdx) { heater.switchState = OFF; } triggerEvent(HEATER_WENT_OFF, heaterIdx, 0); + EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO, + MODE_OFF, MODE_ON); // When all switches are off, also main line switch will be turned off if (allSwitchesOff()) { mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF); From de18ebfbe0798dbcf037d092a6afe58ac82e698b Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 26 May 2023 09:57:03 +0200 Subject: [PATCH 207/506] fixed error angle being calculated but not returned --- mission/controller/acs/Guidance.cpp | 4 ++-- mission/controller/acs/Guidance.h | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 79c6b416..7c8fc2a1 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -412,7 +412,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], - double errorQuat[4], double errorSatRotRate[3], double errorAngle) { + double errorQuat[4], double errorSatRotRate[3], double &errorAngle) { // First calculate error quaternion between current and target orientation QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); // Last calculate add rotation from reference quaternion @@ -443,7 +443,7 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double errorQuat[4], - double errorSatRotRate[3], double errorAngle) { + double errorSatRotRate[3], double &errorAngle) { // First calculate error quaternion between current and target orientation QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); // Keep scalar part of quaternion positive diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index a52c476a..39e77456 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -37,15 +37,15 @@ class Guidance { double targetQuat[4], double refSatRate[3]); // @note: Calculates the error quaternion between the current orientation and the target - // quaternion, considering a reference quaternion. Additionally the difference between the actual + // quaternion, considering a reference quaternion. Additionally the difference between the actual // and a desired satellite rotational rate is calculated, again considering a reference rotational // rate. Lastly gives back the error angle of the error quaternion. void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], - double errorQuat[4], double errorSatRotRate[3], double errorAngle); + double errorQuat[4], double errorSatRotRate[3], double &errorAngle); void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], - double errorAngle); + double &errorAngle); void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *targetSatRotRate); From 335394b8639814edee4b107e087482ac46cd9cd9 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 26 May 2023 13:51:24 +0200 Subject: [PATCH 208/506] fixed use of c abs which will truncate the value --- mission/controller/acs/ActuatorCmd.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index d2fe2d65..a8fab6a4 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -61,7 +61,7 @@ void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentAct // Scaling along largest element if dipol exceeds maximum uint8_t maxIdx = 0; VectorOperations::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx); - double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]); + double maxAbsValue = std::abs(dipolMomentActuatorDouble[maxIdx]); if (maxAbsValue > maxDipol) { double scalingFactor = maxDipol / maxAbsValue; VectorOperations::mulScalar(dipolMomentActuatorDouble, scalingFactor, From a7c6cd017d80b88bfe64418d6f00b6634dc5c264 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 26 May 2023 13:53:46 +0200 Subject: [PATCH 209/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 39f6a0a8..1dfeed76 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -84,6 +84,8 @@ will consitute of a breaking change warranting a new major release: - When a PUS parsing error occured while parsing a TM store file, the dump completion procedure was always executed. - Some smaller logic fixes in the TM store base class +- Fixed usage of C `abs` instead of C++ `std::abs`, which results in MTQ commands not being + scaled correctly between 1Am² and 0.2Am². # [v2.0.5] 2023-05-11 From 9c78362ac534b8f665c72b97f4a64a29b5368553 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 27 May 2023 17:00:41 +0200 Subject: [PATCH 210/506] use read write permissions for uio --- linux/ipcore/PapbVcInterface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/ipcore/PapbVcInterface.cpp b/linux/ipcore/PapbVcInterface.cpp index 7a1a89e4..bf1f48bb 100644 --- a/linux/ipcore/PapbVcInterface.cpp +++ b/linux/ipcore/PapbVcInterface.cpp @@ -16,7 +16,7 @@ PapbVcInterface::~PapbVcInterface() {} ReturnValue_t PapbVcInterface::initialize() { UioMapper uioMapper(uioFile, mapNum); ReturnValue_t result = uioMapper.getMappedAdress(const_cast(&vcBaseReg), - UioMapper::Permissions::WRITE_ONLY); + UioMapper::Permissions::READ_WRITE); if (result != returnvalue::OK) { return result; } From 71eca867ea630fad8bcb4c5feb4d197af0e3c3a7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 27 May 2023 17:03:33 +0200 Subject: [PATCH 211/506] i think the rad sesnor messing everything up --- CMakeLists.txt | 2 +- bsp_q7s/em/emObjectFactory.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b5ecf173..d47be117 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,7 +98,7 @@ set(OBSW_ADD_THERMAL_TEMP_INSERTER ${OBSW_Q7S_EM} CACHE STRING "Add thermal sensor temperature inserter") set(OBSW_ADD_ACS_BOARD - 1 + ${INIT_VAL} CACHE STRING "Add ACS board module") set(OBSW_ADD_GPS_CTRL ${INIT_VAL} diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 59660b86..72a58987 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -98,7 +98,7 @@ void ObjectFactory::produce(void* args) { // and will cause xsc_boot_copy commands to always boot to 0 0 // createRadSensorComponent(gpioComIF); // Still initialize chip select to avoid SPI bus issues. - createRadSensorChipSelect(gpioComIF); + // createRadSensorChipSelect(gpioComIF); #if OBSW_ADD_ACS_BOARD == 1 createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher, true, From ada77ed53bb7f070fa3f2c71cd8443c6bc0aad8a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 May 2023 09:22:59 +0200 Subject: [PATCH 212/506] ACS board still required --- bsp_q7s/em/emObjectFactory.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 72a58987..5b2935b7 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -97,10 +97,10 @@ void ObjectFactory::produce(void* args) { // TODO: Careful! Switching this on somehow messes with the communication with the ProASIC // and will cause xsc_boot_copy commands to always boot to 0 0 // createRadSensorComponent(gpioComIF); - // Still initialize chip select to avoid SPI bus issues. - // createRadSensorChipSelect(gpioComIF); #if OBSW_ADD_ACS_BOARD == 1 + // Still initialize chip select to avoid SPI bus issues. + createRadSensorChipSelect(gpioComIF); createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher, true, adis1650x::Type::ADIS16507); #else From c5640c9fca3f62bef8afe1dba93dbc1bf90bf221 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 May 2023 15:14:07 +0200 Subject: [PATCH 213/506] prevent SPAM --- mission/controller/ThermalController.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 7133dd88..4c475e35 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1631,13 +1631,18 @@ bool ThermalController::selectAndReadSensorTemp(HeaterContext& htrCtx) { bool ThermalController::chooseHeater(heater::Switch& switchNr, heater::Switch redSwitchNr) { bool heaterAvailable = true; - if (heaterHandler.getHealth(switchNr) != HasHealthIF::HEALTHY) { - if (heaterHandler.getHealth(redSwitchNr) == HasHealthIF::HEALTHY) { + HasHealthIF::HealthState mainHealth = heaterHandler.getHealth(switchNr); + HasHealthIF::HealthState redHealth = heaterHandler.getHealth(redSwitchNr); + if (mainHealth != HasHealthIF::HEALTHY) { + if (redHealth == HasHealthIF::HEALTHY) { switchNr = redSwitchNr; redSwitchNrInUse = true; } else { heaterAvailable = false; - triggerEvent(tcsCtrl::NO_HEALTHY_HEATER_AVAILABLE, switchNr, redSwitchNr); + // Special case: Ground might command/do something with the heaters, so prevent spam. + if (not(mainHealth == EXTERNAL_CONTROL and redHealth == EXTERNAL_CONTROL)) { + triggerEvent(tcsCtrl::NO_HEALTHY_HEATER_AVAILABLE, switchNr, redSwitchNr); + } } } else { redSwitchNrInUse = false; From b135e2f6a155fd9accd1165d773294133412bf04 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 May 2023 15:14:47 +0200 Subject: [PATCH 214/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1dfeed76..a9b4f3ee 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -86,6 +86,7 @@ will consitute of a breaking change warranting a new major release: - Some smaller logic fixes in the TM store base class - Fixed usage of C `abs` instead of C++ `std::abs`, which results in MTQ commands not being scaled correctly between 1Am² and 0.2Am². +- Prevent spam of TCS controller heater unavailability event if all heaters are in external control. # [v2.0.5] 2023-05-11 From 7811a4cc6ba36840977c7166dec79f9d43c2e38b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 May 2023 15:17:11 +0200 Subject: [PATCH 215/506] small fix for mode event heater --- mission/tcs/HeaterHandler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/tcs/HeaterHandler.cpp b/mission/tcs/HeaterHandler.cpp index f422f382..de4b600d 100644 --- a/mission/tcs/HeaterHandler.cpp +++ b/mission/tcs/HeaterHandler.cpp @@ -269,7 +269,7 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) { } else { triggerEvent(HEATER_WENT_ON, heaterIdx, 0); EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO, - MODE_ON, MODE_OFF); + MODE_ON, 0); { MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); heater.switchState = ON; @@ -327,7 +327,7 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switch heaterIdx) { } triggerEvent(HEATER_WENT_OFF, heaterIdx, 0); EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO, - MODE_OFF, MODE_ON); + MODE_OFF, 0); // When all switches are off, also main line switch will be turned off if (allSwitchesOff()) { mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF); From ebf5609680f6010976f541c0efad9eb9559173bf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 May 2023 16:45:09 +0200 Subject: [PATCH 216/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1dfeed76..c48e327f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -86,6 +86,8 @@ will consitute of a breaking change warranting a new major release: - Some smaller logic fixes in the TM store base class - Fixed usage of C `abs` instead of C++ `std::abs`, which results in MTQ commands not being scaled correctly between 1Am² and 0.2Am². +- TCS Heater Handler: Always trigger mode event if a heater goes `OFF` or `ON`. This event might + soon replace the `HEATER_WENT_ON` and `HEATER_WENT_OFF` events. # [v2.0.5] 2023-05-11 From 0aa617d4401ee3641b8e167db963366feb69aa04 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 May 2023 16:54:56 +0200 Subject: [PATCH 217/506] switch info fix --- CHANGELOG.md | 2 ++ mission/controller/ThermalController.cpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a12c68bb..990ed751 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -89,6 +89,8 @@ will consitute of a breaking change warranting a new major release: - TCS Heater Handler: Always trigger mode event if a heater goes `OFF` or `ON`. This event might soon replace the `HEATER_WENT_ON` and `HEATER_WENT_OFF` events. - Prevent spam of TCS controller heater unavailability event if all heaters are in external control. +- TCS heater switch info set contained invalid values because of a faulty `memcpy` in the TCS + controller. There is not crash risk but the heater states were invalid. # [v2.0.5] 2023-05-11 diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 4c475e35..6205ce44 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -175,7 +175,7 @@ void ThermalController::performControlOperation() { heaterHandler.getAllSwitchStates(heaterSwitchStateArray); { PoolReadGuard pg(&heaterInfo); - std::memcpy(heaterInfo.heaterSwitchState.value, heaterStates.data(), 8); + std::memcpy(heaterInfo.heaterSwitchState.value, heaterSwitchStateArray.data(), 8); { PoolReadGuard pg2(¤tVecPdu2); if (pg.getReadResult() == returnvalue::OK and pg2.getReadResult() == returnvalue::OK) { From 9921522ce0c1ec36e2faea7e9584cd7c7390b61b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 May 2023 19:06:59 +0200 Subject: [PATCH 218/506] working, remove some old unrequired code --- mission/com/PersistentLogTmStoreTask.cpp | 6 ------ mission/com/PersistentSingleTmStoreTask.cpp | 6 ------ 2 files changed, 12 deletions(-) diff --git a/mission/com/PersistentLogTmStoreTask.cpp b/mission/com/PersistentLogTmStoreTask.cpp index 0d47e50f..28545457 100644 --- a/mission/com/PersistentLogTmStoreTask.cpp +++ b/mission/com/PersistentLogTmStoreTask.cpp @@ -42,13 +42,7 @@ ReturnValue_t PersistentLogTmStoreTask::performOperation(uint8_t opCode) { if (not someonesBusy) { TaskFactory::delayTask(100); } else if (vcBusyDuringDump) { - // TODO: Might not be necessary - sif::debug << "VC busy, delaying" << std::endl; TaskFactory::delayTask(10); - } else { - // TODO: Would be best to remove this, but not delaying here can lead to evil issues. - // Polling the PAPB of the PTME core too often leads to scheuduling issues. - // TaskFactory::delayTask(2); } } } diff --git a/mission/com/PersistentSingleTmStoreTask.cpp b/mission/com/PersistentSingleTmStoreTask.cpp index 2afc8e31..d6f43289 100644 --- a/mission/com/PersistentSingleTmStoreTask.cpp +++ b/mission/com/PersistentSingleTmStoreTask.cpp @@ -24,13 +24,7 @@ ReturnValue_t PersistentSingleTmStoreTask::performOperation(uint8_t opCode) { if (not busy) { TaskFactory::delayTask(100); } else if (dumpContext.vcBusyDuringDump) { - sif::debug << "VC busy, delaying" << std::endl; - // TODO: Might not be necessary TaskFactory::delayTask(10); - } else { - // TODO: Would be best to remove this, but not delaying here can lead to evil issues. - // Polling the PAPB of the PTME core too often leads to scheuduling issues. - // TaskFactory::delayTask(2); } } } From ddcdc38310b1ea0a2f247cb3472f8a29bf0ec0bc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 May 2023 19:22:23 +0200 Subject: [PATCH 219/506] try to get rid of the nanosleep --- linux/ipcore/PapbVcInterface.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/linux/ipcore/PapbVcInterface.cpp b/linux/ipcore/PapbVcInterface.cpp index bf1f48bb..97eb9954 100644 --- a/linux/ipcore/PapbVcInterface.cpp +++ b/linux/ipcore/PapbVcInterface.cpp @@ -65,12 +65,6 @@ ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size) { // idx += 4; // } for (size_t idx = 0; idx < size; idx++) { - // This delay is super-important, DO NOT REMOVE! - // Polling the GPIO or the config register too often messes up the scheduler. - // TODO: Maybe this should not be done like this. It would be better if there was a custom - // FPGA module which can accept packets and then takes care of dumping that packet into - // the PTME. DMA would be an ideal solution for this. - nanosleep(&BETWEEN_POLL_DELAY, &remDelay); if (pollInterfaceReadiness(2, false) == returnvalue::OK) { *(vcBaseReg + DATA_REG_OFFSET) = static_cast(data[idx]); } else { @@ -78,7 +72,6 @@ ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size) { return returnvalue::FAILED; } } - nanosleep(&BETWEEN_POLL_DELAY, &remDelay); if (pollInterfaceReadiness(2, false) == returnvalue::OK) { completePacketTransfer(); } else { From 23625b24963dc2a3d7fd85b47bfbcb6568f7e948 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 May 2023 09:53:13 +0200 Subject: [PATCH 220/506] move 2 pool variables --- mission/power/gsDefs.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/mission/power/gsDefs.h b/mission/power/gsDefs.h index d42cabd2..00bdf589 100644 --- a/mission/power/gsDefs.h +++ b/mission/power/gsDefs.h @@ -404,6 +404,11 @@ class PduCoreHk : public StaticLocalDataSet<9> { /** Battery mode: 1 = Critical, 2 = Safe, 3 = Normal, 4 = Full */ lp_var_t battMode = lp_var_t(sid.objectId, pool::PDU_BATT_MODE, this); lp_var_t temperature = lp_var_t(sid.objectId, pool::PDU_TEMPERATURE, this); + + /** Measured VCC */ + lp_var_t vcc = lp_var_t(sid.objectId, pool::PDU_VCC, this); + /** Measured VBAT */ + lp_var_t vbat = lp_var_t(sid.objectId, pool::PDU_VBAT, this); }; class PduConfig : public StaticLocalDataSet<32> { @@ -451,11 +456,6 @@ class PduAuxHk : public StaticLocalDataSet<36> { PduAuxHk(object_id_t objectId, uint32_t setId) : StaticLocalDataSet(sid_t(objectId, setId)) {} - /** Measured VCC */ - lp_var_t vcc = lp_var_t(sid.objectId, pool::PDU_VCC, this); - /** Measured VBAT */ - lp_var_t vbat = lp_var_t(sid.objectId, pool::PDU_VBAT, this); - /** Output converter enable status */ lp_vec_t converterEnable = lp_vec_t(sid.objectId, pool::PDU_CONV_EN, this); From f4523c8396e1d3684079e3d5b26c9afb948cba12 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 May 2023 10:39:56 +0200 Subject: [PATCH 221/506] that should fix some issues --- mission/power/GomspaceDeviceHandler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/power/GomspaceDeviceHandler.cpp b/mission/power/GomspaceDeviceHandler.cpp index 89fca8ee..c20ac4e9 100644 --- a/mission/power/GomspaceDeviceHandler.cpp +++ b/mission/power/GomspaceDeviceHandler.cpp @@ -595,8 +595,8 @@ ReturnValue_t GomspaceDeviceHandler::parsePduHkTable(PDU::PduCoreHk& coreHk, PDU for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) { coreHk.voltages[idx] = as(packet + 0x12 + (idx * 2)); } - auxHk.vcc.value = as(packet + 0x24); - auxHk.vbat.value = as(packet + 0x26); + coreHk.vcc.value = as(packet + 0x24); + coreHk.vbat.value = as(packet + 0x26); coreHk.temperature = as(packet + 0x28) * 0.1; for (uint8_t idx = 0; idx < 3; idx++) { From 170cb4d99cb970734af34d338be467157cde8f94 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 May 2023 10:45:40 +0200 Subject: [PATCH 222/506] changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 990ed751..7890df35 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,7 +18,7 @@ will consitute of a breaking change warranting a new major release: # [v2.2.0] to be released -# [v2.1.0] to be released +# [v3.0.0] to be released ## Changed From ec8e8533d67d966208c8cc5d59ab53a5dc4210b6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 May 2023 11:49:18 +0200 Subject: [PATCH 223/506] bump version --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d47be117..eaa2d340 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,9 +9,9 @@ # ############################################################################## cmake_minimum_required(VERSION 3.13) -set(OBSW_VERSION_MAJOR 2) +set(OBSW_VERSION_MAJOR 3) set(OBSW_VERSION_MINOR 0) -set(OBSW_VERSION_REVISION 5) +set(OBSW_VERSION_REVISION 0) # set(CMAKE_VERBOSE TRUE) From 0b06bc4c8b286046190e055876644fe6aa384693 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 May 2023 11:52:45 +0200 Subject: [PATCH 224/506] bump major version --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index eaa2d340..26bcbd80 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ # ############################################################################## cmake_minimum_required(VERSION 3.13) -set(OBSW_VERSION_MAJOR 3) +set(OBSW_VERSION_MAJOR 4) set(OBSW_VERSION_MINOR 0) set(OBSW_VERSION_REVISION 0) From 8113a71c79b94ae7b72ff6d5cc4d26715d7ce497 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 May 2023 14:15:54 +0200 Subject: [PATCH 225/506] PAPB VC simplification --- linux/ipcore/PapbVcInterface.cpp | 93 ++++++++++++++++---------------- linux/ipcore/PapbVcInterface.h | 3 +- 2 files changed, 47 insertions(+), 49 deletions(-) diff --git a/linux/ipcore/PapbVcInterface.cpp b/linux/ipcore/PapbVcInterface.cpp index 97eb9954..1920e932 100644 --- a/linux/ipcore/PapbVcInterface.cpp +++ b/linux/ipcore/PapbVcInterface.cpp @@ -28,7 +28,7 @@ ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size) { if (size < 4) { return returnvalue::FAILED; } - if (pollInterfaceReadiness(0, true) == returnvalue::OK) { + if (not pollReadyForPacket()) { startPacketTransfer(ByteWidthCfg::ONE); } else { return DirectTmSinkIF::IS_BUSY; @@ -65,19 +65,19 @@ ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size) { // idx += 4; // } for (size_t idx = 0; idx < size; idx++) { - if (pollInterfaceReadiness(2, false) == returnvalue::OK) { - *(vcBaseReg + DATA_REG_OFFSET) = static_cast(data[idx]); - } else { - abortPacketTransfer(); - return returnvalue::FAILED; - } - } - if (pollInterfaceReadiness(2, false) == returnvalue::OK) { - completePacketTransfer(); - } else { - abortPacketTransfer(); - return returnvalue::FAILED; + // if (pollInterfaceReadiness(2, false) == returnvalue::OK) { + *(vcBaseReg + DATA_REG_OFFSET) = static_cast(data[idx]); + //} else { + // abortPacketTransfer(); + // return returnvalue::FAILED; + //} } + // if (pollInterfaceReadiness(2, false) == returnvalue::OK) { + completePacketTransfer(); + //} else { + // abortPacketTransfer(); + // return returnvalue::FAILED; + //} return returnvalue::OK; } @@ -87,40 +87,39 @@ void PapbVcInterface::startPacketTransfer(ByteWidthCfg initWidth) { void PapbVcInterface::completePacketTransfer() { *vcBaseReg = CONFIG_END; } -ReturnValue_t PapbVcInterface::pollInterfaceReadiness(uint32_t maxPollRetries, - bool checkReadyForPacketState) const { - uint32_t busyIdx = 0; - nextDelay.tv_nsec = FIRST_DELAY_PAPB_POLLING_NS; +bool PapbVcInterface::pollReadyForPacket() const { + // uint32_t busyIdx = 0; + // nextDelay.tv_nsec = FIRST_DELAY_PAPB_POLLING_NS; - while (true) { - // Check if PAPB interface is ready to receive data. Use the configuration register for this. - // Bit 5, see PTME ptme_001_01-0-7-r2 Table 31. - uint32_t reg = *vcBaseReg; - bool busy = (reg >> 5) & 0b1; - bool readyForPacket = (reg >> 6) & 0b1; - if (checkReadyForPacketState) { - if (not busy and readyForPacket) { - return returnvalue::OK; - } else if (not busy and not readyForPacket) { - return PAPB_BUSY; - } - } else if (not busy) { - return returnvalue::OK; - } - - busyIdx++; - if (busyIdx >= maxPollRetries) { - return PAPB_BUSY; - } - - // Ignore signal handling here for now. - nanosleep(&nextDelay, &remDelay); - // Adaptive delay. - if (nextDelay.tv_nsec * 2 <= MAX_DELAY_PAPB_POLLING_NS) { - nextDelay.tv_nsec *= 2; - } - } - return returnvalue::OK; + // while (true) { + // Check if PAPB interface is ready to receive data. Use the configuration register for this. + // Bit 5, see PTME ptme_001_01-0-7-r2 Table 31. + uint32_t reg = *vcBaseReg; + // bool busy = (reg >> 5) & 0b1; + return (reg >> 6) & 0b1; + // if (checkReadyForPacketState) { + // if (not busy and readyForPacket) { + // return returnvalue::OK; + // } else if (not busy and not readyForPacket) { + // return PAPB_BUSY; + // } + // } else if (not busy) { + // return returnvalue::OK; + // } + // + // busyIdx++; + // if (busyIdx >= maxPollRetries) { + // return PAPB_BUSY; + // } + // + // // Ignore signal handling here for now. + // nanosleep(&nextDelay, &remDelay); + // // Adaptive delay. + // if (nextDelay.tv_nsec * 2 <= MAX_DELAY_PAPB_POLLING_NS) { + // nextDelay.tv_nsec *= 2; + // } + // } + // return returnvalue::OK; } bool PapbVcInterface::isVcInterfaceBufferEmpty() { @@ -141,7 +140,7 @@ bool PapbVcInterface::isVcInterfaceBufferEmpty() { return false; } -bool PapbVcInterface::isBusy() const { return pollInterfaceReadiness(0, true) == PAPB_BUSY; } +bool PapbVcInterface::isBusy() const { return not pollReadyForPacket(); } void PapbVcInterface::cancelTransfer() { abortPacketTransfer(); } diff --git a/linux/ipcore/PapbVcInterface.h b/linux/ipcore/PapbVcInterface.h index 71dd143b..ba6063b5 100644 --- a/linux/ipcore/PapbVcInterface.h +++ b/linux/ipcore/PapbVcInterface.h @@ -116,8 +116,7 @@ class PapbVcInterface : public VirtualChannelIF { * * @return returnvalue::OK when ready to receive data else PAPB_BUSY. */ - inline ReturnValue_t pollInterfaceReadiness(uint32_t maxPollRetries, - bool checkReadyForPacketState) const; + inline bool pollReadyForPacket() const; /** * @brief This function can be used for debugging to check whether there are packets in From 442b1c94a62781f8f08f0a2fb1ec21426f09c1dc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 May 2023 15:02:11 +0200 Subject: [PATCH 226/506] small fix --- linux/ipcore/PapbVcInterface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/ipcore/PapbVcInterface.cpp b/linux/ipcore/PapbVcInterface.cpp index 1920e932..fbc13fc5 100644 --- a/linux/ipcore/PapbVcInterface.cpp +++ b/linux/ipcore/PapbVcInterface.cpp @@ -28,7 +28,7 @@ ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size) { if (size < 4) { return returnvalue::FAILED; } - if (not pollReadyForPacket()) { + if (pollReadyForPacket()) { startPacketTransfer(ByteWidthCfg::ONE); } else { return DirectTmSinkIF::IS_BUSY; From 998110dea41ef042681134aec5c49e6de9cfad87 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 May 2023 16:10:52 +0200 Subject: [PATCH 227/506] back to somethng simple --- linux/ipcore/PapbVcInterface.cpp | 67 -------------------------------- 1 file changed, 67 deletions(-) diff --git a/linux/ipcore/PapbVcInterface.cpp b/linux/ipcore/PapbVcInterface.cpp index fbc13fc5..404f3653 100644 --- a/linux/ipcore/PapbVcInterface.cpp +++ b/linux/ipcore/PapbVcInterface.cpp @@ -33,51 +33,11 @@ ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size) { } else { return DirectTmSinkIF::IS_BUSY; } - // TODO: This should work but does not.. :( - // size_t idx = 0; - // while (idx < size) { - // - // nanosleep(&BETWEEN_POLL_DELAY, &remDelay); - // if ((size - idx) < 4) { - // *vcBaseReg = CONFIG_DATA_INPUT | (size - idx - 1); - // usleep(1); - // } - // if (pollPapbBusySignal(2) == returnvalue::OK) { - // // vcBaseReg + DATA_REG_OFFSET + 3 = static_cast(data + idx); - // // vcBaseReg + DATA_REG_OFFSET + 2 = static_cast(data + idx + 1); - // // vcBaseReg + DATA_REG_OFFSET + 1 = static_cast(data + idx + 2); - // // vcBaseReg + DATA_REG_OFFSET = static_cast(data + idx + 3); - // - // // std::memcpy((vcBaseReg + DATA_REG_OFFSET), data + idx , nextWriteSize); - // *(vcBaseReg + DATA_REG_OFFSET) = *reinterpret_cast(data + idx); - // //uint8_t* byteReg = reinterpret_cast(vcBaseReg + DATA_REG_OFFSET); - // - // //byteReg[0] = data[idx]; - // //byteReg[1] = data[idx]; - // } else { - // abortPacketTransfer(); - // return returnvalue::FAILED; - // } - // // TODO: Change this after the bugfix. Right now, the PAPB ignores the content of the byte - // // width configuration.5 - // // It's okay to increment by a larger amount for the last segment here, loop will be over - // // in any case. - // idx += 4; - // } for (size_t idx = 0; idx < size; idx++) { // if (pollInterfaceReadiness(2, false) == returnvalue::OK) { *(vcBaseReg + DATA_REG_OFFSET) = static_cast(data[idx]); - //} else { - // abortPacketTransfer(); - // return returnvalue::FAILED; - //} } - // if (pollInterfaceReadiness(2, false) == returnvalue::OK) { completePacketTransfer(); - //} else { - // abortPacketTransfer(); - // return returnvalue::FAILED; - //} return returnvalue::OK; } @@ -88,38 +48,11 @@ void PapbVcInterface::startPacketTransfer(ByteWidthCfg initWidth) { void PapbVcInterface::completePacketTransfer() { *vcBaseReg = CONFIG_END; } bool PapbVcInterface::pollReadyForPacket() const { - // uint32_t busyIdx = 0; - // nextDelay.tv_nsec = FIRST_DELAY_PAPB_POLLING_NS; - - // while (true) { // Check if PAPB interface is ready to receive data. Use the configuration register for this. // Bit 5, see PTME ptme_001_01-0-7-r2 Table 31. uint32_t reg = *vcBaseReg; // bool busy = (reg >> 5) & 0b1; return (reg >> 6) & 0b1; - // if (checkReadyForPacketState) { - // if (not busy and readyForPacket) { - // return returnvalue::OK; - // } else if (not busy and not readyForPacket) { - // return PAPB_BUSY; - // } - // } else if (not busy) { - // return returnvalue::OK; - // } - // - // busyIdx++; - // if (busyIdx >= maxPollRetries) { - // return PAPB_BUSY; - // } - // - // // Ignore signal handling here for now. - // nanosleep(&nextDelay, &remDelay); - // // Adaptive delay. - // if (nextDelay.tv_nsec * 2 <= MAX_DELAY_PAPB_POLLING_NS) { - // nextDelay.tv_nsec *= 2; - // } - // } - // return returnvalue::OK; } bool PapbVcInterface::isVcInterfaceBufferEmpty() { From a85b0a4a762c173d942d701e9185237601dd4ed6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 May 2023 16:31:50 +0200 Subject: [PATCH 228/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index ffce15e8..87083265 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -29,6 +29,7 @@ TODO: New firmware package version. ## Changed - Removed PTME busy/ready signals. Those were not used anyway because register reads are used now. +- APB bus access busy checking is not done anymore as this is performed by the bus itself now. # [v3.0.0] to be released From f93e299296f6e93ce46f75c1b983fe93cb865e56 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 May 2023 17:29:01 +0200 Subject: [PATCH 229/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 6ec0ce20..6182369e 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 6ec0ce20fa98877c9f88acb5fe9129254291344b +Subproject commit 6182369e4f40872c5c26e59be25d5fa79339176a From bfa0a0707c5ab33707e58e33870d21511a1cc9e4 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 1 Jun 2023 09:36:34 +0200 Subject: [PATCH 230/506] fixed parameter IDs --- mission/controller/acs/AcsParameters.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 61229c77..5c239133 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -551,10 +551,10 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0xB: parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; - case 0xB: + case 0xC: parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); break; - case 0xC: + case 0xD: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); break; default: From 3750f1ac570c9ecae888515713db75b689cbf4de Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 1 Jun 2023 09:36:50 +0200 Subject: [PATCH 231/506] frmt --- mission/controller/acs/ActuatorCmd.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index a3513bad..9013d6f2 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -57,8 +57,8 @@ void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxD const double *dipoleMoment, int16_t *dipoleMomentActuator) { // convert to actuator frame double dipoleMomentActuatorDouble[3] = {0, 0, 0}; - MatrixOperations::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3, 3, - 1); + MatrixOperations::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3, + 3, 1); // scaling along largest element if dipole exceeds maximum uint8_t maxIdx = 0; VectorOperations::maxAbsValue(dipoleMomentActuatorDouble, 3, &maxIdx); @@ -69,7 +69,8 @@ void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxD dipoleMomentActuatorDouble, 3); } // scale dipole from 1 Am^2 to 1e^-4 Am^2 - VectorOperations::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble, 3); + VectorOperations::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble, + 3); for (int i = 0; i < 3; i++) { dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]); } From c6664c5cbf684b904da21c5aa8be85c5f865dd5d Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 1 Jun 2023 16:04:51 +0200 Subject: [PATCH 232/506] fixed RW scale for angular momentum calculation --- mission/controller/acs/control/PtgCtrl.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 1ef88ccc..b6f998ec 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -151,6 +151,8 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); VectorOperations::subtract(speedRws, refSpeedRws, speedRws, 4); + // convert speed from 10 RPM to 1 RPM + VectorOperations::mulScalar(speedRws, 1e-1, speedRws, 4); // convert to rad/s VectorOperations::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4); // calculate angular momentum of each RW From 0fe7b256ecc9729ce1c2e9fb7e5b32615dd9e40d Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 1 Jun 2023 17:11:55 +0200 Subject: [PATCH 233/506] another RW speed scaling fix --- mission/controller/acs/control/PtgCtrl.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index b6f998ec..55a3b268 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -111,6 +111,7 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara // calculate resulting angular momentum double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0}; VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); + VectorOperations::mulScalar(diffRwSpeed, 1e-1, diffRwSpeed, 4); VectorOperations::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel, rwAngMomentum, 4); From 4aca7a91e35019415cfd03c704e8212b934da30c Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 09:33:40 +0200 Subject: [PATCH 234/506] fixed more abs --- mission/controller/acs/control/PtgCtrl.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 55a3b268..c978dafc 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -29,10 +29,10 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters double qErrorLaw[3] = {0, 0, 0}; for (int i = 0; i < 3; i++) { - if (abs(qError[i]) < qErrorMin) { + if (std::abs(qError[i]) < qErrorMin) { qErrorLaw[i] = qErrorMin; } else { - qErrorLaw[i] = abs(qError[i]); + qErrorLaw[i] = std::abs(qError[i]); } } double qErrorLawNorm = VectorOperations::norm(qErrorLaw, 3); @@ -70,7 +70,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters double pErrorSign[3] = {0, 0, 0}; for (int i = 0; i < 3; i++) { - if (abs(pError[i]) > 1) { + if (std::abs(pError[i]) > 1) { pErrorSign[i] = sign(pError[i]); } else { pErrorSign[i] = pError[i]; From f8d9925785e779180db11784600fdc44fbffd429 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 10:43:23 +0200 Subject: [PATCH 235/506] this shouldnt be needed --- mission/controller/acs/Guidance.cpp | 34 +++++++---------------------- 1 file changed, 8 insertions(+), 26 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 7c8fc2a1..329c6bdc 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -424,21 +424,12 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do // Calculate error angle errorAngle = QuaternionOperations::getAngle(errorQuat, true); - // Only give back error satellite rotational rate if orientation has already been aquired - if (errorAngle < 2. / 180. * M_PI) { - // First combine the target and reference satellite rotational rates - double combinedRefSatRotRate[3] = {0, 0, 0}; - VectorOperations::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); - // Then substract the combined required satellite rotational rates from the actual rate - VectorOperations::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, - 3); - } else { - // If orientation has not been aquired yet set satellite rotational rate to zero - errorSatRotRate = 0; - } - - // target flag in matlab, importance, does look like it only gives feedback if pointing control is - // under 150 arcsec ?? + // Calculate error satellite rotational rate + // First combine the target and reference satellite rotational rates + double combinedRefSatRotRate[3] = {0, 0, 0}; + VectorOperations::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); + // Then subtract the combined required satellite rotational rates from the actual rate + VectorOperations::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, 3); } void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], @@ -453,17 +444,8 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do // Calculate error angle errorAngle = QuaternionOperations::getAngle(errorQuat, true); - // Only give back error satellite rotational rate if orientation has already been aquired - if (errorAngle < 2. / 180. * M_PI) { - // Then substract the combined required satellite rotational rates from the actual rate - VectorOperations::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); - } else { - // If orientation has not been aquired yet set satellite rotational rate to zero - errorSatRotRate = 0; - } - - // target flag in matlab, importance, does look like it only gives feedback if pointing control is - // under 150 arcsec ?? + // Calculate error satellite rotational rate + VectorOperations::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); } void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], From bd15d7b0e2ec3da56ccda529ff09a82b14168bc1 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 13:28:59 +0200 Subject: [PATCH 236/506] fixed calculation of rotational rate for ptgCtrl which also is used for idle now --- mission/controller/acs/Guidance.cpp | 29 +++++++++++++++++------------ mission/controller/acs/Guidance.h | 7 ++++--- 2 files changed, 21 insertions(+), 15 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 329c6bdc..98dd56fa 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -266,7 +266,8 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4], + double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- @@ -296,9 +297,8 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double //---------------------------------------------------------------------------- // Calculation of reference rotation rate //---------------------------------------------------------------------------- - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; + int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; + targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], @@ -453,20 +453,25 @@ void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double qua //------------------------------------------------------------------------------------- // Calculation of target rotation rate //------------------------------------------------------------------------------------- - double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - - (timeSavedQuaternion.tv_sec + - timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); + double timeElapsed = now.tv_sec + now.tv_usec * 1e-6 - + (timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6); + if (VectorOperations::norm(savedQuaternion, 4) == 0) { + std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion)); + } if (timeElapsed < timeElapsedMax) { + double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0}; + QuaternionOperations::inverse(quatInertialTarget, q); + QuaternionOperations::inverse(savedQuaternion, qS); double qDiff[4] = {0, 0, 0, 0}; - VectorOperations::subtract(quatInertialTarget, savedQuaternion, qDiff, 4); + VectorOperations::subtract(q, qS, qDiff, 4); VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); - double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, - qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; + double tgtQuatVec[3] = {q[0], q[1], q[2]}; + double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; - VectorOperations::cross(quatInertialTarget, qDiff, sum1); + VectorOperations::cross(tgtQuatVec, qDiffVec, sum1); VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); - VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); + VectorOperations::mulScalar(qDiffVec, q[3], sum3, 3); VectorOperations::add(sum1, sum2, sum, 3); VectorOperations::subtract(sum, sum3, sum, 3); double omegaRefNew[3] = {0, 0, 0}; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 39e77456..7b81e411 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -15,7 +15,7 @@ class Guidance { void getTargetParamsSafe(double sunTargetSafe[3]); ReturnValue_t solarArrayDeploymentComplete(); - // Function to get the target quaternion and refence rotation rate from gps position and + // Function to get the target quaternion and reference rotation rate from gps position and // position of the ground station void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3], double refDirB[3], double quatBI[4], double targetQuat[4], @@ -25,9 +25,10 @@ class Guidance { void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], double targetSatRotRate[3]); - // Function to get the target quaternion and refence rotation rate for sun pointing after ground + // Function to get the target quaternion and reference rotation rate for sun pointing after ground // station - void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]); + void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4], + double targetSatRotRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing From 487b6bf69083a08a5ed23bce273939e14e822a1c Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 13:29:25 +0200 Subject: [PATCH 237/506] frmt --- mission/controller/acs/control/PtgCtrl.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index c978dafc..cfbf85ea 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -35,6 +35,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters qErrorLaw[i] = std::abs(qError[i]); } } + double qErrorLawNorm = VectorOperations::norm(qErrorLaw, 3); double gain1 = cInt * omMax / qErrorLawNorm; From c64ea7f7e659eff1d712141fcf2853fa4270321d Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 14:22:20 +0200 Subject: [PATCH 238/506] fixed nullspace rwSpd range --- mission/controller/acs/control/PtgCtrl.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index cfbf85ea..2f5847cc 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -102,6 +102,8 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara // concentrate RW speeds as vector and convert to double double speedRws[4] = {static_cast(speedRw0), static_cast(speedRw1), static_cast(speedRw2), static_cast(speedRw3)}; + VectorOperations::mulScalar(speedRws, 1e-1, speedRws, 4); + VectorOperations::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4); // calculate RPM offset utilizing the nullspace double rpmOffset[4] = {0, 0, 0, 0}; @@ -112,7 +114,6 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara // calculate resulting angular momentum double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0}; VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); - VectorOperations::mulScalar(diffRwSpeed, 1e-1, diffRwSpeed, 4); VectorOperations::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel, rwAngMomentum, 4); From 099eb488ae6e4398e30fdfdea9cb1acb60369dba Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 17:37:47 +0200 Subject: [PATCH 239/506] who needs comments anyways --- mission/controller/acs/Guidance.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 98dd56fa..1d82d317 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -518,10 +518,6 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double)); return returnvalue::OK; } else { - // @note: This one takes the normal pseudoInverse of all four raction wheels valid. - // Does not make sense, but is implemented that way in MATLAB ?! - // Thought: It does not really play a role, because in case there are more then one - // reaction wheel invalid the pointing control is destined to fail. return returnvalue::FAILED; } } From a58f51ee91bfda9c46f54f12c57e5f10551d5634 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 17:41:25 +0200 Subject: [PATCH 240/506] small fixes --- mission/controller/AcsController.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 557a6105..8b3eee64 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -368,9 +368,11 @@ void AcsController::performPointingCtrl() { // Variables required for setting actuators double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0}, mgtDpDes[3] = {0, 0, 0}; + switch (mode) { case acs::PTG_IDLE: - guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); + guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat, + targetSatRotRate); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, @@ -460,7 +462,7 @@ void AcsController::performPointingCtrl() { case acs::PTG_INERTIAL: std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, - 4 * sizeof(double)); + sizeof(targetQuat)); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef, acsParameters.inertialModeControllerParameters.refRotRate, errorQuat, From 6915f0e003df40cda0302ee1f58b59842a4fd83f Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 17:45:08 +0200 Subject: [PATCH 241/506] changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7890df35..88423f07 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -60,6 +60,7 @@ will consitute of a breaking change warranting a new major release: - ACU dummy HK sets - IMTQ HK sets - IMTQ dummy now handles power switch +- Added some new ACS parameters ## Fixed @@ -91,6 +92,8 @@ will consitute of a breaking change warranting a new major release: - Prevent spam of TCS controller heater unavailability event if all heaters are in external control. - TCS heater switch info set contained invalid values because of a faulty `memcpy` in the TCS controller. There is not crash risk but the heater states were invalid. +- Various fixes for the pointing modes of the `ACS Controller`. All modes should work now as + intended. # [v2.0.5] 2023-05-11 From 5a60558354c6d24249836189705ad1a455acbda9 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Jun 2023 09:24:52 +0200 Subject: [PATCH 242/506] set relevant datasets to invalid on shutdown --- mission/acs/str/StarTrackerHandler.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 0942164a..037cc871 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -100,6 +100,19 @@ void StarTrackerHandler::doShutDown() { startupState = StartupState::IDLE; bootState = FwBootState::NONE; solutionSet.setReportingEnabled(false); + { + PoolReadGuard& pg(solutionSet); + solutionSet.caliQw.value = 0.0; + solutionSet.caliQx.value = 0.0; + solutionSet.caliQy.value = 0.0; + solutionSet.caliQz.value = 0.0; + solutionSet.isTrustWorthy = 0; + solutionSet.setValidity(false, true); + } + { + PoolReadGuard& pg(temperatureSet); + temperatureSet.setValidity(false, true); + } reinitNextSetParam = false; setMode(_MODE_POWER_DOWN); } From 73981006a2a3e0580a8b13ab1b709ab0c9d26e39 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Jun 2023 09:25:31 +0200 Subject: [PATCH 243/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7890df35..455b7373 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -91,6 +91,7 @@ will consitute of a breaking change warranting a new major release: - Prevent spam of TCS controller heater unavailability event if all heaters are in external control. - TCS heater switch info set contained invalid values because of a faulty `memcpy` in the TCS controller. There is not crash risk but the heater states were invalid. +- STR datasets were not set to invalid on shutdown. # [v2.0.5] 2023-05-11 From a1fec93b25edbb23b7891af5d1677c32e0d0e506 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Jun 2023 16:02:01 +0200 Subject: [PATCH 244/506] fix --- mission/acs/str/StarTrackerHandler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 037cc871..cf2a5919 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -101,7 +101,7 @@ void StarTrackerHandler::doShutDown() { bootState = FwBootState::NONE; solutionSet.setReportingEnabled(false); { - PoolReadGuard& pg(solutionSet); + PoolReadGuard pg(&solutionSet); solutionSet.caliQw.value = 0.0; solutionSet.caliQx.value = 0.0; solutionSet.caliQy.value = 0.0; @@ -110,7 +110,7 @@ void StarTrackerHandler::doShutDown() { solutionSet.setValidity(false, true); } { - PoolReadGuard& pg(temperatureSet); + PoolReadGuard pg(&temperatureSet); temperatureSet.setValidity(false, true); } reinitNextSetParam = false; From 939eeb09ec2a14f6d84d0e42fa186d4e7fd79e5a Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Jun 2023 09:37:01 +0200 Subject: [PATCH 245/506] fixed str validity check --- mission/controller/acs/Navigation.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 9e8b3719..7c822409 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -19,9 +19,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, AcsParameters *acsParameters) { double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; - bool quatIBValid = sensorValues->strSet.caliQx.isValid() && - sensorValues->strSet.caliQy.isValid() && - sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid(); + bool quatIBValid = sensorValues->strSet.isTrustWorthy.value; if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) { mekfStatus = multiplicativeKalmanFilter.init( From a5f9bb31774e285e1bf1bb4066c202d7eea4c052 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Jun 2023 09:43:31 +0200 Subject: [PATCH 246/506] reworked multiple rw failure handling --- mission/controller/AcsController.cpp | 3 ++- mission/controller/acs/AcsParameters.cpp | 3 +++ mission/controller/acs/AcsParameters.h | 2 ++ 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 0b49de04..2faa0f64 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -349,7 +349,8 @@ void AcsController::performPointingCtrl() { double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); if (result == returnvalue::FAILED) { - if (multipleRwUnavailableCounter == 5) { + if (multipleRwUnavailableCounter >= + acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { triggerEvent(acs::MULTIPLE_RW_INVALID); } multipleRwUnavailableCounter++; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 792a72ec..d516aebd 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -290,6 +290,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x6: parameterWrapper->set(rwHandlingParameters.rampTime); break; + case 0x7: + parameterWrapper->set(rwHandlingParameters.multipleRwInvalidTimeout); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 9600ca7e..7034c9ae 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -798,6 +798,8 @@ class AcsParameters : public HasParametersIF { double stictionTorque = 0.0006; uint16_t rampTime = 10; + + uint32_t multipleRwInvalidTimeout = 25; } rwHandlingParameters; struct RwMatrices { From 58dee642a6a3296a7fdb08ef3e5468bbccd231da Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Jun 2023 16:06:36 +0200 Subject: [PATCH 247/506] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7890df35..759046bd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -51,6 +51,8 @@ will consitute of a breaking change warranting a new major release: reason. - OFF mode is ignores in TM store for determining whether a store will be written. The modes will only be used to cancel a transfer. +- Handling of multiple RWs in the ACS Controller is improved and can be changed by parameter + commands. ## Added @@ -91,6 +93,8 @@ will consitute of a breaking change warranting a new major release: - Prevent spam of TCS controller heater unavailability event if all heaters are in external control. - TCS heater switch info set contained invalid values because of a faulty `memcpy` in the TCS controller. There is not crash risk but the heater states were invalid. +- Fixed usage of quaternion valid flag, which does not actually represent the validity of the + quaternion. # [v2.0.5] 2023-05-11 From d4b44962f5fce9e1897f4510bc225ac4ead4e7b9 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Jun 2023 16:08:53 +0200 Subject: [PATCH 248/506] prevent 0.4s spam --- mission/controller/AcsController.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 2faa0f64..15e9a840 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -352,6 +352,7 @@ void AcsController::performPointingCtrl() { if (multipleRwUnavailableCounter >= acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { triggerEvent(acs::MULTIPLE_RW_INVALID); + multipleRwUnavailableCounter = 0; } multipleRwUnavailableCounter++; return; From aec383b97434346a954e3834c851dffea1ea143c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 6 Jun 2023 11:11:16 +0200 Subject: [PATCH 249/506] add force flag for cp helper --- bsp_q7s/core/CoreController.cpp | 3 +++ mission/sysDefs.h | 5 ++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 5c4c4d62..2aaef9a5 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -243,6 +243,9 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ return result; } std::ostringstream oss("cp ", std::ostringstream::ate); + if (parser.isForceOptSet()) { + oss << "-f "; + } if (parser.isRecursiveOptSet()) { oss << "-r "; } diff --git a/mission/sysDefs.h b/mission/sysDefs.h index c84c237f..f1de95a3 100644 --- a/mission/sysDefs.h +++ b/mission/sysDefs.h @@ -242,19 +242,22 @@ class CpHelperParser { CpHelperParser(const uint8_t* data, size_t maxLen) : data(data), maxLen(maxLen) {} ReturnValue_t parse() { - if (maxLen < 1) { + if (maxLen < 2) { return SerializeIF::STREAM_TOO_SHORT; } recursiveOpt = data[0]; + forceOpt = data[1]; return parseDestTargetString(data + 1, maxLen - 1, destTgt); } const SourceTargetPair& destTgtPair() const { return destTgt; } bool isRecursiveOptSet() const { return recursiveOpt; } + bool isForceOptSet() const { return forceOpt; } private: const uint8_t* data; size_t maxLen; bool recursiveOpt = false; + bool forceOpt = false; SourceTargetPair destTgt; }; From 8052734028efc0d98b7bbd438a97bea3b63e3aa4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 6 Jun 2023 11:12:41 +0200 Subject: [PATCH 250/506] tiny tweak --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 30376485..d224e629 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,7 +16,7 @@ will consitute of a breaking change warranting a new major release: # [unreleased] -# [v2.2.0] to be released +# [v4.0.0] to be released # [v3.0.0] to be released From 5058b8905d555004f655816df1910fd50e450cda Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 6 Jun 2023 14:05:44 +0200 Subject: [PATCH 251/506] let's not forget this --- mission/sysDefs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/sysDefs.h b/mission/sysDefs.h index f1de95a3..9f402226 100644 --- a/mission/sysDefs.h +++ b/mission/sysDefs.h @@ -247,7 +247,7 @@ class CpHelperParser { } recursiveOpt = data[0]; forceOpt = data[1]; - return parseDestTargetString(data + 1, maxLen - 1, destTgt); + return parseDestTargetString(data + 2, maxLen - 2, destTgt); } const SourceTargetPair& destTgtPair() const { return destTgt; } bool isRecursiveOptSet() const { return recursiveOpt; } From 6a2d5b81612defbb79089977e6f4da25487575aa Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 6 Jun 2023 16:02:11 +0200 Subject: [PATCH 252/506] well this doesnt work --- linux/acs/AcsBoardPolling.cpp | 89 ++++++++++++++++++++++++++++- linux/acs/AcsBoardPolling.h | 3 + mission/acs/GyrAdis1650XHandler.cpp | 12 ++-- mission/acs/GyrAdis1650XHandler.h | 2 +- mission/acs/acsBoardPolling.h | 11 ++-- mission/acs/gyroAdisHelpers.cpp | 11 ++++ mission/acs/gyroAdisHelpers.h | 2 + 7 files changed, 118 insertions(+), 12 deletions(-) diff --git a/linux/acs/AcsBoardPolling.cpp b/linux/acs/AcsBoardPolling.cpp index e525bf53..38bf4351 100644 --- a/linux/acs/AcsBoardPolling.cpp +++ b/linux/acs/AcsBoardPolling.cpp @@ -113,6 +113,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send if (req->mode != adis.mode) { if (req->mode == acs::SimpleSensorMode::NORMAL) { adis.type = req->type; + adis.decRate = req->cfg.decRateReg; // The initial countdown is handled by the device handler now. // adis.countdown.setTimeout(adis1650x::START_UP_TIME); if (adis.type == adis1650x::Type::ADIS16507) { @@ -376,6 +377,81 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) { } } +ReturnValue_t AcsBoardPolling::writeAdisReg(SpiCookie& cookie) { + ReturnValue_t result = returnvalue::OK; + int retval = 0; + // Prepare transfer + int fileDescriptor = 0; + std::string device = spiComIF.getSpiDev(); + UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); + if (fileHelper.getOpenResult() != returnvalue::OK) { + return spi::OPENING_FILE_FAILED; + } + spi::SpiModes spiMode = spi::SpiModes::MODE_0; + uint32_t spiSpeed = 0; + cookie.getSpiParameters(spiMode, spiSpeed, nullptr); + spiComIF.setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); + cookie.assignWriteBuffer(cmdBuf.data()); + cookie.setTransferSize(2); + + gpioId_t gpioId = cookie.getChipSelectPin(); + MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; + uint32_t timeoutMs = 0; + MutexIF* mutex = spiComIF.getCsMutex(); + cookie.getMutexParams(timeoutType, timeoutMs); + if (mutex == nullptr) { + sif::warning << "GyroADIS16507Handler::spiSendCallback: " + "Mutex or GPIO interface invalid" + << std::endl; + return returnvalue::FAILED; + } + + size_t idx = 0; + spi_ioc_transfer* transferStruct = cookie.getTransferStructHandle(); + uint64_t origTx = transferStruct->tx_buf; + uint64_t origRx = transferStruct->rx_buf; + for (idx = 0; idx < 2; idx++) { + result = mutex->lockMutex(timeoutType, timeoutMs); + if (result != returnvalue::OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "AcsBoardPolling: Failed to lock mutex" << std::endl; +#endif + return result; + } + // Pull SPI CS low. For now, no support for active high given + if (gpioId != gpio::NO_GPIO) { + gpioIF.pullLow(gpioId); + } + + // Execute transfer + // Initiate a full duplex SPI transfer. + retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie.getTransferStructHandle()); + if (retval < 0) { + utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); + result = spi::FULL_DUPLEX_TRANSFER_FAILED; + } +#if FSFW_HAL_SPI_WIRETAPPING == 1 + comIf->performSpiWiretapping(cookie); +#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ + + if (gpioId != gpio::NO_GPIO) { + gpioIF.pullHigh(gpioId); + } + mutex->unlockMutex(); + + idx += 2; + transferStruct->tx_buf += 2; + transferStruct->rx_buf += 2; + if (idx < 4) { + usleep(adis1650x::STALL_TIME_MICROSECONDS); + } + } + transferStruct->tx_buf = origTx; + transferStruct->rx_buf = origRx; + cookie.setTransferSize(0); + return returnvalue::OK; +} + ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen) { ReturnValue_t result = returnvalue::OK; int retval = 0; @@ -455,15 +531,20 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { ReturnValue_t result; acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; bool mustPerformStartup = false; + uint16_t decRate = 0; { MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); mode = gyro.mode; + decRate = gyro.decRate; mustPerformStartup = gyro.performStartup; } if (mode == acs::SimpleSensorMode::OFF) { return; } if (mustPerformStartup) { + adis1650x::prepareWriteRegCommand(adis1650x::DEC_RATE_REG, decRate, cmdBuf.data(), + cmdBuf.size()); + writeAdisReg(*gyro.cookie); uint8_t regList[6]; // Read configuration regList[0] = adis1650x::DIAG_STAT_REG; @@ -491,13 +572,19 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { gyro.replyResult = returnvalue::FAILED; return; } + uint16_t decRateReadBack = (rawReply[10] << 8) | rawReply[11]; + if (decRateReadBack != decRate) { + sif::warning << "AcsPollingTask: DEC rate configuration failed. Read " << decRateReadBack + << ", expected " << decRate << std::endl; + gyro.replyResult = returnvalue::FAILED; + } MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); gyro.ownReply.cfgWasSet = true; gyro.ownReply.cfg.diagStat = (rawReply[2] << 8) | rawReply[3]; gyro.ownReply.cfg.filterSetting = (rawReply[4] << 8) | rawReply[5]; gyro.ownReply.cfg.rangMdl = (rawReply[6] << 8) | rawReply[7]; gyro.ownReply.cfg.mscCtrlReg = (rawReply[8] << 8) | rawReply[9]; - gyro.ownReply.cfg.decRateReg = (rawReply[10] << 8) | rawReply[11]; + gyro.ownReply.cfg.decRateReg = decRateReadBack; gyro.ownReply.cfg.prodId = prodId; gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl); gyro.performStartup = false; diff --git a/linux/acs/AcsBoardPolling.h b/linux/acs/AcsBoardPolling.h index 794c9c47..5b4d0390 100644 --- a/linux/acs/AcsBoardPolling.h +++ b/linux/acs/AcsBoardPolling.h @@ -37,6 +37,7 @@ class AcsBoardPolling : public SystemObject, struct GyroAdis : public DevBase { adis1650x::Type type; + uint16_t decRate; Countdown countdown; acs::Adis1650XReply ownReply; acs::Adis1650XReply readerReply; @@ -84,6 +85,8 @@ class AcsBoardPolling : public SystemObject, void gyroAdisHandler(GyroAdis& gyro); void mgmLis3Handler(MgmLis3& mgm); void mgmRm3100Handler(MgmRm3100& mgm); + // This fumction configures the register as specified on p.21 of the datasheet. + ReturnValue_t writeAdisReg(SpiCookie& cookie); // Special readout: 16us stall time between small 2 byte transfers. ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen); }; diff --git a/mission/acs/GyrAdis1650XHandler.cpp b/mission/acs/GyrAdis1650XHandler.cpp index c61187ce..27e8dc75 100644 --- a/mission/acs/GyrAdis1650XHandler.cpp +++ b/mission/acs/GyrAdis1650XHandler.cpp @@ -26,11 +26,11 @@ void GyrAdis1650XHandler::doStartUp() { breakCountdown.setTimeout(adis1650x::START_UP_TIME); commandExecuted = true; } - if (breakCountdown.hasTimedOut()) { - updatePeriodicReply(true, adis1650x::REPLY); - setMode(MODE_ON); - internalState = InternalState::NONE; - } + } + if (internalState == InternalState::STARTUP_DONE) { + updatePeriodicReply(true, adis1650x::REPLY); + setMode(MODE_ON); + internalState = InternalState::NONE; } } @@ -61,6 +61,8 @@ ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_ return NOTHING_TO_SEND; } *id = adis1650x::REQUEST; + adisRequest.cfg.decRateReg = adis1650x::DEC_RATE; + internalState = InternalState::STARTUP_DONE; return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL); } case (InternalState::SHUTDOWN): { diff --git a/mission/acs/GyrAdis1650XHandler.h b/mission/acs/GyrAdis1650XHandler.h index 5d906f61..308d472b 100644 --- a/mission/acs/GyrAdis1650XHandler.h +++ b/mission/acs/GyrAdis1650XHandler.h @@ -48,7 +48,7 @@ class GyrAdis1650XHandler : public DeviceHandlerBase { bool warningSwitch = true; - enum class InternalState { NONE, STARTUP, SHUTDOWN }; + enum class InternalState { NONE, STARTUP, STARTUP_DONE, SHUTDOWN }; InternalState internalState = InternalState::STARTUP; bool commandExecuted = false; diff --git a/mission/acs/acsBoardPolling.h b/mission/acs/acsBoardPolling.h index 90f0f19c..9f794fb0 100644 --- a/mission/acs/acsBoardPolling.h +++ b/mission/acs/acsBoardPolling.h @@ -8,11 +8,6 @@ namespace acs { -struct Adis1650XRequest { - SimpleSensorMode mode; - adis1650x::Type type; -}; - struct Adis1650XConfig { uint16_t diagStat; uint16_t filterSetting; @@ -22,6 +17,12 @@ struct Adis1650XConfig { uint16_t prodId; }; +struct Adis1650XRequest { + SimpleSensorMode mode; + adis1650x::Type type; + Adis1650XConfig cfg; +}; + struct Adis1650XData { double sensitivity = 0.0; // Angular velocities in all axes (X, Y and Z) diff --git a/mission/acs/gyroAdisHelpers.cpp b/mission/acs/gyroAdisHelpers.cpp index 0f41b058..5dd3a217 100644 --- a/mission/acs/gyroAdisHelpers.cpp +++ b/mission/acs/gyroAdisHelpers.cpp @@ -52,3 +52,14 @@ double adis1650x::rangMdlToSensitivity(uint16_t rangMdl) { } } } + +void adis1650x::prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* outBuf, + size_t maxLen) { + if (maxLen < 4) { + return; + } + outBuf[0] = regStart | adis1650x::WRITE_MASK; + outBuf[1] = val & 0Xff; + outBuf[2] = (regStart + 1) | adis1650x::WRITE_MASK; + outBuf[3] = (val >> 8) & 0xff; +} diff --git a/mission/acs/gyroAdisHelpers.h b/mission/acs/gyroAdisHelpers.h index c8a28369..00fda595 100644 --- a/mission/acs/gyroAdisHelpers.h +++ b/mission/acs/gyroAdisHelpers.h @@ -16,6 +16,8 @@ enum class BurstModes { }; size_t prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, size_t maxLen); +void prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* outBuf, size_t maxLen); + BurstModes burstModeFromMscCtrl(uint16_t mscCtrl); double rangMdlToSensitivity(uint16_t rangMdl); From 72b5567f73ee1370503fbd6796edc2e2d89d25b3 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 7 Jun 2023 09:17:49 +0200 Subject: [PATCH 253/506] not that it matters --- mission/acs/gyroAdisHelpers.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/acs/gyroAdisHelpers.cpp b/mission/acs/gyroAdisHelpers.cpp index 5dd3a217..5c1446cc 100644 --- a/mission/acs/gyroAdisHelpers.cpp +++ b/mission/acs/gyroAdisHelpers.cpp @@ -59,7 +59,7 @@ void adis1650x::prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* return; } outBuf[0] = regStart | adis1650x::WRITE_MASK; - outBuf[1] = val & 0Xff; + outBuf[1] = val & 0xff; outBuf[2] = (regStart + 1) | adis1650x::WRITE_MASK; outBuf[3] = (val >> 8) & 0xff; } From 7dbe69ef49be7d86e50880a1cc4fe2074c9e72e0 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 7 Jun 2023 09:20:29 +0200 Subject: [PATCH 254/506] now he reads the comments ... what happened to him --- mission/controller/acs/ActuatorCmd.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 9013d6f2..0fb8bdf5 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -25,7 +25,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const double sampleTime, const double inertiaWheel, const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed) { - // concentrate RW speed values (in 0.1 [RPM]) in vector + // group RW speed values (in 0.1 [RPM]) in vector int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3}; // calculate required RW speed as sum of current RW speed and RW speed delta From f4c9a4bda22edd0df6fc60f6a2bab612499853f3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 7 Jun 2023 11:38:02 +0200 Subject: [PATCH 255/506] fix for indexing --- linux/acs/AcsBoardPolling.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/acs/AcsBoardPolling.cpp b/linux/acs/AcsBoardPolling.cpp index 38bf4351..811c6a72 100644 --- a/linux/acs/AcsBoardPolling.cpp +++ b/linux/acs/AcsBoardPolling.cpp @@ -410,7 +410,7 @@ ReturnValue_t AcsBoardPolling::writeAdisReg(SpiCookie& cookie) { spi_ioc_transfer* transferStruct = cookie.getTransferStructHandle(); uint64_t origTx = transferStruct->tx_buf; uint64_t origRx = transferStruct->rx_buf; - for (idx = 0; idx < 2; idx++) { + for (idx = 0; idx < 4; idx++) { result = mutex->lockMutex(timeoutType, timeoutMs); if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 From a110bf32aa25d36284b28550e1c3e2548c8aaa5b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 7 Jun 2023 11:46:23 +0200 Subject: [PATCH 256/506] another small fix --- linux/acs/AcsBoardPolling.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/linux/acs/AcsBoardPolling.cpp b/linux/acs/AcsBoardPolling.cpp index 811c6a72..1ba55357 100644 --- a/linux/acs/AcsBoardPolling.cpp +++ b/linux/acs/AcsBoardPolling.cpp @@ -410,7 +410,7 @@ ReturnValue_t AcsBoardPolling::writeAdisReg(SpiCookie& cookie) { spi_ioc_transfer* transferStruct = cookie.getTransferStructHandle(); uint64_t origTx = transferStruct->tx_buf; uint64_t origRx = transferStruct->rx_buf; - for (idx = 0; idx < 4; idx++) { + for (idx = 0; idx < 4; idx += 2) { result = mutex->lockMutex(timeoutType, timeoutMs); if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -439,7 +439,6 @@ ReturnValue_t AcsBoardPolling::writeAdisReg(SpiCookie& cookie) { } mutex->unlockMutex(); - idx += 2; transferStruct->tx_buf += 2; transferStruct->rx_buf += 2; if (idx < 4) { From 9aec8960b980502b5d9a1c75eaf9a6c1f66f2188 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 7 Jun 2023 13:43:23 +0200 Subject: [PATCH 257/506] hi --- fsfw | 2 +- mission/genericFactory.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/fsfw b/fsfw index 5eb9ee8b..9903371a 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 5eb9ee8bc1e6f8640cbea46febd11e4b92241881 +Subproject commit 9903371ae9040a8171ead9cda9c8c73177d03d71 diff --git a/mission/genericFactory.cpp b/mission/genericFactory.cpp index 2dd00ee6..4ec4adec 100644 --- a/mission/genericFactory.cpp +++ b/mission/genericFactory.cpp @@ -243,7 +243,7 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5), 80, 160); new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, config::EIVE_PUS_APID, - pus::PUS_SERVICE_8, 16, 60); + pus::PUS_SERVICE_8, 16, 60, 50); new Service9TimeManagement( PsbParams(objects::PUS_SERVICE_9_TIME_MGMT, config::EIVE_PUS_APID, pus::PUS_SERVICE_9)); From 5ca96b2dd359bdd967fd2c82c23e75028b135507 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 7 Jun 2023 13:49:12 +0200 Subject: [PATCH 258/506] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 9903371a..b442ca09 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 9903371ae9040a8171ead9cda9c8c73177d03d71 +Subproject commit b442ca09b91d0cfdc6acd5a30349a25c4051972c From b8064c4a39971b63b445d5b15ea106d71e54146d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 7 Jun 2023 13:50:11 +0200 Subject: [PATCH 259/506] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 4391823f..9372b2a5 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 4391823f01d792bcc078d47b60f7df7624f8cbe4 +Subproject commit 9372b2a575aaa3a2c9e93d6e745a4e3fd08d0004 From 4bcfb8f5a2c9a14f8fcc8fd5f903eead54e8d0b6 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 7 Jun 2023 14:13:32 +0200 Subject: [PATCH 260/506] still doesnt work --- linux/acs/AcsBoardPolling.cpp | 3 +++ mission/acs/GyrAdis1650XHandler.cpp | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/linux/acs/AcsBoardPolling.cpp b/linux/acs/AcsBoardPolling.cpp index 1ba55357..727e8405 100644 --- a/linux/acs/AcsBoardPolling.cpp +++ b/linux/acs/AcsBoardPolling.cpp @@ -572,6 +572,8 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { return; } uint16_t decRateReadBack = (rawReply[10] << 8) | rawReply[11]; + sif::debug << "AcsPollingTask: DEC rate configuration. Read " << decRateReadBack + << ", expected " << decRate << std::endl; if (decRateReadBack != decRate) { sif::warning << "AcsPollingTask: DEC rate configuration failed. Read " << decRateReadBack << ", expected " << decRate << std::endl; @@ -589,6 +591,7 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { gyro.performStartup = false; gyro.replyResult = returnvalue::OK; } + sif::debug << "hello world 2" << std::endl; // Read regular registers std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(), adis1650x::BURST_READ_ENABLE.size()); diff --git a/mission/acs/GyrAdis1650XHandler.cpp b/mission/acs/GyrAdis1650XHandler.cpp index 27e8dc75..6f447178 100644 --- a/mission/acs/GyrAdis1650XHandler.cpp +++ b/mission/acs/GyrAdis1650XHandler.cpp @@ -26,9 +26,9 @@ void GyrAdis1650XHandler::doStartUp() { breakCountdown.setTimeout(adis1650x::START_UP_TIME); commandExecuted = true; } + updatePeriodicReply(true, adis1650x::REPLY); } if (internalState == InternalState::STARTUP_DONE) { - updatePeriodicReply(true, adis1650x::REPLY); setMode(MODE_ON); internalState = InternalState::NONE; } From 06b381d965eae30e0a96203bcfe807d96a0d7c1d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 7 Jun 2023 14:20:58 +0200 Subject: [PATCH 261/506] dir listing dump state machine --- bsp_q7s/core/CoreController.cpp | 92 ++++++++++++++++++++++------ bsp_q7s/core/CoreController.h | 14 +++++ mission/controller/acs/ActuatorCmd.h | 1 - 3 files changed, 87 insertions(+), 20 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 5c4c4d62..9b6a22b0 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -112,6 +112,9 @@ void CoreController::performControlOperation() { sdStateMachine(); performMountedSdCardOperations(); readHkData(); + if (dumpContext.active) { + dirListingDumpHandler(); + } if (shellCmdIsExecuting) { bool replyReceived = false; @@ -1038,7 +1041,6 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI return returnvalue::FAILED; } } - std::array dirListingBuf{}; dirListingBuf[8] = parser.compressionOptionSet(); // First four bytes reserved for segment index. One byte for compression option information std::strcpy(reinterpret_cast(dirListingBuf.data() + 2 * sizeof(uint32_t) + 1), repoName); @@ -1047,38 +1049,46 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI return returnvalue::FAILED; } std::error_code e; - size_t totalFileSize = std::filesystem::file_size(LIST_DIR_DUMP_WORK_FILE, e); - uint32_t segmentIdx = 0; - size_t dumpedBytes = 0; + dumpContext.totalFileSize = std::filesystem::file_size(LIST_DIR_DUMP_WORK_FILE, e); + dumpContext.segmentIdx = 0; + dumpContext.dumpedBytes = 0; size_t nextDumpLen = 0; size_t dummy = 0; - size_t maxDumpLen = dirListingBuf.size() - 2 * sizeof(uint32_t) - 1 - repoNameLen - 1; - size_t listingDataOffset = 2 * sizeof(uint32_t) + 1 + repoNameLen + 1; - uint32_t chunks = totalFileSize / maxDumpLen; - if (totalFileSize % maxDumpLen != 0) { + dumpContext.maxDumpLen = dirListingBuf.size() - 2 * sizeof(uint32_t) - 1 - repoNameLen - 1; + dumpContext.listingDataOffset = 2 * sizeof(uint32_t) + 1 + repoNameLen + 1; + uint32_t chunks = dumpContext.totalFileSize / dumpContext.maxDumpLen; + if (dumpContext.totalFileSize % dumpContext.maxDumpLen != 0) { chunks++; } SerializeAdapter::serialize(&chunks, dirListingBuf.data() + sizeof(uint32_t), &dummy, dirListingBuf.size() - sizeof(uint32_t), SerializeIF::Endianness::NETWORK); - while (dumpedBytes < totalFileSize) { - ifile.seekg(dumpedBytes, std::ios::beg); - nextDumpLen = maxDumpLen; - if (totalFileSize - dumpedBytes < maxDumpLen) { - nextDumpLen = totalFileSize - dumpedBytes; + while (dumpContext.dumpedBytes < dumpContext.totalFileSize) { + ifile.seekg(dumpContext.dumpedBytes, std::ios::beg); + nextDumpLen = dumpContext.maxDumpLen; + if (dumpContext.totalFileSize - dumpContext.dumpedBytes < dumpContext.maxDumpLen) { + nextDumpLen = dumpContext.totalFileSize - dumpContext.dumpedBytes; } - SerializeAdapter::serialize(&segmentIdx, dirListingBuf.data(), &dummy, dirListingBuf.size(), - SerializeIF::Endianness::NETWORK); - ifile.read(reinterpret_cast(dirListingBuf.data() + listingDataOffset), nextDumpLen); + SerializeAdapter::serialize(&dumpContext.segmentIdx, dirListingBuf.data(), &dummy, + dirListingBuf.size(), SerializeIF::Endianness::NETWORK); + ifile.read(reinterpret_cast(dirListingBuf.data() + dumpContext.listingDataOffset), + nextDumpLen); result = actionHelper.reportData(commandedBy, actionId, dirListingBuf.data(), - listingDataOffset + nextDumpLen); + dumpContext.listingDataOffset + nextDumpLen); if (result != returnvalue::OK) { // Remove work file when we are done std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); return result; } - segmentIdx++; - dumpedBytes += nextDumpLen; + dumpContext.segmentIdx++; + dumpContext.dumpedBytes += nextDumpLen; + // Dump takes multiple task cycles, so cache the dump state and continue dump the next cycles. + if (dumpContext.segmentIdx == 25) { + dumpContext.active = true; + dumpContext.commander = commandedBy; + dumpContext.actionId = actionId; + return returnvalue::OK; + } } // Remove work file when we are done std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); @@ -2346,6 +2356,50 @@ MessageQueueId_t CoreController::getCommandQueue() const { return ExtendedControllerBase::getCommandQueue(); } +void CoreController::dirListingDumpHandler() { + size_t nextDumpLen = 0; + size_t dummy = 0; + ReturnValue_t result; + std::error_code e; + std::ifstream ifile(LIST_DIR_DUMP_WORK_FILE, std::ios::binary); + if (ifile.bad()) { + return; + } + while (dumpContext.dumpedBytes < dumpContext.totalFileSize) { + ifile.seekg(dumpContext.dumpedBytes, std::ios::beg); + nextDumpLen = dumpContext.maxDumpLen; + if (dumpContext.totalFileSize - dumpContext.dumpedBytes < dumpContext.maxDumpLen) { + nextDumpLen = dumpContext.totalFileSize - dumpContext.dumpedBytes; + } + SerializeAdapter::serialize(&dumpContext.segmentIdx, dirListingBuf.data(), &dummy, + dirListingBuf.size(), SerializeIF::Endianness::NETWORK); + ifile.read(reinterpret_cast(dirListingBuf.data() + dumpContext.listingDataOffset), + nextDumpLen); + result = + actionHelper.reportData(dumpContext.commander, dumpContext.actionId, dirListingBuf.data(), + dumpContext.listingDataOffset + nextDumpLen); + if (result != returnvalue::OK) { + // Remove work file when we are done + std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); + dumpContext.active = false; + actionHelper.finish(false, dumpContext.commander, dumpContext.actionId, result); + return; + } + dumpContext.segmentIdx++; + dumpContext.dumpedBytes += nextDumpLen; + // Dump takes multiple task cycles, so cache the dump state and continue dump the next cycles. + if (dumpContext.segmentIdx == 25) { + break; + } + } + if (dumpContext.dumpedBytes >= dumpContext.totalFileSize) { + actionHelper.finish(true, dumpContext.commander, dumpContext.actionId, result); + dumpContext.active = false; + // Remove work file when we are done + std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); + } +} + bool CoreController::isNumber(const std::string &s) { return !s.empty() && std::find_if(s.begin(), s.end(), [](unsigned char c) { return !std::isdigit(c); }) == s.end(); diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index d44907e8..a58faf86 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -177,6 +177,19 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe DeviceCommandId_t actionId; } sdCommandingInfo; + struct DirListingDumpContext { + bool active; + size_t dumpedBytes; + size_t totalFileSize; + size_t listingDataOffset; + size_t maxDumpLen; + uint32_t segmentIdx; + MessageQueueId_t commander = MessageQueueIF::NO_QUEUE; + DeviceCommandId_t actionId; + }; + std::array dirListingBuf{}; + DirListingDumpContext dumpContext{}; + RebootFile rebootFile = {}; CommandExecutor cmdExecutor; @@ -274,6 +287,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe void rewriteRebootFile(RebootFile file); void announceBootCounts(); void readHkData(); + void dirListingDumpHandler(); bool isNumber(const std::string& s); }; diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 6a1b3dbb..b14a4a25 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -3,7 +3,6 @@ #include - class ActuatorCmd { public: ActuatorCmd(); From 26ed774806767952cd3ba058312210a5841e634c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 7 Jun 2023 14:25:13 +0200 Subject: [PATCH 262/506] changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 17ac5d5d..d686359d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -53,6 +53,9 @@ will consitute of a breaking change warranting a new major release: only be used to cancel a transfer. - Handling of multiple RWs in the ACS Controller is improved and can be changed by parameter commands. +- The Directory Listing direct dumper now has a state machine to stagger the directory listing dump. + This is required because very large dumps will overload the queue capacities in the framework. +- The PUS Service 8 now has larger queue sizes to handle more action replies. ## Added From 06c5344d8a44b0cd73209c1fc649ca6713a5622a Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 7 Jun 2023 15:14:03 +0200 Subject: [PATCH 263/506] this might actually work --- linux/acs/AcsBoardPolling.cpp | 1 - mission/acs/GyrAdis1650XHandler.cpp | 9 ++++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/linux/acs/AcsBoardPolling.cpp b/linux/acs/AcsBoardPolling.cpp index 727e8405..b8539a8f 100644 --- a/linux/acs/AcsBoardPolling.cpp +++ b/linux/acs/AcsBoardPolling.cpp @@ -591,7 +591,6 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { gyro.performStartup = false; gyro.replyResult = returnvalue::OK; } - sif::debug << "hello world 2" << std::endl; // Read regular registers std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(), adis1650x::BURST_READ_ENABLE.size()); diff --git a/mission/acs/GyrAdis1650XHandler.cpp b/mission/acs/GyrAdis1650XHandler.cpp index 6f447178..fe10c214 100644 --- a/mission/acs/GyrAdis1650XHandler.cpp +++ b/mission/acs/GyrAdis1650XHandler.cpp @@ -15,7 +15,7 @@ GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t devic } void GyrAdis1650XHandler::doStartUp() { - if (internalState != InternalState::STARTUP) { + if (internalState == InternalState::NONE) { internalState = InternalState::STARTUP; commandExecuted = false; } @@ -24,12 +24,13 @@ void GyrAdis1650XHandler::doStartUp() { if (not commandExecuted) { warningSwitch = true; breakCountdown.setTimeout(adis1650x::START_UP_TIME); + updatePeriodicReply(true, adis1650x::REPLY); commandExecuted = true; } - updatePeriodicReply(true, adis1650x::REPLY); } if (internalState == InternalState::STARTUP_DONE) { setMode(MODE_ON); + commandExecuted = false; internalState = InternalState::NONE; } } @@ -62,7 +63,6 @@ ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_ } *id = adis1650x::REQUEST; adisRequest.cfg.decRateReg = adis1650x::DEC_RATE; - internalState = InternalState::STARTUP_DONE; return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL); } case (InternalState::SHUTDOWN): { @@ -93,6 +93,9 @@ ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t rem getMode() == _MODE_POWER_DOWN) { return IGNORE_FULL_PACKET; } + if (internalState == InternalState::STARTUP) { + internalState = InternalState::STARTUP_DONE; + } *foundLen = remainingSize; if (remainingSize != sizeof(acs::Adis1650XReply)) { return returnvalue::FAILED; From dcf3b1b1fee30d09beb40359af7536fdad8b60d8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 7 Jun 2023 15:55:26 +0200 Subject: [PATCH 264/506] added debug statement for testing --- bsp_q7s/core/CoreController.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 9b6a22b0..3c86915f 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -1080,6 +1080,7 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); return result; } + sif::debug << "dumped segment " << dumpContext.segmentIdx << std::endl; dumpContext.segmentIdx++; dumpContext.dumpedBytes += nextDumpLen; // Dump takes multiple task cycles, so cache the dump state and continue dump the next cycles. @@ -2385,6 +2386,7 @@ void CoreController::dirListingDumpHandler() { actionHelper.finish(false, dumpContext.commander, dumpContext.actionId, result); return; } + sif::debug << "dumped segment " << dumpContext.segmentIdx << std::endl; dumpContext.segmentIdx++; dumpContext.dumpedBytes += nextDumpLen; // Dump takes multiple task cycles, so cache the dump state and continue dump the next cycles. From deb154770d0d5b7cdbaa90cf4e71e82d9ebac242 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 7 Jun 2023 16:21:51 +0200 Subject: [PATCH 265/506] increased service 1 queue depth --- bsp_q7s/core/CoreController.cpp | 9 +++++++-- bsp_q7s/core/CoreController.h | 1 + common/config/eive/definitions.h | 1 + mission/genericFactory.cpp | 4 ++-- 4 files changed, 11 insertions(+), 4 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 3c86915f..3304fce8 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -1084,8 +1084,9 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI dumpContext.segmentIdx++; dumpContext.dumpedBytes += nextDumpLen; // Dump takes multiple task cycles, so cache the dump state and continue dump the next cycles. - if (dumpContext.segmentIdx == 25) { + if (dumpContext.segmentIdx == 10) { dumpContext.active = true; + dumpContext.firstDump = true; dumpContext.commander = commandedBy; dumpContext.actionId = actionId; return returnvalue::OK; @@ -2358,6 +2359,10 @@ MessageQueueId_t CoreController::getCommandQueue() const { } void CoreController::dirListingDumpHandler() { + if (dumpContext.firstDump) { + dumpContext.firstDump = false; + return; + } size_t nextDumpLen = 0; size_t dummy = 0; ReturnValue_t result; @@ -2390,7 +2395,7 @@ void CoreController::dirListingDumpHandler() { dumpContext.segmentIdx++; dumpContext.dumpedBytes += nextDumpLen; // Dump takes multiple task cycles, so cache the dump state and continue dump the next cycles. - if (dumpContext.segmentIdx == 25) { + if (dumpContext.segmentIdx == 10) { break; } } diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index a58faf86..05878d6d 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -179,6 +179,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe struct DirListingDumpContext { bool active; + bool firstDump; size_t dumpedBytes; size_t totalFileSize; size_t listingDataOffset; diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h index 78ba52e4..bd93586c 100644 --- a/common/config/eive/definitions.h +++ b/common/config/eive/definitions.h @@ -59,6 +59,7 @@ static constexpr uint32_t CFDP_STORE_QUEUE_SIZE = 300; static constexpr uint32_t MAX_PUS_FUNNEL_QUEUE_DEPTH = 100; static constexpr uint32_t MAX_CFDP_FUNNEL_QUEUE_DEPTH = 80; static constexpr uint32_t HK_SERVICE_QUEUE_DEPTH = 60; +static constexpr uint32_t ACTION_SERVICE_QUEUE_DEPTH = 60; static constexpr uint32_t MAX_STORED_CMDS_UDP = 150; static constexpr uint32_t MAX_STORED_CMDS_TCP = 180; diff --git a/mission/genericFactory.cpp b/mission/genericFactory.cpp index 4ec4adec..36c75f0e 100644 --- a/mission/genericFactory.cpp +++ b/mission/genericFactory.cpp @@ -234,7 +234,7 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun // PUS service stack new Service1TelecommandVerification(objects::PUS_SERVICE_1_VERIFICATION, config::EIVE_PUS_APID, - pus::PUS_SERVICE_1, objects::PUS_TM_FUNNEL, 40); + pus::PUS_SERVICE_1, objects::PUS_TM_FUNNEL, 120); new Service2DeviceAccess(objects::PUS_SERVICE_2_DEVICE_ACCESS, config::EIVE_PUS_APID, pus::PUS_SERVICE_2, 3, 10); new Service3Housekeeping(objects::PUS_SERVICE_3_HOUSEKEEPING, config::EIVE_PUS_APID, @@ -243,7 +243,7 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5), 80, 160); new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, config::EIVE_PUS_APID, - pus::PUS_SERVICE_8, 16, 60, 50); + pus::PUS_SERVICE_8, config::ACTION_SERVICE_QUEUE_DEPTH, 16, 60); new Service9TimeManagement( PsbParams(objects::PUS_SERVICE_9_TIME_MGMT, config::EIVE_PUS_APID, pus::PUS_SERVICE_9)); From 410fd1c4258b9b50441b2c3de50551fdbeff9663 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 8 Jun 2023 09:11:24 +0200 Subject: [PATCH 266/506] final setting of dec rate filter --- mission/acs/gyroAdisHelpers.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/acs/gyroAdisHelpers.h b/mission/acs/gyroAdisHelpers.h index 00fda595..f360db76 100644 --- a/mission/acs/gyroAdisHelpers.h +++ b/mission/acs/gyroAdisHelpers.h @@ -95,7 +95,7 @@ static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA; static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG; static constexpr uint16_t FILT_CTRL = 0x0000; -static constexpr uint16_t DEC_RATE = 0x0013; +static constexpr uint16_t DEC_RATE = 0x00C7; enum GlobCmds : uint8_t { FACTORY_CALIBRATION = 0b0000'0010, From cab20a8c77ffee4b827b66ba95b5a58f814d37f3 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 8 Jun 2023 09:55:46 +0200 Subject: [PATCH 267/506] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index a779c491..f0bb2574 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,8 @@ will consitute of a breaking change warranting a new major release: # [v3.0.0] to be released +- eive-fsfw: v3.0.0 + ## Changed - Adapt EM configuration to include all GomSpace PCDU devices except the ACU. For the ACU @@ -63,6 +65,8 @@ will consitute of a breaking change warranting a new major release: - IMTQ HK sets - IMTQ dummy now handles power switch - Added some new ACS parameters +- Enabled decimation filter for the ADIS GYRs +- Enabled second low-pass filter for L3GD20H GYRs ## Fixed From d90ccd62f88c225588e530e3b0e9d321bfa5da47 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 12:47:07 +0200 Subject: [PATCH 268/506] bump changelog and tmtc --- CHANGELOG.md | 2 ++ tmtc | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 17ac5d5d..6a7f07cd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,8 @@ will consitute of a breaking change warranting a new major release: # [v3.0.0] to be released +- `eive-tmtc` version v4.0.0 + ## Changed - Adapt EM configuration to include all GomSpace PCDU devices except the ACU. For the ACU diff --git a/tmtc b/tmtc index 6182369e..8804a4e8 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 6182369e4f40872c5c26e59be25d5fa79339176a +Subproject commit 8804a4e8e9fce1d45fcf62314affb791114d1517 From 6ef593d8f39a7712c5723b3423e4d46626825992 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 13:04:16 +0200 Subject: [PATCH 269/506] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 258f0d33..3a702295 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 258f0d331329d67e13eec9d7f4053fd269e3f9b6 +Subproject commit 3a70229510c017099ff2d43533e19d41f2030d81 From c1f8512b013eb4f5b0946cc49ec993dd5fcceb13 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 13:10:29 +0200 Subject: [PATCH 270/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1a2af1ca..937df112 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -55,6 +55,7 @@ will consitute of a breaking change warranting a new major release: only be used to cancel a transfer. - Handling of multiple RWs in the ACS Controller is improved and can be changed by parameter commands. +- Moved PDU `vcc` and `vbat` variable from auxiliary dataset to core dataset. ## Added From 18ba3a711a967ee8b74d9409a56a67fca8709af7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 13:11:50 +0200 Subject: [PATCH 271/506] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 3a702295..9a4bf510 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 3a70229510c017099ff2d43533e19d41f2030d81 +Subproject commit 9a4bf5100642301be10689d94ce969d517a6abc4 From 6c1dfafb2eeaa6eecb5b56380b9d5ac6f703fa57 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 13:13:05 +0200 Subject: [PATCH 272/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1a2af1ca..b77cb6de 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -70,6 +70,7 @@ will consitute of a breaking change warranting a new major release: ## Fixed +- CFDP low level protocol bugfix. Requires `fsfw` update and `tmtc` update. - Compile fix if SCEX is compiled for the EM. - Set up Rad Sensor chip select even for EM to avoid SPI bus issues. - Correct ADIS Gyroscope type configuration for the EM, where the 16507 type is used instead of the From bd7f28152a0f223c58687f30d49d455aff60fa88 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 13:42:16 +0200 Subject: [PATCH 273/506] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 9a4bf510..5322de05 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 9a4bf5100642301be10689d94ce969d517a6abc4 +Subproject commit 5322de059916efcf874b10ccc766b46e53d2470b From 1453b2abdc88ca8f9dfb7e3278554b14434cbf4d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 13:48:57 +0200 Subject: [PATCH 274/506] afmt --- mission/controller/acs/ActuatorCmd.h | 1 - 1 file changed, 1 deletion(-) diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 6a1b3dbb..b14a4a25 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -3,7 +3,6 @@ #include - class ActuatorCmd { public: ActuatorCmd(); From 019d1a7b0d01d4896a4d6236b806ccdbe28e5c1c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 13:51:10 +0200 Subject: [PATCH 275/506] that should do the job --- CMakeLists.txt | 7 +++++++ bsp_q7s/OBSWConfig.h.in | 4 ++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index eaa2d340..8256dc27 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -79,6 +79,13 @@ else() set(INIT_VAL 1) set(OBSW_STAR_TRACKER_GROUND_CONFIG 0) endif() + +set(OBSW_ADD_TMTC_TCP_SERVER + ${OBSW_Q7S_EM} + CACHE STRING "Add TCP TMTC Server") +set(OBSW_ADD_TMTC_UDP_SERVER + 0 + CACHE STRING "Add UDP TMTC Server") set(OBSW_ADD_MGT ${INIT_VAL} CACHE STRING "Add MGT module") diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index 1c0a4db8..2555d7cd 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -67,8 +67,8 @@ // because UDP packets are not allowed in the VPN // This will cause the OBSW to initialize the TMTC bridge responsible for exchanging data with the // CCSDS IP Cores. -#define OBSW_ADD_TMTC_TCP_SERVER 1 -#define OBSW_ADD_TMTC_UDP_SERVER 1 +#define OBSW_ADD_TMTC_TCP_SERVER @OBSW_ADD_TMTC_TCP_SERVER@ +#define OBSW_ADD_TMTC_UDP_SERVER @OBSW_ADD_TMTC_UDP_SERVER@ // Can be used to switch device to NORMAL mode immediately #define OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP 0 From fe1cc9444de3c86ffdd9332f5e9ccef49ce6d9a3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 14:10:06 +0200 Subject: [PATCH 276/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8321e0f6..e6dfd20f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -56,6 +56,8 @@ will consitute of a breaking change warranting a new major release: - Handling of multiple RWs in the ACS Controller is improved and can be changed by parameter commands. - Moved PDU `vcc` and `vbat` variable from auxiliary dataset to core dataset. +- Tweak TCP/IP configuration: Only the TCP server will be included on the EM. For the FM, no + TCP/IP servers will be included by default. ## Added From cd6b7d90be4ece727e21d8792ff6407f8dcec862 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 14:25:59 +0200 Subject: [PATCH 277/506] add one more cfg constant --- common/config/eive/definitions.h | 1 + mission/genericFactory.cpp | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h index bd93586c..2083962a 100644 --- a/common/config/eive/definitions.h +++ b/common/config/eive/definitions.h @@ -58,6 +58,7 @@ static constexpr uint32_t CFDP_STORE_QUEUE_SIZE = 300; static constexpr uint32_t MAX_PUS_FUNNEL_QUEUE_DEPTH = 100; static constexpr uint32_t MAX_CFDP_FUNNEL_QUEUE_DEPTH = 80; +static constexpr uint32_t VERIFICATION_SERVICE_QUEUE_DEPTH = 120; static constexpr uint32_t HK_SERVICE_QUEUE_DEPTH = 60; static constexpr uint32_t ACTION_SERVICE_QUEUE_DEPTH = 60; diff --git a/mission/genericFactory.cpp b/mission/genericFactory.cpp index 36c75f0e..466219b3 100644 --- a/mission/genericFactory.cpp +++ b/mission/genericFactory.cpp @@ -234,7 +234,8 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun // PUS service stack new Service1TelecommandVerification(objects::PUS_SERVICE_1_VERIFICATION, config::EIVE_PUS_APID, - pus::PUS_SERVICE_1, objects::PUS_TM_FUNNEL, 120); + pus::PUS_SERVICE_1, objects::PUS_TM_FUNNEL, + config::VERIFICATION_SERVICE_QUEUE_DEPTH); new Service2DeviceAccess(objects::PUS_SERVICE_2_DEVICE_ACCESS, config::EIVE_PUS_APID, pus::PUS_SERVICE_2, 3, 10); new Service3Housekeeping(objects::PUS_SERVICE_3_HOUSEKEEPING, config::EIVE_PUS_APID, From 8e1af9cfba01c146fc1d9699084b03830d649606 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 14:36:10 +0200 Subject: [PATCH 278/506] include stuff --- mission/genericFactory.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/genericFactory.cpp b/mission/genericFactory.cpp index 2dd00ee6..bd22dc6b 100644 --- a/mission/genericFactory.cpp +++ b/mission/genericFactory.cpp @@ -39,6 +39,7 @@ #include #include #include +#include "mission/tmtc/Service15TmStorage.h" #include "OBSWConfig.h" #include "devices/gpioIds.h" @@ -52,13 +53,13 @@ #include "mission/tmtc/tmFilters.h" #include "objects/systemObjectList.h" #include "tmtc/pusIds.h" +#include "devices/gpioIds.h" using persTmStore::PersistentTmStores; #if OBSW_ADD_TCPIP_SERVERS == 1 #if OBSW_ADD_TMTC_UDP_SERVER == 1 // UDP server includes -#include "devices/gpioIds.h" #include "fsfw/osal/common/UdpTcPollingTask.h" #include "fsfw/osal/common/UdpTmTcBridge.h" #endif @@ -66,7 +67,6 @@ using persTmStore::PersistentTmStores; // TCP server includes #include "fsfw/osal/common/TcpTmTcBridge.h" #include "fsfw/osal/common/TcpTmTcServer.h" -#include "mission/tmtc/Service15TmStorage.h" #endif #endif From e13fee75d0e8464649a8e2756441733923e155a2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 14:38:54 +0200 Subject: [PATCH 279/506] this is dumb --- mission/genericFactory.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/mission/genericFactory.cpp b/mission/genericFactory.cpp index bd22dc6b..34c07302 100644 --- a/mission/genericFactory.cpp +++ b/mission/genericFactory.cpp @@ -39,7 +39,6 @@ #include #include #include -#include "mission/tmtc/Service15TmStorage.h" #include "OBSWConfig.h" #include "devices/gpioIds.h" @@ -50,23 +49,23 @@ #include "mission/system/acs/acsModeTree.h" #include "mission/system/tcs/tcsModeTree.h" #include "mission/tcs/defs.h" +#include "mission/tmtc/Service15TmStorage.h" #include "mission/tmtc/tmFilters.h" #include "objects/systemObjectList.h" #include "tmtc/pusIds.h" -#include "devices/gpioIds.h" using persTmStore::PersistentTmStores; #if OBSW_ADD_TCPIP_SERVERS == 1 #if OBSW_ADD_TMTC_UDP_SERVER == 1 // UDP server includes -#include "fsfw/osal/common/UdpTcPollingTask.h" -#include "fsfw/osal/common/UdpTmTcBridge.h" +#include +#include #endif #if OBSW_ADD_TMTC_TCP_SERVER == 1 // TCP server includes -#include "fsfw/osal/common/TcpTmTcBridge.h" -#include "fsfw/osal/common/TcpTmTcServer.h" +#include +#include #endif #endif From 171d4976c3956a9f4609104086c82db68b33985b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 14:40:47 +0200 Subject: [PATCH 280/506] removing printout --- bsp_q7s/core/CoreController.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 3304fce8..76460aa5 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -1080,7 +1080,6 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); return result; } - sif::debug << "dumped segment " << dumpContext.segmentIdx << std::endl; dumpContext.segmentIdx++; dumpContext.dumpedBytes += nextDumpLen; // Dump takes multiple task cycles, so cache the dump state and continue dump the next cycles. @@ -2391,7 +2390,6 @@ void CoreController::dirListingDumpHandler() { actionHelper.finish(false, dumpContext.commander, dumpContext.actionId, result); return; } - sif::debug << "dumped segment " << dumpContext.segmentIdx << std::endl; dumpContext.segmentIdx++; dumpContext.dumpedBytes += nextDumpLen; // Dump takes multiple task cycles, so cache the dump state and continue dump the next cycles. From 9cd38a33d4d997d9d036bc9fff9d10e27e509bc8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 17:16:01 +0200 Subject: [PATCH 281/506] minor tweaks to fix main build --- dummies/helperFactory.h | 2 +- mission/cfdp/CfdpHandler.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/dummies/helperFactory.h b/dummies/helperFactory.h index e3809404..e3d2a647 100644 --- a/dummies/helperFactory.h +++ b/dummies/helperFactory.h @@ -19,7 +19,7 @@ struct DummyCfg { bool addTempSensorDummies = true; bool addRtdComIFDummy = true; bool addPlocDummies = true; - bool addCamSwitcherDummy = true; + bool addCamSwitcherDummy = false; }; void createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitch, GpioIF* gpioIF); diff --git a/mission/cfdp/CfdpHandler.cpp b/mission/cfdp/CfdpHandler.cpp index 902097b6..baf501e4 100644 --- a/mission/cfdp/CfdpHandler.cpp +++ b/mission/cfdp/CfdpHandler.cpp @@ -93,6 +93,7 @@ ReturnValue_t CfdpHandler::handleCfdpPacket(TmTcMessage& msg) { return INVALID_PDU_FORMAT; } if (not FileDirectiveReader::checkFileDirective(pduDataField[0])) { + sif::error << "CfdpHandler: Invalid PDU directive field " << pduDataField[0] << std::endl; return INVALID_DIRECTIVE_FIELD; } auto directive = static_cast(pduDataField[0]); From cf48a187335dbccf1e5b962efaa3ae8e563ec39f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 18:12:32 +0200 Subject: [PATCH 282/506] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 9372b2a5..5322de05 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 9372b2a575aaa3a2c9e93d6e745a4e3fd08d0004 +Subproject commit 5322de059916efcf874b10ccc766b46e53d2470b From 45b9e88915d8c0efc2b0b281bcddce5b80fe3737 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 18:25:59 +0200 Subject: [PATCH 283/506] route CFDP to live VC0 --- bsp_q7s/em/emObjectFactory.cpp | 1 + bsp_q7s/fmObjectFactory.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 5b2935b7..5270e887 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -140,6 +140,7 @@ void ObjectFactory::produce(void* args) { #if OBSW_TM_TO_PTME == 1 if (ccsdsArgs.liveDestination != nullptr) { pusFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); + cfdpFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); } #endif #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index df0bbc91..5eeeef59 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -97,6 +97,7 @@ void ObjectFactory::produce(void* args) { #if OBSW_TM_TO_PTME == 1 if (ccsdsArgs.liveDestination != nullptr) { pusFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); + cfdpFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); } #endif #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ From e3ae6260ad2dea57169a2674953900f16e870d83 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 18:26:41 +0200 Subject: [PATCH 284/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 22b62224..25598cd8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -112,6 +112,7 @@ will consitute of a breaking change warranting a new major release: - Various fixes for the pointing modes of the `ACS Controller`. All modes should work now as intended. - The variance for the ADIS GYRs now represents the used `-3` version and not the `-1` version +- CFDP funnel did not route packets to live channel VC0 # [v2.0.5] 2023-05-11 From bd2f1bf7f24fb288e138c39c93b34969fb160c2c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 18:25:59 +0200 Subject: [PATCH 285/506] route CFDP to live VC0 --- bsp_q7s/em/emObjectFactory.cpp | 1 + bsp_q7s/fmObjectFactory.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 0311a468..eca9847d 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -140,6 +140,7 @@ void ObjectFactory::produce(void* args) { #if OBSW_TM_TO_PTME == 1 if (ccsdsArgs.liveDestination != nullptr) { pusFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); + cfdpFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); } #endif #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index df0bbc91..5eeeef59 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -97,6 +97,7 @@ void ObjectFactory::produce(void* args) { #if OBSW_TM_TO_PTME == 1 if (ccsdsArgs.liveDestination != nullptr) { pusFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); + cfdpFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); } #endif #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ From 4146050807668424ef14113baabd9cb6cff865bd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 18:26:41 +0200 Subject: [PATCH 286/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index dc320982..43f7afc9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -46,6 +46,7 @@ will consitute of a breaking change warranting a new major release: 16505 type. - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. +- CFDP funnel did not route packets to live channel VC0 # [v2.0.5] 2023-05-11 From 7132b0c53f80765044f4a4279ec2507d82233eaf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 9 Jun 2023 19:07:23 +0200 Subject: [PATCH 287/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 8804a4e8..a9694816 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 8804a4e8e9fce1d45fcf62314affb791114d1517 +Subproject commit a969481698a205b8ae1d303cbee5bf88eb3defdc From 407901d990bca02fd3fcb7e0a97d11431cecf6a1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 10 Jun 2023 14:48:10 +0200 Subject: [PATCH 288/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index a9694816..522f273c 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit a969481698a205b8ae1d303cbee5bf88eb3defdc +Subproject commit 522f273c99845f9c50aaf135b1c6f52676b975dd From ef40391c09b4ec0841bbe97189647bde31dde42a Mon Sep 17 00:00:00 2001 From: meggert Date: Sat, 10 Jun 2023 15:34:45 +0200 Subject: [PATCH 289/506] removed debug output --- linux/acs/AcsBoardPolling.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/linux/acs/AcsBoardPolling.cpp b/linux/acs/AcsBoardPolling.cpp index b8539a8f..1ba55357 100644 --- a/linux/acs/AcsBoardPolling.cpp +++ b/linux/acs/AcsBoardPolling.cpp @@ -572,8 +572,6 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { return; } uint16_t decRateReadBack = (rawReply[10] << 8) | rawReply[11]; - sif::debug << "AcsPollingTask: DEC rate configuration. Read " << decRateReadBack - << ", expected " << decRate << std::endl; if (decRateReadBack != decRate) { sif::warning << "AcsPollingTask: DEC rate configuration failed. Read " << decRateReadBack << ", expected " << decRate << std::endl; From a4391c0515590d58491c067f88f6c42d66a6b08b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 11 Jun 2023 18:04:24 +0200 Subject: [PATCH 290/506] increase number of allowed parallel HK commands --- fsfw | 2 +- mission/genericFactory.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/fsfw b/fsfw index 5322de05..71409874 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 5322de059916efcf874b10ccc766b46e53d2470b +Subproject commit 71409874e7af43b116bf7b39dd4a6159e5a59b9c diff --git a/mission/genericFactory.cpp b/mission/genericFactory.cpp index 421d6fa9..cbe78c7f 100644 --- a/mission/genericFactory.cpp +++ b/mission/genericFactory.cpp @@ -238,7 +238,7 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun new Service2DeviceAccess(objects::PUS_SERVICE_2_DEVICE_ACCESS, config::EIVE_PUS_APID, pus::PUS_SERVICE_2, 3, 10); new Service3Housekeeping(objects::PUS_SERVICE_3_HOUSEKEEPING, config::EIVE_PUS_APID, - pus::PUS_SERVICE_3, config::HK_SERVICE_QUEUE_DEPTH); + pus::PUS_SERVICE_3, config::HK_SERVICE_QUEUE_DEPTH, 16); new Service5EventReporting( PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5), 80, 160); From 3fdd1feb94893f609edca9671151c56d1af42762 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 11 Jun 2023 18:46:30 +0200 Subject: [PATCH 291/506] prep release --- CHANGELOG.md | 2 +- fsfw | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 25598cd8..1c450e1a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,7 +18,7 @@ will consitute of a breaking change warranting a new major release: # [v4.0.0] to be released -# [v3.0.0] to be released +# [v3.0.0] 2023-06-11 - `eive-tmtc` version v4.0.0 diff --git a/fsfw b/fsfw index 5322de05..9372b2a5 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 5322de059916efcf874b10ccc766b46e53d2470b +Subproject commit 9372b2a575aaa3a2c9e93d6e745a4e3fd08d0004 From 20e920cde2277d6d43896e674a41666fb8d64454 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 11 Jun 2023 18:49:28 +0200 Subject: [PATCH 292/506] bump fsfw again --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 9372b2a5..5322de05 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 9372b2a575aaa3a2c9e93d6e745a4e3fd08d0004 +Subproject commit 5322de059916efcf874b10ccc766b46e53d2470b From f58f4c302c685b69b6117e8d3727328d93f60b8a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 13 Jun 2023 09:02:38 +0200 Subject: [PATCH 293/506] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 71409874..74b164b1 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 71409874e7af43b116bf7b39dd4a6159e5a59b9c +Subproject commit 74b164b1da9958a5b34a8c3cea05e74d111a6d4d From cb879ea97f1302543a8156f0cbda972f6a9b15b6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 13 Jun 2023 23:25:44 +0200 Subject: [PATCH 294/506] tmp sign bugfix --- mission/tcs/Tmp1075Handler.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/mission/tcs/Tmp1075Handler.cpp b/mission/tcs/Tmp1075Handler.cpp index df57aa8a..054b2118 100644 --- a/mission/tcs/Tmp1075Handler.cpp +++ b/mission/tcs/Tmp1075Handler.cpp @@ -86,8 +86,15 @@ ReturnValue_t Tmp1075Handler::scanForReply(const uint8_t *start, size_t remainin ReturnValue_t Tmp1075Handler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { switch (id) { case TMP1075::GET_TEMP: { - int16_t tempValueRaw = 0; - tempValueRaw = packet[0] << 4 | packet[1] >> 4; + // Ignore the sign bit, subtract it later when applicable. + int16_t tempValueRaw = static_cast((packet[0] << 8) | packet[1]) & 0x7fff; + // 12 bit value, so perform the shift for correct values. + tempValueRaw >>= 4; + if(((packet[0] >> 7) & 0b1) == 0b1) { + // Perform two's complement by subtracting the sign + tempValueRaw -= 0x800; + } + // 0.0625 is the sensor sensitivity. float tempValue = ((static_cast(tempValueRaw)) * 0.0625); #if OBSW_DEBUG_TMP1075 == 1 sif::info << "Tmp1075 with object id: 0x" << std::hex << getObjectId() From 38305e723f0ad3486a1047a022373cfe1d2ab722 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 13 Jun 2023 23:26:27 +0200 Subject: [PATCH 295/506] changelog --- CHANGELOG.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1c450e1a..61434dc0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,13 @@ will consitute of a breaking change warranting a new major release: # [v4.0.0] to be released +# [v3.1.0] + +## Fixed + +- TMP1075 bugfix where negative temperatures could not be measured because of a two's-complement + conversion bug. + # [v3.0.0] 2023-06-11 - `eive-tmtc` version v4.0.0 From 620dc60342c9864d81367fe28fc3ea45088df863 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 14 Jun 2023 03:15:59 +0200 Subject: [PATCH 296/506] heater ordering enum fixes --- mission/controller/ThermalController.cpp | 26 ++++++++++++------------ mission/tcs/defs.h | 6 +++--- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 6205ce44..53babbd5 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1002,7 +1002,7 @@ void ThermalController::copyDevices() { void ThermalController::ctrlAcsBoard() { heater::Switch switchNr = heater::HEATER_2_ACS_BRD; - heater::Switch redSwitchNr = heater::HEATER_0_OBC_BRD; + heater::Switch redSwitchNr = heater::HEATER_3_OBC_BRD; // A side thermalComponent = ACS_BOARD; @@ -1067,7 +1067,7 @@ void ThermalController::ctrlMgt() { sensors[2].first = sensorTemperatures.plpcduHeatspreader.isValid(); sensors[2].second = sensorTemperatures.plpcduHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, mgtLimits); + HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_1_PCDU_PDU, mgtLimits); ctrlComponentTemperature(htrCtx); if (componentAboveUpperLimit and not mgtTooHotFlag) { triggerEvent(tcsCtrl::MGT_OVERHEATING, tempFloatToU32()); @@ -1206,7 +1206,7 @@ void ThermalController::ctrlIfBoard() { sensors[2].first = deviceTemperatures.mgm2SideB.isValid(); sensors[2].second = deviceTemperatures.mgm2SideB.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, ifBoardLimits); + HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_1_PCDU_PDU, ifBoardLimits); ctrlComponentTemperature(htrCtx); // TODO: special event overheating + could go back to safe mode } @@ -1220,7 +1220,7 @@ void ThermalController::ctrlTcsBoard() { sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs1.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, tcsBoardLimits); + HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, tcsBoardLimits); ctrlComponentTemperature(htrCtx); // TODO: special event overheating + could go back to safe mode } @@ -1234,7 +1234,7 @@ void ThermalController::ctrlObc() { sensors[2].first = sensorTemperatures.tmp1075Tcs0.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs0.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, obcLimits); + HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, obcLimits); ctrlComponentTemperature(htrCtx); if (componentAboveUpperLimit and not obcTooHotFlag) { triggerEvent(tcsCtrl::OBC_OVERHEATING, tempFloatToU32()); @@ -1253,7 +1253,7 @@ void ThermalController::ctrlObcIfBoard() { sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs1.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, obcIfBoardLimits); + HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, obcIfBoardLimits); ctrlComponentTemperature(htrCtx); if (componentAboveUpperLimit and not obcTooHotFlag) { triggerEvent(tcsCtrl::OBC_OVERHEATING, tempFloatToU32()); @@ -1288,7 +1288,7 @@ void ThermalController::ctrlPcduP60Board() { sensors[1].first = deviceTemperatures.temp2P60dock.isValid(); sensors[1].second = deviceTemperatures.temp2P60dock.value; numSensors = 2; - HeaterContext htrCtx(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduP60BoardLimits); + HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduP60BoardLimits); ctrlComponentTemperature(htrCtx); if (componentAboveUpperLimit and not pcduSystemTooHotFlag) { triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32()); @@ -1300,7 +1300,7 @@ void ThermalController::ctrlPcduP60Board() { void ThermalController::ctrlPcduAcu() { thermalComponent = PCDUACU; - heater::Switch switchNr = heater::HEATER_3_PCDU_PDU; + heater::Switch switchNr = heater::HEATER_1_PCDU_PDU; heater::Switch redSwitchNr = heater::HEATER_2_ACS_BRD; if (chooseHeater(switchNr, redSwitchNr)) { @@ -1340,7 +1340,7 @@ void ThermalController::ctrlPcduPdu() { sensors[2].first = sensorTemperatures.tmp1075Tcs0.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs0.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduPduLimits); + HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduPduLimits); ctrlComponentTemperature(htrCtx); if (componentAboveUpperLimit and not pcduSystemTooHotFlag) { triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32()); @@ -1361,7 +1361,7 @@ void ThermalController::ctrlPlPcduBoard() { sensors[3].first = sensorTemperatures.plpcduHeatspreader.isValid(); sensors[3].second = sensorTemperatures.plpcduHeatspreader.value; numSensors = 4; - HeaterContext htrCtx(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, plPcduBoardLimits); + HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, plPcduBoardLimits); ctrlComponentTemperature(htrCtx); tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); } @@ -1375,7 +1375,7 @@ void ThermalController::ctrlPlocMissionBoard() { sensors[2].first = sensorTemperatures.dacHeatspreader.isValid(); sensors[2].second = sensorTemperatures.dacHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD, + HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, plocMissionBoardLimits); ctrlComponentTemperature(htrCtx); tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, plocTooHotFlag); @@ -1390,7 +1390,7 @@ void ThermalController::ctrlPlocProcessingBoard() { sensors[2].first = sensorTemperatures.dacHeatspreader.isValid(); sensors[2].second = sensorTemperatures.dacHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD, + HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, plocProcessingBoardLimits); ctrlComponentTemperature(htrCtx); tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, plocTooHotFlag); @@ -1405,7 +1405,7 @@ void ThermalController::ctrlDac() { sensors[2].first = sensorTemperatures.plocHeatspreader.isValid(); sensors[2].second = sensorTemperatures.plocHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD, dacLimits); + HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, dacLimits); ctrlComponentTemperature(htrCtx); tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); } diff --git a/mission/tcs/defs.h b/mission/tcs/defs.h index cb1a73e4..747dbc6d 100644 --- a/mission/tcs/defs.h +++ b/mission/tcs/defs.h @@ -5,10 +5,10 @@ namespace heater { enum Switch : uint8_t { - HEATER_0_OBC_BRD, - HEATER_1_PLOC_PROC_BRD, + HEATER_0_PLOC_PROC_BRD, + HEATER_1_PCDU_PDU, HEATER_2_ACS_BRD, - HEATER_3_PCDU_PDU, + HEATER_3_OBC_BRD, HEATER_4_CAMERA, HEATER_5_STR, HEATER_6_DRO, From 26199e7317b128f7fd199ebab3616514f3f41ca1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 14 Jun 2023 03:18:47 +0200 Subject: [PATCH 297/506] changelog --- CHANGELOG.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1c450e1a..2d963ab4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,13 @@ will consitute of a breaking change warranting a new major release: # [v4.0.0] to be released +# [v3.1.0] + +## Fixed + +- TCS heater switch enumeration naming was old/wrong and was not updated in sync with the object ID + update. This lead to the TCS controller commanding the wrong heaters. + # [v3.0.0] 2023-06-11 - `eive-tmtc` version v4.0.0 From 71d417710ebb1bfd6b953abac85fe77a510cb5cf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 14 Jun 2023 03:39:13 +0200 Subject: [PATCH 298/506] changelog --- CHANGELOG.md | 4 ++++ fsfw | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2d963ab4..dd0e440c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -25,6 +25,10 @@ will consitute of a breaking change warranting a new major release: - TCS heater switch enumeration naming was old/wrong and was not updated in sync with the object ID update. This lead to the TCS controller commanding the wrong heaters. +## Changed + +- Increase number of allowed parallel HK commands to 16 + # [v3.0.0] 2023-06-11 - `eive-tmtc` version v4.0.0 diff --git a/fsfw b/fsfw index 74b164b1..0a977ea6 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 74b164b1da9958a5b34a8c3cea05e74d111a6d4d +Subproject commit 0a977ea688cd78585aabb9ba511eaf8030452712 From 65815e4646cdf4e9538d25d81079b8f4e9d3a860 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 14 Jun 2023 05:17:27 +0200 Subject: [PATCH 299/506] extend bpx handler --- mission/power/BpxBatteryHandler.cpp | 6 +++++- mission/power/bpxBattDefs.h | 2 ++ tmtc | 2 +- 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/mission/power/BpxBatteryHandler.cpp b/mission/power/BpxBatteryHandler.cpp index b4aece40..96253705 100644 --- a/mission/power/BpxBatteryHandler.cpp +++ b/mission/power/BpxBatteryHandler.cpp @@ -1,4 +1,5 @@ #include +#include #include BpxBatteryHandler::BpxBatteryHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, @@ -51,6 +52,9 @@ void BpxBatteryHandler::fillCommandAndReplyMap() { insertInCommandAndReplyMap(bpxBat::RESET_COUNTERS, 1, nullptr, EMPTY_REPLY_LEN); insertInCommandAndReplyMap(bpxBat::CONFIG_CMD, 1, nullptr, EMPTY_REPLY_LEN); insertInCommandAndReplyMap(bpxBat::CONFIG_GET, 1, &cfgSet, CONFIG_GET_REPLY_LEN); + insertInCommandAndReplyMap(bpxBat::CONFIG_SET, 1, nullptr, EMPTY_REPLY_LEN); + insertInCommandAndReplyMap(bpxBat::MAN_HEAT_ON, 1, nullptr, MAN_HEAT_REPLY_LEN); + insertInCommandAndReplyMap(bpxBat::MAN_HEAT_OFF, 1, nullptr, MAN_HEAT_REPLY_LEN); } ReturnValue_t BpxBatteryHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, @@ -155,7 +159,7 @@ ReturnValue_t BpxBatteryHandler::scanForReply(const uint8_t* start, size_t remai case (bpxBat::PING): case (bpxBat::MAN_HEAT_ON): case (bpxBat::MAN_HEAT_OFF): { - if (remainingSize != PING_REPLY_LEN) { + if (remainingSize != MAN_HEAT_REPLY_LEN) { return DeviceHandlerIF::LENGTH_MISSMATCH; } break; diff --git a/mission/power/bpxBattDefs.h b/mission/power/bpxBattDefs.h index 6df87efd..0e167f07 100644 --- a/mission/power/bpxBattDefs.h +++ b/mission/power/bpxBattDefs.h @@ -48,6 +48,7 @@ static constexpr uint32_t CFG_SET_ID = CONFIG_GET; static constexpr size_t GET_HK_REPLY_LEN = 23; static constexpr size_t PING_REPLY_LEN = 3; static constexpr size_t EMPTY_REPLY_LEN = 2; +static constexpr size_t MAN_HEAT_REPLY_LEN = 3; static constexpr size_t CONFIG_GET_REPLY_LEN = 5; static constexpr uint8_t PORT_PING = 1; @@ -219,6 +220,7 @@ class BpxBatteryCfg : public StaticLocalDataSet { if (size < 3) { return SerializeIF::STREAM_TOO_SHORT; } + battheatermode.value = data[0]; battheaterLow.value = data[1]; battheaterHigh.value = data[2]; diff --git a/tmtc b/tmtc index 522f273c..cf55b363 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 522f273c99845f9c50aaf135b1c6f52676b975dd +Subproject commit cf55b3630c82c35639c39c6c0e28506ef55049e4 From 9196b8b0edefdc06b8f757a95e5d8fa622cbe167 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 14 Jun 2023 06:07:46 +0200 Subject: [PATCH 300/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index cf55b363..8a87d836 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit cf55b3630c82c35639c39c6c0e28506ef55049e4 +Subproject commit 8a87d836534d92d47debd42595cd66ef657c2f20 From fab4cdd0dc1983bbafcba64ad05647fd3f5aa687 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 14 Jun 2023 06:09:18 +0200 Subject: [PATCH 301/506] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index dd0e440c..dde7b6af 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -29,6 +29,10 @@ will consitute of a breaking change warranting a new major release: - Increase number of allowed parallel HK commands to 16 +## Added + +- Added `CONFIG_SET`, `MAN_HEATER_ON` and `MAN_HEATER_OFF` support for the BPX battery handler + # [v3.0.0] 2023-06-11 - `eive-tmtc` version v4.0.0 From 885fddd45f457d830204b679b5be1bc47b324080 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 14 Jun 2023 06:15:04 +0200 Subject: [PATCH 302/506] bump changelgo --- CHANGELOG.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index dde7b6af..1a2b2cd3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,7 +18,9 @@ will consitute of a breaking change warranting a new major release: # [v4.0.0] to be released -# [v3.1.0] +# [v3.1.0] 2023-06-14 + +- `eive-tmtc` version v4.1.0 ## Fixed From 4bef1bd56789a7ae00f1089f015139fb30d934c8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 14 Jun 2023 06:17:36 +0200 Subject: [PATCH 303/506] bump tmtc to v4.1.0 --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 8a87d836..970c8998 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 8a87d836534d92d47debd42595cd66ef657c2f20 +Subproject commit 970c8998f0bb719ab4b289fa95406d7037b2bb35 From cf875f78837e899fa4e37980ab9622390c2e5fe8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 14 Jun 2023 06:20:04 +0200 Subject: [PATCH 304/506] bump minor version --- CMakeLists.txt | 2 +- release_checklist.md | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8256dc27..2cd98b13 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 3) -set(OBSW_VERSION_MINOR 0) +set(OBSW_VERSION_MINOR 1) set(OBSW_VERSION_REVISION 0) # set(CMAKE_VERBOSE TRUE) diff --git a/release_checklist.md b/release_checklist.md index 4c8be066..04a0233c 100644 --- a/release_checklist.md +++ b/release_checklist.md @@ -7,7 +7,9 @@ OBSW Release Checklist 2. Re-run the generators with `generators/gen.py all` 3. Re-run the auto-formatters with the `scripts/auto-formatter.sh` script 4. Verify that the Q7S, Q7S EM and Host build are working -5. Wait for CI/CD results +5. Update `CHANGELOG.md`: Add new `unreleased` section, convert old unreleased section to + header containing version number and release date. +6. Wait for CI/CD results # Post-Release From 12bc9268f76e8b6f4dea94c8da74a80ca78351e4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 14 Jun 2023 18:51:06 +0200 Subject: [PATCH 305/506] perform proper sign extension --- mission/tcs/Tmp1075Handler.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/mission/tcs/Tmp1075Handler.cpp b/mission/tcs/Tmp1075Handler.cpp index 054b2118..30331e1f 100644 --- a/mission/tcs/Tmp1075Handler.cpp +++ b/mission/tcs/Tmp1075Handler.cpp @@ -86,14 +86,10 @@ ReturnValue_t Tmp1075Handler::scanForReply(const uint8_t *start, size_t remainin ReturnValue_t Tmp1075Handler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { switch (id) { case TMP1075::GET_TEMP: { - // Ignore the sign bit, subtract it later when applicable. - int16_t tempValueRaw = static_cast((packet[0] << 8) | packet[1]) & 0x7fff; - // 12 bit value, so perform the shift for correct values. - tempValueRaw >>= 4; - if(((packet[0] >> 7) & 0b1) == 0b1) { - // Perform two's complement by subtracting the sign - tempValueRaw -= 0x800; - } + // Convert 12 bit MSB first raw temperature to 16 bit first. + int16_t tempValueRaw = static_cast((packet[0] << 8) | packet[1]) >> 4; + // Sign extension to 16 bits: If the sign bit is set, fill up with ones on the left. + tempValueRaw = (packet[0] & 0x80) ? (tempValueRaw | 0xF000) : tempValueRaw; // 0.0625 is the sensor sensitivity. float tempValue = ((static_cast(tempValueRaw)) * 0.0625); #if OBSW_DEBUG_TMP1075 == 1 From b56f2b4b0ee4327296aa794ebb93da73ae4801a2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 14 Jun 2023 21:28:38 +0200 Subject: [PATCH 306/506] prep patch release --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2cd98b13..409a49a0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 3) set(OBSW_VERSION_MINOR 1) -set(OBSW_VERSION_REVISION 0) +set(OBSW_VERSION_REVISION 1) # set(CMAKE_VERBOSE TRUE) From 8da5f4dd44c98b31a88e0210b7181f59f4a78685 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 15 Jun 2023 08:39:06 +0200 Subject: [PATCH 307/506] add PS I2C resetn --- bsp_q7s/boardconfig/busConf.h | 1 + bsp_q7s/core/ObjectFactory.cpp | 17 +++++++++++++++++ bsp_q7s/core/ObjectFactory.h | 1 + bsp_q7s/em/emObjectFactory.cpp | 1 + bsp_q7s/fmObjectFactory.cpp | 1 + common/config/devices/gpioIds.h | 2 ++ tmtc | 2 +- 7 files changed, 24 insertions(+), 1 deletion(-) diff --git a/bsp_q7s/boardconfig/busConf.h b/bsp_q7s/boardconfig/busConf.h index 304113d2..d5fcd8c3 100644 --- a/bsp_q7s/boardconfig/busConf.h +++ b/bsp_q7s/boardconfig/busConf.h @@ -57,6 +57,7 @@ static constexpr char GYRO_0_ENABLE[] = "enable_gyro_0"; static constexpr char GYRO_2_ENABLE[] = "enable_gyro_2"; static constexpr char GNSS_SELECT[] = "gnss_mux_select"; static constexpr char GNSS_MUX_SELECT[] = "gnss_mux_select"; +static constexpr char PL_I2C_ARESETN[] = "pl_i2c_aresetn"; static constexpr char HEATER_0[] = "heater0"; static constexpr char HEATER_1[] = "heater1"; diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 6e1bffdf..54084089 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -1013,3 +1014,19 @@ void ObjectFactory::createRadSensorChipSelect(LinuxLibgpioIF* gpioIF) { gpioCookieRadSensor->addGpio(gpioIds::ENABLE_RADFET, gpio); gpioChecker(gpioIF->addGpios(gpioCookieRadSensor), "RAD sensor"); } + +void ObjectFactory::createPlI2cResetGpio(LinuxLibgpioIF* gpioIF) { + using namespace gpio; + if (gpioIF == nullptr) { + return; + } + GpioCookie* gpioI2cResetnCookie = new GpioCookie; + GpiodRegularByLineName* gpioI2cResetn = new GpiodRegularByLineName( + q7s::gpioNames::PL_I2C_ARESETN, "PL_I2C_ARESETN", Direction::OUT, Levels::HIGH); + gpioI2cResetnCookie->addGpio(gpioIds::PL_I2C_ARESETN, gpioI2cResetn); + gpioChecker(gpioIF->addGpios(gpioI2cResetnCookie), "PL I2C ARESETN"); + // Reset I2C explicitely again. + gpioIF->pullLow(gpioIds::PL_I2C_ARESETN); + TaskFactory::delayTask(1); + gpioIF->pullHigh(gpioIds::PL_I2C_ARESETN); +} diff --git a/bsp_q7s/core/ObjectFactory.h b/bsp_q7s/core/ObjectFactory.h index df65e1ae..a2edec4c 100644 --- a/bsp_q7s/core/ObjectFactory.h +++ b/bsp_q7s/core/ObjectFactory.h @@ -78,6 +78,7 @@ ReturnValue_t createCcsdsComponents(CcsdsComponentArgs& args); void createMiscComponents(); void createTestComponents(LinuxLibgpioIF* gpioComIF); +void createPlI2cResetGpio(LinuxLibgpioIF* gpioComIF); void testAcsBrdAss(AcsBoardAssembly* assAss); diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 5270e887..464b3398 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -47,6 +47,7 @@ void ObjectFactory::produce(void* args) { /* Adding gpios for chip select decoding to the gpioComIf */ q7s::gpioCallbacks::initSpiCsDecoder(gpioComIF); gpioCallbacks::disableAllDecoder(gpioComIF); + createPlI2cResetGpio(gpioComIF); // Hardware is usually not connected to EM, so we need to create dummies which replace lower // level components. diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 5eeeef59..413e5648 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -45,6 +45,7 @@ void ObjectFactory::produce(void* args) { /* Adding gpios for chip select decoding to the gpioComIf */ q7s::gpioCallbacks::initSpiCsDecoder(gpioComIF); gpioCallbacks::disableAllDecoder(gpioComIF); + createPlI2cResetGpio(gpioComIF); new CoreController(objects::CORE_CONTROLLER, enableHkSets); createPcduComponents(gpioComIF, &pwrSwitcher, enableHkSets); diff --git a/common/config/devices/gpioIds.h b/common/config/devices/gpioIds.h index 573327fa..bed82142 100644 --- a/common/config/devices/gpioIds.h +++ b/common/config/devices/gpioIds.h @@ -77,6 +77,8 @@ enum gpioId_t { CS_RAD_SENSOR, ENABLE_RADFET, + PL_I2C_ARESETN, + PAPB_BUSY_N, PAPB_EMPTY, diff --git a/tmtc b/tmtc index 29fc7a5f..936dcdf3 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 29fc7a5fca197abe44d8bbba6b0db3af2744f01c +Subproject commit 936dcdf334c2258d2256373cd4995b2574202a59 From 40eae48a1a4254adc8a31094296e23163356aa41 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 15 Jun 2023 09:51:34 +0200 Subject: [PATCH 308/506] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7be5b210..e35ba24d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -31,6 +31,11 @@ TODO: New firmware package version. - Removed PTME busy/ready signals. Those were not used anyway because register reads are used now. - APB bus access busy checking is not done anymore as this is performed by the bus itself now. +## Added + +- Added PL I2C reset pin. It is not used for now but could be used for FDIR procedures to restore + the PL I2C. + # [v3.1.1] 2023-06-14 ## Fixed From 456ee156c3964ca655038c1c50409ec423a5f24d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 15 Jun 2023 10:02:12 +0200 Subject: [PATCH 309/506] finally have a firmware version --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e35ba24d..06b3e38c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,7 +19,7 @@ will consitute of a breaking change warranting a new major release: # [v4.0.0] to be released - `eive-tmtc` version v4.0.0 -TODO: New firmware package version. +- `q7s-package` version v3.0.0 ## Fixed From cada6e0440499d7b47ae28f600762ae95914ef09 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 15 Jun 2023 17:56:45 +0200 Subject: [PATCH 310/506] fixed memcpy --- mission/controller/acs/SensorProcessing.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index b106baaa..04cbf2d8 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -265,8 +265,8 @@ void SensorProcessing::processSus( std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(double)); + std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(double)); susDataProcessed->setValidity(false, true); std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); susDataProcessed->sunIjkModel.setValid(true); @@ -274,7 +274,6 @@ void SensorProcessing::processSus( } return; } - // WARNING: NOT TRANSFORMED IN BODY FRAME YET // Transformation into Geomtry Frame float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0}, sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0}, From 51a3a2f5cf3e18185b485e15b59a2b1b6625dcf2 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 15 Jun 2023 17:58:35 +0200 Subject: [PATCH 311/506] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index d403c0b5..dc4499c4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,11 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- SUS total vector was not reset to being a zero vector during eclipse due to a wrong memcpy + length. + # [v4.0.0] to be released # [v3.1.1] 2023-06-14 From 3dbc01bd8ad47cb6e7ba8bb9983ecf06c4c56a5b Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 15 Jun 2023 18:16:24 +0200 Subject: [PATCH 312/506] cleanup --- mission/controller/acs/SensorProcessing.cpp | 55 ++++++++++----------- mission/controller/acs/SensorProcessing.h | 3 ++ 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 04cbf2d8..c4fd8165 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -45,14 +45,13 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const { PoolReadGuard pg(mgmDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - float zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm0vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm1vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm2vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm3vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm4vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgmVecTot.value, ZERO_VEC_D, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(float)); mgmDataProcessed->setValidity(false, true); std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double)); mgmDataProcessed->magIgrfModel.setValid(gpsValid); @@ -252,21 +251,20 @@ void SensorProcessing::processSus( { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - float zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(susDataProcessed->sus0vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus1vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus2vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus3vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus4vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus5vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus6vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus7vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus8vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(double)); - std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(double)); + std::memcpy(susDataProcessed->sus0vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus1vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus2vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus3vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus4vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus5vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus6vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus7vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus8vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus9vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus10vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus11vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTot.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(susDataProcessed->susVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(double)); susDataProcessed->setValidity(false, true); std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); susDataProcessed->sunIjkModel.setValid(true); @@ -458,12 +456,11 @@ void SensorProcessing::processGyr( { PoolReadGuard pg(gyrDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - double zeroVector[3] = {0.0, 0.0, 0.0}; - std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyr3vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyrVecTot.value, zeroVector, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr0vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr1vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr2vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr3vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyrVecTot.value, ZERO_VEC_D, 3 * sizeof(double)); gyrDataProcessed->setValidity(false, true); } } diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 47449a75..1a59291f 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -23,6 +23,9 @@ class SensorProcessing { acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters); // Will call protected functions private: + static constexpr float ZERO_VEC_F[3] = {0, 0, 0}; + static constexpr float ZERO_VEC_D[3] = {0, 0, 0}; + protected: // short description needed for every function void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, From e130d45f0bdb3a12e6efa43e7d1210bb9ed2b023 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 15 Jun 2023 20:07:33 +0200 Subject: [PATCH 313/506] fixed type of doube zero vector --- mission/controller/acs/SensorProcessing.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 1a59291f..6dbc5d58 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -24,7 +24,7 @@ class SensorProcessing { const AcsParameters *acsParameters); // Will call protected functions private: static constexpr float ZERO_VEC_F[3] = {0, 0, 0}; - static constexpr float ZERO_VEC_D[3] = {0, 0, 0}; + static constexpr double ZERO_VEC_D[3] = {0, 0, 0}; protected: // short description needed for every function From 1b919ceaeceaed21e6873565f7c22e1397737553 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 16 Jun 2023 13:54:50 +0200 Subject: [PATCH 314/506] swap i2cs --- bsp_q7s/core/ObjectFactory.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 54084089..e13da256 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -164,7 +164,7 @@ void ObjectFactory::createTmpComponents() { for (size_t idx = 0; idx < tmpDevIds.size(); idx++) { tmpDevCookies.push_back( - new I2cCookie(tmpDevIds[idx].second, TMP1075::MAX_REPLY_LENGTH, q7s::I2C_PS_EIVE)); + new I2cCookie(tmpDevIds[idx].second, TMP1075::MAX_REPLY_LENGTH, q7s::I2C_PL_EIVE)); auto* tmpDevHandler = new Tmp1075Handler(tmpDevIds[idx].first, objects::I2C_COM_IF, tmpDevCookies[idx]); tmpDevHandler->setCustomFdir(new TmpDevFdir(tmpDevIds[idx].first)); @@ -959,7 +959,7 @@ void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher, bool enable imtqAssy->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); new ImtqPollingTask(objects::IMTQ_POLLING, I2C_FATAL_ERRORS); - I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, imtq::MAX_REPLY_SIZE, q7s::I2C_PL_EIVE); + I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, imtq::MAX_REPLY_SIZE, q7s::I2C_PS_EIVE); auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::IMTQ_POLLING, imtqI2cCookie, power::Switches::PDU1_CH3_MGT_5V, enableHkSets); imtqHandler->enableThermalModule(ThermalStateCfg()); @@ -976,7 +976,7 @@ void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher, bool enable } void ObjectFactory::createBpxBatteryComponent(bool enableHkSets) { - I2cCookie* bpxI2cCookie = new I2cCookie(addresses::BPX_BATTERY, 100, q7s::I2C_PL_EIVE); + I2cCookie* bpxI2cCookie = new I2cCookie(addresses::BPX_BATTERY, 100, q7s::I2C_PS_EIVE); BpxBatteryHandler* bpxHandler = new BpxBatteryHandler( objects::BPX_BATT_HANDLER, objects::I2C_COM_IF, bpxI2cCookie, enableHkSets); bpxHandler->setStartUpImmediately(); From df4205c71e93d286249e053119e219f0defeea21 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 16 Jun 2023 14:21:37 +0200 Subject: [PATCH 315/506] fixed sizeof for mgmVec stuff --- mission/controller/acs/SensorProcessing.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index c4fd8165..b726ca53 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -30,10 +30,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const // ------------------------------------------------ double magIgrfModel[3] = {0.0, 0.0, 0.0}; if (gpsValid) { - // Should be existing class object which will be called and modified here. Igrf13Model igrf13; - // So the line above should not be done here. Update: Can be done here as long updated coffs - // stored in acsParameters ? igrf13.schmidtNormalization(); igrf13.updateCoeffGH(timeOfMgmMeasurement); // maybe put a condition here, to only update after a full day, this @@ -50,8 +47,8 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const std::memcpy(mgmDataProcessed->mgm2vec.value, ZERO_VEC_F, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm3vec.value, ZERO_VEC_F, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm4vec.value, ZERO_VEC_F, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTot.value, ZERO_VEC_D, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgmVecTot.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(double)); mgmDataProcessed->setValidity(false, true); std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double)); mgmDataProcessed->magIgrfModel.setValid(gpsValid); From 9c0744ae026107af390e077be2ca7d0008b9ebcb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 17 Jun 2023 12:24:54 +0200 Subject: [PATCH 316/506] minor improvements --- fsfw | 2 +- mission/cfdp/CfdpHandler.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/fsfw b/fsfw index 0a977ea6..74602d11 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 0a977ea688cd78585aabb9ba511eaf8030452712 +Subproject commit 74602d113c4e61caa30739226c66d02f48c611a4 diff --git a/mission/cfdp/CfdpHandler.cpp b/mission/cfdp/CfdpHandler.cpp index baf501e4..fa35535c 100644 --- a/mission/cfdp/CfdpHandler.cpp +++ b/mission/cfdp/CfdpHandler.cpp @@ -93,7 +93,8 @@ ReturnValue_t CfdpHandler::handleCfdpPacket(TmTcMessage& msg) { return INVALID_PDU_FORMAT; } if (not FileDirectiveReader::checkFileDirective(pduDataField[0])) { - sif::error << "CfdpHandler: Invalid PDU directive field " << pduDataField[0] << std::endl; + sif::error << "CfdpHandler: Invalid PDU directive field " << static_cast(pduDataField[0]) + << std::endl; return INVALID_DIRECTIVE_FIELD; } auto directive = static_cast(pduDataField[0]); From d6dd943ebfd84dbe25e5576aa01b828bb8b25f20 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 17 Jun 2023 15:28:05 +0200 Subject: [PATCH 317/506] tmp is back in the house --- bsp_q7s/core/ObjectFactory.cpp | 19 ++----- bsp_q7s/core/ObjectFactory.h | 2 +- bsp_q7s/core/scheduling.cpp | 11 +++- bsp_q7s/em/emObjectFactory.cpp | 9 +++ bsp_q7s/fmObjectFactory.cpp | 10 +++- mission/pollingSeqTables.cpp | 101 +++++++++++++++++++-------------- mission/pollingSeqTables.h | 12 +++- tmtc | 2 +- 8 files changed, 104 insertions(+), 62 deletions(-) diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index e13da256..371d4c6d 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -151,23 +151,16 @@ void Factory::setStaticFrameworkObjectIds() { void ObjectFactory::setStatics() { Factory::setStaticFrameworkObjectIds(); } -void ObjectFactory::createTmpComponents() { - std::vector> tmpDevIds = {{ - {objects::TMP1075_HANDLER_TCS_0, addresses::TMP1075_TCS_0}, - {objects::TMP1075_HANDLER_TCS_1, addresses::TMP1075_TCS_1}, - {objects::TMP1075_HANDLER_PLPCDU_0, addresses::TMP1075_PLPCDU_0}, - // damaged - // {objects::TMP1075_HANDLER_PLPCDU_1, addresses::TMP1075_PLPCDU_1}, - {objects::TMP1075_HANDLER_IF_BOARD, addresses::TMP1075_IF_BOARD}, - }}; +void ObjectFactory::createTmpComponents( + std::vector> tmpDevsToAdd) { std::vector tmpDevCookies; - for (size_t idx = 0; idx < tmpDevIds.size(); idx++) { + for (size_t idx = 0; idx < tmpDevsToAdd.size(); idx++) { tmpDevCookies.push_back( - new I2cCookie(tmpDevIds[idx].second, TMP1075::MAX_REPLY_LENGTH, q7s::I2C_PL_EIVE)); + new I2cCookie(tmpDevsToAdd[idx].second, TMP1075::MAX_REPLY_LENGTH, q7s::I2C_PL_EIVE)); auto* tmpDevHandler = - new Tmp1075Handler(tmpDevIds[idx].first, objects::I2C_COM_IF, tmpDevCookies[idx]); - tmpDevHandler->setCustomFdir(new TmpDevFdir(tmpDevIds[idx].first)); + new Tmp1075Handler(tmpDevsToAdd[idx].first, objects::I2C_COM_IF, tmpDevCookies[idx]); + tmpDevHandler->setCustomFdir(new TmpDevFdir(tmpDevsToAdd[idx].first)); tmpDevHandler->connectModeTreeParent(satsystem::tcs::SUBSYSTEM); // TODO: Remove this after TCS subsystem was added // These devices are connected to the 3V3 stack and should be powered permanently. Therefore, diff --git a/bsp_q7s/core/ObjectFactory.h b/bsp_q7s/core/ObjectFactory.h index a2edec4c..25d06090 100644 --- a/bsp_q7s/core/ObjectFactory.h +++ b/bsp_q7s/core/ObjectFactory.h @@ -58,7 +58,7 @@ void createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher bool enableHkSets); void createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF, PowerSwitchIF* pwrSwitcher, Stack5VHandler& stackHandler); -void createTmpComponents(); +void createTmpComponents(std::vector> tmpDevsToAdd); void createRadSensorChipSelect(LinuxLibgpioIF* gpioIF); ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& handler); void createAcsBoardGpios(GpioCookie& cookie); diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index 71da5bdc..f355d384 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -21,6 +21,7 @@ #include "mission/pollingSeqTables.h" #include "mission/scheduling.h" #include "mission/utility/InitMission.h" +#include "q7sConfig.h" /* This is configured for linux without CR */ #ifdef PLATFORM_UNIX @@ -527,7 +528,15 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction FixedTimeslotTaskIF* i2cPst = factory.createFixedTimeslotTask("I2C_PS_PST", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.6, missedDeadlineFunc, &RR_SCHEDULING); - result = pst::pstI2cProcessingSystem(i2cPst); + pst::TmpSchedConfig tmpSchedConf; +#if OBSW_Q7S_EM == 1 + tmpSchedConf.scheduleTmpDev0 = false; + tmpSchedConf.scheduleTmpDev1 = false; + tmpSchedConf.schedulePlPcduDev0 = true; + tmpSchedConf.schedulePlPcduDev1 = true; + tmpSchedConf.scheduleIfBoardDev = true; +#endif + result = pst::pstProgammableLogicI2c(tmpSchedConf, i2cPst); if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { sif::warning << "scheduling::initTasks: I2C PST is empty" << std::endl; diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 464b3398..91ca0d00 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -12,6 +12,7 @@ #include "bsp_q7s/core/CoreController.h" #include "bsp_q7s/core/ObjectFactory.h" #include "busConf.h" +#include "common/config/devices/addresses.h" #include "devConf.h" #include "dummies/helperFactory.h" #include "eive/objects.h" @@ -60,6 +61,14 @@ void ObjectFactory::produce(void* args) { #if OBSW_ADD_PLOC_SUPERVISOR == 1 || OBSW_ADD_PLOC_MPSOC == 1 dummyCfg.addPlocDummies = false; #endif +#if OBSW_ADD_TMP_DEVICES == 1 + std::vector> tmpDevsToAdd = {{ + {objects::TMP1075_HANDLER_PLPCDU_0, addresses::TMP1075_PLPCDU_0}, + {objects::TMP1075_HANDLER_PLPCDU_1, addresses::TMP1075_PLPCDU_1}, + {objects::TMP1075_HANDLER_IF_BOARD, addresses::TMP1075_IF_BOARD}, + }}; + createTmpComponents(tmpDevsToAdd); +#endif #if OBSW_ADD_GOMSPACE_PCDU == 1 dummyCfg.addPowerDummies = false; // The ACU broke. diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 413e5648..25fb221c 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -67,7 +67,15 @@ void ObjectFactory::produce(void* args) { HeaterHandler* heaterHandler; createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler); #if OBSW_ADD_TMP_DEVICES == 1 - createTmpComponents(); + std::vector> tmpDevsToAdd = {{ + {objects::TMP1075_HANDLER_TCS_0, addresses::TMP1075_TCS_0}, + {objects::TMP1075_HANDLER_TCS_1, addresses::TMP1075_TCS_1}, + {objects::TMP1075_HANDLER_PLPCDU_0, addresses::TMP1075_PLPCDU_0}, + // damaged + // {objects::TMP1075_HANDLER_PLPCDU_1, addresses::TMP1075_PLPCDU_1}, + {objects::TMP1075_HANDLER_IF_BOARD, addresses::TMP1075_IF_BOARD}, + }}; + createTmpComponents(tmpDevsToAdd); #endif createSolarArrayDeploymentComponents(*pwrSwitcher, *gpioComIF); createPlPcduComponents(gpioComIF, spiMainComIF, pwrSwitcher, *stackHandler); diff --git a/mission/pollingSeqTables.cpp b/mission/pollingSeqTables.cpp index 0367ef27..d065b5aa 100644 --- a/mission/pollingSeqTables.cpp +++ b/mission/pollingSeqTables.cpp @@ -39,56 +39,69 @@ ReturnValue_t pst::pstSyrlinks(FixedTimeslotTaskIF *thisSequence) { // I don't think this needs to be in a PST because linux takes care of bus serialization, but // keep it like this for now, it works -ReturnValue_t pst::pstI2cProcessingSystem(FixedTimeslotTaskIF *thisSequence) { +ReturnValue_t pst::pstProgammableLogicI2c(TmpSchedConfig schedConf, + FixedTimeslotTaskIF *thisSequence) { // Length of a communication cycle uint32_t length = thisSequence->getPeriodMs(); static_cast(length); - // These are actually part of another bus, but this works, so keep it like this for now - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.2, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.2, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.2, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.3, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.3, DeviceHandlerIF::GET_READ); + if (schedConf.scheduleTmpDev0) { + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.2, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.2, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.2, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.3, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.3, DeviceHandlerIF::GET_READ); + } - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.5, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.5, DeviceHandlerIF::GET_READ); + if (schedConf.scheduleTmpDev1) { + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.5, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.5, DeviceHandlerIF::GET_READ); + } - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.6, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.6, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.6, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.7, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.7, DeviceHandlerIF::GET_READ); - // damaged - /* - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, DeviceHandlerIF::GET_READ); - */ - thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.8, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.8, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.8, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.9, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.9, DeviceHandlerIF::GET_READ); + if (schedConf.schedulePlPcduDev0) { + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.6, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.6, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.6, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.7, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.7, + DeviceHandlerIF::GET_READ); + } + if (schedConf.schedulePlPcduDev1) { + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, + DeviceHandlerIF::GET_READ); + } + + if (schedConf.scheduleIfBoardDev) { + thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.8, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.8, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.8, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.9, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.9, + DeviceHandlerIF::GET_READ); + } static_cast(length); return thisSequence->checkSequence(); } diff --git a/mission/pollingSeqTables.h b/mission/pollingSeqTables.h index e3bc0ad0..831d78be 100644 --- a/mission/pollingSeqTables.h +++ b/mission/pollingSeqTables.h @@ -39,6 +39,16 @@ struct AcsPstCfg { bool scheduleStr = true; }; +// Default config is for FM. +struct TmpSchedConfig { + bool scheduleTmpDev0 = true; + bool scheduleTmpDev1 = true; + bool schedulePlPcduDev0 = true; + // damaged on FM + bool schedulePlPcduDev1 = false; + bool scheduleIfBoardDev = true; +}; + /** * @brief This function creates the PST for all gomspace devices. * @details @@ -51,7 +61,7 @@ ReturnValue_t pstSyrlinks(FixedTimeslotTaskIF* thisSequence); ReturnValue_t pstTcsAndAcs(FixedTimeslotTaskIF* thisSequence, AcsPstCfg cfg); -ReturnValue_t pstI2cProcessingSystem(FixedTimeslotTaskIF* thisSequence); +ReturnValue_t pstProgammableLogicI2c(TmpSchedConfig schedConf, FixedTimeslotTaskIF* thisSequence); /** * Generic test PST diff --git a/tmtc b/tmtc index 936dcdf3..970c8998 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 936dcdf334c2258d2256373cd4995b2574202a59 +Subproject commit 970c8998f0bb719ab4b289fa95406d7037b2bb35 From 269d5f6a0afe957d649acddd7500f5ad240f0910 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 17 Jun 2023 17:01:01 +0200 Subject: [PATCH 318/506] stupid tmps --- bsp_q7s/em/emObjectFactory.cpp | 9 ++++- dummies/TemperatureSensorInserter.cpp | 12 +++--- dummies/TemperatureSensorInserter.h | 4 +- dummies/helperFactory.cpp | 56 +++++++++++++++++---------- dummies/helperFactory.h | 10 +++++ mission/pollingSeqTables.cpp | 41 ++++++++++---------- 6 files changed, 82 insertions(+), 50 deletions(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 91ca0d00..52754b8a 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -45,7 +45,7 @@ void ObjectFactory::produce(void* args) { SpiComIF* spiMainComIF = nullptr; I2cComIF* i2cComIF = nullptr; createCommunicationInterfaces(&gpioComIF, &uartComIF, &spiMainComIF, &i2cComIF); - /* Adding gpios for chip select decoding to the gpioComIf */ + // Adding GPIOs for chip select decoding and initializing them. q7s::gpioCallbacks::initSpiCsDecoder(gpioComIF); gpioCallbacks::disableAllDecoder(gpioComIF); createPlI2cResetGpio(gpioComIF); @@ -68,6 +68,13 @@ void ObjectFactory::produce(void* args) { {objects::TMP1075_HANDLER_IF_BOARD, addresses::TMP1075_IF_BOARD}, }}; createTmpComponents(tmpDevsToAdd); + dummy::Tmp1075Cfg tmpCfg {}; + tmpCfg.addTcsBrd0 = true; + tmpCfg.addTcsBrd1 = true; + tmpCfg.addPlPcdu0 = false; + tmpCfg.addPlPcdu1 = false; + tmpCfg.addIfBrd = false; + dummyCfg.tmp1075Cfg = tmpCfg; #endif #if OBSW_ADD_GOMSPACE_PCDU == 1 dummyCfg.addPowerDummies = false; diff --git a/dummies/TemperatureSensorInserter.cpp b/dummies/TemperatureSensorInserter.cpp index 14a005aa..0e5d7421 100644 --- a/dummies/TemperatureSensorInserter.cpp +++ b/dummies/TemperatureSensorInserter.cpp @@ -7,9 +7,9 @@ #include #include -TemperatureSensorInserter::TemperatureSensorInserter(object_id_t objectId, - Max31865DummyMap tempSensorDummies_, - Tmp1075DummyMap tempTmpSensorDummies_) +TemperatureSensorInserter::TemperatureSensorInserter( + object_id_t objectId, Max31865DummyMap tempSensorDummies_, + std::optional tempTmpSensorDummies_) : SystemObject(objectId), max31865DummyMap(std::move(tempSensorDummies_)), tmp1075DummyMap(std::move(tempTmpSensorDummies_)) {} @@ -25,8 +25,10 @@ ReturnValue_t TemperatureSensorInserter::performOperation(uint8_t opCode) { for (auto& rtdDummy : max31865DummyMap) { rtdDummy.second->setTemperature(10, true); } - for (auto& tmpDummy : tmp1075DummyMap) { - tmpDummy.second->setTemperature(10, true); + if (tmp1075DummyMap.has_value()) { + for (auto& tmpDummy : tmp1075DummyMap.value()) { + tmpDummy.second->setTemperature(10, true); + } } tempsWereInitialized = true; } diff --git a/dummies/TemperatureSensorInserter.h b/dummies/TemperatureSensorInserter.h index eb6cc1ba..d710b680 100644 --- a/dummies/TemperatureSensorInserter.h +++ b/dummies/TemperatureSensorInserter.h @@ -12,7 +12,7 @@ class TemperatureSensorInserter : public ExecutableObjectIF, public SystemObject using Max31865DummyMap = std::map; using Tmp1075DummyMap = std::map; explicit TemperatureSensorInserter(object_id_t objectId, Max31865DummyMap tempSensorDummies_, - Tmp1075DummyMap tempTmpSensorDummies_); + std::optional tempTmpSensorDummies_); ReturnValue_t initialize() override; ReturnValue_t initializeAfterTaskCreation() override; @@ -22,7 +22,7 @@ class TemperatureSensorInserter : public ExecutableObjectIF, public SystemObject private: Max31865DummyMap max31865DummyMap; - Tmp1075DummyMap tmp1075DummyMap; + std::optional tmp1075DummyMap; enum TestCase { NONE = 0, diff --git a/dummies/helperFactory.cpp b/dummies/helperFactory.cpp index 157d11d3..6cd78f93 100644 --- a/dummies/helperFactory.cpp +++ b/dummies/helperFactory.cpp @@ -191,25 +191,36 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio objects::RTD_15_IC18_IMTQ, new Max31865Dummy(objects::RTD_15_IC18_IMTQ, objects::DUMMY_COM_IF, comCookieDummy)); - std::map tmpSensorDummies; - tmpSensorDummies.emplace( - objects::TMP1075_HANDLER_TCS_0, - new Tmp1075Dummy(objects::TMP1075_HANDLER_TCS_0, objects::DUMMY_COM_IF, comCookieDummy)); - tmpSensorDummies.emplace( - objects::TMP1075_HANDLER_TCS_1, - new Tmp1075Dummy(objects::TMP1075_HANDLER_TCS_1, objects::DUMMY_COM_IF, comCookieDummy)); - tmpSensorDummies.emplace( - objects::TMP1075_HANDLER_PLPCDU_0, - new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_0, objects::DUMMY_COM_IF, comCookieDummy)); - // damaged. - // tmpSensorDummies.emplace( - // objects::TMP1075_HANDLER_PLPCDU_1, - // new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_1, objects::DUMMY_COM_IF, - // comCookieDummy)); - tmpSensorDummies.emplace( - objects::TMP1075_HANDLER_IF_BOARD, - new Tmp1075Dummy(objects::TMP1075_HANDLER_IF_BOARD, objects::DUMMY_COM_IF, comCookieDummy)); - + std::optional tmpSensorDummies; + TemperatureSensorInserter::Tmp1075DummyMap tmpDummyMap; + if (cfg.addTmpDummies) { + tmpSensorDummies = tmpDummyMap; + if (cfg.tmp1075Cfg.addTcsBrd0) { + tmpDummyMap.emplace(objects::TMP1075_HANDLER_TCS_0, + new Tmp1075Dummy(objects::TMP1075_HANDLER_TCS_0, objects::DUMMY_COM_IF, + comCookieDummy)); + } + if (cfg.tmp1075Cfg.addTcsBrd1) { + tmpDummyMap.emplace(objects::TMP1075_HANDLER_TCS_1, + new Tmp1075Dummy(objects::TMP1075_HANDLER_TCS_1, objects::DUMMY_COM_IF, + comCookieDummy)); + } + if (cfg.tmp1075Cfg.addPlPcdu0) { + tmpDummyMap.emplace(objects::TMP1075_HANDLER_PLPCDU_0, + new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_0, + objects::DUMMY_COM_IF, comCookieDummy)); + } + if (cfg.tmp1075Cfg.addPlPcdu1) { + tmpDummyMap.emplace(objects::TMP1075_HANDLER_PLPCDU_1, + new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_1, + objects::DUMMY_COM_IF, comCookieDummy)); + } + if (cfg.tmp1075Cfg.addIfBrd) { + tmpDummyMap.emplace(objects::TMP1075_HANDLER_IF_BOARD, + new Tmp1075Dummy(objects::TMP1075_HANDLER_IF_BOARD, + objects::DUMMY_COM_IF, comCookieDummy)); + } + } new TemperatureSensorInserter(objects::THERMAL_TEMP_INSERTER, rtdSensorDummies, tmpSensorDummies); TcsBoardAssembly* tcsBoardAssy = @@ -217,8 +228,11 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio for (auto& rtd : rtdSensorDummies) { rtd.second->connectModeTreeParent(*tcsBoardAssy); } - for (auto& tmp : tmpSensorDummies) { - tmp.second->connectModeTreeParent(satsystem::tcs::SUBSYSTEM); + if(tmpSensorDummies.has_value()) { + for (auto& tmp : tmpSensorDummies.value()) { + printf("Hello, connecting parent to %08x\n", tmp.second->getObjectId()); + tmp.second->connectModeTreeParent(satsystem::tcs::SUBSYSTEM); + } } } if (cfg.addCamSwitcherDummy) { diff --git a/dummies/helperFactory.h b/dummies/helperFactory.h index bab9d8d8..9c1a93d3 100644 --- a/dummies/helperFactory.h +++ b/dummies/helperFactory.h @@ -6,6 +6,14 @@ class GpioIF; namespace dummy { +struct Tmp1075Cfg { + bool addTcsBrd0 = true; + bool addTcsBrd1 = true; + bool addPlPcdu0 = true; + bool addPlPcdu1 = true; + bool addIfBrd = true; +}; + // Default values targeted towards EM. struct DummyCfg { bool addCoreCtrlCfg = true; @@ -19,6 +27,8 @@ struct DummyCfg { bool addTempSensorDummies = true; bool addRtdComIFDummy = true; bool addPlocDummies = true; + bool addTmpDummies = true; + Tmp1075Cfg tmp1075Cfg; bool addCamSwitcherDummy = false; }; diff --git a/mission/pollingSeqTables.cpp b/mission/pollingSeqTables.cpp index d065b5aa..e2fba92c 100644 --- a/mission/pollingSeqTables.cpp +++ b/mission/pollingSeqTables.cpp @@ -46,47 +46,46 @@ ReturnValue_t pst::pstProgammableLogicI2c(TmpSchedConfig schedConf, static_cast(length); if (schedConf.scheduleTmpDev0) { - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.2, + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.2, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.2, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.3, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.3, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.1, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.1, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.1, DeviceHandlerIF::GET_READ); } if (schedConf.scheduleTmpDev1) { - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.2, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.5, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.5, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.2, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.3, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.3, DeviceHandlerIF::GET_READ); } if (schedConf.schedulePlPcduDev0) { - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.6, + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.4, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.6, + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.4, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.6, + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.4, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.7, + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.5, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.7, + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.5, DeviceHandlerIF::GET_READ); } if (schedConf.schedulePlPcduDev1) { - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.6, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.6, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.6, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.7, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.7, DeviceHandlerIF::GET_READ); } From 727050520b45f9b09bbdb85c82af4c8d587cdbbb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 17 Jun 2023 17:24:51 +0200 Subject: [PATCH 319/506] that should do the job --- dummies/helperFactory.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dummies/helperFactory.cpp b/dummies/helperFactory.cpp index 6cd78f93..cbe979bf 100644 --- a/dummies/helperFactory.cpp +++ b/dummies/helperFactory.cpp @@ -192,9 +192,8 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio new Max31865Dummy(objects::RTD_15_IC18_IMTQ, objects::DUMMY_COM_IF, comCookieDummy)); std::optional tmpSensorDummies; - TemperatureSensorInserter::Tmp1075DummyMap tmpDummyMap; if (cfg.addTmpDummies) { - tmpSensorDummies = tmpDummyMap; + TemperatureSensorInserter::Tmp1075DummyMap tmpDummyMap; if (cfg.tmp1075Cfg.addTcsBrd0) { tmpDummyMap.emplace(objects::TMP1075_HANDLER_TCS_0, new Tmp1075Dummy(objects::TMP1075_HANDLER_TCS_0, objects::DUMMY_COM_IF, @@ -220,6 +219,7 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio new Tmp1075Dummy(objects::TMP1075_HANDLER_IF_BOARD, objects::DUMMY_COM_IF, comCookieDummy)); } + tmpSensorDummies = std::move(tmpDummyMap); } new TemperatureSensorInserter(objects::THERMAL_TEMP_INSERTER, rtdSensorDummies, tmpSensorDummies); From 4155aa8776d98f2b3cbe9a4a471f38f23791dcd5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 17 Jun 2023 19:17:50 +0200 Subject: [PATCH 320/506] v3.1.1 reduced to the bare minimum --- README.md | 18 +++++++--- bsp_q7s/core/ObjectFactory.cpp | 2 +- cmake/Zynq7020CrossCompileConfig.cmake | 4 +-- mission/acs/str/StarTrackerHandler.cpp | 13 +++++++ mission/controller/ThermalController.cpp | 39 ++++++++++++--------- mission/power/BpxBatteryHandler.cpp | 6 +++- mission/power/bpxBattDefs.h | 2 ++ mission/system/acs/DualLaneAssemblyBase.cpp | 9 ++--- mission/tcs/HeaterHandler.cpp | 4 +++ mission/tcs/Tmp1075Handler.cpp | 7 ++-- mission/tcs/defs.h | 6 ++-- q7s-env-em.sh | 4 +-- q7s-env.sh | 4 +-- release_checklist.md | 4 ++- 14 files changed, 83 insertions(+), 39 deletions(-) diff --git a/README.md b/README.md index cd350386..6b06283c 100644 --- a/README.md +++ b/README.md @@ -99,11 +99,21 @@ When using Windows, run theses steps in MSYS2. git submodule update --init ``` -3. Ensure that the cross-compiler is working with `arm-linux-gnueabihf-gcc --version` and that +3. Create two system variables to pass the system root path and the cross-compiler path to the + build system. You only need to do this once when setting up the build system. + Example for Unix: + + ```sh + export CROSS_COMPILE_BIN_PATH= + export ZYNQ_7020_SYSROOT= + ``` + +4. Ensure that the cross-compiler is working with + `${CROSS_COMPILE_BIN_PATH}/arm-linux-gnueabihf-gcc --version` and that the sysroot environmental variables have been set like specified in the [root filesystem chapter](#sysroot). -4. Run the CMake configuration to create the build system in a `build-Debug-Q7S` folder. +5. Run the CMake configuration to create the build system in a `build-Debug-Q7S` folder. Add `-G "MinGW Makefiles` in MinGW64 on Windows. ```sh @@ -112,7 +122,7 @@ When using Windows, run theses steps in MSYS2. cmake --build . -j ``` - You can also use provided shell scripts to perform these commands. + Please note that you can also use provided shell scripts to perform these commands. ```sh cp scripts/q7s-env.sh .. cp scripts/q7s-env-em.sh .. @@ -144,7 +154,7 @@ When using Windows, run theses steps in MSYS2. There are also different values for `-DTGT_BSP` to build for the Raspberry Pi or the Beagle Bone Black: `arm/raspberrypi` and `arm/beagleboneblack`. -5. Build the software with +6. Build the software with ```sh cd cmake-build-debug-q7s diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 964d728b..e8f6fd78 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -816,7 +816,7 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(CcsdsComponentArgs& args) { // Core task which handles the HK store and takes care of dumping it as TM using a VC directly auto* hkStore = new PersistentSingleTmStoreTask( objects::HK_STORE_AND_TM_TASK, args.ipcStore, *args.stores.hkStore, *vc, - persTmStore::DUMP_HK_STORE_DONE, persTmStore::DUMP_HK_STORE_DONE, *SdCardManager::instance(), + persTmStore::DUMP_HK_STORE_DONE, persTmStore::DUMP_HK_CANCELLED, *SdCardManager::instance(), PTME_LOCKED); hkStore->connectModeTreeParent(satsystem::com::SUBSYSTEM); diff --git a/cmake/Zynq7020CrossCompileConfig.cmake b/cmake/Zynq7020CrossCompileConfig.cmake index be6702a1..5e269f1a 100644 --- a/cmake/Zynq7020CrossCompileConfig.cmake +++ b/cmake/Zynq7020CrossCompileConfig.cmake @@ -40,8 +40,8 @@ set(CROSS_COMPILE_OBJCOPY "${CROSS_COMPILE}-objcopy") set(CROSS_COMPILE_SIZE "${CROSS_COMPILE}-size") # At the very least, cross compile gcc and g++ have to be set! -find_program (CMAKE_C_COMPILER ${CROSS_COMPILE_CC} REQUIRED) -find_program (CMAKE_CXX_COMPILER ${CROSS_COMPILE_CXX} REQUIRED) +find_program (CMAKE_C_COMPILER ${CROSS_COMPILE_CC} HINTS $ENV{CROSS_COMPILE_BIN_PATH} REQUIRED) +find_program (CMAKE_CXX_COMPILER ${CROSS_COMPILE_CXX} HINTS $ENV{CROSS_COMPILE_BIN_PATH} REQUIRED) # Useful utilities, not strictly necessary find_program(CMAKE_SIZE ${CROSS_COMPILE_SIZE}) find_program(CMAKE_OBJCOPY ${CROSS_COMPILE_OBJCOPY}) diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index d40e5fab..2e6ccc19 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -99,6 +99,19 @@ void StarTrackerHandler::doShutDown() { startupState = StartupState::IDLE; bootState = FwBootState::NONE; solutionSet.setReportingEnabled(false); + { + PoolReadGuard pg(&solutionSet); + solutionSet.caliQw.value = 0.0; + solutionSet.caliQx.value = 0.0; + solutionSet.caliQy.value = 0.0; + solutionSet.caliQz.value = 0.0; + solutionSet.isTrustWorthy.value = 0; + solutionSet.setValidity(false, true); + } + { + PoolReadGuard pg(&temperatureSet); + temperatureSet.setValidity(false, true); + } reinitNextSetParam = false; setMode(_MODE_POWER_DOWN); } diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 7133dd88..53babbd5 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -175,7 +175,7 @@ void ThermalController::performControlOperation() { heaterHandler.getAllSwitchStates(heaterSwitchStateArray); { PoolReadGuard pg(&heaterInfo); - std::memcpy(heaterInfo.heaterSwitchState.value, heaterStates.data(), 8); + std::memcpy(heaterInfo.heaterSwitchState.value, heaterSwitchStateArray.data(), 8); { PoolReadGuard pg2(¤tVecPdu2); if (pg.getReadResult() == returnvalue::OK and pg2.getReadResult() == returnvalue::OK) { @@ -1002,7 +1002,7 @@ void ThermalController::copyDevices() { void ThermalController::ctrlAcsBoard() { heater::Switch switchNr = heater::HEATER_2_ACS_BRD; - heater::Switch redSwitchNr = heater::HEATER_0_OBC_BRD; + heater::Switch redSwitchNr = heater::HEATER_3_OBC_BRD; // A side thermalComponent = ACS_BOARD; @@ -1067,7 +1067,7 @@ void ThermalController::ctrlMgt() { sensors[2].first = sensorTemperatures.plpcduHeatspreader.isValid(); sensors[2].second = sensorTemperatures.plpcduHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, mgtLimits); + HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_1_PCDU_PDU, mgtLimits); ctrlComponentTemperature(htrCtx); if (componentAboveUpperLimit and not mgtTooHotFlag) { triggerEvent(tcsCtrl::MGT_OVERHEATING, tempFloatToU32()); @@ -1206,7 +1206,7 @@ void ThermalController::ctrlIfBoard() { sensors[2].first = deviceTemperatures.mgm2SideB.isValid(); sensors[2].second = deviceTemperatures.mgm2SideB.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, ifBoardLimits); + HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_1_PCDU_PDU, ifBoardLimits); ctrlComponentTemperature(htrCtx); // TODO: special event overheating + could go back to safe mode } @@ -1220,7 +1220,7 @@ void ThermalController::ctrlTcsBoard() { sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs1.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, tcsBoardLimits); + HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, tcsBoardLimits); ctrlComponentTemperature(htrCtx); // TODO: special event overheating + could go back to safe mode } @@ -1234,7 +1234,7 @@ void ThermalController::ctrlObc() { sensors[2].first = sensorTemperatures.tmp1075Tcs0.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs0.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, obcLimits); + HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, obcLimits); ctrlComponentTemperature(htrCtx); if (componentAboveUpperLimit and not obcTooHotFlag) { triggerEvent(tcsCtrl::OBC_OVERHEATING, tempFloatToU32()); @@ -1253,7 +1253,7 @@ void ThermalController::ctrlObcIfBoard() { sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs1.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, obcIfBoardLimits); + HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, obcIfBoardLimits); ctrlComponentTemperature(htrCtx); if (componentAboveUpperLimit and not obcTooHotFlag) { triggerEvent(tcsCtrl::OBC_OVERHEATING, tempFloatToU32()); @@ -1288,7 +1288,7 @@ void ThermalController::ctrlPcduP60Board() { sensors[1].first = deviceTemperatures.temp2P60dock.isValid(); sensors[1].second = deviceTemperatures.temp2P60dock.value; numSensors = 2; - HeaterContext htrCtx(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduP60BoardLimits); + HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduP60BoardLimits); ctrlComponentTemperature(htrCtx); if (componentAboveUpperLimit and not pcduSystemTooHotFlag) { triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32()); @@ -1300,7 +1300,7 @@ void ThermalController::ctrlPcduP60Board() { void ThermalController::ctrlPcduAcu() { thermalComponent = PCDUACU; - heater::Switch switchNr = heater::HEATER_3_PCDU_PDU; + heater::Switch switchNr = heater::HEATER_1_PCDU_PDU; heater::Switch redSwitchNr = heater::HEATER_2_ACS_BRD; if (chooseHeater(switchNr, redSwitchNr)) { @@ -1340,7 +1340,7 @@ void ThermalController::ctrlPcduPdu() { sensors[2].first = sensorTemperatures.tmp1075Tcs0.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs0.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduPduLimits); + HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduPduLimits); ctrlComponentTemperature(htrCtx); if (componentAboveUpperLimit and not pcduSystemTooHotFlag) { triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32()); @@ -1361,7 +1361,7 @@ void ThermalController::ctrlPlPcduBoard() { sensors[3].first = sensorTemperatures.plpcduHeatspreader.isValid(); sensors[3].second = sensorTemperatures.plpcduHeatspreader.value; numSensors = 4; - HeaterContext htrCtx(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, plPcduBoardLimits); + HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, plPcduBoardLimits); ctrlComponentTemperature(htrCtx); tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); } @@ -1375,7 +1375,7 @@ void ThermalController::ctrlPlocMissionBoard() { sensors[2].first = sensorTemperatures.dacHeatspreader.isValid(); sensors[2].second = sensorTemperatures.dacHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD, + HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, plocMissionBoardLimits); ctrlComponentTemperature(htrCtx); tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, plocTooHotFlag); @@ -1390,7 +1390,7 @@ void ThermalController::ctrlPlocProcessingBoard() { sensors[2].first = sensorTemperatures.dacHeatspreader.isValid(); sensors[2].second = sensorTemperatures.dacHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD, + HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, plocProcessingBoardLimits); ctrlComponentTemperature(htrCtx); tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, plocTooHotFlag); @@ -1405,7 +1405,7 @@ void ThermalController::ctrlDac() { sensors[2].first = sensorTemperatures.plocHeatspreader.isValid(); sensors[2].second = sensorTemperatures.plocHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD, dacLimits); + HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, dacLimits); ctrlComponentTemperature(htrCtx); tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); } @@ -1631,13 +1631,18 @@ bool ThermalController::selectAndReadSensorTemp(HeaterContext& htrCtx) { bool ThermalController::chooseHeater(heater::Switch& switchNr, heater::Switch redSwitchNr) { bool heaterAvailable = true; - if (heaterHandler.getHealth(switchNr) != HasHealthIF::HEALTHY) { - if (heaterHandler.getHealth(redSwitchNr) == HasHealthIF::HEALTHY) { + HasHealthIF::HealthState mainHealth = heaterHandler.getHealth(switchNr); + HasHealthIF::HealthState redHealth = heaterHandler.getHealth(redSwitchNr); + if (mainHealth != HasHealthIF::HEALTHY) { + if (redHealth == HasHealthIF::HEALTHY) { switchNr = redSwitchNr; redSwitchNrInUse = true; } else { heaterAvailable = false; - triggerEvent(tcsCtrl::NO_HEALTHY_HEATER_AVAILABLE, switchNr, redSwitchNr); + // Special case: Ground might command/do something with the heaters, so prevent spam. + if (not(mainHealth == EXTERNAL_CONTROL and redHealth == EXTERNAL_CONTROL)) { + triggerEvent(tcsCtrl::NO_HEALTHY_HEATER_AVAILABLE, switchNr, redSwitchNr); + } } } else { redSwitchNrInUse = false; diff --git a/mission/power/BpxBatteryHandler.cpp b/mission/power/BpxBatteryHandler.cpp index b4aece40..96253705 100644 --- a/mission/power/BpxBatteryHandler.cpp +++ b/mission/power/BpxBatteryHandler.cpp @@ -1,4 +1,5 @@ #include +#include #include BpxBatteryHandler::BpxBatteryHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, @@ -51,6 +52,9 @@ void BpxBatteryHandler::fillCommandAndReplyMap() { insertInCommandAndReplyMap(bpxBat::RESET_COUNTERS, 1, nullptr, EMPTY_REPLY_LEN); insertInCommandAndReplyMap(bpxBat::CONFIG_CMD, 1, nullptr, EMPTY_REPLY_LEN); insertInCommandAndReplyMap(bpxBat::CONFIG_GET, 1, &cfgSet, CONFIG_GET_REPLY_LEN); + insertInCommandAndReplyMap(bpxBat::CONFIG_SET, 1, nullptr, EMPTY_REPLY_LEN); + insertInCommandAndReplyMap(bpxBat::MAN_HEAT_ON, 1, nullptr, MAN_HEAT_REPLY_LEN); + insertInCommandAndReplyMap(bpxBat::MAN_HEAT_OFF, 1, nullptr, MAN_HEAT_REPLY_LEN); } ReturnValue_t BpxBatteryHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, @@ -155,7 +159,7 @@ ReturnValue_t BpxBatteryHandler::scanForReply(const uint8_t* start, size_t remai case (bpxBat::PING): case (bpxBat::MAN_HEAT_ON): case (bpxBat::MAN_HEAT_OFF): { - if (remainingSize != PING_REPLY_LEN) { + if (remainingSize != MAN_HEAT_REPLY_LEN) { return DeviceHandlerIF::LENGTH_MISSMATCH; } break; diff --git a/mission/power/bpxBattDefs.h b/mission/power/bpxBattDefs.h index 6df87efd..0e167f07 100644 --- a/mission/power/bpxBattDefs.h +++ b/mission/power/bpxBattDefs.h @@ -48,6 +48,7 @@ static constexpr uint32_t CFG_SET_ID = CONFIG_GET; static constexpr size_t GET_HK_REPLY_LEN = 23; static constexpr size_t PING_REPLY_LEN = 3; static constexpr size_t EMPTY_REPLY_LEN = 2; +static constexpr size_t MAN_HEAT_REPLY_LEN = 3; static constexpr size_t CONFIG_GET_REPLY_LEN = 5; static constexpr uint8_t PORT_PING = 1; @@ -219,6 +220,7 @@ class BpxBatteryCfg : public StaticLocalDataSet { if (size < 3) { return SerializeIF::STREAM_TOO_SHORT; } + battheatermode.value = data[0]; battheaterLow.value = data[1]; battheaterHigh.value = data[2]; diff --git a/mission/system/acs/DualLaneAssemblyBase.cpp b/mission/system/acs/DualLaneAssemblyBase.cpp index 7d22fbd1..dc97908f 100644 --- a/mission/system/acs/DualLaneAssemblyBase.cpp +++ b/mission/system/acs/DualLaneAssemblyBase.cpp @@ -183,11 +183,11 @@ void DualLaneAssemblyBase::handleModeTransitionFailed(ReturnValue_t result) { // transition to dual mode. if (not tryingOtherSide) { triggerEvent(CANT_KEEP_MODE, mode, submode); - startTransition(mode, nextSubmode); + startTransition(targetMode, nextSubmode); tryingOtherSide = true; } else { - triggerEvent(transitionOtherSideFailedEvent, mode, targetSubmode); - startTransition(mode, Submodes::DUAL_MODE); + triggerEvent(transitionOtherSideFailedEvent, targetMode, targetSubmode); + startTransition(targetMode, Submodes::DUAL_MODE); } } @@ -205,7 +205,8 @@ bool DualLaneAssemblyBase::checkAndHandleRecovery() { opCode = pwrStateMachine.fsm(); if (opCode == OpCodes::TO_OFF_DONE or opCode == OpCodes::TIMEOUT_OCCURED) { customRecoveryStates = RecoveryCustomStates::POWER_SWITCHING_ON; - pwrStateMachine.start(targetMode, targetSubmode); + // Command power back on in any case. + pwrStateMachine.start(HasModesIF::MODE_ON, targetSubmode); } } if (customRecoveryStates == POWER_SWITCHING_ON) { diff --git a/mission/tcs/HeaterHandler.cpp b/mission/tcs/HeaterHandler.cpp index 630dc92c..de4b600d 100644 --- a/mission/tcs/HeaterHandler.cpp +++ b/mission/tcs/HeaterHandler.cpp @@ -268,6 +268,8 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) { triggerEvent(GPIO_PULL_HIGH_FAILED, result); } else { triggerEvent(HEATER_WENT_ON, heaterIdx, 0); + EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO, + MODE_ON, 0); { MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); heater.switchState = ON; @@ -324,6 +326,8 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switch heaterIdx) { heater.switchState = OFF; } triggerEvent(HEATER_WENT_OFF, heaterIdx, 0); + EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO, + MODE_OFF, 0); // When all switches are off, also main line switch will be turned off if (allSwitchesOff()) { mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF); diff --git a/mission/tcs/Tmp1075Handler.cpp b/mission/tcs/Tmp1075Handler.cpp index df57aa8a..30331e1f 100644 --- a/mission/tcs/Tmp1075Handler.cpp +++ b/mission/tcs/Tmp1075Handler.cpp @@ -86,8 +86,11 @@ ReturnValue_t Tmp1075Handler::scanForReply(const uint8_t *start, size_t remainin ReturnValue_t Tmp1075Handler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { switch (id) { case TMP1075::GET_TEMP: { - int16_t tempValueRaw = 0; - tempValueRaw = packet[0] << 4 | packet[1] >> 4; + // Convert 12 bit MSB first raw temperature to 16 bit first. + int16_t tempValueRaw = static_cast((packet[0] << 8) | packet[1]) >> 4; + // Sign extension to 16 bits: If the sign bit is set, fill up with ones on the left. + tempValueRaw = (packet[0] & 0x80) ? (tempValueRaw | 0xF000) : tempValueRaw; + // 0.0625 is the sensor sensitivity. float tempValue = ((static_cast(tempValueRaw)) * 0.0625); #if OBSW_DEBUG_TMP1075 == 1 sif::info << "Tmp1075 with object id: 0x" << std::hex << getObjectId() diff --git a/mission/tcs/defs.h b/mission/tcs/defs.h index cb1a73e4..747dbc6d 100644 --- a/mission/tcs/defs.h +++ b/mission/tcs/defs.h @@ -5,10 +5,10 @@ namespace heater { enum Switch : uint8_t { - HEATER_0_OBC_BRD, - HEATER_1_PLOC_PROC_BRD, + HEATER_0_PLOC_PROC_BRD, + HEATER_1_PCDU_PDU, HEATER_2_ACS_BRD, - HEATER_3_PCDU_PDU, + HEATER_3_OBC_BRD, HEATER_4_CAMERA, HEATER_5_STR, HEATER_6_DRO, diff --git a/q7s-env-em.sh b/q7s-env-em.sh index 86737627..698b6b23 100755 --- a/q7s-env-em.sh +++ b/q7s-env-em.sh @@ -4,10 +4,10 @@ # custom cross-compiler and sysroot path setups # Adapt the following two entries to your need -CROSS_COMPILE_BIN_PATH="/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin" +export CROSS_COMPILE_BIN_PATH="/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin" export ZYNQ_7020_SYSROOT="/opt/xiphos/sdk/ark/sysroots/cortexa9hf-neon-xiphos-linux-gnueabi" -export PATH=$PATH:${CROSS_COMPILE_BIN_PATH} +export PATH=${CROSS_COMPILE_BIN_PATH}:$PATH export CROSS_COMPILE="arm-linux-gnueabihf" export EIVE_Q7S_EM=1 diff --git a/q7s-env.sh b/q7s-env.sh index 5cf608a0..4b752e17 100755 --- a/q7s-env.sh +++ b/q7s-env.sh @@ -4,10 +4,10 @@ # custom cross-compiler and sysroot path setups # Adapt the following two entries to your need -CROSS_COMPILE_BIN_PATH="/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin" +export CROSS_COMPILE_BIN_PATH="/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin" export ZYNQ_7020_SYSROOT="/opt/xiphos/sdk/ark/sysroots/cortexa9hf-neon-xiphos-linux-gnueabi" -export PATH=$PATH:${CROSS_COMPILE_BIN_PATH} +export PATH=${CROSS_COMPILE_BIN_PATH}:$PATH export CROSS_COMPILE="arm-linux-gnueabihf" # export EIVE_Q7S_EM=1 diff --git a/release_checklist.md b/release_checklist.md index 4c8be066..04a0233c 100644 --- a/release_checklist.md +++ b/release_checklist.md @@ -7,7 +7,9 @@ OBSW Release Checklist 2. Re-run the generators with `generators/gen.py all` 3. Re-run the auto-formatters with the `scripts/auto-formatter.sh` script 4. Verify that the Q7S, Q7S EM and Host build are working -5. Wait for CI/CD results +5. Update `CHANGELOG.md`: Add new `unreleased` section, convert old unreleased section to + header containing version number and release date. +6. Wait for CI/CD results # Post-Release From 3314d079422750ce4ce8560e7a53054577bc17d8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 17 Jun 2023 19:21:28 +0200 Subject: [PATCH 321/506] correct patch version --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 358dd8ab..0aa26b8e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 2) set(OBSW_VERSION_MINOR 0) -set(OBSW_VERSION_REVISION 4) +set(OBSW_VERSION_REVISION 6) # set(CMAKE_VERBOSE TRUE) From 68f84e71ffe9612f0c8b452b0ce676c81ce140c3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 17 Jun 2023 20:04:45 +0200 Subject: [PATCH 322/506] version remaings 2.0.5 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0aa26b8e..ace83e22 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 2) set(OBSW_VERSION_MINOR 0) -set(OBSW_VERSION_REVISION 6) +set(OBSW_VERSION_REVISION 5) # set(CMAKE_VERBOSE TRUE) From c3679f044cf1aae23bc6b0cb71449886ee9828f7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 18 Jun 2023 12:49:22 +0200 Subject: [PATCH 323/506] re-assign active file on file corruption when necessary --- mission/tmtc/PersistentTmStore.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index 8174f8a0..b264b1c8 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -268,6 +268,10 @@ ReturnValue_t PersistentTmStore::getNextDumpPacket(PusTmReader& reader, bool& fi // restore the file dump, but for now do not trust the file. std::error_code e; std::filesystem::remove(dumpParams.dirEntry.path().c_str(), e); + if(dumpParams.dirEntry.path() == activeFile) { + activeFile == std::nullopt; + assignAndOrCreateMostRecentFile(); + } fileHasSwapped = true; return loadNextDumpFile(); } From 07bb293816d5e77851d29f095f902b2dc7b11639 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 18 Jun 2023 13:38:00 +0200 Subject: [PATCH 324/506] tmp1075 workssssss --- bsp_q7s/core/scheduling.cpp | 4 ++-- bsp_q7s/em/emObjectFactory.cpp | 2 +- dummies/Tmp1075Dummy.cpp | 17 ++++++++++++++--- dummies/helperFactory.cpp | 7 +++---- mission/tcs/Tmp1075Handler.cpp | 6 +----- tmtc | 2 +- 6 files changed, 22 insertions(+), 16 deletions(-) diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index f355d384..b257ea13 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -530,8 +530,8 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc, &RR_SCHEDULING); pst::TmpSchedConfig tmpSchedConf; #if OBSW_Q7S_EM == 1 - tmpSchedConf.scheduleTmpDev0 = false; - tmpSchedConf.scheduleTmpDev1 = false; + tmpSchedConf.scheduleTmpDev0 = true; + tmpSchedConf.scheduleTmpDev1 = true; tmpSchedConf.schedulePlPcduDev0 = true; tmpSchedConf.schedulePlPcduDev1 = true; tmpSchedConf.scheduleIfBoardDev = true; diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 52754b8a..a2d5662a 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -68,7 +68,7 @@ void ObjectFactory::produce(void* args) { {objects::TMP1075_HANDLER_IF_BOARD, addresses::TMP1075_IF_BOARD}, }}; createTmpComponents(tmpDevsToAdd); - dummy::Tmp1075Cfg tmpCfg {}; + dummy::Tmp1075Cfg tmpCfg{}; tmpCfg.addTcsBrd0 = true; tmpCfg.addTcsBrd1 = true; tmpCfg.addPlPcdu0 = false; diff --git a/dummies/Tmp1075Dummy.cpp b/dummies/Tmp1075Dummy.cpp index 91a50774..96d1f775 100644 --- a/dummies/Tmp1075Dummy.cpp +++ b/dummies/Tmp1075Dummy.cpp @@ -8,35 +8,46 @@ using namespace returnvalue; Tmp1075Dummy::Tmp1075Dummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie), set(this) {} -void Tmp1075Dummy::doStartUp() { setMode(MODE_NORMAL); } +void Tmp1075Dummy::doStartUp() { setMode(MODE_ON); } void Tmp1075Dummy::doShutDown() { setMode(MODE_OFF); } ReturnValue_t Tmp1075Dummy::buildNormalDeviceCommand(DeviceCommandId_t *id) { return NOTHING_TO_SEND; } -ReturnValue_t Tmp1075Dummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { return OK; } + +ReturnValue_t Tmp1075Dummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + return NOTHING_TO_SEND; +} + ReturnValue_t Tmp1075Dummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return 0; + return NOTHING_TO_SEND; } + ReturnValue_t Tmp1075Dummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { return 0; } + ReturnValue_t Tmp1075Dummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { return 0; } + void Tmp1075Dummy::setTemperature(float temperature, bool valid) { PoolReadGuard pg(&set); set.temperatureCelcius.value = temperature; set.setValidity(valid, true); } + void Tmp1075Dummy::fillCommandAndReplyMap() {} + uint32_t Tmp1075Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 1000; } + ReturnValue_t Tmp1075Dummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(TMP1075::TEMPERATURE_C_TMP1075, new PoolEntry({10.0}, true)); return OK; } + LocalPoolDataSetBase *Tmp1075Dummy::getDataSetHandle(sid_t sid) { return &set; } diff --git a/dummies/helperFactory.cpp b/dummies/helperFactory.cpp index cbe979bf..84d6acc9 100644 --- a/dummies/helperFactory.cpp +++ b/dummies/helperFactory.cpp @@ -211,8 +211,8 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio } if (cfg.tmp1075Cfg.addPlPcdu1) { tmpDummyMap.emplace(objects::TMP1075_HANDLER_PLPCDU_1, - new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_1, - objects::DUMMY_COM_IF, comCookieDummy)); + new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_1, + objects::DUMMY_COM_IF, comCookieDummy)); } if (cfg.tmp1075Cfg.addIfBrd) { tmpDummyMap.emplace(objects::TMP1075_HANDLER_IF_BOARD, @@ -228,9 +228,8 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio for (auto& rtd : rtdSensorDummies) { rtd.second->connectModeTreeParent(*tcsBoardAssy); } - if(tmpSensorDummies.has_value()) { + if (tmpSensorDummies.has_value()) { for (auto& tmp : tmpSensorDummies.value()) { - printf("Hello, connecting parent to %08x\n", tmp.second->getObjectId()); tmp.second->connectModeTreeParent(satsystem::tcs::SUBSYSTEM); } } diff --git a/mission/tcs/Tmp1075Handler.cpp b/mission/tcs/Tmp1075Handler.cpp index 30331e1f..07f3703c 100644 --- a/mission/tcs/Tmp1075Handler.cpp +++ b/mission/tcs/Tmp1075Handler.cpp @@ -11,11 +11,7 @@ Tmp1075Handler::Tmp1075Handler(object_id_t objectId, object_id_t comIF, CookieIF Tmp1075Handler::~Tmp1075Handler() {} -void Tmp1075Handler::doStartUp() { - if (getMode() == _MODE_START_UP) { - setMode(MODE_ON); - } -} +void Tmp1075Handler::doStartUp() { setMode(MODE_ON); } void Tmp1075Handler::doShutDown() { communicationStep = CommunicationStep::START_ADC_CONVERSION; diff --git a/tmtc b/tmtc index 970c8998..b9038f1c 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 970c8998f0bb719ab4b289fa95406d7037b2bb35 +Subproject commit b9038f1c86fb2b3cb9cae5be42fafd678e12de52 From a87a01d0724502830457dbfcd494e15a3de6a1ed Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 18 Jun 2023 14:02:53 +0200 Subject: [PATCH 325/506] make it work for EM --- CMakeLists.txt | 5 +++- bsp_q7s/OBSWConfig.h.in | 3 ++ bsp_q7s/core/ObjectFactory.cpp | 7 +++-- bsp_q7s/em/emObjectFactory.cpp | 10 ++++++- dummies/AcuDummy.cpp | 49 +++++++++++++++++++++++++++++++-- dummies/AcuDummy.h | 8 +++++- dummies/ImtqDummy.cpp | 50 ++++++++++++++++++++++++++++++++-- dummies/ImtqDummy.h | 30 +++++++++++++++++++- dummies/helperFactory.cpp | 33 ++++++++++++++-------- dummies/helperFactory.h | 7 ++++- 10 files changed, 179 insertions(+), 23 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ace83e22..9589b20f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -146,8 +146,11 @@ set(OBSW_ADD_TMP_DEVICES ${INIT_VAL} CACHE STRING "Add TMP devices") set(OBSW_ADD_GOMSPACE_PCDU - ${INIT_VAL} + 1 CACHE STRING "Add GomSpace PCDU modules") +set(OBSW_ADD_GOMSPACE_ACU + ${INIT_VAL} + CACHE STRING "Add GomSpace ACU submodule") set(OBSW_ADD_RW ${INIT_VAL} CACHE STRING "Add RW modules") diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index 3502a7bb..51ed8828 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -22,6 +22,9 @@ #define OBSW_COMMAND_SAFE_MODE_AT_STARTUP 1 #define OBSW_ADD_GOMSPACE_PCDU @OBSW_ADD_GOMSPACE_PCDU@ +// This define is necessary because the EM setup has the P60 dock module, but no ACU on the P60 +// module because it broke. +#define OBSW_ADD_GOMSPACE_ACU @OBSW_ADD_GOMSPACE_ACU@ #define OBSW_ADD_MGT @OBSW_ADD_MGT@ #define OBSW_ADD_BPX_BATTERY_HANDLER @OBSW_ADD_BPX_BATTERY_HANDLER@ #define OBSW_ADD_STAR_TRACKER @OBSW_ADD_STAR_TRACKER@ diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index e8f6fd78..c875c150 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -197,7 +197,6 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI CspCookie* p60DockCspCookie = new CspCookie(P60Dock::MAX_REPLY_SIZE, addresses::P60DOCK, 500); CspCookie* pdu1CspCookie = new CspCookie(PDU::MAX_REPLY_SIZE, addresses::PDU1, 500); CspCookie* pdu2CspCookie = new CspCookie(PDU::MAX_REPLY_SIZE, addresses::PDU2, 500); - CspCookie* acuCspCookie = new CspCookie(ACU::MAX_REPLY_SIZE, addresses::ACU, 500); auto p60Fdir = new GomspacePowerFdir(objects::P60DOCK_HANDLER); P60DockHandler* p60dockhandler = new P60DockHandler(objects::P60DOCK_HANDLER, objects::CSP_COM_IF, @@ -210,10 +209,12 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI auto pdu2Fdir = new GomspacePowerFdir(objects::PDU2_HANDLER); Pdu2Handler* pdu2handler = new Pdu2Handler(objects::PDU2_HANDLER, objects::CSP_COM_IF, pdu2CspCookie, pdu2Fdir, enableHkSets); - +#if OBSW_ADD_GOMSPACE_ACU == 1 + CspCookie* acuCspCookie = new CspCookie(ACU::MAX_REPLY_SIZE, addresses::ACU, 500); auto acuFdir = new GomspacePowerFdir(objects::ACU_HANDLER); ACUHandler* acuhandler = new ACUHandler(objects::ACU_HANDLER, objects::CSP_COM_IF, acuCspCookie, acuFdir, enableHkSets); +#endif auto pcduHandler = new PcduHandler(objects::PCDU_HANDLER, 50); /** @@ -223,7 +224,9 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI p60dockhandler->setModeNormal(); pdu1handler->setModeNormal(); pdu2handler->setModeNormal(); +#if OBSW_ADD_GOMSPACE_ACU == 1 acuhandler->setModeNormal(); +#endif if (pwrSwitcher != nullptr) { *pwrSwitcher = pcduHandler; } diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 05a3fee0..ba992b3b 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -51,15 +51,21 @@ void ObjectFactory::produce(void* args) { // level components. dummy::DummyCfg dummyCfg; dummyCfg.addCoreCtrlCfg = false; + dummyCfg.addCamSwitcherDummy = false; #if OBSW_ADD_SYRLINKS == 1 dummyCfg.addSyrlinksDummies = false; #endif #if OBSW_ADD_GOMSPACE_PCDU == 1 dummyCfg.addPowerDummies = false; + // The ACU broke. + dummyCfg.addOnlyAcuDummy = true; #endif #if OBSW_ADD_ACS_BOARD == 1 dummyCfg.addAcsBoardDummies = false; #endif +#if OBSW_ADD_BPX_BATTERY_HANDLER == 1 + dummyCfg.addBpxBattDummy = false; +#endif PowerSwitchIF* pwrSwitcher = nullptr; #if OBSW_ADD_GOMSPACE_PCDU == 0 @@ -69,7 +75,7 @@ void ObjectFactory::produce(void* args) { #endif satsystem::EIVE_SYSTEM.setI2cRecoveryParams(pwrSwitcher); - dummy::createDummies(dummyCfg, *pwrSwitcher, gpioComIF); + dummy::createDummies(dummyCfg, *pwrSwitcher, gpioComIF, enableHkSets); new CoreController(objects::CORE_CONTROLLER, enableHkSets); @@ -117,6 +123,8 @@ void ObjectFactory::produce(void* args) { createStrComponents(pwrSwitcher); #endif /* OBSW_ADD_STAR_TRACKER == 1 */ + createPayloadComponents(gpioComIF, *pwrSwitcher); + #if OBSW_ADD_CCSDS_IP_CORES == 1 CcsdsIpCoreHandler* ipCoreHandler = nullptr; CcsdsComponentArgs ccsdsArgs(*gpioComIF, *ipcStore, *tmStore, stores, *pusFunnel, *cfdpFunnel, diff --git a/dummies/AcuDummy.cpp b/dummies/AcuDummy.cpp index 7c18f6bf..c9844eb1 100644 --- a/dummies/AcuDummy.cpp +++ b/dummies/AcuDummy.cpp @@ -2,8 +2,11 @@ #include -AcuDummy::AcuDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) - : DeviceHandlerBase(objectId, comif, comCookie) {} +AcuDummy::AcuDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, bool enableHkSets) + : DeviceHandlerBase(objectId, comif, comCookie), + coreHk(this), + auxHk(this), + enableHkSets(enableHkSets) {} AcuDummy::~AcuDummy() {} @@ -37,7 +40,49 @@ uint32_t AcuDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return ReturnValue_t AcuDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + using namespace ACU; + localDataPoolMap.emplace(pool::ACU_CURRENT_IN_CHANNELS, new PoolEntry(6)); + localDataPoolMap.emplace(pool::ACU_VOLTAGE_IN_CHANNELS, new PoolEntry(6)); + + localDataPoolMap.emplace(pool::ACU_VCC, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_VBAT, new PoolEntry({0})); + localDataPoolMap.emplace(ACU::pool::ACU_TEMPERATURES, new PoolEntry({10.0, 10.0, 10.0}, true)); + + localDataPoolMap.emplace(pool::ACU_MPPT_MODE, new PoolEntry({0})); + + localDataPoolMap.emplace(pool::ACU_VBOOST_IN_CHANNELS, new PoolEntry(6)); + localDataPoolMap.emplace(pool::ACU_POWER_IN_CHANNELS, new PoolEntry(6)); + + localDataPoolMap.emplace(pool::ACU_DAC_ENABLES, new PoolEntry(3)); + localDataPoolMap.emplace(pool::ACU_DAC_RAW_CHANNELS, new PoolEntry(6)); + + localDataPoolMap.emplace(pool::ACU_BOOTCAUSE, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_BOOTCNT, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_UPTIME, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_RESET_CAUSE, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_MPPT_TIME, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_MPPT_PERIOD, new PoolEntry({0})); + + localDataPoolMap.emplace(pool::ACU_DEVICES, new PoolEntry(8)); + localDataPoolMap.emplace(pool::ACU_DEVICES_STATUS, new PoolEntry(8)); + + localDataPoolMap.emplace(pool::ACU_WDT_CNT_GND, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_WDT_GND_LEFT, new PoolEntry({0})); + + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), enableHkSets, 30.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(auxHk.getSid(), enableHkSets, 6000.0)); return returnvalue::OK; } + +LocalPoolDataSetBase *AcuDummy::getDataSetHandle(sid_t sid) { + if (sid == coreHk.getSid()) { + return &coreHk; + } else if (sid == auxHk.getSid()) { + return &auxHk; + } + return nullptr; +} diff --git a/dummies/AcuDummy.h b/dummies/AcuDummy.h index d5527222..8d855281 100644 --- a/dummies/AcuDummy.h +++ b/dummies/AcuDummy.h @@ -2,6 +2,7 @@ #define DUMMIES_ACUDUMMY_H_ #include +#include class AcuDummy : public DeviceHandlerBase { public: @@ -11,10 +12,14 @@ class AcuDummy : public DeviceHandlerBase { static const uint8_t SIMPLE_COMMAND_DATA = 1; static const uint8_t PERIODIC_REPLY_DATA = 2; - AcuDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie); + AcuDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, bool enableHkSets); virtual ~AcuDummy(); protected: + ACU::CoreHk coreHk; + ACU::AuxHk auxHk; + bool enableHkSets; + void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; @@ -28,6 +33,7 @@ class AcuDummy : public DeviceHandlerBase { uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) override; + LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; }; #endif /* DUMMIES_ACUDUMMY_H_ */ diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index b2f61bb3..0c8f9076 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -2,8 +2,13 @@ #include -ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) - : DeviceHandlerBase(objectId, comif, comCookie) {} +ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, + power::Switch_t pwrSwitcher, bool enableHkSets) + : DeviceHandlerBase(objectId, comif, comCookie), + setNoTorque(this), + setWithTorque(this), + enableHkSets(enableHkSets), + switcher(pwrSwitcher) {} ImtqDummy::~ImtqDummy() = default; @@ -11,6 +16,15 @@ void ImtqDummy::doStartUp() { setMode(MODE_NORMAL); } void ImtqDummy::doShutDown() { setMode(_MODE_POWER_DOWN); } +ReturnValue_t ImtqDummy::getSwitches(const uint8_t **switches, uint8_t *numberOfSwitches) { + if (switcher != power::NO_SWITCH) { + *numberOfSwitches = 1; + *switches = &switcher; + return returnvalue::OK; + } + return DeviceHandlerBase::NO_SWITCH; +} + ReturnValue_t ImtqDummy::buildNormalDeviceCommand(DeviceCommandId_t *id) { return NOTHING_TO_SEND; } ReturnValue_t ImtqDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { @@ -45,5 +59,37 @@ ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataP localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, new PoolEntry({0})); localDataPoolMap.emplace(imtq::DIPOLES_ID, new PoolEntry({0, 0, 0})); localDataPoolMap.emplace(imtq::CURRENT_TORQUE_DURATION, new PoolEntry({0})); + + // ENG HK No Torque + localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::DIGITAL_CURRENT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::ANALOG_CURRENT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::COIL_CURRENTS, &coilCurrentsMilliampsNoTorque); + localDataPoolMap.emplace(imtq::COIL_TEMPERATURES, &coilTempsNoTorque); + localDataPoolMap.emplace(imtq::MCU_TEMPERATURE, new PoolEntry({0})); + + // ENG HK With Torque + localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::DIGITAL_CURRENT_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::ANALOG_CURRENT_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::COIL_CURRENTS_WT, &coilCurrentsMilliampsWithTorque); + localDataPoolMap.emplace(imtq::COIL_TEMPERATURES_WT, &coilTempsWithTorque); + localDataPoolMap.emplace(imtq::MCU_TEMPERATURE_WT, new PoolEntry({0})); + + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(setNoTorque.getSid(), enableHkSets, 30.0)); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(setWithTorque.getSid(), enableHkSets, 30.0)); return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager); } + +LocalPoolDataSetBase *ImtqDummy::getDataSetHandle(sid_t sid) { + if (sid == setNoTorque.getSid()) { + return &setNoTorque; + } else if (sid == setWithTorque.getSid()) { + return &setWithTorque; + } + return nullptr; +} diff --git a/dummies/ImtqDummy.h b/dummies/ImtqDummy.h index 0cfdf518..990df6e0 100644 --- a/dummies/ImtqDummy.h +++ b/dummies/ImtqDummy.h @@ -3,6 +3,8 @@ #include +#include "mission/acs/imtqHelpers.h" + class ImtqDummy : public DeviceHandlerBase { public: static const DeviceCommandId_t SIMPLE_COMMAND = 1; @@ -11,10 +13,35 @@ class ImtqDummy : public DeviceHandlerBase { static const uint8_t SIMPLE_COMMAND_DATA = 1; static const uint8_t PERIODIC_REPLY_DATA = 2; - ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie); + ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, + power::Switch_t pwrSwitcher, bool enableHkSets); ~ImtqDummy() override; protected: + ReturnValue_t getSwitches(const uint8_t **switches, uint8_t *numberOfSwitches) override; + imtq::HkDatasetNoTorque setNoTorque; + imtq::HkDatasetWithTorque setWithTorque; + bool enableHkSets; + + PoolEntry statusMode = PoolEntry({0}); + PoolEntry statusError = PoolEntry({0}); + PoolEntry statusConfig = PoolEntry({0}); + PoolEntry statusUptime = PoolEntry({0}); + + PoolEntry mgmCalEntry = PoolEntry(3); + PoolEntry dipolesPoolEntry = PoolEntry({0, 0, 0}, false); + PoolEntry torqueDurationEntry = PoolEntry({0}, false); + PoolEntry coilCurrentsMilliampsNoTorque = PoolEntry(3); + PoolEntry coilCurrentsMilliampsWithTorque = PoolEntry(3); + PoolEntry coilTempsNoTorque = PoolEntry(3); + PoolEntry coilTempsWithTorque = PoolEntry(3); + PoolEntry mtmRawNoTorque = PoolEntry(3); + PoolEntry actStatusNoTorque = PoolEntry(1); + PoolEntry mtmRawWithTorque = PoolEntry(3); + PoolEntry actStatusWithTorque = PoolEntry(1); + + power::Switch_t switcher = power::NO_SWITCH; + void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; @@ -28,6 +55,7 @@ class ImtqDummy : public DeviceHandlerBase { uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) override; + LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; }; #endif /* DUMMIES_IMTQDUMMY_H_ */ diff --git a/dummies/helperFactory.cpp b/dummies/helperFactory.cpp index 98d2ecfa..157d11d3 100644 --- a/dummies/helperFactory.cpp +++ b/dummies/helperFactory.cpp @@ -42,10 +42,13 @@ #include "mission/system/tree/payloadModeTree.h" #include "mission/tcs/defs.h" -void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpioIF) { +void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpioIF, + bool enableHkSets) { new ComIFDummy(objects::DUMMY_COM_IF); auto* comCookieDummy = new ComCookieDummy(); - new BpxDummy(objects::BPX_BATT_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + if (cfg.addBpxBattDummy) { + new BpxDummy(objects::BPX_BATT_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + } if (cfg.addCoreCtrlCfg) { new CoreControllerDummy(objects::CORE_CONTROLLER); } @@ -72,11 +75,15 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio } auto* imtqAssy = new ImtqAssembly(objects::IMTQ_ASSY); imtqAssy->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); - auto* imtqDummy = new ImtqDummy(objects::IMTQ_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + auto* imtqDummy = new ImtqDummy(objects::IMTQ_HANDLER, objects::DUMMY_COM_IF, comCookieDummy, + power::Switches::PDU1_CH3_MGT_5V, enableHkSets); imtqDummy->enableThermalModule(ThermalStateCfg()); + imtqDummy->setPowerSwitcher(&pwrSwitcher); imtqDummy->connectModeTreeParent(*imtqAssy); - if (cfg.addPowerDummies) { - new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + if (cfg.addOnlyAcuDummy) { + new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy, enableHkSets); + } else if (cfg.addPowerDummies) { + new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy, enableHkSets); new PduDummy(objects::PDU1_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new PduDummy(objects::PDU2_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new P60DockDummy(objects::P60DOCK_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); @@ -195,10 +202,10 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio objects::TMP1075_HANDLER_PLPCDU_0, new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_0, objects::DUMMY_COM_IF, comCookieDummy)); // damaged. - // tmpSensorDummies.emplace( - // objects::TMP1075_HANDLER_PLPCDU_1, - // new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_1, objects::DUMMY_COM_IF, - // comCookieDummy)); + // tmpSensorDummies.emplace( + // objects::TMP1075_HANDLER_PLPCDU_1, + // new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_1, objects::DUMMY_COM_IF, + // comCookieDummy)); tmpSensorDummies.emplace( objects::TMP1075_HANDLER_IF_BOARD, new Tmp1075Dummy(objects::TMP1075_HANDLER_IF_BOARD, objects::DUMMY_COM_IF, comCookieDummy)); @@ -214,9 +221,11 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio tmp.second->connectModeTreeParent(satsystem::tcs::SUBSYSTEM); } } - auto* camSwitcher = - new CamSwitcher(objects::CAM_SWITCHER, pwrSwitcher, power::Switches::PDU2_CH8_PAYLOAD_CAMERA); - camSwitcher->connectModeTreeParent(satsystem::payload::SUBSYSTEM); + if (cfg.addCamSwitcherDummy) { + auto* camSwitcher = new CamSwitcher(objects::CAM_SWITCHER, pwrSwitcher, + power::Switches::PDU2_CH8_PAYLOAD_CAMERA); + camSwitcher->connectModeTreeParent(satsystem::payload::SUBSYSTEM); + } auto* scexDummy = new ScexDummy(objects::SCEX, objects::DUMMY_COM_IF, comCookieDummy); scexDummy->connectModeTreeParent(satsystem::payload::SUBSYSTEM); auto* plPcduDummy = diff --git a/dummies/helperFactory.h b/dummies/helperFactory.h index 2181c79c..bab9d8d8 100644 --- a/dummies/helperFactory.h +++ b/dummies/helperFactory.h @@ -6,17 +6,22 @@ class GpioIF; namespace dummy { +// Default values targeted towards EM. struct DummyCfg { bool addCoreCtrlCfg = true; + // Special variant because the ACU broke. Overrides addPowerDummies, only ACU dummy will be added. + bool addOnlyAcuDummy = false; bool addPowerDummies = true; + bool addBpxBattDummy = true; bool addSyrlinksDummies = true; bool addAcsBoardDummies = true; bool addSusDummies = true; bool addTempSensorDummies = true; bool addRtdComIFDummy = true; bool addPlocDummies = true; + bool addCamSwitcherDummy = false; }; -void createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitch, GpioIF* gpioIF); +void createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitch, GpioIF* gpioIF, bool enableHkSets); } // namespace dummy From 0cd5526de491237e8621b372977e85d331a51919 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 18 Jun 2023 14:19:19 +0200 Subject: [PATCH 326/506] always add TMP devs now --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3c7e1c1e..78657b95 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -150,7 +150,7 @@ set(OBSW_ADD_SYRLINKS 1 CACHE STRING "Add Syrlinks module") set(OBSW_ADD_TMP_DEVICES - ${INIT_VAL} + 1 CACHE STRING "Add TMP devices") set(OBSW_ADD_GOMSPACE_PCDU 1 From ecb8de155065e18d0f1b7827f92a74057e851d52 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 18 Jun 2023 16:23:08 +0200 Subject: [PATCH 327/506] need to verify some things here later.. --- bsp_q7s/em/emObjectFactory.cpp | 2 +- mission/controller/ThermalController.cpp | 27 +++++++++++++----------- mission/controller/ThermalController.h | 5 +++-- mission/genericFactory.cpp | 4 ++-- mission/genericFactory.h | 2 +- tmtc | 2 +- 6 files changed, 23 insertions(+), 19 deletions(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index a2d5662a..9aecc7ba 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -172,6 +172,6 @@ void ObjectFactory::produce(void* args) { createAcsController(true, enableHkSets); HeaterHandler* heaterHandler; createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler); - createThermalController(*heaterHandler); + createThermalController(*heaterHandler, true); satsystem::init(); } diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 53babbd5..9e7b7d24 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -24,9 +24,11 @@ #define LOWER_RW_UPPER_LIMITS 0 ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater, - const std::atomic_bool& tcsBoardShortUnavailable) + const std::atomic_bool& tcsBoardShortUnavailable, + bool pollPcdu1Tmp) : ExtendedControllerBase(objectId), heaterHandler(heater), + pollPcdu1Tmp(pollPcdu1Tmp), sensorTemperatures(this), susTemperatures(this), deviceTemperatures(this), @@ -56,7 +58,7 @@ ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater tmp1075SetTcs1(objects::TMP1075_HANDLER_TCS_1), tmp1075SetPlPcdu0(objects::TMP1075_HANDLER_PLPCDU_0), // damaged - // tmp1075SetPlPcdu1(objects::TMP1075_HANDLER_PLPCDU_1), + tmp1075SetPlPcdu1(objects::TMP1075_HANDLER_PLPCDU_1), tmp1075SetIfBoard(objects::TMP1075_HANDLER_IF_BOARD), susSet0(objects::SUS_0_N_LOC_XFYFZM_PT_XF), susSet1(objects::SUS_1_N_LOC_XBYFZM_PT_XB), @@ -536,19 +538,20 @@ void ThermalController::copySensors() { } } } - // damaged - /* - { - PoolReadGuard pg(&tmp1075SetPlPcdu1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); - if (pg.getReadResult() == returnvalue::OK) { - sensorTemperatures.tmp1075PlPcdu1.value = tmp1075SetPlPcdu1.temperatureCelcius.value; - sensorTemperatures.tmp1075PlPcdu1.setValid(tmp1075SetPlPcdu1.temperatureCelcius.isValid()); - if (not tmp1075SetPlPcdu1.temperatureCelcius.isValid()) { - sensorTemperatures.tmp1075PlPcdu1.value = INVALID_TEMPERATURE; + // damaged on FM, and no dummies for now + + if (pollPcdu1Tmp) { + { + PoolReadGuard pg(&tmp1075SetPlPcdu1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() == returnvalue::OK) { + sensorTemperatures.tmp1075PlPcdu1.value = tmp1075SetPlPcdu1.temperatureCelcius.value; + sensorTemperatures.tmp1075PlPcdu1.setValid(tmp1075SetPlPcdu1.temperatureCelcius.isValid()); + if (not tmp1075SetPlPcdu1.temperatureCelcius.isValid()) { + sensorTemperatures.tmp1075PlPcdu1.value = INVALID_TEMPERATURE; + } } } } - */ { PoolReadGuard pg(&tmp1075SetIfBoard, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() == returnvalue::OK) { diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 4eb41c67..ab5ddd09 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -103,7 +103,7 @@ class ThermalController : public ExtendedControllerBase { static constexpr int16_t SANITY_LIMIT_UPPER_TEMP = 160; ThermalController(object_id_t objectId, HeaterHandler& heater, - const std::atomic_bool& tcsBoardShortUnavailable); + const std::atomic_bool& tcsBoardShortUnavailable, bool pollPcdu1Tmp); ReturnValue_t initialize() override; @@ -142,6 +142,7 @@ class ThermalController : public ExtendedControllerBase { HeaterHandler& heaterHandler; + bool pollPcdu1Tmp; tcsCtrl::SensorTemperatures sensorTemperatures; tcsCtrl::SusTemperatures susTemperatures; tcsCtrl::DeviceTemperatures deviceTemperatures; @@ -173,7 +174,7 @@ class ThermalController : public ExtendedControllerBase { TMP1075::Tmp1075Dataset tmp1075SetTcs1; TMP1075::Tmp1075Dataset tmp1075SetPlPcdu0; // damaged - // TMP1075::Tmp1075Dataset tmp1075SetPlPcdu1; + TMP1075::Tmp1075Dataset tmp1075SetPlPcdu1; TMP1075::Tmp1075Dataset tmp1075SetIfBoard; // SUS diff --git a/mission/genericFactory.cpp b/mission/genericFactory.cpp index cbe78c7f..db3b7282 100644 --- a/mission/genericFactory.cpp +++ b/mission/genericFactory.cpp @@ -305,9 +305,9 @@ void ObjectFactory::createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF& heaterHandler->connectModeTreeParent(satsystem::tcs::SUBSYSTEM); } -void ObjectFactory::createThermalController(HeaterHandler& heaterHandler) { +void ObjectFactory::createThermalController(HeaterHandler& heaterHandler, bool pollPlPcduTmp1) { auto* tcsCtrl = new ThermalController(objects::THERMAL_CONTROLLER, heaterHandler, - tcs::TCS_BOARD_SHORTLY_UNAVAILABLE); + tcs::TCS_BOARD_SHORTLY_UNAVAILABLE, pollPlPcduTmp1); tcsCtrl->connectModeTreeParent(satsystem::tcs::SUBSYSTEM); } void ObjectFactory::createRwAssy(PowerSwitchIF& pwrSwitcher, power::Switch_t theSwitch, diff --git a/mission/genericFactory.h b/mission/genericFactory.h index a3a52704..c9ad778d 100644 --- a/mission/genericFactory.h +++ b/mission/genericFactory.h @@ -49,7 +49,7 @@ void produceGenericObjects(HealthTableIF** healthTable, PusTmFunnel** pusFunnel, void createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF& pwrSwitcher, HeaterHandler*& heaterHandler); -void createThermalController(HeaterHandler& heaterHandler); +void createThermalController(HeaterHandler& heaterHandler, bool pollPlPcduTmp1); void createRwAssy(PowerSwitchIF& pwrSwitcher, power::Switch_t theSwitch, std::array rws, std::array rwIds); void createSusAssy(PowerSwitchIF& pwrSwitcher, std::array suses); diff --git a/tmtc b/tmtc index b9038f1c..79060acf 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit b9038f1c86fb2b3cb9cae5be42fafd678e12de52 +Subproject commit 79060acfb688a8896cf56c27d83da14e5630f091 From 322151acf884dbad3a5df22418112d7f7c655ac7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 18 Jun 2023 16:34:07 +0200 Subject: [PATCH 328/506] tmp fdir bugfix: use strange reply counter --- mission/system/tcs/TmpDevFdir.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/mission/system/tcs/TmpDevFdir.cpp b/mission/system/tcs/TmpDevFdir.cpp index d501dd1a..8123debc 100644 --- a/mission/system/tcs/TmpDevFdir.cpp +++ b/mission/system/tcs/TmpDevFdir.cpp @@ -29,7 +29,9 @@ ReturnValue_t TmpDevFdir::eventReceived(EventMessage* event) { case DeviceHandlerIF::DEVICE_BUILDING_COMMAND_FAILED: // These faults all mean that there were stupid replies from a device. // With now way to do a recovery, set the device to faulty immediately. - setFaulty(event->getEvent()); + if (strangeReplyCount.incrementAndCheck()) { + setFaulty(event->getEvent()); + } break; case DeviceHandlerIF::DEVICE_SENDING_COMMAND_FAILED: case DeviceHandlerIF::DEVICE_REQUESTING_REPLY_FAILED: From 97268af5554438130a8e1406a3a7997c66d7a806 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 18 Jun 2023 17:11:38 +0200 Subject: [PATCH 329/506] another tmp FDIR fix --- mission/system/tcs/TmpDevFdir.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/mission/system/tcs/TmpDevFdir.cpp b/mission/system/tcs/TmpDevFdir.cpp index 8123debc..4034f2e3 100644 --- a/mission/system/tcs/TmpDevFdir.cpp +++ b/mission/system/tcs/TmpDevFdir.cpp @@ -42,7 +42,9 @@ ReturnValue_t TmpDevFdir::eventReceived(EventMessage* event) { break; } // else - setFaulty(event->getEvent()); + if (missedReplyCount.incrementAndCheck()) { + setFaulty(event->getEvent()); + } break; case StorageManagerIF::GET_DATA_FAILED: case StorageManagerIF::STORE_DATA_FAILED: From 8f7d3dd815e4da45cb7b4550c9e158f623c88ff8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 18 Jun 2023 17:24:19 +0200 Subject: [PATCH 330/506] changelog --- CHANGELOG.md | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a6b35942..a4bae081 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,8 +18,17 @@ will consitute of a breaking change warranting a new major release: ## Fixed -- SUS total vector was not reset to being a zero vector during eclipse due to a wrong memcpy +- SUS total vector was not reset to being a zero vector during eclipse due to a wrong `memcpy` length. +- TMP device FDIR + +## Changed + +- Swapped PL and PS I2C, BPX battery and MGT are connected to PS I2C now. + +## Added + +- Added TMP devices for EM build. # [v4.0.0] to be released From df4e657ec3f729af9323c45b63073a084bf56e4e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 18 Jun 2023 17:36:50 +0200 Subject: [PATCH 331/506] create TCS CTRL TMP1075 PLCDU 1 se tdynamically --- bsp_q7s/em/emObjectFactory.cpp | 15 --------------- bsp_q7s/fmObjectFactory.cpp | 2 +- mission/controller/ThermalController.cpp | 20 +++++++++++++------- mission/controller/ThermalController.h | 3 ++- 4 files changed, 16 insertions(+), 24 deletions(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 9aecc7ba..b319c5b6 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -100,21 +100,6 @@ void ObjectFactory::produce(void* args) { new CoreController(objects::CORE_CONTROLLER, enableHkSets); - // Regular FM code, does not work for EM if the hardware is not connected - // createPcduComponents(gpioComIF, &pwrSwitcher); - // createPlPcduComponents(gpioComIF, spiMainComIF, pwrSwitcher); - // createSyrlinksComponents(pwrSwitcher); - // createSunSensorComponents(gpioComIF, spiMainComIF, pwrSwitcher, q7s::SPI_DEFAULT_DEV); - // createRtdComponents(q7s::SPI_DEFAULT_DEV, gpioComIF, pwrSwitcher, spiMainComIF); - // createTmpComponents(); - // createSolarArrayDeploymentComponents(); - // createPayloadComponents(gpioComIF); - // createHeaterComponents(gpioComIF, pwrSwitcher, healthTable); - - // TODO: Careful! Switching this on somehow messes with the communication with the ProASIC - // and will cause xsc_boot_copy commands to always boot to 0 0 - // createRadSensorComponent(gpioComIF); - #if OBSW_ADD_ACS_BOARD == 1 // Still initialize chip select to avoid SPI bus issues. createRadSensorChipSelect(gpioComIF); diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 25fb221c..419b27a8 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -121,7 +121,7 @@ void ObjectFactory::produce(void* args) { #endif /* OBSW_ADD_TEST_CODE == 1 */ createMiscComponents(); - createThermalController(*heaterHandler); + createThermalController(*heaterHandler, false); createAcsController(true, enableHkSets); satsystem::init(); } diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 9e7b7d24..05011b1f 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -57,8 +57,6 @@ ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater tmp1075SetTcs0(objects::TMP1075_HANDLER_TCS_0), tmp1075SetTcs1(objects::TMP1075_HANDLER_TCS_1), tmp1075SetPlPcdu0(objects::TMP1075_HANDLER_PLPCDU_0), - // damaged - tmp1075SetPlPcdu1(objects::TMP1075_HANDLER_PLPCDU_1), tmp1075SetIfBoard(objects::TMP1075_HANDLER_IF_BOARD), susSet0(objects::SUS_0_N_LOC_XFYFZM_PT_XF), susSet1(objects::SUS_1_N_LOC_XBYFZM_PT_XB), @@ -73,6 +71,9 @@ ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater susSet10(objects::SUS_10_N_LOC_XMYBZF_PT_ZF), susSet11(objects::SUS_11_R_LOC_XBYMZB_PT_ZB), tcsBrdShortlyUnavailable(tcsBoardShortUnavailable) { + if (pollPcdu1Tmp) { + tmp1075SetPlPcdu1 = new TMP1075::Tmp1075Dataset(objects::TMP1075_HANDLER_PLPCDU_1); + } resetSensorsArray(); } @@ -539,14 +540,13 @@ void ThermalController::copySensors() { } } // damaged on FM, and no dummies for now - if (pollPcdu1Tmp) { { - PoolReadGuard pg(&tmp1075SetPlPcdu1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + PoolReadGuard pg(tmp1075SetPlPcdu1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() == returnvalue::OK) { - sensorTemperatures.tmp1075PlPcdu1.value = tmp1075SetPlPcdu1.temperatureCelcius.value; - sensorTemperatures.tmp1075PlPcdu1.setValid(tmp1075SetPlPcdu1.temperatureCelcius.isValid()); - if (not tmp1075SetPlPcdu1.temperatureCelcius.isValid()) { + sensorTemperatures.tmp1075PlPcdu1.value = tmp1075SetPlPcdu1->temperatureCelcius.value; + sensorTemperatures.tmp1075PlPcdu1.setValid(tmp1075SetPlPcdu1->temperatureCelcius.isValid()); + if (not tmp1075SetPlPcdu1->temperatureCelcius.isValid()) { sensorTemperatures.tmp1075PlPcdu1.value = INVALID_TEMPERATURE; } } @@ -1831,6 +1831,12 @@ void ThermalController::heaterSwitchHelperAllOff() { } } +ThermalController::~ThermalController() { + if (tmp1075SetPlPcdu1 != nullptr) { + delete tmp1075SetPlPcdu1; + } +} + void ThermalController::tooHotHandlerWhichClearsOneShotFlag(object_id_t object, bool& oneShotFlag) { // Clear the one shot flag is the component is in acceptable temperature range. if (not tooHotHandler(object, oneShotFlag) and not componentAboveUpperLimit) { diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index ab5ddd09..bcbfc004 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -104,6 +104,7 @@ class ThermalController : public ExtendedControllerBase { ThermalController(object_id_t objectId, HeaterHandler& heater, const std::atomic_bool& tcsBoardShortUnavailable, bool pollPcdu1Tmp); + virtual ~ThermalController(); ReturnValue_t initialize() override; @@ -174,7 +175,7 @@ class ThermalController : public ExtendedControllerBase { TMP1075::Tmp1075Dataset tmp1075SetTcs1; TMP1075::Tmp1075Dataset tmp1075SetPlPcdu0; // damaged - TMP1075::Tmp1075Dataset tmp1075SetPlPcdu1; + TMP1075::Tmp1075Dataset* tmp1075SetPlPcdu1; TMP1075::Tmp1075Dataset tmp1075SetIfBoard; // SUS From 0872fad7dc8a0a099567f0a13d520870497015f2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 19 Jun 2023 09:57:01 +0200 Subject: [PATCH 332/506] make host build compile as well --- bsp_hosted/ObjectFactory.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 3d618cdb..5ddc4d2f 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -62,6 +62,10 @@ void ObjectFactory::produce(void* args) { StorageManagerIF* tmStore; StorageManagerIF* ipcStore; PersistentTmStores persistentStores; + bool enableHkSets = false; + #if OBSW_ENABLE_PERIODIC_HK == 1 + enableHkSets = true; + #endif auto sdcMan = new DummySdCardManager("/tmp"); ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel, *sdcMan, &ipcStore, &tmStore, persistentStores); @@ -100,7 +104,7 @@ void ObjectFactory::produce(void* args) { #endif dummy::DummyCfg cfg; - dummy::createDummies(cfg, *dummySwitcher, dummyGpioIF); + dummy::createDummies(cfg, *dummySwitcher, dummyGpioIF, enableHkSets); HeaterHandler* heaterHandler = nullptr; // new ThermalController(objects::THERMAL_CONTROLLER); From 7cc13d20248baf86a15b326fedd74dc8cc69ba4d Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 16 Jun 2023 21:16:39 +0200 Subject: [PATCH 333/506] little cleanup --- mission/controller/acs/SusConverter.cpp | 47 ++++++++++--------------- mission/controller/acs/SusConverter.h | 26 +++++++------- 2 files changed, 31 insertions(+), 42 deletions(-) diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 1a645270..0568ef68 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -1,54 +1,48 @@ #include "SusConverter.h" -#include -#include #include #include -#include - bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) { - if (susChannel[0] <= susChannelValueCheckLow || susChannel[0] > susChannelValueCheckHigh || + if (susChannel[0] <= SUS_CHANNEL_VALUE_LOW || susChannel[0] > SUS_CHANNEL_VALUE_HIGH || susChannel[0] > susChannel[GNDREF]) { return false; } - if (susChannel[1] <= susChannelValueCheckLow || susChannel[1] > susChannelValueCheckHigh || + if (susChannel[1] <= SUS_CHANNEL_VALUE_LOW || susChannel[1] > SUS_CHANNEL_VALUE_HIGH || susChannel[1] > susChannel[GNDREF]) { return false; }; - if (susChannel[2] <= susChannelValueCheckLow || susChannel[2] > susChannelValueCheckHigh || + if (susChannel[2] <= SUS_CHANNEL_VALUE_LOW || susChannel[2] > SUS_CHANNEL_VALUE_HIGH || susChannel[2] > susChannel[GNDREF]) { return false; }; - if (susChannel[3] <= susChannelValueCheckLow || susChannel[3] > susChannelValueCheckHigh || + if (susChannel[3] <= SUS_CHANNEL_VALUE_LOW || susChannel[3] > SUS_CHANNEL_VALUE_HIGH || susChannel[3] > susChannel[GNDREF]) { return false; }; susChannelValueSum = 4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]); - if ((susChannelValueSum < susChannelValueSumHigh) && - (susChannelValueSum > susChannelValueSumLow)) { + if ((susChannelValueSum < SUS_CHANNEL_SUM_HIGH) && + (susChannelValueSum > SUS_CHANNEL_SUM_LOW)) { return false; }; return true; } void SusConverter::calcAngle(const uint16_t susChannel[6]) { - float xout, yout; float s = 0.03; // s=[mm] gap between diodes - uint8_t d = 5; // d=[mm] edge length of the quadratic aperture - uint8_t h = 1; // h=[mm] distance between diodes and aperture - int ch0, ch1, ch2, ch3; + float d = 5; // d=[mm] edge length of the quadratic aperture + float h = 1; // h=[mm] distance between diodes and aperture // Substract measurement values from GNDREF zero current threshold - ch0 = susChannel[GNDREF] - susChannel[0]; - ch1 = susChannel[GNDREF] - susChannel[1]; - ch2 = susChannel[GNDREF] - susChannel[2]; - ch3 = susChannel[GNDREF] - susChannel[3]; + float ch0 = susChannel[GNDREF] - susChannel[0]; + float ch1 = susChannel[GNDREF] - susChannel[1]; + float ch2 = susChannel[GNDREF] - susChannel[2]; + float ch3 = susChannel[GNDREF] - susChannel[3]; // Calculation of x and y - xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] - yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] + float xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] + float yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] // Calculation of the angles alphaBetaRaw[0] = atan2(xout, h) * (180 / M_PI); //[°] @@ -99,15 +93,10 @@ void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffB float* SusConverter::calculateSunVector() { // Calculate the normalized Sun Vector - sunVectorSensorFrame[0] = -(tan(alphaBetaCalibrated[0] * (M_PI / 180)) / - (sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) + - powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); - sunVectorSensorFrame[1] = -(tan(alphaBetaCalibrated[1] * (M_PI / 180)) / - (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + - powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); - sunVectorSensorFrame[2] = - -(-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + - powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); + sunVectorSensorFrame[0] = -tan(alphaBetaCalibrated[0] * (M_PI / 180)); + sunVectorSensorFrame[1] = -tan(alphaBetaCalibrated[1] * (M_PI / 180)); + sunVectorSensorFrame[2] = 1; + VectorOperations::normalize(sunVectorSensorFrame, sunVectorSensorFrame, 3); return sunVectorSensorFrame; } diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index 046b0ca8..c47a91fa 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -29,19 +29,19 @@ class SusConverter { returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK}; static const uint8_t GNDREF = 4; - uint16_t susChannelValueCheckHigh = - 4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check - uint8_t susChannelValueCheckLow = - 0; //[Bit]low borderline for the channel values of one sun sensor for validity Check - uint16_t susChannelValueSumHigh = - 100; // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by - // the reflection of sunlight from the moon/earth - uint8_t susChannelValueSumLow = - 0; //[Bit]low borderline for check if the sun sensor is illuminated - // by the sun or by the reflection of sunlight from the moon/earth - uint8_t completeCellWidth = 140, - halfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in - // which cell a data point should be + // =2^12[Bit]high borderline for the channel values of one sun sensor for validity Check + static constexpr uint16_t SUS_CHANNEL_VALUE_HIGH = 4096; + // [Bit]low borderline for the channel values of one sun sensor for validity Check + static constexpr uint8_t SUS_CHANNEL_VALUE_LOW = 0; + // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by the + // reflection of sunlight from the moon/earth + static constexpr uint16_t SUS_CHANNEL_SUM_HIGH = 100; + // [Bit]low borderline for check if the sun sensor is illuminated by the sun or by the reflection + // of sunlight from the moon/earth + static constexpr uint8_t SUS_CHANNEL_SUM_LOW = 0; + // [°] Width of the calibration cells --> necessary for checking in + // which cell a data point should be + static const uint8_t completeCellWidth = 140, halfCellWidth = 70; uint16_t susChannelValueSum = 0; AcsParameters acsParameters; From b2a666d432b0eec9d21d3f75c8554e1e0b2d784d Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 19 Jun 2023 16:22:32 +0200 Subject: [PATCH 334/506] added parameter --- mission/controller/acs/AcsParameters.cpp | 2 ++ mission/controller/acs/AcsParameters.h | 1 + 2 files changed, 3 insertions(+) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 03500cc3..8f939850 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -221,6 +221,8 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x23: parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta); break; + case 0x24: + parameterWrapper->set(susHandlingParameters.susBrightnessThreshold); default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 9600ca7e..9e3a8b67 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -766,6 +766,7 @@ class AcsParameters : public HasParametersIF { {116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136, 0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513, -0.000889232196185857, -0.00168429567131815}}; + float susBrightnessThreshold = 0.7; } susHandlingParameters; struct GyrHandlingParameters { From 4b0062e3b29c864687562dd977827baec5a6f22e Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 19 Jun 2023 16:33:04 +0200 Subject: [PATCH 335/506] fixed calculation of sun vector --- mission/controller/acs/SensorProcessing.cpp | 255 ++++++++------------ mission/controller/acs/SusConverter.cpp | 110 +++------ mission/controller/acs/SusConverter.h | 37 +-- 3 files changed, 139 insertions(+), 263 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 4cc15a16..35ca9736 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -210,45 +210,26 @@ void SensorProcessing::processSus( sunIjkModel[0] = cos(eclipticLongitude); sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon); sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon); - if (sus0valid) { - sus0valid = susConverter.checkSunSensorData(sus0Value); - } - if (sus1valid) { - sus1valid = susConverter.checkSunSensorData(sus1Value); - } - if (sus2valid) { - sus2valid = susConverter.checkSunSensorData(sus2Value); - } - if (sus3valid) { - sus3valid = susConverter.checkSunSensorData(sus3Value); - } - if (sus4valid) { - sus4valid = susConverter.checkSunSensorData(sus4Value); - } - if (sus5valid) { - sus5valid = susConverter.checkSunSensorData(sus5Value); - } - if (sus6valid) { - sus6valid = susConverter.checkSunSensorData(sus6Value); - } - if (sus7valid) { - sus7valid = susConverter.checkSunSensorData(sus7Value); - } - if (sus8valid) { - sus8valid = susConverter.checkSunSensorData(sus8Value); - } - if (sus9valid) { - sus9valid = susConverter.checkSunSensorData(sus9Value); - } - if (sus10valid) { - sus10valid = susConverter.checkSunSensorData(sus10Value); - } - if (sus11valid) { - sus11valid = susConverter.checkSunSensorData(sus11Value); - } - if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid && - !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) { + uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + susBrightness[0] = susConverter.checkSunSensorData(sus0Value); + susBrightness[1] = susConverter.checkSunSensorData(sus1Value); + susBrightness[2] = susConverter.checkSunSensorData(sus2Value); + susBrightness[3] = susConverter.checkSunSensorData(sus3Value); + susBrightness[4] = susConverter.checkSunSensorData(sus4Value); + susBrightness[5] = susConverter.checkSunSensorData(sus5Value); + susBrightness[6] = susConverter.checkSunSensorData(sus6Value); + susBrightness[7] = susConverter.checkSunSensorData(sus7Value); + susBrightness[8] = susConverter.checkSunSensorData(sus8Value); + susBrightness[9] = susConverter.checkSunSensorData(sus9Value); + susBrightness[10] = susConverter.checkSunSensorData(sus10Value); + susBrightness[11] = susConverter.checkSunSensorData(sus11Value); + + bool susValid[12] = {true, true, true, true, true, true, true, true, true, true, true, true}; + bool allInvalid = + susConverter.checkValidity(susValid, susBrightness, susParameters->susBrightnessThreshold); + + if (allInvalid) { { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { @@ -274,118 +255,78 @@ void SensorProcessing::processSus( } return; } - // WARNING: NOT TRANSFORMED IN BODY FRAME YET - // Transformation into Geomtry Frame - float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0}, - sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0}, - sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0}, - sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0}; - if (sus0valid) { - MatrixOperations::multiply( - susParameters->sus0orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha, - susParameters->sus0coeffBeta), - sus0VecBody, 3, 3, 1); - } - if (sus1valid) { - MatrixOperations::multiply( - susParameters->sus1orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha, - susParameters->sus1coeffBeta), - sus1VecBody, 3, 3, 1); - } - if (sus2valid) { - MatrixOperations::multiply( - susParameters->sus2orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha, - susParameters->sus2coeffBeta), - sus2VecBody, 3, 3, 1); - } - if (sus3valid) { - MatrixOperations::multiply( - susParameters->sus3orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha, - susParameters->sus3coeffBeta), - sus3VecBody, 3, 3, 1); - } - if (sus4valid) { - MatrixOperations::multiply( - susParameters->sus4orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha, - susParameters->sus4coeffBeta), - sus4VecBody, 3, 3, 1); - } - if (sus5valid) { - MatrixOperations::multiply( - susParameters->sus5orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha, - susParameters->sus5coeffBeta), - sus5VecBody, 3, 3, 1); - } - if (sus6valid) { - MatrixOperations::multiply( - susParameters->sus6orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha, - susParameters->sus6coeffBeta), - sus6VecBody, 3, 3, 1); - } - if (sus7valid) { - MatrixOperations::multiply( - susParameters->sus7orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha, - susParameters->sus7coeffBeta), - sus7VecBody, 3, 3, 1); - } - if (sus8valid) { - MatrixOperations::multiply( - susParameters->sus8orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha, - susParameters->sus8coeffBeta), - sus8VecBody, 3, 3, 1); - } - if (sus9valid) { - MatrixOperations::multiply( - susParameters->sus9orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha, - susParameters->sus9coeffBeta), - sus9VecBody, 3, 3, 1); - } - if (sus10valid) { - MatrixOperations::multiply( - susParameters->sus10orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha, - susParameters->sus10coeffBeta), - sus10VecBody, 3, 3, 1); - } - if (sus11valid) { - MatrixOperations::multiply( - susParameters->sus11orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha, - susParameters->sus11coeffBeta), - sus11VecBody, 3, 3, 1); - } + float susVecSensor[12][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, + {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + float susVecBody[12][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, + {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - /* ------ Mean Value: susDirEst ------ */ - bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, - sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; - float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0], - sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0], - sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]}, - {sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1], - sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1], - sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]}, - {sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2], - sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2], - sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}}; + if (susValid[0]) { + susConverter.calculateSunVector(susVecSensor[0], sus0Value); + MatrixOperations::multiply(susParameters->sus0orientationMatrix[0], susVecSensor[0], + susVecBody[0], 3, 3, 1); + } + if (susValid[1]) { + susConverter.calculateSunVector(susVecSensor[1], sus1Value); + MatrixOperations::multiply(susParameters->sus1orientationMatrix[0], susVecSensor[1], + susVecBody[1], 3, 3, 1); + } + if (susValid[2]) { + susConverter.calculateSunVector(susVecSensor[2], sus2Value); + MatrixOperations::multiply(susParameters->sus2orientationMatrix[0], susVecSensor[2], + susVecBody[2], 3, 3, 1); + } + if (susValid[3]) { + susConverter.calculateSunVector(susVecSensor[3], sus3Value); + MatrixOperations::multiply(susParameters->sus3orientationMatrix[0], susVecSensor[3], + susVecBody[3], 3, 3, 1); + } + if (susValid[4]) { + susConverter.calculateSunVector(susVecSensor[4], sus4Value); + MatrixOperations::multiply(susParameters->sus4orientationMatrix[0], susVecSensor[4], + susVecBody[4], 3, 3, 1); + } + if (susValid[5]) { + susConverter.calculateSunVector(susVecSensor[5], sus5Value); + MatrixOperations::multiply(susParameters->sus5orientationMatrix[0], susVecSensor[5], + susVecBody[5], 3, 3, 1); + } + if (susValid[6]) { + susConverter.calculateSunVector(susVecSensor[6], sus6Value); + MatrixOperations::multiply(susParameters->sus6orientationMatrix[0], susVecSensor[6], + susVecBody[6], 3, 3, 1); + } + if (susValid[7]) { + susConverter.calculateSunVector(susVecSensor[7], sus7Value); + MatrixOperations::multiply(susParameters->sus7orientationMatrix[0], susVecSensor[7], + susVecBody[7], 3, 3, 1); + } + if (susValid[8]) { + susConverter.calculateSunVector(susVecSensor[8], sus8Value); + MatrixOperations::multiply(susParameters->sus8orientationMatrix[0], susVecSensor[8], + susVecBody[8], 3, 3, 1); + } + if (susValid[9]) { + susConverter.calculateSunVector(susVecSensor[9], sus9Value); + MatrixOperations::multiply(susParameters->sus9orientationMatrix[0], susVecSensor[9], + susVecBody[9], 3, 3, 1); + } + if (susValid[10]) { + susConverter.calculateSunVector(susVecSensor[10], sus10Value); + MatrixOperations::multiply(susParameters->sus10orientationMatrix[0], susVecSensor[10], + susVecBody[10], 3, 3, 1); + } + if (susValid[11]) { + susConverter.calculateSunVector(susVecSensor[11], sus11Value); + MatrixOperations::multiply(susParameters->sus11orientationMatrix[0], susVecSensor[11], + susVecBody[11], 3, 3, 1); + } double susMeanValue[3] = {0, 0, 0}; for (uint8_t i = 0; i < 12; i++) { - if (validIds[i]) { - susMeanValue[0] += susVecBody[0][i]; - susMeanValue[1] += susVecBody[1][i]; - susMeanValue[2] += susVecBody[2][i]; - } + susMeanValue[0] += susVecBody[i][0]; + susMeanValue[1] += susVecBody[i][1]; + susMeanValue[2] += susVecBody[i][2]; } double susVecTot[3] = {0.0, 0.0, 0.0}; VectorOperations::normalize(susMeanValue, susVecTot, 3); @@ -406,29 +347,29 @@ void SensorProcessing::processSus( { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus0vec.value, susVecBody[0], 3 * sizeof(float)); susDataProcessed->sus0vec.setValid(sus0valid); - std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus1vec.value, susVecBody[1], 3 * sizeof(float)); susDataProcessed->sus1vec.setValid(sus1valid); - std::memcpy(susDataProcessed->sus2vec.value, sus2VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus2vec.value, susVecBody[2], 3 * sizeof(float)); susDataProcessed->sus2vec.setValid(sus2valid); - std::memcpy(susDataProcessed->sus3vec.value, sus3VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus3vec.value, susVecBody[3], 3 * sizeof(float)); susDataProcessed->sus3vec.setValid(sus3valid); - std::memcpy(susDataProcessed->sus4vec.value, sus4VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus4vec.value, susVecBody[4], 3 * sizeof(float)); susDataProcessed->sus4vec.setValid(sus4valid); - std::memcpy(susDataProcessed->sus5vec.value, sus5VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus5vec.value, susVecBody[5], 3 * sizeof(float)); susDataProcessed->sus5vec.setValid(sus5valid); - std::memcpy(susDataProcessed->sus6vec.value, sus6VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus6vec.value, susVecBody[6], 3 * sizeof(float)); susDataProcessed->sus6vec.setValid(sus6valid); - std::memcpy(susDataProcessed->sus7vec.value, sus7VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus7vec.value, susVecBody[7], 3 * sizeof(float)); susDataProcessed->sus7vec.setValid(sus7valid); - std::memcpy(susDataProcessed->sus8vec.value, sus8VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus8vec.value, susVecBody[8], 3 * sizeof(float)); susDataProcessed->sus8vec.setValid(sus8valid); - std::memcpy(susDataProcessed->sus9vec.value, sus9VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus9vec.value, susVecBody[9], 3 * sizeof(float)); susDataProcessed->sus9vec.setValid(sus9valid); - std::memcpy(susDataProcessed->sus10vec.value, sus10VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus10vec.value, susVecBody[10], 3 * sizeof(float)); susDataProcessed->sus10vec.setValid(sus10valid); - std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus11vec.value, susVecBody[11], 3 * sizeof(float)); susDataProcessed->sus11vec.setValid(sus11valid); std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double)); susDataProcessed->susVecTot.setValid(true); diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 0568ef68..31cc0371 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -1,39 +1,51 @@ #include "SusConverter.h" -#include #include -bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) { +uint64_t SusConverter::checkSunSensorData(const uint16_t susChannel[6]) { if (susChannel[0] <= SUS_CHANNEL_VALUE_LOW || susChannel[0] > SUS_CHANNEL_VALUE_HIGH || susChannel[0] > susChannel[GNDREF]) { - return false; + return 0; } if (susChannel[1] <= SUS_CHANNEL_VALUE_LOW || susChannel[1] > SUS_CHANNEL_VALUE_HIGH || susChannel[1] > susChannel[GNDREF]) { - return false; + return 0; }; if (susChannel[2] <= SUS_CHANNEL_VALUE_LOW || susChannel[2] > SUS_CHANNEL_VALUE_HIGH || susChannel[2] > susChannel[GNDREF]) { - return false; + return 0; }; if (susChannel[3] <= SUS_CHANNEL_VALUE_LOW || susChannel[3] > SUS_CHANNEL_VALUE_HIGH || susChannel[3] > susChannel[GNDREF]) { - return false; + return 0; }; - susChannelValueSum = + uint64_t susChannelValueSum = 4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]); - if ((susChannelValueSum < SUS_CHANNEL_SUM_HIGH) && - (susChannelValueSum > SUS_CHANNEL_SUM_LOW)) { - return false; + if (susChannelValueSum < SUS_ALBEDO_CHECK) { + return 0; }; - return true; + return susChannelValueSum; } -void SusConverter::calcAngle(const uint16_t susChannel[6]) { - float s = 0.03; // s=[mm] gap between diodes - float d = 5; // d=[mm] edge length of the quadratic aperture - float h = 1; // h=[mm] distance between diodes and aperture +bool SusConverter::checkValidity(bool* susValid, const uint64_t brightness[12], + const float threshold) { + uint8_t maxBrightness = 0; + VectorOperations::maxValue(brightness, 12, &maxBrightness); + if (brightness[maxBrightness] == 0) { + return true; + } + for (uint8_t idx = 0; idx < 12; idx++) { + if ((idx != maxBrightness) and (brightness[idx] < threshold * brightness[maxBrightness])) { + susValid[idx] = false; + continue; + } + susValid[idx] = true; + } + return false; +} + +void SusConverter::calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]) { // Substract measurement values from GNDREF zero current threshold float ch0 = susChannel[GNDREF] - susChannel[0]; float ch1 = susChannel[GNDREF] - susChannel[1]; @@ -41,70 +53,12 @@ void SusConverter::calcAngle(const uint16_t susChannel[6]) { float ch3 = susChannel[GNDREF] - susChannel[3]; // Calculation of x and y - float xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] - float yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] + float xout = ((D - S) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] + float yout = ((D - S) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] // Calculation of the angles - alphaBetaRaw[0] = atan2(xout, h) * (180 / M_PI); //[°] - alphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI); //[°] -} - -void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) { - uint8_t index, k, l; - - // while loop iterates above all calibration cells to use the different calibration functions in - // each cell - k = 0; - while (k < 3) { - k++; - l = 0; - while (l < 3) { - l++; - // if-condition to check in which cell the data point has to be - if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3.)) - halfCellWidth) && - alphaBetaRaw[0] < ((completeCellWidth * (k / 3.)) - halfCellWidth)) && - (alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3.)) - halfCellWidth) && - alphaBetaRaw[1] < ((completeCellWidth * (l / 3.)) - halfCellWidth))) { - index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell - alphaBetaCalibrated[0] = - coeffAlpha[index][0] + coeffAlpha[index][1] * alphaBetaRaw[0] + - coeffAlpha[index][2] * alphaBetaRaw[1] + - coeffAlpha[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] + - coeffAlpha[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] + - coeffAlpha[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] + - coeffAlpha[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] + - coeffAlpha[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] + - coeffAlpha[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] + - coeffAlpha[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1]; //[°] - alphaBetaCalibrated[1] = - coeffBeta[index][0] + coeffBeta[index][1] * alphaBetaRaw[0] + - coeffBeta[index][2] * alphaBetaRaw[1] + - coeffBeta[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] + - coeffBeta[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] + - coeffBeta[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] + - coeffBeta[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] + - coeffBeta[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] + - coeffBeta[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] + - coeffBeta[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1]; //[°] - } - } - } -} - -float* SusConverter::calculateSunVector() { - // Calculate the normalized Sun Vector - sunVectorSensorFrame[0] = -tan(alphaBetaCalibrated[0] * (M_PI / 180)); - sunVectorSensorFrame[1] = -tan(alphaBetaCalibrated[1] * (M_PI / 180)); - sunVectorSensorFrame[2] = 1; + sunVectorSensorFrame[0] = -xout; + sunVectorSensorFrame[1] = -yout; + sunVectorSensorFrame[2] = H; VectorOperations::normalize(sunVectorSensorFrame, sunVectorSensorFrame, 3); - - return sunVectorSensorFrame; -} - -float* SusConverter::getSunVectorSensorFrame(const uint16_t susChannel[6], - const float coeffAlpha[9][10], - const float coeffBeta[9][10]) { - calcAngle(susChannel); - calibration(coeffAlpha, coeffBeta); - return calculateSunVector(); } diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index c47a91fa..db72f498 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -1,8 +1,4 @@ -#ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ -#define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ - -#include -#include +#include #include "AcsParameters.h" @@ -10,24 +6,11 @@ class SusConverter { public: SusConverter() {} - bool checkSunSensorData(const uint16_t susChannel[6]); - - void calcAngle(const uint16_t susChannel[6]); - void calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]); - float* calculateSunVector(); - - float* getSunVectorSensorFrame(const uint16_t susChannel[6], const float coeffAlpha[9][10], - const float coeffBeta[9][10]); + uint64_t checkSunSensorData(const uint16_t susChannel[6]); + bool checkValidity(bool* susValid, const uint64_t brightness[12], const float threshold); + void calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]); private: - float alphaBetaRaw[2]; //[°] - float alphaBetaCalibrated[2]; //[°] - float sunVectorSensorFrame[3]; //[-] - - bool validFlag[12] = {returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, - returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, - returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK}; - static const uint8_t GNDREF = 4; // =2^12[Bit]high borderline for the channel values of one sun sensor for validity Check static constexpr uint16_t SUS_CHANNEL_VALUE_HIGH = 4096; @@ -35,16 +18,14 @@ class SusConverter { static constexpr uint8_t SUS_CHANNEL_VALUE_LOW = 0; // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by the // reflection of sunlight from the moon/earth - static constexpr uint16_t SUS_CHANNEL_SUM_HIGH = 100; + static constexpr uint16_t SUS_ALBEDO_CHECK = 1000; // [Bit]low borderline for check if the sun sensor is illuminated by the sun or by the reflection // of sunlight from the moon/earth static constexpr uint8_t SUS_CHANNEL_SUM_LOW = 0; - // [°] Width of the calibration cells --> necessary for checking in - // which cell a data point should be - static const uint8_t completeCellWidth = 140, halfCellWidth = 70; - uint16_t susChannelValueSum = 0; + + static constexpr float S = 0.03; // S=[mm] gap between diodes + static constexpr float D = 5; // D=[mm] edge length of the quadratic aperture + static constexpr float H = 1; // H=[mm] distance between diodes and aperture AcsParameters acsParameters; }; - -#endif /* MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ */ From 0732218249cb970f25fad443c0802a6b46a16b4a Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 19 Jun 2023 17:05:23 +0200 Subject: [PATCH 336/506] bumped fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 5eb9ee8b..f80c5980 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 5eb9ee8bc1e6f8640cbea46febd11e4b92241881 +Subproject commit f80c5980ea5bf84828a22dc6b4985a8747f85df4 From a660d1d30a4ca33f20e342cb01a8d9d2cbe44afd Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 19 Jun 2023 17:08:05 +0200 Subject: [PATCH 337/506] mini fix --- mission/controller/acs/AcsParameters.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 8f939850..e3226e3b 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -223,6 +223,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, break; case 0x24: parameterWrapper->set(susHandlingParameters.susBrightnessThreshold); + break; default: return INVALID_IDENTIFIER_ID; } From dcf01d822b5ef37aacd9224c91e59c4187d4b5b7 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 19 Jun 2023 17:22:57 +0200 Subject: [PATCH 338/506] only check validity if sensor itself is valid --- mission/controller/acs/SensorProcessing.cpp | 55 +++++++++++++++------ 1 file changed, 40 insertions(+), 15 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 35ca9736..d6c0d602 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -212,20 +212,45 @@ void SensorProcessing::processSus( sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon); uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - susBrightness[0] = susConverter.checkSunSensorData(sus0Value); - susBrightness[1] = susConverter.checkSunSensorData(sus1Value); - susBrightness[2] = susConverter.checkSunSensorData(sus2Value); - susBrightness[3] = susConverter.checkSunSensorData(sus3Value); - susBrightness[4] = susConverter.checkSunSensorData(sus4Value); - susBrightness[5] = susConverter.checkSunSensorData(sus5Value); - susBrightness[6] = susConverter.checkSunSensorData(sus6Value); - susBrightness[7] = susConverter.checkSunSensorData(sus7Value); - susBrightness[8] = susConverter.checkSunSensorData(sus8Value); - susBrightness[9] = susConverter.checkSunSensorData(sus9Value); - susBrightness[10] = susConverter.checkSunSensorData(sus10Value); - susBrightness[11] = susConverter.checkSunSensorData(sus11Value); + if (sus0valid) { + susBrightness[0] = susConverter.checkSunSensorData(sus0Value); + } + if (sus1valid) { + susBrightness[1] = susConverter.checkSunSensorData(sus1Value); + } + if (sus2valid) { + susBrightness[2] = susConverter.checkSunSensorData(sus2Value); + } + if (sus3valid) { + susBrightness[3] = susConverter.checkSunSensorData(sus3Value); + } + if (sus4valid) { + susBrightness[4] = susConverter.checkSunSensorData(sus4Value); + } + if (sus5valid) { + susBrightness[5] = susConverter.checkSunSensorData(sus5Value); + } + if (sus6valid) { + susBrightness[6] = susConverter.checkSunSensorData(sus6Value); + } + if (sus7valid) { + susBrightness[7] = susConverter.checkSunSensorData(sus7Value); + } + if (sus8valid) { + susBrightness[8] = susConverter.checkSunSensorData(sus8Value); + } + if (sus9valid) { + susBrightness[9] = susConverter.checkSunSensorData(sus9Value); + } + if (sus10valid) { + susBrightness[10] = susConverter.checkSunSensorData(sus10Value); + } + if (sus11valid) { + susBrightness[11] = susConverter.checkSunSensorData(sus11Value); + } - bool susValid[12] = {true, true, true, true, true, true, true, true, true, true, true, true}; + bool susValid[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, + sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; bool allInvalid = susConverter.checkValidity(susValid, susBrightness, susParameters->susBrightnessThreshold); @@ -246,8 +271,8 @@ void SensorProcessing::processSus( std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(double)); + std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(double)); susDataProcessed->setValidity(false, true); std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); susDataProcessed->sunIjkModel.setValid(true); From 147c39d539f4b331c68e1db136ddca0dcc0f12d0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 19 Jun 2023 17:34:13 +0200 Subject: [PATCH 339/506] small fix --- bsp_hosted/ObjectFactory.cpp | 6 +++--- mission/controller/acs/SensorProcessing.cpp | 5 +++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 5ddc4d2f..c1361879 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -63,9 +63,9 @@ void ObjectFactory::produce(void* args) { StorageManagerIF* ipcStore; PersistentTmStores persistentStores; bool enableHkSets = false; - #if OBSW_ENABLE_PERIODIC_HK == 1 - enableHkSets = true; - #endif +#if OBSW_ENABLE_PERIODIC_HK == 1 + enableHkSets = true; +#endif auto sdcMan = new DummySdCardManager("/tmp"); ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel, *sdcMan, &ipcStore, &tmStore, persistentStores); diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index d6c0d602..c00f522a 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -259,6 +259,7 @@ void SensorProcessing::processSus( PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { float zeroVec[3] = {0.0, 0.0, 0.0}; + double zeroVecD[3] = {0, 0, 0}; std::memcpy(susDataProcessed->sus0vec.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus1vec.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus2vec.value, zeroVec, 3 * sizeof(float)); @@ -271,8 +272,8 @@ void SensorProcessing::processSus( std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(double)); - std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(double)); + std::memcpy(susDataProcessed->susVecTot.value, zeroVecD, 3 * sizeof(double)); + std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVecD, 3 * sizeof(double)); susDataProcessed->setValidity(false, true); std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); susDataProcessed->sunIjkModel.setValid(true); From 7a119bab6eb693c3ea181755bd3aa3729d4aded5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 19 Jun 2023 17:34:47 +0200 Subject: [PATCH 340/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3d543d0f..669835d9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,8 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +- Fix sun vector calculation + # [v2.0.5] to be released - The dual lane assembly transition failed handler started new transitions towards the current mode From 6ed2fcd9048b7ca240867be3ca13a9bcf55eb2b3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 20 Jun 2023 18:22:43 +0200 Subject: [PATCH 341/506] small changelog tweak --- CHANGELOG.md | 1 - 1 file changed, 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4dacd8fd..49bee587 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -147,7 +147,6 @@ will consitute of a breaking change warranting a new major release: - CFDP funnel did not route packets to live channel VC0 # [v2.0.5] 2023-05-11 ->>>>>>> origin/main - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial From 5e4032032f54d893ffc8078ea62ac8a5699bf693 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 20 Jun 2023 19:39:22 +0200 Subject: [PATCH 342/506] that should do the job --- CMakeLists.txt | 8 ++++++-- bsp_q7s/fmObjectFactory.cpp | 3 ++- mission/com/TmStoreTaskBase.cpp | 30 ++++++++++++++++++++---------- mission/com/VirtualChannel.cpp | 13 +++++++++++-- 4 files changed, 39 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 409a49a0..a671429b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,10 +81,14 @@ else() endif() set(OBSW_ADD_TMTC_TCP_SERVER - ${OBSW_Q7S_EM} + # TODO: Activate TCP server until VC0 issue has been resolved + # ${OBSW_Q7S_EM} + 1 CACHE STRING "Add TCP TMTC Server") set(OBSW_ADD_TMTC_UDP_SERVER - 0 + # TODO: Activate UDP server until VC0 issue has been resolved + # 0 + 1 CACHE STRING "Add UDP TMTC Server") set(OBSW_ADD_MGT ${INIT_VAL} diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 5eeeef59..ba2f9478 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -97,7 +97,8 @@ void ObjectFactory::produce(void* args) { #if OBSW_TM_TO_PTME == 1 if (ccsdsArgs.liveDestination != nullptr) { pusFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); - cfdpFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); + // Deactivated for tests to avoid changes to VC0 usage. + // cfdpFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); } #endif #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ diff --git a/mission/com/TmStoreTaskBase.cpp b/mission/com/TmStoreTaskBase.cpp index 0470dc04..6598225d 100644 --- a/mission/com/TmStoreTaskBase.cpp +++ b/mission/com/TmStoreTaskBase.cpp @@ -94,6 +94,13 @@ void TmStoreTaskBase::cancelDump(DumpContext& ctx, PersistentTmStore& store, boo ReturnValue_t TmStoreTaskBase::handleOneDump(PersistentTmStoreWithTmQueue& store, DumpContext& dumpContext, bool& dumpPerformed) { ReturnValue_t result = returnvalue::OK; + // The PTME might have been reset an transmitter state change, so there is no point in continuing + // the dump. + // TODO: Will be solved in a cleaner way, this is kind of a hack. + if (not channel.isTxOn()) { + cancelDump(dumpContext, store, false); + return returnvalue::FAILED; + } // It is assumed that the PTME will only be locked for a short period (e.g. to change datarate). if (not channel.isBusy() and not ptmeLocked) { performDump(store, dumpContext, dumpPerformed); @@ -131,22 +138,25 @@ ReturnValue_t TmStoreTaskBase::performDump(PersistentTmStoreWithTmQueue& store, dumpContext.ptmeBusyCounter = 0; tmSinkBusyCd.resetTimer(); ReturnValue_t result = store.getNextDumpPacket(tmReader, fileHasSwapped); - if (fileHasSwapped and result == PersistentTmStore::DUMP_DONE) { + if (result != returnvalue::OK) { + sif::error << "PersistentTmStore: Getting next dump packet failed" << std::endl; + } else if (fileHasSwapped or result == PersistentTmStore::DUMP_DONE) { // This can happen if a file is corrupted and the next file swap completes the dump. dumpDoneHandler(); return returnvalue::OK; - } else if (result != returnvalue::OK) { - sif::error << "PersistentTmStore: Getting next dump packet failed" << std::endl; - return result; } dumpedLen = tmReader.getFullPacketLen(); - result = channel.write(tmReader.getFullData(), dumpedLen); - if (result == DirectTmSinkIF::IS_BUSY) { - sif::warning << "PersistentTmStore: Unexpected VC channel busy" << std::endl; - } else if (result != returnvalue::OK) { - sif::warning << "PersistentTmStore: Unexpected VC channel write failure" << std::endl; + // Only write to VC if mode is on, but always confirm the dump. + // If the mode is OFF, it is assumed the PTME is not usable and is not allowed to be written + // (e.g. to confirm a reset or the transmitter is off anyway). + if (mode == MODE_ON) { + result = channel.write(tmReader.getFullData(), dumpedLen); + if (result == DirectTmSinkIF::IS_BUSY) { + sif::warning << "PersistentTmStore: Unexpected VC channel busy" << std::endl; + } else if (result != returnvalue::OK) { + sif::warning << "PersistentTmStore: Unexpected VC channel write failure" << std::endl; + } } - result = store.confirmDump(tmReader, fileHasSwapped); if ((result == PersistentTmStore::DUMP_DONE or result == returnvalue::OK)) { dumpPerformed = true; diff --git a/mission/com/VirtualChannel.cpp b/mission/com/VirtualChannel.cpp index ff3749a9..8e225674 100644 --- a/mission/com/VirtualChannel.cpp +++ b/mission/com/VirtualChannel.cpp @@ -11,14 +11,23 @@ ReturnValue_t VirtualChannel::sendNextTm(const uint8_t* data, size_t size) { } ReturnValue_t VirtualChannel::write(const uint8_t* data, size_t size) { - return ptme.writeToVc(vcId, data, size); + if (txOn) { + return ptme.writeToVc(vcId, data, size); + } + return returnvalue::OK; } uint8_t VirtualChannel::getVcid() const { return vcId; } const char* VirtualChannel::getName() const { return vcName.c_str(); } -bool VirtualChannel::isBusy() const { return ptme.isBusy(vcId); } +bool VirtualChannel::isBusy() const { + // Data is discarded, so channel is not busy. + if (not txOn) { + return false; + } + return ptme.isBusy(vcId); +} void VirtualChannel::cancelTransfer() { ptme.cancelTransfer(vcId); } From 861ad9e62d466641e847933ed5e92ea0cc9c662b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 20 Jun 2023 20:14:08 +0200 Subject: [PATCH 343/506] ithis is better --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a671429b..b35aaa15 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -86,9 +86,9 @@ set(OBSW_ADD_TMTC_TCP_SERVER 1 CACHE STRING "Add TCP TMTC Server") set(OBSW_ADD_TMTC_UDP_SERVER - # TODO: Activate UDP server until VC0 issue has been resolved + # TODO: Activate UDP server on FM until VC0 issue has been resolved # 0 - 1 + ${INIT_VAL} CACHE STRING "Add UDP TMTC Server") set(OBSW_ADD_MGT ${INIT_VAL} From fe1e2364665b14a3d6f122911155b049169b0c2d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 20 Jun 2023 20:36:13 +0200 Subject: [PATCH 344/506] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 268c2e87..0f76cdb3 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 268c2e87c9948af1382e56763c84f19acef76bd7 +Subproject commit 0f76cdb3ba54f5e90a8eee4316c49cf0f581f996 From 94cf42fbeb7d1a24612c654c385a0173088ab670 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 20 Jun 2023 20:48:46 +0200 Subject: [PATCH 345/506] changelog --- CHANGELOG.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 49bee587..cb045b49 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,7 +18,7 @@ will consitute of a breaking change warranting a new major release: # [v4.0.0] to be released -# [v3.1.2] 2023-06-20 +# [v3.2.0] 2023-06-20 ## Fixed @@ -26,6 +26,10 @@ will consitute of a breaking change warranting a new major release: - SUS total vector was not reset to being a zero vector during eclipse due to a wrong memcpy length. +## Changed + +- Reverted all changes related to VC0 handling to avoid FM bug possibly related to FPGA bug. + # [v3.1.1] 2023-06-14 ## Fixed From 695a663a15f1e6ebed40e3421fd619b17b11318d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 20 Jun 2023 20:52:01 +0200 Subject: [PATCH 346/506] and put everything back --- CHANGELOG.md | 4 ++++ CMakeLists.txt | 8 ++------ bsp_q7s/fmObjectFactory.cpp | 3 +-- mission/com/TmStoreTaskBase.cpp | 30 ++++++++++-------------------- mission/com/VirtualChannel.cpp | 13 ++----------- 5 files changed, 19 insertions(+), 39 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index cb045b49..80658938 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,10 @@ will consitute of a breaking change warranting a new major release: # [v4.0.0] to be released +# [v3.3.0] 2023-06-20 + +Like v3.2.0 but without the custom FM changes related to VC0. + # [v3.2.0] 2023-06-20 ## Fixed diff --git a/CMakeLists.txt b/CMakeLists.txt index b35aaa15..409a49a0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,14 +81,10 @@ else() endif() set(OBSW_ADD_TMTC_TCP_SERVER - # TODO: Activate TCP server until VC0 issue has been resolved - # ${OBSW_Q7S_EM} - 1 + ${OBSW_Q7S_EM} CACHE STRING "Add TCP TMTC Server") set(OBSW_ADD_TMTC_UDP_SERVER - # TODO: Activate UDP server on FM until VC0 issue has been resolved - # 0 - ${INIT_VAL} + 0 CACHE STRING "Add UDP TMTC Server") set(OBSW_ADD_MGT ${INIT_VAL} diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index ba2f9478..5eeeef59 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -97,8 +97,7 @@ void ObjectFactory::produce(void* args) { #if OBSW_TM_TO_PTME == 1 if (ccsdsArgs.liveDestination != nullptr) { pusFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); - // Deactivated for tests to avoid changes to VC0 usage. - // cfdpFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); + cfdpFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); } #endif #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ diff --git a/mission/com/TmStoreTaskBase.cpp b/mission/com/TmStoreTaskBase.cpp index 6598225d..0470dc04 100644 --- a/mission/com/TmStoreTaskBase.cpp +++ b/mission/com/TmStoreTaskBase.cpp @@ -94,13 +94,6 @@ void TmStoreTaskBase::cancelDump(DumpContext& ctx, PersistentTmStore& store, boo ReturnValue_t TmStoreTaskBase::handleOneDump(PersistentTmStoreWithTmQueue& store, DumpContext& dumpContext, bool& dumpPerformed) { ReturnValue_t result = returnvalue::OK; - // The PTME might have been reset an transmitter state change, so there is no point in continuing - // the dump. - // TODO: Will be solved in a cleaner way, this is kind of a hack. - if (not channel.isTxOn()) { - cancelDump(dumpContext, store, false); - return returnvalue::FAILED; - } // It is assumed that the PTME will only be locked for a short period (e.g. to change datarate). if (not channel.isBusy() and not ptmeLocked) { performDump(store, dumpContext, dumpPerformed); @@ -138,25 +131,22 @@ ReturnValue_t TmStoreTaskBase::performDump(PersistentTmStoreWithTmQueue& store, dumpContext.ptmeBusyCounter = 0; tmSinkBusyCd.resetTimer(); ReturnValue_t result = store.getNextDumpPacket(tmReader, fileHasSwapped); - if (result != returnvalue::OK) { - sif::error << "PersistentTmStore: Getting next dump packet failed" << std::endl; - } else if (fileHasSwapped or result == PersistentTmStore::DUMP_DONE) { + if (fileHasSwapped and result == PersistentTmStore::DUMP_DONE) { // This can happen if a file is corrupted and the next file swap completes the dump. dumpDoneHandler(); return returnvalue::OK; + } else if (result != returnvalue::OK) { + sif::error << "PersistentTmStore: Getting next dump packet failed" << std::endl; + return result; } dumpedLen = tmReader.getFullPacketLen(); - // Only write to VC if mode is on, but always confirm the dump. - // If the mode is OFF, it is assumed the PTME is not usable and is not allowed to be written - // (e.g. to confirm a reset or the transmitter is off anyway). - if (mode == MODE_ON) { - result = channel.write(tmReader.getFullData(), dumpedLen); - if (result == DirectTmSinkIF::IS_BUSY) { - sif::warning << "PersistentTmStore: Unexpected VC channel busy" << std::endl; - } else if (result != returnvalue::OK) { - sif::warning << "PersistentTmStore: Unexpected VC channel write failure" << std::endl; - } + result = channel.write(tmReader.getFullData(), dumpedLen); + if (result == DirectTmSinkIF::IS_BUSY) { + sif::warning << "PersistentTmStore: Unexpected VC channel busy" << std::endl; + } else if (result != returnvalue::OK) { + sif::warning << "PersistentTmStore: Unexpected VC channel write failure" << std::endl; } + result = store.confirmDump(tmReader, fileHasSwapped); if ((result == PersistentTmStore::DUMP_DONE or result == returnvalue::OK)) { dumpPerformed = true; diff --git a/mission/com/VirtualChannel.cpp b/mission/com/VirtualChannel.cpp index 8e225674..ff3749a9 100644 --- a/mission/com/VirtualChannel.cpp +++ b/mission/com/VirtualChannel.cpp @@ -11,23 +11,14 @@ ReturnValue_t VirtualChannel::sendNextTm(const uint8_t* data, size_t size) { } ReturnValue_t VirtualChannel::write(const uint8_t* data, size_t size) { - if (txOn) { - return ptme.writeToVc(vcId, data, size); - } - return returnvalue::OK; + return ptme.writeToVc(vcId, data, size); } uint8_t VirtualChannel::getVcid() const { return vcId; } const char* VirtualChannel::getName() const { return vcName.c_str(); } -bool VirtualChannel::isBusy() const { - // Data is discarded, so channel is not busy. - if (not txOn) { - return false; - } - return ptme.isBusy(vcId); -} +bool VirtualChannel::isBusy() const { return ptme.isBusy(vcId); } void VirtualChannel::cancelTransfer() { ptme.cancelTransfer(vcId); } From 6ef8c62aca1a2555ab8e2d2a8e1358200880572d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 20 Jun 2023 20:53:49 +0200 Subject: [PATCH 347/506] BPX batt handler --- bsp_q7s/em/emObjectFactory.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 50881756..5270e887 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -70,9 +70,6 @@ void ObjectFactory::produce(void* args) { #if OBSW_ADD_ACS_BOARD == 1 dummyCfg.addAcsBoardDummies = false; #endif -#if OBSW_ADD_BPX_BATTERY_HANDLER == 1 - dummyCfg.addBpxBattDummy = false; -#endif PowerSwitchIF* pwrSwitcher = nullptr; #if OBSW_ADD_GOMSPACE_PCDU == 0 From b655c0356486bc2603e84125ca13e66dc82d4673 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 20 Jun 2023 20:58:31 +0200 Subject: [PATCH 348/506] cmakelists --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b35aaa15..93eb267b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,12 +81,12 @@ else() endif() set(OBSW_ADD_TMTC_TCP_SERVER - # TODO: Activate TCP server until VC0 issue has been resolved + # TODO: Only activate on EM when VC0 issue has been resolved. # ${OBSW_Q7S_EM} 1 CACHE STRING "Add TCP TMTC Server") set(OBSW_ADD_TMTC_UDP_SERVER - # TODO: Activate UDP server on FM until VC0 issue has been resolved + # TODO: Disable completely when VC0 issue has been resolved # 0 ${INIT_VAL} CACHE STRING "Add UDP TMTC Server") From c43d9a5a9aac7f4849d2f520771b94c1d5b72f06 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 21 Jun 2023 13:44:04 +0200 Subject: [PATCH 349/506] bump version --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 93eb267b..5cb9c35b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,8 +10,8 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 3) -set(OBSW_VERSION_MINOR 1) -set(OBSW_VERSION_REVISION 1) +set(OBSW_VERSION_MINOR 2) +set(OBSW_VERSION_REVISION 0) # set(CMAKE_VERBOSE TRUE) From 54328ff357aefa902ccb022b91447c3cd2f00b42 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 21 Jun 2023 13:44:44 +0200 Subject: [PATCH 350/506] date correction --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index cb045b49..e2f59548 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,7 +18,7 @@ will consitute of a breaking change warranting a new major release: # [v4.0.0] to be released -# [v3.2.0] 2023-06-20 +# [v3.2.0] 2023-06-21 ## Fixed From af8b4d5bc8e324e6187f6c78ae1301a936a724f2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 21 Jun 2023 13:53:34 +0200 Subject: [PATCH 351/506] remove commented code --- bsp_q7s/em/emObjectFactory.cpp | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 5270e887..e8cd595d 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -83,21 +83,6 @@ void ObjectFactory::produce(void* args) { new CoreController(objects::CORE_CONTROLLER, enableHkSets); - // Regular FM code, does not work for EM if the hardware is not connected - // createPcduComponents(gpioComIF, &pwrSwitcher); - // createPlPcduComponents(gpioComIF, spiMainComIF, pwrSwitcher); - // createSyrlinksComponents(pwrSwitcher); - // createSunSensorComponents(gpioComIF, spiMainComIF, pwrSwitcher, q7s::SPI_DEFAULT_DEV); - // createRtdComponents(q7s::SPI_DEFAULT_DEV, gpioComIF, pwrSwitcher, spiMainComIF); - // createTmpComponents(); - // createSolarArrayDeploymentComponents(); - // createPayloadComponents(gpioComIF); - // createHeaterComponents(gpioComIF, pwrSwitcher, healthTable); - - // TODO: Careful! Switching this on somehow messes with the communication with the ProASIC - // and will cause xsc_boot_copy commands to always boot to 0 0 - // createRadSensorComponent(gpioComIF); - #if OBSW_ADD_ACS_BOARD == 1 // Still initialize chip select to avoid SPI bus issues. createRadSensorChipSelect(gpioComIF); From 4893af07ae0764cbbef5656a0406474752c95800 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 21 Jun 2023 13:59:15 +0200 Subject: [PATCH 352/506] extend RW dummy --- dummies/RwDummy.cpp | 23 ++++++++++++++++++++--- dummies/RwDummy.h | 6 ++++++ mission/tmtc/PersistentTmStore.cpp | 2 +- 3 files changed, 27 insertions(+), 4 deletions(-) diff --git a/dummies/RwDummy.cpp b/dummies/RwDummy.cpp index 60652ef7..0bf4db42 100644 --- a/dummies/RwDummy.cpp +++ b/dummies/RwDummy.cpp @@ -3,13 +3,24 @@ #include RwDummy::RwDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) - : DeviceHandlerBase(objectId, comif, comCookie) {} + : DeviceHandlerBase(objectId, comif, comCookie), + + statusSet(this), + lastResetStatusSet(this), + tmDataset(this), + rwSpeedActuationSet(*this) {} RwDummy::~RwDummy() {} -void RwDummy::doStartUp() { setMode(MODE_ON); } +void RwDummy::doStartUp() { + statusSet.setReportingEnabled(true); + setMode(MODE_ON); +} -void RwDummy::doShutDown() { setMode(MODE_OFF); } +void RwDummy::doShutDown() { + statusSet.setReportingEnabled(false); + setMode(MODE_OFF); +} ReturnValue_t RwDummy::buildNormalDeviceCommand(DeviceCommandId_t *id) { return NOTHING_TO_SEND; } @@ -74,5 +85,11 @@ ReturnValue_t RwDummy::initializeLocalDataPool(localpool::DataPool &localDataPoo localDataPoolMap.emplace(rws::SPI_BYTES_READ, new PoolEntry({0})); localDataPoolMap.emplace(rws::SPI_REG_OVERRUN_ERRORS, new PoolEntry({0})); localDataPoolMap.emplace(rws::SPI_TOTAL_ERRORS, new PoolEntry({0})); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(statusSet.getSid(), false, 12.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(tmDataset.getSid(), false, 30.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(lastResetStatusSet.getSid(), false, 30.0)); return returnvalue::OK; } diff --git a/dummies/RwDummy.h b/dummies/RwDummy.h index 5673717a..03629937 100644 --- a/dummies/RwDummy.h +++ b/dummies/RwDummy.h @@ -2,6 +2,7 @@ #define DUMMIES_RWDUMMY_H_ #include +#include class RwDummy : public DeviceHandlerBase { public: @@ -15,6 +16,11 @@ class RwDummy : public DeviceHandlerBase { virtual ~RwDummy(); protected: + rws::StatusSet statusSet; + rws::LastResetSatus lastResetStatusSet; + rws::TmDataset tmDataset; + rws::RwSpeedActuationSet rwSpeedActuationSet; + PoolEntry rwSpeed = PoolEntry({0}); PoolEntry rampTime = PoolEntry({10}); diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index b264b1c8..08a7ea45 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -268,7 +268,7 @@ ReturnValue_t PersistentTmStore::getNextDumpPacket(PusTmReader& reader, bool& fi // restore the file dump, but for now do not trust the file. std::error_code e; std::filesystem::remove(dumpParams.dirEntry.path().c_str(), e); - if(dumpParams.dirEntry.path() == activeFile) { + if (dumpParams.dirEntry.path() == activeFile) { activeFile == std::nullopt; assignAndOrCreateMostRecentFile(); } From 3f9bf8e1731d9a06897ee7a34ebf06874a13392b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 21 Jun 2023 14:29:00 +0200 Subject: [PATCH 353/506] make imtq dummy more similar --- dummies/ImtqDummy.cpp | 43 ++++++++++++++++++++++++++++++++++--------- dummies/ImtqDummy.h | 16 +++++++++++++--- 2 files changed, 47 insertions(+), 12 deletions(-) diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index 0c8f9076..8570a9be 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -5,14 +5,19 @@ ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, power::Switch_t pwrSwitcher, bool enableHkSets) : DeviceHandlerBase(objectId, comif, comCookie), - setNoTorque(this), - setWithTorque(this), enableHkSets(enableHkSets), + statusSet(this), + dipoleSet(*this), + rawMtmNoTorque(this), + hkDatasetNoTorque(this), + rawMtmWithTorque(this), + hkDatasetWithTorque(this), + calMtmMeasurementSet(this), switcher(pwrSwitcher) {} ImtqDummy::~ImtqDummy() = default; -void ImtqDummy::doStartUp() { setMode(MODE_NORMAL); } +void ImtqDummy::doStartUp() { setMode(MODE_ON); } void ImtqDummy::doShutDown() { setMode(_MODE_POWER_DOWN); } @@ -79,17 +84,37 @@ ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataP localDataPoolMap.emplace(imtq::MCU_TEMPERATURE_WT, new PoolEntry({0})); poolManager.subscribeForDiagPeriodicPacket( - subdp::DiagnosticsHkPeriodicParams(setNoTorque.getSid(), enableHkSets, 30.0)); + subdp::DiagnosticsHkPeriodicParams(hkDatasetNoTorque.getSid(), enableHkSets, 30.0)); poolManager.subscribeForDiagPeriodicPacket( - subdp::DiagnosticsHkPeriodicParams(setWithTorque.getSid(), enableHkSets, 30.0)); + subdp::DiagnosticsHkPeriodicParams(hkDatasetWithTorque.getSid(), enableHkSets, 30.0)); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(rawMtmNoTorque.getSid(), false, 10.0)); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(rawMtmWithTorque.getSid(), false, 10.0)); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(calMtmMeasurementSet.getSid(), false, 10.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(statusSet.getSid(), false, 10.0)); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(dipoleSet.getSid(), false, 10.0)); return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager); } LocalPoolDataSetBase *ImtqDummy::getDataSetHandle(sid_t sid) { - if (sid == setNoTorque.getSid()) { - return &setNoTorque; - } else if (sid == setWithTorque.getSid()) { - return &setWithTorque; + if (sid == hkDatasetNoTorque.getSid()) { + return &hkDatasetNoTorque; + } else if (sid == dipoleSet.getSid()) { + return &dipoleSet; + } else if (sid == statusSet.getSid()) { + return &statusSet; + } else if (sid == hkDatasetWithTorque.getSid()) { + return &hkDatasetWithTorque; + } else if (sid == rawMtmWithTorque.getSid()) { + return &rawMtmWithTorque; + } else if (sid == calMtmMeasurementSet.getSid()) { + return &calMtmMeasurementSet; + } else if (sid == rawMtmNoTorque.getSid()) { + return &rawMtmNoTorque; } return nullptr; } diff --git a/dummies/ImtqDummy.h b/dummies/ImtqDummy.h index 990df6e0..3495589d 100644 --- a/dummies/ImtqDummy.h +++ b/dummies/ImtqDummy.h @@ -18,11 +18,19 @@ class ImtqDummy : public DeviceHandlerBase { ~ImtqDummy() override; protected: - ReturnValue_t getSwitches(const uint8_t **switches, uint8_t *numberOfSwitches) override; - imtq::HkDatasetNoTorque setNoTorque; - imtq::HkDatasetWithTorque setWithTorque; + bool enableHkSets; + imtq::StatusDataset statusSet; + imtq::DipoleActuationSet dipoleSet; + imtq::RawMtmMeasurementNoTorque rawMtmNoTorque; + imtq::HkDatasetNoTorque hkDatasetNoTorque; + + imtq::RawMtmMeasurementWithTorque rawMtmWithTorque; + imtq::HkDatasetWithTorque hkDatasetWithTorque; + + imtq::CalibratedMtmMeasurementSet calMtmMeasurementSet; + PoolEntry statusMode = PoolEntry({0}); PoolEntry statusError = PoolEntry({0}); PoolEntry statusConfig = PoolEntry({0}); @@ -42,6 +50,8 @@ class ImtqDummy : public DeviceHandlerBase { power::Switch_t switcher = power::NO_SWITCH; + ReturnValue_t getSwitches(const uint8_t **switches, uint8_t *numberOfSwitches) override; + void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; From 295da50bc7693a1d1011da6da410f42d96ee1df8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 21 Jun 2023 17:46:59 +0200 Subject: [PATCH 354/506] sus convert bugfix --- CHANGELOG.md | 4 ++++ mission/controller/acs/SusConverter.h | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e2df9f02..e26296b8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,10 @@ will consitute of a breaking change warranting a new major release: # [v4.0.0] to be released +## Fixed + +- Fixed H parameter in SUS converter from 1 mm to 2.5 mm. + # [v3.3.0] 2023-06-21 Like v3.2.0 but without the custom FM changes related to VC0. diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index db72f498..8a6c279b 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -25,7 +25,7 @@ class SusConverter { static constexpr float S = 0.03; // S=[mm] gap between diodes static constexpr float D = 5; // D=[mm] edge length of the quadratic aperture - static constexpr float H = 1; // H=[mm] distance between diodes and aperture + static constexpr float H = 2.5; // H=[mm] distance between diodes and aperture AcsParameters acsParameters; }; From 2d4a3c0ee260657236cc96d542e9b033b4829196 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 21 Jun 2023 19:01:44 +0200 Subject: [PATCH 355/506] new FW info event --- bsp_hosted/fsfwconfig/events/translateEvents.cpp | 7 +++++-- .../fsfwconfig/objects/translateObjects.cpp | 2 +- bsp_q7s/boardconfig/busConf.h | 1 + bsp_q7s/core/CoreController.cpp | 15 +++++++++++++++ bsp_q7s/core/CoreController.h | 2 ++ generators/bsp_hosted_events.csv | 1 + generators/bsp_q7s_events.csv | 1 + generators/events/translateEvents.cpp | 7 +++++-- generators/objects/translateObjects.cpp | 2 +- linux/fsfwconfig/events/translateEvents.cpp | 7 +++++-- linux/fsfwconfig/objects/translateObjects.cpp | 2 +- mission/sysDefs.h | 6 ++++++ tmtc | 2 +- 13 files changed, 45 insertions(+), 10 deletions(-) diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index 9a93fe4c..1500301f 100644 --- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp +++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 295 translations. + * @brief Auto-generated event translation file. Contains 296 translations. * @details - * Generated on: 2023-05-17 17:15:34 + * Generated on: 2023-06-21 19:01:02 */ #include "translateEvents.h" @@ -277,6 +277,7 @@ const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS"; const char *TRYING_I2C_RECOVERY_STRING = "TRYING_I2C_RECOVERY"; const char *I2C_REBOOT_STRING = "I2C_REBOOT"; const char *PDEC_REBOOT_STRING = "PDEC_REBOOT"; +const char *FIRMWARE_INFO_STRING = "FIRMWARE_INFO"; const char *NO_VALID_SENSOR_TEMPERATURE_STRING = "NO_VALID_SENSOR_TEMPERATURE"; const char *NO_HEALTHY_HEATER_AVAILABLE_STRING = "NO_HEALTHY_HEATER_AVAILABLE"; const char *SYRLINKS_OVERHEATING_STRING = "SYRLINKS_OVERHEATING"; @@ -847,6 +848,8 @@ const char *translateEvents(Event event) { return I2C_REBOOT_STRING; case (14012): return PDEC_REBOOT_STRING; + case (14013): + return FIRMWARE_INFO_STRING; case (14100): return NO_VALID_SENSOR_TEMPERATURE_STRING; case (14101): diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index 34edfa41..d1d0464c 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 171 translations. - * Generated on: 2023-05-17 17:15:34 + * Generated on: 2023-06-21 19:01:02 */ #include "translateObjects.h" diff --git a/bsp_q7s/boardconfig/busConf.h b/bsp_q7s/boardconfig/busConf.h index d5fcd8c3..146386c4 100644 --- a/bsp_q7s/boardconfig/busConf.h +++ b/bsp_q7s/boardconfig/busConf.h @@ -23,6 +23,7 @@ static constexpr char UART_SCEX_DEV[] = "/dev/scex"; static constexpr char UIO_PDEC_REGISTERS[] = "/dev/uio_pdec_regs"; static constexpr char UIO_PTME[] = "/dev/uio_ptme"; static constexpr char UIO_PDEC_CONFIG_MEMORY[] = "/dev/uio_pdec_cfg_mem"; +static constexpr char UIO_SYS_ROM[] = "/dev/uio_sys_rom"; static constexpr char UIO_PDEC_RAM[] = "/dev/uio_pdec_ram"; static constexpr char UIO_PDEC_IRQ[] = "/dev/uio_pdec_irq"; static constexpr int MAP_ID_PTME_CONFIG = 3; diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 76460aa5..0128d961 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include "commonConfig.h" #include "fsfw/serviceinterface/ServiceInterface.h" @@ -22,6 +23,7 @@ #include #include +#include "bsp_q7s/boardconfig/busConf.h" #include "bsp_q7s/fs/SdCardManager.h" #include "bsp_q7s/memory/scratchApi.h" #include "bsp_q7s/xadc/Xadc.h" @@ -185,6 +187,14 @@ ReturnValue_t CoreController::initialize() { if (result != returnvalue::OK) { sif::warning << "Subscribing for GPS GPS_FIX_CHANGE event failed" << std::endl; } + + UioMapper sysRomMapper(q7s::UIO_SYS_ROM); + result = sysRomMapper.getMappedAdress(&mappedSysRomAddr, UioMapper::Permissions::READ_ONLY); + if (result != returnvalue::OK) { + // TODO: This might be a reason to switch to another image.. + sif::error << "Getting mapped SYS ROM UIO address failed" << std::endl; + return ObjectManager::CHILD_INIT_FAILED; + } return returnvalue::OK; } @@ -223,6 +233,11 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ } triggerEvent(VERSION_INFO, p1, p2); + if (mappedSysRomAddr != nullptr) { + uint32_t p1Firmware = *(reinterpret_cast(mappedSysRomAddr)); + uint32_t p2Firmware = *(reinterpret_cast(mappedSysRomAddr) + 1); + triggerEvent(FIRMWARE_INFO, p1Firmware, p2Firmware); + } return HasActionsIF::EXECUTION_FINISHED; } case (ANNOUNCE_BOOT_COUNTS): { diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index 05878d6d..6703e398 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -142,6 +143,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe static constexpr bool BLOCKING_SD_INIT = false; + uint32_t* mappedSysRomAddr = nullptr; SdCardManager* sdcMan = nullptr; MessageQueueIF* eventQueue = nullptr; diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv index 9dc26e07..17592d1d 100644 --- a/generators/bsp_hosted_events.csv +++ b/generators/bsp_hosted_events.csv @@ -271,6 +271,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14010;0x36ba;TRYING_I2C_RECOVERY;HIGH;I2C is unavailable. Trying recovery of I2C bus by power cycling all I2C devices.;mission/sysDefs.h 14011;0x36bb;I2C_REBOOT;HIGH;I2C is unavailable. Recovery did not work, performing full reboot.;mission/sysDefs.h 14012;0x36bc;PDEC_REBOOT;HIGH;PDEC recovery through reset was not possible, performing full reboot.;mission/sysDefs.h +14013;0x36bd;FIRMWARE_INFO;INFO;Version information of the firmware (not OBSW). P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash P2: First four letters of Git SHA is the last byte of P1 is set.;mission/sysDefs.h 14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/tcsDefs.h 14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/tcsDefs.h 14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 9dc26e07..17592d1d 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -271,6 +271,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14010;0x36ba;TRYING_I2C_RECOVERY;HIGH;I2C is unavailable. Trying recovery of I2C bus by power cycling all I2C devices.;mission/sysDefs.h 14011;0x36bb;I2C_REBOOT;HIGH;I2C is unavailable. Recovery did not work, performing full reboot.;mission/sysDefs.h 14012;0x36bc;PDEC_REBOOT;HIGH;PDEC recovery through reset was not possible, performing full reboot.;mission/sysDefs.h +14013;0x36bd;FIRMWARE_INFO;INFO;Version information of the firmware (not OBSW). P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash P2: First four letters of Git SHA is the last byte of P1 is set.;mission/sysDefs.h 14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/tcsDefs.h 14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/tcsDefs.h 14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index 9a93fe4c..1500301f 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 295 translations. + * @brief Auto-generated event translation file. Contains 296 translations. * @details - * Generated on: 2023-05-17 17:15:34 + * Generated on: 2023-06-21 19:01:02 */ #include "translateEvents.h" @@ -277,6 +277,7 @@ const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS"; const char *TRYING_I2C_RECOVERY_STRING = "TRYING_I2C_RECOVERY"; const char *I2C_REBOOT_STRING = "I2C_REBOOT"; const char *PDEC_REBOOT_STRING = "PDEC_REBOOT"; +const char *FIRMWARE_INFO_STRING = "FIRMWARE_INFO"; const char *NO_VALID_SENSOR_TEMPERATURE_STRING = "NO_VALID_SENSOR_TEMPERATURE"; const char *NO_HEALTHY_HEATER_AVAILABLE_STRING = "NO_HEALTHY_HEATER_AVAILABLE"; const char *SYRLINKS_OVERHEATING_STRING = "SYRLINKS_OVERHEATING"; @@ -847,6 +848,8 @@ const char *translateEvents(Event event) { return I2C_REBOOT_STRING; case (14012): return PDEC_REBOOT_STRING; + case (14013): + return FIRMWARE_INFO_STRING; case (14100): return NO_VALID_SENSOR_TEMPERATURE_STRING; case (14101): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index eb2125b4..1ecd5e71 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-05-17 17:15:34 + * Generated on: 2023-06-21 19:01:02 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index 9a93fe4c..1500301f 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 295 translations. + * @brief Auto-generated event translation file. Contains 296 translations. * @details - * Generated on: 2023-05-17 17:15:34 + * Generated on: 2023-06-21 19:01:02 */ #include "translateEvents.h" @@ -277,6 +277,7 @@ const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS"; const char *TRYING_I2C_RECOVERY_STRING = "TRYING_I2C_RECOVERY"; const char *I2C_REBOOT_STRING = "I2C_REBOOT"; const char *PDEC_REBOOT_STRING = "PDEC_REBOOT"; +const char *FIRMWARE_INFO_STRING = "FIRMWARE_INFO"; const char *NO_VALID_SENSOR_TEMPERATURE_STRING = "NO_VALID_SENSOR_TEMPERATURE"; const char *NO_HEALTHY_HEATER_AVAILABLE_STRING = "NO_HEALTHY_HEATER_AVAILABLE"; const char *SYRLINKS_OVERHEATING_STRING = "SYRLINKS_OVERHEATING"; @@ -847,6 +848,8 @@ const char *translateEvents(Event event) { return I2C_REBOOT_STRING; case (14012): return PDEC_REBOOT_STRING; + case (14013): + return FIRMWARE_INFO_STRING; case (14100): return NO_VALID_SENSOR_TEMPERATURE_STRING; case (14101): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index eb2125b4..1ecd5e71 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-05-17 17:15:34 + * Generated on: 2023-06-21 19:01:02 */ #include "translateObjects.h" diff --git a/mission/sysDefs.h b/mission/sysDefs.h index c84c237f..41c7d43d 100644 --- a/mission/sysDefs.h +++ b/mission/sysDefs.h @@ -44,6 +44,8 @@ static constexpr char VERSION_FILE_NAME[] = "version.txt"; static constexpr char REBOOT_FILE_NAME[] = "reboot.txt"; static constexpr char TIME_FILE_NAME[] = "time_backup.txt"; +static constexpr uint32_t SYS_ROM_BASE_ADDR = 0x80000000; + static constexpr ActionId_t ANNOUNCE_VERSION = 1; static constexpr ActionId_t ANNOUNCE_CURRENT_IMAGE = 2; static constexpr ActionId_t ANNOUNCE_BOOT_COUNTS = 3; @@ -113,6 +115,10 @@ static constexpr Event TRYING_I2C_RECOVERY = event::makeEvent(SUBSYSTEM_ID, 10, static constexpr Event I2C_REBOOT = event::makeEvent(SUBSYSTEM_ID, 11, severity::HIGH); //! [EXPORT] : [COMMENT] PDEC recovery through reset was not possible, performing full reboot. static constexpr Event PDEC_REBOOT = event::makeEvent(SUBSYSTEM_ID, 12, severity::HIGH); +//! [EXPORT] : [COMMENT] Version information of the firmware (not OBSW). +//! P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash +//! P2: First four letters of Git SHA is the last byte of P1 is set. +static constexpr Event FIRMWARE_INFO = event::makeEvent(SUBSYSTEM_ID, 13, severity::INFO); class ListDirectoryCmdBase { public: // TODO: Packet definition for clean deserialization diff --git a/tmtc b/tmtc index 5f44cb96..1bb8bea8 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 5f44cb96be1e5b7f1a49571b6420fa85d6bc847a +Subproject commit 1bb8bea8d92fef2c9ec58ea657b04b56635c16dd From 0c394ad34d6d256e6f4fcdd6e49891ad06bee005 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 11:29:30 +0200 Subject: [PATCH 356/506] set temperature invalid properly --- mission/acs/str/StarTrackerHandler.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 1ba73181..23223bfc 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -12,6 +12,8 @@ extern "C" { #include } +#include "fsfw/thermal/tcsDefinitions.h" + #include #include #include @@ -111,6 +113,9 @@ void StarTrackerHandler::doShutDown() { } { PoolReadGuard pg(&temperatureSet); + temperatureSet.fpgaTemperature = thermal::INVALID_TEMPERATURE; + temperatureSet.cmosTemperature = thermal::INVALID_TEMPERATURE; + temperatureSet.mcuTemperature = thermal::INVALID_TEMPERATURE; temperatureSet.setValidity(false, true); } reinitNextSetParam = false; From 3ae7dab8c766e6ae820c0abe214237cf3d3411a0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 11:33:54 +0200 Subject: [PATCH 357/506] those are the most important cases --- mission/acs/SusHandler.cpp | 1 + mission/tcs/Max31865EiveHandler.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/mission/acs/SusHandler.cpp b/mission/acs/SusHandler.cpp index c347020f..6fa4d231 100644 --- a/mission/acs/SusHandler.cpp +++ b/mission/acs/SusHandler.cpp @@ -30,6 +30,7 @@ void SusHandler::doStartUp() { void SusHandler::doShutDown() { if (internalState != InternalState::SHUTDOWN) { PoolReadGuard pg(&dataset); + dataset.tempC = thermal::INVALID_TEMPERATURE; dataset.setValidity(false, true); internalState = InternalState::SHUTDOWN; commandExecuted = false; diff --git a/mission/tcs/Max31865EiveHandler.cpp b/mission/tcs/Max31865EiveHandler.cpp index 599071ef..a3c1dce1 100644 --- a/mission/tcs/Max31865EiveHandler.cpp +++ b/mission/tcs/Max31865EiveHandler.cpp @@ -37,6 +37,8 @@ void Max31865EiveHandler::doShutDown() { transitionOk = false; } if (state == InternalState::INACTIVE and transitionOk) { + sensorDataset.temperatureCelcius = thermal::INVALID_TEMPERATURE; + sensorDataset.setValidity(false, true); updatePeriodicReply(false, EiveMax31855::RtdCommands::EXCHANGE_SET_ID); setMode(MODE_OFF); } From add083135ee6930046772886bd4ac9657182679a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 11:35:46 +0200 Subject: [PATCH 358/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5f65ab7e..6cb1a9fa 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -25,6 +25,8 @@ will consitute of a breaking change warranting a new major release: - CFDP low level protocol bugfix. Requires fsfw update and tmtc update. - Important bugfixes for PTME. See `q7s-package` CHANGELOG. +- TCS fixes: Set temperature values to invalid value for MAX31865 RTD handler, SUS handler + and STR handler. Also set dataset to invakid for RTD handler. ## Changed From db3a4955c21d0ced957ae09a2bfe06a3264d9849 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 14:06:34 +0200 Subject: [PATCH 359/506] core ctrl provides more info on reboot --- bsp_q7s/core/CoreController.cpp | 49 ++++++++++++++++++++------------- bsp_q7s/core/CoreController.h | 2 ++ 2 files changed, 32 insertions(+), 19 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 0128d961..9c6e2305 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -171,6 +171,8 @@ ReturnValue_t CoreController::initialize() { sdStateMachine(); triggerEvent(core::REBOOT_SW, CURRENT_CHIP, CURRENT_COPY); + announceCurrentImageInfo(); + announceVersionInfo(); EventManagerIF *eventManager = ObjectManager::instance()->get(objects::EVENT_MANAGER); if (eventManager == nullptr or eventQueue == nullptr) { @@ -220,24 +222,7 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ using namespace core; switch (actionId) { case (ANNOUNCE_VERSION): { - uint32_t p1 = (common::OBSW_VERSION_MAJOR << 24) | (common::OBSW_VERSION_MINOR << 16) | - (common::OBSW_VERSION_REVISION << 8); - uint32_t p2 = 0; - if (strcmp("", common::OBSW_VERSION_CST_GIT_SHA1) != 0) { - p1 |= 1; - auto shaAsStr = std::string(common::OBSW_VERSION_CST_GIT_SHA1); - size_t posDash = shaAsStr.find("-"); - auto gitHash = shaAsStr.substr(posDash + 2, 4); - // Only copy first 4 letters of git hash - memcpy(&p2, gitHash.c_str(), 4); - } - - triggerEvent(VERSION_INFO, p1, p2); - if (mappedSysRomAddr != nullptr) { - uint32_t p1Firmware = *(reinterpret_cast(mappedSysRomAddr)); - uint32_t p2Firmware = *(reinterpret_cast(mappedSysRomAddr) + 1); - triggerEvent(FIRMWARE_INFO, p1Firmware, p2Firmware); - } + announceVersionInfo(); return HasActionsIF::EXECUTION_FINISHED; } case (ANNOUNCE_BOOT_COUNTS): { @@ -245,7 +230,7 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ return HasActionsIF::EXECUTION_FINISHED; } case (ANNOUNCE_CURRENT_IMAGE): { - triggerEvent(CURRENT_IMAGE_INFO, CURRENT_CHIP, CURRENT_COPY); + announceCurrentImageInfo(); return HasActionsIF::EXECUTION_FINISHED; } case (LIST_DIRECTORY_INTO_FILE): { @@ -2420,6 +2405,32 @@ void CoreController::dirListingDumpHandler() { } } +void CoreController::announceVersionInfo() { + using namespace core; + uint32_t p1 = (common::OBSW_VERSION_MAJOR << 24) | (common::OBSW_VERSION_MINOR << 16) | + (common::OBSW_VERSION_REVISION << 8); + uint32_t p2 = 0; + if (strcmp("", common::OBSW_VERSION_CST_GIT_SHA1) != 0) { + p1 |= 1; + auto shaAsStr = std::string(common::OBSW_VERSION_CST_GIT_SHA1); + size_t posDash = shaAsStr.find("-"); + auto gitHash = shaAsStr.substr(posDash + 2, 4); + // Only copy first 4 letters of git hash + memcpy(&p2, gitHash.c_str(), 4); + } + + triggerEvent(VERSION_INFO, p1, p2); + if (mappedSysRomAddr != nullptr) { + uint32_t p1Firmware = *(reinterpret_cast(mappedSysRomAddr)); + uint32_t p2Firmware = *(reinterpret_cast(mappedSysRomAddr) + 1); + triggerEvent(FIRMWARE_INFO, p1Firmware, p2Firmware); + } +} + +void CoreController::announceCurrentImageInfo() { + triggerEvent(CURRENT_IMAGE_INFO, CURRENT_CHIP, CURRENT_COPY); +} + bool CoreController::isNumber(const std::string &s) { return !s.empty() && std::find_if(s.begin(), s.end(), [](unsigned char c) { return !std::isdigit(c); }) == s.end(); diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index 6703e398..79b486b4 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -289,6 +289,8 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe bool parseRebootFile(std::string path, RebootFile& file); void rewriteRebootFile(RebootFile file); void announceBootCounts(); + void announceVersionInfo(); + void announceCurrentImageInfo(); void readHkData(); void dirListingDumpHandler(); bool isNumber(const std::string& s); From afcc0cc21d735bbefcc7036fedef793f75640329 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 14:07:37 +0200 Subject: [PATCH 360/506] changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index c642b897..f4a92867 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -30,11 +30,14 @@ will consitute of a breaking change warranting a new major release: - Removed PTME busy/ready signals. Those were not used anyway because register reads are used now. - APB bus access busy checking is not done anymore as this is performed by the bus itself now. +- Core controller will now announce version and image information event in addition to reboot + event in the `inititalize` function. ## Added - Added PL I2C reset pin. It is not used for now but could be used for FDIR procedures to restore the PL I2C. +- Core controller now announces firmware version as well when requesting a version info event # [v3.3.0] 2023-06-21 From 788fbb72f030326ab989ad9e45205deee124a3ef Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 14:25:11 +0200 Subject: [PATCH 361/506] that should do the job --- CHANGELOG.md | 1 + bsp_q7s/core/CoreController.cpp | 13 ++++++-- bsp_q7s/core/CoreController.h | 55 +++++++++++++++++++++++++++++++++ mission/sysDefs.h | 1 + 4 files changed, 68 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index f4a92867..14402d4a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -32,6 +32,7 @@ will consitute of a breaking change warranting a new major release: - APB bus access busy checking is not done anymore as this is performed by the bus itself now. - Core controller will now announce version and image information event in addition to reboot event in the `inititalize` function. +- Added core controller action to read reboot mechansm information ## Added diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 9c6e2305..137abe2b 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -319,7 +319,6 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ return HasActionsIF::INVALID_PARAMETERS; } std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_FILE; - // Disable the reboot file mechanism parseRebootFile(path, rebootFile); if (data[0] == 0) { rebootFile.enabled = false; @@ -332,6 +331,16 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ } return HasActionsIF::EXECUTION_FINISHED; } + case (READ_REBOOT_MECHANISM_INFO): { + std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_FILE; + parseRebootFile(path, rebootFile); + RebootMechanismPacket packet(rebootFile); + ReturnValue_t result = actionHelper.reportData(commandedBy, actionId, &packet); + if (result != returnvalue::OK) { + return result; + } + return HasActionsIF::EXECUTION_FINISHED; + } case (RESET_REBOOT_COUNTERS): { if (size == 0) { resetRebootCount(xsc::ALL_CHIP, xsc::ALL_COPY); @@ -1998,7 +2007,6 @@ bool CoreController::parseRebootFile(std::string path, RebootFile &rf) { void CoreController::resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy) { std::string path = currMntPrefix + REBOOT_FILE; - // Disable the reboot file mechanism parseRebootFile(path, rebootFile); if (tgtChip == xsc::ALL_CHIP and tgtCopy == xsc::ALL_COPY) { rebootFile.img00Cnt = 0; @@ -2428,6 +2436,7 @@ void CoreController::announceVersionInfo() { } void CoreController::announceCurrentImageInfo() { + using namespace core; triggerEvent(CURRENT_IMAGE_INFO, CURRENT_CHIP, CURRENT_COPY); } diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index 79b486b4..7ab80aa7 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -45,6 +45,61 @@ struct RebootFile { xsc::Copy mechanismNextCopy = xsc::Copy::NO_COPY; }; +class RebootMechanismPacket : public SerialLinkedListAdapter { + public: + RebootMechanismPacket(RebootFile& rf) { + enabled = rf.enabled; + maxCount = rf.maxCount; + img00Count = rf.img00Cnt; + img01Count = rf.img01Cnt; + img10Count = rf.img10Cnt; + img11Count = rf.img11Cnt; + img00Lock = rf.img00Lock; + img01Lock = rf.img01Lock; + img10Lock = rf.img10Lock; + img11Lock = rf.img11Lock; + lastChip = static_cast(rf.lastChip); + lastCopy = static_cast(rf.lastCopy); + nextChip = static_cast(rf.mechanismNextChip); + nextCopy = static_cast(rf.mechanismNextCopy); + setLinks(); + } + + private: + void setLinks() { + setStart(&enabled); + enabled.setNext(&maxCount); + maxCount.setNext(&img00Count); + img00Count.setNext(&img01Count); + img01Count.setNext(&img10Count); + img10Count.setNext(&img11Count); + img11Count.setNext(&img00Lock); + img00Lock.setNext(&img01Lock); + img01Lock.setNext(&img10Lock); + img10Lock.setNext(&img11Lock); + img11Lock.setNext(&lastChip); + lastChip.setNext(&lastCopy); + lastCopy.setNext(&nextChip); + nextChip.setNext(&nextCopy); + setLast(&nextCopy); + } + + SerializeElement enabled = false; + SerializeElement maxCount = 0; + SerializeElement img00Count = 0; + SerializeElement img01Count = 0; + SerializeElement img10Count = 0; + SerializeElement img11Count = 0; + SerializeElement img00Lock = false; + SerializeElement img01Lock = false; + SerializeElement img10Lock = false; + SerializeElement img11Lock = false; + SerializeElement lastChip = 0; + SerializeElement lastCopy = 0; + SerializeElement nextChip = 0; + SerializeElement nextCopy = 0; +}; + class CoreController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { public: enum ParamId : uint8_t { PREF_SD = 0, NUM_IDS }; diff --git a/mission/sysDefs.h b/mission/sysDefs.h index 41c7d43d..4adab118 100644 --- a/mission/sysDefs.h +++ b/mission/sysDefs.h @@ -53,6 +53,7 @@ static constexpr ActionId_t SWITCH_REBOOT_FILE_HANDLING = 5; static constexpr ActionId_t RESET_REBOOT_COUNTERS = 6; static constexpr ActionId_t SWITCH_IMG_LOCK = 7; static constexpr ActionId_t SET_MAX_REBOOT_CNT = 8; +static constexpr ActionId_t READ_REBOOT_MECHANISM_INFO = 9; static constexpr ActionId_t OBSW_UPDATE_FROM_SD_0 = 10; static constexpr ActionId_t OBSW_UPDATE_FROM_SD_1 = 11; From 880b8620ba5ae1fa0ce1b465284439924e05c4c8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 14:52:38 +0200 Subject: [PATCH 362/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 1bb8bea8..8239e610 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 1bb8bea8d92fef2c9ec58ea657b04b56635c16dd +Subproject commit 8239e610ccc069db0d314ff4f242bd47617bd548 From a3b140b680f1c2dad52d41f216252d6c3e1b6ef1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 16:09:24 +0200 Subject: [PATCH 363/506] internal error reporter extension --- bsp_q7s/em/emObjectFactory.cpp | 4 ++-- bsp_q7s/fmObjectFactory.cpp | 2 +- fsfw | 2 +- mission/genericFactory.cpp | 4 ++-- mission/genericFactory.h | 3 ++- tmtc | 2 +- 6 files changed, 9 insertions(+), 8 deletions(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 464b3398..d068bed0 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -36,8 +36,8 @@ void ObjectFactory::produce(void* args) { PersistentTmStores stores; ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel, - *SdCardManager::instance(), &ipcStore, &tmStore, stores, - 200); + *SdCardManager::instance(), &ipcStore, &tmStore, stores, 200, + enableHkSets); LinuxLibgpioIF* gpioComIF = nullptr; SerialComIF* uartComIF = nullptr; diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 413e5648..da14f58c 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -34,7 +34,7 @@ void ObjectFactory::produce(void* args) { PersistentTmStores stores; ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel, *SdCardManager::instance(), &ipcStore, &tmStore, stores, - 200); + 200, true); LinuxLibgpioIF* gpioComIF = nullptr; SerialComIF* uartComIF = nullptr; diff --git a/fsfw b/fsfw index 0f76cdb3..a85a38c8 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 0f76cdb3ba54f5e90a8eee4316c49cf0f581f996 +Subproject commit a85a38c882af968200a0dd54ded9fd99b3961e7a diff --git a/mission/genericFactory.cpp b/mission/genericFactory.cpp index cbe78c7f..5edee640 100644 --- a/mission/genericFactory.cpp +++ b/mission/genericFactory.cpp @@ -96,14 +96,14 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan, StorageManagerIF** ipcStore, StorageManagerIF** tmStore, PersistentTmStores& stores, - uint32_t eventManagerQueueDepth) { + uint32_t eventManagerQueueDepth, bool enableHkSets) { // Framework objects new EventManager(objects::EVENT_MANAGER, eventManagerQueueDepth); auto healthTable = new HealthTable(objects::HEALTH_TABLE); if (healthTable_ != nullptr) { *healthTable_ = healthTable; } - new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER); + new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER, 5, enableHkSets, 120); new VerificationReporter(); auto* timeStamper = new CdsShortTimeStamper(objects::TIME_STAMPER); StorageManagerIF* tcStore; diff --git a/mission/genericFactory.h b/mission/genericFactory.h index a3a52704..03bbfaa5 100644 --- a/mission/genericFactory.h +++ b/mission/genericFactory.h @@ -45,7 +45,8 @@ namespace ObjectFactory { void produceGenericObjects(HealthTableIF** healthTable, PusTmFunnel** pusFunnel, CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan, StorageManagerIF** ipcStore, StorageManagerIF** tmStore, - PersistentTmStores& stores, uint32_t eventManagerQueueDepth); + PersistentTmStores& stores, uint32_t eventManagerQueueDepth, + bool enableHkSets); void createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF& pwrSwitcher, HeaterHandler*& heaterHandler); diff --git a/tmtc b/tmtc index 1bb8bea8..e0457831 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 1bb8bea8d92fef2c9ec58ea657b04b56635c16dd +Subproject commit e0457831d72e0de8611c5f56221a30f19aea7e2f From 74e945ddffe101653955c75ea0849293c3c2383c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 16:10:07 +0200 Subject: [PATCH 364/506] changelog --- CHANGELOG.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index c642b897..4ea3ac85 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,12 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v4.0.1] to be released + +## Changed + +- Internal error reporter set is now enabled by defalt and generated every 120 seconds. + # [v4.0.0] to be released - `eive-tmtc` version v4.0.0 From e8bd3f447da6b6b35f0bf2af9e453cbf56e0a366 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 16:19:38 +0200 Subject: [PATCH 365/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 8239e610..daceb6bc 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 8239e610ccc069db0d314ff4f242bd47617bd548 +Subproject commit daceb6bc1d85c3e63b4f2c777668e55223e6cabb From 32b074b86e4636408a3b3353f6608f5d4be3c4b6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 16:29:52 +0200 Subject: [PATCH 366/506] that should fix the build --- bsp_hosted/ObjectFactory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index dd88d552..23f1a79f 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -68,7 +68,7 @@ void ObjectFactory::produce(void* args) { #endif auto sdcMan = new DummySdCardManager("/tmp"); ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel, *sdcMan, &ipcStore, - &tmStore, persistentStores, 120); + &tmStore, persistentStores, 120, enableHkSets); new TmFunnelHandler(objects::LIVE_TM_TASK, *pusFunnel, *cfdpFunnel); auto* dummyGpioIF = new DummyGpioIF(); From 1c27ba7261e45a548ba12e6344f28acb2c399e48 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 16:47:45 +0200 Subject: [PATCH 367/506] bump tmtc and CHANGELOG --- CHANGELOG.md | 4 ++-- tmtc | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d98f83c9..cd227d98 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,8 +18,8 @@ will consitute of a breaking change warranting a new major release: # [v4.0.0] to be released -- `eive-tmtc` version v4.0.0 -- `q7s-package` version v3.0.0 +- `eive-tmtc` version v5.0.0 +- `q7s-package` version v3.1.1 ## Fixed diff --git a/tmtc b/tmtc index daceb6bc..5db9f383 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit daceb6bc1d85c3e63b4f2c777668e55223e6cabb +Subproject commit 5db9f383a6f7349bb0736442ecdf6ff2164b0632 From 9b2b087d08e72cc5e7559e7dc5d17b607a7aeb26 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 16:49:17 +0200 Subject: [PATCH 368/506] remove stray newline --- CHANGELOG.md | 1 - 1 file changed, 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index cd227d98..3177949b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -180,7 +180,6 @@ Like v3.2.0 but without the custom FM changes related to VC0. # [v2.0.5] 2023-05-11 - - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial submode (e.g. mode normal and submode dual side), it will transition back to the current mode, From fb9303a8de78a16b73d7e80a8f0e3629d6ffbf14 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 16:52:46 +0200 Subject: [PATCH 369/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3177949b..49b3e4bc 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -34,6 +34,8 @@ will consitute of a breaking change warranting a new major release: - APB bus access busy checking is not done anymore as this is performed by the bus itself now. - Core controller will now announce version and image information event in addition to reboot event in the `inititalize` function. +- Core controller will now try to request and announce the firmware version in addition to the + OBSW version as well. - Added core controller action to read reboot mechansm information ## Added From de4562dd2f3416a18cea7084ccc986ebcc58c7b2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 16:53:24 +0200 Subject: [PATCH 370/506] changelog tweak 2 --- CHANGELOG.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 49b3e4bc..7d8d82dd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -40,8 +40,8 @@ will consitute of a breaking change warranting a new major release: ## Added -- Added PL I2C reset pin. It is not used for now but could be used for FDIR procedures to restore - the PL I2C. +- Added PL I2C reset pin. It is not used/connected for now but could be used for FDIR procedures to + restore the PL I2C. - Core controller now announces firmware version as well when requesting a version info event # [v3.3.0] 2023-06-21 From b2b7900720c215cf042c89287aee5266ef8ca6f9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 16:54:49 +0200 Subject: [PATCH 371/506] reduce GNSS pin reset assertion time --- CHANGELOG.md | 1 + bsp_q7s/core/ObjectFactory.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7d8d82dd..760456a2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -37,6 +37,7 @@ will consitute of a breaking change warranting a new major release: - Core controller will now try to request and announce the firmware version in addition to the OBSW version as well. - Added core controller action to read reboot mechansm information +- GNSS reset pin will now only be asserted for 5 ms instead of 100 ms. ## Added diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 54084089..35d30c1a 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -506,7 +506,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* debugGps = true; #endif RESET_ARGS_GNSS.gpioComIF = gpioComIF; - RESET_ARGS_GNSS.waitPeriodMs = 100; + RESET_ARGS_GNSS.waitPeriodMs = 5; auto gpsCtrl = new GpsHyperionLinuxController(objects::GPS_CONTROLLER, objects::NO_OBJECT, enableHkSets, debugGps); gpsCtrl->setResetPinTriggerFunction(gps::triggerGpioResetPin, &RESET_ARGS_GNSS); From f67d72a8dd8655e668051f6cb266a39faddfd128 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 17:34:56 +0200 Subject: [PATCH 372/506] prep v4.0.0 final --- CHANGELOG.md | 2 +- release_checklist.md => release-checklist.md | 0 tmtc | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) rename release_checklist.md => release-checklist.md (100%) diff --git a/CHANGELOG.md b/CHANGELOG.md index 760456a2..53ccf1d5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,7 +16,7 @@ will consitute of a breaking change warranting a new major release: # [unreleased] -# [v4.0.0] to be released +# [v4.0.0] 2023-06-22 - `eive-tmtc` version v5.0.0 - `q7s-package` version v3.1.1 diff --git a/release_checklist.md b/release-checklist.md similarity index 100% rename from release_checklist.md rename to release-checklist.md diff --git a/tmtc b/tmtc index 5db9f383..c9f4a807 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 5db9f383a6f7349bb0736442ecdf6ff2164b0632 +Subproject commit c9f4a8070d20bc659809d5b822ac5a17548f57a4 From f79acca1d6c709995b17a997d98c2bf2c3150d7f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 18:34:16 +0200 Subject: [PATCH 373/506] afmt --- dummies/ImtqDummy.h | 1 - 1 file changed, 1 deletion(-) diff --git a/dummies/ImtqDummy.h b/dummies/ImtqDummy.h index 3495589d..8c76affc 100644 --- a/dummies/ImtqDummy.h +++ b/dummies/ImtqDummy.h @@ -18,7 +18,6 @@ class ImtqDummy : public DeviceHandlerBase { ~ImtqDummy() override; protected: - bool enableHkSets; imtq::StatusDataset statusSet; From 253af8193fe4160fee19a4041aabad763d6aac20 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 18:43:04 +0200 Subject: [PATCH 374/506] fix unittest --- unittest/testEnvironment.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unittest/testEnvironment.cpp b/unittest/testEnvironment.cpp index 72726f39..4c19c4cf 100644 --- a/unittest/testEnvironment.cpp +++ b/unittest/testEnvironment.cpp @@ -27,7 +27,7 @@ void factory(void* args) { new HouseKeepingMock(); eventManager = new EventManagerMock(); new HealthTable(objects::HEALTH_TABLE); - new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER); + new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER, 5, false, 60.0); new CdsShortTimeStamper(objects::TIME_STAMPER); { From 1d047f3c9238693486398a59e1f79db918b697e7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 23 Jun 2023 14:55:51 +0200 Subject: [PATCH 375/506] implement ordered sets --- mission/tmtc/PersistentTmStore.cpp | 188 +++++++++++++++++------------ mission/tmtc/PersistentTmStore.h | 17 ++- 2 files changed, 130 insertions(+), 75 deletions(-) diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index 08a7ea45..8f695aa4 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -32,6 +32,57 @@ ReturnValue_t PersistentTmStore::cancelDump() { return returnvalue::OK; } +ReturnValue_t PersistentTmStore::buildDumpSet(uint32_t fromUnixSeconds, uint32_t upToUnixSeconds) { + using namespace std::filesystem; + std::error_code e; + dumpParams.orderedDumpFilestamps.clear(); + for (auto const& fileOrDir : directory_iterator(basePath)) { + if (not fileOrDir.is_regular_file(e)) { + continue; + } + dumpParams.fileSize = std::filesystem::file_size(dumpParams.dirEntry.path(), e); + if (e) { + sif::error << "PersistentTmStore: Could not retrieve file size: " << e.message() << std::endl; + continue; + } + // sif::debug << "Path: " << dumpParams.dirEntry.path() << std::endl; + + // File empty or can't even read CCSDS header. + if (dumpParams.fileSize <= 6) { + continue; + } + if (dumpParams.fileSize > fileBuf.size()) { + sif::error << "PersistentTmStore: File too large, is deleted" << std::endl; + triggerEvent(persTmStore::FILE_TOO_LARGE, dumpParams.fileSize, fileBuf.size()); + std::filesystem::remove(dumpParams.dirEntry.path(), e); + continue; + } + const path& file = fileOrDir.path(); + struct tm fileTime {}; + if (pathToTime(file, fileTime) != returnvalue::OK) { + sif::error << "Time extraction for file " << file << "failed" << std::endl; + continue; + } + auto fileEpoch = static_cast(timegm(&fileTime)); + if ((fileEpoch > dumpParams.fromUnixTime) and + (fileEpoch + rolloverDiffSeconds <= dumpParams.untilUnixTime)) { + std::ifstream ifile(file, std::ios::binary); + if (ifile.bad()) { + sif::error << "PersistentTmStore: File is bad" << std::endl; + // TODO: Consider deleting file here? + continue; + } + + // TODO: Check whether this is a suffixed file. + DumpIndex dumpIndex; + dumpIndex.epoch = fileEpoch; + dumpParams.orderedDumpFilestamps.emplace(dumpIndex); + return returnvalue::OK; + } + } + return returnvalue::OK; +} + ReturnValue_t PersistentTmStore::assignAndOrCreateMostRecentFile() { if (not activeFile.has_value()) { return createMostRecentFile(std::nullopt); @@ -159,6 +210,10 @@ bool PersistentTmStore::updateBaseDir() { if (not exists(basePath, e)) { create_directories(basePath); } + // Each file will have the base name as a prefix again + path preparedFullFilePath = basePath / baseName; + basePathSize = preparedFullFilePath.string().length(); + std::memcpy(filePathBuf.data(), preparedFullFilePath.c_str(), basePathSize); baseDirUninitialized = false; return true; } @@ -189,12 +244,19 @@ ReturnValue_t PersistentTmStore::startDumpFromUpTo(uint32_t fromUnixSeconds, if (state == State::DUMPING) { return returnvalue::FAILED; } - dumpParams.dirIter = directory_iterator(basePath); - if (dumpParams.dirIter == directory_iterator()) { + auto dirIter = directory_iterator(basePath); + // Directory empty case. + if (dirIter == directory_iterator()) { return returnvalue::FAILED; } dumpParams.fromUnixTime = fromUnixSeconds; dumpParams.untilUnixTime = upToUnixSeconds; + buildDumpSet(fromUnixSeconds, upToUnixSeconds); + // No files in time window found. + if (dumpParams.orderedDumpFilestamps.empty()) { + return DUMP_DONE; + } + dumpParams.dumpIter = dumpParams.orderedDumpFilestamps.begin(); state = State::DUMPING; return loadNextDumpFile(); } @@ -203,49 +265,23 @@ ReturnValue_t PersistentTmStore::loadNextDumpFile() { using namespace std::filesystem; dumpParams.currentSize = 0; std::error_code e; - for (; dumpParams.dirIter != directory_iterator(); dumpParams.dirIter++) { - dumpParams.dirEntry = *dumpParams.dirIter; - if (dumpParams.dirEntry.is_directory(e)) { + for (; dumpParams.dumpIter != dumpParams.orderedDumpFilestamps.end(); dumpParams.dumpIter++) { + DumpIndex dumpIndex = *dumpParams.dumpIter; + timeval tv{}; + tv.tv_sec = dumpIndex.epoch; + size_t fullPathLength = 0; + createFileName(tv, dumpIndex.suffixIdx, fullPathLength); + path filePath(std::string(reinterpret_cast(filePathBuf.data()), fullPathLength)); + std::ifstream ifile(filePath, std::ios::binary); + if (ifile.bad()) { + sif::error << "PersistentTmStore: File is bad" << std::endl; continue; } - dumpParams.fileSize = std::filesystem::file_size(dumpParams.dirEntry.path(), e); - if (e) { - sif::error << "PersistentTmStore: Could not retrieve file size: " << e.message() << std::endl; - continue; - } - // sif::debug << "Path: " << dumpParams.dirEntry.path() << std::endl; - - // File empty or can't even read CCSDS header. - if (dumpParams.fileSize <= 6) { - continue; - } - if (dumpParams.fileSize > fileBuf.size()) { - sif::error << "PersistentTmStore: File too large, is deleted" << std::endl; - triggerEvent(persTmStore::FILE_TOO_LARGE, dumpParams.fileSize, fileBuf.size()); - std::filesystem::remove(dumpParams.dirEntry.path(), e); - continue; - } - const path& file = dumpParams.dirEntry.path(); - struct tm fileTime {}; - if (pathToTime(file, fileTime) != returnvalue::OK) { - sif::error << "Time extraction for file " << file << "failed" << std::endl; - continue; - } - auto fileEpoch = static_cast(timegm(&fileTime)); - if ((fileEpoch > dumpParams.fromUnixTime) and - (fileEpoch + rolloverDiffSeconds <= dumpParams.untilUnixTime)) { - dumpParams.currentFileUnixStamp = fileEpoch; - std::ifstream ifile(file, std::ios::binary); - if (ifile.bad()) { - sif::error << "PersistentTmStore: File is bad" << std::endl; - continue; - } - ifile.read(reinterpret_cast(fileBuf.data()), - static_cast(dumpParams.fileSize)); - // Increment iterator for next cycle. - dumpParams.dirIter++; - return returnvalue::OK; - } + ifile.read(reinterpret_cast(fileBuf.data()), + static_cast(dumpParams.fileSize)); + // Increment iterator for next cycle. + dumpParams.dumpIter++; + return returnvalue::OK; } // Directory iterator was consumed and we are done. state = State::IDLE; @@ -302,37 +338,9 @@ ReturnValue_t PersistentTmStore::pathToTime(const std::filesystem::path& path, s ReturnValue_t PersistentTmStore::createMostRecentFile(std::optional suffix) { using namespace std::filesystem; - unsigned currentIdx = 0; - path pathStart = basePath / baseName; - memcpy(fileBuf.data() + currentIdx, pathStart.c_str(), pathStart.string().length()); - currentIdx += pathStart.string().length(); - fileBuf[currentIdx] = '_'; - currentIdx += 1; - time_t epoch = currentTv.tv_sec; - struct tm* time = gmtime(&epoch); - size_t writtenBytes = strftime(reinterpret_cast(fileBuf.data() + currentIdx), - fileBuf.size(), config::FILE_DATE_FORMAT, time); - if (writtenBytes == 0) { - sif::error << "PersistentTmStore::createMostRecentFile: Could not create file timestamp" - << std::endl; - return returnvalue::FAILED; - } - currentIdx += writtenBytes; - char* res = strcpy(reinterpret_cast(fileBuf.data() + currentIdx), ".bin"); - if (res == nullptr) { - return returnvalue::FAILED; - } - currentIdx += 4; - if (suffix.has_value()) { - std::string fullSuffix = "." + std::to_string(suffix.value()); - res = strcpy(reinterpret_cast(fileBuf.data() + currentIdx), fullSuffix.c_str()); - if (res == nullptr) { - return returnvalue::FAILED; - } - currentIdx += fullSuffix.size(); - } - - path newPath(std::string(reinterpret_cast(fileBuf.data()), currentIdx)); + size_t currentIdx; + createFileName(currentTv, suffix, currentIdx); + path newPath(std::string(reinterpret_cast(filePathBuf.data()), currentIdx)); std::ofstream of(newPath, std::ios::binary); activeFile = newPath; activeFileTv = currentTv; @@ -354,3 +362,35 @@ void PersistentTmStore::getStartAndEndTimeCurrentOrLastDump(uint32_t& startTime, startTime = dumpParams.fromUnixTime; endTime = dumpParams.untilUnixTime; } + +ReturnValue_t PersistentTmStore::createFileName(timeval& tv, std::optional suffix, + size_t& fullPathLength) { + unsigned currentIdx = basePathSize; + fileBuf[currentIdx] = '_'; + currentIdx += 1; + time_t epoch = tv.tv_sec; + struct tm* time = gmtime(&epoch); + size_t writtenBytes = strftime(reinterpret_cast(filePathBuf.data() + currentIdx), + filePathBuf.size(), config::FILE_DATE_FORMAT, time); + if (writtenBytes == 0) { + sif::error << "PersistentTmStore::createMostRecentFile: Could not create file timestamp" + << std::endl; + return returnvalue::FAILED; + } + currentIdx += writtenBytes; + char* res = strcpy(reinterpret_cast(filePathBuf.data() + currentIdx), ".bin"); + if (res == nullptr) { + return returnvalue::FAILED; + } + currentIdx += 4; + if (suffix.has_value()) { + std::string fullSuffix = "." + std::to_string(suffix.value()); + res = strcpy(reinterpret_cast(filePathBuf.data() + currentIdx), fullSuffix.c_str()); + if (res == nullptr) { + return returnvalue::FAILED; + } + currentIdx += fullSuffix.size(); + } + fullPathLength = currentIdx; + return returnvalue::OK; +} diff --git a/mission/tmtc/PersistentTmStore.h b/mission/tmtc/PersistentTmStore.h index 17d2c9e7..8684d89a 100644 --- a/mission/tmtc/PersistentTmStore.h +++ b/mission/tmtc/PersistentTmStore.h @@ -10,6 +10,7 @@ #include #include +#include #include "eive/eventSubsystemIds.h" #include "eive/resultClassIds.h" @@ -37,6 +38,13 @@ struct PersistentTmStoreArgs { SdCardMountedIF& sdcMan; }; +struct DumpIndex { + uint32_t epoch; + uint8_t suffixIdx; + // Define a custom comparison function based on the epoch variable + bool operator<(const DumpIndex& other) const { return epoch < other.epoch; } +}; + class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { public: enum class State { IDLE, DUMPING }; @@ -96,7 +104,10 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { std::string baseName; uint8_t currentSameSecNumber = 0; std::filesystem::path basePath; + // std::filesystem::path pathStart = basePath / baseName; uint32_t rolloverDiffSeconds = 0; + std::array filePathBuf{}; + size_t basePathSize; std::array fileBuf{}; timeval currentTv; timeval activeFileTv{}; @@ -106,8 +117,10 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { uint32_t fromUnixTime = 0; uint32_t untilUnixTime = 0; uint32_t currentFileUnixStamp = 0; - std::filesystem::directory_iterator dirIter; + // std::filesystem::directory_iterator dirIter; std::filesystem::directory_entry dirEntry; + std::set orderedDumpFilestamps{}; + std::set::iterator dumpIter; size_t fileSize = 0; size_t currentSize = 0; }; @@ -122,10 +135,12 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { [[nodiscard]] MessageQueueId_t getCommandQueue() const override; void calcDiffSeconds(RolloverInterval intervalUnit, uint32_t intervalCount); + ReturnValue_t buildDumpSet(uint32_t fromUnixSeconds, uint32_t upToUnixSeconds); ReturnValue_t createMostRecentFile(std::optional suffix); static ReturnValue_t pathToTime(const std::filesystem::path& path, struct tm& time); void fileToPackets(const std::filesystem::path& path, uint32_t unixStamp); ReturnValue_t loadNextDumpFile(); + ReturnValue_t createFileName(timeval& tv, std::optional suffix, size_t& fullPathLength); bool updateBaseDir(); ReturnValue_t assignAndOrCreateMostRecentFile(); }; From 9023405b960a1a36d8f417d883916099f314066f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 23 Jun 2023 16:05:38 +0200 Subject: [PATCH 376/506] some fxes --- mission/tmtc/PersistentTmStore.cpp | 31 ++++++++++++++++----- mission/tmtc/PersistentTmStore.h | 4 +-- unittest/CMakeLists.txt | 2 +- unittest/testGenericFilesystem.cpp | 43 ++++++++++++++++++++++++++++++ unittest/testStampInFilename.cpp | 21 --------------- 5 files changed, 70 insertions(+), 31 deletions(-) create mode 100644 unittest/testGenericFilesystem.cpp delete mode 100644 unittest/testStampInFilename.cpp diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index 8f695aa4..db953c4b 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -40,7 +40,7 @@ ReturnValue_t PersistentTmStore::buildDumpSet(uint32_t fromUnixSeconds, uint32_t if (not fileOrDir.is_regular_file(e)) { continue; } - dumpParams.fileSize = std::filesystem::file_size(dumpParams.dirEntry.path(), e); + dumpParams.fileSize = std::filesystem::file_size(fileOrDir.path(), e); if (e) { sif::error << "PersistentTmStore: Could not retrieve file size: " << e.message() << std::endl; continue; @@ -54,7 +54,7 @@ ReturnValue_t PersistentTmStore::buildDumpSet(uint32_t fromUnixSeconds, uint32_t if (dumpParams.fileSize > fileBuf.size()) { sif::error << "PersistentTmStore: File too large, is deleted" << std::endl; triggerEvent(persTmStore::FILE_TOO_LARGE, dumpParams.fileSize, fileBuf.size()); - std::filesystem::remove(dumpParams.dirEntry.path(), e); + std::filesystem::remove(fileOrDir.path(), e); continue; } const path& file = fileOrDir.path(); @@ -73,9 +73,9 @@ ReturnValue_t PersistentTmStore::buildDumpSet(uint32_t fromUnixSeconds, uint32_t continue; } - // TODO: Check whether this is a suffixed file. DumpIndex dumpIndex; dumpIndex.epoch = fileEpoch; + dumpIndex.suffixIdx = extractSuffix(file.string()); dumpParams.orderedDumpFilestamps.emplace(dumpIndex); return returnvalue::OK; } @@ -83,6 +83,21 @@ ReturnValue_t PersistentTmStore::buildDumpSet(uint32_t fromUnixSeconds, uint32_t return returnvalue::OK; } +uint8_t PersistentTmStore::extractSuffix(const std::string& pathStr) { + std::string numberStr; + // Find the position of the dot at the end of the file path + size_t dotPos = pathStr.find_last_of('.'); + if (dotPos != std::string::npos && dotPos < pathStr.length() - 1) { + // Extract the substring after the dot + numberStr = pathStr.substr(dotPos + 1); + } + int number = std::stoi(numberStr); + if (number < 0 or number > std::numeric_limits::max()) { + return 0; + } + return static_cast(number); +} + ReturnValue_t PersistentTmStore::assignAndOrCreateMostRecentFile() { if (not activeFile.has_value()) { return createMostRecentFile(std::nullopt); @@ -271,12 +286,14 @@ ReturnValue_t PersistentTmStore::loadNextDumpFile() { tv.tv_sec = dumpIndex.epoch; size_t fullPathLength = 0; createFileName(tv, dumpIndex.suffixIdx, fullPathLength); - path filePath(std::string(reinterpret_cast(filePathBuf.data()), fullPathLength)); - std::ifstream ifile(filePath, std::ios::binary); + dumpParams.currentFile = + path(std::string(reinterpret_cast(filePathBuf.data()), fullPathLength)); + std::ifstream ifile(dumpParams.currentFile, std::ios::binary); if (ifile.bad()) { sif::error << "PersistentTmStore: File is bad" << std::endl; continue; } + sif::debug << baseName << " dump: Loading " << dumpParams.currentFile << std::endl; ifile.read(reinterpret_cast(fileBuf.data()), static_cast(dumpParams.fileSize)); // Increment iterator for next cycle. @@ -303,8 +320,8 @@ ReturnValue_t PersistentTmStore::getNextDumpPacket(PusTmReader& reader, bool& fi // Delete the file and load next. Could use better algorithm to partially // restore the file dump, but for now do not trust the file. std::error_code e; - std::filesystem::remove(dumpParams.dirEntry.path().c_str(), e); - if (dumpParams.dirEntry.path() == activeFile) { + std::filesystem::remove(dumpParams.currentFile.c_str(), e); + if (dumpParams.currentFile == activeFile) { activeFile == std::nullopt; assignAndOrCreateMostRecentFile(); } diff --git a/mission/tmtc/PersistentTmStore.h b/mission/tmtc/PersistentTmStore.h index 8684d89a..e2ad3cd0 100644 --- a/mission/tmtc/PersistentTmStore.h +++ b/mission/tmtc/PersistentTmStore.h @@ -117,8 +117,7 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { uint32_t fromUnixTime = 0; uint32_t untilUnixTime = 0; uint32_t currentFileUnixStamp = 0; - // std::filesystem::directory_iterator dirIter; - std::filesystem::directory_entry dirEntry; + std::filesystem::path currentFile; std::set orderedDumpFilestamps{}; std::set::iterator dumpIter; size_t fileSize = 0; @@ -141,6 +140,7 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { void fileToPackets(const std::filesystem::path& path, uint32_t unixStamp); ReturnValue_t loadNextDumpFile(); ReturnValue_t createFileName(timeval& tv, std::optional suffix, size_t& fullPathLength); + uint8_t extractSuffix(const std::string& pathStr); bool updateBaseDir(); ReturnValue_t assignAndOrCreateMostRecentFile(); }; diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt index 20b98979..e9a01891 100644 --- a/unittest/CMakeLists.txt +++ b/unittest/CMakeLists.txt @@ -4,7 +4,7 @@ add_subdirectory(mocks) target_sources(${UNITTEST_NAME} PRIVATE main.cpp testEnvironment.cpp - testStampInFilename.cpp + testGenericFilesystem.cpp hdlcEncodingRw.cpp printChar.cpp ) \ No newline at end of file diff --git a/unittest/testGenericFilesystem.cpp b/unittest/testGenericFilesystem.cpp new file mode 100644 index 00000000..e5279bab --- /dev/null +++ b/unittest/testGenericFilesystem.cpp @@ -0,0 +1,43 @@ + +#include +#include + +#include "fsfw/timemanager/Clock.h" + +uint8_t extractSuffix(const std::string& pathStr) { + std::string numberStr; + // Find the position of the dot at the end of the file path + size_t dotPos = pathStr.find_last_of('.'); + if (dotPos != std::string::npos && dotPos < pathStr.length() - 1) { + // Extract the substring after the dot + numberStr = pathStr.substr(dotPos + 1); + } + int number = std::stoi(numberStr); + if (number < 0 or number > std::numeric_limits::max()) { + return 0; + } + return static_cast(number); +} + +TEST_CASE("Stamp in Filename", "[Stamp In Filename]") { + Clock::TimeOfDay_t tod; + std::string baseName = "verif"; + std::string pathStr = "verif_2022-05-25T16:55:23Z.bin"; + unsigned int underscorePos = pathStr.find_last_of('_'); + std::string stampStr = pathStr.substr(underscorePos + 1); + float seconds = 0.0; + char* prefix = nullptr; + int count = + sscanf(stampStr.c_str(), + "%4" SCNu32 "-%2" SCNu32 "-%2" SCNu32 "T%2" SCNu32 ":%2" SCNu32 ":%2" SCNu32 "Z", + &tod.year, &tod.month, &tod.day, &tod.hour, &tod.minute, &tod.second); + static_cast(count); + CHECK(count == 6); +} + +TEST_CASE("Suffix Extraction") { + std::string pathStr = "/mnt/sd0/tm/hk/hk-some-stamp.bin.0"; + CHECK(extractSuffix(pathStr) == 0); + pathStr = "/mnt/sd0/tm/hk/hk-some-stamp.bin.2"; + CHECK(extractSuffix(pathStr) == 2); +} diff --git a/unittest/testStampInFilename.cpp b/unittest/testStampInFilename.cpp deleted file mode 100644 index c66bdce6..00000000 --- a/unittest/testStampInFilename.cpp +++ /dev/null @@ -1,21 +0,0 @@ - -#include -#include - -#include "fsfw/timemanager/Clock.h" - -TEST_CASE("Stamp in Filename", "[Stamp In Filename]") { - Clock::TimeOfDay_t tod; - std::string baseName = "verif"; - std::string pathStr = "verif_2022-05-25T16:55:23Z.bin"; - unsigned int underscorePos = pathStr.find_last_of('_'); - std::string stampStr = pathStr.substr(underscorePos + 1); - float seconds = 0.0; - char* prefix = nullptr; - int count = - sscanf(stampStr.c_str(), - "%4" SCNu32 "-%2" SCNu32 "-%2" SCNu32 "T%2" SCNu32 ":%2" SCNu32 ":%2" SCNu32 "Z", - &tod.year, &tod.month, &tod.day, &tod.hour, &tod.minute, &tod.second); - static_cast(count); - CHECK(count == 6); -} From 2e041c3013549b27940942640c9817c4f55109bb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 23 Jun 2023 17:07:07 +0200 Subject: [PATCH 377/506] some more fixes --- mission/tmtc/PersistentTmStore.cpp | 25 +++++++++++++++++-------- mission/tmtc/PersistentTmStore.h | 4 ++-- 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index db953c4b..f4ce65ef 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -40,12 +40,12 @@ ReturnValue_t PersistentTmStore::buildDumpSet(uint32_t fromUnixSeconds, uint32_t if (not fileOrDir.is_regular_file(e)) { continue; } + sif::debug << "Path: " << fileOrDir.path() << std::endl; dumpParams.fileSize = std::filesystem::file_size(fileOrDir.path(), e); if (e) { sif::error << "PersistentTmStore: Could not retrieve file size: " << e.message() << std::endl; continue; } - // sif::debug << "Path: " << dumpParams.dirEntry.path() << std::endl; // File empty or can't even read CCSDS header. if (dumpParams.fileSize <= 6) { @@ -83,7 +83,7 @@ ReturnValue_t PersistentTmStore::buildDumpSet(uint32_t fromUnixSeconds, uint32_t return returnvalue::OK; } -uint8_t PersistentTmStore::extractSuffix(const std::string& pathStr) { +std::optional PersistentTmStore::extractSuffix(const std::string& pathStr) { std::string numberStr; // Find the position of the dot at the end of the file path size_t dotPos = pathStr.find_last_of('.'); @@ -91,11 +91,18 @@ uint8_t PersistentTmStore::extractSuffix(const std::string& pathStr) { // Extract the substring after the dot numberStr = pathStr.substr(dotPos + 1); } - int number = std::stoi(numberStr); - if (number < 0 or number > std::numeric_limits::max()) { - return 0; + std::optional number; + try { + number = std::stoi(numberStr); + if (number < 0 or number > std::numeric_limits::max()) { + return number; + } + + } catch (std::invalid_argument& exception) { + sif::error << "PersistentTmStore::extractSuffix: Exception " << exception.what() + << ", invald input string: " << numberStr << std::endl; } - return static_cast(number); + return number; } ReturnValue_t PersistentTmStore::assignAndOrCreateMostRecentFile() { @@ -195,6 +202,7 @@ ReturnValue_t PersistentTmStore::storePacket(PusTmReader& reader) { createMostRecentFile(suffix); } + sif::debug << "active file " << activeFile.value() << std::endl; // Rollover conditions were handled, write to file now std::ofstream of(activeFile.value(), std::ios::app | std::ios::binary); of.write(reinterpret_cast(reader.getFullData()), @@ -229,6 +237,8 @@ bool PersistentTmStore::updateBaseDir() { path preparedFullFilePath = basePath / baseName; basePathSize = preparedFullFilePath.string().length(); std::memcpy(filePathBuf.data(), preparedFullFilePath.c_str(), basePathSize); + filePathBuf[basePathSize] = '_'; + basePathSize += 1; baseDirUninitialized = false; return true; } @@ -358,6 +368,7 @@ ReturnValue_t PersistentTmStore::createMostRecentFile(std::optional suf size_t currentIdx; createFileName(currentTv, suffix, currentIdx); path newPath(std::string(reinterpret_cast(filePathBuf.data()), currentIdx)); + sif::debug << "creating new file: " << newPath << std::endl; std::ofstream of(newPath, std::ios::binary); activeFile = newPath; activeFileTv = currentTv; @@ -383,8 +394,6 @@ void PersistentTmStore::getStartAndEndTimeCurrentOrLastDump(uint32_t& startTime, ReturnValue_t PersistentTmStore::createFileName(timeval& tv, std::optional suffix, size_t& fullPathLength) { unsigned currentIdx = basePathSize; - fileBuf[currentIdx] = '_'; - currentIdx += 1; time_t epoch = tv.tv_sec; struct tm* time = gmtime(&epoch); size_t writtenBytes = strftime(reinterpret_cast(filePathBuf.data() + currentIdx), diff --git a/mission/tmtc/PersistentTmStore.h b/mission/tmtc/PersistentTmStore.h index e2ad3cd0..1694a8c9 100644 --- a/mission/tmtc/PersistentTmStore.h +++ b/mission/tmtc/PersistentTmStore.h @@ -40,7 +40,7 @@ struct PersistentTmStoreArgs { struct DumpIndex { uint32_t epoch; - uint8_t suffixIdx; + std::optional suffixIdx; // Define a custom comparison function based on the epoch variable bool operator<(const DumpIndex& other) const { return epoch < other.epoch; } }; @@ -140,7 +140,7 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { void fileToPackets(const std::filesystem::path& path, uint32_t unixStamp); ReturnValue_t loadNextDumpFile(); ReturnValue_t createFileName(timeval& tv, std::optional suffix, size_t& fullPathLength); - uint8_t extractSuffix(const std::string& pathStr); + std::optional extractSuffix(const std::string& pathStr); bool updateBaseDir(); ReturnValue_t assignAndOrCreateMostRecentFile(); }; From 4a8e111a185aef1daf0232bce9784e10199716ba Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 23 Jun 2023 17:44:18 +0200 Subject: [PATCH 378/506] bugfix packet demux --- mission/tmtc/PusLiveDemux.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/mission/tmtc/PusLiveDemux.cpp b/mission/tmtc/PusLiveDemux.cpp index bbe1be4f..28114252 100644 --- a/mission/tmtc/PusLiveDemux.cpp +++ b/mission/tmtc/PusLiveDemux.cpp @@ -10,7 +10,10 @@ ReturnValue_t PusLiveDemux::demultiplexPackets(StorageManagerIF& tmStore, size_t tmSize) { ReturnValue_t result = returnvalue::OK; for (unsigned int idx = 0; idx < destinations.size(); idx++) { + sif::debug << "Destination size: " << destinations.size() << std::endl; + sif::debug << "Sending" << destinations[idx].name << std::endl; const auto& dest = destinations[idx]; + bool setOrigStoreId = false; if (destinations.size() > 1) { if (idx < destinations.size() - 1) { // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need @@ -26,8 +29,14 @@ ReturnValue_t PusLiveDemux::demultiplexPackets(StorageManagerIF& tmStore, #endif } } else { - message.setStorageId(origStoreId); + setOrigStoreId = true; + sif::debug << "Setting org store ID" << std::endl; } + } else { + setOrigStoreId = true; + } + if(setOrigStoreId) { + message.setStorageId(origStoreId); } result = ownerQueue.sendMessage(dest.queueId, &message); if (result != returnvalue::OK) { From db5e7a535eae18fe82ddf2db8abcfaafc72ddf91 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 23 Jun 2023 17:47:18 +0200 Subject: [PATCH 379/506] remove printouts --- mission/tmtc/PusLiveDemux.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/mission/tmtc/PusLiveDemux.cpp b/mission/tmtc/PusLiveDemux.cpp index 28114252..47e1fc1f 100644 --- a/mission/tmtc/PusLiveDemux.cpp +++ b/mission/tmtc/PusLiveDemux.cpp @@ -10,8 +10,6 @@ ReturnValue_t PusLiveDemux::demultiplexPackets(StorageManagerIF& tmStore, size_t tmSize) { ReturnValue_t result = returnvalue::OK; for (unsigned int idx = 0; idx < destinations.size(); idx++) { - sif::debug << "Destination size: " << destinations.size() << std::endl; - sif::debug << "Sending" << destinations[idx].name << std::endl; const auto& dest = destinations[idx]; bool setOrigStoreId = false; if (destinations.size() > 1) { @@ -30,13 +28,12 @@ ReturnValue_t PusLiveDemux::demultiplexPackets(StorageManagerIF& tmStore, } } else { setOrigStoreId = true; - sif::debug << "Setting org store ID" << std::endl; } } else { setOrigStoreId = true; } - if(setOrigStoreId) { - message.setStorageId(origStoreId); + if (setOrigStoreId) { + message.setStorageId(origStoreId); } result = ownerQueue.sendMessage(dest.queueId, &message); if (result != returnvalue::OK) { From b261779ebf79c45ce45bae3057eb70a43e8d20bb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 23 Jun 2023 17:53:19 +0200 Subject: [PATCH 380/506] this iis less confusing --- mission/tmtc/PusLiveDemux.cpp | 29 ++++++++++------------------- 1 file changed, 10 insertions(+), 19 deletions(-) diff --git a/mission/tmtc/PusLiveDemux.cpp b/mission/tmtc/PusLiveDemux.cpp index 47e1fc1f..8ba69e07 100644 --- a/mission/tmtc/PusLiveDemux.cpp +++ b/mission/tmtc/PusLiveDemux.cpp @@ -11,28 +11,19 @@ ReturnValue_t PusLiveDemux::demultiplexPackets(StorageManagerIF& tmStore, ReturnValue_t result = returnvalue::OK; for (unsigned int idx = 0; idx < destinations.size(); idx++) { const auto& dest = destinations[idx]; - bool setOrigStoreId = false; - if (destinations.size() > 1) { - if (idx < destinations.size() - 1) { - // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need - // to bother with send order and where the data is deleted. - store_address_t storeId; - result = tmStore.addData(&storeId, tmData, tmSize); - if (result == returnvalue::OK) { - message.setStorageId(storeId); - } else { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PusLiveDemux::handlePacket: Store too full to create data copy" - << std::endl; -#endif - } + if ((destinations.size() > 1) and (idx < (destinations.size() - 1))) { + // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need + // to bother with send order and where the data is deleted. + store_address_t storeId; + result = tmStore.addData(&storeId, tmData, tmSize); + if (result == returnvalue::OK) { + message.setStorageId(storeId); } else { - setOrigStoreId = true; +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "PusLiveDemux::handlePacket: Store too full to create data copy" << std::endl; +#endif } } else { - setOrigStoreId = true; - } - if (setOrigStoreId) { message.setStorageId(origStoreId); } result = ownerQueue.sendMessage(dest.queueId, &message); From a95ab49690e5302657924e51c9fb4522e343772f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 23 Jun 2023 17:56:54 +0200 Subject: [PATCH 381/506] changelog --- CHANGELOG.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 53ccf1d5..c918c5e7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,13 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v4.0.1] 2023-06-23 + +## Fixed + +- `PusLiveDemux` packet demultiplexing bugfix where the demultiplexing did not work when there was + only one destination available. + # [v4.0.0] 2023-06-22 - `eive-tmtc` version v5.0.0 From 2901ebc15c3320c677c87481ff7995461c0b75db Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 01:21:46 +0200 Subject: [PATCH 382/506] prep v4.0.1 patch --- CHANGELOG.md | 2 +- CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c918c5e7..3a4eac1f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,7 +16,7 @@ will consitute of a breaking change warranting a new major release: # [unreleased] -# [v4.0.1] 2023-06-23 +# [v4.0.1] 2023-06-24 ## Fixed diff --git a/CMakeLists.txt b/CMakeLists.txt index 3c7e1c1e..9e234cae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 4) set(OBSW_VERSION_MINOR 0) -set(OBSW_VERSION_REVISION 0) +set(OBSW_VERSION_REVISION 1) # set(CMAKE_VERBOSE TRUE) From 18b67d18a72a4c224c0bd0bff3ca57a9305ed77f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 02:07:11 +0200 Subject: [PATCH 383/506] seems to work now --- mission/tmtc/PersistentTmStore.cpp | 27 ++++++++++++++++----------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index f4ce65ef..6a92cdf1 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -40,7 +40,6 @@ ReturnValue_t PersistentTmStore::buildDumpSet(uint32_t fromUnixSeconds, uint32_t if (not fileOrDir.is_regular_file(e)) { continue; } - sif::debug << "Path: " << fileOrDir.path() << std::endl; dumpParams.fileSize = std::filesystem::file_size(fileOrDir.path(), e); if (e) { sif::error << "PersistentTmStore: Could not retrieve file size: " << e.message() << std::endl; @@ -76,8 +75,8 @@ ReturnValue_t PersistentTmStore::buildDumpSet(uint32_t fromUnixSeconds, uint32_t DumpIndex dumpIndex; dumpIndex.epoch = fileEpoch; dumpIndex.suffixIdx = extractSuffix(file.string()); + // sif::debug << "Inserting file " << fileOrDir.path() << std::endl; dumpParams.orderedDumpFilestamps.emplace(dumpIndex); - return returnvalue::OK; } } return returnvalue::OK; @@ -87,10 +86,11 @@ std::optional PersistentTmStore::extractSuffix(const std::string& pathS std::string numberStr; // Find the position of the dot at the end of the file path size_t dotPos = pathStr.find_last_of('.'); - if (dotPos != std::string::npos && dotPos < pathStr.length() - 1) { - // Extract the substring after the dot - numberStr = pathStr.substr(dotPos + 1); + if ((dotPos < pathStr.length()) and not std::isdigit(pathStr[dotPos + 1])) { + return std::nullopt; } + // Extract the substring after the dot + numberStr = pathStr.substr(dotPos + 1); std::optional number; try { number = std::stoi(numberStr); @@ -202,7 +202,6 @@ ReturnValue_t PersistentTmStore::storePacket(PusTmReader& reader) { createMostRecentFile(suffix); } - sif::debug << "active file " << activeFile.value() << std::endl; // Rollover conditions were handled, write to file now std::ofstream of(activeFile.value(), std::ios::app | std::ios::binary); of.write(reinterpret_cast(reader.getFullData()), @@ -298,12 +297,19 @@ ReturnValue_t PersistentTmStore::loadNextDumpFile() { createFileName(tv, dumpIndex.suffixIdx, fullPathLength); dumpParams.currentFile = path(std::string(reinterpret_cast(filePathBuf.data()), fullPathLength)); - std::ifstream ifile(dumpParams.currentFile, std::ios::binary); - if (ifile.bad()) { - sif::error << "PersistentTmStore: File is bad" << std::endl; + dumpParams.fileSize = std::filesystem::file_size(dumpParams.currentFile, e); + if (e) { + // TODO: Event? + sif::error << "PersistentTmStore: Could not load next dump file: " << e.message() + << std::endl; continue; } - sif::debug << baseName << " dump: Loading " << dumpParams.currentFile << std::endl; + std::ifstream ifile(dumpParams.currentFile, std::ios::binary); + if (ifile.bad()) { + sif::error << "PersistentTmStore: File is bad. Loading next file" << std::endl; + continue; + } + // sif::debug << baseName << " dump: Loading " << dumpParams.currentFile << std::endl; ifile.read(reinterpret_cast(fileBuf.data()), static_cast(dumpParams.fileSize)); // Increment iterator for next cycle. @@ -368,7 +374,6 @@ ReturnValue_t PersistentTmStore::createMostRecentFile(std::optional suf size_t currentIdx; createFileName(currentTv, suffix, currentIdx); path newPath(std::string(reinterpret_cast(filePathBuf.data()), currentIdx)); - sif::debug << "creating new file: " << newPath << std::endl; std::ofstream of(newPath, std::ios::binary); activeFile = newPath; activeFileTv = currentTv; From a8ab40674fcae17e8e33e3aef43f6838ad9de287 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 11:08:50 +0200 Subject: [PATCH 384/506] seems to work well --- mission/tmtc/PersistentTmStore.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index 6a92cdf1..f8830170 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -17,6 +17,8 @@ using namespace returnvalue; +static constexpr bool DEBUG_DUMPS = false; + PersistentTmStore::PersistentTmStore(PersistentTmStoreArgs args) : SystemObject(args.objectId), tmStore(args.tmStore), @@ -75,7 +77,12 @@ ReturnValue_t PersistentTmStore::buildDumpSet(uint32_t fromUnixSeconds, uint32_t DumpIndex dumpIndex; dumpIndex.epoch = fileEpoch; dumpIndex.suffixIdx = extractSuffix(file.string()); - // sif::debug << "Inserting file " << fileOrDir.path() << std::endl; + if (DEBUG_DUMPS) { + sif::debug << "Inserting file " << fileOrDir.path() << std::endl; + if (dumpIndex.suffixIdx.has_value()) { + sif::debug << "Suffix: " << static_cast(dumpIndex.suffixIdx.value()) << std::endl; + } + } dumpParams.orderedDumpFilestamps.emplace(dumpIndex); } } @@ -94,8 +101,8 @@ std::optional PersistentTmStore::extractSuffix(const std::string& pathS std::optional number; try { number = std::stoi(numberStr); - if (number < 0 or number > std::numeric_limits::max()) { - return number; + if (number.value() > std::numeric_limits::max()) { + return std::nullopt; } } catch (std::invalid_argument& exception) { @@ -309,7 +316,9 @@ ReturnValue_t PersistentTmStore::loadNextDumpFile() { sif::error << "PersistentTmStore: File is bad. Loading next file" << std::endl; continue; } - // sif::debug << baseName << " dump: Loading " << dumpParams.currentFile << std::endl; + if (DEBUG_DUMPS) { + sif::debug << baseName << " dump: Loading " << dumpParams.currentFile << std::endl; + } ifile.read(reinterpret_cast(fileBuf.data()), static_cast(dumpParams.fileSize)); // Increment iterator for next cycle. From 53b48ad99bfea6014721d8d5487e4354cf87b0f4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 11:46:56 +0200 Subject: [PATCH 385/506] this is tricky --- mission/tmtc/PersistentTmStore.cpp | 39 +++++++++++++++++++++++------- mission/tmtc/PersistentTmStore.h | 4 ++- 2 files changed, 33 insertions(+), 10 deletions(-) diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index f8830170..6ee2f3a6 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -17,7 +17,7 @@ using namespace returnvalue; -static constexpr bool DEBUG_DUMPS = false; +static constexpr bool DEBUG_DUMPS = true; PersistentTmStore::PersistentTmStore(PersistentTmStoreArgs args) : SystemObject(args.objectId), @@ -74,16 +74,21 @@ ReturnValue_t PersistentTmStore::buildDumpSet(uint32_t fromUnixSeconds, uint32_t continue; } - DumpIndex dumpIndex; - dumpIndex.epoch = fileEpoch; - dumpIndex.suffixIdx = extractSuffix(file.string()); if (DEBUG_DUMPS) { sif::debug << "Inserting file " << fileOrDir.path() << std::endl; - if (dumpIndex.suffixIdx.has_value()) { - sif::debug << "Suffix: " << static_cast(dumpIndex.suffixIdx.value()) << std::endl; - } } - dumpParams.orderedDumpFilestamps.emplace(dumpIndex); + DumpIndex dumpIndex; + dumpIndex.epoch = fileEpoch; + // Multiple files for the same time are supported via a special suffix. We smply count the + // number of copies and later try to dump the same number of files with the additonal + // suffixes + auto& iter = dumpParams.orderedDumpFilestamps.find(dumpIndex); + if (iter != dumpParams.orderedDumpFilestamps.end()) { + iter->additionalFiles++; + } else { + dumpIndex.additionalFiles = 0; + dumpParams.orderedDumpFilestamps.emplace(dumpIndex); + } } } return returnvalue::OK; @@ -301,7 +306,7 @@ ReturnValue_t PersistentTmStore::loadNextDumpFile() { timeval tv{}; tv.tv_sec = dumpIndex.epoch; size_t fullPathLength = 0; - createFileName(tv, dumpIndex.suffixIdx, fullPathLength); + createFileName(tv, dumpParams.currentSameFileIdx, fullPathLength); dumpParams.currentFile = path(std::string(reinterpret_cast(filePathBuf.data()), fullPathLength)); dumpParams.fileSize = std::filesystem::file_size(dumpParams.currentFile, e); @@ -321,6 +326,22 @@ ReturnValue_t PersistentTmStore::loadNextDumpFile() { } ifile.read(reinterpret_cast(fileBuf.data()), static_cast(dumpParams.fileSize)); + if (dumpIndex.additionalFiles > 0) { + dumpParams.currentSameFileIdx++; + } + if (dumpIndex.additionalFiles > 0 and not dumpParams.currentSameFileIdx.has_value()) { + if (not dumpParams.currentSameFileIdx.has_value()) { + // Initialze the file index and stay on same file + dumpParams.currentSameFileIdx = 0; + continue; + + } else if (dumpParams.currentSameFileIdx.value() < dumpIndex.additionalFiles) { + dumpParams.currentSameFileIdx += 1; + continue; + } + } + // File will change, reset this field for correct state-keeping. + dumpParams.currentSameFileIdx = std::nullopt; // Increment iterator for next cycle. dumpParams.dumpIter++; return returnvalue::OK; diff --git a/mission/tmtc/PersistentTmStore.h b/mission/tmtc/PersistentTmStore.h index 1694a8c9..54b87a42 100644 --- a/mission/tmtc/PersistentTmStore.h +++ b/mission/tmtc/PersistentTmStore.h @@ -40,7 +40,8 @@ struct PersistentTmStoreArgs { struct DumpIndex { uint32_t epoch; - std::optional suffixIdx; + // Number of additional files with a suffix like .0, .1 etc. + uint8_t additionalFiles; // Define a custom comparison function based on the epoch variable bool operator<(const DumpIndex& other) const { return epoch < other.epoch; } }; @@ -120,6 +121,7 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { std::filesystem::path currentFile; std::set orderedDumpFilestamps{}; std::set::iterator dumpIter; + std::optional currentSameFileIdx = 0; size_t fileSize = 0; size_t currentSize = 0; }; From 04f4eedb78e35cfee982ce0741b08872151000f5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 12:01:12 +0200 Subject: [PATCH 386/506] that should make it work --- mission/tmtc/PersistentTmStore.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index 6ee2f3a6..f68299a0 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -82,13 +82,15 @@ ReturnValue_t PersistentTmStore::buildDumpSet(uint32_t fromUnixSeconds, uint32_t // Multiple files for the same time are supported via a special suffix. We smply count the // number of copies and later try to dump the same number of files with the additonal // suffixes - auto& iter = dumpParams.orderedDumpFilestamps.find(dumpIndex); + auto iter = dumpParams.orderedDumpFilestamps.find(dumpIndex); if (iter != dumpParams.orderedDumpFilestamps.end()) { - iter->additionalFiles++; + dumpIndex.epoch = iter->epoch; + dumpIndex.additionalFiles = iter->additionalFiles + 1; + dumpParams.orderedDumpFilestamps.erase(dumpIndex); } else { dumpIndex.additionalFiles = 0; - dumpParams.orderedDumpFilestamps.emplace(dumpIndex); } + dumpParams.orderedDumpFilestamps.emplace(dumpIndex); } } return returnvalue::OK; @@ -326,9 +328,6 @@ ReturnValue_t PersistentTmStore::loadNextDumpFile() { } ifile.read(reinterpret_cast(fileBuf.data()), static_cast(dumpParams.fileSize)); - if (dumpIndex.additionalFiles > 0) { - dumpParams.currentSameFileIdx++; - } if (dumpIndex.additionalFiles > 0 and not dumpParams.currentSameFileIdx.has_value()) { if (not dumpParams.currentSameFileIdx.has_value()) { // Initialze the file index and stay on same file @@ -336,7 +335,7 @@ ReturnValue_t PersistentTmStore::loadNextDumpFile() { continue; } else if (dumpParams.currentSameFileIdx.value() < dumpIndex.additionalFiles) { - dumpParams.currentSameFileIdx += 1; + dumpParams.currentSameFileIdx = dumpParams.currentSameFileIdx.value() + 1; continue; } } From 88e8268c26c141c9a6ff586f4c2658021215aac2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 17:14:56 +0200 Subject: [PATCH 387/506] start refactoring reboot wd handling --- bsp_q7s/core/CoreController.cpp | 14 +++++++------- bsp_q7s/core/CoreController.h | 6 ++++-- mission/sysDefs.h | 3 ++- unittest/rebootLogic/src/CoreController.cpp | 12 ++++++------ unittest/rebootLogic/src/CoreController.h | 2 +- 5 files changed, 20 insertions(+), 17 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 4e006298..0db3d414 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -321,7 +321,7 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ if (size < 1) { return HasActionsIF::INVALID_PARAMETERS; } - std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_FILE; + std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_WATCHDOG_FILE; parseRebootFile(path, rebootFile); if (data[0] == 0) { rebootFile.enabled = false; @@ -335,7 +335,7 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ return HasActionsIF::EXECUTION_FINISHED; } case (READ_REBOOT_MECHANISM_INFO): { - std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_FILE; + std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_WATCHDOG_FILE; parseRebootFile(path, rebootFile); RebootMechanismPacket packet(rebootFile); ReturnValue_t result = actionHelper.reportData(commandedBy, actionId, &packet); @@ -446,7 +446,7 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ if (size < 1) { return HasActionsIF::INVALID_PARAMETERS; } - std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_FILE; + std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism parseRebootFile(path, rebootFile); rebootFile.maxCount = data[0]; @@ -1629,7 +1629,7 @@ ReturnValue_t CoreController::performSdCardCheck() { void CoreController::performRebootFileHandling(bool recreateFile) { using namespace std; - std::string path = currMntPrefix + REBOOT_FILE; + std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; std::error_code e; if (not std::filesystem::exists(path, e) or recreateFile) { #if OBSW_VERBOSE_LEVEL >= 1 @@ -2009,7 +2009,7 @@ bool CoreController::parseRebootFile(std::string path, RebootFile &rf) { } void CoreController::resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy) { - std::string path = currMntPrefix + REBOOT_FILE; + std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; parseRebootFile(path, rebootFile); if (tgtChip == xsc::ALL_CHIP and tgtCopy == xsc::ALL_COPY) { rebootFile.img00Cnt = 0; @@ -2035,7 +2035,7 @@ void CoreController::resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy) { } void CoreController::rewriteRebootFile(RebootFile file) { - std::string path = currMntPrefix + REBOOT_FILE; + std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; std::ofstream rebootFile(path); if (rebootFile.is_open()) { // Initiate reboot file first. Reboot handling will be on on initialization @@ -2052,7 +2052,7 @@ void CoreController::rewriteRebootFile(RebootFile file) { } void CoreController::setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy) { - std::string path = currMntPrefix + REBOOT_FILE; + std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism parseRebootFile(path, rebootFile); if (tgtChip == xsc::CHIP_0) { diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index 7ab80aa7..8d5c16c5 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -113,10 +113,12 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe const std::string VERSION_FILE = "/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::VERSION_FILE_NAME); - const std::string REBOOT_FILE = - "/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::REBOOT_FILE_NAME); + const std::string REBOOT_WATCHDOG_FILE = + "/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::REBOOT_WATCHDOG_FILE_NAME); const std::string BACKUP_TIME_FILE = "/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::TIME_FILE_NAME); + const std::string REBOOT_COUNTERS_FILE = + "/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::REBOOT_COUNTER_FILE_NAME); static constexpr char CHIP_0_COPY_0_MOUNT_DIR[] = "/tmp/mntupdate-xdi-qspi0-nom-rootfs"; static constexpr char CHIP_0_COPY_1_MOUNT_DIR[] = "/tmp/mntupdate-xdi-qspi0-gold-rootfs"; diff --git a/mission/sysDefs.h b/mission/sysDefs.h index 32ce9642..234b1ff3 100644 --- a/mission/sysDefs.h +++ b/mission/sysDefs.h @@ -41,7 +41,8 @@ enum SystemctlCmd : uint8_t { START = 0, STOP = 1, RESTART = 2, NUM_CMDS = 3 }; static constexpr char CONF_FOLDER[] = "conf"; static constexpr char VERSION_FILE_NAME[] = "version.txt"; -static constexpr char REBOOT_FILE_NAME[] = "reboot.txt"; +static constexpr char REBOOT_WATCHDOG_FILE_NAME[] = "reboot_watchdog.txt"; +static constexpr char REBOOT_COUNTER_FILE_NAME[] = "reboot_counters.txt"; static constexpr char TIME_FILE_NAME[] = "time_backup.txt"; static constexpr uint32_t SYS_ROM_BASE_ADDR = 0x80000000; diff --git a/unittest/rebootLogic/src/CoreController.cpp b/unittest/rebootLogic/src/CoreController.cpp index 2bc8bbef..b99c0114 100644 --- a/unittest/rebootLogic/src/CoreController.cpp +++ b/unittest/rebootLogic/src/CoreController.cpp @@ -21,7 +21,7 @@ CoreController::CoreController() { void CoreController::performRebootFileHandling(bool recreateFile) { using namespace std; - std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_FILE; + std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; if (not std::filesystem::exists(path) or recreateFile) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "CoreController::performRebootFileHandling: Recreating reboot file" << std::endl; @@ -400,7 +400,7 @@ bool CoreController::parseRebootFile(std::string path, RebootFile &rf) { } void CoreController::resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy) { - std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_FILE; + std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism parseRebootFile(path, rebootFile); if (tgtChip == xsc::ALL_CHIP and tgtCopy == xsc::ALL_COPY) { @@ -427,7 +427,7 @@ void CoreController::resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy) { } void CoreController::rewriteRebootFile(RebootFile file) { - std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_FILE; + std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; std::ofstream rebootFile(path); if (rebootFile.is_open()) { // Initiate reboot file first. Reboot handling will be on on initialization @@ -450,7 +450,7 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ if (size < 1) { return HasActionsIF::INVALID_PARAMETERS; } - std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_FILE; + std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism parseRebootFile(path, rebootFile); if (data[0] == 0) { @@ -490,7 +490,7 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ if (size < 1) { return HasActionsIF::INVALID_PARAMETERS; } - std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_FILE; + std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism parseRebootFile(path, rebootFile); rebootFile.maxCount = data[0]; @@ -504,7 +504,7 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ } void CoreController::setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy) { - std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_FILE; + std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism parseRebootFile(path, rebootFile); if (tgtChip == xsc::CHIP_0) { diff --git a/unittest/rebootLogic/src/CoreController.h b/unittest/rebootLogic/src/CoreController.h index 1846c27f..4c3f0e31 100644 --- a/unittest/rebootLogic/src/CoreController.h +++ b/unittest/rebootLogic/src/CoreController.h @@ -39,7 +39,7 @@ class SdCardManager; class CoreController { public: - static constexpr char REBOOT_FILE[] = "/conf/reboot.txt"; + static constexpr char REBOOT_WATCHDOG_FILE[] = "/conf/reboot.txt"; //! [EXPORT] : [COMMENT] The reboot mechanism was triggered. //! P1: First 16 bits: Last Chip, Last 16 bits: Last Copy, //! P2: Each byte is the respective reboot count for the slots From 07df6eb95f3392d737bdfbe1fb4a1eebb4afc394 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 17:29:23 +0200 Subject: [PATCH 388/506] continuing --- bsp_q7s/core/CoreController.cpp | 169 ++++++++++---------- bsp_q7s/core/CoreController.h | 49 +++++- unittest/rebootLogic/src/CoreController.cpp | 157 +++++++++--------- unittest/rebootLogic/src/CoreController.h | 12 +- 4 files changed, 212 insertions(+), 175 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 0db3d414..61ec8659 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -322,13 +322,13 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ return HasActionsIF::INVALID_PARAMETERS; } std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_WATCHDOG_FILE; - parseRebootFile(path, rebootFile); + parseRebootWatchdogFile(path, rebootWatchdogFile); if (data[0] == 0) { - rebootFile.enabled = false; - rewriteRebootFile(rebootFile); + rebootWatchdogFile.enabled = false; + rewriteRebootWatchdogFile(rebootWatchdogFile); } else if (data[0] == 1) { - rebootFile.enabled = true; - rewriteRebootFile(rebootFile); + rebootWatchdogFile.enabled = true; + rewriteRebootWatchdogFile(rebootWatchdogFile); } else { return HasActionsIF::INVALID_PARAMETERS; } @@ -336,8 +336,8 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ } case (READ_REBOOT_MECHANISM_INFO): { std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_WATCHDOG_FILE; - parseRebootFile(path, rebootFile); - RebootMechanismPacket packet(rebootFile); + parseRebootWatchdogFile(path, rebootWatchdogFile); + RebootWatchdogPacket packet(rebootWatchdogFile); ReturnValue_t result = actionHelper.reportData(commandedBy, actionId, &packet); if (result != returnvalue::OK) { return result; @@ -448,9 +448,9 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ } std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism - parseRebootFile(path, rebootFile); - rebootFile.maxCount = data[0]; - rewriteRebootFile(rebootFile); + parseRebootWatchdogFile(path, rebootWatchdogFile); + rebootWatchdogFile.maxCount = data[0]; + rewriteRebootWatchdogFile(rebootWatchdogFile); return HasActionsIF::EXECUTION_FINISHED; } case (XSC_REBOOT_OBC): { @@ -1555,7 +1555,7 @@ void CoreController::performMountedSdCardOperations() { if (not timeFileInitDone) { initClockFromTimeFile(); } - performRebootFileHandling(false); + performRebootWatchdogHandling(false); } backupTimeFileHandler(); }; @@ -1627,7 +1627,7 @@ ReturnValue_t CoreController::performSdCardCheck() { return returnvalue::OK; } -void CoreController::performRebootFileHandling(bool recreateFile) { +void CoreController::performRebootWatchdogHandling(bool recreateFile) { using namespace std; std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; std::error_code e; @@ -1635,109 +1635,110 @@ void CoreController::performRebootFileHandling(bool recreateFile) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "CoreController::performRebootFileHandling: Recreating reboot file" << std::endl; #endif - rebootFile.enabled = false; - rebootFile.img00Cnt = 0; - rebootFile.img01Cnt = 0; - rebootFile.img10Cnt = 0; - rebootFile.img11Cnt = 0; - rebootFile.lastChip = xsc::Chip::CHIP_0; - rebootFile.lastCopy = xsc::Copy::COPY_0; - rebootFile.img00Lock = false; - rebootFile.img01Lock = false; - rebootFile.img10Lock = false; - rebootFile.img11Lock = false; - rebootFile.mechanismNextChip = xsc::Chip::NO_CHIP; - rebootFile.mechanismNextCopy = xsc::Copy::NO_COPY; - rebootFile.bootFlag = false; - rewriteRebootFile(rebootFile); + rebootWatchdogFile.enabled = false; + rebootWatchdogFile.img00Cnt = 0; + rebootWatchdogFile.img01Cnt = 0; + rebootWatchdogFile.img10Cnt = 0; + rebootWatchdogFile.img11Cnt = 0; + rebootWatchdogFile.lastChip = xsc::Chip::CHIP_0; + rebootWatchdogFile.lastCopy = xsc::Copy::COPY_0; + rebootWatchdogFile.img00Lock = false; + rebootWatchdogFile.img01Lock = false; + rebootWatchdogFile.img10Lock = false; + rebootWatchdogFile.img11Lock = false; + rebootWatchdogFile.mechanismNextChip = xsc::Chip::NO_CHIP; + rebootWatchdogFile.mechanismNextCopy = xsc::Copy::NO_COPY; + rebootWatchdogFile.bootFlag = false; + rewriteRebootWatchdogFile(rebootWatchdogFile); } else { - if (not parseRebootFile(path, rebootFile)) { - performRebootFileHandling(true); + if (not parseRebootWatchdogFile(path, rebootWatchdogFile)) { + performRebootWatchdogHandling(true); } } if (CURRENT_CHIP == xsc::CHIP_0) { if (CURRENT_COPY == xsc::COPY_0) { - rebootFile.img00Cnt++; + rebootWatchdogFile.img00Cnt++; } else { - rebootFile.img01Cnt++; + rebootWatchdogFile.img01Cnt++; } } else { if (CURRENT_COPY == xsc::COPY_0) { - rebootFile.img10Cnt++; + rebootWatchdogFile.img10Cnt++; } else { - rebootFile.img11Cnt++; + rebootWatchdogFile.img11Cnt++; } } - if (rebootFile.bootFlag) { + if (rebootWatchdogFile.bootFlag) { // Trigger event to inform ground that a reboot was triggered - uint32_t p1 = rebootFile.lastChip << 16 | rebootFile.lastCopy; + uint32_t p1 = rebootWatchdogFile.lastChip << 16 | rebootWatchdogFile.lastCopy; triggerEvent(core::REBOOT_MECHANISM_TRIGGERED, p1, 0); // Clear the boot flag - rebootFile.bootFlag = false; + rebootWatchdogFile.bootFlag = false; } announceBootCounts(); - if (rebootFile.mechanismNextChip != xsc::NO_CHIP and - rebootFile.mechanismNextCopy != xsc::NO_COPY) { - if (CURRENT_CHIP != rebootFile.mechanismNextChip or - CURRENT_COPY != rebootFile.mechanismNextCopy) { - std::string infoString = std::to_string(rebootFile.mechanismNextChip) + " " + - std::to_string(rebootFile.mechanismNextCopy); + if (rebootWatchdogFile.mechanismNextChip != xsc::NO_CHIP and + rebootWatchdogFile.mechanismNextCopy != xsc::NO_COPY) { + if (CURRENT_CHIP != rebootWatchdogFile.mechanismNextChip or + CURRENT_COPY != rebootWatchdogFile.mechanismNextCopy) { + std::string infoString = std::to_string(rebootWatchdogFile.mechanismNextChip) + " " + + std::to_string(rebootWatchdogFile.mechanismNextCopy); sif::warning << "CoreController::performRebootFileHandling: Expected to be on image " << infoString << " but currently on other image. Locking " << infoString << std::endl; // Firmware or other component might be corrupt and we are on another image then the target // image specified by the mechanism. We can't really trust the target image anymore. // Lock it for now - if (rebootFile.mechanismNextChip == xsc::CHIP_0) { - if (rebootFile.mechanismNextCopy == xsc::COPY_0) { - rebootFile.img00Lock = true; + if (rebootWatchdogFile.mechanismNextChip == xsc::CHIP_0) { + if (rebootWatchdogFile.mechanismNextCopy == xsc::COPY_0) { + rebootWatchdogFile.img00Lock = true; } else { - rebootFile.img01Lock = true; + rebootWatchdogFile.img01Lock = true; } } else { - if (rebootFile.mechanismNextCopy == xsc::COPY_0) { - rebootFile.img10Lock = true; + if (rebootWatchdogFile.mechanismNextCopy == xsc::COPY_0) { + rebootWatchdogFile.img10Lock = true; } else { - rebootFile.img11Lock = true; + rebootWatchdogFile.img11Lock = true; } } } } - rebootFile.lastChip = CURRENT_CHIP; - rebootFile.lastCopy = CURRENT_COPY; + rebootWatchdogFile.lastChip = CURRENT_CHIP; + rebootWatchdogFile.lastCopy = CURRENT_COPY; // Only reboot if the reboot functionality is enabled. // The handler will still increment the boot counts - if (rebootFile.enabled and (*rebootFile.relevantBootCnt >= rebootFile.maxCount)) { + if (rebootWatchdogFile.enabled and + (*rebootWatchdogFile.relevantBootCnt >= rebootWatchdogFile.maxCount)) { // Reboot to other image bool doReboot = false; xsc::Chip tgtChip = xsc::NO_CHIP; xsc::Copy tgtCopy = xsc::NO_COPY; - determineAndExecuteReboot(rebootFile, doReboot, tgtChip, tgtCopy); + determineAndExecuteReboot(rebootWatchdogFile, doReboot, tgtChip, tgtCopy); if (doReboot) { - rebootFile.bootFlag = true; + rebootWatchdogFile.bootFlag = true; #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "Boot counter for image " << CURRENT_CHIP << " " << CURRENT_COPY << " too high. Rebooting to " << tgtChip << " " << tgtCopy << std::endl; #endif - rebootFile.mechanismNextChip = tgtChip; - rebootFile.mechanismNextCopy = tgtCopy; - rewriteRebootFile(rebootFile); + rebootWatchdogFile.mechanismNextChip = tgtChip; + rebootWatchdogFile.mechanismNextCopy = tgtCopy; + rewriteRebootWatchdogFile(rebootWatchdogFile); xsc_boot_copy(static_cast(tgtChip), static_cast(tgtCopy)); } } else { - rebootFile.mechanismNextChip = xsc::NO_CHIP; - rebootFile.mechanismNextCopy = xsc::NO_COPY; + rebootWatchdogFile.mechanismNextChip = xsc::NO_CHIP; + rebootWatchdogFile.mechanismNextCopy = xsc::NO_COPY; } - rewriteRebootFile(rebootFile); + rewriteRebootWatchdogFile(rebootWatchdogFile); } -void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot, +void CoreController::determineAndExecuteReboot(RebootWatchdogFile &rf, bool &needsReboot, xsc::Chip &tgtChip, xsc::Copy &tgtCopy) { tgtChip = xsc::CHIP_0; tgtCopy = xsc::COPY_0; @@ -1826,7 +1827,7 @@ void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot } } -bool CoreController::parseRebootFile(std::string path, RebootFile &rf) { +bool CoreController::parseRebootWatchdogFile(std::string path, RebootWatchdogFile &rf) { using namespace std; std::string selfMatch; if (CURRENT_CHIP == xsc::CHIP_0) { @@ -2010,31 +2011,31 @@ bool CoreController::parseRebootFile(std::string path, RebootFile &rf) { void CoreController::resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy) { std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; - parseRebootFile(path, rebootFile); + parseRebootWatchdogFile(path, rebootWatchdogFile); if (tgtChip == xsc::ALL_CHIP and tgtCopy == xsc::ALL_COPY) { - rebootFile.img00Cnt = 0; - rebootFile.img01Cnt = 0; - rebootFile.img10Cnt = 0; - rebootFile.img11Cnt = 0; + rebootWatchdogFile.img00Cnt = 0; + rebootWatchdogFile.img01Cnt = 0; + rebootWatchdogFile.img10Cnt = 0; + rebootWatchdogFile.img11Cnt = 0; } else { if (tgtChip == xsc::CHIP_0) { if (tgtCopy == xsc::COPY_0) { - rebootFile.img00Cnt = 0; + rebootWatchdogFile.img00Cnt = 0; } else { - rebootFile.img01Cnt = 0; + rebootWatchdogFile.img01Cnt = 0; } } else { if (tgtCopy == xsc::COPY_0) { - rebootFile.img10Cnt = 0; + rebootWatchdogFile.img10Cnt = 0; } else { - rebootFile.img11Cnt = 0; + rebootWatchdogFile.img11Cnt = 0; } } } - rewriteRebootFile(rebootFile); + rewriteRebootWatchdogFile(rebootWatchdogFile); } -void CoreController::rewriteRebootFile(RebootFile file) { +void CoreController::rewriteRebootWatchdogFile(RebootWatchdogFile file) { std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; std::ofstream rebootFile(path); if (rebootFile.is_open()) { @@ -2054,21 +2055,21 @@ void CoreController::rewriteRebootFile(RebootFile file) { void CoreController::setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy) { std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism - parseRebootFile(path, rebootFile); + parseRebootWatchdogFile(path, rebootWatchdogFile); if (tgtChip == xsc::CHIP_0) { if (tgtCopy == xsc::COPY_0) { - rebootFile.img00Lock = lock; + rebootWatchdogFile.img00Lock = lock; } else { - rebootFile.img01Lock = lock; + rebootWatchdogFile.img01Lock = lock; } } else { if (tgtCopy == xsc::COPY_0) { - rebootFile.img10Lock = lock; + rebootWatchdogFile.img10Lock = lock; } else { - rebootFile.img11Lock = lock; + rebootWatchdogFile.img11Lock = lock; } } - rewriteRebootFile(rebootFile); + rewriteRebootWatchdogFile(rebootWatchdogFile); } ReturnValue_t CoreController::backupTimeFileHandler() { @@ -2355,10 +2356,12 @@ bool CoreController::startSdStateMachine(sd::SdCard targetActiveSd, SdCfgMode mo } void CoreController::announceBootCounts() { - uint64_t totalBootCount = - rebootFile.img00Cnt + rebootFile.img01Cnt + rebootFile.img10Cnt + rebootFile.img11Cnt; - uint32_t individualBootCountsP1 = (rebootFile.img00Cnt << 16) | rebootFile.img01Cnt; - uint32_t individualBootCountsP2 = (rebootFile.img10Cnt << 16) | rebootFile.img11Cnt; + uint64_t totalBootCount = rebootWatchdogFile.img00Cnt + rebootWatchdogFile.img01Cnt + + rebootWatchdogFile.img10Cnt + rebootWatchdogFile.img11Cnt; + uint32_t individualBootCountsP1 = + (rebootWatchdogFile.img00Cnt << 16) | rebootWatchdogFile.img01Cnt; + uint32_t individualBootCountsP2 = + (rebootWatchdogFile.img10Cnt << 16) | rebootWatchdogFile.img11Cnt; triggerEvent(core::INDIVIDUAL_BOOT_COUNTS, individualBootCountsP1, individualBootCountsP2); triggerEvent(core::REBOOT_COUNTER, (totalBootCount >> 32) & 0xffffffff, totalBootCount & 0xffffffff); diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index 8d5c16c5..778d3fa0 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -24,7 +24,7 @@ class Timer; class SdCardManager; -struct RebootFile { +struct RebootWatchdogFile { static constexpr uint8_t DEFAULT_MAX_BOOT_CNT = 10; bool enabled = true; @@ -45,9 +45,9 @@ struct RebootFile { xsc::Copy mechanismNextCopy = xsc::Copy::NO_COPY; }; -class RebootMechanismPacket : public SerialLinkedListAdapter { +class RebootWatchdogPacket : public SerialLinkedListAdapter { public: - RebootMechanismPacket(RebootFile& rf) { + RebootWatchdogPacket(RebootWatchdogFile& rf) { enabled = rf.enabled; maxCount = rf.maxCount; img00Count = rf.img00Cnt; @@ -100,6 +100,38 @@ class RebootMechanismPacket : public SerialLinkedListAdapter { SerializeElement nextCopy = 0; }; +struct RebootCountersFile { + // 16 bit values so all boot counters fit into one event. + uint16_t img00Cnt = 0; + uint16_t img01Cnt = 0; + uint16_t img10Cnt = 0; + uint16_t img11Cnt = 0; +}; + +class RebootCountersPacket : public SerialLinkedListAdapter { + RebootCountersPacket(RebootCountersFile& rf) { + img00Count = rf.img00Cnt; + img01Count = rf.img01Cnt; + img10Count = rf.img10Cnt; + img11Count = rf.img11Cnt; + setLinks(); + } + + private: + void setLinks() { + setStart(&img00Count); + img00Count.setNext(&img01Count); + img01Count.setNext(&img10Count); + img10Count.setNext(&img11Count); + setLast(&img11Count); + } + + SerializeElement img00Count = 0; + SerializeElement img01Count = 0; + SerializeElement img10Count = 0; + SerializeElement img11Count = 0; +}; + class CoreController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { public: enum ParamId : uint8_t { PREF_SD = 0, NUM_IDS }; @@ -250,7 +282,8 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe std::array dirListingBuf{}; DirListingDumpContext dumpContext{}; - RebootFile rebootFile = {}; + RebootWatchdogFile rebootWatchdogFile = {}; + RebootCountersFile rebootCountersFile = {}; CommandExecutor cmdExecutor; SimpleRingBuffer cmdReplyBuf; @@ -320,7 +353,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe void currentStateSetter(sd::SdCard sdCard, sd::SdState newState); void executeNextExternalSdCommand(); void checkExternalSdCommandStatus(); - void performRebootFileHandling(bool recreateFile); + void performRebootWatchdogHandling(bool recreateFile); ReturnValue_t actionListDirectoryIntoFile(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size); @@ -339,12 +372,12 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe int handleBootCopyProtAtIndex(xsc::Chip targetChip, xsc::Copy targetCopy, bool protect, bool& protOperationPerformed, bool selfChip, bool selfCopy, bool allChips, bool allCopies, uint8_t arrIdx); - void determineAndExecuteReboot(RebootFile& rf, bool& needsReboot, xsc::Chip& tgtChip, + void determineAndExecuteReboot(RebootWatchdogFile& rf, bool& needsReboot, xsc::Chip& tgtChip, xsc::Copy& tgtCopy); void resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy); void setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy); - bool parseRebootFile(std::string path, RebootFile& file); - void rewriteRebootFile(RebootFile file); + bool parseRebootWatchdogFile(std::string path, RebootWatchdogFile& file); + void rewriteRebootWatchdogFile(RebootWatchdogFile file); void announceBootCounts(); void announceVersionInfo(); void announceCurrentImageInfo(); diff --git a/unittest/rebootLogic/src/CoreController.cpp b/unittest/rebootLogic/src/CoreController.cpp index b99c0114..23461ea1 100644 --- a/unittest/rebootLogic/src/CoreController.cpp +++ b/unittest/rebootLogic/src/CoreController.cpp @@ -19,116 +19,117 @@ CoreController::CoreController() { setCurrentBootCopy(xsc::CHIP_0, xsc::COPY_0); } -void CoreController::performRebootFileHandling(bool recreateFile) { +void CoreController::performRebootWatchdogHandling(bool recreateFile) { using namespace std; std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; if (not std::filesystem::exists(path) or recreateFile) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "CoreController::performRebootFileHandling: Recreating reboot file" << std::endl; #endif - rebootFile.enabled = true; - rebootFile.img00Cnt = 0; - rebootFile.img01Cnt = 0; - rebootFile.img10Cnt = 0; - rebootFile.img11Cnt = 0; - rebootFile.lastChip = xsc::Chip::CHIP_0; - rebootFile.lastCopy = xsc::Copy::COPY_0; - rebootFile.img00Lock = false; - rebootFile.img01Lock = false; - rebootFile.img10Lock = false; - rebootFile.img11Lock = false; - rebootFile.mechanismNextChip = xsc::Chip::NO_CHIP; - rebootFile.mechanismNextCopy = xsc::Copy::NO_COPY; - rebootFile.bootFlag = false; - rewriteRebootFile(rebootFile); + rebootWatchdogFile.enabled = true; + rebootWatchdogFile.img00Cnt = 0; + rebootWatchdogFile.img01Cnt = 0; + rebootWatchdogFile.img10Cnt = 0; + rebootWatchdogFile.img11Cnt = 0; + rebootWatchdogFile.lastChip = xsc::Chip::CHIP_0; + rebootWatchdogFile.lastCopy = xsc::Copy::COPY_0; + rebootWatchdogFile.img00Lock = false; + rebootWatchdogFile.img01Lock = false; + rebootWatchdogFile.img10Lock = false; + rebootWatchdogFile.img11Lock = false; + rebootWatchdogFile.mechanismNextChip = xsc::Chip::NO_CHIP; + rebootWatchdogFile.mechanismNextCopy = xsc::Copy::NO_COPY; + rebootWatchdogFile.bootFlag = false; + rewriteRebootWatchdogFile(rebootWatchdogFile); } else { - if (not parseRebootFile(path, rebootFile)) { - performRebootFileHandling(true); + if (not parseRebootWatchdogFile(path, rebootWatchdogFile)) { + performRebootWatchdogHandling(true); } } if (CURRENT_CHIP == xsc::CHIP_0) { if (CURRENT_COPY == xsc::COPY_0) { - rebootFile.img00Cnt++; + rebootWatchdogFile.img00Cnt++; } else { - rebootFile.img01Cnt++; + rebootWatchdogFile.img01Cnt++; } } else { if (CURRENT_COPY == xsc::COPY_0) { - rebootFile.img10Cnt++; + rebootWatchdogFile.img10Cnt++; } else { - rebootFile.img11Cnt++; + rebootWatchdogFile.img11Cnt++; } } - if (rebootFile.bootFlag) { + if (rebootWatchdogFile.bootFlag) { // Trigger event to inform ground that a reboot was triggered - uint32_t p1 = rebootFile.lastChip << 16 | rebootFile.lastCopy; - uint32_t p2 = rebootFile.img00Cnt << 24 | rebootFile.img01Cnt << 16 | rebootFile.img10Cnt << 8 | - rebootFile.img11Cnt; + uint32_t p1 = rebootWatchdogFile.lastChip << 16 | rebootWatchdogFile.lastCopy; + uint32_t p2 = rebootWatchdogFile.img00Cnt << 24 | rebootWatchdogFile.img01Cnt << 16 | + rebootWatchdogFile.img10Cnt << 8 | rebootWatchdogFile.img11Cnt; triggerEvent(REBOOT_MECHANISM_TRIGGERED, p1, p2); // Clear the boot flag - rebootFile.bootFlag = false; + rebootWatchdogFile.bootFlag = false; } - if (rebootFile.mechanismNextChip != xsc::NO_CHIP and - rebootFile.mechanismNextCopy != xsc::NO_COPY) { - if (CURRENT_CHIP != rebootFile.mechanismNextChip or - CURRENT_COPY != rebootFile.mechanismNextCopy) { - std::string infoString = std::to_string(rebootFile.mechanismNextChip) + " " + - std::to_string(rebootFile.mechanismNextCopy); + if (rebootWatchdogFile.mechanismNextChip != xsc::NO_CHIP and + rebootWatchdogFile.mechanismNextCopy != xsc::NO_COPY) { + if (CURRENT_CHIP != rebootWatchdogFile.mechanismNextChip or + CURRENT_COPY != rebootWatchdogFile.mechanismNextCopy) { + std::string infoString = std::to_string(rebootWatchdogFile.mechanismNextChip) + " " + + std::to_string(rebootWatchdogFile.mechanismNextCopy); sif::warning << "CoreController::performRebootFileHandling: Expected to be on image " << infoString << " but currently on other image. Locking " << infoString << std::endl; // Firmware or other component might be corrupt and we are on another image then the target // image specified by the mechanism. We can't really trust the target image anymore. // Lock it for now - if (rebootFile.mechanismNextChip == xsc::CHIP_0) { - if (rebootFile.mechanismNextCopy == xsc::COPY_0) { - rebootFile.img00Lock = true; + if (rebootWatchdogFile.mechanismNextChip == xsc::CHIP_0) { + if (rebootWatchdogFile.mechanismNextCopy == xsc::COPY_0) { + rebootWatchdogFile.img00Lock = true; } else { - rebootFile.img01Lock = true; + rebootWatchdogFile.img01Lock = true; } } else { - if (rebootFile.mechanismNextCopy == xsc::COPY_0) { - rebootFile.img10Lock = true; + if (rebootWatchdogFile.mechanismNextCopy == xsc::COPY_0) { + rebootWatchdogFile.img10Lock = true; } else { - rebootFile.img11Lock = true; + rebootWatchdogFile.img11Lock = true; } } } } - rebootFile.lastChip = CURRENT_CHIP; - rebootFile.lastCopy = CURRENT_COPY; + rebootWatchdogFile.lastChip = CURRENT_CHIP; + rebootWatchdogFile.lastCopy = CURRENT_COPY; // Only reboot if the reboot functionality is enabled. // The handler will still increment the boot counts - if (rebootFile.enabled and (*rebootFile.relevantBootCnt >= rebootFile.maxCount)) { + if (rebootWatchdogFile.enabled and + (*rebootWatchdogFile.relevantBootCnt >= rebootWatchdogFile.maxCount)) { // Reboot to other image bool doReboot = false; xsc::Chip tgtChip = xsc::NO_CHIP; xsc::Copy tgtCopy = xsc::NO_COPY; - determineAndExecuteReboot(rebootFile, doReboot, tgtChip, tgtCopy); + determineAndExecuteReboot(rebootWatchdogFile, doReboot, tgtChip, tgtCopy); if (doReboot) { - rebootFile.bootFlag = true; + rebootWatchdogFile.bootFlag = true; #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "Boot counter for image " << CURRENT_CHIP << " " << CURRENT_COPY << " too high. Rebooting to " << tgtChip << " " << tgtCopy << std::endl; #endif - rebootFile.mechanismNextChip = tgtChip; - rebootFile.mechanismNextCopy = tgtCopy; - rewriteRebootFile(rebootFile); + rebootWatchdogFile.mechanismNextChip = tgtChip; + rebootWatchdogFile.mechanismNextCopy = tgtCopy; + rewriteRebootWatchdogFile(rebootWatchdogFile); xsc_boot_copy(static_cast(tgtChip), static_cast(tgtCopy)); } } else { - rebootFile.mechanismNextChip = xsc::NO_CHIP; - rebootFile.mechanismNextCopy = xsc::NO_COPY; + rebootWatchdogFile.mechanismNextChip = xsc::NO_CHIP; + rebootWatchdogFile.mechanismNextCopy = xsc::NO_COPY; } - rewriteRebootFile(rebootFile); + rewriteRebootWatchdogFile(rebootWatchdogFile); } -void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot, +void CoreController::determineAndExecuteReboot(RebootWatchdogFile &rf, bool &needsReboot, xsc::Chip &tgtChip, xsc::Copy &tgtCopy) { tgtChip = xsc::CHIP_0; tgtCopy = xsc::COPY_0; @@ -217,7 +218,7 @@ void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot } } -bool CoreController::parseRebootFile(std::string path, RebootFile &rf) { +bool CoreController::parseRebootWatchdogFile(std::string path, RebootWatchdogFile &rf) { using namespace std; std::string selfMatch; if (CURRENT_CHIP == xsc::CHIP_0) { @@ -402,31 +403,31 @@ bool CoreController::parseRebootFile(std::string path, RebootFile &rf) { void CoreController::resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy) { std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism - parseRebootFile(path, rebootFile); + parseRebootWatchdogFile(path, rebootWatchdogFile); if (tgtChip == xsc::ALL_CHIP and tgtCopy == xsc::ALL_COPY) { - rebootFile.img00Cnt = 0; - rebootFile.img01Cnt = 0; - rebootFile.img10Cnt = 0; - rebootFile.img11Cnt = 0; + rebootWatchdogFile.img00Cnt = 0; + rebootWatchdogFile.img01Cnt = 0; + rebootWatchdogFile.img10Cnt = 0; + rebootWatchdogFile.img11Cnt = 0; } else { if (tgtChip == xsc::CHIP_0) { if (tgtCopy == xsc::COPY_0) { - rebootFile.img00Cnt = 0; + rebootWatchdogFile.img00Cnt = 0; } else { - rebootFile.img01Cnt = 0; + rebootWatchdogFile.img01Cnt = 0; } } else { if (tgtCopy == xsc::COPY_0) { - rebootFile.img10Cnt = 0; + rebootWatchdogFile.img10Cnt = 0; } else { - rebootFile.img11Cnt = 0; + rebootWatchdogFile.img11Cnt = 0; } } } - rewriteRebootFile(rebootFile); + rewriteRebootWatchdogFile(rebootWatchdogFile); } -void CoreController::rewriteRebootFile(RebootFile file) { +void CoreController::rewriteRebootWatchdogFile(RebootWatchdogFile file) { std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; std::ofstream rebootFile(path); if (rebootFile.is_open()) { @@ -452,13 +453,13 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ } std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism - parseRebootFile(path, rebootFile); + parseRebootWatchdogFile(path, rebootWatchdogFile); if (data[0] == 0) { - rebootFile.enabled = false; - rewriteRebootFile(rebootFile); + rebootWatchdogFile.enabled = false; + rewriteRebootWatchdogFile(rebootWatchdogFile); } else if (data[0] == 1) { - rebootFile.enabled = true; - rewriteRebootFile(rebootFile); + rebootWatchdogFile.enabled = true; + rewriteRebootWatchdogFile(rebootWatchdogFile); } else { return HasActionsIF::INVALID_PARAMETERS; } @@ -492,9 +493,9 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ } std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism - parseRebootFile(path, rebootFile); - rebootFile.maxCount = data[0]; - rewriteRebootFile(rebootFile); + parseRebootWatchdogFile(path, rebootWatchdogFile); + rebootWatchdogFile.maxCount = data[0]; + rewriteRebootWatchdogFile(rebootWatchdogFile); return HasActionsIF::EXECUTION_FINISHED; } default: { @@ -506,21 +507,21 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ void CoreController::setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy) { std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism - parseRebootFile(path, rebootFile); + parseRebootWatchdogFile(path, rebootWatchdogFile); if (tgtChip == xsc::CHIP_0) { if (tgtCopy == xsc::COPY_0) { - rebootFile.img00Lock = lock; + rebootWatchdogFile.img00Lock = lock; } else { - rebootFile.img01Lock = lock; + rebootWatchdogFile.img01Lock = lock; } } else { if (tgtCopy == xsc::COPY_0) { - rebootFile.img10Lock = lock; + rebootWatchdogFile.img10Lock = lock; } else { - rebootFile.img11Lock = lock; + rebootWatchdogFile.img11Lock = lock; } } - rewriteRebootFile(rebootFile); + rewriteRebootWatchdogFile(rebootWatchdogFile); } void CoreController::setCurrentBootCopy(xsc::Chip chip, xsc::Copy copy) { diff --git a/unittest/rebootLogic/src/CoreController.h b/unittest/rebootLogic/src/CoreController.h index 4c3f0e31..34d2a5f7 100644 --- a/unittest/rebootLogic/src/CoreController.h +++ b/unittest/rebootLogic/src/CoreController.h @@ -14,7 +14,7 @@ enum Copy : int { COPY_0, COPY_1, NO_COPY, SELF_COPY, ALL_COPY }; } // namespace xsc -struct RebootFile { +struct RebootWatchdogFile { static constexpr uint8_t DEFAULT_MAX_BOOT_CNT = 10; bool enabled = true; @@ -57,13 +57,13 @@ class CoreController { static void setCurrentBootCopy(xsc::Chip chip, xsc::Copy copy); ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size); - void performRebootFileHandling(bool recreateFile); - void determineAndExecuteReboot(RebootFile& rf, bool& needsReboot, xsc::Chip& tgtChip, + void performRebootWatchdogHandling(bool recreateFile); + void determineAndExecuteReboot(RebootWatchdogFile& rf, bool& needsReboot, xsc::Chip& tgtChip, xsc::Copy& tgtCopy); void setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy); void resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy); - bool parseRebootFile(std::string path, RebootFile& file); - void rewriteRebootFile(RebootFile file); + bool parseRebootWatchdogFile(std::string path, RebootWatchdogFile& file); + void rewriteRebootWatchdogFile(RebootWatchdogFile file); private: struct SdFsmParams { @@ -74,6 +74,6 @@ class CoreController { } sdInfo; SdCardManager* sdcMan = nullptr; - RebootFile rebootFile = {}; + RebootWatchdogFile rebootWatchdogFile = {}; bool doPerformRebootFileHandling = true; }; \ No newline at end of file From 7f3645585728286e1f0e84aa1d4dad989166d900 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 18:06:58 +0200 Subject: [PATCH 389/506] fix project file, continue core ctrl --- bsp_q7s/core/CoreController.cpp | 58 +++++++++++++++++++++++++++++++-- bsp_q7s/core/CoreController.h | 5 ++- misc/eclipse/.cproject | 39 ++++++++++++++-------- 3 files changed, 85 insertions(+), 17 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 61ec8659..25ce04bb 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -346,12 +346,13 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ } case (RESET_REBOOT_COUNTERS): { if (size == 0) { - resetRebootCount(xsc::ALL_CHIP, xsc::ALL_COPY); + resetRebootWatchdogCounters(xsc::ALL_CHIP, xsc::ALL_COPY); } else if (size == 2) { if (data[0] > 1 or data[1] > 1) { return HasActionsIF::INVALID_PARAMETERS; } - resetRebootCount(static_cast(data[0]), static_cast(data[1])); + resetRebootWatchdogCounters(static_cast(data[0]), + static_cast(data[1])); } return HasActionsIF::EXECUTION_FINISHED; } @@ -2009,7 +2010,56 @@ bool CoreController::parseRebootWatchdogFile(std::string path, RebootWatchdogFil return true; } -void CoreController::resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy) { +bool CoreController::parseRebootCountersFile(std::string path, RebootCountersFile &rf) { + using namespace std; + ifstream file(path); + string word; + string line; + uint8_t lineIdx = 0; + while (std::getline(file, line)) { + istringstream iss(line); + switch (lineIdx) { + case 0: { + iss >> word; + if (word.find("img00:") == string::npos) { + return false; + } + iss >> rf.img00Cnt; + + break; + } + case 1: { + iss >> word; + if (word.find("img01:") == string::npos) { + return false; + } + iss >> rf.img01Cnt; + + break; + } + case 2: { + iss >> word; + if (word.find("img10:") == string::npos) { + return false; + } + iss >> rf.img10Cnt; + + break; + } + case 3: { + iss >> word; + if (word.find("img11:") == string::npos) { + return false; + } + iss >> rf.img11Cnt; + break; + } + } + } + return true; +} + +void CoreController::resetRebootWatchdogCounters(xsc::Chip tgtChip, xsc::Copy tgtCopy) { std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; parseRebootWatchdogFile(path, rebootWatchdogFile); if (tgtChip == xsc::ALL_CHIP and tgtCopy == xsc::ALL_COPY) { @@ -2035,6 +2085,8 @@ void CoreController::resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy) { rewriteRebootWatchdogFile(rebootWatchdogFile); } +void CoreController::performRebootCountersHandling(bool recreateFile) { +} void CoreController::rewriteRebootWatchdogFile(RebootWatchdogFile file) { std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; std::ofstream rebootFile(path); diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index 778d3fa0..bfca7f27 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -354,6 +354,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe void executeNextExternalSdCommand(); void checkExternalSdCommandStatus(); void performRebootWatchdogHandling(bool recreateFile); + void performRebootCountersHandling(bool recreateFile); ReturnValue_t actionListDirectoryIntoFile(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size); @@ -374,10 +375,12 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe bool allChips, bool allCopies, uint8_t arrIdx); void determineAndExecuteReboot(RebootWatchdogFile& rf, bool& needsReboot, xsc::Chip& tgtChip, xsc::Copy& tgtCopy); - void resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy); + void resetRebootWatchdogCounters(xsc::Chip tgtChip, xsc::Copy tgtCopy); void setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy); bool parseRebootWatchdogFile(std::string path, RebootWatchdogFile& file); + bool parseRebootCountersFile(std::string path, RebootCountersFile& file); void rewriteRebootWatchdogFile(RebootWatchdogFile file); + void rewriteRebootCountersFile(RebootCountersFile file); void announceBootCounts(); void announceVersionInfo(); void announceCurrentImageInfo(); diff --git a/misc/eclipse/.cproject b/misc/eclipse/.cproject index 4cfe3fa1..154cb27e 100644 --- a/misc/eclipse/.cproject +++ b/misc/eclipse/.cproject @@ -57,7 +57,8 @@ - + + @@ -119,7 +120,8 @@ - + + @@ -187,7 +189,8 @@ - + + @@ -255,7 +258,8 @@ - + + @@ -418,7 +422,8 @@ - + + @@ -580,7 +585,8 @@ - + + @@ -750,7 +756,8 @@ - + + @@ -917,7 +924,8 @@ - + + @@ -1084,7 +1092,8 @@ - + + @@ -1149,7 +1158,8 @@ - + + @@ -1172,7 +1182,7 @@ - + - + + + @@ -1386,7 +1398,8 @@ - + + From 4e7b774d32b93bdf305f5bf252f15bd1febbd753 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 18:37:58 +0200 Subject: [PATCH 390/506] some legacy/backword compatiblity additions --- bsp_q7s/core/CoreController.cpp | 72 +++++++++++++++++++++++++++++---- bsp_q7s/core/CoreController.h | 3 ++ mission/sysDefs.h | 1 + tmtc | 2 +- 4 files changed, 69 insertions(+), 9 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 25ce04bb..b3f91506 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -1557,6 +1557,7 @@ void CoreController::performMountedSdCardOperations() { initClockFromTimeFile(); } performRebootWatchdogHandling(false); + performRebootCountersHandling(false); } backupTimeFileHandler(); }; @@ -1631,10 +1632,17 @@ ReturnValue_t CoreController::performSdCardCheck() { void CoreController::performRebootWatchdogHandling(bool recreateFile) { using namespace std; std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; + std::string legacyPath = currMntPrefix + LEGACY_REBOOT_WATCHDOG_FILE; std::error_code e; + // TODO: Remove at some point in the future. + if (std::filesystem::exists(legacyPath)) { + // Old file might still exist, so copy it to new path + std::filesystem::copy(legacyPath, path); + } if (not std::filesystem::exists(path, e) or recreateFile) { #if OBSW_VERBOSE_LEVEL >= 1 - sif::info << "CoreController::performRebootFileHandling: Recreating reboot file" << std::endl; + sif::info << "CoreController::performRebootFileHandling: Recreating reboot watchdog file" + << std::endl; #endif rebootWatchdogFile.enabled = false; rebootWatchdogFile.img00Cnt = 0; @@ -1654,6 +1662,7 @@ void CoreController::performRebootWatchdogHandling(bool recreateFile) { } else { if (not parseRebootWatchdogFile(path, rebootWatchdogFile)) { performRebootWatchdogHandling(true); + return; } } @@ -1679,8 +1688,6 @@ void CoreController::performRebootWatchdogHandling(bool recreateFile) { rebootWatchdogFile.bootFlag = false; } - announceBootCounts(); - if (rebootWatchdogFile.mechanismNextChip != xsc::NO_CHIP and rebootWatchdogFile.mechanismNextCopy != xsc::NO_COPY) { if (CURRENT_CHIP != rebootWatchdogFile.mechanismNextChip or @@ -2086,9 +2093,45 @@ void CoreController::resetRebootWatchdogCounters(xsc::Chip tgtChip, xsc::Copy tg } void CoreController::performRebootCountersHandling(bool recreateFile) { + std::string path = currMntPrefix + REBOOT_COUNTERS_FILE; + std::error_code e; + if (not std::filesystem::exists(path, e) or recreateFile) { +#if OBSW_VERBOSE_LEVEL >= 1 + sif::info << "CoreController::performRebootFileHandling: Recreating reboot counters file" + << std::endl; +#endif + rebootCountersFile.img00Cnt = 0; + rebootCountersFile.img01Cnt = 0; + rebootCountersFile.img10Cnt = 0; + rebootCountersFile.img11Cnt = 0; + rewriteRebootCountersFile(rebootCountersFile); + } else { + if (not parseRebootCountersFile(path, rebootCountersFile)) { + performRebootCountersHandling(true); + return; + } + } + + if (CURRENT_CHIP == xsc::CHIP_0) { + if (CURRENT_COPY == xsc::COPY_0) { + rebootCountersFile.img00Cnt++; + } else { + rebootCountersFile.img01Cnt++; + } + } else { + if (CURRENT_COPY == xsc::COPY_0) { + rebootCountersFile.img10Cnt++; + } else { + rebootCountersFile.img11Cnt++; + } + } + announceBootCounts(); + rewriteRebootCountersFile(rebootCountersFile); } void CoreController::rewriteRebootWatchdogFile(RebootWatchdogFile file) { + using namespace std::filesystem; std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; + std::string legacyPath = currMntPrefix + LEGACY_REBOOT_WATCHDOG_FILE; std::ofstream rebootFile(path); if (rebootFile.is_open()) { // Initiate reboot file first. Reboot handling will be on on initialization @@ -2102,11 +2145,24 @@ void CoreController::rewriteRebootWatchdogFile(RebootWatchdogFile file) { << "\nnext: " << static_cast(file.mechanismNextChip) << " " << static_cast(file.mechanismNextCopy) << "\n"; } + // TODO: Remove at some point in the future when all images have been updated. + if (std::filesystem::exists(legacyPath)) { + // Keep those two files in sync + std::filesystem::copy(path, legacyPath); + } +} + +void CoreController::rewriteRebootCountersFile(RebootCountersFile file) { + std::string path = currMntPrefix + REBOOT_COUNTERS_FILE; + std::ofstream rebootFile(path); + if (rebootFile.is_open()) { + rebootFile << "\nimg00: " << file.img00Cnt << "\nimg01: " << file.img01Cnt + << "\nimg10: " << file.img10Cnt << "\nimg11: " << file.img11Cnt << "\n"; + } } void CoreController::setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy) { std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; - // Disable the reboot file mechanism parseRebootWatchdogFile(path, rebootWatchdogFile); if (tgtChip == xsc::CHIP_0) { if (tgtCopy == xsc::COPY_0) { @@ -2408,12 +2464,12 @@ bool CoreController::startSdStateMachine(sd::SdCard targetActiveSd, SdCfgMode mo } void CoreController::announceBootCounts() { - uint64_t totalBootCount = rebootWatchdogFile.img00Cnt + rebootWatchdogFile.img01Cnt + - rebootWatchdogFile.img10Cnt + rebootWatchdogFile.img11Cnt; + uint64_t totalBootCount = rebootCountersFile.img00Cnt + rebootCountersFile.img01Cnt + + rebootCountersFile.img10Cnt + rebootCountersFile.img11Cnt; uint32_t individualBootCountsP1 = - (rebootWatchdogFile.img00Cnt << 16) | rebootWatchdogFile.img01Cnt; + (rebootCountersFile.img00Cnt << 16) | rebootCountersFile.img01Cnt; uint32_t individualBootCountsP2 = - (rebootWatchdogFile.img10Cnt << 16) | rebootWatchdogFile.img11Cnt; + (rebootCountersFile.img10Cnt << 16) | rebootCountersFile.img11Cnt; triggerEvent(core::INDIVIDUAL_BOOT_COUNTS, individualBootCountsP1, individualBootCountsP2); triggerEvent(core::REBOOT_COUNTER, (totalBootCount >> 32) & 0xffffffff, totalBootCount & 0xffffffff); diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index bfca7f27..e3f514d9 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -145,6 +145,9 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe const std::string VERSION_FILE = "/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::VERSION_FILE_NAME); + const std::string LEGACY_REBOOT_WATCHDOG_FILE = + "/" + std::string(core::CONF_FOLDER) + "/" + + std::string(core::LEGACY_REBOOT_WATCHDOG_FILE_NAME); const std::string REBOOT_WATCHDOG_FILE = "/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::REBOOT_WATCHDOG_FILE_NAME); const std::string BACKUP_TIME_FILE = diff --git a/mission/sysDefs.h b/mission/sysDefs.h index 234b1ff3..fe572144 100644 --- a/mission/sysDefs.h +++ b/mission/sysDefs.h @@ -41,6 +41,7 @@ enum SystemctlCmd : uint8_t { START = 0, STOP = 1, RESTART = 2, NUM_CMDS = 3 }; static constexpr char CONF_FOLDER[] = "conf"; static constexpr char VERSION_FILE_NAME[] = "version.txt"; +static constexpr char LEGACY_REBOOT_WATCHDOG_FILE_NAME[] = "reboot.txt"; static constexpr char REBOOT_WATCHDOG_FILE_NAME[] = "reboot_watchdog.txt"; static constexpr char REBOOT_COUNTER_FILE_NAME[] = "reboot_counters.txt"; static constexpr char TIME_FILE_NAME[] = "time_backup.txt"; diff --git a/tmtc b/tmtc index c9f4a807..deb0275b 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit c9f4a8070d20bc659809d5b822ac5a17548f57a4 +Subproject commit deb0275bb5603394122e26f74760d2051685f324 From 6040cd2d1ed3de7e98c45e2a26e4641d3fa8b62e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 18:48:12 +0200 Subject: [PATCH 391/506] some important tweaks --- bsp_q7s/core/CoreController.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index b3f91506..9bd323a0 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -1637,7 +1637,10 @@ void CoreController::performRebootWatchdogHandling(bool recreateFile) { // TODO: Remove at some point in the future. if (std::filesystem::exists(legacyPath)) { // Old file might still exist, so copy it to new path - std::filesystem::copy(legacyPath, path); + std::filesystem::copy(legacyPath, path, std::filesystem::copy_options::overwrite_existing, e); + if (e) { + sif::error << "File copy has failed: " << e.message() << std::endl; + } } if (not std::filesystem::exists(path, e) or recreateFile) { #if OBSW_VERBOSE_LEVEL >= 1 @@ -2145,10 +2148,14 @@ void CoreController::rewriteRebootWatchdogFile(RebootWatchdogFile file) { << "\nnext: " << static_cast(file.mechanismNextChip) << " " << static_cast(file.mechanismNextCopy) << "\n"; } + std::error_code e; // TODO: Remove at some point in the future when all images have been updated. if (std::filesystem::exists(legacyPath)) { // Keep those two files in sync - std::filesystem::copy(path, legacyPath); + std::filesystem::copy(path, legacyPath, std::filesystem::copy_options::overwrite_existing, e); + if (e) { + sif::error << "File copy has failed: " << e.message() << std::endl; + } } } From 5420e322f715e0c28cb67cd8ad0c4529e2738f48 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 18:52:29 +0200 Subject: [PATCH 392/506] this is annoying to test.. --- bsp_q7s/core/CoreController.cpp | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 9bd323a0..4257d008 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -2135,18 +2135,20 @@ void CoreController::rewriteRebootWatchdogFile(RebootWatchdogFile file) { using namespace std::filesystem; std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; std::string legacyPath = currMntPrefix + LEGACY_REBOOT_WATCHDOG_FILE; - std::ofstream rebootFile(path); - if (rebootFile.is_open()) { - // Initiate reboot file first. Reboot handling will be on on initialization - rebootFile << "on: " << file.enabled << "\nmaxcnt: " << file.maxCount - << "\nimg00: " << file.img00Cnt << "\nimg01: " << file.img01Cnt - << "\nimg10: " << file.img10Cnt << "\nimg11: " << file.img11Cnt - << "\nimg00lock: " << file.img00Lock << "\nimg01lock: " << file.img01Lock - << "\nimg10lock: " << file.img10Lock << "\nimg11lock: " << file.img11Lock - << "\nbootflag: " << file.bootFlag << "\nlast: " << static_cast(file.lastChip) - << " " << static_cast(file.lastCopy) - << "\nnext: " << static_cast(file.mechanismNextChip) << " " - << static_cast(file.mechanismNextCopy) << "\n"; + { + std::ofstream rebootFile(path); + if (rebootFile.is_open()) { + // Initiate reboot file first. Reboot handling will be on on initialization + rebootFile << "on: " << file.enabled << "\nmaxcnt: " << file.maxCount + << "\nimg00: " << file.img00Cnt << "\nimg01: " << file.img01Cnt + << "\nimg10: " << file.img10Cnt << "\nimg11: " << file.img11Cnt + << "\nimg00lock: " << file.img00Lock << "\nimg01lock: " << file.img01Lock + << "\nimg10lock: " << file.img10Lock << "\nimg11lock: " << file.img11Lock + << "\nbootflag: " << file.bootFlag << "\nlast: " << static_cast(file.lastChip) + << " " << static_cast(file.lastCopy) + << "\nnext: " << static_cast(file.mechanismNextChip) << " " + << static_cast(file.mechanismNextCopy) << "\n"; + } } std::error_code e; // TODO: Remove at some point in the future when all images have been updated. From 4465170747ce9f488266e2b49457e318d5c86c1f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 19:00:09 +0200 Subject: [PATCH 393/506] i hope that is the last bug.. --- bsp_q7s/core/CoreController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 4257d008..6a21156e 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -2165,7 +2165,7 @@ void CoreController::rewriteRebootCountersFile(RebootCountersFile file) { std::string path = currMntPrefix + REBOOT_COUNTERS_FILE; std::ofstream rebootFile(path); if (rebootFile.is_open()) { - rebootFile << "\nimg00: " << file.img00Cnt << "\nimg01: " << file.img01Cnt + rebootFile << "img00: " << file.img00Cnt << "\nimg01: " << file.img01Cnt << "\nimg10: " << file.img10Cnt << "\nimg11: " << file.img11Cnt << "\n"; } } From 5c9d7a43ab7da8eac0d665aaa6e711bd9c08c8a8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 19:22:25 +0200 Subject: [PATCH 394/506] that reboot was not graceful.. --- bsp_q7s/core/CoreController.cpp | 90 ++++++++++++++++++--------------- bsp_q7s/core/CoreController.h | 1 + 2 files changed, 49 insertions(+), 42 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 6a21156e..51c04fcc 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -1209,45 +1209,7 @@ ReturnValue_t CoreController::actionXscReboot(const uint8_t *data, size_t size) auto tgtChip = static_cast(data[1]); auto tgtCopy = static_cast(data[2]); - // This function can not really fail - gracefulShutdownTasks(tgtChip, tgtCopy, protOpPerformed); - - switch (tgtChip) { - case (xsc::Chip::CHIP_0): { - switch (tgtCopy) { - case (xsc::Copy::COPY_0): { - xsc_boot_copy(XSC_LIBNOR_CHIP_0, XSC_LIBNOR_COPY_NOMINAL); - break; - } - case (xsc::Copy::COPY_1): { - xsc_boot_copy(XSC_LIBNOR_CHIP_0, XSC_LIBNOR_COPY_GOLD); - break; - } - default: { - break; - } - } - break; - } - case (xsc::Chip::CHIP_1): { - switch (tgtCopy) { - case (xsc::Copy::COPY_0): { - xsc_boot_copy(XSC_LIBNOR_CHIP_1, XSC_LIBNOR_COPY_NOMINAL); - break; - } - case (xsc::Copy::COPY_1): { - xsc_boot_copy(XSC_LIBNOR_CHIP_1, XSC_LIBNOR_COPY_GOLD); - break; - } - default: { - break; - } - } - break; - } - default: - break; - } + performGracefulShutdown(tgtChip, tgtCopy); return returnvalue::FAILED; } @@ -1267,7 +1229,8 @@ ReturnValue_t CoreController::gracefulShutdownTasks(xsc::Chip chip, xsc::Copy co // Attempt graceful shutdown by unmounting and switching off SD cards sdcMan->switchOffSdCard(sd::SdCard::SLOT_0); sdcMan->switchOffSdCard(sd::SdCard::SLOT_1); - // If any boot copies are unprotected + // If any boot copies are unprotected. + // Actually this function only ensures that reboots to the own image are protected.. ReturnValue_t result = setBootCopyProtection(xsc::Chip::SELF_CHIP, xsc::Copy::SELF_COPY, true, protOpPerformed, false); if (result == returnvalue::OK and protOpPerformed) { @@ -1739,8 +1702,7 @@ void CoreController::performRebootWatchdogHandling(bool recreateFile) { rebootWatchdogFile.mechanismNextChip = tgtChip; rebootWatchdogFile.mechanismNextCopy = tgtCopy; rewriteRebootWatchdogFile(rebootWatchdogFile); - xsc_boot_copy(static_cast(tgtChip), - static_cast(tgtCopy)); + performGracefulShutdown(tgtChip, tgtCopy); } } else { rebootWatchdogFile.mechanismNextChip = xsc::NO_CHIP; @@ -2563,6 +2525,50 @@ void CoreController::announceCurrentImageInfo() { triggerEvent(CURRENT_IMAGE_INFO, CURRENT_CHIP, CURRENT_COPY); } +ReturnValue_t CoreController::performGracefulShutdown(xsc::Chip tgtChip, xsc::Copy tgtCopy) { + bool protOpPerformed = false; + // This function can not really fail + gracefulShutdownTasks(tgtChip, tgtCopy, protOpPerformed); + + switch (tgtChip) { + case (xsc::Chip::CHIP_0): { + switch (tgtCopy) { + case (xsc::Copy::COPY_0): { + xsc_boot_copy(XSC_LIBNOR_CHIP_0, XSC_LIBNOR_COPY_NOMINAL); + break; + } + case (xsc::Copy::COPY_1): { + xsc_boot_copy(XSC_LIBNOR_CHIP_0, XSC_LIBNOR_COPY_GOLD); + break; + } + default: { + break; + } + } + break; + } + case (xsc::Chip::CHIP_1): { + switch (tgtCopy) { + case (xsc::Copy::COPY_0): { + xsc_boot_copy(XSC_LIBNOR_CHIP_1, XSC_LIBNOR_COPY_NOMINAL); + break; + } + case (xsc::Copy::COPY_1): { + xsc_boot_copy(XSC_LIBNOR_CHIP_1, XSC_LIBNOR_COPY_GOLD); + break; + } + default: { + break; + } + } + break; + } + default: + break; + } + return returnvalue::OK; +} + bool CoreController::isNumber(const std::string &s) { return !s.empty() && std::find_if(s.begin(), s.end(), [](unsigned char c) { return !std::isdigit(c); }) == s.end(); diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index e3f514d9..4bce79da 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -363,6 +363,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe const uint8_t* data, size_t size); ReturnValue_t actionListDirectoryDumpDirectly(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size); + ReturnValue_t performGracefulShutdown(xsc::Chip targetChip, xsc::Copy targetCopy); ReturnValue_t actionListDirectoryCommonCommandCreator(const uint8_t* data, size_t size, std::ostringstream& oss); From 966faf51b5345878911e534caffa451c096f8677 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 19:55:56 +0200 Subject: [PATCH 395/506] last important bugfixes --- bsp_q7s/core/CoreController.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 51c04fcc..85093fa3 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -1226,6 +1226,10 @@ ReturnValue_t CoreController::gracefulShutdownTasks(xsc::Chip chip, xsc::Copy co sdcMan->markUnusable(); // Wait two seconds to ensure no one uses the SD cards TaskFactory::delayTask(2000); + + // Ensure that all writes/reads do finish. + sync(); + // Attempt graceful shutdown by unmounting and switching off SD cards sdcMan->switchOffSdCard(sd::SdCard::SLOT_0); sdcMan->switchOffSdCard(sd::SdCard::SLOT_1); @@ -2027,6 +2031,7 @@ bool CoreController::parseRebootCountersFile(std::string path, RebootCountersFil break; } } + lineIdx++; } return true; } From d95ecc36781eae2e4e20df3cc4485f4498185c25 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 19:57:16 +0200 Subject: [PATCH 396/506] changelog --- CHANGELOG.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3a4eac1f..443ea5bc 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,15 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v4.1.0] + +## Changed + +- Added `sync` syscall in graceful shutdown handler +- Graceful shutdown is now performed by the reboot watchdog +- There is now a separate file for the total reboot counter. The reboot watchdog has its own local + counters to determine whether a reboot is necessary. + # [v4.0.1] 2023-06-24 ## Fixed From facfad3367b3fa9a2044a937b436af5da0d5a651 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 20:12:37 +0200 Subject: [PATCH 397/506] v3.1.1 changelog --- CHANGELOG.md | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3a4eac1f..fb62b8fc 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -52,6 +52,17 @@ will consitute of a breaking change warranting a new major release: restore the PL I2C. - Core controller now announces firmware version as well when requesting a version info event +# [v3.3.1] 2023-06-22 + +## Fixed + +- `PusLiveDemux` packet demultiplexing bugfix where the demultiplexing did not work when there was + only one destination available. + +## Fixed + +- Fixed H parameter in SUS converter from 1 mm to 2.5 mm. + # [v3.3.0] 2023-06-21 Like v3.2.0 but without the custom FM changes related to VC0. From 25d10e3877fe85cc9d3ee0f5596337797b0a0505 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 20:57:54 +0200 Subject: [PATCH 398/506] that was all the fixes (hopefully) --- mission/tmtc/PersistentTmStore.cpp | 50 +++++++++++++++++------------- mission/tmtc/PersistentTmStore.h | 4 +-- 2 files changed, 31 insertions(+), 23 deletions(-) diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index f68299a0..825f9d73 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -79,8 +79,8 @@ ReturnValue_t PersistentTmStore::buildDumpSet(uint32_t fromUnixSeconds, uint32_t } DumpIndex dumpIndex; dumpIndex.epoch = fileEpoch; - // Multiple files for the same time are supported via a special suffix. We smply count the - // number of copies and later try to dump the same number of files with the additonal + // Multiple files for the same time are supported via a special suffix. We simply count the + // number of copies and later try to dump the same number of files with the additional // suffixes auto iter = dumpParams.orderedDumpFilestamps.find(dumpIndex); if (iter != dumpParams.orderedDumpFilestamps.end()) { @@ -295,6 +295,7 @@ ReturnValue_t PersistentTmStore::startDumpFromUpTo(uint32_t fromUnixSeconds, return DUMP_DONE; } dumpParams.dumpIter = dumpParams.orderedDumpFilestamps.begin(); + dumpParams.currentSameFileIdx = std::nullopt; state = State::DUMPING; return loadNextDumpFile(); } @@ -303,7 +304,25 @@ ReturnValue_t PersistentTmStore::loadNextDumpFile() { using namespace std::filesystem; dumpParams.currentSize = 0; std::error_code e; - for (; dumpParams.dumpIter != dumpParams.orderedDumpFilestamps.end(); dumpParams.dumpIter++) { + // Handle iteration, which does not necessarily have to increment the set iterator + // if there are multiple files for a given timestamp. + auto handleIteration = [&](DumpIndex& dumpIndex) { + if (dumpIndex.additionalFiles > 0) { + if (not dumpParams.currentSameFileIdx.has_value()) { + // Initialize the file index and stay on same file + dumpParams.currentSameFileIdx = 0; + return; + } else if (dumpParams.currentSameFileIdx.value() < dumpIndex.additionalFiles - 1) { + dumpParams.currentSameFileIdx = dumpParams.currentSameFileIdx.value() + 1; + return; + } + } + // File will change, reset this field for correct state-keeping. + dumpParams.currentSameFileIdx = std::nullopt; + // Increment iterator for next cycle. + dumpParams.dumpIter++; + }; + while (dumpParams.dumpIter != dumpParams.orderedDumpFilestamps.end()) { DumpIndex dumpIndex = *dumpParams.dumpIter; timeval tv{}; tv.tv_sec = dumpIndex.epoch; @@ -311,38 +330,27 @@ ReturnValue_t PersistentTmStore::loadNextDumpFile() { createFileName(tv, dumpParams.currentSameFileIdx, fullPathLength); dumpParams.currentFile = path(std::string(reinterpret_cast(filePathBuf.data()), fullPathLength)); + if (DEBUG_DUMPS) { + sif::debug << baseName << " dump: Loading " << dumpParams.currentFile << std::endl; + } dumpParams.fileSize = std::filesystem::file_size(dumpParams.currentFile, e); if (e) { // TODO: Event? sif::error << "PersistentTmStore: Could not load next dump file: " << e.message() << std::endl; + handleIteration(dumpIndex); continue; } std::ifstream ifile(dumpParams.currentFile, std::ios::binary); if (ifile.bad()) { sif::error << "PersistentTmStore: File is bad. Loading next file" << std::endl; + handleIteration(dumpIndex); continue; } - if (DEBUG_DUMPS) { - sif::debug << baseName << " dump: Loading " << dumpParams.currentFile << std::endl; - } ifile.read(reinterpret_cast(fileBuf.data()), static_cast(dumpParams.fileSize)); - if (dumpIndex.additionalFiles > 0 and not dumpParams.currentSameFileIdx.has_value()) { - if (not dumpParams.currentSameFileIdx.has_value()) { - // Initialze the file index and stay on same file - dumpParams.currentSameFileIdx = 0; - continue; - - } else if (dumpParams.currentSameFileIdx.value() < dumpIndex.additionalFiles) { - dumpParams.currentSameFileIdx = dumpParams.currentSameFileIdx.value() + 1; - continue; - } - } - // File will change, reset this field for correct state-keeping. - dumpParams.currentSameFileIdx = std::nullopt; - // Increment iterator for next cycle. - dumpParams.dumpIter++; + // Next file is loaded, exit the loop. + handleIteration(dumpIndex); return returnvalue::OK; } // Directory iterator was consumed and we are done. diff --git a/mission/tmtc/PersistentTmStore.h b/mission/tmtc/PersistentTmStore.h index 54b87a42..e86acaaf 100644 --- a/mission/tmtc/PersistentTmStore.h +++ b/mission/tmtc/PersistentTmStore.h @@ -39,9 +39,9 @@ struct PersistentTmStoreArgs { }; struct DumpIndex { - uint32_t epoch; + uint32_t epoch = 0; // Number of additional files with a suffix like .0, .1 etc. - uint8_t additionalFiles; + uint8_t additionalFiles = 0; // Define a custom comparison function based on the epoch variable bool operator<(const DumpIndex& other) const { return epoch < other.epoch; } }; From c50a74d11719c2558b7ccc2f2fcc6088b9734a69 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 24 Jun 2023 21:00:57 +0200 Subject: [PATCH 399/506] delete ordering --- mission/tmtc/PersistentTmStore.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index 825f9d73..acef0b3a 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -17,7 +17,7 @@ using namespace returnvalue; -static constexpr bool DEBUG_DUMPS = true; +static constexpr bool DEBUG_DUMPS = false; PersistentTmStore::PersistentTmStore(PersistentTmStoreArgs args) : SystemObject(args.objectId), From c2b415bdd601869811bd1945b32d9ea0d6ab2469 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 25 Jun 2023 01:01:21 +0200 Subject: [PATCH 400/506] update .cproject file --- misc/eclipse/.cproject | 39 ++++++++++++++++++++++++++------------- 1 file changed, 26 insertions(+), 13 deletions(-) diff --git a/misc/eclipse/.cproject b/misc/eclipse/.cproject index 4cfe3fa1..154cb27e 100644 --- a/misc/eclipse/.cproject +++ b/misc/eclipse/.cproject @@ -57,7 +57,8 @@ - + + @@ -119,7 +120,8 @@ - + + @@ -187,7 +189,8 @@ - + + @@ -255,7 +258,8 @@ - + + @@ -418,7 +422,8 @@ - + + @@ -580,7 +585,8 @@ - + + @@ -750,7 +756,8 @@ - + + @@ -917,7 +924,8 @@ - + + @@ -1084,7 +1092,8 @@ - + + @@ -1149,7 +1158,8 @@ - + + @@ -1172,7 +1182,7 @@ - + - + + + @@ -1386,7 +1398,8 @@ - + + From d88ffb17341e5ec56ec715c868e76dc2f9581e7c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 25 Jun 2023 10:24:27 +0200 Subject: [PATCH 401/506] small fix --- bsp_q7s/core/CoreController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 85093fa3..9157c59b 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -1602,7 +1602,7 @@ void CoreController::performRebootWatchdogHandling(bool recreateFile) { std::string legacyPath = currMntPrefix + LEGACY_REBOOT_WATCHDOG_FILE; std::error_code e; // TODO: Remove at some point in the future. - if (std::filesystem::exists(legacyPath)) { + if (std::filesystem::exists(legacyPath, e)) { // Old file might still exist, so copy it to new path std::filesystem::copy(legacyPath, path, std::filesystem::copy_options::overwrite_existing, e); if (e) { From 3ee2e72652262743089022c87b78003a28b2047d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 25 Jun 2023 10:45:04 +0200 Subject: [PATCH 402/506] better name for function --- bsp_q7s/core/CoreController.cpp | 4 ++-- bsp_q7s/core/CoreController.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 9157c59b..a71f455a 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -1696,7 +1696,7 @@ void CoreController::performRebootWatchdogHandling(bool recreateFile) { bool doReboot = false; xsc::Chip tgtChip = xsc::NO_CHIP; xsc::Copy tgtCopy = xsc::NO_COPY; - determineAndExecuteReboot(rebootWatchdogFile, doReboot, tgtChip, tgtCopy); + rebootWatchdogAlgorithm((rebootFile, doReboot, tgtChip, tgtCopy); if (doReboot) { rebootWatchdogFile.bootFlag = true; #if OBSW_VERBOSE_LEVEL >= 1 @@ -1715,7 +1715,7 @@ void CoreController::performRebootWatchdogHandling(bool recreateFile) { rewriteRebootWatchdogFile(rebootWatchdogFile); } -void CoreController::determineAndExecuteReboot(RebootWatchdogFile &rf, bool &needsReboot, +void CoreController::rebootWatchdogAlgorithm(RebootFile &rf, bool &needsReboot, xsc::Chip &tgtChip, xsc::Copy &tgtCopy) { tgtChip = xsc::CHIP_0; tgtCopy = xsc::COPY_0; diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index 4bce79da..fb7becf6 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -377,7 +377,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe int handleBootCopyProtAtIndex(xsc::Chip targetChip, xsc::Copy targetCopy, bool protect, bool& protOperationPerformed, bool selfChip, bool selfCopy, bool allChips, bool allCopies, uint8_t arrIdx); - void determineAndExecuteReboot(RebootWatchdogFile& rf, bool& needsReboot, xsc::Chip& tgtChip, + void rebootWatchdogAlgorithm(RebootFile& rf, bool& needsReboot, xsc::Chip& tgtChip, xsc::Copy& tgtCopy); void resetRebootWatchdogCounters(xsc::Chip tgtChip, xsc::Copy tgtCopy); void setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy); From 5a642b7d2f1264225355f4c690ef120a0ed336d6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 25 Jun 2023 11:04:13 +0200 Subject: [PATCH 403/506] update core ctrl --- bsp_q7s/core/CoreController.cpp | 6 +++--- bsp_q7s/core/CoreController.h | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index a71f455a..f9242da3 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -1696,7 +1696,7 @@ void CoreController::performRebootWatchdogHandling(bool recreateFile) { bool doReboot = false; xsc::Chip tgtChip = xsc::NO_CHIP; xsc::Copy tgtCopy = xsc::NO_COPY; - rebootWatchdogAlgorithm((rebootFile, doReboot, tgtChip, tgtCopy); + rebootWatchdogAlgorithm(rebootWatchdogFile, doReboot, tgtChip, tgtCopy); if (doReboot) { rebootWatchdogFile.bootFlag = true; #if OBSW_VERBOSE_LEVEL >= 1 @@ -1715,8 +1715,8 @@ void CoreController::performRebootWatchdogHandling(bool recreateFile) { rewriteRebootWatchdogFile(rebootWatchdogFile); } -void CoreController::rebootWatchdogAlgorithm(RebootFile &rf, bool &needsReboot, - xsc::Chip &tgtChip, xsc::Copy &tgtCopy) { +void CoreController::rebootWatchdogAlgorithm(RebootWatchdogFile &rf, bool &needsReboot, + xsc::Chip &tgtChip, xsc::Copy &tgtCopy) { tgtChip = xsc::CHIP_0; tgtCopy = xsc::COPY_0; needsReboot = false; diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index fb7becf6..b68ae158 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -377,8 +377,8 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe int handleBootCopyProtAtIndex(xsc::Chip targetChip, xsc::Copy targetCopy, bool protect, bool& protOperationPerformed, bool selfChip, bool selfCopy, bool allChips, bool allCopies, uint8_t arrIdx); - void rebootWatchdogAlgorithm(RebootFile& rf, bool& needsReboot, xsc::Chip& tgtChip, - xsc::Copy& tgtCopy); + void rebootWatchdogAlgorithm(RebootWatchdogFile& rf, bool& needsReboot, xsc::Chip& tgtChip, + xsc::Copy& tgtCopy); void resetRebootWatchdogCounters(xsc::Chip tgtChip, xsc::Copy tgtCopy); void setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy); bool parseRebootWatchdogFile(std::string path, RebootWatchdogFile& file); From 76933671a6321746af4ced168a6c04bc09f7b4e4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 25 Jun 2023 11:07:41 +0200 Subject: [PATCH 404/506] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index fb62b8fc..38c7cd51 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Changed + +- Persistent TM store dumps are now performed in chronological order + # [v4.0.1] 2023-06-24 ## Fixed From 873316d012bec41c10a46d5c2b51c511235dd187 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 25 Jun 2023 12:41:20 +0200 Subject: [PATCH 405/506] sequence counter isn ow persistent, msg type count support --- CHANGELOG.md | 5 ++++ common/config/eive/definitions.h | 2 ++ fsfw | 2 +- mission/genericFactory.cpp | 12 ++++++--- mission/sysDefs.h | 3 +++ mission/tmtc/CfdpTmFunnel.cpp | 10 +++++++- mission/tmtc/CfdpTmFunnel.h | 2 +- mission/tmtc/PusTmFunnel.cpp | 24 +++++++++++++++-- mission/tmtc/PusTmFunnel.h | 7 ++--- mission/tmtc/TmFunnelBase.cpp | 44 +++++++++++++++++++++++++++++++- mission/tmtc/TmFunnelBase.h | 20 +++++++++++++-- 11 files changed, 117 insertions(+), 14 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index fb62b8fc..bc07719d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,11 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Added + +- Sequence counters for PUS and CFDP packets are now stored persistently across graceful reboots. +- The PUS packet message type counter will now be increment properly. + # [v4.0.1] 2023-06-24 ## Fixed diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h index 2083962a..b369f512 100644 --- a/common/config/eive/definitions.h +++ b/common/config/eive/definitions.h @@ -11,6 +11,8 @@ static constexpr char SD_1_MOUNT_POINT[] = "/mnt/sd1"; static constexpr char OBSW_UPDATE_ARCHIVE_FILE_NAME[] = "eive-sw-update.tar.xz"; static constexpr char STRIPPED_OBSW_BINARY_FILE_NAME[] = "eive-obsw-stripped"; static constexpr char OBSW_VERSION_FILE_NAME[] = "obsw_version.txt"; +static constexpr char PUS_SEQUENCE_COUNT_FILE[] = "pus-sequence-count.txt"; +static constexpr char CFDP_SEQUENCE_COUNT_FILE[] = "cfdp-sequence-count.txt"; static constexpr char OBSW_PATH[] = "/usr/bin/eive-obsw"; static constexpr char OBSW_VERSION_FILE_PATH[] = "/usr/share/eive-obsw/obsw_version.txt"; diff --git a/fsfw b/fsfw index 0f76cdb3..c3572e31 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 0f76cdb3ba54f5e90a8eee4316c49cf0f581f996 +Subproject commit c3572e31a8d6845442cf8e54802d94477b2db64e diff --git a/mission/genericFactory.cpp b/mission/genericFactory.cpp index cbe78c7f..7101ab3d 100644 --- a/mission/genericFactory.cpp +++ b/mission/genericFactory.cpp @@ -91,6 +91,8 @@ EiveFaultHandler EIVE_FAULT_HANDLER; } // namespace cfdp std::atomic_bool tcs::TCS_BOARD_SHORTLY_UNAVAILABLE = false; +std::atomic_bool core::SAVE_PUS_SEQUENCE_COUNT = false; +std::atomic_bool core::SAVE_CFDP_SEQUENCE_COUNT = false; void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFunnel** pusFunnel, CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan, @@ -155,9 +157,11 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun new PusDistributor(config::EIVE_PUS_APID, objects::PUS_PACKET_DISTRIBUTOR, ccsdsDistrib); PusTmFunnel::FunnelCfg pusFunnelCfg(objects::PUS_TM_FUNNEL, "PusTmFunnel", **tmStore, **ipcStore, - config::MAX_PUS_FUNNEL_QUEUE_DEPTH); + config::MAX_PUS_FUNNEL_QUEUE_DEPTH, sdcMan, + config::PUS_SEQUENCE_COUNT_FILE, + core::SAVE_PUS_SEQUENCE_COUNT); // The PUS funnel routes all live TM to the live destinations and to the TM stores. - *pusFunnel = new PusTmFunnel(pusFunnelCfg, *ramToFileStore, *timeStamper, sdcMan); + *pusFunnel = new PusTmFunnel(pusFunnelCfg, *ramToFileStore, *timeStamper); // MISC store and PUS funnel to MISC store routing { @@ -216,7 +220,9 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun stores.cfdpStore->getReportReceptionQueue(0)); } PusTmFunnel::FunnelCfg cfdpFunnelCfg(objects::CFDP_TM_FUNNEL, "CfdpTmFunnel", **tmStore, - **ipcStore, config::MAX_CFDP_FUNNEL_QUEUE_DEPTH); + **ipcStore, config::MAX_CFDP_FUNNEL_QUEUE_DEPTH, sdcMan, + config::CFDP_SEQUENCE_COUNT_FILE, + core::SAVE_CFDP_SEQUENCE_COUNT); *cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, stores.cfdpStore->getReportReceptionQueue(0), *ramToFileStore, config::EIVE_CFDP_APID); diff --git a/mission/sysDefs.h b/mission/sysDefs.h index 32ce9642..1184a8a8 100644 --- a/mission/sysDefs.h +++ b/mission/sysDefs.h @@ -34,6 +34,9 @@ enum Copy : int { COPY_0, COPY_1, NO_COPY, SELF_COPY, ALL_COPY }; namespace core { +extern std::atomic_bool SAVE_PUS_SEQUENCE_COUNT; +extern std::atomic_bool SAVE_CFDP_SEQUENCE_COUNT; + // TODO: Support for status? Or maybe some command to quickly get information whether a unit // is running. enum SystemctlCmd : uint8_t { START = 0, STOP = 1, RESTART = 2, NUM_CMDS = 3 }; diff --git a/mission/tmtc/CfdpTmFunnel.cpp b/mission/tmtc/CfdpTmFunnel.cpp index 46915b82..93c294a4 100644 --- a/mission/tmtc/CfdpTmFunnel.cpp +++ b/mission/tmtc/CfdpTmFunnel.cpp @@ -15,8 +15,16 @@ const char* CfdpTmFunnel::getName() const { return "CFDP TM Funnel"; } ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) { TmTcMessage currentMessage; + ReturnValue_t status; unsigned int count = 0; - ReturnValue_t status = tmQueue->receiveMessage(¤tMessage); + if (saveSequenceCount) { + status = saveSequenceCountToFile(); + if (status != returnvalue::OK) { + sif::error << "CfdpTmFunnel: Storing sequence count to file has failed" << std::endl; + } + saveSequenceCount = false; + } + status = tmQueue->receiveMessage(¤tMessage); while (status == returnvalue::OK) { status = handlePacket(currentMessage); if (status != returnvalue::OK) { diff --git a/mission/tmtc/CfdpTmFunnel.h b/mission/tmtc/CfdpTmFunnel.h index a4d1bc70..7b9efd34 100644 --- a/mission/tmtc/CfdpTmFunnel.h +++ b/mission/tmtc/CfdpTmFunnel.h @@ -3,6 +3,7 @@ #include +#include #include #include "fsfw/objectmanager/SystemObject.h" @@ -23,7 +24,6 @@ class CfdpTmFunnel : public TmFunnelBase { MessageQueueId_t fileStoreDest; StorageManagerIF& ramToFileStore; - uint16_t sourceSequenceCount = 0; uint16_t cfdpInCcsdsApid; }; #endif // FSFW_EXAMPLE_COMMON_CFDPTMFUNNEL_H diff --git a/mission/tmtc/PusTmFunnel.cpp b/mission/tmtc/PusTmFunnel.cpp index dbc8f1c4..b49d19d3 100644 --- a/mission/tmtc/PusTmFunnel.cpp +++ b/mission/tmtc/PusTmFunnel.cpp @@ -1,5 +1,7 @@ #include "PusTmFunnel.h" +#include + #include "eive/definitions.h" #include "eive/objects.h" #include "fsfw/ipc/CommandMessage.h" @@ -11,8 +13,8 @@ #include "tmtc/pusIds.h" PusTmFunnel::PusTmFunnel(TmFunnelBase::FunnelCfg cfg, StorageManagerIF &ramToFileStore, - TimeReaderIF &timeReader, SdCardMountedIF &sdcMan) - : TmFunnelBase(cfg), ramToFileStore(ramToFileStore), timeReader(timeReader), sdcMan(sdcMan) {} + TimeReaderIF &timeReader) + : TmFunnelBase(cfg), ramToFileStore(ramToFileStore), timeReader(timeReader) {} PusTmFunnel::~PusTmFunnel() = default; @@ -21,6 +23,13 @@ ReturnValue_t PusTmFunnel::performOperation(uint8_t) { ReturnValue_t result; TmTcMessage currentMessage; unsigned int count = 0; + if (saveSequenceCount) { + result = saveSequenceCountToFile(); + if (result != returnvalue::OK) { + sif::error << "PusTmFunnel: Storing sequence count to file has failed" << std::endl; + } + saveSequenceCount = false; + } result = tmQueue->receiveMessage(¤tMessage); while (result == returnvalue::OK) { result = handleTmPacket(currentMessage); @@ -61,6 +70,17 @@ ReturnValue_t PusTmFunnel::handleTmPacket(TmTcMessage &message) { packet.setSequenceCount(sourceSequenceCount++); sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT; packet.updateErrorControl(); + uint8_t service = packet.getMessageTypeCounter(); + auto mapIter = msgCounterMap.find(service); + if (mapIter == msgCounterMap.end()) { + msgCounterMap.emplace(service, 0); + } + if (mapIter->second == std::numeric_limits::max()) { + mapIter->second = 0; + } else { + mapIter->second++; + } + packet.setMessageCount(mapIter->second); // Send to persistent TM store if the packet matches some filter. MessageQueueId_t destination; diff --git a/mission/tmtc/PusTmFunnel.h b/mission/tmtc/PusTmFunnel.h index ba6e8d3e..3696fdcc 100644 --- a/mission/tmtc/PusTmFunnel.h +++ b/mission/tmtc/PusTmFunnel.h @@ -9,6 +9,8 @@ #include #include +#include +#include #include #include "PersistentTmStore.h" @@ -25,7 +27,7 @@ class PusTmFunnel : public TmFunnelBase { public: PusTmFunnel(TmFunnelBase::FunnelCfg cfg, StorageManagerIF &ramToFileStore, - TimeReaderIF &timeReader, SdCardMountedIF &sdcMan); + TimeReaderIF &timeReader); [[nodiscard]] const char *getName() const override; ~PusTmFunnel() override; @@ -36,11 +38,10 @@ class PusTmFunnel : public TmFunnelBase { // Update TV stamp every 5 minutes static constexpr dur_millis_t TV_UPDATE_INTERVAL_SECS = 60 * 5; - uint16_t sourceSequenceCount = 0; + std::map msgCounterMap; StorageManagerIF &ramToFileStore; TimeReaderIF &timeReader; bool storesInitialized = false; - SdCardMountedIF &sdcMan; PusTmRouteByFilterHelper persistentTmMap; ReturnValue_t handleTmPacket(TmTcMessage &message); diff --git a/mission/tmtc/TmFunnelBase.cpp b/mission/tmtc/TmFunnelBase.cpp index eb480b03..16e4d3bb 100644 --- a/mission/tmtc/TmFunnelBase.cpp +++ b/mission/tmtc/TmFunnelBase.cpp @@ -2,6 +2,10 @@ #include +#include +#include +#include + #include "fsfw/ipc/QueueFactory.h" TmFunnelBase::TmFunnelBase(FunnelCfg cfg) @@ -10,7 +14,10 @@ TmFunnelBase::TmFunnelBase(FunnelCfg cfg) tmStore(cfg.tmStore), ipcStore(cfg.ipcStore), tmQueue(QueueFactory::instance()->createMessageQueue(cfg.tmMsgDepth)), - liveDemux(*tmQueue) {} + liveDemux(*tmQueue), + sdcMan(cfg.sdcMan), + sequenceCounterFilename(cfg.sequenceCounterFilename), + saveSequenceCount(cfg.saveSequenceCount) {} ReturnValue_t TmFunnelBase::demultiplexLivePackets(store_address_t origStoreId, const uint8_t *tmData, size_t tmSize) { @@ -27,3 +34,38 @@ void TmFunnelBase::addLiveDestination(const char *name, const AcceptsTelemetryIF &downlinkDestination, uint8_t vcid) { liveDemux.addDestination(name, downlinkDestination, vcid); } + +ReturnValue_t TmFunnelBase::initialize() { + using namespace std::filesystem; + // The filesystem should always be available at the start.. Let's assume it always is, otherwise + // we just live with a regular 0 initialization. It simplifies a lot. + std::error_code e; + path filePath = + path(sdcMan.getCurrentMountPrefix() / std::string("conf") / sequenceCounterFilename); + if (exists(filePath, e)) { + std::ifstream ifile(filePath); + if (ifile.bad()) { + sif::error << "TmFunnelBase::initialize: Faulty file open for sequence counter initialization" + << std::endl; + return returnvalue::OK; + } + if (not(ifile >> sourceSequenceCount)) { + // Initialize to 0 in any case if reading a number failed. + sourceSequenceCount = 0; + } + } + return returnvalue::OK; +} + +ReturnValue_t TmFunnelBase::saveSequenceCountToFile() { + using namespace std::filesystem; + std::error_code e; + path filePath = + path(sdcMan.getCurrentMountPrefix() / std::string("conf") / sequenceCounterFilename); + std::ofstream ofile(filePath); + if (ofile.bad()) { + return returnvalue::FAILED; + } + ofile << sourceSequenceCount << "\n"; + return returnvalue::OK; +} diff --git a/mission/tmtc/TmFunnelBase.h b/mission/tmtc/TmFunnelBase.h index 51d16626..72d91103 100644 --- a/mission/tmtc/TmFunnelBase.h +++ b/mission/tmtc/TmFunnelBase.h @@ -6,25 +6,34 @@ #include #include #include +#include #include +#include #include class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject { public: struct FunnelCfg { FunnelCfg(object_id_t objId, const char* name, StorageManagerIF& tmStore, - StorageManagerIF& ipcStore, uint32_t tmMsgDepth) + StorageManagerIF& ipcStore, uint32_t tmMsgDepth, SdCardMountedIF& sdcMan, + const char* sequenceCounterFilename, std::atomic_bool& saveSequenceCount) : objectId(objId), name(name), tmStore(tmStore), ipcStore(ipcStore), - tmMsgDepth(tmMsgDepth) {} + tmMsgDepth(tmMsgDepth), + sdcMan(sdcMan), + sequenceCounterFilename(sequenceCounterFilename), + saveSequenceCount(saveSequenceCount) {} object_id_t objectId; const char* name; StorageManagerIF& tmStore; StorageManagerIF& ipcStore; uint32_t tmMsgDepth; + SdCardMountedIF& sdcMan; + const char* sequenceCounterFilename; + std::atomic_bool& saveSequenceCount; }; explicit TmFunnelBase(FunnelCfg cfg); [[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override; @@ -32,6 +41,9 @@ class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject { uint8_t vcid = 0); ReturnValue_t demultiplexLivePackets(store_address_t origStoreId, const uint8_t* tmData, size_t tmSize); + ReturnValue_t initialize() override; + + ReturnValue_t saveSequenceCountToFile(); ~TmFunnelBase() override; @@ -41,6 +53,10 @@ class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject { StorageManagerIF& ipcStore; MessageQueueIF* tmQueue = nullptr; PusLiveDemux liveDemux; + SdCardMountedIF& sdcMan; + const char* sequenceCounterFilename; + std::atomic_bool& saveSequenceCount; + uint16_t sourceSequenceCount = 0; }; #endif /* MISSION_TMTC_TMFUNNELBASE_H_ */ From e036ed2bff981daa040f7e793568ce73a69695b7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 25 Jun 2023 12:47:28 +0200 Subject: [PATCH 406/506] now its complete --- bsp_q7s/core/CoreController.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index f9242da3..ca207719 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -1222,6 +1222,10 @@ ReturnValue_t CoreController::actionReboot(const uint8_t *data, size_t size) { ReturnValue_t CoreController::gracefulShutdownTasks(xsc::Chip chip, xsc::Copy copy, bool &protOpPerformed) { + // Store both sequence counters persistently. + core::SAVE_CFDP_SEQUENCE_COUNT = true; + core::SAVE_PUS_SEQUENCE_COUNT = true; + sdcMan->setBlocking(true); sdcMan->markUnusable(); // Wait two seconds to ensure no one uses the SD cards From 5a15d39a1d5c38af793c52d215d4737ad9529b17 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 25 Jun 2023 12:49:51 +0200 Subject: [PATCH 407/506] important order bugfix --- mission/tmtc/PusTmFunnel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/tmtc/PusTmFunnel.cpp b/mission/tmtc/PusTmFunnel.cpp index b49d19d3..e5285ba5 100644 --- a/mission/tmtc/PusTmFunnel.cpp +++ b/mission/tmtc/PusTmFunnel.cpp @@ -69,7 +69,6 @@ ReturnValue_t PusTmFunnel::handleTmPacket(TmTcMessage &message) { } packet.setSequenceCount(sourceSequenceCount++); sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT; - packet.updateErrorControl(); uint8_t service = packet.getMessageTypeCounter(); auto mapIter = msgCounterMap.find(service); if (mapIter == msgCounterMap.end()) { @@ -81,6 +80,7 @@ ReturnValue_t PusTmFunnel::handleTmPacket(TmTcMessage &message) { mapIter->second++; } packet.setMessageCount(mapIter->second); + packet.updateErrorControl(); // Send to persistent TM store if the packet matches some filter. MessageQueueId_t destination; From 80160e829166c48957b1747df69c98ad649c2a51 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 25 Jun 2023 13:07:31 +0200 Subject: [PATCH 408/506] that was hopefully the last set of fixes --- mission/tmtc/PusTmFunnel.cpp | 29 ++++++++++++++++++++++------- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/mission/tmtc/PusTmFunnel.cpp b/mission/tmtc/PusTmFunnel.cpp index e5285ba5..87648e55 100644 --- a/mission/tmtc/PusTmFunnel.cpp +++ b/mission/tmtc/PusTmFunnel.cpp @@ -68,18 +68,33 @@ ReturnValue_t PusTmFunnel::handleTmPacket(TmTcMessage &message) { return result; } packet.setSequenceCount(sourceSequenceCount++); + // NOTE: This only works because the limit value bit width is below 16 bits! sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT; - uint8_t service = packet.getMessageTypeCounter(); + + // Message type counter handling. + uint8_t service = packet.getService(); + bool insertionFailed = false; auto mapIter = msgCounterMap.find(service); if (mapIter == msgCounterMap.end()) { - msgCounterMap.emplace(service, 0); + auto iterPair = msgCounterMap.emplace(service, 0); + if (iterPair.second) { + mapIter = iterPair.first; + } else { + // Should really never never happen but you never know.. + insertionFailed = true; + } } - if (mapIter->second == std::numeric_limits::max()) { - mapIter->second = 0; - } else { - mapIter->second++; + if (not insertionFailed) { + packet.setMessageCount(mapIter->second); + // Sane overflow handling. + if (mapIter->second == std::numeric_limits::max()) { + mapIter->second = 0; + } else { + mapIter->second++; + } } - packet.setMessageCount(mapIter->second); + + // Re-calculate CRC after changing the fields. This operation HAS to come last! packet.updateErrorControl(); // Send to persistent TM store if the packet matches some filter. From 93fb2070320f7482f10e6a15d6448c8f2e86f91b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 25 Jun 2023 13:12:39 +0200 Subject: [PATCH 409/506] C++ insanity.. --- mission/tmtc/TmFunnelBase.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/tmtc/TmFunnelBase.cpp b/mission/tmtc/TmFunnelBase.cpp index 16e4d3bb..06bdd15f 100644 --- a/mission/tmtc/TmFunnelBase.cpp +++ b/mission/tmtc/TmFunnelBase.cpp @@ -40,8 +40,8 @@ ReturnValue_t TmFunnelBase::initialize() { // The filesystem should always be available at the start.. Let's assume it always is, otherwise // we just live with a regular 0 initialization. It simplifies a lot. std::error_code e; - path filePath = - path(sdcMan.getCurrentMountPrefix() / std::string("conf") / sequenceCounterFilename); + path filePath = path(std::string(sdcMan.getCurrentMountPrefix()) / std::string("conf") / + std::string(sequenceCounterFilename)); if (exists(filePath, e)) { std::ifstream ifile(filePath); if (ifile.bad()) { @@ -60,8 +60,8 @@ ReturnValue_t TmFunnelBase::initialize() { ReturnValue_t TmFunnelBase::saveSequenceCountToFile() { using namespace std::filesystem; std::error_code e; - path filePath = - path(sdcMan.getCurrentMountPrefix() / std::string("conf") / sequenceCounterFilename); + path filePath = path(std::string(sdcMan.getCurrentMountPrefix()) / std::string("conf") / + std::string(sequenceCounterFilename)); std::ofstream ofile(filePath); if (ofile.bad()) { return returnvalue::FAILED; From c806aa81f07560c9d06b56116225835c72e845b8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 26 Jun 2023 11:14:11 +0200 Subject: [PATCH 410/506] what does the new compiler want? --- mission/tmtc/TmFunnelBase.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/tmtc/TmFunnelBase.cpp b/mission/tmtc/TmFunnelBase.cpp index 06bdd15f..fc2e4b76 100644 --- a/mission/tmtc/TmFunnelBase.cpp +++ b/mission/tmtc/TmFunnelBase.cpp @@ -40,8 +40,8 @@ ReturnValue_t TmFunnelBase::initialize() { // The filesystem should always be available at the start.. Let's assume it always is, otherwise // we just live with a regular 0 initialization. It simplifies a lot. std::error_code e; - path filePath = path(std::string(sdcMan.getCurrentMountPrefix()) / std::string("conf") / - std::string(sequenceCounterFilename)); + path filePath = + path(path(sdcMan.getCurrentMountPrefix()) / path("conf") / path(sequenceCounterFilename)); if (exists(filePath, e)) { std::ifstream ifile(filePath); if (ifile.bad()) { @@ -60,8 +60,8 @@ ReturnValue_t TmFunnelBase::initialize() { ReturnValue_t TmFunnelBase::saveSequenceCountToFile() { using namespace std::filesystem; std::error_code e; - path filePath = path(std::string(sdcMan.getCurrentMountPrefix()) / std::string("conf") / - std::string(sequenceCounterFilename)); + path filePath = + path(path(sdcMan.getCurrentMountPrefix()) / path("conf") / path(sequenceCounterFilename)); std::ofstream ofile(filePath); if (ofile.bad()) { return returnvalue::FAILED; From 05a5c63765f59a3f0f2f975258125610b9df822a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 26 Jun 2023 11:20:33 +0200 Subject: [PATCH 411/506] changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 57355ad0..128aa034 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,7 +16,7 @@ will consitute of a breaking change warranting a new major release: # [unreleased] -- Internal error reporter set is now enabled by defalt and generated every 120 seconds. +- Internal error reporter set is now enabled by default and generated every 120 seconds. # [v4.0.1] 2023-06-24 From 1cc62238c2f35af3186e9d7cff77295f59770faf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 26 Jun 2023 14:39:03 +0200 Subject: [PATCH 412/506] only reset PTME on rate change init --- linux/ipcore/AxiPtmeConfig.cpp | 6 ++++++ linux/ipcore/AxiPtmeConfig.h | 1 + linux/ipcore/PtmeConfig.cpp | 5 +++++ linux/ipcore/PtmeConfig.h | 1 + mission/com/CcsdsIpCoreHandler.cpp | 1 + 5 files changed, 14 insertions(+) diff --git a/linux/ipcore/AxiPtmeConfig.cpp b/linux/ipcore/AxiPtmeConfig.cpp index 6dee3e2f..d3d5662a 100644 --- a/linux/ipcore/AxiPtmeConfig.cpp +++ b/linux/ipcore/AxiPtmeConfig.cpp @@ -41,6 +41,12 @@ ReturnValue_t AxiPtmeConfig::writeCaduRateReg(uint8_t rateVal) { return returnvalue::OK; } +uint8_t AxiPtmeConfig::readCaduRateReg() { + ReturnValue_t result = returnvalue::OK; + MutexGuard mg(mutex); + return static_cast(*(baseAddress + CADU_BITRATE_REG)); +} + void AxiPtmeConfig::enableTxclockManipulator() { writeBit(COMMON_CONFIG_REG, true, BitPos::EN_TX_CLK_MANIPULATOR); } diff --git a/linux/ipcore/AxiPtmeConfig.h b/linux/ipcore/AxiPtmeConfig.h index 98188775..ebdf4d38 100644 --- a/linux/ipcore/AxiPtmeConfig.h +++ b/linux/ipcore/AxiPtmeConfig.h @@ -38,6 +38,7 @@ class AxiPtmeConfig : public SystemObject { * frequency of the clock connected to the bit clock input of PTME. */ ReturnValue_t writeCaduRateReg(uint8_t rateVal); + uint8_t readCaduRateReg(); /** * @brief Next to functions control the tx clock manipulator component diff --git a/linux/ipcore/PtmeConfig.cpp b/linux/ipcore/PtmeConfig.cpp index 5f247b54..5b6b9343 100644 --- a/linux/ipcore/PtmeConfig.cpp +++ b/linux/ipcore/PtmeConfig.cpp @@ -26,6 +26,11 @@ ReturnValue_t PtmeConfig::setRate(uint32_t bitRate) { return axiPtmeConfig->writeCaduRateReg(static_cast(rateVal)); } +uint32_t PtmeConfig::getRate() { + uint8_t rateReg = axiPtmeConfig->readCaduRateReg(); + return (BIT_CLK_FREQ / (rateReg + 1)); +} + void PtmeConfig::invertTxClock(bool invert) { if (invert) { axiPtmeConfig->enableTxclockInversion(); diff --git a/linux/ipcore/PtmeConfig.h b/linux/ipcore/PtmeConfig.h index 87614187..11eeff7d 100644 --- a/linux/ipcore/PtmeConfig.h +++ b/linux/ipcore/PtmeConfig.h @@ -32,6 +32,7 @@ class PtmeConfig : public SystemObject { * of the CADU clock due to the convolutional code added by the s-Band transceiver. */ ReturnValue_t setRate(uint32_t bitRate); + uint32_t getRate(); /** * @brief Will change the time the tx data signal is updated with respect to the tx clock diff --git a/mission/com/CcsdsIpCoreHandler.cpp b/mission/com/CcsdsIpCoreHandler.cpp index 625c90cd..766fd67c 100644 --- a/mission/com/CcsdsIpCoreHandler.cpp +++ b/mission/com/CcsdsIpCoreHandler.cpp @@ -255,6 +255,7 @@ void CcsdsIpCoreHandler::startTransition(Mode_t mode, Submode_t submode) { updateContext.setModeAfterUpdate = true; return; } + if(ptmeConfig.setRate(bitRate)) // No rate change, so enable transmitter right away. enableTransmit(); } else if (mode == HasModesIF::MODE_OFF) { From fca6834d170d2799db62ce9c46f74b67dc72d59d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 26 Jun 2023 15:32:52 +0200 Subject: [PATCH 413/506] higher rx set rate during pass --- mission/com/SyrlinksHandler.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mission/com/SyrlinksHandler.cpp b/mission/com/SyrlinksHandler.cpp index 6fbc8dc2..50e6a56e 100644 --- a/mission/com/SyrlinksHandler.cpp +++ b/mission/com/SyrlinksHandler.cpp @@ -773,11 +773,13 @@ void SyrlinksHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { auto txStandbyHandler = [&]() { txDataset.setReportingEnabled(false); poolManager.changeCollectionInterval(temperatureSet.getSid(), 60.0); + poolManager.changeCollectionInterval(rxDataset.getSid(), 60.0); transState = TransitionState::SET_TX_STANDBY; internalState = InternalState::TX_TRANSITION; }; auto txOnHandler = [&](TransitionState tgtTransitionState) { txDataset.setReportingEnabled(true); + poolManager.changeCollectionInterval(rxDataset.getSid(), 5.0); poolManager.changeCollectionInterval(txDataset.getSid(), 10.0); poolManager.changeCollectionInterval(temperatureSet.getSid(), 5.0); transState = tgtTransitionState; From 1f1890481067c57d850a4866dad0a53fd6506de6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 26 Jun 2023 15:38:37 +0200 Subject: [PATCH 414/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index deb0275b..ec0ebc36 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit deb0275bb5603394122e26f74760d2051685f324 +Subproject commit ec0ebc365308198046addc94909b1bca8678aa5a From 73498ac9afc306c8aedbc51378b67a1e8204ca43 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 26 Jun 2023 16:53:54 +0200 Subject: [PATCH 415/506] prepare v5.0.0, revert firmware interfacing changes --- CHANGELOG.md | 14 ++- bsp_q7s/boardconfig/busConf.h | 4 + bsp_q7s/core/ObjectFactory.cpp | 29 +++-- common/config/devices/gpioIds.h | 4 + linux/ipcore/PapbVcInterface.cpp | 120 ++++++++++++++++---- linux/ipcore/PapbVcInterface.h | 10 +- mission/com/PersistentLogTmStoreTask.cpp | 6 + mission/com/PersistentSingleTmStoreTask.cpp | 6 + 8 files changed, 160 insertions(+), 33 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8e4bf09f..b62369e0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,10 +16,22 @@ will consitute of a breaking change warranting a new major release: # [unreleased] -# [v4.1.0] +# [v6.0.0] to be released + +- Important bugfixes for PTME. See `q7s-package` CHANGELOG. + +# [v5.0.0] to be released + +v3.3.1 and all following version will now be moved to v5.0.0 with the additional changes listed +here. This was done because the firmware update (v4.0.0) is not working right now and it is not +known when and how it will be fixed. Because of that, all updates to make the SW work with the new +firmware, which are limited to a few files will be moved to a dev branch so regular development +compatible to the old firmware can continue. ## Changed +- This version is compatible to the old firmware and some changes which only work with the new + firmware have been reverted. - Added `sync` syscall in graceful shutdown handler - Graceful shutdown is now performed by the reboot watchdog - There is now a separate file for the total reboot counter. The reboot watchdog has its own local diff --git a/bsp_q7s/boardconfig/busConf.h b/bsp_q7s/boardconfig/busConf.h index 146386c4..6a3e0d9e 100644 --- a/bsp_q7s/boardconfig/busConf.h +++ b/bsp_q7s/boardconfig/busConf.h @@ -85,9 +85,13 @@ static constexpr char EN_RW_4[] = "enable_rw_4"; static constexpr char RAD_SENSOR_CHIP_SELECT[] = "rad_sensor_chip_select"; static constexpr char ENABLE_RADFET[] = "enable_radfet"; +static constexpr char PAPB_BUSY_SIGNAL_VC0[] = "papb_busy_signal_vc0"; static constexpr char PAPB_EMPTY_SIGNAL_VC0[] = "papb_empty_signal_vc0"; +static constexpr char PAPB_BUSY_SIGNAL_VC1[] = "papb_busy_signal_vc1"; static constexpr char PAPB_EMPTY_SIGNAL_VC1[] = "papb_empty_signal_vc1"; +static constexpr char PAPB_BUSY_SIGNAL_VC2[] = "papb_busy_signal_vc2"; static constexpr char PAPB_EMPTY_SIGNAL_VC2[] = "papb_empty_signal_vc2"; +static constexpr char PAPB_BUSY_SIGNAL_VC3[] = "papb_busy_signal_vc3"; static constexpr char PAPB_EMPTY_SIGNAL_VC3[] = "papb_empty_signal_vc3"; static constexpr char PTME_RESETN[] = "ptme_resetn"; diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 35d30c1a..af568553 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -731,12 +731,20 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(CcsdsComponentArgs& args) { // GPIO definitions of signals connected to the virtual channel interfaces of the PTME IP Core GpioCookie* gpioCookiePtmeIp = new GpioCookie; GpiodRegularByLineName* gpio = nullptr; + gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC0, "PAPB VC0"); + gpioCookiePtmeIp->addGpio(gpioIds::VC0_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC0, "PAPB VC0"); gpioCookiePtmeIp->addGpio(gpioIds::VC0_PAPB_EMPTY, gpio); + gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC1, "PAPB VC1"); + gpioCookiePtmeIp->addGpio(gpioIds::VC1_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC1, "PAPB VC1"); gpioCookiePtmeIp->addGpio(gpioIds::VC1_PAPB_EMPTY, gpio); + gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC2, "PAPB VC2"); + gpioCookiePtmeIp->addGpio(gpioIds::VC2_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC2, "PAPB VC2"); gpioCookiePtmeIp->addGpio(gpioIds::VC2_PAPB_EMPTY, gpio); + gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC3, "PAPB VC3"); + gpioCookiePtmeIp->addGpio(gpioIds::VC3_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC3, "PAPB VC3"); gpioCookiePtmeIp->addGpio(gpioIds::VC3_PAPB_EMPTY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PTME_RESETN, "PTME RESETN", @@ -745,14 +753,19 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(CcsdsComponentArgs& args) { gpioChecker(args.gpioComIF.addGpios(gpioCookiePtmeIp), "PTME PAPB VCs"); // Creating virtual channel interfaces - VirtualChannelIF* vc0 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC0_PAPB_EMPTY, - q7s::UIO_PTME, q7s::uiomapids::PTME_VC0); - VirtualChannelIF* vc1 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC1_PAPB_EMPTY, - q7s::UIO_PTME, q7s::uiomapids::PTME_VC1); - VirtualChannelIF* vc2 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC2_PAPB_EMPTY, - q7s::UIO_PTME, q7s::uiomapids::PTME_VC2); - VirtualChannelIF* vc3 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC3_PAPB_EMPTY, - q7s::UIO_PTME, q7s::uiomapids::PTME_VC3); + VirtualChannelIF* vc0 = + new PapbVcInterface(&args.gpioComIF, gpioIds::VC0_PAPB_BUSY, gpioIds::VC0_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC0); + VirtualChannelIF* vc1 = + new PapbVcInterface(&args.gpioComIF, gpioIds::VC1_PAPB_BUSY, gpioIds::VC1_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC1); + VirtualChannelIF* vc2 = + new PapbVcInterface(&args.gpioComIF, gpioIds::VC2_PAPB_BUSY, gpioIds::VC2_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC2); + VirtualChannelIF* vc3 = + new PapbVcInterface(&args.gpioComIF, gpioIds::VC3_PAPB_BUSY, gpioIds::VC3_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC3); + // Creating ptme object and adding virtual channel interfaces Ptme* ptme = new Ptme(objects::PTME); ptme->addVcInterface(ccsds::VC0, vc0); diff --git a/common/config/devices/gpioIds.h b/common/config/devices/gpioIds.h index bed82142..ac193bd2 100644 --- a/common/config/devices/gpioIds.h +++ b/common/config/devices/gpioIds.h @@ -96,9 +96,13 @@ enum gpioId_t { SPI_MUX, VC0_PAPB_EMPTY, + VC0_PAPB_BUSY, VC1_PAPB_EMPTY, + VC1_PAPB_BUSY, VC2_PAPB_EMPTY, + VC2_PAPB_BUSY, VC3_PAPB_EMPTY, + VC3_PAPB_BUSY, PTME_RESETN, PDEC_RESET, diff --git a/linux/ipcore/PapbVcInterface.cpp b/linux/ipcore/PapbVcInterface.cpp index 404f3653..60968cc6 100644 --- a/linux/ipcore/PapbVcInterface.cpp +++ b/linux/ipcore/PapbVcInterface.cpp @@ -7,16 +7,20 @@ #include "fsfw/serviceinterface/ServiceInterface.h" -PapbVcInterface::PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbEmptyId, - std::string uioFile, int mapNum) - : gpioComIF(gpioComIF), papbEmptyId(papbEmptyId), uioFile(std::move(uioFile)), mapNum(mapNum) {} +PapbVcInterface::PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbBusyId, + gpioId_t papbEmptyId, std::string uioFile, int mapNum) + : gpioComIF(gpioComIF), + papbBusyId(papbBusyId), + papbEmptyId(papbEmptyId), + uioFile(std::move(uioFile)), + mapNum(mapNum) {} PapbVcInterface::~PapbVcInterface() {} ReturnValue_t PapbVcInterface::initialize() { UioMapper uioMapper(uioFile, mapNum); ReturnValue_t result = uioMapper.getMappedAdress(const_cast(&vcBaseReg), - UioMapper::Permissions::READ_WRITE); + UioMapper::Permissions::WRITE_ONLY); if (result != returnvalue::OK) { return result; } @@ -28,16 +32,63 @@ ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size) { if (size < 4) { return returnvalue::FAILED; } - if (pollReadyForPacket()) { + if (pollInterfaceReadiness(0, true) == returnvalue::OK) { startPacketTransfer(ByteWidthCfg::ONE); } else { return DirectTmSinkIF::IS_BUSY; } + // TODO: This should work but does not.. :( + // size_t idx = 0; + // while (idx < size) { + // + // nanosleep(&BETWEEN_POLL_DELAY, &remDelay); + // if ((size - idx) < 4) { + // *vcBaseReg = CONFIG_DATA_INPUT | (size - idx - 1); + // usleep(1); + // } + // if (pollPapbBusySignal(2) == returnvalue::OK) { + // // vcBaseReg + DATA_REG_OFFSET + 3 = static_cast(data + idx); + // // vcBaseReg + DATA_REG_OFFSET + 2 = static_cast(data + idx + 1); + // // vcBaseReg + DATA_REG_OFFSET + 1 = static_cast(data + idx + 2); + // // vcBaseReg + DATA_REG_OFFSET = static_cast(data + idx + 3); + // + // // std::memcpy((vcBaseReg + DATA_REG_OFFSET), data + idx , nextWriteSize); + // *(vcBaseReg + DATA_REG_OFFSET) = *reinterpret_cast(data + idx); + // //uint8_t* byteReg = reinterpret_cast(vcBaseReg + DATA_REG_OFFSET); + // + // //byteReg[0] = data[idx]; + // //byteReg[1] = data[idx]; + // } else { + // abortPacketTransfer(); + // return returnvalue::FAILED; + // } + // // TODO: Change this after the bugfix. Right now, the PAPB ignores the content of the byte + // // width configuration.5 + // // It's okay to increment by a larger amount for the last segment here, loop will be over + // // in any case. + // idx += 4; + // } for (size_t idx = 0; idx < size; idx++) { - // if (pollInterfaceReadiness(2, false) == returnvalue::OK) { - *(vcBaseReg + DATA_REG_OFFSET) = static_cast(data[idx]); + // This delay is super-important, DO NOT REMOVE! + // Polling the GPIO or the config register too often messes up the scheduler. + // TODO: Maybe this should not be done like this. It would be better if there was a custom + // FPGA module which can accept packets and then takes care of dumping that packet into + // the PTME. DMA would be an ideal solution for this. + nanosleep(&BETWEEN_POLL_DELAY, &remDelay); + if (pollInterfaceReadiness(2, false) == returnvalue::OK) { + *(vcBaseReg + DATA_REG_OFFSET) = static_cast(data[idx]); + } else { + abortPacketTransfer(); + return returnvalue::FAILED; + } + } + nanosleep(&BETWEEN_POLL_DELAY, &remDelay); + if (pollInterfaceReadiness(2, false) == returnvalue::OK) { + completePacketTransfer(); + } else { + abortPacketTransfer(); + return returnvalue::FAILED; } - completePacketTransfer(); return returnvalue::OK; } @@ -47,33 +98,60 @@ void PapbVcInterface::startPacketTransfer(ByteWidthCfg initWidth) { void PapbVcInterface::completePacketTransfer() { *vcBaseReg = CONFIG_END; } -bool PapbVcInterface::pollReadyForPacket() const { - // Check if PAPB interface is ready to receive data. Use the configuration register for this. - // Bit 5, see PTME ptme_001_01-0-7-r2 Table 31. - uint32_t reg = *vcBaseReg; - // bool busy = (reg >> 5) & 0b1; - return (reg >> 6) & 0b1; +ReturnValue_t PapbVcInterface::pollInterfaceReadiness(uint32_t maxPollRetries, + bool checkReadyState) const { + uint32_t busyIdx = 0; + nextDelay.tv_nsec = FIRST_DELAY_PAPB_POLLING_NS; + + while (true) { + // Check if PAPB interface is ready to receive data. Use the configuration register for this. + // Bit 5, see PTME ptme_001_01-0-7-r2 Table 31. + uint32_t reg = *vcBaseReg; + bool busy = (reg >> 5) & 0b1; + bool ready = (reg >> 6) & 0b1; + if (not busy) { + return returnvalue::OK; + } + if (checkReadyState and not ready) { + return PAPB_BUSY; + } + + busyIdx++; + if (busyIdx >= maxPollRetries) { + return PAPB_BUSY; + } + + // Ignore signal handling here for now. + nanosleep(&nextDelay, &remDelay); + // Adaptive delay. + if (nextDelay.tv_nsec * 2 <= MAX_DELAY_PAPB_POLLING_NS) { + nextDelay.tv_nsec *= 2; + } + } + return returnvalue::OK; } -bool PapbVcInterface::isVcInterfaceBufferEmpty() { +void PapbVcInterface::isVcInterfaceBufferEmpty() { ReturnValue_t result = returnvalue::OK; gpio::Levels papbEmptyState = gpio::Levels::HIGH; result = gpioComIF->readGpio(papbEmptyId, papbEmptyState); if (result != returnvalue::OK) { - sif::error << "PapbVcInterface::isVcInterfaceBufferEmpty: Failed to read papb empty signal" - << std::endl; - return true; + sif::warning << "PapbVcInterface::isVcInterfaceBufferEmpty: Failed to read papb empty signal" + << std::endl; + return; } if (papbEmptyState == gpio::Levels::HIGH) { - return true; + sif::debug << "PapbVcInterface::isVcInterfaceBufferEmpty: Buffer is empty" << std::endl; + } else { + sif::debug << "PapbVcInterface::isVcInterfaceBufferEmpty: Buffer is not empty" << std::endl; } - return false; + return; } -bool PapbVcInterface::isBusy() const { return not pollReadyForPacket(); } +bool PapbVcInterface::isBusy() const { return pollInterfaceReadiness(0, true) == PAPB_BUSY; } void PapbVcInterface::cancelTransfer() { abortPacketTransfer(); } diff --git a/linux/ipcore/PapbVcInterface.h b/linux/ipcore/PapbVcInterface.h index ba6063b5..e54def5d 100644 --- a/linux/ipcore/PapbVcInterface.h +++ b/linux/ipcore/PapbVcInterface.h @@ -30,7 +30,8 @@ class PapbVcInterface : public VirtualChannelIF { * @param uioFile UIO file providing access to the PAPB bus * @param mapNum Map number of UIO map associated with this virtual channel */ - PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbEmptyId, std::string uioFile, int mapNum); + PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbBusyId, gpioId_t papbEmptyId, + std::string uioFile, int mapNum); virtual ~PapbVcInterface(); bool isBusy() const override; @@ -82,6 +83,9 @@ class PapbVcInterface : public VirtualChannelIF { static constexpr long int MAX_DELAY_PAPB_POLLING_NS = 40; LinuxLibgpioIF* gpioComIF = nullptr; + + /** Pulled to low when virtual channel not ready to receive data */ + gpioId_t papbBusyId = gpio::NO_GPIO; /** High when external buffer memory of virtual channel is empty */ gpioId_t papbEmptyId = gpio::NO_GPIO; @@ -116,13 +120,13 @@ class PapbVcInterface : public VirtualChannelIF { * * @return returnvalue::OK when ready to receive data else PAPB_BUSY. */ - inline bool pollReadyForPacket() const; + inline ReturnValue_t pollInterfaceReadiness(uint32_t maxPollRetries, bool checkReadyState) const; /** * @brief This function can be used for debugging to check whether there are packets in * the packet buffer of the virtual channel or not. */ - bool isVcInterfaceBufferEmpty(); + void isVcInterfaceBufferEmpty(); /** * @brief This function sends a complete telemetry transfer frame data field (1105 bytes) diff --git a/mission/com/PersistentLogTmStoreTask.cpp b/mission/com/PersistentLogTmStoreTask.cpp index 28545457..77f2bb7d 100644 --- a/mission/com/PersistentLogTmStoreTask.cpp +++ b/mission/com/PersistentLogTmStoreTask.cpp @@ -42,7 +42,13 @@ ReturnValue_t PersistentLogTmStoreTask::performOperation(uint8_t opCode) { if (not someonesBusy) { TaskFactory::delayTask(100); } else if (vcBusyDuringDump) { + // TODO: Might not be necessary + sif::debug << "VC busy, delaying" << std::endl; TaskFactory::delayTask(10); + } else { + // TODO: Would be best to remove this, but not delaying here can lead to evil issues. + // Polling the PAPB of the PTME core too often leads to scheuduling issues. + TaskFactory::delayTask(2); } } } diff --git a/mission/com/PersistentSingleTmStoreTask.cpp b/mission/com/PersistentSingleTmStoreTask.cpp index d6f43289..1b77365b 100644 --- a/mission/com/PersistentSingleTmStoreTask.cpp +++ b/mission/com/PersistentSingleTmStoreTask.cpp @@ -24,7 +24,13 @@ ReturnValue_t PersistentSingleTmStoreTask::performOperation(uint8_t opCode) { if (not busy) { TaskFactory::delayTask(100); } else if (dumpContext.vcBusyDuringDump) { + sif::debug << "VC busy, delaying" << std::endl; + // TODO: Might not be necessary TaskFactory::delayTask(10); + } else { + // TODO: Would be best to remove this, but not delaying here can lead to evil issues. + // Polling the PAPB of the PTME core too often leads to scheuduling issues. + TaskFactory::delayTask(2); } } } From 6c39e7dd0c78dc822f9ed9e47be20902c8be06a3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 26 Jun 2023 17:08:39 +0200 Subject: [PATCH 416/506] and go back to newest version here --- bsp_q7s/boardconfig/busConf.h | 4 - bsp_q7s/core/ObjectFactory.cpp | 29 ++--- common/config/devices/gpioIds.h | 4 - linux/ipcore/PapbVcInterface.cpp | 120 ++++---------------- linux/ipcore/PapbVcInterface.h | 10 +- mission/com/PersistentLogTmStoreTask.cpp | 6 - mission/com/PersistentSingleTmStoreTask.cpp | 6 - 7 files changed, 32 insertions(+), 147 deletions(-) diff --git a/bsp_q7s/boardconfig/busConf.h b/bsp_q7s/boardconfig/busConf.h index 6a3e0d9e..146386c4 100644 --- a/bsp_q7s/boardconfig/busConf.h +++ b/bsp_q7s/boardconfig/busConf.h @@ -85,13 +85,9 @@ static constexpr char EN_RW_4[] = "enable_rw_4"; static constexpr char RAD_SENSOR_CHIP_SELECT[] = "rad_sensor_chip_select"; static constexpr char ENABLE_RADFET[] = "enable_radfet"; -static constexpr char PAPB_BUSY_SIGNAL_VC0[] = "papb_busy_signal_vc0"; static constexpr char PAPB_EMPTY_SIGNAL_VC0[] = "papb_empty_signal_vc0"; -static constexpr char PAPB_BUSY_SIGNAL_VC1[] = "papb_busy_signal_vc1"; static constexpr char PAPB_EMPTY_SIGNAL_VC1[] = "papb_empty_signal_vc1"; -static constexpr char PAPB_BUSY_SIGNAL_VC2[] = "papb_busy_signal_vc2"; static constexpr char PAPB_EMPTY_SIGNAL_VC2[] = "papb_empty_signal_vc2"; -static constexpr char PAPB_BUSY_SIGNAL_VC3[] = "papb_busy_signal_vc3"; static constexpr char PAPB_EMPTY_SIGNAL_VC3[] = "papb_empty_signal_vc3"; static constexpr char PTME_RESETN[] = "ptme_resetn"; diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index af568553..35d30c1a 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -731,20 +731,12 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(CcsdsComponentArgs& args) { // GPIO definitions of signals connected to the virtual channel interfaces of the PTME IP Core GpioCookie* gpioCookiePtmeIp = new GpioCookie; GpiodRegularByLineName* gpio = nullptr; - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC0, "PAPB VC0"); - gpioCookiePtmeIp->addGpio(gpioIds::VC0_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC0, "PAPB VC0"); gpioCookiePtmeIp->addGpio(gpioIds::VC0_PAPB_EMPTY, gpio); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC1, "PAPB VC1"); - gpioCookiePtmeIp->addGpio(gpioIds::VC1_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC1, "PAPB VC1"); gpioCookiePtmeIp->addGpio(gpioIds::VC1_PAPB_EMPTY, gpio); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC2, "PAPB VC2"); - gpioCookiePtmeIp->addGpio(gpioIds::VC2_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC2, "PAPB VC2"); gpioCookiePtmeIp->addGpio(gpioIds::VC2_PAPB_EMPTY, gpio); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC3, "PAPB VC3"); - gpioCookiePtmeIp->addGpio(gpioIds::VC3_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC3, "PAPB VC3"); gpioCookiePtmeIp->addGpio(gpioIds::VC3_PAPB_EMPTY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PTME_RESETN, "PTME RESETN", @@ -753,19 +745,14 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(CcsdsComponentArgs& args) { gpioChecker(args.gpioComIF.addGpios(gpioCookiePtmeIp), "PTME PAPB VCs"); // Creating virtual channel interfaces - VirtualChannelIF* vc0 = - new PapbVcInterface(&args.gpioComIF, gpioIds::VC0_PAPB_BUSY, gpioIds::VC0_PAPB_EMPTY, - q7s::UIO_PTME, q7s::uiomapids::PTME_VC0); - VirtualChannelIF* vc1 = - new PapbVcInterface(&args.gpioComIF, gpioIds::VC1_PAPB_BUSY, gpioIds::VC1_PAPB_EMPTY, - q7s::UIO_PTME, q7s::uiomapids::PTME_VC1); - VirtualChannelIF* vc2 = - new PapbVcInterface(&args.gpioComIF, gpioIds::VC2_PAPB_BUSY, gpioIds::VC2_PAPB_EMPTY, - q7s::UIO_PTME, q7s::uiomapids::PTME_VC2); - VirtualChannelIF* vc3 = - new PapbVcInterface(&args.gpioComIF, gpioIds::VC3_PAPB_BUSY, gpioIds::VC3_PAPB_EMPTY, - q7s::UIO_PTME, q7s::uiomapids::PTME_VC3); - + VirtualChannelIF* vc0 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC0_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC0); + VirtualChannelIF* vc1 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC1_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC1); + VirtualChannelIF* vc2 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC2_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC2); + VirtualChannelIF* vc3 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC3_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC3); // Creating ptme object and adding virtual channel interfaces Ptme* ptme = new Ptme(objects::PTME); ptme->addVcInterface(ccsds::VC0, vc0); diff --git a/common/config/devices/gpioIds.h b/common/config/devices/gpioIds.h index ac193bd2..bed82142 100644 --- a/common/config/devices/gpioIds.h +++ b/common/config/devices/gpioIds.h @@ -96,13 +96,9 @@ enum gpioId_t { SPI_MUX, VC0_PAPB_EMPTY, - VC0_PAPB_BUSY, VC1_PAPB_EMPTY, - VC1_PAPB_BUSY, VC2_PAPB_EMPTY, - VC2_PAPB_BUSY, VC3_PAPB_EMPTY, - VC3_PAPB_BUSY, PTME_RESETN, PDEC_RESET, diff --git a/linux/ipcore/PapbVcInterface.cpp b/linux/ipcore/PapbVcInterface.cpp index 60968cc6..404f3653 100644 --- a/linux/ipcore/PapbVcInterface.cpp +++ b/linux/ipcore/PapbVcInterface.cpp @@ -7,20 +7,16 @@ #include "fsfw/serviceinterface/ServiceInterface.h" -PapbVcInterface::PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbBusyId, - gpioId_t papbEmptyId, std::string uioFile, int mapNum) - : gpioComIF(gpioComIF), - papbBusyId(papbBusyId), - papbEmptyId(papbEmptyId), - uioFile(std::move(uioFile)), - mapNum(mapNum) {} +PapbVcInterface::PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbEmptyId, + std::string uioFile, int mapNum) + : gpioComIF(gpioComIF), papbEmptyId(papbEmptyId), uioFile(std::move(uioFile)), mapNum(mapNum) {} PapbVcInterface::~PapbVcInterface() {} ReturnValue_t PapbVcInterface::initialize() { UioMapper uioMapper(uioFile, mapNum); ReturnValue_t result = uioMapper.getMappedAdress(const_cast(&vcBaseReg), - UioMapper::Permissions::WRITE_ONLY); + UioMapper::Permissions::READ_WRITE); if (result != returnvalue::OK) { return result; } @@ -32,63 +28,16 @@ ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size) { if (size < 4) { return returnvalue::FAILED; } - if (pollInterfaceReadiness(0, true) == returnvalue::OK) { + if (pollReadyForPacket()) { startPacketTransfer(ByteWidthCfg::ONE); } else { return DirectTmSinkIF::IS_BUSY; } - // TODO: This should work but does not.. :( - // size_t idx = 0; - // while (idx < size) { - // - // nanosleep(&BETWEEN_POLL_DELAY, &remDelay); - // if ((size - idx) < 4) { - // *vcBaseReg = CONFIG_DATA_INPUT | (size - idx - 1); - // usleep(1); - // } - // if (pollPapbBusySignal(2) == returnvalue::OK) { - // // vcBaseReg + DATA_REG_OFFSET + 3 = static_cast(data + idx); - // // vcBaseReg + DATA_REG_OFFSET + 2 = static_cast(data + idx + 1); - // // vcBaseReg + DATA_REG_OFFSET + 1 = static_cast(data + idx + 2); - // // vcBaseReg + DATA_REG_OFFSET = static_cast(data + idx + 3); - // - // // std::memcpy((vcBaseReg + DATA_REG_OFFSET), data + idx , nextWriteSize); - // *(vcBaseReg + DATA_REG_OFFSET) = *reinterpret_cast(data + idx); - // //uint8_t* byteReg = reinterpret_cast(vcBaseReg + DATA_REG_OFFSET); - // - // //byteReg[0] = data[idx]; - // //byteReg[1] = data[idx]; - // } else { - // abortPacketTransfer(); - // return returnvalue::FAILED; - // } - // // TODO: Change this after the bugfix. Right now, the PAPB ignores the content of the byte - // // width configuration.5 - // // It's okay to increment by a larger amount for the last segment here, loop will be over - // // in any case. - // idx += 4; - // } for (size_t idx = 0; idx < size; idx++) { - // This delay is super-important, DO NOT REMOVE! - // Polling the GPIO or the config register too often messes up the scheduler. - // TODO: Maybe this should not be done like this. It would be better if there was a custom - // FPGA module which can accept packets and then takes care of dumping that packet into - // the PTME. DMA would be an ideal solution for this. - nanosleep(&BETWEEN_POLL_DELAY, &remDelay); - if (pollInterfaceReadiness(2, false) == returnvalue::OK) { - *(vcBaseReg + DATA_REG_OFFSET) = static_cast(data[idx]); - } else { - abortPacketTransfer(); - return returnvalue::FAILED; - } - } - nanosleep(&BETWEEN_POLL_DELAY, &remDelay); - if (pollInterfaceReadiness(2, false) == returnvalue::OK) { - completePacketTransfer(); - } else { - abortPacketTransfer(); - return returnvalue::FAILED; + // if (pollInterfaceReadiness(2, false) == returnvalue::OK) { + *(vcBaseReg + DATA_REG_OFFSET) = static_cast(data[idx]); } + completePacketTransfer(); return returnvalue::OK; } @@ -98,60 +47,33 @@ void PapbVcInterface::startPacketTransfer(ByteWidthCfg initWidth) { void PapbVcInterface::completePacketTransfer() { *vcBaseReg = CONFIG_END; } -ReturnValue_t PapbVcInterface::pollInterfaceReadiness(uint32_t maxPollRetries, - bool checkReadyState) const { - uint32_t busyIdx = 0; - nextDelay.tv_nsec = FIRST_DELAY_PAPB_POLLING_NS; - - while (true) { - // Check if PAPB interface is ready to receive data. Use the configuration register for this. - // Bit 5, see PTME ptme_001_01-0-7-r2 Table 31. - uint32_t reg = *vcBaseReg; - bool busy = (reg >> 5) & 0b1; - bool ready = (reg >> 6) & 0b1; - if (not busy) { - return returnvalue::OK; - } - if (checkReadyState and not ready) { - return PAPB_BUSY; - } - - busyIdx++; - if (busyIdx >= maxPollRetries) { - return PAPB_BUSY; - } - - // Ignore signal handling here for now. - nanosleep(&nextDelay, &remDelay); - // Adaptive delay. - if (nextDelay.tv_nsec * 2 <= MAX_DELAY_PAPB_POLLING_NS) { - nextDelay.tv_nsec *= 2; - } - } - return returnvalue::OK; +bool PapbVcInterface::pollReadyForPacket() const { + // Check if PAPB interface is ready to receive data. Use the configuration register for this. + // Bit 5, see PTME ptme_001_01-0-7-r2 Table 31. + uint32_t reg = *vcBaseReg; + // bool busy = (reg >> 5) & 0b1; + return (reg >> 6) & 0b1; } -void PapbVcInterface::isVcInterfaceBufferEmpty() { +bool PapbVcInterface::isVcInterfaceBufferEmpty() { ReturnValue_t result = returnvalue::OK; gpio::Levels papbEmptyState = gpio::Levels::HIGH; result = gpioComIF->readGpio(papbEmptyId, papbEmptyState); if (result != returnvalue::OK) { - sif::warning << "PapbVcInterface::isVcInterfaceBufferEmpty: Failed to read papb empty signal" - << std::endl; - return; + sif::error << "PapbVcInterface::isVcInterfaceBufferEmpty: Failed to read papb empty signal" + << std::endl; + return true; } if (papbEmptyState == gpio::Levels::HIGH) { - sif::debug << "PapbVcInterface::isVcInterfaceBufferEmpty: Buffer is empty" << std::endl; - } else { - sif::debug << "PapbVcInterface::isVcInterfaceBufferEmpty: Buffer is not empty" << std::endl; + return true; } - return; + return false; } -bool PapbVcInterface::isBusy() const { return pollInterfaceReadiness(0, true) == PAPB_BUSY; } +bool PapbVcInterface::isBusy() const { return not pollReadyForPacket(); } void PapbVcInterface::cancelTransfer() { abortPacketTransfer(); } diff --git a/linux/ipcore/PapbVcInterface.h b/linux/ipcore/PapbVcInterface.h index e54def5d..ba6063b5 100644 --- a/linux/ipcore/PapbVcInterface.h +++ b/linux/ipcore/PapbVcInterface.h @@ -30,8 +30,7 @@ class PapbVcInterface : public VirtualChannelIF { * @param uioFile UIO file providing access to the PAPB bus * @param mapNum Map number of UIO map associated with this virtual channel */ - PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbBusyId, gpioId_t papbEmptyId, - std::string uioFile, int mapNum); + PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbEmptyId, std::string uioFile, int mapNum); virtual ~PapbVcInterface(); bool isBusy() const override; @@ -83,9 +82,6 @@ class PapbVcInterface : public VirtualChannelIF { static constexpr long int MAX_DELAY_PAPB_POLLING_NS = 40; LinuxLibgpioIF* gpioComIF = nullptr; - - /** Pulled to low when virtual channel not ready to receive data */ - gpioId_t papbBusyId = gpio::NO_GPIO; /** High when external buffer memory of virtual channel is empty */ gpioId_t papbEmptyId = gpio::NO_GPIO; @@ -120,13 +116,13 @@ class PapbVcInterface : public VirtualChannelIF { * * @return returnvalue::OK when ready to receive data else PAPB_BUSY. */ - inline ReturnValue_t pollInterfaceReadiness(uint32_t maxPollRetries, bool checkReadyState) const; + inline bool pollReadyForPacket() const; /** * @brief This function can be used for debugging to check whether there are packets in * the packet buffer of the virtual channel or not. */ - void isVcInterfaceBufferEmpty(); + bool isVcInterfaceBufferEmpty(); /** * @brief This function sends a complete telemetry transfer frame data field (1105 bytes) diff --git a/mission/com/PersistentLogTmStoreTask.cpp b/mission/com/PersistentLogTmStoreTask.cpp index 77f2bb7d..28545457 100644 --- a/mission/com/PersistentLogTmStoreTask.cpp +++ b/mission/com/PersistentLogTmStoreTask.cpp @@ -42,13 +42,7 @@ ReturnValue_t PersistentLogTmStoreTask::performOperation(uint8_t opCode) { if (not someonesBusy) { TaskFactory::delayTask(100); } else if (vcBusyDuringDump) { - // TODO: Might not be necessary - sif::debug << "VC busy, delaying" << std::endl; TaskFactory::delayTask(10); - } else { - // TODO: Would be best to remove this, but not delaying here can lead to evil issues. - // Polling the PAPB of the PTME core too often leads to scheuduling issues. - TaskFactory::delayTask(2); } } } diff --git a/mission/com/PersistentSingleTmStoreTask.cpp b/mission/com/PersistentSingleTmStoreTask.cpp index 1b77365b..d6f43289 100644 --- a/mission/com/PersistentSingleTmStoreTask.cpp +++ b/mission/com/PersistentSingleTmStoreTask.cpp @@ -24,13 +24,7 @@ ReturnValue_t PersistentSingleTmStoreTask::performOperation(uint8_t opCode) { if (not busy) { TaskFactory::delayTask(100); } else if (dumpContext.vcBusyDuringDump) { - sif::debug << "VC busy, delaying" << std::endl; - // TODO: Might not be necessary TaskFactory::delayTask(10); - } else { - // TODO: Would be best to remove this, but not delaying here can lead to evil issues. - // Polling the PAPB of the PTME core too often leads to scheuduling issues. - TaskFactory::delayTask(2); } } } From 52f5b088bfdbf3dbaaac93bdce33c624b5be34be Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 26 Jun 2023 17:21:57 +0200 Subject: [PATCH 417/506] add version constraints for FW version read --- bsp_q7s/core/CoreController.cpp | 25 +++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index f9242da3..9e285c4f 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -190,12 +190,14 @@ ReturnValue_t CoreController::initialize() { sif::warning << "Subscribing for GPS GPS_FIX_CHANGE event failed" << std::endl; } - UioMapper sysRomMapper(q7s::UIO_SYS_ROM); - result = sysRomMapper.getMappedAdress(&mappedSysRomAddr, UioMapper::Permissions::READ_ONLY); - if (result != returnvalue::OK) { - // TODO: This might be a reason to switch to another image.. - sif::error << "Getting mapped SYS ROM UIO address failed" << std::endl; - return ObjectManager::CHILD_INIT_FAILED; + if (common::OBSW_VERSION_MAJOR >= 6 or common::OBSW_VERSION_MAJOR == 4) { + UioMapper sysRomMapper(q7s::UIO_SYS_ROM); + result = sysRomMapper.getMappedAdress(&mappedSysRomAddr, UioMapper::Permissions::READ_ONLY); + if (result != returnvalue::OK) { + // TODO: This might be a reason to switch to another image.. + sif::error << "Getting mapped SYS ROM UIO address failed" << std::endl; + return ObjectManager::CHILD_INIT_FAILED; + } } return returnvalue::OK; } @@ -2518,10 +2520,13 @@ void CoreController::announceVersionInfo() { } triggerEvent(VERSION_INFO, p1, p2); - if (mappedSysRomAddr != nullptr) { - uint32_t p1Firmware = *(reinterpret_cast(mappedSysRomAddr)); - uint32_t p2Firmware = *(reinterpret_cast(mappedSysRomAddr) + 1); - triggerEvent(FIRMWARE_INFO, p1Firmware, p2Firmware); + + if (common::OBSW_VERSION_MAJOR >= 6 or common::OBSW_VERSION_MAJOR == 4) { + if (mappedSysRomAddr != nullptr) { + uint32_t p1Firmware = *(reinterpret_cast(mappedSysRomAddr)); + uint32_t p2Firmware = *(reinterpret_cast(mappedSysRomAddr) + 1); + triggerEvent(FIRMWARE_INFO, p1Firmware, p2Firmware); + } } } From 72ffa365d48432c324aeea2cc1355113988162b1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 26 Jun 2023 17:29:53 +0200 Subject: [PATCH 418/506] version constraints for PL I2C reset pin --- bsp_q7s/core/ObjectFactory.cpp | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index af568553..0e55661e 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -1030,16 +1030,18 @@ void ObjectFactory::createRadSensorChipSelect(LinuxLibgpioIF* gpioIF) { void ObjectFactory::createPlI2cResetGpio(LinuxLibgpioIF* gpioIF) { using namespace gpio; - if (gpioIF == nullptr) { - return; + if (common::OBSW_VERSION_MAJOR >= 6 or common::OBSW_VERSION_MAJOR == 4) { + if (gpioIF == nullptr) { + return; + } + GpioCookie* gpioI2cResetnCookie = new GpioCookie; + GpiodRegularByLineName* gpioI2cResetn = new GpiodRegularByLineName( + q7s::gpioNames::PL_I2C_ARESETN, "PL_I2C_ARESETN", Direction::OUT, Levels::HIGH); + gpioI2cResetnCookie->addGpio(gpioIds::PL_I2C_ARESETN, gpioI2cResetn); + gpioChecker(gpioIF->addGpios(gpioI2cResetnCookie), "PL I2C ARESETN"); + // Reset I2C explicitely again. + gpioIF->pullLow(gpioIds::PL_I2C_ARESETN); + TaskFactory::delayTask(1); + gpioIF->pullHigh(gpioIds::PL_I2C_ARESETN); } - GpioCookie* gpioI2cResetnCookie = new GpioCookie; - GpiodRegularByLineName* gpioI2cResetn = new GpiodRegularByLineName( - q7s::gpioNames::PL_I2C_ARESETN, "PL_I2C_ARESETN", Direction::OUT, Levels::HIGH); - gpioI2cResetnCookie->addGpio(gpioIds::PL_I2C_ARESETN, gpioI2cResetn); - gpioChecker(gpioIF->addGpios(gpioI2cResetnCookie), "PL I2C ARESETN"); - // Reset I2C explicitely again. - gpioIF->pullLow(gpioIds::PL_I2C_ARESETN); - TaskFactory::delayTask(1); - gpioIF->pullHigh(gpioIds::PL_I2C_ARESETN); } From bd4e05c944eb25c58a2578172b174847b8078ecd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 26 Jun 2023 17:30:30 +0200 Subject: [PATCH 419/506] bump version specifier --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e234cae..d391877c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,9 +9,9 @@ # ############################################################################## cmake_minimum_required(VERSION 3.13) -set(OBSW_VERSION_MAJOR 4) +set(OBSW_VERSION_MAJOR 5) set(OBSW_VERSION_MINOR 0) -set(OBSW_VERSION_REVISION 1) +set(OBSW_VERSION_REVISION 0) # set(CMAKE_VERBOSE TRUE) From 27f77799a6e9614fc13857635bb06c388e2406e5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 26 Jun 2023 17:33:17 +0200 Subject: [PATCH 420/506] bump major version --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d391877c..60412e93 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ # ############################################################################## cmake_minimum_required(VERSION 3.13) -set(OBSW_VERSION_MAJOR 5) +set(OBSW_VERSION_MAJOR 6) set(OBSW_VERSION_MINOR 0) set(OBSW_VERSION_REVISION 0) From 3a20748ff636f3466465bb5db379b7176f92ae37 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 26 Jun 2023 17:36:47 +0200 Subject: [PATCH 421/506] create a v5.0.0 snapshot --- CHANGELOG.md | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index b62369e0..7a89e679 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,7 +20,16 @@ will consitute of a breaking change warranting a new major release: - Important bugfixes for PTME. See `q7s-package` CHANGELOG. -# [v5.0.0] to be released +# [v5.1.0] to be released + +## Changed + +- Added `sync` syscall in graceful shutdown handler +- Graceful shutdown is now performed by the reboot watchdog +- There is now a separate file for the total reboot counter. The reboot watchdog has its own local + counters to determine whether a reboot is necessary. + +# [v5.0.0] 2023-06-26 v3.3.1 and all following version will now be moved to v5.0.0 with the additional changes listed here. This was done because the firmware update (v4.0.0) is not working right now and it is not @@ -28,14 +37,8 @@ known when and how it will be fixed. Because of that, all updates to make the SW firmware, which are limited to a few files will be moved to a dev branch so regular development compatible to the old firmware can continue. -## Changed - -- This version is compatible to the old firmware and some changes which only work with the new - firmware have been reverted. -- Added `sync` syscall in graceful shutdown handler -- Graceful shutdown is now performed by the reboot watchdog -- There is now a separate file for the total reboot counter. The reboot watchdog has its own local - counters to determine whether a reboot is necessary. +TLDR: This version is compatible to the old firmware and some changes which only work with the new +firmware have been reverted. # [v4.0.1] 2023-06-24 From 67024ec933db2e12d3c3352712048f382d64a040 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 26 Jun 2023 17:39:41 +0200 Subject: [PATCH 422/506] that was already pulled in --- CHANGELOG.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7a89e679..9b0d0e5b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,13 +22,6 @@ will consitute of a breaking change warranting a new major release: # [v5.1.0] to be released -## Changed - -- Added `sync` syscall in graceful shutdown handler -- Graceful shutdown is now performed by the reboot watchdog -- There is now a separate file for the total reboot counter. The reboot watchdog has its own local - counters to determine whether a reboot is necessary. - # [v5.0.0] 2023-06-26 v3.3.1 and all following version will now be moved to v5.0.0 with the additional changes listed @@ -40,6 +33,13 @@ compatible to the old firmware can continue. TLDR: This version is compatible to the old firmware and some changes which only work with the new firmware have been reverted. +## Changed + +- Added `sync` syscall in graceful shutdown handler +- Graceful shutdown is now performed by the reboot watchdog +- There is now a separate file for the total reboot counter. The reboot watchdog has its own local + counters to determine whether a reboot is necessary. + # [v4.0.1] 2023-06-24 ## Fixed From e752cdcc67701a05085339c188bf2248e4eb39ca Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 26 Jun 2023 18:10:41 +0200 Subject: [PATCH 423/506] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index a85a38c8..6aff3250 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit a85a38c882af968200a0dd54ded9fd99b3961e7a +Subproject commit 6aff3250c29f5243eb4a6111ba0a5c0cc1a3033c From 15a67b81f98b9037c8fc06d260b8e94bd462bcfc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 27 Jun 2023 17:36:52 +0200 Subject: [PATCH 424/506] various improvements --- CHANGELOG.md | 6 ++ bsp_q7s/fmObjectFactory.cpp | 4 +- dummies/TemperatureSensorInserter.cpp | 21 ++++- dummies/TemperatureSensorInserter.h | 1 + mission/tcs/HeaterHandler.cpp | 107 +++++++++++++++----------- mission/tcs/HeaterHandler.h | 2 + 6 files changed, 92 insertions(+), 49 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index dbe08848..d446bd49 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,6 +27,12 @@ will consitute of a breaking change warranting a new major release: - Internal error reporter set is now enabled by default and generated every 120 seconds. - Persistent TM store dumps are now performed in chronological order. +## Fixed + +- Various robustness improvements for the heater handler. The heater handler will now only + process the command queue if it is not busy with switch commanding which reduced the amount + of possible bugs. + # [v5.0.0] 2023-06-26 v3.3.1 and all following version will now be moved to v5.0.0 with the additional changes listed diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index da14f58c..d9329fc0 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -33,8 +33,8 @@ void ObjectFactory::produce(void* args) { PersistentTmStores stores; ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel, - *SdCardManager::instance(), &ipcStore, &tmStore, stores, - 200, true); + *SdCardManager::instance(), &ipcStore, &tmStore, stores, 200, + true); LinuxLibgpioIF* gpioComIF = nullptr; SerialComIF* uartComIF = nullptr; diff --git a/dummies/TemperatureSensorInserter.cpp b/dummies/TemperatureSensorInserter.cpp index 14a005aa..c58416f7 100644 --- a/dummies/TemperatureSensorInserter.cpp +++ b/dummies/TemperatureSensorInserter.cpp @@ -15,7 +15,7 @@ TemperatureSensorInserter::TemperatureSensorInserter(object_id_t objectId, tmp1075DummyMap(std::move(tempTmpSensorDummies_)) {} ReturnValue_t TemperatureSensorInserter::initialize() { - testCase = TestCase::NONE; + testCase = TestCase::COLD_PLOC_CONSECUTIVE; return returnvalue::OK; } @@ -96,6 +96,25 @@ ReturnValue_t TemperatureSensorInserter::performOperation(uint8_t opCode) { } break; } + case (TestCase::COLD_PLOC_CONSECUTIVE): { + if (cycles == 15) { + sif::debug << "Setting cold PLOC temperature" << std::endl; + max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(-15, true); + } + if (cycles == 30) { + sif::debug << "Setting warmer PLOC temperature" << std::endl; + max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(0, true); + } + if (cycles == 45) { + sif::debug << "Setting cold PLOC temperature again" << std::endl; + max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(-15, true); + } + if (cycles == 60) { + sif::debug << "Setting warmer PLOC temperature again" << std::endl; + max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(0, true); + } + break; + } case (TestCase::COLD_CAMERA): { if (cycles == 15) { sif::debug << "Setting cold CAM temperature" << std::endl; diff --git a/dummies/TemperatureSensorInserter.h b/dummies/TemperatureSensorInserter.h index eb6cc1ba..009f5b7d 100644 --- a/dummies/TemperatureSensorInserter.h +++ b/dummies/TemperatureSensorInserter.h @@ -32,6 +32,7 @@ class TemperatureSensorInserter : public ExecutableObjectIF, public SystemObject COLD_STR = 4, COLD_STR_CONSECUTIVE = 5, COLD_CAMERA = 6, + COLD_PLOC_CONSECUTIVE = 7, }; int iteration = 0; uint32_t cycles = 0; diff --git a/mission/tcs/HeaterHandler.cpp b/mission/tcs/HeaterHandler.cpp index de4b600d..9f7ba43c 100644 --- a/mission/tcs/HeaterHandler.cpp +++ b/mission/tcs/HeaterHandler.cpp @@ -51,9 +51,13 @@ ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) { if (mainLineSwitcher->getSwitchState(mainLineSwitch) == SWITCH_OFF) { waitForSwitchOff = false; mode = MODE_OFF; + busyWithSwitchCommanding = false; modeHelper.modeChanged(mode, submode); } } + if (busyWithSwitchCommanding and heaterCmdBusyCd.hasTimedOut()) { + busyWithSwitchCommanding = false; + } } catch (const std::out_of_range& e) { sif::warning << "HeaterHandler::performOperation: " "Out of range error | " @@ -102,20 +106,22 @@ void HeaterHandler::readCommandQueue() { ReturnValue_t result = returnvalue::OK; CommandMessage command; do { - result = commandQueue->receiveMessage(&command); - if (result == MessageQueueIF::EMPTY) { - break; - } else if (result != returnvalue::OK) { - sif::warning << "HeaterHandler::readCommandQueue: Message reception error" << std::endl; - break; - } - result = actionHelper.handleActionMessage(&command); - if (result == returnvalue::OK) { - continue; - } - result = modeHelper.handleModeCommand(&command); - if (result == returnvalue::OK) { - continue; + if (not busyWithSwitchCommanding) { + result = commandQueue->receiveMessage(&command); + if (result == MessageQueueIF::EMPTY) { + break; + } else if (result != returnvalue::OK) { + sif::warning << "HeaterHandler::readCommandQueue: Message reception error" << std::endl; + break; + } + result = modeHelper.handleModeCommand(&command); + if (result == returnvalue::OK) { + continue; + } + result = actionHelper.handleActionMessage(&command); + if (result == returnvalue::OK) { + continue; + } } } while (result == returnvalue::OK); } @@ -167,6 +173,8 @@ ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t heater.action = action; heater.cmdActive = true; heater.replyQueue = commandedBy; + busyWithSwitchCommanding = true; + heaterCmdBusyCd.resetTimer(); return returnvalue::OK; } @@ -249,6 +257,7 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) { sif::error << "HeaterHandler::handleSwitchOnCommand: Main switch setting on timeout" << std::endl; heater.cmdActive = false; + busyWithSwitchCommanding = false; heater.waitMainSwitchOn = false; if (heater.replyQueue != commandQueue->getId()) { actionHelper.finish(false, heater.replyQueue, heater.action, MAIN_SWITCH_SET_TIMEOUT); @@ -259,27 +268,30 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) { // Check state of main line switch ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch); if (mainSwitchState == PowerSwitchIF::SWITCH_ON) { - if (getSwitchState(heaterIdx) == SwitchState::OFF) { - gpioId_t gpioId = heater.gpioId; - result = gpioInterface->pullHigh(gpioId); - if (result != returnvalue::OK) { - sif::error << "HeaterHandler::handleSwitchOnCommand: Failed to pull gpio with id " << gpioId - << " high" << std::endl; - triggerEvent(GPIO_PULL_HIGH_FAILED, result); - } else { + gpioId_t gpioId = heater.gpioId; + result = gpioInterface->pullHigh(gpioId); + if (result != returnvalue::OK) { + sif::error << "HeaterHandler::handleSwitchOnCommand: Failed to pull GPIO with ID " << gpioId + << " high" << std::endl; + triggerEvent(GPIO_PULL_HIGH_FAILED, result); + } + if (result == returnvalue::OK) { + if (getSwitchState(heaterIdx) == SwitchState::OFF) { triggerEvent(HEATER_WENT_ON, heaterIdx, 0); - EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO, - MODE_ON, 0); { MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); heater.switchState = ON; } + + } else { + triggerEvent(SWITCH_ALREADY_ON, heaterIdx); } - } else { - triggerEvent(SWITCH_ALREADY_ON, heaterIdx); + EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO, + MODE_ON, 0); + busyWithSwitchCommanding = false; + mode = HasModesIF::MODE_ON; + modeHelper.modeChanged(mode, submode); } - mode = HasModesIF::MODE_ON; - modeHelper.modeChanged(mode, submode); // There is no need to send action finish replies if the sender was the // HeaterHandler itself if (heater.replyQueue != commandQueue->getId()) { @@ -312,30 +324,33 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) { void HeaterHandler::handleSwitchOffCommand(heater::Switch heaterIdx) { ReturnValue_t result = returnvalue::OK; auto& heater = heaterVec.at(heaterIdx); - // Check whether switch is already off - if (getSwitchState(heaterIdx)) { - gpioId_t gpioId = heater.gpioId; - result = gpioInterface->pullLow(gpioId); - if (result != returnvalue::OK) { - sif::error << "HeaterHandler::handleSwitchOffCommand: Failed to pull gpio with id" << gpioId - << " low" << std::endl; - triggerEvent(GPIO_PULL_LOW_FAILED, result); - } else { + gpioId_t gpioId = heater.gpioId; + result = gpioInterface->pullLow(gpioId); + if (result != returnvalue::OK) { + sif::error << "HeaterHandler::handleSwitchOffCommand: Failed to pull gpio with id" << gpioId + << " low" << std::endl; + triggerEvent(GPIO_PULL_LOW_FAILED, result); + } + if (result == returnvalue::OK) { + // Check whether switch is already off + if (getSwitchState(heaterIdx) == SwitchState::ON) { { MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); heater.switchState = OFF; } triggerEvent(HEATER_WENT_OFF, heaterIdx, 0); - EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO, - MODE_OFF, 0); - // When all switches are off, also main line switch will be turned off - if (allSwitchesOff()) { - mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF); - waitForSwitchOff = true; - } + } else { + triggerEvent(SWITCH_ALREADY_OFF, heaterIdx); + } + EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO, + MODE_OFF, 0); + // When all switches are off, also main line switch will be turned off + if (allSwitchesOff()) { + mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF); + waitForSwitchOff = true; + } else { + busyWithSwitchCommanding = false; } - } else { - triggerEvent(SWITCH_ALREADY_OFF, heaterIdx); } if (heater.replyQueue != NO_COMMANDER) { // Report back switch command reply if necessary diff --git a/mission/tcs/HeaterHandler.h b/mission/tcs/HeaterHandler.h index 609ac725..0198ab04 100644 --- a/mission/tcs/HeaterHandler.h +++ b/mission/tcs/HeaterHandler.h @@ -148,6 +148,7 @@ class HeaterHandler : public ExecutableObjectIF, /** Size of command queue */ size_t cmdQueueSize = 20; bool waitForSwitchOff = true; + bool busyWithSwitchCommanding = false; GpioIF* gpioInterface = nullptr; @@ -163,6 +164,7 @@ class HeaterHandler : public ExecutableObjectIF, power::Switch_t mainLineSwitch; ActionHelper actionHelper; + Countdown heaterCmdBusyCd = Countdown(2000); StorageManagerIF* ipcStore = nullptr; From 1e767afa118c19ef884b11ca8d93130645782b9b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 27 Jun 2023 17:55:49 +0200 Subject: [PATCH 425/506] only handle one message per cycle --- mission/tcs/HeaterHandler.cpp | 47 +++++++++++++++-------------------- 1 file changed, 20 insertions(+), 27 deletions(-) diff --git a/mission/tcs/HeaterHandler.cpp b/mission/tcs/HeaterHandler.cpp index 9f7ba43c..8416a7f7 100644 --- a/mission/tcs/HeaterHandler.cpp +++ b/mission/tcs/HeaterHandler.cpp @@ -105,25 +105,23 @@ ReturnValue_t HeaterHandler::initializeHeaterMap() { void HeaterHandler::readCommandQueue() { ReturnValue_t result = returnvalue::OK; CommandMessage command; - do { - if (not busyWithSwitchCommanding) { - result = commandQueue->receiveMessage(&command); - if (result == MessageQueueIF::EMPTY) { - break; - } else if (result != returnvalue::OK) { - sif::warning << "HeaterHandler::readCommandQueue: Message reception error" << std::endl; - break; - } - result = modeHelper.handleModeCommand(&command); - if (result == returnvalue::OK) { - continue; - } - result = actionHelper.handleActionMessage(&command); - if (result == returnvalue::OK) { - continue; - } + if (not busyWithSwitchCommanding) { + result = commandQueue->receiveMessage(&command); + if (result == MessageQueueIF::EMPTY) { + return; + } else if (result != returnvalue::OK) { + sif::warning << "HeaterHandler::readCommandQueue: Message reception error" << std::endl; + return; } - } while (result == returnvalue::OK); + result = modeHelper.handleModeCommand(&command); + if (result == returnvalue::OK) { + return; + } + result = actionHelper.handleActionMessage(&command); + if (result == returnvalue::OK) { + return; + } + } } ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, @@ -276,15 +274,10 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) { triggerEvent(GPIO_PULL_HIGH_FAILED, result); } if (result == returnvalue::OK) { - if (getSwitchState(heaterIdx) == SwitchState::OFF) { - triggerEvent(HEATER_WENT_ON, heaterIdx, 0); - { - MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); - heater.switchState = ON; - } - - } else { - triggerEvent(SWITCH_ALREADY_ON, heaterIdx); + triggerEvent(HEATER_WENT_ON, heaterIdx, 0); + { + MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + heater.switchState = ON; } EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO, MODE_ON, 0); From 50a8a6fffe974e0cb2ba9288f265f2c5ff35b505 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 27 Jun 2023 18:02:49 +0200 Subject: [PATCH 426/506] more changelog --- CHANGELOG.md | 6 +++--- bsp_q7s/core/scheduling.cpp | 4 ++++ dummies/TemperatureSensorInserter.cpp | 2 +- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d446bd49..0974f124 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -26,12 +26,12 @@ will consitute of a breaking change warranting a new major release: - Internal error reporter set is now enabled by default and generated every 120 seconds. - Persistent TM store dumps are now performed in chronological order. - -## Fixed - - Various robustness improvements for the heater handler. The heater handler will now only process the command queue if it is not busy with switch commanding which reduced the amount of possible bugs. +- The heater handler is only able to process messages stricly sequentially now but is scheduled + twice in a 0.5 second slot to something like a consecutive heater ON or OFF command can still + be handled relatively quickly. # [v5.0.0] 2023-06-26 diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index 71da5bdc..90918cf3 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -324,6 +324,10 @@ void scheduling::initTasks() { if (result != returnvalue::OK) { scheduling::printAddObjectError("HEATER_HANDLER", objects::HEATER_HANDLER); } + result = tcsSystemTask->addComponent(objects::HEATER_HANDLER); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("HEATER_HANDLER", objects::HEATER_HANDLER); + } #if OBSW_ADD_SYRLINKS == 1 PeriodicTaskIF* syrlinksCom = factory->createPeriodicTask( diff --git a/dummies/TemperatureSensorInserter.cpp b/dummies/TemperatureSensorInserter.cpp index c58416f7..5c89d258 100644 --- a/dummies/TemperatureSensorInserter.cpp +++ b/dummies/TemperatureSensorInserter.cpp @@ -15,7 +15,7 @@ TemperatureSensorInserter::TemperatureSensorInserter(object_id_t objectId, tmp1075DummyMap(std::move(tempTmpSensorDummies_)) {} ReturnValue_t TemperatureSensorInserter::initialize() { - testCase = TestCase::COLD_PLOC_CONSECUTIVE; + testCase = TestCase::NONE; return returnvalue::OK; } From fba87cc0e8cb067ef25e3b9fd6b032df97a8161f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 27 Jun 2023 18:09:38 +0200 Subject: [PATCH 427/506] changelog typos --- CHANGELOG.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0974f124..0675eea0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,10 +27,10 @@ will consitute of a breaking change warranting a new major release: - Internal error reporter set is now enabled by default and generated every 120 seconds. - Persistent TM store dumps are now performed in chronological order. - Various robustness improvements for the heater handler. The heater handler will now only - process the command queue if it is not busy with switch commanding which reduced the amount + process the command queue if it is not busy with switch commanding which reduces the amount of possible bugs. - The heater handler is only able to process messages stricly sequentially now but is scheduled - twice in a 0.5 second slot to something like a consecutive heater ON or OFF command can still + twice in a 0.5 second slot so something like a consecutive heater ON or OFF command can still be handled relatively quickly. # [v5.0.0] 2023-06-26 From c3e66f5320ba79f4c4a7615429d177cf1e55a11e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 28 Jun 2023 13:18:00 +0200 Subject: [PATCH 428/506] changelogge --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index dbe08848..eeaed035 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -26,6 +26,7 @@ will consitute of a breaking change warranting a new major release: - Internal error reporter set is now enabled by default and generated every 120 seconds. - Persistent TM store dumps are now performed in chronological order. +- Increase Syrlinks RX HK rate to 5.0 seconds during a pass. # [v5.0.0] 2023-06-26 From 52fedb94e806bb7173cf65d70304c05e1f96bc78 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 28 Jun 2023 13:20:50 +0200 Subject: [PATCH 429/506] bump fsfw and tmtc --- fsfw | 2 +- tmtc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/fsfw b/fsfw index 7e8845f2..8da89eba 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 7e8845f2c2565b1486ff37be0519d3e15b4e1a13 +Subproject commit 8da89eba80f73cb05e5c38fc012456f1d9569af5 diff --git a/tmtc b/tmtc index ec0ebc36..8caa408c 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit ec0ebc365308198046addc94909b1bca8678aa5a +Subproject commit 8caa408cbdf7eebdd441cff580063283ab81ffe1 From eb27ab4bb037981f20714318440baf16678353a9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 28 Jun 2023 13:32:35 +0200 Subject: [PATCH 430/506] fix and optimization --- mission/com/CcsdsIpCoreHandler.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/mission/com/CcsdsIpCoreHandler.cpp b/mission/com/CcsdsIpCoreHandler.cpp index 766fd67c..78533165 100644 --- a/mission/com/CcsdsIpCoreHandler.cpp +++ b/mission/com/CcsdsIpCoreHandler.cpp @@ -246,7 +246,12 @@ ReturnValue_t CcsdsIpCoreHandler::checkModeCommand(Mode_t mode, Submode_t submod void CcsdsIpCoreHandler::startTransition(Mode_t mode, Submode_t submode) { triggerEvent(CHANGING_MODE, mode, submode); if (mode == HasModesIF::MODE_ON) { - if (this->submode != submode) { + // Check whether the rate actually changes. + if ((this->submode != submode) and + ((submode == static_cast(com::CcsdsSubmode::DATARATE_LOW) and + ptmeConfig.getRate() != RATE_100KBPS)) or + ((submode == static_cast(com::CcsdsSubmode::DATARATE_HIGH) and + ptmeConfig.getRate() != RATE_500KBPS))) { initPtmeUpdateAfterXCycles(); updateContext.enableTransmitAfterPtmeUpdate = true; updateContext.updateClockRate = true; @@ -255,7 +260,6 @@ void CcsdsIpCoreHandler::startTransition(Mode_t mode, Submode_t submode) { updateContext.setModeAfterUpdate = true; return; } - if(ptmeConfig.setRate(bitRate)) // No rate change, so enable transmitter right away. enableTransmit(); } else if (mode == HasModesIF::MODE_OFF) { From 4c63fed69d0a3737d5157b7743bf74fab82c927e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 28 Jun 2023 13:35:38 +0200 Subject: [PATCH 431/506] that is annoying --- mission/com/CcsdsIpCoreHandler.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/mission/com/CcsdsIpCoreHandler.cpp b/mission/com/CcsdsIpCoreHandler.cpp index 78533165..2e068add 100644 --- a/mission/com/CcsdsIpCoreHandler.cpp +++ b/mission/com/CcsdsIpCoreHandler.cpp @@ -246,12 +246,13 @@ ReturnValue_t CcsdsIpCoreHandler::checkModeCommand(Mode_t mode, Submode_t submod void CcsdsIpCoreHandler::startTransition(Mode_t mode, Submode_t submode) { triggerEvent(CHANGING_MODE, mode, submode); if (mode == HasModesIF::MODE_ON) { + uint32_t currenRate = ptmeConfig.getRate(); // Check whether the rate actually changes. if ((this->submode != submode) and - ((submode == static_cast(com::CcsdsSubmode::DATARATE_LOW) and - ptmeConfig.getRate() != RATE_100KBPS)) or - ((submode == static_cast(com::CcsdsSubmode::DATARATE_HIGH) and - ptmeConfig.getRate() != RATE_500KBPS))) { + (((submode == static_cast(com::CcsdsSubmode::DATARATE_LOW) and + (currenRate != RATE_100KBPS))) or + ((submode == static_cast(com::CcsdsSubmode::DATARATE_HIGH) and + (currenRate != RATE_500KBPS))))) { initPtmeUpdateAfterXCycles(); updateContext.enableTransmitAfterPtmeUpdate = true; updateContext.updateClockRate = true; From 90bafa717bd5d81b3c1d29e3082415d82bd8e895 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 28 Jun 2023 13:41:51 +0200 Subject: [PATCH 432/506] prep v5.1.0 --- CHANGELOG.md | 2 ++ CMakeLists.txt | 2 +- tmtc | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4ea572c4..4fda0e54 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,6 +22,8 @@ will consitute of a breaking change warranting a new major release: # [v5.1.0] to be released +- `eive-tmtc` version v5.1.0 + ## Changed - Persistent TM store dumps are now performed in chronological order. diff --git a/CMakeLists.txt b/CMakeLists.txt index d391877c..c63d32ad 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 5) -set(OBSW_VERSION_MINOR 0) +set(OBSW_VERSION_MINOR 1) set(OBSW_VERSION_REVISION 0) # set(CMAKE_VERBOSE TRUE) diff --git a/tmtc b/tmtc index 8caa408c..18304c31 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 8caa408cbdf7eebdd441cff580063283ab81ffe1 +Subproject commit 18304c31fa423b1af6ff47764d4be81c7f20c8f2 From c881b36630b28a37c5f3164cc62b6fea82f01217 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 28 Jun 2023 15:28:27 +0200 Subject: [PATCH 433/506] prevent warning --- linux/ipcore/AxiPtmeConfig.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/linux/ipcore/AxiPtmeConfig.cpp b/linux/ipcore/AxiPtmeConfig.cpp index d3d5662a..7f044c79 100644 --- a/linux/ipcore/AxiPtmeConfig.cpp +++ b/linux/ipcore/AxiPtmeConfig.cpp @@ -26,8 +26,7 @@ ReturnValue_t AxiPtmeConfig::initialize() { } ReturnValue_t AxiPtmeConfig::writeCaduRateReg(uint8_t rateVal) { - ReturnValue_t result = returnvalue::OK; - result = mutex->lockMutex(timeoutType, mutexTimeout); + ReturnValue_t result = mutex->lockMutex(timeoutType, mutexTimeout); if (result != returnvalue::OK) { sif::warning << "AxiPtmeConfig::writeCaduRateReg: Failed to lock mutex" << std::endl; return returnvalue::FAILED; @@ -42,7 +41,6 @@ ReturnValue_t AxiPtmeConfig::writeCaduRateReg(uint8_t rateVal) { } uint8_t AxiPtmeConfig::readCaduRateReg() { - ReturnValue_t result = returnvalue::OK; MutexGuard mg(mutex); return static_cast(*(baseAddress + CADU_BITRATE_REG)); } From 87c05705ec4f10ec020e160d432de77365cb7149 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 28 Jun 2023 15:43:44 +0200 Subject: [PATCH 434/506] core ctrl bugfix --- bsp_q7s/core/CoreController.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 4387e788..cea6c700 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -170,9 +170,6 @@ ReturnValue_t CoreController::initialize() { sdStateMachine(); - triggerEvent(core::REBOOT_SW, CURRENT_CHIP, CURRENT_COPY); - announceCurrentImageInfo(); - announceVersionInfo(); EventManagerIF *eventManager = ObjectManager::instance()->get(objects::EVENT_MANAGER); if (eventManager == nullptr or eventQueue == nullptr) { @@ -189,17 +186,22 @@ ReturnValue_t CoreController::initialize() { if (result != returnvalue::OK) { sif::warning << "Subscribing for GPS GPS_FIX_CHANGE event failed" << std::endl; } - + triggerEvent(core::REBOOT_SW, CURRENT_CHIP, CURRENT_COPY); + announceCurrentImageInfo(); + // This has to come before the version announce because it might be required for retrieving + // the firmware version. if (common::OBSW_VERSION_MAJOR >= 6 or common::OBSW_VERSION_MAJOR == 4) { UioMapper sysRomMapper(q7s::UIO_SYS_ROM); result = sysRomMapper.getMappedAdress(&mappedSysRomAddr, UioMapper::Permissions::READ_ONLY); if (result != returnvalue::OK) { // TODO: This might be a reason to switch to another image.. sif::error << "Getting mapped SYS ROM UIO address failed" << std::endl; - return ObjectManager::CHILD_INIT_FAILED; + result = ObjectManager::CHILD_INIT_FAILED; } } - return returnvalue::OK; + announceVersionInfo(); + + return result; } ReturnValue_t CoreController::initializeAfterTaskCreation() { From 9dfdf388225f7cbc9d5e7faf4085b42aff570cf1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 28 Jun 2023 17:20:10 +0200 Subject: [PATCH 435/506] important bugfix for live channel --- linux/ipcore/PapbVcInterface.cpp | 1 - mission/com/LiveTmTask.cpp | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/linux/ipcore/PapbVcInterface.cpp b/linux/ipcore/PapbVcInterface.cpp index 404f3653..98cb1b41 100644 --- a/linux/ipcore/PapbVcInterface.cpp +++ b/linux/ipcore/PapbVcInterface.cpp @@ -34,7 +34,6 @@ ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size) { return DirectTmSinkIF::IS_BUSY; } for (size_t idx = 0; idx < size; idx++) { - // if (pollInterfaceReadiness(2, false) == returnvalue::OK) { *(vcBaseReg + DATA_REG_OFFSET) = static_cast(data[idx]); } completePacketTransfer(); diff --git a/mission/com/LiveTmTask.cpp b/mission/com/LiveTmTask.cpp index d09c6ced..b3358789 100644 --- a/mission/com/LiveTmTask.cpp +++ b/mission/com/LiveTmTask.cpp @@ -20,7 +20,7 @@ ReturnValue_t LiveTmTask::performOperation(uint8_t opCode) { readCommandQueue(); while (true) { bool performWriteOp = true; - if (mode == MODE_OFF or ptmeLocked) { + if (ptmeLocked) { performWriteOp = false; } From b57283101b9501c88ae1d87f5de0e37e7f5163f3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 28 Jun 2023 17:20:10 +0200 Subject: [PATCH 436/506] important bugfix for live channel --- mission/com/LiveTmTask.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/mission/com/LiveTmTask.cpp b/mission/com/LiveTmTask.cpp index d09c6ced..39648c15 100644 --- a/mission/com/LiveTmTask.cpp +++ b/mission/com/LiveTmTask.cpp @@ -19,13 +19,8 @@ LiveTmTask::LiveTmTask(object_id_t objectId, PusTmFunnel& pusFunnel, CfdpTmFunne ReturnValue_t LiveTmTask::performOperation(uint8_t opCode) { readCommandQueue(); while (true) { - bool performWriteOp = true; - if (mode == MODE_OFF or ptmeLocked) { - performWriteOp = false; - } - // The funnel tasks are scheduled here directly as well. - ReturnValue_t result = channel.handleNextTm(performWriteOp); + ReturnValue_t result = channel.handleNextTm(!ptmeLocked); if (result == DirectTmSinkIF::IS_BUSY) { sif::error << "Lost live TM, PAPB busy" << std::endl; } From 25da1316535cdc053ee96bc2036e5a082f50ccfe Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Jun 2023 00:33:58 +0200 Subject: [PATCH 437/506] seems to work well --- linux/ipcore/PapbVcInterface.cpp | 27 ++++++++++++++++++++++++++- linux/ipcore/PapbVcInterface.h | 3 +++ 2 files changed, 29 insertions(+), 1 deletion(-) diff --git a/linux/ipcore/PapbVcInterface.cpp b/linux/ipcore/PapbVcInterface.cpp index 98cb1b41..5dcb4519 100644 --- a/linux/ipcore/PapbVcInterface.cpp +++ b/linux/ipcore/PapbVcInterface.cpp @@ -33,9 +33,21 @@ ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size) { } else { return DirectTmSinkIF::IS_BUSY; } + if (not pollReadyForOctet(MAX_BUSY_POLLS)) { + abortPacketTransfer(); + return returnvalue::FAILED; + } for (size_t idx = 0; idx < size; idx++) { + if (not pollReadyForOctet(MAX_BUSY_POLLS)) { + abortPacketTransfer(); + return returnvalue::FAILED; + } *(vcBaseReg + DATA_REG_OFFSET) = static_cast(data[idx]); } + if (not pollReadyForOctet(MAX_BUSY_POLLS)) { + abortPacketTransfer(); + return returnvalue::FAILED; + } completePacketTransfer(); return returnvalue::OK; } @@ -50,7 +62,6 @@ bool PapbVcInterface::pollReadyForPacket() const { // Check if PAPB interface is ready to receive data. Use the configuration register for this. // Bit 5, see PTME ptme_001_01-0-7-r2 Table 31. uint32_t reg = *vcBaseReg; - // bool busy = (reg >> 5) & 0b1; return (reg >> 6) & 0b1; } @@ -76,6 +87,20 @@ bool PapbVcInterface::isBusy() const { return not pollReadyForPacket(); } void PapbVcInterface::cancelTransfer() { abortPacketTransfer(); } +inline bool PapbVcInterface::pollReadyForOctet(uint32_t maxCycles) const { + uint32_t reg; + uint32_t idx = 0; + while (idx < maxCycles) { + reg = *vcBaseReg; + // Busy bit. + if (not((reg >> 5) & 0b1)) { + return true; + } + idx++; + } + return false; +} + ReturnValue_t PapbVcInterface::sendTestFrame() { /** Size of one complete transfer frame data field amounts to 1105 bytes */ uint8_t testPacket[1105]; diff --git a/linux/ipcore/PapbVcInterface.h b/linux/ipcore/PapbVcInterface.h index ba6063b5..9c6734e8 100644 --- a/linux/ipcore/PapbVcInterface.h +++ b/linux/ipcore/PapbVcInterface.h @@ -80,6 +80,7 @@ class PapbVcInterface : public VirtualChannelIF { static constexpr long int FIRST_DELAY_PAPB_POLLING_NS = 10; static constexpr long int MAX_DELAY_PAPB_POLLING_NS = 40; + static constexpr uint32_t MAX_BUSY_POLLS = 100000; LinuxLibgpioIF* gpioComIF = nullptr; /** High when external buffer memory of virtual channel is empty */ @@ -118,6 +119,8 @@ class PapbVcInterface : public VirtualChannelIF { */ inline bool pollReadyForPacket() const; + inline bool pollReadyForOctet(uint32_t maxCycles) const; + /** * @brief This function can be used for debugging to check whether there are packets in * the packet buffer of the virtual channel or not. From 03e772246f1a5211f815ac8691da86d36b8b32e4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Jun 2023 00:35:06 +0200 Subject: [PATCH 438/506] less cycles.. --- linux/ipcore/PapbVcInterface.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/ipcore/PapbVcInterface.h b/linux/ipcore/PapbVcInterface.h index 9c6734e8..b5160748 100644 --- a/linux/ipcore/PapbVcInterface.h +++ b/linux/ipcore/PapbVcInterface.h @@ -80,7 +80,7 @@ class PapbVcInterface : public VirtualChannelIF { static constexpr long int FIRST_DELAY_PAPB_POLLING_NS = 10; static constexpr long int MAX_DELAY_PAPB_POLLING_NS = 40; - static constexpr uint32_t MAX_BUSY_POLLS = 100000; + static constexpr uint32_t MAX_BUSY_POLLS = 1000; LinuxLibgpioIF* gpioComIF = nullptr; /** High when external buffer memory of virtual channel is empty */ From ad3c0e2a0e4d061302d9cd1e12c4f46d23f58d70 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Jun 2023 01:28:40 +0200 Subject: [PATCH 439/506] small fix for persistent TM store --- CHANGELOG.md | 8 +++++++- mission/com/TmStoreTaskBase.cpp | 30 ++++++++++++++++++------------ mission/com/TmStoreTaskBase.h | 2 ++ mission/tmtc/PersistentTmStore.cpp | 10 +++++++--- 4 files changed, 34 insertions(+), 16 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4fda0e54..95718644 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,7 +20,13 @@ will consitute of a breaking change warranting a new major release: - Important bugfixes for PTME. See `q7s-package` CHANGELOG. -# [v5.1.0] to be released +# [v5.2.0] 2023-06-29 + +## Fixed + +- Empty dumps (no TM in time range) will now correctly be completed immediately + +# [v5.1.0] 2023-06-28 - `eive-tmtc` version v5.1.0 diff --git a/mission/com/TmStoreTaskBase.cpp b/mission/com/TmStoreTaskBase.cpp index 0470dc04..80900975 100644 --- a/mission/com/TmStoreTaskBase.cpp +++ b/mission/com/TmStoreTaskBase.cpp @@ -45,13 +45,19 @@ bool TmStoreTaskBase::handleOneStore(PersistentTmStoreWithTmQueue& store, } else { Command_t execCmd; // Handle TC requests, for example deletion or retrieval requests. + // TODO: Not really clean here.. would be better if the executed command is returns as an + // enumeration. result = store.handleCommandQueue(ipcStore, execCmd); - if (result == returnvalue::OK) { - if (execCmd == TmStoreMessage::DOWNLINK_STORE_CONTENT_TIME) { + if (execCmd == TmStoreMessage::DOWNLINK_STORE_CONTENT_TIME) { + if (result == PersistentTmStore::DUMP_DONE) { + dumpDoneHandler(store, dumpContext); + } else if (result == returnvalue::OK) { cancelDumpCd.resetTimer(); tmSinkBusyCd.resetTimer(); dumpContext.reset(); } + } + if (execCmd != CommandMessageIF::CMD_NONE) { tcRequestReceived = true; } } @@ -119,21 +125,13 @@ ReturnValue_t TmStoreTaskBase::performDump(PersistentTmStoreWithTmQueue& store, DumpContext& dumpContext, bool& dumpPerformed) { size_t dumpedLen = 0; - auto dumpDoneHandler = [&]() { - uint32_t startTime; - uint32_t endTime; - store.getStartAndEndTimeCurrentOrLastDump(startTime, endTime); - triggerEvent(dumpContext.eventIfDone, dumpContext.numberOfDumpedPackets, - dumpContext.dumpedBytes); - dumpContext.reset(); - }; // Dump the next packet into the PTME. dumpContext.ptmeBusyCounter = 0; tmSinkBusyCd.resetTimer(); ReturnValue_t result = store.getNextDumpPacket(tmReader, fileHasSwapped); if (fileHasSwapped and result == PersistentTmStore::DUMP_DONE) { // This can happen if a file is corrupted and the next file swap completes the dump. - dumpDoneHandler(); + dumpDoneHandler(store, dumpContext); return returnvalue::OK; } else if (result != returnvalue::OK) { sif::error << "PersistentTmStore: Getting next dump packet failed" << std::endl; @@ -157,7 +155,7 @@ ReturnValue_t TmStoreTaskBase::performDump(PersistentTmStoreWithTmQueue& store, } } if (result == PersistentTmStore::DUMP_DONE) { - dumpDoneHandler(); + dumpDoneHandler(store, dumpContext); } return returnvalue::OK; } @@ -198,6 +196,14 @@ ReturnValue_t TmStoreTaskBase::connectModeTreeParent(HasModeTreeChildrenIF& pare return modetree::connectModeTreeParent(parent, *this, nullptr, modeHelper); } +void TmStoreTaskBase::dumpDoneHandler(PersistentTmStore& store, DumpContext& dumpContext) { + uint32_t startTime; + uint32_t endTime; + store.getStartAndEndTimeCurrentOrLastDump(startTime, endTime); + triggerEvent(dumpContext.eventIfDone, dumpContext.numberOfDumpedPackets, dumpContext.dumpedBytes); + dumpContext.reset(); +} + ModeTreeChildIF& TmStoreTaskBase::getModeTreeChildIF() { return *this; } void TmStoreTaskBase::readCommandQueue(void) { diff --git a/mission/com/TmStoreTaskBase.h b/mission/com/TmStoreTaskBase.h index ef61bd19..2bcd3b1e 100644 --- a/mission/com/TmStoreTaskBase.h +++ b/mission/com/TmStoreTaskBase.h @@ -96,6 +96,8 @@ class TmStoreTaskBase : public SystemObject, ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t* msToReachTheMode) override; + void dumpDoneHandler(PersistentTmStore& store, DumpContext& dumpContext); + void announceMode(bool recursive) override; object_id_t getObjectId() const override; const HasHealthIF* getOptHealthIF() const override; diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index acef0b3a..2f884dac 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -128,6 +128,7 @@ ReturnValue_t PersistentTmStore::assignAndOrCreateMostRecentFile() { ReturnValue_t PersistentTmStore::handleCommandQueue(StorageManagerIF& ipcStore, Command_t& execCmd) { + execCmd = CommandMessageIF::CMD_NONE; CommandMessage cmdMessage; ReturnValue_t result = tcQueue->receiveMessage(&cmdMessage); if (result != returnvalue::OK) { @@ -162,9 +163,9 @@ ReturnValue_t PersistentTmStore::handleCommandQueue(StorageManagerIF& ipcStore, result = startDumpFromUpTo(dumpFromUnixSeconds, dumpUntilUnixSeconds); if (result == BUSY_DUMPING) { triggerEvent(persTmStore::BUSY_DUMPING_EVENT); - } else { - execCmd = cmd; + return result; } + execCmd = cmd; } } return result; @@ -359,7 +360,10 @@ ReturnValue_t PersistentTmStore::loadNextDumpFile() { } ReturnValue_t PersistentTmStore::getNextDumpPacket(PusTmReader& reader, bool& fileHasSwapped) { - if (state == State::IDLE or dumpParams.pendingPacketDump) { + if (state == State::IDLE) { + return DUMP_DONE; + } + if (dumpParams.pendingPacketDump) { return returnvalue::FAILED; } fileHasSwapped = false; From 0a12dbf2bed3fd0557b556ed527398b4324f2692 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Jun 2023 08:53:08 +0200 Subject: [PATCH 440/506] changelog --- CHANGELOG.md | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4fda0e54..5d82a812 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,7 +20,14 @@ will consitute of a breaking change warranting a new major release: - Important bugfixes for PTME. See `q7s-package` CHANGELOG. -# [v5.1.0] to be released +# [v5.2.0] to be released + +## Fixed + +- The firmware information event was not triggered even when possible because of an ordering + bug in the initializer function. + +# [v5.1.0] 2023-06-28 - `eive-tmtc` version v5.1.0 From 2173d890475d3e685ebc2d5e0454268c6228b1e8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Jun 2023 08:54:34 +0200 Subject: [PATCH 441/506] changelog --- CHANGELOG.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4fda0e54..95a4d7d0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,7 +20,12 @@ will consitute of a breaking change warranting a new major release: - Important bugfixes for PTME. See `q7s-package` CHANGELOG. -# [v5.1.0] to be released +## Fixed + +- For the live channel (VC0), telemetry was still only dumped if the transmitter is active. + Please note that this fix will lead to crashes for FW version v4. + +# [v5.1.0] 2023-06-28 - `eive-tmtc` version v5.1.0 From 4f5903227e92ce01b0654e7291966b9848a835c6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Jun 2023 08:54:58 +0200 Subject: [PATCH 442/506] changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 95a4d7d0..9e74bdd0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -23,7 +23,7 @@ will consitute of a breaking change warranting a new major release: ## Fixed - For the live channel (VC0), telemetry was still only dumped if the transmitter is active. - Please note that this fix will lead to crashes for FW version v4. + Please note that this fix will lead to crashes for FW versions below v3.2. # [v5.1.0] 2023-06-28 From 33985937b7e9adf4918268bac36f2c910bc9ec99 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Jun 2023 09:44:09 +0200 Subject: [PATCH 443/506] typo --- mission/com/CcsdsIpCoreHandler.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/com/CcsdsIpCoreHandler.cpp b/mission/com/CcsdsIpCoreHandler.cpp index 2e068add..19dd4f5a 100644 --- a/mission/com/CcsdsIpCoreHandler.cpp +++ b/mission/com/CcsdsIpCoreHandler.cpp @@ -246,13 +246,13 @@ ReturnValue_t CcsdsIpCoreHandler::checkModeCommand(Mode_t mode, Submode_t submod void CcsdsIpCoreHandler::startTransition(Mode_t mode, Submode_t submode) { triggerEvent(CHANGING_MODE, mode, submode); if (mode == HasModesIF::MODE_ON) { - uint32_t currenRate = ptmeConfig.getRate(); + uint32_t currentRate = ptmeConfig.getRate(); // Check whether the rate actually changes. if ((this->submode != submode) and (((submode == static_cast(com::CcsdsSubmode::DATARATE_LOW) and - (currenRate != RATE_100KBPS))) or + (currentRate != RATE_100KBPS))) or ((submode == static_cast(com::CcsdsSubmode::DATARATE_HIGH) and - (currenRate != RATE_500KBPS))))) { + (currentRate != RATE_500KBPS))))) { initPtmeUpdateAfterXCycles(); updateContext.enableTransmitAfterPtmeUpdate = true; updateContext.updateClockRate = true; From 825de04f3be535880e61e1fcdac9d3acef725f95 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Jun 2023 10:15:36 +0200 Subject: [PATCH 444/506] changelog --- CHANGELOG.md | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4fda0e54..4f8b027b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,7 +20,14 @@ will consitute of a breaking change warranting a new major release: - Important bugfixes for PTME. See `q7s-package` CHANGELOG. -# [v5.1.0] to be released +# [v5.2.0] to be released + +## Changed + +- PTME was always reset on submode changes. The reset will now only be performed if the actual data + rate changes. + +# [v5.1.0] 2023-06-28 - `eive-tmtc` version v5.1.0 From 51cbe46cf52d601cb22e0f4e45c749f472608823 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Jun 2023 14:49:57 +0200 Subject: [PATCH 445/506] why indeed --- linux/ipcore/AxiPtmeConfig.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/linux/ipcore/AxiPtmeConfig.cpp b/linux/ipcore/AxiPtmeConfig.cpp index d3d5662a..b21edf5b 100644 --- a/linux/ipcore/AxiPtmeConfig.cpp +++ b/linux/ipcore/AxiPtmeConfig.cpp @@ -16,9 +16,9 @@ AxiPtmeConfig::AxiPtmeConfig(object_id_t objectId, std::string axiUio, int mapNu AxiPtmeConfig::~AxiPtmeConfig() {} ReturnValue_t AxiPtmeConfig::initialize() { - ReturnValue_t result = returnvalue::OK; UioMapper uioMapper(axiUio, mapNum); - result = uioMapper.getMappedAdress(&baseAddress, UioMapper::Permissions::READ_WRITE); + ReturnValue_t result = + uioMapper.getMappedAdress(&baseAddress, UioMapper::Permissions::READ_WRITE); if (result != returnvalue::OK) { return result; } @@ -26,8 +26,7 @@ ReturnValue_t AxiPtmeConfig::initialize() { } ReturnValue_t AxiPtmeConfig::writeCaduRateReg(uint8_t rateVal) { - ReturnValue_t result = returnvalue::OK; - result = mutex->lockMutex(timeoutType, mutexTimeout); + ReturnValue_t result = mutex->lockMutex(timeoutType, mutexTimeout); if (result != returnvalue::OK) { sif::warning << "AxiPtmeConfig::writeCaduRateReg: Failed to lock mutex" << std::endl; return returnvalue::FAILED; @@ -42,7 +41,6 @@ ReturnValue_t AxiPtmeConfig::writeCaduRateReg(uint8_t rateVal) { } uint8_t AxiPtmeConfig::readCaduRateReg() { - ReturnValue_t result = returnvalue::OK; MutexGuard mg(mutex); return static_cast(*(baseAddress + CADU_BITRATE_REG)); } From 04d24e74b1b1e00277f71e2c788f00ac657ee4b5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Jun 2023 15:01:54 +0200 Subject: [PATCH 446/506] q7s-package version specification --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 77281df0..2b3ef9f1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,7 @@ will consitute of a breaking change warranting a new major release: # [v6.0.0] to be released +- `q7s-package` version v3.2.0 - Important bugfixes for PTME. See `q7s-package` CHANGELOG. ## Fixed From 4fc999e10226cf93653ab9e6dd2bb47e801707c1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Jun 2023 15:03:45 +0200 Subject: [PATCH 447/506] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index b5881296..116649da 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,11 @@ will consitute of a breaking change warranting a new major release: - Important bugfixes for PTME. See `q7s-package` CHANGELOG. +## Changed + +- Added back PTME busy bit polling. This is necessary due to changes to the AXI APB interface + to the PTME core. + # [v5.2.0] 2023-06-29 ## Fixed From 56ea9af615e97dfc22c1113f52f94d701dcbef0c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Jun 2023 15:06:25 +0200 Subject: [PATCH 448/506] add documentation of initial delay --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index 6b06283c..ce57c33f 100644 --- a/README.md +++ b/README.md @@ -964,6 +964,12 @@ used by other software components to read the current chip and copy. This is a configuration scripts which runs after the Network Time Protocol has run. This script currently sets the static IP address `192.168.133.10` and starts the `can` interface. +## Initial boot delay + +You can create a file named `boot_delays_secs.txt` inside the home folder to delay the OBSW boot +for 6 seconds if the file is empty of for the number of seconds specified inside the file. This +can be helpful if something inside the OBSW leads to an immediate crash of the OBC. + ## PCDU Connect to serial console of P60 Dock From e19ddcfbd87e6e7b1535d6f068af283c7ced983e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 2 Jul 2023 14:02:37 +0200 Subject: [PATCH 449/506] EM ACS board --- CMakeLists.txt | 4 ++-- bsp_q7s/em/emObjectFactory.cpp | 5 +++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c63d32ad..b07423ac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 5) -set(OBSW_VERSION_MINOR 1) +set(OBSW_VERSION_MINOR 2) set(OBSW_VERSION_REVISION 0) # set(CMAKE_VERBOSE TRUE) @@ -105,7 +105,7 @@ set(OBSW_ADD_THERMAL_TEMP_INSERTER ${OBSW_Q7S_EM} CACHE STRING "Add thermal sensor temperature inserter") set(OBSW_ADD_ACS_BOARD - ${INIT_VAL} + 1 CACHE STRING "Add ACS board module") set(OBSW_ADD_GPS_CTRL ${INIT_VAL} diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 2e171e02..1b6fd290 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -84,9 +84,10 @@ void ObjectFactory::produce(void* args) { new CoreController(objects::CORE_CONTROLLER, enableHkSets); -#if OBSW_ADD_ACS_BOARD == 1 - // Still initialize chip select to avoid SPI bus issues. + // Initialize chip select to avoid SPI bus issues. createRadSensorChipSelect(gpioComIF); + +#if OBSW_ADD_ACS_BOARD == 1 createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher, true, adis1650x::Type::ADIS16507); #else From a5c9e1481b4a54e2e1753176281e23e333801409 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 2 Jul 2023 14:04:29 +0200 Subject: [PATCH 450/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index b5881296..55a1b6d5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -32,6 +32,8 @@ will consitute of a breaking change warranting a new major release: - PTME was always reset on submode changes. The reset will now only be performed if the actual data rate changes. +- Add back ACS board code for the EM. Now that the radiation sensor was removed, the image swichting + issue has disappeared and adding back the ACS board is worth it for the GPS timekeeping. # [v5.1.0] 2023-06-28 From 0e9035d6c1095439d50cf6d808449d3b7870851e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 2 Jul 2023 14:21:35 +0200 Subject: [PATCH 451/506] changelog, prep v5.2.0 --- CHANGELOG.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 55a1b6d5..e1d14b27 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,7 +20,7 @@ will consitute of a breaking change warranting a new major release: - Important bugfixes for PTME. See `q7s-package` CHANGELOG. -# [v5.2.0] 2023-06-29 +# [v5.2.0] 2023-07-02 ## Fixed @@ -32,7 +32,7 @@ will consitute of a breaking change warranting a new major release: - PTME was always reset on submode changes. The reset will now only be performed if the actual data rate changes. -- Add back ACS board code for the EM. Now that the radiation sensor was removed, the image swichting +- Add back ACS board code for the EM. Now that the radiation sensor was removed, the image switching issue has disappeared and adding back the ACS board is worth it for the GPS timekeeping. # [v5.1.0] 2023-06-28 From dea3077f29c85dbfcffea834595146cc16a79bbe Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 2 Jul 2023 14:44:15 +0200 Subject: [PATCH 452/506] changelog, prep v6.0.0 --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index b29d00b0..38d76dac 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,7 +16,7 @@ will consitute of a breaking change warranting a new major release: # [unreleased] -# [v6.0.0] to be released +# [v6.0.0] 2023-07-02 - `q7s-package` version v3.2.0 - Important bugfixes for PTME. See `q7s-package` CHANGELOG. From 9ece676313a1c21b6c71b1c196f9100a35889e2e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 3 Jul 2023 16:55:45 +0200 Subject: [PATCH 453/506] lets see if this works --- bsp_q7s/CMakeLists.txt | 3 +- bsp_q7s/core/CMakeLists.txt | 5 +-- bsp_q7s/core/CoreController.cpp | 23 ++--------- bsp_q7s/core/CoreController.h | 2 +- bsp_q7s/core/{CoreDefinitions.h => defs.h} | 12 ++++-- bsp_q7s/em/emObjectFactory.cpp | 3 +- bsp_q7s/fmObjectFactory.cpp | 3 +- .../ObjectFactory.cpp => objectFactory.cpp} | 41 ++++++++++++++++++- .../{core/ObjectFactory.h => objectFactory.h} | 1 + bsp_q7s/obsw.cpp | 2 +- bsp_q7s/{core => }/scheduling.cpp | 2 +- bsp_q7s/{core => }/scheduling.h | 0 dummies/CoreControllerDummy.cpp | 2 +- mission/controller/ThermalController.cpp | 2 +- mission/controller/ThermalController.h | 2 +- 15 files changed, 67 insertions(+), 36 deletions(-) rename bsp_q7s/core/{CoreDefinitions.h => defs.h} (81%) rename bsp_q7s/{core/ObjectFactory.cpp => objectFactory.cpp} (96%) rename bsp_q7s/{core/ObjectFactory.h => objectFactory.h} (98%) rename bsp_q7s/{core => }/scheduling.cpp (99%) rename bsp_q7s/{core => }/scheduling.h (100%) diff --git a/bsp_q7s/CMakeLists.txt b/bsp_q7s/CMakeLists.txt index cf8fcacd..e3232363 100644 --- a/bsp_q7s/CMakeLists.txt +++ b/bsp_q7s/CMakeLists.txt @@ -7,7 +7,8 @@ target_link_libraries(${SIMPLE_OBSW_NAME} PUBLIC ${LIB_FSFW_NAME}) target_compile_definitions(${SIMPLE_OBSW_NAME} PRIVATE "Q7S_SIMPLE_MODE") add_subdirectory(simple) -target_sources(${OBSW_NAME} PUBLIC main.cpp obsw.cpp) +target_sources(${OBSW_NAME} PUBLIC main.cpp obsw.cpp scheduling.cpp + objectFactory.cpp) add_subdirectory(boardtest) diff --git a/bsp_q7s/core/CMakeLists.txt b/bsp_q7s/core/CMakeLists.txt index b726885b..33550144 100644 --- a/bsp_q7s/core/CMakeLists.txt +++ b/bsp_q7s/core/CMakeLists.txt @@ -1,4 +1 @@ -target_sources(${OBSW_NAME} PRIVATE CoreController.cpp scheduling.cpp - ObjectFactory.cpp WatchdogHandler.cpp) - -target_sources(${SIMPLE_OBSW_NAME} PRIVATE scheduling.cpp) +target_sources(${OBSW_NAME} PRIVATE CoreController.cpp WatchdogHandler.cpp) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index cea6c700..c2e73c52 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -188,17 +188,6 @@ ReturnValue_t CoreController::initialize() { } triggerEvent(core::REBOOT_SW, CURRENT_CHIP, CURRENT_COPY); announceCurrentImageInfo(); - // This has to come before the version announce because it might be required for retrieving - // the firmware version. - if (common::OBSW_VERSION_MAJOR >= 6 or common::OBSW_VERSION_MAJOR == 4) { - UioMapper sysRomMapper(q7s::UIO_SYS_ROM); - result = sysRomMapper.getMappedAdress(&mappedSysRomAddr, UioMapper::Permissions::READ_ONLY); - if (result != returnvalue::OK) { - // TODO: This might be a reason to switch to another image.. - sif::error << "Getting mapped SYS ROM UIO address failed" << std::endl; - result = ObjectManager::CHILD_INIT_FAILED; - } - } announceVersionInfo(); return result; @@ -2526,14 +2515,10 @@ void CoreController::announceVersionInfo() { } triggerEvent(VERSION_INFO, p1, p2); - - if (common::OBSW_VERSION_MAJOR >= 6 or common::OBSW_VERSION_MAJOR == 4) { - if (mappedSysRomAddr != nullptr) { - uint32_t p1Firmware = *(reinterpret_cast(mappedSysRomAddr)); - uint32_t p2Firmware = *(reinterpret_cast(mappedSysRomAddr) + 1); - triggerEvent(FIRMWARE_INFO, p1Firmware, p2Firmware); - } - } + p1 = (core::FW_VERSION_MAJOR << 24) | (core::FW_VERSION_MINOR << 16) | + (core::FW_VERSION_REVISION << 8) | (core::FW_VERSION_HAS_SHA); + std::memcpy(&p2, core::FW_VERSION_GIT_SHA, 4); + triggerEvent(FIRMWARE_INFO, p1, p2); } void CoreController::announceCurrentImageInfo() { diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index b68ae158..b6bc9bfb 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -1,6 +1,7 @@ #ifndef BSP_Q7S_CORE_CORECONTROLLER_H_ #define BSP_Q7S_CORE_CORECONTROLLER_H_ +#include #include #include #include @@ -14,7 +15,6 @@ #include #include -#include "CoreDefinitions.h" #include "OBSWConfig.h" #include "bsp_q7s/fs/SdCardManager.h" #include "events/subsystemIdRanges.h" diff --git a/bsp_q7s/core/CoreDefinitions.h b/bsp_q7s/core/defs.h similarity index 81% rename from bsp_q7s/core/CoreDefinitions.h rename to bsp_q7s/core/defs.h index 91896301..a82c1c56 100644 --- a/bsp_q7s/core/CoreDefinitions.h +++ b/bsp_q7s/core/defs.h @@ -1,10 +1,16 @@ -#ifndef BSP_Q7S_CORE_COREDEFINITIONS_H_ -#define BSP_Q7S_CORE_COREDEFINITIONS_H_ +#ifndef BSP_Q7S_CORE_DEFS_H_ +#define BSP_Q7S_CORE_DEFS_H_ #include namespace core { +extern uint8_t FW_VERSION_MAJOR; +extern uint8_t FW_VERSION_MINOR; +extern uint8_t FW_VERSION_REVISION; +extern bool FW_VERSION_HAS_SHA; +extern char FW_VERSION_GIT_SHA[4]; + static const uint8_t HK_SET_ENTRIES = 3; static const uint32_t HK_SET_ID = 5; @@ -36,4 +42,4 @@ class HkSet : public StaticLocalDataSet { } // namespace core -#endif /* BSP_Q7S_CORE_COREDEFINITIONS_H_ */ +#endif /* BSP_Q7S_CORE_DEFS_H_ */ diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index a6e10e57..68855d01 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -10,7 +11,6 @@ #include "OBSWConfig.h" #include "bsp_q7s/core/CoreController.h" -#include "bsp_q7s/core/ObjectFactory.h" #include "busConf.h" #include "common/config/devices/addresses.h" #include "devConf.h" @@ -36,6 +36,7 @@ void ObjectFactory::produce(void* args) { #endif PersistentTmStores stores; + readFirmwareVersion(); ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel, *SdCardManager::instance(), &ipcStore, &tmStore, stores, 200, enableHkSets); diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index f4bb461a..e97930d2 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -7,7 +8,6 @@ #include "OBSWConfig.h" #include "bsp_q7s/core/CoreController.h" -#include "bsp_q7s/core/ObjectFactory.h" #include "busConf.h" #include "devConf.h" #include "eive/objects.h" @@ -32,6 +32,7 @@ void ObjectFactory::produce(void* args) { #endif PersistentTmStores stores; + readFirmwareVersion(); ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel, *SdCardManager::instance(), &ipcStore, &tmStore, stores, 200, true); diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/objectFactory.cpp similarity index 96% rename from bsp_q7s/core/ObjectFactory.cpp rename to bsp_q7s/objectFactory.cpp index ca09be97..ed07b131 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/objectFactory.cpp @@ -1,8 +1,9 @@ -#include "ObjectFactory.h" +#include "objectFactory.h" #include #include #include +#include #include #include #include @@ -32,6 +33,9 @@ #include #include +#include +#include + #include "OBSWConfig.h" #include "bsp_q7s/boardtest/Q7STestTask.h" #include "bsp_q7s/callbacks/gnssCallback.h" @@ -100,6 +104,7 @@ #include +#include "bsp_q7s/core/defs.h" #include "fsfw/datapoollocal/LocalDataPoolManager.h" #include "fsfw/tmtcpacket/pus/tm.h" #include "fsfw/tmtcservices/CommandingServiceBase.h" @@ -130,6 +135,11 @@ ResetArgs RESET_ARGS_GNSS; std::atomic_bool LINK_STATE = CcsdsIpCoreHandler::LINK_DOWN; std::atomic_bool PTME_LOCKED = false; std::atomic_uint16_t I2C_FATAL_ERRORS = 0; +uint8_t core::FW_VERSION_MAJOR = 0; +uint8_t core::FW_VERSION_MINOR = 0; +uint8_t core::FW_VERSION_REVISION = 0; +bool core::FW_VERSION_HAS_SHA = false; +char core::FW_VERSION_GIT_SHA[4] = {}; void Factory::setStaticFrameworkObjectIds() { PusServiceBase::PUS_DISTRIBUTOR = objects::PUS_PACKET_DISTRIBUTOR; @@ -1025,3 +1035,32 @@ void ObjectFactory::createPlI2cResetGpio(LinuxLibgpioIF* gpioIF) { gpioIF->pullHigh(gpioIds::PL_I2C_ARESETN); } } + +ReturnValue_t ObjectFactory::readFirmwareVersion() { + uint32_t* mappedSysRomAddr = nullptr; + // The SYS ROM FPGA block is only available in those versions. + if (not(common::OBSW_VERSION_MAJOR >= 6) or (common::OBSW_VERSION_MAJOR == 4)) { + return returnvalue::OK; + } + // This has to come before the version announce because it might be required for retrieving + // the firmware version. + UioMapper sysRomMapper(q7s::UIO_SYS_ROM); + ReturnValue_t result = + sysRomMapper.getMappedAdress(&mappedSysRomAddr, UioMapper::Permissions::READ_ONLY); + if (result != returnvalue::OK) { + sif::error << "Getting mapped SYS ROM UIO address failed" << std::endl; + return returnvalue::FAILED; + } + if (mappedSysRomAddr != nullptr) { + uint32_t firstEntry = *(reinterpret_cast(mappedSysRomAddr)); + uint32_t secondEntry = *(reinterpret_cast(mappedSysRomAddr) + 1); + core::FW_VERSION_MAJOR = (firstEntry >> 24) & 0xff; + core::FW_VERSION_MINOR = (firstEntry >> 16) & 0xff; + core::FW_VERSION_REVISION = (firstEntry >> 8) & 0xff; + bool hasGitSha = (firstEntry & 0x0b1); + if (hasGitSha) { + std::memcpy(core::FW_VERSION_GIT_SHA, &secondEntry, 4); + } + } + return returnvalue::OK; +} diff --git a/bsp_q7s/core/ObjectFactory.h b/bsp_q7s/objectFactory.h similarity index 98% rename from bsp_q7s/core/ObjectFactory.h rename to bsp_q7s/objectFactory.h index 25d06090..e4694bf4 100644 --- a/bsp_q7s/core/ObjectFactory.h +++ b/bsp_q7s/objectFactory.h @@ -75,6 +75,7 @@ void createSyrlinksComponents(PowerSwitchIF* pwrSwitcher); void createPayloadComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF& pwrSwitcher); void createReactionWheelComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF* pwrSwitcher); ReturnValue_t createCcsdsComponents(CcsdsComponentArgs& args); +ReturnValue_t readFirmwareVersion(); void createMiscComponents(); void createTestComponents(LinuxLibgpioIF* gpioComIF); diff --git a/bsp_q7s/obsw.cpp b/bsp_q7s/obsw.cpp index dc547b03..fc49c369 100644 --- a/bsp_q7s/obsw.cpp +++ b/bsp_q7s/obsw.cpp @@ -11,13 +11,13 @@ #include "OBSWConfig.h" #include "bsp_q7s/core/WatchdogHandler.h" #include "commonConfig.h" -#include "core/scheduling.h" #include "fsfw/tasks/TaskFactory.h" #include "fsfw/version.h" #include "mission/acs/defs.h" #include "mission/com/defs.h" #include "mission/system/systemTree.h" #include "q7sConfig.h" +#include "scheduling.h" #include "watchdog/definitions.h" static constexpr int OBSW_ALREADY_RUNNING = -2; diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/scheduling.cpp similarity index 99% rename from bsp_q7s/core/scheduling.cpp rename to bsp_q7s/scheduling.cpp index a7c41bb6..b41eade2 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/scheduling.cpp @@ -9,7 +9,6 @@ #include #include "OBSWConfig.h" -#include "bsp_q7s/core/ObjectFactory.h" #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/objectmanager/ObjectManagerIF.h" #include "fsfw/platform.h" @@ -21,6 +20,7 @@ #include "mission/pollingSeqTables.h" #include "mission/scheduling.h" #include "mission/utility/InitMission.h" +#include "objectFactory.h" #include "q7sConfig.h" /* This is configured for linux without CR */ diff --git a/bsp_q7s/core/scheduling.h b/bsp_q7s/scheduling.h similarity index 100% rename from bsp_q7s/core/scheduling.h rename to bsp_q7s/scheduling.h diff --git a/dummies/CoreControllerDummy.cpp b/dummies/CoreControllerDummy.cpp index 8a027dbf..df2bef03 100644 --- a/dummies/CoreControllerDummy.cpp +++ b/dummies/CoreControllerDummy.cpp @@ -1,6 +1,6 @@ #include "CoreControllerDummy.h" -#include +#include #include #include diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 05011b1f..fca71acc 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1,6 +1,6 @@ #include "ThermalController.h" -#include +#include #include #include #include diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index bcbfc004..2518a13e 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -1,7 +1,7 @@ #ifndef MISSION_CONTROLLER_THERMALCONTROLLER_H_ #define MISSION_CONTROLLER_THERMALCONTROLLER_H_ -#include +#include #include #include #include From 25c7058da5eab75c73499c9ecfb0b81a9a03c6d8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 3 Jul 2023 16:56:36 +0200 Subject: [PATCH 454/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8397f4d9..a13d3d1d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,8 @@ will consitute of a breaking change warranting a new major release: - Swapped PL and PS I2C, BPX battery and MGT are connected to PS I2C now for firmware versions above v4. However, this software version ins compatible to both v3 and v4 of the firmware. +- The firmware version variables are global statics inititalized early during the program + runtime now. This makes it possible to check the firmware version earlier. # [v6.0.0] 2023-07-02 From d1b9608a1465e971f12963bd5a08a43345881452 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 3 Jul 2023 17:13:11 +0200 Subject: [PATCH 455/506] that should do the job --- bsp_q7s/core/CoreController.cpp | 2 +- bsp_q7s/em/emObjectFactory.cpp | 14 +++++++++++--- bsp_q7s/fmObjectFactory.cpp | 14 +++++++++++--- bsp_q7s/objectFactory.cpp | 15 ++++++++------- bsp_q7s/objectFactory.h | 7 ++++--- 5 files changed, 35 insertions(+), 17 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index c2e73c52..00c7f248 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -2516,7 +2516,7 @@ void CoreController::announceVersionInfo() { triggerEvent(VERSION_INFO, p1, p2); p1 = (core::FW_VERSION_MAJOR << 24) | (core::FW_VERSION_MINOR << 16) | - (core::FW_VERSION_REVISION << 8) | (core::FW_VERSION_HAS_SHA); + (core::FW_VERSION_REVISION << 8) | (core::FW_VERSION_HAS_SHA); std::memcpy(&p2, core::FW_VERSION_GIT_SHA, 4); triggerEvent(FIRMWARE_INFO, p1, p2); } diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 68855d01..28fabe7a 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -68,7 +68,11 @@ void ObjectFactory::produce(void* args) { {objects::TMP1075_HANDLER_PLPCDU_1, addresses::TMP1075_PLPCDU_1}, {objects::TMP1075_HANDLER_IF_BOARD, addresses::TMP1075_IF_BOARD}, }}; - createTmpComponents(tmpDevsToAdd); + const char* tmpI2cDev = q7s::I2C_PS_EIVE; + if (core::FW_VERSION_MAJOR >= 4) { + tmpI2cDev = q7s::I2C_PL_EIVE; + } + createTmpComponents(tmpDevsToAdd, tmpI2cDev); dummy::Tmp1075Cfg tmpCfg{}; tmpCfg.addTcsBrd0 = true; tmpCfg.addTcsBrd1 = true; @@ -114,8 +118,12 @@ void ObjectFactory::produce(void* args) { gpioChecker(gpioComIF->addGpios(acsBoardGpios), "ACS Board"); #endif + const char* battAndImtqI2cDev = q7s::I2C_PL_EIVE; + if (core::FW_VERSION_MAJOR >= 4) { + battAndImtqI2cDev = q7s::I2C_PS_EIVE; + } #if OBSW_ADD_MGT == 1 - createImtqComponents(pwrSwitcher, enableHkSets); + createImtqComponents(pwrSwitcher, enableHkSets, battAndImtqI2cDev); #endif #if OBSW_ADD_SYRLINKS == 1 @@ -127,7 +135,7 @@ void ObjectFactory::produce(void* args) { #endif #if OBSW_ADD_BPX_BATTERY_HANDLER == 1 - createBpxBatteryComponent(enableHkSets); + createBpxBatteryComponent(enableHkSets, battAndImtqI2cDev); #endif #if OBSW_ADD_STAR_TRACKER == 1 diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index e97930d2..74b3e725 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -76,7 +76,11 @@ void ObjectFactory::produce(void* args) { // {objects::TMP1075_HANDLER_PLPCDU_1, addresses::TMP1075_PLPCDU_1}, {objects::TMP1075_HANDLER_IF_BOARD, addresses::TMP1075_IF_BOARD}, }}; - createTmpComponents(tmpDevsToAdd); + const char* tmpI2cDev = q7s::I2C_PS_EIVE; + if (core::FW_VERSION_MAJOR >= 4) { + tmpI2cDev = q7s::I2C_PL_EIVE; + } + createTmpComponents(tmpDevsToAdd, tmpI2cDev); #endif createSolarArrayDeploymentComponents(*pwrSwitcher, *gpioComIF); createPlPcduComponents(gpioComIF, spiMainComIF, pwrSwitcher, *stackHandler); @@ -86,13 +90,17 @@ void ObjectFactory::produce(void* args) { createRtdComponents(q7s::SPI_DEFAULT_DEV, gpioComIF, pwrSwitcher, spiMainComIF); createPayloadComponents(gpioComIF, *pwrSwitcher); + const char* battAndImtqI2cDev = q7s::I2C_PL_EIVE; + if (core::FW_VERSION_MAJOR >= 4) { + battAndImtqI2cDev = q7s::I2C_PS_EIVE; + } #if OBSW_ADD_MGT == 1 - createImtqComponents(pwrSwitcher, enableHkSets); + createImtqComponents(pwrSwitcher, enableHkSets, battAndImtqI2cDev); #endif createReactionWheelComponents(gpioComIF, pwrSwitcher); #if OBSW_ADD_BPX_BATTERY_HANDLER == 1 - createBpxBatteryComponent(enableHkSets); + createBpxBatteryComponent(enableHkSets, battAndImtqI2cDev); #endif #if OBSW_ADD_STAR_TRACKER == 1 diff --git a/bsp_q7s/objectFactory.cpp b/bsp_q7s/objectFactory.cpp index ed07b131..cd4dd32c 100644 --- a/bsp_q7s/objectFactory.cpp +++ b/bsp_q7s/objectFactory.cpp @@ -161,13 +161,13 @@ void Factory::setStaticFrameworkObjectIds() { void ObjectFactory::setStatics() { Factory::setStaticFrameworkObjectIds(); } -void ObjectFactory::createTmpComponents( - std::vector> tmpDevsToAdd) { +void ObjectFactory::createTmpComponents(std::vector> tmpDevsToAdd, + const char* i2cDev) { std::vector tmpDevCookies; for (size_t idx = 0; idx < tmpDevsToAdd.size(); idx++) { tmpDevCookies.push_back( - new I2cCookie(tmpDevsToAdd[idx].second, TMP1075::MAX_REPLY_LENGTH, q7s::I2C_PL_EIVE)); + new I2cCookie(tmpDevsToAdd[idx].second, TMP1075::MAX_REPLY_LENGTH, i2cDev)); auto* tmpDevHandler = new Tmp1075Handler(tmpDevsToAdd[idx].first, objects::I2C_COM_IF, tmpDevCookies[idx]); tmpDevHandler->setCustomFdir(new TmpDevFdir(tmpDevsToAdd[idx].first)); @@ -957,12 +957,13 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher) { starTracker->setCustomFdir(strFdir); } -void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher, bool enableHkSets) { +void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher, bool enableHkSets, + const char* i2cDev) { auto* imtqAssy = new ImtqAssembly(objects::IMTQ_ASSY); imtqAssy->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); new ImtqPollingTask(objects::IMTQ_POLLING, I2C_FATAL_ERRORS); - I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, imtq::MAX_REPLY_SIZE, q7s::I2C_PS_EIVE); + I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, imtq::MAX_REPLY_SIZE, i2cDev); auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::IMTQ_POLLING, imtqI2cCookie, power::Switches::PDU1_CH3_MGT_5V, enableHkSets); imtqHandler->enableThermalModule(ThermalStateCfg()); @@ -978,8 +979,8 @@ void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher, bool enable #endif } -void ObjectFactory::createBpxBatteryComponent(bool enableHkSets) { - I2cCookie* bpxI2cCookie = new I2cCookie(addresses::BPX_BATTERY, 100, q7s::I2C_PS_EIVE); +void ObjectFactory::createBpxBatteryComponent(bool enableHkSets, const char* i2cDev) { + I2cCookie* bpxI2cCookie = new I2cCookie(addresses::BPX_BATTERY, 100, i2cDev); BpxBatteryHandler* bpxHandler = new BpxBatteryHandler( objects::BPX_BATT_HANDLER, objects::I2C_COM_IF, bpxI2cCookie, enableHkSets); bpxHandler->setStartUpImmediately(); diff --git a/bsp_q7s/objectFactory.h b/bsp_q7s/objectFactory.h index e4694bf4..779ad204 100644 --- a/bsp_q7s/objectFactory.h +++ b/bsp_q7s/objectFactory.h @@ -58,7 +58,8 @@ void createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher bool enableHkSets); void createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF, PowerSwitchIF* pwrSwitcher, Stack5VHandler& stackHandler); -void createTmpComponents(std::vector> tmpDevsToAdd); +void createTmpComponents(std::vector> tmpDevsToAdd, + const char* i2cDev); void createRadSensorChipSelect(LinuxLibgpioIF* gpioIF); ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& handler); void createAcsBoardGpios(GpioCookie& cookie); @@ -67,8 +68,8 @@ void createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF, Ser adis1650x::Type adisType); void createHeaterComponents(GpioIF* gpioIF, PowerSwitchIF* pwrSwitcher, HealthTableIF* healthTable, HeaterHandler*& heaterHandler); -void createImtqComponents(PowerSwitchIF* pwrSwitcher, bool enableHkSets); -void createBpxBatteryComponent(bool enableHkSets); +void createImtqComponents(PowerSwitchIF* pwrSwitcher, bool enableHkSets, const char* i2cDev); +void createBpxBatteryComponent(bool enableHkSets, const char* i2cDev); void createStrComponents(PowerSwitchIF* pwrSwitcher); void createSolarArrayDeploymentComponents(PowerSwitchIF& pwrSwitcher, GpioIF& gpioIF); void createSyrlinksComponents(PowerSwitchIF* pwrSwitcher); From d8ab6abc4939232cf8005f5ba30595c0e4b7a46a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 3 Jul 2023 17:16:51 +0200 Subject: [PATCH 456/506] FM should build again --- bsp_q7s/fmObjectFactory.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 74b3e725..fa6e7220 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -1,5 +1,5 @@ #include -#include +#include #include #include #include @@ -10,6 +10,7 @@ #include "bsp_q7s/core/CoreController.h" #include "busConf.h" #include "devConf.h" +#include "devices/addresses.h" #include "eive/objects.h" #include "fsfw_hal/linux/gpio/LinuxLibgpioIF.h" #include "linux/ObjectFactory.h" From 5c430212e70acf404c1632081bdd3c3094306a47 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 3 Jul 2023 17:32:35 +0200 Subject: [PATCH 457/506] that should fix the host build --- bsp_hosted/ObjectFactory.cpp | 2 +- unittest/controller/testThermalController.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 23f1a79f..4873bc58 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -113,7 +113,7 @@ void ObjectFactory::produce(void* args) { if (heaterHandler == nullptr) { sif::error << "HeaterHandler could not be created" << std::endl; } else { - ObjectFactory::createThermalController(*heaterHandler); + ObjectFactory::createThermalController(*heaterHandler, true); } new TestTask(objects::TEST_TASK); } diff --git a/unittest/controller/testThermalController.cpp b/unittest/controller/testThermalController.cpp index c2166ca0..287943ce 100644 --- a/unittest/controller/testThermalController.cpp +++ b/unittest/controller/testThermalController.cpp @@ -30,7 +30,8 @@ TEST_CASE("Thermal Controller", "[ThermalController]") { // testEnvironment::initialize(); - ThermalController controller(THERMAL_CONTROLLER_ID, *heaterHandler, tcsBrdShortlyUnavailable); + ThermalController controller(THERMAL_CONTROLLER_ID, *heaterHandler, tcsBrdShortlyUnavailable, + true); ReturnValue_t result = controller.initialize(); REQUIRE(result == returnvalue::OK); From cd9cb48b04ef9ea7e65cb0f7c8a1c36c17914f24 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 5 Jul 2023 16:31:41 +0200 Subject: [PATCH 458/506] set dataset invalid on OFF mode --- mission/tcs/Tmp1075Handler.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/tcs/Tmp1075Handler.cpp b/mission/tcs/Tmp1075Handler.cpp index 30331e1f..905219ca 100644 --- a/mission/tcs/Tmp1075Handler.cpp +++ b/mission/tcs/Tmp1075Handler.cpp @@ -19,6 +19,7 @@ void Tmp1075Handler::doStartUp() { void Tmp1075Handler::doShutDown() { communicationStep = CommunicationStep::START_ADC_CONVERSION; + dataset.setValidity(false, true); setMode(_MODE_POWER_DOWN); } From 52eba5cae6d3b1d10f8be8d1818ee0f66ad91fb3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 5 Jul 2023 17:20:42 +0200 Subject: [PATCH 459/506] small fix/tweaks --- CHANGELOG.md | 5 +++++ mission/system/tcs/TmpDevFdir.cpp | 10 +++++++--- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 38d76dac..68caa64b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,11 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- TMP1075: Set dataset invalid on shutdown explicitely +- Small fixes for TMP1075 FDIR: Use strange and missed reply counters. + # [v6.0.0] 2023-07-02 - `q7s-package` version v3.2.0 diff --git a/mission/system/tcs/TmpDevFdir.cpp b/mission/system/tcs/TmpDevFdir.cpp index d501dd1a..e191f7fb 100644 --- a/mission/system/tcs/TmpDevFdir.cpp +++ b/mission/system/tcs/TmpDevFdir.cpp @@ -20,7 +20,7 @@ ReturnValue_t TmpDevFdir::eventReceived(EventMessage* event) { // We'll try a recovery as long as defined in MAX_REBOOT. // Might cause some AssemblyBase cycles, so keep number low. // Ignored for TMP device, no way to power cycle it without going to OFF/BOOT mode. - // handleRecovery(event->getEvent()); + setFaulty(event->getEvent()); break; case DeviceHandlerIF::DEVICE_INTERPRETING_REPLY_FAILED: case DeviceHandlerIF::DEVICE_READING_REPLY_FAILED: @@ -29,7 +29,9 @@ ReturnValue_t TmpDevFdir::eventReceived(EventMessage* event) { case DeviceHandlerIF::DEVICE_BUILDING_COMMAND_FAILED: // These faults all mean that there were stupid replies from a device. // With now way to do a recovery, set the device to faulty immediately. - setFaulty(event->getEvent()); + if (strangeReplyCount.incrementAndCheck()) { + setFaulty(event->getEvent()); + } break; case DeviceHandlerIF::DEVICE_SENDING_COMMAND_FAILED: case DeviceHandlerIF::DEVICE_REQUESTING_REPLY_FAILED: @@ -40,7 +42,9 @@ ReturnValue_t TmpDevFdir::eventReceived(EventMessage* event) { break; } // else - setFaulty(event->getEvent()); + if (missedReplyCount.incrementAndCheck()) { + setFaulty(event->getEvent()); + } break; case StorageManagerIF::GET_DATA_FAILED: case StorageManagerIF::STORE_DATA_FAILED: From 1e98fcbbd597e6393b60cdb4d55877c28fcf41a9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 12:01:48 +0200 Subject: [PATCH 460/506] that really should not cause issues though.. --- mission/controller/ThermalController.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 53babbd5..7de3fc6c 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1596,8 +1596,7 @@ void ThermalController::ctrlComponentTemperature(HeaterContext& htrCtx) { // No sensors available, so switch the heater off. We can not perform control tasks if we // are blind.. if (chooseHeater(htrCtx.switchNr, htrCtx.redSwitchNr)) { - if (heaterCtrlAllowed() and - (heaterHandler.getSwitchState(htrCtx.switchNr) == HeaterHandler::SwitchState::ON)) { + if (heaterCtrlAllowed()) { heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent); } } @@ -1680,7 +1679,7 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { return; } - htrCtx.switchState = heaterHandler.getSwitchState(htrCtx.switchNr); + htrCtx.switchState = static_cast(heaterInfo.heaterSwitchState[htrCtx.switchNr]); // Heater off if (htrCtx.switchState == HeaterHandler::SwitchState::OFF) { if (sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) { From 1d4b815452439d29d9c85478bab9c21504af2223 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 12:04:36 +0200 Subject: [PATCH 461/506] this is better to read.. --- mission/controller/ThermalController.cpp | 3 ++- mission/tcs/HeaterHandler.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 7de3fc6c..dd0581af 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1679,7 +1679,8 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { return; } - htrCtx.switchState = static_cast(heaterInfo.heaterSwitchState[htrCtx.switchNr]); + htrCtx.switchState = + static_cast(heaterInfo.heaterSwitchState[htrCtx.switchNr]); // Heater off if (htrCtx.switchState == HeaterHandler::SwitchState::OFF) { if (sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) { diff --git a/mission/tcs/HeaterHandler.cpp b/mission/tcs/HeaterHandler.cpp index 8416a7f7..07e6f0eb 100644 --- a/mission/tcs/HeaterHandler.cpp +++ b/mission/tcs/HeaterHandler.cpp @@ -419,13 +419,13 @@ ReturnValue_t HeaterHandler::getAllSwitchStates(std::array& stat } bool HeaterHandler::allSwitchesOff() { - bool allSwitchesOrd = false; MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); - /* Or all switches. As soon one switch is on, allSwitchesOrd will be true */ for (power::Switch_t switchNr = 0; switchNr < heater::NUMBER_OF_SWITCHES; switchNr++) { - allSwitchesOrd = allSwitchesOrd || heaterVec.at(switchNr).switchState; + if (heaterVec.at(switchNr).switchState == SwitchState::ON) { + return false; + } } - return !allSwitchesOrd; + return true; } MessageQueueId_t HeaterHandler::getCommandQueue() const { return commandQueue->getId(); } From f4f365b11016a8bb05d1e28e3810a951237339d1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 13:56:43 +0200 Subject: [PATCH 462/506] pool read guard --- mission/tcs/Tmp1075Handler.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mission/tcs/Tmp1075Handler.cpp b/mission/tcs/Tmp1075Handler.cpp index 905219ca..3f2da98f 100644 --- a/mission/tcs/Tmp1075Handler.cpp +++ b/mission/tcs/Tmp1075Handler.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -19,6 +20,7 @@ void Tmp1075Handler::doStartUp() { void Tmp1075Handler::doShutDown() { communicationStep = CommunicationStep::START_ADC_CONVERSION; + PoolReadGuard pg(&dataset); dataset.setValidity(false, true); setMode(_MODE_POWER_DOWN); } From b7e49531269eed57ffd34d2af2917501c0800f45 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 16:00:39 +0200 Subject: [PATCH 463/506] the TCS subsystem does not work on the EM.. --- bsp_q7s/em/emObjectFactory.cpp | 2 +- bsp_q7s/fmObjectFactory.cpp | 2 +- dummies/Tmp1075Dummy.cpp | 13 ++++++++++++- dummies/Tmp1075Dummy.h | 1 + mission/controller/ThermalController.cpp | 16 ++++++++++++++++ mission/system/systemTree.cpp | 4 ++-- mission/system/systemTree.h | 2 +- mission/system/tcs/tcsModeTree.cpp | 22 +++++++++++++--------- mission/system/tcs/tcsModeTree.h | 2 +- mission/tcs/Tmp1075Handler.cpp | 7 ++++++- tmtc | 2 +- 11 files changed, 55 insertions(+), 18 deletions(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 28fabe7a..085d5184 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -168,5 +168,5 @@ void ObjectFactory::produce(void* args) { HeaterHandler* heaterHandler; createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler); createThermalController(*heaterHandler, true); - satsystem::init(); + satsystem::init(true); } diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index fa6e7220..5e2cdb59 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -133,5 +133,5 @@ void ObjectFactory::produce(void* args) { createMiscComponents(); createThermalController(*heaterHandler, false); createAcsController(true, enableHkSets); - satsystem::init(); + satsystem::init(false); } diff --git a/dummies/Tmp1075Dummy.cpp b/dummies/Tmp1075Dummy.cpp index 96d1f775..7e61acef 100644 --- a/dummies/Tmp1075Dummy.cpp +++ b/dummies/Tmp1075Dummy.cpp @@ -9,7 +9,11 @@ Tmp1075Dummy::Tmp1075Dummy(object_id_t objectId, object_id_t comif, CookieIF *co : DeviceHandlerBase(objectId, comif, comCookie), set(this) {} void Tmp1075Dummy::doStartUp() { setMode(MODE_ON); } -void Tmp1075Dummy::doShutDown() { setMode(MODE_OFF); } +void Tmp1075Dummy::doShutDown() { + PoolReadGuard pg(&set); + set.setValidity(false, true); + setMode(MODE_OFF); +} ReturnValue_t Tmp1075Dummy::buildNormalDeviceCommand(DeviceCommandId_t *id) { return NOTHING_TO_SEND; @@ -50,4 +54,11 @@ ReturnValue_t Tmp1075Dummy::initializeLocalDataPool(localpool::DataPool &localDa return OK; } +ReturnValue_t Tmp1075Dummy::setHealth(HealthState health) { + if (health == FAULTY or health == PERMANENT_FAULTY) { + setMode(_MODE_SHUT_DOWN); + } + return DeviceHandlerBase::setHealth(health); +} + LocalPoolDataSetBase *Tmp1075Dummy::getDataSetHandle(sid_t sid) { return &set; } diff --git a/dummies/Tmp1075Dummy.h b/dummies/Tmp1075Dummy.h index 570fcd42..feab4f98 100644 --- a/dummies/Tmp1075Dummy.h +++ b/dummies/Tmp1075Dummy.h @@ -26,6 +26,7 @@ class Tmp1075Dummy : public DeviceHandlerBase { uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) override; + ReturnValue_t setHealth(HealthState health) override; protected: LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index fca71acc..cb7af849 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1209,6 +1209,9 @@ void ThermalController::ctrlIfBoard() { sensors[2].first = deviceTemperatures.mgm2SideB.isValid(); sensors[2].second = deviceTemperatures.mgm2SideB.value; numSensors = 3; + if (not sensors[1].first) { + sif::debug << "TMP1075 IF Board not valid" << std::endl; + } HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_1_PCDU_PDU, ifBoardLimits); ctrlComponentTemperature(htrCtx); // TODO: special event overheating + could go back to safe mode @@ -1249,12 +1252,19 @@ void ThermalController::ctrlObc() { void ThermalController::ctrlObcIfBoard() { thermalComponent = OBCIF_BOARD; + sensors[0].first = deviceTemperatures.q7s.isValid(); sensors[0].second = deviceTemperatures.q7s.value; sensors[1].first = sensorTemperatures.tmp1075Tcs0.isValid(); sensors[1].second = sensorTemperatures.tmp1075Tcs0.value; sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs1.value; + if (not sensors[1].first) { + sif::debug << "TMP1075 TCS 0 not valid" << std::endl; + } + if (not sensors[2].first) { + sif::debug << "TMP1075 TCS 1 not valid" << std::endl; + } numSensors = 3; HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, obcIfBoardLimits); ctrlComponentTemperature(htrCtx); @@ -1363,6 +1373,12 @@ void ThermalController::ctrlPlPcduBoard() { sensors[2].second = deviceTemperatures.adcPayloadPcdu.value; sensors[3].first = sensorTemperatures.plpcduHeatspreader.isValid(); sensors[3].second = sensorTemperatures.plpcduHeatspreader.value; + if (not sensors[0].first) { + sif::debug << "TMP1075 PL PCDU 0 not valid" << std::endl; + } + if (not sensors[1].first) { + sif::debug << "TMP1075 PL PCDU 1 not valid" << std::endl; + } numSensors = 4; HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, plPcduBoardLimits); ctrlComponentTemperature(htrCtx); diff --git a/mission/system/systemTree.cpp b/mission/system/systemTree.cpp index 41bf3216..10c40617 100644 --- a/mission/system/systemTree.cpp +++ b/mission/system/systemTree.cpp @@ -31,12 +31,12 @@ void buildPtgInertialSequence(Subsystem& ss, ModeListEntry& eh); static const auto OFF = HasModesIF::MODE_OFF; static const auto NML = DeviceHandlerIF::MODE_NORMAL; -void satsystem::init() { +void satsystem::init(bool commandPlPcdu1) { auto& acsSubsystem = acs::init(); acsSubsystem.connectModeTreeParent(EIVE_SYSTEM); auto& payloadSubsystem = payload::init(); payloadSubsystem.connectModeTreeParent(EIVE_SYSTEM); - auto& tcsSubsystem = tcs::init(); + auto& tcsSubsystem = tcs::init(commandPlPcdu1); tcsSubsystem.connectModeTreeParent(EIVE_SYSTEM); auto& comSubsystem = com::init(); comSubsystem.connectModeTreeParent(EIVE_SYSTEM); diff --git a/mission/system/systemTree.h b/mission/system/systemTree.h index 9d769277..0ed73a89 100644 --- a/mission/system/systemTree.h +++ b/mission/system/systemTree.h @@ -5,7 +5,7 @@ namespace satsystem { -void init(); +void init(bool commandPlPcdu1); extern EiveSystem EIVE_SYSTEM; diff --git a/mission/system/tcs/tcsModeTree.cpp b/mission/system/tcs/tcsModeTree.cpp index 6cc57c6f..4186f9dd 100644 --- a/mission/system/tcs/tcsModeTree.cpp +++ b/mission/system/tcs/tcsModeTree.cpp @@ -10,8 +10,8 @@ TcsSubsystem satsystem::tcs::SUBSYSTEM(objects::TCS_SUBSYSTEM, 12, 24); namespace { // Alias for checker function const auto check = subsystem::checkInsert; -void buildOffSequence(Subsystem& ss, ModeListEntry& eh); -void buildNormalSequence(Subsystem& ss, ModeListEntry& eh); +void buildOffSequence(Subsystem& ss, ModeListEntry& eh, bool commandPlPcdu1); +void buildNormalSequence(Subsystem& ss, ModeListEntry& eh, bool commandPlPcdu1); } // namespace static const auto OFF = HasModesIF::MODE_OFF; @@ -27,17 +27,17 @@ auto TCS_TABLE_NORMAL_TGT = std::make_pair((NML << 24) | 1, FixedArrayList()); auto TCS_TABLE_NORMAL_TRANS_1 = std::make_pair((NML << 24) | 3, FixedArrayList()); -Subsystem& satsystem::tcs::init() { +Subsystem& satsystem::tcs::init(bool commandPlPcdu1) { ModeListEntry entry; - buildOffSequence(SUBSYSTEM, entry); - buildNormalSequence(SUBSYSTEM, entry); + buildOffSequence(SUBSYSTEM, entry, commandPlPcdu1); + buildNormalSequence(SUBSYSTEM, entry, commandPlPcdu1); SUBSYSTEM.setInitialMode(OFF); return SUBSYSTEM; } namespace { -void buildOffSequence(Subsystem& ss, ModeListEntry& eh) { +void buildOffSequence(Subsystem& ss, ModeListEntry& eh, bool commandPlPcdu1) { std::string context = "satsystem::tcs::buildOffSequence"; auto ctxc = context.c_str(); // Insert Helper Table @@ -67,7 +67,9 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) { iht(objects::TMP1075_HANDLER_TCS_0, OFF, 0, TCS_TABLE_OFF_TRANS_1.second); iht(objects::TMP1075_HANDLER_TCS_1, OFF, 0, TCS_TABLE_OFF_TRANS_1.second); iht(objects::TMP1075_HANDLER_PLPCDU_0, OFF, 0, TCS_TABLE_OFF_TRANS_1.second); - // TMP PL PCDU 1 is damaged + if (commandPlPcdu1) { + iht(objects::TMP1075_HANDLER_PLPCDU_1, OFF, 0, TCS_TABLE_OFF_TRANS_1.second); + } iht(objects::TMP1075_HANDLER_IF_BOARD, OFF, 0, TCS_TABLE_OFF_TRANS_1.second); check(ss.addTable(TableEntry(TCS_TABLE_OFF_TRANS_1.first, &TCS_TABLE_OFF_TRANS_1.second)), ctxc); @@ -79,7 +81,7 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) { ctxc); } -void buildNormalSequence(Subsystem& ss, ModeListEntry& eh) { +void buildNormalSequence(Subsystem& ss, ModeListEntry& eh, bool commandPlPcdu1) { std::string context = "satsystem::tcs::buildNormalSequence"; auto ctxc = context.c_str(); // Insert Helper Table @@ -105,7 +107,9 @@ void buildNormalSequence(Subsystem& ss, ModeListEntry& eh) { iht(objects::TMP1075_HANDLER_TCS_0, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); iht(objects::TMP1075_HANDLER_TCS_1, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); iht(objects::TMP1075_HANDLER_PLPCDU_0, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); - // TMP PL PCDU 1 is damaged + if (commandPlPcdu1) { + iht(objects::TMP1075_HANDLER_PLPCDU_1, NML, 0, TCS_TABLE_OFF_TRANS_0.second); + } iht(objects::TMP1075_HANDLER_IF_BOARD, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); check(ss.addTable(TableEntry(TCS_TABLE_NORMAL_TRANS_0.first, &TCS_TABLE_NORMAL_TRANS_0.second)), ctxc); diff --git a/mission/system/tcs/tcsModeTree.h b/mission/system/tcs/tcsModeTree.h index e5973641..4370ae73 100644 --- a/mission/system/tcs/tcsModeTree.h +++ b/mission/system/tcs/tcsModeTree.h @@ -7,7 +7,7 @@ namespace satsystem { namespace tcs { extern TcsSubsystem SUBSYSTEM; -Subsystem& init(); +Subsystem& init(bool commandPlPcdu1); } // namespace tcs } // namespace satsystem diff --git a/mission/tcs/Tmp1075Handler.cpp b/mission/tcs/Tmp1075Handler.cpp index 575a5fdf..7a7a48fe 100644 --- a/mission/tcs/Tmp1075Handler.cpp +++ b/mission/tcs/Tmp1075Handler.cpp @@ -15,6 +15,7 @@ Tmp1075Handler::~Tmp1075Handler() {} void Tmp1075Handler::doStartUp() { setMode(MODE_ON); } void Tmp1075Handler::doShutDown() { + sif::debug << "TMP1075: Going off" << std::endl; communicationStep = CommunicationStep::START_ADC_CONVERSION; PoolReadGuard pg(&dataset); dataset.setValidity(false, true); @@ -140,5 +141,9 @@ ReturnValue_t Tmp1075Handler::setHealth(HealthState health) { health != EXTERNAL_CONTROL) { return returnvalue::FAILED; } - return returnvalue::OK; + // Required because we do not have an assembly. + if (health == FAULTY) { + setMode(_MODE_SHUT_DOWN); + } + return DeviceHandlerBase::setHealth(health); } diff --git a/tmtc b/tmtc index c48f04ee..6d5bde40 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit c48f04eed5152f319b217870292968fb67b763d4 +Subproject commit 6d5bde40db69534a8cea923f96145bdeb62b79af From 61822188138e2149c48405627d3c4ebb4adaabad Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 17:23:55 +0200 Subject: [PATCH 464/506] this works but is kind of ugly --- bsp_q7s/objectFactory.cpp | 4 ---- dummies/Max31865Dummy.cpp | 12 +++++++++--- mission/controller/ThermalController.cpp | 15 --------------- mission/system/tcs/tcsModeTree.cpp | 2 +- mission/tcs/Tmp1075Handler.cpp | 4 +--- mission/tcs/Tmp1075Handler.h | 2 -- 6 files changed, 11 insertions(+), 28 deletions(-) diff --git a/bsp_q7s/objectFactory.cpp b/bsp_q7s/objectFactory.cpp index cd4dd32c..a96051f2 100644 --- a/bsp_q7s/objectFactory.cpp +++ b/bsp_q7s/objectFactory.cpp @@ -172,10 +172,6 @@ void ObjectFactory::createTmpComponents(std::vectorsetCustomFdir(new TmpDevFdir(tmpDevsToAdd[idx].first)); tmpDevHandler->connectModeTreeParent(satsystem::tcs::SUBSYSTEM); - // TODO: Remove this after TCS subsystem was added - // These devices are connected to the 3V3 stack and should be powered permanently. Therefore, - // we set them to normal mode immediately here. - tmpDevHandler->setModeNormal(); } } diff --git a/dummies/Max31865Dummy.cpp b/dummies/Max31865Dummy.cpp index 6ec1a716..99fc336a 100644 --- a/dummies/Max31865Dummy.cpp +++ b/dummies/Max31865Dummy.cpp @@ -7,15 +7,21 @@ using namespace returnvalue; Max31865Dummy::Max31865Dummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie), set(this, EiveMax31855::EXCHANGE_SET_ID) {} void Max31865Dummy::doStartUp() { setMode(MODE_ON); } -void Max31865Dummy::doShutDown() { setMode(_MODE_POWER_DOWN); } +void Max31865Dummy::doShutDown() { + PoolReadGuard pg(&set); + set.setValidity(false, true); + setMode(MODE_OFF); +} ReturnValue_t Max31865Dummy::buildNormalDeviceCommand(DeviceCommandId_t *id) { return NOTHING_TO_SEND; } -ReturnValue_t Max31865Dummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { return OK; } +ReturnValue_t Max31865Dummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + return NOTHING_TO_SEND; +} ReturnValue_t Max31865Dummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return 0; + return NOTHING_TO_SEND; } ReturnValue_t Max31865Dummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index cb7af849..dcd20a0c 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1209,9 +1209,6 @@ void ThermalController::ctrlIfBoard() { sensors[2].first = deviceTemperatures.mgm2SideB.isValid(); sensors[2].second = deviceTemperatures.mgm2SideB.value; numSensors = 3; - if (not sensors[1].first) { - sif::debug << "TMP1075 IF Board not valid" << std::endl; - } HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_1_PCDU_PDU, ifBoardLimits); ctrlComponentTemperature(htrCtx); // TODO: special event overheating + could go back to safe mode @@ -1259,12 +1256,6 @@ void ThermalController::ctrlObcIfBoard() { sensors[1].second = sensorTemperatures.tmp1075Tcs0.value; sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs1.value; - if (not sensors[1].first) { - sif::debug << "TMP1075 TCS 0 not valid" << std::endl; - } - if (not sensors[2].first) { - sif::debug << "TMP1075 TCS 1 not valid" << std::endl; - } numSensors = 3; HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, obcIfBoardLimits); ctrlComponentTemperature(htrCtx); @@ -1373,12 +1364,6 @@ void ThermalController::ctrlPlPcduBoard() { sensors[2].second = deviceTemperatures.adcPayloadPcdu.value; sensors[3].first = sensorTemperatures.plpcduHeatspreader.isValid(); sensors[3].second = sensorTemperatures.plpcduHeatspreader.value; - if (not sensors[0].first) { - sif::debug << "TMP1075 PL PCDU 0 not valid" << std::endl; - } - if (not sensors[1].first) { - sif::debug << "TMP1075 PL PCDU 1 not valid" << std::endl; - } numSensors = 4; HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, plPcduBoardLimits); ctrlComponentTemperature(htrCtx); diff --git a/mission/system/tcs/tcsModeTree.cpp b/mission/system/tcs/tcsModeTree.cpp index 4186f9dd..09a4fa61 100644 --- a/mission/system/tcs/tcsModeTree.cpp +++ b/mission/system/tcs/tcsModeTree.cpp @@ -108,7 +108,7 @@ void buildNormalSequence(Subsystem& ss, ModeListEntry& eh, bool commandPlPcdu1) iht(objects::TMP1075_HANDLER_TCS_1, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); iht(objects::TMP1075_HANDLER_PLPCDU_0, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); if (commandPlPcdu1) { - iht(objects::TMP1075_HANDLER_PLPCDU_1, NML, 0, TCS_TABLE_OFF_TRANS_0.second); + iht(objects::TMP1075_HANDLER_PLPCDU_1, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); } iht(objects::TMP1075_HANDLER_IF_BOARD, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); check(ss.addTable(TableEntry(TCS_TABLE_NORMAL_TRANS_0.first, &TCS_TABLE_NORMAL_TRANS_0.second)), diff --git a/mission/tcs/Tmp1075Handler.cpp b/mission/tcs/Tmp1075Handler.cpp index 7a7a48fe..a2daa819 100644 --- a/mission/tcs/Tmp1075Handler.cpp +++ b/mission/tcs/Tmp1075Handler.cpp @@ -134,15 +134,13 @@ ReturnValue_t Tmp1075Handler::initializeLocalDataPool(localpool::DataPool &local return returnvalue::OK; } -void Tmp1075Handler::setModeNormal() { setMode(_MODE_TO_NORMAL); } - ReturnValue_t Tmp1075Handler::setHealth(HealthState health) { if (health != FAULTY and health != PERMANENT_FAULTY and health != HEALTHY and health != EXTERNAL_CONTROL) { return returnvalue::FAILED; } // Required because we do not have an assembly. - if (health == FAULTY) { + if (health == FAULTY or health == PERMANENT_FAULTY) { setMode(_MODE_SHUT_DOWN); } return DeviceHandlerBase::setHealth(health); diff --git a/mission/tcs/Tmp1075Handler.h b/mission/tcs/Tmp1075Handler.h index 02fd6823..4badca39 100644 --- a/mission/tcs/Tmp1075Handler.h +++ b/mission/tcs/Tmp1075Handler.h @@ -20,8 +20,6 @@ class Tmp1075Handler : public DeviceHandlerBase { Tmp1075Handler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie); virtual ~Tmp1075Handler(); - void setModeNormal(); - protected: void doStartUp() override; void doShutDown() override; From 61ea80f92c1fb78ff57439c19e7c45de9c23120d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 17:26:09 +0200 Subject: [PATCH 465/506] remove some cruft --- mission/controller/ThermalController.cpp | 1 - mission/tcs/Tmp1075Handler.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index dcd20a0c..fca71acc 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1249,7 +1249,6 @@ void ThermalController::ctrlObc() { void ThermalController::ctrlObcIfBoard() { thermalComponent = OBCIF_BOARD; - sensors[0].first = deviceTemperatures.q7s.isValid(); sensors[0].second = deviceTemperatures.q7s.value; sensors[1].first = sensorTemperatures.tmp1075Tcs0.isValid(); diff --git a/mission/tcs/Tmp1075Handler.cpp b/mission/tcs/Tmp1075Handler.cpp index a2daa819..7e8569b6 100644 --- a/mission/tcs/Tmp1075Handler.cpp +++ b/mission/tcs/Tmp1075Handler.cpp @@ -15,7 +15,6 @@ Tmp1075Handler::~Tmp1075Handler() {} void Tmp1075Handler::doStartUp() { setMode(MODE_ON); } void Tmp1075Handler::doShutDown() { - sif::debug << "TMP1075: Going off" << std::endl; communicationStep = CommunicationStep::START_ADC_CONVERSION; PoolReadGuard pg(&dataset); dataset.setValidity(false, true); From 5630ae4962db3ada1a81b7987a13815d8ce4c0b7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 17:28:12 +0200 Subject: [PATCH 466/506] changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 08f904a9..40622385 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,6 +27,9 @@ will consitute of a breaking change warranting a new major release: - TMP1075: Set dataset invalid on shutdown explicitely - Small fixes for TMP1075 FDIR: Use strange and missed reply counters. +- TMP1075: Devices did not go to OFF mode when being set faulty. +- Update PL PCDU 1 in TCS mode tree on the EM. +- TMP1075: Possibly ignored health commands. # [v6.0.0] 2023-07-02 From 3c9eec7b3fb85cde00b210d2a64c81550348aac4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 18:58:57 +0200 Subject: [PATCH 467/506] heater events for TCS ctrl --- mission/controller/ThermalController.cpp | 97 +++++++++++++----------- mission/controller/ThermalController.h | 2 +- mission/controller/tcsDefs.h | 4 + tmtc | 2 +- 4 files changed, 57 insertions(+), 48 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index fca71acc..229742cf 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1008,7 +1008,7 @@ void ThermalController::ctrlAcsBoard() { heater::Switch redSwitchNr = heater::HEATER_3_OBC_BRD; // A side - thermalComponent = ACS_BOARD; + currThermalComponent = ACS_BOARD; sensors[0].first = deviceTemperatures.gyro0SideA.isValid(); sensors[0].second = deviceTemperatures.gyro0SideA.value; sensors[1].first = deviceTemperatures.gyro2SideB.isValid(); @@ -1052,7 +1052,7 @@ void ThermalController::ctrlAcsBoard() { if (chooseHeater(switchNr, redSwitchNr)) { if (heaterHandler.getSwitchState(switchNr)) { if (submode != SUBMODE_NO_HEATER_CTRL) { - heaterSwitchHelper(switchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + heaterSwitchHelper(switchNr, HeaterHandler::SwitchState::OFF, currThermalComponent); } } } @@ -1062,7 +1062,7 @@ void ThermalController::ctrlAcsBoard() { } void ThermalController::ctrlMgt() { - thermalComponent = MGT; + currThermalComponent = MGT; sensors[0].first = sensorTemperatures.mgt.isValid(); sensors[0].second = sensorTemperatures.mgt.value; sensors[1].first = deviceTemperatures.mgt.isValid(); @@ -1087,7 +1087,7 @@ void ThermalController::ctrlRw() { std::array sensorTemps{}; // RW1 - thermalComponent = RW; + currThermalComponent = RW; sensors[0].first = sensorTemperatures.rw1.isValid(); sensors[0].second = sensorTemperatures.rw1.value; sensors[1].first = deviceTemperatures.rw1.isValid(); @@ -1108,7 +1108,7 @@ void ThermalController::ctrlRw() { } // RW2 - thermalComponent = RW; + currThermalComponent = RW; sensors[0].first = deviceTemperatures.rw2.isValid(); sensors[0].second = deviceTemperatures.rw2.value; sensors[1].first = deviceTemperatures.rw3.isValid(); @@ -1130,7 +1130,7 @@ void ThermalController::ctrlRw() { } } // RW3 - thermalComponent = RW; + currThermalComponent = RW; sensors[0].first = deviceTemperatures.rw3.isValid(); sensors[0].second = deviceTemperatures.rw3.value; sensors[1].first = deviceTemperatures.rw4.isValid(); @@ -1153,7 +1153,7 @@ void ThermalController::ctrlRw() { } // RW4 - thermalComponent = RW; + currThermalComponent = RW; sensors[0].first = deviceTemperatures.rw4.isValid(); sensors[0].second = deviceTemperatures.rw4.value; sensors[1].first = deviceTemperatures.rw1.isValid(); @@ -1187,7 +1187,7 @@ void ThermalController::ctrlRw() { } void ThermalController::ctrlStr() { - thermalComponent = STR; + currThermalComponent = STR; sensors[0].first = sensorTemperatures.startracker.isValid(); sensors[0].second = sensorTemperatures.startracker.value; sensors[1].first = deviceTemperatures.startracker.isValid(); @@ -1201,7 +1201,7 @@ void ThermalController::ctrlStr() { } void ThermalController::ctrlIfBoard() { - thermalComponent = IF_BOARD; + currThermalComponent = IF_BOARD; sensors[0].first = sensorTemperatures.tmp1075IfBrd.isValid(); sensors[0].second = sensorTemperatures.tmp1075IfBrd.value; sensors[1].first = sensorTemperatures.mgt.isValid(); @@ -1215,7 +1215,7 @@ void ThermalController::ctrlIfBoard() { } void ThermalController::ctrlTcsBoard() { - thermalComponent = TCS_BOARD; + currThermalComponent = TCS_BOARD; sensors[0].first = sensorTemperatures.tcsBoard.isValid(); sensors[0].second = sensorTemperatures.tcsBoard.value; sensors[1].first = sensorTemperatures.tmp1075Tcs0.isValid(); @@ -1229,7 +1229,7 @@ void ThermalController::ctrlTcsBoard() { } void ThermalController::ctrlObc() { - thermalComponent = OBC; + currThermalComponent = OBC; sensors[0].first = deviceTemperatures.q7s.isValid(); sensors[0].second = deviceTemperatures.q7s.value; sensors[1].first = sensorTemperatures.tmp1075Tcs1.isValid(); @@ -1248,7 +1248,7 @@ void ThermalController::ctrlObc() { } void ThermalController::ctrlObcIfBoard() { - thermalComponent = OBCIF_BOARD; + currThermalComponent = OBCIF_BOARD; sensors[0].first = deviceTemperatures.q7s.isValid(); sensors[0].second = deviceTemperatures.q7s.value; sensors[1].first = sensorTemperatures.tmp1075Tcs0.isValid(); @@ -1267,7 +1267,7 @@ void ThermalController::ctrlObcIfBoard() { } void ThermalController::ctrlSBandTransceiver() { - thermalComponent = SBAND_TRANSCEIVER; + currThermalComponent = SBAND_TRANSCEIVER; sensors[0].first = deviceTemperatures.syrlinksPowerAmplifier.isValid(); sensors[0].second = deviceTemperatures.syrlinksPowerAmplifier.value; sensors[1].first = deviceTemperatures.syrlinksBasebandBoard.isValid(); @@ -1285,7 +1285,7 @@ void ThermalController::ctrlSBandTransceiver() { } } void ThermalController::ctrlPcduP60Board() { - thermalComponent = PCDUP60_BOARD; + currThermalComponent = PCDUP60_BOARD; sensors[0].first = deviceTemperatures.temp1P60dock.isValid(); sensors[0].second = deviceTemperatures.temp1P60dock.value; sensors[1].first = deviceTemperatures.temp2P60dock.isValid(); @@ -1302,7 +1302,7 @@ void ThermalController::ctrlPcduP60Board() { } void ThermalController::ctrlPcduAcu() { - thermalComponent = PCDUACU; + currThermalComponent = PCDUACU; heater::Switch switchNr = heater::HEATER_1_PCDU_PDU; heater::Switch redSwitchNr = heater::HEATER_2_ACS_BRD; @@ -1318,7 +1318,7 @@ void ThermalController::ctrlPcduAcu() { } else if (sensorTemperatures.acu.isValid()) { sensorTemp = sensorTemperatures.acu.value; } else { - triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, thermalComponent); + triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, currThermalComponent); sensorTempAvailable = false; } if (sensorTempAvailable) { @@ -1335,7 +1335,7 @@ void ThermalController::ctrlPcduAcu() { } void ThermalController::ctrlPcduPdu() { - thermalComponent = PCDUPDU; + currThermalComponent = PCDUPDU; sensors[0].first = deviceTemperatures.pdu1.isValid(); sensors[0].second = deviceTemperatures.pdu1.value; sensors[1].first = deviceTemperatures.pdu2.isValid(); @@ -1354,7 +1354,7 @@ void ThermalController::ctrlPcduPdu() { } void ThermalController::ctrlPlPcduBoard() { - thermalComponent = PLPCDU_BOARD; + currThermalComponent = PLPCDU_BOARD; sensors[0].first = sensorTemperatures.tmp1075PlPcdu0.isValid(); sensors[0].second = sensorTemperatures.tmp1075PlPcdu0.value; sensors[1].first = sensorTemperatures.tmp1075PlPcdu1.isValid(); @@ -1370,7 +1370,7 @@ void ThermalController::ctrlPlPcduBoard() { } void ThermalController::ctrlPlocMissionBoard() { - thermalComponent = PLOCMISSION_BOARD; + currThermalComponent = PLOCMISSION_BOARD; sensors[0].first = sensorTemperatures.plocHeatspreader.isValid(); sensors[0].second = sensorTemperatures.plocHeatspreader.value; sensors[1].first = sensorTemperatures.plocMissionboard.isValid(); @@ -1385,7 +1385,7 @@ void ThermalController::ctrlPlocMissionBoard() { } void ThermalController::ctrlPlocProcessingBoard() { - thermalComponent = PLOCPROCESSING_BOARD; + currThermalComponent = PLOCPROCESSING_BOARD; sensors[0].first = sensorTemperatures.plocMissionboard.isValid(); sensors[0].second = sensorTemperatures.plocMissionboard.value; sensors[1].first = sensorTemperatures.plocHeatspreader.isValid(); @@ -1400,7 +1400,7 @@ void ThermalController::ctrlPlocProcessingBoard() { } void ThermalController::ctrlDac() { - thermalComponent = DAC; + currThermalComponent = DAC; sensors[0].first = sensorTemperatures.dacHeatspreader.isValid(); sensors[0].second = sensorTemperatures.dacHeatspreader.value; sensors[1].first = sensorTemperatures.plocMissionboard.isValid(); @@ -1414,7 +1414,7 @@ void ThermalController::ctrlDac() { } void ThermalController::ctrlCameraBody() { - thermalComponent = CAMERA; + currThermalComponent = CAMERA; sensors[0].first = sensorTemperatures.payload4kCamera.isValid(); sensors[0].second = sensorTemperatures.payload4kCamera.value; sensors[1].first = sensorTemperatures.dro.isValid(); @@ -1440,7 +1440,7 @@ void ThermalController::ctrlCameraBody() { } void ThermalController::ctrlDro() { - thermalComponent = DRO; + currThermalComponent = DRO; sensors[0].first = sensorTemperatures.dro.isValid(); sensors[0].second = sensorTemperatures.dro.value; sensors[1].first = sensorTemperatures.payload4kCamera.isValid(); @@ -1454,7 +1454,7 @@ void ThermalController::ctrlDro() { } void ThermalController::ctrlX8() { - thermalComponent = X8; + currThermalComponent = X8; sensors[0].first = sensorTemperatures.x8.isValid(); sensors[0].second = sensorTemperatures.x8.value; sensors[1].first = sensorTemperatures.hpa.isValid(); @@ -1468,7 +1468,7 @@ void ThermalController::ctrlX8() { } void ThermalController::ctrlTx() { - thermalComponent = TX; + currThermalComponent = TX; sensors[0].first = sensorTemperatures.eBandTx.isValid(); sensors[0].second = sensorTemperatures.eBandTx.value; sensors[1].first = sensorTemperatures.x8.isValid(); @@ -1482,7 +1482,7 @@ void ThermalController::ctrlTx() { } void ThermalController::ctrlMpa() { - thermalComponent = MPA; + currThermalComponent = MPA; sensors[0].first = sensorTemperatures.mpa.isValid(); sensors[0].second = sensorTemperatures.mpa.value; sensors[1].first = sensorTemperatures.hpa.isValid(); @@ -1496,7 +1496,7 @@ void ThermalController::ctrlMpa() { } void ThermalController::ctrlHpa() { - thermalComponent = HPA; + currThermalComponent = HPA; sensors[0].first = sensorTemperatures.hpa.isValid(); sensors[0].second = sensorTemperatures.hpa.value; sensors[1].first = sensorTemperatures.x8.isValid(); @@ -1510,7 +1510,7 @@ void ThermalController::ctrlHpa() { } void ThermalController::ctrlScexBoard() { - thermalComponent = SCEX_BOARD; + currThermalComponent = SCEX_BOARD; sensors[0].first = sensorTemperatures.scex.isValid(); sensors[0].second = sensorTemperatures.scex.value; sensors[1].first = sensorTemperatures.x8.isValid(); @@ -1601,7 +1601,7 @@ void ThermalController::ctrlComponentTemperature(HeaterContext& htrCtx) { if (chooseHeater(htrCtx.switchNr, htrCtx.redSwitchNr)) { if (heaterCtrlAllowed() and (heaterHandler.getSwitchState(htrCtx.switchNr) == HeaterHandler::SwitchState::ON)) { - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, currThermalComponent); } } } @@ -1613,19 +1613,19 @@ bool ThermalController::selectAndReadSensorTemp(HeaterContext& htrCtx) { sensors[i].second > SANITY_LIMIT_LOWER_TEMP and sensors[i].second < SANITY_LIMIT_UPPER_TEMP) { sensorTemp = sensors[i].second; - thermalStates[thermalComponent].errorCounter = 0; + thermalStates[currThermalComponent].errorCounter = 0; return true; } } - thermalStates[thermalComponent].errorCounter++; - if (thermalComponent != RW and thermalComponent != ACS_BOARD) { - if (thermalStates[thermalComponent].errorCounter <= 3) { - triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, thermalComponent); + thermalStates[currThermalComponent].errorCounter++; + if (currThermalComponent != RW and currThermalComponent != ACS_BOARD) { + if (thermalStates[currThermalComponent].errorCounter <= 3) { + triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, currThermalComponent); } } else { - if (thermalStates[thermalComponent].errorCounter <= 8) { - triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, thermalComponent); + if (thermalStates[currThermalComponent].errorCounter <= 8) { + triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, currThermalComponent); } } @@ -1658,14 +1658,14 @@ void ThermalController::heaterCtrlTempTooHighHandler(HeaterContext& htrCtx, cons return; } if (htrCtx.switchState == HeaterHandler::SwitchState::ON) { - sif::info << "TCS: Component " << static_cast(thermalComponent) << " too warm, above " + sif::info << "TCS: Component " << static_cast(currThermalComponent) << " too warm, above " << whatLimit << ", switching off heater" << std::endl; - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, currThermalComponent); heaterStates[htrCtx.switchNr].switchTransition = true; heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::OFF; } if (heaterHandler.getSwitchState(htrCtx.redSwitchNr) == HeaterHandler::SwitchState::ON) { - heaterSwitchHelper(htrCtx.redSwitchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + heaterSwitchHelper(htrCtx.redSwitchNr, HeaterHandler::SwitchState::OFF, currThermalComponent); heaterStates[htrCtx.redSwitchNr].switchTransition = true; heaterStates[htrCtx.redSwitchNr].target = HeaterHandler::SwitchState::OFF; } @@ -1687,13 +1687,13 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { // Heater off if (htrCtx.switchState == HeaterHandler::SwitchState::OFF) { if (sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) { - sif::info << "TCS: Heater " << static_cast(thermalComponent) << " ON" << std::endl; - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::ON, thermalComponent); + sif::info << "TCS: Heater " << static_cast(currThermalComponent) << " ON" << std::endl; + heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::ON, currThermalComponent); heaterStates[htrCtx.switchNr].switchTransition = true; heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::ON; } else { // Even if heater control is now allowed, we can update the state. - thermalStates[thermalComponent].heating = false; + thermalStates[currThermalComponent].heating = false; } heaterCtrlCheckUpperLimits(htrCtx); return; @@ -1701,11 +1701,12 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { // Heater on if (htrCtx.switchState == HeaterHandler::SwitchState::ON) { - if (thermalStates[thermalComponent].heating) { + if (thermalStates[currThermalComponent].heating) { // We are already in a heating cycle, so need to check whether heating task is complete. if (sensorTemp >= htrCtx.tempLimit.opLowerLimit + TEMP_OFFSET and heaterCtrlAllowed()) { - sif::info << "TCS: Heater " << static_cast(thermalComponent) << " OFF" << std::endl; - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + sif::info << "TCS: Heater " << static_cast(currThermalComponent) << " OFF" + << std::endl; + heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, currThermalComponent); heaterStates[htrCtx.switchNr].switchTransition = true; heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::OFF; } @@ -1747,7 +1748,7 @@ void ThermalController::resetSensorsArray() { validValuePair.first = false; validValuePair.second = INVALID_TEMPERATURE; } - thermalComponent = NONE; + currThermalComponent = NONE; } void ThermalController::heaterTransitionControl(const HeaterSwitchStates& currentHeaterStates) { @@ -1809,10 +1810,14 @@ void ThermalController::heaterSwitchHelper(heater::Switch switchNr, Clock::getClockMonotonic(¤tTime); if (state == HeaterHandler::SwitchState::ON) { heaterHandler.switchHeater(switchNr, state); + triggerEvent(tcsCtrl::TCS_SWITCHING_HEATER_ON, static_cast(currThermalComponent), + static_cast(switchNr)); thermalStates[componentIdx].heating = true; thermalStates[componentIdx].heaterStartTime = currentTime.tv_sec; } else { heaterHandler.switchHeater(switchNr, state); + triggerEvent(tcsCtrl::TCS_SWITCHING_HEATER_OFF, static_cast(currThermalComponent), + static_cast(switchNr)); thermalStates[componentIdx].heating = false; thermalStates[componentIdx].heaterEndTime = currentTime.tv_sec; } diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 2518a13e..b571b950 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -256,7 +256,7 @@ class ThermalController : public ExtendedControllerBase { TempLimits scexBoardLimits = TempLimits(-60.0, -40.0, 80.0, 85.0, 150.0); double sensorTemp = INVALID_TEMPERATURE; - ThermalComponents thermalComponent = NONE; + ThermalComponents currThermalComponent = NONE; bool redSwitchNrInUse = false; MessageQueueId_t camId = MessageQueueIF::NO_QUEUE; bool componentAboveCutOffLimit = false; diff --git a/mission/controller/tcsDefs.h b/mission/controller/tcsDefs.h index e2efd98c..c77d9352 100644 --- a/mission/controller/tcsDefs.h +++ b/mission/controller/tcsDefs.h @@ -18,6 +18,10 @@ static constexpr Event CAMERA_OVERHEATING = MAKE_EVENT(5, severity::HIGH); static constexpr Event PCDU_SYSTEM_OVERHEATING = MAKE_EVENT(6, severity::HIGH); static constexpr Event HEATER_NOT_OFF_FOR_OFF_MODE = MAKE_EVENT(7, severity::MEDIUM); static constexpr Event MGT_OVERHEATING = MAKE_EVENT(8, severity::HIGH); +//! [EXPORT] : [COMMENT] P1: Module index. P2: Heater index +static constexpr Event TCS_SWITCHING_HEATER_ON = MAKE_EVENT(9, severity::INFO); +//! [EXPORT] : [COMMENT] P1: Module index. P2: Heater index +static constexpr Event TCS_SWITCHING_HEATER_OFF = MAKE_EVENT(10, severity::INFO); enum SetId : uint32_t { SENSOR_TEMPERATURES = 0, diff --git a/tmtc b/tmtc index c48f04ee..6d5bde40 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit c48f04eed5152f319b217870292968fb67b763d4 +Subproject commit 6d5bde40db69534a8cea923f96145bdeb62b79af From 785c1c65a0696d20e3c718bdf676c291144dd99f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 19:01:49 +0200 Subject: [PATCH 468/506] add new events --- bsp_hosted/fsfwconfig/events/translateEvents.cpp | 10 ++++++++-- bsp_hosted/fsfwconfig/objects/translateObjects.cpp | 2 +- generators/bsp_hosted_events.csv | 2 ++ generators/bsp_q7s_events.csv | 2 ++ generators/events/translateEvents.cpp | 10 ++++++++-- generators/objects/translateObjects.cpp | 2 +- linux/fsfwconfig/events/translateEvents.cpp | 10 ++++++++-- linux/fsfwconfig/objects/translateObjects.cpp | 2 +- tmtc | 2 +- 9 files changed, 32 insertions(+), 10 deletions(-) diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index 1500301f..30774cf9 100644 --- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp +++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 296 translations. + * @brief Auto-generated event translation file. Contains 298 translations. * @details - * Generated on: 2023-06-21 19:01:02 + * Generated on: 2023-07-06 19:00:21 */ #include "translateEvents.h" @@ -286,6 +286,8 @@ const char *CAMERA_OVERHEATING_STRING = "CAMERA_OVERHEATING"; const char *PCDU_SYSTEM_OVERHEATING_STRING = "PCDU_SYSTEM_OVERHEATING"; const char *HEATER_NOT_OFF_FOR_OFF_MODE_STRING = "HEATER_NOT_OFF_FOR_OFF_MODE"; const char *MGT_OVERHEATING_STRING = "MGT_OVERHEATING"; +const char *TCS_SWITCHING_HEATER_ON_STRING = "TCS_SWITCHING_HEATER_ON"; +const char *TCS_SWITCHING_HEATER_OFF_STRING = "TCS_SWITCHING_HEATER_OFF"; const char *TX_TIMER_EXPIRED_STRING = "TX_TIMER_EXPIRED"; const char *BIT_LOCK_TX_ON_STRING = "BIT_LOCK_TX_ON"; const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION"; @@ -866,6 +868,10 @@ const char *translateEvents(Event event) { return HEATER_NOT_OFF_FOR_OFF_MODE_STRING; case (14108): return MGT_OVERHEATING_STRING; + case (14109): + return TCS_SWITCHING_HEATER_ON_STRING; + case (14110): + return TCS_SWITCHING_HEATER_OFF_STRING; case (14201): return TX_TIMER_EXPIRED_STRING; case (14202): diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index d1d0464c..c073905f 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 171 translations. - * Generated on: 2023-06-21 19:01:02 + * Generated on: 2023-07-06 19:00:21 */ #include "translateObjects.h" diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv index 17592d1d..a26b0cdc 100644 --- a/generators/bsp_hosted_events.csv +++ b/generators/bsp_hosted_events.csv @@ -280,6 +280,8 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14106;0x371a;PCDU_SYSTEM_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14107;0x371b;HEATER_NOT_OFF_FOR_OFF_MODE;MEDIUM;No description;mission/controller/tcsDefs.h 14108;0x371c;MGT_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h +14109;0x371d;TCS_SWITCHING_HEATER_ON;INFO;P1: Module index. P2: Heater index;mission/controller/tcsDefs.h +14110;0x371e;TCS_SWITCHING_HEATER_OFF;INFO;P1: Module index. P2: Heater index;mission/controller/tcsDefs.h 14201;0x3779;TX_TIMER_EXPIRED;INFO;The transmit timer to protect the Syrlinks expired P1: The current timer value;mission/system/com/ComSubsystem.h 14202;0x377a;BIT_LOCK_TX_ON;INFO;Transmitter will be turned on due to detection of bitlock;mission/system/com/ComSubsystem.h 14300;0x37dc;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/persistentTmStoreDefs.h diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 17592d1d..a26b0cdc 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -280,6 +280,8 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14106;0x371a;PCDU_SYSTEM_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14107;0x371b;HEATER_NOT_OFF_FOR_OFF_MODE;MEDIUM;No description;mission/controller/tcsDefs.h 14108;0x371c;MGT_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h +14109;0x371d;TCS_SWITCHING_HEATER_ON;INFO;P1: Module index. P2: Heater index;mission/controller/tcsDefs.h +14110;0x371e;TCS_SWITCHING_HEATER_OFF;INFO;P1: Module index. P2: Heater index;mission/controller/tcsDefs.h 14201;0x3779;TX_TIMER_EXPIRED;INFO;The transmit timer to protect the Syrlinks expired P1: The current timer value;mission/system/com/ComSubsystem.h 14202;0x377a;BIT_LOCK_TX_ON;INFO;Transmitter will be turned on due to detection of bitlock;mission/system/com/ComSubsystem.h 14300;0x37dc;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/persistentTmStoreDefs.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index 1500301f..30774cf9 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 296 translations. + * @brief Auto-generated event translation file. Contains 298 translations. * @details - * Generated on: 2023-06-21 19:01:02 + * Generated on: 2023-07-06 19:00:21 */ #include "translateEvents.h" @@ -286,6 +286,8 @@ const char *CAMERA_OVERHEATING_STRING = "CAMERA_OVERHEATING"; const char *PCDU_SYSTEM_OVERHEATING_STRING = "PCDU_SYSTEM_OVERHEATING"; const char *HEATER_NOT_OFF_FOR_OFF_MODE_STRING = "HEATER_NOT_OFF_FOR_OFF_MODE"; const char *MGT_OVERHEATING_STRING = "MGT_OVERHEATING"; +const char *TCS_SWITCHING_HEATER_ON_STRING = "TCS_SWITCHING_HEATER_ON"; +const char *TCS_SWITCHING_HEATER_OFF_STRING = "TCS_SWITCHING_HEATER_OFF"; const char *TX_TIMER_EXPIRED_STRING = "TX_TIMER_EXPIRED"; const char *BIT_LOCK_TX_ON_STRING = "BIT_LOCK_TX_ON"; const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION"; @@ -866,6 +868,10 @@ const char *translateEvents(Event event) { return HEATER_NOT_OFF_FOR_OFF_MODE_STRING; case (14108): return MGT_OVERHEATING_STRING; + case (14109): + return TCS_SWITCHING_HEATER_ON_STRING; + case (14110): + return TCS_SWITCHING_HEATER_OFF_STRING; case (14201): return TX_TIMER_EXPIRED_STRING; case (14202): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index 1ecd5e71..379265c6 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-06-21 19:01:02 + * Generated on: 2023-07-06 19:00:21 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index 1500301f..30774cf9 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 296 translations. + * @brief Auto-generated event translation file. Contains 298 translations. * @details - * Generated on: 2023-06-21 19:01:02 + * Generated on: 2023-07-06 19:00:21 */ #include "translateEvents.h" @@ -286,6 +286,8 @@ const char *CAMERA_OVERHEATING_STRING = "CAMERA_OVERHEATING"; const char *PCDU_SYSTEM_OVERHEATING_STRING = "PCDU_SYSTEM_OVERHEATING"; const char *HEATER_NOT_OFF_FOR_OFF_MODE_STRING = "HEATER_NOT_OFF_FOR_OFF_MODE"; const char *MGT_OVERHEATING_STRING = "MGT_OVERHEATING"; +const char *TCS_SWITCHING_HEATER_ON_STRING = "TCS_SWITCHING_HEATER_ON"; +const char *TCS_SWITCHING_HEATER_OFF_STRING = "TCS_SWITCHING_HEATER_OFF"; const char *TX_TIMER_EXPIRED_STRING = "TX_TIMER_EXPIRED"; const char *BIT_LOCK_TX_ON_STRING = "BIT_LOCK_TX_ON"; const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION"; @@ -866,6 +868,10 @@ const char *translateEvents(Event event) { return HEATER_NOT_OFF_FOR_OFF_MODE_STRING; case (14108): return MGT_OVERHEATING_STRING; + case (14109): + return TCS_SWITCHING_HEATER_ON_STRING; + case (14110): + return TCS_SWITCHING_HEATER_OFF_STRING; case (14201): return TX_TIMER_EXPIRED_STRING; case (14202): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index 1ecd5e71..379265c6 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-06-21 19:01:02 + * Generated on: 2023-07-06 19:00:21 */ #include "translateObjects.h" diff --git a/tmtc b/tmtc index 6d5bde40..5785bbd0 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 6d5bde40db69534a8cea923f96145bdeb62b79af +Subproject commit 5785bbd0ccb045e072f347a5ff2265c610d3872c From 12ff3654332ce61e82a89b06b8e8dc4e46b532bf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 19:57:23 +0200 Subject: [PATCH 469/506] TCS ctrl bugfix --- CHANGELOG.md | 3 +++ mission/controller/ThermalController.cpp | 13 ++++++++----- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 08f904a9..50471069 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,6 +27,9 @@ will consitute of a breaking change warranting a new major release: - TMP1075: Set dataset invalid on shutdown explicitely - Small fixes for TMP1075 FDIR: Use strange and missed reply counters. +- TCS controller: A helper member to track the elapsed heater control cycles was not reset + properly, which could lead to switch transitions being completed immediately. This can + lead to weird bugs like switched being commanded on twice. # [v6.0.0] 2023-07-02 diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index fca71acc..5a544b3c 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1687,8 +1687,9 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { // Heater off if (htrCtx.switchState == HeaterHandler::SwitchState::OFF) { if (sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) { - sif::info << "TCS: Heater " << static_cast(thermalComponent) << " ON" << std::endl; - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::ON, thermalComponent); + sif::info << "TCS: Heater " << (int)htrCtx.switchNr << " for component " + << static_cast(currThermalComponent) << " ON" << std::endl; + heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::ON, currThermalComponent); heaterStates[htrCtx.switchNr].switchTransition = true; heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::ON; } else { @@ -1704,8 +1705,9 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { if (thermalStates[thermalComponent].heating) { // We are already in a heating cycle, so need to check whether heating task is complete. if (sensorTemp >= htrCtx.tempLimit.opLowerLimit + TEMP_OFFSET and heaterCtrlAllowed()) { - sif::info << "TCS: Heater " << static_cast(thermalComponent) << " OFF" << std::endl; - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + sif::info << "TCS: Heater " << (int)htrCtx.switchNr << " for component " + << static_cast(currThermalComponent) << " OFF" << std::endl; + heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, currThermalComponent); heaterStates[htrCtx.switchNr].switchTransition = true; heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::OFF; } @@ -1755,9 +1757,10 @@ void ThermalController::heaterTransitionControl(const HeaterSwitchStates& curren if (heaterStates[i].switchTransition) { if (currentHeaterStates[i] == heaterStates[i].target) { heaterStates[i].switchTransition = false; + heaterStates[i].heaterSwitchControlCycles = 0; continue; } - if (heaterStates[i].heaterSwitchControlCycles > 3) { + if (heaterStates[i].heaterSwitchControlCycles > 5) { heaterStates[i].switchTransition = false; heaterStates[i].heaterSwitchControlCycles = 0; } From 64ca329d5acba5ebb8c65d459c295fac8dc40dc8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 19:58:24 +0200 Subject: [PATCH 470/506] update changelog --- CHANGELOG.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 50471069..021ceedd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -29,7 +29,8 @@ will consitute of a breaking change warranting a new major release: - Small fixes for TMP1075 FDIR: Use strange and missed reply counters. - TCS controller: A helper member to track the elapsed heater control cycles was not reset properly, which could lead to switch transitions being completed immediately. This can - lead to weird bugs like switched being commanded on twice. + lead to weird bugs like heaters being commanded ON twice and can potentially lead to + other bugs. # [v6.0.0] 2023-07-02 From 3e92d175d50a18c5da9f71af79c156b2ca98fe9f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 20:01:57 +0200 Subject: [PATCH 471/506] form fix --- mission/controller/ThermalController.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 5a544b3c..e873d638 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1687,7 +1687,7 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { // Heater off if (htrCtx.switchState == HeaterHandler::SwitchState::OFF) { if (sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) { - sif::info << "TCS: Heater " << (int)htrCtx.switchNr << " for component " + sif::info << "TCS: Heater " << static_cast(htrCtx.switchNr) << " for component " << static_cast(currThermalComponent) << " ON" << std::endl; heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::ON, currThermalComponent); heaterStates[htrCtx.switchNr].switchTransition = true; @@ -1705,7 +1705,7 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { if (thermalStates[thermalComponent].heating) { // We are already in a heating cycle, so need to check whether heating task is complete. if (sensorTemp >= htrCtx.tempLimit.opLowerLimit + TEMP_OFFSET and heaterCtrlAllowed()) { - sif::info << "TCS: Heater " << (int)htrCtx.switchNr << " for component " + sif::info << "TCS: Heater " << static_cast(htrCtx.switchNr) << " for component " << static_cast(currThermalComponent) << " OFF" << std::endl; heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, currThermalComponent); heaterStates[htrCtx.switchNr].switchTransition = true; From 7b611810df4af42e130e09ef592922cdd8198c89 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 20:13:33 +0200 Subject: [PATCH 472/506] there are actually 8 heaters --- CHANGELOG.md | 2 ++ mission/controller/ThermalController.cpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 08f904a9..a74c99fd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,6 +27,8 @@ will consitute of a breaking change warranting a new major release: - TMP1075: Set dataset invalid on shutdown explicitely - Small fixes for TMP1075 FDIR: Use strange and missed reply counters. +- TCS controller: Last heater (S-band heater) was skipped for transition completion + checks. # [v6.0.0] 2023-07-02 diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index fca71acc..3f6b2995 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1751,7 +1751,7 @@ void ThermalController::resetSensorsArray() { } void ThermalController::heaterTransitionControl(const HeaterSwitchStates& currentHeaterStates) { - for (unsigned i = 0; i < 7; i++) { + for (unsigned i = 0; i < heater::Switch::NUMBER_OF_SWITCHES; i++) { if (heaterStates[i].switchTransition) { if (currentHeaterStates[i] == heaterStates[i].target) { heaterStates[i].switchTransition = false; From faffca0c2c85abed8233cf3c54c62c3d3e940f30 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 20:19:05 +0200 Subject: [PATCH 473/506] compile fix --- mission/controller/ThermalController.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index e873d638..3cbd5d4d 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1688,8 +1688,8 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { if (htrCtx.switchState == HeaterHandler::SwitchState::OFF) { if (sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) { sif::info << "TCS: Heater " << static_cast(htrCtx.switchNr) << " for component " - << static_cast(currThermalComponent) << " ON" << std::endl; - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::ON, currThermalComponent); + << static_cast(thermalComponent) << " ON" << std::endl; + heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::ON, thermalComponent); heaterStates[htrCtx.switchNr].switchTransition = true; heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::ON; } else { @@ -1706,8 +1706,8 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { // We are already in a heating cycle, so need to check whether heating task is complete. if (sensorTemp >= htrCtx.tempLimit.opLowerLimit + TEMP_OFFSET and heaterCtrlAllowed()) { sif::info << "TCS: Heater " << static_cast(htrCtx.switchNr) << " for component " - << static_cast(currThermalComponent) << " OFF" << std::endl; - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, currThermalComponent); + << static_cast(thermalComponent) << " OFF" << std::endl; + heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent); heaterStates[htrCtx.switchNr].switchTransition = true; heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::OFF; } From ce19f30325b6bf29cc41694ebfab9ca04a2ccc48 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 22:06:54 +0200 Subject: [PATCH 474/506] continue max heater duration --- mission/controller/ThermalController.cpp | 46 +++++++++++++++++++++--- mission/controller/ThermalController.h | 25 +++++++++++++ tmtc | 2 +- 3 files changed, 67 insertions(+), 6 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index fca71acc..603a72ea 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -209,8 +209,12 @@ void ThermalController::performControlOperation() { } else { transitionWhenHeatersOffCycles++; } - } else if (mode != MODE_OFF and not tcsBrdShortlyUnavailable) { - performThermalModuleCtrl(heaterSwitchStateArray); + } else if (mode != MODE_OFF) { + if (not tcsBrdShortlyUnavailable) { + performThermalModuleCtrl(heaterSwitchStateArray); + } + heaterTransitionControl(heaterSwitchStateArray); + heaterMaxDurationControl(heaterSwitchStateArray); } } @@ -1586,9 +1590,8 @@ void ThermalController::performThermalModuleCtrl(const HeaterSwitchStates& heate eBandTooHotFlag = false; } } - - heaterTransitionControl(heaterSwitchStates); } + void ThermalController::ctrlComponentTemperature(HeaterContext& htrCtx) { if (selectAndReadSensorTemp(htrCtx)) { if (chooseHeater(htrCtx.switchNr, htrCtx.redSwitchNr)) { @@ -1613,6 +1616,7 @@ bool ThermalController::selectAndReadSensorTemp(HeaterContext& htrCtx) { sensors[i].second > SANITY_LIMIT_LOWER_TEMP and sensors[i].second < SANITY_LIMIT_UPPER_TEMP) { sensorTemp = sensors[i].second; + currentSensorIndex = i; thermalStates[thermalComponent].errorCounter = 0; return true; } @@ -1689,7 +1693,6 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { if (sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) { sif::info << "TCS: Heater " << static_cast(thermalComponent) << " ON" << std::endl; heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::ON, thermalComponent); - heaterStates[htrCtx.switchNr].switchTransition = true; heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::ON; } else { // Even if heater control is now allowed, we can update the state. @@ -1754,6 +1757,11 @@ void ThermalController::heaterTransitionControl(const HeaterSwitchStates& curren for (unsigned i = 0; i < 7; i++) { if (heaterStates[i].switchTransition) { if (currentHeaterStates[i] == heaterStates[i].target) { + // Required for max heat period control + if (currentHeaterStates[i] == HeaterHandler::SwitchState::ON) { + heaterStates[i].heaterOnPeriod.setTimeout(MAX_HEATER_ON_DURATIONS[i]); + heaterStates[i].heaterOnPeriod.resetTimer(); + } heaterStates[i].switchTransition = false; continue; } @@ -1765,6 +1773,26 @@ void ThermalController::heaterTransitionControl(const HeaterSwitchStates& curren } } } + +void ThermalController::heaterMaxDurationControl(const HeaterSwitchStates& currentHeaterStates) { + for (unsigned i = 0; i < heater::Switch::NUMBER_OF_SWITCHES; i++) { + if (currentHeaterStates[i] == HeaterHandler::SwitchState::ON and + heaterStates[i].heaterOnPeriod.hasTimedOut()) { + heaterStates[i].switchTransition = false; + heaterStates[i].heaterSwitchControlCycles = 0; + heaterHandler.switchHeater(static_cast(i), HeaterHandler::SwitchState::OFF); + for (unsigned j = 0; j < thermalStates.size(); j++) { + if (thermalStates[j].heating and thermalStates[j].heaterSwitch == i) { + timeval currentTime; + Clock::getClockMonotonic(¤tTime); + thermalStates[j].heating = false; + thermalStates[j].heaterEndTime = currentTime.tv_sec; + } + } + } + } +} + uint32_t ThermalController::tempFloatToU32() const { auto sensorTempAsFloat = static_cast(sensorTemp); uint32_t tempRaw = 0; @@ -1799,6 +1827,11 @@ bool ThermalController::heaterCtrlAllowed() const { return submode != SUBMODE_NO void ThermalController::resetThermalStates() { for (auto& thermalState : thermalStates) { thermalState.heating = false; + thermalState.errorCounter = 0; + thermalState.heaterStartTime = 0; + thermalState.heaterEndTime = 0; + thermalState.sensorIndex = 0; + thermalState.heaterSwitch = heater::Switch::NUMBER_OF_SWITCHES; } } @@ -1809,6 +1842,9 @@ void ThermalController::heaterSwitchHelper(heater::Switch switchNr, Clock::getClockMonotonic(¤tTime); if (state == HeaterHandler::SwitchState::ON) { heaterHandler.switchHeater(switchNr, state); + heaterStates[switchNr].switchTransition = true; + thermalStates[componentIdx].sensorIndex = currentSensorIndex; + thermalStates[componentIdx].heaterSwitch = switchNr; thermalStates[componentIdx].heating = true; thermalStates[componentIdx].heaterStartTime = currentTime.tv_sec; } else { diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 2518a13e..6695e7e7 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -48,8 +48,11 @@ struct TempLimits { struct ThermalState { uint8_t errorCounter; + // Which sensor is used for this component? + uint8_t sensorIndex = 0; // Is heating on for that thermal module? bool heating = false; + // Which switch is being used for heating the component heater::Switch heaterSwitch = heater::Switch::NUMBER_OF_SWITCHES; // Heater start time and end times as UNIX seconds. Please note that these times will be updated // when a switch command is sent, with no guarantess that the heater actually went on. @@ -61,6 +64,7 @@ struct HeaterState { bool switchTransition; HeaterHandler::SwitchState target; uint8_t heaterSwitchControlCycles; + Countdown heaterOnPeriod; }; using HeaterSwitchStates = std::array; @@ -102,6 +106,25 @@ class ThermalController : public ExtendedControllerBase { static constexpr int16_t SANITY_LIMIT_LOWER_TEMP = -80; static constexpr int16_t SANITY_LIMIT_UPPER_TEMP = 160; + // 1 hour + static constexpr uint32_t MAX_HEATER_ON_DURATION_MS = 60 * 60 * 1000; + static constexpr uint32_t MAX_HEATER_ON_DURATIONS[8] = {// PLOC PROC board + MAX_HEATER_ON_DURATION_MS, + // PCDU PDU + MAX_HEATER_ON_DURATION_MS, + // ACS Board + MAX_HEATER_ON_DURATION_MS, + // OBC Board + MAX_HEATER_ON_DURATION_MS, + // Camera + MAX_HEATER_ON_DURATION_MS, + // STR + MAX_HEATER_ON_DURATION_MS, + // DRO + MAX_HEATER_ON_DURATION_MS, + // S-Band + MAX_HEATER_ON_DURATION_MS}; + ThermalController(object_id_t objectId, HeaterHandler& heater, const std::atomic_bool& tcsBoardShortUnavailable, bool pollPcdu1Tmp); virtual ~ThermalController(); @@ -256,6 +279,7 @@ class ThermalController : public ExtendedControllerBase { TempLimits scexBoardLimits = TempLimits(-60.0, -40.0, 80.0, 85.0, 150.0); double sensorTemp = INVALID_TEMPERATURE; + uint8_t currentSensorIndex = 0; ThermalComponents thermalComponent = NONE; bool redSwitchNrInUse = false; MessageQueueId_t camId = MessageQueueIF::NO_QUEUE; @@ -348,6 +372,7 @@ class ThermalController : public ExtendedControllerBase { void ctrlMpa(); void ctrlScexBoard(); void heaterTransitionControl(const HeaterSwitchStates& currentHeaterStates); + void heaterMaxDurationControl(const HeaterSwitchStates& currentHeaterStates); void setMode(Mode_t mode, Submode_t submode); uint32_t tempFloatToU32() const; bool tooHotHandler(object_id_t object, bool& oneShotFlag); diff --git a/tmtc b/tmtc index c48f04ee..5785bbd0 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit c48f04eed5152f319b217870292968fb67b763d4 +Subproject commit 5785bbd0ccb045e072f347a5ff2265c610d3872c From c558a117b038fb678221f311f530c4f00097ff5b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 22:21:00 +0200 Subject: [PATCH 475/506] some docs --- mission/controller/ThermalController.cpp | 39 +++++++++++++++--------- mission/controller/ThermalController.h | 14 +++++++++ 2 files changed, 39 insertions(+), 14 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index da508f33..4ff06fef 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1693,7 +1693,6 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { if (sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) { sif::info << "TCS: Heater " << static_cast(thermalComponent) << " ON" << std::endl; heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::ON, thermalComponent); - heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::ON; } else { // Even if heater control is now allowed, we can update the state. thermalStates[thermalComponent].heating = false; @@ -1757,10 +1756,14 @@ void ThermalController::heaterTransitionControl(const HeaterSwitchStates& curren for (unsigned i = 0; i < heater::Switch::NUMBER_OF_SWITCHES; i++) { if (heaterStates[i].switchTransition) { if (currentHeaterStates[i] == heaterStates[i].target) { - // Required for max heat period control + // Required for max heater period control if (currentHeaterStates[i] == HeaterHandler::SwitchState::ON) { heaterStates[i].heaterOnPeriod.setTimeout(MAX_HEATER_ON_DURATIONS[i]); heaterStates[i].heaterOnPeriod.resetTimer(); + } else { + // The heater might still be one for some thermal components, so cross-check + // those components + crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(static_cast(i)); } heaterStates[i].switchTransition = false; continue; @@ -1781,14 +1784,9 @@ void ThermalController::heaterMaxDurationControl(const HeaterSwitchStates& curre heaterStates[i].switchTransition = false; heaterStates[i].heaterSwitchControlCycles = 0; heaterHandler.switchHeater(static_cast(i), HeaterHandler::SwitchState::OFF); - for (unsigned j = 0; j < thermalStates.size(); j++) { - if (thermalStates[j].heating and thermalStates[j].heaterSwitch == i) { - timeval currentTime; - Clock::getClockMonotonic(¤tTime); - thermalStates[j].heating = false; - thermalStates[j].heaterEndTime = currentTime.tv_sec; - } - } + // The heater might still be one for some thermal components, so cross-check + // those components + crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(static_cast(i)); } } } @@ -1836,19 +1834,20 @@ void ThermalController::resetThermalStates() { } void ThermalController::heaterSwitchHelper(heater::Switch switchNr, - HeaterHandler::SwitchState state, + HeaterHandler::SwitchState targetState, unsigned componentIdx) { timeval currentTime; Clock::getClockMonotonic(¤tTime); - if (state == HeaterHandler::SwitchState::ON) { - heaterHandler.switchHeater(switchNr, state); + if (targetState == HeaterHandler::SwitchState::ON) { + heaterHandler.switchHeater(switchNr, targetState); + heaterStates[switchNr].target = HeaterHandler::SwitchState::ON; heaterStates[switchNr].switchTransition = true; thermalStates[componentIdx].sensorIndex = currentSensorIndex; thermalStates[componentIdx].heaterSwitch = switchNr; thermalStates[componentIdx].heating = true; thermalStates[componentIdx].heaterStartTime = currentTime.tv_sec; } else { - heaterHandler.switchHeater(switchNr, state); + heaterHandler.switchHeater(switchNr, targetState); thermalStates[componentIdx].heating = false; thermalStates[componentIdx].heaterEndTime = currentTime.tv_sec; } @@ -1873,6 +1872,18 @@ ThermalController::~ThermalController() { } } +void ThermalController::crossCheckHeaterStateOfComponentsWhenHeaterGoesOff( + heater::Switch switchIdx) { + for (unsigned j = 0; j < thermalStates.size(); j++) { + if (thermalStates[j].heating and thermalStates[j].heaterSwitch == switchIdx) { + timeval currentTime; + Clock::getClockMonotonic(¤tTime); + thermalStates[j].heating = false; + thermalStates[j].heaterEndTime = currentTime.tv_sec; + } + } +} + void ThermalController::tooHotHandlerWhichClearsOneShotFlag(object_id_t object, bool& oneShotFlag) { // Clear the one shot flag is the component is in acceptable temperature range. if (not tooHotHandler(object, oneShotFlag) and not componentAboveUpperLimit) { diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 6695e7e7..021be00e 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -371,8 +371,22 @@ class ThermalController : public ExtendedControllerBase { void ctrlTx(); void ctrlMpa(); void ctrlScexBoard(); + + /** + * The transition of heaters might take some time. As long as a transition is + * going on, the TCS controller works in a reduced form. This function takes care + * of tracking transition and capturing their completion. + * @param currentHeaterStates + */ void heaterTransitionControl(const HeaterSwitchStates& currentHeaterStates); + /** + * Control tasks to prevent heaters being on for prolonged periods. Ideally, this + * should never happen, but this task prevents bugs from causing heaters to stay on + * for a long time, which draws a lot of power. + * @param currentHeaterStates + */ void heaterMaxDurationControl(const HeaterSwitchStates& currentHeaterStates); + void crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(heater::Switch switchIdx); void setMode(Mode_t mode, Submode_t submode); uint32_t tempFloatToU32() const; bool tooHotHandler(object_id_t object, bool& oneShotFlag); From 027955597f0b9c102fb047142629edaf927dc458 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 23:06:50 +0200 Subject: [PATCH 476/506] done --- mission/controller/ThermalController.cpp | 346 ++++++++++++----------- mission/controller/ThermalController.h | 187 +++++------- mission/controller/tcsDefs.h | 110 ++++++- mission/tcs/HeaterHandler.cpp | 26 +- mission/tcs/HeaterHandler.h | 11 +- mission/tcs/defs.h | 6 +- 6 files changed, 380 insertions(+), 306 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 4ff06fef..0ed8a4d9 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -33,6 +33,7 @@ ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater susTemperatures(this), deviceTemperatures(this), heaterInfo(this), + tcsCtrlInfo(this), imtqThermalSet(objects::IMTQ_HANDLER, ThermalStateCfg()), maxSet0PlocHspd(objects::RTD_0_IC3_PLOC_HEATSPREADER, EiveMax31855::RtdCommands::EXCHANGE_SET_ID), @@ -174,7 +175,7 @@ void ThermalController::performControlOperation() { } } - HeaterSwitchStates heaterSwitchStateArray{}; + tcsCtrl::HeaterSwitchStates heaterSwitchStateArray{}; heaterHandler.getAllSwitchStates(heaterSwitchStateArray); { PoolReadGuard pg(&heaterInfo); @@ -191,12 +192,11 @@ void ThermalController::performControlOperation() { if (transitionWhenHeatersOff) { bool allSwitchersOff = true; for (size_t idx = 0; idx < heaterSwitchStateArray.size(); idx++) { - if (heaterSwitchStateArray[idx] != HeaterHandler::SwitchState::OFF) { + if (heaterSwitchStateArray[idx] != heater::SwitchState::OFF) { allSwitchersOff = false; // if heater still ON after 3 cycles, switch OFF again if (transitionWhenHeatersOffCycles == 3) { - heaterHandler.switchHeater(static_cast(idx), - HeaterHandler::SwitchState::OFF); + heaterHandler.switchHeater(static_cast(idx), heater::SwitchState::OFF); triggerEvent(tcsCtrl::HEATER_NOT_OFF_FOR_OFF_MODE); } } @@ -215,6 +215,14 @@ void ThermalController::performControlOperation() { } heaterTransitionControl(heaterSwitchStateArray); heaterMaxDurationControl(heaterSwitchStateArray); + PoolReadGuard pg(&tcsCtrlInfo); + for (uint8_t i = 0; i < thermalStates.size(); i++) { + tcsCtrlInfo.heatingOnVec[i] = thermalStates[i].heating; + tcsCtrlInfo.sensorIdxUsedForTcsCtrl[i] = thermalStates[i].sensorIndex; + tcsCtrlInfo.heaterSwitchIdx[i] = thermalStates[i].heaterSwitch; + tcsCtrlInfo.heaterStartTimes[i] = thermalStates[i].heaterStartTime; + tcsCtrlInfo.heaterEndTimes[i] = thermalStates[i].heaterEndTime; + } } } @@ -284,6 +292,11 @@ ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& lo localDataPoolMap.emplace(tcsCtrl::TEMP_ADC_PAYLOAD_PCDU, new PoolEntry({0.0})); localDataPoolMap.emplace(tcsCtrl::HEATER_SWITCH_LIST, &heaterSwitchStates); localDataPoolMap.emplace(tcsCtrl::HEATER_CURRENT, &heaterCurrent); + localDataPoolMap.emplace(tcsCtrl::HEATER_ON_FOR_COMPONENT_VEC, &tcsCtrlHeaterOn); + localDataPoolMap.emplace(tcsCtrl::SENSOR_USED_FOR_TCS_CTRL, &tcsCtrlSensorIdx); + localDataPoolMap.emplace(tcsCtrl::HEATER_IDX_USED_FOR_TCS_CTRL, &tcsCtrlHeaterIdx); + localDataPoolMap.emplace(tcsCtrl::HEATER_START_TIME, &tcsCtrlStartTimes); + localDataPoolMap.emplace(tcsCtrl::HEATER_END_TIME, &tcsCtrlEndTimes); bool enableHkSets = false; #if OBSW_ENABLE_PERIODIC_HK == 1 @@ -297,6 +310,8 @@ ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& lo subdp::RegularHkPeriodicParams(deviceTemperatures.getSid(), enableHkSets, 120.0)); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(heaterInfo.getSid(), enableHkSets, 120.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(tcsCtrlInfo.getSid(), enableHkSets, 120.0)); return returnvalue::OK; } @@ -311,6 +326,8 @@ LocalPoolDataSetBase* ThermalController::getDataSetHandle(sid_t sid) { return &deviceTemperatures; case tcsCtrl::HEATER_SET: return &heaterInfo; + case tcsCtrl::TCS_CTRL_INFO: + return &tcsCtrlInfo; default: return nullptr; } @@ -1012,7 +1029,7 @@ void ThermalController::ctrlAcsBoard() { heater::Switch redSwitchNr = heater::HEATER_3_OBC_BRD; // A side - thermalComponent = ACS_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::ACS_BOARD; sensors[0].first = deviceTemperatures.gyro0SideA.isValid(); sensors[0].second = deviceTemperatures.gyro0SideA.value; sensors[1].first = deviceTemperatures.gyro2SideB.isValid(); @@ -1056,7 +1073,7 @@ void ThermalController::ctrlAcsBoard() { if (chooseHeater(switchNr, redSwitchNr)) { if (heaterHandler.getSwitchState(switchNr)) { if (submode != SUBMODE_NO_HEATER_CTRL) { - heaterSwitchHelper(switchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + heaterSwitchHelper(switchNr, heater::SwitchState::OFF, ctrlCtx.thermalComponent); } } } @@ -1066,7 +1083,7 @@ void ThermalController::ctrlAcsBoard() { } void ThermalController::ctrlMgt() { - thermalComponent = MGT; + ctrlCtx.thermalComponent = tcsCtrl::MGT; sensors[0].first = sensorTemperatures.mgt.isValid(); sensors[0].second = sensorTemperatures.mgt.value; sensors[1].first = deviceTemperatures.mgt.isValid(); @@ -1076,11 +1093,11 @@ void ThermalController::ctrlMgt() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_1_PCDU_PDU, mgtLimits); ctrlComponentTemperature(htrCtx); - if (componentAboveUpperLimit and not mgtTooHotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not tooHotFlags.mgtTooHotFlag) { triggerEvent(tcsCtrl::MGT_OVERHEATING, tempFloatToU32()); - mgtTooHotFlag = true; - } else if (not componentAboveUpperLimit) { - mgtTooHotFlag = false; + tooHotFlags.mgtTooHotFlag = true; + } else if (not ctrlCtx.componentAboveUpperLimit) { + tooHotFlags.mgtTooHotFlag = false; } } @@ -1091,7 +1108,7 @@ void ThermalController::ctrlRw() { std::array sensorTemps{}; // RW1 - thermalComponent = RW; + ctrlCtx.thermalComponent = tcsCtrl::RW; sensors[0].first = sensorTemperatures.rw1.isValid(); sensors[0].second = sensorTemperatures.rw1.value; sensors[1].first = deviceTemperatures.rw1.isValid(); @@ -1105,14 +1122,14 @@ void ThermalController::ctrlRw() { HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits); ctrlComponentTemperature(htrCtx); sensorTemps[0] = tempFloatToU32(); - if (componentAboveUpperLimit) { + if (ctrlCtx.componentAboveUpperLimit) { oneIsAboveLimit = true; - eventToTrigger = overHeatEventToTrigger; + eventToTrigger = ctrlCtx.overHeatEventToTrigger; } } // RW2 - thermalComponent = RW; + ctrlCtx.thermalComponent = tcsCtrl::RW; sensors[0].first = deviceTemperatures.rw2.isValid(); sensors[0].second = deviceTemperatures.rw2.value; sensors[1].first = deviceTemperatures.rw3.isValid(); @@ -1126,15 +1143,15 @@ void ThermalController::ctrlRw() { HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits); ctrlComponentTemperature(htrCtx); sensorTemps[1] = tempFloatToU32(); - if (componentAboveUpperLimit) { + if (ctrlCtx.componentAboveUpperLimit) { oneIsAboveLimit = true; if (eventToTrigger != ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH) { - eventToTrigger = overHeatEventToTrigger; + eventToTrigger = ctrlCtx.overHeatEventToTrigger; } } } // RW3 - thermalComponent = RW; + ctrlCtx.thermalComponent = tcsCtrl::RW; sensors[0].first = deviceTemperatures.rw3.isValid(); sensors[0].second = deviceTemperatures.rw3.value; sensors[1].first = deviceTemperatures.rw4.isValid(); @@ -1148,16 +1165,16 @@ void ThermalController::ctrlRw() { HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits); ctrlComponentTemperature(htrCtx); sensorTemps[2] = tempFloatToU32(); - if (componentAboveUpperLimit) { + if (ctrlCtx.componentAboveUpperLimit) { oneIsAboveLimit = true; if (eventToTrigger != ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH) { - eventToTrigger = overHeatEventToTrigger; + eventToTrigger = ctrlCtx.overHeatEventToTrigger; } } } // RW4 - thermalComponent = RW; + ctrlCtx.thermalComponent = tcsCtrl::RW; sensors[0].first = deviceTemperatures.rw4.isValid(); sensors[0].second = deviceTemperatures.rw4.value; sensors[1].first = deviceTemperatures.rw1.isValid(); @@ -1171,27 +1188,27 @@ void ThermalController::ctrlRw() { HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits); ctrlComponentTemperature(htrCtx); sensorTemps[3] = tempFloatToU32(); - if (componentAboveUpperLimit) { + if (ctrlCtx.componentAboveUpperLimit) { oneIsAboveLimit = true; if (eventToTrigger != ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH) { - eventToTrigger = overHeatEventToTrigger; + eventToTrigger = ctrlCtx.overHeatEventToTrigger; } } } - if (oneIsAboveLimit and not rwTooHotFlag) { + if (oneIsAboveLimit and not tooHotFlags.rwTooHotFlag) { EventManagerIF::triggerEvent(objects::RW1, eventToTrigger, sensorTemps[0]); EventManagerIF::triggerEvent(objects::RW2, eventToTrigger, sensorTemps[1]); EventManagerIF::triggerEvent(objects::RW3, eventToTrigger, sensorTemps[2]); EventManagerIF::triggerEvent(objects::RW4, eventToTrigger, sensorTemps[3]); - rwTooHotFlag = true; + tooHotFlags.rwTooHotFlag = true; } else if (not oneIsAboveLimit) { - rwTooHotFlag = false; + tooHotFlags.rwTooHotFlag = false; } } void ThermalController::ctrlStr() { - thermalComponent = STR; + ctrlCtx.thermalComponent = tcsCtrl::STR; sensors[0].first = sensorTemperatures.startracker.isValid(); sensors[0].second = sensorTemperatures.startracker.value; sensors[1].first = deviceTemperatures.startracker.isValid(); @@ -1201,11 +1218,11 @@ void ThermalController::ctrlStr() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_5_STR, heater::HEATER_6_DRO, strLimits); ctrlComponentTemperature(htrCtx); - tooHotHandlerWhichClearsOneShotFlag(objects::STAR_TRACKER, strTooHotFlag); + tooHotHandlerWhichClearsOneShotFlag(objects::STAR_TRACKER, tooHotFlags.strTooHotFlag); } void ThermalController::ctrlIfBoard() { - thermalComponent = IF_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::IF_BOARD; sensors[0].first = sensorTemperatures.tmp1075IfBrd.isValid(); sensors[0].second = sensorTemperatures.tmp1075IfBrd.value; sensors[1].first = sensorTemperatures.mgt.isValid(); @@ -1219,7 +1236,7 @@ void ThermalController::ctrlIfBoard() { } void ThermalController::ctrlTcsBoard() { - thermalComponent = TCS_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::TCS_BOARD; sensors[0].first = sensorTemperatures.tcsBoard.isValid(); sensors[0].second = sensorTemperatures.tcsBoard.value; sensors[1].first = sensorTemperatures.tmp1075Tcs0.isValid(); @@ -1233,7 +1250,7 @@ void ThermalController::ctrlTcsBoard() { } void ThermalController::ctrlObc() { - thermalComponent = OBC; + ctrlCtx.thermalComponent = tcsCtrl::OBC; sensors[0].first = deviceTemperatures.q7s.isValid(); sensors[0].second = deviceTemperatures.q7s.value; sensors[1].first = sensorTemperatures.tmp1075Tcs1.isValid(); @@ -1243,16 +1260,16 @@ void ThermalController::ctrlObc() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, obcLimits); ctrlComponentTemperature(htrCtx); - if (componentAboveUpperLimit and not obcTooHotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not tooHotFlags.obcTooHotFlag) { triggerEvent(tcsCtrl::OBC_OVERHEATING, tempFloatToU32()); - obcTooHotFlag = true; - } else if (not componentAboveUpperLimit) { - obcTooHotFlag = false; + tooHotFlags.obcTooHotFlag = true; + } else if (not ctrlCtx.componentAboveUpperLimit) { + tooHotFlags.obcTooHotFlag = false; } } void ThermalController::ctrlObcIfBoard() { - thermalComponent = OBCIF_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::OBCIF_BOARD; sensors[0].first = deviceTemperatures.q7s.isValid(); sensors[0].second = deviceTemperatures.q7s.value; sensors[1].first = sensorTemperatures.tmp1075Tcs0.isValid(); @@ -1262,16 +1279,16 @@ void ThermalController::ctrlObcIfBoard() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, obcIfBoardLimits); ctrlComponentTemperature(htrCtx); - if (componentAboveUpperLimit and not obcTooHotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not tooHotFlags.obcTooHotFlag) { triggerEvent(tcsCtrl::OBC_OVERHEATING, tempFloatToU32()); - obcTooHotFlag = true; - } else if (not componentAboveUpperLimit) { - obcTooHotFlag = false; + tooHotFlags.obcTooHotFlag = true; + } else if (not ctrlCtx.componentAboveUpperLimit) { + tooHotFlags.obcTooHotFlag = false; } } void ThermalController::ctrlSBandTransceiver() { - thermalComponent = SBAND_TRANSCEIVER; + ctrlCtx.thermalComponent = tcsCtrl::SBAND_TRANSCEIVER; sensors[0].first = deviceTemperatures.syrlinksPowerAmplifier.isValid(); sensors[0].second = deviceTemperatures.syrlinksPowerAmplifier.value; sensors[1].first = deviceTemperatures.syrlinksBasebandBoard.isValid(); @@ -1281,15 +1298,15 @@ void ThermalController::ctrlSBandTransceiver() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_7_S_BAND, heater::HEATER_4_CAMERA, sBandTransceiverLimits); ctrlComponentTemperature(htrCtx); - if (componentAboveUpperLimit and not syrlinksTooHotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not tooHotFlags.syrlinksTooHotFlag) { triggerEvent(tcsCtrl::SYRLINKS_OVERHEATING, tempFloatToU32()); - syrlinksTooHotFlag = true; - } else if (not componentAboveUpperLimit) { - syrlinksTooHotFlag = false; + tooHotFlags.syrlinksTooHotFlag = true; + } else if (not ctrlCtx.componentAboveUpperLimit) { + tooHotFlags.syrlinksTooHotFlag = false; } } void ThermalController::ctrlPcduP60Board() { - thermalComponent = PCDUP60_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::PCDUP60_BOARD; sensors[0].first = deviceTemperatures.temp1P60dock.isValid(); sensors[0].second = deviceTemperatures.temp1P60dock.value; sensors[1].first = deviceTemperatures.temp2P60dock.isValid(); @@ -1297,16 +1314,16 @@ void ThermalController::ctrlPcduP60Board() { numSensors = 2; HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduP60BoardLimits); ctrlComponentTemperature(htrCtx); - if (componentAboveUpperLimit and not pcduSystemTooHotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not tooHotFlags.pcduSystemTooHotFlag) { triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32()); - pcduSystemTooHotFlag = true; - } else if (not componentAboveUpperLimit) { - pcduSystemTooHotFlag = false; + tooHotFlags.pcduSystemTooHotFlag = true; + } else if (not ctrlCtx.componentAboveUpperLimit) { + tooHotFlags.pcduSystemTooHotFlag = false; } // TODO: ! } void ThermalController::ctrlPcduAcu() { - thermalComponent = PCDUACU; + ctrlCtx.thermalComponent = tcsCtrl::PCDUACU; heater::Switch switchNr = heater::HEATER_1_PCDU_PDU; heater::Switch redSwitchNr = heater::HEATER_2_ACS_BRD; @@ -1314,15 +1331,15 @@ void ThermalController::ctrlPcduAcu() { bool sensorTempAvailable = true; // TODO: check if (deviceTemperatures.acu.value[0] != INVALID_TEMPERATURE) { - sensorTemp = deviceTemperatures.acu.value[0]; + ctrlCtx.sensorTemp = deviceTemperatures.acu.value[0]; } else if (deviceTemperatures.acu.value[1] != INVALID_TEMPERATURE) { - sensorTemp = deviceTemperatures.acu.value[1]; + ctrlCtx.sensorTemp = deviceTemperatures.acu.value[1]; } else if (deviceTemperatures.acu.value[2] != INVALID_TEMPERATURE) { - sensorTemp = deviceTemperatures.acu.value[2]; + ctrlCtx.sensorTemp = deviceTemperatures.acu.value[2]; } else if (sensorTemperatures.acu.isValid()) { - sensorTemp = sensorTemperatures.acu.value; + ctrlCtx.sensorTemp = sensorTemperatures.acu.value; } else { - triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, thermalComponent); + triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, ctrlCtx.thermalComponent); sensorTempAvailable = false; } if (sensorTempAvailable) { @@ -1330,16 +1347,16 @@ void ThermalController::ctrlPcduAcu() { checkLimitsAndCtrlHeater(htrCtx); } } - if (componentAboveUpperLimit and not pcduSystemTooHotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not tooHotFlags.pcduSystemTooHotFlag) { triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32()); - pcduSystemTooHotFlag = true; - } else if (not componentAboveUpperLimit) { - pcduSystemTooHotFlag = false; + tooHotFlags.pcduSystemTooHotFlag = true; + } else if (not ctrlCtx.componentAboveUpperLimit) { + tooHotFlags.pcduSystemTooHotFlag = false; } } void ThermalController::ctrlPcduPdu() { - thermalComponent = PCDUPDU; + ctrlCtx.thermalComponent = tcsCtrl::PCDUPDU; sensors[0].first = deviceTemperatures.pdu1.isValid(); sensors[0].second = deviceTemperatures.pdu1.value; sensors[1].first = deviceTemperatures.pdu2.isValid(); @@ -1349,16 +1366,16 @@ void ThermalController::ctrlPcduPdu() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduPduLimits); ctrlComponentTemperature(htrCtx); - if (componentAboveUpperLimit and not pcduSystemTooHotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not tooHotFlags.pcduSystemTooHotFlag) { triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32()); - pcduSystemTooHotFlag = true; - } else if (not componentAboveUpperLimit) { - pcduSystemTooHotFlag = false; + tooHotFlags.pcduSystemTooHotFlag = true; + } else if (not ctrlCtx.componentAboveUpperLimit) { + tooHotFlags.pcduSystemTooHotFlag = false; } } void ThermalController::ctrlPlPcduBoard() { - thermalComponent = PLPCDU_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::PLPCDU_BOARD; sensors[0].first = sensorTemperatures.tmp1075PlPcdu0.isValid(); sensors[0].second = sensorTemperatures.tmp1075PlPcdu0.value; sensors[1].first = sensorTemperatures.tmp1075PlPcdu1.isValid(); @@ -1370,11 +1387,11 @@ void ThermalController::ctrlPlPcduBoard() { numSensors = 4; HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, plPcduBoardLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); + tooHotHandler(objects::PLPCDU_HANDLER, tooHotFlags.eBandTooHotFlag); } void ThermalController::ctrlPlocMissionBoard() { - thermalComponent = PLOCMISSION_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::PLOCMISSION_BOARD; sensors[0].first = sensorTemperatures.plocHeatspreader.isValid(); sensors[0].second = sensorTemperatures.plocHeatspreader.value; sensors[1].first = sensorTemperatures.plocMissionboard.isValid(); @@ -1385,11 +1402,11 @@ void ThermalController::ctrlPlocMissionBoard() { HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, plocMissionBoardLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, plocTooHotFlag); + tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, tooHotFlags.plocTooHotFlag); } void ThermalController::ctrlPlocProcessingBoard() { - thermalComponent = PLOCPROCESSING_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::PLOCPROCESSING_BOARD; sensors[0].first = sensorTemperatures.plocMissionboard.isValid(); sensors[0].second = sensorTemperatures.plocMissionboard.value; sensors[1].first = sensorTemperatures.plocHeatspreader.isValid(); @@ -1400,11 +1417,11 @@ void ThermalController::ctrlPlocProcessingBoard() { HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, plocProcessingBoardLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, plocTooHotFlag); + tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, tooHotFlags.plocTooHotFlag); } void ThermalController::ctrlDac() { - thermalComponent = DAC; + ctrlCtx.thermalComponent = tcsCtrl::DAC; sensors[0].first = sensorTemperatures.dacHeatspreader.isValid(); sensors[0].second = sensorTemperatures.dacHeatspreader.value; sensors[1].first = sensorTemperatures.plocMissionboard.isValid(); @@ -1414,11 +1431,11 @@ void ThermalController::ctrlDac() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, dacLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); + tooHotHandler(objects::PLPCDU_HANDLER, tooHotFlags.eBandTooHotFlag); } void ThermalController::ctrlCameraBody() { - thermalComponent = CAMERA; + ctrlCtx.thermalComponent = tcsCtrl::CAMERA; sensors[0].first = sensorTemperatures.payload4kCamera.isValid(); sensors[0].second = sensorTemperatures.payload4kCamera.value; sensors[1].first = sensorTemperatures.dro.isValid(); @@ -1428,7 +1445,7 @@ void ThermalController::ctrlCameraBody() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_4_CAMERA, heater::HEATER_6_DRO, cameraLimits); ctrlComponentTemperature(htrCtx); - if (componentAboveUpperLimit and not camTooHotOneShotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not tooHotFlags.camTooHotOneShotFlag) { triggerEvent(tcsCtrl::CAMERA_OVERHEATING, tempFloatToU32()); CommandMessage msg; HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HealthState::FAULTY); @@ -1437,14 +1454,14 @@ void ThermalController::ctrlCameraBody() { sif::error << "ThermalController::ctrlCameraBody(): Sending health message failed" << std::endl; } - camTooHotOneShotFlag = true; - } else if (not componentAboveUpperLimit) { - camTooHotOneShotFlag = false; + tooHotFlags.camTooHotOneShotFlag = true; + } else if (not ctrlCtx.componentAboveUpperLimit) { + tooHotFlags.camTooHotOneShotFlag = false; } } void ThermalController::ctrlDro() { - thermalComponent = DRO; + ctrlCtx.thermalComponent = tcsCtrl::DRO; sensors[0].first = sensorTemperatures.dro.isValid(); sensors[0].second = sensorTemperatures.dro.value; sensors[1].first = sensorTemperatures.payload4kCamera.isValid(); @@ -1454,11 +1471,11 @@ void ThermalController::ctrlDro() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, droLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); + tooHotHandler(objects::PLPCDU_HANDLER, tooHotFlags.eBandTooHotFlag); } void ThermalController::ctrlX8() { - thermalComponent = X8; + ctrlCtx.thermalComponent = tcsCtrl::X8; sensors[0].first = sensorTemperatures.x8.isValid(); sensors[0].second = sensorTemperatures.x8.value; sensors[1].first = sensorTemperatures.hpa.isValid(); @@ -1468,11 +1485,11 @@ void ThermalController::ctrlX8() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, x8Limits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); + tooHotHandler(objects::PLPCDU_HANDLER, tooHotFlags.eBandTooHotFlag); } void ThermalController::ctrlTx() { - thermalComponent = TX; + ctrlCtx.thermalComponent = tcsCtrl::TX; sensors[0].first = sensorTemperatures.eBandTx.isValid(); sensors[0].second = sensorTemperatures.eBandTx.value; sensors[1].first = sensorTemperatures.x8.isValid(); @@ -1482,11 +1499,11 @@ void ThermalController::ctrlTx() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, txLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); + tooHotHandler(objects::PLPCDU_HANDLER, tooHotFlags.eBandTooHotFlag); } void ThermalController::ctrlMpa() { - thermalComponent = MPA; + ctrlCtx.thermalComponent = tcsCtrl::MPA; sensors[0].first = sensorTemperatures.mpa.isValid(); sensors[0].second = sensorTemperatures.mpa.value; sensors[1].first = sensorTemperatures.hpa.isValid(); @@ -1496,11 +1513,11 @@ void ThermalController::ctrlMpa() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, mpaLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); + tooHotHandler(objects::PLPCDU_HANDLER, tooHotFlags.eBandTooHotFlag); } void ThermalController::ctrlHpa() { - thermalComponent = HPA; + ctrlCtx.thermalComponent = tcsCtrl::HPA; sensors[0].first = sensorTemperatures.hpa.isValid(); sensors[0].second = sensorTemperatures.hpa.value; sensors[1].first = sensorTemperatures.x8.isValid(); @@ -1510,11 +1527,11 @@ void ThermalController::ctrlHpa() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, hpaLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); + tooHotHandler(objects::PLPCDU_HANDLER, tooHotFlags.eBandTooHotFlag); } void ThermalController::ctrlScexBoard() { - thermalComponent = SCEX_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::SCEX_BOARD; sensors[0].first = sensorTemperatures.scex.isValid(); sensors[0].second = sensorTemperatures.scex.value; sensors[1].first = sensorTemperatures.x8.isValid(); @@ -1524,10 +1541,11 @@ void ThermalController::ctrlScexBoard() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_5_STR, scexBoardLimits); ctrlComponentTemperature(htrCtx); - tooHotHandlerWhichClearsOneShotFlag(objects::SCEX, scexTooHotFlag); + tooHotHandlerWhichClearsOneShotFlag(objects::SCEX, tooHotFlags.scexTooHotFlag); } -void ThermalController::performThermalModuleCtrl(const HeaterSwitchStates& heaterSwitchStates) { +void ThermalController::performThermalModuleCtrl( + const tcsCtrl::HeaterSwitchStates& heaterSwitchStates) { ctrlAcsBoard(); ctrlMgt(); ctrlRw(); @@ -1544,11 +1562,11 @@ void ThermalController::performThermalModuleCtrl(const HeaterSwitchStates& heate // Payload components std::array plocInAllowedRange{}; ctrlPlocMissionBoard(); - plocInAllowedRange.at(0) = not componentAboveUpperLimit; + plocInAllowedRange.at(0) = not ctrlCtx.componentAboveUpperLimit; ctrlPlocProcessingBoard(); - plocInAllowedRange.at(1) = not componentAboveUpperLimit; + plocInAllowedRange.at(1) = not ctrlCtx.componentAboveUpperLimit; - if (plocTooHotFlag) { + if (tooHotFlags.plocTooHotFlag) { bool clearFlag = true; for (const auto& inRange : plocInAllowedRange) { if (not inRange) { @@ -1556,7 +1574,7 @@ void ThermalController::performThermalModuleCtrl(const HeaterSwitchStates& heate } } if (clearFlag) { - plocTooHotFlag = false; + tooHotFlags.plocTooHotFlag = false; } } ctrlCameraBody(); @@ -1565,21 +1583,21 @@ void ThermalController::performThermalModuleCtrl(const HeaterSwitchStates& heate // E-Band std::array eBandInAllowedRange{}; ctrlPlPcduBoard(); - eBandInAllowedRange.at(0) = not componentAboveUpperLimit; + eBandInAllowedRange.at(0) = not ctrlCtx.componentAboveUpperLimit; ctrlDac(); - eBandInAllowedRange.at(1) = not componentAboveUpperLimit; + eBandInAllowedRange.at(1) = not ctrlCtx.componentAboveUpperLimit; ctrlDro(); - eBandInAllowedRange.at(2) = not componentAboveUpperLimit; + eBandInAllowedRange.at(2) = not ctrlCtx.componentAboveUpperLimit; ctrlX8(); - eBandInAllowedRange.at(3) = not componentAboveUpperLimit; + eBandInAllowedRange.at(3) = not ctrlCtx.componentAboveUpperLimit; ctrlHpa(); - eBandInAllowedRange.at(4) = not componentAboveUpperLimit; + eBandInAllowedRange.at(4) = not ctrlCtx.componentAboveUpperLimit; ctrlTx(); - eBandInAllowedRange.at(5) = not componentAboveUpperLimit; + eBandInAllowedRange.at(5) = not ctrlCtx.componentAboveUpperLimit; ctrlMpa(); - eBandInAllowedRange.at(6) = not componentAboveUpperLimit; + eBandInAllowedRange.at(6) = not ctrlCtx.componentAboveUpperLimit; - if (eBandTooHotFlag) { + if (tooHotFlags.eBandTooHotFlag) { bool clearFlag = true; for (const auto& inRange : eBandInAllowedRange) { if (not inRange) { @@ -1587,7 +1605,7 @@ void ThermalController::performThermalModuleCtrl(const HeaterSwitchStates& heate } } if (clearFlag) { - eBandTooHotFlag = false; + tooHotFlags.eBandTooHotFlag = false; } } } @@ -1603,8 +1621,8 @@ void ThermalController::ctrlComponentTemperature(HeaterContext& htrCtx) { // are blind.. if (chooseHeater(htrCtx.switchNr, htrCtx.redSwitchNr)) { if (heaterCtrlAllowed() and - (heaterHandler.getSwitchState(htrCtx.switchNr) == HeaterHandler::SwitchState::ON)) { - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + (heaterHandler.getSwitchState(htrCtx.switchNr) == heater::SwitchState::ON)) { + heaterSwitchHelper(htrCtx.switchNr, heater::SwitchState::OFF, ctrlCtx.thermalComponent); } } } @@ -1615,21 +1633,21 @@ bool ThermalController::selectAndReadSensorTemp(HeaterContext& htrCtx) { if (sensors[i].first and sensors[i].second != INVALID_TEMPERATURE and sensors[i].second > SANITY_LIMIT_LOWER_TEMP and sensors[i].second < SANITY_LIMIT_UPPER_TEMP) { - sensorTemp = sensors[i].second; - currentSensorIndex = i; - thermalStates[thermalComponent].errorCounter = 0; + ctrlCtx.sensorTemp = sensors[i].second; + ctrlCtx.currentSensorIndex = i; + thermalStates[ctrlCtx.thermalComponent].errorCounter = 0; return true; } } - thermalStates[thermalComponent].errorCounter++; - if (thermalComponent != RW and thermalComponent != ACS_BOARD) { - if (thermalStates[thermalComponent].errorCounter <= 3) { - triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, thermalComponent); + thermalStates[ctrlCtx.thermalComponent].errorCounter++; + if (ctrlCtx.thermalComponent != tcsCtrl::RW and ctrlCtx.thermalComponent != tcsCtrl::ACS_BOARD) { + if (thermalStates[ctrlCtx.thermalComponent].errorCounter <= 3) { + triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, ctrlCtx.thermalComponent); } } else { - if (thermalStates[thermalComponent].errorCounter <= 8) { - triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, thermalComponent); + if (thermalStates[ctrlCtx.thermalComponent].errorCounter <= 8) { + triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, ctrlCtx.thermalComponent); } } @@ -1643,7 +1661,7 @@ bool ThermalController::chooseHeater(heater::Switch& switchNr, heater::Switch re if (mainHealth != HasHealthIF::HEALTHY) { if (redHealth == HasHealthIF::HEALTHY) { switchNr = redSwitchNr; - redSwitchNrInUse = true; + ctrlCtx.redSwitchNrInUse = true; } else { heaterAvailable = false; // Special case: Ground might command/do something with the heaters, so prevent spam. @@ -1652,7 +1670,7 @@ bool ThermalController::chooseHeater(heater::Switch& switchNr, heater::Switch re } } } else { - redSwitchNrInUse = false; + ctrlCtx.redSwitchNrInUse = false; } return heaterAvailable; } @@ -1661,23 +1679,23 @@ void ThermalController::heaterCtrlTempTooHighHandler(HeaterContext& htrCtx, cons if (not heaterCtrlAllowed()) { return; } - if (htrCtx.switchState == HeaterHandler::SwitchState::ON) { - sif::info << "TCS: Component " << static_cast(thermalComponent) << " too warm, above " - << whatLimit << ", switching off heater" << std::endl; - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + if (htrCtx.switchState == heater::SwitchState::ON) { + sif::info << "TCS: Component " << static_cast(ctrlCtx.thermalComponent) + << " too warm, above " << whatLimit << ", switching off heater" << std::endl; + heaterSwitchHelper(htrCtx.switchNr, heater::SwitchState::OFF, ctrlCtx.thermalComponent); heaterStates[htrCtx.switchNr].switchTransition = true; - heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::OFF; + heaterStates[htrCtx.switchNr].target = heater::SwitchState::OFF; } - if (heaterHandler.getSwitchState(htrCtx.redSwitchNr) == HeaterHandler::SwitchState::ON) { - heaterSwitchHelper(htrCtx.redSwitchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + if (heaterHandler.getSwitchState(htrCtx.redSwitchNr) == heater::SwitchState::ON) { + heaterSwitchHelper(htrCtx.redSwitchNr, heater::SwitchState::OFF, ctrlCtx.thermalComponent); heaterStates[htrCtx.redSwitchNr].switchTransition = true; - heaterStates[htrCtx.redSwitchNr].target = HeaterHandler::SwitchState::OFF; + heaterStates[htrCtx.redSwitchNr].target = heater::SwitchState::OFF; } } void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { - componentAboveCutOffLimit = false; - componentAboveUpperLimit = false; + ctrlCtx.componentAboveCutOffLimit = false; + ctrlCtx.componentAboveUpperLimit = false; // Stay passive during switch transitions, wait for heater switching to complete. Otherwise, // still check whether components are out of range, which might be important information for the // top level control loop. @@ -1689,35 +1707,38 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { htrCtx.switchState = heaterHandler.getSwitchState(htrCtx.switchNr); // Heater off - if (htrCtx.switchState == HeaterHandler::SwitchState::OFF) { - if (sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) { - sif::info << "TCS: Heater " << static_cast(thermalComponent) << " ON" << std::endl; - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::ON, thermalComponent); + if (htrCtx.switchState == heater::SwitchState::OFF) { + if (ctrlCtx.sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) { + sif::info << "TCS: Heater " << static_cast(ctrlCtx.thermalComponent) << " ON" + << std::endl; + heaterSwitchHelper(htrCtx.switchNr, heater::SwitchState::ON, ctrlCtx.thermalComponent); } else { // Even if heater control is now allowed, we can update the state. - thermalStates[thermalComponent].heating = false; + thermalStates[ctrlCtx.thermalComponent].heating = false; } heaterCtrlCheckUpperLimits(htrCtx); return; } // Heater on - if (htrCtx.switchState == HeaterHandler::SwitchState::ON) { - if (thermalStates[thermalComponent].heating) { + if (htrCtx.switchState == heater::SwitchState::ON) { + if (thermalStates[ctrlCtx.thermalComponent].heating) { // We are already in a heating cycle, so need to check whether heating task is complete. - if (sensorTemp >= htrCtx.tempLimit.opLowerLimit + TEMP_OFFSET and heaterCtrlAllowed()) { - sif::info << "TCS: Heater " << static_cast(thermalComponent) << " OFF" << std::endl; - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + if (ctrlCtx.sensorTemp >= htrCtx.tempLimit.opLowerLimit + TEMP_OFFSET and + heaterCtrlAllowed()) { + sif::info << "TCS: Heater " << static_cast(ctrlCtx.thermalComponent) << " OFF" + << std::endl; + heaterSwitchHelper(htrCtx.switchNr, heater::SwitchState::OFF, ctrlCtx.thermalComponent); heaterStates[htrCtx.switchNr].switchTransition = true; - heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::OFF; + heaterStates[htrCtx.switchNr].target = heater::SwitchState::OFF; } return; } // This can happen if heater is used as alternative heater (no regular heating cycle), so we // should still check the upper limits. bool tooHighHandlerAlreadyCalled = heaterCtrlCheckUpperLimits(htrCtx); - if (sensorTemp >= htrCtx.tempLimit.cutOffLimit) { - componentAboveCutOffLimit = true; + if (ctrlCtx.sensorTemp >= htrCtx.tempLimit.cutOffLimit) { + ctrlCtx.componentAboveCutOffLimit = true; if (not tooHighHandlerAlreadyCalled) { heaterCtrlTempTooHighHandler(htrCtx, "CutOff-Limit"); } @@ -1726,19 +1747,19 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { } bool ThermalController::heaterCtrlCheckUpperLimits(HeaterContext& htrCtx) { - if (sensorTemp >= htrCtx.tempLimit.nopUpperLimit) { - componentAboveUpperLimit = true; + if (ctrlCtx.sensorTemp >= htrCtx.tempLimit.nopUpperLimit) { + ctrlCtx.componentAboveUpperLimit = true; if (htrCtx.doHeaterHandling) { heaterCtrlTempTooHighHandler(htrCtx, "NOP-Limit"); } - overHeatEventToTrigger = ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH; + ctrlCtx.overHeatEventToTrigger = ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH; return true; - } else if (sensorTemp >= htrCtx.tempLimit.opUpperLimit) { - componentAboveUpperLimit = true; + } else if (ctrlCtx.sensorTemp >= htrCtx.tempLimit.opUpperLimit) { + ctrlCtx.componentAboveUpperLimit = true; if (htrCtx.doHeaterHandling) { heaterCtrlTempTooHighHandler(htrCtx, "OP-Limit"); } - overHeatEventToTrigger = ThermalComponentIF::COMPONENT_TEMP_HIGH; + ctrlCtx.overHeatEventToTrigger = ThermalComponentIF::COMPONENT_TEMP_HIGH; return true; } return false; @@ -1749,15 +1770,16 @@ void ThermalController::resetSensorsArray() { validValuePair.first = false; validValuePair.second = INVALID_TEMPERATURE; } - thermalComponent = NONE; + ctrlCtx.thermalComponent = tcsCtrl::NONE; } -void ThermalController::heaterTransitionControl(const HeaterSwitchStates& currentHeaterStates) { +void ThermalController::heaterTransitionControl( + const tcsCtrl::HeaterSwitchStates& currentHeaterStates) { for (unsigned i = 0; i < heater::Switch::NUMBER_OF_SWITCHES; i++) { if (heaterStates[i].switchTransition) { if (currentHeaterStates[i] == heaterStates[i].target) { // Required for max heater period control - if (currentHeaterStates[i] == HeaterHandler::SwitchState::ON) { + if (currentHeaterStates[i] == heater::SwitchState::ON) { heaterStates[i].heaterOnPeriod.setTimeout(MAX_HEATER_ON_DURATIONS[i]); heaterStates[i].heaterOnPeriod.resetTimer(); } else { @@ -1777,13 +1799,14 @@ void ThermalController::heaterTransitionControl(const HeaterSwitchStates& curren } } -void ThermalController::heaterMaxDurationControl(const HeaterSwitchStates& currentHeaterStates) { +void ThermalController::heaterMaxDurationControl( + const tcsCtrl::HeaterSwitchStates& currentHeaterStates) { for (unsigned i = 0; i < heater::Switch::NUMBER_OF_SWITCHES; i++) { - if (currentHeaterStates[i] == HeaterHandler::SwitchState::ON and + if (currentHeaterStates[i] == heater::SwitchState::ON and heaterStates[i].heaterOnPeriod.hasTimedOut()) { heaterStates[i].switchTransition = false; heaterStates[i].heaterSwitchControlCycles = 0; - heaterHandler.switchHeater(static_cast(i), HeaterHandler::SwitchState::OFF); + heaterHandler.switchHeater(static_cast(i), heater::SwitchState::OFF); // The heater might still be one for some thermal components, so cross-check // those components crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(static_cast(i)); @@ -1792,7 +1815,7 @@ void ThermalController::heaterMaxDurationControl(const HeaterSwitchStates& curre } uint32_t ThermalController::tempFloatToU32() const { - auto sensorTempAsFloat = static_cast(sensorTemp); + auto sensorTempAsFloat = static_cast(ctrlCtx.sensorTemp); uint32_t tempRaw = 0; size_t dummyLen = 0; SerializeAdapter::serialize(&sensorTempAsFloat, reinterpret_cast(&tempRaw), &dummyLen, @@ -1811,9 +1834,9 @@ void ThermalController::setMode(Mode_t mode, Submode_t submode) { } bool ThermalController::tooHotHandler(object_id_t object, bool& oneShotFlag) { - if (componentAboveUpperLimit and not oneShotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not oneShotFlag) { // Too hot -> returns true - EventManagerIF::triggerEvent(object, overHeatEventToTrigger, tempFloatToU32()); + EventManagerIF::triggerEvent(object, ctrlCtx.overHeatEventToTrigger, tempFloatToU32()); oneShotFlag = true; return true; } @@ -1833,16 +1856,15 @@ void ThermalController::resetThermalStates() { } } -void ThermalController::heaterSwitchHelper(heater::Switch switchNr, - HeaterHandler::SwitchState targetState, +void ThermalController::heaterSwitchHelper(heater::Switch switchNr, heater::SwitchState targetState, unsigned componentIdx) { timeval currentTime; Clock::getClockMonotonic(¤tTime); - if (targetState == HeaterHandler::SwitchState::ON) { + if (targetState == heater::SwitchState::ON) { heaterHandler.switchHeater(switchNr, targetState); - heaterStates[switchNr].target = HeaterHandler::SwitchState::ON; + heaterStates[switchNr].target = heater::SwitchState::ON; heaterStates[switchNr].switchTransition = true; - thermalStates[componentIdx].sensorIndex = currentSensorIndex; + thermalStates[componentIdx].sensorIndex = ctrlCtx.currentSensorIndex; thermalStates[componentIdx].heaterSwitch = switchNr; thermalStates[componentIdx].heating = true; thermalStates[componentIdx].heaterStartTime = currentTime.tv_sec; @@ -1858,7 +1880,7 @@ void ThermalController::heaterSwitchHelperAllOff() { Clock::getClockMonotonic(¤tTime); size_t idx = 0; for (; idx < heater::Switch::NUMBER_OF_SWITCHES; idx++) { - heaterHandler.switchHeater(static_cast(idx), HeaterHandler::SwitchState::OFF); + heaterHandler.switchHeater(static_cast(idx), heater::SwitchState::OFF); } for (idx = 0; idx < thermalStates.size(); idx++) { thermalStates[idx].heating = false; @@ -1886,7 +1908,7 @@ void ThermalController::crossCheckHeaterStateOfComponentsWhenHeaterGoesOff( void ThermalController::tooHotHandlerWhichClearsOneShotFlag(object_id_t object, bool& oneShotFlag) { // Clear the one shot flag is the component is in acceptable temperature range. - if (not tooHotHandler(object, oneShotFlag) and not componentAboveUpperLimit) { + if (not tooHotHandler(object, oneShotFlag) and not ctrlCtx.componentAboveUpperLimit) { oneShotFlag = false; } } diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 021be00e..04dc211a 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -25,78 +25,6 @@ #include #include -/** - * NOP Limit: Hard limit for device, usually from datasheet. Device damage is possible lif NOP limit - * is exceeded. - * OP Limit: Soft limit. Device should be switched off or TCS controller should take action if the - * limit is exceeded to avoid reaching NOP limit - */ -struct TempLimits { - TempLimits(float nopLowerLimit, float opLowerLimit, float cutOffLimit, float opUpperLimit, - float nopUpperLimit) - : opLowerLimit(opLowerLimit), - opUpperLimit(opUpperLimit), - cutOffLimit(cutOffLimit), - nopLowerLimit(nopLowerLimit), - nopUpperLimit(nopUpperLimit) {} - float opLowerLimit; - float opUpperLimit; - float cutOffLimit; - float nopLowerLimit; - float nopUpperLimit; -}; - -struct ThermalState { - uint8_t errorCounter; - // Which sensor is used for this component? - uint8_t sensorIndex = 0; - // Is heating on for that thermal module? - bool heating = false; - // Which switch is being used for heating the component - heater::Switch heaterSwitch = heater::Switch::NUMBER_OF_SWITCHES; - // Heater start time and end times as UNIX seconds. Please note that these times will be updated - // when a switch command is sent, with no guarantess that the heater actually went on. - uint32_t heaterStartTime = 0; - uint32_t heaterEndTime = 0; -}; - -struct HeaterState { - bool switchTransition; - HeaterHandler::SwitchState target; - uint8_t heaterSwitchControlCycles; - Countdown heaterOnPeriod; -}; - -using HeaterSwitchStates = std::array; - -enum ThermalComponents : uint8_t { - NONE = 0, - ACS_BOARD = 1, - MGT = 2, - RW = 3, - STR = 4, - IF_BOARD = 5, - TCS_BOARD = 6, - OBC = 7, - OBCIF_BOARD = 8, - SBAND_TRANSCEIVER = 9, - PCDUP60_BOARD = 10, - PCDUACU = 11, - PCDUPDU = 12, - PLPCDU_BOARD = 13, - PLOCMISSION_BOARD = 14, - PLOCPROCESSING_BOARD = 15, - DAC = 16, - CAMERA = 17, - DRO = 18, - X8 = 19, - HPA = 20, - TX = 21, - MPA = 22, - SCEX_BOARD = 23, - NUM_ENTRIES -}; - class ThermalController : public ExtendedControllerBase { public: static constexpr uint8_t SUBMODE_NO_HEATER_CTRL = 1; @@ -135,16 +63,16 @@ class ThermalController : public ExtendedControllerBase { struct HeaterContext { public: HeaterContext(heater::Switch switchNr, heater::Switch redundantSwitchNr, - const TempLimits& tempLimit) + const tcsCtrl::TempLimits& tempLimit) : switchNr(switchNr), redSwitchNr(redundantSwitchNr), tempLimit(tempLimit) {} bool doHeaterHandling = true; heater::Switch switchNr; - HeaterHandler::SwitchState switchState = HeaterHandler::SwitchState::OFF; + heater::SwitchState switchState = heater::SwitchState::OFF; heater::Switch redSwitchNr; - const TempLimits& tempLimit; + const tcsCtrl::TempLimits& tempLimit; }; - void performThermalModuleCtrl(const HeaterSwitchStates& heaterSwitchStates); + void performThermalModuleCtrl(const tcsCtrl::HeaterSwitchStates& heaterSwitchStates); ReturnValue_t handleCommandMessage(CommandMessage* message) override; void performControlOperation() override; ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, @@ -171,8 +99,7 @@ class ThermalController : public ExtendedControllerBase { tcsCtrl::SusTemperatures susTemperatures; tcsCtrl::DeviceTemperatures deviceTemperatures; tcsCtrl::HeaterInfo heaterInfo; - lp_vec_t currentVecPdu2 = - lp_vec_t(gp_id_t(objects::PDU2_HANDLER, PDU::pool::PDU_CURRENTS)); + tcsCtrl::TcsCtrlInfo tcsCtrlInfo; DeviceHandlerThermalSet imtqThermalSet; @@ -252,58 +179,66 @@ class ThermalController : public ExtendedControllerBase { lp_var_t tempMgm2 = lp_var_t(objects::MGM_2_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS); lp_var_t tempAdcPayloadPcdu = lp_var_t(objects::PLPCDU_HANDLER, plpcdu::TEMP); + lp_vec_t currentVecPdu2 = + lp_vec_t(gp_id_t(objects::PDU2_HANDLER, PDU::pool::PDU_CURRENTS)); // TempLimits - TempLimits acsBoardLimits = TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0); - TempLimits mgtLimits = TempLimits(-40.0, -40.0, 65.0, 70.0, 70.0); - TempLimits rwLimits = TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0); - TempLimits strLimits = TempLimits(-30.0, -20.0, 65.0, 70.0, 80.0); - TempLimits ifBoardLimits = TempLimits(-65.0, -40.0, 80.0, 85.0, 150.0); - TempLimits tcsBoardLimits = TempLimits(-60.0, -40.0, 80.0, 85.0, 130.0); - TempLimits obcLimits = TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0); - TempLimits obcIfBoardLimits = TempLimits(-65.0, -40.0, 80.0, 85.0, 125.0); - TempLimits sBandTransceiverLimits = TempLimits(-40.0, -25.0, 35.0, 40.0, 65.0); - TempLimits pcduP60BoardLimits = TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0); - TempLimits pcduAcuLimits = TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0); - TempLimits pcduPduLimits = TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0); - TempLimits plPcduBoardLimits = TempLimits(-55.0, -40.0, 80.0, 85.0, 125.0); - TempLimits plocMissionBoardLimits = TempLimits(-30.0, -10.0, 40.0, 45.0, 60); - TempLimits plocProcessingBoardLimits = TempLimits(-30.0, -10.0, 40.0, 45.0, 60.0); - TempLimits dacLimits = TempLimits(-65.0, -40.0, 113.0, 118.0, 150.0); - TempLimits cameraLimits = TempLimits(-40.0, -30.0, 60.0, 65.0, 85.0); - TempLimits droLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); - TempLimits x8Limits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); - TempLimits hpaLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); - TempLimits txLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); - TempLimits mpaLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); - TempLimits scexBoardLimits = TempLimits(-60.0, -40.0, 80.0, 85.0, 150.0); + tcsCtrl::TempLimits acsBoardLimits = tcsCtrl::TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0); + tcsCtrl::TempLimits mgtLimits = tcsCtrl::TempLimits(-40.0, -40.0, 65.0, 70.0, 70.0); + tcsCtrl::TempLimits rwLimits = tcsCtrl::TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0); + tcsCtrl::TempLimits strLimits = tcsCtrl::TempLimits(-30.0, -20.0, 65.0, 70.0, 80.0); + tcsCtrl::TempLimits ifBoardLimits = tcsCtrl::TempLimits(-65.0, -40.0, 80.0, 85.0, 150.0); + tcsCtrl::TempLimits tcsBoardLimits = tcsCtrl::TempLimits(-60.0, -40.0, 80.0, 85.0, 130.0); + tcsCtrl::TempLimits obcLimits = tcsCtrl::TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0); + tcsCtrl::TempLimits obcIfBoardLimits = tcsCtrl::TempLimits(-65.0, -40.0, 80.0, 85.0, 125.0); + tcsCtrl::TempLimits sBandTransceiverLimits = tcsCtrl::TempLimits(-40.0, -25.0, 35.0, 40.0, 65.0); + tcsCtrl::TempLimits pcduP60BoardLimits = tcsCtrl::TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0); + tcsCtrl::TempLimits pcduAcuLimits = tcsCtrl::TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0); + tcsCtrl::TempLimits pcduPduLimits = tcsCtrl::TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0); + tcsCtrl::TempLimits plPcduBoardLimits = tcsCtrl::TempLimits(-55.0, -40.0, 80.0, 85.0, 125.0); + tcsCtrl::TempLimits plocMissionBoardLimits = tcsCtrl::TempLimits(-30.0, -10.0, 40.0, 45.0, 60); + tcsCtrl::TempLimits plocProcessingBoardLimits = + tcsCtrl::TempLimits(-30.0, -10.0, 40.0, 45.0, 60.0); + tcsCtrl::TempLimits dacLimits = tcsCtrl::TempLimits(-65.0, -40.0, 113.0, 118.0, 150.0); + tcsCtrl::TempLimits cameraLimits = tcsCtrl::TempLimits(-40.0, -30.0, 60.0, 65.0, 85.0); + tcsCtrl::TempLimits droLimits = tcsCtrl::TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); + tcsCtrl::TempLimits x8Limits = tcsCtrl::TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); + tcsCtrl::TempLimits hpaLimits = tcsCtrl::TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); + tcsCtrl::TempLimits txLimits = tcsCtrl::TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); + tcsCtrl::TempLimits mpaLimits = tcsCtrl::TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); + tcsCtrl::TempLimits scexBoardLimits = tcsCtrl::TempLimits(-60.0, -40.0, 80.0, 85.0, 150.0); - double sensorTemp = INVALID_TEMPERATURE; - uint8_t currentSensorIndex = 0; - ThermalComponents thermalComponent = NONE; - bool redSwitchNrInUse = false; + struct CtrlContext { + double sensorTemp = INVALID_TEMPERATURE; + uint8_t currentSensorIndex = 0; + tcsCtrl::ThermalComponents thermalComponent = tcsCtrl::NONE; + bool redSwitchNrInUse = false; + bool componentAboveCutOffLimit = false; + bool componentAboveUpperLimit = false; + Event overHeatEventToTrigger; + } ctrlCtx; MessageQueueId_t camId = MessageQueueIF::NO_QUEUE; - bool componentAboveCutOffLimit = false; - bool componentAboveUpperLimit = false; - Event overHeatEventToTrigger; - bool eBandTooHotFlag = false; - bool camTooHotOneShotFlag = false; - bool scexTooHotFlag = false; - bool plocTooHotFlag = false; - bool pcduSystemTooHotFlag = false; - bool syrlinksTooHotFlag = false; - bool obcTooHotFlag = false; - bool mgtTooHotFlag = false; - bool strTooHotFlag = false; - bool rwTooHotFlag = false; + + struct TooHotFlags { + bool eBandTooHotFlag = false; + bool camTooHotOneShotFlag = false; + bool scexTooHotFlag = false; + bool plocTooHotFlag = false; + bool pcduSystemTooHotFlag = false; + bool syrlinksTooHotFlag = false; + bool obcTooHotFlag = false; + bool mgtTooHotFlag = false; + bool strTooHotFlag = false; + bool rwTooHotFlag = false; + } tooHotFlags; bool transitionWhenHeatersOff = false; uint32_t transitionWhenHeatersOffCycles = 0; Mode_t targetMode = MODE_OFF; Submode_t targetSubmode = SUBMODE_NONE; uint32_t cycles = 0; - std::array thermalStates{}; - std::array heaterStates{}; + std::array thermalStates{}; + std::array heaterStates{}; // Initial delay to make sure all pool variables have been initialized their owners. // Also, wait for system initialization to complete. @@ -324,6 +259,12 @@ class ThermalController : public ExtendedControllerBase { PoolEntry heaterSwitchStates = PoolEntry(heater::NUMBER_OF_SWITCHES); PoolEntry heaterCurrent = PoolEntry(); + PoolEntry tcsCtrlHeaterOn = PoolEntry(tcsCtrl::NUM_THERMAL_COMPONENTS); + PoolEntry tcsCtrlSensorIdx = PoolEntry(tcsCtrl::NUM_THERMAL_COMPONENTS); + PoolEntry tcsCtrlHeaterIdx = PoolEntry(tcsCtrl::NUM_THERMAL_COMPONENTS); + PoolEntry tcsCtrlStartTimes = PoolEntry(tcsCtrl::NUM_THERMAL_COMPONENTS); + PoolEntry tcsCtrlEndTimes = PoolEntry(tcsCtrl::NUM_THERMAL_COMPONENTS); + static constexpr dur_millis_t MUTEX_TIMEOUT = 50; void startTransition(Mode_t mode, Submode_t submode) override; @@ -345,7 +286,7 @@ class ThermalController : public ExtendedControllerBase { bool selectAndReadSensorTemp(HeaterContext& htrCtx); void heaterSwitchHelperAllOff(); - void heaterSwitchHelper(heater::Switch switchNr, HeaterHandler::SwitchState state, + void heaterSwitchHelper(heater::Switch switchNr, heater::SwitchState state, unsigned componentIdx); void ctrlAcsBoard(); @@ -378,14 +319,14 @@ class ThermalController : public ExtendedControllerBase { * of tracking transition and capturing their completion. * @param currentHeaterStates */ - void heaterTransitionControl(const HeaterSwitchStates& currentHeaterStates); + void heaterTransitionControl(const tcsCtrl::HeaterSwitchStates& currentHeaterStates); /** * Control tasks to prevent heaters being on for prolonged periods. Ideally, this * should never happen, but this task prevents bugs from causing heaters to stay on * for a long time, which draws a lot of power. * @param currentHeaterStates */ - void heaterMaxDurationControl(const HeaterSwitchStates& currentHeaterStates); + void heaterMaxDurationControl(const tcsCtrl::HeaterSwitchStates& currentHeaterStates); void crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(heater::Switch switchIdx); void setMode(Mode_t mode, Submode_t submode); uint32_t tempFloatToU32() const; diff --git a/mission/controller/tcsDefs.h b/mission/controller/tcsDefs.h index e2efd98c..fda2a546 100644 --- a/mission/controller/tcsDefs.h +++ b/mission/controller/tcsDefs.h @@ -9,6 +9,84 @@ namespace tcsCtrl { +/** + * NOP Limit: Hard limit for device, usually from datasheet. Device damage is possible lif NOP limit + * is exceeded. + * OP Limit: Soft limit. Device should be switched off or TCS controller should take action if the + * limit is exceeded to avoid reaching NOP limit + */ +struct TempLimits { + TempLimits(float nopLowerLimit, float opLowerLimit, float cutOffLimit, float opUpperLimit, + float nopUpperLimit) + : opLowerLimit(opLowerLimit), + opUpperLimit(opUpperLimit), + cutOffLimit(cutOffLimit), + nopLowerLimit(nopLowerLimit), + nopUpperLimit(nopUpperLimit) {} + float opLowerLimit; + float opUpperLimit; + float cutOffLimit; + float nopLowerLimit; + float nopUpperLimit; +}; + +/** + * Abstraction for the state of a single thermal component + */ +struct ThermalState { + uint8_t errorCounter; + // Which sensor is used for this component? + uint8_t sensorIndex = 0; + // Is heating on for that thermal module? + bool heating = false; + // Which switch is being used for heating the component + heater::Switch heaterSwitch = heater::Switch::NUMBER_OF_SWITCHES; + // Heater start time and end times as UNIX seconds. Please note that these times will be updated + // when a switch command is sent, with no guarantess that the heater actually went on. + uint32_t heaterStartTime = 0; + uint32_t heaterEndTime = 0; +}; + +/** + * Abstraction for the state of a single heater. + */ +struct HeaterState { + bool switchTransition; + heater::SwitchState target; + uint8_t heaterSwitchControlCycles; + Countdown heaterOnPeriod; +}; + +using HeaterSwitchStates = std::array; + +enum ThermalComponents : uint8_t { + NONE = 0, + ACS_BOARD = 1, + MGT = 2, + RW = 3, + STR = 4, + IF_BOARD = 5, + TCS_BOARD = 6, + OBC = 7, + OBCIF_BOARD = 8, + SBAND_TRANSCEIVER = 9, + PCDUP60_BOARD = 10, + PCDUACU = 11, + PCDUPDU = 12, + PLPCDU_BOARD = 13, + PLOCMISSION_BOARD = 14, + PLOCPROCESSING_BOARD = 15, + DAC = 16, + CAMERA = 17, + DRO = 18, + X8 = 19, + HPA = 20, + TX = 21, + MPA = 22, + SCEX_BOARD = 23, + NUM_THERMAL_COMPONENTS +}; + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::TCS_CONTROLLER; static constexpr Event NO_VALID_SENSOR_TEMPERATURE = MAKE_EVENT(0, severity::MEDIUM); static constexpr Event NO_HEALTHY_HEATER_AVAILABLE = MAKE_EVENT(1, severity::MEDIUM); @@ -25,6 +103,7 @@ enum SetId : uint32_t { SUS_TEMPERATURES = 2, COMPONENT_TEMPERATURES = 3, HEATER_SET = 4, + TCS_CTRL_INFO = 5 }; enum PoolIds : lp_id_t { @@ -92,7 +171,13 @@ enum PoolIds : lp_id_t { TEMP_ADC_PAYLOAD_PCDU, HEATER_SWITCH_LIST, - HEATER_CURRENT + HEATER_CURRENT, + + HEATER_ON_FOR_COMPONENT_VEC, + SENSOR_USED_FOR_TCS_CTRL, + HEATER_IDX_USED_FOR_TCS_CTRL, + HEATER_START_TIME, + HEATER_END_TIME }; static const uint8_t ENTRIES_SENSOR_TEMPERATURE_SET = 25; @@ -232,6 +317,29 @@ class HeaterInfo : public StaticLocalDataSet<3> { lp_var_t heaterCurrent = lp_var_t(sid.objectId, PoolIds::HEATER_CURRENT, this); }; +class TcsCtrlInfo : public StaticLocalDataSet<6> { + public: + explicit TcsCtrlInfo(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, TCS_CTRL_INFO) {} + + explicit TcsCtrlInfo(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, TCS_CTRL_INFO)) {} + + lp_vec_t heatingOnVec = + lp_vec_t( + sid.objectId, PoolIds::HEATER_ON_FOR_COMPONENT_VEC, this); + lp_vec_t sensorIdxUsedForTcsCtrl = + lp_vec_t(sid.objectId, + PoolIds::SENSOR_USED_FOR_TCS_CTRL, this); + lp_vec_t heaterSwitchIdx = + lp_vec_t( + sid.objectId, PoolIds::HEATER_IDX_USED_FOR_TCS_CTRL, this); + lp_vec_t heaterStartTimes = + lp_vec_t(sid.objectId, PoolIds::HEATER_START_TIME, + this); + lp_vec_t heaterEndTimes = + lp_vec_t(sid.objectId, PoolIds::HEATER_END_TIME, + this); +}; + } // namespace tcsCtrl #endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_THERMALCONTROLLERDEFINITIONS_H_ */ diff --git a/mission/tcs/HeaterHandler.cpp b/mission/tcs/HeaterHandler.cpp index 8416a7f7..c97a404a 100644 --- a/mission/tcs/HeaterHandler.cpp +++ b/mission/tcs/HeaterHandler.cpp @@ -97,7 +97,7 @@ ReturnValue_t HeaterHandler::initialize() { ReturnValue_t HeaterHandler::initializeHeaterMap() { for (power::Switch_t switchNr = 0; switchNr < heater::NUMBER_OF_SWITCHES; switchNr++) { - heaterVec.push_back(HeaterWrapper(helper.heaters[switchNr], SwitchState::OFF)); + heaterVec.push_back(HeaterWrapper(helper.heaters[switchNr], heater::SwitchState::OFF)); } return returnvalue::OK; } @@ -214,7 +214,7 @@ ReturnValue_t HeaterHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t o void HeaterHandler::handleSwitchHandling() { for (uint8_t idx = 0; idx < heater::NUMBER_OF_SWITCHES; idx++) { auto health = heaterVec[idx].healthDevice->getHealth(); - if (heaterVec[idx].switchState == SwitchState::ON) { + if (heaterVec[idx].switchState == heater::SwitchState::ON) { // If a heater is still on but the device was marked faulty by the operator, the SW // will shut down the heater if (health == HasHealthIF::FAULTY or health == HasHealthIF::PERMANENT_FAULTY) { @@ -277,7 +277,7 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) { triggerEvent(HEATER_WENT_ON, heaterIdx, 0); { MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); - heater.switchState = ON; + heater.switchState = heater::SwitchState::ON; } EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO, MODE_ON, 0); @@ -326,10 +326,10 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switch heaterIdx) { } if (result == returnvalue::OK) { // Check whether switch is already off - if (getSwitchState(heaterIdx) == SwitchState::ON) { + if (getSwitchState(heaterIdx) == heater::SwitchState::ON) { { MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); - heater.switchState = OFF; + heater.switchState = heater::SwitchState::OFF; } triggerEvent(HEATER_WENT_OFF, heaterIdx, 0); } else { @@ -356,15 +356,15 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switch heaterIdx) { heater.cmdActive = false; } -HeaterHandler::SwitchState HeaterHandler::getSwitchState(heater::Switch switchNr) const { +heater::SwitchState HeaterHandler::getSwitchState(heater::Switch switchNr) const { MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); return heaterVec.at(switchNr).switchState; } -ReturnValue_t HeaterHandler::switchHeater(heater::Switch heater, SwitchState switchState) { - if (switchState == SwitchState::ON) { +ReturnValue_t HeaterHandler::switchHeater(heater::Switch heater, heater::SwitchState switchState) { + if (switchState == heater::SwitchState::ON) { return sendSwitchCommand(heater, PowerSwitchIF::SWITCH_ON); - } else if (switchState == SwitchState::OFF) { + } else if (switchState == heater::SwitchState::OFF) { return sendSwitchCommand(heater, PowerSwitchIF::SWITCH_OFF); } return returnvalue::FAILED; @@ -373,10 +373,10 @@ ReturnValue_t HeaterHandler::switchHeater(heater::Switch heater, SwitchState swi void HeaterHandler::announceMode(bool recursive) { triggerEvent(MODE_INFO, mode, submode); - std::array states; + std::array states; getAllSwitchStates(states); for (unsigned idx = 0; idx < helper.heaters.size(); idx++) { - if (states[idx] == ON) { + if (states[idx] == heater::SwitchState::ON) { EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_ON, 0); } else { EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_OFF, @@ -405,7 +405,7 @@ ModeTreeChildIF& HeaterHandler::getModeTreeChildIF() { return *this; } object_id_t HeaterHandler::getObjectId() const { return SystemObject::getObjectId(); } -ReturnValue_t HeaterHandler::getAllSwitchStates(std::array& statesBuf) { +ReturnValue_t HeaterHandler::getAllSwitchStates(std::array& statesBuf) { { MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); if (mg.getLockResult() != returnvalue::OK) { @@ -440,7 +440,7 @@ ReturnValue_t HeaterHandler::getSwitchState(uint8_t switchNr) const { if (switchNr > 7) { return returnvalue::FAILED; } - if (getSwitchState(static_cast(switchNr)) == SwitchState::ON) { + if (getSwitchState(static_cast(switchNr)) == heater::SwitchState::ON) { return PowerSwitchIF::SWITCH_ON; } return PowerSwitchIF::SWITCH_OFF; diff --git a/mission/tcs/HeaterHandler.h b/mission/tcs/HeaterHandler.h index 0198ab04..4901c505 100644 --- a/mission/tcs/HeaterHandler.h +++ b/mission/tcs/HeaterHandler.h @@ -60,7 +60,6 @@ class HeaterHandler : public ExecutableObjectIF, static const ReturnValue_t COMMAND_ALREADY_WAITING = MAKE_RETURN_CODE(0xA5); enum CmdSourceParam : uint8_t { INTERNAL = 0, EXTERNAL = 1 }; - enum SwitchState : uint8_t { ON = 1, OFF = 0 }; /** Device command IDs */ static const DeviceCommandId_t SWITCH_HEATER = 0x0; @@ -69,14 +68,14 @@ class HeaterHandler : public ExecutableObjectIF, PowerSwitchIF* mainLineSwitcherObjectId, power::Switch_t mainLineSwitch); ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent) override; - ReturnValue_t getAllSwitchStates(std::array& statesBuf); + ReturnValue_t getAllSwitchStates(std::array& statesBuf); virtual ~HeaterHandler(); protected: enum SwitchAction : uint8_t { SET_SWITCH_OFF, SET_SWITCH_ON, NONE }; - ReturnValue_t switchHeater(heater::Switch heater, SwitchState switchState); + ReturnValue_t switchHeater(heater::Switch heater, heater::SwitchState switchState); HasHealthIF::HealthState getHealth(heater::Switch heater); ReturnValue_t performOperation(uint8_t operationCode = 0) override; @@ -121,14 +120,14 @@ class HeaterHandler : public ExecutableObjectIF, * @param mainSwitchCountdown Sets timeout to wait for main switch being set on. */ struct HeaterWrapper { - HeaterWrapper(HeaterPair pair, SwitchState initState) + HeaterWrapper(HeaterPair pair, heater::SwitchState initState) : healthDevice(pair.first), gpioId(pair.second), switchState(initState) {} HealthDevice* healthDevice = nullptr; gpioId_t gpioId = gpio::NO_GPIO; SwitchAction action = SwitchAction::NONE; MessageQueueId_t replyQueue = MessageQueueIF::NO_QUEUE; bool cmdActive = false; - SwitchState switchState = SwitchState::OFF; + heater::SwitchState switchState = heater::SwitchState::OFF; bool waitMainSwitchOn = false; Countdown mainSwitchCountdown; }; @@ -177,7 +176,7 @@ class HeaterHandler : public ExecutableObjectIF, * @brief Returns the state of a switch (ON - true, or OFF - false). * @param switchNr The number of the switch to check. */ - SwitchState getSwitchState(heater::Switch switchNr) const; + heater::SwitchState getSwitchState(heater::Switch switchNr) const; /** * @brief This function runs commands waiting for execution. diff --git a/mission/tcs/defs.h b/mission/tcs/defs.h index 747dbc6d..e9f59e1a 100644 --- a/mission/tcs/defs.h +++ b/mission/tcs/defs.h @@ -4,6 +4,7 @@ #include namespace heater { + enum Switch : uint8_t { HEATER_0_PLOC_PROC_BRD, HEATER_1_PCDU_PDU, @@ -15,7 +16,10 @@ enum Switch : uint8_t { HEATER_7_S_BAND, NUMBER_OF_SWITCHES }; -} + +enum SwitchState : uint8_t { ON = 1, OFF = 0 }; + +} // namespace heater namespace tcs { From f1ca4b174f540fa4a355db514a5c0be34376ecbc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Jul 2023 23:07:54 +0200 Subject: [PATCH 477/506] comment --- mission/controller/ThermalController.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 0ed8a4d9..6a624c62 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -215,6 +215,7 @@ void ThermalController::performControlOperation() { } heaterTransitionControl(heaterSwitchStateArray); heaterMaxDurationControl(heaterSwitchStateArray); + // This dataset makes the TCS CTRL observable. PoolReadGuard pg(&tcsCtrlInfo); for (uint8_t i = 0; i < thermalStates.size(); i++) { tcsCtrlInfo.heatingOnVec[i] = thermalStates[i].heating; From 351c33e5043341daafff5ebac792794c19728404 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 7 Jul 2023 10:55:13 +0200 Subject: [PATCH 478/506] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 08f904a9..696f7293 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -28,6 +28,10 @@ will consitute of a breaking change warranting a new major release: - TMP1075: Set dataset invalid on shutdown explicitely - Small fixes for TMP1075 FDIR: Use strange and missed reply counters. +# Added + +- Two events for heaters being commanded ON and OFF by the TCS controller + # [v6.0.0] 2023-07-02 - `q7s-package` version v3.2.0 From 377cc64d0fa4994b70ad3af96ce45fadfed84566 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 7 Jul 2023 10:55:39 +0200 Subject: [PATCH 479/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 5785bbd0..069f84d2 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 5785bbd0ccb045e072f347a5ff2265c610d3872c +Subproject commit 069f84d2207c03e8d334cfce3f7dd530babe17b0 From 2bad2a142b1c0a72223b81fde145765c1e77a503 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 7 Jul 2023 10:58:42 +0200 Subject: [PATCH 480/506] changelog --- CHANGELOG.md | 2 ++ tmtc | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 40622385..d88cf5ff 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,6 +22,8 @@ will consitute of a breaking change warranting a new major release: equal or above v4. However, this software version is compatible to both v3 and v4 of the firmware. - The firmware version variables are global statics inititalized early during the program runtime now. This makes it possible to check the firmware version earlier. +- The TCS controller will now always command heaters OFF when being blind for thermal + components (no sensors available), irrespective of current switch state. ## Fixed diff --git a/tmtc b/tmtc index 6d5bde40..069f84d2 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 6d5bde40db69534a8cea923f96145bdeb62b79af +Subproject commit 069f84d2207c03e8d334cfce3f7dd530babe17b0 From 6bf9fd5969c5f5358ca11423732c4089d2664bc1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 7 Jul 2023 11:51:11 +0200 Subject: [PATCH 481/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5788400d..e70d4d49 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -43,6 +43,8 @@ will consitute of a breaking change warranting a new major release: - Two events for heaters being commanded ON and OFF by the TCS controller - Upper limit for burn time of TCS heaters. Currently set to 1 hour for each heater. +- TCS controller is now observable by introducing a new HK dataset which exposes some internal + fields related to TCS control. # [v6.0.0] 2023-07-02 From ff7d0b3989d5cc0014bbcaa549227625ae035b10 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 7 Jul 2023 11:57:00 +0200 Subject: [PATCH 482/506] compile fixes --- mission/controller/ThermalController.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index f0cf3bd2..be88b760 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1693,7 +1693,7 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { if (sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) { sif::info << "TCS: Heater " << static_cast(htrCtx.switchNr) << " for component " << static_cast(currThermalComponent) << " ON" << std::endl; - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::ON, thermalComponent); + heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::ON, currThermalComponent); } else { // Even if heater control is now allowed, we can update the state. thermalStates[currThermalComponent].heating = false; @@ -1790,6 +1790,8 @@ void ThermalController::heaterMaxDurationControl(const HeaterSwitchStates& curre // The heater might still be one for some thermal components, so cross-check // those components crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(static_cast(i)); + } else if (currentHeaterStates[i] == HeaterHandler::SwitchState::OFF) { + heaterStates[i].heaterOnPeriod.resetTimer(); } } } @@ -1841,8 +1843,8 @@ void ThermalController::heaterSwitchHelper(heater::Switch switchNr, unsigned componentIdx) { timeval currentTime; Clock::getClockMonotonic(¤tTime); - if (state == HeaterHandler::SwitchState::ON) { - heaterHandler.switchHeater(switchNr, state); + if (targetState == HeaterHandler::SwitchState::ON) { + heaterHandler.switchHeater(switchNr, targetState); heaterStates[switchNr].target = HeaterHandler::SwitchState::ON; heaterStates[switchNr].switchTransition = true; thermalStates[componentIdx].sensorIndex = currentSensorIndex; @@ -1852,7 +1854,7 @@ void ThermalController::heaterSwitchHelper(heater::Switch switchNr, triggerEvent(tcsCtrl::TCS_SWITCHING_HEATER_ON, static_cast(currThermalComponent), static_cast(switchNr)); } else { - heaterHandler.switchHeater(switchNr, state); + heaterHandler.switchHeater(switchNr, targetState); thermalStates[componentIdx].heating = false; thermalStates[componentIdx].heaterEndTime = currentTime.tv_sec; triggerEvent(tcsCtrl::TCS_SWITCHING_HEATER_OFF, static_cast(currThermalComponent), From f2f73b088df7e4b864ba9faadeaf091259d721e6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 7 Jul 2023 12:03:34 +0200 Subject: [PATCH 483/506] add event --- mission/controller/ThermalController.cpp | 4 ++- mission/controller/ThermalController.h | 34 ++++++++++++------------ mission/controller/tcsDefs.h | 2 ++ 3 files changed, 22 insertions(+), 18 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 50762e2f..cffa751f 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1781,7 +1781,7 @@ void ThermalController::heaterTransitionControl( if (currentHeaterStates[i] == heaterStates[i].target) { // Required for max heater period control if (currentHeaterStates[i] == heater::SwitchState::ON) { - heaterStates[i].heaterOnPeriod.setTimeout(MAX_HEATER_ON_DURATIONS[i]); + heaterStates[i].heaterOnPeriod.setTimeout(MAX_HEATER_ON_DURATIONS_MS[i]); heaterStates[i].heaterOnPeriod.resetTimer(); } else { // The heater might still be one for some thermal components, so cross-check @@ -1809,6 +1809,8 @@ void ThermalController::heaterMaxDurationControl( heaterStates[i].switchTransition = false; heaterStates[i].heaterSwitchControlCycles = 0; heaterHandler.switchHeater(static_cast(i), heater::SwitchState::OFF); + triggerEvent(tcsCtrl::TCS_HEATER_MAX_BURN_TIME_REACHED, static_cast(i), + MAX_HEATER_ON_DURATIONS_MS[i]); // The heater might still be one for some thermal components, so cross-check // those components crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(static_cast(i)); diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index b6f888c2..f228a991 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -35,23 +35,23 @@ class ThermalController : public ExtendedControllerBase { static constexpr int16_t SANITY_LIMIT_UPPER_TEMP = 160; // 1 hour - static constexpr uint32_t MAX_HEATER_ON_DURATION_MS = 60 * 60 * 1000; - static constexpr uint32_t MAX_HEATER_ON_DURATIONS[8] = {// PLOC PROC board - MAX_HEATER_ON_DURATION_MS, - // PCDU PDU - MAX_HEATER_ON_DURATION_MS, - // ACS Board - MAX_HEATER_ON_DURATION_MS, - // OBC Board - MAX_HEATER_ON_DURATION_MS, - // Camera - MAX_HEATER_ON_DURATION_MS, - // STR - MAX_HEATER_ON_DURATION_MS, - // DRO - MAX_HEATER_ON_DURATION_MS, - // S-Band - MAX_HEATER_ON_DURATION_MS}; + static constexpr uint32_t DEFAULT_MAX_HEATER_ON_DURATION_MS = 60 * 60 * 1000; + static constexpr uint32_t MAX_HEATER_ON_DURATIONS_MS[8] = {// PLOC PROC board + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // PCDU PDU + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // ACS Board + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // OBC Board + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // Camera + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // STR + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // DRO + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // S-Band + DEFAULT_MAX_HEATER_ON_DURATION_MS}; ThermalController(object_id_t objectId, HeaterHandler& heater, const std::atomic_bool& tcsBoardShortUnavailable, bool pollPcdu1Tmp); diff --git a/mission/controller/tcsDefs.h b/mission/controller/tcsDefs.h index 7993a0e2..1b2b968b 100644 --- a/mission/controller/tcsDefs.h +++ b/mission/controller/tcsDefs.h @@ -100,6 +100,8 @@ static constexpr Event MGT_OVERHEATING = MAKE_EVENT(8, severity::HIGH); static constexpr Event TCS_SWITCHING_HEATER_ON = MAKE_EVENT(9, severity::INFO); //! [EXPORT] : [COMMENT] P1: Module index. P2: Heater index static constexpr Event TCS_SWITCHING_HEATER_OFF = MAKE_EVENT(10, severity::INFO); +//! [EXPORT] : [COMMENT] P1: Heater index. P2: Maximum burn time for heater. +static constexpr Event TCS_HEATER_MAX_BURN_TIME_REACHED = MAKE_EVENT(11, severity::MEDIUM); enum SetId : uint32_t { SENSOR_TEMPERATURES = 0, From 7337595d47af96639654325998c8ae2b79e9326b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 7 Jul 2023 12:03:34 +0200 Subject: [PATCH 484/506] add event --- mission/controller/ThermalController.cpp | 8 +++--- mission/controller/ThermalController.h | 34 ++++++++++++------------ mission/controller/tcsDefs.h | 2 ++ 3 files changed, 24 insertions(+), 20 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index be88b760..aefce4ca 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1759,8 +1759,8 @@ void ThermalController::heaterTransitionControl(const HeaterSwitchStates& curren if (heaterStates[i].switchTransition) { if (currentHeaterStates[i] == heaterStates[i].target) { // Required for max heater period control - if (currentHeaterStates[i] == HeaterHandler::SwitchState::ON) { - heaterStates[i].heaterOnPeriod.setTimeout(MAX_HEATER_ON_DURATIONS[i]); + if (currentHeaterStates[i] == heater::SwitchState::ON) { + heaterStates[i].heaterOnPeriod.setTimeout(MAX_HEATER_ON_DURATIONS_MS[i]); heaterStates[i].heaterOnPeriod.resetTimer(); } else { // The heater might still be one for some thermal components, so cross-check @@ -1786,7 +1786,9 @@ void ThermalController::heaterMaxDurationControl(const HeaterSwitchStates& curre heaterStates[i].heaterOnPeriod.hasTimedOut()) { heaterStates[i].switchTransition = false; heaterStates[i].heaterSwitchControlCycles = 0; - heaterHandler.switchHeater(static_cast(i), HeaterHandler::SwitchState::OFF); + heaterHandler.switchHeater(static_cast(i), heater::SwitchState::OFF); + triggerEvent(tcsCtrl::TCS_HEATER_MAX_BURN_TIME_REACHED, static_cast(i), + MAX_HEATER_ON_DURATIONS_MS[i]); // The heater might still be one for some thermal components, so cross-check // those components crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(static_cast(i)); diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 82896116..6376f501 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -107,23 +107,23 @@ class ThermalController : public ExtendedControllerBase { static constexpr int16_t SANITY_LIMIT_UPPER_TEMP = 160; // 1 hour - static constexpr uint32_t MAX_HEATER_ON_DURATION_MS = 60 * 60 * 1000; - static constexpr uint32_t MAX_HEATER_ON_DURATIONS[8] = {// PLOC PROC board - MAX_HEATER_ON_DURATION_MS, - // PCDU PDU - MAX_HEATER_ON_DURATION_MS, - // ACS Board - MAX_HEATER_ON_DURATION_MS, - // OBC Board - MAX_HEATER_ON_DURATION_MS, - // Camera - MAX_HEATER_ON_DURATION_MS, - // STR - MAX_HEATER_ON_DURATION_MS, - // DRO - MAX_HEATER_ON_DURATION_MS, - // S-Band - MAX_HEATER_ON_DURATION_MS}; + static constexpr uint32_t DEFAULT_MAX_HEATER_ON_DURATION_MS = 60 * 60 * 1000; + static constexpr uint32_t MAX_HEATER_ON_DURATIONS_MS[8] = {// PLOC PROC board + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // PCDU PDU + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // ACS Board + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // OBC Board + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // Camera + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // STR + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // DRO + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // S-Band + DEFAULT_MAX_HEATER_ON_DURATION_MS}; ThermalController(object_id_t objectId, HeaterHandler& heater, const std::atomic_bool& tcsBoardShortUnavailable, bool pollPcdu1Tmp); diff --git a/mission/controller/tcsDefs.h b/mission/controller/tcsDefs.h index c77d9352..6442b51a 100644 --- a/mission/controller/tcsDefs.h +++ b/mission/controller/tcsDefs.h @@ -22,6 +22,8 @@ static constexpr Event MGT_OVERHEATING = MAKE_EVENT(8, severity::HIGH); static constexpr Event TCS_SWITCHING_HEATER_ON = MAKE_EVENT(9, severity::INFO); //! [EXPORT] : [COMMENT] P1: Module index. P2: Heater index static constexpr Event TCS_SWITCHING_HEATER_OFF = MAKE_EVENT(10, severity::INFO); +//! [EXPORT] : [COMMENT] P1: Heater index. P2: Maximum burn time for heater. +static constexpr Event TCS_HEATER_MAX_BURN_TIME_REACHED = MAKE_EVENT(11, severity::MEDIUM); enum SetId : uint32_t { SENSOR_TEMPERATURES = 0, From b662e220e13c563ca0a915feef58c92031192828 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 7 Jul 2023 12:06:39 +0200 Subject: [PATCH 485/506] bump tmtc --- bsp_hosted/fsfwconfig/events/translateEvents.cpp | 7 +++++-- bsp_hosted/fsfwconfig/objects/translateObjects.cpp | 2 +- generators/bsp_hosted_events.csv | 1 + generators/bsp_q7s_events.csv | 1 + generators/events/translateEvents.cpp | 7 +++++-- generators/objects/translateObjects.cpp | 2 +- linux/fsfwconfig/events/translateEvents.cpp | 7 +++++-- linux/fsfwconfig/objects/translateObjects.cpp | 2 +- tmtc | 2 +- 9 files changed, 21 insertions(+), 10 deletions(-) diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index 30774cf9..b98926e5 100644 --- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp +++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 298 translations. + * @brief Auto-generated event translation file. Contains 299 translations. * @details - * Generated on: 2023-07-06 19:00:21 + * Generated on: 2023-07-07 12:06:06 */ #include "translateEvents.h" @@ -288,6 +288,7 @@ const char *HEATER_NOT_OFF_FOR_OFF_MODE_STRING = "HEATER_NOT_OFF_FOR_OFF_MODE"; const char *MGT_OVERHEATING_STRING = "MGT_OVERHEATING"; const char *TCS_SWITCHING_HEATER_ON_STRING = "TCS_SWITCHING_HEATER_ON"; const char *TCS_SWITCHING_HEATER_OFF_STRING = "TCS_SWITCHING_HEATER_OFF"; +const char *TCS_HEATER_MAX_BURN_TIME_REACHED_STRING = "TCS_HEATER_MAX_BURN_TIME_REACHED"; const char *TX_TIMER_EXPIRED_STRING = "TX_TIMER_EXPIRED"; const char *BIT_LOCK_TX_ON_STRING = "BIT_LOCK_TX_ON"; const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION"; @@ -872,6 +873,8 @@ const char *translateEvents(Event event) { return TCS_SWITCHING_HEATER_ON_STRING; case (14110): return TCS_SWITCHING_HEATER_OFF_STRING; + case (14111): + return TCS_HEATER_MAX_BURN_TIME_REACHED_STRING; case (14201): return TX_TIMER_EXPIRED_STRING; case (14202): diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index c073905f..458081f9 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 171 translations. - * Generated on: 2023-07-06 19:00:21 + * Generated on: 2023-07-07 12:06:06 */ #include "translateObjects.h" diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv index a26b0cdc..3f1bbb77 100644 --- a/generators/bsp_hosted_events.csv +++ b/generators/bsp_hosted_events.csv @@ -282,6 +282,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14108;0x371c;MGT_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14109;0x371d;TCS_SWITCHING_HEATER_ON;INFO;P1: Module index. P2: Heater index;mission/controller/tcsDefs.h 14110;0x371e;TCS_SWITCHING_HEATER_OFF;INFO;P1: Module index. P2: Heater index;mission/controller/tcsDefs.h +14111;0x371f;TCS_HEATER_MAX_BURN_TIME_REACHED;MEDIUM;P1: Heater index. P2: Maximum burn time for heater.;mission/controller/tcsDefs.h 14201;0x3779;TX_TIMER_EXPIRED;INFO;The transmit timer to protect the Syrlinks expired P1: The current timer value;mission/system/com/ComSubsystem.h 14202;0x377a;BIT_LOCK_TX_ON;INFO;Transmitter will be turned on due to detection of bitlock;mission/system/com/ComSubsystem.h 14300;0x37dc;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/persistentTmStoreDefs.h diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index a26b0cdc..3f1bbb77 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -282,6 +282,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14108;0x371c;MGT_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14109;0x371d;TCS_SWITCHING_HEATER_ON;INFO;P1: Module index. P2: Heater index;mission/controller/tcsDefs.h 14110;0x371e;TCS_SWITCHING_HEATER_OFF;INFO;P1: Module index. P2: Heater index;mission/controller/tcsDefs.h +14111;0x371f;TCS_HEATER_MAX_BURN_TIME_REACHED;MEDIUM;P1: Heater index. P2: Maximum burn time for heater.;mission/controller/tcsDefs.h 14201;0x3779;TX_TIMER_EXPIRED;INFO;The transmit timer to protect the Syrlinks expired P1: The current timer value;mission/system/com/ComSubsystem.h 14202;0x377a;BIT_LOCK_TX_ON;INFO;Transmitter will be turned on due to detection of bitlock;mission/system/com/ComSubsystem.h 14300;0x37dc;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/persistentTmStoreDefs.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index 30774cf9..b98926e5 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 298 translations. + * @brief Auto-generated event translation file. Contains 299 translations. * @details - * Generated on: 2023-07-06 19:00:21 + * Generated on: 2023-07-07 12:06:06 */ #include "translateEvents.h" @@ -288,6 +288,7 @@ const char *HEATER_NOT_OFF_FOR_OFF_MODE_STRING = "HEATER_NOT_OFF_FOR_OFF_MODE"; const char *MGT_OVERHEATING_STRING = "MGT_OVERHEATING"; const char *TCS_SWITCHING_HEATER_ON_STRING = "TCS_SWITCHING_HEATER_ON"; const char *TCS_SWITCHING_HEATER_OFF_STRING = "TCS_SWITCHING_HEATER_OFF"; +const char *TCS_HEATER_MAX_BURN_TIME_REACHED_STRING = "TCS_HEATER_MAX_BURN_TIME_REACHED"; const char *TX_TIMER_EXPIRED_STRING = "TX_TIMER_EXPIRED"; const char *BIT_LOCK_TX_ON_STRING = "BIT_LOCK_TX_ON"; const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION"; @@ -872,6 +873,8 @@ const char *translateEvents(Event event) { return TCS_SWITCHING_HEATER_ON_STRING; case (14110): return TCS_SWITCHING_HEATER_OFF_STRING; + case (14111): + return TCS_HEATER_MAX_BURN_TIME_REACHED_STRING; case (14201): return TX_TIMER_EXPIRED_STRING; case (14202): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index 379265c6..5a9af4d7 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-07-06 19:00:21 + * Generated on: 2023-07-07 12:06:06 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index 30774cf9..b98926e5 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 298 translations. + * @brief Auto-generated event translation file. Contains 299 translations. * @details - * Generated on: 2023-07-06 19:00:21 + * Generated on: 2023-07-07 12:06:06 */ #include "translateEvents.h" @@ -288,6 +288,7 @@ const char *HEATER_NOT_OFF_FOR_OFF_MODE_STRING = "HEATER_NOT_OFF_FOR_OFF_MODE"; const char *MGT_OVERHEATING_STRING = "MGT_OVERHEATING"; const char *TCS_SWITCHING_HEATER_ON_STRING = "TCS_SWITCHING_HEATER_ON"; const char *TCS_SWITCHING_HEATER_OFF_STRING = "TCS_SWITCHING_HEATER_OFF"; +const char *TCS_HEATER_MAX_BURN_TIME_REACHED_STRING = "TCS_HEATER_MAX_BURN_TIME_REACHED"; const char *TX_TIMER_EXPIRED_STRING = "TX_TIMER_EXPIRED"; const char *BIT_LOCK_TX_ON_STRING = "BIT_LOCK_TX_ON"; const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION"; @@ -872,6 +873,8 @@ const char *translateEvents(Event event) { return TCS_SWITCHING_HEATER_ON_STRING; case (14110): return TCS_SWITCHING_HEATER_OFF_STRING; + case (14111): + return TCS_HEATER_MAX_BURN_TIME_REACHED_STRING; case (14201): return TX_TIMER_EXPIRED_STRING; case (14202): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index 379265c6..5a9af4d7 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-07-06 19:00:21 + * Generated on: 2023-07-07 12:06:06 */ #include "translateObjects.h" diff --git a/tmtc b/tmtc index 069f84d2..15d25b4c 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 069f84d2207c03e8d334cfce3f7dd530babe17b0 +Subproject commit 15d25b4c5b7ad21603a83a6c09835f5c97273b17 From fa85cc4d78aa5f6aa53635e36979e2929463475c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 7 Jul 2023 12:22:37 +0200 Subject: [PATCH 486/506] pls compile --- mission/controller/ThermalController.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index aefce4ca..ce66ccc1 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1759,7 +1759,7 @@ void ThermalController::heaterTransitionControl(const HeaterSwitchStates& curren if (heaterStates[i].switchTransition) { if (currentHeaterStates[i] == heaterStates[i].target) { // Required for max heater period control - if (currentHeaterStates[i] == heater::SwitchState::ON) { + if (currentHeaterStates[i] == HeaterHandler::SwitchState::ON) { heaterStates[i].heaterOnPeriod.setTimeout(MAX_HEATER_ON_DURATIONS_MS[i]); heaterStates[i].heaterOnPeriod.resetTimer(); } else { @@ -1786,7 +1786,7 @@ void ThermalController::heaterMaxDurationControl(const HeaterSwitchStates& curre heaterStates[i].heaterOnPeriod.hasTimedOut()) { heaterStates[i].switchTransition = false; heaterStates[i].heaterSwitchControlCycles = 0; - heaterHandler.switchHeater(static_cast(i), heater::SwitchState::OFF); + heaterHandler.switchHeater(static_cast(i), HeaterHandler::SwitchState::OFF); triggerEvent(tcsCtrl::TCS_HEATER_MAX_BURN_TIME_REACHED, static_cast(i), MAX_HEATER_ON_DURATIONS_MS[i]); // The heater might still be one for some thermal components, so cross-check From 43d86c9aadb86220541c1ef2bd75c9dde3414daa Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 7 Jul 2023 12:24:48 +0200 Subject: [PATCH 487/506] resolve merge conflicts properly --- mission/controller/ThermalController.cpp | 12 ++++++------ mission/tcs/HeaterHandler.cpp | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 88058ecc..f932c762 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1706,7 +1706,7 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { } htrCtx.switchState = - static_cast(heaterInfo.heaterSwitchState[htrCtx.switchNr]); + static_cast(heaterInfo.heaterSwitchState[htrCtx.switchNr]); // Heater off if (htrCtx.switchState == heater::SwitchState::OFF) { if (ctrlCtx.sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) { @@ -1780,7 +1780,7 @@ void ThermalController::heaterTransitionControl( if (heaterStates[i].switchTransition) { if (currentHeaterStates[i] == heaterStates[i].target) { // Required for max heater period control - if (currentHeaterStates[i] == HeaterHandler::SwitchState::ON) { + if (currentHeaterStates[i] == heater::SwitchState::ON) { heaterStates[i].heaterOnPeriod.setTimeout(MAX_HEATER_ON_DURATIONS_MS[i]); heaterStates[i].heaterOnPeriod.resetTimer(); } else { @@ -1808,13 +1808,13 @@ void ThermalController::heaterMaxDurationControl( heaterStates[i].heaterOnPeriod.hasTimedOut()) { heaterStates[i].switchTransition = false; heaterStates[i].heaterSwitchControlCycles = 0; - heaterHandler.switchHeater(static_cast(i), HeaterHandler::SwitchState::OFF); + heaterHandler.switchHeater(static_cast(i), heater::SwitchState::OFF); triggerEvent(tcsCtrl::TCS_HEATER_MAX_BURN_TIME_REACHED, static_cast(i), MAX_HEATER_ON_DURATIONS_MS[i]); // The heater might still be one for some thermal components, so cross-check // those components crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(static_cast(i)); - } else if (currentHeaterStates[i] == HeaterHandler::SwitchState::OFF) { + } else if (currentHeaterStates[i] == heater::SwitchState::OFF) { heaterStates[i].heaterOnPeriod.resetTimer(); } } @@ -1874,13 +1874,13 @@ void ThermalController::heaterSwitchHelper(heater::Switch switchNr, heater::Swit thermalStates[componentIdx].heaterSwitch = switchNr; thermalStates[componentIdx].heating = true; thermalStates[componentIdx].heaterStartTime = currentTime.tv_sec; - triggerEvent(tcsCtrl::TCS_SWITCHING_HEATER_ON, static_cast(currThermalComponent), + triggerEvent(tcsCtrl::TCS_SWITCHING_HEATER_ON, static_cast(ctrlCtx.thermalComponent), static_cast(switchNr)); } else { heaterHandler.switchHeater(switchNr, targetState); thermalStates[componentIdx].heating = false; thermalStates[componentIdx].heaterEndTime = currentTime.tv_sec; - triggerEvent(tcsCtrl::TCS_SWITCHING_HEATER_OFF, static_cast(currThermalComponent), + triggerEvent(tcsCtrl::TCS_SWITCHING_HEATER_OFF, static_cast(ctrlCtx.thermalComponent), static_cast(switchNr)); } } diff --git a/mission/tcs/HeaterHandler.cpp b/mission/tcs/HeaterHandler.cpp index a1964d74..b88dfe75 100644 --- a/mission/tcs/HeaterHandler.cpp +++ b/mission/tcs/HeaterHandler.cpp @@ -421,7 +421,7 @@ ReturnValue_t HeaterHandler::getAllSwitchStates(std::array Date: Fri, 7 Jul 2023 15:33:48 +0200 Subject: [PATCH 488/506] that should be sufficient --- bsp_q7s/boardconfig/busConf.h | 3 +++ bsp_q7s/em/emObjectFactory.cpp | 4 +++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/bsp_q7s/boardconfig/busConf.h b/bsp_q7s/boardconfig/busConf.h index 146386c4..dc3779a7 100644 --- a/bsp_q7s/boardconfig/busConf.h +++ b/bsp_q7s/boardconfig/busConf.h @@ -12,6 +12,9 @@ static constexpr char SPI_RW_DEV[] = "/dev/spi_rw"; static constexpr char I2C_PL_EIVE[] = "/dev/i2c_pl"; //! I2C bus using the I2C peripheral of the ARM processing system (PS) static constexpr char I2C_PS_EIVE[] = "/dev/i2c_ps"; +//! I2C bus using the first I2C peripheral of the ARM processing system (PS). +//! Named like this because it is used by default for the Q7 devices. +static constexpr char I2C_Q7_EIVE[] = "/dev/i2c_q7"; static constexpr char UART_GNSS_DEV[] = "/dev/gps0"; static constexpr char UART_PLOC_MPSOC_DEV[] = "/dev/ul_plmpsoc"; diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 085d5184..58b9a7f9 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -69,8 +69,10 @@ void ObjectFactory::produce(void* args) { {objects::TMP1075_HANDLER_IF_BOARD, addresses::TMP1075_IF_BOARD}, }}; const char* tmpI2cDev = q7s::I2C_PS_EIVE; - if (core::FW_VERSION_MAJOR >= 4) { + if (core::FW_VERSION_MAJOR == 4) { tmpI2cDev = q7s::I2C_PL_EIVE; + } else if(core::FW_VERSION_MAJOR >= 5) { + tmpI2cDev = q7s::I2C_Q7_EIVE; } createTmpComponents(tmpDevsToAdd, tmpI2cDev); dummy::Tmp1075Cfg tmpCfg{}; From a351dcf99e46ccfa1c2a7e9cbb5de6f51701ec61 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 7 Jul 2023 15:59:15 +0200 Subject: [PATCH 489/506] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 709436f5..77862553 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,6 +24,8 @@ will consitute of a breaking change warranting a new major release: runtime now. This makes it possible to check the firmware version earlier. - The TCS controller will now always command heaters OFF when being blind for thermal components (no sensors available), irrespective of current switch state. +- Make OBSW compatible to prospective FW version v5.0.0, where the Q7 I2C devices were + moved to a PL I2C block and the TMP sensor devices were moved to the PS I2C0. ## Fixed From df3755d6cce0ceb9dd65f9f27ec1b2bf0a42bf2a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 7 Jul 2023 16:18:16 +0200 Subject: [PATCH 490/506] done --- CHANGELOG.md | 2 ++ mission/controller/ThermalController.cpp | 7 ++++++- mission/controller/ThermalController.h | 7 ++++--- 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5788400d..4a257fc0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -43,6 +43,8 @@ will consitute of a breaking change warranting a new major release: - Two events for heaters being commanded ON and OFF by the TCS controller - Upper limit for burn time of TCS heaters. Currently set to 1 hour for each heater. + This mechanism will only track the burn time for heaters which were commanded by the + TCS controller. # [v6.0.0] 2023-07-02 diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index ce66ccc1..b0f0f9d1 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1762,7 +1762,9 @@ void ThermalController::heaterTransitionControl(const HeaterSwitchStates& curren if (currentHeaterStates[i] == HeaterHandler::SwitchState::ON) { heaterStates[i].heaterOnPeriod.setTimeout(MAX_HEATER_ON_DURATIONS_MS[i]); heaterStates[i].heaterOnPeriod.resetTimer(); + heaterStates[i].trackHeaterMaxPeriod = true; } else { + heaterStates[i].trackHeaterMaxPeriod = false; // The heater might still be one for some thermal components, so cross-check // those components crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(static_cast(i)); @@ -1782,10 +1784,13 @@ void ThermalController::heaterTransitionControl(const HeaterSwitchStates& curren void ThermalController::heaterMaxDurationControl(const HeaterSwitchStates& currentHeaterStates) { for (unsigned i = 0; i < heater::Switch::NUMBER_OF_SWITCHES; i++) { + // Right now, we only track the maximum duration for heater which were commanded by the TCS + // controller. if (currentHeaterStates[i] == HeaterHandler::SwitchState::ON and - heaterStates[i].heaterOnPeriod.hasTimedOut()) { + heaterStates[i].trackHeaterMaxPeriod and heaterStates[i].heaterOnPeriod.hasTimedOut()) { heaterStates[i].switchTransition = false; heaterStates[i].heaterSwitchControlCycles = 0; + heaterStates[i].trackHeaterMaxPeriod = false; heaterHandler.switchHeater(static_cast(i), HeaterHandler::SwitchState::OFF); triggerEvent(tcsCtrl::TCS_HEATER_MAX_BURN_TIME_REACHED, static_cast(i), MAX_HEATER_ON_DURATIONS_MS[i]); diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 6376f501..74e06cf5 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -61,9 +61,10 @@ struct ThermalState { }; struct HeaterState { - bool switchTransition; - HeaterHandler::SwitchState target; - uint8_t heaterSwitchControlCycles; + bool switchTransition = false; + HeaterHandler::SwitchState target = HeaterHandler::SwitchState::OFF; + uint8_t heaterSwitchControlCycles = 0; + bool trackHeaterMaxPeriod = false; Countdown heaterOnPeriod; }; From 647f3d00bbcc5845fd1496c6621a1394ab968609 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 10 Jul 2023 11:03:17 +0200 Subject: [PATCH 491/506] this should do the job --- bsp_q7s/em/emObjectFactory.cpp | 8 +------- bsp_q7s/fmObjectFactory.cpp | 7 ++----- bsp_q7s/objectFactory.cpp | 10 ++++++++-- bsp_q7s/objectFactory.h | 3 +-- 4 files changed, 12 insertions(+), 16 deletions(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 58b9a7f9..a7b05e3d 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -68,13 +68,7 @@ void ObjectFactory::produce(void* args) { {objects::TMP1075_HANDLER_PLPCDU_1, addresses::TMP1075_PLPCDU_1}, {objects::TMP1075_HANDLER_IF_BOARD, addresses::TMP1075_IF_BOARD}, }}; - const char* tmpI2cDev = q7s::I2C_PS_EIVE; - if (core::FW_VERSION_MAJOR == 4) { - tmpI2cDev = q7s::I2C_PL_EIVE; - } else if(core::FW_VERSION_MAJOR >= 5) { - tmpI2cDev = q7s::I2C_Q7_EIVE; - } - createTmpComponents(tmpDevsToAdd, tmpI2cDev); + createTmpComponents(tmpDevsToAdd); dummy::Tmp1075Cfg tmpCfg{}; tmpCfg.addTcsBrd0 = true; tmpCfg.addTcsBrd1 = true; diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 5e2cdb59..e43e3551 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -77,11 +77,8 @@ void ObjectFactory::produce(void* args) { // {objects::TMP1075_HANDLER_PLPCDU_1, addresses::TMP1075_PLPCDU_1}, {objects::TMP1075_HANDLER_IF_BOARD, addresses::TMP1075_IF_BOARD}, }}; - const char* tmpI2cDev = q7s::I2C_PS_EIVE; - if (core::FW_VERSION_MAJOR >= 4) { - tmpI2cDev = q7s::I2C_PL_EIVE; - } - createTmpComponents(tmpDevsToAdd, tmpI2cDev); + + createTmpComponents(tmpDevsToAdd); #endif createSolarArrayDeploymentComponents(*pwrSwitcher, *gpioComIF); createPlPcduComponents(gpioComIF, spiMainComIF, pwrSwitcher, *stackHandler); diff --git a/bsp_q7s/objectFactory.cpp b/bsp_q7s/objectFactory.cpp index a96051f2..c18a48a6 100644 --- a/bsp_q7s/objectFactory.cpp +++ b/bsp_q7s/objectFactory.cpp @@ -161,8 +161,14 @@ void Factory::setStaticFrameworkObjectIds() { void ObjectFactory::setStatics() { Factory::setStaticFrameworkObjectIds(); } -void ObjectFactory::createTmpComponents(std::vector> tmpDevsToAdd, - const char* i2cDev) { +void ObjectFactory::createTmpComponents( + std::vector> tmpDevsToAdd) { + const char* tmpI2cDev = q7s::I2C_PS_EIVE; + if (core::FW_VERSION_MAJOR == 4) { + tmpI2cDev = q7s::I2C_PL_EIVE; + } else if (core::FW_VERSION_MAJOR >= 5) { + tmpI2cDev = q7s::I2C_Q7_EIVE; + } std::vector tmpDevCookies; for (size_t idx = 0; idx < tmpDevsToAdd.size(); idx++) { diff --git a/bsp_q7s/objectFactory.h b/bsp_q7s/objectFactory.h index 779ad204..491720ac 100644 --- a/bsp_q7s/objectFactory.h +++ b/bsp_q7s/objectFactory.h @@ -58,8 +58,7 @@ void createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher bool enableHkSets); void createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF, PowerSwitchIF* pwrSwitcher, Stack5VHandler& stackHandler); -void createTmpComponents(std::vector> tmpDevsToAdd, - const char* i2cDev); +void createTmpComponents(std::vector> tmpDevsToAdd); void createRadSensorChipSelect(LinuxLibgpioIF* gpioIF); ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& handler); void createAcsBoardGpios(GpioCookie& cookie); From 268f10ced5dacf242c086db8a088dae6cc3312ec Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 10 Jul 2023 11:14:19 +0200 Subject: [PATCH 492/506] COMPILE --- bsp_q7s/objectFactory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bsp_q7s/objectFactory.cpp b/bsp_q7s/objectFactory.cpp index c18a48a6..e6ae6603 100644 --- a/bsp_q7s/objectFactory.cpp +++ b/bsp_q7s/objectFactory.cpp @@ -173,7 +173,7 @@ void ObjectFactory::createTmpComponents( for (size_t idx = 0; idx < tmpDevsToAdd.size(); idx++) { tmpDevCookies.push_back( - new I2cCookie(tmpDevsToAdd[idx].second, TMP1075::MAX_REPLY_LENGTH, i2cDev)); + new I2cCookie(tmpDevsToAdd[idx].second, TMP1075::MAX_REPLY_LENGTH, tmpI2cDev)); auto* tmpDevHandler = new Tmp1075Handler(tmpDevsToAdd[idx].first, objects::I2C_COM_IF, tmpDevCookies[idx]); tmpDevHandler->setCustomFdir(new TmpDevFdir(tmpDevsToAdd[idx].first)); From 33de08eafc010e78c6c5caa7751801f68adea39f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 10 Jul 2023 13:28:36 +0200 Subject: [PATCH 493/506] this should be better --- dummies/TemperatureSensorInserter.cpp | 9 ++++++++- dummies/TemperatureSensorInserter.h | 1 + mission/controller/ThermalController.cpp | 22 ++++++++++++++-------- mission/controller/ThermalController.h | 5 +++-- 4 files changed, 26 insertions(+), 11 deletions(-) diff --git a/dummies/TemperatureSensorInserter.cpp b/dummies/TemperatureSensorInserter.cpp index dc94195e..43065b83 100644 --- a/dummies/TemperatureSensorInserter.cpp +++ b/dummies/TemperatureSensorInserter.cpp @@ -15,7 +15,7 @@ TemperatureSensorInserter::TemperatureSensorInserter( tmp1075DummyMap(std::move(tempTmpSensorDummies_)) {} ReturnValue_t TemperatureSensorInserter::initialize() { - testCase = TestCase::NONE; + testCase = TestCase::COLD_PLOC_STAYS_COLD; return returnvalue::OK; } @@ -126,6 +126,13 @@ ReturnValue_t TemperatureSensorInserter::performOperation(uint8_t opCode) { sif::debug << "Setting CAM temperature back to normal" << std::endl; max31865DummyMap[objects::RTD_2_IC5_4K_CAMERA]->setTemperature(0, true); } + break; + } + case (TestCase::COLD_PLOC_STAYS_COLD): { + if (cycles == 15) { + sif::debug << "Setting cold PLOC temperature" << std::endl; + max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(-40, true); + } } } cycles++; diff --git a/dummies/TemperatureSensorInserter.h b/dummies/TemperatureSensorInserter.h index 006b0639..9da83bc9 100644 --- a/dummies/TemperatureSensorInserter.h +++ b/dummies/TemperatureSensorInserter.h @@ -33,6 +33,7 @@ class TemperatureSensorInserter : public ExecutableObjectIF, public SystemObject COLD_STR_CONSECUTIVE = 5, COLD_CAMERA = 6, COLD_PLOC_CONSECUTIVE = 7, + COLD_PLOC_STAYS_COLD = 8 }; int iteration = 0; uint32_t cycles = 0; diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index b0f0f9d1..acfa8568 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1791,9 +1791,10 @@ void ThermalController::heaterMaxDurationControl(const HeaterSwitchStates& curre heaterStates[i].switchTransition = false; heaterStates[i].heaterSwitchControlCycles = 0; heaterStates[i].trackHeaterMaxPeriod = false; - heaterHandler.switchHeater(static_cast(i), HeaterHandler::SwitchState::OFF); triggerEvent(tcsCtrl::TCS_HEATER_MAX_BURN_TIME_REACHED, static_cast(i), MAX_HEATER_ON_DURATIONS_MS[i]); + heaterSwitchHelper(static_cast(i), HeaterHandler::SwitchState::OFF, + std::nullopt); // The heater might still be one for some thermal components, so cross-check // those components crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(static_cast(i)); @@ -1847,23 +1848,28 @@ void ThermalController::resetThermalStates() { void ThermalController::heaterSwitchHelper(heater::Switch switchNr, HeaterHandler::SwitchState targetState, - unsigned componentIdx) { + std::optional componentIdx) { timeval currentTime; Clock::getClockMonotonic(¤tTime); if (targetState == HeaterHandler::SwitchState::ON) { heaterHandler.switchHeater(switchNr, targetState); heaterStates[switchNr].target = HeaterHandler::SwitchState::ON; heaterStates[switchNr].switchTransition = true; - thermalStates[componentIdx].sensorIndex = currentSensorIndex; - thermalStates[componentIdx].heaterSwitch = switchNr; - thermalStates[componentIdx].heating = true; - thermalStates[componentIdx].heaterStartTime = currentTime.tv_sec; + if (componentIdx.has_value()) { + unsigned componentIdxVal = componentIdx.value(); + thermalStates[componentIdxVal].sensorIndex = currentSensorIndex; + thermalStates[componentIdxVal].heaterSwitch = switchNr; + thermalStates[componentIdxVal].heating = true; + thermalStates[componentIdxVal].heaterStartTime = currentTime.tv_sec; + } triggerEvent(tcsCtrl::TCS_SWITCHING_HEATER_ON, static_cast(currThermalComponent), static_cast(switchNr)); } else { heaterHandler.switchHeater(switchNr, targetState); - thermalStates[componentIdx].heating = false; - thermalStates[componentIdx].heaterEndTime = currentTime.tv_sec; + if (componentIdx.has_value()) { + thermalStates[componentIdx.value()].heating = false; + thermalStates[componentIdx.value()].heaterEndTime = currentTime.tv_sec; + } triggerEvent(tcsCtrl::TCS_SWITCHING_HEATER_OFF, static_cast(currThermalComponent), static_cast(switchNr)); } diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 74e06cf5..7ba86487 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -24,6 +24,7 @@ #include #include +#include /** * NOP Limit: Hard limit for device, usually from datasheet. Device damage is possible lif NOP limit @@ -110,7 +111,7 @@ class ThermalController : public ExtendedControllerBase { // 1 hour static constexpr uint32_t DEFAULT_MAX_HEATER_ON_DURATION_MS = 60 * 60 * 1000; static constexpr uint32_t MAX_HEATER_ON_DURATIONS_MS[8] = {// PLOC PROC board - DEFAULT_MAX_HEATER_ON_DURATION_MS, + 60 * 1000, // PCDU PDU DEFAULT_MAX_HEATER_ON_DURATION_MS, // ACS Board @@ -347,7 +348,7 @@ class ThermalController : public ExtendedControllerBase { void heaterSwitchHelperAllOff(); void heaterSwitchHelper(heater::Switch switchNr, HeaterHandler::SwitchState state, - unsigned componentIdx); + std::optional componentIdx); void ctrlAcsBoard(); void ctrlMgt(); From ce2755e161f3a0525d12be0c7e26d84e81a3b5a0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 10 Jul 2023 13:56:30 +0200 Subject: [PATCH 494/506] that should be it --- dummies/TemperatureSensorInserter.cpp | 10 +++++++++- dummies/TemperatureSensorInserter.h | 3 ++- mission/controller/ThermalController.cpp | 2 -- mission/controller/ThermalController.h | 2 +- 4 files changed, 12 insertions(+), 5 deletions(-) diff --git a/dummies/TemperatureSensorInserter.cpp b/dummies/TemperatureSensorInserter.cpp index 43065b83..bce48606 100644 --- a/dummies/TemperatureSensorInserter.cpp +++ b/dummies/TemperatureSensorInserter.cpp @@ -15,7 +15,7 @@ TemperatureSensorInserter::TemperatureSensorInserter( tmp1075DummyMap(std::move(tempTmpSensorDummies_)) {} ReturnValue_t TemperatureSensorInserter::initialize() { - testCase = TestCase::COLD_PLOC_STAYS_COLD; + testCase = TestCase::COLD_CAMERA_STAYS_COLD; return returnvalue::OK; } @@ -133,6 +133,14 @@ ReturnValue_t TemperatureSensorInserter::performOperation(uint8_t opCode) { sif::debug << "Setting cold PLOC temperature" << std::endl; max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(-40, true); } + break; + } + case (TestCase::COLD_CAMERA_STAYS_COLD): { + if (cycles == 15) { + sif::debug << "Setting cold PLOC temperature" << std::endl; + max31865DummyMap[objects::RTD_2_IC5_4K_CAMERA]->setTemperature(-40, true); + } + break; } } cycles++; diff --git a/dummies/TemperatureSensorInserter.h b/dummies/TemperatureSensorInserter.h index 9da83bc9..9ca3c936 100644 --- a/dummies/TemperatureSensorInserter.h +++ b/dummies/TemperatureSensorInserter.h @@ -33,7 +33,8 @@ class TemperatureSensorInserter : public ExecutableObjectIF, public SystemObject COLD_STR_CONSECUTIVE = 5, COLD_CAMERA = 6, COLD_PLOC_CONSECUTIVE = 7, - COLD_PLOC_STAYS_COLD = 8 + COLD_PLOC_STAYS_COLD = 8, + COLD_CAMERA_STAYS_COLD = 9 }; int iteration = 0; uint32_t cycles = 0; diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index acfa8568..7451e684 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1798,8 +1798,6 @@ void ThermalController::heaterMaxDurationControl(const HeaterSwitchStates& curre // The heater might still be one for some thermal components, so cross-check // those components crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(static_cast(i)); - } else if (currentHeaterStates[i] == HeaterHandler::SwitchState::OFF) { - heaterStates[i].heaterOnPeriod.resetTimer(); } } } diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 7ba86487..9399cab1 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -111,7 +111,7 @@ class ThermalController : public ExtendedControllerBase { // 1 hour static constexpr uint32_t DEFAULT_MAX_HEATER_ON_DURATION_MS = 60 * 60 * 1000; static constexpr uint32_t MAX_HEATER_ON_DURATIONS_MS[8] = {// PLOC PROC board - 60 * 1000, + DEFAULT_MAX_HEATER_ON_DURATION_MS, // PCDU PDU DEFAULT_MAX_HEATER_ON_DURATION_MS, // ACS Board From be1d1cf5bf0113b60559d8f43c57e76b93a4e1af Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 10 Jul 2023 14:15:56 +0200 Subject: [PATCH 495/506] minor tweaks for XADC code --- bsp_q7s/xadc/Xadc.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/bsp_q7s/xadc/Xadc.cpp b/bsp_q7s/xadc/Xadc.cpp index 34a4e159..43fc7b75 100644 --- a/bsp_q7s/xadc/Xadc.cpp +++ b/bsp_q7s/xadc/Xadc.cpp @@ -129,7 +129,7 @@ ReturnValue_t Xadc::readValFromFile(const char* filename, T& val) { sif::warning << "Xadc::readValFromFile: Failed to open file " << filename << std::endl; return returnvalue::FAILED; } - char valstring[MAX_STR_LENGTH] = ""; + char valstring[MAX_STR_LENGTH]{}; char* returnVal = fgets(valstring, MAX_STR_LENGTH, fp); if (returnVal == nullptr) { sif::warning << "Xadc::readValFromFile: Failed to read string from file " << filename @@ -139,6 +139,11 @@ ReturnValue_t Xadc::readValFromFile(const char* filename, T& val) { } std::istringstream valSstream(valstring); valSstream >> val; + if(valSstream.bad()) { + sif::warning << "Xadc: Conversion of value to target type failed" << std::endl; + fclose(fp); + return returnvalue::FAILED; + } fclose(fp); return returnvalue::OK; } From 7e01a9602efad0d7c6212df38ecc055ae1cd5fe7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 10 Jul 2023 14:46:35 +0200 Subject: [PATCH 496/506] TCS improvements --- CHANGELOG.md | 4 +++ mission/controller/ThermalController.cpp | 36 ++++++------------------ mission/controller/ThermalController.h | 4 +-- 3 files changed, 14 insertions(+), 30 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 77862553..62b7a7c3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,8 @@ will consitute of a breaking change warranting a new major release: ## Changed +- TCS: Remove OBC IF board thermal module, which is exactly identical to OBC module and therefore + obsolete. - Swapped PL and PS I2C, BPX battery and MGT are connected to PS I2C now for firmware versions equal or above v4. However, this software version is compatible to both v3 and v4 of the firmware. - The firmware version variables are global statics inititalized early during the program @@ -40,6 +42,8 @@ will consitute of a breaking change warranting a new major release: - TMP1075: Devices did not go to OFF mode when being set faulty. - Update PL PCDU 1 in TCS mode tree on the EM. - TMP1075: Possibly ignored health commands. +- TCS CTRL: Limit number of heater handler messages sent in case there are not sensors available + anymore. # Added diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 79f11785..b6b58946 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1247,25 +1247,6 @@ void ThermalController::ctrlObc() { } } -void ThermalController::ctrlObcIfBoard() { - currThermalComponent = OBCIF_BOARD; - sensors[0].first = deviceTemperatures.q7s.isValid(); - sensors[0].second = deviceTemperatures.q7s.value; - sensors[1].first = sensorTemperatures.tmp1075Tcs0.isValid(); - sensors[1].second = sensorTemperatures.tmp1075Tcs0.value; - sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid(); - sensors[2].second = sensorTemperatures.tmp1075Tcs1.value; - numSensors = 3; - HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, obcIfBoardLimits); - ctrlComponentTemperature(htrCtx); - if (componentAboveUpperLimit and not obcTooHotFlag) { - triggerEvent(tcsCtrl::OBC_OVERHEATING, tempFloatToU32()); - obcTooHotFlag = true; - } else if (not componentAboveUpperLimit) { - obcTooHotFlag = false; - } -} - void ThermalController::ctrlSBandTransceiver() { currThermalComponent = SBAND_TRANSCEIVER; sensors[0].first = deviceTemperatures.syrlinksPowerAmplifier.isValid(); @@ -1531,7 +1512,6 @@ void ThermalController::performThermalModuleCtrl(const HeaterSwitchStates& heate ctrlIfBoard(); ctrlTcsBoard(); ctrlObc(); - ctrlObcIfBoard(); ctrlSBandTransceiver(); ctrlPcduP60Board(); ctrlPcduAcu(); @@ -1599,9 +1579,11 @@ void ThermalController::ctrlComponentTemperature(HeaterContext& htrCtx) { // No sensors available, so switch the heater off. We can not perform control tasks if we // are blind.. if (chooseHeater(htrCtx.switchNr, htrCtx.redSwitchNr)) { - if (heaterCtrlAllowed()) { - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, currThermalComponent); - } + // Also track the counter to prevent heater handler message spam. The heater handle can only + // process 2 messages per cycle. + if (heaterCtrlAllowed() and (thermalStates[currThermalComponent].noSensorAvailableCounter < 3)) { + heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, currThermalComponent); + } } } resetSensorsArray(); @@ -1612,18 +1594,18 @@ bool ThermalController::selectAndReadSensorTemp(HeaterContext& htrCtx) { sensors[i].second > SANITY_LIMIT_LOWER_TEMP and sensors[i].second < SANITY_LIMIT_UPPER_TEMP) { sensorTemp = sensors[i].second; - thermalStates[currThermalComponent].errorCounter = 0; + thermalStates[currThermalComponent].noSensorAvailableCounter = 0; return true; } } - thermalStates[currThermalComponent].errorCounter++; + thermalStates[currThermalComponent].noSensorAvailableCounter++; if (currThermalComponent != RW and currThermalComponent != ACS_BOARD) { - if (thermalStates[currThermalComponent].errorCounter <= 3) { + if (thermalStates[currThermalComponent].noSensorAvailableCounter <= 3) { triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, currThermalComponent); } } else { - if (thermalStates[currThermalComponent].errorCounter <= 8) { + if (thermalStates[currThermalComponent].noSensorAvailableCounter <= 8) { triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, currThermalComponent); } } diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index b571b950..24a016c3 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -47,7 +47,7 @@ struct TempLimits { }; struct ThermalState { - uint8_t errorCounter; + uint8_t noSensorAvailableCounter; // Is heating on for that thermal module? bool heating = false; heater::Switch heaterSwitch = heater::Switch::NUMBER_OF_SWITCHES; @@ -74,7 +74,6 @@ enum ThermalComponents : uint8_t { IF_BOARD = 5, TCS_BOARD = 6, OBC = 7, - OBCIF_BOARD = 8, SBAND_TRANSCEIVER = 9, PCDUP60_BOARD = 10, PCDUACU = 11, @@ -331,7 +330,6 @@ class ThermalController : public ExtendedControllerBase { void ctrlIfBoard(); void ctrlTcsBoard(); void ctrlObc(); - void ctrlObcIfBoard(); void ctrlSBandTransceiver(); void ctrlPcduP60Board(); void ctrlPcduAcu(); From 7c4f98ec223618534edc03f135ed90ff95c6f61d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 10 Jul 2023 15:23:04 +0200 Subject: [PATCH 497/506] add back with other name and comment --- mission/controller/ThermalController.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 24a016c3..9df9a7a0 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -74,6 +74,8 @@ enum ThermalComponents : uint8_t { IF_BOARD = 5, TCS_BOARD = 6, OBC = 7, + // Not used anymore, was identical to OBC module. + LEGACY_OBCIF_BOARD = 8, SBAND_TRANSCEIVER = 9, PCDUP60_BOARD = 10, PCDUACU = 11, From 84e37d8b7b11cc6554d7b0b16fdd4de6f0322501 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 10 Jul 2023 16:30:18 +0200 Subject: [PATCH 498/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 15d25b4c..ba47757b 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 15d25b4c5b7ad21603a83a6c09835f5c97273b17 +Subproject commit ba47757b508976943f2e042a8f42673ba1424a28 From 53d53e8771b45ac6ad3b000987e60ca8856970a8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 10 Jul 2023 16:33:29 +0200 Subject: [PATCH 499/506] disable test case --- dummies/TemperatureSensorInserter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dummies/TemperatureSensorInserter.cpp b/dummies/TemperatureSensorInserter.cpp index bce48606..942231f5 100644 --- a/dummies/TemperatureSensorInserter.cpp +++ b/dummies/TemperatureSensorInserter.cpp @@ -15,7 +15,7 @@ TemperatureSensorInserter::TemperatureSensorInserter( tmp1075DummyMap(std::move(tempTmpSensorDummies_)) {} ReturnValue_t TemperatureSensorInserter::initialize() { - testCase = TestCase::COLD_CAMERA_STAYS_COLD; + testCase = TestCase::NONE; return returnvalue::OK; } From c7f716c433199c18bd281b9c71456140895df605 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 10 Jul 2023 17:51:37 +0200 Subject: [PATCH 500/506] fsfw bump and changelog --- CHANGELOG.md | 2 ++ fsfw | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 77862553..1bf7fe9f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -40,6 +40,8 @@ will consitute of a breaking change warranting a new major release: - TMP1075: Devices did not go to OFF mode when being set faulty. - Update PL PCDU 1 in TCS mode tree on the EM. - TMP1075: Possibly ignored health commands. +- Bugfix in FSFW where certain packet types could only be sent with source data fields with a + maximum size of 255 bytes. # Added diff --git a/fsfw b/fsfw index 8da89eba..88e86652 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 8da89eba80f73cb05e5c38fc012456f1d9569af5 +Subproject commit 88e8665280a0381c41b724ab035a8c3eff1a23c1 From fdf1c7a61164d6cb46361a1bbb5160184615e056 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 10 Jul 2023 18:29:55 +0200 Subject: [PATCH 501/506] seems to work now --- mission/controller/ThermalController.cpp | 6 ++---- mission/controller/tcsDefs.h | 2 +- mission/tcs/defs.h | 3 ++- tmtc | 2 +- 4 files changed, 6 insertions(+), 7 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index b5807d0a..2c5e2c9e 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1816,8 +1816,7 @@ void ThermalController::heaterMaxDurationControl( heaterStates[i].trackHeaterMaxBurnTime = false; triggerEvent(tcsCtrl::TCS_HEATER_MAX_BURN_TIME_REACHED, static_cast(i), MAX_HEATER_ON_DURATIONS_MS[i]); - heaterSwitchHelper(static_cast(i), heater::SwitchState::OFF, - std::nullopt); + heaterSwitchHelper(static_cast(i), heater::SwitchState::OFF, std::nullopt); // The heater might still be one for some thermal components, so cross-check // those components crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(static_cast(i)); @@ -1867,8 +1866,7 @@ void ThermalController::resetThermalStates() { } } -void ThermalController::heaterSwitchHelper(heater::Switch switchNr, - heater::SwitchState targetState, +void ThermalController::heaterSwitchHelper(heater::Switch switchNr, heater::SwitchState targetState, std::optional componentIdx) { timeval currentTime; Clock::getClockMonotonic(¤tTime); diff --git a/mission/controller/tcsDefs.h b/mission/controller/tcsDefs.h index 48f5f605..af03f146 100644 --- a/mission/controller/tcsDefs.h +++ b/mission/controller/tcsDefs.h @@ -40,7 +40,7 @@ struct ThermalState { // Is heating on for that thermal module? bool heating = false; // Which switch is being used for heating the component - heater::Switch heaterSwitch = heater::Switch::NUMBER_OF_SWITCHES; + heater::Switch heaterSwitch = heater::Switch::HEATER_NONE; // Heater start time and end times as UNIX seconds. Please note that these times will be updated // when a switch command is sent, with no guarantess that the heater actually went on. uint32_t heaterStartTime = 0; diff --git a/mission/tcs/defs.h b/mission/tcs/defs.h index e9f59e1a..e2843570 100644 --- a/mission/tcs/defs.h +++ b/mission/tcs/defs.h @@ -14,7 +14,8 @@ enum Switch : uint8_t { HEATER_5_STR, HEATER_6_DRO, HEATER_7_S_BAND, - NUMBER_OF_SWITCHES + NUMBER_OF_SWITCHES = 8, + HEATER_NONE = 0xff }; enum SwitchState : uint8_t { ON = 1, OFF = 0 }; diff --git a/tmtc b/tmtc index ba47757b..e27ad147 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit ba47757b508976943f2e042a8f42673ba1424a28 +Subproject commit e27ad147d4a3e8090f5a3efa18c1abbb0912a4b3 From 852f1203c64477b8f8527f09883e22930d4fb22f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 11 Jul 2023 09:39:21 +0200 Subject: [PATCH 502/506] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 62b7a7c3..9b80df07 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -28,6 +28,7 @@ will consitute of a breaking change warranting a new major release: components (no sensors available), irrespective of current switch state. - Make OBSW compatible to prospective FW version v5.0.0, where the Q7 I2C devices were moved to a PL I2C block and the TMP sensor devices were moved to the PS I2C0. +- Made `Xadc` code a little bit more robust against errors. ## Fixed From ed7d75a777eead05d90c1374282efc9882c4fa48 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 11 Jul 2023 10:44:46 +0200 Subject: [PATCH 503/506] thermal controller --- mission/controller/ThermalController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index c21d3e08..49fa7e20 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1818,7 +1818,7 @@ bool ThermalController::heaterCtrlAllowed() const { return submode != SUBMODE_NO void ThermalController::resetThermalStates() { for (auto& thermalState : thermalStates) { thermalState.heating = false; - thermalState.errorCounter = 0; + thermalState.noSensorAvailableCounter = 0; thermalState.heaterStartTime = 0; thermalState.heaterEndTime = 0; thermalState.sensorIndex = 0; From 46bb602c962e87d8b86bbc1c91e64b44270181bc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 11 Jul 2023 11:01:32 +0200 Subject: [PATCH 504/506] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index e27ad147..9be81f17 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit e27ad147d4a3e8090f5a3efa18c1abbb0912a4b3 +Subproject commit 9be81f1725004b55e937718fbaddc4f4e4e74081 From 13e965948ccf7206a3a1cc5c1f71cf28f0e3c97d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 11 Jul 2023 13:52:41 +0200 Subject: [PATCH 505/506] TCS observability --- bsp_q7s/xadc/Xadc.cpp | 2 +- mission/controller/ThermalController.cpp | 11 ++-- mission/controller/ThermalController.h | 74 ------------------------ mission/controller/tcsDefs.h | 4 +- 4 files changed, 9 insertions(+), 82 deletions(-) diff --git a/bsp_q7s/xadc/Xadc.cpp b/bsp_q7s/xadc/Xadc.cpp index 43fc7b75..da3fbf7f 100644 --- a/bsp_q7s/xadc/Xadc.cpp +++ b/bsp_q7s/xadc/Xadc.cpp @@ -139,7 +139,7 @@ ReturnValue_t Xadc::readValFromFile(const char* filename, T& val) { } std::istringstream valSstream(valstring); valSstream >> val; - if(valSstream.bad()) { + if (valSstream.bad()) { sif::warning << "Xadc: Conversion of value to target type failed" << std::endl; fclose(fp); return returnvalue::FAILED; diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index b87540ec..553dd9c2 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1603,9 +1603,10 @@ void ThermalController::ctrlComponentTemperature(HeaterContext& htrCtx) { if (chooseHeater(htrCtx.switchNr, htrCtx.redSwitchNr)) { // Also track the counter to prevent heater handler message spam. The heater handle can only // process 2 messages per cycle. - if (heaterCtrlAllowed() and (thermalStates[currThermalComponent].noSensorAvailableCounter < 3)) { - heaterSwitchHelper(htrCtx.switchNr, heater::SwitchState::OFF, currThermalComponent); - } + if (heaterCtrlAllowed() and + (thermalStates[ctrlCtx.thermalComponent].noSensorAvailableCounter < 3)) { + heaterSwitchHelper(htrCtx.switchNr, heater::SwitchState::OFF, ctrlCtx.thermalComponent); + } } } resetSensorsArray(); @@ -1622,8 +1623,8 @@ bool ThermalController::selectAndReadSensorTemp(HeaterContext& htrCtx) { } } - thermalStates[currThermalComponent].noSensorAvailableCounter++; - if (currThermalComponent != RW and currThermalComponent != ACS_BOARD) { + thermalStates[ctrlCtx.thermalComponent].noSensorAvailableCounter++; + if (ctrlCtx.thermalComponent != tcsCtrl::RW and ctrlCtx.thermalComponent != tcsCtrl::ACS_BOARD) { if (thermalStates[ctrlCtx.thermalComponent].noSensorAvailableCounter <= 3) { triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, ctrlCtx.thermalComponent); } diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 508b49cd..1062fe97 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -26,80 +26,6 @@ #include #include -/** - * NOP Limit: Hard limit for device, usually from datasheet. Device damage is possible lif NOP limit - * is exceeded. - * OP Limit: Soft limit. Device should be switched off or TCS controller should take action if the - * limit is exceeded to avoid reaching NOP limit - */ -struct TempLimits { - TempLimits(float nopLowerLimit, float opLowerLimit, float cutOffLimit, float opUpperLimit, - float nopUpperLimit) - : opLowerLimit(opLowerLimit), - opUpperLimit(opUpperLimit), - cutOffLimit(cutOffLimit), - nopLowerLimit(nopLowerLimit), - nopUpperLimit(nopUpperLimit) {} - float opLowerLimit; - float opUpperLimit; - float cutOffLimit; - float nopLowerLimit; - float nopUpperLimit; -}; - -struct ThermalState { - uint8_t noSensorAvailableCounter; - // Which sensor is used for this component? - uint8_t sensorIndex = 0; - // Is heating on for that thermal module? - bool heating = false; - // Which switch is being used for heating the component - heater::Switch heaterSwitch = heater::Switch::NUMBER_OF_SWITCHES; - // Heater start time and end times as UNIX seconds. Please note that these times will be updated - // when a switch command is sent, with no guarantess that the heater actually went on. - uint32_t heaterStartTime = 0; - uint32_t heaterEndTime = 0; -}; - -struct HeaterState { - bool switchTransition = false; - HeaterHandler::SwitchState target = HeaterHandler::SwitchState::OFF; - uint8_t heaterSwitchControlCycles = 0; - bool trackHeaterMaxPeriod = false; - Countdown heaterOnPeriod; -}; - -using HeaterSwitchStates = std::array; - -enum ThermalComponents : uint8_t { - NONE = 0, - ACS_BOARD = 1, - MGT = 2, - RW = 3, - STR = 4, - IF_BOARD = 5, - TCS_BOARD = 6, - OBC = 7, - // Not used anymore, was identical to OBC module. - LEGACY_OBCIF_BOARD = 8, - SBAND_TRANSCEIVER = 9, - PCDUP60_BOARD = 10, - PCDUACU = 11, - PCDUPDU = 12, - PLPCDU_BOARD = 13, - PLOCMISSION_BOARD = 14, - PLOCPROCESSING_BOARD = 15, - DAC = 16, - CAMERA = 17, - DRO = 18, - X8 = 19, - HPA = 20, - TX = 21, - MPA = 22, - SCEX_BOARD = 23, - NUM_ENTRIES -}; - class ThermalController : public ExtendedControllerBase { public: static constexpr uint8_t SUBMODE_NO_HEATER_CTRL = 1; diff --git a/mission/controller/tcsDefs.h b/mission/controller/tcsDefs.h index af03f146..f5d9d180 100644 --- a/mission/controller/tcsDefs.h +++ b/mission/controller/tcsDefs.h @@ -34,7 +34,7 @@ struct TempLimits { * Abstraction for the state of a single thermal component */ struct ThermalState { - uint8_t errorCounter; + uint8_t noSensorAvailableCounter; // Which sensor is used for this component? uint8_t sensorIndex = 0; // Is heating on for that thermal module? @@ -69,7 +69,7 @@ enum ThermalComponents : uint8_t { IF_BOARD = 5, TCS_BOARD = 6, OBC = 7, - OBCIF_BOARD = 8, + LEGACY_OBCIF_BOARD = 8, SBAND_TRANSCEIVER = 9, PCDUP60_BOARD = 10, PCDUACU = 11, From 54309b512f87a0cc0fea406961226168486d9056 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 11 Jul 2023 17:29:27 +0200 Subject: [PATCH 506/506] use correct clock API --- mission/controller/ThermalController.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 553dd9c2..5f2277a9 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1852,7 +1852,7 @@ void ThermalController::resetThermalStates() { void ThermalController::heaterSwitchHelper(heater::Switch switchNr, heater::SwitchState targetState, std::optional componentIdx) { timeval currentTime; - Clock::getClockMonotonic(¤tTime); + Clock::getClock(¤tTime); if (targetState == heater::SwitchState::ON) { heaterHandler.switchHeater(switchNr, targetState); heaterStates[switchNr].target = heater::SwitchState::ON; @@ -1879,7 +1879,7 @@ void ThermalController::heaterSwitchHelper(heater::Switch switchNr, heater::Swit void ThermalController::heaterSwitchHelperAllOff() { timeval currentTime; - Clock::getClockMonotonic(¤tTime); + Clock::getClock(¤tTime); size_t idx = 0; for (; idx < heater::Switch::NUMBER_OF_SWITCHES; idx++) { heaterHandler.switchHeater(static_cast(idx), heater::SwitchState::OFF); @@ -1901,7 +1901,7 @@ void ThermalController::crossCheckHeaterStateOfComponentsWhenHeaterGoesOff( for (unsigned j = 0; j < thermalStates.size(); j++) { if (thermalStates[j].heating and thermalStates[j].heaterSwitch == switchIdx) { timeval currentTime; - Clock::getClockMonotonic(¤tTime); + Clock::getClock(¤tTime); thermalStates[j].heating = false; thermalStates[j].heaterEndTime = currentTime.tv_sec; }