added 5v stack status read in PCDU handler

mission/devices/PCDUHandler.cpp

@@ -11,6 +11,7 @@
 PCDUHandler::PCDUHandler(object_id_t setObjectId, size_t cmdQueueSize)
     : SystemObject(setObjectId),
       poolManager(this, nullptr),
+      p60CoreHk(objects::P60DOCK_HANDLER),
       pdu1CoreHk(this),
       pdu2CoreHk(this),
       switcherSet(this),
@@ -28,6 +29,22 @@ ReturnValue_t PCDUHandler::performOperation(uint8_t counter) {
     readCommandQueue();
     return returnvalue::OK;
   }
+  uint8_t switchState = 0;
+  {
+    PoolReadGuard pg(&p60CoreHk.outputEnables);
+    if (pg.getReadResult() == returnvalue::OK) {
+      switchState = p60CoreHk.outputEnables.value[10];
+    }
+  }
+  {
+    PoolReadGuard pg(&switcherSet.p60Dock5VStack);
+    if (pg.getReadResult() == returnvalue::OK) {
+      if (switcherSet.p60Dock5VStack.value != switchState) {
+        triggerEvent(power::SWITCH_HAS_CHANGED, switchState, pcdu::Switches::P60_DOCK_5V_STACK);
+      }
+      switcherSet.p60Dock5VStack.value = switchState;
+    }
+  }
   return returnvalue::OK;
 }
 
@@ -398,6 +415,7 @@ ReturnValue_t PCDUHandler::initializeLocalDataPool(localpool::DataPool& localDat
   using namespace pcdu;
   localDataPoolMap.emplace(PoolIds::PDU1_SWITCHES, &pdu1Switches);
   localDataPoolMap.emplace(PoolIds::PDU2_SWITCHES, &pdu2Switches);
+  localDataPoolMap.emplace(PoolIds::P60DOCK_SWITCHES, &p60Dock5VSwitch);
   poolManager.subscribeForRegularPeriodicPacket(
       subdp::RegularHkPeriodicParams(switcherSet.getSid(), false, 5.0));
   return returnvalue::OK;

mission/devices/PCDUHandler.h

@@ -56,6 +56,8 @@ class PCDUHandler : public PowerSwitchIF,
   /** Housekeeping manager. Handles updates of local pool variables. */
   LocalDataPoolManager poolManager;
 
+  P60Dock::CoreHkSet p60CoreHk;
+
   /** Hk table dataset of PDU1 */
   PDU1::Pdu1CoreHk pdu1CoreHk;
   /**
@@ -71,6 +73,7 @@ class PCDUHandler : public PowerSwitchIF,
       PoolEntry<uint8_t>(pcdu::INIT_SWITCHES_PDU1.data(), pcdu::INIT_SWITCHES_PDU1.size());
   PoolEntry<uint8_t> pdu2Switches =
       PoolEntry<uint8_t>(pcdu::INIT_SWITCHES_PDU2.data(), pcdu::INIT_SWITCHES_PDU2.size());
+  PoolEntry<uint8_t> p60Dock5VSwitch = PoolEntry<uint8_t>();
 
   /** The timeStamp of the current pdu2HkTableDataset */
   CCSDSTime::CDS_short timeStampPdu2HkDataset;

mission/devices/devicedefinitions/GomspaceDefinitions.h

@@ -713,7 +713,7 @@ class AuxHk : public StaticLocalDataSet<12> {
 
 namespace pcdu {
 
-enum PoolIds : uint32_t { PDU1_SWITCHES, PDU2_SWITCHES };
+enum PoolIds : uint32_t { PDU1_SWITCHES, PDU2_SWITCHES, P60DOCK_SWITCHES };
 
 /* Switches are uint8_t datatype and go from 0 to 255 */
 enum Switches : power::Switch_t {
@@ -771,6 +771,7 @@ class SwitcherStates : public StaticLocalDataSet<NUMBER_OF_SWITCHES> {
       lp_vec_t<uint8_t, PDU::CHANNELS_LEN>(sid.objectId, PDU1_SWITCHES, this);
   lp_vec_t<uint8_t, PDU::CHANNELS_LEN> pdu2Switches =
       lp_vec_t<uint8_t, PDU::CHANNELS_LEN>(sid.objectId, PDU2_SWITCHES, this);
+  lp_var_t<uint8_t> p60Dock5VStack = lp_var_t<uint8_t>(sid.objectId, P60DOCK_SWITCHES, this);
 };
 
 }  // namespace pcdu

mission/system/objects/Stack5VHandler.cpp

@@ -6,13 +6,16 @@ Stack5VHandler::Stack5VHandler(PowerSwitchIF& switcher) : switcher(switcher) {
 
 ReturnValue_t Stack5VHandler::deviceToOn(StackCommander commander) {
   MutexGuard mg(stackLock);
-  if (switchIsOn or handlerState == HandlerState::SWITCH_PENDING) {
+  if (switchIsOn) {
+    if (commander == StackCommander::PL_PCDU) {
+      plPcduIsOn = true;
+    } else {
+      radSensorIsOn = true;
+    }
     return returnvalue::OK;
   }
-  if (commander == StackCommander::PL_PCDU) {
-    plPcduCommandedOn = true;
-  } else if (commander == StackCommander::RAD_SENSOR) {
-    radSensorCommandedOn = true;
+  if (handlerState == HandlerState::SWITCH_PENDING) {
+    return BUSY;
   }
   return switcher.sendSwitchCommand(stackSwitch, PowerSwitchIF::SWITCH_ON);
 }
@@ -20,13 +23,21 @@ ReturnValue_t Stack5VHandler::deviceToOn(StackCommander commander) {
 ReturnValue_t Stack5VHandler::deviceToOff(StackCommander commander) {
   MutexGuard mg(stackLock);
   if (not switchIsOn) {
+    if (commander == StackCommander::PL_PCDU) {
+      plPcduIsOn = false;
+    } else {
+      radSensorIsOn = false;
+    }
     return returnvalue::OK;
   }
-  if (commander == StackCommander::PL_PCDU) {
-    plPcduCommandedOn = true;
-  } else if (commander == StackCommander::RAD_SENSOR) {
-    radSensorCommandedOn = true;
+  if (handlerState == HandlerState::SWITCH_PENDING) {
+    return BUSY;
   }
+  if ((commander == StackCommander::PL_PCDU and radSensorIsOn) or
+      (commander == StackCommander::RAD_SENSOR and plPcduIsOn)) {
+    return returnvalue::OK;
+  }
+
   return switcher.sendSwitchCommand(stackSwitch, PowerSwitchIF::SWITCH_OFF);
 }
 
@@ -39,12 +50,6 @@ bool Stack5VHandler::isSwitchOn() {
       }
       switchIsOn = true;
     }
-    if (plPcduCommandedOn) {
-      plPcduIsOn = true;
-    }
-    if (radSensorCommandedOn) {
-      radSensorIsOn = true;
-    }
     return true;
   } else {
     if (switchIsOn) {
@@ -55,8 +60,6 @@ bool Stack5VHandler::isSwitchOn() {
     }
     radSensorIsOn = false;
     plPcduIsOn = false;
-    radSensorCommandedOn = false;
-    plPcduCommandedOn = false;
   }
   return false;
 }

mission/system/objects/Stack5VHandler.h

@@ -10,6 +10,7 @@ enum class HandlerState { SWITCH_PENDING, IDLE };
 
 class Stack5VHandler {
  public:
+  static constexpr ReturnValue_t BUSY = returnvalue::makeCode(1, 0);
   Stack5VHandler(PowerSwitchIF& switcher);
 
   ReturnValue_t deviceToOn(StackCommander commander);
@@ -22,8 +23,6 @@ class Stack5VHandler {
   PowerSwitchIF& switcher;
   bool switchIsOn = false;
   HandlerState handlerState = HandlerState::IDLE;
-  bool plPcduCommandedOn = false;
-  bool radSensorCommandedOn = false;
   bool radSensorIsOn = false;
   bool plPcduIsOn = false;
   pcdu::Switches stackSwitch = pcdu::Switches::P60_DOCK_5V_STACK;