2929 lines
97 KiB
C++
2929 lines
97 KiB
C++
#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); |