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

使用状态机增加了初始状态和故障状态,编写了gui调试程序

Browse Source
This commit is contained in:
zjk
2025-01-02 18:28:47 +08:00
parent d9d715326a
commit d2961cf005
19 changed files with 1085 additions and 196 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/pPowerManger/PowerManger.cpp
+105 -49
View File
@@ -9,7 +9,6 @@
#include "MBUtils.h"
#include "ACTable.h"
#include "PowerManger.h"
#include "MOOS/libMOOS/Utils/ConsoleColours.h"
#include <memory>
using namespace std;
@@ -32,33 +31,18 @@ PowerManger::PowerManger()
falutLevel = 0;
soc = 0;
sustainableTime = 0;
m_connected = false;
faultCode.push_back(0);
//注册状态机
m_fsm.RegisterState("PM_STATE", 1, m_state1);
m_fsm.RegisterState("PM_STATE2", 2, m_state2);
m_fsm.RegisterState("PM_STATE3", 3, m_state3);
m_fsm.RegisterState("Init", 1, m_Inite_s1);
m_fsm.RegisterState("Fault", 99, m_Fault_s99);
m_fsm.RegisterHandleFunction(&m_state1,handleState1);
m_fsm.RegisterHandleFunction(&m_state2,handleState2);
m_fsm.RegisterHandleFunction(&m_state3,handleState3);
// 注册状态机转换条件
// m_fsm.RegisterEvent(&m_state1, "a", &m_state2);
// m_fsm.RegisterEvent(&m_state2, "b", &m_state3);
// m_fsm.RegisterEvent(&m_state3, "c", &m_state1);
// cout << m_state1.event["a"]->name << endl;
// cout << m_state2.event["b"]->name << endl;
// cout << m_state1.event["a"]->id << endl;
// m_pmEvent = "a";
m_fsm.RegisterHandleFunction(&m_Inite_s1, handleInite_s1);
m_fsm.RegisterHandleFunction(&m_Fault_s99, handleFault_s99);
// 初始化状态机
m_pmState = m_state1;
m_pmState = m_Inite_s1;
}
//---------------------------------------------------------
@@ -115,20 +99,17 @@ bool PowerManger::OnConnectToServer()
bool PowerManger::Iterate()
{
m_fsm.run(m_pmState,this);
// m_fsm.run(&m_pmState,this,m_pmEvent);
cout <<"CURRENT STATE:" << m_pmState.name << " | CURRENT EVENT :" << m_pmEvent << endl;
// cout <<"____________________________________-" << m_pmState.event[m_pmEvent]->name << endl;
// cout << m_pmState.name << endl;
// cout << m_pmEvent << endl;
AppCastingMOOSApp::Iterate();
// 更新当前状态
//1. 更新当前状态
updatePmState();
updateTime();
//2. 运行状态机
m_fsm.run(m_pmState,this);
cout <<"CURRENT STATE:" << m_pmState.name << " | CURRENT EVENT :" << m_pmEvent << endl;
AppCastingMOOSApp::Iterate();
//3. 输出报告
buildPowerSysReport();
//输出错误信息
//4. 打印错误信息
if(!m_udpComm.sError.empty())
{
for(int i=0;i<m_udpComm.sError.size();i++)
@@ -138,6 +119,7 @@ bool PowerManger::Iterate()
m_udpComm.sError.clear();
}
//AppCastingMOOSApp::PostReport();
//5. 清空UDP消息队列
m_udpComm.clearMsgQueue();
return(true);
}
@@ -176,10 +158,13 @@ bool PowerManger::OnStartUp()
}
registerVariables();
//绑定UDP端口
if(m_pCcuUdpComm = setUdpScoket(IPORT))
{
cout<<MOOS::ConsoleColours::green()<< "UDP set succeed!"<< MOOS::ConsoleColours::reset() << endl;
m_udpComm.ccuHost = CCUHOST;
m_udpComm.ccuPort = CCUPORT;
m_udpComm.udpScoket = m_pCcuUdpComm;
MOOSTrace("UDP set succeed");
}
else
@@ -253,7 +238,9 @@ bool PowerManger::ListenLoopUDP()
cout<<MOOS::ConsoleColours::red()<< m_udpComm.sError[i] << MOOS::ConsoleColours::reset() << endl;
}
}
m_connected = true;
}
// updatePmState();
}
return true;
}
@@ -794,28 +781,97 @@ bool PowerManger::saveStringToJsonFile(const string &strJson, const string &file
return false;
}
}
//TODO:配电系统操作函数
bool PowerManger::disSystemOperation(disBreaker optBreaker, disBreakerOpt)
{
// switch (disBreaker)
// {
// case bus1_fuelCellCircuitBreaker :
// // send()
// break;
// default:
// break;
// }
}
int PowerManger::handleState1(FSM_State &fsm,void* pData)
int PowerManger::handleInite_s1(FSM_State &fsm, void* pData)
{
cout << fsm.name << endl;
PowerManger* pm = (PowerManger*)pData;
fsm = pm->m_fsm.getState("PM_STATE2");
// pm->m_pmEvent = "a";
//1. 检查复合管控器连接
double StartTime = pm->GetAppStartTime();
while (!pm->m_connected)
{
pm->updateTime();
double dft = abs(pm->m_current_time - StartTime);
if(dft > 1)
{
pm->m_pmState = pm->m_Fault_s99;
break;
}
string desp = "等待复合管控消息 : 超时 : " + to_string(10-dft);
pm->printState(fsm,desp);
usleep(1000000);
}
//2. 检查复合管控器是否上报故障 如果故障则进入故障状态
bool ok = true;
if(pm->m_pmCurrentState.ccustate.fcuGZ_1H != 0 ||
pm->m_pmCurrentState.ccustate.fcuGZ_1L != 0 ||
pm->m_pmCurrentState.ccustate.fcuGZ_2H != 0 ||
pm->m_pmCurrentState.ccustate.fcuGZ_2L != 0 ||
pm->m_pmCurrentState.ccustate.fcuGZ_3H != 0 ||
pm->m_pmCurrentState.ccustate.fcuGZ_3L != 0 ||
pm->m_pmCurrentState.ccustate.insEmergencyStatusWord != 0)
{
ok = false;
}
if(!ok)
{
pm->m_pmState = pm->m_Fault_s99;
}
//3. 检查当前配电装置状态
// 首先闭合应急电源开关
//TODO 检查并配置当前配电装置状态
disHighVolBusCmd a;
a.fuelCellCircuitBreaker = 0xAA;
a.propulsionMotorCircuitBreaker = 0xAA;
pm->m_udpComm.sendDisSysCmd(a);
MOOSPause(1000);
// pm->workCondition = "";
return 0;
}
int PowerManger::handleState2(FSM_State &fsm,void* pData)
int PowerManger::handleFault_s99(FSM_State &fsm, void* pData)
{
cout << fsm.name << endl;
PowerManger* pm = (PowerManger*)pData;
fsm = pm->m_fsm.getState("PM_STATE3");
// pm->m_pmEvent = "b";
return 0;
}
int PowerManger::handleState3(FSM_State &fsm,void* pData)
{
cout << fsm.name << endl;
PowerManger* pm = (PowerManger*)pData;
fsm = pm->m_fsm.getState("PM_STATE");
// pm->m_pmEvent = "c";
return 0;
}
// int PowerManger::handleState1(FSM_State &fsm,void* pData)
// {
// cout << fsm.name << endl;
// PowerManger* pm = (PowerManger*)pData;
// fsm = pm->m_fsm.getState("PM_STATE2");
// // pm->m_pmEvent = "a";
// return 0;
// }
// int PowerManger::handleState2(FSM_State &fsm,void* pData)
// {
// cout << fsm.name << endl;
// PowerManger* pm = (PowerManger*)pData;
// fsm = pm->m_fsm.getState("PM_STATE3");
// // pm->m_pmEvent = "b";
// return 0;
// }
// int PowerManger::handleState3(FSM_State &fsm,void* pData)
// {
// cout << fsm.name << endl;
// PowerManger* pm = (PowerManger*)pData;
// fsm = pm->m_fsm.getState("PM_STATE");
// // pm->m_pmEvent = "c";
// return 0;
// }
src/pPowerManger/PowerManger.h
+29 -11
View File
@@ -24,6 +24,7 @@
#include "MOOS/libMOOS/Utils/MOOSThread.h"
#include "MOOS/libMOOS/Comms/XPCUdpSocket.h"
#include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h"
#include "MOOS/libMOOS/Utils/ConsoleColours.h"
#include "ccuUdpMsg.h"
#include "json/json.h"
#include "udpComm.h"
@@ -33,6 +34,8 @@
#define VAR_TO_STR(var) #var
#define IPORT 5000
#define CCUPORT 5001
#define CCUHOST "127.0.0.1"
#define MAX_UDP_PKT_SIZE 65535
#define SKEW_TOLERANCE 5
@@ -61,17 +64,17 @@ class PowerManger : public AppCastingMOOSApp
protected: // app function
// bool _ListenCB();
XPCUdpSocket* setUdpScoket(long lPort);
bool updatePmState();
bool updatePmState();
void updateTime(){m_current_time = MOOSTime();}
bool IsSkewed(double dfTimeNow, double * pdfSkew = NULL);
string getTypeName(const string typeName);
//通信相关
XPCUdpSocket* setUdpScoket(long lPort);
bool disSystemOperation(disBreaker optBreaker, disBreakerOpt);
bool buildPowerSysReport();
bool buildDisributionReport();
bool buildSubDistributionReport();
bool buildPredictionReport();
protected: // app variables
//系统状态
@@ -81,8 +84,9 @@ class PowerManger : public AppCastingMOOSApp
unsigned char falutLevel;
vector<unsigned int> faultCode;
unsigned int soc;
unsigned long int sustainableTime;
unsigned int sustainableTime;
pmState m_pmCurrentState;
bool m_connected;
// 当前传输状态
msg_CcuStateFbMsg m_CcuCurrentState;
msg_disHighVolBusFbMsg m_disHighVolBusCurrentState;
@@ -105,16 +109,30 @@ class PowerManger : public AppCastingMOOSApp
public: //状态实现
FSM_State m_pmState;
void printState(FSM_State st, string desp){
cout << MOOS::ConsoleColours::Yellow() << "STATE " << st.id << " [" << st.name << "] " << MOOS::ConsoleColours::reset();
cout << MOOS::ConsoleColours::Blue() << " -> " << MOOS::ConsoleColours::reset();
cout << MOOS::ConsoleColours::Green() << desp << MOOS::ConsoleColours::reset() << endl;
}
FSM_State m_pmState; //当前状态
string m_pmEvent;
FSM m_fsm;
//系统状态定义
FSM_State m_state1;
FSM_State m_state2;
FSM_State m_state3;
static int handleState1(FSM_State &fsm, void* pData);
static int handleState2(FSM_State &fsm, void* pData);
static int handleState3(FSM_State &fsm, void* pData);
FSM_State startState;
FSM_State stopState;
//初始化状态
FSM_State m_Inite_s1;
static int handleInite_s1(FSM_State &fsm, void* pData);
//故障处理
FSM_State m_Fault_s99;
static int handleFault_s99(FSM_State &fsm, void* pData);
};
src/pPowerManger/ccuUdpMsg.h
+70 -37
View File
@@ -16,10 +16,15 @@
#define DIS_HVBUSB_FB_ID 0x0007
#define DIS_LVBUS_FB_ID 0x0008
#pragma pack (1)
#define DIS_HVBUS_CMD_ID 0x0001
#define DIS_HVBUSA_CMD_ID 0x0002
#define DIS_HVBUSB_CMD_ID 0x0003
#define DIS_LVBUS_CMD_ID 0x0004
typedef unsigned int Checksum;
typedef unsigned char cChecksum;
#pragma pack (1)
typedef struct {
unsigned char start1;
unsigned char start2;
@@ -68,43 +73,71 @@ typedef struct {
} msg_CcuSetParmMsg;
// 配电指令
typedef struct {
typedef struct
{
m_header header;
unsigned char dcb1;
unsigned char dcb2;
unsigned char dcb3;
unsigned char dcb4;
unsigned char dcb5;
unsigned char sw1;
unsigned char sw2;
unsigned char qf1;
unsigned char qf2;
unsigned char qf3;
unsigned char qf4;
unsigned char qf5;
unsigned char qf6;
unsigned char qf7;
unsigned char qf8;
unsigned char qf9;
unsigned char qf10;
unsigned char qf11;
unsigned char qf12;
unsigned char qf13;
unsigned char qf14;
unsigned char qf15;
unsigned char qf16;
unsigned char qf17;
unsigned char qf18;
unsigned char qf19;
unsigned char qf20;
unsigned char qf21;
unsigned char qf22;
unsigned char qf23;
unsigned char qf24;
unsigned char qf25;
unsigned char qf26;
Checksum checkCode;
} msg_disSysCmdMsg;
disHighVolBusCmd data;
cChecksum checkCode;
}msg_disHVBCmd;
typedef struct
{
m_header header;
disHighAVolBusCmd data;
cChecksum checkCode;
}msg_disHVBACmd;
typedef struct
{
m_header header;
disHighBVolBusCmd data;
cChecksum checkCode;
}msg_disHVBBCmd;
typedef struct
{
m_header header;
disLowBusCmd data;
cChecksum checkCode;
}msg_disLVBCmd;
// typedef struct {
// m_header header;
// unsigned char dcb1;
// unsigned char dcb2;
// unsigned char dcb3;
// unsigned char dcb4;
// unsigned char dcb5;
// unsigned char sw1;
// unsigned char sw2;
// unsigned char qf1;
// unsigned char qf2;
// unsigned char qf3;
// unsigned char qf4;
// unsigned char qf5;
// unsigned char qf6;
// unsigned char qf7;
// unsigned char qf8;
// unsigned char qf9;
// unsigned char qf10;
// unsigned char qf11;
// unsigned char qf12;
// unsigned char qf13;
// unsigned char qf14;
// unsigned char qf15;
// unsigned char qf16;
// unsigned char qf17;
// unsigned char qf18;
// unsigned char qf19;
// unsigned char qf20;
// unsigned char qf21;
// unsigned char qf22;
// unsigned char qf23;
// unsigned char qf24;
// unsigned char qf25;
// unsigned char qf26;
// Checksum checkCode;
// } msg_disSysCmdMsg;
// 复合管控参数设定反馈指令
typedef struct {
src/pPowerManger/fsm.h
+1 -1
View File
@@ -10,7 +10,7 @@ typedef struct FSM_State
std::string name;
unsigned int id;
std::map<std::string,FSM_State*> event;
};
}FSM_State;
typedef int (*FSM_Handle)(FSM_State &fsm, void *data);
// typedef int (*FSM_EventHandle)(FSM_State &fsm,std::string event,void *data);
src/pPowerManger/pmSysvariable.h
+97
View File
@@ -196,6 +196,57 @@ typedef struct disLowBusState
unsigned short emergencyLithiumBatteryGroup1Current;
unsigned short emergency1BusbarVoltage;
}disLowBusState;
typedef struct disHighVolBusCmd
{
unsigned char fuelCellCircuitBreaker;
unsigned char powerLithiumBatteryCircuitBreaker;
unsigned char propulsionMotorCircuitBreaker;
unsigned char lithiumBatteryGroupInstrumentCircuitBreaker;
unsigned char dcDc5ModuleCircuitBreaker;
unsigned char bowHighVoltageDistributionBoxCircuitBreaker;
unsigned char sternFTDevice45CircuitBreaker;
unsigned char sternRudderSwitch1CircuitBreaker;
unsigned char sternRudderSwitch2CircuitBreaker;
unsigned char fbReservedCircuitBreaker;
unsigned char tyzReservedCircuitBreaker;
unsigned char reservedCircuitBreaker;
}disHighVolBusCmd;
typedef struct disHighAVolBusCmd
{
unsigned char bowFTDevice123CircuitBreaker;
unsigned char actuatorCircuitBreaker;
unsigned char mastSteeringGearControlBoxCircuitBreaker;
unsigned char xczCircuitBreaker;
unsigned char bowRudderControlBoxCircuitBreaker;
unsigned char openWaterCoverStartCylinderCircuitBreaker;
}disHighAVolBusCmd;
typedef struct disHighBVolBusCmd
{
unsigned char lithiumBatteryGroupInstrumentCircuitBreaker;
unsigned char bowLowVoltageDistributionBoxCircuitBreaker;
unsigned char unit4InstrumentDC48VCircuitBreaker;
unsigned char dcC1DCDistributionPanelCircuitBreaker;
unsigned char reservedCircuitBreaker1; // 为了区分，添加了数字后缀
unsigned char reservedCircuitBreaker2; // 为了区分，添加了数字后缀
unsigned char emergencyLithiumBatteryGroup2CircuitBreaker;
}disHighBVolBusCmd;
typedef struct disLowBusCmd
{
unsigned char unit1CircuitBreaker;
unsigned char unit2CircuitBreaker;
unsigned char unit3CircuitBreaker;
unsigned char unit5CircuitBreaker;
unsigned char bowPZDeviceCircuitBreaker;
unsigned char reservedCircuitBreaker1; // 为了区分，添加了数字后缀
unsigned char reservedCircuitBreaker2; // 为了区分，添加了数字后缀
unsigned char emergencyLithiumBatteryGroup1CircuitBreaker;
}disLowBusCmd;
#pragma pack (0)
@@ -270,4 +321,50 @@ typedef struct pmState
/* data */
} disBus4Breaker;
enum disBreaker{
bus1_fuelCellCircuitBreaker,
bus1_powerLithiumBatteryCircuitBreaker,
bus1_propulsionMotorCircuitBreaker,
bus1_lithiumBatteryGroupInstrumentCircuitBreaker,
bus1_dcDc5ModuleCircuitBreaker,
bus1_bowHighVoltageDistributionBoxCircuitBreaker,
bus1_sternFTDevice45CircuitBreaker,
bus1_sternRudderSwitch1CircuitBreaker,
bus1_sternRudderSwitch2CircuitBreaker,
bus1_fbReservedCircuitBreaker,
bus1_tyzReservedCircuitBreaker,
bus1_reservedCircuitBreaker,
bus2_bowFTDevice123CircuitBreaker,
bus2_actuatorCircuitBreaker,
bus2_mastSteeringGearControlBoxCircuitBreaker,
bus2_xczCircuitBreaker,
bus2_bowRudderControlBoxCircuitBreaker,
bus2_openWaterCoverStartCylinderCircuitBreaker,
bus3_lithiumBatteryGroupInstrumentCircuitBreaker,
bus3_bowLowVoltageDistributionBoxCircuitBreaker,
bus3_unit4InstrumentDC48VCircuitBreaker,
bus3_dcC1DCDistributionPanelCircuitBreaker,
bus3_reservedCircuitBreaker1,
bus3_reservedCircuitBreaker2,
bus3_emergencyLithiumBatteryGroup2CircuitBreaker,
bus4_unit1CircuitBreaker,
bus4_unit2CircuitBreaker,
bus4_unit3CircuitBreaker,
bus4_unit5CircuitBreaker,
bus4_bowPZDeviceCircuitBreaker,
bus4_reservedCircuitBreaker1,
bus4_reservedCircuitBreaker2
};
enum disBreakerOpt{
hold,
turn_off,
turn_on
};
#endif
src/pPowerManger/udpComm.cpp
+102
View File
@@ -366,4 +366,106 @@ bool udpComm::popMsgFormQueue(double &time,msg_disLowBusFbMsg &m)
m = m_qReceiveDisLowBusBuffer.front().second;
m_qReceiveDisLowBusBuffer.pop();
return true;
}
bool udpComm::sendDisSysCmd(const disHighVolBusCmd cmd)
{
unsigned char* msgBuf;
Checksum cc;
msg_disHVBCmd* msg = new msg_disHVBCmd;
msg->header.id = DIS_HVBUS_CMD_ID;
msg->header.start1 = DIS_MSG_HEAD1;
msg->header.start2 = DIS_MSG_HEAD2;
msg->header.length = sizeof(disHighVolBusCmd);
msg->data = cmd;
msgBuf = reinterpret_cast<unsigned char *>(msg);
cc = calculateChecksum(msgBuf,sizeof(msg_disHVBCmd));
msg->checkCode = static_cast<cChecksum>(cc & 0xFF);
std::cout << (int)sizeof(msg_disHVBCmd) << endl;
try
{
sendMsg(msgBuf,sizeof(msg_disHVBCmd));
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
delete msg;
return true;
}
bool udpComm::snedDisSysCmd(const disHighAVolBusCmd cmd)
{
unsigned char* msgBuf;
Checksum cc;
msg_disHVBACmd* msg = new msg_disHVBACmd;
msg->header.id = DIS_HVBUS_CMD_ID;
msg->header.start1 = DIS_MSG_HEAD1;
msg->header.start2 = DIS_MSG_HEAD2;
msg->header.length = sizeof(disHighAVolBusCmd);
msg->data = cmd;
msgBuf = reinterpret_cast<unsigned char *>(msg);
cc = calculateChecksum(msgBuf,sizeof(msg_disHVBACmd));
msg->checkCode = static_cast<cChecksum>(cc & 0xFF);
try
{
sendMsg(msgBuf,sizeof(msg_disHVBACmd));
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
delete msg;
return true;
}
bool udpComm::snedDisSysCmd(const disHighBVolBusCmd cmd)
{
unsigned char* msgBuf;
Checksum cc;
msg_disHVBBCmd* msg = new msg_disHVBBCmd;
msg->header.id = DIS_HVBUS_CMD_ID;
msg->header.start1 = DIS_MSG_HEAD1;
msg->header.start2 = DIS_MSG_HEAD2;
msg->header.length = sizeof(disHighBVolBusCmd);
msg->data = cmd;
msgBuf = reinterpret_cast<unsigned char *>(msg);
cc = calculateChecksum(msgBuf,sizeof(msg_disHVBBCmd));
msg->checkCode = static_cast<cChecksum>(cc & 0xFF);
try
{
sendMsg(msgBuf,sizeof(msg_disHVBBCmd));
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
delete msg;
return true;
}
bool udpComm::sendDisSysCmd(const disLowBusCmd cmd)
{
unsigned char* msgBuf;
Checksum cc;
msg_disLVBCmd* msg = new msg_disLVBCmd;
msg->header.id = DIS_HVBUS_CMD_ID;
msg->header.start1 = DIS_MSG_HEAD1;
msg->header.start2 = DIS_MSG_HEAD2;
msg->header.length = sizeof(disLowBusCmd);
msg->data = cmd;
msgBuf = reinterpret_cast<unsigned char *>(msg);
cc = calculateChecksum(msgBuf,sizeof(msg_disLVBCmd));
msg->checkCode = static_cast<cChecksum>(cc & 0xFF);
try
{
sendMsg(msgBuf,sizeof(msg_disLVBCmd));
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
delete msg;
return true;
}
src/pPowerManger/udpComm.h
+15
View File
@@ -1,11 +1,15 @@
#ifndef UDPCOMM__H
#define UDPCOMM__H
#define UNIX
#include "ccuUdpMsg.h"
#include <string>
#include <vector>
#include <queue>
#include <string.h>
#include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h"
#include "MOOS/libMOOS/Comms/XPCUdpSocket.h"
#include <iostream>
#include <utility> // for std::pair
@@ -18,6 +22,10 @@ private:
public:
udpComm();
~udpComm();
XPCUdpSocket *udpScoket;
string ccuHost;
long int ccuPort;
bool pushMsgToQueue(unsigned char *m);
bool clearMsgQueue();
@@ -38,6 +46,13 @@ public:
bool sendCcuColCmd(const msg_CcuColCmdMsg m);
bool sendCcuSetParmCmd(const msg_CcuSetParmMsg m);
bool sendDisSysCmd(const disHighVolBusCmd cmd);
bool snedDisSysCmd(const disHighAVolBusCmd cmd);
bool snedDisSysCmd(const disHighBVolBusCmd cmd);
bool sendDisSysCmd(const disLowBusCmd cmd);
bool sendMsg(const unsigned char* msg,unsigned int size){
udpScoket->iSendMessageTo((void*) msg, size, ccuPort, ccuHost);
}
bool getMsgId(char *m,unsigned short &id);
Checksum calculateChecksum(const unsigned char* data, int size);