diff --git a/Tools/AP_Periph/FSOPowerStack.cpp b/Tools/AP_Periph/FSOPowerStack.cpp
index 3d397ffdaac2d5..94dae5fc9774fd 100644
--- a/Tools/AP_Periph/FSOPowerStack.cpp
+++ b/Tools/AP_Periph/FSOPowerStack.cpp
@@ -143,60 +143,162 @@ void FSOPowerStack::update_switches()
 {
     uint32_t now_ms = AP_HAL::millis();
     bool main = hal.gpio->read(FSO_SWITCH_MAIN_PIN);
-    auto &batt = AP::battery();
+    bool payload = hal.gpio->read(FSO_SWITCH_PAYLOAD_PIN);
 
     if (!main) {
         last_main_on_ms = now_ms;
-        last_main_switch = false;
+        main_switch_released = true;
+    }
+    
+    if (main_turn_on_state == Off){
+        if (main_switch_released && (now_ms - last_main_on_ms > FSO_SWITCH_ON_TIME_MS)) {
+            main_turn_on_state = PreChargeStart;
+            if (payload_turn_on_state == Off){
+                payload_turn_on_state = PreChargeStart;
+            }
+            main_switch_released = false;
+        }
+    } else {
+        if (main_switch_released && (now_ms - last_main_on_ms > FSO_SWITCH_OFF_TIME_MS)) {
+            main_turn_on_state = ShutDown;
+            payload_turn_on_state = ShutDown;
+            main_switch_released = false;
+        }
+    }
+
+    if (!payload) {
+        last_payload_on_ms = now_ms;
+        payload_switch_released = true;
     }
     
-    // Alt Hold State Machine
+    if (payload_turn_on_state == Off){
+        if (payload_switch_released && (now_ms - last_payload_on_ms > FSO_SWITCH_ON_TIME_MS)) {
+            payload_turn_on_state = PreChargeStart;
+            payload_switch_released = false;
+        }
+    } else {
+        if (payload_switch_released && (now_ms - last_payload_on_ms > FSO_SWITCH_OFF_TIME_MS)) {
+            payload_turn_on_state = ShutDown;
+            payload_switch_released = false;
+        }
+    }
+
+}
+
+void FSOPowerStack::update_main_power()
+{
+    uint32_t now_ms = AP_HAL::millis();
+    auto &batt = AP::battery();
+    
+    // Main Turn On State Machine
     switch (main_turn_on_state) {
 
     case Off:
-        if (!last_main_switch && (now_ms - last_main_on_ms > FSO_SWITCH_ON_TIME_MS)) {
+        break;
+
+    case PreChargeStart:
+        if (fabsf(batt.voltage(0) - batt.voltage(1)) < FSO_BATT_VOLTS_DIFF_MAX) {
+            hal.gpio->write(FSO_LED_DEBUG_PIN, 1);
+            hal.gpio->write(FSO_MAIN_PC_PIN, 1);
             start_main_precharge_ms = now_ms;
-            last_main_switch = true;
-            if (fabsf(batt.voltage(0) - batt.voltage(1)) < FSO_BATT_VOLTS_DIFF_MAX) {
-                hal.gpio->write(FSO_LED_DEBUG_PIN, 1);
-                hal.gpio->write(FSO_MAIN_PC_PIN, 1);
-                main_turn_on_state = PreCharge;
-            } else {
-                GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Unbalanced Battery Voltage");
-            }
+            main_turn_on_state = PreCharge;
+            break;
+        } else {
+            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Unbalanced Battery Voltage");
+            main_turn_on_state = ShutDown;
         }
         break;
 
     case PreCharge:
-        last_main_on_ms = now_ms;
-        if ((batt.voltage(0) - batt.voltage(8) < FSO_OUT_VOLTS_DIFF_MAX) && (batt.voltage(1) - batt.voltage(8) < FSO_OUT_VOLTS_DIFF_MAX)) {
+        if (MAX(batt.voltage(0), batt.voltage(1)) - batt.voltage(2) < FSO_OUT_VOLTS_DIFF_MAX) {
+            // Turn off Pre-Charge
             hal.gpio->write(FSO_LED_DEBUG_PIN, 0);
             hal.gpio->write(FSO_MAIN_PC_PIN, 0);
+
+            // Turn on Main Power and Led indicator
             hal.gpio->write(FSO_LED_MAIN_PIN, 1);
             hal.gpio->write(FSO_BAT_1_EN_PIN, 1);
             hal.gpio->write(FSO_BAT_2_EN_PIN, 1);
             main_turn_on_state = On;
-            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Capacitance: %.2f uF", 2.0*1e3*(now_ms - start_main_precharge_ms)/batt.voltage(8));
-            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Turn On Time: %lu ms", now_ms - start_main_precharge_ms);
         }
         if (now_ms - start_main_precharge_ms > FSO_PRECHARGE_TIMEOUT_MS) {
-            hal.gpio->write(FSO_LED_DEBUG_PIN, 0);
-            hal.gpio->write(FSO_MAIN_PC_PIN, 0);
-            main_turn_on_state = Off;
+            main_turn_on_state = ShutDown;
         }
         break;
 
     case On:
-        if (!last_main_switch && (now_ms - last_main_on_ms > FSO_SWITCH_OFF_TIME_MS)) {
-            hal.gpio->write(FSO_LED_MAIN_PIN, 0);
-            hal.gpio->write(FSO_BAT_1_EN_PIN, 0);
-            hal.gpio->write(FSO_BAT_2_EN_PIN, 0);
-            main_turn_on_state = Off;
-            last_main_switch = true;
-        }
+        break;
+
+    case ShutDown:
+        // Turn off Pre-Charge
+        hal.gpio->write(FSO_LED_DEBUG_PIN, 0);
+        hal.gpio->write(FSO_MAIN_PC_PIN, 0);
+
+        // Turn off Main Power and Led indicator
+        hal.gpio->write(FSO_LED_MAIN_PIN, 0);
+        hal.gpio->write(FSO_BAT_1_EN_PIN, 0);
+        hal.gpio->write(FSO_BAT_2_EN_PIN, 0);
+        main_turn_on_state = Off;
         break;
     }
+}
 
+void FSOPowerStack::update_payload_power()
+{
+    uint32_t now_ms = AP_HAL::millis();
+    auto &batt = AP::battery();
+    
+    // Payload Turn On State Machine
+    switch (payload_turn_on_state) {
+
+    case Off:
+        break;
+
+    case PreChargeStart:
+        if (fabsf(batt.voltage(0) - batt.voltage(1)) < FSO_BATT_VOLTS_DIFF_MAX) {
+            hal.gpio->write(FSO_LED_DEBUG_PIN, 1);
+            hal.gpio->write(FSO_PAYLOAD_PC_PIN, 1);
+            start_payload_precharge_ms = now_ms;
+            payload_turn_on_state = PreCharge;
+            break;
+        } else {
+            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Unbalanced Battery Voltage");
+            payload_turn_on_state = ShutDown;
+        }
+        break;
+
+    case PreCharge:
+        if (MAX(batt.voltage(0), batt.voltage(1)) - batt.voltage(2) < FSO_OUT_VOLTS_DIFF_MAX) {
+            // Turn off Pre-Charge
+            hal.gpio->write(FSO_LED_DEBUG_PIN, 0);
+            hal.gpio->write(FSO_PAYLOAD_PC_PIN, 0);
+
+            // Turn on Main Power and Led indicator
+            hal.gpio->write(FSO_LED_PAYLOAD_PIN, 1);
+            hal.gpio->write(FSO_BAT_1_EN_PIN, 1);
+            hal.gpio->write(FSO_BAT_2_EN_PIN, 1);
+            payload_turn_on_state = On;
+        }
+        if (now_ms - start_payload_precharge_ms > FSO_PRECHARGE_TIMEOUT_MS) {
+            payload_turn_on_state = ShutDown;
+        }
+        break;
+
+    case On:
+        break;
+
+    case ShutDown:
+        // Turn off Pre-Charge
+        hal.gpio->write(FSO_LED_DEBUG_PIN, 0);
+        hal.gpio->write(FSO_PAYLOAD_PC_PIN, 0);
+
+        // Turn off Main Power and Led indicator
+        hal.gpio->write(FSO_LED_PAYLOAD_PIN, 0);
+        hal.gpio->write(FSO_BAT_1_EN_PIN, 0);
+        hal.gpio->write(FSO_BAT_2_EN_PIN, 0);
+        payload_turn_on_state = Off;
+        break;
+    }
 }
 
 /*
@@ -204,7 +306,6 @@ void FSOPowerStack::update_switches()
  */
 void FSOPowerStack::update()
 {
-    update_switches();
     
     uint32_t now_ms = AP_HAL::millis();
     if (now_ms - last_update_ms < 100) {
@@ -213,9 +314,15 @@ void FSOPowerStack::update()
     }
     last_update_ms = now_ms;
 
+    update_switches();
+
     update_fans();
 
     report_voltages();
+
+    update_main_power();
+
+    update_payload_power();
 }
 
 #endif  // HAL_PERIPH_ENABLE_FSO_POWER_STACK
diff --git a/Tools/AP_Periph/FSOPowerStack.h b/Tools/AP_Periph/FSOPowerStack.h
index 3ad8f3db4ff758..c9eed85fa6b552 100644
--- a/Tools/AP_Periph/FSOPowerStack.h
+++ b/Tools/AP_Periph/FSOPowerStack.h
@@ -27,11 +27,13 @@ class FSOPowerStack {
         float rpm;
     };
 
-    // Alt_Hold based flight mode states used in Alt_Hold, Loiter, and Sport
+    // States used during turn on
     enum TurnOnState {
         Off,
+        PreChargeStart,
         PreCharge,
-        On
+        On,
+        ShutDown
     };
 
     uint32_t last_fan_ms;
@@ -44,18 +46,23 @@ class FSOPowerStack {
     void init_fan(uint8_t pin, FAN &fan);
     void update_fans();
     void update_switches();
+    void update_main_power();
+    void update_payload_power();
 
     uint32_t last_report_ms;
     void report_voltages();
 
-    bool last_main_switch;
-    bool last_payload_switch;
+    bool main_switch_released;
+    bool payload_switch_released;
 
     uint32_t start_main_precharge_ms;
+    uint32_t start_payload_precharge_ms;
+
     uint32_t last_main_on_ms;
+    uint32_t last_payload_on_ms;
 
-    TurnOnState main_turn_on_state;
-    TurnOnState payload_turn_on_state;
+    TurnOnState main_turn_on_state = Off;
+    TurnOnState payload_turn_on_state = Off;
 };
 
 #endif // HAL_PERIPH_ENABLE_FSO_POWER_STACK