72 lines
3.7 KiB
C++
72 lines
3.7 KiB
C++
#include "PowerManagerFsm.hpp"
|
|
|
|
// FaultState实现
|
|
void FaultState::entry() {
|
|
PowerManagerFsm::entry();
|
|
if(pm->m_missionState.type == TaskType::THROW_LOAD)
|
|
{
|
|
LOG_F(INFO, "检测到抛载");
|
|
pm->m_systemData->setSystemInfo("ERROR:检测到抛载");
|
|
//1 解除启闭机构
|
|
unsigned char cmd = 0;
|
|
pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(0);
|
|
MOOSPause(3);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover1, (unsigned char)cmd);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover2, (unsigned char)cmd);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover3, (unsigned char)cmd);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover4, (unsigned char)cmd);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover5, (unsigned char)cmd);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover6, (unsigned char)cmd);
|
|
pm->m_upperCommManager->processPeriodicTasks();
|
|
//2 解除敞水机构
|
|
pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(0);
|
|
MOOSPause(3);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet1, (unsigned char)cmd);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet2, (unsigned char)cmd);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet3, (unsigned char)cmd);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet4, (unsigned char)cmd);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet5, (unsigned char)cmd);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet6, (unsigned char)cmd);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet7, (unsigned char)cmd);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet8, (unsigned char)cmd);
|
|
pm->m_upperCommManager->processPeriodicTasks();
|
|
//3 解除艏部舵机
|
|
pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(0);
|
|
MOOSPause(3);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowRudderDeploy, (unsigned char)cmd);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowRudderServo1, (unsigned char)cmd);
|
|
pm->m_upperCommManager->processPeriodicTasks();
|
|
//4 解除艉部舵机
|
|
pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(0);
|
|
MOOSPause(3);
|
|
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::mastLiftingServo, (unsigned char)cmd);
|
|
pm->m_upperCommManager->processPeriodicTasks();
|
|
//关闭推进电机
|
|
pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(0);
|
|
//检测电机去使能状态，5秒超时
|
|
auto startTime = std::chrono::steady_clock::now();
|
|
while(pm->m_thrustEnable == 1)
|
|
{
|
|
// 检查是否超时（5秒）
|
|
auto currentTime = std::chrono::steady_clock::now();
|
|
auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(currentTime - startTime);
|
|
if(elapsed.count() >= 20)
|
|
{
|
|
LOG_F(WARNING, "电机去使能超时（20秒）");
|
|
pm->m_systemData->setSystemInfo("ERROR:电机去使能超时（20秒）");
|
|
return;
|
|
}
|
|
MOOSPause(100);
|
|
}
|
|
}
|
|
}
|
|
|
|
void FaultState::react(FaultEvent const & e) {
|
|
LOG_F(INFO, "FaultState 处理故障事件");
|
|
}
|
|
|
|
void FaultState::react(MasterCommandEvent const &) {
|
|
PowerManagerFsm::handelPowerCmd();
|
|
PowerManagerFsm::handleModCmd();
|
|
PowerManagerFsm::handleWorkCmd();
|
|
} |