重构了与DB的交互逻辑,以及与任务\上位机指令的消息,使用新的loguru库进行日志管理
This commit is contained in:
@@ -240,3 +240,6 @@ compile_commands.json
|
||||
CTestTestfile.cmake
|
||||
_deps
|
||||
|
||||
|
||||
# PowerManager specific log files
|
||||
bin/pPowerManger.log
|
||||
|
||||
@@ -0,0 +1,9 @@
|
||||
//------------------------------------------------
|
||||
// pPMtest config block
|
||||
|
||||
ProcessConfig = pPMtest
|
||||
{
|
||||
AppTick = 4
|
||||
CommsTick = 4
|
||||
}
|
||||
|
||||
@@ -0,0 +1,326 @@
|
||||
{
|
||||
"navigationParam_obj" :
|
||||
{
|
||||
"limitedNavRegion_obj" : [],
|
||||
"maxDepth_fp32" : -1,
|
||||
"maxSpeed_fp32" : -1,
|
||||
"maxVerSpeed_fp32" : -1,
|
||||
"minHeight_fp32" : -1,
|
||||
"minPitch_fp32" : -1,
|
||||
"minTurnLimit_fp32" : -1
|
||||
},
|
||||
"planHash_int32" : -2031201612,
|
||||
"planID_int32" : 1817399773,
|
||||
"planName_str" : "pl_test2",
|
||||
"taskSort_array_int32" :
|
||||
[
|
||||
68991759,
|
||||
1726048938,
|
||||
68991758,
|
||||
-2127541611,
|
||||
1275525031,
|
||||
-725793762
|
||||
],
|
||||
"task_obj" :
|
||||
{
|
||||
"-2127541611" :
|
||||
{
|
||||
"bowRudderUnfold_bool" : false,
|
||||
"depthPoints_array_fp32" :
|
||||
[
|
||||
0.0
|
||||
],
|
||||
"desiredDepth_fp32" : 0,
|
||||
"desiredHeight_fp32" : 0,
|
||||
"desiredSpeed_fp32" : 2.0577776432037354,
|
||||
"destination_array_fp64" :
|
||||
[
|
||||
20.636590882471154,
|
||||
111.41261861598258
|
||||
],
|
||||
"endDuration_int32" : 30,
|
||||
"estimatedConsume_fp32" : 0,
|
||||
"estimatedTime_fp32" : 0,
|
||||
"fixedType_int32" : 1,
|
||||
"latPoints_array_fp64" :
|
||||
[
|
||||
20.636590882471154
|
||||
],
|
||||
"lonPoints_array_fp64" :
|
||||
[
|
||||
111.41261861598258
|
||||
],
|
||||
"pastPointIndex_int32" : -1,
|
||||
"pathLength_fp32" : 0,
|
||||
"pointID_array_str" :
|
||||
[
|
||||
"-2127541611_0"
|
||||
],
|
||||
"radius_fp32" : 20,
|
||||
"source_int32" : 1,
|
||||
"speedType_int32" : 1,
|
||||
"taskID_int32" : -2127541611,
|
||||
"taskName_str" : "Hover1",
|
||||
"taskType_str" : "hover"
|
||||
},
|
||||
"-725793762" :
|
||||
{
|
||||
"bowRudderUnfold_bool" : false,
|
||||
"cyclePoints_array_int32" :
|
||||
[
|
||||
1
|
||||
],
|
||||
"depthPoints_array_fp32" :
|
||||
[
|
||||
0.0
|
||||
],
|
||||
"desiredSpeed_fp32" : 1,
|
||||
"destination_array_fp64" :
|
||||
[
|
||||
20.633257427671467,
|
||||
111.42364785945182
|
||||
],
|
||||
"endWay_int32" : 1,
|
||||
"estimatedConsume_fp32" : 0,
|
||||
"estimatedTime_fp32" : 0,
|
||||
"latPoints_array_fp64" :
|
||||
[
|
||||
20.633257427671467
|
||||
],
|
||||
"lonPoints_array_fp64" :
|
||||
[
|
||||
111.42364785945182
|
||||
],
|
||||
"pastPointIndex_int32" : -1,
|
||||
"pathLength_fp32" : 0,
|
||||
"pointID_array_str" :
|
||||
[
|
||||
"-725793762_0"
|
||||
],
|
||||
"radius_fp32" : 20,
|
||||
"source_int32" : 1,
|
||||
"speedType_int32" : 2,
|
||||
"taskID_int32" : -725793762,
|
||||
"taskName_str" : "Recycle1",
|
||||
"taskType_str" : "recycle"
|
||||
},
|
||||
"1275525031" :
|
||||
{
|
||||
"bowRudderUnfold_bool" : false,
|
||||
"calibrateMode_int32" : 1,
|
||||
"depthPoints_array_fp32" :
|
||||
[
|
||||
0.0
|
||||
],
|
||||
"desiredSpeed_fp32" : 1,
|
||||
"destination_array_fp64" :
|
||||
[
|
||||
20.641771649173037,
|
||||
111.42562196528678
|
||||
],
|
||||
"doubleCalibrateSetting_int32" : 0,
|
||||
"estimatedConsume_fp32" : 0,
|
||||
"estimatedTime_fp32" : 0,
|
||||
"latPoints_array_fp64" :
|
||||
[
|
||||
20.641771649173037
|
||||
],
|
||||
"lonPoints_array_fp64" :
|
||||
[
|
||||
111.42562196528678
|
||||
],
|
||||
"pastPointIndex_int32" : -1,
|
||||
"pathLength_fp32" : 0,
|
||||
"pointID_array_str" :
|
||||
[
|
||||
"1275525031_0"
|
||||
],
|
||||
"source_int32" : 1,
|
||||
"speedType_int32" : 2,
|
||||
"taskID_int32" : 1275525031,
|
||||
"taskName_str" : "Calibration1",
|
||||
"taskType_str" : "satCalibrate"
|
||||
},
|
||||
"1726048938" :
|
||||
{
|
||||
"attack_int32" : 0,
|
||||
"balanceEnabled_bool" : false,
|
||||
"bowRudderUnfold_bool" : false,
|
||||
"calibPointsExtend_array_int32" :
|
||||
[
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"calibPoints_array_int32" :
|
||||
[
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"calibrateMode_int32" : 1,
|
||||
"depthPointsExtend_array_fp32" :
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"desiredDepth_fp32" : 0,
|
||||
"desiredHeight_fp32" : 0,
|
||||
"desiredSpeed_fp32" : 2,
|
||||
"estimatedConsume_fp32" : 0,
|
||||
"estimatedTime_fp32" : 0,
|
||||
"fixedType_int32" : 1,
|
||||
"flankArray_int32" : 0,
|
||||
"latPointsExtend_array_fp64" :
|
||||
[
|
||||
20.672185454579221,
|
||||
20.654677472562906,
|
||||
20.660540464584184,
|
||||
20.672185454579221,
|
||||
20.654677472562906,
|
||||
20.660540464584184,
|
||||
20.672185454579221,
|
||||
20.654677472562906,
|
||||
20.660540464584184
|
||||
],
|
||||
"latPoints_array_fp64" :
|
||||
[
|
||||
20.672185454579221,
|
||||
20.654677472562906,
|
||||
20.660540464584184
|
||||
],
|
||||
"lonPointsExtend_array_fp64" :
|
||||
[
|
||||
111.41573055186512,
|
||||
111.41349821869244,
|
||||
111.39761978934951,
|
||||
111.41573055186512,
|
||||
111.41349821869244,
|
||||
111.39761978934951,
|
||||
111.41573055186512,
|
||||
111.41349821869244,
|
||||
111.39761978934951
|
||||
],
|
||||
"lonPoints_array_fp64" :
|
||||
[
|
||||
111.41573055186512,
|
||||
111.41349821869244,
|
||||
111.39761978934951
|
||||
],
|
||||
"loop_int32" : 3,
|
||||
"magnet_int32" : 0,
|
||||
"optronic_int32" : 0,
|
||||
"pastPointIndex_int32" : -1,
|
||||
"pathLength_fp32" : 0,
|
||||
"pointID_array_str" :
|
||||
[
|
||||
"1726048938_0",
|
||||
"1726048938_1",
|
||||
"1726048938_2",
|
||||
"1726048938_3",
|
||||
"1726048938_4",
|
||||
"1726048938_5",
|
||||
"1726048938_6",
|
||||
"1726048938_7",
|
||||
"1726048938_8"
|
||||
],
|
||||
"source_int32" : 1,
|
||||
"speedType_int32" : 2,
|
||||
"taskID_int32" : 1726048938,
|
||||
"taskName_str" : "AreaSurvey1",
|
||||
"taskType_str" : "tour",
|
||||
"towArray_int32" : 0
|
||||
},
|
||||
"68991758" :
|
||||
{
|
||||
"balanceEnabled_bool" : false,
|
||||
"bowRudderUnfold_bool" : false,
|
||||
"depthPoints_array_fp32" :
|
||||
[
|
||||
0.0
|
||||
],
|
||||
"desiredDepth_fp32" : 0,
|
||||
"desiredHeight_fp32" : 0,
|
||||
"desiredSpeed_fp32" : 2,
|
||||
"destination_array_fp64" :
|
||||
[
|
||||
20.643378052620122,
|
||||
111.4050226000524
|
||||
],
|
||||
"estimatedConsume_fp32" : 0,
|
||||
"estimatedTime_fp32" : 0,
|
||||
"fixedType_int32" : 1,
|
||||
"latPoints_array_fp64" :
|
||||
[
|
||||
20.643378052620122
|
||||
],
|
||||
"lonPoints_array_fp64" :
|
||||
[
|
||||
111.4050226000524
|
||||
],
|
||||
"pastPointIndex_int32" : -1,
|
||||
"pathLength_fp32" : 0,
|
||||
"pointID_array_str" :
|
||||
[
|
||||
"68991758_0"
|
||||
],
|
||||
"source_int32" : 1,
|
||||
"speedType_int32" : 2,
|
||||
"taskID_int32" : 68991758,
|
||||
"taskName_str" : "Goto1",
|
||||
"taskType_str" : "sail"
|
||||
},
|
||||
"68991759" :
|
||||
{
|
||||
"balanceEnabled_bool" : false,
|
||||
"bowRudderUnfold_bool" : false,
|
||||
"depthPoints_array_fp32" :
|
||||
[
|
||||
0.0
|
||||
],
|
||||
"desiredDepth_fp32" : 0,
|
||||
"desiredHeight_fp32" : 0,
|
||||
"desiredSpeed_fp32" : 2,
|
||||
"destination_array_fp64" :
|
||||
[
|
||||
20.650305473106499,
|
||||
111.4256434229589
|
||||
],
|
||||
"estimatedConsume_fp32" : 0,
|
||||
"estimatedTime_fp32" : 0,
|
||||
"fixedType_int32" : 1,
|
||||
"latPoints_array_fp64" :
|
||||
[
|
||||
20.650305473106499
|
||||
],
|
||||
"lonPoints_array_fp64" :
|
||||
[
|
||||
111.4256434229589
|
||||
],
|
||||
"pastPointIndex_int32" : -1,
|
||||
"pathLength_fp32" : 0,
|
||||
"pointID_array_str" :
|
||||
[
|
||||
"68991759_0"
|
||||
],
|
||||
"source_int32" : 1,
|
||||
"speedType_int32" : 2,
|
||||
"taskID_int32" : 68991759,
|
||||
"taskName_str" : "Goto2",
|
||||
"taskType_str" : "sail"
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,4 +1,4 @@
|
||||
{
|
||||
{"uPower_currentState_st":{
|
||||
"HVABFTDev123Cur_uint16" : 0,
|
||||
"HVABactCur_uint16" : 0,
|
||||
"HVABbowRudCur_uint16" : 0,
|
||||
@@ -6,7 +6,7 @@
|
||||
"HVABbusBVol_uint16" : 0,
|
||||
"HVABmastSteGeaCur_uint16" : 0,
|
||||
"HVABopenWaterCur_uint16" : 0,
|
||||
"HVABxczCur_uint16": 1,
|
||||
"HVABxczCur_uint16" : 0,
|
||||
"HVBBCCUCur_uint16" : 0,
|
||||
"HVBBFCSSecurSysCur_uint16" : 0,
|
||||
"HVBBbatInsCur_uint16" : 0,
|
||||
@@ -45,3 +45,4 @@
|
||||
"LVBBunit5Cur_uint16" : 0,
|
||||
"LVBinsBusVol_uint16" : 0
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,10 +1,11 @@
|
||||
{
|
||||
{"uPower_disSysState_st":{
|
||||
"HVolABusBreaker_unit16" : 0,
|
||||
"HVolBBusBreaker_unit16": 79,
|
||||
"HVolBusBreaker_uint16": 58,
|
||||
"LVolBusBreaker_uint16": 159,
|
||||
"HVolBBusBreaker_unit16" : 0,
|
||||
"HVolBusBreaker_uint16" : 0,
|
||||
"LVolBusBreaker_uint16" : 0,
|
||||
"faultCode_array" :
|
||||
[
|
||||
0
|
||||
]
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +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
|
||||
}
|
||||
}
|
||||
@@ -1,30 +1,31 @@
|
||||
{
|
||||
"backpressure1_fp32": 1.0,
|
||||
"backpressure2_fp32": 2.0,
|
||||
"ch4oConcent1_fp32": 29.0,
|
||||
"ch4oConcent2_fp32": 96.0,
|
||||
{"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": 23.0,
|
||||
"excTemp_fp32" : 0.0,
|
||||
"faultCode1_uint16" : 0,
|
||||
"faultCode2_uint16" : 0,
|
||||
"faultCode3_uint16" : 0,
|
||||
"fcuCurIns_fp32" : 0.0,
|
||||
"fcuCurPower_fp32" : 0.0,
|
||||
"fcuCur_fp32": 84.0,
|
||||
"fcuCur_fp32" : 0.0,
|
||||
"fcuFaultLeve_uint8" : 0,
|
||||
"fcuOutIns_fp32": 53.0,
|
||||
"fcuOutPower_fp32": 100.0,
|
||||
"fcuOutIns_fp32" : 0.0,
|
||||
"fcuOutPower_fp32" : 0.0,
|
||||
"fcuState_uint8" : 0,
|
||||
"fcuVolIns_fp32" : 0.0,
|
||||
"fcuVolPower_fp32" : 0.0,
|
||||
"fcuVol_fp32": 377.0,
|
||||
"flameDet_uint8": 1,
|
||||
"h2Concent1_fp32": 89.0,
|
||||
"h2Concent2_fp32": 62.0,
|
||||
"o2Concent1_fp32": 74.0,
|
||||
"o2Concent2_fp32": 21.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": 88.0,
|
||||
"powerGen_fp32": 622.0
|
||||
"powLimit_fp32" : 0.0,
|
||||
"powerGen_fp32" : 0.0
|
||||
}
|
||||
}
|
||||
@@ -1,38 +1,39 @@
|
||||
{
|
||||
{"uPower_liBatState_st":{
|
||||
"fcuCurToInsLimit1_uint8" : 0,
|
||||
"fcuCurToinsLimit2_uint8" : 0,
|
||||
"insBatCurElect_uint16" : 0,
|
||||
"insBatLimit_uint16": 41,
|
||||
"insBatSoc_uint8": 42,
|
||||
"insBatTotalElect_uint16": 617,
|
||||
"insBatLimit_uint16" : 0,
|
||||
"insBatSoc_uint8" : 0,
|
||||
"insBatTotalElect_uint16" : 0,
|
||||
"insChargeState_uint8" : 0,
|
||||
"insCurSocHold1_uint8" : 0,
|
||||
"insCurSocHold2_uint8" : 0,
|
||||
"insCurSocHold3_uint8" : 0,
|
||||
"insCurrent_uint16": 2,
|
||||
"insCurrent_uint16" : 0,
|
||||
"insLinkVol_uint16" : 0,
|
||||
"insNegRelayState_uint8" : 0,
|
||||
"insNegRes_uint16" : 0,
|
||||
"insPackVol_uint16" : 0,
|
||||
"insPosRelayState_uint8": 1,
|
||||
"insPosRelayState_uint8" : 0,
|
||||
"insPosRes_uint16" : 0,
|
||||
"insfaultCode_uint8" : 0,
|
||||
"powBatCurElect_uint16" : 0,
|
||||
"powBatSoc_uint8": 31,
|
||||
"powBatTotalElect_uint16": 763,
|
||||
"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": 21,
|
||||
"powCur_uint16" : 0,
|
||||
"powLinkVol_uint16" : 0,
|
||||
"powNegRelayState_uint8": 1,
|
||||
"powNegRelayState_uint8" : 0,
|
||||
"powNegRes_uint16" : 0,
|
||||
"powPosRelayState_uint8": 1,
|
||||
"powPosRelayState_uint8" : 0,
|
||||
"powPosRes_uint16" : 0,
|
||||
"powerBatLimit_uint16": 13,
|
||||
"powerBatLimit_uint16" : 0,
|
||||
"powerPackVol_uint16" : 0,
|
||||
"powfaultCode_uint8" : 0
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
{
|
||||
"currentMod_uint8": 0,
|
||||
{"uPower_pmState_st":{
|
||||
"currentMod_uint8" : 1,
|
||||
"falutLevel_uint8" : 0,
|
||||
"faultCode_array" :
|
||||
[
|
||||
@@ -8,5 +8,6 @@
|
||||
"powerState_uint8" : 0,
|
||||
"soc_uint32" : 0,
|
||||
"sustainableTime_uint32" : 0,
|
||||
"workCondition_uint8": 0
|
||||
"workCondition_uint8" : 1
|
||||
}
|
||||
}
|
||||
@@ -0,0 +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
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
{
|
||||
"value_array_uint16" :[2,1,3]
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
{
|
||||
"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
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
{
|
||||
"balanceEnabled_bool" : false,
|
||||
"bowRudderUnfold_bool" : false,
|
||||
"depthPoints_array_fp32" :
|
||||
[
|
||||
0.0
|
||||
],
|
||||
"desiredDepth_fp32" : 0,
|
||||
"desiredHeight_fp32" : 0,
|
||||
"desiredSpeed_fp32" : 2,
|
||||
"destination_array_fp64" :
|
||||
[
|
||||
20.650305473106499,
|
||||
111.4256434229589
|
||||
],
|
||||
"estimatedConsume_fp32" : 0,
|
||||
"estimatedTime_fp32" : 0,
|
||||
"fixedType_int32" : 1,
|
||||
"latPoints_array_fp64" :
|
||||
[
|
||||
20.650305473106499
|
||||
],
|
||||
"lonPoints_array_fp64" :
|
||||
[
|
||||
111.4256434229589
|
||||
],
|
||||
"pastPointIndex_int32" : -1,
|
||||
"pathLength_fp32" : 0,
|
||||
"pointID_array_str" :
|
||||
[
|
||||
"68991759_0"
|
||||
],
|
||||
"source_int32" : 1,
|
||||
"speedType_int32" : 2,
|
||||
"taskID_int32" : 68991759,
|
||||
"taskName_str" : "Goto2",
|
||||
"taskType_str" : "sail"
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
{
|
||||
"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
|
||||
}
|
||||
@@ -0,0 +1,5 @@
|
||||
{
|
||||
"powerCMD":4,
|
||||
"modCMD":1,
|
||||
"workCMD":12
|
||||
}
|
||||
+1
-1
@@ -17,7 +17,7 @@ INCLUDE_DIRECTORIES(${LOCAL_LIBRARY_DIRS})
|
||||
#============================================================================
|
||||
ADD_SUBDIRECTORY(pPowerManger)
|
||||
ADD_SUBDIRECTORY(appPowerMangerWebUI)
|
||||
|
||||
add_subdirectory(pPMtest)
|
||||
##############################################################################
|
||||
# END of CMakeLists.txt
|
||||
##############################################################################
|
||||
|
||||
@@ -0,0 +1,19 @@
|
||||
#--------------------------------------------------------
|
||||
# The CMakeLists.txt for: pPMtest
|
||||
# Author(s):
|
||||
#--------------------------------------------------------
|
||||
|
||||
SET(SRC
|
||||
PMtest.cpp
|
||||
PMtest_Info.cpp
|
||||
main.cpp
|
||||
)
|
||||
|
||||
ADD_EXECUTABLE(pPMtest ${SRC})
|
||||
|
||||
TARGET_LINK_LIBRARIES(pPMtest
|
||||
${MOOS_LIBRARIES}
|
||||
mbutil
|
||||
m
|
||||
pthread)
|
||||
|
||||
@@ -0,0 +1,233 @@
|
||||
/************************************************************/
|
||||
/* NAME: */
|
||||
/* ORGN: MIT */
|
||||
/* FILE: PMtest.cpp */
|
||||
/* DATE: */
|
||||
/************************************************************/
|
||||
|
||||
#include <iterator>
|
||||
#include "MBUtils.h"
|
||||
#include "PMtest.h"
|
||||
#include <iostream>
|
||||
#include <unistd.h>
|
||||
#include <termios.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Constructor
|
||||
|
||||
PMtest::PMtest()
|
||||
{
|
||||
}
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Destructor
|
||||
|
||||
PMtest::~PMtest()
|
||||
{
|
||||
}
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Procedure: OnNewMail
|
||||
|
||||
#include <fstream>
|
||||
#include <ctime>
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
#include <map>
|
||||
|
||||
std::string getCurrentTimeStamp() {
|
||||
auto now = std::time(nullptr);
|
||||
auto tm = *std::localtime(&now);
|
||||
std::ostringstream oss;
|
||||
oss << std::put_time(&tm, "%Y%m%d_%H%M%S");
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
bool PMtest::OnNewMail(MOOSMSG_LIST &NewMail)
|
||||
{
|
||||
// 宏名到消息字符串的映射
|
||||
static const std::map<std::string, std::string> msg_map = {
|
||||
{PM_TO_DB_STATE, "uPower_pmState_st"},
|
||||
{PM_TO_DB_CURRENTSTATE, "uPower_currentState_st"},
|
||||
{PM_TO_DB_DISSTATE, "uPower_disSysState_st"},
|
||||
{PM_TO_DB_LIBATSTATE, "uPower_liBatState_st"},
|
||||
{PM_TO_DB_FCSSTATE, "uPower_fcsState_st"},
|
||||
{PM_TO_DB_SUBDISSTATE, "uPower_secdistState_cmd"},
|
||||
{PM_TO_DB_EQUIPMENTSTATE, "uPower_equState_fb"}
|
||||
};
|
||||
|
||||
MOOSMSG_LIST::iterator p;
|
||||
|
||||
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
|
||||
CMOOSMsg &msg = *p;
|
||||
string key = msg.GetKey();
|
||||
// 只处理PM_TO_DB开头的消息
|
||||
string sval = msg.GetString();
|
||||
auto it = msg_map.find(key);
|
||||
string msg_name = (it != msg_map.end()) ? it->second : key;
|
||||
string filename = "/home/zjk/project/H100/H100PowerManger/data/msg/" + msg_name + ".json";
|
||||
|
||||
ofstream outfile(filename);
|
||||
if(outfile.is_open()) {
|
||||
outfile << "{\"" << msg_name << "\":" << sval << "}" << endl;
|
||||
outfile.close();
|
||||
}
|
||||
}
|
||||
return(true);
|
||||
}
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Procedure: OnConnectToServer
|
||||
|
||||
bool PMtest::OnConnectToServer()
|
||||
{
|
||||
RegisterVariables();
|
||||
return(true);
|
||||
}
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Procedure: Iterate()
|
||||
// happens AppTick times per second
|
||||
|
||||
// 添加非阻塞键盘检测函数
|
||||
int kbhit() {
|
||||
struct termios oldt, newt;
|
||||
int ch;
|
||||
int oldf;
|
||||
|
||||
tcgetattr(STDIN_FILENO, &oldt);
|
||||
newt = oldt;
|
||||
newt.c_lflag &= ~(ICANON | ECHO);
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
|
||||
oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
|
||||
fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
|
||||
|
||||
ch = getchar();
|
||||
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
|
||||
fcntl(STDIN_FILENO, F_SETFL, oldf);
|
||||
|
||||
if(ch != EOF) {
|
||||
ungetc(ch, stdin);
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool PMtest::Iterate()
|
||||
{
|
||||
handleDBToPMMsgs();
|
||||
//非阻塞检测键盘输入
|
||||
if(kbhit()) {
|
||||
char c = getchar(); // 修改为getchar()
|
||||
if(c == 'a') {
|
||||
//发送DB_TO_PM_MISSION消息
|
||||
std::cout << "发送DB_TO_PM_MISSION消息" << std::endl;
|
||||
string filename = "/home/zjk/project/H100/H100PowerManger/data/testmsg/uPlan_taskStart_cmd.json";
|
||||
string content;
|
||||
if(readJsonFromFile(filename, content)) {
|
||||
Notify(DB_TO_PM_MISSION, content);
|
||||
}
|
||||
}
|
||||
else if (c == 's')
|
||||
{
|
||||
//发送DB_TO_PM_MISSION消息
|
||||
std::cout << "发送DB_TO_PM_MISSION消息" << std::endl;
|
||||
string filename = "/home/zjk/project/H100/H100PowerManger/data/testmsg/uPower_sysState_cmd.json";
|
||||
string content;
|
||||
if(readJsonFromFile(filename, content)) {
|
||||
Notify(DB_TO_PM_MSCMD, content);
|
||||
}
|
||||
}
|
||||
}
|
||||
return(true);
|
||||
}
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Procedure: readJsonFromFile
|
||||
// 从文件中读取json内容
|
||||
|
||||
bool PMtest::readJsonFromFile(string file, string& content)
|
||||
{
|
||||
ifstream infile(file);
|
||||
if(!infile.is_open()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
content.assign((istreambuf_iterator<char>(infile)),
|
||||
(istreambuf_iterator<char>()));
|
||||
|
||||
infile.close();
|
||||
return true;
|
||||
}
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Procedure: handleDBToPMMsgs
|
||||
// 处理所有DB_TO_PM消息
|
||||
|
||||
void PMtest::handleDBToPMMsgs()
|
||||
{
|
||||
// DB_TO_PM消息类型映射表
|
||||
static const vector<pair<string, string>> db_to_pm_msgs = {
|
||||
// {DB_TO_PM_MSCMD, "uPower_sysState_cmd"},
|
||||
{DB_TO_PM_EQUIPMENTCMD, "uPower_equState_cmd"},
|
||||
{DB_TO_PM_SUBDISSTATE, "uDevice_secdistState_st"},
|
||||
{DB_TO_PM_DEEPTH, "uDevice_depth_st"},
|
||||
// {DB_TO_PM_MISSION, "uPlan_taskStart_cmd"}
|
||||
};
|
||||
|
||||
for (const auto& msg : db_to_pm_msgs) {
|
||||
string filename = "/home/zjk/project/H100/H100PowerManger/data/testmsg/" + msg.second + ".json";
|
||||
string content;
|
||||
|
||||
if(readJsonFromFile(filename, content)) {
|
||||
Notify(msg.first, content);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Procedure: OnStartUp()
|
||||
// happens before connection is open
|
||||
|
||||
bool PMtest::OnStartUp()
|
||||
{
|
||||
list<string> sParams;
|
||||
m_MissionReader.EnableVerbatimQuoting(false);
|
||||
if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) {
|
||||
list<string>::iterator p;
|
||||
for(p=sParams.begin(); p!=sParams.end(); p++) {
|
||||
string line = *p;
|
||||
string param = tolower(biteStringX(line, '='));
|
||||
string value = line;
|
||||
|
||||
if(param == "foo") {
|
||||
//handled
|
||||
}
|
||||
else if(param == "bar") {
|
||||
//handled
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
RegisterVariables();
|
||||
return(true);
|
||||
}
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Procedure: RegisterVariables
|
||||
|
||||
void PMtest::RegisterVariables()
|
||||
{
|
||||
Register(PM_TO_DB_STATE, 0);
|
||||
Register(PM_TO_DB_CURRENTSTATE, 0);
|
||||
Register(PM_TO_DB_DISSTATE, 0);
|
||||
Register(PM_TO_DB_LIBATSTATE, 0);
|
||||
Register(PM_TO_DB_FCSSTATE, 0);
|
||||
Register(PM_TO_DB_SUBDISSTATE, 0);
|
||||
Register(PM_TO_DB_EQUIPMENTSTATE, 0);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,57 @@
|
||||
/************************************************************/
|
||||
/* NAME: */
|
||||
/* ORGN: MIT */
|
||||
/* FILE: PMtest.h */
|
||||
/* DATE: */
|
||||
/************************************************************/
|
||||
|
||||
#ifndef PMtest_HEADER
|
||||
#define PMtest_HEADER
|
||||
|
||||
#include "MOOS/libMOOS/MOOSLib.h"
|
||||
#include "json/json.h"
|
||||
#include "string"
|
||||
using namespace std;
|
||||
//能源管理反馈的消息
|
||||
#define PM_TO_DB_STATE "uPower_pmState_st"
|
||||
#define PM_TO_DB_CURRENTSTATE "uPower_currentState_st"
|
||||
#define PM_TO_DB_DISSTATE "uPower_disSysState_st"
|
||||
#define PM_TO_DB_LIBATSTATE "uPower_liBatState_st"
|
||||
#define PM_TO_DB_FCSSTATE "uPower_fcsState_st"
|
||||
|
||||
#define PM_TO_DB_SUBDISSTATE "uPower_secdistState_cmd"
|
||||
#define PM_TO_DB_EQUIPMENTSTATE "uPower_equState_fb"
|
||||
|
||||
//控制消息
|
||||
#define DB_TO_PM_MSCMD "uPower_sysState_cmd"
|
||||
#define DB_TO_PM_EQUIPMENTCMD "uPower_equState_cmd"
|
||||
#define DB_TO_PM_SUBDISSTATE "uDevice_secdistState_st"
|
||||
|
||||
//决策消息
|
||||
#define DB_TO_PM_DEEPTH "uDevice_depth_st"
|
||||
#define DB_TO_PM_MISSION "uPlan_taskStart_cmd"
|
||||
|
||||
class PMtest : public CMOOSApp
|
||||
{
|
||||
public:
|
||||
PMtest();
|
||||
~PMtest();
|
||||
|
||||
protected: // Standard MOOSApp functions to overload
|
||||
bool OnNewMail(MOOSMSG_LIST &NewMail);
|
||||
bool Iterate();
|
||||
bool OnConnectToServer();
|
||||
bool OnStartUp();
|
||||
protected:
|
||||
void RegisterVariables();
|
||||
void handleDBToPMMsgs();
|
||||
int snedFlag;
|
||||
|
||||
private: // Configuration variables
|
||||
|
||||
private: // State variables
|
||||
// 从文件中读取json内容
|
||||
bool readJsonFromFile(string file, string& content);
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,115 @@
|
||||
/****************************************************************/
|
||||
/* NAME: */
|
||||
/* ORGN: MIT Cambridge MA */
|
||||
/* FILE: PMtest_Info.cpp */
|
||||
/* DATE: Dec 29th 1963 */
|
||||
/****************************************************************/
|
||||
|
||||
#include <cstdlib>
|
||||
#include <iostream>
|
||||
#include "PMtest_Info.h"
|
||||
#include "ColorParse.h"
|
||||
#include "ReleaseInfo.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
//----------------------------------------------------------------
|
||||
// Procedure: showSynopsis
|
||||
|
||||
void showSynopsis()
|
||||
{
|
||||
blk("SYNOPSIS: ");
|
||||
blk("------------------------------------ ");
|
||||
blk(" The pPMtest application is used for ");
|
||||
blk(" ");
|
||||
blk(" ");
|
||||
blk(" ");
|
||||
blk(" ");
|
||||
}
|
||||
|
||||
//----------------------------------------------------------------
|
||||
// Procedure: showHelpAndExit
|
||||
|
||||
void showHelpAndExit()
|
||||
{
|
||||
blk(" ");
|
||||
blu("=============================================================== ");
|
||||
blu("Usage: pPMtest file.moos [OPTIONS] ");
|
||||
blu("=============================================================== ");
|
||||
blk(" ");
|
||||
showSynopsis();
|
||||
blk(" ");
|
||||
blk("Options: ");
|
||||
mag(" --alias","=<ProcessName> ");
|
||||
blk(" Launch pPMtest with the given process name ");
|
||||
blk(" rather than pPMtest. ");
|
||||
mag(" --example, -e ");
|
||||
blk(" Display example MOOS configuration block. ");
|
||||
mag(" --help, -h ");
|
||||
blk(" Display this help message. ");
|
||||
mag(" --interface, -i ");
|
||||
blk(" Display MOOS publications and subscriptions. ");
|
||||
mag(" --version,-v ");
|
||||
blk(" Display the release version of pPMtest. ");
|
||||
blk(" ");
|
||||
blk("Note: If argv[2] does not otherwise match a known option, ");
|
||||
blk(" then it will be interpreted as a run alias. This is ");
|
||||
blk(" to support pAntler launching conventions. ");
|
||||
blk(" ");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
//----------------------------------------------------------------
|
||||
// Procedure: showExampleConfigAndExit
|
||||
|
||||
void showExampleConfigAndExit()
|
||||
{
|
||||
blk(" ");
|
||||
blu("=============================================================== ");
|
||||
blu("pPMtest Example MOOS Configuration ");
|
||||
blu("=============================================================== ");
|
||||
blk(" ");
|
||||
blk("ProcessConfig = pPMtest ");
|
||||
blk("{ ");
|
||||
blk(" AppTick = 4 ");
|
||||
blk(" CommsTick = 4 ");
|
||||
blk(" ");
|
||||
blk("} ");
|
||||
blk(" ");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
||||
//----------------------------------------------------------------
|
||||
// Procedure: showInterfaceAndExit
|
||||
|
||||
void showInterfaceAndExit()
|
||||
{
|
||||
blk(" ");
|
||||
blu("=============================================================== ");
|
||||
blu("pPMtest INTERFACE ");
|
||||
blu("=============================================================== ");
|
||||
blk(" ");
|
||||
showSynopsis();
|
||||
blk(" ");
|
||||
blk("SUBSCRIPTIONS: ");
|
||||
blk("------------------------------------ ");
|
||||
blk(" NODE_MESSAGE = src_node=alpha,dest_node=bravo,var_name=FOO, ");
|
||||
blk(" string_val=BAR ");
|
||||
blk(" ");
|
||||
blk("PUBLICATIONS: ");
|
||||
blk("------------------------------------ ");
|
||||
blk(" Publications are determined by the node message content. ");
|
||||
blk(" ");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
//----------------------------------------------------------------
|
||||
// Procedure: showReleaseInfoAndExit
|
||||
|
||||
void showReleaseInfoAndExit()
|
||||
{
|
||||
showReleaseInfo("pPMtest", "gpl");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
/****************************************************************/
|
||||
/* NAME: */
|
||||
/* ORGN: MIT Cambridge MA */
|
||||
/* FILE: PMtest_Info.h */
|
||||
/* DATE: Dec 29th 1963 */
|
||||
/****************************************************************/
|
||||
|
||||
#ifndef PMtest_INFO_HEADER
|
||||
#define PMtest_INFO_HEADER
|
||||
|
||||
void showSynopsis();
|
||||
void showHelpAndExit();
|
||||
void showExampleConfigAndExit();
|
||||
void showInterfaceAndExit();
|
||||
void showReleaseInfoAndExit();
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,52 @@
|
||||
/************************************************************/
|
||||
/* NAME: */
|
||||
/* ORGN: MIT */
|
||||
/* FILE: main.cpp */
|
||||
/* DATE: */
|
||||
/************************************************************/
|
||||
|
||||
#include <string>
|
||||
#include "MBUtils.h"
|
||||
#include "ColorParse.h"
|
||||
#include "PMtest.h"
|
||||
#include "PMtest_Info.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
string mission_file;
|
||||
string run_command = argv[0];
|
||||
|
||||
for(int i=1; i<argc; i++) {
|
||||
string argi = argv[i];
|
||||
if((argi=="-v") || (argi=="--version") || (argi=="-version"))
|
||||
showReleaseInfoAndExit();
|
||||
else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
|
||||
showExampleConfigAndExit();
|
||||
else if((argi == "-h") || (argi == "--help") || (argi=="-help"))
|
||||
showHelpAndExit();
|
||||
else if((argi == "-i") || (argi == "--interface"))
|
||||
showInterfaceAndExit();
|
||||
else if(strEnds(argi, ".moos") || strEnds(argi, ".moos++"))
|
||||
mission_file = argv[i];
|
||||
else if(strBegins(argi, "--alias="))
|
||||
run_command = argi.substr(8);
|
||||
else if(i==2)
|
||||
run_command = argi;
|
||||
}
|
||||
|
||||
if(mission_file == "")
|
||||
showHelpAndExit();
|
||||
|
||||
cout << termColor("green");
|
||||
cout << "pPMtest launching as " << run_command << endl;
|
||||
cout << termColor() << endl;
|
||||
|
||||
PMtest PMtest;
|
||||
|
||||
PMtest.Run(run_command.c_str(), mission_file.c_str());
|
||||
|
||||
return(0);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,9 @@
|
||||
//------------------------------------------------
|
||||
// pPMtest config block
|
||||
|
||||
ProcessConfig = pPMtest
|
||||
{
|
||||
AppTick = 4
|
||||
CommsTick = 4
|
||||
}
|
||||
|
||||
@@ -28,6 +28,7 @@ SET(SRC
|
||||
driver.cpp
|
||||
LowerCommManager.cpp
|
||||
UpperCommManager.cpp
|
||||
loguru.cpp
|
||||
)
|
||||
|
||||
ADD_EXECUTABLE(pPowerManger ${SRC})
|
||||
|
||||
@@ -17,9 +17,19 @@ LowerCommManager::LowerCommManager(PowerManger* pm, long myPort, long ccuPort, c
|
||||
m_udpComm->ccuPort = ccuPort;
|
||||
m_udpComm->udpScoket = new XPCUdpSocket(m_port);
|
||||
|
||||
std::cout << "PORT = " << m_port << std::endl;
|
||||
std::cout << "CCUHOST = " << m_udpComm->ccuHost << std::endl;
|
||||
std::cout << "CCUPORT = " << m_udpComm->ccuPort << std::endl;
|
||||
std::cout << MOOS::ConsoleColours::yellow() << "╔════════════════════════╗" << MOOS::ConsoleColours::reset() << std::endl;
|
||||
std::cout << MOOS::ConsoleColours::yellow() << "║ [LOCAL PORT] "
|
||||
<< MOOS::ConsoleColours::cyan() << m_port
|
||||
<< MOOS::ConsoleColours::yellow() << std::setw(15) << " ║" << MOOS::ConsoleColours::reset() << std::endl;
|
||||
|
||||
std::cout << MOOS::ConsoleColours::yellow() << "║ [CCU HOST ] "
|
||||
<< MOOS::ConsoleColours::cyan() << m_udpComm->ccuHost
|
||||
<< MOOS::ConsoleColours::yellow() << std::setw(8) << " ║" << MOOS::ConsoleColours::reset() << std::endl;
|
||||
|
||||
std::cout << MOOS::ConsoleColours::yellow() << "║ [CCU PORT ] "
|
||||
<< MOOS::ConsoleColours::cyan() << m_udpComm->ccuPort
|
||||
<< MOOS::ConsoleColours::yellow() << std::setw(15) << " ║" << MOOS::ConsoleColours::reset() << std::endl;
|
||||
std::cout << MOOS::ConsoleColours::yellow() << "╚════════════════════════╝" << MOOS::ConsoleColours::reset() << std::endl;
|
||||
}
|
||||
|
||||
LowerCommManager::~LowerCommManager(){
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
#include "PowerManagerFsm.hpp"
|
||||
#include "PowerManger.h"
|
||||
#include "loguru.hpp"
|
||||
#include <string>
|
||||
#include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h"
|
||||
|
||||
@@ -9,9 +10,8 @@ PowerManger* PowerManagerFsm::pm = nullptr;
|
||||
// InitState实现
|
||||
void InitState::entry() {
|
||||
PowerManagerFsm::entry();
|
||||
std::cout << "初始化配电指令" << std::endl;
|
||||
LOG_F(INFO, "进入初始化状态");
|
||||
pm->initControlCmd();
|
||||
// PowerManagerFsm::pm->printState("InitState", "进入初始化状态");
|
||||
transit<StandbyState>();
|
||||
}
|
||||
|
||||
@@ -21,18 +21,16 @@ void StandbyState::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<StandbyState>();
|
||||
}
|
||||
|
||||
|
||||
void StandbyState::handlePowerCommands() {
|
||||
switch (PowerManagerFsm::pm->m_msCmd.powerCMD) {
|
||||
case 0: PowerManagerFsm::pm->m_pmEvent = "收到上位机控制指令:关机"; break;
|
||||
case 1: handlePowerOn(); break;
|
||||
}
|
||||
void StandbyState::react(MasterCommandEvent const &) {
|
||||
PowerManagerFsm::handelPowerCmd();
|
||||
PowerManagerFsm::handleModCmd();
|
||||
PowerManagerFsm::handleWorkCmd();
|
||||
}
|
||||
|
||||
void StandbyState::handlePowerOn() {
|
||||
@@ -61,5 +59,331 @@ void FaultState::react(FaultEvent const & e) {
|
||||
}
|
||||
}
|
||||
|
||||
void PowerManagerFsm::handelPowerCmd(){
|
||||
switch (pm->m_msCmd.powerCMD)
|
||||
{
|
||||
case 0:
|
||||
LOG_F(INFO, "收到上位机控制指令:关机");
|
||||
break;
|
||||
case 1:
|
||||
LOG_F(INFO, "收到上位机控制指令:动力电上电");
|
||||
break;
|
||||
case 2:
|
||||
LOG_F(INFO, "收到上位机控制指令:锂电池启动");
|
||||
break;
|
||||
case 3:
|
||||
LOG_F(INFO, "收到上位机控制指令:燃料电池");
|
||||
break;
|
||||
case 4:
|
||||
LOG_F(INFO, "收到上位机控制指令:锂动力电关闭");
|
||||
break;
|
||||
case 5:
|
||||
LOG_F(INFO, "收到上位机控制指令:燃料电池停机");
|
||||
break;
|
||||
case 6:
|
||||
LOG_F(INFO, "收到上位机控制指令:燃料电池预热指令");
|
||||
break;
|
||||
case 7:
|
||||
LOG_F(INFO, "收到上位机控制指令:燃料电池自检指令");
|
||||
break;
|
||||
case 8:
|
||||
LOG_F(INFO, "收到上位机控制指令:燃料电池紧急停机");
|
||||
break;
|
||||
case 9:
|
||||
LOG_F(INFO, "收到上位机控制指令:燃料电池复位指令");
|
||||
break;
|
||||
case 10:
|
||||
LOG_F(INFO, "收到上位机控制指令:燃料电池补给指令");
|
||||
break;
|
||||
case 11:
|
||||
LOG_F(INFO, "收到上位机控制指令:燃料电池停机维护");
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void PowerManagerFsm::handleWorkCmd(){
|
||||
switch (pm->m_msCmd.workCMD)
|
||||
{
|
||||
case 1:
|
||||
LOG_F(INFO, "收到上位机控制指令:待机工况");
|
||||
transit<StandbyState>();
|
||||
break;
|
||||
case 2:
|
||||
LOG_F(INFO, "收到上位机控制指令:岸基备航");
|
||||
transit<ShoreBasedReady>();
|
||||
break;
|
||||
case 3:
|
||||
LOG_F(INFO, "收到上位机控制指令:水中备航");
|
||||
transit<WaterBasedReady>();
|
||||
break;
|
||||
case 4:
|
||||
LOG_F(INFO, "收到上位机控制指令:遥控工况");
|
||||
transit<RemoteControl>();
|
||||
break;
|
||||
case 5:
|
||||
LOG_F(INFO, "收到上位机控制指令:巡航模式");
|
||||
transit<CruiseMode>();
|
||||
break;
|
||||
case 6:
|
||||
LOG_F(INFO, "收到上位机控制指令:高速模式");
|
||||
transit<HighSpeedMode>();
|
||||
break;
|
||||
case 7:
|
||||
LOG_F(INFO, "收到上位机控制指令:上浮下潜");
|
||||
transit<FloatDown>();
|
||||
break;
|
||||
case 8:
|
||||
LOG_F(INFO, "收到上位机控制指令:浮调模式");
|
||||
transit<FloatAdjust>();
|
||||
break;
|
||||
case 9:
|
||||
LOG_F(INFO, "收到上位机控制指令:水下侦查");
|
||||
transit<UnderwaterSurvey>();
|
||||
break;
|
||||
case 10:
|
||||
LOG_F(INFO, "收到上位机控制指令:水面侦查");
|
||||
transit<SurfaceSurvey>();
|
||||
break;
|
||||
case 11:
|
||||
LOG_F(INFO, "收到上位机控制指令:DJ模式");
|
||||
transit<DJMode>();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void PowerManagerFsm::handleModCmd(){
|
||||
switch (pm->m_msCmd.modCMD)
|
||||
{
|
||||
case 0:
|
||||
LOG_F(INFO, "收到上位机控制指令:正常模式");
|
||||
pm->currentMod = PMMode::Normal;
|
||||
break;
|
||||
case 1:
|
||||
LOG_F(INFO, "收到上位机控制指令:试验模式");
|
||||
pm->currentMod = PMMode::Test;
|
||||
break;
|
||||
case 2:
|
||||
LOG_F(INFO, "收到上位机控制指令:调试模式");
|
||||
pm->currentMod = PMMode::Debug;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
LOG_F(INFO, "当前工作模式: %d", pm->currentMod);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// 设置初始状态
|
||||
// ShoreBasedReady实现
|
||||
void ShoreBasedReady::react(MasterCommandEvent const &) {
|
||||
PowerManagerFsm::handelPowerCmd();
|
||||
PowerManagerFsm::handleModCmd();
|
||||
PowerManagerFsm::handleWorkCmd();
|
||||
}
|
||||
|
||||
void ShoreBasedReady::entry() {
|
||||
PowerManagerFsm::entry();
|
||||
pm->workCondition = WorkCondition::SHORE_BASED_READY;
|
||||
LOG_F(INFO, "进入岸基备航状态");
|
||||
}
|
||||
|
||||
// WaterBasedReady实现
|
||||
void WaterBasedReady::react(MasterCommandEvent const &) {
|
||||
PowerManagerFsm::handelPowerCmd();
|
||||
PowerManagerFsm::handleModCmd();
|
||||
PowerManagerFsm::handleWorkCmd();
|
||||
}
|
||||
|
||||
void WaterBasedReady::entry() {
|
||||
PowerManagerFsm::entry();
|
||||
pm->workCondition = WorkCondition::WATER_BASED_READY;
|
||||
LOG_F(INFO, "进入水中备航状态");
|
||||
}
|
||||
|
||||
// CruiseMode实现
|
||||
void CruiseMode::react(MasterCommandEvent const &) {
|
||||
PowerManagerFsm::handelPowerCmd();
|
||||
PowerManagerFsm::handleModCmd();
|
||||
PowerManagerFsm::handleWorkCmd();
|
||||
}
|
||||
|
||||
void CruiseMode::entry() {
|
||||
PowerManagerFsm::entry();
|
||||
pm->workCondition = WorkCondition::CRUISE_MODE;
|
||||
LOG_F(INFO, "进入巡航模式");
|
||||
}
|
||||
|
||||
// HighSpeedMode实现
|
||||
void HighSpeedMode::react(MasterCommandEvent const &) {
|
||||
PowerManagerFsm::handelPowerCmd();
|
||||
PowerManagerFsm::handleModCmd();
|
||||
PowerManagerFsm::handleWorkCmd();
|
||||
}
|
||||
|
||||
void HighSpeedMode::entry() {
|
||||
PowerManagerFsm::entry();
|
||||
pm->workCondition = WorkCondition::HIGH_SPEED_MODE;
|
||||
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();
|
||||
PowerManagerFsm::handleModCmd();
|
||||
PowerManagerFsm::handleWorkCmd();
|
||||
}
|
||||
|
||||
void FloatAdjust::entry() {
|
||||
PowerManagerFsm::entry();
|
||||
pm->workCondition = WorkCondition::BUOYANCY_ADJUST;
|
||||
LOG_F(INFO, "进入浮调模式");
|
||||
}
|
||||
|
||||
// UnderwaterSurvey实现
|
||||
void UnderwaterSurvey::react(MasterCommandEvent const &) {
|
||||
PowerManagerFsm::handelPowerCmd();
|
||||
PowerManagerFsm::handleModCmd();
|
||||
PowerManagerFsm::handleWorkCmd();
|
||||
}
|
||||
|
||||
void UnderwaterSurvey::entry() {
|
||||
PowerManagerFsm::entry();
|
||||
pm->workCondition = WorkCondition::UNDERWATER_RECON;
|
||||
LOG_F(INFO, "进入水下侦查模式");
|
||||
}
|
||||
|
||||
// SurfaceSurvey实现
|
||||
void SurfaceSurvey::react(MasterCommandEvent const &) {
|
||||
PowerManagerFsm::handelPowerCmd();
|
||||
PowerManagerFsm::handleModCmd();
|
||||
PowerManagerFsm::handleWorkCmd();
|
||||
}
|
||||
|
||||
void SurfaceSurvey::entry() {
|
||||
PowerManagerFsm::entry();
|
||||
pm->workCondition = WorkCondition::SURFACE_RECON;
|
||||
LOG_F(INFO, "进入水面侦查模式");
|
||||
}
|
||||
|
||||
// DJMode实现
|
||||
void DJMode::react(MasterCommandEvent const &) {
|
||||
PowerManagerFsm::handelPowerCmd();
|
||||
PowerManagerFsm::handleModCmd();
|
||||
PowerManagerFsm::handleWorkCmd();
|
||||
}
|
||||
|
||||
void DJMode::entry() {
|
||||
PowerManagerFsm::entry();
|
||||
pm->workCondition = WorkCondition::DJ_MODE;
|
||||
LOG_F(INFO, "进入DJ模式");
|
||||
}
|
||||
|
||||
// RemoteControl实现
|
||||
void RemoteControl::react(MasterCommandEvent const &) {
|
||||
PowerManagerFsm::handelPowerCmd();
|
||||
PowerManagerFsm::handleModCmd();
|
||||
PowerManagerFsm::handleWorkCmd();
|
||||
}
|
||||
|
||||
void RemoteControl::entry() {
|
||||
PowerManagerFsm::entry();
|
||||
pm->workCondition = WorkCondition::REMOTE_CONTROL;
|
||||
LOG_F(INFO, "进入遥控工况");
|
||||
}
|
||||
|
||||
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 RemoteControl::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 FloatAdjust::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, "进入水面侦查模式");
|
||||
}
|
||||
|
||||
//DJ模式
|
||||
void DJMode::react(TaskStart const &)
|
||||
{
|
||||
LOG_F(INFO, "接收到任务启动信息");
|
||||
LOG_F(INFO, "进入DJ模式");
|
||||
}
|
||||
|
||||
// 上浮下潜
|
||||
void FloatDown::react(TaskStart const &)
|
||||
{
|
||||
LOG_F(INFO, "接收到任务启动信息");
|
||||
LOG_F(INFO, "进入上浮下潜模式");
|
||||
}
|
||||
|
||||
FSM_INITIAL_STATE(PowerManagerFsm, InitState);
|
||||
@@ -1,6 +1,7 @@
|
||||
#ifndef POWER_MANAGER_FSM_HPP
|
||||
#define POWER_MANAGER_FSM_HPP
|
||||
#include "tinyfsm.hpp"
|
||||
#include "loguru.hpp"
|
||||
#include <iostream>
|
||||
#include <chrono> // 新增头文件
|
||||
#include <iomanip> // 新增头文件
|
||||
@@ -43,9 +44,14 @@ public:
|
||||
virtual void react(InitEvent const &) {};
|
||||
virtual void react(StandbyEvent const &) { };
|
||||
virtual void react(FaultEvent const &) { };
|
||||
virtual void react(MasterCommandEvent const &) {};
|
||||
virtual void react(TaskStart const &) {};
|
||||
virtual void react(DeviceCmd const &) {};
|
||||
virtual void react(MasterCommandEvent const &) { std::cout << "[ Reseve Envt ] " << "No Idear" << std::endl; };
|
||||
virtual void react(TaskStart const &) { std::cout << "[ Reseve Envt ] " << "No Idear" << std::endl; };
|
||||
virtual void react(DeviceCmd const &) { std::cout << "[ Reseve Envt ] " << "No Idear" << std::endl; };
|
||||
|
||||
//操作函数
|
||||
void handelPowerCmd();
|
||||
void handleModCmd();
|
||||
void handleWorkCmd();
|
||||
|
||||
// 状态机生命周期方法
|
||||
virtual void entry() {
|
||||
@@ -61,6 +67,8 @@ public:
|
||||
<< "│ \033[1;38;5;226m⮞⮞⮞ \033[0;38;5;51m[" << std::put_time(std::localtime(&now_time), "%Y-%m-%d %H:%M:%S")
|
||||
<< "] \033[1;38;5;255m进入状态: \033[1;38;5;213m" << typeName << "\n"
|
||||
<< "│ \033[0m" << std::endl;
|
||||
|
||||
LOG_F(INFO, "进入状态: %s", typeName.c_str());
|
||||
}
|
||||
|
||||
virtual void exit() {
|
||||
@@ -76,26 +84,137 @@ public:
|
||||
<< "│ \033[1;38;5;208m⮜⮜⮜ \033[0;38;5;203m[" << std::put_time(std::localtime(&now_time), "%Y-%m-%d %H:%M:%S")
|
||||
<< "] \033[1;38;5;255m离开状态: \033[1;38;5;213m" << typeName << "\n"
|
||||
<< "╰──────────────────────────────────────────────────────\033[0m" << std::endl;
|
||||
|
||||
LOG_F(INFO, "离开状态: %s", typeName.c_str());
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
// 初始化
|
||||
class InitState : public PowerManagerFsm {
|
||||
public:
|
||||
void entry() override;
|
||||
};
|
||||
|
||||
//待机状态
|
||||
class StandbyState : public PowerManagerFsm {
|
||||
public:
|
||||
void entry() override;
|
||||
void react(StandbyEvent const &) override;
|
||||
// void react(MasterCommandEvent const &) override;
|
||||
void react(MasterCommandEvent const &) override;
|
||||
void react(TaskStart const &) override;
|
||||
// void react(DeviceCmd const &) override;
|
||||
private:
|
||||
void handlePowerCommands();
|
||||
void handlePowerOn();
|
||||
};
|
||||
|
||||
// 岸基备航
|
||||
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();
|
||||
};
|
||||
// 水中备航
|
||||
class WaterBasedReady : public PowerManagerFsm {
|
||||
public:
|
||||
void entry() override;
|
||||
void react(MasterCommandEvent const &) override;
|
||||
// void react(DeviceCmd const &) override;
|
||||
void react(TaskStart const &) override;
|
||||
private:
|
||||
// void handlePowerCommands();
|
||||
// void handlePowerOn();
|
||||
};
|
||||
// 巡航模式
|
||||
class CruiseMode : public PowerManagerFsm {
|
||||
public:
|
||||
void entry() override;
|
||||
void react(MasterCommandEvent const &) override;
|
||||
// void react(DeviceCmd const &) override;
|
||||
void react(TaskStart const &) override;
|
||||
private:
|
||||
// void handlePowerCommands();
|
||||
};
|
||||
|
||||
// 高速模式
|
||||
class HighSpeedMode : public PowerManagerFsm {
|
||||
public:
|
||||
void entry() override;
|
||||
void react(MasterCommandEvent const &) override;
|
||||
// void react(DeviceCmd const &) override;
|
||||
void react(TaskStart const &) override;
|
||||
private:
|
||||
// void handlePowerCommands();
|
||||
};
|
||||
// 上浮下潜
|
||||
class FloatDown : public PowerManagerFsm {
|
||||
public:
|
||||
void entry() override;
|
||||
void react(MasterCommandEvent const &) override;
|
||||
// void react(DeviceCmd const &) override;
|
||||
void react(TaskStart const &) override;
|
||||
private:
|
||||
// void handlePowerCommands();
|
||||
};
|
||||
// 浮调模式
|
||||
class FloatAdjust : public PowerManagerFsm {
|
||||
public:
|
||||
void entry() override;
|
||||
void react(MasterCommandEvent const &) override;
|
||||
// void react(DeviceCmd const &) override;
|
||||
void react(TaskStart const &) override;
|
||||
private:
|
||||
// void handlePowerCommands();
|
||||
};
|
||||
// 水下侦查
|
||||
class UnderwaterSurvey : public PowerManagerFsm {
|
||||
public:
|
||||
void entry() override;
|
||||
void react(MasterCommandEvent const &) override;
|
||||
// void react(DeviceCmd const &) override;
|
||||
void react(TaskStart const &) override;
|
||||
private:
|
||||
// void handlePowerCommands();
|
||||
|
||||
};
|
||||
// 水面侦查
|
||||
class SurfaceSurvey : public PowerManagerFsm {
|
||||
public:
|
||||
void entry() override;
|
||||
void react(MasterCommandEvent const &) override;
|
||||
// void react(DeviceCmd const &) override;
|
||||
void react(TaskStart const &) override;
|
||||
private:
|
||||
// void handlePowerCommands();
|
||||
|
||||
};
|
||||
// DJ模式
|
||||
class DJMode : public PowerManagerFsm {
|
||||
public:
|
||||
void entry() override;
|
||||
void react(MasterCommandEvent const &) override;
|
||||
// void react(DeviceCmd const &) override;
|
||||
void react(TaskStart const &) override;
|
||||
private:
|
||||
// void handlePowerCommands();
|
||||
|
||||
};
|
||||
// 遥控工况
|
||||
class RemoteControl : public PowerManagerFsm {
|
||||
public:
|
||||
void entry() override;
|
||||
void react(MasterCommandEvent const &) override;
|
||||
// void react(DeviceCmd const &) override;
|
||||
void react(TaskStart const &) override;
|
||||
private:
|
||||
// void handlePowerCommands();
|
||||
};
|
||||
|
||||
class FaultState : public PowerManagerFsm {
|
||||
public:
|
||||
void entry() override;
|
||||
|
||||
@@ -9,6 +9,7 @@
|
||||
#include "MBUtils.h"
|
||||
#include "ACTable.h"
|
||||
#include "PowerManger.h"
|
||||
#include "loguru.hpp"
|
||||
#include <memory>
|
||||
|
||||
using namespace std;
|
||||
@@ -83,7 +84,7 @@ bool PowerManger::OnNewMail(MOOSMSG_LIST &NewMail)
|
||||
//处理上位机发送的控制指令
|
||||
if(!m_upperCommManager->processUpperMsg(key,strJson))
|
||||
{
|
||||
cout << "processUpperMsg failed" << endl;
|
||||
LOG_F(ERROR, "上位机消息处理错误: %s", key.c_str());
|
||||
}
|
||||
|
||||
#if 0 // Keep these around just for template
|
||||
@@ -97,7 +98,7 @@ bool PowerManger::OnNewMail(MOOSMSG_LIST &NewMail)
|
||||
#endif
|
||||
|
||||
if(key == "FOO")
|
||||
cout << "great!";
|
||||
LOG_F(INFO, "Received FOO message");
|
||||
|
||||
else if(key != "APPCAST_REQ") // handled by AppCastingMOOSApp
|
||||
reportRunWarning("Unhandled Mail: " + key);
|
||||
@@ -193,6 +194,8 @@ void PowerManger::registerVariables()
|
||||
Register(DB_TO_PM_MSCMD, 0);
|
||||
Register(DB_TO_PM_EQUIPMENTCMD, 0);
|
||||
Register(DB_TO_PM_SUBDISSTATE, 0);
|
||||
Register(DB_TO_PM_MISSION,0);
|
||||
Register(DB_TO_PM_DEEPTH,0);
|
||||
}
|
||||
|
||||
|
||||
@@ -266,14 +269,11 @@ int PowerManger::checkError()
|
||||
}
|
||||
|
||||
void PowerManger::printState(const std::string& stateName, const std::string& desp){
|
||||
cout << MOOS::ConsoleColours::Yellow() << "STATE " << stateName << " " << MOOS::ConsoleColours::reset();
|
||||
cout << MOOS::ConsoleColours::Blue() << " -> " << MOOS::ConsoleColours::reset();
|
||||
// 检查desp是否有“故障”
|
||||
if (desp.find("故障") != string::npos) {
|
||||
cout << MOOS::ConsoleColours::Red() << desp << MOOS::ConsoleColours::reset() << endl;
|
||||
return;
|
||||
LOG_F(ERROR, "STATE %s -> %s", stateName.c_str(), desp.c_str());
|
||||
} else {
|
||||
LOG_F(INFO, "STATE %s -> %s", stateName.c_str(), desp.c_str());
|
||||
}
|
||||
cout << MOOS::ConsoleColours::Green() << desp << MOOS::ConsoleColours::reset() << endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -21,6 +21,7 @@
|
||||
#define JSON_USE_EXCEPTION 1
|
||||
|
||||
#include <queue>
|
||||
#include "loguru.hpp"
|
||||
#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
|
||||
#include "MOOS/libMOOS/Utils/MOOSThread.h"
|
||||
// #include "MOOS/libMOOS/Comms/XPCUdpSocket.h"
|
||||
@@ -86,7 +87,7 @@ class PowerManger : public AppCastingMOOSApp
|
||||
DriverTable m_subDisSysState;
|
||||
|
||||
int m_depth;
|
||||
int m_missionCode;
|
||||
MissionState m_missionState;
|
||||
//===========================STATE=========================================
|
||||
unsigned char workCondition;
|
||||
unsigned char currentMod;
|
||||
@@ -125,8 +126,6 @@ class PowerManger : public AppCastingMOOSApp
|
||||
//==================================================================================
|
||||
int checkError();
|
||||
bool shutdown();
|
||||
bool powerOn();
|
||||
bool powerOff();
|
||||
|
||||
bool sendNotification(const std::string& key, const std::string& value) {
|
||||
return Notify(key, value);
|
||||
|
||||
@@ -1,12 +1,16 @@
|
||||
#include "UpperCommManager.h"
|
||||
#include "PowerManger.h"
|
||||
#include "driver.h"
|
||||
#include "PowerManagerFsm.hpp"
|
||||
|
||||
UpperCommManager::UpperCommManager(PowerManger* pm) : m_pm(pm) {}
|
||||
|
||||
bool UpperCommManager::processUpperMsg(const string& key, const string& strJson) {
|
||||
if(key == DB_TO_PM_MSCMD) {
|
||||
m_pm->m_msCmd = getMasterControlCmd(key, strJson);
|
||||
cout << "m_msCmd: " << endl;
|
||||
MasterCommandEvent event;
|
||||
PowerManagerFsm::dispatch(event);
|
||||
return true;
|
||||
}
|
||||
else if(key == DB_TO_PM_EQUIPMENTCMD) {
|
||||
@@ -21,6 +25,10 @@ bool UpperCommManager::processUpperMsg(const string& key, const string& strJson)
|
||||
m_pm->m_depth = getDeepth(key, strJson);
|
||||
return true;
|
||||
}
|
||||
else if(key == DB_TO_PM_MISSION) {
|
||||
m_pm->m_missionState = getMission(key, strJson);
|
||||
return true;
|
||||
}
|
||||
std::cout << "Unknown key: " << key << std::endl;
|
||||
return false;
|
||||
}
|
||||
@@ -89,6 +97,42 @@ const int UpperCommManager::getDeepth(const std::string& key, const std::string&
|
||||
return deepth;
|
||||
}
|
||||
|
||||
MissionState UpperCommManager::getMission(const std::string& key, const std::string& msg) {
|
||||
MissionState mission;
|
||||
Json::Value root;
|
||||
Json::CharReaderBuilder builder;
|
||||
JSONCPP_STRING errs;
|
||||
|
||||
// 解析JSON字符串
|
||||
std::istringstream stream(msg);
|
||||
if (!Json::parseFromStream(builder, stream, &root, &errs)) {
|
||||
std::cerr << "JSON解析失败: " << errs << std::endl;
|
||||
LOG_F(WARNING, "JSON解析失败: %s", errs.c_str());
|
||||
return mission;
|
||||
}
|
||||
|
||||
// 提取taskType_str
|
||||
if (root.isMember("taskType_str") && root["taskType_str"].isString()) {
|
||||
mission.type = TaskType::fromString(root["taskType_str"].asString());
|
||||
LOG_F(INFO, "taskType: %d", mission.type, root["taskType_str"].asString());
|
||||
}
|
||||
|
||||
// 提取balanceEnabled_bool
|
||||
if (root.isMember("balanceEnabled_bool") && root["balanceEnabled_bool"].isBool()) {
|
||||
mission.enableBuoyancyAdjustment = root["balanceEnabled_bool"].asBool();
|
||||
LOG_F(INFO, "balanceEnabled_bool: %d", mission.enableBuoyancyAdjustment);
|
||||
}
|
||||
|
||||
// 提取bowRudderUnfold_bool
|
||||
if (root.isMember("bowRudderUnfold_bool") && root["bowRudderUnfold_bool"].isBool()) {
|
||||
mission.enableBowRudder = root["bowRudderUnfold_bool"].asBool();
|
||||
LOG_F(INFO, "bowRudderUnfold_bool: %d", mission.enableBowRudder);
|
||||
}
|
||||
|
||||
|
||||
return mission;
|
||||
}
|
||||
|
||||
const std::string UpperCommManager::getCurrentEquState(DriverTable& driver)
|
||||
{
|
||||
return m_pm->m_driver.saveDriverTableToJson(&driver);
|
||||
|
||||
@@ -50,6 +50,7 @@ public:
|
||||
const DriverTable getEquipmentCmd(const std::string& key, const std::string& msg);
|
||||
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);
|
||||
|
||||
|
||||
const std::string getCurrentEquState(DriverTable& driver);
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -10,11 +10,16 @@
|
||||
#include "ColorParse.h"
|
||||
#include "PowerManger.h"
|
||||
#include "PowerManger_Info.h"
|
||||
#include "loguru.hpp"
|
||||
|
||||
using namespace std;
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
// 初始化loguru
|
||||
loguru::init(argc, argv);
|
||||
loguru::add_file("pPowerManger.log", loguru::Append, loguru::Verbosity_MAX);
|
||||
|
||||
string mission_file;
|
||||
string run_command = argv[0];
|
||||
|
||||
@@ -39,9 +44,7 @@ int main(int argc, char *argv[])
|
||||
if(mission_file == "")
|
||||
showHelpAndExit();
|
||||
|
||||
cout << termColor("green");
|
||||
cout << "pPowerManger launching as " << run_command << endl;
|
||||
cout << termColor() << endl;
|
||||
LOG_F(INFO, "pPowerManger launching as %s", run_command.c_str());
|
||||
|
||||
PowerManger PowerManger;
|
||||
|
||||
|
||||
@@ -2,6 +2,8 @@
|
||||
#define PMSYSVARIABLE_H
|
||||
|
||||
//宏定义
|
||||
#include<string>
|
||||
#include <map>
|
||||
|
||||
|
||||
#define BREAK_OFF 0xAA
|
||||
@@ -408,6 +410,89 @@ namespace BatteryCommand {
|
||||
Start = 0x10,
|
||||
Stop = 0x20
|
||||
};
|
||||
};
|
||||
|
||||
namespace PMMode {
|
||||
enum Type : unsigned char {
|
||||
Normal = 0,
|
||||
Test = 1,
|
||||
Debug = 2
|
||||
};
|
||||
};
|
||||
|
||||
//工况定义
|
||||
namespace WorkCondition {
|
||||
enum Type : unsigned char {
|
||||
STANDBY = 1, // 待机工况
|
||||
SHORE_BASED_READY = 2, // 岸基备航
|
||||
WATER_BASED_READY = 3, // 水中备航
|
||||
REMOTE_CONTROL = 4, // 遥控工况
|
||||
CRUISE_MODE = 5, // 巡航模式
|
||||
HIGH_SPEED_MODE = 6, // 高速模式
|
||||
ASCEND_DESCEND = 7, // 上浮下潜
|
||||
BUOYANCY_ADJUST = 8, // 浮调模式
|
||||
UNDERWATER_RECON = 9, // 水下侦查
|
||||
SURFACE_RECON = 10, // 水面侦查
|
||||
DJ_MODE = 11, // DJ模式
|
||||
DEBUG_MODE = 12 // 调试模式
|
||||
};
|
||||
};
|
||||
// 抛载 throwLoad
|
||||
// 运动上浮 floatUp
|
||||
// 回收 recycle
|
||||
// 悬停 hover
|
||||
// 上浮通信 satComm
|
||||
// 上浮校准 satCalibrate
|
||||
// 游曳 tour
|
||||
// 航行 sail
|
||||
namespace TaskType {
|
||||
enum Type : unsigned char {
|
||||
THROW_LOAD = 1, // 抛载
|
||||
FLOAT_UP = 2, // 运动上浮
|
||||
RECYCLE = 3, // 回收
|
||||
HOVER = 4, // 悬停
|
||||
SAT_COMM = 5, // 上浮通信
|
||||
SAT_CALIBRATE = 6, // 上浮校准
|
||||
TOUR = 7, // 游曳
|
||||
SAIL = 8 // 航行
|
||||
};
|
||||
|
||||
inline const std::string toString(Type type) {
|
||||
static const std::map<Type, std::string> names = {
|
||||
{THROW_LOAD, "throwLoad"},
|
||||
{FLOAT_UP, "floatUp"},
|
||||
{RECYCLE, "recycle"},
|
||||
{HOVER, "hover"},
|
||||
{SAT_COMM, "satComm"},
|
||||
{SAT_CALIBRATE, "satCalibrate"},
|
||||
{TOUR, "tour"},
|
||||
{SAIL, "sail"}
|
||||
};
|
||||
auto it = names.find(type);
|
||||
return it != names.end() ? it->second : "unknown";
|
||||
}
|
||||
inline Type fromString(const std::string& str) {
|
||||
static const std::map<std::string, Type> stringToEnum = {
|
||||
{"throwLoad", THROW_LOAD},
|
||||
{"floatUp", FLOAT_UP},
|
||||
{"recycle", RECYCLE},
|
||||
{"hover", HOVER},
|
||||
{"satComm", SAT_COMM},
|
||||
{"satCalibrate", SAT_CALIBRATE},
|
||||
{"tour", TOUR},
|
||||
{"sail", SAIL}
|
||||
};
|
||||
auto it = stringToEnum.find(str);
|
||||
return it != stringToEnum.end() ? it->second : static_cast<Type>(0); // 返回0表示无效类型
|
||||
}
|
||||
}
|
||||
|
||||
struct MissionState
|
||||
{
|
||||
int type;
|
||||
bool enableBowRudder;
|
||||
bool enableBuoyancyAdjustment;
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user