系修复了dcdc和冷却系统获取状态的bug,优化故障码变量

.vscode/settings.json

@@ -85,7 +85,8 @@
         "locale": "cpp",
         "stack": "cpp",
         "cstdbool": "cpp",
-        "version": "cpp"
+        "version": "cpp",
+        "unordered_set": "cpp"
     },
     "C_Cpp.errorSquiggles": "disabled",
     "marscode.chatLanguage": "cn",

src/pPowerManger/LowerCommManager.cpp

@@ -580,21 +580,106 @@ bool LowerCommManager::operate_HV_reserved_Breaker(char opt){
         [&](){ return m_pm->m_pmCurrentState.bus1State.reservedCircuitBreaker; }
     );
 }
-bool LowerCommManager::operate_HV_dcDc5_Breaker(char opt){
+// bool LowerCommManager::operate_HV_dcDc5_Breaker(char opt){
-    return operateBreakerGeneric<disHighVolBusCmd, char>(
+//     return operateBreakerGeneric<disHighVolBusCmd, char>(
-        "HV_dcDc5_Breaker",
+//         "HV_dcDc5_Breaker",
-        opt,
+//         opt,
-        [&](disHighVolBusCmd& c){ c.dcdc5Module = getBreakerOption(opt); },
+//         [&](disHighVolBusCmd& c){ c.dcdc5Module = getBreakerOption(opt); },
-        [&](){ return m_pm->m_pmCurrentState.bus1State.dcDcFaultWord1&(0x02); }
+//         [&](){ return m_pm->m_pmCurrentState.bus1State.dcDcFaultWord1&(0x02); }
-    );
+//     );
+// }
+bool LowerCommManager::operate_HV_dcDc5_Breaker(char opt)
+{
+    disHighVolBusCmd cmd{};                           // 默认全零
+    double t0 = MOOSTime();
+    bool ok = false;
+    string name = "DCDC Module";
+    LOG_F(INFO, "operate %s: opt=%d", name, opt);
+
+    cmd.dcdc5Module = getBreakerOption(opt);
+
+    m_udpComm->sendDisSysCmd(cmd);
+
+    ok = m_pm->m_pmCurrentState.bus1State.dcDcFaultWord1&(0x02);
+
+    MOOSPause(100);
+    while ( !ok ) {
+        if( m_pm->m_pmCurrentState.bus1State.dcDcFaultWord1&(0x02) == 1 )
+        {
+            ok = true;
+            break;
+        }
+        if (!m_udpComm->sendDisSysCmd(cmd)) {
+            LOG_F(ERROR, "operate %s: sendDisSysCmd failed", name);
+            ok =  false;
+            break;
+        }
+        if (MOOSTime() > t0 + 6) {
+            LOG_F(ERROR, "operate %s: timeout", name);
+            ok =  false;
+            break;
+        }
+        MOOSPause(200);
+    }
+    //继电器复位
+    disHighVolBusCmd clr{}; 
+    for (int i = 0; i < 3; ++i)
+    {
+        m_udpComm->sendDisSysCmd(clr);
+        MOOSPause(200);
+    }
+    return ok;
 }
-bool LowerCommManager::operate_HV_coolingSystem_Breaker(char opt){
+
-    return operateBreakerGeneric<disHighVolBusCmd, char>(
+// bool LowerCommManager::operate_HV_coolingSystem_Breaker(char opt){
-        "HV_coolingSystem_Breaker",
+//     return operateBreakerGeneric<disHighVolBusCmd, char>(
-        opt,
+//         "HV_coolingSystem_Breaker",
-        [&](disHighVolBusCmd& c){ c.coolingSystem = getBreakerOption(opt); },
+//         opt,
-        [&](){ return m_pm->m_pmCurrentState.bus1State.dcDcFaultWord1&(0x20); }
+//         [&](disHighVolBusCmd& c){ c.coolingSystem = getBreakerOption(opt); },
-    );
+//         [&](){ return (m_pm->m_pmCurrentState.bus1State.dcDcFaultWord1 & 0x20) ? 0x55 : 0xAA;  }
+//     );
+// }
+bool LowerCommManager::operate_HV_coolingSystem_Breaker(char opt)
+{
+    disHighVolBusCmd cmd{};                           // 默认全零
+    double t0 = MOOSTime();
+    bool ok = false;
+    string name = "CoolingSystem";
+    LOG_F(INFO, "operate %s: opt=%d", name, opt);
+
+    cmd.coolingSystem = getBreakerOption(opt);
+
+    m_udpComm->sendDisSysCmd(cmd);
+
+    ok = m_pm->m_pmCurrentState.bus1State.dcDcFaultWord1 & 0x60;
+
+    MOOSPause(100);
+    while ( !ok ) {
+        if( m_pm->m_pmCurrentState.bus1State.dcDcFaultWord1 & 0x60 )
+        {
+            ok = true;
+            break;
+        }
+        if (!m_udpComm->sendDisSysCmd(cmd)) {
+            LOG_F(ERROR, "operate %s: sendDisSysCmd failed", name);
+            ok =  false;
+            break;
+        }
+        if (MOOSTime() > t0 + 6) {
+            LOG_F(ERROR, "operate %s: timeout", name);
+            ok =  false;
+            break;
+        }
+        MOOSPause(200);
+    }
+    //继电器复位
+    disHighVolBusCmd clr{}; 
+    for (int i = 0; i < 3; ++i)
+    {
+        m_udpComm->sendDisSysCmd(clr);
+        MOOSPause(200);
+    }
+    return ok;
 }
 
 // 高压汇流排断路器操作函数

src/pPowerManger/PowerManger.cpp

@@ -151,16 +151,6 @@ bool PowerManger::Iterate()
   PowerManagerFsm::dispatch(cycleEvent);
   faultLevel = getFaultLevel();
   // 更新系统数据
-  // m_systemData->updateSystemData(m_pmCurrentState.ccustate);
-  // m_systemData->updateSystemData(m_pmCurrentState.bus1State);
-  // m_systemData->updateSystemData(m_pmCurrentState.bus2State);
-  // m_systemData->updateSystemData(m_pmCurrentState.bus3State);
-  // m_systemData->updateSystemData(m_pmCurrentState.bus4State);
-  // m_systemData->device_common_status.ccu_status.lastUpdateTime = m_lowerCommManager->msg_CcuStateFbMsg_Update_time;
-  // m_systemData->device_common_status.dis_hv_c1.lastUpdateTime = m_lowerCommManager->msg_disHighVolBusFbMsg_Update_time;
-  // m_systemData->device_common_status.dis_hv_c2.lastUpdateTime = m_lowerCommManager->msg_disHighAVolBusFbMsg_Update_time;
-  // m_systemData->device_common_status.dis_lv_c1.lastUpdateTime = m_lowerCommManager->msg_disHighBVolBusFbMsg_Update_time;
-  // m_systemData->device_common_status.dis_lv_c2.lastUpdateTime = m_lowerCommManager->msg_disLowBusFbMsg_Update_time;
   m_systemData->updateCommonStatus();
   // m_CcuColCmd.heartbeat
   if (m_ticker++ % 5 == 0)

src/pPowerManger/PowerManger.h

@@ -102,6 +102,8 @@ class PowerManger : public AppCastingMOOSApp
   unsigned int  m_faultNum;
   Driver m_driver; //设备表处理
   unsigned char m_leakage;
+  std::set<unsigned int> m_faultCodes;
+  // std::set<unsigned int> m_disSysFaultCodes;
   //============================Device Option================================
   std::queue<DeviceSwitchCmd> m_deviceCmdQuenue;
   mutable std::mutex m_deviceCmdMutex;

src/pPowerManger/fsm/PowerManagerFsm.hpp

@@ -100,33 +100,30 @@ public:
     void handleWorkCmd();
     void handleDeviceCmd();
     
-    // 非水声仪表设备
+    void handleNoPowerDeviceCmd(int id , int cmd); // 非水声仪表设备
-    void handleNoPowerDeviceCmd(int id , int cmd);
+    void handleHydroacousticDeviceCmd(int id , int cmd); // 水声设备处理
-    // 水声设备处理
+    void handleBowRudderSystemCmd(int id, int cmd); // 艏舵设备处理
-    void handleHydroacousticDeviceCmd(int id , int cmd);
+    void handleActuatorCmd(int id, int cmd); // 启闭机构处理
-    // 艏舵设备处理
+    void handleOpenWaterCmd(int id, int cmd); // 敞水机构处理
-    void handleBowRudderSystemCmd(int id, int cmd);
+    void handleSternRudderCmd(int id, int cmd); // 艉舵设备处理
-    // 启闭机构处理
+    void handlePropulsionMotorCmd(int id, int cmd); // 推进电机处理
-    void handleActuatorCmd(int id, int cmd);
+    void handleBuoyancyAdjustmentCmd(int id, int cmd); // 浮调设备处理
-    // 艉舵设备处理
+    void handleFBSystemCmd(int id, int cmd); // FB设备处理
-    void handleSternRudderCmd(int id, int cmd);
+    void handleSideSonarCmd(int id, int cmd); // 舷侧设备处理
-    // 推进电机处理
+    void handleMastFoldingSystemCmd(int id, int cmd); // 桅杆设备处理
-    void handlePropulsionMotorCmd(int id, int cmd);
-    // 浮调设备处理
-    void handleBuoyancyAdjustmentCmd(int id, int cmd);
-    // FB设备处理
-    void handleFBSystemCmd(int id, int cmd);
-    // 舷侧设备处理
-    void handleSideSonarCmd(int id, int cmd);
-    // 桅杆设备处理
-    void handleMastFoldingSystemCmd(int id, int cmd);
 
     //获取仅仪表设备状态
-    // void getNoPowerDeviceStatus()
+    void getNoPowerDeviceStatus(DriverTable &s);
+    void getBowRudderSystemStatus(DriverTable &s);
+    void getActuatorStatus(DriverTable &s);
+    void getSternRudderStatus(DriverTable &s);
+    void getPropulsionMotorStatus(DriverTable &s);
+    void getBuoyancyAdjustmentStatus(DriverTable &s);
+    void getFBSystemCmdStatus(DriverTable &s);
+    void getSideSonarStatus(DriverTable &s);
+    void getMastFoldingSystem(DriverTable &s);
+    void getOpenWaterStatus(DriverTable &s);
 
-
-
-    bool handPowerProtection(DevCmdExt const &e);
     //故障处理
     void basicNavigationFault(FaultEvent const &e);
     void extendedNavigationFault(FaultEvent const &e);
@@ -260,7 +257,6 @@ class UnderwaterSurvey : public PowerManagerFsm {
         // void react(TaskStart const &) override;
     private:
         // void handlePowerCommands();
-
 };
 // 水面侦查
 class SurfaceSurvey : public PowerManagerFsm {

src/pPowerManger/pmSysvariable.h

@@ -4,7 +4,7 @@
 //宏定义
 #include<string>
 #include <map>
-
+#include <unordered_set> 
 
 #define BREAK_OFF 0xAA
 #define BREAK_ON  0x55
@@ -170,12 +170,14 @@ typedef struct disHighVolBusState
   unsigned char bowHighVoltageDistributionBoxCircuitBreaker;
   // 11:  艉部FT装置4/5断路器
   unsigned char sternFTDevice45CircuitBreaker;
+  
+  // 14:  FB预留断路器
+  unsigned char fbReservedCircuitBreaker;
   // 12:  艉舵开关1断路器
   unsigned char sternRudderSwitch1CircuitBreaker;
   // 13:  艉舵开关2断路器
   unsigned char sternRudderSwitch2CircuitBreaker;
-  // 14:  FB预留断路器
+
-  unsigned char fbReservedCircuitBreaker;
   // 15:  TYZ预留断路器
   unsigned char tyzReservedCircuitBreaker;
   // 16:  预留断路器
@@ -396,12 +398,20 @@ typedef struct disHighVolBusCmd
   unsigned char bowHighVoltageDistributionBoxCircuitBreaker;
   // 11:  艉部FT装置4/5断路器
   unsigned char sternFTDevice45CircuitBreaker;
-  // 12:  艉舵开关1断路器
+  // // 12:  艉舵开关1断路器
-  unsigned char sternRudderSwitch1CircuitBreaker;
+  // unsigned char sternRudderSwitch1CircuitBreaker;
-  // 13:  艉舵开关2断路器
+  // // 13:  艉舵开关2断路器
-  unsigned char sternRudderSwitch2CircuitBreaker;
+  // unsigned char sternRudderSwitch2CircuitBreaker;
-  // 14:  FB预留断路器
+  // // 14:  FB预留断路器
+  // unsigned char fbReservedCircuitBreaker;
+
+  // 12:  FB预留断路器
   unsigned char fbReservedCircuitBreaker;
+  // 13:  艉舵开关1断路器
+  unsigned char sternRudderSwitch1CircuitBreaker;
+  // 14:  艉舵开关2断路器
+  unsigned char sternRudderSwitch2CircuitBreaker;
+  
   // 15:  TYZ预留断路器
   unsigned char tyzReservedCircuitBreaker;
   // 16:  预留断路器
@@ -843,6 +853,43 @@ namespace DeviceId {
   }
 }
 
+// 设备类id存储
+static const std::unordered_set<int> underWterDevices = {
+    DeviceId::singleBeamSonar1,
+    DeviceId::singleBeamSonar2,
+    DeviceId::singleBeamSonar3,
+    DeviceId::singleBeamSonar4,
+    DeviceId::singleBeamSonar5,
+    DeviceId::singleBeamSonar6,
+    DeviceId::singleBeamSonar7,
+    DeviceId::singleBeamSonar8,
+    DeviceId::forwardSonar,
+    DeviceId::USBL,
+    DeviceId::DVL,
+    DeviceId::lowFreqSonar
+};
+static const std::unordered_set<int> noPowerDevices = {
+    DeviceId::depth1,
+    DeviceId::depth2,
+    DeviceId::depth3,
+    DeviceId::overDeep1,
+    DeviceId::overDeep2,
+    DeviceId::ctd1,
+    DeviceId::ctd2,
+    DeviceId::soundVeloc,
+    DeviceId::waterInject,
+    DeviceId::launchControler,
+    DeviceId::photoSensor,
+    DeviceId::electSensor,
+    DeviceId::commMast,
+    DeviceId::ntp,
+    DeviceId::bowThrow,
+    DeviceId::sternThrow,
+    DeviceId::ins,
+    DeviceId::inputWaterSensor1,
+    DeviceId::inputWaterSensor2,
+    DeviceId::inputWaterSensor3
+};
 namespace FaultEnable {
   enum Type : bool {
     BasicNavigationFault_1 = false, // 使能故障

src/pPowerManger/systemData.h

@@ -2,6 +2,7 @@
 #define SYSTEMDATA_H
 #include "pmSysvariable.h"
 #include "sqlit3/SQLite.h"
+#include <set>
 #define DEBUFG
 class SystemData
 {
@@ -161,7 +162,7 @@ public:
     unsigned char powfaultCode;
     unsigned char insfaultCode;
     //配电系统故障码
-    std::vector<unsigned char> distributeFaultCode;
+    std::set<unsigned int> distributeFaultCode;
 
     // MOOSD 状态
     bool moosd_status;

test/disSysTest/udpMsg.py

@@ -62,9 +62,9 @@ class msg_disHighVolBusFbMsg:
         data += struct.pack('<B', self.dcDc5ModuleCircuitBreaker)
         data += struct.pack('<B', self.bowHighVoltageDistributionBoxCircuitBreaker)
         data += struct.pack('<B', self.sternFTDevice45CircuitBreaker)
+        data += struct.pack('<B', self.fbReservedCircuitBreaker)
         data += struct.pack('<B', self.sternRudderSwitch1CircuitBreaker)
         data += struct.pack('<B', self.sternRudderSwitch2CircuitBreaker)
-        data += struct.pack('<B', self.fbReservedCircuitBreaker)
         data += struct.pack('<B', self.tyzReservedCircuitBreaker)
         data += struct.pack('<B', self.reservedCircuitBreaker)
         data += struct.pack('<B', self.dcDcFaultWord1)
@@ -116,9 +116,9 @@ class msg_disHighVolBusFbMsg:
         self.dcDc5ModuleCircuitBreaker = data[offset]; offset +=1
         self.bowHighVoltageDistributionBoxCircuitBreaker = data[offset]; offset +=1
         self.sternFTDevice45CircuitBreaker = data[offset]; offset +=1
+        self.fbReservedCircuitBreaker = data[offset]; offset +=1
         self.sternRudderSwitch1CircuitBreaker = data[offset]; offset +=1
         self.sternRudderSwitch2CircuitBreaker = data[offset]; offset +=1
-        self.fbReservedCircuitBreaker = data[offset]; offset +=1
         self.tyzReservedCircuitBreaker = data[offset]; offset +=1
         self.reservedCircuitBreaker = data[offset]; offset +=1
         self.dcDcFaultWord1 = data[offset]; offset +=1
@@ -506,9 +506,9 @@ class msg_disHVBCmdMsg:
         data += struct.pack('<B', self.dcDc5ModuleCircuitBreaker)
         data += struct.pack('<B', self.bowHighVoltageDistributionBoxCircuitBreaker)
         data += struct.pack('<B', self.sternFTDevice45CircuitBreaker)
+        data += struct.pack('<B', self.fbReservedCircuitBreaker)
         data += struct.pack('<B', self.sternRudderSwitch1CircuitBreaker)
         data += struct.pack('<B', self.sternRudderSwitch2CircuitBreaker)
-        data += struct.pack('<B', self.fbReservedCircuitBreaker)
         data += struct.pack('<B', self.tyzReservedCircuitBreaker)
         data += struct.pack('<B', self.reservedCircuitBreaker)
         data += struct.pack('<B', self.dcdc5Module)
@@ -538,9 +538,9 @@ class msg_disHVBCmdMsg:
         self.dcDc5ModuleCircuitBreaker = data[offset]; offset +=1
         self.bowHighVoltageDistributionBoxCircuitBreaker = data[offset]; offset +=1
         self.sternFTDevice45CircuitBreaker = data[offset]; offset +=1
+        self.fbReservedCircuitBreaker = data[offset]; offset +=1
         self.sternRudderSwitch1CircuitBreaker = data[offset]; offset +=1
         self.sternRudderSwitch2CircuitBreaker = data[offset]; offset +=1
-        self.fbReservedCircuitBreaker = data[offset]; offset +=1
         self.tyzReservedCircuitBreaker = data[offset]; offset +=1
         self.reservedCircuitBreaker = data[offset]; offset +=1
         self.dcdc5Module = data[offset]; offset +=1