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-07-12 15:57:21 +08:00
parent 2ef5fd92ac
commit 5360521750
2 changed files with 24 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/pPowerManger/fsm/PowerManagerFsm.cpp
+15 -40
View File
@@ -138,6 +138,21 @@ void PowerManagerFsm::handleCuriseWork()
MOOSPause(3);
pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(1);
}
//2.6 艉部舵机
if(pm->m_currentDriverState.sternRudderServo1 != 1 ||
pm->m_currentDriverState.sternRudderServo2 != 1 ||
pm->m_currentDriverState.sternRudderServo3!= 1 ||
pm->m_currentDriverState.sternRudderServo4 != 1)
{
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo1, (unsigned char)cmd);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo2, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(1);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo3, (unsigned char)cmd);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo4, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(1);
}
}
void PowerManagerFsm::handleNoPowerDeviceCmd(int id , int cmd){
@@ -178,7 +193,6 @@ void PowerManagerFsm::handleBowRudderSystemCmd(int id, int cmd){
if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(0);
MOOSPause(10);
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_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowRudderServo2, (unsigned char)cmd);
@@ -189,7 +203,6 @@ void PowerManagerFsm::handleBowRudderSystemCmd(int id, int cmd){
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowRudderServo1, (unsigned char)cmd);
// pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowRudderServo2, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(10);
pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(1);
}
}
@@ -200,13 +213,11 @@ void PowerManagerFsm::handleBowRudderSystemCmd(int id, int cmd){
{
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(5);
pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(1);
}
else if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(0);
MOOSPause(5);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
}
@@ -242,7 +253,6 @@ void PowerManagerFsm::handleActuatorCmd(int id, int cmd){
if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(0);
MOOSPause(10);
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);
@@ -259,7 +269,6 @@ void PowerManagerFsm::handleActuatorCmd(int id, int 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();
MOOSPause(3);
pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(1);
}
}
@@ -270,13 +279,11 @@ void PowerManagerFsm::handleActuatorCmd(int id, int cmd){
{
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(5);
pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(1);
}
else if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(0);
MOOSPause(5);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
}
@@ -317,7 +324,6 @@ void PowerManagerFsm::handleOpenWaterCmd(int id, int cmd)
if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(0);
MOOSPause(10);
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);
@@ -338,7 +344,6 @@ void PowerManagerFsm::handleOpenWaterCmd(int id, int 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();
MOOSPause(10);
pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(1);
}
}
@@ -349,13 +354,11 @@ void PowerManagerFsm::handleOpenWaterCmd(int id, int cmd)
{
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(5);
pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(1);
}
else if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(0);
MOOSPause(5);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
}
@@ -389,7 +392,6 @@ void PowerManagerFsm::handleSternRudderCmd(int id, int cmd)
if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(0);
MOOSPause(10);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo1, (unsigned char)cmd);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo2, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
@@ -399,7 +401,6 @@ void PowerManagerFsm::handleSternRudderCmd(int id, int cmd)
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo1, (unsigned char)cmd);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo2, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(10);
pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(1);
}
}
@@ -410,13 +411,11 @@ void PowerManagerFsm::handleSternRudderCmd(int id, int cmd)
{
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(5);
pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(1);
}
else if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(0);
MOOSPause(5);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
}
@@ -447,7 +446,6 @@ void PowerManagerFsm::handleSternRudderCmd(int id, int cmd)
if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(0);
MOOSPause(10);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo3, (unsigned char)cmd);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo4, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
@@ -457,7 +455,6 @@ void PowerManagerFsm::handleSternRudderCmd(int id, int cmd)
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo3, (unsigned char)cmd);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo4, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(10);
pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(1);
}
}
@@ -468,13 +465,11 @@ void PowerManagerFsm::handleSternRudderCmd(int id, int cmd)
{
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(5);
pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(1);
}
else if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(0);
MOOSPause(5);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
}
@@ -507,14 +502,12 @@ void PowerManagerFsm::handlePropulsionMotorCmd(int id, int cmd)
if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(0);
MOOSPause(10);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::thruster, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
} else if(cmd == EquipmentCommand::ON)
{
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::thruster, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(10);
pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(1);
}
}
@@ -525,13 +518,11 @@ void PowerManagerFsm::handlePropulsionMotorCmd(int id, int cmd)
{
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(5);
pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(1);
}
else if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(0);
MOOSPause(5);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
}
@@ -566,7 +557,6 @@ void PowerManagerFsm::handleBuoyancyAdjustmentCmd(int id, int cmd)
if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(0);
MOOSPause(10);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust1, (unsigned char)cmd);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust2, (unsigned char)cmd);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust3, (unsigned char)cmd);
@@ -577,7 +567,6 @@ void PowerManagerFsm::handleBuoyancyAdjustmentCmd(int id, int cmd)
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust2, (unsigned char)cmd);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust3, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(10);
pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(1);
}
}
@@ -588,13 +577,11 @@ void PowerManagerFsm::handleBuoyancyAdjustmentCmd(int id, int cmd)
{
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(5);
pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(1);
}
else if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(0);
MOOSPause(5);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
}
@@ -625,7 +612,6 @@ void PowerManagerFsm::handleBuoyancyAdjustmentCmd(int id, int cmd)
if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HV_sternFt45_Breaker(0);
MOOSPause(10);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust4, (unsigned char)cmd);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust5, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
@@ -634,7 +620,6 @@ void PowerManagerFsm::handleBuoyancyAdjustmentCmd(int id, int cmd)
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust4, (unsigned char)cmd);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust5, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(10);
pm->m_lowerCommManager->operate_HV_sternFt45_Breaker(1);
}
}
@@ -645,13 +630,11 @@ void PowerManagerFsm::handleBuoyancyAdjustmentCmd(int id, int cmd)
{
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(5);
pm->m_lowerCommManager->operate_HV_sternFt45_Breaker(1);
}
else if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HV_sternFt45_Breaker(0);
MOOSPause(5);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
}
@@ -685,12 +668,10 @@ void PowerManagerFsm::handleFBSystemCmd(int id, int cmd)
{
pm->m_lowerCommManager->operate_HV_fbReserved_Breaker(0);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(10);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyage, (unsigned char)cmd);
} else if(cmd == EquipmentCommand::ON)
{
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyage, (unsigned char)cmd);
MOOSPause(10);
pm->m_lowerCommManager->operate_HV_fbReserved_Breaker(1);
pm->m_upperCommManager->processPeriodicTasks();
}
@@ -702,13 +683,11 @@ void PowerManagerFsm::handleFBSystemCmd(int id, int cmd)
{
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(5);
pm->m_lowerCommManager->operate_HV_fbReserved_Breaker(1);
}
else if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HV_fbReserved_Breaker(0);
MOOSPause(5);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
}
@@ -807,14 +786,12 @@ void PowerManagerFsm::handleMastFoldingSystemCmd(int id, int cmd)
if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(0);
MOOSPause(10);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::mastLiftingServo, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
} else if(cmd == EquipmentCommand::ON)
{
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::mastLiftingServo, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(10);
pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(1);
}
}
@@ -825,13 +802,11 @@ void PowerManagerFsm::handleMastFoldingSystemCmd(int id, int cmd)
{
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
MOOSPause(5);
pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(1);
}
else if(cmd == EquipmentCommand::OFF)
{
pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(0);
MOOSPause(5);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, static_cast<DeviceId::Type>(id), (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
}
src/pPowerManger/fsm/ShoreBasedReady.cpp
+9
View File
@@ -27,6 +27,15 @@ void ShoreBasedReady::entry() {
MOOSPause(3);
//2. 启动动力电设备
unsigned char cmd = 1;
//2.0 艉舵
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo1, (unsigned char)cmd);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo2, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(1);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo3, (unsigned char)cmd);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo4, (unsigned char)cmd);
pm->m_upperCommManager->processPeriodicTasks();
pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(1);
//2.1 启闭机构
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover1, (unsigned char)cmd);
pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover2, (unsigned char)cmd);