diff --git a/src/pPowerManger/LowerCommManager.cpp b/src/pPowerManger/LowerCommManager.cpp index 1cad642..4d289cc 100644 --- a/src/pPowerManger/LowerCommManager.cpp +++ b/src/pPowerManger/LowerCommManager.cpp @@ -81,6 +81,10 @@ void LowerCommManager::processPeriodicTasks() { //延时5ms usleep(5000); // sendDisSysCommand(m_pm->m_targetDisBreakerState); + // if(m_pm->m_systemData->device_common_status.dis_hv_c1.isTimeout) + // { + + // } m_udpComm->clearMsgQueue(); } diff --git a/src/pPowerManger/PowerManger.h b/src/pPowerManger/PowerManger.h index ea12784..51059b3 100644 --- a/src/pPowerManger/PowerManger.h +++ b/src/pPowerManger/PowerManger.h @@ -141,6 +141,9 @@ class PowerManger : public AppCastingMOOSApp bool sendNotification(const std::string& key, const std::string& value) { return Notify(key, value); } + bool sendNotification(const std::string& key, double value) { + return Notify(key, value); + } public: double GetPublicAppFreq() { return GetAppFreq(); } unsigned int GetPublicCommsFreq() { return GetCommsFreq(); } diff --git a/src/pPowerManger/fsm/PowerManagerFsm.cpp b/src/pPowerManger/fsm/PowerManagerFsm.cpp index 991fdbc..bc3ef82 100644 --- a/src/pPowerManger/fsm/PowerManagerFsm.cpp +++ b/src/pPowerManger/fsm/PowerManagerFsm.cpp @@ -33,7 +33,7 @@ void PowerManagerFsm::react(CycleTriggeredEvent const &){ //非调试模式下，对水声设备进行保护 if(pm->currentMod != PMMode::Debug) { - if(pm->m_depth > 10) + if(pm->m_depth < OVER_DEPTH) { if(pm->m_systemData->power_a_bus_status.xczCircuitBreaker == 1) pm->m_lowerCommManager->operate_HVA_xczCircuit_Breaker(0); @@ -100,7 +100,6 @@ void PowerManagerFsm::handleCuriseWork() 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(); - MOOSPause(3); pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(1); } //2.2 桅杆舵机 @@ -108,7 +107,6 @@ void PowerManagerFsm::handleCuriseWork() { pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::mastLiftingServo, (unsigned char)cmd); pm->m_upperCommManager->processPeriodicTasks(); - MOOSPause(3); pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(1); } //2.3 艏部浮调机构 @@ -118,7 +116,6 @@ void PowerManagerFsm::handleCuriseWork() 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(); - MOOSPause(3); pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(1); } //2.4 艉部浮调机构 @@ -127,7 +124,6 @@ void PowerManagerFsm::handleCuriseWork() 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(); - MOOSPause(3); pm->m_lowerCommManager->operate_HV_sternFt45_Breaker(1); } //2.5 推进电机 @@ -168,14 +164,25 @@ void PowerManagerFsm::handleHydroacousticDeviceCmd(int id , int cmd) { if (pm->currentMod != PMMode::Debug) { - if(pm->m_depth < 10) + if(pm->m_depth < OVER_DEPTH) { LOG_F(INFO, "水下设备操作上电保护 %d ", id); return; } } - pm->m_driver.setDeviceState(pm->m_subDisSysCmd, (DeviceId::Type)id, (unsigned char)cmd); + // 如果是前视声纳断电则先关机再断电 + 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); + } } @@ -717,7 +724,7 @@ void PowerManagerFsm::handleSideSonarCmd(int id, int cmd) { if (pm->currentMod != PMMode::Debug) { - if(pm->m_depth < 10) + if(pm->m_depth < OVER_DEPTH) { LOG_F(INFO, "水下设备操作上电保护 %d ", id); return; @@ -891,7 +898,6 @@ void PowerManagerFsm::react(DevCmdExt const &e) void PowerManagerFsm::getNoPowerDeviceStatus(DriverTable &s) { - s.bowThrow = pm->m_subDisSysState.bowThrow; s.commMast = pm->m_subDisSysState.commMast; s.ctd1 = pm->m_subDisSysState.ctd1; s.ctd2 = pm->m_subDisSysState.ctd2; @@ -922,6 +928,8 @@ void PowerManagerFsm::getNoPowerDeviceStatus(DriverTable &s) 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) diff --git a/src/pPowerManger/fsm/PowerManagerFsm.hpp b/src/pPowerManger/fsm/PowerManagerFsm.hpp index 97b190e..2ff3ab4 100644 --- a/src/pPowerManger/fsm/PowerManagerFsm.hpp +++ b/src/pPowerManger/fsm/PowerManagerFsm.hpp @@ -10,6 +10,10 @@ #include "../pmSysvariable.h" #include "../driver.h" #include "../PowerManger.h" + + +#define OVER_DEPTH 0.1 + // 前向声明 class PowerManger; diff --git a/src/pPowerManger/fsm/WaterBasedReady.cpp b/src/pPowerManger/fsm/WaterBasedReady.cpp index 82d39f6..5d90661 100644 --- a/src/pPowerManger/fsm/WaterBasedReady.cpp +++ b/src/pPowerManger/fsm/WaterBasedReady.cpp @@ -8,6 +8,7 @@ void WaterBasedReady::entry() { //启动水声设备 pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::DVL, (unsigned char)EquipmentCommand::ON); pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::USBL, (unsigned char)EquipmentCommand::ON); + pm->m_upperCommManager->processPeriodicTasks(); pm->workCondition = WorkCondition::WATER_BASED_READY; LOG_F(INFO, "进入水中备航状态"); } @@ -27,14 +28,24 @@ void WaterBasedReady::react(DevCmdExt const &e) { if (pm->currentMod != PMMode::Debug) { - if(pm->m_depth < 10) + if(pm->m_depth < OVER_DEPTH) { LOG_F(INFO, "水下设备操作上电保护 %d ", id); return; } } - pm->m_driver.setDeviceState(pm->m_subDisSysCmd, (DeviceId::Type)id, (unsigned char)cmd); + // 如果是前视声纳断电则先关机再断电 + if(id == DeviceId::forwardSonar) + { + if(cmd == 0) + { + LOG_F(INFO,"前视声纳关机指令"); + pm->sendNotification("uExternComm_frontSonarControl_cmd","0"); + MOOSPause(10000); + } + } LOG_F(INFO, "水声设备操作: %s,%d, %d", DeviceId::toString((DeviceId::Type)id), id, cmd); + pm->m_driver.setDeviceState(pm->m_subDisSysCmd, (DeviceId::Type)id, (unsigned char)cmd); } else { diff --git a/src/pPowerManger/systemData.h b/src/pPowerManger/systemData.h index 11f4f59..20f0a2f 100644 --- a/src/pPowerManger/systemData.h +++ b/src/pPowerManger/systemData.h @@ -712,6 +712,12 @@ public: disSysFaultCode.erase(faultCode); } } + void updateBreakerFault(unsigned char& breakerStatus, unsigned char rawStatus) + { + std::lock_guard lock(faultCodeMutex); + unsigned char newStatus = rawStatus == 0x55 ? 1 : (rawStatus == 0xAA ? 0 : 2); + breakerStatus = newStatus; // 保证状态更新也是原子的 + } void updateSystemData(disHighVolBusState data){ updateBreakerFault(power_bus_status.fuelCellCircuitBreaker, data.fuelCellCircuitBreaker, 1); updateBreakerFault(power_bus_status.powerLithiumBatteryCircuitBreaker, data.powerLithiumBatteryCircuitBreaker, 2); @@ -722,9 +728,9 @@ public: updateBreakerFault(power_bus_status.sternFTDevice45CircuitBreaker, data.sternFTDevice45CircuitBreaker, 7); updateBreakerFault(power_bus_status.sternRudderSwitch1CircuitBreaker, data.sternRudderSwitch1CircuitBreaker, 8); updateBreakerFault(power_bus_status.sternRudderSwitch2CircuitBreaker, data.sternRudderSwitch2CircuitBreaker, 9); - updateBreakerFault(power_bus_status.fbReservedCircuitBreaker, data.fbReservedCircuitBreaker, 10); - updateBreakerFault(power_bus_status.tyzReservedCircuitBreaker, data.tyzReservedCircuitBreaker, 11); - updateBreakerFault(power_bus_status.reservedCircuitBreaker, data.reservedCircuitBreaker, 12); + updateBreakerFault(power_bus_status.fbReservedCircuitBreaker, data.fbReservedCircuitBreaker); + updateBreakerFault(power_bus_status.tyzReservedCircuitBreaker, data.tyzReservedCircuitBreaker); + updateBreakerFault(power_bus_status.reservedCircuitBreaker, data.reservedCircuitBreaker); //模块状态 power_bus_status.dcDcModuleStatus = data.dcDcFaultWord1; power_bus_status.coolingSystemStatus = data.dcDcFaultWord2; @@ -819,8 +825,8 @@ public: updateBreakerFault(instrument_main_bus_status.bowLowVoltageDistributionBoxCircuitBreaker,data.bowLowVoltageDistributionBoxCircuitBreaker, 20); updateBreakerFault(instrument_main_bus_status.unit4InstrumentDC48VCircuitBreaker,data.unit4InstrumentDC48VCircuitBreaker, 21); updateBreakerFault(instrument_main_bus_status.dcC1DCDistributionPanelCircuitBreaker, data.dcC1DCDistributionPanelCircuitBreaker, 22); - updateBreakerFault(instrument_main_bus_status.reservedCircuitBreaker1, data.reservedCircuitBreaker1, 23); - updateBreakerFault(instrument_main_bus_status.reservedCircuitBreaker2, data.reservedCircuitBreaker2, 24); + updateBreakerFault(instrument_main_bus_status.reservedCircuitBreaker1, data.reservedCircuitBreaker1); + updateBreakerFault(instrument_main_bus_status.reservedCircuitBreaker2, data.reservedCircuitBreaker2); updateBreakerFault(instrument_main_bus_status.emergencyLithiumBatteryGroup2CircuitBreaker, data.emergencyLithiumBatteryGroup2CircuitBreaker, 25); //电源状态 @@ -860,9 +866,9 @@ public: updateBreakerFault(instrument_bus_status.unit2CircuitBreaker, data.unit2CircuitBreaker, 27); updateBreakerFault(instrument_bus_status.unit3CircuitBreaker, data.unit3CircuitBreaker, 28); updateBreakerFault(instrument_bus_status.unit5CircuitBreaker, data.unit5CircuitBreaker, 29); - updateBreakerFault(instrument_bus_status.bowPZDeviceCircuitBreaker, data.bowPZDeviceCircuitBreaker, 30); - updateBreakerFault(instrument_bus_status.reservedCircuitBreaker1, data.reservedCircuitBreaker1, 31); - updateBreakerFault(instrument_bus_status.reservedCircuitBreaker2, data.reservedCircuitBreaker2, 32); + updateBreakerFault(instrument_bus_status.bowPZDeviceCircuitBreaker, data.bowPZDeviceCircuitBreaker); + updateBreakerFault(instrument_bus_status.reservedCircuitBreaker1, data.reservedCircuitBreaker1); + updateBreakerFault(instrument_bus_status.reservedCircuitBreaker2, data.reservedCircuitBreaker2); updateBreakerFault(instrument_bus_status.emergencyLithiumBatteryGroup1CircuitBreaker, data.emergencyLithiumBatteryGroup1CircuitBreaker, 33); //电源状态