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

0712提交

Browse Source
This commit is contained in:
zjk
2025-07-12 15:39:28 +08:00
parent 4116397f3e
commit 2ef5fd92ac
32 changed files with 3601 additions and 458 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/pPMtest/PMtest.cpp
+2 -2
View File
@@ -186,10 +186,10 @@ bool PMtest::Iterate()
{
//发送DB_TO_PM_MISSION消息
std::cout << "发送uExternComm_setPowerSystem_cmd消息" << std::endl;
string filename = "/home/zjk/project/H100/H100PowerManger/data/testmsg/uPlan_planStop_cmd.json";
string filename = "/home/zjk/project/H100/H100PowerManger/data/testmsg/uPlan_planStop_st.json";
string content;
if(readJsonFromFile(filename, content)) {
Notify("uPlan_planStop_cmd", content);
Notify("uPlan_planStop_st", content);
}
}
src/pPMtest/PMtest.h
+1 -1
View File
@@ -30,7 +30,7 @@ using namespace std;
//决策消息
#define DB_TO_PM_DEEPTH "uDevice_depth_st"
#define DB_TO_PM_MISSION "uPlan_taskStart_cmd"
#define DB_TO_PM_TASKSTOP "uPlan_planStop_cmd"
#define DB_TO_PM_TASKSTOP "uPlan_planStop_st"
//外部通信消息
#define DB_TO_PM_SETOPCONDITION "uExternComm_setOpCondition_cmd"
src/pPowerManger/PowerManger.cpp
+1
View File
@@ -46,6 +46,7 @@ PowerManger::PowerManger()
m_MSDriverCmd = {};
m_subDisSysCmd = {};
m_subDisSysState = {};
m_subDisSysFault = {};
//初始化控制指令
initControlCmd();
src/pPowerManger/PowerManger.h
+2
View File
@@ -89,6 +89,7 @@ class PowerManger : public AppCastingMOOSApp
disSysBreakerList m_currentDisBreakerState; //配电断路器状态
DriverTable m_currentDriverState;
DriverTable m_subDisSysState;
DriverTable m_subDisSysFault;
int m_depth;
MissionState m_missionState;
//===========================STATE=========================================
@@ -97,6 +98,7 @@ class PowerManger : public AppCastingMOOSApp
unsigned char powerState;
unsigned char faultLevel;
vector<unsigned int> faultCode;
unsigned int soc;
unsigned int sustainableTime;
unsigned int m_faultNum;
src/pPowerManger/UpperCommManager.cpp
+10 -8
View File
@@ -24,6 +24,7 @@ UpperCommManager::UpperCommManager(PowerManger* pm) :
m_msCmd(),
m_driver() {
}
DriverTable uDevice_secdistStateMsg::fault_state = {0};
bool UpperCommManager::processUpperMsg(const string& key, const string& strJson) {
if (key == m_sysStateMsg.key)
@@ -63,8 +64,7 @@ bool UpperCommManager::processUpperMsg(const string& key, const string& strJson)
m_secdistStateMsg_fb_debug = std::make_pair(key, strJson);
#endif
m_secdistStateMsg.parseMsg(strJson, m_pm->m_subDisSysState);
//debug 接受到次级配电状态，自动更新到设备状态
//TODO:自动更新到设备状态
//TODO:自动更新到设备状态 调试
//m_pm->m_subDisSysState = m_pm->m_deviceState;
return true;
}
@@ -107,6 +107,7 @@ bool UpperCommManager::processUpperMsg(const string& key, const string& strJson)
m_pm->m_msCmd.workCMD = m_setOpConditionMsg.getSetOpCondition(key, strJson);
MasterCommandEvent event;
PowerManagerFsm::dispatch(event);
return true;
}
else if(key == m_setPMSysMsg.key)
{
@@ -161,13 +162,12 @@ void UpperCommManager::processPeriodicTasks() {
buildSystemReport();
std::string strJson;
//发送当前设备状态
//TODO: 修改设备状态
m_pm->m_currentDriverState.ins = 1;
strJson = m_equStateMsg.buildMsg(m_pm->m_currentDriverState);
m_pm->sendNotification(m_equStateMsg.key, strJson);
//发送次级配电指令 修改后的代码
// strJson = getSubDisCmd(m_pm->m_subDisSysCmd);
// TODO : 修改指令逻辑，当状态一致时发送保持指令
// strJson = m_secdistStateMsg.buildMsg(m_pm->m_subDisSysCmd);
strJson = m_secdistStateMsg.buildMsg(m_pm->m_subDisSysCmd,m_pm->m_subDisSysState,2);
strJson = m_secdistStateMsg.buildMsg(m_pm->m_subDisSysCmd,m_pm->m_subDisSysState, m_pm->m_subDisSysFault);
m_pm->sendNotification(m_secdistStateMsg.key_cmd, strJson);
//发送仪表电状态
m_pm->sendNotification(m_equInsStateMsg.key,m_equInsStateMsg.buildMsg(m_pm->m_subDisSysState));
@@ -175,6 +175,8 @@ void UpperCommManager::processPeriodicTasks() {
m_equInsStateMsg_fb_degub = std::make_pair(m_equInsStateMsg.key,m_equInsStateMsg.buildMsg(m_pm->m_subDisSysState));
m_secdistStateMsg_cmd_debug = std::make_pair(m_secdistStateMsg.key_cmd,strJson);
m_equStateMsg_fb_debug = std::make_pair(m_equStateMsg.key_cmd,m_equStateMsg.buildMsg(m_pm->m_currentDriverState));
strJson = m_pm->m_driver.saveDriverTableToJson(&m_pm->m_subDisSysFault);
m_pm->sendNotification("uPower_subDisFault_st",strJson);
#endif
}
@@ -190,14 +192,14 @@ void UpperCommManager::buildSystemReport() {
m_pmStateMsg.state.m_leakage = m_pm->m_leakage;
m_pmStateMsg.state.m_faultNum = m_pm->m_faultNum;
m_pm->sendNotification(m_pmStateMsg.key, m_pmStateMsg.buildMsg());
m_pm->sendNotification(m_pmStateMsg.key, m_pmStateMsg.buildMsg(*(m_pm->m_systemData)));
m_pm->sendNotification(m_fcsStateMsg.key, m_fcsStateMsg.buildMsg(*(m_pm->m_systemData)));
m_pm->sendNotification(m_liBatStateMsg.key, m_liBatStateMsg.buildMsg(*(m_pm->m_systemData)));
m_pm->sendNotification(m_disSysStateMsg.key, m_disSysStateMsg.buildMsg(*(m_pm->m_systemData),m_pm->m_currentDisBreakerState));
m_pm->sendNotification(m_currentStateMsg.key, m_currentStateMsg.buildMsg(m_pm->m_pmCurrentState));
#ifdef _DEBUG
m_pmStateMsg_fb_debug = std::make_pair(m_pmStateMsg.key, m_pmStateMsg.buildMsg());
m_pmStateMsg_fb_debug = std::make_pair(m_pmStateMsg.key, m_pmStateMsg.buildMsg(*(m_pm->m_systemData)));
m_fcsStateMsg_st_debug = std::make_pair(m_fcsStateMsg.key, m_fcsStateMsg.buildMsg(*(m_pm->m_systemData)));
m_liBatStateMsg_st_debug = std::make_pair(m_liBatStateMsg.key, m_liBatStateMsg.buildMsg(*(m_pm->m_systemData)));
m_disSysStateMsg_st_debug = std::make_pair(m_disSysStateMsg.key, m_disSysStateMsg.buildMsg(*(m_pm->m_systemData),m_pm->m_currentDisBreakerState));
src/pPowerManger/UpperCommManager.h
+1 -1
View File
@@ -37,7 +37,7 @@ using namespace std;
//决策消息
#define DB_TO_PM_DEEPTH "uDevice_depth_st"
// #define DB_TO_PM_MISSION "uPlan_taskStart_cmd"
#define DB_TO_PM_TASKSTOP "uPlan_planStop_cmd"
#define DB_TO_PM_TASKSTOP "uPlan_planStop_st"
//外部通信消息
#define DB_TO_PM_SETOPCONDITION "uExternComm_setOpCondition_cmd"
src/pPowerManger/fsm/CruiseMode.cpp
+44 -5
View File
@@ -3,11 +3,50 @@
void CruiseMode::entry() {
PowerManagerFsm::entry();
pm->workCondition = WorkCondition::CHANING;
pm->m_driver.getCRUISE_MODEable(&pm->m_subDisSysCmd);
//依次接入执行机构动力电设备
//TODO:接入执行机构动力电设备
//TODO：解除非必要动力电设备
// pm->m_driver.getCRUISE_MODEable(&pm->m_subDisSysCmd);
handleCuriseWork();
//解除非必要动力电设备
//1 解除启闭机构
unsigned char cmd = 0;
if(pm->m_currentDriverState.bowCover1 != 0 ||
pm->m_currentDriverState.bowCover2 != 0 ||
pm->m_currentDriverState.bowCover3 != 0 ||
pm->m_currentDriverState.bowCover4 != 0 ||
pm->m_currentDriverState.bowCover5 != 0 ||
pm->m_currentDriverState.bowCover6 != 0)
{
pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(0);
MOOSPause(3);
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();
}
//2 解除敞水机构
if(pm->m_currentDriverState.bowWaterInet1 != 0 ||
pm->m_currentDriverState.bowWaterInet2 != 0 ||
pm->m_currentDriverState.bowWaterInet3 != 0 ||
pm->m_currentDriverState.bowWaterInet4 != 0 ||
pm->m_currentDriverState.bowWaterInet5 != 0 ||
pm->m_currentDriverState.bowWaterInet6 != 0 ||
pm->m_currentDriverState.bowWaterInet7 != 0 ||
pm->m_currentDriverState.bowWaterInet8 != 0)
{
pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(0);
MOOSPause(3);
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->workCondition = WorkCondition::CRUISE_MODE;
LOG_F(INFO, "进入巡航模式");
}
src/pPowerManger/fsm/DJMode.cpp
+56
View File
@@ -10,6 +10,62 @@ void DJMode::react(MasterCommandEvent const &) {
void DJMode::entry() {
PowerManagerFsm::entry();
pm->workCondition = WorkCondition::CHANING;
handleCuriseWork();
//1 启动载荷装置
pm->m_subDisSysCmd.launchControler = 1;
//启动启闭机构和电缸
unsigned char cmd = 1;
//2.1 启闭机构
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();
MOOSPause(10);
pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(1);
//2.2 敞水电缸
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();
MOOSPause(10);
pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(1);
pm->workCondition = WorkCondition::DJ_MODE;
LOG_F(INFO, "进入DJ模式");
}
void DJMode::exit() {
PowerManagerFsm::exit();
//1 解除启闭机构
unsigned char cmd = 0;
pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(0);
MOOSPause(3);
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();
//2 解除敞水机构
pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(0);
MOOSPause(3);
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();
}
src/pPowerManger/fsm/FaultState.cpp
+7 -6
View File
@@ -3,17 +3,18 @@
// FaultState实现
void FaultState::entry() {
PowerManagerFsm::entry();
// PowerManagerFsm::pm->printState("FaultState", "进入故障状态");
if(pm->m_missionState.type == TaskType::THROW_LOAD)
{
LOG_F(INFO, "检测到抛载，所有水声探测设备");
}
else
{
// LOG_F(INFO, "检测到抛载，所有水声设备关闭");
LOG_F(INFO, "检测到抛载");
}
}
void FaultState::react(FaultEvent const & e) {
LOG_F(INFO, "FaultState 处理故障事件");
}
void FaultState::react(MasterCommandEvent const &) {
PowerManagerFsm::handelPowerCmd();
PowerManagerFsm::handleModCmd();
PowerManagerFsm::handleWorkCmd();
}
src/pPowerManger/fsm/FloatAdjust.cpp
+1 -6
View File
@@ -10,12 +10,7 @@ void FloatAdjust::react(MasterCommandEvent const &) {
void FloatAdjust::entry() {
PowerManagerFsm::entry();
pm->workCondition = WorkCondition::CHANING;
//与巡航模式相同
pm->m_driver.getCRUISE_MODEable(&pm->m_subDisSysCmd);
//依次接入执行机构动力电设备
//TODO:接入执行机构动力电设备
//TODO：解除非必要动力电设备
handleCuriseWork();
pm->workCondition = WorkCondition::BUOYANCY_ADJUST;
LOG_F(INFO, "进入浮调模式");
}
src/pPowerManger/fsm/FloatDown.cpp
+2 -6
View File
@@ -2,12 +2,8 @@
void FloatDown::entry() {
PowerManagerFsm::entry();
//与巡航模式相同
pm->m_driver.getCRUISE_MODEable(&pm->m_subDisSysCmd);
//依次接入执行机构动力电设备
//TODO:接入执行机构动力电设备
//TODO：解除非必要动力电设备
pm->workCondition = WorkCondition::CHANING;
handleCuriseWork();
pm->workCondition = WorkCondition::ASCEND_DESCEND;
LOG_F(INFO, "进入上浮下潜模式");
}
src/pPowerManger/fsm/HighSpeedMode.cpp
+2
View File
@@ -2,6 +2,8 @@
void HighSpeedMode::entry() {
PowerManagerFsm::entry();
pm->workCondition = WorkCondition::CHANING;
handleCuriseWork();
pm->workCondition = WorkCondition::HIGH_SPEED_MODE;
LOG_F(INFO, "进入高速模式");
}
src/pPowerManger/fsm/PowerManagerFsm.cpp
+917 -70
View File
File diff suppressed because it is too large Load Diff
src/pPowerManger/fsm/PowerManagerFsm.hpp
+15 -34
View File
@@ -99,6 +99,8 @@ public:
void handleModCmd();
void handleWorkCmd();
void handleDeviceCmd();
void handleCuriseWork();
void handleNoPowerDeviceCmd(int id , int cmd); // 非水声仪表设备
void handleHydroacousticDeviceCmd(int id , int cmd); // 水声设备处理
@@ -134,6 +136,16 @@ public:
void emergencySystemFault(FaultEvent const &e);
void sensorSystemFault(FaultEvent const &e);
void basicNavigationFault();
void extendedNavigationFault();
void obstacleAvoidanceFault();
void buoyancyControlFault();
void communicationSystemFault();
void payloadSystemFault();
void emergencySystemFault();
void sensorSystemFault();
// void handleFaultOfDisSys();
// 状态机生命周期方法
virtual void entry() {
auto now = std::chrono::system_clock::now();
@@ -183,8 +195,6 @@ public:
void react(StandbyEvent const &) override;
void react(DevCmdExt const &e) override;
void react(MasterCommandEvent const &) override;
private:
void handlePowerOn();
};
// 岸基备航
@@ -193,49 +203,35 @@ class ShoreBasedReady : public PowerManagerFsm {
void entry() override;
void react(MasterCommandEvent const &) override;
private:
// void handlePowerCommands();
// void handlePowerOn();
};
// 水中备航
class WaterBasedReady : public PowerManagerFsm {
public:
void entry() override;
void react(MasterCommandEvent const &) override;
// void react(DeviceCmd const &) override;
// void react(TaskStart const &) override;
void react(DevCmdExt const &e) override;
private:
// void handlePowerCommands();
// void handlePowerOn();
};
// 巡航模式
class CruiseMode : public PowerManagerFsm {
public:
void entry() override;
void react(MasterCommandEvent const &) override;
// void react(DeviceCmd const &) override;
// void react(TaskStart const &) override;
private:
// void handlePowerCommands();
};
// 上浮下潜
class FloatDown : public PowerManagerFsm {
public:
void entry() override;
void react(MasterCommandEvent const &) override;
// void react(DeviceCmd const &) override;
// void react(TaskStart const &) override;
private:
// void handlePowerCommands();
};
// 高速模式
class HighSpeedMode : public PowerManagerFsm {
public:
void entry() override;
void react(MasterCommandEvent const &) override;
// void react(DeviceCmd const &) override;
// void react(TaskStart const &) override;
private:
// void handlePowerCommands();
};
// 浮调模式
@@ -243,58 +239,43 @@ class FloatAdjust : public PowerManagerFsm {
public:
void entry() override;
void react(MasterCommandEvent const &) override;
// void react(DeviceCmd const &) override;
// void react(TaskStart const &) override;
private:
// void handlePowerCommands();
};
// 水下侦查
class UnderwaterSurvey : public PowerManagerFsm {
public:
void entry() override;
void react(MasterCommandEvent const &) override;
// void react(DeviceCmd const &) override;
// void react(TaskStart const &) override;
private:
// void handlePowerCommands();
};
// 水面侦查
class SurfaceSurvey : public PowerManagerFsm {
public:
void entry() override;
void react(MasterCommandEvent const &) override;
// void react(DeviceCmd const &) override;
// void react(TaskStart const &) override;
private:
// void handlePowerCommands();
};
// DJ模式
class DJMode : public PowerManagerFsm {
public:
void entry() override;
void react(MasterCommandEvent const &) override;
// void react(DeviceCmd const &) override;
// void react(TaskStart const &) override;
void exit() override;
private:
// void handlePowerCommands();
};
// 遥控工况
class RemoteControl : public PowerManagerFsm {
public:
void entry() override;
void react(MasterCommandEvent const &) override;
// void react(DeviceCmd const &) override;
// void react(TaskStart const &) override;
private:
// void handlePowerCommands();
};
class FaultState : public PowerManagerFsm {
public:
void entry() override;
void react(FaultEvent const &);
void react(MasterCommandEvent const &) override;
};
src/pPowerManger/fsm/ShoreBasedReady.cpp
+66 -4
View File
@@ -7,15 +7,77 @@ void ShoreBasedReady::react(MasterCommandEvent const &) {
PowerManagerFsm::handleWorkCmd();
}
void ShoreBasedReady::entry() {
PowerManagerFsm::entry();
pm->m_driver.getSHORE_BASED_READYable(&pm->m_subDisSysCmd);
// pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Start;
pm->workCondition = WorkCondition::CHANING;
//依次接入执行机构动力电设备
//TODO:接入执行机构动力电设备
//1. 开启动力电
//1.1 动力锂电池上电
pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Start;
//1.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(10);
pm->m_lowerCommManager->operate_HV_bowHighVoltageBox_Breaker(1);
MOOSPause(3);
//2. 启动动力电设备
unsigned char cmd = 1;
//2.1 启闭机构
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();
MOOSPause(3);
pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(1);
//2.2 敞水电缸
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();
MOOSPause(3);
pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(1);
//2.3 艏舵
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.4 桅杆舵机
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.5 艏部浮调机构
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();
MOOSPause(3);
pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(1);
//2.6 艉部浮调机构
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.7 推进电机
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);
pm->workCondition = WorkCondition::SHORE_BASED_READY;
LOG_F(INFO, "进入岸基备航状态");
}
src/pPowerManger/fsm/StandbyState.cpp
-41
View File
@@ -3,14 +3,8 @@
// StandbyState实现
void StandbyState::entry() {
PowerManagerFsm::entry();
// pm->m_CcuColCmd.fcuCmd = FcuCommand::Stop;
// pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Stop;
// 启动流程 仪表电一直设置为启动
// pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Start;
// 启动设备
pm->workCondition = WorkCondition::STANDBY;
pm->m_driver.getSTANDBYTable(&pm->m_subDisSysCmd);
LOG_F(INFO, "WorkCondition %d", pm->workCondition);
}
void StandbyState::react(StandbyEvent const &) {
@@ -25,40 +19,5 @@ void StandbyState::react(MasterCommandEvent const &) {
void StandbyState::react(DevCmdExt const &e)
{
LOG_F(INFO, "DevCmdExt 执行");
cout << "DevCmdExt 执行" << (int)e.id << (int)e.cmd << endl;
PowerManagerFsm::react(e);
cout << (int) pm->m_subDisSysCmd.commMast << endl;
cout << (int) pm->m_subDisSysCmd.mastLiftingServo << endl;
cout << "DevCmdExt 执行完成" << (int)e.id << (int)e.cmd << endl;
//TODO：屏蔽上电保护
//正常模式下，不允许执行DevCmdExt
// if (pm->currentMod == RunMode::Normal )
// {
// LOG_F(INFO, "正常模式下,不允许执行DevCmdExt");
// }
// else if (pm->currentMod == RunMode::Test)
// {
// if(handPowerProtection(e))
// PowerManagerFsm::react(e);
// else
// LOG_F(INFO, "DevCmdExt 执行失败");
// }
// else if (pm->currentMod == RunMode::Debug)
// {
// LOG_F(INFO, "调试模式下,直接执行DevCmdExt");
// PowerManagerFsm::react(e);
// }
// else
// {
// LOG_F(INFO, "当前模式未知");
// }
}
void StandbyState::handlePowerOn() {
LOG_F(INFO,"动力电上电");
//TODO: 电源上电 更新协议
// PowerManagerFsm::pm->m_CcuColCmd.powerBatCmd = 0x10;
// if(PowerManagerFsm::pm->m_pmCurrentState.ccustate.fcuState == 0x04)
// PowerManagerFsm::pm->m_CcuColCmd.fcuCmd = 0x02;
}
src/pPowerManger/fsm/SurfaceSurvey.cpp
+2
View File
@@ -9,6 +9,8 @@ void SurfaceSurvey::react(MasterCommandEvent const &) {
void SurfaceSurvey::entry() {
PowerManagerFsm::entry();
pm->workCondition = WorkCondition::CHANING;
handleCuriseWork();
pm->workCondition = WorkCondition::SURFACE_RECON;
LOG_F(INFO, "进入水面侦查模式");
}
src/pPowerManger/fsm/UnderwaterSurvey.cpp
+2
View File
@@ -2,6 +2,8 @@
void UnderwaterSurvey::entry() {
PowerManagerFsm::entry();
pm->workCondition = WorkCondition::CHANING;
handleCuriseWork();
pm->workCondition = WorkCondition::UNDERWATER_RECON;
LOG_F(INFO, "进入水下侦查模式");
}
src/pPowerManger/fsm/WaterBasedReady.cpp
+35 -4
View File
@@ -4,10 +4,10 @@
void WaterBasedReady::entry() {
PowerManagerFsm::entry();
pm->workCondition = WorkCondition::CHANING;
pm->m_driver.getWATER_BASED_READYable(&pm->m_subDisSysCmd);
// pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Start;
//依次接入执行机构动力电设备
//TODO:接入执行机构动力电设备
PowerManagerFsm::handleCuriseWork();
//启动水声设备
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->workCondition = WorkCondition::WATER_BASED_READY;
LOG_F(INFO, "进入水中备航状态");
}
@@ -18,3 +18,34 @@ void WaterBasedReady::react(MasterCommandEvent const &) {
PowerManagerFsm::handleModCmd();
PowerManagerFsm::handleWorkCmd();
}
void WaterBasedReady::react(DevCmdExt const &e)
{
int id = e.id;
int cmd = e.cmd;
if(underWterDevices.find(id) != underWterDevices.end())
{
if (pm->currentMod != PMMode::Debug)
{
if(pm->m_depth < 10)
{
LOG_F(INFO, "水下设备操作上电保护 %d ", id);
return;
}
}
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, (DeviceId::Type)id, (unsigned char)cmd);
LOG_F(INFO, "水声设备操作: %s,%d, %d", DeviceId::toString((DeviceId::Type)id), id, cmd);
}
else
{
if(pm->currentMod == PMMode::Normal)
{
LOG_F(INFO,"正常模式:操作禁止");
return;
}
else
{
PowerManagerFsm::react(e);
}
}
}
src/pPowerManger/httpserver/httpserver.cpp
+27
View File
@@ -579,6 +579,33 @@ void HttpServer::pushSystemData() {
j["power_bus_status"]["reservedCurrent3"] = Json::Value(m_systemData->power_bus_status.reservedCurrent3); // 预留电流3
j["power_bus_status"]["coolWaterPressure"] = Json::Value(m_systemData->power_bus_status.coolWaterPressure); // 冷却水压力
// union {
// unsigned char lowFaultWord;
// struct {
// bool overTemperatureShutdown : 1; // Bit0: DC/DC模块过温关机
// bool outputOverCurrent : 1; // Bit1: 输出过流
// bool outputOverVoltage : 1; // Bit2: 输出过压
// bool outputUnderVoltage : 1; // Bit3: 输出欠压
// bool inputOverVoltage : 1; // Bit4: 输入过压
// bool inputUnderVoltage : 1; // Bit5: 输入欠压
// bool outputShortCircuit : 1; // Bit6: 输出短路保护
// bool internalFault : 1; // Bit7: 内部故障
// };
// };
// // DC/DC5故障字高
// union {
// unsigned char highFaultWord;
// struct {
// bool communicationFault : 1; // Bit0: 通讯故障报警
// bool workingStatus : 1; // Bit1: 工作状态(1:启动, 0:关闭)
// bool coolingPump1Fault : 1; // Bit2: 冷却泵1故障
// bool coolingPump2Fault : 1; // Bit3: 冷却泵2故障
// bool coolingWaterLeak : 1; // Bit4: 冷却水泄露
// bool coolingPump1Running : 1; // Bit5: 冷却泵1运行状态
// bool coolingPump2Running : 1; // Bit6: 冷却泵2运行状态
// bool reserved : 1; // Bit7: 保留位
// };
// };
j["power_bus_status"]["dcdcStatus1"] = Json::Value(m_systemData->m_dcdcStatus.lowFaultWord);
j["power_bus_status"]["dcdcStatus2"] = Json::Value(m_systemData->m_dcdcStatus.highFaultWord);
src/pPowerManger/httpserver/index.h
+80 -26
View File
@@ -808,7 +808,27 @@ body {
<div class="panel-title">配电系统状态</div>
<div class="battery-cards">
<div class="battery-card">
<h3>动力母线状态</h3>
<h3>DC/DC模块状态</h3>
<div class="battery-data">
<div class="data-row"><span>过温关机:</span><span id="dcdc-over-temp">-</span></div>
<div class="data-row"><span>输出过流:</span><span id="dcdc-over-current">-</span></div>
<div class="data-row"><span>输出过压:</span><span id="dcdc-over-voltage">-</span></div>
<div class="data-row"><span>输出欠压:</span><span id="dcdc-under-voltage">-</span></div>
<div class="data-row"><span>输入过压:</span><span id="dcdc-input-over-voltage">-</span></div>
<div class="data-row"><span>输入欠压:</span><span id="dcdc-input-under-voltage">-</span></div>
<div class="data-row"><span>输出短路:</span><span id="dcdc-short-circuit">-</span></div>
<div class="data-row"><span>内部故障:</span><span id="dcdc-internal-fault">-</span></div>
<div class="data-row"><span>通讯故障:</span><span id="dcdc-comm-fault">-</span></div>
<div class="data-row"><span>工作状态:</span><span id="dcdc-working-status">-</span></div>
<div class="data-row"><span>冷却泵1故障:</span><span id="dcdc-cooling-pump1-fault">-</span></div>
<div class="data-row"><span>冷却泵2故障:</span><span id="dcdc-cooling-pump2-fault">-</span></div>
<div class="data-row"><span>冷却水泄露:</span><span id="dcdc-cooling-water-leak">-</span></div>
<div class="data-row"><span>冷却泵1运行:</span><span id="dcdc-cooling-pump1-running">-</span></div>
<div class="data-row"><span>冷却泵2运行:</span><span id="dcdc-cooling-pump2-running">-</span></div>
</div>
</div>
<div class="battery-card">
<h3>艉高压状态</h3>
<div class="battery-data">
<div class="data-row"><span>燃料电池断路器:</span><span id="power-bus-fuel-cell-circuit-breaker" class="relay-status">-</span></div>
<div class="data-row"><span>动力锂电池断路器:</span><span id="power-bus-power-lithium-circuit-breaker" class="relay-status">-</span></div>
@@ -850,7 +870,7 @@ body {
</div>
<div class="battery-card">
<h3>仪表主母线状态</h3>
<h3>艉低压状态</h3>
<div class="battery-data">
<div class="data-row"><span>锂电池组(仪表)断路器:</span><span id="instrument-main-bus-lithium-meter-circuit-breaker" class="relay-status">-</span></div>
<div class="data-row"><span>艏部低压配电箱断路器:</span><span id="instrument-main-bus-bow-low-voltage-circuit-breaker" class="relay-status">-</span></div>
@@ -878,7 +898,30 @@ body {
</div>
<div class="battery-card">
<h3>仪表母线状态</h3>
<h3>艏高压状态</h3>·
<div class="battery-data">
<div class="data-row"><span>艏部FT装置1/2/3断路器:</span><span id="power-a-bus-bow-ft-circuit-breaker" class="relay-status">-</span></div>
<div class="data-row"><span>启闭机构断路器:</span><span id="power-a-bus-actuator-circuit-breaker" class="relay-status">-</span></div>
<div class="data-row"><span>桅杆舵机控制箱断路器:</span><span id="power-a-bus-mast-steering-circuit-breaker" class="relay-status">-</span></div>
<div class="data-row"><span>XCZ断路器:</span><span id="power-a-bus-xcz-circuit-breaker" class="relay-status">-</span></div>
<div class="data-row"><span>艏舵控制箱断路器:</span><span id="power-a-bus-bow-rudder-circuit-breaker" class="relay-status">-</span></div>
<div class="data-row"><span>敞水盖启动电缸断路器:</span><span id="power-a-bus-water-cover-circuit-breaker" class="relay-status">-</span></div>
<div class="data-row"><span>仪表电源状态:</span><span id="power-a-bus-power-status">-</span></div>
<div class="data-row"><span>应急电源状态:</span><span id="power-a-bus-emergency-power-status">-</span></div>
<div class="data-row"><span>浸水报警:</span><span id="power-a-bus-water-alarm">-</span></div>
<div class="data-row"><span>汇流排B电压:</span><span id="power-a-bus-b-voltage">-</span>V</div>
<div class="data-row"><span>汇流排B电流:</span><span id="power-a-bus-b-current">-</span>A</div>
<div class="data-row"><span>艏部FT装置1/2/3电流:</span><span id="power-a-bus-bow-ft-current">-</span>A</div>
<div class="data-row"><span>启闭机构电流:</span><span id="power-a-bus-actuator-current">-</span>A</div>
<div class="data-row"><span>桅杆舵机控制箱电流:</span><span id="power-a-bus-mast-steering-current">-</span>A</div>
<div class="data-row"><span>XCZ电流:</span><span id="power-a-bus-xcz-current">-</span>A</div>
<div class="data-row"><span>艏舵控制箱电流:</span><span id="power-a-bus-bow-rudder-current">-</span>A</div>
<div class="data-row"><span>敞水盖启动电缸电流:</span><span id="power-a-bus-water-cover-current">-</span>A</div>
</div>
</div>
<div class="battery-card">
<h3>艏低压状态</h3>
<div class="battery-data">
<div class="data-row"><span>UNIT1断路器:</span><span id="instrument-bus-unit1-circuit-breaker" class="relay-status">-</span></div>
<div class="data-row"><span>UNIT2断路器:</span><span id="instrument-bus-unit2-circuit-breaker" class="relay-status">-</span></div>
@@ -902,29 +945,7 @@ body {
<div class="data-row"><span>应急1汇流排电压:</span><span id="instrument-bus-emergency1-voltage">-</span>V</div>
</div>
</div>
<div class="battery-card">
<h3>动力母线A状态</h3>
<div class="battery-data">
<div class="data-row"><span>艏部FT装置1/2/3断路器:</span><span id="power-a-bus-bow-ft-circuit-breaker" class="relay-status">-</span></div>
<div class="data-row"><span>启闭机构断路器:</span><span id="power-a-bus-actuator-circuit-breaker" class="relay-status">-</span></div>
<div class="data-row"><span>桅杆舵机控制箱断路器:</span><span id="power-a-bus-mast-steering-circuit-breaker" class="relay-status">-</span></div>
<div class="data-row"><span>XCZ断路器:</span><span id="power-a-bus-xcz-circuit-breaker" class="relay-status">-</span></div>
<div class="data-row"><span>艏舵控制箱断路器:</span><span id="power-a-bus-bow-rudder-circuit-breaker" class="relay-status">-</span></div>
<div class="data-row"><span>敞水盖启动电缸断路器:</span><span id="power-a-bus-water-cover-circuit-breaker" class="relay-status">-</span></div>
<div class="data-row"><span>仪表电源状态:</span><span id="power-a-bus-power-status">-</span></div>
<div class="data-row"><span>应急电源状态:</span><span id="power-a-bus-emergency-power-status">-</span></div>
<div class="data-row"><span>浸水报警:</span><span id="power-a-bus-water-alarm">-</span></div>
<div class="data-row"><span>汇流排B电压:</span><span id="power-a-bus-b-voltage">-</span>V</div>
<div class="data-row"><span>汇流排B电流:</span><span id="power-a-bus-b-current">-</span>A</div>
<div class="data-row"><span>艏部FT装置1/2/3电流:</span><span id="power-a-bus-bow-ft-current">-</span>A</div>
<div class="data-row"><span>启闭机构电流:</span><span id="power-a-bus-actuator-current">-</span>A</div>
<div class="data-row"><span>桅杆舵机控制箱电流:</span><span id="power-a-bus-mast-steering-current">-</span>A</div>
<div class="data-row"><span>XCZ电流:</span><span id="power-a-bus-xcz-current">-</span>A</div>
<div class="data-row"><span>艏舵控制箱电流:</span><span id="power-a-bus-bow-rudder-current">-</span>A</div>
<div class="data-row"><span>敞水盖启动电缸电流:</span><span id="power-a-bus-water-cover-current">-</span>A</div>
</div>
</div>
</div>
</div>
@@ -1236,6 +1257,31 @@ body {
updateCcuControlData(data);
updateAlarmState(data);
updateTime();
// 更新DC/DC模块状态
if (data.power_bus_status && data.power_bus_status.dcdcStatus1 !== undefined && data.power_bus_status.dcdcStatus2 !== undefined) {
const lowFaultWord = data.power_bus_status.dcdcStatus1;
const highFaultWord = data.power_bus_status.dcdcStatus2;
// 解析lowFaultWord (dcdcStatus1)
updateStatusWithDebounce('dcdc-over-temp', (lowFaultWord & 0x01) ? 1 : 0, getBooleanStatusText);
updateStatusWithDebounce('dcdc-over-current', (lowFaultWord & 0x02) ? 1 : 0, getBooleanStatusText);
updateStatusWithDebounce('dcdc-over-voltage', (lowFaultWord & 0x04) ? 1 : 0, getBooleanStatusText);
updateStatusWithDebounce('dcdc-under-voltage', (lowFaultWord & 0x08) ? 1 : 0, getBooleanStatusText);
updateStatusWithDebounce('dcdc-input-over-voltage', (lowFaultWord & 0x10) ? 1 : 0, getBooleanStatusText);
updateStatusWithDebounce('dcdc-input-under-voltage', (lowFaultWord & 0x20) ? 1 : 0, getBooleanStatusText);
updateStatusWithDebounce('dcdc-short-circuit', (lowFaultWord & 0x40) ? 1 : 0, getBooleanStatusText);
updateStatusWithDebounce('dcdc-internal-fault', (lowFaultWord & 0x80) ? 1 : 0, getBooleanStatusText);
// 解析highFaultWord (dcdcStatus2)
updateStatusWithDebounce('dcdc-comm-fault', (highFaultWord & 0x01) ? 1 : 0, getBooleanStatusText);
updateStatusWithDebounce('dcdc-working-status', (highFaultWord & 0x02) ? 1 : 0, getWorkingStatusText);
updateStatusWithDebounce('dcdc-cooling-pump1-fault', (highFaultWord & 0x04) ? 1 : 0, getBooleanStatusText);
updateStatusWithDebounce('dcdc-cooling-pump2-fault', (highFaultWord & 0x08) ? 1 : 0, getBooleanStatusText);
updateStatusWithDebounce('dcdc-cooling-water-leak', (highFaultWord & 0x10) ? 1 : 0, getBooleanStatusText);
updateStatusWithDebounce('dcdc-cooling-pump1-running', (highFaultWord & 0x20) ? 1 : 0, getBooleanStatusText);
updateStatusWithDebounce('dcdc-cooling-pump2-running', (highFaultWord & 0x40) ? 1 : 0, getBooleanStatusText);
}
} catch (e) {
console.error('数据处理错误:', e, '原始数据:', event.data);
}
@@ -1366,6 +1412,14 @@ body {
return statuses[status] || '未知';
}
function getBooleanStatusText(status) {
return status ? '是' : '否';
}
function getWorkingStatusText(status) {
return status ? '启动' : '关闭';
}
function getOnlineStateText(state) {
const states = {
0: "未上线",
src/pPowerManger/pmSysvariable.h
+14 -6
View File
@@ -654,9 +654,12 @@ namespace TaskType {
RECYCLE = 3, // 回收
HOVER = 4, // 悬停
SAT_COMM = 5, // 上浮通信
SAT_CALIBRATE = 6, // 上浮校准
TOUR = 7, // 游曳
SAIL = 8 // 航行
SAT_CALIBRATE = 6, // 上浮校准
TOUR = 7, // 游曳
SAIL = 8, // 航行
EMER_SAT_COMM = 9, //紧急上浮通信
EMER_SAIL = 10 //紧急航行
};
inline const std::string toString(Type type) {
@@ -668,7 +671,9 @@ namespace TaskType {
{SAT_COMM, "satComm"},
{SAT_CALIBRATE, "satCalibrate"},
{TOUR, "tour"},
{SAIL, "sail"}
{SAIL, "sail"},
{EMER_SAT_COMM,"emerSatComm"},
{EMER_SAIL,"emerSail"}
};
auto it = names.find(type);
return it != names.end() ? it->second : "unknown";
@@ -680,9 +685,12 @@ namespace TaskType {
{"recycle", RECYCLE},
{"hover", HOVER},
{"satComm", SAT_COMM},
{"satCalibrate", SAT_CALIBRATE},
{"emerSatComm",SAT_COMM},
{"satCalibrate",SAT_CALIBRATE},
{"tour", TOUR},
{"sail", SAIL}
{"sail", SAIL},
{"emerSatComm",EMER_SAT_COMM},
{"emerSail",EMER_SAIL}
};
auto it = stringToEnum.find(str);
return it != stringToEnum.end() ? it->second : static_cast<Type>(0); // 返回0表示无效类型
src/pPowerManger/systemData.h
+42
View File
@@ -9,7 +9,48 @@ class SystemData
{
private:
SQLite* m_db;
std::set<unsigned int> sysFaultCodes;
mutable std::mutex sysFaultCodesMutex;
public:
/**
* @brief 向系统故障码集合中插入一个故障码
* @param faultCode 要插入的故障码
*/
void insertFaultCode(unsigned int faultCode) {
std::lock_guard<std::mutex> lock(sysFaultCodesMutex);
sysFaultCodes.insert(faultCode);
}
/**
* @brief 从系统故障码集合中消除一个故障码
* @param faultCode 要消除的故障码
*/
void eraseFaultCode(unsigned int faultCode) {
std::lock_guard<std::mutex> lock(sysFaultCodesMutex);
sysFaultCodes.erase(faultCode);
}
/**
* @brief 获取系统当前的故障等级
* @return uint8_t 故障等级，00H-无故障, 10H-一级, 20H-二级, 30H-三级, 40H-四级
*/
uint8_t getFaultCodeLevel() const {
uint8_t maxLevel = 0;
// 遍历 sysFaultCodes 集合中的每个故障码
for (auto code : sysFaultCodes) {
// 提取0xabcd中的b位 (右移8位后取低4位)
uint8_t level = (code >> 8) & 0xF;
if (level > maxLevel) {
maxLevel = level;
}
}
return maxLevel;
}
/**
* @brief 获取系统当前的故障码集合
* @return std::set<unsigned int>& 系统当前的故障码集合
*/
const std::set<unsigned int>& getFaultCode() const {
return sysFaultCodes;
}
//燃料电池系统参数
uint8_t fc_mode; // 燃料电池系统运行模式：00H-无效, 01H-调试, 02H-自动, 03H-补给
uint8_t fc_status; // 燃料电池状态：00H-无效, 01H-初始化, ..., 0DH-维护完成
@@ -477,6 +518,7 @@ public:
//初始化变量全部为0
memset(this, 0, sizeof(SystemData));
disSysFaultCode.clear();
sysFaultCodes.clear();
};
~SystemData(){};
src/pPowerManger/udpcomm/udpComm.cpp
+88 -73
View File
@@ -315,81 +315,109 @@ bool udpComm::pushMsgToQueue(unsigned char *m)
bool udpComm::clearMsgQueue()
{
while(!m_qReceiveCcuStateBuffer.empty())
m_qReceiveCcuStateBuffer.pop();
{
std::lock_guard<std::mutex> lock(m_mutexCcuState);
while(!m_qReceiveCcuStateBuffer.empty())
m_qReceiveCcuStateBuffer.pop();
}
while(!m_qReceiveCcuSetParmBuffer.empty())
m_qReceiveCcuSetParmBuffer.pop();
{
std::lock_guard<std::mutex> lock(m_mutexCcuSetParm);
while(!m_qReceiveCcuSetParmBuffer.empty())
m_qReceiveCcuSetParmBuffer.pop();
}
while(!m_qReceiveDisHighVolBusBuffer.empty())
m_qReceiveDisHighVolBusBuffer.pop();
{
std::lock_guard<std::mutex> lock(m_mutexDisHighVolBus);
while(!m_qReceiveDisHighVolBusBuffer.empty())
m_qReceiveDisHighVolBusBuffer.pop();
}
while(!m_qReceiveDisHighAVolBusBuffer.empty())
m_qReceiveDisHighAVolBusBuffer.pop();
{
std::lock_guard<std::mutex> lock(m_mutexDisHighAVolBus);
while(!m_qReceiveDisHighAVolBusBuffer.empty())
m_qReceiveDisHighAVolBusBuffer.pop();
}
while(!m_qReceiveDisHighBVolBusBuffer.empty())
m_qReceiveDisHighBVolBusBuffer.pop();
while(!m_qReceiveDisLowBusBuffer.empty())
m_qReceiveDisLowBusBuffer.pop();
return true;
{
std::lock_guard<std::mutex> lock(m_mutexDisHighBVolBus);
while(!m_qReceiveDisHighBVolBusBuffer.empty())
m_qReceiveDisHighBVolBusBuffer.pop();
}
{
std::lock_guard<std::mutex> lock(m_mutexDisLowBus);
while(!m_qReceiveDisLowBusBuffer.empty())
m_qReceiveDisLowBusBuffer.pop();
}
return true;
}
bool udpComm::popMsgFormQueue(double &time,msg_CcuStateFbMsg &m)
{
if(m_qReceiveCcuStateBuffer.empty())
return false;
time = m_qReceiveCcuStateBuffer.front().first;
m = m_qReceiveCcuStateBuffer.front().second;
m_qReceiveCcuStateBuffer.pop();
return true;
std::lock_guard<std::mutex> lock(m_mutexCcuState);
if(m_qReceiveCcuStateBuffer.empty())
return false;
time = m_qReceiveCcuStateBuffer.front().first;
m = m_qReceiveCcuStateBuffer.front().second;
m_qReceiveCcuStateBuffer.pop();
return true;
}
bool udpComm::popMsgFormQueue(double &time,msg_CcuSetParmFbMsg &m)
{
if(m_qReceiveCcuSetParmBuffer.empty())
return false;
time = m_qReceiveCcuSetParmBuffer.front().first;
m = m_qReceiveCcuSetParmBuffer.front().second;
m_qReceiveCcuSetParmBuffer.pop();
return true;
std::lock_guard<std::mutex> lock(m_mutexCcuSetParm);
if(m_qReceiveCcuSetParmBuffer.empty())
return false;
time = m_qReceiveCcuSetParmBuffer.front().first;
m = m_qReceiveCcuSetParmBuffer.front().second;
m_qReceiveCcuSetParmBuffer.pop();
return true;
}
bool udpComm::popMsgFormQueue(double &time,msg_disHighVolBusFbMsg &m)
{
if(m_qReceiveDisHighVolBusBuffer.empty())
return false;
time = m_qReceiveDisHighVolBusBuffer.front().first;
m = m_qReceiveDisHighVolBusBuffer.front().second;
m_qReceiveDisHighVolBusBuffer.pop();
return true;
std::lock_guard<std::mutex> lock(m_mutexDisHighVolBus);
if(m_qReceiveDisHighVolBusBuffer.empty())
return false;
time = m_qReceiveDisHighVolBusBuffer.front().first;
m = m_qReceiveDisHighVolBusBuffer.front().second;
m_qReceiveDisHighVolBusBuffer.pop();
return true;
}
bool udpComm::popMsgFormQueue(double &time,msg_disHighAVolBusFbMsg &m)
{
if(m_qReceiveDisHighAVolBusBuffer.empty())
return false;
time = m_qReceiveDisHighAVolBusBuffer.front().first;
m = m_qReceiveDisHighAVolBusBuffer.front().second;
m_qReceiveDisHighAVolBusBuffer.pop();
return true;
std::lock_guard<std::mutex> lock(m_mutexDisHighAVolBus);
if(m_qReceiveDisHighAVolBusBuffer.empty())
return false;
time = m_qReceiveDisHighAVolBusBuffer.front().first;
m = m_qReceiveDisHighAVolBusBuffer.front().second;
m_qReceiveDisHighAVolBusBuffer.pop();
return true;
}
bool udpComm::popMsgFormQueue(double &time,msg_disLowMainBusFbMsg &m)
{
if(m_qReceiveDisHighBVolBusBuffer.empty())
return false;
time = m_qReceiveDisHighBVolBusBuffer.front().first;
m = m_qReceiveDisHighBVolBusBuffer.front().second;
m_qReceiveDisHighBVolBusBuffer.pop();
return true;
std::lock_guard<std::mutex> lock(m_mutexDisHighBVolBus);
if(m_qReceiveDisHighBVolBusBuffer.empty())
return false;
time = m_qReceiveDisHighBVolBusBuffer.front().first;
m = m_qReceiveDisHighBVolBusBuffer.front().second;
m_qReceiveDisHighBVolBusBuffer.pop();
return true;
}
bool udpComm::popMsgFormQueue(double &time,msg_disLowBusFbMsg &m)
{
if(m_qReceiveDisLowBusBuffer.empty())
return false;
time = m_qReceiveDisLowBusBuffer.front().first;
m = m_qReceiveDisLowBusBuffer.front().second;
m_qReceiveDisLowBusBuffer.pop();
return true;
std::lock_guard<std::mutex> lock(m_mutexDisLowBus);
if(m_qReceiveDisLowBusBuffer.empty())
return false;
time = m_qReceiveDisLowBusBuffer.front().first;
m = m_qReceiveDisLowBusBuffer.front().second;
m_qReceiveDisLowBusBuffer.pop();
return true;
}
@@ -528,32 +556,19 @@ bool udpComm::sendCcuColCmd(const ccuColCmd cmd)
msgBuf = reinterpret_cast<unsigned char *>(msg);
cc = calculateChecksum(msgBuf,sizeof(msg_CcuColCmdMsg));
msg->checkCode = static_cast<cChecksum>(cc & 0xFF);
try
{
if(udpScoket == nullptr) {
throw std::runtime_error("UDP socket not initialized");
bool sendResult = false;
try {
sendResult = sendMsg(msgBuf,sizeof(msg_CcuColCmdMsg));
if (!sendResult) {
LOG_F(ERROR, "Failed to send CCU command");
}
if(ccuHost.empty() || ccuPort <= 0 || ccuPort > 65535) {
throw std::runtime_error("Invalid CCU address: " + ccuHost + ":" + std::to_string(ccuPort));
}
sendMsg(msgBuf,sizeof(msg_CcuColCmdMsg));
}
catch(const std::invalid_argument& e)
{
std::cerr << "Invalid argument error in CCU command: " << e.what() << '\n';
LOG_F(ERROR, "Invalid argument in CCU command: %s", e.what());
delete msg;
return false;
}
catch(const std::out_of_range& e)
{
std::cerr << "Out of range error in CCU command: " << e.what() << '\n';
LOG_F(ERROR, "Out of range in CCU command: %s", e.what());
delete msg;
return false;
} catch (const std::exception& e) {
LOG_F(ERROR, "Exception in sendCcuColCmd: %s", e.what());
}
delete msg;
return true;
return sendResult;
}
bool udpComm::sendCcuSetParmCmd(const ccuSetParmCmd cmd)
{
src/pPowerManger/udpcomm/udpComm.h
+32 -4
View File
@@ -10,6 +10,7 @@
#include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h"
#include "MOOS/libMOOS/Comms/XPCUdpSocket.h"
#include <ctime>
#include <mutex>
#include <chrono>
#include <iostream>
#include <utility>
@@ -49,11 +50,31 @@ public:
bool sendDisSysCmd(const disHighAVolBusCmd cmd);
bool sendDisSysCmd(const disHighBVolBusCmd cmd);
bool sendDisSysCmd(const disLowBusCmd cmd);
bool sendMsg(const unsigned char* msg,unsigned int size){
// cout << "sendMsgTo" << ccuHost << ":" << ccuPort << endl;
udpScoket->iSendMessageTo((void*) msg, size, ccuPort, ccuHost);
bool sendMsg(const unsigned char* msg, unsigned int size) {
if (!udpScoket) {
LOG_F(ERROR, "UDP socket is null in sendMsg");
return false;
}
if (!msg || size == 0) {
LOG_F(ERROR, "Invalid message parameters in sendMsg");
return false;
}
if (ccuHost.empty() || ccuPort <= 0) {
LOG_F(ERROR, "Invalid CCU address: %s:%d", ccuHost.c_str(), ccuPort);
return false;
}
return true;
try {
udpScoket->iSendMessageTo((void*)msg, size, ccuPort, ccuHost);
return true;
} catch (const XPCException& e) {
LOG_F(ERROR, "XPCException in sendMsg");
} catch (const std::exception& e) {
LOG_F(ERROR, "Exception in sendMsg: %s", e.what());
} catch (...) {
LOG_F(ERROR, "Unknown exception in sendMsg");
}
return false;
}
bool getMsgId(char *m,unsigned short &id);
@@ -64,6 +85,13 @@ public:
std::vector<std::string> sError;
std::mutex m_mutexCcuState;
std::mutex m_mutexCcuSetParm;
std::mutex m_mutexDisHighVolBus;
std::mutex m_mutexDisHighAVolBus;
std::mutex m_mutexDisHighBVolBus;
std::mutex m_mutexDisLowBus;
// std::queue<msgWithTime> m_qReceiveCcuStateBuffer;
std::queue<std::pair<double,msg_CcuStateFbMsg>> m_qReceiveCcuStateBuffer;
std::queue<std::pair<double,msg_CcuSetParmFbMsg>> m_qReceiveCcuSetParmBuffer;
src/pPowerManger/upmsg/uDevice_secdistState.h
+39 -4
View File
@@ -291,7 +291,7 @@ public:
state.forwardSonar = driver.forwardSonar;
state.depth3 = driver.depth3;
state.soundVeloc = driver.soundVeloc;
state.bowRudderServo = driver.bowRudderServo1 | driver.bowRudderServo2;
state.bowRudderServo = driver.bowRudderServo1;
state.bowRudderDeploy = driver.bowRudderDeploy;
state.ctd1 = driver.ctd1;
state.waterInject = driver.waterInject;
@@ -320,8 +320,10 @@ public:
state.mastLiftingServo = driver.mastLiftingServo;
state.DVL = driver.DVL;
state.ADCP = driver.DVL; // Assuming ADCP is same as DVL
//出入水传感器1、2共用一个继电器
state.inOutLetWater1 = driver.inputWaterSensor1;
state.inOutLetWater2 = driver.inputWaterSensor2;
state.inOutLetWater2 = driver.inputWaterSensor1;
// state.inOutLetWater2 = driver.inputWaterSensor2;
// UNIT4
state.singleBeamSonar7 = driver.singleBeamSonar7;
@@ -490,7 +492,40 @@ public:
// std::cout << buildSecdistStateToJson(s) << std::endl;
return buildSecdistStateToJson(s);
};
static DriverTable fault_state;
std::string buildMsg(DriverTable driver, DriverTable driver_state, DriverTable &driver_fault_state){
static size_t f_callCount = 0; // 静态变量，记录函数调用次数
unsigned int nCheck = 10;
DeviceSecdistState s;
DriverTable driverCopy = driver;
for (size_t i = 0; i < sizeof(DriverTable); ++i)
{
unsigned char* driverCopyPtr = reinterpret_cast<unsigned char*>(&driverCopy) + i;
unsigned char* statePtr = reinterpret_cast<unsigned char*>(&driver_state) + i;
unsigned char* faultStatePtr = reinterpret_cast<unsigned char*>(&driver_fault_state) + i;
unsigned char* faultState = reinterpret_cast<unsigned char*>(&fault_state) + i;
if (*driverCopyPtr == *statePtr)
{
*driverCopyPtr = 2;
*faultStatePtr = 0;
*faultState = 0;
}
else
{
*faultState = *faultState + 1;
if(*faultState >= 5)
{
//只有开启失败才计入故障
if(*driverCopyPtr == 1)
*faultStatePtr = 1;
*faultState = 5;
}
}
}
f_callCount ++;
converFromDriverTable(s, driverCopy);
return buildSecdistStateToJson(s);
};
};
#endif
src/pPowerManger/upmsg/uPlan_planStop.h
+2 -2
View File
@@ -2,13 +2,13 @@
#define UPLAN_PLANSTOP_CMD_H
#include <string>
#include "../logc/loguru.hpp"
#define DB_TO_PM_TASKSTOP "uPlan_planStop_cmd"
#define DB_TO_PM_TASKSTOP "uPlan_planStop_st"
class uPlan_planStopMsg
{
private:
/* data */
public:
std::string key = "uPlan_planStop_cmd";
std::string key = "uPlan_planStop_st";
uPlan_planStopMsg(/* args */){};
~uPlan_planStopMsg(){};
bool getTaskStop(const std::string& key, const std::string& msg) {
src/pPowerManger/upmsg/uPower_equState.h
+1 -1
View File
@@ -26,7 +26,7 @@ public:
const DriverTable getEquipmentCmd(const std::string& key, const std::string& msg)
{
DriverTable cmd;
if (driver.loadDriverTableFromJson(msg, cmd) < 0)
if (driver.loadDriverTableFromJson(msg, cmd) < 0)
{
LOG_F(WARNING, "get equipment cmd error:%s", msg.c_str());
}
src/pPowerManger/upmsg/uPower_pmState.h
+8 -5
View File
@@ -4,7 +4,7 @@
#include "string"
#include "json/json.h"
#include <bitset>
#include "../systemData.h"
class uPower_pmStateMsg
{
private:
@@ -37,7 +37,7 @@ public:
unsigned char m_leakage;
} state;
std::string buildMsg()
std::string buildMsg(const SystemData& data)
{
// 状态赋值
Json::Value pms;
@@ -45,13 +45,16 @@ public:
pms["workCondition_uint8"] = state.workCondition;
pms["currentMod_uint8"] = state.currentMod;
pms["powerState_uint8"] = state.powerState;
pms["faultLevel_uint8"] = state.faultLevel;
// pms["faultLevel_uint8"] = state.faultLevel;
pms["faultLevel_uint8"] = data.getFaultCodeLevel();
Json::Value faultCode_array = Json::Value(Json::arrayValue);
for(auto val : state.faultCode)
//TODO：屏蔽所有故障
for(auto faultCode : data.getFaultCode())
{
faultCode_array.append(val);
faultCode_array.append(faultCode);
}
pms["faultCode_array"] = faultCode_array;
pms["soc_uint16"] = state.soc;