#include "PowerManagerFsm.hpp" #include "../PowerManger.h" #include "../logc/loguru.hpp" #include #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(); LOG_F(INFO, "使命结束"); }; void PowerManagerFsm::react(CycleTriggeredEvent const &){ // 更新当前设备状态 #ifdef DEBUG_1 pm->m_currentDriverState = pm->m_subDisSysState; #endif // cout << "CycleTriggeredEvent received" << endl; } bool PowerManagerFsm::handPowerProtection(DevCmdExt const &e){ if(pm->workCondition > WorkCondition::REMOTE_CONTROL){ LOG_F(INFO, "上电保护，执行任务时候禁止操作设备"); return false; } if(e.id == DeviceId::singleBeamSonar1 || e.id == DeviceId::singleBeamSonar2 || e.id == DeviceId::singleBeamSonar3 || e.id == DeviceId::singleBeamSonar4 || e.id == DeviceId::singleBeamSonar5 || e.id == DeviceId::singleBeamSonar6 || e.id == DeviceId::singleBeamSonar7 || e.id == DeviceId::singleBeamSonar8 || e.id == DeviceId::sideSonar || e.id == DeviceId::forwardSonar || e.id == DeviceId::lowFreqSonar || e.id == DeviceId::DVL || e.id == DeviceId::USBL) { if(e.cmd == 1) { if(pm->m_depth < 10) { LOG_F(INFO, "水下设备操作上电保护 %d ", e.id); return false; } else return true; } } } void PowerManagerFsm::handleNoPowerDeviceCmd(int id , int cmd){ if(id == DeviceId::depth1 || id == DeviceId::depth2 || id == DeviceId::depth3 || id == DeviceId::overDeep1 || id == DeviceId::overDeep2 || id == DeviceId::ctd1 || id == DeviceId::ctd2 || id == DeviceId::soundVeloc || id == DeviceId::waterInject || id == DeviceId::launchControler || id == DeviceId::photoSensor || id == DeviceId::electSensor || id == DeviceId::commMast || id == DeviceId::ntp || id == DeviceId::bowThrow || id == DeviceId::sternThrow || id == DeviceId::ins || id == DeviceId::ntp || id == DeviceId::inputWaterSensor1 || id == DeviceId::inputWaterSensor2 || id == DeviceId::inputWaterSensor3 ) { LOG_F(INFO, "无动力电设备操作"); } } void PowerManagerFsm::react(DeviceCmd const &){ LOG_F(INFO, "DeviceCmd received"); } 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(); break; case TaskType::FLOAT_UP: transit(); break; case TaskType::RECYCLE: transit(); break; case TaskType::HOVER: transit(); break; case TaskType::SAT_COMM: transit(); break; case TaskType::SAT_CALIBRATE: transit(); break; case TaskType::TOUR: transit(); break; case TaskType::SAIL: transit(); break; default: break; } } void PowerManagerFsm::react(DevCmdExt const &e) { LOG_F(INFO, "DevCmdExt received - id: %d, cmd: %d", e.id, e.cmd); switch (e.id) { case 0: { LOG_F(INFO, "DevCmdExt received singleBeamSonar1 - id: %d, cmd: %d", e.id, e.cmd); if(e.cmd <=1) { pm->m_subDisSysCmd.singleBeamSonar1 = e.cmd; #ifdef DEBUG pm->m_currentDriverState.singleBeamSonar1 = e.cmd; #endif } else LOG_F(WARNING, "DevCmdExt received singleBeamSonar1 - id: %d, cmd: %d is out of range", e.id, e.cmd); break; } case 1: { LOG_F(INFO, "DevCmdExt received singleBeamSonar2 - id: %d, cmd: %d", e.id, e.cmd); if (e.cmd <= 1) { pm->m_subDisSysCmd.singleBeamSonar2 = e.cmd; #ifdef DEBUG pm->m_currentDriverState.singleBeamSonar2 = e.cmd; #endif } else { LOG_F(WARNING, "DevCmdExt received singleBeamSonar2 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 2: { LOG_F(INFO, "DevCmdExt received singleBeamSonar3 - id: %d, cmd: %d", e.id, e.cmd); if (e.cmd <= 1) { pm->m_subDisSysCmd.singleBeamSonar3 = e.cmd; #ifdef DEBUG pm->m_currentDriverState.singleBeamSonar3 = e.cmd; #endif } else { LOG_F(WARNING, "DevCmdExt received singleBeamSonar3 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 3: { LOG_F(INFO, "DevCmdExt received singleBeamSonar4 - id: %d, cmd: %d", e.id, e.cmd); if (e.cmd <= 1) { pm->m_subDisSysCmd.singleBeamSonar4 = e.cmd; #ifdef DEBUG pm->m_currentDriverState.singleBeamSonar4 = e.cmd; #endif } else { LOG_F(WARNING, "DevCmdExt received singleBeamSonar4 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 4: { LOG_F(INFO, "DevCmdExt received singleBeamSonar5 - id: %d, cmd: %d", e.id, e.cmd); if (e.cmd <= 1) { pm->m_subDisSysCmd.singleBeamSonar5 = e.cmd; #ifdef DEBUG pm->m_currentDriverState.singleBeamSonar5 = e.cmd; #endif } else { LOG_F(WARNING, "DevCmdExt received singleBeamSonar5 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 5: { LOG_F(INFO, "DevCmdExt received singleBeamSonar6 - id: %d, cmd: %d", e.id, e.cmd); if (e.cmd <= 1) { pm->m_subDisSysCmd.singleBeamSonar6 = e.cmd; #ifdef DEBUG pm->m_currentDriverState.singleBeamSonar6 = e.cmd; #endif } else { LOG_F(WARNING, "DevCmdExt received singleBeamSonar6 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 6: { LOG_F(INFO, "DevCmdExt received singleBeamSonar7 - id: %d, cmd: %d", e.id, e.cmd); if (e.cmd <= 1) { pm->m_subDisSysCmd.singleBeamSonar7 = e.cmd; #ifdef DEBUG pm->m_currentDriverState.singleBeamSonar7 = e.cmd; #endif } else { LOG_F(WARNING, "DevCmdExt received singleBeamSonar7 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 7: { LOG_F(INFO, "DevCmdExt received singleBeamSonar8 - id: %d, cmd: %d", e.id, e.cmd); if (e.cmd <= 1) { pm->m_subDisSysCmd.singleBeamSonar8 = e.cmd; #ifdef DEBUG pm->m_currentDriverState.singleBeamSonar8 = e.cmd; #endif } else { LOG_F(WARNING, "DevCmdExt received singleBeamSonar8 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 8: { LOG_F(INFO, "DevCmdExt received forwardSonar - id: %d, cmd: %d", e.id, e.cmd); if (e.cmd <= 1) { pm->m_subDisSysCmd.forwardSonar = e.cmd; #ifdef DEBUG pm->m_currentDriverState.forwardSonar = e.cmd; #endif } else { LOG_F(WARNING, "DevCmdExt received forwardSonar - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 9: { LOG_F(INFO, "DevCmdExt received bowCover1 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowCover1 = e.cmd; #endif if (e.cmd == 0) { pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_OFF); MOOSPause(10); pm->m_subDisSysCmd.bowCover1 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { //TODO ：动力电上电 pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_ON); MOOSPause(1); pm->m_subDisSysCmd.bowCover1 = 1; } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowCover1 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_ON); } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowCover1 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_OFF); } else { LOG_F(WARNING, "DevCmdExt received bowCover1 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 10: { LOG_F(INFO, "DevCmdExt received bowCover2 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowCover2 = e.cmd; #endif if (e.cmd == 0) { //TODO ：动力电断电 pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_OFF); MOOSPause(1); pm->m_subDisSysCmd.bowCover2 = 0; } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowCover2 = 1; //TODO ：动力电上电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_ON); } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowCover2 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowCover2 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_OFF); } else { LOG_F(WARNING, "DevCmdExt received bowCover2 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 11: { LOG_F(INFO, "DevCmdExt received bowCover3 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowCover3 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.bowCover3 = 0; //TODO ：动力电断电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_OFF); } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowCover3 = 1; //TODO ：动力电上电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_ON); } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowCover3 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_ON); } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowCover3 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_OFF); } else { LOG_F(WARNING, "DevCmdExt received bowCover2 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 12: { LOG_F(INFO, "DevCmdExt received bowCover4 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowCover4 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.bowCover4 = 0; //TODO ：动力电断电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_OFF); } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowCover4 = 1; //TODO ：动力电上电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_ON); } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowCover4 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_ON); } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowCover4 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_OFF); } else { LOG_F(WARNING, "DevCmdExt received bowCover2 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 13: { LOG_F(INFO, "DevCmdExt received bowCover5 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowCover5 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.bowCover5 = 0; //TODO ：动力电断电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_OFF); } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowCover5 = 1; //TODO ：动力电上电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_ON); } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowCover5 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_ON); } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowCover5 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_OFF); } else { LOG_F(WARNING, "DevCmdExt received bowCover5 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 14: { LOG_F(INFO, "DevCmdExt received bowCover6 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowCover6 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.bowCover6 = 0; //TODO ：动力电断电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_OFF); } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowCover6 = 1; //TODO ：动力电上电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_ON); } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowCover6 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_ON); } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowCover6 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_OFF); } else { LOG_F(WARNING, "DevCmdExt received bowCover6 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 15: { LOG_F(INFO, "DevCmdExt received bowRudderDeploy - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowRudderDeploy = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.bowRudderDeploy = 0; //TODO ：动力电断电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(BREAK_OFF); } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowRudderDeploy = 1; //TODO ：动力电上电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(BREAK_ON); } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowRudderDeploy = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(BREAK_ON); } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowRudderDeploy = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(BREAK_OFF); } else { LOG_F(WARNING, "DevCmdExt received bowRudderDeploy - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 16: { LOG_F(INFO, "DevCmdExt received bowRudderServo1 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowRudderServo1 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.bowRudderServo1 = 0; //TODO ：动力电断电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(BREAK_OFF); } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowRudderServo1 = 1; //TODO ：动力电上电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(BREAK_ON); } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowRudderServo1 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(BREAK_ON); } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowRudderServo1 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(BREAK_OFF); } else { LOG_F(WARNING, "DevCmdExt received bowRudderServo1 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 17: { LOG_F(INFO, "DevCmdExt received bowRudderServo2 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowRudderServo2 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.bowRudderServo2 = 0; //TODO ：动力电断电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(BREAK_OFF); } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowRudderServo2 = 1; //TODO ：动力电上电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(BREAK_ON); } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowRudderServo2 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(BREAK_ON); } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowRudderServo2 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(BREAK_OFF); } else { LOG_F(WARNING, "DevCmdExt received bowRudderServo2 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 18: { LOG_F(INFO, "DevCmdExt received depth1 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.depth1 = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.depth1 = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received depth1 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 19: { LOG_F(INFO, "DevCmdExt received depth2 - id: %d, cmd: %d", e.id, e.cmd); if (e.cmd <= 1) { pm->m_subDisSysCmd.depth2 = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received depth2 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 20: { LOG_F(INFO, "DevCmdExt received depth3 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.depth3 = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.depth3 = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received depth3 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 21: { LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.overDeep1 = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.overDeep1 = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received overDeep1 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 22: { LOG_F(INFO, " - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.overDeep2 = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.overDeep2 = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received overDeep2 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 23: { LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.ctd1 = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.ctd1 = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received ctd1 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 24: { LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.ctd2 = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.ctd2 = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received ctd2 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 25: { LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.soundVeloc = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.soundVeloc = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received soundVeloc - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 26: { LOG_F(INFO, "DevCmdExt received waterInject - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.waterInject = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.waterInject = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.waterInject = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.waterInject = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.waterInject = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received waterInject - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 27: { LOG_F(INFO, "DevCmdExt received bowWaterInet1 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowWaterInet1 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.bowWaterInet1 = 0; //TODO ：动力电断电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_OFF); } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowWaterInet1 = 1; //TODO ：动力电上电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_ON); } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowWaterInet1 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_ON); } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowWaterInet1 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_OFF); } else { LOG_F(WARNING, "DevCmdExt received bowWaterInet1 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 28: { LOG_F(INFO, "DevCmdExt received bowWaterInet2 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowWaterInet2 = e.cmd; #endif if (e.cmd == 0) { pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_OFF); MOOSPause(1); pm->m_subDisSysCmd.bowWaterInet2 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowWaterInet2 = 1; MOOSPause(1); pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_ON); //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowWaterInet2 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_ON); } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowWaterInet2 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_OFF); } else { LOG_F(WARNING, "DevCmdExt received bowWaterInet2 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 29: { LOG_F(INFO, "DevCmdExt received bowWaterInet2 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowWaterInet3 = e.cmd; #endif if (e.cmd == 0) { pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_OFF); MOOSPause(1); pm->m_subDisSysCmd.bowWaterInet3 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowWaterInet3 = 1; //TODO ：动力电上电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_ON); } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowWaterInet3 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_ON); } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowWaterInet3 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_OFF); } else { LOG_F(WARNING, "DevCmdExt received bowWaterInet3 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 30: { LOG_F(INFO, "DevCmdExt received bowWaterInet4 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowWaterInet4 = e.cmd; #endif if (e.cmd == 0) { pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_OFF); MOOSPause(1); pm->m_subDisSysCmd.bowWaterInet4 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowWaterInet4 = 1; //TODO ：动力电上电 MOOSPause(1); pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_ON); } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowWaterInet4 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_ON); } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowWaterInet4 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_OFF); } else { LOG_F(WARNING, "DevCmdExt received bowWaterInet4 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 31: { LOG_F(INFO, "DevCmdExt received bowWaterInet4 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowWaterInet5 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.bowWaterInet5 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowWaterInet5 = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowWaterInet5 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowWaterInet5 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received bowWaterInet5 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 32: { LOG_F(INFO, "DevCmdExt received bowWaterInet6 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowWaterInet6 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.bowWaterInet6 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowWaterInet6 = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowWaterInet6 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowWaterInet6 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received bowWaterInet6 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 33: { LOG_F(INFO, "DevCmdExt received bowWaterInet7 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowWaterInet7 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.bowWaterInet7 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowWaterInet7 = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowWaterInet7 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowWaterInet7 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received bowWaterInet7 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 34: { LOG_F(INFO, "DevCmdExt received bowWaterInet8 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowWaterInet8 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.bowWaterInet8 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.bowWaterInet8 = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.bowWaterInet8 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.bowWaterInet8 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received bowWaterInet8 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 35: { LOG_F(INFO, "DevCmdExt received sideSonar - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.sideSonar = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.sideSonar = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.sideSonar = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.sideSonar = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.sideSonar = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received sideSonar - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 36: { LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.lowFreqSonar = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.lowFreqSonar = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received lowFreqSonar - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 37: { LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.launchControler = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.launchControler = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received launchControler - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 38: { LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.photoSensor = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.photoSensor = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received photoSensor - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 39: { LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.electSensor = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.electSensor = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received electSensor - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 40: { LOG_F(INFO, "DevCmdExt received commMast - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.commMast = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.commMast = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received commMast - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 41: { LOG_F(INFO, "DevCmdExt received mastLiftingServo - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.mastLiftingServo = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.mastLiftingServo = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.mastLiftingServo = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.mastLiftingServo = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.mastLiftingServo = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received mastLiftingServo - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 42: { LOG_F(INFO, "DevCmdExt received USBL - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.USBL = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.USBL = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received USBL - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 43: { LOG_F(INFO, "DevCmdExt received DVL - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.DVL = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.DVL = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received DVL - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 44: { LOG_F(INFO, "DevCmdExt received buoyancyAdjust1 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.buoyancyAdjust1 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.buoyancyAdjust1 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.buoyancyAdjust1 = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.buoyancyAdjust1 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.buoyancyAdjust1 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received buoyancyAdjust1 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 45: { LOG_F(INFO, "DevCmdExt received buoyancyAdjust2 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.buoyancyAdjust2 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.buoyancyAdjust2 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.buoyancyAdjust2 = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.buoyancyAdjust2 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.buoyancyAdjust2 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received buoyancyAdjust2 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 46: { LOG_F(INFO, "DevCmdExt received buoyancyAdjust3 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.buoyancyAdjust3 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.buoyancyAdjust3 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.buoyancyAdjust3 = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.buoyancyAdjust3 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.buoyancyAdjust3 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received buoyancyAdjust3 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 47: { LOG_F(INFO, "DevCmdExt received buoyancyAdjust4 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.buoyancyAdjust4 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.buoyancyAdjust4 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.buoyancyAdjust4 = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.buoyancyAdjust4 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.buoyancyAdjust4 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received buoyancyAdjust4 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 48: { LOG_F(INFO, "DevCmdExt received buoyancyAdjust5 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.buoyancyAdjust5 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.buoyancyAdjust5 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.buoyancyAdjust5 = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.buoyancyAdjust5 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.buoyancyAdjust5 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received buoyancyAdjust5 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 49: { LOG_F(INFO, "DevCmdExt received sternRudderServo1 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.sternRudderServo1 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.sternRudderServo1 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.sternRudderServo1 = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.sternRudderServo1 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.sternRudderServo1 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received sternRudderServo1 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 50: { LOG_F(INFO, "DevCmdExt received sternRudderServo2 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.sternRudderServo2 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.sternRudderServo2 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.sternRudderServo2 = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.sternRudderServo2 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.sternRudderServo2 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received sternRudderServo2 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 51: { LOG_F(INFO, "DevCmdExt received sternRudderServo3 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.sternRudderServo3 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.sternRudderServo3 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.sternRudderServo3 = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.sternRudderServo3 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.sternRudderServo3 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received sternRudderServo3 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 52: { LOG_F(INFO, "DevCmdExt received sternRudderServo4 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.sternRudderServo4 = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.sternRudderServo4 = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.sternRudderServo4 = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.sternRudderServo4 = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.sternRudderServo4 = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received sternRudderServo4 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 53: { LOG_F(INFO, "DevCmdExt received thruster - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.thruster = e.cmd; #endif if (e.cmd == 0) { pm->m_subDisSysCmd.thruster = 0; //TODO ：动力电断电 } else if (e.cmd == 1) { pm->m_subDisSysCmd.thruster = 1; //TODO ：动力电上电 } else if (e.cmd == 2) { pm->m_subDisSysCmd.thruster = 1; } else if (e.cmd == 3) { //TODO ：动力电上电 } else if (e.cmd == 4) { pm->m_subDisSysCmd.thruster = 0; } else if (e.cmd == 5) { //TODO ：动力电断电 } else { LOG_F(WARNING, "DevCmdExt received thruster - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 54: { LOG_F(INFO, "DevCmdExt received bowThrow - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.bowThrow = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.bowThrow = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received bowThrow - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 55: { LOG_F(INFO, "DevCmdExt received sternThrow - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.sternThrow = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.sternThrow = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received sternThrow - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 56: { LOG_F(INFO, "DevCmdExt received buoyage - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.buoyage = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.buoyage = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received buoyage - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 57: { { LOG_F(INFO, "DevCmdExt received buoyage - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.ins = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.ins = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received ins - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } } case 58: { LOG_F(INFO, "DevCmdExt received buoyage - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.ntp = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.ntp = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received ntp - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 59: { LOG_F(INFO, "DevCmdExt received inputWaterSensor1 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.inputWaterSensor1 = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.inputWaterSensor1 = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received inputWaterSensor1 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 60: { LOG_F(INFO, "DevCmdExt received inputWaterSensor2 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.inputWaterSensor2 = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.inputWaterSensor2 = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received inputWaterSensor2 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } case 61: { LOG_F(INFO, "DevCmdExt received inputWaterSensor3 - id: %d, cmd: %d", e.id, e.cmd); #ifdef DEBUG pm->m_currentDriverState.inputWaterSensor3 = e.cmd; #endif if (e.cmd <= 1) { pm->m_subDisSysCmd.inputWaterSensor3 = e.cmd; } else { LOG_F(WARNING, "DevCmdExt received inputWaterSensor3 - id: %d, cmd: %d is out of range", e.id, e.cmd); } break; } default: LOG_F(WARNING, "DevCmdExt received unknown id: %d, cmd: %d", e.id, e.cmd); break; } } 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(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(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(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(0x4301); } } 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(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(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(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); while (!(pm->m_systemData->dyn_pos_relay_status == 0x02 && pm->m_systemData->dyn_neg_relay_status == 0x02)) { LOG_F(INFO,"动力锂电池下电中..."); MOOSPause(3); } LOG_F(INFO, "动力锂电池已下电"); //2. 断掉所有动力电断路器 //2.1 断开所有艏部高压断路器 pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_HVA_xczCircuit_Breaker(BREAK_OFF); //2.2 断开所有艉部高压设备断路器 pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_HV_sternFt45_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_HV_fbReserved_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_HV_tyzReserved_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_HV_reserved_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_HV_dcDc5_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_HV_coolingSystem_Breaker(BREAK_OFF); //2.3 断开仪表预留断路器 pm->m_lowerCommManager->operate_LV_reservedCircuit1_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_LV_reservedCircuit2_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_LVA_reservedCircuit1_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_LVA_reservedCircuit2_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_LVA_bowPZDeviceCircuit_Breaker(BREAK_OFF); //2.4 断开应急电池断路器 pm->m_lowerCommManager->operate_LV_mergencyLithiumBatteryGroup2Circuit_Breaker(BREAK_OFF); pm->m_lowerCommManager->operate_LVA_mergencyLithiumBatteryGroup1Circuit_Breaker(BREAK_OFF); //3. 次级配电系统恢复待机 pm->m_driver.getSTANDBYTable(&pm->m_subDisSysCmd); MOOSPause(30); //4. 执行锂电池关机指令 pm->m_CcuColCmd.insBatCmd = BatteryCommand::Stop; break; case 1: LOG_F(INFO, "收到上位机控制指令:动力电上电"); //闭合动力电断路器 if(pm->m_lowerCommManager->operate_HV_powerLithiumBattery_Breaker(BREAK_ON)) LOG_F(INFO,"动力锂电池断路器闭合成功"); else LOG_F(INFO,"动力锂电池断路器闭合操作失败"); //1. 动力锂电池上电 pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Start; //2. 燃料电池上电 break; case 2: LOG_F(INFO, "收到上位机控制指令:锂动力电上电"); //1. 闭合动力电断路器 pm->m_lowerCommManager->operate_HV_powerLithiumBattery_Breaker(BREAK_ON); //2. 动力锂电池上电 pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Start; break; case 3: LOG_F(INFO, "收到上位机控制指令:燃料电池上电"); //备用 break; case 4: LOG_F(INFO, "收到上位机控制指令:锂动力电下电"); //1. 动力锂电池下电 MOOSPause(3); while (!(pm->m_systemData->dyn_pos_relay_status == 0x02 && pm->m_systemData->dyn_neg_relay_status == 0x02)) { LOG_F(INFO,"动力锂电池下电中..."); MOOSPause(3); } 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; } } void PowerManagerFsm::handleWorkCmd(){ switch (pm->m_msCmd.workCMD) { case 1: LOG_F(INFO, "收到上位机控制指令:待机工况"); transit(); break; case 2: LOG_F(INFO, "收到上位机控制指令:岸基备航"); transit(); break; case 3: LOG_F(INFO, "收到上位机控制指令:水中备航"); transit(); break; case 4: LOG_F(INFO, "收到上位机控制指令:遥控工况"); transit(); break; case 5: LOG_F(INFO, "收到上位机控制指令:巡航模式"); transit(); break; case 6: LOG_F(INFO, "收到上位机控制指令:高速模式"); transit(); break; case 7: LOG_F(INFO, "收到上位机控制指令:上浮下潜"); transit(); break; case 8: LOG_F(INFO, "收到上位机控制指令:浮调模式"); transit(); break; case 9: LOG_F(INFO, "收到上位机控制指令:水下侦查"); transit(); break; case 10: LOG_F(INFO, "收到上位机控制指令:水面侦查"); transit(); break; case 11: LOG_F(INFO, "收到上位机控制指令:DJ模式"); transit(); break; default: break; } } 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); } 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->distributeFaultCode.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; cout << "=======================" << (int)pm->m_systemData->insfaultCode << endl; cout << "=======================" << (int)pm->m_systemData->powfaultCode << endl; 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); } default: LOG_F(INFO, "测试消息类型错误"); break; } } FSM_INITIAL_STATE(PowerManagerFsm, InitState);