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-05-19 20:42:20 +08:00
parent baa2e395fa
commit fea319d3bf
35 changed files with 1188 additions and 558 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/pPowerManger/fsm/PowerManagerFsm.cpp
+232 -103
View File
@@ -3,8 +3,10 @@
#include "../loguru.hpp"
#include <string>
#include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h"
#include "../faultCode.h"
#define DEBUG
// #define DEBUG
#define DEBUG_1
// 定义静态成员
PowerManger* PowerManagerFsm::pm = nullptr;
void PowerManagerFsm::react(TaskStopEvent const &e) {
@@ -14,10 +16,10 @@ void PowerManagerFsm::react(TaskStopEvent const &e) {
void PowerManagerFsm::react(CycleTriggeredEvent const &){
// 更新当前设备状态
// cout << "CycleTriggeredEvent" << endl;
pm->m_currentDriverState = pm->m_subDisSysState;
// cout << (int)pm->m_subDisSysState.depth1 << endl;
// cout << (int)pm->m_subDisSysState.depth1 << endl;
#ifdef DEBUG_1
pm->m_currentDriverState = pm->m_subDisSysState;
#endif
// cout << "CycleTriggeredEvent received" << endl;
}
bool PowerManagerFsm::handPowerProtection(DevCmdExt const &e){
if(pm->workCondition > WorkCondition::REMOTE_CONTROL){
@@ -735,23 +737,23 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
case 27:
{
LOG_F(INFO, "DevCmdExt received bowWaterlnet1 - id: %d, cmd: %d", e.id, e.cmd);
LOG_F(INFO, "DevCmdExt received bowWaterInet1 - id: %d, cmd: %d", e.id, e.cmd);
#ifdef DEBUG
pm->m_currentDriverState.bowWaterlnet1 = e.cmd;
pm->m_currentDriverState.bowWaterInet1 = e.cmd;
#endif
if (e.cmd == 0)
{
pm->m_subDisSysCmd.bowWaterlnet1 = 0;
pm->m_subDisSysCmd.bowWaterInet1 = 0;
//TODO ：动力电断电
}
else if (e.cmd == 1)
{
pm->m_subDisSysCmd.bowWaterlnet1 = 1;
pm->m_subDisSysCmd.bowWaterInet1 = 1;
//TODO ：动力电上电
}
else if (e.cmd == 2)
{
pm->m_subDisSysCmd.bowWaterlnet1 = 1;
pm->m_subDisSysCmd.bowWaterInet1 = 1;
}
else if (e.cmd == 3)
{
@@ -759,7 +761,7 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else if (e.cmd == 4)
{
pm->m_subDisSysCmd.bowWaterlnet1 = 0;
pm->m_subDisSysCmd.bowWaterInet1 = 0;
}
else if (e.cmd == 5)
{
@@ -767,29 +769,29 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else
{
LOG_F(WARNING, "DevCmdExt received bowWaterlnet1 - id: %d, cmd: %d is out of range", e.id, e.cmd);
LOG_F(WARNING, "DevCmdExt received bowWaterInet1 - id: %d, cmd: %d is out of range", e.id, e.cmd);
}
break;
}
case 28:
{
LOG_F(INFO, "DevCmdExt received bowWaterlnet2 - id: %d, cmd: %d", e.id, e.cmd);
LOG_F(INFO, "DevCmdExt received bowWaterInet2 - id: %d, cmd: %d", e.id, e.cmd);
#ifdef DEBUG
pm->m_currentDriverState.bowWaterlnet2 = e.cmd;
pm->m_currentDriverState.bowWaterInet2 = e.cmd;
#endif
if (e.cmd == 0)
{
pm->m_subDisSysCmd.bowWaterlnet2 = 0;
pm->m_subDisSysCmd.bowWaterInet2 = 0;
//TODO ：动力电断电
}
else if (e.cmd == 1)
{
pm->m_subDisSysCmd.bowWaterlnet2 = 1;
pm->m_subDisSysCmd.bowWaterInet2 = 1;
//TODO ：动力电上电
}
else if (e.cmd == 2)
{
pm->m_subDisSysCmd.bowWaterlnet2 = 1;
pm->m_subDisSysCmd.bowWaterInet2 = 1;
}
else if (e.cmd == 3)
{
@@ -797,7 +799,7 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else if (e.cmd == 4)
{
pm->m_subDisSysCmd.bowWaterlnet2 = 0;
pm->m_subDisSysCmd.bowWaterInet2 = 0;
}
else if (e.cmd == 5)
{
@@ -805,29 +807,29 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else
{
LOG_F(WARNING, "DevCmdExt received bowWaterlnet2 - id: %d, cmd: %d is out of range", e.id, e.cmd);
LOG_F(WARNING, "DevCmdExt received bowWaterInet2 - id: %d, cmd: %d is out of range", e.id, e.cmd);
}
break;
}
case 29:
{
LOG_F(INFO, "DevCmdExt received bowWaterlnet2 - id: %d, cmd: %d", e.id, e.cmd);
LOG_F(INFO, "DevCmdExt received bowWaterInet2 - id: %d, cmd: %d", e.id, e.cmd);
#ifdef DEBUG
pm->m_currentDriverState.bowWaterlnet3 = e.cmd;
pm->m_currentDriverState.bowWaterInet3 = e.cmd;
#endif
if (e.cmd == 0)
{
pm->m_subDisSysCmd.bowWaterlnet3 = 0;
pm->m_subDisSysCmd.bowWaterInet3 = 0;
//TODO ：动力电断电
}
else if (e.cmd == 1)
{
pm->m_subDisSysCmd.bowWaterlnet3 = 1;
pm->m_subDisSysCmd.bowWaterInet3 = 1;
//TODO ：动力电上电
}
else if (e.cmd == 2)
{
pm->m_subDisSysCmd.bowWaterlnet3 = 1;
pm->m_subDisSysCmd.bowWaterInet3 = 1;
}
else if (e.cmd == 3)
{
@@ -835,7 +837,7 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else if (e.cmd == 4)
{
pm->m_subDisSysCmd.bowWaterlnet3 = 0;
pm->m_subDisSysCmd.bowWaterInet3 = 0;
}
else if (e.cmd == 5)
{
@@ -843,29 +845,29 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else
{
LOG_F(WARNING, "DevCmdExt received bowWaterlnet3 - id: %d, cmd: %d is out of range", e.id, e.cmd);
LOG_F(WARNING, "DevCmdExt received bowWaterInet3 - id: %d, cmd: %d is out of range", e.id, e.cmd);
}
break;
}
case 30:
{
LOG_F(INFO, "DevCmdExt received bowWaterlnet4 - id: %d, cmd: %d", e.id, e.cmd);
LOG_F(INFO, "DevCmdExt received bowWaterInet4 - id: %d, cmd: %d", e.id, e.cmd);
#ifdef DEBUG
pm->m_currentDriverState.bowWaterlnet4 = e.cmd;
pm->m_currentDriverState.bowWaterInet4 = e.cmd;
#endif
if (e.cmd == 0)
{
pm->m_subDisSysCmd.bowWaterlnet4 = 0;
pm->m_subDisSysCmd.bowWaterInet4 = 0;
//TODO ：动力电断电
}
else if (e.cmd == 1)
{
pm->m_subDisSysCmd.bowWaterlnet4 = 1;
pm->m_subDisSysCmd.bowWaterInet4 = 1;
//TODO ：动力电上电
}
else if (e.cmd == 2)
{
pm->m_subDisSysCmd.bowWaterlnet4 = 1;
pm->m_subDisSysCmd.bowWaterInet4 = 1;
}
else if (e.cmd == 3)
{
@@ -873,7 +875,7 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else if (e.cmd == 4)
{
pm->m_subDisSysCmd.bowWaterlnet4 = 0;
pm->m_subDisSysCmd.bowWaterInet4 = 0;
}
else if (e.cmd == 5)
{
@@ -881,29 +883,29 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else
{
LOG_F(WARNING, "DevCmdExt received bowWaterlnet4 - id: %d, cmd: %d is out of range", e.id, e.cmd);
LOG_F(WARNING, "DevCmdExt received bowWaterInet4 - id: %d, cmd: %d is out of range", e.id, e.cmd);
}
break;
}
case 31:
{
LOG_F(INFO, "DevCmdExt received bowWaterlnet4 - id: %d, cmd: %d", e.id, e.cmd);
LOG_F(INFO, "DevCmdExt received bowWaterInet4 - id: %d, cmd: %d", e.id, e.cmd);
#ifdef DEBUG
pm->m_currentDriverState.bowWaterlnet5 = e.cmd;
pm->m_currentDriverState.bowWaterInet5 = e.cmd;
#endif
if (e.cmd == 0)
{
pm->m_subDisSysCmd.bowWaterlnet5 = 0;
pm->m_subDisSysCmd.bowWaterInet5 = 0;
//TODO ：动力电断电
}
else if (e.cmd == 1)
{
pm->m_subDisSysCmd.bowWaterlnet5 = 1;
pm->m_subDisSysCmd.bowWaterInet5 = 1;
//TODO ：动力电上电
}
else if (e.cmd == 2)
{
pm->m_subDisSysCmd.bowWaterlnet5 = 1;
pm->m_subDisSysCmd.bowWaterInet5 = 1;
}
else if (e.cmd == 3)
{
@@ -911,7 +913,7 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else if (e.cmd == 4)
{
pm->m_subDisSysCmd.bowWaterlnet5 = 0;
pm->m_subDisSysCmd.bowWaterInet5 = 0;
}
else if (e.cmd == 5)
{
@@ -919,29 +921,29 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else
{
LOG_F(WARNING, "DevCmdExt received bowWaterlnet5 - id: %d, cmd: %d is out of range", e.id, e.cmd);
LOG_F(WARNING, "DevCmdExt received bowWaterInet5 - id: %d, cmd: %d is out of range", e.id, e.cmd);
}
break;
}
case 32:
{
LOG_F(INFO, "DevCmdExt received bowWaterlnet6 - id: %d, cmd: %d", e.id, e.cmd);
LOG_F(INFO, "DevCmdExt received bowWaterInet6 - id: %d, cmd: %d", e.id, e.cmd);
#ifdef DEBUG
pm->m_currentDriverState.bowWaterlnet6 = e.cmd;
pm->m_currentDriverState.bowWaterInet6 = e.cmd;
#endif
if (e.cmd == 0)
{
pm->m_subDisSysCmd.bowWaterlnet6 = 0;
pm->m_subDisSysCmd.bowWaterInet6 = 0;
//TODO ：动力电断电
}
else if (e.cmd == 1)
{
pm->m_subDisSysCmd.bowWaterlnet6 = 1;
pm->m_subDisSysCmd.bowWaterInet6 = 1;
//TODO ：动力电上电
}
else if (e.cmd == 2)
{
pm->m_subDisSysCmd.bowWaterlnet6 = 1;
pm->m_subDisSysCmd.bowWaterInet6 = 1;
}
else if (e.cmd == 3)
{
@@ -949,7 +951,7 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else if (e.cmd == 4)
{
pm->m_subDisSysCmd.bowWaterlnet6 = 0;
pm->m_subDisSysCmd.bowWaterInet6 = 0;
}
else if (e.cmd == 5)
{
@@ -957,29 +959,29 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else
{
LOG_F(WARNING, "DevCmdExt received bowWaterlnet6 - id: %d, cmd: %d is out of range", e.id, e.cmd);
LOG_F(WARNING, "DevCmdExt received bowWaterInet6 - id: %d, cmd: %d is out of range", e.id, e.cmd);
}
break;
}
case 33:
{
LOG_F(INFO, "DevCmdExt received bowWaterlnet7 - id: %d, cmd: %d", e.id, e.cmd);
LOG_F(INFO, "DevCmdExt received bowWaterInet7 - id: %d, cmd: %d", e.id, e.cmd);
#ifdef DEBUG
pm->m_currentDriverState.bowWaterlnet7 = e.cmd;
pm->m_currentDriverState.bowWaterInet7 = e.cmd;
#endif
if (e.cmd == 0)
{
pm->m_subDisSysCmd.bowWaterlnet7 = 0;
pm->m_subDisSysCmd.bowWaterInet7 = 0;
//TODO ：动力电断电
}
else if (e.cmd == 1)
{
pm->m_subDisSysCmd.bowWaterlnet7 = 1;
pm->m_subDisSysCmd.bowWaterInet7 = 1;
//TODO ：动力电上电
}
else if (e.cmd == 2)
{
pm->m_subDisSysCmd.bowWaterlnet7 = 1;
pm->m_subDisSysCmd.bowWaterInet7 = 1;
}
else if (e.cmd == 3)
{
@@ -987,7 +989,7 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else if (e.cmd == 4)
{
pm->m_subDisSysCmd.bowWaterlnet7 = 0;
pm->m_subDisSysCmd.bowWaterInet7 = 0;
}
else if (e.cmd == 5)
{
@@ -995,29 +997,29 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else
{
LOG_F(WARNING, "DevCmdExt received bowWaterlnet7 - id: %d, cmd: %d is out of range", e.id, e.cmd);
LOG_F(WARNING, "DevCmdExt received bowWaterInet7 - id: %d, cmd: %d is out of range", e.id, e.cmd);
}
break;
}
case 34:
{
LOG_F(INFO, "DevCmdExt received bowWaterlnet8 - id: %d, cmd: %d", e.id, e.cmd);
LOG_F(INFO, "DevCmdExt received bowWaterInet8 - id: %d, cmd: %d", e.id, e.cmd);
#ifdef DEBUG
pm->m_currentDriverState.bowWaterlnet8 = e.cmd;
pm->m_currentDriverState.bowWaterInet8 = e.cmd;
#endif
if (e.cmd == 0)
{
pm->m_subDisSysCmd.bowWaterlnet8 = 0;
pm->m_subDisSysCmd.bowWaterInet8 = 0;
//TODO ：动力电断电
}
else if (e.cmd == 1)
{
pm->m_subDisSysCmd.bowWaterlnet8 = 1;
pm->m_subDisSysCmd.bowWaterInet8 = 1;
//TODO ：动力电上电
}
else if (e.cmd == 2)
{
pm->m_subDisSysCmd.bowWaterlnet8 = 1;
pm->m_subDisSysCmd.bowWaterInet8 = 1;
}
else if (e.cmd == 3)
{
@@ -1025,7 +1027,7 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else if (e.cmd == 4)
{
pm->m_subDisSysCmd.bowWaterlnet8 = 0;
pm->m_subDisSysCmd.bowWaterInet8 = 0;
}
else if (e.cmd == 5)
{
@@ -1033,7 +1035,7 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
}
else
{
LOG_F(WARNING, "DevCmdExt received bowWaterlnet8 - id: %d, cmd: %d is out of range", e.id, e.cmd);
LOG_F(WARNING, "DevCmdExt received bowWaterInet8 - id: %d, cmd: %d is out of range", e.id, e.cmd);
}
break;
}
@@ -1744,57 +1746,57 @@ void PowerManagerFsm::react(DevCmdExt const &e) {
void PowerManagerFsm::basicNavigationFault(FaultEvent const &e)
{
LOG_F(INFO, "basicNavigationFault");
if(FaultEnable::BasicNavigationFault_1)
if((e.injectionFaultCode==1))
{
LOG_F(INFO, "部分深度计失效");
pm->faultCode.push_back(0x1102);
pm->faultCode.push_back(0x1103);
pm->faultCode.push_back(0x1104);
}
if(FaultEnable::BasicNavigationFault_2)
if((e.injectionFaultCode==2))
{
LOG_F(INFO, "DVL失效");
pm->faultCode.push_back(0x1301);
}
if (FaultEnable::BasicNavigationFault_3)
if ((e.injectionFaultCode==3))
{
LOG_F(INFO, "单个艉舵失效");
pm->faultCode.push_back(0x1207);
}
if (FaultEnable::BasicNavigationFault_4)
if ((e.injectionFaultCode==4))
{
LOG_F(INFO, "部分艉舵失效");
pm->faultCode.push_back(0x1308);
pm->faultCode.push_back(0x1307);
}
if (FaultEnable::BasicNavigationFault_5)
if ((e.injectionFaultCode==5))
{
LOG_F(INFO, "动力锂电池失效");
pm->faultCode.push_back(0x130B);
}
if (FaultEnable::BasicNavigationFault_6)
if ((e.injectionFaultCode==6))
{
LOG_F(INFO, "推进电机失效");
pm->faultCode.push_back(0x130A);
}
if (FaultEnable::BasicNavigationFault_7)
if ((e.injectionFaultCode==7))
{
LOG_F(INFO, "惯导失效");
pm->faultCode.push_back(0x1305);
}
if (FaultEnable::BasicNavigationFault_8)
if ((e.injectionFaultCode==8))
{
LOG_F(INFO, "深度失效");
pm->faultCode.push_back(0x1302);
pm->faultCode.push_back(0x1303);
pm->faultCode.push_back(0x1304);
}
if (FaultEnable::BasicNavigationFault_9)
if ((e.injectionFaultCode==9))
{
LOG_F(INFO, "仪表锂电池失效");
pm->faultCode.push_back(0x1412);
}
if (FaultEnable::BasicNavigationFault_9)
if ((e.injectionFaultCode==10))
{
LOG_F(INFO, "复合管控器失效");
pm->faultCode.push_back(0x1413);
@@ -1803,22 +1805,22 @@ void PowerManagerFsm::basicNavigationFault(FaultEvent const &e)
void PowerManagerFsm::extendedNavigationFault(FaultEvent const &e)
{
LOG_F(INFO, "extendedNavigationFault");
if (FaultEnable::ExtendedNavigationFault_1)
if ((e.injectionFaultCode==1))
{
LOG_F(INFO, "艏舵展开电缸失效");
pm->faultCode.push_back(0x2311);
}
if (FaultEnable::ExtendedNavigationFault_2)
if ((e.injectionFaultCode==2))
{
LOG_F(INFO, "艏舵控制控制失效");
pm->faultCode.push_back(0x2312);
}
if (FaultEnable::ExtendedNavigationFault_3)
if ((e.injectionFaultCode==3))
{
LOG_F(INFO, "动力锂电池功率不满/失效");
pm->faultCode.push_back(0x2323);
}
if (FaultEnable::ExtendedNavigationFault_4)
if ((e.injectionFaultCode==4))
{
LOG_F(INFO, "燃料电池失效");
pm->faultCode.push_back(0x2334);
@@ -1827,12 +1829,12 @@ void PowerManagerFsm::extendedNavigationFault(FaultEvent const &e)
void PowerManagerFsm::obstacleAvoidanceFault(FaultEvent const &e)
{
LOG_F(INFO, "obstacleAvoidanceFault");
if (FaultEnable::ObstacleAvoidanceFault_1)
if ((e.injectionFaultCode==1))
{
pm->faultCode.push_back(0x3108);
pm->faultCode.push_back(0x3109);
}
if (FaultEnable::ObstacleAvoidanceFault_2)
if ((e.injectionFaultCode==2))
{
pm->faultCode.push_back(0x3201);
pm->faultCode.push_back(0x3202);
@@ -1842,7 +1844,7 @@ void PowerManagerFsm::obstacleAvoidanceFault(FaultEvent const &e)
pm->faultCode.push_back(0x3206);
pm->faultCode.push_back(0x3207);
}
if (FaultEnable::ObstacleAvoidanceFault_2)
if ((e.injectionFaultCode==3))
{
pm->faultCode.push_back(0x3301);
}
@@ -1851,24 +1853,24 @@ void PowerManagerFsm::obstacleAvoidanceFault(FaultEvent const &e)
void PowerManagerFsm::buoyancyControlFault(FaultEvent const &e)
{
LOG_F(INFO, "buoyancyControlFault");
if (FaultEnable::BuoyancyControlFault_1)
if ((e.injectionFaultCode==1))
{
LOG_F(INFO, "单一浮调机构失效");
pm->faultCode.push_back(0x4101);
pm->faultCode.push_back(0x4201);
}
if (FaultEnable::BuoyancyControlFault_2)
if ((e.injectionFaultCode==2))
{
LOG_F(INFO, "2-4及以上浮调装置失效");
pm->faultCode.push_back(0x4301);
pm->faultCode.push_back(0x4302);
pm->faultCode.push_back(0x4303);
}
if (FaultEnable::BuoyancyControlFault_3)
if ((e.injectionFaultCode==3))
{
LOG_F(INFO, "功率不足满足浮调要求");
pm->faultCode.push_back(0x4336);
pm->faultCode.push_back(0x4436);
}
if (FaultEnable::BuoyancyControlFault_4)
if ((e.injectionFaultCode==4))
{
LOG_F(INFO, "所有浮调系统失效");
pm->faultCode.push_back(0x4301);
@@ -1878,21 +1880,21 @@ void PowerManagerFsm::buoyancyControlFault(FaultEvent const &e)
void PowerManagerFsm::communicationSystemFault(FaultEvent const &e)
{
LOG_F(INFO, "communicationSystemFault");
if (FaultEnable::CommunicationSystemFault_1)
if ((e.injectionFaultCode==1))
{
LOG_F(INFO, "部分水下通信失效");
pm->faultCode.push_back(0x5203);
pm->faultCode.push_back(0x5204);
pm->faultCode.push_back(0x5205);
}
if (FaultEnable::CommunicationSystemFault_2)
if ((e.injectionFaultCode==2))
{
LOG_F(INFO, "水下通信失效");
pm->faultCode.push_back(0x5303);
pm->faultCode.push_back(0x5304);
pm->faultCode.push_back(0x5305);
}
if (FaultEnable::CommunicationSystemFault_2)
if ((e.injectionFaultCode==3))
{
LOG_F(INFO, "水面通信失效");
pm->faultCode.push_back(0x5301);
@@ -1902,7 +1904,7 @@ void PowerManagerFsm::communicationSystemFault(FaultEvent const &e)
void PowerManagerFsm::payloadSystemFault(FaultEvent const &e)
{
LOG_F(INFO, "payloadSystemFault");
if (FaultEnable::PayloadSystemFault_1)
if ((e.injectionFaultCode==1))
{
LOG_F(INFO, "存在启闭机构或者敞水电缸失效");
pm->faultCode.push_back(0x6211);
@@ -1920,32 +1922,32 @@ void PowerManagerFsm::payloadSystemFault(FaultEvent const &e)
pm->faultCode.push_back(0x621D);
pm->faultCode.push_back(0x621E);
}
if (FaultEnable::PayloadSystemFault_2)
if ((e.injectionFaultCode==2))
{
LOG_F(INFO, "舷侧探测失效");
pm->faultCode.push_back(0x6321);
}
if (FaultEnable::PayloadSystemFault_3)
if ((e.injectionFaultCode==3))
{
LOG_F(INFO, "舷侧主动探测失效");
pm->faultCode.push_back(0x6323);
}
if (FaultEnable::PayloadSystemFault_4)
if ((e.injectionFaultCode==4))
{
LOG_F(INFO, "TYZ失效");
pm->faultCode.push_back(0x6322);
}
if (FaultEnable::PayloadSystemFault_5)
if ((e.injectionFaultCode==5))
{
LOG_F(INFO, "光电探测失效");
pm->faultCode.push_back(0x6332);
}
if (FaultEnable::PayloadSystemFault_6)
if ((e.injectionFaultCode==6))
{
LOG_F(INFO, "电磁探测失效");
pm->faultCode.push_back(0x6331);
}
if (FaultEnable::PayloadSystemFault_7)
if ((e.injectionFaultCode==7))
{
LOG_F(INFO, "载荷失效或者全部载荷启闭机构均失效");
pm->faultCode.push_back(0x633F);
@@ -1954,36 +1956,36 @@ void PowerManagerFsm::payloadSystemFault(FaultEvent const &e)
void PowerManagerFsm::emergencySystemFault(FaultEvent const &e)
{
LOG_F(INFO, "emergencySystemFault");
if (FaultEnable::EmergencySystemFault_1)
if ((e.injectionFaultCode==1))
{
//备注：抛载具有自己的电池
LOG_F(INFO, "部分抛载失效");
pm->faultCode.push_back(0x7102);
pm->faultCode.push_back(0x7103);
}
if (FaultEnable::EmergencySystemFault_2)
if ((e.injectionFaultCode==2))
{
LOG_F(INFO, "一个超深检测装置失效");
pm->faultCode.push_back(0x7204);
pm->faultCode.push_back(0x7205);
}
if (FaultEnable::EmergencySystemFault_3)
if ((e.injectionFaultCode==3))
{
LOG_F(INFO, "注水机构失效");
pm->faultCode.push_back(0x7301);
}
if (FaultEnable::EmergencySystemFault_4)
if ((e.injectionFaultCode==4))
{
LOG_F(INFO, "抛载失效");
pm->faultCode.push_back(0x7302);
pm->faultCode.push_back(0x7303);
}
if (FaultEnable::EmergencySystemFault_5)
if ((e.injectionFaultCode==5))
{
LOG_F(INFO, "艏部应急电源失效");
pm->faultCode.push_back(0x7306);
}
if (FaultEnable::EmergencySystemFault_6)
if ((e.injectionFaultCode==6))
{
LOG_F(INFO, "艉部应急电源失效");
pm->faultCode.push_back(0x7307);
@@ -1992,18 +1994,18 @@ void PowerManagerFsm::emergencySystemFault(FaultEvent const &e)
void PowerManagerFsm::sensorSystemFault(FaultEvent const &e)
{
LOG_F(INFO, "sensorSystemFault");
if (FaultEnable::SensorSystemFault_1)
if ((e.injectionFaultCode==1))
{
LOG_F(INFO, "部分CTD失效");
pm->faultCode.push_back(0x8202);
}
if (FaultEnable::SensorSystemFault_2)
if ((e.injectionFaultCode==2))
{
LOG_F(INFO, "CTD失效");
pm->faultCode.push_back(0x8302);
pm->faultCode.push_back(0x8303);
}
if (FaultEnable::SensorSystemFault_2)
if ((e.injectionFaultCode==3))
{
LOG_F(INFO, "声速仪失效");
pm->faultCode.push_back(0x8301);
@@ -2145,5 +2147,132 @@ void PowerManagerFsm::handleDeviceCmd() {
LOG_F(INFO, "开始处理设备命令:");
}
void PowerManagerFsm::react(TestEvent const &e)
{
LOG_F(INFO, "测试消息");
if(e.type == 0){
LOG_F(INFO, "清空故障代码");
pm->faultCode.clear();
pm->m_leakage = 0x00;
pm->m_systemData->insfaultCode = 0x00;
pm->m_systemData->powfaultCode = 0x00;
pm->m_systemData->fc_fault_level_4 = 0;
pm->m_systemData->fc_fault_level_3 = 0;
pm->m_systemData->fc_fault_level_2 = 0;
pm->m_systemData->fc_fault_level_1 = 0;
pm->m_systemData->distributeFaultCode.clear();
cout << "清空故障代码" << endl;
};
// if(e.eventId == 0) return ;
LOG_F(INFO, "测试消息类型: %d,测试代码：%d", e.type, e.eventId);
FaultEvent faultEvent;
faultEvent.injectionFaultCode = e.eventId;
//故障注入
switch (e.type)
{
case 0:
break;
case 1:
basicNavigationFault(faultEvent);
break;
case 2:
extendedNavigationFault(faultEvent);
break;
case 3:
obstacleAvoidanceFault(faultEvent);
break;
case 4:
buoyancyControlFault(faultEvent);
break;
case 5:
communicationSystemFault(faultEvent);
break;
case 6:
payloadSystemFault(faultEvent);
break;
case 7:
emergencySystemFault(faultEvent);
break;
case 8:
sensorSystemFault(faultEvent);
break;
case 9:
LOG_F(INFO, "漏水故障");
switch (e.eventId)
{
case 1:
pm->m_leakage = 0x10; //锂电池舱
break;
case 2:
pm->m_leakage = 0x20; //艉应急
break;
case 3:
pm->m_leakage = 0x40;
break;
case 4:
pm->m_leakage = 0x80;
break;
case 5:
pm->m_leakage = 0x01; //艏部应急漏水
break;
case 6:
pm->m_leakage = 0x02; //艏高压漏水
break;
case 7:
pm->m_leakage = 0x04; //艏低压漏水
break;
case 8:
pm->m_leakage = 0x08; //燃电舱漏水
break;
default:
break;
}
break;
case 10:
LOG_F(INFO,"锂电池故障测试");
pm->m_systemData->insfaultCode = batfaultMap[e.eventId].code;
pm->m_systemData->powfaultCode = batfaultMap[e.eventId].code;
cout << "=======================" << (int)pm->m_systemData->insfaultCode << endl;
cout << "=======================" << (int)pm->m_systemData->powfaultCode << endl;
break;
case 11:
{
LOG_F(INFO,"燃料电池1级故障测试");
uint16_t code = 0x0001;
pm->m_systemData->fc_fault_level_1 = code << (e.eventId);
break;
}
case 12:
{
LOG_F(INFO,"燃料电池2级故障测试");
uint16_t code = 0x0001;
pm->m_systemData->fc_fault_level_2 = code << (e.eventId);
break;
}
case 13:
{
LOG_F(INFO,"燃料电池3级故障测试");
uint16_t code = 0x0001;
pm->m_systemData->fc_fault_level_3 = code << (e.eventId);
break;
}
case 14:
{
LOG_F(INFO,"燃料电池4级故障测试");
uint16_t code = 0x0001;
pm->m_systemData->fc_fault_level_4 = code << (e.eventId);
break;
}
case 15:
{
LOG_F(INFO,"配电系统故障测试");
pm->m_systemData->distributeFaultCode.push_back(disfaultMap[e.eventId].code);
}
default:
LOG_F(INFO, "测试消息类型错误");
break;
}
}
FSM_INITIAL_STATE(PowerManagerFsm, InitState);