Merge branch 'develop' into bugfixes_code_review_msg_handling

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_),
@@ -28,8 +30,8 @@ HeaterHandler::HeaterHandler(object_id_t setObjectId_, GpioIF* gpioInterface_, H
   if (mainLineSwitcher == nullptr) {
     throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid PowerSwitchIF");
   }
-  heaterMutex = MutexFactory::instance()->createMutex();
-  if (heaterMutex == nullptr) {
+  heaterHealthAndStateMutex = MutexFactory::instance()->createMutex();
+  if (heaterHealthAndStateMutex == nullptr) {
     throw std::runtime_error("HeaterHandler::HeaterHandler: Creating Mutex failed");
   }
   auto mqArgs = MqArgs(setObjectId_, static_cast<void*>(this));
@@ -46,6 +48,13 @@ 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;
+        modeHelper.modeChanged(mode, submode);
+      }
+    }
   } catch (const std::out_of_range& e) {
     sif::warning << "HeaterHandler::performOperation: "
                     "Out of range error | "
@@ -76,6 +85,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;
 }
 
@@ -101,6 +114,10 @@ void HeaterHandler::readCommandQueue() {
     if (result == returnvalue::OK) {
       continue;
     }
+    result = modeHelper.handleModeCommand(&command);
+    if (result == returnvalue::OK) {
+      continue;
+    }
   } while (result == returnvalue::OK);
 }
 
@@ -125,7 +142,11 @@ ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t
   auto action = static_cast<SwitchAction>(data[1]);
   // Always accepts OFF commands
   if (action == SwitchAction::SET_SWITCH_ON) {
-    HasHealthIF::HealthState health = heater.healthDevice->getHealth();
+    HasHealthIF::HealthState health;
+    {
+      MutexGuard mg(heaterHealthAndStateMutex);
+      health = heater.healthDevice->getHealth();
+    }
     if (health == HasHealthIF::FAULTY or health == HasHealthIF::PERMANENT_FAULTY or
         health == HasHealthIF::NEEDS_RECOVERY) {
       return HasHealthIF::OBJECT_NOT_HEALTHY;
@@ -219,6 +240,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
@@ -245,11 +269,16 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
         triggerEvent(GPIO_PULL_HIGH_FAILED, result);
       } else {
         triggerEvent(HEATER_WENT_ON, heaterIdx, 0);
-        heater.switchState = ON;
+        {
+          MutexGuard mg(heaterHealthAndStateMutex);
+          heater.switchState = ON;
+        }
       }
     } else {
       triggerEvent(SWITCH_ALREADY_ON, heaterIdx);
     }
+    mode = HasModesIF::MODE_ON;
+    modeHelper.modeChanged(mode, submode);
     // There is no need to send action finish replies if the sender was the
     // HeaterHandler itself
     if (heater.replyQueue != commandQueue->getId()) {
@@ -290,15 +319,15 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
                  << " low" << std::endl;
       triggerEvent(GPIO_PULL_LOW_FAILED, result);
     } else {
-      result = heaterMutex->lockMutex();
-      heater.switchState = OFF;
-      if (result == returnvalue::OK) {
-        heaterMutex->unlockMutex();
+      {
+        MutexGuard mg(heaterHealthAndStateMutex);
+        heater.switchState = OFF;
       }
       triggerEvent(HEATER_WENT_OFF, heaterIdx, 0);
       // When all switches are off, also main line switch will be turned off
       if (allSwitchesOff()) {
         mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
+        waitForSwitchOff = true;
       }
     }
   } else {
@@ -317,7 +346,7 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
 }
 
 HeaterHandler::SwitchState HeaterHandler::checkSwitchState(heater::Switchers switchNr) const {
-  MutexGuard mg(heaterMutex);
+  MutexGuard mg(heaterHealthAndStateMutex);
   return heaterVec.at(switchNr).switchState;
 }
 
@@ -330,9 +359,57 @@ ReturnValue_t HeaterHandler::switchHeater(heater::Switchers heater, SwitchState
   return returnvalue::FAILED;
 }
 
+void HeaterHandler::announceMode(bool recursive) {
+  triggerEvent(MODE_INFO, mode, submode);
+
+  std::array<SwitchState, 8> states;
+  getAllSwitchStates(states);
+  for (unsigned idx = 0; idx < helper.heaters.size(); idx++) {
+    if (states[idx] == 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(); }
+
+ReturnValue_t HeaterHandler::getAllSwitchStates(std::array<SwitchState, 8>& statesBuf) {
+  {
+    MutexGuard mg(heaterHealthAndStateMutex);
+    if (mg.getLockResult() != returnvalue::OK) {
+      return returnvalue::FAILED;
+    }
+    for (unsigned idx = 0; idx < helper.heaters.size(); idx++) {
+      statesBuf[idx] = heaterVec[idx].switchState;
+    }
+  }
+  return returnvalue::OK;
+}
+
 bool HeaterHandler::allSwitchesOff() {
   bool allSwitchesOrd = false;
-  MutexGuard mg(heaterMutex);
+  MutexGuard mg(heaterHealthAndStateMutex);
   /* Or all switches. As soon one switch is on, allSwitchesOrd will be true */
   for (power::Switch_t switchNr = 0; switchNr < heater::NUMBER_OF_SWITCHES; switchNr++) {
     allSwitchesOrd = allSwitchesOrd || heaterVec.at(switchNr).switchState;
@@ -365,7 +442,7 @@ uint32_t HeaterHandler::getSwitchDelayMs(void) const { return 2000; }
 HasHealthIF::HealthState HeaterHandler::getHealth(heater::Switchers heater) {
   auto* healthDev = heaterVec.at(heater).healthDevice;
   if (healthDev != nullptr) {
-    MutexGuard mg(heaterMutex);
+    MutexGuard mg(heaterHealthAndStateMutex);
     return healthDev->getHealth();
   }
   return HasHealthIF::HealthState::FAULTY;