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(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 {