zjk/H100PowerManger
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

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

Browse Source
This commit is contained in:
zjk
2025-06-30 18:49:16 +08:00
parent 45fdc455e9
commit 16d4c25c77
11 changed files with 2525 additions and 224 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.vscode/settings.json
Vendored
+2 -1
View File
@@ -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.zip
BIN
View File
Binary file not shown.
src/pPowerManger/LowerCommManager.cpp
+99 -14
View File
@@ -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){
return operateBreakerGeneric<disHighVolBusCmd, char>(
"HV_dcDc5_Breaker",
opt,
[&](disHighVolBusCmd& c){ c.dcdc5Module = getBreakerOption(opt); },
[&](){ return m_pm->m_pmCurrentState.bus1State.dcDcFaultWord1&(0x02); }
);
// bool LowerCommManager::operate_HV_dcDc5_Breaker(char opt){
// return operateBreakerGeneric<disHighVolBusCmd, char>(
// "HV_dcDc5_Breaker",
// opt,
// [&](disHighVolBusCmd& c){ c.dcdc5Module = getBreakerOption(opt); },
// [&](){ 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>(
"HV_coolingSystem_Breaker",
opt,
[&](disHighVolBusCmd& c){ c.coolingSystem = getBreakerOption(opt); },
[&](){ return m_pm->m_pmCurrentState.bus1State.dcDcFaultWord1&(0x20); }
);
// bool LowerCommManager::operate_HV_coolingSystem_Breaker(char opt){
// return operateBreakerGeneric<disHighVolBusCmd, char>(
// "HV_coolingSystem_Breaker",
// opt,
// [&](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
-10
View File
@@ -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
+2
View File
@@ -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.cpp
+761 -160
View File
File diff suppressed because it is too large Load Diff
src/pPowerManger/fsm/PowerManagerFsm.hpp
+22 -26
View File
@@ -99,34 +99,31 @@ public:
void handleModCmd();
void handleWorkCmd();
void handleDeviceCmd();
// 非水声仪表设备
void handleNoPowerDeviceCmd(int id , int cmd);
// 水声设备处理
void handleHydroacousticDeviceCmd(int id , int cmd);
// 艏舵设备处理
void handleBowRudderSystemCmd(int id, int cmd);
// 启闭机构处理
void handleActuatorCmd(int id, int cmd);
// 艉舵设备处理
void handleSternRudderCmd(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 handleNoPowerDeviceCmd(int id , int cmd); // 非水声仪表设备
void handleHydroacousticDeviceCmd(int id , int cmd); // 水声设备处理
void handleBowRudderSystemCmd(int id, int cmd); // 艏舵设备处理
void handleActuatorCmd(int id, int cmd); // 启闭机构处理
void handleOpenWaterCmd(int id, int cmd); // 敞水机构处理
void handleSternRudderCmd(int id, int cmd); // 艉舵设备处理
void handlePropulsionMotorCmd(int id, int cmd); // 推进电机处理
void handleBuoyancyAdjustmentCmd(int id, int cmd); // 浮调设备处理
void handleFBSystemCmd(int id, int cmd); // FB设备处理
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
+55 -8
View File
@@ -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断路器
unsigned char sternRudderSwitch1CircuitBreaker;
// 13: 艉舵开关2断路器
unsigned char sternRudderSwitch2CircuitBreaker;
// 14: FB预留断路器
// // 12: 艉舵开关1断路器
// unsigned char sternRudderSwitch1CircuitBreaker;
// // 13: 艉舵开关2断路器
// unsigned char sternRudderSwitch2CircuitBreaker;
// // 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 -1
View File
@@ -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/PowerManagerApp.py.bck
+1578
View File
File diff suppressed because it is too large Load Diff
test/disSysTest/udpMsg.py
+4 -4
View File
@@ -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