Payload mode

FprimeZephyrReference/Components/ModeManager/ModeManager.cpp

@@ -18,7 +18,11 @@ namespace Components {
 // ----------------------------------------------------------------------
 
 ModeManager ::ModeManager(const char* const compName)
-    : ModeManagerComponentBase(compName), m_mode(SystemMode::NORMAL), m_safeModeEntryCount(0), m_runCounter(0) {}
+    : ModeManagerComponentBase(compName),
+      m_mode(SystemMode::NORMAL),
+      m_safeModeEntryCount(0),
+      m_payloadModeEntryCount(0),
+      m_runCounter(0) {}
 
 ModeManager ::~ModeManager() {}
 
@@ -41,12 +45,12 @@ void ModeManager ::run_handler(FwIndexType portNum, U32 context) {
 
 void ModeManager ::forceSafeMode_handler(FwIndexType portNum) {
     // Force entry into safe mode (called by other components)
-    // Provides immediate safe mode entry for critical component-detected faults
-    this->log_WARNING_HI_ExternalFaultDetected();
-
+    // Only allowed from NORMAL (sequential +1/-1 transitions)
     if (this->m_mode == SystemMode::NORMAL) {
+        this->log_WARNING_HI_ExternalFaultDetected();
         this->enterSafeMode("External component request");
     }
+    // Note: Request ignored if in PAYLOAD_MODE or already in SAFE_MODE
 }
 
 Components::SystemMode ModeManager ::getMode_handler(FwIndexType portNum) {
@@ -60,13 +64,26 @@ Components::SystemMode ModeManager ::getMode_handler(FwIndexType portNum) {
 // ----------------------------------------------------------------------
 
 void ModeManager ::FORCE_SAFE_MODE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) {
-    // Force entry into safe mode
-    this->log_ACTIVITY_HI_ManualSafeModeEntry();
+    // Force entry into safe mode - only allowed from NORMAL (sequential +1/-1 transitions)
 
-    if (this->m_mode == SystemMode::NORMAL) {
-        this->enterSafeMode("Ground command");
+    // Reject if in payload mode - must exit payload mode first
+    if (this->m_mode == SystemMode::PAYLOAD_MODE) {
+        Fw::LogStringArg cmdNameStr("FORCE_SAFE_MODE");
+        Fw::LogStringArg reasonStr("Must exit payload mode first");
+        this->log_WARNING_LO_CommandValidationFailed(cmdNameStr, reasonStr);
+        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::VALIDATION_ERROR);
+        return;
     }
 
+    // Already in safe mode - idempotent success
+    if (this->m_mode == SystemMode::SAFE_MODE) {
+        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
+        return;
+    }
+
+    // Enter safe mode from NORMAL
+    this->log_ACTIVITY_HI_ManualSafeModeEntry();
+    this->enterSafeMode("Ground command");
     this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
 }
 
@@ -87,6 +104,48 @@ void ModeManager ::EXIT_SAFE_MODE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) {
     this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
 }
 
+void ModeManager ::ENTER_PAYLOAD_MODE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) {
+    // Command to enter payload mode - only allowed from NORMAL mode
+
+    // Check if currently in safe mode (not allowed)
+    if (this->m_mode == SystemMode::SAFE_MODE) {
+        Fw::LogStringArg cmdNameStr("ENTER_PAYLOAD_MODE");
+        Fw::LogStringArg reasonStr("Cannot enter payload mode from safe mode");
+        this->log_WARNING_LO_CommandValidationFailed(cmdNameStr, reasonStr);
+        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::VALIDATION_ERROR);
+        return;
+    }
+
+    // Check if already in payload mode
+    if (this->m_mode == SystemMode::PAYLOAD_MODE) {
+        // Already in payload mode - success (idempotent)
+        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
+        return;
+    }
+
+    // Enter payload mode
+    this->log_ACTIVITY_HI_ManualPayloadModeEntry();
+    this->enterPayloadMode("Ground command");
+    this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
+}
+
+void ModeManager ::EXIT_PAYLOAD_MODE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) {
+    // Command to exit payload mode
+
+    // Check if currently in payload mode
+    if (this->m_mode != SystemMode::PAYLOAD_MODE) {
+        Fw::LogStringArg cmdNameStr("EXIT_PAYLOAD_MODE");
+        Fw::LogStringArg reasonStr("Not currently in payload mode");
+        this->log_WARNING_LO_CommandValidationFailed(cmdNameStr, reasonStr);
+        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::VALIDATION_ERROR);
+        return;
+    }
+
+    // Exit payload mode
+    this->exitPayloadMode();
+    this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
+}
+
 // ----------------------------------------------------------------------
 // Private helper methods
 // ----------------------------------------------------------------------
@@ -102,11 +161,13 @@ void ModeManager ::loadState() {
         status = file.read(reinterpret_cast<U8*>(&state), bytesRead, Os::File::WaitType::WAIT);
 
         if (status == Os::File::OP_OK && bytesRead == sizeof(PersistentState)) {
-            // Validate state data before restoring
-            if (state.mode <= static_cast<U8>(SystemMode::SAFE_MODE)) {
+            // Validate state data before restoring (valid range: 1-3 for SAFE, NORMAL, PAYLOAD)
+            if (state.mode >= static_cast<U8>(SystemMode::SAFE_MODE) &&
+                state.mode <= static_cast<U8>(SystemMode::PAYLOAD_MODE)) {
                 // Valid mode value - restore state
                 this->m_mode = static_cast<SystemMode>(state.mode);
                 this->m_safeModeEntryCount = state.safeModeEntryCount;
+                this->m_payloadModeEntryCount = state.payloadModeEntryCount;
 
                 // Restore physical hardware state to match loaded mode
                 if (this->m_mode == SystemMode::SAFE_MODE) {
@@ -116,6 +177,14 @@ void ModeManager ::loadState() {
                     // Log that we're restoring safe mode (not entering it fresh)
                     Fw::LogStringArg reasonStr("State restored from persistent storage");
                     this->log_WARNING_HI_EnteringSafeMode(reasonStr);
+                } else if (this->m_mode == SystemMode::PAYLOAD_MODE) {
+                    // PAYLOAD_MODE - turn on face switches AND payload switches
+                    this->turnOnComponents();  // Face switches (0-5)
+                    this->turnOnPayload();     // Payload switches (6-7)
+
+                    // Log that we're restoring payload mode
+                    Fw::LogStringArg reasonStr("State restored from persistent storage");
+                    this->log_ACTIVITY_HI_EnteringPayloadMode(reasonStr);
                 } else {
                     // NORMAL mode - ensure components are turned on
                     this->turnOnComponents();
@@ -124,6 +193,7 @@ void ModeManager ::loadState() {
                 // Corrupted state - use defaults
                 this->m_mode = SystemMode::NORMAL;
                 this->m_safeModeEntryCount = 0;
+                this->m_payloadModeEntryCount = 0;
                 this->turnOnComponents();
             }
         }
@@ -133,6 +203,7 @@ void ModeManager ::loadState() {
         // File doesn't exist or can't be opened - initialize to default state
         this->m_mode = SystemMode::NORMAL;
         this->m_safeModeEntryCount = 0;
+        this->m_payloadModeEntryCount = 0;
         this->turnOnComponents();
     }
 }
@@ -151,6 +222,7 @@ void ModeManager ::saveState() {
     PersistentState state;
     state.mode = static_cast<U8>(this->m_mode);
     state.safeModeEntryCount = this->m_safeModeEntryCount;
+    state.payloadModeEntryCount = this->m_payloadModeEntryCount;
 
     FwSizeType bytesToWrite = sizeof(PersistentState);
     FwSizeType bytesWritten = bytesToWrite;
@@ -223,6 +295,66 @@ void ModeManager ::exitSafeMode() {
     this->saveState();
 }
 
+void ModeManager ::enterPayloadMode(const char* reasonOverride) {
+    // Transition to payload mode
+    this->m_mode = SystemMode::PAYLOAD_MODE;
+    this->m_payloadModeEntryCount++;
+
+    // Build reason string
+    Fw::LogStringArg reasonStr;
+    char reasonBuf[100];
+    if (reasonOverride != nullptr) {
+        reasonStr = reasonOverride;
+    } else {
+        snprintf(reasonBuf, sizeof(reasonBuf), "Unknown");
+        reasonStr = reasonBuf;
+    }
+
+    this->log_ACTIVITY_HI_EnteringPayloadMode(reasonStr);
+
+    // Turn on payload switches (6 & 7)
+    this->turnOnPayload();
+
+    // Update telemetry
+    this->tlmWrite_CurrentMode(static_cast<U8>(this->m_mode));
+    this->tlmWrite_PayloadModeEntryCount(this->m_payloadModeEntryCount);
+
+    // Notify other components of mode change with new mode value
+    if (this->isConnected_modeChanged_OutputPort(0)) {
+        Components::SystemMode fppMode = static_cast<Components::SystemMode::T>(this->m_mode);
+        this->modeChanged_out(0, fppMode);
+    }
+
+    // Save state
+    this->saveState();
+}
+
+void ModeManager ::exitPayloadMode() {
+    // Transition back to normal mode
+    this->m_mode = SystemMode::NORMAL;
+
+    this->log_ACTIVITY_HI_ExitingPayloadMode();
+
+    // Turn off payload switches
+    this->turnOffPayload();
+
+    // Ensure face switches (0-5) are ON for NORMAL mode
+    // This guarantees consistent state regardless of transition path
+    this->turnOnComponents();
+
+    // Update telemetry
+    this->tlmWrite_CurrentMode(static_cast<U8>(this->m_mode));
+
+    // Notify other components of mode change with new mode value
+    if (this->isConnected_modeChanged_OutputPort(0)) {
+        Components::SystemMode fppMode = static_cast<Components::SystemMode::T>(this->m_mode);
+        this->modeChanged_out(0, fppMode);
+    }
+
+    // Save state
+    this->saveState();
+}
+
 void ModeManager ::turnOffNonCriticalComponents() {
     // Turn OFF:
     // - Satellite faces 0-5 (LoadSwitch instances 0-5)
@@ -240,16 +372,37 @@ void ModeManager ::turnOffNonCriticalComponents() {
 }
 
 void ModeManager ::turnOnComponents() {
-    // Turn ON all load switches to restore normal operation
+    // Turn ON face load switches (0-5) to restore normal operation
+    // Note: Payload switches (6-7) are NOT turned on here - they require PAYLOAD_MODE
 
-    // Send turn on signal to all 8 load switches
-    for (FwIndexType i = 0; i < 8; i++) {
+    // Send turn on signal to face load switches only (indices 0-5)
+    for (FwIndexType i = 0; i < 6; i++) {
         if (this->isConnected_loadSwitchTurnOn_OutputPort(i)) {
             this->loadSwitchTurnOn_out(i);
         }
     }
 }
 
+void ModeManager ::turnOnPayload() {
+    // Turn ON payload load switches (6 = payload power, 7 = payload battery)
+    if (this->isConnected_loadSwitchTurnOn_OutputPort(6)) {
+        this->loadSwitchTurnOn_out(6);
+    }
+    if (this->isConnected_loadSwitchTurnOn_OutputPort(7)) {
+        this->loadSwitchTurnOn_out(7);
+    }
+}
+
+void ModeManager ::turnOffPayload() {
+    // Turn OFF payload load switches (6 = payload power, 7 = payload battery)
+    if (this->isConnected_loadSwitchTurnOff_OutputPort(6)) {
+        this->loadSwitchTurnOff_out(6);
+    }
+    if (this->isConnected_loadSwitchTurnOff_OutputPort(7)) {
+        this->loadSwitchTurnOff_out(7);
+    }
+}
+
 F32 ModeManager ::getCurrentVoltage(bool& valid) {
     // Call the voltage get port to get current system voltage
     if (this->isConnected_voltageGet_OutputPort(0)) {

FprimeZephyrReference/Components/ModeManager/ModeManager.fpp

@@ -1,9 +1,10 @@
 module Components {
 
-    @ System mode enumeration
+    @ System mode enumeration (values ordered for +1/-1 sequential transitions)
     enum SystemMode {
-        NORMAL = 0 @< Normal operational mode
         SAFE_MODE = 1 @< Safe mode with non-critical components powered off
+        NORMAL = 2 @< Normal operational mode
+        PAYLOAD_MODE = 3 @< Payload mode with payload power and battery enabled
     }
 
     @ Port for notifying about mode changes
@@ -57,6 +58,14 @@ module Components {
         @ Only succeeds if currently in safe mode
         sync command EXIT_SAFE_MODE()
 
+        @ Command to enter payload mode
+        @ Only succeeds if currently in normal mode
+        sync command ENTER_PAYLOAD_MODE()
+
+        @ Command to exit payload mode
+        @ Only succeeds if currently in payload mode
+        sync command EXIT_PAYLOAD_MODE()
+
         # ----------------------------------------------------------------------
         # Events
         # ----------------------------------------------------------------------
@@ -83,6 +92,22 @@ module Components {
             severity warning high \
             format "External fault detected - external component forced safe mode"
 
+        @ Event emitted when entering payload mode
+        event EnteringPayloadMode(
+            reason: string size 100 @< Reason for entering payload mode
+        ) \
+            severity activity high \
+            format "ENTERING PAYLOAD MODE: {}"
+
+        @ Event emitted when exiting payload mode
+        event ExitingPayloadMode() \
+            severity activity high \
+            format "Exiting payload mode"
+
+        @ Event emitted when payload mode is manually commanded
+        event ManualPayloadModeEntry() \
+            severity activity high \
+            format "Payload mode entry commanded manually"
 
         @ Event emitted when command validation fails
         event CommandValidationFailed(
@@ -110,6 +135,9 @@ module Components {
         @ Number of times safe mode has been entered
         telemetry SafeModeEntryCount: U32
 
+        @ Number of times payload mode has been entered
+        telemetry PayloadModeEntryCount: U32
+
 
         ###############################################################################
         # Standard AC Ports: Required for Channels, Events, Commands, and Parameters  #