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

完善待机工况及相关逻辑

Browse Source
This commit is contained in:
zjk
2025-04-09 23:28:05 +08:00
parent 785a47ff99
commit b653f17b03
14 changed files with 241 additions and 196 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/pPowerManger/LowerCommManager.cpp
+1 -1
View File
@@ -238,7 +238,7 @@ bool LowerCommManager::listenThreadFunc()
try
{
int nRead = m_udpComm->udpScoket->iRecieveMessage(Buff, sizeof(Buff));
cout<<MOOS::ConsoleColours::yellow()<< "RECEVE DATA COUNT:" << nRead << MOOS::ConsoleColours::reset() << endl;
//cout<<MOOS::ConsoleColours::yellow()<< "RECEVE DATA COUNT:" << nRead << MOOS::ConsoleColours::reset() << endl;
if(nRead > 0)
{
try {
src/pPowerManger/PowerManagerFsm.cpp
+9 -54
View File
@@ -9,71 +9,25 @@ PowerManger* PowerManagerFsm::pm = nullptr;
// InitState实现
void InitState::entry() {
PowerManagerFsm::entry();
std::cout << "===========================" << std::endl;
// PowerManagerFsm::pm->m_pmEvent = "初始化状态";
PowerManagerFsm::pm->printState("InitState", "进入初始化状态");
}
void InitState::react(InitEvent const &) {
// double StartTime = PowerManagerFsm::pm->GetAppStartTime();
std::cout << "开始初始化" << std::endl;
double StartTime = 0;
while (PowerManagerFsm::pm->connectState < 5) {
MOOSPause(1000);
PowerManagerFsm::pm->updateTime();
double dft = abs(PowerManagerFsm::pm->m_current_time - StartTime);
if(dft > 10) {
transit<FaultState>();
PowerManagerFsm::pm->m_faultNum = 1;
return;
}
string desp = "等待复合管控消息 : " + to_string(10-dft);
PowerManagerFsm::pm->printState("InitState", desp);
}
if(PowerManagerFsm::pm->checkError() < 0) {
transit<FaultState>();
PowerManagerFsm::pm->m_faultNum = 2;
return;
}
PowerManagerFsm::pm->initControlCmd();
while (true) {
if(PowerManagerFsm::pm->m_currentDisBreakerState.bus3Breaker.unit4InstrumentDC48VCircuitBreaker == 1 ||
PowerManagerFsm::pm->m_currentDisBreakerState.bus4Breaker.unit5CircuitBreaker == 1 ||
PowerManagerFsm::pm->m_currentDisBreakerState.bus4Breaker.unit2CircuitBreaker == 1 ||
PowerManagerFsm::pm->m_currentDisBreakerState.bus4Breaker.unit3CircuitBreaker == 1 ||
PowerManagerFsm::pm->m_currentDisBreakerState.bus4Breaker.unit1CircuitBreaker == 1) {
transit<StandbyState>();
return;
}
PowerManagerFsm::pm->m_pmEvent = "等待关键断路器闭合";
MOOSPause(1000);
PowerManagerFsm::pm->printState("InitState", PowerManagerFsm::pm->m_pmEvent);
}
std::cout << "InitState: 初始化完成" << std::endl;
//转换到待机状态
std::cout << "初始化配电指令" << std::endl;
pm->initControlCmd();
// PowerManagerFsm::pm->printState("InitState", "进入初始化状态");
transit<StandbyState>();
}
// StandbyState实现
void StandbyState::entry() {
PowerManagerFsm::entry();
PowerManagerFsm::pm->printState("StandbyState", "进入待机状态");
pm->m_driver.getSTANDBYTable(&pm->m_subDisSysCmd);
pm->m_CcuColCmd.fcuCmd = FcuCommand::Stop;
pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Stop;
}
void StandbyState::react(StandbyEvent const &) {
while (true) {
if(PowerManagerFsm::pm->checkError() < 0) {
transit<FaultState>();
return;
}
handlePowerCommands();
MOOSPause(1000);
}
transit<StandbyState>();
}
void StandbyState::handlePowerCommands() {
switch (PowerManagerFsm::pm->m_msCmd.powerCMD) {
case 0: PowerManagerFsm::pm->m_pmEvent = "收到上位机控制指令:关机"; break;
@@ -90,6 +44,7 @@ void StandbyState::handlePowerOn() {
// FaultState实现
void FaultState::entry() {
PowerManagerFsm::entry();
PowerManagerFsm::pm->printState("FaultState", "进入故障状态");
}
src/pPowerManger/PowerManagerFsm.hpp
+42 -19
View File
@@ -2,7 +2,12 @@
#define POWER_MANAGER_FSM_HPP
#include "tinyfsm.hpp"
#include <iostream>
#include <chrono> // 新增头文件
#include <iomanip> // 新增头文件
#include <ctime> // 新增头文件，用于localtime函数
#include <cxxabi.h> // 新增头文件，用于demangle功能
#include "pmSysvariable.h"
#include "driver.h"
// 前向声明
class PowerManger;
@@ -12,7 +17,6 @@ enum class PowerSystemState {
FAULT,
};
// 定义事件类型
//==================状态切换事件=============================
struct InitEvent : tinyfsm::Event { };
struct StandbyEvent : tinyfsm::Event { };
@@ -21,18 +25,16 @@ struct FaultEvent : tinyfsm::Event {
FaultEvent(unsigned int code) : faultCode(code) {}
};
//==================状态流程事件============================
struct MasterCommandEvent : tinyfsm::Event { };
struct MasterCommandEvent : tinyfsm::Event {};
struct TaskStart : tinyfsm::Event { };
// struct
struct DeviceCmd : tinyfsm::Event { };
// 定义状态机基类
class PowerManagerFsm : public tinyfsm::Fsm<PowerManagerFsm> {
protected:
static PowerManger* pm;
static DriverTable m_currentDriver;
public:
// 初始化状态机
static void init(PowerManger* powerManager) {
pm = powerManager;
}
@@ -43,30 +45,52 @@ public:
virtual void react(FaultEvent const &) {};
virtual void react(MasterCommandEvent const &) {};
virtual void react(TaskStart const &) {};
virtual void react(DeviceCmd const &) {};
// 状态机生命周期方法
virtual void entry() {std::cout << "[ Enter state ] : " << typeid(*this).name() << std::endl;} // 进入状态
virtual void exit() {std::cout << "[ Exit state ] : " << typeid(*this).name() << std::endl;} // 退出状态
// 状态转换接口
void triggerInitEvent() { dispatch(InitEvent()); }
void triggerStandbyEvent() { dispatch(StandbyEvent()); }
void triggerFaultEvent(unsigned int faultCode) { dispatch(FaultEvent(faultCode)); }
virtual void entry() {
auto now = std::chrono::system_clock::now();
auto now_time = std::chrono::system_clock::to_time_t(now);
int status = 0;
char* demangled = abi::__cxa_demangle(typeid(*this).name(), nullptr, nullptr, &status);
std::string typeName = (status == 0) ? demangled : typeid(*this).name();
if (demangled) free(demangled);
std::cout << "\033[1;38;5;51m╭──────────────────────────────────────────────────────\n"
<< "│ \033[1;38;5;226m⮞⮞⮞ \033[0;38;5;51m[" << std::put_time(std::localtime(&now_time), "%Y-%m-%d %H:%M:%S")
<< "] \033[1;38;5;255m进入状态: \033[1;38;5;213m" << typeName << "\n"
<< "│ \033[0m" << std::endl;
}
virtual void exit() {
auto now = std::chrono::system_clock::now();
auto now_time = std::chrono::system_clock::to_time_t(now);
int status = 0;
char* demangled = abi::__cxa_demangle(typeid(*this).name(), nullptr, nullptr, &status);
std::string typeName = (status == 0) ? demangled : typeid(*this).name();
if (demangled) free(demangled);
std::cout << "\033[1;38;5;203m│ \n"
<< "│ \033[1;38;5;208m⮜⮜⮜ \033[0;38;5;203m[" << std::put_time(std::localtime(&now_time), "%Y-%m-%d %H:%M:%S")
<< "] \033[1;38;5;255m离开状态: \033[1;38;5;213m" << typeName << "\n"
<< "╰──────────────────────────────────────────────────────\033[0m" << std::endl;
}
};
class InitState : public PowerManagerFsm {
public:
void entry() override;
void react(InitEvent const &);
};
class StandbyState : public PowerManagerFsm {
public:
void entry() override;
void react(StandbyEvent const &);
PowerSystemState getCurrentState();
void react(StandbyEvent const &) override;
// void react(MasterCommandEvent const &) override;
// void react(DeviceCmd const &) override;
private:
void handlePowerCommands();
void handlePowerOn();
@@ -78,6 +102,5 @@ public:
void react(FaultEvent const &);
};
// FSM_INITIAL_STATE(PowerManagerFsm, InitState)
#endif // POWER_MANAGER_FSM_HPP
src/pPowerManger/PowerManger.cpp
+1 -19
View File
@@ -126,29 +126,11 @@ bool PowerManger::Iterate()
// cout << " Current State : " << (int)PowerManagerFsm::current_state_ptr->getCurrentState();
if (m_ticker++ % 5 == 0)
{
m_CcuColCmd.heartbeat++;
m_lowerCommManager->processPeriodicTasks();
m_upperCommManager->processPeriodicTasks();
// buildPowerSysReport();
//Notify(PM_TO_DB_SUBDISCMD,m_driver.saveDriverTableToJson(&m_subDisSysCmd));
//Notify(PM_TO_DB_EQUIPMENTSTATE,m_driver.saveDriverTableToJson(&m_currentDriverState));
//d.保存到数据库
writeDataToDB();
}
///////////////////////
// string a = m_driver.getSubDisCmdJson(&m_subDisSysCmd);
// saveStringToJsonFile(a,"/home/zjk/project/H100/H100PowerManger/data/subDisSysCmd.json");
///////////////////////
//4. 写状态信息到数据库
// m_stateData.state_id = m_pmState.id;
// m_stateData.state_name = m_pmState.name;
// m_stateData.event = m_pmEvent;
// m_stateData.fault_code = m_faultNum;
// if(!m_db->insertStateData(m_stateData))
// return false;
//AppCastingMOOSApp::PostReport();
//5. 清空UDP消息队列
// m_udpComm.clearMsgQueue();
return(true);
}
src/pPowerManger/PowerManger.h
+3 -7
View File
@@ -114,18 +114,14 @@ class PowerManger : public AppCastingMOOSApp
LowerCommManager* m_lowerCommManager;
UpperCommManager* m_upperCommManager;
// 状态机实例
PowerManagerFsm m_fsm;
// // 状态机实例
// PowerManagerFsm m_fsm;
public:
//===========================状态机接口=======================================
void printState(const std::string& stateName, const std::string& desp);
bool FSM_RUN_IS_OK = true;
// 状态机事件触发接口
// void triggerInitEvent() { m_fsm.triggerInitEvent(); }
// void triggerStandbyEvent() { m_fsm.triggerStandbyEvent(); }
// void triggerFaultEvent(unsigned int faultCode) { m_fsm.triggerFaultEvent(faultCode); }
//==================================================================================
int checkError();
bool shutdown();
src/pPowerManger/UpperCommManager.cpp
+36
View File
@@ -17,6 +17,10 @@ bool UpperCommManager::processUpperMsg(const string& key, const string& strJson)
m_pm->m_subDisSysState = getSubDisState(key, strJson);
return true;
}
else if(key == DB_TO_PM_DEEPTH) {
m_pm->m_depth = getDeepth(key, strJson);
return true;
}
std::cout << "Unknown key: " << key << std::endl;
return false;
}
@@ -25,6 +29,7 @@ const MSControlCmd UpperCommManager::getMasterControlCmd(const std::string& key,
{
MSControlCmd cmd;
if (parseControlCmdFromJson(cmd, msg)) {
return cmd;
}
else
@@ -53,6 +58,37 @@ const DriverTable UpperCommManager::getSubDisState(const std::string& key, const
}
}
const int UpperCommManager::getDeepth(const std::string& key, const std::string& msg){
int deepth;
Json::Value root;
Json::CharReaderBuilder builder;
JSONCPP_STRING errs;
// 解析JSON字符串
std::istringstream stream(msg);
if (!Json::parseFromStream(builder, stream, &root, &errs)) {
std::cerr << "JSON解析失败: " << errs << std::endl;
return -1;
}
// 检查并读取深度数组
if (root.isMember("value_array_uint16") && root["value_array_uint16"].isArray()) {
const Json::Value& array = root["value_array_uint16"];
if (array.size() >= 3) {
// 计算三个深度值的平均值
deepth = (array[0].asInt() + array[1].asInt() + array[2].asInt()) / 3;
} else {
std::cerr << "深度数组元素不足3个" << std::endl;
return -1;
}
} else {
std::cerr << "JSON格式错误" << std::endl;
return -1;
}
return deepth;
}
const std::string UpperCommManager::getCurrentEquState(DriverTable& driver)
{
return m_pm->m_driver.saveDriverTableToJson(&driver);
src/pPowerManger/UpperCommManager.h
+4 -3
View File
@@ -26,7 +26,7 @@ using namespace std;
#define DB_TO_PM_SUBDISSTATE "uDevice_secdistState_st"
//决策消息
#define DB_TO_PM_DEEPTH "uPower_depth_fb"
#define DB_TO_PM_DEEPTH "uDevice_depth_st"
#define DB_TO_PM_MISSION "uPlan_taskStart_cmd"
class PowerManger; // 前向声明
@@ -36,7 +36,7 @@ public:
UpperCommManager(PowerManger* powerManager);
~UpperCommManager();
//周期调用
// 周期调用
void processPeriodicTasks();
// 处理上位机消息
@@ -49,10 +49,11 @@ public:
const MSControlCmd getMasterControlCmd(const std::string& key, const std::string& msg);
const DriverTable getEquipmentCmd(const std::string& key, const std::string& msg);
const DriverTable getSubDisState(const std::string& key, const std::string& msg);
const int getDeepth(const std::string& key, const std::string& msg);
const std::string getCurrentEquState(DriverTable& driver);
const std::string getSubDisCmd(DriverTable& driver);
// 其他公共函数
string getTypeName(const string typeName);
bool saveStringToJsonFile(const string &strJson, const string &filePath);
src/pPowerManger/driver.cpp
+62 -62
View File
@@ -59,72 +59,72 @@ int Driver::getDriverTable(workCondition condition, DriverTable* driverTable)
void Driver::getSTANDBYTable(DriverTable* driverTable)
{
//打开所有传感器
driverTable->depth1 = DRIVER_FULL_POWER_ON;
driverTable->depth2 = DRIVER_FULL_POWER_ON;
driverTable->depth3 = DRIVER_FULL_POWER_ON;
driverTable->overDeep1 = DRIVER_FULL_POWER_ON;
driverTable->overDeep2 = DRIVER_FULL_POWER_ON;
driverTable->ctd1 = DRIVER_FULL_POWER_ON;
driverTable->ctd2 = DRIVER_FULL_POWER_ON;
driverTable->soundVeloc = DRIVER_FULL_POWER_ON;
driverTable->commMast = DRIVER_FULL_POWER_ON;
driverTable->ins = DRIVER_FULL_POWER_ON;
driverTable->ntp = DRIVER_FULL_POWER_ON;
driverTable->inputWaterSensor1 = DRIVER_FULL_POWER_ON;
driverTable->inputWaterSensor2 = DRIVER_FULL_POWER_ON;
driverTable->inputWaterSensor3 = DRIVER_FULL_POWER_ON;
driverTable->depth1 = 1;
driverTable->depth2 = 1;
driverTable->depth3 = 1;
driverTable->overDeep1 = 1;
driverTable->overDeep2 = 1;
driverTable->ctd1 = 1;
driverTable->ctd2 = 1;
driverTable->soundVeloc = 1;
driverTable->commMast = 1;
driverTable->ins = 1;
driverTable->ntp = 1;
driverTable->inputWaterSensor1 = 1;
driverTable->inputWaterSensor2 = 1;
driverTable->inputWaterSensor3 = 1;
//关闭所有负载
driverTable->photoSensor = DRIVER_STANDBY;
driverTable->electSensor = DRIVER_STANDBY;
driverTable->launchControler = DRIVER_STANDBY;
driverTable->sideSonar = DRIVER_POWER_OFF;
driverTable->buoyage = DRIVER_POWER_OFF;
driverTable->photoSensor = 0;
driverTable->electSensor = 0;
driverTable->launchControler = 0;
driverTable->sideSonar = 0;
driverTable->buoyage = 0;
//关闭所有水声设备
driverTable->singleBeamSonar1 = DRIVER_POWER_OFF;
driverTable->singleBeamSonar2 = DRIVER_POWER_OFF;
driverTable->singleBeamSonar3 = DRIVER_POWER_OFF;
driverTable->singleBeamSonar4 = DRIVER_POWER_OFF;
driverTable->singleBeamSonar5 = DRIVER_POWER_OFF;
driverTable->singleBeamSonar6 = DRIVER_POWER_OFF;
driverTable->singleBeamSonar7 = DRIVER_POWER_OFF;
driverTable->singleBeamSonar8 = DRIVER_POWER_OFF;
driverTable->DVL = DRIVER_POWER_OFF;
driverTable->forwardSonar = DRIVER_POWER_OFF;
driverTable->lowFreqSonar = DRIVER_POWER_OFF;
driverTable->USBL = DRIVER_POWER_OFF;
driverTable->singleBeamSonar1 = 0;
driverTable->singleBeamSonar2 = 0;
driverTable->singleBeamSonar3 = 0;
driverTable->singleBeamSonar4 = 0;
driverTable->singleBeamSonar5 = 0;
driverTable->singleBeamSonar6 = 0;
driverTable->singleBeamSonar7 = 0;
driverTable->singleBeamSonar8 = 0;
driverTable->DVL = 0;
driverTable->forwardSonar = 0;
driverTable->lowFreqSonar = 0;
driverTable->USBL = 0;
//关闭所有执行机构
driverTable->bowCover1 = DRIVER_POWER_OFF;
driverTable->bowCover2 = DRIVER_POWER_OFF;
driverTable->bowCover3 = DRIVER_POWER_OFF;
driverTable->bowCover4 = DRIVER_POWER_OFF;
driverTable->bowCover5 = DRIVER_POWER_OFF;
driverTable->bowCover6 = DRIVER_POWER_OFF;
driverTable->bowRudderDeploy = DRIVER_POWER_OFF;
driverTable->bowRudderServo1 = DRIVER_POWER_OFF;
driverTable->bowRudderServo2 = DRIVER_POWER_OFF;
driverTable->bowWaterlnet1 = DRIVER_POWER_OFF;
driverTable->bowWaterlnet2 = DRIVER_POWER_OFF;
driverTable->bowWaterlnet3 = DRIVER_POWER_OFF;
driverTable->bowWaterlnet4 = DRIVER_POWER_OFF;
driverTable->bowWaterlnet5 = DRIVER_POWER_OFF;
driverTable->bowWaterlnet6 = DRIVER_POWER_OFF;
driverTable->bowWaterlnet7 = DRIVER_POWER_OFF;
driverTable->bowWaterlnet8 = DRIVER_POWER_OFF;
driverTable->waterInject = DRIVER_POWER_OFF;
driverTable->mastLiftingServo = DRIVER_POWER_OFF;
driverTable->buoyancyAdjust1 = DRIVER_POWER_OFF;
driverTable->buoyancyAdjust2 = DRIVER_POWER_OFF;
driverTable->buoyancyAdjust3 = DRIVER_POWER_OFF;
driverTable->buoyancyAdjust4 = DRIVER_POWER_OFF;
driverTable->buoyancyAdjust5 = DRIVER_POWER_OFF;
driverTable->sternRudderServo1 = DRIVER_POWER_OFF;
driverTable->sternRudderServo2 = DRIVER_POWER_OFF;
driverTable->sternRudderServo3 = DRIVER_POWER_OFF;
driverTable->sternRudderServo4 = DRIVER_POWER_OFF;
driverTable->thruster = DRIVER_POWER_OFF;
driverTable->bowThrow = DRIVER_POWER_OFF;
driverTable->sternThrow = DRIVER_POWER_OFF;
driverTable->bowCover1 = 0;
driverTable->bowCover2 = 0;
driverTable->bowCover3 = 0;
driverTable->bowCover4 = 0;
driverTable->bowCover5 = 0;
driverTable->bowCover6 = 0;
driverTable->bowRudderDeploy = 0;
driverTable->bowRudderServo1 = 0;
driverTable->bowRudderServo2 = 0;
driverTable->bowWaterlnet1 = 0;
driverTable->bowWaterlnet2 = 0;
driverTable->bowWaterlnet3 = 0;
driverTable->bowWaterlnet4 = 0;
driverTable->bowWaterlnet5 = 0;
driverTable->bowWaterlnet6 = 0;
driverTable->bowWaterlnet7 = 0;
driverTable->bowWaterlnet8 = 0;
driverTable->waterInject = 0;
driverTable->mastLiftingServo = 0;
driverTable->buoyancyAdjust1 = 0;
driverTable->buoyancyAdjust2 = 0;
driverTable->buoyancyAdjust3 = 0;
driverTable->buoyancyAdjust4 = 0;
driverTable->buoyancyAdjust5 = 0;
driverTable->sternRudderServo1 = 0;
driverTable->sternRudderServo2 = 0;
driverTable->sternRudderServo3 = 0;
driverTable->sternRudderServo4 = 0;
driverTable->thruster = 0;
driverTable->bowThrow = 0;
driverTable->sternThrow = 0;
}
void Driver::getSHORE_BASED_READYable(DriverTable* driverTable)
src/pPowerManger/pmSysvariable.h
+49
View File
@@ -360,5 +360,54 @@ typedef struct pmState
unsigned int workCMD;
}MSControlCmd;
namespace FcuCommand {
enum Type : unsigned char {
SelfCheck = 0x01,
Start = 0x02,
Stop = 0x03,
Reset = 0x04,
EmergencyStop = 0x05,
Supply = 0x06,
WarmUp = 0x07,
Maintenance = 0x08
};
}
enum class FcuState : unsigned char {
//初始化
Init = 0x01,
//待机
Standby = 0x02,
//自检异常
SelfCheckError = 0x03,
//就绪
Ready = 0x04,
//启动
Start = 0x05,
//运行
Running = 0x06,
//停机
Stop = 0x07,
//故障
Fault = 0x08,
//补给
Supply = 0x09,
//预热
WarmUp = 0x0A,
//预热完成
WarmUpComplete = 0x0B,
//维护
Maintenance = 0x0C,
//维护完成
MaintenanceComplete = 0x0D
};
namespace BatteryCommand {
enum Type : unsigned char {
Start = 0x10,
Stop = 0x20
};
}
#endif