diff --git a/src/pPowerManger/fsm/PowerManagerFsm.cpp b/src/pPowerManger/fsm/PowerManagerFsm.cpp index 894f64d..991fdbc 100644 --- a/src/pPowerManger/fsm/PowerManagerFsm.cpp +++ b/src/pPowerManger/fsm/PowerManagerFsm.cpp @@ -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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(id), (unsigned char)cmd); pm->m_upperCommManager->processPeriodicTasks(); } diff --git a/src/pPowerManger/fsm/ShoreBasedReady.cpp b/src/pPowerManger/fsm/ShoreBasedReady.cpp index 82093bf..8b8648c 100644 --- a/src/pPowerManger/fsm/ShoreBasedReady.cpp +++ b/src/pPowerManger/fsm/ShoreBasedReady.cpp @@ -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);