add more mode support for heaters

mission/devices/HeaterHandler.cpp

@@ -8,11 +8,13 @@
 #include <stdexcept>
 
 #include "devices/powerSwitcherList.h"
+#include "fsfw/subsystem/helper.h"
 
 HeaterHandler::HeaterHandler(object_id_t setObjectId_, GpioIF* gpioInterface_, HeaterHelper helper,
                              PowerSwitchIF* mainLineSwitcher_, power::Switch_t mainLineSwitch_)
     : SystemObject(setObjectId_),
       helper(helper),
+      modeHelper(this),
       gpioInterface(gpioInterface_),
       mainLineSwitcher(mainLineSwitcher_),
       mainLineSwitch(mainLineSwitch_),
@@ -46,6 +48,12 @@ ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) {
       heater.first->performOperation(0);
     }
     handleSwitchHandling();
+    if (waitForSwitchOff) {
+      if (mainLineSwitcher->getSwitchState(mainLineSwitch) == SWITCH_OFF) {
+        waitForSwitchOff = false;
+        mode = MODE_OFF;
+      }
+    }
   } catch (const std::out_of_range& e) {
     sif::warning << "HeaterHandler::performOperation: "
                     "Out of range error | "
@@ -76,6 +84,10 @@ ReturnValue_t HeaterHandler::initialize() {
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
 
+  result = modeHelper.initialize();
+  if (result != returnvalue::OK) {
+    return ObjectManagerIF::CHILD_INIT_FAILED;
+  }
   return returnvalue::OK;
 }
 
@@ -218,6 +230,9 @@ void HeaterHandler::handleSwitchHandling() {
 void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
   ReturnValue_t result = returnvalue::OK;
   auto& heater = heaterVec.at(heaterIdx);
+  if (waitForSwitchOff) {
+    waitForSwitchOff = false;
+  }
   /* Check if command waits for main switch being set on and whether the timeout has expired */
   if (heater.waitMainSwitchOn && heater.mainSwitchCountdown.hasTimedOut()) {
     // TODO - This requires the initiation of an FDIR procedure
@@ -235,6 +250,7 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
   // Check state of main line switch
   ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch);
   if (mainSwitchState == PowerSwitchIF::SWITCH_ON) {
+    mode = HasModesIF::MODE_ON;
     if (checkSwitchState(heaterIdx) == SwitchState::OFF) {
       gpioId_t gpioId = heater.gpioId;
       result = gpioInterface->pullHigh(gpioId);
@@ -298,6 +314,7 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
       // When all switches are off, also main line switch will be turned off
       if (allSwitchesOff()) {
         mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
+        waitForSwitchOff = true;
       }
     }
   } else {
@@ -329,6 +346,38 @@ ReturnValue_t HeaterHandler::switchHeater(heater::Switchers heater, SwitchState
   return returnvalue::FAILED;
 }
 
+void HeaterHandler::announceMode(bool recursive) {
+  triggerEvent(MODE_INFO, mode, submode);
+  for (unsigned idx = 0; idx < helper.heaters.size(); idx++) {
+    if (heaterVec[idx].switchState == SWITCH_ON) {
+      EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_ON, 0);
+    } else {
+      EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_OFF,
+                                   0);
+    }
+  }
+}
+
+void HeaterHandler::getMode(Mode_t* mode, Submode_t* submode) {
+  if (!mode || !submode) {
+    return;
+  }
+  *mode = this->mode;
+  *submode = this->submode;
+}
+
+const HasHealthIF* HeaterHandler::getOptHealthIF() const { return nullptr; }
+
+const HasModesIF& HeaterHandler::getModeIF() const { return *this; }
+
+ReturnValue_t HeaterHandler::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
+  return modetree::connectModeTreeParent(parent, *this, nullptr, modeHelper);
+}
+
+ModeTreeChildIF& HeaterHandler::getModeTreeChildIF() { return *this; }
+
+object_id_t HeaterHandler::getObjectId() const { return SystemObject::getObjectId(); }
+
 bool HeaterHandler::allSwitchesOff() {
   bool allSwitchesOrd = false;
   MutexGuard mg(heaterMutex);