impl proper NORMAL mode for MPSoC

linux/payload/FreshMpsocHandler.cpp

@@ -8,22 +8,29 @@
 #include "fsfw/devicehandlers/FreshDeviceHandlerBase.h"
 #include "fsfw/ipc/MessageQueueIF.h"
 #include "fsfw/ipc/QueueFactory.h"
+#include "fsfw/ipc/messageQueueDefinitions.h"
+#include "fsfw/power/PowerSwitchIF.h"
+#include "fsfw/power/definitions.h"
 #include "fsfw/returnvalues/returnvalue.h"
 #include "fsfw/serialize/SerializeAdapter.h"
 #include "linux/payload/MpsocCommunication.h"
 #include "linux/payload/plocMpsocHelpers.h"
 #include "linux/payload/plocSupvDefs.h"
+#include "mission/power/gsDefs.h"
 
 FreshMpsocHandler::FreshMpsocHandler(DhbConfig cfg, MpsocCommunication& comInterface,
                                      PlocMpsocSpecialComHelper& specialComHelper,
-                                     Gpio uartIsolatorSwitch, object_id_t supervisorHandler)
+                                     Gpio uartIsolatorSwitch, object_id_t supervisorHandler,
+                                     PowerSwitchIF& powerSwitcher, power::Switch_t camSwitchId)
     : FreshDeviceHandlerBase(cfg),
       comInterface(comInterface),
       specialComHelper(specialComHelper),
       commandActionHelper(this),
       uartIsolatorSwitch(uartIsolatorSwitch),
       hkReport(this),
-      supervisorHandler(supervisorHandler) {
+      supervisorHandler(supervisorHandler),
+      powerSwitcher(powerSwitcher),
+      camSwitchId(camSwitchId) {
   commandActionHelperQueue = QueueFactory::instance()->createMessageQueue(10);
   eventQueue = QueueFactory::instance()->createMessageQueue(10);
   spParams.maxSize = sizeof(commandBuffer);
@@ -77,8 +84,13 @@ void FreshMpsocHandler::performDefaultDeviceOperation() {
     }
   }
 
+  // We checked the action queue beforehand, so action commands should always be performed
+  // before normal commands.
   if (mode == MODE_NORMAL and not activeCmdInfo.pending) {
-    // TODO: Take care of regular periodic commanding here.
+    ReturnValue_t result = commandTcGetHkReport();
+    if (result == returnvalue::OK) {
+      commandInitHandling(mpsoc::TC_GET_HK_REPORT, MessageQueueIF::NO_QUEUE);
+    }
   }
 
   if (activeCmdInfo.pending and activeCmdInfo.cmdCountdown.hasTimedOut()) {
@@ -224,6 +236,11 @@ ReturnValue_t FreshMpsocHandler::checkModeCommand(Mode_t mode, Submode_t submode
       return HasModesIF::INVALID_SUBMODE;
     }
   }
+  if (submode == mpsoc::Submode::SNAPSHOT and
+      powerSwitcher.getSwitchState(camSwitchId) != PowerSwitchIF::SWITCH_ON) {
+    triggerEvent(mpsoc::CAM_MUST_BE_ON_FOR_SNAPSHOT_MODE);
+    return HasModesIF::TRANS_NOT_ALLOWED;
+  }
   *msToReachTheMode = MPSOC_MODE_CMD_TIMEOUT_MS;
   return returnvalue::OK;
 }
@@ -583,16 +600,20 @@ ReturnValue_t FreshMpsocHandler::executeRegularCmd(ActionId_t actionId,
   }
 
   if (result == returnvalue::OK) {
-    activeCmdInfo.start(actionId, commandedBy);
-    /**
-     * Flushing the receive buffer to make sure there are no data left from a faulty reply.
-     */
-    comInterface.getComHelper().flushUartRxBuffer();
+    commandInitHandling(actionId, commandedBy);
   }
 
   return result;
 }
 
+void FreshMpsocHandler::commandInitHandling(ActionId_t actionId, MessageQueueId_t commandedBy) {
+  activeCmdInfo.start(actionId, commandedBy);
+  /**
+   * Flushing the receive buffer to make sure there are no data left from a faulty reply.
+   */
+  comInterface.getComHelper().flushUartRxBuffer();
+}
+
 ReturnValue_t FreshMpsocHandler::commandTcMemWrite(const uint8_t* commandData,
                                                    size_t commandDataLen) {
   ReturnValue_t result = returnvalue::OK;
@@ -1229,7 +1250,9 @@ bool FreshMpsocHandler::handleHwShutdown() {
       supvTransitionCd.resetTimer();
       powerState = PowerState::PENDING_SHUTDOWN;
     } else {
-      triggerEvent(mpsoc::SUPV_NOT_ON, 0);
+      if ((this->mode != MODE_OFF) and (this->mode != MODE_UNDEFINED)) {
+        triggerEvent(mpsoc::SUPV_NOT_ON, 0);
+      }
       powerState = PowerState::DONE;
     }
   }