From 97a86acae4880ffae4f9ec98cbf616e9db06b107 Mon Sep 17 00:00:00 2001 From: zhaojingkui <1553836110@qq.com> Date: Mon, 28 Apr 2025 19:41:37 +0800 Subject: [PATCH] =?UTF-8?q?=E8=BF=9B=E8=A1=8C=E4=BA=86=E6=B0=B4=E9=9D=A2?= =?UTF-8?q?=E7=AB=AF=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- data/msg/uPower_currentState_st.json | 95 +- data/msg/uPower_disSysState_st.json | 21 +- data/msg/uPower_equState_fb.json | 124 +- data/msg/uPower_fcsState_st.json | 61 +- data/msg/uPower_liBatState_st.json | 77 +- data/msg/uPower_pmState_fb.json | 4 +- data/msg/uPower_secdistState_cmd.json | 124 +- .../uExternComm_setDeviceSwitch_cmd.json | 4 + .../uExternComm_setOpCondition_cmd.json | 3 + data/testmsg/uExternComm_setOpMode_cmd.json | 3 + .../uExternComm_setPowerSystem_cmd.json | 3 + data/testmsg/uPlan_planStop_cmd.json | 4 + src/pPMtest/PMtest.cpp | 51 + src/pPMtest/PMtest.h | 7 + src/pPowerManger/CMakeLists.txt | 1 + src/pPowerManger/PowerManagerFsm.cpp | 2173 +++++++++++++++-- src/pPowerManger/PowerManagerFsm.hpp | 72 +- src/pPowerManger/PowerManger.cpp | 31 +- src/pPowerManger/PowerManger.h | 5 +- src/pPowerManger/UpperCommManager.cpp | 53 +- src/pPowerManger/UpperCommManager.h | 4 +- src/pPowerManger/driver.cpp | 474 ++-- src/pPowerManger/pmSysvariable.h | 53 + test/scripts/TestGUI.py | 1 + 24 files changed, 2734 insertions(+), 714 deletions(-) create mode 100644 data/testmsg/uExternComm_setDeviceSwitch_cmd.json create mode 100644 data/testmsg/uExternComm_setOpCondition_cmd.json create mode 100644 data/testmsg/uExternComm_setOpMode_cmd.json create mode 100644 data/testmsg/uExternComm_setPowerSystem_cmd.json create mode 100644 data/testmsg/uPlan_planStop_cmd.json diff --git a/data/msg/uPower_currentState_st.json b/data/msg/uPower_currentState_st.json index 41648bf..0edf428 100644 --- a/data/msg/uPower_currentState_st.json +++ b/data/msg/uPower_currentState_st.json @@ -1,47 +1,48 @@ -{ - "HVABFTDev123Cur_uint16": 0, - "HVABactCur_uint16": 0, - "HVABbowRudCur_uint16": 0, - "HVABbusBCur_uint16": 0, - "HVABbusBVol_uint16": 0, - "HVABmastSteGeaCur_uint16": 0, - "HVABopenWaterCur_uint16": 0, - "HVABxczCur_uint16": 0, - "HVBBCCUCur_uint16": 0, - "HVBBFCSSecurSysCur_uint16": 0, - "HVBBbatInsCur_uint16": 0, - "HVBBbowLowVolDisBoxCur_uint16": 0, - "HVBBdcC1InsCur_uint16": 0, - "HVBBemer2BusVol_uint16": 0, - "HVBBemerBatCur_uint16": 0, - "HVBBinsBusVol_uint16": 0, - "HVBBplcCur_uint16": 0, - "HVBBreservedCur1_uint16": 0, - "HVBBreservedCur2_uint16": 0, - "HVBBunit4Cur_uint16": 0, - "HVBFTDev45Cur_uint16": 0, - "HVBbatGroupCur_uint16": 0, - "HVBbowHVolDisBoxCur_uint16": 0, - "HVBbusACur_uint16": 0, - "HVBbusAVol_uint16": 0, - "HVBdcDc5Cur_uint16": 0, - "HVBfbCur_uint16": 0, - "HVBmotorCur_uint16": 0, - "HVBpowerBusCur_uint16": 0, - "HVBpowerBusVol_uint16": 0, - "HVBreservedCur1_uint16": 0, - "HVBreservedCur2_uint16": 0, - "HVBreservedCur3_uint16": 0, - "HVBsternRud1Cur_uint16": 0, - "HVBsternRud2Cur_uint16": 0, - "HVBtyzCur_uint16": 0, - "LVBBbowPZDevCur_uint16": 0, - "LVBBemerBatCur_uint16": 0, - "LVBBemerBusVol_uint16": 0, - "LVBBinsBusCur_uint16": 0, - "LVBBunit1Cur_uint16": 0, - "LVBBunit2Cur_uint16": 0, - "LVBBunit3Cur_uint16": 0, - "LVBBunit5Cur_uint16": 0, - "LVBinsBusVol_uint16": 0 -} \ No newline at end of file +{"uPower_currentState_st":{ + "HVABFTDev123Cur_uint16" : 0, + "HVABactCur_uint16" : 0, + "HVABbowRudCur_uint16" : 0, + "HVABbusBCur_uint16" : 0, + "HVABbusBVol_uint16" : 0, + "HVABmastSteGeaCur_uint16" : 0, + "HVABopenWaterCur_uint16" : 0, + "HVABxczCur_uint16" : 0, + "HVBBCCUCur_uint16" : 0, + "HVBBFCSSecurSysCur_uint16" : 0, + "HVBBbatInsCur_uint16" : 0, + "HVBBbowLowVolDisBoxCur_uint16" : 0, + "HVBBdcC1InsCur_uint16" : 0, + "HVBBemer2BusVol_uint16" : 0, + "HVBBemerBatCur_uint16" : 0, + "HVBBinsBusVol_uint16" : 0, + "HVBBplcCur_uint16" : 0, + "HVBBreservedCur1_uint16" : 0, + "HVBBreservedCur2_uint16" : 0, + "HVBBunit4Cur_uint16" : 0, + "HVBFTDev45Cur_uint16" : 0, + "HVBbatGroupCur_uint16" : 0, + "HVBbowHVolDisBoxCur_uint16" : 0, + "HVBbusACur_uint16" : 0, + "HVBbusAVol_uint16" : 0, + "HVBdcDc5Cur_uint16" : 0, + "HVBfbCur_uint16" : 0, + "HVBmotorCur_uint16" : 0, + "HVBpowerBusCur_uint16" : 0, + "HVBpowerBusVol_uint16" : 0, + "HVBreservedCur1_uint16" : 0, + "HVBreservedCur2_uint16" : 0, + "HVBreservedCur3_uint16" : 0, + "HVBsternRud1Cur_uint16" : 0, + "HVBsternRud2Cur_uint16" : 0, + "HVBtyzCur_uint16" : 0, + "LVBBbowPZDevCur_uint16" : 0, + "LVBBemerBatCur_uint16" : 0, + "LVBBemerBusVol_uint16" : 0, + "LVBBinsBusCur_uint16" : 0, + "LVBBunit1Cur_uint16" : 0, + "LVBBunit2Cur_uint16" : 0, + "LVBBunit3Cur_uint16" : 0, + "LVBBunit5Cur_uint16" : 0, + "LVBinsBusVol_uint16" : 0 +} +} diff --git a/data/msg/uPower_disSysState_st.json b/data/msg/uPower_disSysState_st.json index ed8e1bf..adb658e 100644 --- a/data/msg/uPower_disSysState_st.json +++ b/data/msg/uPower_disSysState_st.json @@ -1,10 +1,11 @@ -{ - "HVolABusBreaker_unit16": 0, - "HVolBBusBreaker_unit16": 0, - "HVolBusBreaker_uint16": 0, - "LVolBusBreaker_uint16": 0, - "faultCode_array": - [ - 0 - ] -} \ No newline at end of file +{"uPower_disSysState_st":{ + "HVolABusBreaker_unit16" : 0, + "HVolBBusBreaker_unit16" : 0, + "HVolBusBreaker_uint16" : 0, + "LVolBusBreaker_uint16" : 0, + "faultCode_array" : + [ + 0 + ] +} +} diff --git a/data/msg/uPower_equState_fb.json b/data/msg/uPower_equState_fb.json index 8a49970..558c0af 100644 --- a/data/msg/uPower_equState_fb.json +++ b/data/msg/uPower_equState_fb.json @@ -1,65 +1,65 @@ {"uPower_equState_fb":{ - "DVL" : 0, - "USBL" : 0, - "bowCover1" : 0, - "bowCover2" : 0, - "bowCover3" : 0, - "bowCover4" : 0, - "bowCover5" : 0, - "bowCover6" : 0, - "bowRudderDeploy" : 0, - "bowRudderServo1" : 0, - "bowRudderServo2" : 0, - "bowThrow" : 0, - "bowWaterlnet1" : 0, - "bowWaterlnet2" : 0, - "bowWaterlnet3" : 0, - "bowWaterlnet4" : 0, - "bowWaterlnet5" : 0, - "bowWaterlnet6" : 0, - "bowWaterlnet7" : 0, - "bowWaterlnet8" : 0, - "buoyage" : 0, - "buoyancyAdjust1" : 0, - "buoyancyAdjust2" : 0, - "buoyancyAdjust3" : 0, - "buoyancyAdjust4" : 0, - "buoyancyAdjust5" : 0, - "commMast" : 0, - "ctd1" : 0, - "ctd2" : 0, - "depth1" : 0, - "depth2" : 0, - "depth3" : 0, - "electSensor" : 0, - "forwardSonar" : 0, - "inputWaterSensor1" : 0, - "inputWaterSensor2" : 0, - "inputWaterSensor3" : 0, - "ins" : 0, - "launchControler" : 0, - "lowFreqSonar" : 0, - "mastLiftingServo" : 0, - "ntp" : 0, - "overDeep1" : 0, - "overDeep2" : 0, - "photoSensor" : 0, - "sideSonar" : 0, - "singleBeamSonar1" : 0, - "singleBeamSonar2" : 0, - "singleBeamSonar3" : 0, - "singleBeamSonar4" : 0, - "singleBeamSonar5" : 0, - "singleBeamSonar6" : 0, - "singleBeamSonar7" : 0, - "singleBeamSonar8" : 0, - "soundVeloc" : 0, - "sternRudderServo1" : 0, - "sternRudderServo2" : 0, - "sternRudderServo3" : 0, - "sternRudderServo4" : 0, - "sternThrow" : 0, - "thruster" : 0, - "waterInject" : 0 + "DVL_uni8" : 0, + "USBL_uni8" : 0, + "bowCover1_uni8" : 0, + "bowCover2_uni8" : 0, + "bowCover3_uni8" : 0, + "bowCover4_uni8" : 0, + "bowCover5_uni8" : 0, + "bowCover6_uni8" : 0, + "bowRudderDeploy_uni8" : 0, + "bowRudderServo1_uni8" : 0, + "bowRudderServo2_uni8" : 0, + "bowThrow_uni8" : 0, + "bowWaterlnet1_uni8" : 0, + "bowWaterlnet2_uni8" : 0, + "bowWaterlnet3_uni8" : 0, + "bowWaterlnet4_uni8" : 0, + "bowWaterlnet5_uni8" : 0, + "bowWaterlnet6_uni8" : 0, + "bowWaterlnet7_uni8" : 0, + "bowWaterlnet8_uni8" : 0, + "buoyage_uni8" : 0, + "buoyancyAdjust1_uni8" : 0, + "buoyancyAdjust2_uni8" : 0, + "buoyancyAdjust3_uni8" : 0, + "buoyancyAdjust4_uni8" : 0, + "buoyancyAdjust5_uni8" : 0, + "commMast_uni8" : 0, + "ctd1_uni8" : 0, + "ctd2_uni8" : 0, + "depth1_uni8" : 0, + "depth2_uni8" : 0, + "depth3_uni8" : 0, + "electSensor_uni8" : 0, + "forwardSonar_uni8" : 0, + "inputWaterSensor1_uni8" : 0, + "inputWaterSensor2_uni8" : 0, + "inputWaterSensor3_uni8" : 0, + "ins_uni8" : 0, + "launchControler_uni8" : 0, + "lowFreqSonar_uni8" : 0, + "mastLiftingServo_uni8" : 0, + "ntp_uni8" : 0, + "overDeep1_uni8" : 0, + "overDeep2_uni8" : 0, + "photoSensor_uni8" : 0, + "sideSonar_uni8" : 0, + "singleBeamSonar1_uni8" : 0, + "singleBeamSonar2_uni8" : 0, + "singleBeamSonar3_uni8" : 0, + "singleBeamSonar4_uni8" : 0, + "singleBeamSonar5_uni8" : 0, + "singleBeamSonar6_uni8" : 0, + "singleBeamSonar7_uni8" : 0, + "singleBeamSonar8_uni8" : 0, + "soundVeloc_uni8" : 0, + "sternRudderServo1_uni8" : 0, + "sternRudderServo2_uni8" : 0, + "sternRudderServo3_uni8" : 0, + "sternRudderServo4_uni8" : 0, + "sternThrow_uni8" : 0, + "thruster_uni8" : 0, + "waterInject_uni8" : 0 } } diff --git a/data/msg/uPower_fcsState_st.json b/data/msg/uPower_fcsState_st.json index 31141fb..2e81843 100644 --- a/data/msg/uPower_fcsState_st.json +++ b/data/msg/uPower_fcsState_st.json @@ -1,30 +1,31 @@ -{ - "backpressure1_fp32": 0.0, - "backpressure2_fp32": 0.0, - "ch4oConcent1_fp32": 0.0, - "ch4oConcent2_fp32": 0.0, - "ch4o_fp32": 0.0, - "excTemp_fp32": 0.0, - "faultCode1_uint16": 0, - "faultCode2_uint16": 0, - "faultCode3_uint16": 0, - "fcuCurIns_fp32": 0.0, - "fcuCurPower_fp32": 0.0, - "fcuCur_fp32": 0.0, - "fcuFaultLeve_uint8": 0, - "fcuOutIns_fp32": 0.0, - "fcuOutPower_fp32": 0.0, - "fcuState_uint8": 0, - "fcuVolIns_fp32": 0.0, - "fcuVolPower_fp32": 0.0, - "fcuVol_fp32": 0.0, - "flameDet_uint8": 0, - "h2Concent1_fp32": 0.0, - "h2Concent2_fp32": 0.0, - "o2Concent1_fp32": 0.0, - "o2Concent2_fp32": 0.0, - "o2Level_fp32": 0.0, - "o2_fp32": 0.0, - "powLimit_fp32": 0.0, - "powerGen_fp32": 0.0 -} \ No newline at end of file +{"uPower_fcsState_st":{ + "backpressure1_fp32" : 0.0, + "backpressure2_fp32" : 0.0, + "ch4oConcent1_fp32" : 0.0, + "ch4oConcent2_fp32" : 0.0, + "ch4o_fp32" : 0.0, + "excTemp_fp32" : 0.0, + "faultCode1_uint16" : 0, + "faultCode2_uint16" : 0, + "faultCode3_uint16" : 0, + "fcuCurIns_fp32" : 0.0, + "fcuCurPower_fp32" : 0.0, + "fcuCur_fp32" : 0.0, + "fcuFaultLeve_uint8" : 0, + "fcuOutIns_fp32" : 0.0, + "fcuOutPower_fp32" : 0.0, + "fcuState_uint8" : 0, + "fcuVolIns_fp32" : 0.0, + "fcuVolPower_fp32" : 0.0, + "fcuVol_fp32" : 0.0, + "flameDet_uint8" : 0, + "h2Concent1_fp32" : 0.0, + "h2Concent2_fp32" : 0.0, + "o2Concent1_fp32" : 0.0, + "o2Concent2_fp32" : 0.0, + "o2Level_fp32" : 0.0, + "o2_fp32" : 0.0, + "powLimit_fp32" : 0.0, + "powerGen_fp32" : 0.0 +} +} diff --git a/data/msg/uPower_liBatState_st.json b/data/msg/uPower_liBatState_st.json index b3e9fd4..9c92b81 100644 --- a/data/msg/uPower_liBatState_st.json +++ b/data/msg/uPower_liBatState_st.json @@ -1,38 +1,39 @@ -{ - "fcuCurToInsLimit1_uint8": 0, - "fcuCurToinsLimit2_uint8": 0, - "insBatCurElect_uint16": 0, - "insBatLimit_uint16": 0, - "insBatSoc_uint8": 0, - "insBatTotalElect_uint16": 0, - "insChargeState_uint8": 0, - "insCurSocHold1_uint8": 0, - "insCurSocHold2_uint8": 0, - "insCurSocHold3_uint8": 0, - "insCurrent_uint16": 0, - "insLinkVol_uint16": 0, - "insNegRelayState_uint8": 0, - "insNegRes_uint16": 0, - "insPackVol_uint16": 0, - "insPosRelayState_uint8": 0, - "insPosRes_uint16": 0, - "insfaultCode_uint8": 0, - "powBatCurElect_uint16": 0, - "powBatSoc_uint8": 0, - "powBatTotalElect_uint16": 0, - "powChargeState_uint8": 0, - "powCurSocHold1_uint8": 0, - "powCurSocHold2_uint8": 0, - "powCurSocHold3_uint8": 0, - "powCurToInsLimit1_uint8": 0, - "powCurToinsLimit2_uint8": 0, - "powCur_uint16": 0, - "powLinkVol_uint16": 0, - "powNegRelayState_uint8": 0, - "powNegRes_uint16": 0, - "powPosRelayState_uint8": 0, - "powPosRes_uint16": 0, - "powerBatLimit_uint16": 0, - "powerPackVol_uint16": 0, - "powfaultCode_uint8": 0 -} \ No newline at end of file +{"uPower_liBatState_st":{ + "fcuCurToInsLimit1_uint8" : 0, + "fcuCurToinsLimit2_uint8" : 0, + "insBatCurElect_uint16" : 0, + "insBatLimit_uint16" : 0, + "insBatSoc_uint8" : 0, + "insBatTotalElect_uint16" : 0, + "insChargeState_uint8" : 0, + "insCurSocHold1_uint8" : 0, + "insCurSocHold2_uint8" : 0, + "insCurSocHold3_uint8" : 0, + "insCurrent_uint16" : 0, + "insLinkVol_uint16" : 0, + "insNegRelayState_uint8" : 0, + "insNegRes_uint16" : 0, + "insPackVol_uint16" : 0, + "insPosRelayState_uint8" : 0, + "insPosRes_uint16" : 0, + "insfaultCode_uint8" : 0, + "powBatCurElect_uint16" : 0, + "powBatSoc_uint8" : 0, + "powBatTotalElect_uint16" : 0, + "powChargeState_uint8" : 0, + "powCurSocHold1_uint8" : 0, + "powCurSocHold2_uint8" : 0, + "powCurSocHold3_uint8" : 0, + "powCurToInsLimit1_uint8" : 0, + "powCurToinsLimit2_uint8" : 0, + "powCur_uint16" : 0, + "powLinkVol_uint16" : 0, + "powNegRelayState_uint8" : 0, + "powNegRes_uint16" : 0, + "powPosRelayState_uint8" : 0, + "powPosRes_uint16" : 0, + "powerBatLimit_uint16" : 0, + "powerPackVol_uint16" : 0, + "powfaultCode_uint8" : 0 +} +} diff --git a/data/msg/uPower_pmState_fb.json b/data/msg/uPower_pmState_fb.json index ae91ee7..debded9 100644 --- a/data/msg/uPower_pmState_fb.json +++ b/data/msg/uPower_pmState_fb.json @@ -1,5 +1,5 @@ { - "currentMod_uint8": 0, + "currentMod_uint8": 1, "falutLevel_uint8": 0, "faultCode_array": [ @@ -8,5 +8,5 @@ "powerState_uint8": 0, "soc_uint32": 0, "sustainableTime_uint32": 0, - "workCondition_uint8": 1 + "workCondition_uint8": 5 } \ No newline at end of file diff --git a/data/msg/uPower_secdistState_cmd.json b/data/msg/uPower_secdistState_cmd.json index ea0f3eb..37c6554 100644 --- a/data/msg/uPower_secdistState_cmd.json +++ b/data/msg/uPower_secdistState_cmd.json @@ -1,65 +1,65 @@ {"uPower_secdistState_cmd":{ - "DVL" : 0, - "USBL" : 0, - "bowCover1" : 0, - "bowCover2" : 0, - "bowCover3" : 0, - "bowCover4" : 0, - "bowCover5" : 0, - "bowCover6" : 0, - "bowRudderDeploy" : 0, - "bowRudderServo1" : 0, - "bowRudderServo2" : 0, - "bowThrow" : 0, - "bowWaterlnet1" : 0, - "bowWaterlnet2" : 0, - "bowWaterlnet3" : 0, - "bowWaterlnet4" : 0, - "bowWaterlnet5" : 0, - "bowWaterlnet6" : 0, - "bowWaterlnet7" : 0, - "bowWaterlnet8" : 0, - "buoyage" : 0, - "buoyancyAdjust1" : 0, - "buoyancyAdjust2" : 0, - "buoyancyAdjust3" : 0, - "buoyancyAdjust4" : 0, - "buoyancyAdjust5" : 0, - "commMast" : 1, - "ctd1" : 1, - "ctd2" : 1, - "depth1" : 1, - "depth2" : 1, - "depth3" : 1, - "electSensor" : 0, - "forwardSonar" : 0, - "inputWaterSensor1" : 1, - "inputWaterSensor2" : 1, - "inputWaterSensor3" : 1, - "ins" : 1, - "launchControler" : 0, - "lowFreqSonar" : 0, - "mastLiftingServo" : 0, - "ntp" : 1, - "overDeep1" : 1, - "overDeep2" : 1, - "photoSensor" : 0, - "sideSonar" : 0, - "singleBeamSonar1" : 0, - "singleBeamSonar2" : 0, - "singleBeamSonar3" : 0, - "singleBeamSonar4" : 0, - "singleBeamSonar5" : 0, - "singleBeamSonar6" : 0, - "singleBeamSonar7" : 0, - "singleBeamSonar8" : 0, - "soundVeloc" : 1, - "sternRudderServo1" : 0, - "sternRudderServo2" : 0, - "sternRudderServo3" : 0, - "sternRudderServo4" : 0, - "sternThrow" : 0, - "thruster" : 0, - "waterInject" : 0 + "DVL_uni8" : 0, + "USBL_uni8" : 0, + "bowCover1_uni8" : 0, + "bowCover2_uni8" : 0, + "bowCover3_uni8" : 0, + "bowCover4_uni8" : 0, + "bowCover5_uni8" : 0, + "bowCover6_uni8" : 0, + "bowRudderDeploy_uni8" : 0, + "bowRudderServo1_uni8" : 0, + "bowRudderServo2_uni8" : 0, + "bowThrow_uni8" : 0, + "bowWaterlnet1_uni8" : 0, + "bowWaterlnet2_uni8" : 0, + "bowWaterlnet3_uni8" : 0, + "bowWaterlnet4_uni8" : 0, + "bowWaterlnet5_uni8" : 0, + "bowWaterlnet6_uni8" : 0, + "bowWaterlnet7_uni8" : 0, + "bowWaterlnet8_uni8" : 0, + "buoyage_uni8" : 0, + "buoyancyAdjust1_uni8" : 0, + "buoyancyAdjust2_uni8" : 0, + "buoyancyAdjust3_uni8" : 0, + "buoyancyAdjust4_uni8" : 0, + "buoyancyAdjust5_uni8" : 0, + "commMast_uni8" : 1, + "ctd1_uni8" : 1, + "ctd2_uni8" : 1, + "depth1_uni8" : 1, + "depth2_uni8" : 1, + "depth3_uni8" : 1, + "electSensor_uni8" : 0, + "forwardSonar_uni8" : 0, + "inputWaterSensor1_uni8" : 1, + "inputWaterSensor2_uni8" : 1, + "inputWaterSensor3_uni8" : 1, + "ins_uni8" : 1, + "launchControler_uni8" : 0, + "lowFreqSonar_uni8" : 0, + "mastLiftingServo_uni8" : 0, + "ntp_uni8" : 1, + "overDeep1_uni8" : 1, + "overDeep2_uni8" : 1, + "photoSensor_uni8" : 0, + "sideSonar_uni8" : 0, + "singleBeamSonar1_uni8" : 0, + "singleBeamSonar2_uni8" : 1, + "singleBeamSonar3_uni8" : 0, + "singleBeamSonar4_uni8" : 0, + "singleBeamSonar5_uni8" : 0, + "singleBeamSonar6_uni8" : 0, + "singleBeamSonar7_uni8" : 0, + "singleBeamSonar8_uni8" : 0, + "soundVeloc_uni8" : 1, + "sternRudderServo1_uni8" : 0, + "sternRudderServo2_uni8" : 0, + "sternRudderServo3_uni8" : 0, + "sternRudderServo4_uni8" : 0, + "sternThrow_uni8" : 0, + "thruster_uni8" : 0, + "waterInject_uni8" : 0 } } diff --git a/data/testmsg/uExternComm_setDeviceSwitch_cmd.json b/data/testmsg/uExternComm_setDeviceSwitch_cmd.json new file mode 100644 index 0000000..a39f963 --- /dev/null +++ b/data/testmsg/uExternComm_setDeviceSwitch_cmd.json @@ -0,0 +1,4 @@ +{ + "id_uint8" : 1, + "cmd_uint8" : 1 +} \ No newline at end of file diff --git a/data/testmsg/uExternComm_setOpCondition_cmd.json b/data/testmsg/uExternComm_setOpCondition_cmd.json new file mode 100644 index 0000000..6c4e59c --- /dev/null +++ b/data/testmsg/uExternComm_setOpCondition_cmd.json @@ -0,0 +1,3 @@ +{ + "state_uint8" : 1 +} \ No newline at end of file diff --git a/data/testmsg/uExternComm_setOpMode_cmd.json b/data/testmsg/uExternComm_setOpMode_cmd.json new file mode 100644 index 0000000..6c4e59c --- /dev/null +++ b/data/testmsg/uExternComm_setOpMode_cmd.json @@ -0,0 +1,3 @@ +{ + "state_uint8" : 1 +} \ No newline at end of file diff --git a/data/testmsg/uExternComm_setPowerSystem_cmd.json b/data/testmsg/uExternComm_setPowerSystem_cmd.json new file mode 100644 index 0000000..c744c79 --- /dev/null +++ b/data/testmsg/uExternComm_setPowerSystem_cmd.json @@ -0,0 +1,3 @@ +{ + "powerCmd_uint8" :1 +} \ No newline at end of file diff --git a/data/testmsg/uPlan_planStop_cmd.json b/data/testmsg/uPlan_planStop_cmd.json new file mode 100644 index 0000000..432e152 --- /dev/null +++ b/data/testmsg/uPlan_planStop_cmd.json @@ -0,0 +1,4 @@ +{ + "stamp_fp64" : 0, + "op_int32" : 1 +} \ No newline at end of file diff --git a/src/pPMtest/PMtest.cpp b/src/pPMtest/PMtest.cpp index 5e4d20a..cc483de 100644 --- a/src/pPMtest/PMtest.cpp +++ b/src/pPMtest/PMtest.cpp @@ -142,6 +142,57 @@ bool PMtest::Iterate() Notify(DB_TO_PM_MSCMD, content); } } + else if (c == 'e') + { + //发送DB_TO_PM_MISSION消息 + std::cout << "发送uExternComm_setDeviceSwitch_cmd消息" << std::endl; + string filename = "/home/zjk/project/H100/H100PowerManger/data/testmsg/uExternComm_setDeviceSwitch_cmd.json"; + string content; + if(readJsonFromFile(filename, content)) { + Notify("uExternComm_setDeviceSwitch_cmd", content); + } + } + else if (c == 'q') + { + //发送DB_TO_PM_MISSION消息 + std::cout << "发送uExternComm_setOpCondition_cmd消息" << std::endl; + string filename = "/home/zjk/project/H100/H100PowerManger/data/testmsg/uExternComm_setOpCondition_cmd.json"; + string content; + if(readJsonFromFile(filename, content)) { + Notify("uExternComm_setOpCondition_cmd", content); + } + } + else if (c == 'w') + { + //发送DB_TO_PM_MISSION消息 + std::cout << "发送uExternComm_setOpMode_cmd消息" << std::endl; + string filename = "/home/zjk/project/H100/H100PowerManger/data/testmsg/uExternComm_setOpMode_cmd.json"; + string content; + if(readJsonFromFile(filename, content)) { + Notify("uExternComm_setOpMode_cmd", content); + } + } + else if (c == 'r') + { + //发送DB_TO_PM_MISSION消息 + std::cout << "发送uExternComm_setPowerSystem_cmd消息" << std::endl; + string filename = "/home/zjk/project/H100/H100PowerManger/data/testmsg/uExternComm_setPowerSystem_cmd.json"; + string content; + if(readJsonFromFile(filename, content)) { + Notify("uExternComm_setPowerSystem_cmd", content); + } + } + else if (c == 't') + { + //发送DB_TO_PM_MISSION消息 + std::cout << "发送uExternComm_setPowerSystem_cmd消息" << std::endl; + string filename = "/home/zjk/project/H100/H100PowerManger/data/testmsg/uPlan_planStop_cmd.json"; + string content; + if(readJsonFromFile(filename, content)) { + Notify("uPlan_planStop_cmd", content); + } +} + } return(true); } diff --git a/src/pPMtest/PMtest.h b/src/pPMtest/PMtest.h index 5c02464..a16f9eb 100644 --- a/src/pPMtest/PMtest.h +++ b/src/pPMtest/PMtest.h @@ -30,6 +30,13 @@ using namespace std; //决策消息 #define DB_TO_PM_DEEPTH "uDevice_depth_st" #define DB_TO_PM_MISSION "uPlan_taskStart_cmd" +#define DB_TO_PM_TASKSTOP "uPlan_planStop_cmd" + +//外部通信消息 +#define DB_TO_PM_SETOPCONDITION "uExternComm_setOpCondition_cmd" +#define DB_TO_PM_SETOPMODE "uExternComm_setOpMode_cmd" +#define DB_TO_PM_SETPMSYS "uExternComm_setPowerSystem_cmd" +#define DB_TO_PM_SETDEVSWITCH "uExternComm_setDeviceSwitch_cmd" class PMtest : public CMOOSApp { diff --git a/src/pPowerManger/CMakeLists.txt b/src/pPowerManger/CMakeLists.txt index 520e5cb..4cef8d9 100644 --- a/src/pPowerManger/CMakeLists.txt +++ b/src/pPowerManger/CMakeLists.txt @@ -41,6 +41,7 @@ TARGET_LINK_LIBRARIES(pPowerManger pthread jsoncpp ${SYSTEM_LIBS} + dl # SQLiteDB # sqlite3 ) diff --git a/src/pPowerManger/PowerManagerFsm.cpp b/src/pPowerManger/PowerManagerFsm.cpp index b899237..f634a7a 100644 --- a/src/pPowerManger/PowerManagerFsm.cpp +++ b/src/pPowerManger/PowerManagerFsm.cpp @@ -4,206 +4,1985 @@ #include #include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h" +#define DEBUG // 定义静态成员 PowerManger* PowerManagerFsm::pm = nullptr; +void PowerManagerFsm::react(TaskStopEvent const &e) { + transit(); + LOG_F(INFO, "使命结束"); +}; +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(); + break; + case TaskType::FLOAT_UP: + transit(); + break; + case TaskType::RECYCLE: + transit(); + break; + case TaskType::HOVER: + transit(); + break; + case TaskType::SAT_COMM: + transit(); + break; + case TaskType::SAT_CALIBRATE: + transit(); + break; + case TaskType::TOUR: + transit(); + break; + case TaskType::SAIL: + transit(); + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); - break; + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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, "DevCmdExt received overDeep2 - id: %d, cmd: %d", e.id, e.cmd); + { + LOG_F(INFO, "DevCmdExt received overDeep1 - 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 ctd1 - id: %d, cmd: %d", e.id, e.cmd); + { + 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 ctd2 - id: %d, cmd: %d", e.id, e.cmd); + { + 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 soundVeloc - id: %d, cmd: %d", e.id, e.cmd); + { + 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); + { + 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 bowWaterlnet1 - id: %d, cmd: %d", e.id, e.cmd); + { + LOG_F(INFO, "DevCmdExt received bowWaterlnet1 - id: %d, cmd: %d", e.id, e.cmd); + #ifdef DEBUG + pm->m_currentDriverState.bowWaterlnet1 = e.cmd; + #endif + if (e.cmd == 0) + { + pm->m_subDisSysCmd.bowWaterlnet1 = 0; + //TODO ：动力电断电 + } + else if (e.cmd == 1) + { + pm->m_subDisSysCmd.bowWaterlnet1 = 1; + //TODO ：动力电上电 + } + else if (e.cmd == 2) + { + pm->m_subDisSysCmd.bowWaterlnet1 = 1; + } + else if (e.cmd == 3) + { + //TODO ：动力电上电 + } + else if (e.cmd == 4) + { + pm->m_subDisSysCmd.bowWaterlnet1 = 0; + } + else if (e.cmd == 5) + { + //TODO ：动力电断电 + } + else + { + LOG_F(WARNING, "DevCmdExt received bowWaterlnet1 - 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 bowWaterlnet2 - id: %d, cmd: %d", e.id, e.cmd); + #ifdef DEBUG + pm->m_currentDriverState.bowWaterlnet2 = e.cmd; + #endif + if (e.cmd == 0) + { + pm->m_subDisSysCmd.bowWaterlnet2 = 0; + //TODO ：动力电断电 + } + else if (e.cmd == 1) + { + pm->m_subDisSysCmd.bowWaterlnet2 = 1; + //TODO ：动力电上电 + } + else if (e.cmd == 2) + { + pm->m_subDisSysCmd.bowWaterlnet2 = 1; + } + else if (e.cmd == 3) + { + //TODO ：动力电上电 + } + else if (e.cmd == 4) + { + pm->m_subDisSysCmd.bowWaterlnet2 = 0; + } + else if (e.cmd == 5) + { + //TODO ：动力电断电 + } + else + { + LOG_F(WARNING, "DevCmdExt received bowWaterlnet2 - id: %d, cmd: %d is out of range", e.id, e.cmd); + } break; + } case 29: - LOG_F(INFO, "DevCmdExt received bowWaterlnet3 - id: %d, cmd: %d", e.id, e.cmd); + { + LOG_F(INFO, "DevCmdExt received bowWaterlnet2 - id: %d, cmd: %d", e.id, e.cmd); + #ifdef DEBUG + pm->m_currentDriverState.bowWaterlnet3 = e.cmd; + #endif + if (e.cmd == 0) + { + pm->m_subDisSysCmd.bowWaterlnet3 = 0; + //TODO ：动力电断电 + } + else if (e.cmd == 1) + { + pm->m_subDisSysCmd.bowWaterlnet3 = 1; + //TODO ：动力电上电 + } + else if (e.cmd == 2) + { + pm->m_subDisSysCmd.bowWaterlnet3 = 1; + } + else if (e.cmd == 3) + { + //TODO ：动力电上电 + } + else if (e.cmd == 4) + { + pm->m_subDisSysCmd.bowWaterlnet3 = 0; + } + else if (e.cmd == 5) + { + //TODO ：动力电断电 + } + else + { + LOG_F(WARNING, "DevCmdExt received bowWaterlnet3 - 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 bowWaterlnet4 - id: %d, cmd: %d", e.id, e.cmd); + #ifdef DEBUG + pm->m_currentDriverState.bowWaterlnet4 = e.cmd; + #endif + if (e.cmd == 0) + { + pm->m_subDisSysCmd.bowWaterlnet4 = 0; + //TODO ：动力电断电 + } + else if (e.cmd == 1) + { + pm->m_subDisSysCmd.bowWaterlnet4 = 1; + //TODO ：动力电上电 + } + else if (e.cmd == 2) + { + pm->m_subDisSysCmd.bowWaterlnet4 = 1; + } + else if (e.cmd == 3) + { + //TODO ：动力电上电 + } + else if (e.cmd == 4) + { + pm->m_subDisSysCmd.bowWaterlnet4 = 0; + } + else if (e.cmd == 5) + { + //TODO ：动力电断电 + } + else + { + LOG_F(WARNING, "DevCmdExt received bowWaterlnet4 - id: %d, cmd: %d is out of range", e.id, e.cmd); + } break; + } case 31: - LOG_F(INFO, "DevCmdExt received bowWaterlnet5 - id: %d, cmd: %d", e.id, e.cmd); + { + LOG_F(INFO, "DevCmdExt received bowWaterlnet4 - id: %d, cmd: %d", e.id, e.cmd); + #ifdef DEBUG + pm->m_currentDriverState.bowWaterlnet5 = e.cmd; + #endif + if (e.cmd == 0) + { + pm->m_subDisSysCmd.bowWaterlnet5 = 0; + //TODO ：动力电断电 + } + else if (e.cmd == 1) + { + pm->m_subDisSysCmd.bowWaterlnet5 = 1; + //TODO ：动力电上电 + } + else if (e.cmd == 2) + { + pm->m_subDisSysCmd.bowWaterlnet5 = 1; + } + else if (e.cmd == 3) + { + //TODO ：动力电上电 + } + else if (e.cmd == 4) + { + pm->m_subDisSysCmd.bowWaterlnet5 = 0; + } + else if (e.cmd == 5) + { + //TODO ：动力电断电 + } + else + { + LOG_F(WARNING, "DevCmdExt received bowWaterlnet5 - 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 bowWaterlnet6 - id: %d, cmd: %d", e.id, e.cmd); + #ifdef DEBUG + pm->m_currentDriverState.bowWaterlnet6 = e.cmd; + #endif + if (e.cmd == 0) + { + pm->m_subDisSysCmd.bowWaterlnet6 = 0; + //TODO ：动力电断电 + } + else if (e.cmd == 1) + { + pm->m_subDisSysCmd.bowWaterlnet6 = 1; + //TODO ：动力电上电 + } + else if (e.cmd == 2) + { + pm->m_subDisSysCmd.bowWaterlnet6 = 1; + } + else if (e.cmd == 3) + { + //TODO ：动力电上电 + } + else if (e.cmd == 4) + { + pm->m_subDisSysCmd.bowWaterlnet6 = 0; + } + else if (e.cmd == 5) + { + //TODO ：动力电断电 + } + else + { + LOG_F(WARNING, "DevCmdExt received bowWaterlnet6 - 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 bowWaterlnet7 - id: %d, cmd: %d", e.id, e.cmd); + #ifdef DEBUG + pm->m_currentDriverState.bowWaterlnet7 = e.cmd; + #endif + if (e.cmd == 0) + { + pm->m_subDisSysCmd.bowWaterlnet7 = 0; + //TODO ：动力电断电 + } + else if (e.cmd == 1) + { + pm->m_subDisSysCmd.bowWaterlnet7 = 1; + //TODO ：动力电上电 + } + else if (e.cmd == 2) + { + pm->m_subDisSysCmd.bowWaterlnet7 = 1; + } + else if (e.cmd == 3) + { + //TODO ：动力电上电 + } + else if (e.cmd == 4) + { + pm->m_subDisSysCmd.bowWaterlnet7 = 0; + } + else if (e.cmd == 5) + { + //TODO ：动力电断电 + } + else + { + LOG_F(WARNING, "DevCmdExt received bowWaterlnet7 - 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 bowWaterlnet8 - id: %d, cmd: %d", e.id, e.cmd); + #ifdef DEBUG + pm->m_currentDriverState.bowWaterlnet8 = e.cmd; + #endif + if (e.cmd == 0) + { + pm->m_subDisSysCmd.bowWaterlnet8 = 0; + //TODO ：动力电断电 + } + else if (e.cmd == 1) + { + pm->m_subDisSysCmd.bowWaterlnet8 = 1; + //TODO ：动力电上电 + } + else if (e.cmd == 2) + { + pm->m_subDisSysCmd.bowWaterlnet8 = 1; + } + else if (e.cmd == 3) + { + //TODO ：动力电上电 + } + else if (e.cmd == 4) + { + pm->m_subDisSysCmd.bowWaterlnet8 = 0; + } + else if (e.cmd == 5) + { + //TODO ：动力电断电 + } + else + { + LOG_F(WARNING, "DevCmdExt received bowWaterlnet8 - 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); + { + 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 lowFreqSonar - id: %d, cmd: %d", e.id, e.cmd); + { + 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 launchControler - id: %d, cmd: %d", e.id, e.cmd); + { + 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 photoSensor - id: %d, cmd: %d", e.id, e.cmd); + { + 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 electSensor - id: %d, cmd: %d", e.id, e.cmd); + { + 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); + { + LOG_F(INFO, "DevCmdExt received overDeep1 - 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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 bowRudderServo3 - id: %d, cmd: %d", e.id, e.cmd); + { + 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 bowRudderServo4 - id: %d, cmd: %d", e.id, e.cmd); + { + 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); + { + 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); + { + 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); + { + 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); + { + 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 ins - id: %d, cmd: %d", e.id, e.cmd); - break; + { + { + 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 ntp - id: %d, cmd: %d", e.id, e.cmd); + { + 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); + { + 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); + { + 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); + { + 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(FaultEnable::BasicNavigationFault_1) + { + LOG_F(INFO, "部分深度计失效"); + pm->faultCode.push_back(0x1102); + pm->faultCode.push_back(0x1103); + } + if(FaultEnable::BasicNavigationFault_2) + { + LOG_F(INFO, "DVL失效"); + pm->faultCode.push_back(0x1301); + } + if (FaultEnable::BasicNavigationFault_3) + { + LOG_F(INFO, "单个艉舵失效"); + pm->faultCode.push_back(0x1207); + } + if (FaultEnable::BasicNavigationFault_4) + { + LOG_F(INFO, "部分艉舵失效"); + pm->faultCode.push_back(0x1308); + pm->faultCode.push_back(0x1307); + } + if (FaultEnable::BasicNavigationFault_5) + { + LOG_F(INFO, "动力锂电池失效"); + pm->faultCode.push_back(0x130B); + } + if (FaultEnable::BasicNavigationFault_6) + { + LOG_F(INFO, "推进电机失效"); + pm->faultCode.push_back(0x130A); + } + if (FaultEnable::BasicNavigationFault_7) + { + LOG_F(INFO, "惯导失效"); + pm->faultCode.push_back(0x1305); + } + if (FaultEnable::BasicNavigationFault_8) + { + LOG_F(INFO, "深度失效"); + pm->faultCode.push_back(0x1302); + pm->faultCode.push_back(0x1303); + pm->faultCode.push_back(0x1304); + } + if (FaultEnable::BasicNavigationFault_9) + { + LOG_F(INFO, "仪表锂电池失效"); + pm->faultCode.push_back(0x1412); + } + if (FaultEnable::BasicNavigationFault_9) + { + LOG_F(INFO, "复合管控器失效"); + pm->faultCode.push_back(0x1413); + } +} +void PowerManagerFsm::extendedNavigationFault(FaultEvent const &e) +{ + LOG_F(INFO, "extendedNavigationFault"); + if (FaultEnable::ExtendedNavigationFault_1) + { + LOG_F(INFO, "艏舵展开电缸失效"); + pm->faultCode.push_back(0x2311); + } + if (FaultEnable::ExtendedNavigationFault_2) + { + LOG_F(INFO, "艏舵控制控制失效"); + pm->faultCode.push_back(0x2312); + } + if (FaultEnable::ExtendedNavigationFault_3) + { + LOG_F(INFO, "动力锂电池功率不满/失效"); + pm->faultCode.push_back(0x2323); + } + if (FaultEnable::ExtendedNavigationFault_4) + { + LOG_F(INFO, "燃料电池失效"); + pm->faultCode.push_back(0x2334); + } +} +void PowerManagerFsm::obstacleAvoidanceFault(FaultEvent const &e) +{ + LOG_F(INFO, "obstacleAvoidanceFault"); + if (FaultEnable::ObstacleAvoidanceFault_1) + { + pm->faultCode.push_back(0x3108); + pm->faultCode.push_back(0x3109); + } + if (FaultEnable::ObstacleAvoidanceFault_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 (FaultEnable::ObstacleAvoidanceFault_2) + { + pm->faultCode.push_back(0x3301); + } + +} +void PowerManagerFsm::buoyancyControlFault(FaultEvent const &e) +{ + LOG_F(INFO, "buoyancyControlFault"); + if (FaultEnable::BuoyancyControlFault_1) + { + LOG_F(INFO, "单一浮调机构失效"); + pm->faultCode.push_back(0x4101); + } + if (FaultEnable::BuoyancyControlFault_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) + { + LOG_F(INFO, "功率不足满足浮调要求"); + pm->faultCode.push_back(0x4336); + } + if (FaultEnable::BuoyancyControlFault_4) + { + LOG_F(INFO, "所有浮调系统失效"); + pm->faultCode.push_back(0x4301); + } - +} +void PowerManagerFsm::communicationSystemFault(FaultEvent const &e) +{ + LOG_F(INFO, "communicationSystemFault"); + if (FaultEnable::CommunicationSystemFault_1) + { + LOG_F(INFO, "部分水下通信失效"); + pm->faultCode.push_back(0x5203); + pm->faultCode.push_back(0x5204); + pm->faultCode.push_back(0x5205); + } + if (FaultEnable::CommunicationSystemFault_2) + { + LOG_F(INFO, "水下通信失效"); + pm->faultCode.push_back(0x5303); + pm->faultCode.push_back(0x5304); + pm->faultCode.push_back(0x5305); + } + if (FaultEnable::CommunicationSystemFault_2) + { + LOG_F(INFO, "水面通信失效"); + pm->faultCode.push_back(0x5301); + pm->faultCode.push_back(0x5302); + } +} +void PowerManagerFsm::payloadSystemFault(FaultEvent const &e) +{ + LOG_F(INFO, "payloadSystemFault"); + if (FaultEnable::PayloadSystemFault_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 (FaultEnable::PayloadSystemFault_2) + { + LOG_F(INFO, "舷侧探测失效"); + pm->faultCode.push_back(0x6321); + } + if (FaultEnable::PayloadSystemFault_3) + { + LOG_F(INFO, "舷侧主动探测失效"); + pm->faultCode.push_back(0x6323); + } + if (FaultEnable::PayloadSystemFault_4) + { + LOG_F(INFO, "TYZ失效"); + pm->faultCode.push_back(0x6322); + } + if (FaultEnable::PayloadSystemFault_5) + { + LOG_F(INFO, "光电探测失效"); + pm->faultCode.push_back(0x6332); + } + if (FaultEnable::PayloadSystemFault_6) + { + LOG_F(INFO, "电磁探测失效"); + pm->faultCode.push_back(0x6331); + } + if (FaultEnable::PayloadSystemFault_7) + { + LOG_F(INFO, "载荷失效或者全部载荷启闭机构均失效"); + pm->faultCode.push_back(0x633F); + } +} +void PowerManagerFsm::emergencySystemFault(FaultEvent const &e) +{ + LOG_F(INFO, "emergencySystemFault"); + if (FaultEnable::EmergencySystemFault_1) + { + //备注：抛载具有自己的电池 + LOG_F(INFO, "部分抛载失效"); + pm->faultCode.push_back(0x7102); + pm->faultCode.push_back(0x7103); + } + if (FaultEnable::EmergencySystemFault_2) + { + LOG_F(INFO, "一个超深检测装置失效"); + pm->faultCode.push_back(0x7204); + pm->faultCode.push_back(0x7205); + } + if (FaultEnable::EmergencySystemFault_3) + { + LOG_F(INFO, "注水机构失效"); + pm->faultCode.push_back(0x7301); + } + if (FaultEnable::EmergencySystemFault_4) + { + LOG_F(INFO, "抛载失效"); + pm->faultCode.push_back(0x7302); + pm->faultCode.push_back(0x7303); + } + if (FaultEnable::EmergencySystemFault_5) + { + LOG_F(INFO, "艏部应急电源失效"); + pm->faultCode.push_back(0x7306); + } + if (FaultEnable::EmergencySystemFault_6) + { + LOG_F(INFO, "艉部应急电源失效"); + pm->faultCode.push_back(0x7307); + } +} +void PowerManagerFsm::sensorSystemFault(FaultEvent const &e) +{ + LOG_F(INFO, "sensorSystemFault"); + if (FaultEnable::SensorSystemFault_1) + { + LOG_F(INFO, "部分CTD失效"); + pm->faultCode.push_back(0x8202); + } + if (FaultEnable::SensorSystemFault_2) + { + LOG_F(INFO, "CTD失效"); + pm->faultCode.push_back(0x8302); + pm->faultCode.push_back(0x8303); + } + if (FaultEnable::SensorSystemFault_2) + { + 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); +} // InitState实现 void InitState::entry() { PowerManagerFsm::entry(); @@ -212,14 +1991,7 @@ void InitState::entry() { transit(); } -// StandbyState实现 -void StandbyState::entry() { - PowerManagerFsm::entry(); - pm->m_driver.getSTANDBYTable(&pm->m_subDisSysCmd); - pm->m_CcuColCmd.fcuCmd = FcuCommand::Stop; - pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Stop; - pm->workCondition = WorkCondition::STANDBY; -} + void StandbyState::react(StandbyEvent const &) { transit(); @@ -240,20 +2012,19 @@ void StandbyState::handlePowerOn() { // FaultState实现 void FaultState::entry() { PowerManagerFsm::entry(); - PowerManagerFsm::pm->printState("FaultState", "进入故障状态"); + // PowerManagerFsm::pm->printState("FaultState", "进入故障状态"); + if(pm->m_missionState.type == TaskType::THROW_LOAD) + { + LOG_F(INFO, "检测到抛载，所有水声探测设备"); + } + else + { + // LOG_F(INFO, "检测到抛载，所有水声设备关闭"); + } } void FaultState::react(FaultEvent const & e) { - switch (e.faultCode) { - case 1: - PowerManagerFsm::pm->m_pmEvent = "复合管控连接超时"; - PowerManagerFsm::pm->printState("FaultState", "通信故障"); - break; - case 2: - PowerManagerFsm::pm->m_pmEvent = "能源系统故障"; - PowerManagerFsm::pm->printState("FaultState", "能源故障"); - break; - } + LOG_F(INFO, "FaultState 处理故障事件"); } @@ -388,12 +2159,83 @@ void ShoreBasedReady::react(MasterCommandEvent const &) { PowerManagerFsm::handleWorkCmd(); } +// StandbyState实现 +void StandbyState::entry() { + PowerManagerFsm::entry(); + pm->m_driver.getSTANDBYTable(&pm->m_subDisSysCmd); + pm->m_CcuColCmd.fcuCmd = FcuCommand::Stop; + pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Stop; + pm->workCondition = WorkCondition::STANDBY; + LOG_F(INFO, "WorkCondition %d", pm->workCondition); +} + void ShoreBasedReady::entry() { PowerManagerFsm::entry(); + pm->m_driver.getSHORE_BASED_READYable(&pm->m_subDisSysCmd); + pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Start; + pm->workCondition = WorkCondition::CHANING; + //依次接入执行机构动力电设备 + //TODO:接入执行机构动力电设备 pm->workCondition = WorkCondition::SHORE_BASED_READY; LOG_F(INFO, "进入岸基备航状态"); } +void WaterBasedReady::entry() { + PowerManagerFsm::entry(); + pm->workCondition = WorkCondition::CHANING; + pm->m_driver.getWATER_BASED_READYable(&pm->m_subDisSysCmd); + pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Start; + //依次接入执行机构动力电设备 + //TODO:接入执行机构动力电设备 + pm->workCondition = WorkCondition::WATER_BASED_READY; + LOG_F(INFO, "进入水中备航状态"); +} + +void CruiseMode::entry() { + PowerManagerFsm::entry(); + pm->workCondition = WorkCondition::CHANING; + pm->m_driver.getCRUISE_MODEable(&pm->m_subDisSysCmd); + //依次接入执行机构动力电设备 + //TODO:接入执行机构动力电设备 + + //TODO：解除非必要动力电设备 + pm->workCondition = WorkCondition::CRUISE_MODE; + LOG_F(INFO, "进入巡航模式"); +} + +void FloatAdjust::entry() { + PowerManagerFsm::entry(); + pm->workCondition = WorkCondition::CHANING; + //与巡航模式相同 + pm->m_driver.getCRUISE_MODEable(&pm->m_subDisSysCmd); + //依次接入执行机构动力电设备 + //TODO:接入执行机构动力电设备 + + //TODO：解除非必要动力电设备 + pm->workCondition = WorkCondition::BUOYANCY_ADJUST; + LOG_F(INFO, "进入浮调模式"); +} + +void FloatDown::entry() { + PowerManagerFsm::entry(); + //与巡航模式相同 + pm->m_driver.getCRUISE_MODEable(&pm->m_subDisSysCmd); + //依次接入执行机构动力电设备 + //TODO:接入执行机构动力电设备 + + //TODO：解除非必要动力电设备 + pm->workCondition = WorkCondition::ASCEND_DESCEND; + LOG_F(INFO, "进入上浮下潜模式"); +} + + +// FloatDown实现 +void FloatDown::react(MasterCommandEvent const &) { + PowerManagerFsm::handelPowerCmd(); + PowerManagerFsm::handleModCmd(); + PowerManagerFsm::handleWorkCmd(); +} + // WaterBasedReady实现 void WaterBasedReady::react(MasterCommandEvent const &) { PowerManagerFsm::handelPowerCmd(); @@ -401,11 +2243,7 @@ void WaterBasedReady::react(MasterCommandEvent const &) { PowerManagerFsm::handleWorkCmd(); } -void WaterBasedReady::entry() { - PowerManagerFsm::entry(); - pm->workCondition = WorkCondition::WATER_BASED_READY; - LOG_F(INFO, "进入水中备航状态"); -} + // CruiseMode实现 void CruiseMode::react(MasterCommandEvent const &) { @@ -414,11 +2252,7 @@ void CruiseMode::react(MasterCommandEvent const &) { PowerManagerFsm::handleWorkCmd(); } -void CruiseMode::entry() { - PowerManagerFsm::entry(); - pm->workCondition = WorkCondition::CRUISE_MODE; - LOG_F(INFO, "进入巡航模式"); -} + // HighSpeedMode实现 void HighSpeedMode::react(MasterCommandEvent const &) { @@ -433,19 +2267,6 @@ void HighSpeedMode::entry() { LOG_F(INFO, "进入高速模式"); } -// FloatDown实现 -void FloatDown::react(MasterCommandEvent const &) { - PowerManagerFsm::handelPowerCmd(); - PowerManagerFsm::handleModCmd(); - PowerManagerFsm::handleWorkCmd(); -} - -void FloatDown::entry() { - PowerManagerFsm::entry(); - pm->workCondition = WorkCondition::ASCEND_DESCEND; - LOG_F(INFO, "进入上浮下潜模式"); -} - // FloatAdjust实现 void FloatAdjust::react(MasterCommandEvent const &) { PowerManagerFsm::handelPowerCmd(); @@ -453,12 +2274,6 @@ void FloatAdjust::react(MasterCommandEvent const &) { PowerManagerFsm::handleWorkCmd(); } -void FloatAdjust::entry() { - PowerManagerFsm::entry(); - pm->workCondition = WorkCondition::BUOYANCY_ADJUST; - LOG_F(INFO, "进入浮调模式"); -} - // UnderwaterSurvey实现 void UnderwaterSurvey::react(MasterCommandEvent const &) { PowerManagerFsm::handelPowerCmd(); @@ -511,81 +2326,81 @@ void RemoteControl::entry() { LOG_F(INFO, "进入遥控工况"); } -void StandbyState::react(TaskStart const &) -{ - LOG_F(INFO, "接收到任务启动信息"); - //日志保存错误 - LOG_F(WARNING, "待机状态无法直接启动任务"); -} +// void StandbyState::react(TaskStart const &) +// { +// LOG_F(INFO, "接收到任务启动信息"); +// //日志保存错误 +// LOG_F(WARNING, "待机状态无法直接启动任务"); +// } //岸基备航 -void ShoreBasedReady::react(TaskStart const &) -{ - LOG_F(INFO, "接收到任务启动信息"); - //日志保存错误 - LOG_F(WARNING, "岸基备航状态无法直接启动任务"); -} -//水中备航 -void WaterBasedReady::react(TaskStart const &) -{ - LOG_F(INFO, "接收到任务启动信息"); - LOG_F(INFO, "开始执行启动任务"); -} +// void ShoreBasedReady::react(TaskStart const &) +// { +// LOG_F(INFO, "接收到任务启动信息"); +// //日志保存错误 +// LOG_F(WARNING, "岸基备航状态无法直接启动任务"); +// } +// //水中备航 +// void WaterBasedReady::react(TaskStart const &) +// { +// LOG_F(INFO, "接收到任务启动信息"); +// LOG_F(INFO, "开始执行启动任务"); +// } -//遥控工况 -void RemoteControl::react(TaskStart const &) -{ - LOG_F(INFO, "接收到任务启动信息"); - LOG_F(INFO, "开始执行启动任务"); -} +// //遥控工况 +// void RemoteControl::react(TaskStart const &) +// { +// LOG_F(INFO, "接收到任务启动信息"); +// LOG_F(INFO, "开始执行启动任务"); +// } -//巡航模式 -void CruiseMode::react(TaskStart const &) -{ - LOG_F(INFO, "接收到任务启动信息"); - LOG_F(INFO, "进入巡航模式"); -} +// //巡航模式 +// void CruiseMode::react(TaskStart const &) +// { +// LOG_F(INFO, "接收到任务启动信息"); +// LOG_F(INFO, "进入巡航模式"); +// } -//高速模式 -void HighSpeedMode::react(TaskStart const &) -{ - LOG_F(INFO, "接收到任务启动信息"); - LOG_F(INFO, "进入高速模式"); -} +// //高速模式 +// void HighSpeedMode::react(TaskStart const &) +// { +// LOG_F(INFO, "接收到任务启动信息"); +// LOG_F(INFO, "进入高速模式"); +// } -//浮调模式 -void FloatAdjust::react(TaskStart const &) -{ - LOG_F(INFO, "接收到任务启动信息"); - LOG_F(INFO, "进入浮调模式"); -} +// //浮调模式 +// void FloatAdjust::react(TaskStart const &) +// { +// LOG_F(INFO, "接收到任务启动信息"); +// LOG_F(INFO, "进入浮调模式"); +// } -//水下侦查 -void UnderwaterSurvey::react(TaskStart const &) -{ - LOG_F(INFO, "接收到任务启动信息"); - LOG_F(INFO, "进入水下侦查模式"); -} +// //水下侦查 +// void UnderwaterSurvey::react(TaskStart const &) +// { +// LOG_F(INFO, "接收到任务启动信息"); +// LOG_F(INFO, "进入水下侦查模式"); +// } -//水面侦查 -void SurfaceSurvey::react(TaskStart const &) -{ - LOG_F(INFO, "接收到任务启动信息"); - LOG_F(INFO, "进入水面侦查模式"); -} +// //水面侦查 +// void SurfaceSurvey::react(TaskStart const &) +// { +// LOG_F(INFO, "接收到任务启动信息"); +// LOG_F(INFO, "进入水面侦查模式"); +// } -//DJ模式 -void DJMode::react(TaskStart const &) -{ - LOG_F(INFO, "接收到任务启动信息"); - LOG_F(INFO, "进入DJ模式"); -} +// //DJ模式 +// void DJMode::react(TaskStart const &) +// { +// LOG_F(INFO, "接收到任务启动信息"); +// LOG_F(INFO, "进入DJ模式"); +// } -// 上浮下潜 -void FloatDown::react(TaskStart const &) -{ - LOG_F(INFO, "接收到任务启动信息"); - LOG_F(INFO, "进入上浮下潜模式"); -} +// // 上浮下潜 +// void FloatDown::react(TaskStart const &) +// { +// LOG_F(INFO, "接收到任务启动信息"); +// LOG_F(INFO, "进入上浮下潜模式"); +// } FSM_INITIAL_STATE(PowerManagerFsm, InitState); \ No newline at end of file diff --git a/src/pPowerManger/PowerManagerFsm.hpp b/src/pPowerManger/PowerManagerFsm.hpp index bbbc747..aa3d06c 100644 --- a/src/pPowerManger/PowerManagerFsm.hpp +++ b/src/pPowerManger/PowerManagerFsm.hpp @@ -9,6 +9,7 @@ #include // 新增头文件，用于demangle功能 #include "pmSysvariable.h" #include "driver.h" +#include "PowerManger.h" // 前向声明 class PowerManger; @@ -21,13 +22,17 @@ enum class PowerSystemState { //==================状态切换事件============================= struct InitEvent : tinyfsm::Event { }; struct StandbyEvent : tinyfsm::Event { }; -struct FaultEvent : tinyfsm::Event { - unsigned int faultCode; - FaultEvent(unsigned int code) : faultCode(code) {} -}; +struct TaskStopEvent : tinyfsm::Event { }; + //==================状态流程事件============================ struct MasterCommandEvent : tinyfsm::Event {}; -struct TaskStart : tinyfsm::Event { }; +struct TaskStartEvent : tinyfsm::Event { + int type; + bool enableBowRudder; + bool enableBuoyancyAdjustment; + TaskStartEvent() : type(0), enableBowRudder(false), enableBuoyancyAdjustment(false) {}; + TaskStartEvent(int type, bool bowRudder, bool buoyancyAdjustment) : type(type), enableBowRudder(bowRudder), enableBuoyancyAdjustment(buoyancyAdjustment) {}; +}; struct DeviceCmd : tinyfsm::Event { }; struct DevCmdExt : tinyfsm::Event { int id; @@ -35,7 +40,24 @@ struct DevCmdExt : tinyfsm::Event { DevCmdExt() : id(0), cmd(0) {}; DevCmdExt(int id, int cmd) : id(id), cmd(cmd) {}; }; - +struct FaultEvent : tinyfsm::Event { + unsigned int subDisSysFaultCode; + unsigned int disSysFaultCode; + unsigned int powerFaultCode; + unsigned int fcsFaultCode; + unsigned int ccsFaultCode; + FaultEvent() : subDisSysFaultCode(0), disSysFaultCode(0), powerFaultCode(0), fcsFaultCode(0), ccsFaultCode(0) {}; + FaultEvent(unsigned int subDisSysFaultCode, + unsigned int disSysFaultCode, + unsigned int powerFaultCode, + unsigned int fcsFaultCode, + unsigned int ccsFaultCode) + : subDisSysFaultCode(subDisSysFaultCode), + disSysFaultCode(disSysFaultCode), + powerFaultCode(powerFaultCode), + fcsFaultCode(fcsFaultCode), + ccsFaultCode(ccsFaultCode) {}; +}; class PowerManagerFsm : public tinyfsm::Fsm { protected: @@ -46,21 +68,31 @@ public: static void init(PowerManger* powerManager) { pm = powerManager; } - // 响应事件默认实现 + virtual void react(tinyfsm::Event const &) { std::cout << "[ Reseve Envt ] " << "No Idear" << std::endl; } virtual void react(InitEvent const &) {}; virtual void react(StandbyEvent const &) { }; - virtual void react(FaultEvent const &) { }; virtual void react(MasterCommandEvent const &) { LOG_F(INFO, "MasterCommandEvent");}; - virtual void react(TaskStart const &) { LOG_F(INFO, "TaskStart"); }; virtual void react(DeviceCmd const &) { LOG_F(INFO, "DeviceCmd"); }; virtual void react(DevCmdExt const &e); + virtual void react(TaskStartEvent const &e); + virtual void react(FaultEvent const &e); + virtual void react(TaskStopEvent const &e); //操作函数 void handelPowerCmd(); void handleModCmd(); void handleWorkCmd(); void handleDeviceCmd(); + //故障处理 + void basicNavigationFault(FaultEvent const &e); + void extendedNavigationFault(FaultEvent const &e); + void obstacleAvoidanceFault(FaultEvent const &e); + void buoyancyControlFault(FaultEvent const &e); + void communicationSystemFault(FaultEvent const &e); + void payloadSystemFault(FaultEvent const &e); + void emergencySystemFault(FaultEvent const &e); + void sensorSystemFault(FaultEvent const &e); // 状态机生命周期方法 virtual void entry() { @@ -110,8 +142,6 @@ public: void entry() override; void react(StandbyEvent const &) override; void react(MasterCommandEvent const &) override; - void react(TaskStart const &) override; - // void react(DeviceCmd const &) override; private: void handlePowerOn(); }; @@ -121,8 +151,6 @@ class ShoreBasedReady : public PowerManagerFsm { public: void entry() override; void react(MasterCommandEvent const &) override; - void react(TaskStart const &) override; - // void react(DeviceCmd const &) override; private: // void handlePowerCommands(); // void handlePowerOn(); @@ -133,7 +161,7 @@ class WaterBasedReady : public PowerManagerFsm { void entry() override; void react(MasterCommandEvent const &) override; // void react(DeviceCmd const &) override; - void react(TaskStart const &) override; + // void react(TaskStart const &) override; private: // void handlePowerCommands(); // void handlePowerOn(); @@ -144,7 +172,7 @@ class CruiseMode : public PowerManagerFsm { void entry() override; void react(MasterCommandEvent const &) override; // void react(DeviceCmd const &) override; - void react(TaskStart const &) override; + // void react(TaskStart const &) override; private: // void handlePowerCommands(); }; @@ -155,7 +183,7 @@ class HighSpeedMode : public PowerManagerFsm { void entry() override; void react(MasterCommandEvent const &) override; // void react(DeviceCmd const &) override; - void react(TaskStart const &) override; + // void react(TaskStart const &) override; private: // void handlePowerCommands(); }; @@ -165,7 +193,7 @@ class FloatDown : public PowerManagerFsm { void entry() override; void react(MasterCommandEvent const &) override; // void react(DeviceCmd const &) override; - void react(TaskStart const &) override; + // void react(TaskStart const &) override; private: // void handlePowerCommands(); }; @@ -175,7 +203,7 @@ class FloatAdjust : public PowerManagerFsm { void entry() override; void react(MasterCommandEvent const &) override; // void react(DeviceCmd const &) override; - void react(TaskStart const &) override; + // void react(TaskStart const &) override; private: // void handlePowerCommands(); }; @@ -185,7 +213,7 @@ class UnderwaterSurvey : public PowerManagerFsm { void entry() override; void react(MasterCommandEvent const &) override; // void react(DeviceCmd const &) override; - void react(TaskStart const &) override; + // void react(TaskStart const &) override; private: // void handlePowerCommands(); @@ -196,7 +224,7 @@ class SurfaceSurvey : public PowerManagerFsm { void entry() override; void react(MasterCommandEvent const &) override; // void react(DeviceCmd const &) override; - void react(TaskStart const &) override; + // void react(TaskStart const &) override; private: // void handlePowerCommands(); @@ -207,7 +235,7 @@ class DJMode : public PowerManagerFsm { void entry() override; void react(MasterCommandEvent const &) override; // void react(DeviceCmd const &) override; - void react(TaskStart const &) override; + // void react(TaskStart const &) override; private: // void handlePowerCommands(); @@ -218,7 +246,7 @@ class RemoteControl : public PowerManagerFsm { void entry() override; void react(MasterCommandEvent const &) override; // void react(DeviceCmd const &) override; - void react(TaskStart const &) override; + // void react(TaskStart const &) override; private: // void handlePowerCommands(); }; diff --git a/src/pPowerManger/PowerManger.cpp b/src/pPowerManger/PowerManger.cpp index b587c58..2bdd2e4 100644 --- a/src/pPowerManger/PowerManger.cpp +++ b/src/pPowerManger/PowerManger.cpp @@ -29,7 +29,7 @@ PowerManger::PowerManger() workCondition = 0; currentMod = 0; powerState = 0; - falutLevel = 0; + faultLevel = 0; soc = 0; sustainableTime = 0; m_faultNum = 0; @@ -125,6 +125,7 @@ bool PowerManger::Iterate() AppCastingMOOSApp::Iterate(); // 打印当前状态 // cout << " Current State : " << (int)PowerManagerFsm::current_state_ptr->getCurrentState(); + faultLevel = getFaultLevel(); if (m_ticker++ % 5 == 0) { m_CcuColCmd.heartbeat++; @@ -196,6 +197,11 @@ void PowerManger::registerVariables() Register(DB_TO_PM_SUBDISSTATE, 0); Register(DB_TO_PM_MISSION,0); Register(DB_TO_PM_DEEPTH,0); + Register(DB_TO_PM_SETOPCONDITION,0); + Register(DB_TO_PM_SETOPMODE,0); + Register(DB_TO_PM_SETPMSYS,0); + Register(DB_TO_PM_SETDEVSWITCH,0); + Register(DB_TO_PM_TASKSTOP,0); } @@ -277,14 +283,6 @@ void PowerManger::printState(const std::string& stateName, const std::string& de } -//==============================系统操作函数==================== -bool PowerManger::shutdown() -{ - m_CcuColCmd.masterState = 0xFF; //关机 - return true; -} - - void PowerManger::initControlCmd() { //================初始化配电指令============================= @@ -340,4 +338,17 @@ void PowerManger::initControlCmd() m_msCmd.workCMD = 99; //================初始化设备驱动指令======================== -} \ No newline at end of file +} + +unsigned char PowerManger::getFaultLevel() +{ + unsigned char maxLevel = 0; + for (auto code : faultCode) { + // 提取0xabcd中的b位 (右移8位后取低4位) + unsigned char level = (code >> 8) & 0xF; + if (level > maxLevel) { + maxLevel = level; + } + } + return maxLevel; +} \ No newline at end of file diff --git a/src/pPowerManger/PowerManger.h b/src/pPowerManger/PowerManger.h index 0d3190c..df730ec 100644 --- a/src/pPowerManger/PowerManger.h +++ b/src/pPowerManger/PowerManger.h @@ -62,6 +62,7 @@ class PowerManger : public AppCastingMOOSApp public: void updateTime(){m_current_time = MOOSTime();} + unsigned char getFaultLevel(); //数据库 // SQLiteDB* m_db; bool writeDataToDB(); @@ -85,14 +86,13 @@ class PowerManger : public AppCastingMOOSApp disSysBreakerList m_currentDisBreakerState; //配电断路器状态 DriverTable m_currentDriverState; DriverTable m_subDisSysState; - int m_depth; MissionState m_missionState; //===========================STATE========================================= unsigned char workCondition; unsigned char currentMod; unsigned char powerState; - unsigned char falutLevel; + unsigned char faultLevel; vector faultCode; unsigned int soc; unsigned int sustainableTime; @@ -125,7 +125,6 @@ class PowerManger : public AppCastingMOOSApp //================================================================================== int checkError(); - bool shutdown(); bool sendNotification(const std::string& key, const std::string& value) { return Notify(key, value); diff --git a/src/pPowerManger/UpperCommManager.cpp b/src/pPowerManger/UpperCommManager.cpp index 7d28acc..4c5f7fa 100644 --- a/src/pPowerManger/UpperCommManager.cpp +++ b/src/pPowerManger/UpperCommManager.cpp @@ -27,7 +27,11 @@ bool UpperCommManager::processUpperMsg(const string& key, const string& strJson) } else if(key == DB_TO_PM_MISSION) { m_pm->m_missionState = getMission(key, strJson); - //TODO：转移到对应工况下 + TaskStartEvent event; + event.type = m_pm->m_missionState.type; + event.enableBowRudder = m_pm->m_missionState.enableBowRudder; + event.enableBuoyancyAdjustment = m_pm->m_missionState.enableBuoyancyAdjustment; + PowerManagerFsm::dispatch(event); return true; } else if(key == DB_TO_PM_SETOPCONDITION) { m_pm->m_msCmd.workCMD = getSetOpCondition(key, strJson); @@ -58,6 +62,16 @@ bool UpperCommManager::processUpperMsg(const string& key, const string& strJson) } LOG_F(INFO, "set dev switch success"); return true; + } else if(key == DB_TO_PM_TASKSTOP) { + bool stop = getTaskStop(key, strJson); + if (stop) { + LOG_F(INFO, "task stop success"); + return true; + } + else { + LOG_F(WARNING, "task stop error:%s", strJson.c_str()); + return false; + } } LOG_F(WARNING, "Unknown key: %s", key.c_str()); return false; @@ -178,8 +192,8 @@ int UpperCommManager::getSetOpCondition(const std::string& key, const std::strin // 检查并读取操作条件 if (root.isMember("state_uint8") && root["state_uint8"].isUInt()) { - return root["opCondition"].asUInt(); - LOG_F(INFO, "上位机指令:opCondition: %d", root["opCondition"].asUInt()); + return root["state_uint8"].asUInt(); + LOG_F(INFO, "上位机指令:opCondition: %d", root["state_uint8"].asUInt()); } else { LOG_F(WARNING, "缺少或类型不匹配: opCondition"); return -1; @@ -256,9 +270,28 @@ DeviceSwitchCmd UpperCommManager::getSetDevSwitch(const std::string& key, const LOG_F(WARNING, "缺少或类型不匹配: state_bool"); } return cmd; -} - -const std::string UpperCommManager::getCurrentEquState(DriverTable& driver) + } + + bool UpperCommManager::getTaskStop(const std::string& key, const std::string& msg) { + Json::Value root; + Json::CharReaderBuilder builder; + JSONCPP_STRING errs; + + // 解析JSON字符串 + std::istringstream stream(msg); + if (!Json::parseFromStream(builder, stream, &root, &errs)) { + LOG_F(WARNING, "JSON解析失败: %s", errs.c_str()); + return false; + } + + // 检查op_int32字段 + if (root.isMember("op_int32") && root["op_int32"].isInt()) { + return root["op_int32"].asInt() == 1; + } + return false; + } + + const std::string UpperCommManager::getCurrentEquState(DriverTable& driver) { return m_pm->m_driver.saveDriverTableToJson(&driver); } @@ -337,8 +370,8 @@ void UpperCommManager::buildSystemReport() { suffix = getTypeName(typeid(m_pm->powerState).name()); pms[VAR_TO_STR(powerState)+suffix] = m_pm->powerState; - suffix = getTypeName(typeid(m_pm->falutLevel).name()); - pms[VAR_TO_STR(falutLevel)+suffix] = m_pm->falutLevel; + suffix = getTypeName(typeid(m_pm->faultLevel).name()); + pms[VAR_TO_STR(faultLevel)+suffix] = m_pm->faultLevel; Json::Value faultCode_array; for(auto val : m_pm->faultCode) @@ -348,10 +381,10 @@ void UpperCommManager::buildSystemReport() { pms[VAR_TO_STR(faultCode_array)] = faultCode_array; suffix = getTypeName(typeid(m_pm->soc).name()); - pms[VAR_TO_STR(soc)+suffix] = m_pm->soc; + pms["soc_uint16"] = m_pm->soc; suffix = getTypeName(typeid(m_pm->sustainableTime).name()); - pms[VAR_TO_STR(sustainableTime)+suffix] = m_pm->sustainableTime; + pms["sustainableTime_int64"] = m_pm->sustainableTime; //2.构建燃料电池状态报告 suffix = getTypeName(typeid(m_pm->m_pmCurrentState.ccustate.fcuState).name()); diff --git a/src/pPowerManger/UpperCommManager.h b/src/pPowerManger/UpperCommManager.h index 28eecd8..3cc774c 100644 --- a/src/pPowerManger/UpperCommManager.h +++ b/src/pPowerManger/UpperCommManager.h @@ -27,7 +27,8 @@ using namespace std; //决策消息 #define DB_TO_PM_DEEPTH "uDevice_depth_st" -#define DB_TO_PM_MISSION "uPlan_taskStart_cmd" +#define DB_TO_PM_MISSION "uPlan_taskReport_st" +#define DB_TO_PM_TASKSTOP "uPlan_planStop_cmd" //外部通信消息 #define DB_TO_PM_SETOPCONDITION "uExternComm_setOpCondition_cmd" @@ -57,6 +58,7 @@ public: const DriverTable getSubDisState(const std::string& key, const std::string& msg); const int getDeepth(const std::string& key, const std::string& msg); MissionState getMission(const std::string& key, const std::string& msg); + bool getTaskStop(const std::string& key, const std::string& msg); // 处理外部通信消息 int getSetOpCondition(const std::string& key, const std::string& msg); int getSetOpMode(const std::string& key, const std::string& msg); diff --git a/src/pPowerManger/driver.cpp b/src/pPowerManger/driver.cpp index 40198e1..5bfddec 100644 --- a/src/pPowerManger/driver.cpp +++ b/src/pPowerManger/driver.cpp @@ -93,209 +93,208 @@ void Driver::getSTANDBYTable(DriverTable* driverTable) void Driver::getSHORE_BASED_READYable(DriverTable* driverTable) { // 打开所有传感器 - driverTable->depth1 = DRIVER_FULL_POWER_ON; - driverTable->depth2 = DRIVER_FULL_POWER_ON; - driverTable->depth3 = DRIVER_FULL_POWER_ON; - driverTable->overDeep1 = DRIVER_FULL_POWER_ON; - driverTable->overDeep2 = DRIVER_FULL_POWER_ON; - driverTable->ctd1 = DRIVER_FULL_POWER_ON; - driverTable->ctd2 = DRIVER_FULL_POWER_ON; - driverTable->soundVeloc = DRIVER_FULL_POWER_ON; - driverTable->commMast = DRIVER_FULL_POWER_ON; - driverTable->ins = DRIVER_FULL_POWER_ON; - driverTable->ntp = DRIVER_FULL_POWER_ON; - driverTable->inputWaterSensor1 = DRIVER_FULL_POWER_ON; - driverTable->inputWaterSensor2 = DRIVER_FULL_POWER_ON; - driverTable->inputWaterSensor3 = DRIVER_FULL_POWER_ON; + driverTable->depth1 = 1; + driverTable->depth2 = 1; + driverTable->depth3 = 1; + driverTable->overDeep1 = 1; + driverTable->overDeep2 = 1; + driverTable->ctd1 = 1; + driverTable->ctd2 = 1; + driverTable->soundVeloc = 1; + driverTable->commMast = 1; + driverTable->ins = 1; + driverTable->ntp = 1; + driverTable->inputWaterSensor1 = 1; + driverTable->inputWaterSensor2 = 1; + driverTable->inputWaterSensor3 = 1; // 允许启动负载 // 关闭xcz - driverTable->photoSensor = DRIVER_STANDBY; - driverTable->electSensor = DRIVER_STANDBY; - driverTable->launchControler = DRIVER_STANDBY; - driverTable->sideSonar = DRIVER_POWER_OFF; - driverTable->buoyage = DRIVER_STANDBY; + driverTable->photoSensor = 0; + driverTable->electSensor = 0; + driverTable->launchControler = 0; + driverTable->sideSonar = 0; + driverTable->buoyage = 0; // 关闭所有水声设备 - driverTable->singleBeamSonar1 = DRIVER_POWER_OFF; - driverTable->singleBeamSonar2 = DRIVER_POWER_OFF; - driverTable->singleBeamSonar3 = DRIVER_POWER_OFF; - driverTable->singleBeamSonar4 = DRIVER_POWER_OFF; - driverTable->singleBeamSonar5 = DRIVER_POWER_OFF; - driverTable->singleBeamSonar6 = DRIVER_POWER_OFF; - driverTable->singleBeamSonar7 = DRIVER_POWER_OFF; - driverTable->singleBeamSonar8 = DRIVER_POWER_OFF; - driverTable->DVL = DRIVER_POWER_OFF; - driverTable->forwardSonar = DRIVER_POWER_OFF; - driverTable->lowFreqSonar = DRIVER_POWER_OFF; - driverTable->USBL = DRIVER_POWER_OFF; + driverTable->singleBeamSonar1 = 0; + driverTable->singleBeamSonar2 = 0; + driverTable->singleBeamSonar3 = 0; + driverTable->singleBeamSonar4 = 0; + driverTable->singleBeamSonar5 = 0; + driverTable->singleBeamSonar6 = 0; + driverTable->singleBeamSonar7 = 0; + driverTable->singleBeamSonar8 = 0; + driverTable->DVL = 0; + driverTable->forwardSonar = 0; + driverTable->lowFreqSonar = 0; + driverTable->USBL = 0; // 打开必要的执行机构 - driverTable->bowCover1 = DRIVER_FULL_POWER_ON; - driverTable->bowCover2 = DRIVER_FULL_POWER_ON; - driverTable->bowCover3 = DRIVER_FULL_POWER_ON; - driverTable->bowCover4 = DRIVER_FULL_POWER_ON; - driverTable->bowCover5 = DRIVER_FULL_POWER_ON; - driverTable->bowCover6 = DRIVER_FULL_POWER_ON; - driverTable->bowRudderDeploy = DRIVER_FULL_POWER_ON; - driverTable->bowRudderServo1 = DRIVER_FULL_POWER_ON; - driverTable->bowRudderServo2 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet1 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet2 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet3 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet4 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet5 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet6 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet7 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet8 = DRIVER_FULL_POWER_ON; - driverTable->waterInject = DRIVER_FULL_POWER_ON; - driverTable->mastLiftingServo = DRIVER_FULL_POWER_ON; - driverTable->buoyancyAdjust1 = DRIVER_FULL_POWER_ON; - driverTable->buoyancyAdjust2 = DRIVER_FULL_POWER_ON; - driverTable->buoyancyAdjust3 = DRIVER_FULL_POWER_ON; - driverTable->buoyancyAdjust4 = DRIVER_FULL_POWER_ON; - driverTable->buoyancyAdjust5 = DRIVER_FULL_POWER_ON; - driverTable->sternRudderServo1 = DRIVER_FULL_POWER_ON; - driverTable->sternRudderServo2 = DRIVER_FULL_POWER_ON; - driverTable->sternRudderServo3 = DRIVER_FULL_POWER_ON; - driverTable->sternRudderServo4 = DRIVER_FULL_POWER_ON; - driverTable->thruster = DRIVER_FULL_POWER_ON; - driverTable->bowThrow = DRIVER_FULL_POWER_ON; - driverTable->sternThrow = DRIVER_FULL_POWER_ON; + driverTable->bowCover1 = 1; + driverTable->bowCover2 = 1; + driverTable->bowCover3 = 1; + driverTable->bowCover4 = 1; + driverTable->bowCover5 = 1; + driverTable->bowCover6 = 1; + driverTable->bowRudderDeploy = 1; + driverTable->bowRudderServo1 = 1; + driverTable->bowRudderServo2 = 1; + driverTable->bowWaterlnet1 = 1; + driverTable->bowWaterlnet2 = 1; + driverTable->bowWaterlnet3 = 1; + driverTable->bowWaterlnet4 = 1; + driverTable->bowWaterlnet5 = 1; + driverTable->bowWaterlnet6 = 1; + driverTable->bowWaterlnet7 = 1; + driverTable->bowWaterlnet8 = 1; + driverTable->waterInject = 1; + driverTable->mastLiftingServo = 1; + driverTable->buoyancyAdjust1 = 1; + driverTable->buoyancyAdjust2 = 1; + driverTable->buoyancyAdjust3 = 1; + driverTable->buoyancyAdjust4 = 1; + driverTable->buoyancyAdjust5 = 1; + driverTable->sternRudderServo1 = 1; + driverTable->sternRudderServo2 = 1; + driverTable->sternRudderServo3 = 1; + driverTable->sternRudderServo4 = 1; + driverTable->thruster = 1; + driverTable->bowThrow = 1; + driverTable->sternThrow = 1; } void Driver::getWATER_BASED_READYable(DriverTable* driverTable) { // 打开所有传感器 - driverTable->depth1 = DRIVER_FULL_POWER_ON; - driverTable->depth2 = DRIVER_FULL_POWER_ON; - driverTable->depth3 = DRIVER_FULL_POWER_ON; - driverTable->overDeep1 = DRIVER_FULL_POWER_ON; - driverTable->overDeep2 = DRIVER_FULL_POWER_ON; - driverTable->ctd1 = DRIVER_FULL_POWER_ON; - driverTable->ctd2 = DRIVER_FULL_POWER_ON; - driverTable->soundVeloc = DRIVER_FULL_POWER_ON; - driverTable->commMast = DRIVER_FULL_POWER_ON; - driverTable->ins = DRIVER_FULL_POWER_ON; - driverTable->ntp = DRIVER_FULL_POWER_ON; - driverTable->inputWaterSensor1 = DRIVER_FULL_POWER_ON; - driverTable->inputWaterSensor2 = DRIVER_FULL_POWER_ON; - driverTable->inputWaterSensor3 = DRIVER_FULL_POWER_ON; + driverTable->depth1 = 1; + driverTable->depth2 = 1; + driverTable->depth3 = 1; + driverTable->overDeep1 = 1; + driverTable->overDeep2 = 1; + driverTable->ctd1 = 1; + driverTable->ctd2 = 1; + driverTable->soundVeloc = 1; + driverTable->commMast = 1; + driverTable->ins = 1; + driverTable->ntp = 1; + driverTable->inputWaterSensor1 = 1; + driverTable->inputWaterSensor2 = 1; + driverTable->inputWaterSensor3 = 1; // 允许启动负载 - driverTable->photoSensor = DRIVER_STANDBY; - driverTable->electSensor = DRIVER_STANDBY; - driverTable->launchControler = DRIVER_STANDBY; - driverTable->sideSonar = DRIVER_STANDBY; - driverTable->buoyage = DRIVER_STANDBY; + driverTable->photoSensor = 0; + driverTable->electSensor = 0; + driverTable->launchControler = 0; + driverTable->sideSonar = 0; + driverTable->buoyage = 0; // 所有水声设备待机 - // 打开DVL - driverTable->singleBeamSonar1 = DRIVER_STANDBY; - driverTable->singleBeamSonar2 = DRIVER_STANDBY; - driverTable->singleBeamSonar3 = DRIVER_STANDBY; - driverTable->singleBeamSonar4 = DRIVER_STANDBY; - driverTable->singleBeamSonar5 = DRIVER_STANDBY; - driverTable->singleBeamSonar6 = DRIVER_STANDBY; - driverTable->singleBeamSonar7 = DRIVER_STANDBY; - driverTable->singleBeamSonar8 = DRIVER_STANDBY; - driverTable->DVL = DRIVER_FULL_POWER_ON; - driverTable->forwardSonar = DRIVER_STANDBY; - driverTable->lowFreqSonar = DRIVER_STANDBY; - driverTable->USBL = DRIVER_STANDBY; + driverTable->singleBeamSonar1 = 0; + driverTable->singleBeamSonar2 = 0; + driverTable->singleBeamSonar3 = 0; + driverTable->singleBeamSonar4 = 0; + driverTable->singleBeamSonar5 = 0; + driverTable->singleBeamSonar6 = 0; + driverTable->singleBeamSonar7 = 0; + driverTable->singleBeamSonar8 = 0; + driverTable->DVL = 0; + driverTable->forwardSonar = 0; + driverTable->lowFreqSonar = 0; + driverTable->USBL = 0; // 打开必要的执行机构 - driverTable->bowCover1 = DRIVER_FULL_POWER_ON; - driverTable->bowCover2 = DRIVER_FULL_POWER_ON; - driverTable->bowCover3 = DRIVER_FULL_POWER_ON; - driverTable->bowCover4 = DRIVER_FULL_POWER_ON; - driverTable->bowCover5 = DRIVER_FULL_POWER_ON; - driverTable->bowCover6 = DRIVER_FULL_POWER_ON; - driverTable->bowRudderDeploy = DRIVER_FULL_POWER_ON; - driverTable->bowRudderServo1 = DRIVER_FULL_POWER_ON; - driverTable->bowRudderServo2 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet1 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet2 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet3 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet4 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet5 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet6 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet7 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet8 = DRIVER_FULL_POWER_ON; - driverTable->waterInject = DRIVER_FULL_POWER_ON; - driverTable->mastLiftingServo = DRIVER_FULL_POWER_ON; - driverTable->buoyancyAdjust1 = DRIVER_FULL_POWER_ON; - driverTable->buoyancyAdjust2 = DRIVER_FULL_POWER_ON; - driverTable->buoyancyAdjust3 = DRIVER_FULL_POWER_ON; - driverTable->buoyancyAdjust4 = DRIVER_FULL_POWER_ON; - driverTable->buoyancyAdjust5 = DRIVER_FULL_POWER_ON; - driverTable->sternRudderServo1 = DRIVER_FULL_POWER_ON; - driverTable->sternRudderServo2 = DRIVER_FULL_POWER_ON; - driverTable->sternRudderServo3 = DRIVER_FULL_POWER_ON; - driverTable->sternRudderServo4 = DRIVER_FULL_POWER_ON; - driverTable->thruster = DRIVER_FULL_POWER_ON; - driverTable->bowThrow = DRIVER_FULL_POWER_ON; - driverTable->sternThrow = DRIVER_FULL_POWER_ON; + driverTable->bowCover1 = 1; + driverTable->bowCover2 = 1; + driverTable->bowCover3 = 1; + driverTable->bowCover4 = 1; + driverTable->bowCover5 = 1; + driverTable->bowCover6 = 1; + driverTable->bowRudderDeploy = 1; + driverTable->bowRudderServo1 = 1; + driverTable->bowRudderServo2 = 1; + driverTable->bowWaterlnet1 = 1; + driverTable->bowWaterlnet2 = 1; + driverTable->bowWaterlnet3 = 1; + driverTable->bowWaterlnet4 = 1; + driverTable->bowWaterlnet5 = 1; + driverTable->bowWaterlnet6 = 1; + driverTable->bowWaterlnet7 = 1; + driverTable->bowWaterlnet8 = 1; + driverTable->waterInject = 1; + driverTable->mastLiftingServo = 1; + driverTable->buoyancyAdjust1 = 1; + driverTable->buoyancyAdjust2 = 1; + driverTable->buoyancyAdjust3 = 1; + driverTable->buoyancyAdjust4 = 1; + driverTable->buoyancyAdjust5 = 1; + driverTable->sternRudderServo1 = 1; + driverTable->sternRudderServo2 = 1; + driverTable->sternRudderServo3 = 1; + driverTable->sternRudderServo4 = 1; + driverTable->thruster = 1; + driverTable->bowThrow = 1; + driverTable->sternThrow = 1; } void Driver::getCRUISE_MODEable(DriverTable* driverTable) { // 打开所有传感器 - driverTable->depth1 = DRIVER_FULL_POWER_ON; - driverTable->depth2 = DRIVER_FULL_POWER_ON; - driverTable->depth3 = DRIVER_FULL_POWER_ON; - driverTable->overDeep1 = DRIVER_FULL_POWER_ON; - driverTable->overDeep2 = DRIVER_FULL_POWER_ON; - driverTable->ctd1 = DRIVER_FULL_POWER_ON; - driverTable->ctd2 = DRIVER_FULL_POWER_ON; - driverTable->soundVeloc = DRIVER_FULL_POWER_ON; - driverTable->commMast = DRIVER_FULL_POWER_ON; - driverTable->ins = DRIVER_FULL_POWER_ON; - driverTable->ntp = DRIVER_FULL_POWER_ON; - driverTable->inputWaterSensor1 = DRIVER_FULL_POWER_ON; - driverTable->inputWaterSensor2 = DRIVER_FULL_POWER_ON; - driverTable->inputWaterSensor3 = DRIVER_FULL_POWER_ON; + driverTable->depth1 = 1; + driverTable->depth2 = 1; + driverTable->depth3 = 1; + driverTable->overDeep1 = 1; + driverTable->overDeep2 = 1; + driverTable->ctd1 = 1; + driverTable->ctd2 = 1; + driverTable->soundVeloc = 1; + driverTable->commMast = 1; + driverTable->ins = 1; + driverTable->ntp = 1; + driverTable->inputWaterSensor1 = 1; + driverTable->inputWaterSensor2 = 1; + driverTable->inputWaterSensor3 = 1; // 所有负载设备关机 - driverTable->photoSensor = DRIVER_POWER_OFF; - driverTable->electSensor = DRIVER_POWER_OFF; - driverTable->launchControler = DRIVER_POWER_OFF; - driverTable->sideSonar = DRIVER_POWER_OFF; - driverTable->buoyage = DRIVER_POWER_OFF; + driverTable->photoSensor = 0; + driverTable->electSensor = 0; + driverTable->launchControler = 0; + driverTable->sideSonar = 0; + driverTable->buoyage = 0; // 打开必要的执行机构 // 关闭非必要的执行机构 - driverTable->bowCover1 = DRIVER_POWER_OFF; - driverTable->bowCover2 = DRIVER_POWER_OFF; - driverTable->bowCover3 = DRIVER_POWER_OFF; - driverTable->bowCover4 = DRIVER_POWER_OFF; - driverTable->bowCover5 = DRIVER_POWER_OFF; - driverTable->bowCover6 = DRIVER_POWER_OFF; - driverTable->bowRudderDeploy = DRIVER_FULL_POWER_ON; - driverTable->bowRudderServo1 = DRIVER_FULL_POWER_ON; - driverTable->bowRudderServo2 = DRIVER_FULL_POWER_ON; - driverTable->bowWaterlnet1 = DRIVER_POWER_OFF; - driverTable->bowWaterlnet2 = DRIVER_POWER_OFF; - driverTable->bowWaterlnet3 = DRIVER_POWER_OFF; - driverTable->bowWaterlnet4 = DRIVER_POWER_OFF; - driverTable->bowWaterlnet5 = DRIVER_POWER_OFF; - driverTable->bowWaterlnet6 = DRIVER_POWER_OFF; - driverTable->bowWaterlnet7 = DRIVER_POWER_OFF; - driverTable->bowWaterlnet8 = DRIVER_POWER_OFF; - driverTable->waterInject = DRIVER_POWER_OFF; - driverTable->mastLiftingServo = DRIVER_FULL_POWER_ON; - driverTable->buoyancyAdjust1 = DRIVER_FULL_POWER_ON; - driverTable->buoyancyAdjust2 = DRIVER_FULL_POWER_ON; - driverTable->buoyancyAdjust3 = DRIVER_FULL_POWER_ON; - driverTable->buoyancyAdjust4 = DRIVER_FULL_POWER_ON; - driverTable->buoyancyAdjust5 = DRIVER_FULL_POWER_ON; - driverTable->sternRudderServo1 = DRIVER_FULL_POWER_ON; - driverTable->sternRudderServo2 = DRIVER_FULL_POWER_ON; - driverTable->sternRudderServo3 = DRIVER_FULL_POWER_ON; - driverTable->sternRudderServo4 = DRIVER_FULL_POWER_ON; - driverTable->thruster = DRIVER_FULL_POWER_ON; - driverTable->bowThrow = DRIVER_FULL_POWER_ON; - driverTable->sternThrow = DRIVER_FULL_POWER_ON; + driverTable->bowCover1 = 0; + driverTable->bowCover2 = 0; + driverTable->bowCover3 = 0; + driverTable->bowCover4 = 0; + driverTable->bowCover5 = 0; + driverTable->bowCover6 = 0; + driverTable->bowRudderDeploy = 1; + driverTable->bowRudderServo1 = 1; + driverTable->bowRudderServo2 = 1; + driverTable->bowWaterlnet1 = 0; + driverTable->bowWaterlnet2 = 0; + driverTable->bowWaterlnet3 = 0; + driverTable->bowWaterlnet4 = 0; + driverTable->bowWaterlnet5 = 0; + driverTable->bowWaterlnet6 = 0; + driverTable->bowWaterlnet7 = 0; + driverTable->bowWaterlnet8 = 0; + driverTable->waterInject = 0; + driverTable->mastLiftingServo = 1; + driverTable->buoyancyAdjust1 = 1; + driverTable->buoyancyAdjust2 = 1; + driverTable->buoyancyAdjust3 = 1; + driverTable->buoyancyAdjust4 = 1; + driverTable->buoyancyAdjust5 = 1; + driverTable->sternRudderServo1 = 1; + driverTable->sternRudderServo2 = 1; + driverTable->sternRudderServo3 = 1; + driverTable->sternRudderServo4 = 1; + driverTable->thruster = 1; + driverTable->bowThrow = 1; + driverTable->sternThrow = 1; } @@ -668,70 +667,69 @@ std::string Driver::saveDriverTableToJson(const DriverTable* driverTable) Json::Value root; // 添加所有成员到json - root["depth1"] = driverTable->depth1; - root["depth2"] = driverTable->depth2; - root["depth3"] = driverTable->depth3; - root["overDeep1"] = driverTable->overDeep1; - root["overDeep2"] = driverTable->overDeep2; - root["ctd1"] = driverTable->ctd1; - root["ctd2"] = driverTable->ctd2; - root["soundVeloc"] = driverTable->soundVeloc; - root["commMast"] = driverTable->commMast; - root["ins"] = driverTable->ins; - root["ntp"] = driverTable->ntp; - root["inputWaterSensor1"] = driverTable->inputWaterSensor1; - root["inputWaterSensor2"] = driverTable->inputWaterSensor2; - root["inputWaterSensor3"] = driverTable->inputWaterSensor3; - - root["singleBeamSonar1"] = driverTable->singleBeamSonar1; - root["singleBeamSonar2"] = driverTable->singleBeamSonar2; - root["singleBeamSonar3"] = driverTable->singleBeamSonar3; - root["singleBeamSonar4"] = driverTable->singleBeamSonar4; - root["singleBeamSonar5"] = driverTable->singleBeamSonar5; - root["singleBeamSonar6"] = driverTable->singleBeamSonar6; - root["singleBeamSonar7"] = driverTable->singleBeamSonar7; - root["singleBeamSonar8"] = driverTable->singleBeamSonar8; - root["forwardSonar"] = driverTable->forwardSonar; - root["DVL"] = driverTable->DVL; - root["lowFreqSonar"] = driverTable->lowFreqSonar; - root["USBL"] = driverTable->USBL; - root["bowCover1"] = driverTable->bowCover1; - root["bowCover2"] = driverTable->bowCover2; - root["bowCover3"] = driverTable->bowCover3; - root["bowCover4"] = driverTable->bowCover4; - root["bowCover5"] = driverTable->bowCover5; - root["bowCover6"] = driverTable->bowCover6; - root["bowRudderDeploy"] = driverTable->bowRudderDeploy; - root["bowRudderServo1"] = driverTable->bowRudderServo1; - root["bowRudderServo2"] = driverTable->bowRudderServo2; - root["bowWaterlnet1"] = driverTable->bowWaterlnet1; - root["bowWaterlnet2"] = driverTable->bowWaterlnet2; - root["bowWaterlnet3"] = driverTable->bowWaterlnet3; - root["bowWaterlnet4"] = driverTable->bowWaterlnet4; - root["bowWaterlnet5"] = driverTable->bowWaterlnet5; - root["bowWaterlnet6"] = driverTable->bowWaterlnet6; - root["bowWaterlnet7"] = driverTable->bowWaterlnet7; - root["bowWaterlnet8"] = driverTable->bowWaterlnet8; - root["waterInject"] = driverTable->waterInject; - root["mastLiftingServo"] = driverTable->mastLiftingServo; - root["buoyancyAdjust1"] = driverTable->buoyancyAdjust1; - root["buoyancyAdjust2"] = driverTable->buoyancyAdjust2; - root["buoyancyAdjust3"] = driverTable->buoyancyAdjust3; - root["buoyancyAdjust4"] = driverTable->buoyancyAdjust4; - root["buoyancyAdjust5"] = driverTable->buoyancyAdjust5; - root["sternRudderServo1"] = driverTable->sternRudderServo1; - root["sternRudderServo2"] = driverTable->sternRudderServo2; - root["sternRudderServo3"] = driverTable->sternRudderServo3; - root["sternRudderServo4"] = driverTable->sternRudderServo4; - root["thruster"] = driverTable->thruster; - root["bowThrow"] = driverTable->bowThrow; - root["sternThrow"] = driverTable->sternThrow; - root["photoSensor"] = driverTable->photoSensor; - root["electSensor"] = driverTable->electSensor; - root["launchControler"] = driverTable->launchControler; - root["sideSonar"] = driverTable->sideSonar; - root["buoyage"] = driverTable->buoyage; + root["depth1_uni8"] = driverTable->depth1; + root["depth2_uni8"] = driverTable->depth2; + root["depth3_uni8"] = driverTable->depth3; + root["overDeep1_uni8"] = driverTable->overDeep1; + root["overDeep2_uni8"] = driverTable->overDeep2; + root["ctd1_uni8"] = driverTable->ctd1; + root["ctd2_uni8"] = driverTable->ctd2; + root["soundVeloc_uni8"] = driverTable->soundVeloc; + root["commMast_uni8"] = driverTable->commMast; + root["ins_uni8"] = driverTable->ins; + root["ntp_uni8"] = driverTable->ntp; + root["inputWaterSensor1_uni8"] = driverTable->inputWaterSensor1; + root["inputWaterSensor2_uni8"] = driverTable->inputWaterSensor2; + root["inputWaterSensor3_uni8"] = driverTable->inputWaterSensor3; + root["singleBeamSonar1_uni8"] = driverTable->singleBeamSonar1; + root["singleBeamSonar2_uni8"] = driverTable->singleBeamSonar2; + root["singleBeamSonar3_uni8"] = driverTable->singleBeamSonar3; + root["singleBeamSonar4_uni8"] = driverTable->singleBeamSonar4; + root["singleBeamSonar5_uni8"] = driverTable->singleBeamSonar5; + root["singleBeamSonar6_uni8"] = driverTable->singleBeamSonar6; + root["singleBeamSonar7_uni8"] = driverTable->singleBeamSonar7; + root["singleBeamSonar8_uni8"] = driverTable->singleBeamSonar8; + root["forwardSonar_uni8"] = driverTable->forwardSonar; + root["DVL_uni8"] = driverTable->DVL; + root["lowFreqSonar_uni8"] = driverTable->lowFreqSonar; + root["USBL_uni8"] = driverTable->USBL; + root["bowCover1_uni8"] = driverTable->bowCover1; + root["bowCover2_uni8"] = driverTable->bowCover2; + root["bowCover3_uni8"] = driverTable->bowCover3; + root["bowCover4_uni8"] = driverTable->bowCover4; + root["bowCover5_uni8"] = driverTable->bowCover5; + root["bowCover6_uni8"] = driverTable->bowCover6; + root["bowRudderDeploy_uni8"] = driverTable->bowRudderDeploy; + root["bowRudderServo1_uni8"] = driverTable->bowRudderServo1; + root["bowRudderServo2_uni8"] = driverTable->bowRudderServo2; + root["bowWaterlnet1_uni8"] = driverTable->bowWaterlnet1; + root["bowWaterlnet2_uni8"] = driverTable->bowWaterlnet2; + root["bowWaterlnet3_uni8"] = driverTable->bowWaterlnet3; + root["bowWaterlnet4_uni8"] = driverTable->bowWaterlnet4; + root["bowWaterlnet5_uni8"] = driverTable->bowWaterlnet5; + root["bowWaterlnet6_uni8"] = driverTable->bowWaterlnet6; + root["bowWaterlnet7_uni8"] = driverTable->bowWaterlnet7; + root["bowWaterlnet8_uni8"] = driverTable->bowWaterlnet8; + root["waterInject_uni8"] = driverTable->waterInject; + root["mastLiftingServo_uni8"] = driverTable->mastLiftingServo; + root["buoyancyAdjust1_uni8"] = driverTable->buoyancyAdjust1; + root["buoyancyAdjust2_uni8"] = driverTable->buoyancyAdjust2; + root["buoyancyAdjust3_uni8"] = driverTable->buoyancyAdjust3; + root["buoyancyAdjust4_uni8"] = driverTable->buoyancyAdjust4; + root["buoyancyAdjust5_uni8"] = driverTable->buoyancyAdjust5; + root["sternRudderServo1_uni8"] = driverTable->sternRudderServo1; + root["sternRudderServo2_uni8"] = driverTable->sternRudderServo2; + root["sternRudderServo3_uni8"] = driverTable->sternRudderServo3; + root["sternRudderServo4_uni8"] = driverTable->sternRudderServo4; + root["thruster_uni8"] = driverTable->thruster; + root["bowThrow_uni8"] = driverTable->bowThrow; + root["sternThrow_uni8"] = driverTable->sternThrow; + root["photoSensor_uni8"] = driverTable->photoSensor; + root["electSensor_uni8"] = driverTable->electSensor; + root["launchControler_uni8"] = driverTable->launchControler; + root["sideSonar_uni8"] = driverTable->sideSonar; + root["buoyage_uni8"] = driverTable->buoyage; return root.toStyledString(); } std::string Driver::getDriverTableJson() diff --git a/src/pPowerManger/pmSysvariable.h b/src/pPowerManger/pmSysvariable.h index 822be2d..48d7da8 100644 --- a/src/pPowerManger/pmSysvariable.h +++ b/src/pPowerManger/pmSysvariable.h @@ -423,6 +423,7 @@ namespace PMMode { //工况定义 namespace WorkCondition { enum Type : unsigned char { + CHANING = 0, // 工况切换中 STANDBY = 1, // 待机工况 SHORE_BASED_READY = 2, // 岸基备航 WATER_BASED_READY = 3, // 水中备航 @@ -509,5 +510,57 @@ struct DeviceSwitchCmd int cmd; }; +namespace FaultEnable { + enum Type : bool { + BasicNavigationFault_1 = true, // 使能故障 + BasicNavigationFault_2 = false, + BasicNavigationFault_3 = false, + BasicNavigationFault_4 = false, + BasicNavigationFault_5 = false, + BasicNavigationFault_6 = false, + BasicNavigationFault_7 = false, + BasicNavigationFault_8 = false, + BasicNavigationFault_9 = false, + BasicNavigationFault_10 = false, + + ExtendedNavigationFault_1 = false, // 使能故障 + ExtendedNavigationFault_2 = false, + ExtendedNavigationFault_3 = false, + ExtendedNavigationFault_4 = false, + + ObstacleAvoidanceFault_1 = false, // 使能故障 + ObstacleAvoidanceFault_2 = false, + ObstacleAvoidanceFault_3 = false, + + BuoyancyControlFault_1 = false, // 使能故障 + BuoyancyControlFault_2 = false, + BuoyancyControlFault_3 = false, + BuoyancyControlFault_4 = false, + + CommunicationSystemFault_1 = false, // 使能故障 + CommunicationSystemFault_2 = false, + CommunicationSystemFault_3 = false, + + PayloadSystemFault_1 = false, // 使能故障 + PayloadSystemFault_2 = false, + PayloadSystemFault_3 = false, + PayloadSystemFault_4 = false, + PayloadSystemFault_5 = false, + PayloadSystemFault_6 = false, + PayloadSystemFault_7 = false, + + EmergencySystemFault_1 = false, // 使能故障 + EmergencySystemFault_2 = false, + EmergencySystemFault_3 = false, + EmergencySystemFault_4 = false, + EmergencySystemFault_5 = false, + EmergencySystemFault_6 = false, + + SensorSystemFault_1 = false, // 使能故障 + SensorSystemFault_2 = false, + SensorSystemFault_3 = false, + }; +} + #endif \ No newline at end of file diff --git a/test/scripts/TestGUI.py b/test/scripts/TestGUI.py index 473696e..69ac902 100644 --- a/test/scripts/TestGUI.py +++ b/test/scripts/TestGUI.py @@ -13,6 +13,7 @@ from ccuSys import * # 创建UDP通信套接字 sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # 目标IP地址和端口号 +# target_ip = 'localhost' target_ip = 'localhost' target_port = 5001 server_address = (target_ip, target_port)