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

使用tinyfsm

Browse Source
This commit is contained in:
zjk
2025-04-06 20:16:37 +08:00
parent 811fbf2924
commit c51eb7073a
18 changed files with 1720 additions and 322 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/pPowerManger/PowerManger.cpp
+26 -220
View File
@@ -71,17 +71,9 @@ PowerManger::PowerManger()
m_db->createDriversTables(VAR_TO_STR(m_subDisSysCmd));
m_db->createDriversTables(VAR_TO_STR(m_MSDriverCmd));
//注册状态机
m_fsm.RegisterState("Init", 1, m_Inite_s1);
m_fsm.RegisterState("Stady", 2, m_Standby_s2);
m_fsm.RegisterState("Fault", 99, m_Fault_s99);
m_fsm.RegisterHandleFunction(&m_Inite_s1, handleInite_s1);
m_fsm.RegisterHandleFunction(&m_Standby_s2, handleStandby_s2);
m_fsm.RegisterHandleFunction(&m_Fault_s99, handleFault_s99);
// 初始化状态机
m_pmState = m_Inite_s1;
m_fsm.init(this);
PowerManagerFsm::start();
}
//---------------------------------------------------------
@@ -344,7 +336,14 @@ bool PowerManger::FsmLoop()
//状态机循环
while (!m_FsmThread.IsQuitRequested())
{
m_fsm.run(m_pmState,this);
// 检查故障并触发事件
if(int error = checkError()) {
triggerFaultEvent(error);
} else {
// 自动状态转换由状态机内部处理
MOOSPause(1000);
}
//延时1s
usleep(1000000);
}
@@ -599,7 +598,22 @@ int PowerManger::checkError()
}
void PowerManger::printState(FSM_State st, string desp){
void PowerManger::triggerInitEvent()
{
PowerManagerFsm::dispatch(InitEvent());
}
void PowerManger::triggerStandbyEvent()
{
PowerManagerFsm::dispatch(StandbyEvent());
}
void PowerManger::triggerFaultEvent(unsigned int faultCode)
{
PowerManagerFsm::dispatch(FaultEvent(faultCode));
}
void PowerManger::printState(const std::string& stateName, const std::string& desp){
cout << MOOS::ConsoleColours::Yellow() << "STATE " << st.id << " [" << st.name << "] " << MOOS::ConsoleColours::reset();
cout << MOOS::ConsoleColours::Blue() << " -> " << MOOS::ConsoleColours::reset();
// 检查desp是否有“故障”
@@ -610,214 +624,6 @@ void PowerManger::printState(FSM_State st, string desp){
cout << MOOS::ConsoleColours::Green() << desp << MOOS::ConsoleColours::reset() << endl;
}
int PowerManger::handleInite_s1(FSM_State &fsm, void* pData)
{
PowerManger* pm = (PowerManger*)pData;
bool ok = true;
//1. 检查通信是否正常 如果超时则进入故障状态
double StartTime = pm->GetAppStartTime();
while (pm->connectState < 5)
{
MOOSPause(1000);
pm->updateTime();
double dft = abs(pm->m_current_time - StartTime);
if(dft > 10)
{
pm->m_pmState = pm->m_Fault_s99;
pm->m_faultNum = 1; //通信故障
ok = false;
break;
}
string desp = "等待复合管控消息 : " + to_string(10-dft);
pm->printState(fsm,desp);
}
//2. 检查复合管控器是否上报故障 如果故障则进入故障状态
if(pm->checkError() < 0)
{
ok = false;
pm->m_faultNum = 2; //能源故障
}
//3. 初始化系统为待机状态
pm->initControlCmd();
//4.判断关键断路器是否闭合
ok = false;
while (!ok)
{
if(pm->m_currentDisBreakerState.bus3Breaker.unit4InstrumentDC48VCircuitBreaker == 1 ||
pm->m_currentDisBreakerState.bus4Breaker.unit5CircuitBreaker == 1 ||
pm->m_currentDisBreakerState.bus4Breaker.unit2CircuitBreaker == 1 ||
pm->m_currentDisBreakerState.bus4Breaker.unit3CircuitBreaker == 1 ||
pm->m_currentDisBreakerState.bus4Breaker.unit1CircuitBreaker == 1)
{
ok = true;
}
pm->m_pmEvent = "等待关键断路器闭合";
MOOSPause(1000);
pm->printState(fsm,pm->m_pmEvent);
}
//5. 进入下一状态
if(ok)
{
pm->m_pmState = pm->m_Standby_s2;
}
if(!ok)
{
pm->m_pmState = pm->m_Fault_s99;
}
return 0;
}
int PowerManger::handleStandby_s2(FSM_State &fsm, void* pData)
{
PowerManger* pm = (PowerManger*)pData;
pm->m_stateData.note = "待机状态";
while (true)
{
string desp = "待机状态";
pm->printState(fsm,desp);
//1. 检查当前是否存在故障
if(pm->checkError() < 0)
{
pm->m_pmState = pm->m_Fault_s99;
break;
}
//2. 检查上位机上电控制指令
switch (pm->m_msCmd.powerCMD)
{
case 0:
pm->m_pmEvent = "收到上位机控制指令:关机";
// pm->shutdown();
break;
case 1:
pm->m_pmEvent = "动力电上电";
// pm->powerOn();
//1.先上动力锂电池
pm->m_CcuColCmd.powerBatCmd = 0x10;
//2.检查燃料电池是否就绪，如果就绪则启动
if(pm->m_pmCurrentState.ccustate.fcuState = 0x04)
pm->m_CcuColCmd.fcuCmd = 0x02;
break;
case 2:
pm->m_pmEvent = "锂动力电启动";
pm->m_CcuColCmd.powerBatCmd = 0x10;
break;
case 3:
pm->m_pmEvent = "燃料电池启动";
pm->m_CcuColCmd.fcuCmd = 0x02;
break;
case 4:
pm->m_pmEvent = "锂动力电关闭";
pm->m_CcuColCmd.powerBatCmd = 0x20;
case 5:
pm->m_pmEvent = "燃料电池停机";
pm->m_CcuColCmd.fcuCmd = 0x03;
break;
case 6:
pm->m_pmEvent = "燃料电池预热指令";
pm->m_CcuColCmd.fcuCmd = 0x07;
break;
case 7:
pm->m_pmEvent = "燃料电池自检指令";
pm->m_CcuColCmd.fcuCmd = 0x01;
break;
case 8:
pm->m_pmEvent = "燃料电池紧急停机";
pm->m_CcuColCmd.fcuCmd = 0x05;
break;
case 9:
pm->m_pmEvent = "燃料电池复位指令";
pm->m_CcuColCmd.fcuCmd = 0x04;
break;
case 10:
pm->m_pmEvent = "燃料电池补给指令";
pm->m_CcuColCmd.fcuCmd = 0x06;
break;
case 11:
pm->m_pmEvent = "燃料电池停机维护";
pm->m_CcuColCmd.fcuCmd = 0x08;
break;
default:
break;
}
//2. 检查上位机模式切换指令
switch (pm->m_msCmd.modCMD)
{
case 0:
//无效
break;
case 1:
pm->m_pmEvent = "待机工况";
break;
case 2:
pm->m_pmEvent = "岸基备航";
break;
case 3:
pm->m_pmEvent = "水中备航";
break;
case 4:
pm->m_pmEvent = "遥控工况";
break;
case 5:
pm->m_pmEvent = "巡航模式";
break;
case 6:
pm->m_pmEvent = "高速模式";
break;
case 7:
pm->m_pmEvent = "上浮下潜";
break;
case 8:
pm->m_pmEvent = "浮调模式";
break;
case 9:
pm->m_pmEvent = "水下侦查";
break;
case 10:
pm->m_pmEvent = "水面侦查";
break;
case 11:
pm->m_pmEvent = "DJ模式";
break;
default:
break;
}
//3. 检查上位机工作模式切换控制指令
switch (pm->m_msCmd.workCMD)
{
case 0:
pm->m_pmEvent = "正常模式（默认）";
break;
case 1:
pm->m_pmEvent = "试验模式";
pm->currentMod = 1;
break;
case 2:
pm->m_pmEvent = "调试模式";
pm->currentMod = 2;
break;
}
MOOSPause(1000);
}
}
int PowerManger::handleFault_s99(FSM_State &fsm, void* pData)
{
// cout << fsm.name << endl;
PowerManger* pm = (PowerManger*)pData;
switch (pm->m_faultNum)
{
case 1:
pm->printState(fsm,"通信故障");
pm->m_pmEvent = "复合管控连接超时";
break;
default:
break;
}
pm->printState(fsm,"故障");
return 0;
}
//==============================系统操作函数====================