From 365fa0adc415970e56203603fa6259a139da0321 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Sun, 8 Aug 2021 12:43:02 +0200
Subject: [PATCH] tweaks and fixes for state machine

---
 bsp_q7s/core/CoreController.cpp | 17 ++++++++++++-----
 1 file changed, 12 insertions(+), 5 deletions(-)

diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp
index 5f6c1ecc..f92e6d60 100644
--- a/bsp_q7s/core/CoreController.cpp
+++ b/bsp_q7s/core/CoreController.cpp
@@ -38,10 +38,11 @@ CoreController::CoreController(object_id_t objectId):
         if(sdcMan == nullptr) {
             sif::error << "CoreController::CoreController: SD card manager invalid!" << std::endl;
         }
+
         if(not BLOCKING_SD_INIT) {
-            sdStateMachine();
             sdcMan->setBlocking(false);
         }
+        sdStateMachine();
 
         result = initBootCopy();
         if(result != HasReturnvaluesIF::RETURN_OK) {
@@ -59,7 +60,7 @@ ReturnValue_t CoreController::handleCommandMessage(CommandMessage *message) {
 }
 
 void CoreController::performControlOperation() {
-    if(not BLOCKING_SD_INIT and not sdInfo.initFinished) {
+    if(not BLOCKING_SD_INIT) {
         sdStateMachine();
     }
     performWatchdogControlOperation();
@@ -83,9 +84,8 @@ ReturnValue_t CoreController::initialize() {
                 "count failed" << std::endl;
     }
 
-    if(not BLOCKING_SD_INIT) {
-        sdStateMachine();
-    }
+    sdStateMachine();
+
 
     return ExtendedControllerBase::initialize();
 }
@@ -150,6 +150,13 @@ ReturnValue_t CoreController::sdStateMachine() {
         sdInfo.initFinished = true;
         return result;
 #else
+        // Init will be performed by separate function
+        if(not BLOCKING_SD_INIT) {
+            sdInfo.state = SdStates::IDLE;
+            sdInfo.initFinished = true;
+            return result;
+        }
+#endif
         sdInfo.cycleCount = 0;
         sdInfo.commandExecuted = false;
         sdInfo.state = SdStates::GET_INFO;