2278 lines
61 KiB
C++
2278 lines
61 KiB
C++
#include "PowerManagerFsm.hpp"
|
|
#include "../PowerManger.h"
|
|
#include "../loguru.hpp"
|
|
#include <string>
|
|
#include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h"
|
|
#include "../faultCode.h"
|
|
|
|
// #define DEBUG
|
|
#define DEBUG_1
|
|
// 定义静态成员
|
|
PowerManger* PowerManagerFsm::pm = nullptr;
|
|
void PowerManagerFsm::react(TaskStopEvent const &e) {
|
|
transit<RemoteControl>();
|
|
LOG_F(INFO, "使命结束");
|
|
};
|
|
|
|
void PowerManagerFsm::react(CycleTriggeredEvent const &){
|
|
// 更新当前设备状态
|
|
#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){
|
|
LOG_F(INFO, "上电保护，执行任务时候禁止操作设备");
|
|
return false;
|
|
}
|
|
if(e.id == DeviceId::singleBeamSonar1 ||
|
|
e.id == DeviceId::singleBeamSonar2 ||
|
|
e.id == DeviceId::singleBeamSonar3 ||
|
|
e.id == DeviceId::singleBeamSonar4 ||
|
|
e.id == DeviceId::singleBeamSonar5 ||
|
|
e.id == DeviceId::singleBeamSonar6 ||
|
|
e.id == DeviceId::singleBeamSonar7 ||
|
|
e.id == DeviceId::singleBeamSonar8 ||
|
|
e.id == DeviceId::sideSonar ||
|
|
e.id == DeviceId::forwardSonar ||
|
|
e.id == DeviceId::lowFreqSonar ||
|
|
e.id == DeviceId::DVL ||
|
|
e.id == DeviceId::USBL){
|
|
if(e.cmd == 1){
|
|
LOG_F(INFO, "水声设备操作上电保护 %d 已关闭", e.id);
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
void PowerManagerFsm::react(DeviceCmd const &){
|
|
LOG_F(INFO, "DeviceCmd received");
|
|
}
|
|
void PowerManagerFsm::react(TaskStartEvent const &e){
|
|
LOG_F(INFO, "TaskStartEvent received - type: %d, enableBowRudder: %d, enableBuoyancyAdjustment: %d", e.type, e.enableBowRudder, e.enableBuoyancyAdjustment);
|
|
switch (e.type)
|
|
{
|
|
case TaskType::THROW_LOAD:
|
|
transit<FaultState>();
|
|
break;
|
|
case TaskType::FLOAT_UP:
|
|
transit<FloatDown>();
|
|
break;
|
|
case TaskType::RECYCLE:
|
|
transit<CruiseMode>();
|
|
break;
|
|
case TaskType::HOVER:
|
|
transit<FloatAdjust>();
|
|
break;
|
|
case TaskType::SAT_COMM:
|
|
transit<FloatDown>();
|
|
break;
|
|
case TaskType::SAT_CALIBRATE:
|
|
transit<FloatDown>();
|
|
break;
|
|
case TaskType::TOUR:
|
|
transit<UnderwaterSurvey>();
|
|
break;
|
|
case TaskType::SAIL:
|
|
transit<CruiseMode>();
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void PowerManagerFsm::react(DevCmdExt const &e) {
|
|
LOG_F(INFO, "DevCmdExt received - id: %d, cmd: %d", e.id, e.cmd);
|
|
switch (e.id)
|
|
{
|
|
case 0:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received singleBeamSonar1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
if(e.cmd <=1)
|
|
{
|
|
pm->m_subDisSysCmd.singleBeamSonar1 = e.cmd;
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.singleBeamSonar1 = e.cmd;
|
|
#endif
|
|
}
|
|
else
|
|
LOG_F(WARNING, "DevCmdExt received singleBeamSonar1 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
break;
|
|
}
|
|
case 1:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received singleBeamSonar2 - id: %d, cmd: %d", e.id, e.cmd);
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.singleBeamSonar2 = e.cmd;
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.singleBeamSonar2 = e.cmd;
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received singleBeamSonar2 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 2:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received singleBeamSonar3 - id: %d, cmd: %d", e.id, e.cmd);
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.singleBeamSonar3 = e.cmd;
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.singleBeamSonar3 = e.cmd;
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received singleBeamSonar3 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 3:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received singleBeamSonar4 - id: %d, cmd: %d", e.id, e.cmd);
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.singleBeamSonar4 = e.cmd;
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.singleBeamSonar4 = e.cmd;
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received singleBeamSonar4 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 4:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received singleBeamSonar5 - id: %d, cmd: %d", e.id, e.cmd);
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.singleBeamSonar5 = e.cmd;
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.singleBeamSonar5 = e.cmd;
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received singleBeamSonar5 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 5:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received singleBeamSonar6 - id: %d, cmd: %d", e.id, e.cmd);
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.singleBeamSonar6 = e.cmd;
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.singleBeamSonar6 = e.cmd;
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received singleBeamSonar6 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 6:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received singleBeamSonar7 - id: %d, cmd: %d", e.id, e.cmd);
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.singleBeamSonar7 = e.cmd;
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.singleBeamSonar7 = e.cmd;
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received singleBeamSonar7 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 7:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received singleBeamSonar8 - id: %d, cmd: %d", e.id, e.cmd);
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.singleBeamSonar8 = e.cmd;
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.singleBeamSonar8 = e.cmd;
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received singleBeamSonar8 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
|
|
case 8:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received forwardSonar - id: %d, cmd: %d", e.id, e.cmd);
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.forwardSonar = e.cmd;
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.forwardSonar = e.cmd;
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received forwardSonar - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 9:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received bowCover1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowCover1 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover1 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover1 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover1 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover1 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received bowCover1 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
|
|
case 10:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received bowCover2 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowCover2 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover2 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover2 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover2 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover2 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received bowCover2 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 11:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received bowCover3 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowCover3 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover3 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover3 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover3 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover3 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received bowCover2 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 12:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received bowCover4 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowCover4 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover4 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover4 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover4 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover4 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received bowCover2 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 13:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received bowCover5 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowCover5 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover5 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover5 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover5 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover5 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received bowCover5 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 14:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received bowCover6 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowCover6 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover6 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover6 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover6 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowCover6 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received bowCover6 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 15:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received bowRudderDeploy - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowRudderDeploy = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowRudderDeploy = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowRudderDeploy = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowRudderDeploy = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowRudderDeploy = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received bowRudderDeploy - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 16:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received bowRudderServo1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowRudderServo1 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowRudderServo1 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowRudderServo1 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowRudderServo1 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowRudderServo1 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received bowRudderServo1 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 17:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received bowRudderServo2 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowRudderServo2 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowRudderServo2 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowRudderServo2 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowRudderServo2 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowRudderServo2 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received bowRudderServo2 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 18:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received depth1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.depth1 = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.depth1 = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received depth1 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 19:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received depth2 - id: %d, cmd: %d", e.id, e.cmd);
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.depth2 = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received depth2 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 20:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received depth3 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.depth3 = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.depth3 = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received depth3 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 21:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.overDeep1 = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.overDeep1 = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received overDeep1 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 22:
|
|
{
|
|
LOG_F(INFO, " - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.overDeep2 = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.overDeep2 = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received overDeep2 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 23:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.ctd1 = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.ctd1 = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received ctd1 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 24:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.ctd2 = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.ctd2 = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received ctd2 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 25:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.soundVeloc = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.soundVeloc = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received soundVeloc - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 26:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received waterInject - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.waterInject = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.waterInject = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.waterInject = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.waterInject = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.waterInject = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received waterInject - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 27:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received bowWaterInet1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowWaterInet1 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet1 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet1 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet1 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet1 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
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 bowWaterInet2 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowWaterInet2 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet2 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet2 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet2 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet2 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
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 bowWaterInet2 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowWaterInet3 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet3 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet3 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet3 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet3 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
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 bowWaterInet4 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowWaterInet4 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet4 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet4 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet4 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet4 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
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 bowWaterInet4 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowWaterInet5 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet5 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet5 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet5 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet5 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
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 bowWaterInet6 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowWaterInet6 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet6 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet6 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet6 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet6 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
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 bowWaterInet7 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowWaterInet7 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet7 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet7 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet7 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet7 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
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 bowWaterInet8 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowWaterInet8 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet8 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet8 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet8 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.bowWaterInet8 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received bowWaterInet8 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 35:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received sideSonar - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.sideSonar = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.sideSonar = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.sideSonar = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.sideSonar = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.sideSonar = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received sideSonar - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 36:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.lowFreqSonar = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.lowFreqSonar = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received lowFreqSonar - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 37:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.launchControler = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.launchControler = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received launchControler - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 38:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.photoSensor = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.photoSensor = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received photoSensor - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 39:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received overDeep1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.electSensor = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.electSensor = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received electSensor - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 40:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received commMast - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.commMast = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.commMast = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received commMast - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 41:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received mastLiftingServo - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.mastLiftingServo = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.mastLiftingServo = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.mastLiftingServo = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.mastLiftingServo = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.mastLiftingServo = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received mastLiftingServo - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 42:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received USBL - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.USBL = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.USBL = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received USBL - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 43:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received DVL - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.DVL = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.DVL = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received DVL - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 44:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received buoyancyAdjust1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.buoyancyAdjust1 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust1 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust1 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust1 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust1 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received buoyancyAdjust1 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 45:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received buoyancyAdjust2 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.buoyancyAdjust2 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust2 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust2 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust2 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust2 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received buoyancyAdjust2 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 46:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received buoyancyAdjust3 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.buoyancyAdjust3 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust3 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust3 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust3 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust3 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received buoyancyAdjust3 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 47:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received buoyancyAdjust4 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.buoyancyAdjust4 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust4 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust4 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust4 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust4 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received buoyancyAdjust4 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 48:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received buoyancyAdjust5 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.buoyancyAdjust5 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust5 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust5 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust5 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.buoyancyAdjust5 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received buoyancyAdjust5 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 49:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received sternRudderServo1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.sternRudderServo1 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo1 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo1 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo1 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo1 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received sternRudderServo1 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 50:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received sternRudderServo2 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.sternRudderServo2 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo2 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo2 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo2 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo2 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received sternRudderServo2 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 51:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received sternRudderServo3 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.sternRudderServo3 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo3 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo3 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo3 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo3 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received sternRudderServo3 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 52:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received sternRudderServo4 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.sternRudderServo4 = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo4 = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo4 = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo4 = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.sternRudderServo4 = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received sternRudderServo4 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 53:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received thruster - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.thruster = e.cmd;
|
|
#endif
|
|
if (e.cmd == 0)
|
|
{
|
|
pm->m_subDisSysCmd.thruster = 0;
|
|
//TODO ：动力电断电
|
|
}
|
|
else if (e.cmd == 1)
|
|
{
|
|
pm->m_subDisSysCmd.thruster = 1;
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 2)
|
|
{
|
|
pm->m_subDisSysCmd.thruster = 1;
|
|
}
|
|
else if (e.cmd == 3)
|
|
{
|
|
//TODO ：动力电上电
|
|
}
|
|
else if (e.cmd == 4)
|
|
{
|
|
pm->m_subDisSysCmd.thruster = 0;
|
|
}
|
|
else if (e.cmd == 5)
|
|
{
|
|
//TODO ：动力电断电
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received thruster - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 54:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received bowThrow - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.bowThrow = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.bowThrow = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received bowThrow - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 55:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received sternThrow - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.sternThrow = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.sternThrow = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received sternThrow - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 56:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received buoyage - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.buoyage = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.buoyage = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received buoyage - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 57:
|
|
{
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received buoyage - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.ins = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.ins = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received ins - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
case 58:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received buoyage - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.ntp = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.ntp = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received ntp - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 59:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received inputWaterSensor1 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.inputWaterSensor1 = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.inputWaterSensor1 = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received inputWaterSensor1 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 60:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received inputWaterSensor2 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.inputWaterSensor2 = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.inputWaterSensor2 = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received inputWaterSensor2 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
case 61:
|
|
{
|
|
LOG_F(INFO, "DevCmdExt received inputWaterSensor3 - id: %d, cmd: %d", e.id, e.cmd);
|
|
#ifdef DEBUG
|
|
pm->m_currentDriverState.inputWaterSensor3 = e.cmd;
|
|
#endif
|
|
if (e.cmd <= 1)
|
|
{
|
|
pm->m_subDisSysCmd.inputWaterSensor3 = e.cmd;
|
|
}
|
|
else
|
|
{
|
|
LOG_F(WARNING, "DevCmdExt received inputWaterSensor3 - id: %d, cmd: %d is out of range", e.id, e.cmd);
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
LOG_F(WARNING, "DevCmdExt received unknown id: %d, cmd: %d", e.id, e.cmd);
|
|
break;
|
|
}
|
|
}
|
|
|
|
void PowerManagerFsm::basicNavigationFault(FaultEvent const &e)
|
|
{
|
|
LOG_F(INFO, "basicNavigationFault");
|
|
if((e.injectionFaultCode==1))
|
|
{
|
|
LOG_F(INFO, "部分深度计失效");
|
|
pm->faultCode.push_back(0x1102);
|
|
pm->faultCode.push_back(0x1103);
|
|
pm->faultCode.push_back(0x1104);
|
|
}
|
|
if((e.injectionFaultCode==2))
|
|
{
|
|
LOG_F(INFO, "DVL失效");
|
|
pm->faultCode.push_back(0x1301);
|
|
}
|
|
if ((e.injectionFaultCode==3))
|
|
{
|
|
LOG_F(INFO, "单个艉舵失效");
|
|
pm->faultCode.push_back(0x1207);
|
|
}
|
|
if ((e.injectionFaultCode==4))
|
|
{
|
|
LOG_F(INFO, "部分艉舵失效");
|
|
pm->faultCode.push_back(0x1308);
|
|
pm->faultCode.push_back(0x1307);
|
|
}
|
|
if ((e.injectionFaultCode==5))
|
|
{
|
|
LOG_F(INFO, "动力锂电池失效");
|
|
pm->faultCode.push_back(0x130B);
|
|
}
|
|
if ((e.injectionFaultCode==6))
|
|
{
|
|
LOG_F(INFO, "推进电机失效");
|
|
pm->faultCode.push_back(0x130A);
|
|
}
|
|
if ((e.injectionFaultCode==7))
|
|
{
|
|
LOG_F(INFO, "惯导失效");
|
|
pm->faultCode.push_back(0x1305);
|
|
}
|
|
if ((e.injectionFaultCode==8))
|
|
{
|
|
LOG_F(INFO, "深度失效");
|
|
pm->faultCode.push_back(0x1302);
|
|
pm->faultCode.push_back(0x1303);
|
|
pm->faultCode.push_back(0x1304);
|
|
}
|
|
if ((e.injectionFaultCode==9))
|
|
{
|
|
LOG_F(INFO, "仪表锂电池失效");
|
|
pm->faultCode.push_back(0x1412);
|
|
}
|
|
if ((e.injectionFaultCode==10))
|
|
{
|
|
LOG_F(INFO, "复合管控器失效");
|
|
pm->faultCode.push_back(0x1413);
|
|
}
|
|
}
|
|
void PowerManagerFsm::extendedNavigationFault(FaultEvent const &e)
|
|
{
|
|
LOG_F(INFO, "extendedNavigationFault");
|
|
if ((e.injectionFaultCode==1))
|
|
{
|
|
LOG_F(INFO, "艏舵展开电缸失效");
|
|
pm->faultCode.push_back(0x2311);
|
|
}
|
|
if ((e.injectionFaultCode==2))
|
|
{
|
|
LOG_F(INFO, "艏舵控制控制失效");
|
|
pm->faultCode.push_back(0x2312);
|
|
}
|
|
if ((e.injectionFaultCode==3))
|
|
{
|
|
LOG_F(INFO, "动力锂电池功率不满/失效");
|
|
pm->faultCode.push_back(0x2323);
|
|
}
|
|
if ((e.injectionFaultCode==4))
|
|
{
|
|
LOG_F(INFO, "燃料电池失效");
|
|
pm->faultCode.push_back(0x2334);
|
|
}
|
|
}
|
|
void PowerManagerFsm::obstacleAvoidanceFault(FaultEvent const &e)
|
|
{
|
|
LOG_F(INFO, "obstacleAvoidanceFault");
|
|
if ((e.injectionFaultCode==1))
|
|
{
|
|
pm->faultCode.push_back(0x3108);
|
|
pm->faultCode.push_back(0x3109);
|
|
}
|
|
if ((e.injectionFaultCode==2))
|
|
{
|
|
pm->faultCode.push_back(0x3201);
|
|
pm->faultCode.push_back(0x3202);
|
|
pm->faultCode.push_back(0x3203);
|
|
pm->faultCode.push_back(0x3204);
|
|
pm->faultCode.push_back(0x3205);
|
|
pm->faultCode.push_back(0x3206);
|
|
pm->faultCode.push_back(0x3207);
|
|
}
|
|
if ((e.injectionFaultCode==3))
|
|
{
|
|
pm->faultCode.push_back(0x3301);
|
|
}
|
|
|
|
}
|
|
void PowerManagerFsm::buoyancyControlFault(FaultEvent const &e)
|
|
{
|
|
LOG_F(INFO, "buoyancyControlFault");
|
|
if ((e.injectionFaultCode==1))
|
|
{
|
|
LOG_F(INFO, "单一浮调机构失效");
|
|
pm->faultCode.push_back(0x4201);
|
|
}
|
|
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 ((e.injectionFaultCode==3))
|
|
{
|
|
LOG_F(INFO, "功率不足满足浮调要求");
|
|
pm->faultCode.push_back(0x4436);
|
|
}
|
|
if ((e.injectionFaultCode==4))
|
|
{
|
|
LOG_F(INFO, "所有浮调系统失效");
|
|
pm->faultCode.push_back(0x4301);
|
|
}
|
|
|
|
}
|
|
void PowerManagerFsm::communicationSystemFault(FaultEvent const &e)
|
|
{
|
|
LOG_F(INFO, "communicationSystemFault");
|
|
if ((e.injectionFaultCode==1))
|
|
{
|
|
LOG_F(INFO, "部分水下通信失效");
|
|
pm->faultCode.push_back(0x5203);
|
|
pm->faultCode.push_back(0x5204);
|
|
pm->faultCode.push_back(0x5205);
|
|
}
|
|
if ((e.injectionFaultCode==2))
|
|
{
|
|
LOG_F(INFO, "水下通信失效");
|
|
pm->faultCode.push_back(0x5303);
|
|
pm->faultCode.push_back(0x5304);
|
|
pm->faultCode.push_back(0x5305);
|
|
}
|
|
if ((e.injectionFaultCode==3))
|
|
{
|
|
LOG_F(INFO, "水面通信失效");
|
|
pm->faultCode.push_back(0x5301);
|
|
pm->faultCode.push_back(0x5302);
|
|
}
|
|
}
|
|
void PowerManagerFsm::payloadSystemFault(FaultEvent const &e)
|
|
{
|
|
LOG_F(INFO, "payloadSystemFault");
|
|
if ((e.injectionFaultCode==1))
|
|
{
|
|
LOG_F(INFO, "存在启闭机构或者敞水电缸失效");
|
|
pm->faultCode.push_back(0x6211);
|
|
pm->faultCode.push_back(0x6212);
|
|
pm->faultCode.push_back(0x6213);
|
|
pm->faultCode.push_back(0x6214);
|
|
pm->faultCode.push_back(0x6215);
|
|
pm->faultCode.push_back(0x6216);
|
|
pm->faultCode.push_back(0x6217);
|
|
pm->faultCode.push_back(0x6218);
|
|
pm->faultCode.push_back(0x6219);
|
|
pm->faultCode.push_back(0x621A);
|
|
pm->faultCode.push_back(0x621B);
|
|
pm->faultCode.push_back(0x621C);
|
|
pm->faultCode.push_back(0x621D);
|
|
pm->faultCode.push_back(0x621E);
|
|
}
|
|
if ((e.injectionFaultCode==2))
|
|
{
|
|
LOG_F(INFO, "舷侧探测失效");
|
|
pm->faultCode.push_back(0x6321);
|
|
}
|
|
if ((e.injectionFaultCode==3))
|
|
{
|
|
LOG_F(INFO, "舷侧主动探测失效");
|
|
pm->faultCode.push_back(0x6323);
|
|
}
|
|
if ((e.injectionFaultCode==4))
|
|
{
|
|
LOG_F(INFO, "TYZ失效");
|
|
pm->faultCode.push_back(0x6322);
|
|
}
|
|
if ((e.injectionFaultCode==5))
|
|
{
|
|
LOG_F(INFO, "光电探测失效");
|
|
pm->faultCode.push_back(0x6332);
|
|
}
|
|
if ((e.injectionFaultCode==6))
|
|
{
|
|
LOG_F(INFO, "电磁探测失效");
|
|
pm->faultCode.push_back(0x6331);
|
|
}
|
|
if ((e.injectionFaultCode==7))
|
|
{
|
|
LOG_F(INFO, "载荷失效或者全部载荷启闭机构均失效");
|
|
pm->faultCode.push_back(0x633F);
|
|
}
|
|
}
|
|
void PowerManagerFsm::emergencySystemFault(FaultEvent const &e)
|
|
{
|
|
LOG_F(INFO, "emergencySystemFault");
|
|
if ((e.injectionFaultCode==1))
|
|
{
|
|
//备注：抛载具有自己的电池
|
|
LOG_F(INFO, "部分抛载失效");
|
|
pm->faultCode.push_back(0x7202);
|
|
pm->faultCode.push_back(0x7203);
|
|
}
|
|
if ((e.injectionFaultCode==2))
|
|
{
|
|
LOG_F(INFO, "一个超深检测装置失效");
|
|
pm->faultCode.push_back(0x7204);
|
|
pm->faultCode.push_back(0x7205);
|
|
}
|
|
if ((e.injectionFaultCode==3))
|
|
{
|
|
LOG_F(INFO, "注水机构失效");
|
|
pm->faultCode.push_back(0x7301);
|
|
}
|
|
if ((e.injectionFaultCode==4))
|
|
{
|
|
LOG_F(INFO, "抛载失效");
|
|
pm->faultCode.push_back(0x7302);
|
|
pm->faultCode.push_back(0x7303);
|
|
}
|
|
if ((e.injectionFaultCode==5))
|
|
{
|
|
LOG_F(INFO, "艏部应急电源失效");
|
|
pm->faultCode.push_back(0x7306);
|
|
}
|
|
if ((e.injectionFaultCode==6))
|
|
{
|
|
LOG_F(INFO, "艉部应急电源失效");
|
|
pm->faultCode.push_back(0x7307);
|
|
}
|
|
}
|
|
void PowerManagerFsm::sensorSystemFault(FaultEvent const &e)
|
|
{
|
|
LOG_F(INFO, "sensorSystemFault");
|
|
if ((e.injectionFaultCode==1))
|
|
{
|
|
LOG_F(INFO, "部分CTD失效");
|
|
pm->faultCode.push_back(0x8202);
|
|
}
|
|
if ((e.injectionFaultCode==2))
|
|
{
|
|
LOG_F(INFO, "CTD失效");
|
|
pm->faultCode.push_back(0x8302);
|
|
pm->faultCode.push_back(0x8303);
|
|
}
|
|
if ((e.injectionFaultCode==3))
|
|
{
|
|
LOG_F(INFO, "声速仪失效");
|
|
pm->faultCode.push_back(0x8301);
|
|
}
|
|
}
|
|
void PowerManagerFsm::react(FaultEvent const &e)
|
|
{
|
|
LOG_F(INFO, "处理故障事件");
|
|
basicNavigationFault(e);
|
|
extendedNavigationFault(e);
|
|
obstacleAvoidanceFault(e);
|
|
buoyancyControlFault(e);
|
|
communicationSystemFault(e);
|
|
payloadSystemFault(e);
|
|
emergencySystemFault(e);
|
|
sensorSystemFault(e);
|
|
}
|
|
|
|
|
|
|
|
void PowerManagerFsm::handelPowerCmd(){
|
|
switch (pm->m_msCmd.powerCMD)
|
|
{
|
|
case 0:
|
|
LOG_F(INFO, "收到上位机控制指令:关机");
|
|
break;
|
|
case 1:
|
|
LOG_F(INFO, "收到上位机控制指令:动力电上电");
|
|
break;
|
|
case 2:
|
|
LOG_F(INFO, "收到上位机控制指令:锂电池启动");
|
|
break;
|
|
case 3:
|
|
LOG_F(INFO, "收到上位机控制指令:燃料电池");
|
|
break;
|
|
case 4:
|
|
LOG_F(INFO, "收到上位机控制指令:锂动力电关闭");
|
|
break;
|
|
case 5:
|
|
LOG_F(INFO, "收到上位机控制指令:燃料电池停机");
|
|
break;
|
|
case 6:
|
|
LOG_F(INFO, "收到上位机控制指令:燃料电池预热指令");
|
|
break;
|
|
case 7:
|
|
LOG_F(INFO, "收到上位机控制指令:燃料电池自检指令");
|
|
break;
|
|
case 8:
|
|
LOG_F(INFO, "收到上位机控制指令:燃料电池紧急停机");
|
|
break;
|
|
case 9:
|
|
LOG_F(INFO, "收到上位机控制指令:燃料电池复位指令");
|
|
break;
|
|
case 10:
|
|
LOG_F(INFO, "收到上位机控制指令:燃料电池补给指令");
|
|
break;
|
|
case 11:
|
|
LOG_F(INFO, "收到上位机控制指令:燃料电池停机维护");
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void PowerManagerFsm::handleWorkCmd(){
|
|
switch (pm->m_msCmd.workCMD)
|
|
{
|
|
case 1:
|
|
LOG_F(INFO, "收到上位机控制指令:待机工况");
|
|
transit<StandbyState>();
|
|
break;
|
|
case 2:
|
|
LOG_F(INFO, "收到上位机控制指令:岸基备航");
|
|
transit<ShoreBasedReady>();
|
|
break;
|
|
case 3:
|
|
LOG_F(INFO, "收到上位机控制指令:水中备航");
|
|
transit<WaterBasedReady>();
|
|
break;
|
|
case 4:
|
|
LOG_F(INFO, "收到上位机控制指令:遥控工况");
|
|
transit<RemoteControl>();
|
|
break;
|
|
case 5:
|
|
LOG_F(INFO, "收到上位机控制指令:巡航模式");
|
|
transit<CruiseMode>();
|
|
break;
|
|
case 6:
|
|
LOG_F(INFO, "收到上位机控制指令:高速模式");
|
|
transit<HighSpeedMode>();
|
|
break;
|
|
case 7:
|
|
LOG_F(INFO, "收到上位机控制指令:上浮下潜");
|
|
transit<FloatDown>();
|
|
break;
|
|
case 8:
|
|
LOG_F(INFO, "收到上位机控制指令:浮调模式");
|
|
transit<FloatAdjust>();
|
|
break;
|
|
case 9:
|
|
LOG_F(INFO, "收到上位机控制指令:水下侦查");
|
|
transit<UnderwaterSurvey>();
|
|
break;
|
|
case 10:
|
|
LOG_F(INFO, "收到上位机控制指令:水面侦查");
|
|
transit<SurfaceSurvey>();
|
|
break;
|
|
case 11:
|
|
LOG_F(INFO, "收到上位机控制指令:DJ模式");
|
|
transit<DJMode>();
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void PowerManagerFsm::handleModCmd(){
|
|
switch (pm->m_msCmd.modCMD)
|
|
{
|
|
case 0:
|
|
LOG_F(INFO, "收到上位机控制指令:正常模式");
|
|
pm->currentMod = PMMode::Normal;
|
|
break;
|
|
case 1:
|
|
LOG_F(INFO, "收到上位机控制指令:试验模式");
|
|
pm->currentMod = PMMode::Test;
|
|
break;
|
|
case 2:
|
|
LOG_F(INFO, "收到上位机控制指令:调试模式");
|
|
pm->currentMod = PMMode::Debug;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
LOG_F(INFO, "当前工作模式: %d", pm->currentMod);
|
|
}
|
|
|
|
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); |