zjk
2590282aed
build-test-deploy / ci (push) Successful in 1h13m13s
FC协议(0827 docx)： - 控制指令域：字节12,13/16,17由纵倾/横倾姿态数据2改为预留（编码恒0、解码忽略） - 状态反馈域：字节147,148由预留12改为剩余发电量(MWh)，贯通JSON/网页显示 - PM→CCU链路：姿态字段4xuchar改为2xshort(数据1)，结构体大小与线上布局不变 - 同步更新：CCU转发映射、SnapshotBuilder、pCCU网页、fuelcell网页、SQLite列描述、单测/模拟器 - 修正集成测试遗留断言(20B→24B、心跳d[19]→d[23])并增强预留字节校验 CCU地址可配置： - pPowerManger 支持 moos 配置项 ccuhost/ccuport（默认127.0.0.1:7000） - build-board.sh 增加 --ccu-host/--ccu-port 参数 文档：FC协议文档更新为0827版，清理过期协议docx
481 lines
15 KiB
C++
481 lines
15 KiB
C++
/************************************************************/
|
||
/* NAME: */
|
||
/* ORGN: MIT, Cambridge MA */
|
||
/* FILE: PowerManger.cpp */
|
||
/* DATE: December 29th, 1963 */
|
||
/************************************************************/
|
||
|
||
#include <iterator>
|
||
#include "MBUtils.h"
|
||
#include "ACTable.h"
|
||
#include "PowerManger.h"
|
||
#include "logc/loguru.hpp"
|
||
#include <memory>
|
||
|
||
using namespace std;
|
||
|
||
|
||
bool _FsmCB(void * pParam)
|
||
{
|
||
PowerManger* pMe = (PowerManger* )pParam;
|
||
return pMe->FsmLoop();
|
||
}
|
||
|
||
bool _CommCB(void * pParam)
|
||
{
|
||
PowerManger* pMe = (PowerManger* )pParam;
|
||
return pMe->commLoop();
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
// Constructor()
|
||
|
||
PowerManger::PowerManger()
|
||
{
|
||
workCondition = 0;
|
||
currentMod = 0;
|
||
powerState = 0;
|
||
faultLevel = 0;
|
||
soc = 0;
|
||
sustainableTime = 0;
|
||
m_faultNum = 0;
|
||
m_ticker = 0;
|
||
m_leakage = 0;
|
||
m_thrustRpm = 0;
|
||
m_thrustEnable = 0;
|
||
m_insData = {};
|
||
faultCode.clear();
|
||
|
||
m_skew = 10;
|
||
|
||
// CCU 地址默认值，可在 moos 文件中用 ccuhost/ccuport 覆盖
|
||
m_ccuHost = CCUHOST;
|
||
m_ccuPort = CCUPORT;
|
||
|
||
m_pmCurrentState = {};
|
||
m_currentDisBreakerState = {};
|
||
m_targetDisBreakerState = {};
|
||
m_currentDriverState = {};
|
||
m_MSDriverCmd = {};
|
||
m_subDisSysCmd = {};
|
||
m_subDisSysState = {};
|
||
m_subDisSysFault = {};
|
||
|
||
//初始化系统数据
|
||
m_systemData = new SystemData();
|
||
|
||
//初始化控制指令
|
||
initControlCmd();
|
||
|
||
//初始化下位机通信
|
||
m_lowerCommManager = new LowerCommManager(this,IPORT,m_ccuPort,m_ccuHost);
|
||
m_lowerCommManager->initUdpComm();
|
||
|
||
//初始化上位机通信
|
||
m_upperCommManager = new UpperCommManager(this);
|
||
|
||
//数据库在 OnStartUp 读取配置后初始化
|
||
m_db = nullptr;
|
||
|
||
//初始化http服务器
|
||
m_httpServer = new HttpServer(8090);
|
||
m_httpServer->setPowerManger(this);
|
||
m_httpServer->setLowerCommManager(m_lowerCommManager);
|
||
m_httpServer->setSystemData(m_systemData);
|
||
m_httpServer->start();
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
// Destructor
|
||
|
||
PowerManger::~PowerManger()
|
||
{
|
||
m_commThread.Stop();
|
||
// 必须先停 HTTP 线程（stop() 内部 pthread_join 阻塞等待线程退出），
|
||
// 否则 HTTP 线程每秒仍会读取 m_systemData / m_lowerCommManager / m_upperCommManager，
|
||
// 导致 use-after-free。
|
||
delete m_httpServer;
|
||
delete m_lowerCommManager; // 停止监听线程，确保其不再调用 DB
|
||
delete m_db;
|
||
delete m_upperCommManager;
|
||
delete m_systemData; // 最后删除，此时无任何线程引用
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
// Procedure: OnNewMail()
|
||
|
||
bool PowerManger::OnNewMail(MOOSMSG_LIST &NewMail)
|
||
{
|
||
AppCastingMOOSApp::OnNewMail(NewMail);
|
||
|
||
MOOSMSG_LIST::iterator p;
|
||
for(p=NewMail.begin(); p!=NewMail.end(); p++)
|
||
{
|
||
CMOOSMsg &msg = *p;
|
||
string key = msg.GetKey();
|
||
string strJson = msg.GetString();
|
||
double dfT;
|
||
msg.IsSkewed(m_curr_time, &dfT);
|
||
if(fabs(dfT) > m_skew)
|
||
{
|
||
LOG_F(INFO, "Message from %s at time %f is Skewed", msg.GetSource().c_str(), dfT);
|
||
return true;
|
||
}
|
||
//处理上位机发送的控制指令
|
||
if(!m_upperCommManager->processUpperMsg(key,strJson))
|
||
{
|
||
LOG_F(ERROR, "上位机消息处理错误: %s", key.c_str());
|
||
}
|
||
|
||
#if 0 // Keep these around just for template
|
||
string comm = msg.GetCommunity();
|
||
double dval = msg.GetDouble();
|
||
string sval = msg.GetString();
|
||
string msrc = msg.GetSource();
|
||
double mtime = msg.GetTime();
|
||
bool mdbl = msg.IsDouble();
|
||
bool mstr = msg.IsString();
|
||
#endif
|
||
|
||
if(key == "FOO")
|
||
LOG_F(INFO, "Received FOO message");
|
||
|
||
else if(key != "APPCAST_REQ") // handled by AppCastingMOOSApp
|
||
reportRunWarning("Unhandled Mail: " + key);
|
||
}
|
||
|
||
return(true);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
// Procedure: OnConnectToServer()
|
||
|
||
bool PowerManger::OnConnectToServer()
|
||
{
|
||
registerVariables();
|
||
return(true);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
// Procedure: Iterate()
|
||
// happens AppTick times per second
|
||
//固定为4Hz
|
||
bool PowerManger::Iterate()
|
||
{
|
||
AppCastingMOOSApp::Iterate();
|
||
// 触发定时任务
|
||
CycleTriggeredEvent cycleEvent;
|
||
PowerManagerFsm::dispatch(cycleEvent);
|
||
faultLevel = getFaultLevel();
|
||
m_systemData->updateCommonStatus();
|
||
// m_CcuColCmd.heartbeat
|
||
// if (m_ticker++ % 5 == 0)
|
||
// {
|
||
// m_CcuColCmd.heartbeat++;
|
||
// m_lowerCommManager->processPeriodicTasks();
|
||
// m_upperCommManager->processPeriodicTasks();
|
||
// }
|
||
return(true);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
// Procedure: OnStartUp()
|
||
// happens before connection is open
|
||
|
||
bool PowerManger::OnStartUp()
|
||
{
|
||
AppCastingMOOSApp::OnStartUp();
|
||
|
||
STRING_LIST sParams;
|
||
m_MissionReader.EnableVerbatimQuoting(false);
|
||
if(!m_MissionReader.GetConfiguration(GetAppName(), sParams))
|
||
reportConfigWarning("No config block found for " + GetAppName());
|
||
|
||
std::string dbPath; // 数据库路径，不配置则默认当前目录 power_data.db
|
||
std::string logPath; // 日志文件路径，不配置则默认当前目录 pPowerManger.log
|
||
std::string ccuHost; // CCU 地址，不配置则沿用默认值
|
||
long ccuPort = 0; // CCU 端口，不配置则沿用默认值
|
||
|
||
STRING_LIST::iterator p;
|
||
for(p=sParams.begin(); p!=sParams.end(); p++) {
|
||
string orig = *p;
|
||
string line = *p;
|
||
string param = stripBlankEnds(tolower(biteStringX(line, '=')));
|
||
string value = stripBlankEnds(line);
|
||
|
||
bool handled = false;
|
||
if(param == "foo") {
|
||
handled = true;
|
||
}
|
||
else if(param == "bar") {
|
||
handled = true;
|
||
}
|
||
else if(param == "dbpath") {
|
||
dbPath = value;
|
||
handled = true;
|
||
}
|
||
else if(param == "logpath") {
|
||
logPath = value;
|
||
handled = true;
|
||
}
|
||
else if(param == "ccuhost") {
|
||
ccuHost = value;
|
||
handled = true;
|
||
}
|
||
else if(param == "ccuport") {
|
||
ccuPort = atoi(value.c_str());
|
||
handled = true;
|
||
}
|
||
|
||
if(!handled)
|
||
reportUnhandledConfigWarning(orig);
|
||
|
||
}
|
||
|
||
registerVariables();
|
||
|
||
// 初始化日志文件：logpath 未配置则默认当前目录
|
||
if (logPath.empty()) logPath = "pPowerManger.log";
|
||
loguru::add_file(logPath.c_str(), loguru::Append, loguru::Verbosity_MAX);
|
||
LOG_F(INFO, "日志文件路径: %s", logPath.c_str());
|
||
|
||
// 初始化数据库：dbpath 未配置则默认当前目录
|
||
if (dbPath.empty()) dbPath = "power_data.db";
|
||
LOG_F(INFO, "数据库存储路径: %s", dbPath.c_str());
|
||
m_db = new SQLite(dbPath);
|
||
m_db->createTable();
|
||
|
||
// 应用 moos 文件中的 CCU 地址配置（在监听/通信线程启动前生效）
|
||
if (!ccuHost.empty()) m_ccuHost = ccuHost;
|
||
if (ccuPort > 0) m_ccuPort = ccuPort;
|
||
m_lowerCommManager->setCcuAddress(m_ccuHost, m_ccuPort);
|
||
LOG_F(INFO, "CCU 地址: %s:%ld", m_ccuHost.c_str(), m_ccuPort);
|
||
|
||
// 注入原始帧日志回调（RX/TX 双向落库）
|
||
m_lowerCommManager->setCommLogger(m_db);
|
||
// 数据库就绪后再启动下位机监听线程
|
||
m_lowerCommManager->startListenThread();
|
||
|
||
// 初始化状态机
|
||
PowerManagerFsm::init(this);
|
||
PowerManagerFsm::start();
|
||
InitEvent initEvent;
|
||
PowerManagerFsm::dispatch(initEvent);
|
||
|
||
//初始化通信线程
|
||
m_commThread.Initialise(_CommCB,this);
|
||
m_commThread.Start();
|
||
return(true);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
// Procedure: registerVariables()
|
||
|
||
void PowerManger::registerVariables()
|
||
{
|
||
AppCastingMOOSApp::RegisterVariables();
|
||
// Register(DB_TO_PM_SUBDISSTATE, 0);
|
||
|
||
Register(DB_TO_PM_MSCMD, 0);
|
||
Register(DB_TO_PM_EQUIPMENTCMD, 0);
|
||
Register(m_upperCommManager->m_secdistStateMsg.key_fb,0);
|
||
Register(DB_TO_PM_MISSION,0);
|
||
Register(DB_TO_PM_DEEPTH,0);
|
||
Register(DB_TO_PM_INSDATA,0);
|
||
Register(DB_TO_PM_SETOPCONDITION,0);
|
||
Register(DB_TO_PM_SETOPMODE,0);
|
||
Register(DB_TO_PM_SETPMSYS,0);
|
||
Register(DB_TO_PM_SETDEVSWITCH,0);
|
||
Register(DB_TO_PM_TASKSTOP,0);
|
||
|
||
Register(uDetect_xcVoltage_cmd::key,0);
|
||
Register(uDetect_zdpVoltage_cmd::key,0);
|
||
Register(uDevice_thruster_st::key,0);
|
||
// Register(uExternComm_setThrustSpeed_cmd::key,0);
|
||
}
|
||
|
||
|
||
//------------------------------------------------------------
|
||
// Procedure: buildReport()
|
||
|
||
bool PowerManger::buildReport()
|
||
{
|
||
m_msgs << "============================================" << endl;
|
||
m_msgs << "File: " << endl;
|
||
m_msgs << "============================================" << endl;
|
||
|
||
ACTable actab(4);
|
||
actab << "Alpha | Bravo | Charlie | Delta";
|
||
actab.addHeaderLines();
|
||
actab << "one" << "two" << "three" << "four";
|
||
m_msgs << actab.getFormattedString();
|
||
|
||
return(true);
|
||
}
|
||
|
||
|
||
|
||
bool PowerManger::FsmLoop()
|
||
{
|
||
return true;
|
||
}
|
||
|
||
int PowerManger::checkError()
|
||
{
|
||
//TODO：更新协议
|
||
//1.检查复合管控器状态
|
||
// if(m_pmCurrentState.ccustate.fcuGZ_1H != 0 ||
|
||
// m_pmCurrentState.ccustate.fcuGZ_1L != 0 ||
|
||
// m_pmCurrentState.ccustate.fcuGZ_2H != 0 ||
|
||
// m_pmCurrentState.ccustate.fcuGZ_2L != 0 ||
|
||
// m_pmCurrentState.ccustate.fcuGZ_3H != 0 ||
|
||
// m_pmCurrentState.ccustate.fcuGZ_3L != 0 ||
|
||
// m_pmCurrentState.ccustate.insEmergencyStatusWord != 0)
|
||
return 0;
|
||
}
|
||
|
||
void PowerManger::printState(const std::string& stateName, const std::string& desp){
|
||
if (desp.find("故障") != string::npos) {
|
||
LOG_F(ERROR, "STATE %s -> %s", stateName.c_str(), desp.c_str());
|
||
} else {
|
||
LOG_F(INFO, "STATE %s -> %s", stateName.c_str(), desp.c_str());
|
||
}
|
||
}
|
||
|
||
|
||
void PowerManger::initControlCmd()
|
||
{
|
||
//================初始化配电指令=============================
|
||
m_targetDisBreakerState.bus1Breaker.fuelCellCircuitBreaker = 0;
|
||
m_targetDisBreakerState.bus1Breaker.powerLithiumBatteryCircuitBreaker = 1; //动力锂电池默认接入
|
||
m_targetDisBreakerState.bus1Breaker.propulsionMotorCircuitBreaker = 0;
|
||
m_targetDisBreakerState.bus1Breaker.lithiumBatteryGroupInstrumentCircuitBreaker = 1; //仪表锂电池默认接入
|
||
m_targetDisBreakerState.bus1Breaker.dcDc5ModuleCircuitBreaker = 1; //DC-DC模块默认接入
|
||
m_targetDisBreakerState.bus1Breaker.bowHighVoltageDistributionBoxCircuitBreaker = 1; //艏部高压母线断路器默认接入
|
||
m_targetDisBreakerState.bus1Breaker.sternFTDevice45CircuitBreaker = 0; // sternFTDevice45CircuitBreaker默认接入
|
||
m_targetDisBreakerState.bus1Breaker.sternRudderSwitch1CircuitBreaker = 0;
|
||
m_targetDisBreakerState.bus1Breaker.sternRudderSwitch2CircuitBreaker = 0;
|
||
m_targetDisBreakerState.bus1Breaker.fbReservedCircuitBreaker = 0;
|
||
m_targetDisBreakerState.bus1Breaker.tyzReservedCircuitBreaker = 0;
|
||
m_targetDisBreakerState.bus1Breaker.reservedCircuitBreaker = 0;
|
||
|
||
m_targetDisBreakerState.bus2Breaker.bowFTDevice123CircuitBreaker = 0;
|
||
m_targetDisBreakerState.bus2Breaker.actuatorCircuitBreaker = 0;
|
||
m_targetDisBreakerState.bus2Breaker.mastSteeringGearControlBoxCircuitBreaker = 0;
|
||
m_targetDisBreakerState.bus2Breaker.xczCircuitBreaker = 0;
|
||
m_targetDisBreakerState.bus2Breaker.bowRudderControlBoxCircuitBreaker = 0;
|
||
m_targetDisBreakerState.bus2Breaker.openWaterCoverStartCylinderCircuitBreaker = 0;
|
||
//锂电池默认接入
|
||
m_targetDisBreakerState.bus3Breaker.lithiumBatteryGroupInstrumentCircuitBreaker = 1;
|
||
m_targetDisBreakerState.bus3Breaker.bowLowVoltageDistributionBoxCircuitBreaker = 1;
|
||
m_targetDisBreakerState.bus3Breaker.unit4InstrumentDC48VCircuitBreaker = 1;
|
||
m_targetDisBreakerState.bus3Breaker.dcC1DCDistributionPanelCircuitBreaker = 1;
|
||
m_targetDisBreakerState.bus3Breaker.reservedCircuitBreaker1 = 0;
|
||
m_targetDisBreakerState.bus3Breaker.reservedCircuitBreaker2 = 0;
|
||
m_targetDisBreakerState.bus3Breaker.emergencyLithiumBatteryGroup2CircuitBreaker = 1;
|
||
|
||
m_targetDisBreakerState.bus4Breaker.unit1CircuitBreaker = 1;
|
||
m_targetDisBreakerState.bus4Breaker.unit2CircuitBreaker = 1;
|
||
m_targetDisBreakerState.bus4Breaker.unit3CircuitBreaker = 1;
|
||
m_targetDisBreakerState.bus4Breaker.unit5CircuitBreaker = 1;
|
||
m_targetDisBreakerState.bus4Breaker.bowPZDeviceCircuitBreaker = 1;
|
||
m_targetDisBreakerState.bus4Breaker.reservedCircuitBreaker1 = 0;
|
||
m_targetDisBreakerState.bus4Breaker.reservedCircuitBreaker2 = 0;
|
||
m_targetDisBreakerState.bus4Breaker.emergencyLithiumBatteryGroup1CircuitBreaker = 1; //紧急锂电池默认接入
|
||
//================初始化复合管控=============================
|
||
m_CcuColCmd.fcmode = 0x00; //默认无效
|
||
m_CcuColCmd.fcuCmd = 0x00; //无效指令
|
||
m_CcuColCmd.fcuPowerEfficiency = 0; //输出功率指令，0~250，分辨率0.1kW
|
||
m_CcuColCmd.pitchData1 = 0; //纵倾姿态数据1（int16，分辨率0.1°；0824协议：姿态数据2已删除）
|
||
m_CcuColCmd.rollData1 = 0; //横倾姿态数据1
|
||
m_CcuColCmd.emergencyAllow = 0; //应急允许
|
||
m_CcuColCmd.depth = 0; //潜深深度
|
||
m_CcuColCmd.supplyExhaustCmd = 0x00; //补给/排放指令
|
||
m_CcuColCmd.reservedCmd5 = 0; //预留指令5
|
||
m_CcuColCmd.reservedCmd6 = 0; //预留指令6
|
||
m_CcuColCmd.insBatCmd = 0x00; //无效指令
|
||
m_CcuColCmd.powerBatCmd = 0x00; //无效指令
|
||
m_CcuColCmd.powerBatEfficiency = 0; //动力锂电池功率配置值
|
||
m_CcuColCmd.heartbeat = 0;
|
||
m_CcuColCmd.masterState = 0xAA;//主站状态,运行中
|
||
//================初始化上位机指令=========================
|
||
m_msCmd.modCMD = 99;
|
||
m_msCmd.powerCMD = 99;
|
||
m_msCmd.workCMD = 99;
|
||
//================初始化设备驱动指令========================
|
||
|
||
}
|
||
|
||
void PowerManger::addFaultCode(unsigned int code)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_stateMutex);
|
||
faultCode.push_back(code);
|
||
}
|
||
|
||
void PowerManger::clearFaultCodes()
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_stateMutex);
|
||
faultCode.clear();
|
||
}
|
||
|
||
std::vector<unsigned int> PowerManger::getFaultCodes() const
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_stateMutex);
|
||
return faultCode;
|
||
}
|
||
|
||
unsigned char PowerManger::getFaultLevel()
|
||
{
|
||
unsigned char maxLevel = 0;
|
||
for (auto code : getFaultCodes()) {
|
||
// 提取0xabcd中的b位 (右移8位后取低4位)
|
||
unsigned char level = (code >> 8) & 0xF;
|
||
if (level > maxLevel) {
|
||
maxLevel = level;
|
||
}
|
||
}
|
||
return maxLevel;
|
||
}
|
||
|
||
bool PowerManger::commLoop()
|
||
{
|
||
while(!m_commThread.IsQuitRequested())
|
||
{
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_stateMutex);
|
||
m_CcuColCmd.heartbeat++;
|
||
}
|
||
m_lowerCommManager->processPeriodicTasks();
|
||
m_upperCommManager->processPeriodicTasks();
|
||
// 延时1秒
|
||
usleep(1000000);
|
||
}
|
||
return true;
|
||
}
|
||
|
||
void PowerManger::injectFault(unsigned type,unsigned int faultCode){
|
||
LOG_F(INFO, "注入故障: %d %d", type, faultCode);
|
||
TestEvent testEvent("故障注入测试",type,faultCode);
|
||
PowerManagerFsm::dispatch(testEvent);
|
||
}
|
||
|
||
|
||
void PowerManger::pushDeviceCommand(DeviceSwitchCmd cmd) {
|
||
std::lock_guard<std::mutex> lock(m_deviceCmdMutex);
|
||
m_deviceCmdQuenue.push(cmd);
|
||
}
|
||
|
||
bool PowerManger::popDeviceCommand(DeviceSwitchCmd &cmd) {
|
||
std::lock_guard<std::mutex> lock(m_deviceCmdMutex);
|
||
if(m_deviceCmdQuenue.empty()) {
|
||
return false;
|
||
}
|
||
cmd = m_deviceCmdQuenue.front();
|
||
m_deviceCmdQuenue.pop();
|
||
return true;
|
||
}
|
||
|
||
bool PowerManger::isDeviceCmdEmpty() const {
|
||
std::lock_guard<std::mutex> lock(m_deviceCmdMutex);
|
||
return m_deviceCmdQuenue.empty();
|
||
} |