From d5507b2e00cdaf57e3846587203bba6b20b259ff Mon Sep 17 00:00:00 2001
From: zhaojingkui <1553836110@qq.com>
Date: Thu, 26 Feb 2026 10:47:26 +0800
Subject: [PATCH] =?UTF-8?q?=E5=9C=A8=E6=95=85=E9=9A=9C=E6=A8=A1=E5=BC=8F?=
 =?UTF-8?q?=E4=B8=8B=E6=96=AD=E5=BC=80=E6=8E=A8=E8=BF=9B=E7=94=B5=E6=9C=BA?=
 =?UTF-8?q?\=E8=88=B5=E6=9C=BA\=E5=90=AF=E9=97=AD=E6=9C=BA=E6=9E=84?=
 =?UTF-8?q?=E5=92=8C=E6=95=9E=E6=B0=B4=E6=9C=BA=E6=9E=84=E7=9A=84=E7=94=B5?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/pPowerManger/fsm/FaultState.cpp | 55 +++++++++++++++++++++++++++--
 src/pPowerManger/fsm/Lifting.cpp    |  4 ++-
 src/pPowerManger/systemData.h       |  4 +--
 3 files changed, 58 insertions(+), 5 deletions(-)

diff --git a/src/pPowerManger/fsm/FaultState.cpp b/src/pPowerManger/fsm/FaultState.cpp
index c6b3dae..1bc65ba 100644
--- a/src/pPowerManger/fsm/FaultState.cpp
+++ b/src/pPowerManger/fsm/FaultState.cpp
@@ -5,8 +5,59 @@ void FaultState::entry() {
     PowerManagerFsm::entry();
     if(pm->m_missionState.type == TaskType::THROW_LOAD)
     {
-       LOG_F(INFO, "检测到抛载");
-       pm->m_systemData->setSystemInfo("ERROR:检测到抛载");
+        LOG_F(INFO, "检测到抛载");
+        pm->m_systemData->setSystemInfo("ERROR:检测到抛载");
+        //1 解除启闭机构
+        unsigned char cmd = 0;
+        pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(0);
+        MOOSPause(3);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover1, (unsigned char)cmd);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover2, (unsigned char)cmd);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover3, (unsigned char)cmd);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover4, (unsigned char)cmd);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover5, (unsigned char)cmd);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover6, (unsigned char)cmd);
+        pm->m_upperCommManager->processPeriodicTasks();
+        //2 解除敞水机构
+        pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(0);
+        MOOSPause(3);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet1, (unsigned char)cmd);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet2, (unsigned char)cmd);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet3, (unsigned char)cmd);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet4, (unsigned char)cmd);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet5, (unsigned char)cmd);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet6, (unsigned char)cmd);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet7, (unsigned char)cmd);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet8, (unsigned char)cmd);
+        pm->m_upperCommManager->processPeriodicTasks();
+        //3 解除艏部舵机
+        pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(0);
+        MOOSPause(3);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowRudderDeploy, (unsigned char)cmd);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowRudderServo1, (unsigned char)cmd);
+        pm->m_upperCommManager->processPeriodicTasks();
+        //4 解除艉部舵机
+        pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(0);
+        MOOSPause(3);
+        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::mastLiftingServo, (unsigned char)cmd);
+        pm->m_upperCommManager->processPeriodicTasks();
+        //关闭推进电机
+        pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(0);
+        //检测电机去使能状态，5秒超时
+        auto startTime = std::chrono::steady_clock::now();
+        while(pm->m_thrustEnable == 1)
+        {
+            // 检查是否超时（5秒）
+            auto currentTime = std::chrono::steady_clock::now();
+            auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(currentTime - startTime);
+            if(elapsed.count() >= 20)
+            {
+                LOG_F(WARNING, "电机去使能超时（20秒）");
+                pm->m_systemData->setSystemInfo("ERROR:电机去使能超时（20秒）");
+                return;
+            }
+            MOOSPause(100);
+        }
     }
 }
 
diff --git a/src/pPowerManger/fsm/Lifting.cpp b/src/pPowerManger/fsm/Lifting.cpp
index e27bd29..5b7f8b2 100644
--- a/src/pPowerManger/fsm/Lifting.cpp
+++ b/src/pPowerManger/fsm/Lifting.cpp
@@ -63,7 +63,9 @@ void Lifting::entry() {
     MOOSPause(3);
     pm->m_lowerCommManager->operate_HV_coolingSystem_Breaker(0);
     MOOSPause(3);
-
+    // 仪表电恢复待机状态
+    pm->m_driver.getSTANDBYTable(&pm->m_subDisSysCmd); 
+    pm->m_upperCommManager->processPeriodicTasks();
     //2.5 断开动力电断路器
     pm->m_lowerCommManager->operate_HV_bowHighVoltageBox_Breaker(0);
     MOOSPause(3);
diff --git a/src/pPowerManger/systemData.h b/src/pPowerManger/systemData.h
index d65b2f1..8ca9951 100644
--- a/src/pPowerManger/systemData.h
+++ b/src/pPowerManger/systemData.h
@@ -864,8 +864,8 @@ public:
             if(power_bus_status.meterPowerLossSignal == 1) disSysFaultCode.insert(36); else disSysFaultCode.erase(36);
             if(power_bus_status.powerLithiumBatteryCircuitBreaker == 1)
             {
-                if(power_bus_status.aBusInsulationStatus == 1) disSysFaultCode.insert(35);
-                if(power_bus_status.powerBusInsulationStatus == 1) disSysFaultCode.insert(34);
+                if(power_bus_status.aBusInsulationStatus == 1) disSysFaultCode.insert(35); else disSysFaultCode.erase(35);
+                if(power_bus_status.powerBusInsulationStatus == 1) disSysFaultCode.insert(34); else disSysFaultCode.erase(34);
             }
             else
             {