From f50b9de4a5acf392ab50a300fb6b5fc2d6de7502 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 8 Aug 2021 01:24:18 +0200 Subject: [PATCH] extneded state machine for commanding support --- bsp_q7s/core/CoreController.cpp | 163 +++++++++++++++++++++++++------- bsp_q7s/core/CoreController.h | 13 ++- 2 files changed, 138 insertions(+), 38 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 6bc0abbb..b484c4cb 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -135,13 +135,27 @@ ReturnValue_t CoreController::initSdCardBlocking() { } ReturnValue_t CoreController::sdStateMachine() { -#if Q7S_SD_CARD_CONFIG == Q7S_SD_NONE - sif::info << "No SD card initialization will be performed" << std::endl; - return HasReturnvaluesIF::RETURN_OK; -#else ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; SdCardManager::Operations operation; + if(sdInfo.state == SdStates::IDLE) { + // Nothing to do + return result; + } + + if(sdInfo.state == SdStates::START) { +#if Q7S_SD_CARD_CONFIG == Q7S_SD_NONE + sif::info << "No SD card initialization will be performed" << std::endl; + sdInfo.state == SdStates::IDLE; + sdInfo.initFinished = true; +#else + sdInfo.cycleCount = 0; + sdInfo.commandExecuted = false; + sdInfo.state = SdStates::GET_INFO; + } + + // This lambda checks the non-blocking operation and assigns the new state on success. + // It returns true for an operation success and false otherwise auto nonBlockingOpChecking = [&](SdStates newStateOnSuccess, uint16_t maxCycleCount, std::string opPrintout) { SdCardManager::OpStatus status = sdcMan->checkCurrentOp(operation); @@ -149,28 +163,16 @@ ReturnValue_t CoreController::sdStateMachine() { sdInfo.state = newStateOnSuccess; sdInfo.commandExecuted = false; sdInfo.cycleCount = 0; + return true; } else if(sdInfo.cycleCount > 4) { sif::warning << "CoreController::sdInitStateMachine: " << opPrintout << " takes too long" << std::endl; + return false; } + return false; }; - auto currentStateSetter = [&](sd::SdCard sdCard, sd::SdState newState) { - if(sdCard == sd::SdCard::SLOT_0) { - sdInfo.currentState.first = newState; - } - else { - sdInfo.currentState.second = newState; - } - }; - - if(sdInfo.state == SdStates::START) { - sdInfo.cycleCount = 0; - sdInfo.commandExecuted = false; - sdInfo.state = SdStates::GET_INFO; - } - if(sdInfo.state == SdStates::GET_INFO) { if(not sdInfo.commandExecuted) { // Create update status file @@ -222,9 +224,10 @@ ReturnValue_t CoreController::sdStateMachine() { sdInfo.commandExecuted = true; } else { - nonBlockingOpChecking(SdStates::DETERMINE_OTHER, 5, "Mounting SD card"); - sdInfo.prefState = sd::SdState::MOUNTED; - currentStateSetter(sdInfo.pref, sd::SdState::MOUNTED); + if(nonBlockingOpChecking(SdStates::DETERMINE_OTHER, 5, "Mounting SD card")) { + sdInfo.prefState = sd::SdState::MOUNTED; + currentStateSetter(sdInfo.pref, sd::SdState::MOUNTED); + } } } @@ -265,10 +268,11 @@ ReturnValue_t CoreController::sdStateMachine() { sdInfo.commandExecuted = true; } else { - nonBlockingOpChecking(SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE, 10, - "Switching off other SD card"); - sdInfo.otherState = sd::SdState::OFF; - currentStateSetter(sdInfo.other, sd::SdState::OFF); + if(nonBlockingOpChecking(SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE, 10, + "Switching off other SD card")) { + sdInfo.otherState = sd::SdState::OFF; + currentStateSetter(sdInfo.other, sd::SdState::OFF); + } } #elif Q7S_SD_CARD_CONFIG == Q7S_SD_HOT_REDUNDANT if(not sdInfo.commandExecuted) { @@ -282,7 +286,6 @@ ReturnValue_t CoreController::sdStateMachine() { currentStateSetter(sdInfo.other, sd::SdState::ON); } #endif - } if(sdInfo.state == SdStates::MOUNT_UNMOUNT_OTHER) { @@ -293,9 +296,10 @@ ReturnValue_t CoreController::sdStateMachine() { sdInfo.commandExecuted = true; } else { - nonBlockingOpChecking(SdStates::SET_STATE_OTHER, 10, "Unmounting other SD card"); - sdInfo.otherState = sd::SdState::ON; - currentStateSetter(sdInfo.other, sd::SdState::ON); + if(nonBlockingOpChecking(SdStates::SET_STATE_OTHER, 10, "Unmounting other SD card")) { + sdInfo.otherState = sd::SdState::ON; + currentStateSetter(sdInfo.other, sd::SdState::ON); + } } #elif Q7S_SD_CARD_CONFIG == Q7S_SD_HOT_REDUNDANT if(not sdInfo.commandExecuted) { @@ -303,9 +307,10 @@ ReturnValue_t CoreController::sdStateMachine() { sdInfo.commandExecuted = true; } else { - nonBlockingOpChecking(SdStates::UPDATE_INFO, 4, "Mounting other SD card"); - sdInfo.otherState = sd::SdState::MOUNTED; - currentStateSetter(sdInfo.other, sd::SdState::MOUNTED); + if(nonBlockingOpChecking(SdStates::UPDATE_INFO, 4, "Mounting other SD card")) { + sdInfo.otherState = sd::SdState::MOUNTED; + currentStateSetter(sdInfo.other, sd::SdState::MOUNTED); + } } #endif } @@ -325,7 +330,7 @@ ReturnValue_t CoreController::sdStateMachine() { << std::endl; } sdInfo.commandExecuted = false; - sdInfo.state = SdStates::DONE; + sdInfo.state = SdStates::IDLE; sdInfo.cycleCount = 0; sdcMan->setBlocking(false); sdcMan->getSdCardActiveStatus(sdInfo.currentState); @@ -336,11 +341,100 @@ ReturnValue_t CoreController::sdStateMachine() { } } + if(sdInfo.state == SdStates::SET_STATE_FROM_COMMAND) { + if(not sdInfo.commandExecuted) { + executeNextExternalSdCommand(); + } + else { + checkExternalSdCommandStatus(); + } + } + sdInfo.cycleCount++; return HasReturnvaluesIF::RETURN_OK; #endif /* Q7S_SD_CARD_CONFIG != Q7S_SD_NONE */ } +void CoreController::executeNextExternalSdCommand() { + std::string sdChar; + sd::SdState currentStateOfCard = sd::SdState::OFF; + if(sdInfo.commandedCard == sd::SdCard::SLOT_0) { + sdChar = "0"; + currentStateOfCard = sdInfo.currentState.first; + } + else { + sdChar = "1"; + currentStateOfCard = sdInfo.currentState.second; + } + if(currentStateOfCard == sd::SdState::OFF) { + if(sdInfo.commandedState == sd::SdState::ON) { + sdInfo.currentlyCommandedState = sdInfo.commandedState; + } + else if(sdInfo.commandedState == sd::SdState::MOUNTED) { + sdInfo.currentlyCommandedState = sd::SdState::ON; + } + else { + // SD card is already on target state + sdInfo.commandFinished = true; + sdInfo.state = SdStates::IDLE; + } + } + else if(currentStateOfCard == sd::SdState::ON) { + if(sdInfo.commandedState == sd::SdState::OFF or + sdInfo.commandedState == sd::SdState::MOUNTED) { + sdInfo.currentlyCommandedState = sdInfo.commandedState; + } + else { + // Already on target state + sdInfo.commandFinished = true; + sdInfo.state = SdStates::IDLE; + } + } + else if(currentStateOfCard == sd::SdState::MOUNTED) { + if(sdInfo.commandedState == sd::SdState::ON) { + sdInfo.currentlyCommandedState = sdInfo.commandedState; + } + else if(sdInfo.commandedState == sd::SdState::OFF) { + // This causes an unmount in sdCardSetup + sdInfo.currentlyCommandedState = sd::SdState::ON; + } + else { + sdInfo.commandFinished = true; + } + } + sdCardSetup(sdInfo.commandedCard, sdInfo.commandedState, sdChar); + sdInfo.commandExecuted = true; +} + +void CoreController::checkExternalSdCommandStatus() { + SdCardManager::Operations operation; + SdCardManager::OpStatus status = sdcMan->checkCurrentOp(operation); + if(status == SdCardManager::OpStatus::SUCCESS) { + if(sdInfo.currentlyCommandedState == sdInfo.commandedState) { + sdInfo.state = SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE; + sdInfo.commandFinished = true; + } + else { + // stay on same state machine state because the target state was not reached yet. + sdInfo.cycleCount = 0; + } + currentStateSetter(sdInfo.commandedCard, sdInfo.currentlyCommandedState); + sdInfo.commandExecuted = false; + } + else if(sdInfo.cycleCount > 4) { + sif::warning << "CoreController::sdStateMachine: Commanding SD state " + "takes too long" << std::endl; + } +} + +void CoreController::currentStateSetter(sd::SdCard sdCard, sd::SdState newState) { + if(sdCard == sd::SdCard::SLOT_0) { + sdInfo.currentState.first = newState; + } + else { + sdInfo.currentState.second = newState; + } +} ReturnValue_t CoreController::sdCardSetup(sd::SdCard sdCard, sd::SdState targetState, std::string sdChar, bool printOutput) { @@ -610,6 +704,7 @@ ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId, if(RFlag) { oss << "R"; } + oss << " " << repoName << " > " << targetFileName; int result = std::system(oss.str().c_str()); if(result != 0) { diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index da587807..b26c99c5 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -77,12 +77,9 @@ private: SKIP_CYCLE_BEFORE_INFO_UPDATE, UPDATE_INFO, // SD initialization done - DONE, + IDLE, // Used if SD switches or mount commands are issued via telecommand SET_STATE_FROM_COMMAND, - MOUNT_FROM_COMMAND, - UNMOUNT_FROM_COMMAND, - }; static constexpr bool BLOCKING_SD_INIT = false; @@ -99,10 +96,15 @@ private: std::string prefChar = "0"; std::string otherChar = "1"; SdStates state = SdStates::START; + // Used to track whether a command was executed bool commandExecuted = true; bool initFinished = false; SdCardManager::SdStatePair currentState; uint16_t cycleCount = 0; + // These two flags are related to external commanding + bool commandIssued = false; + bool commandFinished = false; + sd::SdState currentlyCommandedState = sd::SdState::OFF; sd::SdCard commandedCard = sd::SdCard::NONE; sd::SdState commandedState = sd::SdState::OFF; }; @@ -113,7 +115,10 @@ private: ReturnValue_t sdCardSetup(sd::SdCard sdCard, sd::SdState targetState, std::string sdChar, bool printOutput = true); ReturnValue_t sdColdRedundantBlockingInit(); + void currentStateSetter(sd::SdCard sdCard, sd::SdState newState); void determinePreferredSdCard(); + void executeNextExternalSdCommand(); + void checkExternalSdCommandStatus(); ReturnValue_t initVersionFile(); ReturnValue_t initBootCopy();