Heaters are own objects with HealthIF now

mission/devices/HeaterHandler.cpp

@@ -10,16 +10,22 @@
 #include "devices/gpioIds.h"
 #include "devices/powerSwitcherList.h"
 
-HeaterHandler::HeaterHandler(object_id_t setObjectId_, object_id_t gpioDriverId_,
-                             CookieIF* gpioCookie_, PowerSwitchIF* mainLineSwitcher_,
-                             power::Switch_t mainLineSwitch_)
+HeaterHandler::HeaterHandler(object_id_t setObjectId_, GpioIF* gpioInterface_, HeaterHelper helper,
+                             PowerSwitchIF* mainLineSwitcher_, power::Switch_t mainLineSwitch_)
     : SystemObject(setObjectId_),
-      gpioDriverId(gpioDriverId_),
-      gpioCookie(gpioCookie_),
+      helper(helper),
+      gpioInterface(gpioInterface_),
       mainLineSwitcher(mainLineSwitcher_),
       mainLineSwitch(mainLineSwitch_),
-      healthHelper(this, getObjectId()),
       actionHelper(this, nullptr) {
+  for (const auto& heater : helper.heaters) {
+    if (heater.first == nullptr) {
+      throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid Heater Object");
+    }
+  }
+  if (gpioInterface_ == nullptr) {
+    throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid Gpio IF");
+  }
   if (mainLineSwitcher == nullptr) {
     throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid PowerSwitchIF");
   }
@@ -31,10 +37,10 @@ HeaterHandler::HeaterHandler(object_id_t setObjectId_, object_id_t gpioDriverId_
 HeaterHandler::~HeaterHandler() {}
 
 ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) {
-  if (operationCode == DeviceHandlerIF::PERFORM_OPERATION) {
-    readCommandQueue();
-    handleActiveCommands();
-    return RETURN_OK;
+  readCommandQueue();
+  handleActiveCommands();
+  for (const auto& heater : helper.heaters) {
+    heater.first->performOperation(0);
   }
   return RETURN_OK;
 }
@@ -50,22 +56,8 @@ ReturnValue_t HeaterHandler::initialize() {
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
 
-  gpioInterface = ObjectManager::instance()->get<GpioIF>(gpioDriverId);
-  if (gpioInterface == nullptr) {
-    sif::error << "HeaterHandler::initialize: Invalid Gpio interface." << std::endl;
-    return ObjectManagerIF::CHILD_INIT_FAILED;
-  }
-
-  result = gpioInterface->addGpios(dynamic_cast<GpioCookie*>(gpioCookie));
-  if (result != RETURN_OK) {
-    sif::error << "HeaterHandler::initialize: Failed to initialize Gpio interface" << std::endl;
-    return ObjectManagerIF::CHILD_INIT_FAILED;
-  }
-
-  healthHelper.initialize(commandQueue->getId());
-
-  IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
-  if (IPCStore == nullptr) {
+  ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+  if (ipcStore == nullptr) {
     sif::error << "HeaterHandler::initialize: IPC store not set up in factory." << std::endl;
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
@@ -79,24 +71,12 @@ ReturnValue_t HeaterHandler::initialize() {
 }
 
 ReturnValue_t HeaterHandler::initializeHeaterMap() {
-  HeaterCommandInfo_t heaterCommandInfo;
-  for (switchNr_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
-    std::pair status = heaterMap.emplace(switchNr, heaterCommandInfo);
-    if (status.second == false) {
-      sif::error << "HeaterHandler::initializeHeaterMap: Failed to initialize heater map"
-                 << std::endl;
-      return RETURN_FAILED;
-    }
+  for (power::Switch_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
+    heaterVec.push_back(HeaterWrapper(helper.heaters[switchNr]));
   }
   return RETURN_OK;
 }
 
-void HeaterHandler::setInitialSwitchStates() {
-  for (switchNr_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
-    switchStates[switchNr] = OFF;
-  }
-}
-
 void HeaterHandler::readCommandQueue() {
   ReturnValue_t result = RETURN_OK;
   CommandMessage command;
@@ -111,41 +91,44 @@ void HeaterHandler::readCommandQueue() {
     if (result == RETURN_OK) {
       continue;
     }
-    result = healthHelper.handleHealthCommand(&command);
-    if (result == RETURN_OK) {
-      continue;
-    }
   } while (result == RETURN_OK);
 }
 
 ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                            const uint8_t* data, size_t size) {
-  ReturnValue_t result;
-  if (actionId != SWITCH_HEATER) {
-    result = COMMAND_NOT_SUPPORTED;
-  } else {
-    switchNr_t switchNr = *data;
-    HeaterMapIter heaterMapIter = heaterMap.find(switchNr);
-    if (heaterMapIter != heaterMap.end()) {
-      if (heaterMapIter->second.active) {
-        return COMMAND_ALREADY_WAITING;
-      }
-      heaterMapIter->second.action = *(data + 1);
-      heaterMapIter->second.active = true;
-      heaterMapIter->second.replyQueue = commandedBy;
-    } else {
-      sif::error << "HeaterHandler::executeAction: Invalid switchNr" << std::endl;
-      return INVALID_SWITCH_NR;
-    }
-    result = RETURN_OK;
+  if (data == nullptr or size < 2) {
+    return HasActionsIF::INVALID_PARAMETERS;
   }
-  return result;
+  if (actionId != SWITCH_HEATER) {
+    return COMMAND_NOT_SUPPORTED;
+  }
+  auto switchNr = *data;
+  if (switchNr > 7) {
+    return HasActionsIF::INVALID_PARAMETERS;
+  }
+  auto heater = heaterVec.at(switchNr);
+  HasHealthIF::HealthState health = heater.healthDevice->getHealth();
+  if (health == HasHealthIF::FAULTY or health == HasHealthIF::PERMANENT_FAULTY or
+      health == HasHealthIF::NEEDS_RECOVERY) {
+    return HasHealthIF::OBJECT_NOT_HEALTHY;
+  }
+  if (heater.active) {
+    return COMMAND_ALREADY_WAITING;
+  }
+  auto action = data[1];
+  if (action != 0 and action != 1) {
+    return HasActionsIF::INVALID_PARAMETERS;
+  }
+  heater.action = static_cast<SwitchAction>(data[1]);
+  heater.active = true;
+  heater.replyQueue = commandedBy;
+  return RETURN_OK;
 }
 
 ReturnValue_t HeaterHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) {
   ReturnValue_t result;
   store_address_t storeAddress;
-  uint8_t commandData[2];
+  uint8_t commandData[2] = {};
 
   switch (onOff) {
     case PowerSwitchIF::SWITCH_ON:
@@ -161,7 +144,7 @@ ReturnValue_t HeaterHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t o
       break;
   }
 
-  result = IPCStore->addData(&storeAddress, commandData, sizeof(commandData));
+  result = ipcStore->addData(&storeAddress, commandData, sizeof(commandData));
   if (result == RETURN_OK) {
     CommandMessage message;
     ActionMessage::setCommand(&message, SWITCH_HEATER, storeAddress);
@@ -176,15 +159,14 @@ ReturnValue_t HeaterHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t o
 }
 
 void HeaterHandler::handleActiveCommands() {
-  HeaterMapIter heaterMapIter = heaterMap.begin();
-  for (; heaterMapIter != heaterMap.end(); heaterMapIter++) {
-    if (heaterMapIter->second.active) {
-      switch (heaterMapIter->second.action) {
+  for (uint8_t idx = 0; idx < heater::NUMBER_OF_HEATERS; idx++) {
+    if (heaterVec[idx].active) {
+      switch (heaterVec[idx].action) {
         case SET_SWITCH_ON:
-          handleSwitchOnCommand(heaterMapIter);
+          handleSwitchOnCommand(idx);
           break;
         case SET_SWITCH_OFF:
-          handleSwitchOffCommand(heaterMapIter);
+          handleSwitchOffCommand(idx);
           break;
         default:
           sif::error << "HeaterHandler::handleActiveCommands: Invalid action commanded"
@@ -195,156 +177,113 @@ void HeaterHandler::handleActiveCommands() {
   }
 }
 
-void HeaterHandler::handleSwitchOnCommand(HeaterMapIter heaterMapIter) {
+void HeaterHandler::handleSwitchOnCommand(uint8_t heaterIdx) {
   ReturnValue_t result = RETURN_OK;
-  switchNr_t switchNr;
-
+  auto& heater = heaterVec.at(heaterIdx);
   /* Check if command waits for main switch being set on and whether the timeout has expired */
-  if (heaterMapIter->second.waitMainSwitchOn &&
-      heaterMapIter->second.mainSwitchCountdown.hasTimedOut()) {
+  if (heater.waitMainSwitchOn && heater.mainSwitchCountdown.hasTimedOut()) {
     // TODO - This requires the initiation of an FDIR procedure
     triggerEvent(MAIN_SWITCH_TIMEOUT);
     sif::error << "HeaterHandler::handleSwitchOnCommand: Main switch setting on timeout"
                << std::endl;
-    heaterMapIter->second.active = false;
-    heaterMapIter->second.waitMainSwitchOn = false;
-    if (heaterMapIter->second.replyQueue != commandQueue->getId()) {
-      actionHelper.finish(false, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
-                          MAIN_SWITCH_SET_TIMEOUT);
+    heater.active = false;
+    heater.waitMainSwitchOn = false;
+    if (heater.replyQueue != commandQueue->getId()) {
+      actionHelper.finish(false, heater.replyQueue, heater.action, MAIN_SWITCH_SET_TIMEOUT);
     }
     return;
   }
 
-  switchNr = heaterMapIter->first;
-  /* Check state of main line switch */
+  // Check state of main line switch
   ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch);
   if (mainSwitchState == PowerSwitchIF::SWITCH_ON) {
-    if (!checkSwitchState(switchNr)) {
-      gpioId_t gpioId = getGpioIdFromSwitchNr(switchNr);
+    if (!checkSwitchState(heaterIdx)) {
+      gpioId_t gpioId = heater.gpioId;
       result = gpioInterface->pullHigh(gpioId);
       if (result != RETURN_OK) {
         sif::error << "HeaterHandler::handleSwitchOnCommand: Failed to pull gpio with id " << gpioId
                    << " high" << std::endl;
         triggerEvent(GPIO_PULL_HIGH_FAILED, result);
       } else {
-        switchStates[switchNr] = ON;
+        heater.switchState = ON;
       }
     } else {
-      triggerEvent(SWITCH_ALREADY_ON, switchNr);
+      triggerEvent(SWITCH_ALREADY_ON, heaterIdx);
     }
-    /* There is no need to send action finish replies if the sender was the
-     * HeaterHandler itself. */
-    if (heaterMapIter->second.replyQueue != commandQueue->getId()) {
+    // There is no need to send action finish replies if the sender was the
+    // HeaterHandler itself
+    if (heater.replyQueue != commandQueue->getId()) {
       if (result == RETURN_OK) {
-        actionHelper.finish(true, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
-                            result);
+        actionHelper.finish(true, heater.replyQueue, heater.action, result);
       } else {
-        actionHelper.finish(false, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
-                            result);
+        actionHelper.finish(false, heater.replyQueue, heater.action, result);
       }
     }
-    heaterMapIter->second.active = false;
-    heaterMapIter->second.waitMainSwitchOn = false;
-  } else if (mainSwitchState == PowerSwitchIF::SWITCH_OFF &&
-             heaterMapIter->second.waitMainSwitchOn) {
-    /* Just waiting for the main switch being set on */
+    heater.active = false;
+    heater.waitMainSwitchOn = false;
+  } else if (mainSwitchState == PowerSwitchIF::SWITCH_OFF && heater.waitMainSwitchOn) {
+    // Just waiting for the main switch being set on
     return;
   } else if (mainSwitchState == PowerSwitchIF::SWITCH_OFF) {
     mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
-    heaterMapIter->second.mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
-    heaterMapIter->second.waitMainSwitchOn = true;
+    heater.mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
+    heater.waitMainSwitchOn = true;
   } else {
     sif::debug << "HeaterHandler::handleActiveCommands: Failed to get state of"
                << " main line switch" << std::endl;
-    if (heaterMapIter->second.replyQueue != commandQueue->getId()) {
-      actionHelper.finish(false, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
-                          mainSwitchState);
+    if (heater.replyQueue != commandQueue->getId()) {
+      actionHelper.finish(false, heater.replyQueue, heater.action, mainSwitchState);
     }
-    heaterMapIter->second.active = false;
+    heater.active = false;
   }
 }
 
-void HeaterHandler::handleSwitchOffCommand(HeaterMapIter heaterMapIter) {
+void HeaterHandler::handleSwitchOffCommand(uint8_t heaterIdx) {
   ReturnValue_t result = RETURN_OK;
-  switchNr_t switchNr = heaterMapIter->first;
-  /* Check whether switch is already off */
-  if (checkSwitchState(switchNr)) {
-    gpioId_t gpioId = getGpioIdFromSwitchNr(switchNr);
+  auto& heater = heaterVec.at(heaterIdx);
+  // Check whether switch is already off
+  if (checkSwitchState(heaterIdx)) {
+    gpioId_t gpioId = heater.gpioId;
     result = gpioInterface->pullLow(gpioId);
     if (result != RETURN_OK) {
       sif::error << "HeaterHandler::handleSwitchOffCommand: Failed to pull gpio with id" << gpioId
                  << " low" << std::endl;
       triggerEvent(GPIO_PULL_LOW_FAILED, result);
     } else {
-      switchStates[switchNr] = OFF;
-      /* When all switches are off, also main line switch will be turned off */
+      heater.switchState = OFF;
+      // When all switches are off, also main line switch will be turned off
       if (allSwitchesOff()) {
         mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
       }
     }
   } else {
     sif::info << "HeaterHandler::handleSwitchOffCommand: Switch already off" << std::endl;
-    triggerEvent(SWITCH_ALREADY_OFF, switchNr);
+    triggerEvent(SWITCH_ALREADY_OFF, heaterIdx);
   }
-  if (heaterMapIter->second.replyQueue != NO_COMMANDER) {
-    /* Report back switch command reply if necessary */
+  if (heater.replyQueue != NO_COMMANDER) {
+    // Report back switch command reply if necessary
     if (result == HasReturnvaluesIF::RETURN_OK) {
-      actionHelper.finish(true, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
-                          result);
+      actionHelper.finish(true, heater.replyQueue, heater.action, result);
     } else {
-      actionHelper.finish(false, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
-                          result);
+      actionHelper.finish(false, heater.replyQueue, heater.action, result);
     }
   }
-  heaterMapIter->second.active = false;
+  heater.active = false;
 }
 
-bool HeaterHandler::checkSwitchState(int switchNr) { return switchStates[switchNr]; }
+HeaterHandler::SwitchState HeaterHandler::checkSwitchState(uint8_t switchNr) {
+  return heaterVec.at(switchNr).switchState;
+}
 
 bool HeaterHandler::allSwitchesOff() {
   bool allSwitchesOrd = false;
   /* Or all switches. As soon one switch is on, allSwitchesOrd will be true */
-  for (switchNr_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
-    allSwitchesOrd = allSwitchesOrd || switchStates[switchNr];
+  for (power::Switch_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
+    allSwitchesOrd = allSwitchesOrd || heaterVec.at(switchNr).switchState;
   }
   return !allSwitchesOrd;
 }
 
-gpioId_t HeaterHandler::getGpioIdFromSwitchNr(int switchNr) {
-  gpioId_t gpioId = 0xFFFF;
-  switch (switchNr) {
-    case heaterSwitches::HEATER_0:
-      gpioId = gpioIds::HEATER_0;
-      break;
-    case heaterSwitches::HEATER_1:
-      gpioId = gpioIds::HEATER_1;
-      break;
-    case heaterSwitches::HEATER_2:
-      gpioId = gpioIds::HEATER_2;
-      break;
-    case heaterSwitches::HEATER_3:
-      gpioId = gpioIds::HEATER_3;
-      break;
-    case heaterSwitches::HEATER_4:
-      gpioId = gpioIds::HEATER_4;
-      break;
-    case heaterSwitches::HEATER_5:
-      gpioId = gpioIds::HEATER_5;
-      break;
-    case heaterSwitches::HEATER_6:
-      gpioId = gpioIds::HEATER_6;
-      break;
-    case heaterSwitches::HEATER_7:
-      gpioId = gpioIds::HEATER_7;
-      break;
-    default:
-      sif::error << "HeaterHandler::getGpioIdFromSwitchNr: Unknown heater switch number"
-                 << std::endl;
-      break;
-  }
-  return gpioId;
-}
-
 MessageQueueId_t HeaterHandler::getCommandQueue() const { return commandQueue->getId(); }
 
 ReturnValue_t HeaterHandler::sendFuseOnCommand(uint8_t fuseNr) { return RETURN_OK; }
@@ -354,10 +293,3 @@ ReturnValue_t HeaterHandler::getSwitchState(uint8_t switchNr) const { return 0;
 ReturnValue_t HeaterHandler::getFuseState(uint8_t fuseNr) const { return 0; }
 
 uint32_t HeaterHandler::getSwitchDelayMs(void) const { return 2000; }
-
-ReturnValue_t HeaterHandler::setHealth(HealthState health) {
-  healthHelper.setHealth(health);
-  return RETURN_OK;
-}
-
-HasHealthIF::HealthState HeaterHandler::getHealth() { return healthHelper.getHealth(); }