上传文件至 launch

岸基程序模版

launch/launch.moos

@@ -273,7 +273,7 @@ ProcessConfig = pMotionControler
   cheak_stalensee = true
   delta_freqency = 5
   
-  config_file = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/ControlParam.json
+  config_file = /home/jhl/project/moos-ivp-pi/setting/ControlParam.json
 }
 
 ProcessConfig = pEmulator

launch/launch_local.moos

@@ -0,0 +1,328 @@
+//======================================
+//1. 在phare配置块里面添加需要的变量和调试端端口配置
+//2. Our define process中加入自定义程序
+//3. For test process 中加入配合调试的程序
+
+ServerHost = localhost
+ServerPort = 9000
+Simulator  = true
+
+Community  = pi
+
+MOOSTimeWarp = 1
+
+LatOrigin  = 43.825300 
+LongOrigin = -70.330400 
+AltOrigin = 0
+
+VehicleName = lauv-150
+
+LogEnable = false
+//LogDir = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/log/
+LogDir = /home/ben/project/moos-ivp-pi/log/
+AuvDataLog = auvData.mdat
+MissionHistoryLog = missionHistory.txt
+ClientCommandLog = clientCommand.txt
+FaultLog = faultLog.txt
+MotionControlLog = motionControl.txt
+
+//llaOriginPath = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/Origin.json
+llaOriginPath = /home/ben/project/moos-ivp-pi/setting/Origin.json
+
+//------------------------------------------
+// Antler configuration  block
+ProcessConfig = ANTLER
+{
+  MSBetweenLaunches = 200
+  //============MOOS process=========================
+  Run = MOOSDB		          @ NewConsole = false
+  Run = pHelmIvP	          @ NewConsole = false
+  Run = pNodeReporter	      @ NewConsole = false
+  Run = uProcessWatch         @ NewConsole = false
+  Run = pRealm                @ NewConsole = false 
+  //Run = pShare                @ NewConsole = false
+  Run = pMarineViewer	        @ NewConsole = false
+  //===========Our define process====================
+  Run = pBoardSupportComm	    @ NewConsole = false  
+  Run = pTaskManagement             @ NewConsole = false 
+  Run = pMotionControler        @ NewConsole = false 
+  Run = pSurfaceSupportComm   @ NewConsole = false
+  Run = pDataManagement   @ NewConsole = false
+  Run = pFaultHandle               @ NewConsole = false
+  Run = pStateManagement            @ NewConsole = false  
+  //===============For test process===================
+  Run = pEmulator               @ NewConsole = false
+  //Run = uSimMarine	          @ NewConsole = false
+  //Run = pMarinePID	          @ NewConsole = false
+
+}
+
+ProcessConfig = pMarineViewer
+{
+  AppTick    = 4
+  CommsTick  = 4
+
+  tiff_file            = forrest19.tif
+	//tiff_file            = MIT_SP.tif
+  vehicles_name_mode = names+depth //+shortmode
+
+
+  set_pan_x            = -90
+  set_pan_y            = -280
+  zoom                 = 0.65
+  vehicle_shape_scale  = 1.5
+  hash_delta           = 50
+  hash_shade           = 0.22
+  hash_viewable        = true
+
+  trails_point_size   = 1
+
+	//op_vertex = x=-83, y=-47,    lwidth=1,lcolor=yellow,looped=true,group=moa
+  //op_vertex = x=-46.4, y=-129.2, lwidth=1,lcolor=yellow,looped=true,group=moa
+  //op_vertex = x=94.6, y=-62.2,   lwidth=1,lcolor=yellow,looped=true,group=moa
+  //op_vertex = x=58,   y=20,    lwidth=1,lcolor=yellow,looped=true,group=moa
+
+  // Appcast configuration
+  appcast_height       = 75
+  appcast_width        = 30
+  appcast_viewable     = true
+  appcast_color_scheme = indigo
+  nodes_font_size      = xlarge
+  procs_font_size      = xlarge
+  appcast_font_size    = large
+  
+  // datum_viewable = true
+  // datum_size     = 18
+  // gui_size = small
+
+  // left_context[survey-point] = DEPLOY=true
+  // left_context[survey-point] = MOOS_MANUAL_OVERRIDE=false
+  // left_context[survey-point] = RETURN=false
+
+  right_context[return] = DEPLOY=true
+  right_context[return] = MOOS_MANUAL_OVERRIDE=false
+  right_context[return] = RETURN=false
+
+  scope  = RETURN
+  scope  = WPT_STAT
+  scope  = VIEW_SEGLIST
+  scope  = VIEW_POINT
+  scope  = VIEW_POLYGON
+  scope  = MVIEWER_LCLICK
+  scope  = MVIEWER_RCLICK
+
+  //button_one = START # START=true
+  button_one = START # uMission_action_cmd={"taskName":"east_waypt_survey","action":"start"}
+  //button_one = MOOS_MANUAL_OVERRIDE=false
+  button_two = STOP # uMission_action_cmd={"taskName":"east_waypt_survey","action":"stop"}
+  //button_two = MOOS_MANUAL_OVERRIDE=true
+  //button_three = FaultClear # ClearFalut = true
+  //button_four = SendSecurityZone # SendSaftRules = true
+
+
+  action  = MENU_KEY=deploy # DEPLOY = true # RETURN = false
+  action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false
+  action  = RETURN=true
+  action  = UPDATES_RETURN=speed=1.4
+  cmd = label=DEPLOY, var=DEPLOY,  sval=true,  receivers=all:$(VNAMES)
+  cmd = label=DEPLOY, var=MOOS_MANUAL_OVERRIDE,  sval=false,  receivers=all:$(VNAMES)
+  cmd = label=DEPLOY, var=AVOID,  sval=true,  receivers=all:$(VNAMES)
+  cmd = label=DEPLOY, var=RETURN,  sval=false,  receivers=all:$(VNAMES)
+  cmd = label=DEPLOY, var=STATION_KEEP,  sval=false,  receivers=all:$(VNAMES)
+
+  cmd = label=RETURN, var=RETURN, sval=true, receivers=all:$(VNAMES)
+  cmd = label=RETURN, var=STATION_KEEP, sval=false, receivers=all:$(VNAMES)
+
+  cmd = label=PERMUTE, var=UTS_FORWARD,  dval=0,  receivers=shore
+
+  cmd = label=STATION, var=STATION_KEEP,  sval=true,  receivers=all:$(VNAMES), color=pink
+
+  cmd = label=LOITER-FAST, var=UP_LOITER,  sval=speed=2.8,  receivers=all:$(VNAMES)
+  cmd = label=LOITER-SLOW, var=UP_LOITER,  sval=speed=1.4,  receivers=all:$(VNAMES)
+
+  cmd = label=LOITER-CLOCKWISE-TRUE,  var=UP_LOITER, sval=clockwise=true,  receivers=all:$(VNAMES)
+  cmd = label=LOITER-CLOCKWISE-FALSE, var=UP_LOITER, sval=clockwise=false, receivers=all:$(VNAMES)
+  cmd = label=LOITER-CLOCKWISE-BEST,  var=UP_LOITER, sval=clockwise=false, receivers=all:$(VNAMES)
+}
+
+ProcessConfig = pHelmIvP
+{
+  AppTick    = 4
+  CommsTick  = 4
+
+  behaviors  = alpha.bhv
+  domain     = course:0:359:360
+  domain     = speed:0:10:101
+  domain     = depth:0:100:101
+
+  park_on_allstop = false
+  //park_on_allstop = true
+  
+}
+ProcessConfig = uProcessWatch
+{
+  AppTick   = 4
+  CommsTick = 4
+
+  watch_all = true
+  nowatch   = uPokeDB*
+  nowatch   = uQueryDB*
+  nowatch   = uXMS*
+  nowatch   = uMAC*
+  nowatch   = pShare*
+  nowatch   = pRealm*
+  nowatch   = pNodeReporter*
+}
+
+ProcessConfig = pNodeReporter
+{
+  AppTick    = 2
+  CommsTick	 = 2
+
+  platform_type = UUV 
+  platform_color  = red
+  platform_length = 4
+}
+
+
+ProcessConfig = pShare
+{
+  AppTick    = 2
+  CommsTick  = 2
+  //UUV 信息
+  output = src_name = NODE_REPORT*, route = 10.25.0.160:8085
+  output = src_name = NODE_REPORT*, route = 10.25.0.248:8085
+  output = src_name = NODE_REPORT*, route = 10.25.0.165:8085
+  //App 信息
+  output = src_name = APPCAST*, route = 10.25.0.160:8085
+  output = src_name = APPCAST*, route = 10.25.0.165:8085
+  output = src_name = APPCAST*, route = 10.25.0.248:8085
+  //路径点信息
+  output = src_name = VIEW*, route = 10.25.0.160:8085
+  output = src_name = VIEW*, route = 10.25.0.165:8085
+  output = src_name = VIEW*, route = 10.25.0.248:8085
+  //调试端输入端口
+  input = route = localhost:8081
+  input = route = localhost:8082
+  input = route = localhost:8083
+}
+
+ProcessConfig = pTaskManagement
+{
+  AppTick   = 8
+  CommsTick = 8
+  planConfigPath  = /home/zjk/project/work/moos-ivp-pi/setting/PlanConfigure.json
+  safetyRulesPath  = /home/zjk/project/work/moos-ivp-pi/setting/SafetyRules.json
+  wayConfigParamPath = /home/zjk/project/work/moos-ivp-pi/setting/WayConfigParam.json
+}
+
+ProcessConfig = pBoardSupportComm
+{
+   AppTick   = 5
+   CommsTick = 5
+}
+
+ProcessConfig = uSimMarine
+{
+  AppTick	= 10
+  CommsTick	= 10
+
+  START_X       = 0
+  START_Y       = 0
+  START_SPEED   = 0
+  START_HEADING = 180
+
+  PREFIX        = NAV
+}
+
+ProcessConfig = pMarinePID
+{
+  AppTick    = 20
+  CommsTick  = 20
+
+  VERBOSE       = true
+  DEPTH_CONTROL = false
+
+  // Yaw PID controller
+  YAW_PID_KP		 = 0.5
+  YAW_PID_KD		 = 0.0
+  YAW_PID_KI		 = 0.0
+  YAW_PID_INTEGRAL_LIMIT = 0.07
+
+  // Speed PID controller
+  SPEED_PID_KP		 = 1.0
+  SPEED_PID_KD		 = 0.0
+  SPEED_PID_KI		 = 0.0
+  SPEED_PID_INTEGRAL_LIMIT = 0.07
+
+  //MAXIMUMS
+  MAXRUDDER    = 100
+  MAXTHRUST    = 100
+
+  // A non-zero SPEED_FACTOR overrides use of SPEED_PID
+  // Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
+  SPEED_FACTOR = 20
+  
+}
+
+ProcessConfig = pMotionControler
+{
+  AppTick    = 5
+  CommsTick  = 5
+  tardy_nav_thresh = 2.0
+  tardy_helm_thresh = 2.0
+
+  cheak_stalensee = true
+  delta_freqency = 5
+  
+  //config_file = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/ControlParam.json
+  config_file = /home/zjk/project/work/moos-ivp-pi/setting/ControlParam.json
+}
+
+ProcessConfig = pEmulator
+{
+  AppTick   = 20
+  CommsTick = 5
+  matlab_host = 10.25.0.137
+  matlab_port = 8085
+  local_port = 8080
+  prefix = NAV
+
+  start_x = 10
+  start_y = 9
+  start_z = 1
+  start_heading = 30
+}
+
+ProcessConfig = pDataManagement
+{
+  AppTick   = 4
+  CommsTick = 4
+}
+
+ProcessConfig = pLogger
+{
+  AppTick   = 8
+  CommsTick = 8
+
+  AsyncLog = true
+  FileTimeStamp   = true
+
+  // For variables that are published in a bundle on their first post,
+  // explicitly declare their logging request
+  Log = IVPHELM_LIFE_EVENT @ 0 NOSYNC
+  Log = REPORT @ 0 NOSYNC
+
+  LogAuxSrc = true
+  WildCardLogging = true
+  WildCardOmitPattern = *_STATUS
+  WildCardOmitPattern = DB_VARSUMMARY
+  WildCardOmitPattern = DB_RWSUMMARY
+  WildCardExclusionLog = true
+}
+ProcessConfig = pFaultHandle
+{
+   AppTick   = 4
+   CommsTick = 4
+}
+

src/pEmulator/Emulator.cpp

@@ -2,7 +2,7 @@
  * @Author: zjk 1553836110@qq.com
  * @Date: 2023-10-12 09:52:27
  * @LastEditors: zhaojingkui 1553836110@qq.com
- * @LastEditTime: 2023-11-28 11:32:48
+ * @LastEditTime: 2023-11-30 12:51:13
  * @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.cpp
  * @Description: 
  * 
@@ -253,7 +253,8 @@ bool Emulator::buildReport()
   m_msgs << "Lat   : " <<  doubleToStringX(s[12],5) << " (du)" << endl;
   m_msgs << "Lon   : " <<  doubleToStringX(s[13],5) << " (du)" << endl;
   m_msgs << "Alt   : " <<  doubleToStringX(s[14],5) << " (m)" << endl;
-  
+
+  cout << "------------------------------------------------------" << endl;  
 
   return(true);
 }
@@ -358,6 +359,7 @@ bool Emulator::receiveUdpDate()
           record.setPitch(data);
           break;
         case 11: //psi
+          data = angle360(data);
           remus100.yaw = data*100;
           record.setHeading(data);
           break;

src/pEmulator/_150server.cpp

@@ -2,13 +2,14 @@
  * @Author: zjk 1553836110@qq.com
  * @Date: 2023-11-07 14:59:47
  * @LastEditors: zhaojingkui 1553836110@qq.com
- * @LastEditTime: 2023-11-28 18:20:41
+ * @LastEditTime: 2023-11-30 11:58:46
  * @FilePath: /moos-ivp-pi/src/pEmulator/_150server.cpp
  * @Description: 
  * 
  * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
  */
 #include "_150server.hpp"
+#include <unistd.h>
 
 uint16_t _150server::serializeFields(AUVEmbedded &embeddedInfo, uint8_t* bfr)
 {
@@ -172,7 +173,13 @@ void _150server::_150_startServer()
     saddr.sin_family = PF_INET;
     saddr.sin_addr.s_addr = INADDR_ANY; //0.0.0.0
     saddr.sin_port = htons(8001);
-    int ret = bind(lfd, (struct sockaddr *)&saddr, sizeof(saddr));
+    int ret = -1;
+    do
+    {
+        ret = bind(lfd, (struct sockaddr *)&saddr, sizeof(saddr));
+        std::cout << "Try bind adress..." << std::endl;
+        sleep(1);
+    } while (ret==-1);
 
     if(ret == -1)
     {
@@ -181,7 +188,13 @@ void _150server::_150_startServer()
     }
 
     //3.监听
-    listen(lfd, 5);
+    do
+    {
+        listen(lfd, 5);
+        std::cout << "Try to listening..." << std::endl;
+        sleep(1);
+    } while (ret==-1);
+
     if(ret==-1)
     {
         perror("listen");

src/pFaultHandle/FaultHandle.cpp

@@ -169,6 +169,7 @@ bool FaultHandle::Iterate()
   m_proc_watch_summary_changed = false; 
 
   CPUTempMonitor();//监控CPU温度并实时反馈
+  
   //----------------------------------------
   AppCastingMOOSApp::PostReport();
 
@@ -890,7 +891,7 @@ void FaultHandle::MoosAppStatusMonitor()
   
   if(MoosAppMissing())
   { 
-    uFH_errorMsg2_fb["FaultLevel"] = 2;
+    //uFH_errorMsg2_fb["FaultLevel"] = 2;
     //uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
     uFH_errorMsg2_fb["FaultMsgs"]["uSoftWare_appMiss_fault"]["FirstRecvTime"] = FirstRecvTime;
     uFH_errorMsg2_fb["FaultMsgs"]["uSoftWare_appMiss_fault"]["Source"] = "MOOSDB";
@@ -936,6 +937,10 @@ void FaultHandle::MoosAppStatusMonitor()
       } 
       //APP故障预留ID：906-1000
 
+  }
+  else if(!MoosAppMissing())
+  {
+    uFH_errorMsg2_fb["FaultMsgs"].removeMember("uSoftWare_appMiss_fault");
   }     
 //发布当前时间
 string str_time = convertSecondsToString(m_curr_time);
@@ -1007,7 +1012,7 @@ void FaultHandle::MoosAppOverloadMonitor()
 
   if(MoosAppOverload())
   { 
-    uFH_errorMsg1_fb["FaultLevel"] = 1;
+    //uFH_errorMsg1_fb["FaultLevel"] = 1;
     //uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
     uFH_errorMsg1_fb["FaultMsgs"]["uSoftWare_appOverload_fault"]["FirstRecvTime"] = FirstRecvTime;
     uFH_errorMsg1_fb["FaultMsgs"]["uSoftWare_appOverload_fault"]["Source"] = "MOOSDB";
@@ -1056,6 +1061,11 @@ void FaultHandle::MoosAppOverloadMonitor()
       // }
 
 }
+else if(!MoosAppOverload())
+{
+  uFH_errorMsg1_fb["FaultMsgs"].removeMember("uSoftWare_appOverload_fault");
+}
+
 }
 
 //---------------------------------------------------------------------------
@@ -1072,7 +1082,13 @@ void FaultHandle::MissionFaultMonitor()
 
   static int misson_fault_lastTime = 0;
 
-  if(m_Mission_fault_fb != 0 & misson_fault_lastTime != m_Mission_fault_fb)
+  //
+  if(m_Mission_fault_fb == 0)
+  {
+    uFH_errorMsg2_fb["FaultMsgs"].removeMember("uMission_status_fault");
+  }
+
+  else if(m_Mission_fault_fb != 0 & misson_fault_lastTime != m_Mission_fault_fb)
   {
     //---------------------------------------------------------------
     //test
@@ -1080,7 +1096,7 @@ void FaultHandle::MissionFaultMonitor()
     // m_msgs << "FirstRecvTime:" << FirstRecvTime << endl;
     //----------------------------------------------------------------
 
-    uFH_errorMsg2_fb["FaultLevel"] = 2;
+    //uFH_errorMsg2_fb["FaultLevel"] = 2;
     //uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
     uFH_errorMsg2_fb["FaultMsgs"]["uMission_status_fault"]["FirstRecvTime"] = FirstRecvTime;
     uFH_errorMsg2_fb["FaultMsgs"]["uMission_status_fault"]["Source"] = "TaskManagement";
@@ -1134,7 +1150,7 @@ void FaultHandle::MotionControlMonitor()//20231109未完成
     static bool motionFaultFlag1 = false;
     if((m_Motion_fault_fb == 1) && (motionFaultFlag1 == false))
     {
-      uFH_errorMsg2_fb["FaultLevel"] = 2;
+      //uFH_errorMsg2_fb["FaultLevel"] = 2;
       //uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
       uFH_errorMsg2_fb["FaultMsgs"]["uMotion_control_fault"]["FirstRecvTime"] = FirstRecvTime;
       uFH_errorMsg2_fb["FaultMsgs"]["uMotion_control_fault"]["Source"] = "MotionControl";
@@ -1146,7 +1162,7 @@ void FaultHandle::MotionControlMonitor()//20231109未完成
     static bool motionFaultFlag2 = false;
     if((m_Motion_fault_fb > 2) && (motionFaultFlag2 == false))
     {
-      uFH_errorMsg2_fb["FaultLevel"] = 2;
+      //uFH_errorMsg2_fb["FaultLevel"] = 2;
       //uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
       uFH_errorMsg2_fb["FaultMsgs"]["uMotion_control_fault"]["FirstRecvTime"] = FirstRecvTime;
       uFH_errorMsg2_fb["FaultMsgs"]["uMotion_control_fault"]["Source"] = "MotionControl";
@@ -1158,7 +1174,7 @@ void FaultHandle::MotionControlMonitor()//20231109未完成
     
     if((m_Motion_fault_fb == 2) && (motionFaultFlag3 == false))
     {    
-      uFH_errorMsg1_fb["FaultLevel"] = 1;
+      //uFH_errorMsg1_fb["FaultLevel"] = 1;
       //uFH_errorMsg1_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
       uFH_errorMsg1_fb["FaultMsgs"]["uMotion_control_fault"]["FirstRecvTime"] = FirstRecvTime;
       uFH_errorMsg1_fb["FaultMsgs"]["uMotion_control_fault"]["Source"] = "MotionControl";
@@ -1167,6 +1183,11 @@ void FaultHandle::MotionControlMonitor()//20231109未完成
     }
     //此处，关于uMotion_control_fault的FaultID预留了1104-1200，后续有需要可添加。
   }
+  else if(m_Motion_fault_fb == 0)
+  {
+    uFH_errorMsg1_fb["FaultMsgs"].removeMember("uMotion_control_fault");
+    uFH_errorMsg2_fb["FaultMsgs"].removeMember("uMotion_control_fault");
+  }
 
 }
 //-------------------------------------------------------------------------
@@ -1186,7 +1207,7 @@ void FaultHandle::DeviceLeakageMonitor()
 
   if((m_faultLeakSensor != 0) & (m_faultLeakSensorLastTime != m_faultLeakSensor))
     {
-      uFH_errorMsg3_fb["FaultLevel"] = 3;
+      //uFH_errorMsg3_fb["FaultLevel"] = 3;
       //uFH_errorMsg3_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
       uFH_errorMsg3_fb["FaultMsgs"]["uSensor_leakage_fault"]["FirstRecvTime"] = FirstRecvTime;
       uFH_errorMsg3_fb["FaultMsgs"]["uSensor_leakage_fault"]["Source"] = "AUV";
@@ -1202,6 +1223,10 @@ void FaultHandle::DeviceLeakageMonitor()
       uFH_errorMsg3_fb["FaultMsgs"]["uSensor_leakage_fault"]["FaultID"] = msg3_faultID_array;
       m_faultLeakSensorLastTime = m_faultLeakSensor;
     }
+  else if(m_faultLeakSensor == 0)
+  {
+    uFH_errorMsg3_fb["FaultMsgs"].removeMember("uSensor_leakage_fault");
+  }
 
 //----------------------changed by zhangwei 20231115---------------------------------------
 
@@ -1216,13 +1241,17 @@ void FaultHandle::BatteryStatusMonitor()
   static bool FirstLowPower_flag = false;
   if((m_batteryLevel < 20) && (FirstLowPower_flag == false))
   {     
-    uFH_errorMsg1_fb["FaultLevel"] = 1;
+    //uFH_errorMsg1_fb["FaultLevel"] = 1;
     //uFH_errorMsg1_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
     uFH_errorMsg1_fb["FaultMsgs"]["uBattery_level_warning"]["FirstRecvTime"] = MOOSTime();
     uFH_errorMsg1_fb["FaultMsgs"]["uBattery_level_warning"]["Source"] = "AUV";
     uFH_errorMsg1_fb["FaultMsgs"]["uBattery_level_warning"]["FaultID"].append(700);
     FirstLowPower_flag = true;
   }
+  else if(m_batteryLevel >= 20)
+  {
+    uFH_errorMsg1_fb["FaultMsgs"].removeMember("uBattery_level_warning");
+  }
 
   //---------------------------------------------------------------------------------------
   //电池状态监控
@@ -1237,7 +1266,7 @@ void FaultHandle::BatteryStatusMonitor()
 
   if((m_faultBattery != 0) && (m_faultBatteryLastTime != m_faultBattery))
     {
-      uFH_errorMsg2_fb["FaultLevel"] = 2;
+      //uFH_errorMsg2_fb["FaultLevel"] = 2;
       //uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
       uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FirstRecvTime"] = FirstRecvTime;
       uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["Source"] = "AUV";
@@ -1253,6 +1282,13 @@ void FaultHandle::BatteryStatusMonitor()
       uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = msg2_PSfaultID_array;
       m_faultBatteryLastTime = m_faultBattery;
     }
+    else if(m_faultBattery == 0)
+    {
+      uFH_errorMsg2_fb["FaultMsgs"].removeMember("uBattery_status_fault");
+    }
+
+//----------------------changed by zhangwei 20231115---------------------------------------
+
 }
 //-------------------------------------------------------------------------
 //获取AUV设备信息，判断以及反馈应急电池状态//20231110修改
@@ -1271,7 +1307,7 @@ void FaultHandle::EmergencyBatteryMonitor()
 
   if((m_faultEmergencyBattery != 0) & (m_faultEmergencyBatteryLastTime != m_faultEmergencyBattery))
     {
-      uFH_errorMsg2_fb["FaultLevel"] = 2;
+      //uFH_errorMsg2_fb["FaultLevel"] = 2;
       //uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
       uFH_errorMsg2_fb["FaultMsgs"]["uEmergency_Battery_fault"]["FirstRecvTime"] = FirstRecvTime;
       uFH_errorMsg2_fb["FaultMsgs"]["uEmergency_Battery_fault"]["Source"] = "AUV";
@@ -1287,6 +1323,10 @@ void FaultHandle::EmergencyBatteryMonitor()
       uFH_errorMsg2_fb["FaultMsgs"]["uEmergency_Battery_fault"]["FaultID"] = msg2_emergencyPS_array;
       m_faultEmergencyBatteryLastTime = m_faultEmergencyBattery;
     }
+  else if(m_faultEmergencyBattery == 0)
+    {
+      uFH_errorMsg2_fb["FaultMsgs"].removeMember("uEmergency_Battery_fault");
+    }
 
 //----------------------changed by zhangwei 20231115---------------------------------------
 }
@@ -1304,11 +1344,13 @@ void FaultHandle::ThrustStatusMonitor()
       firstRecvFlag = true;
   }
 
+//----------------------changed by zhangwei 20231115---------------------------------------
+
   static uint32_t m_faultThrustLastTime = m_faultThrust;
 
   if((m_faultThrust != 0) & (m_faultThrustLastTime != m_faultThrust))
     {
-      uFH_errorMsg2_fb["FaultLevel"] = 2;
+      //uFH_errorMsg2_fb["FaultLevel"] = 2;
       //uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
       uFH_errorMsg2_fb["FaultMsgs"]["uPropeller_status_fault"]["FirstRecvTime"] = FirstRecvTime;
       uFH_errorMsg2_fb["FaultMsgs"]["uPropeller_status_fault"]["Source"] = "AUV";
@@ -1326,9 +1368,15 @@ void FaultHandle::ThrustStatusMonitor()
         uFH_errorMsg2_fb["FaultMsgs"]["uPropeller_status_fault"]["FaultID"] = msg2_thrust_Status_array;
         m_faultThrustLastTime = m_faultThrust;
       }
+  else if(m_faultThrust == 0)
+    {
+      uFH_errorMsg2_fb["FaultMsgs"].removeMember("uPropeller_status_fault");
+    }
 
 
 }
+//----------------------changed by zhangwei 20231115---------------------------------------
+
 
 
 //-------------------------------------------------------------------------
@@ -1390,22 +1438,23 @@ void FaultHandle::CPUTempMonitor()
         {  
           Notify("CPUTemperature", "Failed");
         }
-
 }
 
 void FaultHandle::errorMsgInit()
 {
-  uFH_errorMsg1_fb["FaultLevel"] = 0;
+  //uFH_errorMsg1_fb["FaultLevel"] = 0;
   uFH_errorMsg1_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
-  uFH_errorMsg1_fb["FaultMsgs"];
+  uFH_errorMsg1_fb["FaultMsgs"] = Json::objectValue;
 
-  uFH_errorMsg2_fb["FaultLevel"] = 0;
+  //uFH_errorMsg2_fb["FaultLevel"] = 0;
   uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
-  uFH_errorMsg2_fb["FaultMsgs"];
+  uFH_errorMsg2_fb["FaultMsgs"] = Json::objectValue;
 
-  uFH_errorMsg3_fb["FaultLevel"] = 0;
+
+  //uFH_errorMsg3_fb["FaultLevel"] = 0;
   uFH_errorMsg3_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
-  uFH_errorMsg3_fb["FaultMsgs"];
+  uFH_errorMsg3_fb["FaultMsgs"] = Json::objectValue;
+
 }
 
 //-----------------------------------------------------------------------------
@@ -1436,21 +1485,21 @@ bool FaultHandle::MoosAppOverload()
       return false;
 }
 //-----------------------------------------------------------------------
-//接收外部故障复位命令，将故障上报信息重置,暂时未启用 20231121
+//接收外部故障复位命令，将故障上报信息重置,暂时未启用 20231121，现已更改为自动恢复故障逻辑
 void FaultHandle::FaultReset()
 {
   if(m_resetMsg_fb)
   {
     Json::Value empty;
-    uFH_errorMsg1_fb["FaultLevel"] = 0;
+    //uFH_errorMsg1_fb["FaultLevel"] = 0;
     uFH_errorMsg1_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
     uFH_errorMsg1_fb["FaultMsgs"] = empty;
 
-    uFH_errorMsg2_fb["FaultLevel"] = 0;
+    //uFH_errorMsg2_fb["FaultLevel"] = 0;
     uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
     uFH_errorMsg2_fb["FaultMsgs"] = empty;
 
-    uFH_errorMsg3_fb["FaultLevel"] = 0;
+    //uFH_errorMsg3_fb["FaultLevel"] = 0;
     uFH_errorMsg3_fb["CurrentTime"] = convertSecondsToString(m_curr_time);
     uFH_errorMsg3_fb["FaultMsgs"] = empty;
   }

src/pMotionControler/MotionControler.cpp

@@ -1,8 +1,8 @@
 /*
  * @Author: zjk 1553836110@qq.com
  * @Date: 2023-10-12 09:52:27
- * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditors: zhaojingkui 1553836110@qq.com
- * @LastEditTime: 2023-11-07 12:01:57
+ * @LastEditTime: 2023-11-30 11:05:18
  * @FilePath: /moos-ivp-pi/src/pMotionControler/MotionControler.cpp
  * @Description: 
  * 
@@ -29,19 +29,33 @@ bool MotionControler::OnNewMail(MOOSMSG_LIST &NewMail)
       if((key == "MOOS_MANUAL_OVERIDE") || (key == "MOOS_MANUAL_OVERRIDE"))
         pengine.overrived(sval);
       else if(key == "DESIRED_HEADING")
+      {
         pengine.setDesHeading(dval, MOOSTime());
+      }
       else if(key == "DESIRED_SPEED")
         pengine.setDesSpeed(dval, MOOSTime());
       else if(key == "DESIRED_DEPTH") 
         pengine.setDesDepth(dval, MOOSTime());
       else if(key == "NAV_HEADING")
+      {
         pengine.setCurHeading(angle360(dval), MOOSTime());
+        RepList["NAV"]["Heading"] = dval;
+      }
       else if(key == "NAV_SPEED")
+      {
         pengine.setCurSpeed(dval, MOOSTime());
+        RepList["NAV"]["Speed"] = dval;
+      }
       else if(key == "NAV_DEPTH")
+      {
         pengine.setCurDepth(dval, MOOSTime());
+        RepList["NAV"]["Depth"] = dval;
+      }
       else if(key == "NAV_PITCH")
-        pengine.setCurPitch(dval, MOOSTime());      
+      {
+        pengine.setCurPitch(dval, MOOSTime());    
+        RepList["NAV"]["Pitch"] = dval;
+      }
       else if(key == MSG_ReadConfig) //重新读取配置参数可以清除故障码
       {
         int e = pengine.setParam(configFilePath);
@@ -159,7 +173,7 @@ bool MotionControler::buildReport()
   << intToString(pengine.hasDphCtrl())+" |" << endl;
 
   RepList["to BS"] = colVar;
-  RepList["PID"] = pengine.getReport()["W"];
+  // RepList["PID"] = pengine.getReport()["W"];
   string rep = Json::writeString(RepJsBuilder, RepList);
   m_msgs << rep << endl;
 

src/pTaskManagement/TaskManger.cpp

@@ -140,7 +140,7 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail)
       Json::Reader a;
       a.parse(msg_str,j);
       // int fault_level = j["FaultLevel"].asInt();
-      if(!j["FaultLevel"].empty()) //TODO: 修改故障码支持
+      if(!j["FaultMsgs"].empty()) //TODO: 修改故障码支持
       {
         state = FAULT;
         if(!j["FaultMsgs"].isMember("uMission_status_fault"))