修改了故障代码读取的bug

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

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
- * @LastEditTime: 2023-11-07 12:01:57
+ * @LastEditors: zhaojingkui 1553836110@qq.com
+ * @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"))