eive-obsw/mission/system/EiveSystem.cpp

#include "EiveSystem.h"

#include <eive/objects.h>
#include <fsfw/events/EventManager.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/power/PowerSwitchIF.h>
#include <mission/acs/defs.h>
#include <mission/com/defs.h>
#include <mission/controller/tcsDefs.h>

#include "mission/power/bpxBattDefs.h"
#include "mission/power/defs.h"
#include "mission/sysDefs.h"

EiveSystem::EiveSystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
                       uint32_t maxNumberOfTables)
    : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables),
      actionHelper(this, commandQueue) {
  auto mqArgs = MqArgs(SubsystemBase::getObjectId(), static_cast<void*>(this));
  eventQueue =
      QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
}

void EiveSystem::announceMode(bool recursive) {
  const char* modeStr = "UNKNOWN";
  switch (mode) {
    case (satsystem::Mode::BOOT): {
      modeStr = "OFF/BOOT";
      break;
    }
    case (satsystem::Mode::SAFE): {
      modeStr = "SAFE";
      break;
    }
    case (satsystem::Mode::PTG_IDLE): {
      modeStr = "POINTING IDLE";
      break;
    }
    case (acs::AcsMode::PTG_INERTIAL): {
      modeStr = "POINTING INERTIAL";
      break;
    }
    case (acs::AcsMode::PTG_TARGET): {
      modeStr = "POINTING TARGET";
      break;
    }
    case (acs::AcsMode::PTG_TARGET_GS): {
      modeStr = "POINTING TARGET GS";
      break;
    }
  }
  sif::info << "EIVE system is now in " << modeStr << " mode" << std::endl;
  return Subsystem::announceMode(recursive);
}

void EiveSystem::performChildOperation() {
  ReturnValue_t result;
  Subsystem::performChildOperation();
  handleEventMessages();
  if (not isInTransition and performSafeRecovery) {
    commandSelfToSafe();
    performSafeRecovery = false;
    return;
  }
  if (not isInTransition and performI2cReboot) {
    if (i2cRebootState == I2cRebootState::SYSTEM_MODE_BOOT) {
      startTransition(satsystem::Mode::BOOT, 0);
      i2cRebootState = I2cRebootState::SWITCH_3V3_STACK_OFF_AND_BATT_REBOOT;
      i2cRebootCountdown.resetTimer();
    } else if (i2cRebootState == I2cRebootState::SWITCH_3V3_STACK_OFF_AND_BATT_REBOOT) {
      if (mode == satsystem::Mode::BOOT) {
        result = powerSwitcher->sendSwitchCommand(power::Switches::P60_DOCK_3V3_STACK,
                                                  PowerSwitchIF::SWITCH_OFF);
        if (result != returnvalue::OK) {
          actionHelper.finish(false, actionCommandedBy, EXECUTE_I2C_REBOOT, result);
          performI2cReboot = false;
        }
        CommandMessage msg;
        ActionMessage::setCommand(&msg, BpxBattery::REBOOT, store_address_t());
        result = commandQueue->sendMessage(bpxBattQueueId, &msg);
        if (result != returnvalue::OK) {
          actionHelper.finish(false, actionCommandedBy, EXECUTE_I2C_REBOOT, result);
          performI2cReboot = false;
        }
        i2cRebootState = I2cRebootState::SWITCH_3V3_STACK_ON;
      }
      if (i2cRebootCountdown.hasTimedOut()) {
        actionHelper.finish(false, actionCommandedBy, EXECUTE_I2C_REBOOT, returnvalue::FAILED);
        performI2cReboot = false;
      }
    } else if (i2cRebootState == I2cRebootState::SWITCH_3V3_STACK_ON) {
      result = powerSwitcher->sendSwitchCommand(power::Switches::P60_DOCK_3V3_STACK,
                                                PowerSwitchIF::SWITCH_ON);
      if (result != returnvalue::OK) {
        actionHelper.finish(false, actionCommandedBy, EXECUTE_I2C_REBOOT, result);
        performI2cReboot = false;
      }
      i2cRebootState = I2cRebootState::SYSTEM_MODE_SAFE;
    } else if (i2cRebootState == I2cRebootState::SYSTEM_MODE_SAFE) {
      if (powerSwitcher->getSwitchState(power::Switches::P60_DOCK_3V3_STACK) ==
          PowerSwitchIF::SWITCH_ON) {
        // This should always be accepted
        commandSelfToSafe();
        i2cRebootState = I2cRebootState::NONE;
        performI2cReboot = false;
        actionHelper.finish(true, actionCommandedBy, EXECUTE_I2C_REBOOT);
      }
    }

    // Timeout handling for the internal procedure.
    if (i2cRebootState != I2cRebootState::NONE and i2cRebootCountdown.hasTimedOut()) {
      actionHelper.finish(false, actionCommandedBy, EXECUTE_I2C_REBOOT, returnvalue::FAILED);
      powerSwitcher->sendSwitchCommand(power::Switches::P60_DOCK_3V3_STACK,
                                       PowerSwitchIF::SWITCH_ON);
      // This should always be accepted
      commandSelfToSafe();
    }
  }
}

ReturnValue_t EiveSystem::initialize() {
  if (powerSwitcher == nullptr) {
    return ObjectManager::CHILD_INIT_FAILED;
  }
  ReturnValue_t result = actionHelper.initialize();
  if (result != returnvalue::OK) {
    return result;
  }
  auto* bpxDest = ObjectManager::instance()->get<HasActionsIF>(objects::BPX_BATT_HANDLER);
  if (bpxDest == nullptr) {
    return ObjectManager::CHILD_INIT_FAILED;
  }
  bpxBattQueueId = bpxDest->getCommandQueue();
  auto* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
  if (manager == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "AcsSubsystem::initialize: Invalid event manager" << std::endl;
#endif
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  result = manager->registerListener(eventQueue->getId());
  if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::warning << "AcsSubsystem::registerListener: Failed to register as "
                    "listener"
                 << std::endl;
#endif
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  manager->subscribeToEvent(eventQueue->getId(),
                            event::getEventId(tcsCtrl::PCDU_SYSTEM_OVERHEATING));
  manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::OBC_OVERHEATING));

  return Subsystem::initialize();
}

void EiveSystem::handleEventMessages() {
  EventMessage event;
  for (ReturnValue_t status = eventQueue->receiveMessage(&event); status == returnvalue::OK;
       status = eventQueue->receiveMessage(&event)) {
    switch (event.getMessageId()) {
      case EventMessage::EVENT_MESSAGE:
        switch (event.getEvent()) {
          case tcsCtrl::OBC_OVERHEATING:
          case tcsCtrl::PCDU_SYSTEM_OVERHEATING: {
            if (isInTransition) {
              performSafeRecovery = true;
              return;
            }

            commandSelfToSafe();
            break;
          }
        }
        break;
      default:
        sif::debug << "EiveSystem: Did not subscribe to event " << event.getEvent() << std::endl;
        break;
    }
  }
}

MessageQueueId_t EiveSystem::getCommandQueue() const { return Subsystem::getCommandQueue(); }

ReturnValue_t EiveSystem::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                        const uint8_t* data, size_t size) {
  switch (actionId) {
    case (EXECUTE_I2C_REBOOT): {
      performI2cReboot = true;
      i2cRebootState = I2cRebootState::SYSTEM_MODE_BOOT;
      this->actionCommandedBy = commandedBy;
      return returnvalue::OK;
    }
    default: {
      return HasActionsIF::INVALID_ACTION_ID;
    }
  }
  return returnvalue::OK;
}

void EiveSystem::setPowerSwitcher(PowerSwitchIF* pwrSwitcher) {}

void EiveSystem::commandSelfToSafe() { startTransition(satsystem::Mode::SAFE, 0); }