mission/system/EiveSystem.cpp

@@ -81,6 +81,55 @@ ReturnValue_t EiveSystem::initialize() {
     return result;
   }
 
+  auto* plSs = ObjectManager::instance()->get<HasModesIF>(objects::PL_SUBSYSTEM);
+  if (plSs == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  plSsQueueId = plSs->getCommandQueue();
+
+  auto* plPcdu = ObjectManager::instance()->get<HasHealthIF>(objects::PLPCDU_HANDLER);
+  if (plPcdu == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  plPcduQueueId = plPcdu->getCommandQueue();
+
+  auto* plocMpsoc = ObjectManager::instance()->get<HasHealthIF>(objects::PLOC_MPSOC_HANDLER);
+  if (plocMpsoc == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  plocMpsocQueueId = plocMpsoc->getCommandQueue();
+
+  auto* plocSupervisor =
+      ObjectManager::instance()->get<HasHealthIF>(objects::PLOC_SUPERVISOR_HANDLER);
+  if (plocSupervisor == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  plocSupervisorQueueId = plocSupervisor->getCommandQueue();
+
+  auto* camera = ObjectManager::instance()->get<HasHealthIF>(objects::CAM_SWITCHER);
+  if (camera == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  cameraQueueId = camera->getCommandQueue();
+
+  auto* scex = ObjectManager::instance()->get<HasHealthIF>(objects::SCEX);
+  if (scex == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  scexQueueId = scex->getCommandQueue();
+
+  auto* radSensor = ObjectManager::instance()->get<HasHealthIF>(objects::RAD_SENSOR);
+  if (radSensor == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  radSensorQueueId = radSensor->getCommandQueue();
+
+  auto* str = ObjectManager::instance()->get<HasHealthIF>(objects::STAR_TRACKER);
+  if (str == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  strQueueId = str->getCommandQueue();
+
   auto* bpxDest = ObjectManager::instance()->get<HasActionsIF>(objects::BPX_BATT_HANDLER);
   if (bpxDest == nullptr) {
     return ObjectManager::CHILD_INIT_FAILED;
@@ -120,6 +169,8 @@ ReturnValue_t EiveSystem::initialize() {
   manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::OBC_OVERHEATING));
   manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::MGT_OVERHEATING));
   manager->subscribeToEvent(eventQueue->getId(), event::getEventId(pdec::INVALID_TC_FRAME));
+  manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_LOW));
+  manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_CRITICAL));
   return Subsystem::initialize();
 }
 
@@ -151,6 +202,22 @@ void EiveSystem::handleEventMessages() {
             commandSelfToSafe();
             break;
           }
+          case power::POWER_LEVEL_LOW: {
+            forceOffPayload();
+            break;
+          }
+          case power::POWER_LEVEL_CRITICAL:
+            CommandMessage msg;
+            ModeMessage::setModeMessage(&msg, ModeMessage::CMD_MODE_COMMAND_FORCED,
+                                        HasModesIF::MODE_OFF, HasModesIF::SUBMODE_NONE);
+            HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HasHealthIF::FAULTY);
+            ReturnValue_t result = MessageQueueSenderIF::sendMessage(
+                strQueueId, &msg, MessageQueueIF::NO_QUEUE, false);
+            if (result != returnvalue::OK) {
+              sif::error << "EIVE System: Sending FAULTY command to STR Assembly failed"
+                         << std::endl;
+            }
+            break;
         }
         break;
       default:
@@ -336,6 +403,49 @@ void EiveSystem::pdecRecoveryLogic() {
   }
 }
 
+void EiveSystem::forceOffPayload() {
+  CommandMessage msg;
+  // turn off PL Subsystem
+  ModeMessage::setModeMessage(&msg, ModeMessage::CMD_MODE_COMMAND_FORCED, HasModesIF::MODE_OFF,
+                              HasModesIF::SUBMODE_NONE);
+  ReturnValue_t result = commandQueue->sendMessage(plSsQueueId, &msg);
+  if (result != returnvalue::OK) {
+    sif::error << "EIVE System: Sending OFF command to PL Subsystem failed" << std::endl;
+  }
+  // set PL to faulty
+  HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HasHealthIF::FAULTY);
+
+  result = commandQueue->sendMessage(plPcduQueueId, &msg);
+  if (result != returnvalue::OK) {
+    sif::error << "EIVE System: Sending FAULTY command to PL PCDU failed" << std::endl;
+  }
+
+  result = commandQueue->sendMessage(plocMpsocQueueId, &msg);
+  if (result != returnvalue::OK) {
+    sif::error << "EIVE System: Sending FAULTY command to PLOC MPSOC failed" << std::endl;
+  }
+
+  result = commandQueue->sendMessage(plocSupervisorQueueId, &msg);
+  if (result != returnvalue::OK) {
+    sif::error << "EIVE System: Sending FAULTY command to PLOC SUPERVISOR failed" << std::endl;
+  }
+
+  result = commandQueue->sendMessage(cameraQueueId, &msg);
+  if (result != returnvalue::OK) {
+    sif::error << "EIVE System: Sending FAULTY command to PL CAM failed" << std::endl;
+  }
+
+  result = commandQueue->sendMessage(scexQueueId, &msg);
+  if (result != returnvalue::OK) {
+    sif::error << "EIVE System: Sending FAULTY command to SCEX failed" << std::endl;
+  }
+
+  result = commandQueue->sendMessage(radSensorQueueId, &msg);
+  if (result != returnvalue::OK) {
+    sif::error << "EIVE System: Sending FAULTY command to RAD SENSOR failed" << std::endl;
+  }
+}
+
 void EiveSystem::commonI2cRecoverySequenceFinish() {
   alreadyTriedI2cRecovery = true;
   performI2cReboot = false;

mission/system/EiveSystem.h

@@ -49,6 +49,15 @@ class EiveSystem : public Subsystem, public HasActionsIF {
   PowerSwitchIF* powerSwitcher = nullptr;
   std::atomic_uint16_t& i2cErrors;
 
+  MessageQueueId_t plSsQueueId = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t plPcduQueueId = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t plocMpsocQueueId = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t plocSupervisorQueueId = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t cameraQueueId = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t scexQueueId = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t radSensorQueueId = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t strQueueId = MessageQueueIF::NO_QUEUE;
+
   MessageQueueId_t pdecHandlerQueueId = MessageQueueIF::NO_QUEUE;
 
   MessageQueueId_t bpxBattQueueId = MessageQueueIF::NO_QUEUE;
@@ -68,6 +77,8 @@ class EiveSystem : public Subsystem, public HasActionsIF {
   ReturnValue_t sendFullRebootCommand();
   ReturnValue_t sendSelfRebootCommand();
 
+  void forceOffPayload();
+
   void pdecRecoveryLogic();
 
   void i2cRecoveryLogic();