Normal <-> Payload Auto Switch

FprimeZephyrReference/Components/ModeManager/ModeManager.cpp

@@ -18,7 +18,12 @@ 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),
+      m_lowVoltageCounter(0) {}
 
 ModeManager ::~ModeManager() {}
 
@@ -35,18 +40,45 @@ void ModeManager ::run_handler(FwIndexType portNum, U32 context) {
     // Increment run counter (1Hz tick counter)
     this->m_runCounter++;
 
+    // Check for low voltage fault when in PAYLOAD_MODE
+    if (this->m_mode == SystemMode::PAYLOAD_MODE) {
+        bool valid = false;
+        F32 voltage = this->getCurrentVoltage(valid);
+
+        // Treat both low voltage AND invalid readings as fault conditions
+        // Invalid readings (sensor failure/disconnection) should not mask brownout protection
+        bool isFault = !valid || (voltage < LOW_VOLTAGE_THRESHOLD);
+
+        if (isFault) {
+            this->m_lowVoltageCounter++;
+
+            if (this->m_lowVoltageCounter >= LOW_VOLTAGE_DEBOUNCE_SECONDS) {
+                // Trigger automatic exit from payload mode
+                // Use 0.0 for voltage if reading was invalid
+                this->exitPayloadModeAutomatic(valid ? voltage : 0.0f);
+                this->m_lowVoltageCounter = 0;  // Reset counter
+            }
+        } else {
+            // Voltage OK and valid - reset counter
+            this->m_lowVoltageCounter = 0;
+        }
+    } else {
+        // Not in payload mode - reset counter
+        this->m_lowVoltageCounter = 0;
+    }
+
     // Update telemetry
     this->tlmWrite_CurrentMode(static_cast<U8>(this->m_mode));
 }
 
 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 +92,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 +132,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 +189,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 +205,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 +221,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 +231,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 +250,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 +323,92 @@ void ModeManager ::exitSafeMode() {
     this->saveState();
 }
 
+void ModeManager ::enterPayloadMode(const char* reasonOverride) {
+    // Transition to payload mode
+    this->m_mode = SystemMode::PAYLOAD_MODE;
+    this->m_payloadModeEntryCount++;
+    this->m_lowVoltageCounter = 0;  // Reset low voltage counter on mode entry
+
+    // 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 ALL load switches (faces 0-5 AND payload 6-7)
+    // This ensures proper state even after automatic fault exit
+    this->turnOnComponents();  // Face switches (0-5)
+    this->turnOnPayload();     // Payload switches (6-7)
+
+    // 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 (manual exit)
+    this->m_mode = SystemMode::NORMAL;
+
+    this->log_ACTIVITY_HI_ExitingPayloadMode();
+
+    // Turn off payload switches
+    this->turnOffPayload();
+
+    // Ensure face switches are ON for NORMAL mode
+    // This guarantees consistent state even if faces were turned off during payload mode
+    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 ::exitPayloadModeAutomatic(F32 voltage) {
+    // Automatic exit from payload mode due to fault condition (e.g., low voltage)
+    // More aggressive than manual exit - turns off ALL switches
+    this->m_mode = SystemMode::NORMAL;
+
+    this->log_WARNING_HI_AutoPayloadModeExit(voltage);
+
+    // Turn OFF all load switches (aggressive - includes faces 0-5 and payload 6-7)
+    this->turnOffNonCriticalComponents();
+
+    // 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 +426,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)) {