From 0de484e42308bcea7b1e88d64f7f3d2c8b9e4bcf Mon Sep 17 00:00:00 2001
From: "Jakob.Meier" <meierj@irs.uni-stuttgart.de>
Date: Wed, 9 Jun 2021 12:00:41 +0200
Subject: [PATCH] bug fixes in ImtqHandler

---
 mission/devices/IMTQHandler.cpp | 21 ++++++++++++++++++---
 mission/devices/IMTQHandler.h   |  3 ++-
 2 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp
index 124213a0..5d4e7d1d 100644
--- a/mission/devices/IMTQHandler.cpp
+++ b/mission/devices/IMTQHandler.cpp
@@ -18,13 +18,13 @@ IMTQHandler::~IMTQHandler() {
 
 
 void IMTQHandler::doStartUp(){
-	if(selfTestSuccess == true){
+	if(selfTestPerformed == true){
 #if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
         setMode(MODE_NORMAL);
 #else
         setMode(_MODE_TO_ON);
 #endif
-        selfTestSuccess = false;
+        selfTestPerformed = false;
 	}
 }
 
@@ -65,9 +65,11 @@ ReturnValue_t IMTQHandler::buildTransitionDeviceCommand(
         switch (startupStep) {
         case StartupStep::COMMAND_SELF_TEST:
             *id = IMTQ::SELF_TEST_CMD;
+            startupStep = StartupStep::GET_SELF_TEST_RESULT;
             break;
         case StartupStep::GET_SELF_TEST_RESULT:
             *id = IMTQ::GET_SELF_TEST_RESULT;
+            startupStep = StartupStep::NONE;
             break;
         default:
             sif::debug << "IMTQHandler::buildTransitionDeviceCommand: Unknown startup step"
@@ -148,6 +150,8 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(
 void IMTQHandler::fillCommandAndReplyMap() {
     this->insertInCommandAndReplyMap(IMTQ::SELF_TEST_CMD, 1, nullptr,
             IMTQ::SIZE_STATUS_REPLY);
+    this->insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr,
+            IMTQ::SIZE_SELF_TEST_RESULTS);
     this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr,
             IMTQ::SIZE_STATUS_REPLY);
     this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset,
@@ -192,6 +196,14 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start,
         *foundLen = IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT;
         *foundId = IMTQ::GET_RAW_MTM_MEASUREMENT;
 		break;
+	case(IMTQ::CC::SELF_TEST_CMD):
+        *foundLen = IMTQ::SIZE_STATUS_REPLY;
+        *foundId = IMTQ::SELF_TEST_CMD;
+		break;
+	case(IMTQ::CC::GET_SELF_TEST_RESULT):
+        *foundLen = IMTQ::SIZE_SELF_TEST_RESULTS;
+        *foundId = IMTQ::GET_SELF_TEST_RESULT;
+		break;
 	default:
         sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl;
         result = IGNORE_REPLY_DATA;
@@ -247,7 +259,7 @@ void IMTQHandler::setNormalDatapoolEntriesInvalid(){
 }
 
 uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){
-    return 1000;
+    return 5000;
 }
 
 ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
@@ -1060,6 +1072,9 @@ void IMTQHandler::handleSelfTestReply(const uint8_t* packet) {
     sif::info << "IMTQ self test (FINA) coil Z temperature: "
             << selfTestDataset.finaCoilZTemperature << " °C" << std::endl;
 #endif
+    if (mode == _MODE_START_UP) {
+        selfTestPerformed = true;
+    }
 }
 
 void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
diff --git a/mission/devices/IMTQHandler.h b/mission/devices/IMTQHandler.h
index 5565b261..dd5ae8cf 100644
--- a/mission/devices/IMTQHandler.h
+++ b/mission/devices/IMTQHandler.h
@@ -91,13 +91,14 @@ private:
 	CommunicationStep communicationStep = CommunicationStep::GET_ENG_HK_DATA;
 
 	enum class StartupStep {
+	    NONE,
 	    COMMAND_SELF_TEST,
 	    GET_SELF_TEST_RESULT
     };
 
 	StartupStep startupStep = StartupStep::COMMAND_SELF_TEST;
 
-	bool selfTestSuccess = false;
+	bool selfTestPerformed = false;
 
 	/**
 	 * @brief   Each reply contains a status byte giving information about a request. This function