From 0e8a085f979c6aaa072b052a8c0283bd15393b60 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 13 Oct 2022 17:48:32 +0200
Subject: [PATCH] add debug mode

---
 .../devices/SolarArrayDeploymentHandler.cpp   | 73 ++++++++++++-------
 1 file changed, 47 insertions(+), 26 deletions(-)

diff --git a/mission/devices/SolarArrayDeploymentHandler.cpp b/mission/devices/SolarArrayDeploymentHandler.cpp
index e350c803..92283749 100644
--- a/mission/devices/SolarArrayDeploymentHandler.cpp
+++ b/mission/devices/SolarArrayDeploymentHandler.cpp
@@ -9,6 +9,8 @@
 #include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw_hal/common/gpio/GpioCookie.h"
 
+static constexpr bool DEBUG_MODE = true;
+
 SolarArrayDeploymentHandler::SolarArrayDeploymentHandler(object_id_t setObjectId_,
                                                          GpioIF& gpioInterface,
                                                          PowerSwitchIF& mainLineSwitcher_,
@@ -177,51 +179,59 @@ bool SolarArrayDeploymentHandler::autonomousDeplForFile(const char* filename, bo
   return true;
 }
 
-ReturnValue_t SolarArrayDeploymentHandler::initialize() {
-  ReturnValue_t result = actionHelper.initialize(commandQueue);
-  if (result != returnvalue::OK) {
-    return ObjectManagerIF::CHILD_INIT_FAILED;
-  }
-  return SystemObject::initialize();
-}
-
 void SolarArrayDeploymentHandler::handleStateMachine() {
   if (stateMachine == MAIN_POWER_ON) {
     mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
     mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs());
     stateMachine = WAIT_MAIN_POWER_ON;
+    if (DEBUG_MODE) {
+      sif::debug << "SA DEPL FSM: MAIN_POWER_ON done -> WAIT_MAIN_POWER_ON" << std::endl;
+    }
   }
   if (stateMachine == MAIN_POWER_OFF) {
     // These should never fail
     allOff();
     stateMachine = WAIT_MAIN_POWER_OFF;
+    if (DEBUG_MODE) {
+      sif::debug << "SA DEPL FSM: MAIN_POWER_OFF done -> WAIT_MAIN_POWER_OFF" << std::endl;
+    }
   }
   if (stateMachine == WAIT_MAIN_POWER_ON) {
     if (checkMainPowerOn()) {
+      if (DEBUG_MODE) {
+        sif::debug << "SA DEPL FSM: WAIT_MAIN_POWER_ON done -> SWITCH_DEPL_GPIOS" << std::endl;
+      }
       stateMachine = SWITCH_DEPL_GPIOS;
     }
   }
   if (stateMachine == WAIT_MAIN_POWER_OFF) {
     if (checkMainPowerOff()) {
+      if (DEBUG_MODE) {
+        sif::debug << "SA DEPL FSM: WAIT_MAIN_POWER_OFF done -> FSM DONE" << std::endl;
+      }
       finishFsm(returnvalue::OK);
     }
   }
   if (stateMachine == SWITCH_DEPL_GPIOS) {
     burnCountdown.setTimeout(fsmInfo.burnCountdown);
     // This should never fail
+    channelAlternationCd.resetTimer();
     if (not fsmInfo.dryRun) {
-      channelAlternationCd.resetTimer();
       sa2Off();
       sa1On();
       fsmInfo.alternationDummy = true;
     }
+    if (DEBUG_MODE) {
+      sif::debug << "SA DEPL FSM: SWITCH_DEPL_GPIOS done -> BURNING" << std::endl;
+    }
     stateMachine = BURNING;
   }
   if (stateMachine == BURNING) {
-    if (not fsmInfo.dryRun) {
-      saGpioAlternation();
-    }
+    saGpioAlternation();
     if (burnCountdown.hasTimedOut()) {
+      if (DEBUG_MODE) {
+        sif::debug << "SA DEPL FSM: BURNING done -> WAIT_MAIN_POWER_OFF" << std::endl;
+      }
       allOff();
       stateMachine = WAIT_MAIN_POWER_OFF;
     }
@@ -285,6 +295,9 @@ void SolarArrayDeploymentHandler::startFsmOff() {
 void SolarArrayDeploymentHandler::finishFsm(ReturnValue_t resultForActionHelper) {
   retryCounter = 0;
   stateMachine = StateMachine::IDLE;
+  fsmInfo.dryRun = false;
+  fsmInfo.onOff = false;
+  fsmInfo.alternationDummy = false;
   if (actionActive) {
     bool success = false;
     if (resultForActionHelper == returnvalue::OK or
@@ -315,19 +328,6 @@ bool SolarArrayDeploymentHandler::dryRunStringInFile(const char* filename) {
   return false;
 }
 
-void SolarArrayDeploymentHandler::readCommandQueue() {
-  CommandMessage command;
-  ReturnValue_t result = commandQueue->receiveMessage(&command);
-  if (result != returnvalue::OK) {
-    return;
-  }
-
-  result = actionHelper.handleActionMessage(&command);
-  if (result == returnvalue::OK) {
-    return;
-  }
-}
-
 ReturnValue_t SolarArrayDeploymentHandler::executeAction(ActionId_t actionId,
                                                          MessageQueueId_t commandedBy,
                                                          const uint8_t* data, size_t size) {
@@ -363,7 +363,7 @@ ReturnValue_t SolarArrayDeploymentHandler::executeAction(ActionId_t actionId,
 ReturnValue_t SolarArrayDeploymentHandler::saGpioAlternation() {
   ReturnValue_t status = returnvalue::OK;
   ReturnValue_t result;
-  if (channelAlternationCd.hasTimedOut()) {
+  if (channelAlternationCd.hasTimedOut() and not fsmInfo.dryRun) {
     if (fsmInfo.alternationDummy) {
       result = sa1Off();
       if (result != returnvalue::OK) {
@@ -454,6 +454,27 @@ ReturnValue_t SolarArrayDeploymentHandler::sa2Off() {
   return result;
 }
 
+void SolarArrayDeploymentHandler::readCommandQueue() {
+  CommandMessage command;
+  ReturnValue_t result = commandQueue->receiveMessage(&command);
+  if (result != returnvalue::OK) {
+    return;
+  }
+
+  result = actionHelper.handleActionMessage(&command);
+  if (result == returnvalue::OK) {
+    return;
+  }
+}
+
 MessageQueueId_t SolarArrayDeploymentHandler::getCommandQueue() const {
   return commandQueue->getId();
 }
+
+ReturnValue_t SolarArrayDeploymentHandler::initialize() {
+  ReturnValue_t result = actionHelper.initialize(commandQueue);
+  if (result != returnvalue::OK) {
+    return ObjectManagerIF::CHILD_INIT_FAILED;
+  }
+  return SystemObject::initialize();
+}