From bee3fea02df75f0953525b9e74bbdad6f8d6b80c Mon Sep 17 00:00:00 2001
From: chenlizhi <you@example.com>
Date: Tue, 28 Nov 2023 09:52:36 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86=E7=8A=B6=E6=80=81?=
 =?UTF-8?q?=E7=AE=A1=E7=90=86=E4=BB=A3=E7=A0=81?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/pStateManagement/CMakeLists.txt      |  6 +++++-
 src/pStateManagement/StateManagement.cpp | 23 +++++++++++------------
 src/pStateManagement/StateManagement.h   |  6 ++++++
 3 files changed, 22 insertions(+), 13 deletions(-)

diff --git a/src/pStateManagement/CMakeLists.txt b/src/pStateManagement/CMakeLists.txt
index bde6d92..0e5676b 100644
--- a/src/pStateManagement/CMakeLists.txt
+++ b/src/pStateManagement/CMakeLists.txt
@@ -17,6 +17,9 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 
 #find_package (jsoncpp NO_MODULE REQUIRED)
 
+find_package (GeographicLib REQUIRED)
+include_directories(${GeographicLib_INCLUDE_DIRS})
+
 include_directories(/usr/include/jsoncpp/)
 link_directories(/usr/local/lib/)
 
@@ -26,7 +29,8 @@ TARGET_LINK_LIBRARIES(pStateManagement
    ${MOOS_LIBRARIES}
    ${CMAKE_DL_LIBS}
    ${SYSTEM_LIBS} 
-   ${DUNE_LIB}       
+   ${DUNE_LIB} 
+   ${GeographicLib_LIBRARIES}       
    mbutil
    m
    pthread
diff --git a/src/pStateManagement/StateManagement.cpp b/src/pStateManagement/StateManagement.cpp
index 787cf75..5dbdb31 100644
--- a/src/pStateManagement/StateManagement.cpp
+++ b/src/pStateManagement/StateManagement.cpp
@@ -9,6 +9,7 @@
 #include "MBUtils.h"
 #include "StateManagement.h"
 #include <json/json.h>
+#include <cmath>
 
 using namespace std;
 
@@ -36,7 +37,6 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail)
   for(p=NewMail.begin(); p!=NewMail.end(); p++) {
     CMOOSMsg &msg = *p;
 
-#if 1 // Keep these around just for template
     string key   = msg.GetKey();
     string comm  = msg.GetCommunity();
     double dval  = msg.GetDouble();
@@ -45,11 +45,10 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail)
     double mtime = msg.GetTime();
     bool   mdbl  = msg.IsDouble();
     bool   mstr  = msg.IsString();
-#endif
 
 	  Json::Value deviceState;
-    double manualState;
-    double missionState;
+    double      manualState;
+    int         missionState;
 
     if(key == "uManual_enable_cmd")
     {
@@ -75,18 +74,18 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail)
       deviceState["opMode"] = opModeLists.external;
     }
     else if (fabs(manualState - 0) < 1e-6) //manualState=0
-    {
-      if(missionState == 0)
+    {     
+      switch (missionState)
       {
+      case 0:
         deviceState["opMode"] = opModeLists.error;
-      }
-      if(missionState == 1)
-      {
+        break;
+      case 1:
         deviceState["opMode"] = opModeLists.service;
-      }
-      else if((missionState == 3) )
-      {
+        break;
+      case 3:
         deviceState["opMode"] = opModeLists.maneuver;
+        break;
       }
     }
 
diff --git a/src/pStateManagement/StateManagement.h b/src/pStateManagement/StateManagement.h
index 5da5f97..6635540 100644
--- a/src/pStateManagement/StateManagement.h
+++ b/src/pStateManagement/StateManagement.h
@@ -9,6 +9,12 @@
 #define StateManagement_HEADER
 
 #include "MOOS/libMOOS/MOOSLib.h"
+#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
+#include <DUNE/DUNE.hpp>
+#include <GeographicLib/LocalCartesian.hpp>
+#include <json/json.h>
+#include <thread>
+#include <chrono>
 
 class StateManagement : public CMOOSApp
 {