From 9270165bf8ef23ccaeefb56381d0f661bb0a5a2c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 4 Apr 2023 01:35:05 +0200 Subject: [PATCH] fix STR bugs and improve code - Reset reply size after returning a reply - Reset data link layer and flush RX for regular commands and before performing special commands to ensure consistent start state - Clean up DHB a bit - Set STR dev to OFF in assembly when it is faulty. --- linux/acs/StrComHandler.cpp | 14 +++- linux/acs/StrComHandler.h | 2 + mission/acs/str/StarTrackerHandler.cpp | 106 ++++++++++++------------- mission/system/acs/StrAssembly.cpp | 7 +- tmtc | 2 +- 5 files changed, 72 insertions(+), 59 deletions(-) diff --git a/linux/acs/StrComHandler.cpp b/linux/acs/StrComHandler.cpp index c0fe3c45..303c2228 100644 --- a/linux/acs/StrComHandler.cpp +++ b/linux/acs/StrComHandler.cpp @@ -42,7 +42,6 @@ ReturnValue_t StrComHandler::performOperation(uint8_t operationCode) { while (true) { lock->lockMutex(); state = InternalState::SLEEPING; - datalinkLayer.reset(); lock->unlockMutex(); semaphore.acquire(); switch (state) { @@ -58,6 +57,7 @@ ReturnValue_t StrComHandler::performOperation(uint8_t operationCode) { } case InternalState::UPLOAD_IMAGE: { replyTimeout.setTimeout(200); + resetReplyHandlingState(); result = performImageUpload(); if (result == returnvalue::OK) { triggerEvent(IMAGE_UPLOAD_SUCCESSFUL); @@ -68,6 +68,7 @@ ReturnValue_t StrComHandler::performOperation(uint8_t operationCode) { } case InternalState::DOWNLOAD_IMAGE: { replyTimeout.setTimeout(200); + resetReplyHandlingState(); result = performImageDownload(); if (result == returnvalue::OK) { triggerEvent(IMAGE_DOWNLOAD_SUCCESSFUL); @@ -78,6 +79,7 @@ ReturnValue_t StrComHandler::performOperation(uint8_t operationCode) { } case InternalState::FLASH_READ: { replyTimeout.setTimeout(200); + resetReplyHandlingState(); result = performFlashRead(); if (result == returnvalue::OK) { triggerEvent(FLASH_READ_SUCCESSFUL); @@ -88,6 +90,7 @@ ReturnValue_t StrComHandler::performOperation(uint8_t operationCode) { } case InternalState::FIRMWARE_UPDATE: { replyTimeout.setTimeout(200); + resetReplyHandlingState(); result = performFirmwareUpdate(); if (result == returnvalue::OK) { triggerEvent(FIRMWARE_UPDATE_SUCCESSFUL); @@ -645,7 +648,8 @@ ReturnValue_t StrComHandler::sendMessage(CookieIF* cookie, const uint8_t* sendDa return BUSY; } } - serial::flushRxBuf(serialPort); + // Ensure consistent state. + resetReplyHandlingState(); const uint8_t* txFrame; size_t frameLen; @@ -697,6 +701,7 @@ ReturnValue_t StrComHandler::readReceivedMessage(CookieIF* cookie, uint8_t** buf *buffer = const_cast(replyPtr); *size = replyLen; } + replyLen = 0; return replyResult; } @@ -781,3 +786,8 @@ ReturnValue_t StrComHandler::readOneReply(uint32_t failParameter) { } } } + +void StrComHandler::resetReplyHandlingState() { + serial::flushRxBuf(serialPort); + datalinkLayer.reset(); +} diff --git a/linux/acs/StrComHandler.h b/linux/acs/StrComHandler.h index 52210036..385e8b2e 100644 --- a/linux/acs/StrComHandler.h +++ b/linux/acs/StrComHandler.h @@ -374,6 +374,8 @@ class StrComHandler : public SystemObject, public DeviceCommunicationIF, public * @return */ ReturnValue_t readOneReply(uint32_t failParameter); + + void resetReplyHandlingState(); }; #endif /* BSP_Q7S_DEVICES_STRHELPER_H_ */ diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 5392a600..52751f24 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -56,6 +56,49 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, StarTrackerHandler::~StarTrackerHandler() {} +void StarTrackerHandler::doStartUp() { + switch (startupState) { + case StartupState::IDLE: + startupState = StartupState::CHECK_PROGRAM; + return; + case StartupState::BOOT_BOOTLOADER: + // This step is required in case the star tracker is already in firmware mode to harmonize + // the device handler's submode to the star tracker's mode + return; + case StartupState::DONE: + if (jcfgCountdown.isBusy()) { + startupState = StartupState::WAIT_JCFG; + return; + } + startupState = StartupState::IDLE; + break; + case StartupState::WAIT_JCFG: { + if (jcfgCountdown.hasTimedOut()) { + startupState = StartupState::IDLE; + break; + } + return; + } + default: + return; + } + solutionSet.setReportingEnabled(true); + startupState = StartupState::DONE; + internalState = InternalState::IDLE; + setMode(_MODE_TO_ON); +} + +void StarTrackerHandler::doShutDown() { + // If the star tracker is shutdown also stop all running processes in the image loader task + strHelper->stopProcess(); + internalState = InternalState::IDLE; + startupState = StartupState::IDLE; + bootState = FwBootState::NONE; + solutionSet.setReportingEnabled(false); + reinitNextSetParam = false; + setMode(_MODE_POWER_DOWN); +} + ReturnValue_t StarTrackerHandler::initialize() { ReturnValue_t result = returnvalue::OK; result = DeviceHandlerBase::initialize(); @@ -236,49 +279,6 @@ void StarTrackerHandler::performOperationHook() { Submode_t StarTrackerHandler::getInitialSubmode() { return SUBMODE_BOOTLOADER; } -void StarTrackerHandler::doStartUp() { - switch (startupState) { - case StartupState::IDLE: - startupState = StartupState::CHECK_PROGRAM; - return; - case StartupState::BOOT_BOOTLOADER: - // This step is required in case the star tracker is already in firmware mode to harmonize - // the device handler's submode to the star tracker's mode - return; - case StartupState::DONE: - if (jcfgCountdown.isBusy()) { - startupState = StartupState::WAIT_JCFG; - return; - } - startupState = StartupState::IDLE; - break; - case StartupState::WAIT_JCFG: { - if (jcfgCountdown.hasTimedOut()) { - startupState = StartupState::IDLE; - break; - } - return; - } - default: - return; - } - solutionSet.setReportingEnabled(true); - startupState = StartupState::DONE; - internalState = InternalState::IDLE; - setMode(_MODE_TO_ON); -} - -void StarTrackerHandler::doShutDown() { - // If the star tracker is shutdown also stop all running processes in the image loader task - strHelper->stopProcess(); - internalState = InternalState::IDLE; - startupState = StartupState::IDLE; - bootState = FwBootState::NONE; - solutionSet.setReportingEnabled(false); - reinitNextSetParam = false; - setMode(_MODE_POWER_DOWN); -} - ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { if (strHelperHandlingSpecialRequest) { return NOTHING_TO_SEND; @@ -831,7 +831,6 @@ void StarTrackerHandler::bootBootloader() { ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { ReturnValue_t result = returnvalue::OK; - size_t bytesLeft = 0; if (remainingSize == 0) { *foundLen = remainingSize; @@ -845,24 +844,24 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t* start, size_t rema switch (startracker::getReplyFrameType(start)) { case TMTC_ACTIONREPLY: { - *foundLen = remainingSize - bytesLeft; - result = scanForActionReply(startracker::getId(start), foundId); + *foundLen = remainingSize; + return scanForActionReply(startracker::getId(start), foundId); break; } case TMTC_SETPARAMREPLY: { - *foundLen = remainingSize - bytesLeft; - result = scanForSetParameterReply(startracker::getId(start), foundId); + *foundLen = remainingSize; + return scanForSetParameterReply(startracker::getId(start), foundId); break; } case TMTC_PARAMREPLY: { - *foundLen = remainingSize - bytesLeft; - result = scanForGetParameterReply(startracker::getId(start), foundId); + *foundLen = remainingSize; + return scanForGetParameterReply(startracker::getId(start), foundId); break; } case TMTC_TELEMETRYREPLYA: case TMTC_TELEMETRYREPLY: { - *foundLen = remainingSize - bytesLeft; - result = scanForTmReply(startracker::getId(start), foundId); + *foundLen = remainingSize; + return scanForTmReply(startracker::getId(start), foundId); break; } default: { @@ -870,9 +869,6 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t* start, size_t rema result = returnvalue::FAILED; } } - - remainingSize = bytesLeft; - return result; } diff --git a/mission/system/acs/StrAssembly.cpp b/mission/system/acs/StrAssembly.cpp index 10ca5759..abd7a4ce 100644 --- a/mission/system/acs/StrAssembly.cpp +++ b/mission/system/acs/StrAssembly.cpp @@ -11,8 +11,13 @@ StrAssembly::StrAssembly(object_id_t objectId) : AssemblyBase(objectId) { } ReturnValue_t StrAssembly::commandChildren(Mode_t mode, Submode_t submode) { - commandTable[0].setMode(mode); + // To ensure consistent state. + commandTable[0].setMode(MODE_OFF); commandTable[0].setSubmode(submode); + if (healthHelper.healthTable->getHealth(objects::STAR_TRACKER) != FAULTY) { + commandTable[0].setMode(mode); + commandTable[0].setSubmode(submode); + } HybridIterator iter(commandTable.begin(), commandTable.end()); executeTable(iter); return returnvalue::OK; diff --git a/tmtc b/tmtc index 5f870924..0202c932 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 5f87092465b3d298b9f66410d333e8069e78df95 +Subproject commit 0202c93252d71e9acc420dbb357a26ea0c46b760