From af67e6a839766ef53136646802f17d249a4fbb5e Mon Sep 17 00:00:00 2001
From: zhaojingkui <1553836110@qq.com>
Date: Thu, 30 Nov 2023 11:07:05 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86=E8=BF=90=E5=8A=A8?=
 =?UTF-8?q?=E6=8E=A7=E5=88=B6=E7=9A=84=E6=98=BE=E7=A4=BA=E4=BF=A1=E6=81=AF?=
 =?UTF-8?q?,=E6=96=B9=E4=BE=BF=E8=B0=83=E8=AF=95?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/pMotionControler/MotionControler.cpp | 22 ++++++++++++++++++----
 1 file changed, 18 insertions(+), 4 deletions(-)

diff --git a/src/pMotionControler/MotionControler.cpp b/src/pMotionControler/MotionControler.cpp
index fbc7d91..45a3eb2 100755
--- a/src/pMotionControler/MotionControler.cpp
+++ b/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;