#include "PowerManagerFsm.hpp"
#include "../PowerManger.h"
#include "../logc/loguru.hpp"
#include <string>
#include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h"
#include "../faultCode.h"

// #define DEBUG
// #define DEBUG_1
// 定义静态成员
PowerManger* PowerManagerFsm::pm = nullptr;
void PowerManagerFsm::react(TaskStopEvent const &e) {
    transit<RemoteControl>();
    LOG_F(INFO, "使命结束"); 
};

void PowerManagerFsm::react(CycleTriggeredEvent const &){
    // 检测和更新状态
    #ifdef DEBUG_1
        pm->m_currentDriverState = pm->m_subDisSysState;
    #else
        getNoPowerDeviceStatus(pm->m_currentDriverState);
        getBowRudderSystemStatus(pm->m_currentDriverState);
        getActuatorStatus(pm->m_currentDriverState);
        getSternRudderStatus(pm->m_currentDriverState);
        getPropulsionMotorStatus(pm->m_currentDriverState);
        getBuoyancyAdjustmentStatus(pm->m_currentDriverState);
        getFBSystemCmdStatus(pm->m_currentDriverState);
        getSideSonarStatus(pm->m_currentDriverState);
        getMastFoldingSystem(pm->m_currentDriverState);
        getOpenWaterStatus(pm->m_currentDriverState);
    #endif
    //非调试模式下，对水声设备进行保护
    if(pm->currentMod  != PMMode::Debug)
    {
        if(pm->m_depth < OVER_DEPTH)
        {
            if(pm->m_systemData->power_a_bus_status.xczCircuitBreaker == 1)
                pm->m_lowerCommManager->operate_HVA_xczCircuit_Breaker(0);
            //舷侧阵自动下电
            MOOSPause(3);
            pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sideSonar, 0);
            //所有水声设备下电
            for(auto iter : underWterDevices)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(iter), 0);
            }
        }
    }
    // 故障编码
    basicNavigationFault();
    extendedNavigationFault();
    basicNavigationFault();
    extendedNavigationFault();
    obstacleAvoidanceFault();
    buoyancyControlFault();
    communicationSystemFault();
    payloadSystemFault();
    emergencySystemFault();
    sensorSystemFault();
}

void PowerManagerFsm::handleCuriseWork()
{
    //依次接入执行机构动力电设备
    //1. 开启动力电
    //1.1 动力锂电池上电
    pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Start;
    //1.2 闭合动力电断路器
    if(pm->m_systemData->power_bus_status.lithiumBatteryGroupInstrumentCircuitBreaker != 1)
    {
        pm->m_lowerCommManager->operate_HV_powerLithiumBattery_Breaker(1);
        MOOSPause(3);
    }
    if(pm->m_systemData->power_bus_status.dcDc5ModuleCircuitBreaker != 1)
    {
        pm->m_lowerCommManager->operate_HV_dcDc5Module_Breaker(1);
        MOOSPause(3);
    }
    if(pm->m_systemData->m_dcdcStatus.workingStatus != 1)
    {
        pm->m_lowerCommManager->operate_HV_dcDc5_Breaker(1);
        MOOSPause(3);
    }
    if(!(pm->m_systemData->m_dcdcStatus.coolingPump1Running || pm->m_systemData->m_dcdcStatus.coolingPump2Running))
    {
        pm->m_lowerCommManager->operate_HV_coolingSystem_Breaker(1);
        MOOSPause(10);
    }
    if(pm->m_systemData->power_bus_status.bowHighVoltageDistributionBoxCircuitBreaker != 1)
    {
        pm->m_lowerCommManager->operate_HV_bowHighVoltageBox_Breaker(1);
        MOOSPause(3);
    }
    //2.  启动动力电设备 
    unsigned char cmd = 1;
    //2.1 艏舵
    if(pm->m_currentDriverState.bowRudderDeploy != 1 || pm->m_currentDriverState.bowRudderServo1 != 1)
    {
        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();
        pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(1);
    }
    //2.2 桅杆舵机
    if(pm->m_currentDriverState.mastLiftingServo != 1)
    {
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::mastLiftingServo, (unsigned char)cmd);
        pm->m_upperCommManager->processPeriodicTasks();
        pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(1);
    }
    //2.3 艏部浮调机构
    if(pm->m_currentDriverState.buoyancyAdjust1 != 1 || pm->m_currentDriverState.buoyancyAdjust2 != 1 || pm->m_currentDriverState.buoyancyAdjust3 != 1)
    {
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust1, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust2, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust3, (unsigned char)cmd);    
        pm->m_upperCommManager->processPeriodicTasks();            
        pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(1);
    }
    //2.4 艉部浮调机构
    if(pm->m_currentDriverState.buoyancyAdjust4 != 1 || pm->m_currentDriverState.buoyancyAdjust5 != 1)
    {
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust4, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust5, (unsigned char)cmd);
        pm->m_upperCommManager->processPeriodicTasks();
        pm->m_lowerCommManager->operate_HV_sternFt45_Breaker(1);
    }
    //2.5 推进电机
    if(pm->m_currentDriverState.thruster != 1)
    {
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::thruster, (unsigned char)cmd);
        pm->m_upperCommManager->processPeriodicTasks();
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(1);
    }
    //2.6 艉部舵机
    if(pm->m_currentDriverState.sternRudderServo1 != 1 || 
        pm->m_currentDriverState.sternRudderServo2 != 1 ||
        pm->m_currentDriverState.sternRudderServo3!= 1 ||
        pm->m_currentDriverState.sternRudderServo4 != 1)
    {
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo1, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo2, (unsigned char)cmd);
        pm->m_upperCommManager->processPeriodicTasks();
        pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(1);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo3, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo4, (unsigned char)cmd);
        pm->m_upperCommManager->processPeriodicTasks();
        pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(1);        
    }
}

void PowerManagerFsm::handleNoPowerDeviceCmd(int id , int cmd){
    if(noPowerDevices.find(id) != noPowerDevices.end())
    {
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, (DeviceId::Type)id, (unsigned char)cmd);
        LOG_F(INFO, "无动力电设备操作: %d, %d", DeviceId::toString((DeviceId::Type)id),id, cmd);
    }
}
void PowerManagerFsm::handleHydroacousticDeviceCmd(int id , int cmd)
{
    if(underWterDevices.find(id) != underWterDevices.end())
    {
        if (pm->currentMod  != PMMode::Debug)
        {
            if(pm->m_depth < OVER_DEPTH)
            {
                LOG_F(INFO, "水下设备操作上电保护 %d ", id);
                return;
            }
        }
        // 如果是前视声纳断电则先关机再断电
        if(id == DeviceId::forwardSonar)
        {
            if(cmd == 0)
            {
                LOG_F(INFO,"前视声纳关机指令");
                pm->sendNotification("uExternComm_frontSonarControl_cmd","0");
                MOOSPause(10000);
            }
        }
        LOG_F(INFO, "水声设备操作: %d, %d", DeviceId::toString((DeviceId::Type)id), id, cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, (DeviceId::Type)id, (unsigned char)cmd);

    }
}

void PowerManagerFsm::handleBowRudderSystemCmd(int id, int cmd){
    if(id == DeviceId::bowRudderDeploy || 
       id == DeviceId::bowRudderServo1 ||
       id == DeviceId::bowRudderServo2)
    {
        //仅用艏舵1指令进行上电
        if(id == DeviceId::bowRudderServo2)
            id = DeviceId::bowRudderServo1;
        //正常上电流程
        if(pm->currentMod  == PMMode::Test)
        {
            if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(0);
                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_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowRudderServo2, (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            } else if( cmd == EquipmentCommand::ON)
            {
                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_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowRudderServo2, (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(1);
            }
        }
        else if(pm->currentMod ==  PMMode::Debug)
        {

            if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(1);
            }
            else if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            }
            else if(cmd == EquipmentCommand::POWER_ON)
            {
                pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(1);
            }
            else if(cmd == EquipmentCommand::POWER_OFF)
            {
                pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(0);
            }
            else if(cmd == EquipmentCommand::METER_OFF)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)0);
            }
            else if(cmd == EquipmentCommand::METER_ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)1);
            }
        }
    }
}
void PowerManagerFsm::handleActuatorCmd(int id, int cmd){
    if(id == DeviceId::bowCover1 ||
       id == DeviceId::bowCover2 ||
       id == DeviceId::bowCover3 ||
       id == DeviceId::bowCover4 ||
       id == DeviceId::bowCover5 ||
       id == DeviceId::bowCover6)
    {
        if(pm->currentMod  == PMMode::Test)
        {
            if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(0);
                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();
            } else if(cmd == EquipmentCommand::ON)
            {
                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();
                pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(1);
            }
        }
        else if(pm->currentMod ==  PMMode::Debug)
        {

            if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(1);
            }
            else if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            }
            else if(cmd == EquipmentCommand::POWER_ON)
            {
                pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(1);
            }
            else if(cmd == EquipmentCommand::POWER_OFF)
            {
                pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(0);
            }
            else if(cmd == EquipmentCommand::METER_OFF)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)0);
            }
            else if(cmd == EquipmentCommand::METER_ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)1);
            }
        }
    }

}

void PowerManagerFsm::handleOpenWaterCmd(int id, int cmd)
{
    if(id == DeviceId::bowWaterInet1 ||
       id == DeviceId::bowWaterInet2 ||
       id == DeviceId::bowWaterInet3 ||
       id == DeviceId::bowWaterInet4 ||
       id == DeviceId::bowWaterInet5 ||
       id == DeviceId::bowWaterInet6 ||
       id == DeviceId::bowWaterInet7 ||
       id == DeviceId::bowWaterInet8)
    {
        if(pm->currentMod  == PMMode::Test)
        {
            if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(0);
                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();
            } else if (cmd == EquipmentCommand::ON)
            {
                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();
                pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(1);
            }
        }
        else if(pm->currentMod ==  PMMode::Debug)
        {

            if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(1);
            }
            else if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            }
            else if(cmd == EquipmentCommand::POWER_ON)
            {
                pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(1);
            }
            else if(cmd == EquipmentCommand::POWER_OFF)
            {
                pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(0);
            }
            else if(cmd == EquipmentCommand::METER_OFF)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)0);
            }
            else if(cmd == EquipmentCommand::METER_ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)1);
            }
        }
    }
}

void PowerManagerFsm::handleSternRudderCmd(int id, int cmd)
{   
    if(id == DeviceId::sternRudderServo1 ||
       id == DeviceId::sternRudderServo2)
    {
        if(pm->currentMod  == PMMode::Test)
        {
            if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo1, (unsigned char)cmd);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo2, (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();

            } else if (cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo1, (unsigned char)cmd);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo2, (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(1);
            }
        }
        else if(pm->currentMod ==  PMMode::Debug)
        {

            if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(1);
            }
            else if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            }
            else if(cmd == EquipmentCommand::POWER_ON)
            {
                pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(1);
            }
            else if(cmd == EquipmentCommand::POWER_OFF)
            {
                pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(0);
            }
            else if(cmd == EquipmentCommand::METER_OFF)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)0);
            }
            else if(cmd == EquipmentCommand::METER_ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)1);
            }
        }
    }

    if(id == DeviceId::sternRudderServo3 ||
       id == DeviceId::sternRudderServo4 )
    {
        if(pm->currentMod  == PMMode::Test)
        {
            if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo3, (unsigned char)cmd);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo4, (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();

            } else if (cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo3, (unsigned char)cmd);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo4, (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(1);
            }
        }
        else if(pm->currentMod ==  PMMode::Debug)
        {

            if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(1);
            }
            else if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            }
            else if(cmd == EquipmentCommand::POWER_ON)
            {
                pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(1);
            }
            else if(cmd == EquipmentCommand::POWER_OFF)
            {
                pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(0);
            }
            else if(cmd == EquipmentCommand::METER_OFF)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)0);
            }
            else if(cmd == EquipmentCommand::METER_ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)1);
            }
        }
    }
}

void PowerManagerFsm::handlePropulsionMotorCmd(int id, int cmd)
{
   if(id == DeviceId::thruster)
    {
        if(pm->currentMod  == PMMode::Test)
        {
            if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::thruster, (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            } else if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::thruster, (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(1);
            }
        }
        else if(pm->currentMod ==  PMMode::Debug)
        {

            if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(1);
            }
            else if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            }
            else if(cmd == EquipmentCommand::POWER_ON)
            {
                pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(1);
            }
            else if(cmd == EquipmentCommand::POWER_OFF)
            {
                pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(0);
            }
            else if(cmd == EquipmentCommand::METER_OFF)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)0);
            }
            else if(cmd == EquipmentCommand::METER_ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)1);
            }
        }
    }
}

void PowerManagerFsm::handleBuoyancyAdjustmentCmd(int id, int cmd)
{  
    if(id == DeviceId::buoyancyAdjust1 ||
       id == DeviceId::buoyancyAdjust2 ||
       id == DeviceId::buoyancyAdjust3)
    {
        if (pm->currentMod  == PMMode::Test)
        {
            if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust1, (unsigned char)cmd);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust2, (unsigned char)cmd);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust3, (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            } else if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust1, (unsigned char)cmd);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust2, (unsigned char)cmd);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust3, (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();                
                pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(1);
            }
        }
        else if(pm->currentMod ==  PMMode::Debug)
        {

            if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(1);
            }
            else if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            }
            else if(cmd == EquipmentCommand::POWER_ON)
            {
                pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(1);
            }
            else if(cmd == EquipmentCommand::POWER_OFF)
            {
                pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(0);
            }
            else if(cmd == EquipmentCommand::METER_OFF)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)0);
            }
            else if(cmd == EquipmentCommand::METER_ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)1);
            }
        }
    }

    if(id == DeviceId::buoyancyAdjust4 ||
       id == DeviceId::buoyancyAdjust5)
    {
        if (pm->currentMod  == PMMode::Test)
        {
            if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HV_sternFt45_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust4, (unsigned char)cmd);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust5, (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            } else if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust4, (unsigned char)cmd);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust5, (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HV_sternFt45_Breaker(1);
            }
        }
        else if(pm->currentMod ==  PMMode::Debug)
        {

            if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HV_sternFt45_Breaker(1);
            }
            else if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HV_sternFt45_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            }
            else if(cmd == EquipmentCommand::POWER_ON)
            {
                pm->m_lowerCommManager->operate_HV_sternFt45_Breaker(1);
            }
            else if(cmd == EquipmentCommand::POWER_OFF)
            {
                pm->m_lowerCommManager->operate_HV_sternFt45_Breaker(0);
            }
            else if(cmd == EquipmentCommand::METER_OFF)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)0);
            }
            else if(cmd == EquipmentCommand::METER_ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)1);
            }
        }
    }
}

void PowerManagerFsm::handleFBSystemCmd(int id, int cmd)
{
  if(id == DeviceId::buoyage)
    {
        if(pm->currentMod  == PMMode::Test)
        {
            if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HV_fbReserved_Breaker(0);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyage, (unsigned char)cmd);
            } else if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyage, (unsigned char)cmd);
                pm->m_lowerCommManager->operate_HV_fbReserved_Breaker(1);
                pm->m_upperCommManager->processPeriodicTasks();
            }            
        }
        else if(pm->currentMod ==  PMMode::Debug)
        {

            if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HV_fbReserved_Breaker(1);
            }
            else if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HV_fbReserved_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            }
            else if(cmd == EquipmentCommand::POWER_ON)
            {
                pm->m_lowerCommManager->operate_HV_fbReserved_Breaker(1);
            }
            else if(cmd == EquipmentCommand::POWER_OFF)
            {
                pm->m_lowerCommManager->operate_HV_fbReserved_Breaker(0);
            }
            else if(cmd == EquipmentCommand::METER_OFF)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)0);
            }
            else if(cmd == EquipmentCommand::METER_ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)1);
            }
        }
    }
}

void PowerManagerFsm::handleSideSonarCmd(int id, int cmd)
{   
    if(id == DeviceId::sideSonar)
    {
        if (pm->currentMod  != PMMode::Debug)
        {
            if(pm->m_depth < OVER_DEPTH)
            {
                LOG_F(INFO, "水下设备操作上电保护 %d ", id);
                return;
            }
        }
        if(pm->currentMod == PMMode::Test)
        {
            if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HVA_xczCircuit_Breaker(0);
                pm->m_upperCommManager->processPeriodicTasks();
                MOOSPause(10);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sideSonar, (unsigned char)cmd);
            } else if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sideSonar, (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                MOOSPause(10);
                pm->m_lowerCommManager->operate_HVA_xczCircuit_Breaker(1);
            }     
        }
        else if(pm->currentMod ==  PMMode::Debug)
        {

            if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                MOOSPause(5);
                pm->m_lowerCommManager->operate_HVA_xczCircuit_Breaker(1);
            }
            else if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HVA_xczCircuit_Breaker(0);
                MOOSPause(5);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            }
            else if(cmd == EquipmentCommand::POWER_ON)
            {
                pm->m_lowerCommManager->operate_HVA_xczCircuit_Breaker(1);
            }
            else if(cmd == EquipmentCommand::POWER_OFF)
            {
                pm->m_lowerCommManager->operate_HVA_xczCircuit_Breaker(0);
            }
            else if(cmd == EquipmentCommand::METER_OFF)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)0);
            }
            else if(cmd == EquipmentCommand::METER_ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)1);
            }
        }
    }
}

void PowerManagerFsm::handleMastFoldingSystemCmd(int id, int cmd)
{
   
    if(id == DeviceId::mastLiftingServo)
    {
        if(pm->currentMod == PMMode::Test)
        {
            if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::mastLiftingServo, (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            } else if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::mastLiftingServo, (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(1);
            }
        }
        else if(pm->currentMod ==  PMMode::Debug)
        {

            if(cmd == EquipmentCommand::ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
                pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(1);
            }
            else if(cmd == EquipmentCommand::OFF)
            {
                pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(0);
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
                pm->m_upperCommManager->processPeriodicTasks();
            }
            else if(cmd == EquipmentCommand::POWER_ON)
            {
                pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(1);
            }
            else if(cmd == EquipmentCommand::POWER_OFF)
            {
                pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(0);
            }
            else if(cmd == EquipmentCommand::METER_OFF)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)0);
            }
            else if(cmd == EquipmentCommand::METER_ON)
            {
                pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)1);
            }
        }
    }
}

void PowerManagerFsm::react(TaskStartEvent const &e){
    LOG_F(INFO, "TaskStartEvent received - type: %d, enableBowRudder: %d, enableBuoyancyAdjustment: %d", e.type, e.enableBowRudder, e.enableBuoyancyAdjustment);
    switch (e.type)
    {
    case TaskType::THROW_LOAD:
        transit<FaultState>();
        break;
    case TaskType::FLOAT_UP:
        transit<FloatDown>();
        break;
    case TaskType::RECYCLE:
        transit<CruiseMode>();
        break;
    case TaskType::HOVER:
        transit<FloatAdjust>();
        break;
    case TaskType::SAT_COMM:
        transit<FloatDown>();
        break;
    case TaskType::SAT_CALIBRATE:
        transit<FloatDown>();
        break;
    case TaskType::TOUR:
        transit<UnderwaterSurvey>();
        break;
    case TaskType::SAIL:
        transit<CruiseMode>();
        break;
    case TaskType::EMER_SAT_COMM:
        transit<FloatDown>();
        break;
    case TaskType::EMER_SAIL:
        transit<CruiseMode>();
        break;
    default:
        break;
    }
}
void PowerManagerFsm::react(DevCmdExt const &e) 
{
    LOG_F(INFO, "DevCmdExt received - id: %d, cmd: %d", e.id, e.cmd); 
    if(pm->currentMod == PMMode::Normal)
    {
        LOG_F(INFO,"正常模式 : 操作禁止");
        return;
    }
    handleNoPowerDeviceCmd(e.id , e.cmd);     // 非水声仪表设备
    handleHydroacousticDeviceCmd(e.id , e.cmd);     // 水声设备处理
    handleBowRudderSystemCmd(e.id , e.cmd);     // 艏舵设备处理
    handleActuatorCmd(e.id , e.cmd);        // 启闭机构处理
    handleSternRudderCmd(e.id , e.cmd);     // 艉舵设备处理
    handlePropulsionMotorCmd(e.id , e.cmd);     // 推进电机处理
    handleBuoyancyAdjustmentCmd(e.id , e.cmd);  // 浮调设备处理
    handleFBSystemCmd(e.id , e.cmd); // FB设备处理
    handleSideSonarCmd(e.id , e.cmd); // 舷侧设备处理
    handleMastFoldingSystemCmd(e.id , e.cmd); // 桅杆设备处理
    handleOpenWaterCmd(e.id , e.cmd); // 敞水盖电缸处理
}

void PowerManagerFsm::getNoPowerDeviceStatus(DriverTable &s)
{
    s.commMast          = pm->m_subDisSysState.commMast;
    s.ctd1              = pm->m_subDisSysState.ctd1;
    s.ctd2              = pm->m_subDisSysState.ctd2;
    s.depth1            = pm->m_subDisSysState.depth1;
    s.depth2            = pm->m_subDisSysState.depth2;
    s.depth3            = pm->m_subDisSysState.depth3;
    s.DVL               = pm->m_subDisSysState.DVL;
    s.electSensor       = pm->m_subDisSysState.electSensor;
    s.forwardSonar      = pm->m_subDisSysState.forwardSonar;
    s.inputWaterSensor1 = pm->m_subDisSysState.inputWaterSensor1;
    s.inputWaterSensor2 = pm->m_subDisSysState.inputWaterSensor2;
    s.inputWaterSensor3 = pm->m_subDisSysState.inputWaterSensor3;
    s.ins               = pm->m_subDisSysState.ins;
    s.launchControler   = pm->m_subDisSysState.launchControler;
    s.ntp               = pm->m_subDisSysState.ntp;
    s.overDeep1         = pm->m_subDisSysState.overDeep1;
    s.overDeep2         = pm->m_subDisSysState.overDeep2;
    s.photoSensor       = pm->m_subDisSysState.photoSensor;
    s.singleBeamSonar1  = pm->m_subDisSysState.singleBeamSonar1;
    s.singleBeamSonar2  = pm->m_subDisSysState.singleBeamSonar2;
    s.singleBeamSonar3  = pm->m_subDisSysState.singleBeamSonar3;
    s.singleBeamSonar4  = pm->m_subDisSysState.singleBeamSonar4;
    s.singleBeamSonar5  = pm->m_subDisSysState.singleBeamSonar5;
    s.singleBeamSonar6  = pm->m_subDisSysState.singleBeamSonar6;
    s.singleBeamSonar7  = pm->m_subDisSysState.singleBeamSonar7;
    s.singleBeamSonar8  = pm->m_subDisSysState.singleBeamSonar8;
    s.soundVeloc        = pm->m_subDisSysState.soundVeloc;
    s.waterInject       = pm->m_subDisSysState.waterInject;
    s.USBL              = pm->m_subDisSysState.USBL;
    s.lowFreqSonar      = pm->m_subDisSysState.lowFreqSonar;
    s.sternThrow        = pm->m_subDisSysState.sternThrow;
    s.bowThrow          = pm->m_subDisSysState.bowThrow;
}

void PowerManagerFsm::getBowRudderSystemStatus(DriverTable &s)
{
    if(pm->m_systemData->power_a_bus_status.bowRudderControlBoxCircuitBreaker == 1)
    {
        //检查艏舵电缸
        if(pm->m_subDisSysState.bowRudderDeploy == 1)
        {
            s.bowRudderDeploy = 1;
        }
        else
        {
            s.bowRudderDeploy = 3;
        }
        //检查左右艏舵
        if(pm->m_subDisSysState.bowRudderServo1 == 1)
        {
            s.bowRudderServo1 = 1;
        }
        else
        {
            s.bowRudderServo1 = 3;
        }

        if(pm->m_subDisSysState.bowRudderServo2 == 1)
        {
            s.bowRudderServo2 = 1;
        }
        else
        {
            s.bowRudderServo2 = 3;
        }
    }
    else
    {
        if(pm->m_subDisSysState.bowRudderDeploy == 1)
        {
            s.bowRudderDeploy = 2;
        }
        else
        {
            s.bowRudderDeploy = 0;
        }

        if(pm->m_subDisSysState.bowRudderServo1 == 1)
        {
            s.bowRudderServo1 = 2;
        }
        else
        {
            s.bowRudderServo1 = 0;
        }

        if(pm->m_subDisSysState.bowRudderServo2 == 1)
        {
            s.bowRudderServo2 = 2;
        }
        else
        {
            s.bowRudderServo2 = 0;
        }

    }
}
void PowerManagerFsm::getActuatorStatus(DriverTable &s)
{
    if(pm->m_systemData->power_a_bus_status.actuatorCircuitBreaker == 1)
    {
        if(pm->m_subDisSysState.bowCover1 == 1)
        {
            s.bowCover1 = 1;
        }
        else
        {
            s.bowCover1 = 3;
        }

        if(pm->m_subDisSysState.bowCover2 == 1)
        {
            s.bowCover2 = 1;
        }
        else
        {
            s.bowCover2 = 3;
        }

        if(pm->m_subDisSysState.bowCover3 == 1)
        {
            s.bowCover3 = 1;
        }
        else
        {
            s.bowCover3 = 3;
        }

        if(pm->m_subDisSysState.bowCover4 == 1)
        {
            s.bowCover4 = 1;
        }
        else
        {
            s.bowCover4 = 3;
        }

        if(pm->m_subDisSysState.bowCover5 == 1)
        {
            s.bowCover5 = 1;
        }
        else
        {
            s.bowCover5 = 3;
        }

        if(pm->m_subDisSysState.bowCover6 == 1)
        {
            s.bowCover6 = 1;
        }
        else
        {
            s.bowCover6 = 3;
        }
    }
    else
    {
        if(pm->m_subDisSysState.bowCover1 == 1)
        {
            s.bowCover1 = 2;
        }
        else
        {
            s.bowCover1 = 0;
        }

        if(pm->m_subDisSysState.bowCover2 == 1)
        {
            s.bowCover2 = 2;
        }
        else
        {
            s.bowCover2 = 0;
        }

        if(pm->m_subDisSysState.bowCover3 == 1)
        {
            s.bowCover3 = 2;
        }
        else
        {
            s.bowCover3 = 0;
        }

        if(pm->m_subDisSysState.bowCover4 == 1)
        {
            s.bowCover4 = 2;
        }
        else
        {
            s.bowCover4 = 0;
        }

        if(pm->m_subDisSysState.bowCover5 == 1)
        {
            s.bowCover5 = 2;
        }
        else
        {
            s.bowCover5 = 0;
        }

        if(pm->m_subDisSysState.bowCover6 == 1)
        {
            s.bowCover6 = 2;
        }
        else
        {
            s.bowCover6 = 0;
        }
    }
}

void PowerManagerFsm::getSternRudderStatus(DriverTable &s)
{
    //艉舵1 2
    if(pm->m_systemData->power_bus_status.sternRudderSwitch1CircuitBreaker == 1)
    {
        if(pm->m_subDisSysState.sternRudderServo1== 1)
        {
            s.sternRudderServo1 = 1;
        }
        else
        {
            s.sternRudderServo1 = 3;
        }

        if(pm->m_subDisSysState.sternRudderServo2 == 1)
        {
            s.sternRudderServo2 = 1;
        }
        else
        {
            s.sternRudderServo2 = 3;
        }
    }
    else
    {
        if(pm->m_subDisSysState.sternRudderServo1== 1)
        {
            s.sternRudderServo1 = 2;
        }
        else
        {
            s.sternRudderServo1 = 0;
        }

        if(pm->m_subDisSysState.sternRudderServo2 == 1)
        {
            s.sternRudderServo2 = 2;
        }
        else
        {
            s.sternRudderServo2 = 0;
        }
    }
    //艉舵3 4
    if(pm->m_systemData->power_bus_status.sternRudderSwitch2CircuitBreaker == 1)
    {
        if(pm->m_subDisSysState.sternRudderServo3 == 1)
        {
            s.sternRudderServo3 = 1;
        }
        else
        {
            s.sternRudderServo3 = 3;
        }

        if(pm->m_subDisSysState.sternRudderServo4 == 1)
        {
            s.sternRudderServo4 = 1;
        }
        else
        {
            s.sternRudderServo4 = 3;
        }
    }
    else
    {
        if(pm->m_subDisSysState.sternRudderServo3 == 1)
        {
            s.sternRudderServo3 = 2;
        }
        else
        {
            s.sternRudderServo3 = 0;
        }

        if(pm->m_subDisSysState.sternRudderServo4 == 1)
        {
            s.sternRudderServo4 = 2;
        }
        else
        {
            s.sternRudderServo4 = 0;
        }
    }
}

void PowerManagerFsm::getPropulsionMotorStatus(DriverTable &s)
{
    if(pm->m_systemData->power_bus_status.propulsionMotorCircuitBreaker == 1)
    {
        if(pm->m_subDisSysState.thruster == 1)
        {
            s.thruster = 1;
        }
        else
        {
            s.thruster = 3;
        }
    }
    else
    {
        if(pm->m_subDisSysState.thruster == 1)
        {
            s.thruster = 2;
        }
        else
        {
            s.thruster = 0;
        }
    }
}
void PowerManagerFsm::getBuoyancyAdjustmentStatus(DriverTable &s)
{
    // 艉部浮调
    if(pm->m_systemData->power_bus_status.sternFTDevice45CircuitBreaker == 1)
    {
        if(pm->m_subDisSysState.buoyancyAdjust4 == 1)
        {
            s.buoyancyAdjust4 = 1;
        }
        else
        {
            s.buoyancyAdjust4 = 3;
        }

        if(pm->m_subDisSysState.buoyancyAdjust5 == 1)
        {
            s.buoyancyAdjust5 = 1;
        }
        else
        {
            s.buoyancyAdjust5 = 3;
        }
    }
    else
    {
        if(pm->m_subDisSysState.buoyancyAdjust4 == 1)
        {
            s.buoyancyAdjust4 = 2;
        }
        else
        {
            s.buoyancyAdjust4 = 0;
        }

        if(pm->m_subDisSysState.buoyancyAdjust5 == 1)
        {
            s.buoyancyAdjust5 = 2;
        }
        else
        {
            s.buoyancyAdjust5 = 0;
        }
    }

    // 艏部浮调
    if(pm->m_systemData->power_a_bus_status.bowFTDevice123CircuitBreaker == 1)
    {
        if(pm->m_subDisSysState.buoyancyAdjust1 == 1)
        {
            s.buoyancyAdjust1 = 1;
        }
        else
        {
            s.buoyancyAdjust1 = 3;
        }

        if(pm->m_subDisSysState.buoyancyAdjust2 == 1)
        {
            s.buoyancyAdjust2 = 1;
        }
        else
        {
            s.buoyancyAdjust2 = 3;
        }

        if(pm->m_subDisSysState.buoyancyAdjust3 == 1)
        {
            s.buoyancyAdjust3 = 1;
        }
        else
        {
            s.buoyancyAdjust3 = 3;
        }
    }
    else
    {
        if(pm->m_subDisSysState.buoyancyAdjust1 == 1)
        {
            s.buoyancyAdjust1 = 2;
        }
        else
        {
            s.buoyancyAdjust1 = 0;
        }

        if(pm->m_subDisSysState.buoyancyAdjust2 == 1)
        {
            s.buoyancyAdjust2 = 2;
        }
        else
        {
            s.buoyancyAdjust2 = 0;
        }

        if(pm->m_subDisSysState.buoyancyAdjust3 == 1)
        {
            s.buoyancyAdjust3 = 2;
        }
        else
        {
            s.buoyancyAdjust3 = 0;
        }
    }
}

void PowerManagerFsm::getSideSonarStatus(DriverTable &s)
{
    if(pm->m_systemData->power_a_bus_status.xczCircuitBreaker == 1)
    {
        if(pm->m_subDisSysState.sideSonar == 1)
        {
            s.sideSonar = 1;
        }
        else
        {
            s.sideSonar = 3;
        }
    }
    else
    {
        if(pm->m_subDisSysState.sideSonar == 1)
        {
            s.sideSonar = 2;
        }
        else
        {
            s.sideSonar = 0;
        }
    }
}

void PowerManagerFsm::getFBSystemCmdStatus(DriverTable &s)
{
    if(pm->m_systemData->power_bus_status.fbReservedCircuitBreaker == 1)
    {
        if(pm->m_subDisSysState.buoyage == 1)
        {
            s.buoyage == 1;
        }
        else
        {
            s.buoyage == 3;
        }
    }
    else
    {
        if(pm->m_subDisSysState.buoyage == 1)
        {
            s.buoyage == 2;
        }
        else
        {
            s.buoyage == 0;
        }
    }
}

void PowerManagerFsm::getMastFoldingSystem(DriverTable &s)
{
    if(pm->m_systemData->power_a_bus_status.mastSteeringGearControlBoxCircuitBreaker == 1)
    {
        if(pm->m_subDisSysState.mastLiftingServo == 1)
        {
            s.mastLiftingServo = 1;
        }
        else
        {
            s.mastLiftingServo = 3;
        }
    }
    else
    {
        if(pm->m_subDisSysState.mastLiftingServo == 1)
        {
            s.mastLiftingServo = 2;
        }
        else
        {
            s.mastLiftingServo = 0;
        }
    }
}

void PowerManagerFsm::getOpenWaterStatus(DriverTable &s)
{
    if(pm->m_systemData->power_a_bus_status.openWaterCoverStartCylinderCircuitBreaker == 1)
    {
        if(pm->m_subDisSysState.bowWaterInet1 == 1)
        {
            s.bowWaterInet1 = 1;
        }
        else
        {
            s.bowWaterInet1 = 3;
        }

        if(pm->m_subDisSysState.bowWaterInet2 == 1)
        {
            s.bowWaterInet2 = 1;
        }
        else
        {
            s.bowWaterInet2 = 3;
        }

        if(pm->m_subDisSysState.bowWaterInet3 == 1)
        {
            s.bowWaterInet3 = 1;
        }
        else
        {
            s.bowWaterInet3 = 3;
        }

        if(pm->m_subDisSysState.bowWaterInet4 == 1)
        {
            s.bowWaterInet4 = 1;
        }
        else
        {
            s.bowWaterInet4 = 3;
        }

        if(pm->m_subDisSysState.bowWaterInet5 == 1)
        {
            s.bowWaterInet5 = 1;
        }
        else
        {
            s.bowWaterInet5 = 3;
        }

        if(pm->m_subDisSysState.bowWaterInet6 == 1)
        {
            s.bowWaterInet6 = 1;
        }
        else
        {
            s.bowWaterInet6 = 3;
        }

        if(pm->m_subDisSysState.bowWaterInet7 == 1)
        {
            s.bowWaterInet7 = 1;
        }
        else
        {
            s.bowWaterInet7 = 3;
        }

        if(pm->m_subDisSysState.bowWaterInet8 == 1)
        {
            s.bowWaterInet8 = 1;
        }
        else
        {
            s.bowWaterInet8 = 3;
        }
    }
    else
    {
        if(pm->m_subDisSysState.bowWaterInet1 == 1)
        {
            s.bowWaterInet1 = 2;
        }
        else
        {
            s.bowWaterInet1 = 0;
        }

        if(pm->m_subDisSysState.bowWaterInet2 == 1)
        {
            s.bowWaterInet2 = 2;
        }
        else
        {
            s.bowWaterInet2 = 0;
        }

        if(pm->m_subDisSysState.bowWaterInet3 == 1)
        {
            s.bowWaterInet3 = 2;
        }
        else
        {
            s.bowWaterInet3 = 0;
        }

        if(pm->m_subDisSysState.bowWaterInet4 == 1)
        {
            s.bowWaterInet4 = 2;
        }
        else
        {
            s.bowWaterInet4 = 0;
        }

        if(pm->m_subDisSysState.bowWaterInet5 == 1)
        {
            s.bowWaterInet5 = 2;
        }
        else
        {
            s.bowWaterInet5 = 0;
        }

        if(pm->m_subDisSysState.bowWaterInet6 == 1)
        {
            s.bowWaterInet6 = 2;
        }
        else
        {
            s.bowWaterInet6 = 0;
        }

        if(pm->m_subDisSysState.bowWaterInet7 == 1)
        {
            s.bowWaterInet7 = 2;
        }
        else
        {
            s.bowWaterInet7 = 0;
        }

        if(pm->m_subDisSysState.bowWaterInet8 == 1)
        {
            s.bowWaterInet8 = 2;
        }
        else
        {
            s.bowWaterInet8 = 0;
        }
    }

}
void PowerManagerFsm::basicNavigationFault()
{
    //深度计故障处理
    int count = (pm->m_subDisSysFault.depth1 == 1) + (pm->m_subDisSysFault.depth2 == 1) + (pm->m_subDisSysFault.depth3 == 1);
    if (count  == 3) 
    {
        // LOG_F(INFO, "深度失效");
        pm->m_systemData->insertFaultCode(0x1302);
        pm->m_systemData->insertFaultCode(0x1303);
        pm->m_systemData->insertFaultCode(0x1304);
    } else
    {
        pm->m_systemData->eraseFaultCode(0x1302);
        pm->m_systemData->eraseFaultCode(0x1303);
        pm->m_systemData->eraseFaultCode(0x1304);
    }
    if((count > 0) && (count <3))
    {
        // LOG_F(INFO, "部分深度计失效");
        if(pm->m_subDisSysFault.depth1 == 1)
            pm->m_systemData->insertFaultCode(0x1102);
        else
            pm->m_systemData->eraseFaultCode(0x1102);
        
        if (pm->m_subDisSysFault.depth2 == 1)
            pm->m_systemData->insertFaultCode(0x1103);
        else
            pm->m_systemData->eraseFaultCode(0x1103);

        if (pm->m_subDisSysFault.depth3 == 1)
            pm->m_systemData->insertFaultCode(0x1104);
        else
            pm->m_systemData->eraseFaultCode(0x1104);
    }
    if(count == 0)
    {
        // LOG_F(INFO, "深度恢复");
        pm->m_systemData->eraseFaultCode(0x1302);
        pm->m_systemData->eraseFaultCode(0x1303);
        pm->m_systemData->eraseFaultCode(0x1304);
        pm->m_systemData->eraseFaultCode(0x1102);
        pm->m_systemData->eraseFaultCode(0x1103);
        pm->m_systemData->eraseFaultCode(0x1104);
    }

    // DVL故障处理
    if(pm->m_subDisSysFault.DVL == 1)
    {
        // LOG_F(INFO, "DVL失效");
        pm->m_systemData->insertFaultCode(0x1301);
    } 
    else
    {
        // LOG_F(INFO, "DVL恢复");
        pm->m_systemData->eraseFaultCode(0x1301);
    }
    
    //========================处理艉舵故障问题==================================================
    count = (pm->m_subDisSysFault.sternRudderServo1 == 1) 
    + (pm->m_subDisSysFault.sternRudderServo2 == 1) 
    + (pm->m_subDisSysFault.sternRudderServo3 == 1) 
    + (pm->m_subDisSysFault.sternRudderServo4 == 1);
    if(count == 1)
    {
        // LOG_F(INFO, "单个舵机失效");
        if(pm->m_subDisSysFault.sternRudderServo1 == 1)
            pm->m_systemData->insertFaultCode(0x1206);

        if(pm->m_subDisSysFault.sternRudderServo2 == 1)
            pm->m_systemData->insertFaultCode(0x1207);
        
        if(pm->m_subDisSysFault.sternRudderServo3 == 1)
            pm->m_systemData->insertFaultCode(0x1208);

        if(pm->m_subDisSysFault.sternRudderServo4 == 1)
            pm->m_systemData->insertFaultCode(0x1209);
    }
    if(count >= 2)
    {
        // LOG_F(INFO, "艉舵两个及以上失效");
        if(pm->m_subDisSysFault.sternRudderServo1 == 1)
            pm->m_systemData->insertFaultCode(0x1306);
        if(pm->m_subDisSysFault.sternRudderServo2 == 1)
            pm->m_systemData->insertFaultCode(0x1307);
        if(pm->m_subDisSysFault.sternRudderServo3 == 1)
            pm->m_systemData->insertFaultCode(0x1308);
        if(pm->m_subDisSysFault.sternRudderServo4 == 1)
            pm->m_systemData->insertFaultCode(0x1309);
    }
    if (pm->m_systemData->disSysFaultCode.find(7) != pm->m_systemData->disSysFaultCode.end())
    {
        pm->m_systemData->insertFaultCode(0x1306);
        pm->m_systemData->insertFaultCode(0x1307);
    }
    if (pm->m_systemData->disSysFaultCode.find(8) != pm->m_systemData->disSysFaultCode.end())
    {
        pm->m_systemData->insertFaultCode(0x1308);
        pm->m_systemData->insertFaultCode(0x1309);
    }
    if(count == 0)
    {
        // 清空故障码
        if(pm->m_systemData->disSysFaultCode.find(7) == pm->m_systemData->disSysFaultCode.end())
        {
            pm->m_systemData->eraseFaultCode(0x1306);
            pm->m_systemData->eraseFaultCode(0x1307);
        }
        if(pm->m_systemData->disSysFaultCode.find(8) == pm->m_systemData->disSysFaultCode.end())
        {
            pm->m_systemData->eraseFaultCode(0x1308);
            pm->m_systemData->eraseFaultCode(0x1309);
        }
    }
    //=====处理动力电池故障======================
    if(pm->m_systemData->dyn_emergency_status >= 1 )
    {
        pm->m_systemData->insertFaultCode(0x130B);
    }
    //======处理推进电机故障=========================
    if(pm->m_subDisSysFault.thruster == 1)
    {
        pm->m_systemData->insertFaultCode(0x130A);
    }
    else
    {
        if(pm->m_systemData->disSysFaultCode.find(3) == pm->m_systemData->disSysFaultCode.end())
        {
            pm->m_systemData->eraseFaultCode(0x130A);
        }
        else
        {
            pm->m_systemData->insertFaultCode(0x130A);
        }
    }
    //======处理惯导故障=========================
    if(pm->m_subDisSysFault.ins == 1)
    {
        pm->m_systemData->insertFaultCode(0x1305);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x1305);
    }
    //=====处理仪表锂电池失效=================
    if(pm->m_systemData->ins_emergency_status > 1)
    {
        pm->m_systemData->insertFaultCode(0x1412);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x1412);
    }
    //====处理复合管控器失效
    if(pm->m_systemData->device_common_status.ccu_status.isTimeout)
    {
        pm->m_systemData->insertFaultCode(0x1413);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x1413);
    }
}
void PowerManagerFsm::basicNavigationFault(FaultEvent const &e)
{
   LOG_F(INFO, "basicNavigationFault");
   if((e.injectionFaultCode==1))
   {
        LOG_F(INFO, "部分深度计失效");
        pm->faultCode.push_back(0x1102);
        pm->faultCode.push_back(0x1103);
        pm->faultCode.push_back(0x1104);
   }
   if((e.injectionFaultCode==2))
   {
        LOG_F(INFO, "DVL失效");
        pm->faultCode.push_back(0x1301);
   }
   if ((e.injectionFaultCode==3))
   {
        LOG_F(INFO, "单个艉舵失效");
        pm->faultCode.push_back(0x1207);
   }
   if ((e.injectionFaultCode==4))
   {
        LOG_F(INFO, "部分艉舵失效");
        pm->faultCode.push_back(0x1308);
        pm->faultCode.push_back(0x1307);
   }
   if ((e.injectionFaultCode==5))
   {
        LOG_F(INFO, "动力锂电池失效");
        pm->faultCode.push_back(0x130B);
   }
   if ((e.injectionFaultCode==6))
   {
        LOG_F(INFO, "推进电机失效");
        pm->faultCode.push_back(0x130A);
   }
   if ((e.injectionFaultCode==7))
   {
        LOG_F(INFO, "惯导失效");
        pm->faultCode.push_back(0x1305);
   }
   if ((e.injectionFaultCode==8))
   {
        LOG_F(INFO, "深度失效");
        pm->faultCode.push_back(0x1302);
        pm->faultCode.push_back(0x1303);
        pm->faultCode.push_back(0x1304);
   }
   if ((e.injectionFaultCode==9))
   {
        LOG_F(INFO, "仪表锂电池失效");
        pm->faultCode.push_back(0x1412);
   }
   if ((e.injectionFaultCode==10))
   {
        LOG_F(INFO, "复合管控器失效");
        pm->faultCode.push_back(0x1413);
   }
}
void PowerManagerFsm::extendedNavigationFault()
{
    //处理艏舵故障
    if(pm->m_systemData->disSysFaultCode.find(17) != pm->m_systemData->disSysFaultCode.end())
    {
        pm->m_systemData->insertFaultCode(0x2311);
        pm->m_systemData->insertFaultCode(0x2312);
    }
    else
    {
        if(pm->m_subDisSysFault.bowRudderDeploy == 1)
        {
            pm->m_systemData->insertFaultCode(0x2311);
        }
        else
        {
            pm->m_systemData->eraseFaultCode(0x2311);
        }

        if(pm->m_subDisSysFault.bowRudderServo1 == 1)
        {
            pm->m_systemData->insertFaultCode(0x2312);
        }
        else
        {
            pm->m_systemData->eraseFaultCode(0x2312);
        }
    }

    //===处理动力锂电池功率不满
    if(pm->m_systemData->ins_max_discharge_power < 500)
    {
        pm->m_systemData->insertFaultCode(0x2323);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x2323);
    }

    //====处理燃料电池失效========
    if(pm->m_systemData->fc_fault_level > 0)
    {
        pm->m_systemData->insertFaultCode(0x2334);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x2334);
    }
}
void PowerManagerFsm::extendedNavigationFault(FaultEvent const &e)
{
   LOG_F(INFO, "extendedNavigationFault");
   if ((e.injectionFaultCode==1))
   {
        LOG_F(INFO, "艏舵展开电缸失效");
        pm->faultCode.push_back(0x2311);
   }
   if ((e.injectionFaultCode==2))
   {
        LOG_F(INFO, "艏舵控制控制失效");
        pm->faultCode.push_back(0x2312);
   }
   if ((e.injectionFaultCode==3))
   {
        LOG_F(INFO, "动力锂电池功率不满/失效");
        pm->faultCode.push_back(0x2323);
   }
   if ((e.injectionFaultCode==4))
   {
        LOG_F(INFO, "燃料电池失效");
        pm->faultCode.push_back(0x2334);
   }
}
void PowerManagerFsm::obstacleAvoidanceFault()
{
    //处理单波束故障
    if(pm->m_subDisSysFault.singleBeamSonar5 == 1)
    {
        pm->m_systemData->insertFaultCode(0x3108);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x3108);
    }

    if(pm->m_subDisSysFault.singleBeamSonar6 == 1)
    {
        pm->m_systemData->insertFaultCode(0x3109);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x3109);
    }

    if(pm->m_subDisSysFault.forwardSonar == 1)
    {
        pm->m_systemData->insertFaultCode(0x3201);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x3201);
    }

    if(pm->m_subDisSysFault.singleBeamSonar1 == 1)
    {
        pm->m_systemData->insertFaultCode(0x3202);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x3202);
    }

    if(pm->m_subDisSysFault.singleBeamSonar2 == 1)
    {
        pm->m_systemData->insertFaultCode(0x3203);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x3203);
    }

    if (pm->m_subDisSysFault.singleBeamSonar3 == 1)
    {
        pm->m_systemData->insertFaultCode(0x3204);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x3204);
    }

    if (pm->m_subDisSysFault.singleBeamSonar4 == 1)
    {
        pm->m_systemData->insertFaultCode(0x3205);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x3205);
    }

    if (pm->m_subDisSysFault.singleBeamSonar5 == 1)
    {
        pm->m_systemData->insertFaultCode(0x3206);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x3206);
    }

    if (pm->m_subDisSysFault.singleBeamSonar7 == 1)
    {
        pm->m_systemData->insertFaultCode(0x3207);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x3207);
    }

    if( (pm->m_subDisSysFault.singleBeamSonar1 == 1) 
        && (pm->m_subDisSysFault.singleBeamSonar2 == 1)
        && (pm->m_subDisSysFault.singleBeamSonar3 == 1)
        && (pm->m_subDisSysFault.singleBeamSonar4 == 1)
        && (pm->m_subDisSysFault.forwardSonar == 1)
        )
    {
        pm->m_systemData->insertFaultCode(0x3301);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x3301);
    }
    
}
void PowerManagerFsm::obstacleAvoidanceFault(FaultEvent const &e)
{
    LOG_F(INFO, "obstacleAvoidanceFault");
    if ((e.injectionFaultCode==1))
    {
        pm->faultCode.push_back(0x3108);
        pm->faultCode.push_back(0x3109);
    }
    if ((e.injectionFaultCode==2))
    {
        pm->faultCode.push_back(0x3201);
        pm->faultCode.push_back(0x3202);
        pm->faultCode.push_back(0x3203);
        pm->faultCode.push_back(0x3204);
        pm->faultCode.push_back(0x3205);
        pm->faultCode.push_back(0x3206);
        pm->faultCode.push_back(0x3207);
    }
    if ((e.injectionFaultCode==3))
    {
        pm->faultCode.push_back(0x3301);
    }
    
}

void PowerManagerFsm::buoyancyControlFault()
{
    int count = (pm->m_subDisSysFault.buoyancyAdjust1 == 1) 
    + (pm->m_subDisSysFault.buoyancyAdjust2 == 1) 
    + (pm->m_subDisSysFault.buoyancyAdjust3 == 1)
    + (pm->m_subDisSysFault.buoyancyAdjust4 == 1)
    + (pm->m_subDisSysFault.buoyancyAdjust5 == 1);

    bool breaker7_fault = pm->m_systemData->disSysFaultCode.find(7) != pm->m_systemData->disSysFaultCode.end();
    bool breaker13_fault = pm->m_systemData->disSysFaultCode.find(13) != pm->m_systemData->disSysFaultCode.end();
    //===========艏部浮调====================
    if(breaker7_fault)
    {
       pm->m_systemData->insertFaultCode(0x4304);
       pm->m_systemData->insertFaultCode(0x4305);
    }
    else
    {
        if(count == 1)
        {
            if(pm->m_subDisSysFault.buoyancyAdjust4 == 1)
            {
                pm->m_systemData->insertFaultCode(0x4204);
            }
            else
            {
                pm->m_systemData->eraseFaultCode(0x4204);
            }

            if(pm->m_subDisSysFault.buoyancyAdjust5 == 1)
            {
                pm->m_systemData->insertFaultCode(0x4205);
            }
            else
            {
                pm->m_systemData->eraseFaultCode(0x4205);
            }
        }

        if((count >= 2) && (count <5))
        {
            if(pm->m_subDisSysFault.buoyancyAdjust4 == 1)
            {
                pm->m_systemData->insertFaultCode(0x4304);
            }
            else
            {
                pm->m_systemData->eraseFaultCode(0x4304);
            }

            if(pm->m_subDisSysFault.buoyancyAdjust5 == 1)
            {
                pm->m_systemData->insertFaultCode(0x4305);
            }
            else
            {
                pm->m_systemData->eraseFaultCode(0x4305);
            }
        }
    }
    if(breaker13_fault)
       {
            pm->m_systemData->insertFaultCode(0x4301);
            pm->m_systemData->insertFaultCode(0x4302);
            pm->m_systemData->insertFaultCode(0x4303);
       }
       else
       {
            if(count == 1)
            {
                if(pm->m_subDisSysFault.buoyancyAdjust1 == 1)
                {
                    pm->m_systemData->insertFaultCode(0x4201);
                }
                else
                {
                    pm->m_systemData->eraseFaultCode(0x4201);
                }

                if(pm->m_subDisSysFault.buoyancyAdjust2 == 1)
                {
                    pm->m_systemData->insertFaultCode(0x4202);
                }
                else
                {
                    pm->m_systemData->eraseFaultCode(0x4202);
                }

                if(pm->m_subDisSysFault.buoyancyAdjust3 == 1)
                {
                    pm->m_systemData->insertFaultCode(0x4203);
                }
                else
                {
                    pm->m_systemData->eraseFaultCode(0x4203);
                }
            }

            if((count >= 2) && (count <5))
            {
                if(pm->m_subDisSysFault.buoyancyAdjust1 == 1)
                {
                    pm->m_systemData->insertFaultCode(0x4301);
                }
                else
                {
                    pm->m_systemData->eraseFaultCode(0x4301);
                }

                if(pm->m_subDisSysFault.buoyancyAdjust2 == 1)
                {
                    pm->m_systemData->insertFaultCode(0x4302);
                }
                else
                {
                    pm->m_systemData->eraseFaultCode(0x4302);
                }

                if(pm->m_subDisSysFault.buoyancyAdjust3 == 1)
                {
                    pm->m_systemData->insertFaultCode(0x4303);
                }
                else
                {
                    pm->m_systemData->eraseFaultCode(0x4303);
                }
            }
       }

    if(breaker7_fault && breaker13_fault && count == 5)
    {
        //清除其他故障码
        
        pm->m_systemData->eraseFaultCode(0x4301);
        pm->m_systemData->eraseFaultCode(0x4302);
        pm->m_systemData->eraseFaultCode(0x4303);
        pm->m_systemData->eraseFaultCode(0x4304);
        pm->m_systemData->eraseFaultCode(0x4305);

        pm->m_systemData->eraseFaultCode(0x4201);
        pm->m_systemData->eraseFaultCode(0x4202);
        pm->m_systemData->eraseFaultCode(0x4203);
        pm->m_systemData->eraseFaultCode(0x4204);
        pm->m_systemData->eraseFaultCode(0x4205);
        
        pm->m_systemData->insertFaultCode(0x4401);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x4401);
    }

    if(count == 0 && (!breaker7_fault) && (!breaker13_fault)) //清除所有故障码
    {
        pm->m_systemData->eraseFaultCode(0x4301);
        pm->m_systemData->eraseFaultCode(0x4302);
        pm->m_systemData->eraseFaultCode(0x4303);
        pm->m_systemData->eraseFaultCode(0x4304);
        pm->m_systemData->eraseFaultCode(0x4305);

        pm->m_systemData->eraseFaultCode(0x4201);
        pm->m_systemData->eraseFaultCode(0x4202);
        pm->m_systemData->eraseFaultCode(0x4203);
        pm->m_systemData->eraseFaultCode(0x4204);
        pm->m_systemData->eraseFaultCode(0x4205);
        pm->m_systemData->eraseFaultCode(0x4401);
    }
}

void PowerManagerFsm::buoyancyControlFault(FaultEvent const &e)
{
    LOG_F(INFO, "buoyancyControlFault");
    if ((e.injectionFaultCode==1))
    {
        LOG_F(INFO, "单一浮调机构失效");
        pm->faultCode.push_back(0x4201);
    }
    if ((e.injectionFaultCode==2))
    {
        LOG_F(INFO, "2-4及以上浮调装置失效");
        pm->faultCode.push_back(0x4301);
        pm->faultCode.push_back(0x4302);
        pm->faultCode.push_back(0x4303);
    }
    if ((e.injectionFaultCode==3))
    {
        LOG_F(INFO, "功率不足满足浮调要求");
        pm->faultCode.push_back(0x4436);
    }
    if ((e.injectionFaultCode==4))
    {
        LOG_F(INFO, "所有浮调系统失效");
        pm->faultCode.push_back(0x4401);
    }
}

void PowerManagerFsm::communicationSystemFault()
{
    int count = (pm->m_subDisSysFault.lowFreqSonar == 1) 
    + (pm->m_subDisSysFault.USBL == 1) 
    + (pm->m_subDisSysFault.buoyage == 1);

    if(count < 3)
    {
        if(pm->m_subDisSysFault.lowFreqSonar == 1)
            pm->m_systemData->insertFaultCode(0x5203);
        else
            pm->m_systemData->eraseFaultCode(0x5203);

        if(pm->m_subDisSysFault.USBL == 1)
            pm->m_systemData->insertFaultCode(0x5204);
        else
            pm->m_systemData->eraseFaultCode(0x5204);

        if(pm->m_subDisSysFault.buoyage == 1)
            pm->m_systemData->insertFaultCode(0x5205);
        else
            pm->m_systemData->eraseFaultCode(0x5205);
    }

    if(count == 3)
    {
        pm->m_systemData->insertFaultCode(0x5303);
        pm->m_systemData->insertFaultCode(0x5304);
        pm->m_systemData->insertFaultCode(0x5305);
    }
    else
    {
        pm->m_systemData->eraseFaultCode(0x5303);
        pm->m_systemData->eraseFaultCode(0x5304);
        pm->m_systemData->eraseFaultCode(0x5305);
    }

    if(pm->m_subDisSysFault.commMast == 1)
        pm->m_systemData->insertFaultCode(0x5301);
    else
        pm->m_systemData->eraseFaultCode(0x5301);

    if(pm->m_subDisSysFault.mastLiftingServo == 1 || (pm->m_systemData->disSysFaultCode.find(15) != pm->m_systemData->disSysFaultCode.end()))
        pm->m_systemData->insertFaultCode(0x5302);
    else
        pm->m_systemData->eraseFaultCode(0x5302);
}

void PowerManagerFsm::communicationSystemFault(FaultEvent const &e)
{
    LOG_F(INFO, "communicationSystemFault");
    if ((e.injectionFaultCode==1))
    {
        LOG_F(INFO, "部分水下通信失效");
        pm->faultCode.push_back(0x5203);
        pm->faultCode.push_back(0x5204);
        pm->faultCode.push_back(0x5205);
    }
    if ((e.injectionFaultCode==2))
    {
        LOG_F(INFO, "水下通信失效");
        pm->faultCode.push_back(0x5303);
        pm->faultCode.push_back(0x5304);
        pm->faultCode.push_back(0x5305);
    }
    if ((e.injectionFaultCode==3))
    {
        LOG_F(INFO, "水面通信失效");
        pm->faultCode.push_back(0x5301);
        pm->faultCode.push_back(0x5302);
    }
}
void PowerManagerFsm::payloadSystemFault()
{
    int count = (pm->m_subDisSysFault.bowCover1 == 1) 
    + (pm->m_subDisSysFault.bowCover2 == 1) 
    + (pm->m_subDisSysFault.bowCover3 == 1)
    + (pm->m_subDisSysFault.bowCover4 == 1)
    + (pm->m_subDisSysFault.bowCover5 == 1)
    + (pm->m_subDisSysFault.bowCover6 == 1)
    + (pm->m_subDisSysFault.bowWaterInet1 == 1)
    + (pm->m_subDisSysFault.bowWaterInet2 == 1)
    + (pm->m_subDisSysFault.bowWaterInet3 == 1)
    + (pm->m_subDisSysFault.bowWaterInet4 == 1)
    + (pm->m_subDisSysFault.bowWaterInet5 == 1)
    + (pm->m_subDisSysFault.bowWaterInet6 == 1)
    + (pm->m_subDisSysFault.bowWaterInet7 == 1)
    + (pm->m_subDisSysFault.bowWaterInet8 == 1);

    if((pm->m_systemData->disSysFaultCode.find(14) == pm->m_systemData->disSysFaultCode.end())
      && (pm->m_systemData->disSysFaultCode.find(18) == pm->m_systemData->disSysFaultCode.end()))
    {
        if(count > 0 && count < 14)
        {
            if(pm->m_subDisSysFault.bowCover1 == 1) pm->m_systemData->insertFaultCode(0x6211); else pm->m_systemData->eraseFaultCode(0x6211);
            if(pm->m_subDisSysFault.bowCover2 == 1) pm->m_systemData->insertFaultCode(0x6212); else pm->m_systemData->eraseFaultCode(0x6212);
            if(pm->m_subDisSysFault.bowCover3 == 1) pm->m_systemData->insertFaultCode(0x6213); else pm->m_systemData->eraseFaultCode(0x6213);
            if(pm->m_subDisSysFault.bowCover4 == 1) pm->m_systemData->insertFaultCode(0x6214); else pm->m_systemData->eraseFaultCode(0x6214);
            if(pm->m_subDisSysFault.bowCover5 == 1) pm->m_systemData->insertFaultCode(0x6215); else pm->m_systemData->eraseFaultCode(0x6215);
            if(pm->m_subDisSysFault.bowCover6 == 1) pm->m_systemData->insertFaultCode(0x6216); else pm->m_systemData->eraseFaultCode(0x6216);
            if(pm->m_subDisSysFault.bowWaterInet1 == 1) pm->m_systemData->insertFaultCode(0x6217); else pm->m_systemData->eraseFaultCode(0x6217);
            if(pm->m_subDisSysFault.bowWaterInet2 == 1) pm->m_systemData->insertFaultCode(0x6218); else pm->m_systemData->eraseFaultCode(0x6218);
            if(pm->m_subDisSysFault.bowWaterInet3 == 1) pm->m_systemData->insertFaultCode(0x6219); else pm->m_systemData->eraseFaultCode(0x6219);
            if(pm->m_subDisSysFault.bowWaterInet4 == 1) pm->m_systemData->insertFaultCode(0x621A); else pm->m_systemData->eraseFaultCode(0x621A);
            if(pm->m_subDisSysFault.bowWaterInet5 == 1) pm->m_systemData->insertFaultCode(0x621B); else pm->m_systemData->eraseFaultCode(0x621B);
            if(pm->m_subDisSysFault.bowWaterInet6 == 1) pm->m_systemData->insertFaultCode(0x621C); else pm->m_systemData->eraseFaultCode(0x621C);
            if(pm->m_subDisSysFault.bowWaterInet7 == 1) pm->m_systemData->insertFaultCode(0x621D); else pm->m_systemData->eraseFaultCode(0x621D);
            if(pm->m_subDisSysFault.bowWaterInet8 == 1) pm->m_systemData->insertFaultCode(0x621E); else pm->m_systemData->eraseFaultCode(0x621E);
        }
    }
    if((pm->m_systemData->disSysFaultCode.find(14) != pm->m_systemData->disSysFaultCode.end())
        && (pm->m_systemData->disSysFaultCode.find(18) == pm->m_systemData->disSysFaultCode.end()))
    {
        pm->m_systemData->insertFaultCode(0x633F);
    }
    if(count >= 14 )
    {
        pm->m_systemData->insertFaultCode(0x633F);
    }
    if(pm->m_subDisSysFault.launchControler == 1)
    {
        pm->m_systemData->insertFaultCode(0x633F);
    }
    if(count == 0 && ((pm->m_systemData->disSysFaultCode.find(14) == pm->m_systemData->disSysFaultCode.end())
        && (pm->m_systemData->disSysFaultCode.find(18) == pm->m_systemData->disSysFaultCode.end())
        && (pm->m_subDisSysFault.launchControler == 0))
    )
    {
        pm->m_systemData->eraseFaultCode(0x633F);
    }
    //===========处理xc===============
    if(pm->m_subDisSysFault.sideSonar == 1)
        pm->m_systemData->insertFaultCode(0x6321);
    else
        pm->m_systemData->eraseFaultCode(0x6321);

    if(pm->m_systemData->disSysFaultCode.find(16) == pm->m_systemData->disSysFaultCode.end())
        pm->m_systemData->eraseFaultCode(0x6323);
    else
        pm->m_systemData->insertFaultCode(0x6323);

    // if(pm->m_subDisSysFault)
    if(pm->m_subDisSysFault.photoSensor == 1)
        pm->m_systemData->insertFaultCode(0x6332);
    else
        pm->m_systemData->eraseFaultCode(0x6332);

    if(pm->m_subDisSysFault.electSensor == 1)
        pm->m_systemData->insertFaultCode(0x6331);
    else
        pm->m_systemData->eraseFaultCode(0x6331);
}


void PowerManagerFsm::payloadSystemFault(FaultEvent const &e)
{
    LOG_F(INFO, "payloadSystemFault");
    if ((e.injectionFaultCode==1))
    {
        LOG_F(INFO, "存在启闭机构或者敞水电缸失效");
        pm->faultCode.push_back(0x6211);
        pm->faultCode.push_back(0x6212);
        pm->faultCode.push_back(0x6213);
        pm->faultCode.push_back(0x6214);
        pm->faultCode.push_back(0x6215);
        pm->faultCode.push_back(0x6216);
        pm->faultCode.push_back(0x6217);
        pm->faultCode.push_back(0x6218);
        pm->faultCode.push_back(0x6219);
        pm->faultCode.push_back(0x621A);
        pm->faultCode.push_back(0x621B);
        pm->faultCode.push_back(0x621C);
        pm->faultCode.push_back(0x621D);
        pm->faultCode.push_back(0x621E);
    }
    if ((e.injectionFaultCode==2))
    {
        LOG_F(INFO, "舷侧探测失效");
        pm->faultCode.push_back(0x6321);
    }
    if ((e.injectionFaultCode==3))
    {
        LOG_F(INFO, "舷侧主动探测失效");
        pm->faultCode.push_back(0x6323);
    }
    if ((e.injectionFaultCode==4))
    {
        LOG_F(INFO, "TYZ失效");
        pm->faultCode.push_back(0x6322);
    }
    if ((e.injectionFaultCode==5))
    {
        LOG_F(INFO, "光电探测失效");
        pm->faultCode.push_back(0x6332);
    }
    if ((e.injectionFaultCode==6))
    {
        LOG_F(INFO, "电磁探测失效");
        pm->faultCode.push_back(0x6331);
    }
    if ((e.injectionFaultCode==7))
    {
        LOG_F(INFO, "载荷失效或者全部载荷启闭机构均失效");
        pm->faultCode.push_back(0x633F);
    }
}
void PowerManagerFsm::emergencySystemFault()
{
    //==处理抛载故障
    if(pm->m_subDisSysFault.bowThrow == 1 || pm->m_subDisSysFault.sternThrow == 1)
    {
        if(pm->m_subDisSysFault.bowThrow == 1) 
            pm->m_systemData->insertFaultCode(0x7202);
        if(pm->m_subDisSysFault.sternThrow == 1) 
            pm->m_systemData->insertFaultCode(0x7203);
    } else if (pm->m_subDisSysFault.bowThrow == 1 && pm->m_subDisSysFault.sternThrow == 1)
    {
        pm->m_systemData->insertFaultCode(0x7302);
        pm->m_systemData->insertFaultCode(0x7303);
    } else 
    {
        pm->m_systemData->eraseFaultCode(0x7202);
        pm->m_systemData->eraseFaultCode(0x7203);
        pm->m_systemData->eraseFaultCode(0x7302);
        pm->m_systemData->eraseFaultCode(0x7303);
    }
    //==处理注水机构故障
    if(pm->m_subDisSysFault.waterInject == 1)
        pm->m_systemData->insertFaultCode(0x7301);
    else
        pm->m_systemData->eraseFaultCode(0x7301);

    if(pm->m_systemData->power_bus_status.emergencyPowerLossSignal || pm->m_systemData->instrument_main_bus_status.emergencyPowerFailureSignal
     || pm->m_systemData->disSysFaultCode.find(25) != pm->m_systemData->disSysFaultCode.end())
        pm->m_systemData->insertFaultCode(0x7306);
    else
        pm->m_systemData->eraseFaultCode(0x7306);

    if(pm->m_systemData->power_a_bus_status.emergencyPowerFailureSignal || pm->m_systemData->instrument_bus_status.emergencyPowerFailureSignal
     || pm->m_systemData->disSysFaultCode.find(33) != pm->m_systemData->disSysFaultCode.end())
        pm->m_systemData->insertFaultCode(0x7307);
    else
        pm->m_systemData->eraseFaultCode(0x7307);

}
void PowerManagerFsm::emergencySystemFault(FaultEvent const &e)
{
    LOG_F(INFO, "emergencySystemFault");
    if ((e.injectionFaultCode==1))
    {
        //备注：抛载具有自己的电池
        LOG_F(INFO, "部分抛载失效");
        pm->faultCode.push_back(0x7202);
        pm->faultCode.push_back(0x7203);
    }
    if ((e.injectionFaultCode==2))
    {
        LOG_F(INFO, "一个超深检测装置失效");
        pm->faultCode.push_back(0x7204);
        pm->faultCode.push_back(0x7205);
    }
    if ((e.injectionFaultCode==3))
    {
        LOG_F(INFO, "注水机构失效");
        pm->faultCode.push_back(0x7301);
    }
    if ((e.injectionFaultCode==4))
    {
        LOG_F(INFO, "抛载失效");
        pm->faultCode.push_back(0x7302);
        pm->faultCode.push_back(0x7303);
    }
    if ((e.injectionFaultCode==5))
    {
        LOG_F(INFO, "艏部应急电源失效");
        pm->faultCode.push_back(0x7306);
    }
    if ((e.injectionFaultCode==6))
    {
        LOG_F(INFO, "艉部应急电源失效");
        pm->faultCode.push_back(0x7307);
    }
}

void PowerManagerFsm::sensorSystemFault()
{
    if(pm->m_subDisSysFault.ctd1 == 1 && pm->m_subDisSysFault.ctd2 == 1)
    {
        pm->m_systemData->insertFaultCode(0x8302);
        pm->m_systemData->insertFaultCode(0x8303);
    } else 
    {
        pm->m_systemData->eraseFaultCode(0x8302);
        pm->m_systemData->eraseFaultCode(0x8303);
    }

    if(pm->m_subDisSysFault.ctd1 == 1 && pm->m_subDisSysFault.ctd2 == 0)
        pm->m_systemData->insertFaultCode(0x8202);
    else
        pm->m_systemData->eraseFaultCode(0x8202);

    if(pm->m_subDisSysFault.ctd1 == 0 && pm->m_subDisSysFault.ctd2 == 1)
        pm->m_systemData->insertFaultCode(0x8203);
    else
        pm->m_systemData->eraseFaultCode(0x8203);

    if(pm->m_subDisSysFault.soundVeloc == 1)
        pm->m_systemData->insertFaultCode(0x8301);
    else
        pm->m_systemData->eraseFaultCode(0x8301);
}

void PowerManagerFsm::sensorSystemFault(FaultEvent const &e)
{
    LOG_F(INFO, "sensorSystemFault");
    if ((e.injectionFaultCode==1))
    {
        LOG_F(INFO, "部分CTD失效");
        pm->faultCode.push_back(0x8202);
    }
    if ((e.injectionFaultCode==2))
    {
        LOG_F(INFO, "CTD失效");
        pm->faultCode.push_back(0x8302);
        pm->faultCode.push_back(0x8303);
    }
    if ((e.injectionFaultCode==3))
    {
        LOG_F(INFO, "声速仪失效");
        pm->faultCode.push_back(0x8301);
    }
}
void PowerManagerFsm::react(FaultEvent const &e)
{
   LOG_F(INFO, "处理故障事件");
   basicNavigationFault(e);
   extendedNavigationFault(e);
   obstacleAvoidanceFault(e);
   buoyancyControlFault(e);
   communicationSystemFault(e);
   payloadSystemFault(e);
   emergencySystemFault(e);
   sensorSystemFault(e);
}

void PowerManagerFsm::handelPowerCmd(){
    switch (pm->m_msCmd.powerCMD)
    {
    case 0:
    {
        LOG_F(INFO, "收到上位机控制指令:关机");
        //1. 关闭动力电池
        pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Stop;
        MOOSPause(3);
        int count=0;
        while (!(pm->m_systemData->dyn_pos_relay_status == 0x02 && pm->m_systemData->dyn_neg_relay_status == 0x02))
        {
            LOG_F(INFO,"动力锂电池下电中...");
            MOOSPause(3);
            if(count>10)
            {
                LOG_F(INFO, "动力锂电池无下电反馈"); 
                break;
            }
            count++;
        }
        LOG_F(INFO, "动力锂电池已下电"); 
        //2. 断掉所有动力电断路器
        //2.1 断开所有艏部高压断路器
        pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HVA_xczCircuit_Breaker(0);
        MOOSPause(3);
        //2.2 断开所有艉部高压设备断路器
        pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_sternFt45_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_fbReserved_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_tyzReserved_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_reserved_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_dcDc5_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_coolingSystem_Breaker(0);
        MOOSPause(3);
        
        //2.3 断开仪表预留断路器
        pm->m_lowerCommManager->operate_LV_reservedCircuit1_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_LV_reservedCircuit2_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_LVA_reservedCircuit1_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_LVA_reservedCircuit2_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_LVA_bowPZDeviceCircuit_Breaker(0);
        MOOSPause(3);
        
        //2.4 断开应急电池断路器
        pm->m_lowerCommManager->operate_LV_mergencyLithiumBatteryGroup2Circuit_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_LVA_mergencyLithiumBatteryGroup1Circuit_Breaker(0);
        MOOSPause(3);

        //2.5 断开动力电断路器
        pm->m_lowerCommManager->operate_HV_bowHighVoltageBox_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_dcDc5_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_coolingSystem_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_dcDc5Module_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_powerLithiumBattery_Breaker(0);
        MOOSPause(3);
        {
            int count=0;
            while (!(pm->m_systemData->dyn_pos_relay_status == 0x02 && pm->m_systemData->dyn_neg_relay_status == 0x02))
            {
                LOG_F(INFO,"动力锂电池下电中...");
                MOOSPause(3);
                if(count>10)
                {
                    LOG_F(INFO, "动力锂电池无下电反馈"); 
                    break;
                }
                count++;
            }
        }
        LOG_F(INFO, "动力锂电池已下电");     
        //3. 次级配电系统恢复待机
        pm->m_driver.getSTANDBYTable(&pm->m_subDisSysCmd);
        MOOSPause(30);
        //4. 执行锂电池关机指令 
        pm->m_CcuColCmd.insBatCmd = BatteryCommand::Stop;
        break;
    }
    case 1:
    {
        LOG_F(INFO, "收到上位机控制指令:动力电上电");
        //1. 动力锂电池上电
        pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Start;
        //2. 闭合动力电断路器
        pm->m_lowerCommManager->operate_HV_powerLithiumBattery_Breaker(1);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_dcDc5Module_Breaker(1);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_dcDc5_Breaker(1);
        pm->m_lowerCommManager->operate_HV_coolingSystem_Breaker(1);
        MOOSPause(20);
        pm->m_lowerCommManager->operate_HV_bowHighVoltageBox_Breaker(1);
        MOOSPause(3);
        //2. 燃料电池上电
        break;
    }
    case 2:
    {
        LOG_F(INFO, "收到上位机控制指令:锂动力电上电");
        //1. 闭合动力电断路器
        pm->m_lowerCommManager->operate_HV_powerLithiumBattery_Breaker(1);
        //2. 动力锂电池上电
        pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Start;
        break;
    }
    case 3:
    {
        LOG_F(INFO, "收到上位机控制指令:燃料电池上电");
        break;
    }
    case 4:
    {
        LOG_F(INFO, "收到上位机控制指令:锂动力电下电");
        //1. 动力锂电池下电
        pm->m_lowerCommManager->operate_HV_bowHighVoltageBox_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_dcDc5_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_coolingSystem_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_dcDc5Module_Breaker(0);
        MOOSPause(3);
        pm->m_lowerCommManager->operate_HV_powerLithiumBattery_Breaker(0);
        MOOSPause(3);
        int count=0;
        while (!(pm->m_systemData->dyn_pos_relay_status == 0x02 && pm->m_systemData->dyn_neg_relay_status == 0x02))
        {
            LOG_F(INFO,"动力锂电池下电中...");
            MOOSPause(3);
            if(count>10)
            {
                LOG_F(INFO, "动力锂电池无下电反馈"); 
                break;
            }
            count++;
        }
        LOG_F(INFO, "动力锂电池已下电");     
        break;
    }
    case 5:
        LOG_F(INFO, "收到上位机控制指令:燃料电池下电");
        break;
    case 6:
        LOG_F(INFO, "收到上位机控制指令:燃料电池预热指令");
        //备用
        break;
    case 7:
        LOG_F(INFO, "收到上位机控制指令:燃料电池自检指令");
        //备用
        break;
    case 8:
        LOG_F(INFO, "收到上位机控制指令:燃料电池紧急停机");
        //备用
        break;
    case 9:
        LOG_F(INFO, "收到上位机控制指令:燃料电池复位指令");
        //备用
        break;
    case 10:
        LOG_F(INFO, "收到上位机控制指令:燃料电池补给指令");
        //备用
        break;
    case 11:
        LOG_F(INFO, "收到上位机控制指令:燃料电池停机维护");
        //备用
        break;
    default:
        break;
    }
    //清空指令
    pm->m_msCmd.powerCMD = 99;
}

void PowerManagerFsm::handleWorkCmd(){
    switch (pm->m_msCmd.workCMD)
    {
    case 1:
        LOG_F(INFO, "收到上位机控制指令:待机工况");
        transit<StandbyState>();
        break;
    case 2:
        LOG_F(INFO, "收到上位机控制指令:岸基备航");
        transit<ShoreBasedReady>();
        break;
    case 3:
        LOG_F(INFO, "收到上位机控制指令:水中备航");
        transit<WaterBasedReady>();
        break;
    case 4:
        LOG_F(INFO, "收到上位机控制指令:遥控工况");
        transit<RemoteControl>();
        break;
    case 5:
        LOG_F(INFO, "收到上位机控制指令:巡航模式");
        transit<CruiseMode>();
        break;
    case 6:
        LOG_F(INFO, "收到上位机控制指令:高速模式");
        transit<HighSpeedMode>();
        break;
    case 7:
        LOG_F(INFO, "收到上位机控制指令:上浮下潜");
        transit<FloatDown>();
        break;
    case 8:
        LOG_F(INFO, "收到上位机控制指令:浮调模式");
        transit<FloatAdjust>();
        break;
    case 9:
        LOG_F(INFO, "收到上位机控制指令:水下侦查");
        transit<UnderwaterSurvey>();
        break;
    case 10:
        LOG_F(INFO, "收到上位机控制指令:水面侦查");
        transit<SurfaceSurvey>();
        break;
    case 11:
        LOG_F(INFO, "收到上位机控制指令:DJ模式");
        transit<DJMode>();
        break;
    default:
        break;
    }
    //清空指令
    pm->m_msCmd.workCMD = 99;
}

void PowerManagerFsm::handleModCmd(){
    switch (pm->m_msCmd.modCMD)
    {
    case 0:
        LOG_F(INFO, "收到上位机控制指令:正常模式");
        pm->currentMod = PMMode::Normal;
        break;
    case 1:
        LOG_F(INFO, "收到上位机控制指令:试验模式");
        pm->currentMod = PMMode::Test;
        break;
    case 2:
        LOG_F(INFO, "收到上位机控制指令:调试模式");
        pm->currentMod = PMMode::Debug;
        break;
    default:
        break;
    }
    LOG_F(INFO, "当前工作模式: %d", pm->currentMod);
    // 清空指令
    pm->m_msCmd.modCMD = 99;
}

void PowerManagerFsm::handleDeviceCmd() {
    LOG_F(INFO, "开始处理设备命令:");
}

void PowerManagerFsm::react(TestEvent const &e)
{
    LOG_F(INFO, "测试消息");
    if(e.type == 0){
        LOG_F(INFO, "清空故障代码");
        pm->faultCode.clear();
        pm->m_leakage = 0x00;
        pm->m_systemData->insfaultCode = 0x00;
        pm->m_systemData->powfaultCode = 0x00;
        pm->m_systemData->fc_fault_level_4 = 0;
        pm->m_systemData->fc_fault_level_3 = 0;
        pm->m_systemData->fc_fault_level_2 = 0;
        pm->m_systemData->fc_fault_level_1 = 0;
        pm->m_systemData->disSysFaultCode.clear();
        cout << "清空故障代码" << endl;
    };
    // if(e.eventId == 0) return ;
    LOG_F(INFO, "测试消息类型: %d,测试代码：%d", e.type, e.eventId);
    FaultEvent faultEvent;
    faultEvent.injectionFaultCode = e.eventId;
    //故障注入
    switch (e.type)
    {
    case 0:
        break;
    case 1:
        basicNavigationFault(faultEvent);
        break;
    case 2:
        extendedNavigationFault(faultEvent);
        break;
    case 3:
        obstacleAvoidanceFault(faultEvent);
        break;
    case 4:
        buoyancyControlFault(faultEvent);
        break;
    case 5:
        communicationSystemFault(faultEvent);
        break;
    case 6:
        payloadSystemFault(faultEvent);
        break;
    case 7:
        emergencySystemFault(faultEvent);
        break;
    case 8:
        sensorSystemFault(faultEvent);
        break;
    case 9:
        LOG_F(INFO, "漏水故障");
        switch (e.eventId)
        {
            case 1:
                    pm->m_leakage = 0x10; //锂电池舱
                break;
            case 2:
                    pm->m_leakage = 0x20; //艉应急
                break;
            case 3:
                    pm->m_leakage = 0x40; //燃料电池舱漏水
                break;
            case 4:
                    pm->m_leakage = 0x80; //艏部应急漏水
                break;
            case 5:
                    pm->m_leakage = 0x01; //艏部应急漏水
                break;
            case 6:
                    pm->m_leakage = 0x02; //艏高压漏水
                break;
            case 7:
                    pm->m_leakage = 0x04; //艏低压漏水
                break;
            case 8:
                    pm->m_leakage = 0x08; //燃电舱漏水
                break;
            default:
                break;
        }
        break;
    case 10:
        LOG_F(INFO,"锂电池故障测试");
        pm->m_systemData->insfaultCode = batfaultMap[e.eventId].code;
        pm->m_systemData->powfaultCode = batfaultMap[e.eventId].code;
        break;
    case 11:
    {
        LOG_F(INFO,"燃料电池1级故障测试");
        uint16_t code = 0x0001;
        pm->m_systemData->fc_fault_level_1 = code << (e.eventId);
        break;
    }
    case 12:
    {
        LOG_F(INFO,"燃料电池2级故障测试");
        uint16_t code = 0x0001;
        pm->m_systemData->fc_fault_level_2 = code << (e.eventId);
        break;
    }
    case 13:
    {
        LOG_F(INFO,"燃料电池3级故障测试");        
        uint16_t code = 0x0001;
        pm->m_systemData->fc_fault_level_3 = code << (e.eventId);
        break;
    }
    case 14:
    {
        LOG_F(INFO,"燃料电池4级故障测试");
        uint16_t code = 0x0001;
        pm->m_systemData->fc_fault_level_4 = code << (e.eventId);
        break;
    }
    case 15:
    {
        LOG_F(INFO,"配电系统故障测试");
        // pm->m_systemData->distributeFaultCode.push_back(disfaultMap[e.eventId].code);
        pm->m_systemData->disSysFaultCode.insert(disfaultMap[e.eventId].code);
    }
    default:
        LOG_F(INFO, "测试消息类型错误");
        break;
    }

}

FSM_INITIAL_STATE(PowerManagerFsm, InitState);