0626提交

上传了model
修复了航向的问题
2025-06-26 21:00:14 +08:00 · 2025-06-18 23:25:52 +08:00 · 2025-06-18 23:10:16 +08:00 · 2025-06-18 22:30:06 +08:00 · 2025-06-18 11:15:01 +08:00 · 2025-06-17 23:55:57 +08:00
35 changed files with 4989 additions and 98 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.0)
 PROJECT( IVP_EXTEND )

 set (CMAKE_CXX_STANDARD 11)
-set (MOOSIVP_SOURCE_TREE_BASE "/home/zjk/project/lib/moos-ivp")
+set (MOOSIVP_SOURCE_TREE_BASE "/home/zjk/software/moos-ivp-main")

 #=======================================================================
 # Set the output directories for the binary and library files
--- a/doc/a.md
+++ b/doc/a.md
@@ -14,4 +14,27 @@ NAV_LONG: The ownship vehicle position on the x axis of global coordinates.
 NAV_HEADING: The ownship vehicle heading in degrees.
 NAV_YAW: The ownship vehicle yaw in radians.
 NAV_SPEED: The ownship vehicle speed in meters per second.
-NAV_DEPTH: The ownship vehicle depth in meters.
+NAV_DEPTH: The ownship vehicle depth in meters.
+
+
+# 时序控制
+
+采样时间、指令发送和指令计算是三个独立的线程进行的，需要对时序进行控制，理论上的时序应该是：
+
+采样->指令计算->指令发送
+
+采样频率为5Hz，即为200ms，指令计算在200ms内，指令计算完成后再发送指令，整个过程在200ms内完成
+
+方案1：使用时间戳进行同步：
+
+    m_listen_time < m_calc_time < m_send_time
+
+    使用这种方式程序灵活性较高，交互程序和控制程序可以写成两个独立的程序，但是时序控制更加复杂，由于通信延迟会造成一部分实时性的性能损失
+
+方案2：在采样时间循环内进行：在模块采样时进行计算，程序灵活性降低，需要在交互程序中同时包含到控制程序，但是实时性会较好
+
+使用方案2可以考虑使用simulink进行代码生成和集成，将整个控制也集成到里面
+
+# 惯导精度问题
+
+目前惯导设备经纬度的精度为1e-4，大概为11米所有的精度，精度比较低，或许可以使用航位推算结合无迹卡尔曼滤波弥补并提高这个精度
--- a/doc/p.md
+++ b/doc/p.md
@@ -0,0 +1,25 @@
+# 参数记录
+
+  {
+    1,
+    0.1,
+    0.01,
+    10.0
+  },
+
+  // Variable: pid_pitch
+  //  Referenced by:
+  //    '<S142>/Derivative Gain'
+  //    '<S146>/Integral Gain'
+  //    '<S152>/Filter Coefficient'
+  //    '<S154>/Proportional Gain'
+
+  {
+    0.5,
+    0.1,
+    0.1,
+    10.0
+  }
+};
+
+
--- a/missions/alder/ControlParam.json
+++ b/missions/alder/ControlParam.json
@@ -0,0 +1,43 @@
+{
+	"speed" : 
+	{
+		"Kp" : 10.0,
+		"Ki" : 5.0,
+		"Kd" : 0.0,
+		"LimitDelta" : 50,
+		"MaxOut" : 100,
+		"MinOut" : 0
+	},
+	"heading" :
+	{
+		"Kp" : 10.0,
+		"Ki" : 0.5,
+		"Kd" : 2.2,
+		"LimitDelta" : 5,
+		"MaxOut" : 30,
+		"MinOut" : -30
+	},
+	"depth" : 
+	{
+		"Kp" : 10.0,
+		"Ki" : 0.3,
+		"Kd" : 2.0,
+		"LimitDelta" : 5,
+		"MaxOut" : 10,    
+		"MinOut" : -10
+	},
+	"pitch" : 
+	{
+		"Kp" : 0.6,
+		"Ki" : 0.03,
+		"Kd" : 1.5,
+		"LimitDelta" : 5,
+		"MaxOut" : 30,
+		"MinOut" : -30
+	},
+	"const_thrust" : 0,
+	"dead_zone" : 10,
+	"speedCol" : true,
+	"depthCol" : true,
+	"HeadingCol" : true
+}
--- a/missions/alder/XLOG_SHORESIDE_18_6_2025_____23_04_26/XLOG_SHORESIDE_18_6_2025_____23_04_26.slog
+++ b/missions/alder/XLOG_SHORESIDE_18_6_2025_____23_04_26/XLOG_SHORESIDE_18_6_2025_____23_04_26.slog
--- a/missions/alder/alder.moos
+++ b/missions/alder/alder.moos
@@ -16,11 +16,13 @@ ProcessConfig = ANTLER
  Run = MOOSDB        @ NewConsole = false
  //Run = uSimMarineV22	@ NewConsole = false
  Run = pNodeReporter	@ NewConsole = false
-  Run = pMarinePIDV22 @ NewConsole = false
+  //Run = pLogger         @ NewConsole = false
+  //Run = pMarinePIDV22 @ NewConsole = false
  Run = pMarineViewer	@ NewConsole = false
  Run = uProcessWatch @ NewConsole = false
  Run = pHelmIvP      @ NewConsole = false
-  Run = pAUV150       @ NewConsole = false
+  Run = pAUV150       @ NewConsole = true
+  //Run = pMotionControler @ NewConsole = false
 }

 //------------------------------------------
@@ -80,17 +82,17 @@ ProcessConfig = pHelmIvP

 ProcessConfig = pMarinePIDV22
 {
-  AppTick    = 5
-  CommsTick  = 5
+  AppTick    = 10
+  CommsTick  = 10

  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
+  YAW_PID_KP		 = 1
+  YAW_PID_KD		 = 1
+  YAW_PID_KI		 = 1
+  YAW_PID_INTEGRAL_LIMIT = 1

  // Speed PID controller
  SPEED_PID_KP		 = 1.0
@@ -99,8 +101,8 @@ ProcessConfig = pMarinePIDV22
  SPEED_PID_INTEGRAL_LIMIT = 0.07

  //MAXIMUMS
-  MAXRUDDER    = 100
-  MAXTHRUST    = 100
+  MAXRUDDER    = 35
+  MAXTHRUST    = 35

  // A non-zero SPEED_FACTOR overrides use of SPEED_PID
  // Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
@@ -157,13 +159,43 @@ ProcessConfig = pNodeReporter

 ProcessConfig = pAUV150
 {
-  AppTick   = 4
-  CommsTick = 4
+  AppTick   = 10
+  CommsTick = 10

-  server_host = 10.127.0.18
+  //server_host = 10.127.0.18
+  //server_host = 127.0.0.1
+  server_host = 172.18.96.1
  LatOrigin  = 43.825300 
  LongOrigin = -70.330400 

-  ControlFrequency = 5
+  ControlFrequency = 10

 }
+
+ProcessConfig = pMotionControler
+{
+  AppTick    = 10
+  CommsTick  = 10
+  config_file = /home/zjk/project/moos-ivp-extend/missions/alder/ControlParam.json
+}
+
+ProcessConfig = pLogger
+{
+  AppTick       = 10
+  CommsTick     = 10
+  
+  File          = XLOG_SHORESIDE
+  PATH          = ./
+  SyncLog       = true @ 0.2
+  AsyncLog      = true
+  FileTimeStamp = true
+
+  LogAuxSrc     = true
+
+  // Log it all!!!!!
+  WildCardLogging = true 
+
+  WildCardOmitPattern = *_STATUS
+  WildCardOmitPattern = DB_VARSUMMARY
+  WildCardOmitPattern = DB_RWSUMMARY
+}
--- a/model/R2022b/SimModelR2022b_0618.slx
+++ b/model/R2022b/SimModelR2022b_0618.slx
--- a/model/R2024b/SimModel0618.slx
+++ b/model/R2024b/SimModel0618.slx
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -19,6 +19,7 @@ ADD_SUBDIRECTORY(lib_behaviors-test)
 ADD_SUBDIRECTORY(pExampleApp)
 ADD_SUBDIRECTORY(pXRelayTest)
 add_subdirectory(pAUV150)
+add_subdirectory(pMotionControler)
 ##############################################################################
 #                           END of CMakeLists.txt
 ##############################################################################
--- a/src/pAUV150/AUV150.cpp
+++ b/src/pAUV150/AUV150.cpp
@@ -13,10 +13,13 @@
 #include <string>
 #include <iostream>

-// #define DEBUG_
+#define DEBUG_

 using namespace std;

+// 添加静态成员定义
+Controler AUV150::m_control;
+
 static bool TCP_ReadLoop(void *p)
 {
    AUV150 *pAUV150 = (AUV150 *)p;
@@ -49,6 +52,9 @@ AUV150::AUV150()
    m_desiredVarTime = 0;
    m_controlTime = 0;
    m_samplingTime = 0;
+
+    m_overrived = true; //手动控制
+    m_control.initialize();
 }

 //---------------------------------------------------------
@@ -74,7 +80,6 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
  for(p=NewMail.begin(); p!=NewMail.end(); p++) {
    CMOOSMsg &msg = *p;
    string key    = msg.GetKey();
-
 #if 0 // Keep these around just for template
    string comm  = msg.GetCommunity();
    double dval  = msg.GetDouble();
@@ -84,25 +89,33 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
    bool   mdbl  = msg.IsDouble();
    bool   mstr  = msg.IsString();
 #endif
-
-     if(key == "FOO") 
-       cout << "great!";
-    else if(key == "DESIRED_RUDDER")
+    if((key == "MOOS_MANUAL_OVERIDE") || (key == "MOOS_MANUAL_OVERRIDE"))
    {
-        m_CommandFrame.rudderUp = msg.GetDouble();
-        m_CommandFrame.rudderDown = msg.GetDouble();
-        m_desiredVarTime = msg.GetTime();
+        string sval = msg.GetString();
+        if(sval == "false")
+        {
+            std::cout << "======================" << std::endl;
+            m_overrived == false;
+        }
+        else
+        {
+            std::cout << "=============//=========" << std::endl;
+            m_overrived == true;
+        }
    }
-    else if(key == "DESIRED_ELEVATOR")
+    else if(key == "DESIRED_HEADING")
    {
-        m_CommandFrame.rudderLeft = msg.GetDouble();
-        m_CommandFrame.rudderRight = msg.GetDouble();
+        m_control.rtU.heading_cmd = msg.GetDouble();
    }
-    else if(key == "DESIRED_THRUST")
+    else if(key == "DESIRED_SPEED")
    {
-        m_CommandFrame.mainThruster = msg.GetDouble();
+        m_control.rtU.speed_cmd = msg.GetDouble();
    }
-    else if(key != "APPCAST_REQ") // handled by AppCastingMOOSApp
+    else if(key == "DESIRED_DEPTH") 
+    {
+        m_control.rtU.depth_cmd = msg.GetDouble();
+    }
+    else if(key != "APPCAST_REQ") 
       reportRunWarning("Unhandled Mail: " + key);
   }
 	
@@ -132,7 +145,7 @@ bool AUV150::Iterate()
        m_geodesy.LatLong2LocalGrid(m_status.insLatitude, m_status.insLongitude, m_pos_x, m_pos_y);
        postStatusUpdate("NAV");
    #endif
-    AppCastingMOOSApp::PostReport();
+    // AppCastingMOOSApp::PostReport();
    return(true);
 }

@@ -202,10 +215,14 @@ bool AUV150::OnStartUp()
 void AUV150::registerVariables()
 {
  AppCastingMOOSApp::RegisterVariables();
-  Register("DESIRED_RUDDER",0);
-  Register("DESIRED_THRUST",0);
-  Register("DESIRED_ELEVATOR",0);
-  // Register("FOOBAR", 0);
+//   Register("DESIRED_RUDDER",0);
+//   Register("DESIRED_THRUST",0);
+//   Register("DESIRED_ELEVATOR",0);
+  Register("DESIRED_HEADING", 0);
+  Register("DESIRED_SPEED", 0);
+  Register("DESIRED_DEPTH", 0);
+  Register("MOOS_MANUAL_OVERIDE", 0);
+  Register("MOOS_MANUAL_OVERRIDE", 0);
 }


@@ -247,7 +264,7 @@ bool AUV150::ListenLoop()
        {
            if((m_TcpSocket!=NULL) && m_bConnected)
            {
-                int count = m_TcpSocket->iRecieveMessage(buffer, sizeof(FeedbackFrame_150AUV));
+                int count = m_TcpSocket->iReadMessageWithTimeOut(buffer, sizeof(FeedbackFrame_150AUV),3);
                if(count > 0) {
                    std::cout << "Received " << count << " bytes" << std::endl;
                    FeedbackFrame_150AUV *p = reinterpret_cast<FeedbackFrame_150AUV *>(buffer);
@@ -262,7 +279,47 @@ bool AUV150::ListenLoop()
                    m_counter = p->counter;
                    updateStatus(*p);
                    postStatusUpdate("NAV");
+                    // if(!m_overrived)
+                    // {
+                        // 计算控制量
+                        m_control.rtU.u = m_status.dvlVelX;
+                        m_control.rtU.v = m_status.dvlVelY;
+                        m_control.rtU.w = m_status.dvlVelZ;
+                        m_control.rtU.dx = m_status.velocityEast;
+                        m_control.rtU.dy = m_status.velocityNorth;
+                        m_control.rtU.dz = m_status.velocityDown;
+                        m_control.rtU.phi = m_status.roll;
+                        m_control.rtU.psi = (m_status.trueHeading);
+                        m_control.rtU.theta = (m_status.pitch);
+                        m_control.rtU.x = m_status.x;
+                        m_control.rtU.y = m_status.y;
+                        m_control.rtU.z = m_status.z;
+                        m_control.step();
+                        m_CommandFrame.rudderUp = m_control.rtY.DirectUpperRudderServoAngleCmd + 35.0f; 
+                        m_CommandFrame.rudderDown = m_control.rtY.DirectLowerRudderServoAngleCmd + 35.0f;
+                        m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd + 35.0f;
+                        m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd + 35.0f;
+                        m_CommandFrame.mainThruster = m_control.rtY.MainThrusterSpeedCmd;
+                        m_status.depth_error = m_control.rtY.depth_error;
+                        m_status.heading_error = m_control.rtY.heading_error;
+                        m_status.pitch_error = m_control.rtY.pitch_error;
+                    // }
+                    // 发送消息 
+                    try
+                    {
+                        m_TcpSocket->iSendMessage(&m_CommandFrame, sizeof(CommandFrame_150AUV));
+                        m_controlGap =  MOOSTime() - m_desiredVarTime;
+                        m_controlTime = MOOSTime();
+                        m_bConnected = true;
+                    }
+                    catch(XPCException &e)
+                    {
+                        m_bConnected = false;
+                        std::cerr << "there was trouble sending message: "
+                                << e.sGetException() << "\n";
+                    }

+                    // 计算控制频率
                    m_samplingTime = MOOSTime();
                    message_count++;
                    double elapsed_time = m_samplingTime - start_time;
@@ -281,7 +338,6 @@ bool AUV150::ListenLoop()
                    m_faultCodes.insert(0x12); //无数据接收
                }
            }
-
        }
        catch (XPCException & e)
        {
@@ -295,38 +351,8 @@ bool AUV150::ListenLoop()
 }

 bool AUV150::WriteLoop()
-{
-    double start_time = MOOSTime();
-    int message_count = 0;
-    double last_send_time = MOOSTime();
-    
-    //5Hz发送
+{   
    while(!WritingThread_.IsQuitRequested()){
-        
-        if(MOOSTime() - last_send_time >= m_controlCycle)
-        {
-            try
-            {
-                m_TcpSocket->iSendMessage(&m_CommandFrame, sizeof(CommandFrame_150AUV));
-                m_controlTime = MOOSTime();
-                m_controlGap =  m_controlTime - m_desiredVarTime;
-                m_bConnected = true;
-            }
-            catch(XPCException &e)
-            {
-                m_bConnected = false;
-                std::cerr << "there was trouble sending message: "
-                        << e.sGetException() << "\n";
-            }
-            message_count++;
-            double elapsed_time = MOOSTime() - start_time;
-            if(elapsed_time > 1.0) {  // 每秒更新一次频率
-                m_real_write_freq = message_count / elapsed_time;
-                message_count = 0;
-                start_time = MOOSTime();
-            }
-            last_send_time = MOOSTime();
-        }
        if(!m_bConnected)
        {
            m_real_read_freq = 0;
@@ -425,13 +451,19 @@ bool AUV150::buildReport()
        m_msgs << oss.str() << std::endl << std::endl;
    };

+    //=================DEGUB==========================
+    m_msgs << "MOOS_MANUAL_OVERIDE : " << m_overrived << std::endl;
+    m_msgs << "DirectUpperRudderServoAngleCmd : " << m_control.rtY.DirectUpperRudderServoAngleCmd << std::endl;
+    m_msgs << "DirectLowerRudderServoAngleCmd : " << m_control.rtY.DirectLowerRudderServoAngleCmd << std::endl;
+    m_msgs << "DirectLeftRudderServoAngleCmd  : " << m_control.rtY.DirectLeftRudderServoAngleCmd << std::endl;
+    m_msgs << "DirectRightRudderServoAngleCmd : " << m_control.rtY.DirectRightRudderServoAngleCmd << std::endl;
+    m_msgs << "mainThruster                   : " << m_control.rtY.MainThrusterSpeedCmd << std::endl;
    // =============== Navigation Info ===============
      printTable(
-      {"Connect", "Read", "Writ" ,"Conter" ,"Server ip","Latitude", "Longitude","Gap"},
+      {"Connect", "Fre","Cont" ,"ip","OLat", "OLon","Gap"},
      {
          (isConnected() ? "Yes" : "No"),
-          doubleToString(getRealReadFreq(), 2) + " Hz",
-          doubleToString(getRealWriteFreq(), 2) + " Hz",
+          doubleToString(getRealReadFreq(), 4) + " Hz",
          ulintToString(getCounter()),
          m_serverHost,
          doubleToString(m_OriginLatitude, 5) + "deg",
@@ -445,9 +477,6 @@ bool AUV150::buildReport()
    }
    m_msgs << std::endl;
    m_msgs << "Navigation Mode  : " << uintToString(m_status.navModeFb) << std::endl;
-    // m_msgs << "Origin Latitude  : " << m_OriginLatitude << endl;
-    // m_msgs << "Origin Longitude : " << m_OriginLongitude << endl;
-    // m_msgs << "---------------------------------\n";
    // =============== Heading & Attitude ===============
    printTable(
        {"Heading", "Pitch", "Roll", "X", "Y", "Z", "H"},
@@ -465,7 +494,7 @@ bool AUV150::buildReport()

    // =============== Velocity Info ===============
    printTable(
-    {"Latitude", "Longitude", "Altitude","East Velocity","North Velocity","Down Velocity"},
+    {"Lat", "Lon", "Alt","East Vel","North Vel","Down Vel"},
    {
        doubleToString(m_status.insLatitude, 4) + "deg",
        doubleToString(m_status.insLongitude, 4) + "deg",
@@ -477,7 +506,7 @@ bool AUV150::buildReport()
    );
    m_msgs << "----------------DVL-----------------\n";
    printTable(
-        {"DVL X Velocity", "DVL Y Velocity", "DVL Z Velocity"},
+        {"DVL X Vel", "DVL Y Vel", "DVL Z Vel"},
        {
            doubleToString(m_status.dvlVelX, 2) + "m/s",
            doubleToString(m_status.dvlVelY, 2) + "m/s",
@@ -488,9 +517,14 @@ bool AUV150::buildReport()

    // =============== Propulsion System ===============
    printTable(
-        {"Thruster RPM"},
+        {"Thruster RPM", "Thruster CMD" ,"up", "down", "left", "right"},
        {
            doubleToString(m_status.thrusterRPM, 1) + "rpm",
+            doubleToString(m_CommandFrame.mainThruster, 1) + "%",
+            doubleToString(m_CommandFrame.rudderUp-35.0f, 1) + "deg",
+            doubleToString(m_CommandFrame.rudderDown-35.0f, 1) + "deg",
+            doubleToString(m_CommandFrame.rudderLeft-35.0f, 1) + "deg",
+            doubleToString(m_CommandFrame.rudderRight-35.0f, 1) + "deg",
        }
    );
    // m_msgs << "---------------------------------\n";
@@ -508,7 +542,7 @@ bool AUV150::buildReport()

    // =============== System Status ===============
    printTable(
-        {"Light", "Leak", "Power" , "Emergency power" , "Payload", "DVL Status","Thruster Status"},
+        {"Light", "Leak", "Power" , "Emergency power" , "PZ", "DVL","Thruster Status"},
        {
            m_status.ledSwitch ? "ON" : "OFF",
            m_status.leakStatus ? "LEAK" : "NORMAL",
@@ -527,25 +561,26 @@ bool AUV150::buildReport()
 bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame)
 {
    m_status.navModeFb = feedbackFrame.navModeFb;
-    m_status.altimeterHeight = feedbackFrame.altimeterHeight / 100.0;
-    m_status.depthSensor = feedbackFrame.depthSensor / 100.0;
-    m_status.trueHeading = feedbackFrame.trueHeading / 100.0;
-    m_status.pitch = feedbackFrame.pitch / 100.0;
-    m_status.roll = feedbackFrame.roll / 100.0;
-    m_status.velocityEast = feedbackFrame.velocityEast / 100.0;
-    m_status.velocityNorth = feedbackFrame.velocityNorth / 100.0;
-    m_status.velocityDown = feedbackFrame.velocityDown / 100.0;
-    m_status.insLongitude = feedbackFrame.insLongitude * 360.0 / (1ULL << 32);
-    m_status.insLatitude = feedbackFrame.insLatitude * 180.0 / (1ULL << 32) - 90.0;
-    m_status.insAltitude = feedbackFrame.insAltitude / 100.0;
-    m_status.dvlVelX = feedbackFrame.dvlVelX / 100.0;
-    m_status.dvlVelY = feedbackFrame.dvlVelY / 100.0;
-    m_status.dvlVelZ = feedbackFrame.dvlVelZ / 100.0;
+    m_status.altimeterHeight = feedbackFrame.altimeterHeight / 100.0f;
+    m_status.depthSensor = feedbackFrame.depthSensor / 100.0f;
+    m_status.trueHeading = feedbackFrame.trueHeading / 100.0f;
+
+    m_status.pitch = feedbackFrame.pitch / 100.0f;
+    m_status.roll = feedbackFrame.roll / 100.0f;
+    m_status.velocityEast = feedbackFrame.velocityEast / 100.0f;
+    m_status.velocityNorth = feedbackFrame.velocityNorth / 100.0f;
+    m_status.velocityDown = feedbackFrame.velocityDown / 100.0f;
+    m_status.insLongitude = feedbackFrame.insLongitude / 1e5 - 180.0f; 
+    m_status.insLatitude = feedbackFrame.insLatitude / 1e5 - 90.0f;
+    m_status.insAltitude = feedbackFrame.insAltitude / 100.0f;
+    m_status.dvlVelX = feedbackFrame.dvlVelX / 100.0f;
+    m_status.dvlVelY = feedbackFrame.dvlVelY / 100.0f;
+    m_status.dvlVelZ = feedbackFrame.dvlVelZ / 100.0f;
    m_status.thrusterRPM = feedbackFrame.thrusterRPM;
    m_status.ledSwitch = feedbackFrame.ledSwitch;
-    m_status.batteryVoltage = feedbackFrame.batteryVoltage / 1000.0;
+    m_status.batteryVoltage = feedbackFrame.batteryVoltage / 1000.0f;
    m_status.batteryLevel = feedbackFrame.batteryLevel;
-    m_status.batteryTemp = feedbackFrame.batteryTemp / 10.0;
+    m_status.batteryTemp = feedbackFrame.batteryTemp / 10.0f;
    m_status.leakStatus = feedbackFrame.leakStatus;
    m_status.powerModule = feedbackFrame.powerModule;
    m_status.backupPower = feedbackFrame.backupPower;
@@ -555,6 +590,10 @@ bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame)
    m_status.dvlStatus = feedbackFrame.dvlStatus;

    m_geodesy.LatLong2LocalGrid(m_status.insLatitude, m_status.insLongitude, m_pos_x, m_pos_y);
+    // 以下为转换的数据
+    m_status.x = m_pos_x;
+    m_status.y = m_pos_y;
+    m_status.z = m_status.depthSensor;

    return true;
 }
@@ -594,7 +633,14 @@ void AUV150::postStatusUpdate(std::string prefix)
    if(m_status.insAltitude > 0) {
        Notify(prefix+"_ALTITUDE", m_status.insAltitude, m_curr_time);
    }
-    
-    // // 发布模拟模式标志
-    // Notify("SIMULATION_MODE","TRUE", m_curr_time);
+
+    // 发布AUV信息
+    prefix = "AUV150";
+    Notify(prefix+"_HEADING_ERROR", m_status.heading_error, m_curr_time);
+    Notify(prefix+"_PITCH_ERROR", m_status.pitch_error, m_curr_time);
+    Notify(prefix+"_DEPTH_ERROR", m_status.depth_error, m_curr_time);
+
+    Notify(prefix+"_DESIRED_RUDDER",m_status.desired_rudder);
+    Notify(prefix+"_DESIRED_EVEVATOR",m_status.desired_elevator);
+    Notify(prefix+"_DESIRED_THRUST",m_status.desired_thrust);
 }
--- a/src/pAUV150/AUV150.h
+++ b/src/pAUV150/AUV150.h
@@ -14,6 +14,8 @@
 #include "MOOS/libMOOS/Utils/MOOSThread.h"
 #include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h"
 #include "MOOS/libMOOSGeodesy/MOOSGeodesy.h"
+#include "Controler/Controler.h"
+#include "AngleUtils.h"

 #include <iostream>
 #include <string>
@@ -110,6 +112,18 @@ struct AUV150_Status
    unsigned char  reserved;       // 30. 预留 @55   Uint8   单位:—         
    unsigned char  payloadStatus;  // 28. 抛载状态反馈 @53 Uint8   单位:—         0xFF: 默认，0x00: 已抛载
    unsigned char dvlStatus;      // 29. DVL传感器状态反馈 @54 Uint8   单位:—       0x00: 关，0x01: 开
+    //以下为转换状态
+    double x;
+    double y;
+    double z;
+
+    double heading_error;
+    double pitch_error;
+    double depth_error;
+
+    double desired_rudder;
+    double desired_elevator;
+    double desired_thrust;
 };

 class AUV150 : public AppCastingMOOSApp
@@ -179,6 +193,11 @@ private: // State variables

  double m_controlTime;
  double m_samplingTime;
+
+  bool m_overrived;
+  
+  //控制器
+  static Controler m_control;
 };

 #endif
--- a/src/pAUV150/CMakeLists.txt
+++ b/src/pAUV150/CMakeLists.txt
@@ -12,10 +12,22 @@ message(STATUS "Manually set MOOSGeodesy paths:")
 message(STATUS "  - Include dirs: ${MOOSGEODESY_INCLUDE_DIRS}")
 message(STATUS "  - Libraries: ${MOOSGEODESY_LIBRARIES}")

+file(GLOB SRC_FILES 
+  "*.cpp"
+  "${CMAKE_CURRENT_SOURCE_DIR}/Controler/*.cpp"
+)
+
 SET(SRC
  AUV150.cpp
  AUV150_Info.cpp
  main.cpp
+  ${SRC_FILES}
+)
+
+# 添加头文件目录
+include_directories(
+  ${MOOSGEODESY_INCLUDE_DIRS}
+  ${CMAKE_CURRENT_SOURCE_DIR}/Controler
 )

 ADD_EXECUTABLE(pAUV150 ${SRC})
@@ -23,6 +35,7 @@ ADD_EXECUTABLE(pAUV150 ${SRC})
 TARGET_LINK_LIBRARIES(pAUV150
   ${MOOS_LIBRARIES}
   ${MOOSGEODESY_LIBRARIES}
+   geometry
   apputil
   mbutil
   m
--- a/src/pAUV150/Controler/Controler.cpp
+++ b/src/pAUV150/Controler/Controler.cpp
@@ -0,0 +1,442 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+//
+// File: Controler.cpp
+//
+// Code generated for Simulink model 'Controler'.
+//
+// Model version                  : 5.56
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+//
+// Target selection: ert.tlc
+// Embedded hardware selection: Intel->x86-64 (Linux 64)
+// Code generation objectives:
+//    1. Execution efficiency
+//    2. RAM efficiency
+// Validation result: Not run
+//
+#include "Controler.h"
+#include <cmath>
+#include "rtwtypes.h"
+#include "cmath"
+
+// Model step function
+void Controler::step()
+{
+  real_T mod_error;
+  real_T rtb_FilterCoefficient;
+  real_T rtb_FilterCoefficient_a;
+  real_T rtb_FilterCoefficient_lc;
+  real_T rtb_Filter_b;
+  real_T rtb_IntegralGain;
+  real_T rtb_Integrator_d;
+  real_T rtb_Switch;
+  real_T rtb_depth_error;
+  int8_T rtb_Switch1;
+  int8_T tmp;
+  boolean_T rtb_RelationalOperator;
+  boolean_T rtb_fixforDTpropagationissue;
+
+  // Outputs for Atomic SubSystem: '<Root>/Controler'
+  // MATLAB Function: '<S1>/MATLAB Function' incorporates:
+  //   Inport: '<Root>/heading_cmd'
+  //   Inport: '<Root>/psi'
+
+  mod_error = rtU.heading_cmd - rtU.psi;
+  if (!std::isnan(mod_error)) {
+    if (std::isinf(mod_error)) {
+      mod_error = (rtNaN);
+    } else if (mod_error == 0.0) {
+      mod_error = 0.0;
+    } else {
+      mod_error = std::fmod(mod_error, 360.0);
+      if (mod_error == 0.0) {
+        mod_error = 0.0;
+      } else if (mod_error < 0.0) {
+        mod_error += 360.0;
+      }
+    }
+  }
+
+  if (mod_error >= 180.0) {
+    mod_error -= 360.0;
+  }
+
+  // End of MATLAB Function: '<S1>/MATLAB Function'
+
+  // Gain: '<S45>/Filter Coefficient' incorporates:
+  //   DiscreteIntegrator: '<S37>/Filter'
+  //   Gain: '<S35>/Derivative Gain'
+  //   Sum: '<S37>/SumD'
+
+  rtb_FilterCoefficient = (rtP.pid_heading.D * mod_error - rtDW.Filter_DSTATE) *
+    rtP.pid_heading.N;
+
+  // Sum: '<S51>/Sum' incorporates:
+  //   DiscreteIntegrator: '<S42>/Integrator'
+  //   Gain: '<S47>/Proportional Gain'
+
+  rtb_IntegralGain = (rtP.pid_heading.P * mod_error + rtDW.Integrator_DSTATE) +
+    rtb_FilterCoefficient;
+
+  // Saturate: '<S49>/Saturation' incorporates:
+  //   DeadZone: '<S34>/DeadZone'
+
+  if (rtb_IntegralGain > 35.0) {
+    // Outport: '<Root>/DirectUpperRudderServoAngleCmd'
+    rtY.DirectUpperRudderServoAngleCmd = 35.0;
+    rtb_IntegralGain -= 35.0;
+  } else {
+    if (rtb_IntegralGain < -35.0) {
+      // Outport: '<Root>/DirectUpperRudderServoAngleCmd'
+      rtY.DirectUpperRudderServoAngleCmd = -35.0;
+    } else {
+      // Outport: '<Root>/DirectUpperRudderServoAngleCmd'
+      rtY.DirectUpperRudderServoAngleCmd = rtb_IntegralGain;
+    }
+
+    if (rtb_IntegralGain >= -35.0) {
+      rtb_IntegralGain = 0.0;
+    } else {
+      rtb_IntegralGain -= -35.0;
+    }
+  }
+
+  // End of Saturate: '<S49>/Saturation'
+
+  // RelationalOperator: '<S32>/Relational Operator' incorporates:
+  //   Constant: '<S32>/Clamping_zero'
+
+  rtb_RelationalOperator = (rtb_IntegralGain != 0.0);
+
+  // RelationalOperator: '<S32>/fix for DT propagation issue' incorporates:
+  //   Constant: '<S32>/Clamping_zero'
+
+  rtb_fixforDTpropagationissue = (rtb_IntegralGain > 0.0);
+
+  // Gain: '<S39>/Integral Gain'
+  rtb_IntegralGain = rtP.pid_heading.I * mod_error;
+
+  // Switch: '<S32>/Switch1' incorporates:
+  //   Constant: '<S32>/Constant'
+  //   Constant: '<S32>/Constant2'
+
+  if (rtb_fixforDTpropagationissue) {
+    tmp = 1;
+  } else {
+    tmp = -1;
+  }
+
+  // Switch: '<S32>/Switch2' incorporates:
+  //   Constant: '<S32>/Clamping_zero'
+  //   Constant: '<S32>/Constant3'
+  //   Constant: '<S32>/Constant4'
+  //   RelationalOperator: '<S32>/fix for DT propagation issue1'
+
+  if (rtb_IntegralGain > 0.0) {
+    rtb_Switch1 = 1;
+  } else {
+    rtb_Switch1 = -1;
+  }
+
+  // Switch: '<S32>/Switch' incorporates:
+  //   Constant: '<S32>/Constant1'
+  //   Logic: '<S32>/AND3'
+  //   RelationalOperator: '<S32>/Equal1'
+  //   Switch: '<S32>/Switch1'
+  //   Switch: '<S32>/Switch2'
+
+  if (rtb_RelationalOperator && (tmp == rtb_Switch1)) {
+    rtb_Switch = 0.0;
+  } else {
+    rtb_Switch = rtb_IntegralGain;
+  }
+
+  // End of Switch: '<S32>/Switch'
+
+  // Sum: '<S1>/Sum1' incorporates:
+  //   Inport: '<Root>/depth_cmd'
+  //   Inport: '<Root>/z'
+
+  rtb_depth_error = rtU.z - rtU.depth_cmd;
+
+  // Gain: '<S93>/Integral Gain'
+  rtb_IntegralGain = rtP.pid_depth.I * rtb_depth_error;
+
+  // Gain: '<S99>/Filter Coefficient' incorporates:
+  //   DiscreteIntegrator: '<S91>/Filter'
+  //   Gain: '<S89>/Derivative Gain'
+  //   Sum: '<S91>/SumD'
+
+  rtb_FilterCoefficient_lc = (rtP.pid_depth.D * rtb_depth_error -
+    rtDW.Filter_DSTATE_m) * rtP.pid_depth.N;
+
+  // Sum: '<S105>/Sum' incorporates:
+  //   DiscreteIntegrator: '<S96>/Integrator'
+  //   Gain: '<S101>/Proportional Gain'
+
+  rtb_Filter_b = (rtP.pid_depth.P * rtb_depth_error + rtDW.Integrator_DSTATE_o)
+    + rtb_FilterCoefficient_lc;
+
+  // DeadZone: '<S88>/DeadZone'
+  if (rtb_Filter_b > 30.0) {
+    rtb_Integrator_d = rtb_Filter_b - 30.0;
+
+    // Switch: '<S86>/Switch1' incorporates:
+    //   Constant: '<S86>/Constant'
+
+    tmp = 1;
+  } else {
+    if (rtb_Filter_b >= -30.0) {
+      rtb_Integrator_d = 0.0;
+    } else {
+      rtb_Integrator_d = rtb_Filter_b - -30.0;
+    }
+
+    // Switch: '<S86>/Switch1' incorporates:
+    //   Constant: '<S86>/Constant2'
+
+    tmp = -1;
+  }
+
+  // End of DeadZone: '<S88>/DeadZone'
+
+  // Switch: '<S86>/Switch2' incorporates:
+  //   Constant: '<S86>/Clamping_zero'
+  //   Constant: '<S86>/Constant3'
+  //   Constant: '<S86>/Constant4'
+  //   RelationalOperator: '<S86>/fix for DT propagation issue1'
+
+  if (rtb_IntegralGain > 0.0) {
+    rtb_Switch1 = 1;
+  } else {
+    rtb_Switch1 = -1;
+  }
+
+  // Switch: '<S86>/Switch' incorporates:
+  //   Constant: '<S86>/Clamping_zero'
+  //   Constant: '<S86>/Constant1'
+  //   Logic: '<S86>/AND3'
+  //   RelationalOperator: '<S86>/Equal1'
+  //   RelationalOperator: '<S86>/Relational Operator'
+  //   Switch: '<S86>/Switch1'
+  //   Switch: '<S86>/Switch2'
+
+  if ((rtb_Integrator_d != 0.0) && (tmp == rtb_Switch1)) {
+    rtb_Integrator_d = 0.0;
+  } else {
+    rtb_Integrator_d = rtb_IntegralGain;
+  }
+
+  // End of Switch: '<S86>/Switch'
+
+  // Saturate: '<S103>/Saturation'
+  if (rtb_Filter_b > 30.0) {
+    rtb_Filter_b = 30.0;
+  } else if (rtb_Filter_b < -30.0) {
+    rtb_Filter_b = -30.0;
+  }
+
+  // Sum: '<S1>/Sum2' incorporates:
+  //   Inport: '<Root>/theta'
+  //   Saturate: '<S103>/Saturation'
+
+  rtb_Filter_b -= rtU.theta;
+
+  // Gain: '<S153>/Filter Coefficient' incorporates:
+  //   DiscreteIntegrator: '<S145>/Filter'
+  //   Gain: '<S143>/Derivative Gain'
+  //   Sum: '<S145>/SumD'
+
+  rtb_FilterCoefficient_a = (rtP.pid_pitch.D * rtb_Filter_b -
+    rtDW.Filter_DSTATE_l) * rtP.pid_pitch.N;
+
+  // Sum: '<S159>/Sum' incorporates:
+  //   DiscreteIntegrator: '<S150>/Integrator'
+  //   Gain: '<S155>/Proportional Gain'
+
+  rtb_IntegralGain = (rtP.pid_pitch.P * rtb_Filter_b + rtDW.Integrator_DSTATE_f)
+    + rtb_FilterCoefficient_a;
+
+  // Saturate: '<S157>/Saturation' incorporates:
+  //   DeadZone: '<S142>/DeadZone'
+
+  if (rtb_IntegralGain > 35.0) {
+    // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
+    rtY.DirectLeftRudderServoAngleCmd = 35.0;
+    rtb_IntegralGain -= 35.0;
+  } else {
+    if (rtb_IntegralGain < -35.0) {
+      // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
+      rtY.DirectLeftRudderServoAngleCmd = -35.0;
+    } else {
+      // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
+      rtY.DirectLeftRudderServoAngleCmd = rtb_IntegralGain;
+    }
+
+    if (rtb_IntegralGain >= -35.0) {
+      rtb_IntegralGain = 0.0;
+    } else {
+      rtb_IntegralGain -= -35.0;
+    }
+  }
+
+  // End of Saturate: '<S157>/Saturation'
+
+  // RelationalOperator: '<S140>/Relational Operator' incorporates:
+  //   Constant: '<S140>/Clamping_zero'
+
+  rtb_RelationalOperator = (rtb_IntegralGain != 0.0);
+
+  // Switch: '<S140>/Switch1' incorporates:
+  //   Constant: '<S140>/Clamping_zero'
+  //   Constant: '<S140>/Constant'
+  //   Constant: '<S140>/Constant2'
+  //   RelationalOperator: '<S140>/fix for DT propagation issue'
+
+  if (rtb_IntegralGain > 0.0) {
+    rtb_Switch1 = 1;
+  } else {
+    rtb_Switch1 = -1;
+  }
+
+  // End of Switch: '<S140>/Switch1'
+
+  // Gain: '<S147>/Integral Gain'
+  rtb_IntegralGain = rtP.pid_pitch.I * rtb_Filter_b;
+
+  // Update for DiscreteIntegrator: '<S42>/Integrator'
+  rtDW.Integrator_DSTATE += rtb_Switch;
+  if (rtDW.Integrator_DSTATE > 15.0) {
+    rtDW.Integrator_DSTATE = 15.0;
+  } else if (rtDW.Integrator_DSTATE < -15.0) {
+    rtDW.Integrator_DSTATE = -15.0;
+  }
+
+  // End of Update for DiscreteIntegrator: '<S42>/Integrator'
+
+  // Update for DiscreteIntegrator: '<S37>/Filter'
+  rtDW.Filter_DSTATE += 0.1 * rtb_FilterCoefficient;
+
+  // Update for DiscreteIntegrator: '<S96>/Integrator'
+  rtDW.Integrator_DSTATE_o += rtb_Integrator_d;
+  if (rtDW.Integrator_DSTATE_o > 10.0) {
+    rtDW.Integrator_DSTATE_o = 10.0;
+  } else if (rtDW.Integrator_DSTATE_o < -10.0) {
+    rtDW.Integrator_DSTATE_o = -10.0;
+  }
+
+  // End of Update for DiscreteIntegrator: '<S96>/Integrator'
+
+  // Update for DiscreteIntegrator: '<S91>/Filter'
+  rtDW.Filter_DSTATE_m += 0.1 * rtb_FilterCoefficient_lc;
+
+  // Switch: '<S140>/Switch2' incorporates:
+  //   Constant: '<S140>/Clamping_zero'
+  //   Constant: '<S140>/Constant3'
+  //   Constant: '<S140>/Constant4'
+  //   RelationalOperator: '<S140>/fix for DT propagation issue1'
+
+  if (rtb_IntegralGain > 0.0) {
+    tmp = 1;
+  } else {
+    tmp = -1;
+  }
+
+  // Switch: '<S140>/Switch' incorporates:
+  //   Constant: '<S140>/Constant1'
+  //   Logic: '<S140>/AND3'
+  //   RelationalOperator: '<S140>/Equal1'
+  //   Switch: '<S140>/Switch2'
+
+  if (rtb_RelationalOperator && (rtb_Switch1 == tmp)) {
+    rtb_IntegralGain = 0.0;
+  }
+
+  // Update for DiscreteIntegrator: '<S150>/Integrator' incorporates:
+  //   Switch: '<S140>/Switch'
+
+  rtDW.Integrator_DSTATE_f += rtb_IntegralGain;
+  if (rtDW.Integrator_DSTATE_f > 15.0) {
+    rtDW.Integrator_DSTATE_f = 15.0;
+  } else if (rtDW.Integrator_DSTATE_f < -15.0) {
+    rtDW.Integrator_DSTATE_f = -15.0;
+  }
+
+  // End of Update for DiscreteIntegrator: '<S150>/Integrator'
+
+  // Update for DiscreteIntegrator: '<S145>/Filter'
+  rtDW.Filter_DSTATE_l += 0.1 * rtb_FilterCoefficient_a;
+
+  // End of Outputs for SubSystem: '<Root>/Controler'
+
+  // Outport: '<Root>/MainThrusterSpeedCmd' incorporates:
+  //   Constant: '<S1>/Constant'
+
+  rtY.MainThrusterSpeedCmd = 100.0;
+
+  // Outport: '<Root>/DirectLowerRudderServoAngleCmd' incorporates:
+  //   Constant: '<S1>/Constant2'
+
+  rtY.DirectLowerRudderServoAngleCmd = 1.0;
+
+  // Outport: '<Root>/DirectRightRudderServoAngleCmd' incorporates:
+  //   Constant: '<S1>/Constant4'
+
+  rtY.DirectRightRudderServoAngleCmd = 1.0;
+
+  // Outport: '<Root>/heading_error'
+  rtY.heading_error = mod_error;
+
+  // Outport: '<Root>/pitch_error'
+  rtY.pitch_error = rtb_Filter_b;
+
+  // Outport: '<Root>/depth_error'
+  rtY.depth_error = rtb_depth_error;
+}
+
+// Model initialize function
+void Controler::initialize()
+{
+  // (no initialization code required)
+}
+
+const char_T* Controler::RT_MODEL::getErrorStatus() const
+{
+  return (errorStatus);
+}
+
+void Controler::RT_MODEL::setErrorStatus(const char_T* const volatile
+  aErrorStatus)
+{
+  (errorStatus = aErrorStatus);
+}
+
+// Constructor
+Controler::Controler() :
+  rtU(),
+  rtY(),
+  rtDW(),
+  rtM()
+{
+  // Currently there is no constructor body generated.
+}
+
+// Destructor
+// Currently there is no destructor body generated.
+Controler::~Controler() = default;
+
+// Real-Time Model get method
+Controler::RT_MODEL * Controler::getRTM()
+{
+  return (&rtM);
+}
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//
--- a/src/pAUV150/Controler/Controler.h
+++ b/src/pAUV150/Controler/Controler.h
@@ -0,0 +1,360 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+//
+// File: Controler.h
+//
+// Code generated for Simulink model 'Controler'.
+//
+// Model version                  : 5.56
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+//
+// Target selection: ert.tlc
+// Embedded hardware selection: Intel->x86-64 (Linux 64)
+// Code generation objectives:
+//    1. Execution efficiency
+//    2. RAM efficiency
+// Validation result: Not run
+//
+#ifndef Controler_h_
+#define Controler_h_
+#include <cmath>
+#include "rtwtypes.h"
+#include "rt_nonfinite.h"
+#include "Controler_types.h"
+
+extern "C"
+{
+
+#include "rtGetNaN.h"
+
+}
+
+// Class declaration for model Controler
+class Controler final
+{
+  // public data and function members
+ public:
+  // Block signals and states (default storage) for system '<Root>'
+  struct DW {
+    real_T Integrator_DSTATE;          // '<S42>/Integrator'
+    real_T Filter_DSTATE;              // '<S37>/Filter'
+    real_T Integrator_DSTATE_o;        // '<S96>/Integrator'
+    real_T Filter_DSTATE_m;            // '<S91>/Filter'
+    real_T Integrator_DSTATE_f;        // '<S150>/Integrator'
+    real_T Filter_DSTATE_l;            // '<S145>/Filter'
+  };
+
+  // External inputs (root inport signals with default storage)
+  struct ExtU {
+    real_T u;                          // '<Root>/u'
+    real_T v;                          // '<Root>/v'
+    real_T w;                          // '<Root>/w'
+    real_T phi;                        // '<Root>/phi'
+    real_T theta;                      // '<Root>/theta'
+    real_T psi;                        // '<Root>/psi'
+    real_T x;                          // '<Root>/x'
+    real_T y;                          // '<Root>/y'
+    real_T z;                          // '<Root>/z'
+    real_T dx;                         // '<Root>/dx'
+    real_T dy;                         // '<Root>/dy'
+    real_T dz;                         // '<Root>/dz'
+    real_T speed_cmd;                  // '<Root>/speed_cmd'
+    real_T heading_cmd;                // '<Root>/heading_cmd'
+    real_T depth_cmd;                  // '<Root>/depth_cmd'
+  };
+
+  // External outputs (root outports fed by signals with default storage)
+  struct ExtY {
+    real_T MainThrusterSpeedCmd;       // '<Root>/MainThrusterSpeedCmd'
+    real_T DirectUpperRudderServoAngleCmd;
+                                     // '<Root>/DirectUpperRudderServoAngleCmd'
+    real_T DirectLowerRudderServoAngleCmd;
+                                     // '<Root>/DirectLowerRudderServoAngleCmd'
+    real_T DirectLeftRudderServoAngleCmd;
+                                      // '<Root>/DirectLeftRudderServoAngleCmd'
+    real_T DirectRightRudderServoAngleCmd;
+                                     // '<Root>/DirectRightRudderServoAngleCmd'
+    real_T heading_error;              // '<Root>/heading_error'
+    real_T pitch_error;                // '<Root>/pitch_error'
+    real_T depth_error;                // '<Root>/depth_error'
+  };
+
+  // Parameters (default storage)
+  struct P {
+    struct_n3jlyiLoC0PV1TieyhpPwD pid_depth;// Variable: pid_depth
+                                               //  Referenced by:
+                                               //    '<S89>/Derivative Gain'
+                                               //    '<S93>/Integral Gain'
+                                               //    '<S99>/Filter Coefficient'
+                                               //    '<S101>/Proportional Gain'
+
+    struct_n3jlyiLoC0PV1TieyhpPwD pid_heading;// Variable: pid_heading
+                                                 //  Referenced by:
+                                                 //    '<S35>/Derivative Gain'
+                                                 //    '<S39>/Integral Gain'
+                                                 //    '<S45>/Filter Coefficient'
+                                                 //    '<S47>/Proportional Gain'
+
+    struct_n3jlyiLoC0PV1TieyhpPwD pid_pitch;// Variable: pid_pitch
+                                               //  Referenced by:
+                                               //    '<S143>/Derivative Gain'
+                                               //    '<S147>/Integral Gain'
+                                               //    '<S153>/Filter Coefficient'
+                                               //    '<S155>/Proportional Gain'
+
+  };
+
+  // Real-time Model Data Structure
+  struct RT_MODEL {
+    const char_T * volatile errorStatus;
+    const char_T* getErrorStatus() const;
+    void setErrorStatus(const char_T* const volatile aErrorStatus);
+  };
+
+  // Copy Constructor
+  Controler(Controler const&) = delete;
+
+  // Assignment Operator
+  Controler& operator= (Controler const&) & = delete;
+
+  // Move Constructor
+  Controler(Controler &&) = delete;
+
+  // Move Assignment Operator
+  Controler& operator= (Controler &&) = delete;
+
+  // Real-Time Model get method
+  Controler::RT_MODEL * getRTM();
+
+  // External inputs
+  ExtU rtU;
+
+  // External outputs
+  ExtY rtY;
+
+  // model initialize function
+  static void initialize();
+
+  // model step function
+  void step();
+
+  // Constructor
+  Controler();
+
+  // Destructor
+  ~Controler();
+
+  // private data and function members
+ private:
+  // Block states
+  DW rtDW;
+
+  // Tunable parameters
+  static P rtP;
+
+  // Real-Time Model
+  RT_MODEL rtM;
+};
+
+//-
+//  These blocks were eliminated from the model due to optimizations:
+//
+//  Block '<S1>/Scope' : Unused code path elimination
+
+
+//-
+//  The generated code includes comments that allow you to trace directly
+//  back to the appropriate location in the model.  The basic format
+//  is <system>/block_name, where system is the system number (uniquely
+//  assigned by Simulink) and block_name is the name of the block.
+//
+//  Note that this particular code originates from a subsystem build,
+//  and has its own system numbers different from the parent model.
+//  Refer to the system hierarchy for this subsystem below, and use the
+//  MATLAB hilite_system command to trace the generated code back
+//  to the parent model.  For example,
+//
+//  hilite_system('SimModel0618/Controler')    - opens subsystem SimModel0618/Controler
+//  hilite_system('SimModel0618/Controler/Kp') - opens and selects block Kp
+//
+//  Here is the system hierarchy for this model
+//
+//  '<Root>' : 'SimModel0618'
+//  '<S1>'   : 'SimModel0618/Controler'
+//  '<S2>'   : 'SimModel0618/Controler/MATLAB Function'
+//  '<S3>'   : 'SimModel0618/Controler/PID Controller'
+//  '<S4>'   : 'SimModel0618/Controler/PID Controller1'
+//  '<S5>'   : 'SimModel0618/Controler/PID Controller2'
+//  '<S6>'   : 'SimModel0618/Controler/PID Controller/Anti-windup'
+//  '<S7>'   : 'SimModel0618/Controler/PID Controller/D Gain'
+//  '<S8>'   : 'SimModel0618/Controler/PID Controller/External Derivative'
+//  '<S9>'   : 'SimModel0618/Controler/PID Controller/Filter'
+//  '<S10>'  : 'SimModel0618/Controler/PID Controller/Filter ICs'
+//  '<S11>'  : 'SimModel0618/Controler/PID Controller/I Gain'
+//  '<S12>'  : 'SimModel0618/Controler/PID Controller/Ideal P Gain'
+//  '<S13>'  : 'SimModel0618/Controler/PID Controller/Ideal P Gain Fdbk'
+//  '<S14>'  : 'SimModel0618/Controler/PID Controller/Integrator'
+//  '<S15>'  : 'SimModel0618/Controler/PID Controller/Integrator ICs'
+//  '<S16>'  : 'SimModel0618/Controler/PID Controller/N Copy'
+//  '<S17>'  : 'SimModel0618/Controler/PID Controller/N Gain'
+//  '<S18>'  : 'SimModel0618/Controler/PID Controller/P Copy'
+//  '<S19>'  : 'SimModel0618/Controler/PID Controller/Parallel P Gain'
+//  '<S20>'  : 'SimModel0618/Controler/PID Controller/Reset Signal'
+//  '<S21>'  : 'SimModel0618/Controler/PID Controller/Saturation'
+//  '<S22>'  : 'SimModel0618/Controler/PID Controller/Saturation Fdbk'
+//  '<S23>'  : 'SimModel0618/Controler/PID Controller/Sum'
+//  '<S24>'  : 'SimModel0618/Controler/PID Controller/Sum Fdbk'
+//  '<S25>'  : 'SimModel0618/Controler/PID Controller/Tracking Mode'
+//  '<S26>'  : 'SimModel0618/Controler/PID Controller/Tracking Mode Sum'
+//  '<S27>'  : 'SimModel0618/Controler/PID Controller/Tsamp - Integral'
+//  '<S28>'  : 'SimModel0618/Controler/PID Controller/Tsamp - Ngain'
+//  '<S29>'  : 'SimModel0618/Controler/PID Controller/postSat Signal'
+//  '<S30>'  : 'SimModel0618/Controler/PID Controller/preInt Signal'
+//  '<S31>'  : 'SimModel0618/Controler/PID Controller/preSat Signal'
+//  '<S32>'  : 'SimModel0618/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel'
+//  '<S33>'  : 'SimModel0618/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel/Dead Zone'
+//  '<S34>'  : 'SimModel0618/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
+//  '<S35>'  : 'SimModel0618/Controler/PID Controller/D Gain/Internal Parameters'
+//  '<S36>'  : 'SimModel0618/Controler/PID Controller/External Derivative/Error'
+//  '<S37>'  : 'SimModel0618/Controler/PID Controller/Filter/Disc. Forward Euler Filter'
+//  '<S38>'  : 'SimModel0618/Controler/PID Controller/Filter ICs/Internal IC - Filter'
+//  '<S39>'  : 'SimModel0618/Controler/PID Controller/I Gain/Internal Parameters'
+//  '<S40>'  : 'SimModel0618/Controler/PID Controller/Ideal P Gain/Passthrough'
+//  '<S41>'  : 'SimModel0618/Controler/PID Controller/Ideal P Gain Fdbk/Disabled'
+//  '<S42>'  : 'SimModel0618/Controler/PID Controller/Integrator/Discrete'
+//  '<S43>'  : 'SimModel0618/Controler/PID Controller/Integrator ICs/Internal IC'
+//  '<S44>'  : 'SimModel0618/Controler/PID Controller/N Copy/Disabled'
+//  '<S45>'  : 'SimModel0618/Controler/PID Controller/N Gain/Internal Parameters'
+//  '<S46>'  : 'SimModel0618/Controler/PID Controller/P Copy/Disabled'
+//  '<S47>'  : 'SimModel0618/Controler/PID Controller/Parallel P Gain/Internal Parameters'
+//  '<S48>'  : 'SimModel0618/Controler/PID Controller/Reset Signal/Disabled'
+//  '<S49>'  : 'SimModel0618/Controler/PID Controller/Saturation/Enabled'
+//  '<S50>'  : 'SimModel0618/Controler/PID Controller/Saturation Fdbk/Disabled'
+//  '<S51>'  : 'SimModel0618/Controler/PID Controller/Sum/Sum_PID'
+//  '<S52>'  : 'SimModel0618/Controler/PID Controller/Sum Fdbk/Disabled'
+//  '<S53>'  : 'SimModel0618/Controler/PID Controller/Tracking Mode/Disabled'
+//  '<S54>'  : 'SimModel0618/Controler/PID Controller/Tracking Mode Sum/Passthrough'
+//  '<S55>'  : 'SimModel0618/Controler/PID Controller/Tsamp - Integral/TsSignalSpecification'
+//  '<S56>'  : 'SimModel0618/Controler/PID Controller/Tsamp - Ngain/Passthrough'
+//  '<S57>'  : 'SimModel0618/Controler/PID Controller/postSat Signal/Forward_Path'
+//  '<S58>'  : 'SimModel0618/Controler/PID Controller/preInt Signal/Internal PreInt'
+//  '<S59>'  : 'SimModel0618/Controler/PID Controller/preSat Signal/Forward_Path'
+//  '<S60>'  : 'SimModel0618/Controler/PID Controller1/Anti-windup'
+//  '<S61>'  : 'SimModel0618/Controler/PID Controller1/D Gain'
+//  '<S62>'  : 'SimModel0618/Controler/PID Controller1/External Derivative'
+//  '<S63>'  : 'SimModel0618/Controler/PID Controller1/Filter'
+//  '<S64>'  : 'SimModel0618/Controler/PID Controller1/Filter ICs'
+//  '<S65>'  : 'SimModel0618/Controler/PID Controller1/I Gain'
+//  '<S66>'  : 'SimModel0618/Controler/PID Controller1/Ideal P Gain'
+//  '<S67>'  : 'SimModel0618/Controler/PID Controller1/Ideal P Gain Fdbk'
+//  '<S68>'  : 'SimModel0618/Controler/PID Controller1/Integrator'
+//  '<S69>'  : 'SimModel0618/Controler/PID Controller1/Integrator ICs'
+//  '<S70>'  : 'SimModel0618/Controler/PID Controller1/N Copy'
+//  '<S71>'  : 'SimModel0618/Controler/PID Controller1/N Gain'
+//  '<S72>'  : 'SimModel0618/Controler/PID Controller1/P Copy'
+//  '<S73>'  : 'SimModel0618/Controler/PID Controller1/Parallel P Gain'
+//  '<S74>'  : 'SimModel0618/Controler/PID Controller1/Reset Signal'
+//  '<S75>'  : 'SimModel0618/Controler/PID Controller1/Saturation'
+//  '<S76>'  : 'SimModel0618/Controler/PID Controller1/Saturation Fdbk'
+//  '<S77>'  : 'SimModel0618/Controler/PID Controller1/Sum'
+//  '<S78>'  : 'SimModel0618/Controler/PID Controller1/Sum Fdbk'
+//  '<S79>'  : 'SimModel0618/Controler/PID Controller1/Tracking Mode'
+//  '<S80>'  : 'SimModel0618/Controler/PID Controller1/Tracking Mode Sum'
+//  '<S81>'  : 'SimModel0618/Controler/PID Controller1/Tsamp - Integral'
+//  '<S82>'  : 'SimModel0618/Controler/PID Controller1/Tsamp - Ngain'
+//  '<S83>'  : 'SimModel0618/Controler/PID Controller1/postSat Signal'
+//  '<S84>'  : 'SimModel0618/Controler/PID Controller1/preInt Signal'
+//  '<S85>'  : 'SimModel0618/Controler/PID Controller1/preSat Signal'
+//  '<S86>'  : 'SimModel0618/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel'
+//  '<S87>'  : 'SimModel0618/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone'
+//  '<S88>'  : 'SimModel0618/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
+//  '<S89>'  : 'SimModel0618/Controler/PID Controller1/D Gain/Internal Parameters'
+//  '<S90>'  : 'SimModel0618/Controler/PID Controller1/External Derivative/Error'
+//  '<S91>'  : 'SimModel0618/Controler/PID Controller1/Filter/Disc. Forward Euler Filter'
+//  '<S92>'  : 'SimModel0618/Controler/PID Controller1/Filter ICs/Internal IC - Filter'
+//  '<S93>'  : 'SimModel0618/Controler/PID Controller1/I Gain/Internal Parameters'
+//  '<S94>'  : 'SimModel0618/Controler/PID Controller1/Ideal P Gain/Passthrough'
+//  '<S95>'  : 'SimModel0618/Controler/PID Controller1/Ideal P Gain Fdbk/Disabled'
+//  '<S96>'  : 'SimModel0618/Controler/PID Controller1/Integrator/Discrete'
+//  '<S97>'  : 'SimModel0618/Controler/PID Controller1/Integrator ICs/Internal IC'
+//  '<S98>'  : 'SimModel0618/Controler/PID Controller1/N Copy/Disabled'
+//  '<S99>'  : 'SimModel0618/Controler/PID Controller1/N Gain/Internal Parameters'
+//  '<S100>' : 'SimModel0618/Controler/PID Controller1/P Copy/Disabled'
+//  '<S101>' : 'SimModel0618/Controler/PID Controller1/Parallel P Gain/Internal Parameters'
+//  '<S102>' : 'SimModel0618/Controler/PID Controller1/Reset Signal/Disabled'
+//  '<S103>' : 'SimModel0618/Controler/PID Controller1/Saturation/Enabled'
+//  '<S104>' : 'SimModel0618/Controler/PID Controller1/Saturation Fdbk/Disabled'
+//  '<S105>' : 'SimModel0618/Controler/PID Controller1/Sum/Sum_PID'
+//  '<S106>' : 'SimModel0618/Controler/PID Controller1/Sum Fdbk/Disabled'
+//  '<S107>' : 'SimModel0618/Controler/PID Controller1/Tracking Mode/Disabled'
+//  '<S108>' : 'SimModel0618/Controler/PID Controller1/Tracking Mode Sum/Passthrough'
+//  '<S109>' : 'SimModel0618/Controler/PID Controller1/Tsamp - Integral/TsSignalSpecification'
+//  '<S110>' : 'SimModel0618/Controler/PID Controller1/Tsamp - Ngain/Passthrough'
+//  '<S111>' : 'SimModel0618/Controler/PID Controller1/postSat Signal/Forward_Path'
+//  '<S112>' : 'SimModel0618/Controler/PID Controller1/preInt Signal/Internal PreInt'
+//  '<S113>' : 'SimModel0618/Controler/PID Controller1/preSat Signal/Forward_Path'
+//  '<S114>' : 'SimModel0618/Controler/PID Controller2/Anti-windup'
+//  '<S115>' : 'SimModel0618/Controler/PID Controller2/D Gain'
+//  '<S116>' : 'SimModel0618/Controler/PID Controller2/External Derivative'
+//  '<S117>' : 'SimModel0618/Controler/PID Controller2/Filter'
+//  '<S118>' : 'SimModel0618/Controler/PID Controller2/Filter ICs'
+//  '<S119>' : 'SimModel0618/Controler/PID Controller2/I Gain'
+//  '<S120>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain'
+//  '<S121>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain Fdbk'
+//  '<S122>' : 'SimModel0618/Controler/PID Controller2/Integrator'
+//  '<S123>' : 'SimModel0618/Controler/PID Controller2/Integrator ICs'
+//  '<S124>' : 'SimModel0618/Controler/PID Controller2/N Copy'
+//  '<S125>' : 'SimModel0618/Controler/PID Controller2/N Gain'
+//  '<S126>' : 'SimModel0618/Controler/PID Controller2/P Copy'
+//  '<S127>' : 'SimModel0618/Controler/PID Controller2/Parallel P Gain'
+//  '<S128>' : 'SimModel0618/Controler/PID Controller2/Reset Signal'
+//  '<S129>' : 'SimModel0618/Controler/PID Controller2/Saturation'
+//  '<S130>' : 'SimModel0618/Controler/PID Controller2/Saturation Fdbk'
+//  '<S131>' : 'SimModel0618/Controler/PID Controller2/Sum'
+//  '<S132>' : 'SimModel0618/Controler/PID Controller2/Sum Fdbk'
+//  '<S133>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode'
+//  '<S134>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode Sum'
+//  '<S135>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Integral'
+//  '<S136>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Ngain'
+//  '<S137>' : 'SimModel0618/Controler/PID Controller2/postSat Signal'
+//  '<S138>' : 'SimModel0618/Controler/PID Controller2/preInt Signal'
+//  '<S139>' : 'SimModel0618/Controler/PID Controller2/preSat Signal'
+//  '<S140>' : 'SimModel0618/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel'
+//  '<S141>' : 'SimModel0618/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel/Dead Zone'
+//  '<S142>' : 'SimModel0618/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
+//  '<S143>' : 'SimModel0618/Controler/PID Controller2/D Gain/Internal Parameters'
+//  '<S144>' : 'SimModel0618/Controler/PID Controller2/External Derivative/Error'
+//  '<S145>' : 'SimModel0618/Controler/PID Controller2/Filter/Disc. Forward Euler Filter'
+//  '<S146>' : 'SimModel0618/Controler/PID Controller2/Filter ICs/Internal IC - Filter'
+//  '<S147>' : 'SimModel0618/Controler/PID Controller2/I Gain/Internal Parameters'
+//  '<S148>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain/Passthrough'
+//  '<S149>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain Fdbk/Disabled'
+//  '<S150>' : 'SimModel0618/Controler/PID Controller2/Integrator/Discrete'
+//  '<S151>' : 'SimModel0618/Controler/PID Controller2/Integrator ICs/Internal IC'
+//  '<S152>' : 'SimModel0618/Controler/PID Controller2/N Copy/Disabled'
+//  '<S153>' : 'SimModel0618/Controler/PID Controller2/N Gain/Internal Parameters'
+//  '<S154>' : 'SimModel0618/Controler/PID Controller2/P Copy/Disabled'
+//  '<S155>' : 'SimModel0618/Controler/PID Controller2/Parallel P Gain/Internal Parameters'
+//  '<S156>' : 'SimModel0618/Controler/PID Controller2/Reset Signal/Disabled'
+//  '<S157>' : 'SimModel0618/Controler/PID Controller2/Saturation/Enabled'
+//  '<S158>' : 'SimModel0618/Controler/PID Controller2/Saturation Fdbk/Disabled'
+//  '<S159>' : 'SimModel0618/Controler/PID Controller2/Sum/Sum_PID'
+//  '<S160>' : 'SimModel0618/Controler/PID Controller2/Sum Fdbk/Disabled'
+//  '<S161>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode/Disabled'
+//  '<S162>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode Sum/Passthrough'
+//  '<S163>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Integral/TsSignalSpecification'
+//  '<S164>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Ngain/Passthrough'
+//  '<S165>' : 'SimModel0618/Controler/PID Controller2/postSat Signal/Forward_Path'
+//  '<S166>' : 'SimModel0618/Controler/PID Controller2/preInt Signal/Internal PreInt'
+//  '<S167>' : 'SimModel0618/Controler/PID Controller2/preSat Signal/Forward_Path'
+
+#endif                                 // Controler_h_
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//
--- a/src/pAUV150/Controler/Controler_data.cpp
+++ b/src/pAUV150/Controler/Controler_data.cpp
@@ -0,0 +1,72 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+//
+// File: Controler_data.cpp
+//
+// Code generated for Simulink model 'Controler'.
+//
+// Model version                  : 5.56
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+//
+// Target selection: ert.tlc
+// Embedded hardware selection: Intel->x86-64 (Linux 64)
+// Code generation objectives:
+//    1. Execution efficiency
+//    2. RAM efficiency
+// Validation result: Not run
+//
+#include "Controler.h"
+
+// Block parameters (default storage)
+Controler::P Controler::rtP{
+  // Variable: pid_depth
+  //  Referenced by:
+  //    '<S89>/Derivative Gain'
+  //    '<S93>/Integral Gain'
+  //    '<S99>/Filter Coefficient'
+  //    '<S101>/Proportional Gain'
+
+  {
+    2.0,
+    0.0,
+    0.0,
+    10.0
+  },
+
+  // Variable: pid_heading
+  //  Referenced by:
+  //    '<S35>/Derivative Gain'
+  //    '<S39>/Integral Gain'
+  //    '<S45>/Filter Coefficient'
+  //    '<S47>/Proportional Gain'
+
+  {
+    1,
+    0.1,
+    0.01,
+    10.0
+  },
+
+  // Variable: pid_pitch
+  //  Referenced by:
+  //    '<S143>/Derivative Gain'
+  //    '<S147>/Integral Gain'
+  //    '<S153>/Filter Coefficient'
+  //    '<S155>/Proportional Gain'
+
+  {
+    0.5,
+    0.0,
+    0.1,
+    10.0
+  }
+};
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//
--- a/src/pAUV150/Controler/Controler_private.h
+++ b/src/pAUV150/Controler/Controler_private.h
@@ -0,0 +1,31 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+//
+// File: Controler_private.h
+//
+// Code generated for Simulink model 'Controler'.
+//
+// Model version                  : 5.56
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+//
+// Target selection: ert.tlc
+// Embedded hardware selection: Intel->x86-64 (Linux 64)
+// Code generation objectives:
+//    1. Execution efficiency
+//    2. RAM efficiency
+// Validation result: Not run
+//
+#ifndef Controler_private_h_
+#define Controler_private_h_
+#include "rtwtypes.h"
+#include "Controler_types.h"
+#endif                                 // Controler_private_h_
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//
--- a/src/pAUV150/Controler/Controler_types.h
+++ b/src/pAUV150/Controler/Controler_types.h
@@ -0,0 +1,42 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+//
+// File: Controler_types.h
+//
+// Code generated for Simulink model 'Controler'.
+//
+// Model version                  : 5.56
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+//
+// Target selection: ert.tlc
+// Embedded hardware selection: Intel->x86-64 (Linux 64)
+// Code generation objectives:
+//    1. Execution efficiency
+//    2. RAM efficiency
+// Validation result: Not run
+//
+#ifndef Controler_types_h_
+#define Controler_types_h_
+#include "rtwtypes.h"
+#ifndef DEFINED_TYPEDEF_FOR_struct_n3jlyiLoC0PV1TieyhpPwD_
+#define DEFINED_TYPEDEF_FOR_struct_n3jlyiLoC0PV1TieyhpPwD_
+
+struct struct_n3jlyiLoC0PV1TieyhpPwD
+{
+  real_T P;
+  real_T I;
+  real_T D;
+  real_T N;
+};
+
+#endif
+#endif                                 // Controler_types_h_
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//
--- a/src/pAUV150/Controler/rtGetNaN.cpp
+++ b/src/pAUV150/Controler/rtGetNaN.cpp
@@ -0,0 +1,50 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+//
+// File: rtGetNaN.cpp
+//
+// Code generated for Simulink model 'Controler'.
+//
+// Model version                  : 5.56
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+//
+// Target selection: ert.tlc
+// Embedded hardware selection: Intel->x86-64 (Linux 64)
+// Code generation objectives:
+//    1. Execution efficiency
+//    2. RAM efficiency
+// Validation result: Not run
+//
+
+#include "rtwtypes.h"
+
+extern "C"
+{
+
+#include "rtGetNaN.h"
+
+}
+
+extern "C"
+{
+  // Return rtNaN needed by the generated code.
+  real_T rtGetNaN(void)
+  {
+    return rtNaN;
+  }
+
+  // Return rtNaNF needed by the generated code.
+  real32_T rtGetNaNF(void)
+  {
+    return rtNaNF;
+  }
+}
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//
--- a/src/pAUV150/Controler/rtGetNaN.h
+++ b/src/pAUV150/Controler/rtGetNaN.h
@@ -0,0 +1,53 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+//
+// File: rtGetNaN.h
+//
+// Code generated for Simulink model 'Controler'.
+//
+// Model version                  : 5.56
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+//
+// Target selection: ert.tlc
+// Embedded hardware selection: Intel->x86-64 (Linux 64)
+// Code generation objectives:
+//    1. Execution efficiency
+//    2. RAM efficiency
+// Validation result: Not run
+//
+#ifndef rtGetNaN_h_
+#define rtGetNaN_h_
+
+extern "C"
+{
+
+#include "rt_nonfinite.h"
+
+}
+
+#include "rtwtypes.h"
+#ifdef __cplusplus
+
+extern "C"
+{
+
+#endif
+
+  extern real_T rtGetNaN(void);
+  extern real32_T rtGetNaNF(void);
+
+#ifdef __cplusplus
+
+}                                      // extern "C"
+
+#endif
+#endif                                 // rtGetNaN_h_
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//
--- a/src/pAUV150/Controler/rt_nonfinite.cpp
+++ b/src/pAUV150/Controler/rt_nonfinite.cpp
@@ -0,0 +1,80 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+//
+// File: rt_nonfinite.cpp
+//
+// Code generated for Simulink model 'Controler'.
+//
+// Model version                  : 5.56
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+//
+// Target selection: ert.tlc
+// Embedded hardware selection: Intel->x86-64 (Linux 64)
+// Code generation objectives:
+//    1. Execution efficiency
+//    2. RAM efficiency
+// Validation result: Not run
+//
+
+#include "rtwtypes.h"
+
+extern "C"
+{
+
+#include "rt_nonfinite.h"
+
+}
+
+#include "limits"
+#include "cmath"
+
+extern "C"
+{
+  real_T rtNaN { -std::numeric_limits<real_T>::quiet_NaN() };
+
+  real_T rtInf { std::numeric_limits<real_T>::infinity() };
+
+  real_T rtMinusInf { -std::numeric_limits<real_T>::infinity() };
+
+  real32_T rtNaNF { -std::numeric_limits<real32_T>::quiet_NaN() };
+
+  real32_T rtInfF { std::numeric_limits<real32_T>::infinity() };
+
+  real32_T rtMinusInfF { -std::numeric_limits<real32_T>::infinity() };
+}
+
+extern "C"
+{
+  // Test if value is infinite
+  boolean_T rtIsInf(real_T value)
+  {
+    return std::isinf(value);
+  }
+
+  // Test if single-precision value is infinite
+  boolean_T rtIsInfF(real32_T value)
+  {
+    return std::isinf(value);
+  }
+
+  // Test if value is not a number
+  boolean_T rtIsNaN(real_T value)
+  {
+    return std::isnan(value);
+  }
+
+  // Test if single-precision value is not a number
+  boolean_T rtIsNaNF(real32_T value)
+  {
+    return std::isnan(value);
+  }
+}
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//
--- a/src/pAUV150/Controler/rt_nonfinite.h
+++ b/src/pAUV150/Controler/rt_nonfinite.h
@@ -0,0 +1,53 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+//
+// File: rt_nonfinite.h
+//
+// Code generated for Simulink model 'Controler'.
+//
+// Model version                  : 5.56
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+//
+// Target selection: ert.tlc
+// Embedded hardware selection: Intel->x86-64 (Linux 64)
+// Code generation objectives:
+//    1. Execution efficiency
+//    2. RAM efficiency
+// Validation result: Not run
+//
+#ifndef rt_nonfinite_h_
+#define rt_nonfinite_h_
+#include "rtwtypes.h"
+#ifdef __cplusplus
+
+extern "C"
+{
+
+#endif
+
+  extern real_T rtInf;
+  extern real_T rtMinusInf;
+  extern real_T rtNaN;
+  extern real32_T rtInfF;
+  extern real32_T rtMinusInfF;
+  extern real32_T rtNaNF;
+  extern boolean_T rtIsInf(real_T value);
+  extern boolean_T rtIsInfF(real32_T value);
+  extern boolean_T rtIsNaN(real_T value);
+  extern boolean_T rtIsNaNF(real32_T value);
+
+#ifdef __cplusplus
+
+}                                      // extern "C"
+
+#endif
+#endif                                 // rt_nonfinite_h_
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//
--- a/src/pAUV150/Controler/rtwtypes.h
+++ b/src/pAUV150/Controler/rtwtypes.h
@@ -0,0 +1,106 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+//
+// File: rtwtypes.h
+//
+// Code generated for Simulink model 'Controler'.
+//
+// Model version                  : 5.56
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+//
+// Target selection: ert.tlc
+// Embedded hardware selection: Intel->x86-64 (Linux 64)
+// Code generation objectives:
+//    1. Execution efficiency
+//    2. RAM efficiency
+// Validation result: Not run
+//
+
+#ifndef RTWTYPES_H
+#define RTWTYPES_H
+
+// Logical type definitions
+#if (!defined(__cplusplus))
+#ifndef false
+#define false                          (0U)
+#endif
+
+#ifndef true
+#define true                           (1U)
+#endif
+#endif
+
+//=======================================================================*
+//  Target hardware information
+//    Device type: Intel->x86-64 (Linux 64)
+//    Number of bits:     char:   8    short:   16    int:  32
+//                        long:  64    long long:  64
+//                        native word size:  64
+//    Byte ordering: LittleEndian
+//    Signed integer division rounds to: Zero
+//    Shift right on a signed integer as arithmetic shift: on
+// =======================================================================
+
+//=======================================================================*
+//  Fixed width word size data types:                                     *
+//    int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+//    uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+//    real32_T, real64_T           - 32 and 64 bit floating point numbers *
+// =======================================================================
+typedef signed char int8_T;
+typedef unsigned char uint8_T;
+typedef short int16_T;
+typedef unsigned short uint16_T;
+typedef int int32_T;
+typedef unsigned int uint32_T;
+typedef long int64_T;
+typedef unsigned long uint64_T;
+typedef float real32_T;
+typedef double real64_T;
+
+//===========================================================================*
+//  Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T,       *
+//                            real_T, time_T, ulong_T, ulonglong_T.           *
+// ===========================================================================
+typedef double real_T;
+typedef double time_T;
+typedef unsigned char boolean_T;
+typedef int int_T;
+typedef unsigned int uint_T;
+typedef unsigned long ulong_T;
+typedef unsigned long long ulonglong_T;
+typedef char char_T;
+typedef unsigned char uchar_T;
+typedef char_T byte_T;
+
+//=======================================================================*
+//  Min and Max:                                                          *
+//    int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+//    uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+// =======================================================================
+#define MAX_int8_T                     ((int8_T)(127))
+#define MIN_int8_T                     ((int8_T)(-128))
+#define MAX_uint8_T                    ((uint8_T)(255U))
+#define MAX_int16_T                    ((int16_T)(32767))
+#define MIN_int16_T                    ((int16_T)(-32768))
+#define MAX_uint16_T                   ((uint16_T)(65535U))
+#define MAX_int32_T                    ((int32_T)(2147483647))
+#define MIN_int32_T                    ((int32_T)(-2147483647-1))
+#define MAX_uint32_T                   ((uint32_T)(0xFFFFFFFFU))
+#define MAX_int64_T                    ((int64_T)(9223372036854775807L))
+#define MIN_int64_T                    ((int64_T)(-9223372036854775807L-1L))
+#define MAX_uint64_T                   ((uint64_T)(0xFFFFFFFFFFFFFFFFUL))
+
+// Block D-Work pointer type
+typedef void * pointer_T;
+
+#endif                                 // RTWTYPES_H
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//
--- a/src/pMotionControler/CMakeLists.txt
+++ b/src/pMotionControler/CMakeLists.txt
@@ -0,0 +1,22 @@
+#--------------------------------------------------------
+# The CMakeLists.txt for:                       pTaskManger
+# Author(s):                              zjk
+#--------------------------------------------------------
+
+FILE(GLOB SRC *.cpp *.hpp)
+
+include_directories(/usr/include/jsoncpp/)
+link_directories(/usr/local/lib/)
+
+ADD_EXECUTABLE(pMotionControler ${SRC})
+
+#需要把被依赖的库放到依赖库的后面
+TARGET_LINK_LIBRARIES(pMotionControler
+   ${MOOS_LIBRARIES}
+   geometry
+   mbutil
+   m
+   pthread
+   jsoncpp
+   )
+
--- a/src/pMotionControler/ControlParam.json
+++ b/src/pMotionControler/ControlParam.json
@@ -0,0 +1,43 @@
+{
+	"speed" : 
+	{
+		"Kp" : 10.0,
+		"Ki" : 5.0,
+		"Kd" : 0.0,
+		"LimitDelta" : 50,
+		"MaxOut" : 100,
+		"MinOut" : 0
+	},
+	"heading" :
+	{
+		"Kp" : 0.8,
+		"Ki" : 0.05,
+		"Kd" : 2.2,
+		"LimitDelta" : 5,
+		"MaxOut" : 30,
+		"MinOut" : -30
+	},
+	"depth" : 
+	{
+		"Kp" : 10.0,
+		"Ki" : 0.3,
+		"Kd" : 2.0,
+		"LimitDelta" : 5,
+		"MaxOut" : 10,    
+		"MinOut" : -10
+	},
+	"pitch" : 
+	{
+		"Kp" : 0.6,
+		"Ki" : 0.03,
+		"Kd" : 1.5,
+		"LimitDelta" : 5,
+		"MaxOut" : 30,
+		"MinOut" : -30
+	},
+	"const_thrust" : 0,
+	"dead_zone" : 10,
+	"speedCol" : true,
+	"depthCol" : true,
+	"HeadingCol" : true
+}
--- a/src/pMotionControler/Controler.cpp
+++ b/src/pMotionControler/Controler.cpp
@@ -0,0 +1,424 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-16 14:04:53
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-02 15:17:38
+ * @FilePath: /moos-ivp-pi/src/pMotionControler/Controler.cpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#include"Controler.hpp"
+
+Controler::Controler()
+{
+    // Error_capacity = 10;
+    initVariable();
+}
+
+void Controler::initVariable()
+{
+    ClearValTim(desired_speed);
+    ClearValTim(desired_heading);
+    ClearValTim(desired_depth);
+    ClearValTim(desired_pitch);
+
+    ClearValTim(current_speed);
+    ClearValTim(current_heading);
+    ClearValTim(current_pitch);
+    ClearValTim(current_depth);
+    current_time = 0;
+    start_time = 0;
+
+    ClearValTim(desired_thrust);
+    ClearValTim(desired_rudder);
+    ClearValTim(desired_elevator);
+
+    iterations = 0;
+    start_time = 0.0;
+
+    tardy_helm_thresh = 2.0;
+    tardy_nav_thresh  = 2.0;
+    // current_error = 0;
+    // last_error = 0;
+    // Error.clear();
+}
+
+bool Controler::setDesSpeed(double spd, double tim)
+{
+    desired_speed.value = spd;
+    desired_speed.time = tim;
+    return true;
+}
+bool Controler::setDesDepth(double dph, double tim)
+{
+    desired_depth.value = dph;
+    desired_depth.time = tim;
+    return true;
+}
+bool Controler::setDesPitch(double pth, double tim)
+{
+    desired_pitch.value = pth;
+    desired_pitch.time = tim;
+    return true;
+}
+bool Controler::setDesHeading(double hdg, double tim)
+{
+    if(is_rad)
+        hdg = radToDegrees(hdg);
+
+    desired_heading.value = angle180(hdg);
+    desired_heading.time = tim;
+    return true;
+}
+bool Controler::setCurSpeed(double spd, double tim)
+{
+    current_speed.value = spd;
+    current_speed.time = tim;
+    return true;
+}
+bool Controler::setCurDepth(double dph, double tim)
+{
+    current_depth.value = dph;
+    current_depth.time = tim;
+    return true;
+}
+bool Controler::setCurPitch(double pth, double tim)
+{
+    current_pitch.value = pth;
+    current_pitch.time = tim;
+    return true;
+}
+bool Controler::setCurHeading(double hdg, double tim)
+{
+    current_heading.value = hdg;
+    current_heading.time = tim;
+    return true;
+}
+bool Controler::setDesiredValues()
+{
+    iterations++;
+
+    desired_thrust.value = 0;
+    desired_rudder.value = 0;
+    desired_elevator.value = 0;
+
+    //=============================================================
+    // Part 1: Ensure all nav and helm messages have been received 
+    // for first time. If not, we simply wait, without declaring an
+    // override. After all messages have been received at least
+    // once, staleness is checked below.
+    //=============================================================
+    runMsg.push_back("desired_heading-time:" + doubleToString(desired_heading.time));
+    runMsg.push_back("desired_speed-time:" + doubleToString(desired_speed.time));
+    runMsg.push_back("current_heading-time:" + doubleToString(current_heading.time));
+    runMsg.push_back("current_speed-time:" + doubleToString(current_speed.time));
+    runMsg.push_back("desired_depth-time:" + doubleToString(desired_depth.time));
+    runMsg.push_back("current_depth-time:" + doubleToString(current_depth.time));
+    runMsg.push_back("current_pitch-time:" + doubleToString(current_pitch.time));
+    // runMsg.push_back("current_depth-time:" + doubleToString(current_depth.time));
+
+    if(has_speedCtrl)
+    {
+        if((desired_heading.time == 0) || (desired_speed.time == 0))
+            return true;
+    }
+    if(has_headCtrl)
+    {
+        if((current_heading.time == 0) || (current_speed.time == 0))
+            return true;
+    }
+    if(has_depthCtrl) 
+    {
+        if(desired_depth.time == 0)
+            return true;
+        if((current_depth.time == 0) || (current_pitch.time == 0))
+            return true;
+    }
+    //=============================================================
+    // Part 2: Critical info staleness (if not in simulation mode)
+    //=============================================================
+    //TODO: 验证信息过时操作
+    if(cheakStalensee) 
+    {
+        bool is_stale = checkForStaleness();
+        if(is_stale) 
+        {
+            has_override = true;
+            return false;
+        }
+    }
+
+    //=============================================================
+    // Part 3: Set the actuator values. Note: If desired thrust is 
+    // zero others desired values will automatically be zero too.
+    //=============================================================
+    
+    // m_desired_thrust = setDesiredThrust();
+
+    // if(m_desired_thrust > 0)
+    // m_desired_rudder = setDesiredRudder();
+
+    // if(m_desired_thrust > 0 && m_depth_control) 
+    // m_desired_elevator = setDesiredElevator();
+    return true;
+}
+//TODO： 添加操控权判断
+bool Controler::overrived(string sval)
+{
+    if(tolower(sval) == "true")
+    {
+        if(has_override == false)
+            addPosting("HAS_CONTROL", "false");    
+        has_override = true;
+    }
+    else if(tolower(sval) == "false") 
+    {
+    // Upon lifting an override, the timestamps are reset. New values
+    // for all will need to be received before desired_* outputs will
+    // be produced.  If we do not reset, it's possible we may
+    // interpret older pubs as being stale but they may have been
+    // paused also during an override.
+        if(has_override == true) 
+        {
+            desired_speed.time = 0;
+            desired_heading.time = 0;
+            desired_depth.time = 0;
+            desired_pitch.time = 0;
+
+            current_speed.time = 0;
+            current_heading.time = 0;
+            current_pitch.time = 0;
+            current_depth.time = 0;
+        }
+        has_override = false;
+        addPosting("HAS_CONTROL", "true");
+    }
+    return true;
+}
+
+bool Controler::checkForStaleness()
+{
+    bool is_stale = false;
+
+    // =========================================================
+    // Part 1: Check for Helm staleness
+    // =========================================================
+    double hdg_delta = (current_time - desired_heading.time);
+    if(hdg_delta > tardy_helm_thresh) {
+    string staleness = doubleToStringX(hdg_delta,3);
+    addPosting("PID_STALE", "Stale DesHdg:" + staleness);
+    is_stale = true;
+    }
+    double spd_delta = (current_time - desired_speed.time);
+    if(spd_delta > tardy_helm_thresh) {
+    string staleness = doubleToStringX(spd_delta,3);
+    addPosting("PID_STALE", "Stale DesSpd:" + staleness);
+    is_stale = true;
+    }
+
+    // =========================================================
+    // Part 2B: Check for Nav staleness
+    // =========================================================
+    double nav_hdg_delta = (current_time - current_heading.time);
+    if(nav_hdg_delta > tardy_nav_thresh) {
+    string staleness = doubleToStringX(nav_hdg_delta,3);
+    addPosting("PID_STALE", "Stale NavHdg:" + staleness);
+    is_stale = true;
+    }
+    double nav_spd_delta = (current_time - current_speed.time);
+    if(nav_spd_delta > tardy_nav_thresh) {
+    string staleness = doubleToStringX(nav_spd_delta,3);
+    addPosting("PID_STALE", "Stale NavSpd:" + staleness);
+    is_stale = true;
+    }
+    // =========================================================
+    // Part 2C: If depth control, check for Helm Depth staleness 
+    // =========================================================
+    if(has_depthCtrl) {
+    double dep_delta = (current_time - desired_depth.time);
+    if(dep_delta > tardy_helm_thresh) {
+        string staleness = doubleToStringX(dep_delta,3);
+        addPosting("PID_STALE", "Stale DesDepth:" + staleness);
+        is_stale = true;
+    }
+    }
+
+    // =========================================================
+    // Part 2D: If depth control, check for Nav Depth staleness 
+    // =========================================================
+    if(has_depthCtrl) {    
+    double nav_dep_delta = (current_time - current_depth.time);
+    if(nav_dep_delta > tardy_nav_thresh) {
+        string staleness = doubleToStringX(nav_dep_delta,3);
+        addPosting("PID_STALE", "Stale NavDep:" + staleness);
+        is_stale = true;
+    }
+    double nav_pit_delta = (current_time - current_pitch.time);
+    if(nav_pit_delta > tardy_nav_thresh) {
+        string staleness = doubleToStringX(nav_pit_delta,3);
+        addPosting("PID_STALE", "Stale NavPitch:" + staleness);
+        is_stale = true;
+    }
+    }
+    return(is_stale);
+}
+
+// bool Controler::setLimit(double max_thrush, double min_thrush, double max_rudder, double min_rubber, double max_elevator, double min_elevator)
+// {
+//     limit_thrust[0] = min_thrush;
+//     limit_thrust[1] = max_thrush;
+
+//     limit_rudder[0] = min_rubber;
+//     limit_rudder[1] = max_rudder;
+
+//     limit_elevator[0] = min_elevator;
+//     limit_elevator[1] = max_elevator;
+// }
+
+bool Controler::Limit(double &a, double max, double min)
+{
+    a = (a < min) ? min : a;
+    a = (a > max) ? max : a;
+    return true;
+}
+
+bool Controler::setCtrl(bool speed, bool heading, bool depth)
+{
+    has_speedCtrl = speed;
+    has_headCtrl = heading;
+    has_depthCtrl = depth;
+    return true;
+}
+
+void Controler::addPosting(std::string var, std::string sval)
+{
+    VarDataPair pair(var, sval);
+    postings.push_back(pair);
+}
+
+void Controler::addPosting(std::string var, double val)
+{
+    VarDataPair pair(var, val);
+    postings.push_back(pair);
+}
+
+double Controler::getFrequency() const
+{
+    double elapsed = current_time - start_time;
+
+    double frequency = 0;
+    if(elapsed > 0) 
+    frequency = ((double)(iterations)) / elapsed;
+
+    return(frequency);
+}
+
+bool Controler::ClearValTim(vlaTim &a)
+{
+    if(&a == NULL)
+        return false;
+    else
+    {
+        a.value = 0;
+        a.time = 0;
+    }
+
+}
+//TODO: 控制器参数配置函数
+list<std::string> Controler::setConfigParams(std::list<std::string>)
+{
+  list<string> unhandled_params;
+  list<string>::iterator p;
+//   for(p=param_lines.begin(); p!=param_lines.end(); p++) {
+//     string orig  = *p;
+//     string line  = tolower(orig);
+//     string param = biteStringX(line, '=');
+//     if(param == "speed_factor")
+//       m_config_params.push_front(orig);
+//     else if(param == "simulation")
+//       m_config_params.push_front(orig);
+//     else if(param == "sim_instability")
+//       m_config_params.push_front(orig);
+//     else if(param == "tardy_helm_threshold")
+//       m_config_params.push_front(orig);
+//     else if(param == "tardy_nav_threshold")
+//       m_config_params.push_front(orig);
+//     else if(param == "yaw_pid_kp")
+//       m_config_params.push_front(orig);
+//     else if(param == "yaw_pid_kd")
+//       m_config_params.push_front(orig);
+//     else if(param == "yaw_pid_ki")
+//       m_config_params.push_front(orig);
+//     else if(param == "yaw_pid_integral_limit")
+//       m_config_params.push_front(orig);
+//     else if(param == "yaw_pid_ki_limit")
+//       m_config_params.push_front(orig);
+//     else if(param == "maxrudder")
+//       m_config_params.push_front(orig);
+//     else if(param == "heading_debug")
+//       m_config_params.push_front(orig);
+
+//     else if(param == "speed_pid_kp")
+//       m_config_params.push_front(orig);
+//     else if(param == "speed_pid_kd")
+//       m_config_params.push_front(orig);
+//     else if(param == "speed_pid_ki")
+//       m_config_params.push_front(orig);
+//     else if(param == "speed_pid_integral_limit")
+//       m_config_params.push_front(orig);
+//     else if(param == "maxthrust")
+//       m_config_params.push_front(orig);
+//     else if(param == "speed_debug")
+//       m_config_params.push_front(orig);
+
+//     else if(param == "depth_control")
+//       m_config_params.push_front(orig);
+//     else if(param == "z_to_pitch_pid_kp")
+//       m_config_params.push_front(orig);
+//     else if(param == "z_to_pitch_pid_kd")
+//       m_config_params.push_front(orig);
+//     else if(param == "z_to_pitch_pid_ki")
+//       m_config_params.push_front(orig);
+//     else if(param == "z_to_pitch_pid_integral_limit")
+//       m_config_params.push_front(orig);
+//     else if(param == "maxpitch")
+//       m_config_params.push_front(orig);
+//     else if(param == "depth_debug")
+//       m_config_params.push_front(orig);
+
+//     else if(param == "pitch_pid_kp")
+//       m_config_params.push_front(orig);
+//     else if(param == "pitch_pid_kd")
+//       m_config_params.push_front(orig);
+//     else if(param == "pitch_pid_ki")
+//       m_config_params.push_front(orig);
+//     else if(param == "pitch_pid_integral_limit")
+//       m_config_params.push_front(orig);
+//     else if(param == "maxelevator")
+//       m_config_params.push_front(orig);
+
+//     else
+//       unhandled_params.push_front(orig);    
+//   }
+  return(unhandled_params);
+}
+
+Json::Value Controler::getConfig(string path)
+{  
+    ifstream ifs;
+    ifs.open(path, ios::in);
+    Json::Reader taskConfigureReader;
+    Json::Value inputJsonValue;
+    taskConfigureReader.parse(ifs, inputJsonValue);
+    ifs.close();
+    return inputJsonValue;
+}
+
+void Controler::setCheakStalensee(string s)
+{
+    if(s == "true")
+        cheakStalensee = true;
+    else
+        cheakStalensee = false;
+}
--- a/src/pMotionControler/Controler.hpp
+++ b/src/pMotionControler/Controler.hpp
@@ -0,0 +1,155 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-16 14:05:16
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-02 14:31:16
+ * @FilePath: /moos-ivp-pi/src/pMotionControler/Controler.hpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#ifndef __COBTROLLER_H
+#define __CONTROLLER_H
+#include<iostream>
+#include<vector>
+#include "VarDataPair.h"
+#include "MBUtils.h"
+#include "AngleUtils.h"
+#include <list>
+#include "json/json.h"
+#include <fstream>
+#include <map>
+
+// #include "VarDataPair.h"
+
+using namespace std;
+
+typedef struct
+{
+    double value;
+    double time;
+} vlaTim;
+
+class Controler
+{
+    public:
+    Controler();
+    ~Controler(){};
+
+    virtual int step(){return 0;}
+    virtual bool setConfigParams(){return false;}
+    virtual bool overrived(string sval);
+
+    virtual bool handleYawSettings(){return false;}
+    virtual bool handleSpeedSettings(){return false;};
+    virtual bool handleDepthSettings(){return false;};
+    virtual bool hasConfigSettings() const{return false;};
+    // bool setError(double err);
+
+    bool setDesSpeed(double spd, double tim);
+    bool setDesDepth(double dph, double tim);
+    bool setDesPitch(double pth, double tim);
+    bool setDesHeading(double hdg, double tim);
+
+    bool setCurSpeed(double spd, double tim);
+    bool setCurDepth(double dph, double tim);
+    bool setCurPitch(double pth, double tim);
+    bool setCurHeading(double hdg, double tim);
+    // bool setCurTime(double tim){current_time = tim;}
+    void updateTime(double tim){current_time = tim;}
+    double getCurTime(){return current_time;}
+    void setStartTime(double tim){start_time = tim;}
+    void setOverride(bool v){has_override = v;}
+    void setTardyHelm(double t){ tardy_helm_thresh = t;}
+    void setTardyNav(double t){ tardy_nav_thresh = t;}
+    void setCheakStalensee(string s);
+    void setConstThrust(double v){const_thrust = v;}
+    void setDeadZone(double v){dead_zone=v;}
+    void setDepthControl(bool v){has_depthCtrl=v;}
+    void setSpeedControl(bool v){has_speedCtrl=v;}
+    void setHeadingControl(bool v){has_headCtrl=v;}
+    bool setDesiredValues();
+
+    // bool setLimit(double max_thrush, double min_thrush, double max_rudder, double min_rubber, double max_elevator, double min_elevator);
+    bool setCtrl(bool speed, bool heading, bool depth);
+    bool Limit(double &a, double max, double min);
+
+    bool checkForStaleness();
+    void addPosting(std::string var, std::string sval);
+    void addPosting(std::string var, double val);
+    vector<VarDataPair> getPostings() {return(postings);}
+    void clearPostings() {postings.clear();}
+    bool ClearValTim(vlaTim &a);
+    list<std::string> setConfigParams(std::list<std::string>); //使用MOOS风格配置参数函数
+    Json::Value getConfig(string path);
+
+    double getFrequency() const;
+    bool hasControl(){return(!has_override);}
+    bool hasSpdCtrl(){return has_speedCtrl;}
+    bool hasDphCtrl(){return has_depthCtrl;}
+    bool hasHdgCtrl(){return has_headCtrl;}
+
+    double getDesiredRudder() const   {return(desired_rudder.value);}
+    double getDesiredThrust() const   {return(desired_thrust.value);}
+    double getDesiredElevator() const {return(desired_elevator.value);}
+    // bool setErrorCap(int c){Error_capacity = c;}
+
+    inline void initVariable();
+    list<string> getConfigParams(){return config_params;}
+    vector<string> getConfigErrors(){return config_errors;}
+    vector<string> getConfigInfo(){return config_info;}
+    vector<string> getRunMsg(){return runMsg;}
+    Json::Value getReport(){return RepList;}
+    void ClearRunMsg(){runMsg.clear();}
+
+    protected:
+    vlaTim desired_speed;
+    vlaTim desired_heading;
+    vlaTim desired_depth;
+    vlaTim desired_pitch;
+
+    vlaTim current_speed;
+    vlaTim current_heading;
+    vlaTim current_pitch;
+    vlaTim current_depth;
+    double current_time;
+    double start_time;
+
+    vlaTim desired_thrust;
+    vlaTim desired_rudder;
+    vlaTim desired_elevator;
+
+    double limit_thrust[2];
+    double limit_rudder[2];
+    double limit_elevator[2];
+    double max_rudder;
+    double max_thrust;
+    double max_pitch;
+    double max_elevator;
+
+    bool has_speedCtrl = true;
+    bool has_headCtrl = true;
+    bool has_depthCtrl = true;
+    bool has_override = true;
+    bool cheakStalensee = true; 
+
+    vector<VarDataPair> postings;
+    unsigned int iterations;
+
+    double tardy_helm_thresh;
+    double tardy_nav_thresh;
+    double const_thrust;
+    double dead_zone;
+    
+
+
+    double is_rad = false;
+
+    list<string> config_params;
+    vector<string> config_errors;
+    vector<string> config_info;
+    vector<string> runMsg;
+    Json::Value RepList;  
+};
+
+#endif
--- a/src/pMotionControler/MotionControler.cpp
+++ b/src/pMotionControler/MotionControler.cpp
@@ -0,0 +1,256 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-12 09:52:27
+ * @LastEditors: zhaojingkui 1553836110@qq.com
+ * @LastEditTime: 2023-11-30 11:05:18
+ * @FilePath: /moos-ivp-pi/src/pMotionControler/MotionControler.cpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+// #define DEBUG
+#include"MotionControler.hpp"
+//TODO：增加启用哪个控制器功能
+bool MotionControler::OnNewMail(MOOSMSG_LIST &NewMail)
+{
+  AppCastingMOOSApp::OnNewMail(NewMail);
+  MOOSMSG_LIST::iterator p;
+  for(p=NewMail.begin(); p!=NewMail.end(); p++)
+  {
+    CMOOSMsg &msg = *p;
+
+    string key  = msg.m_sKey;
+    string sval = msg.m_sVal;
+    double dval = msg.m_dfVal;
+    double dfT;
+    msg.IsSkewed(m_curr_time, &dfT);
+    if(fabs(dfT) < ok_skew) 
+    {
+      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());    
+        RepList["NAV"]["Pitch"] = dval;
+      }
+      else if(key == MSG_ReadConfig) //重新读取配置参数可以清除故障码
+      {
+        int e = pengine.setParam(configFilePath);
+        pengine.setOverride(true);
+        if(e != 0)
+          faultCode = 10 + e;
+        else
+          faultCode = 0;
+      }
+    }
+  }
+    return true;
+}
+bool MotionControler::Iterate()
+{
+    bool a;
+    AppCastingMOOSApp::Iterate();
+    pengine.updateTime(m_curr_time);
+    int i = pengine.step();
+    switch (i)
+    {
+      case 0:
+        RepList["State : "] = "Run";
+        break;
+      case 1:
+        RepList["State : "] = "Ready";
+        break;      
+      case -1:
+        RepList["State : "] = "Fault";
+        faultCode = 1;//信息超时
+        break;  
+      default:
+        break;
+    }
+    postPengineResults();
+    postPenginePostings();
+    postCharStatus();
+    Notify(MSG_FALUT,faultCode);
+    AppCastingMOOSApp::PostReport(); 
+    return true;
+}
+bool MotionControler::OnConnectToServer()
+{
+    registerVariables();
+    return true;
+}
+bool MotionControler::OnStartUp()
+{
+  AppCastingMOOSApp::OnStartUp();
+  pengine.setStartTime(MOOSTime());
+  STRING_LIST sParams;
+  m_MissionReader.GetConfiguration(GetAppName(), sParams);
+  //pengine.setConfigParams(sParams);
+  //bool handled = true;
+  STRING_LIST::iterator p;
+  for(p=sParams.begin(); p!=sParams.end(); p++) {
+      string orig  = *p;
+      string line  = (orig); //识别大小写
+      string param = biteStringX(line, '=');
+      string value = line;
+      double dval  = atof(value.c_str());
+      if(param == "config_file")
+        configFilePath = value;
+      else if(param == "tardy_helm_thresh")
+        pengine.setTardyHelm(dval);
+      else if(param == "tardy_nav_thresh")
+        pengine.setTardyNav(dval);
+      else if(param == "cheak_stalensee")
+        pengine.setCheakStalensee(value);
+      else if(param == "AppTick")
+        setFrequency = dval;
+      else if(param == "delta_freqency")
+        frequency_delta = dval;
+      else
+        reportUnhandledConfigWarning(orig);
+  }
+  int e = pengine.setParam(configFilePath);
+  // if(e != 0)
+  // {
+  //   faultCode = 10 + e;
+  //   // return false;
+  // }
+  return true;
+}
+
+void MotionControler::registerVariables()
+{
+      AppCastingMOOSApp::RegisterVariables();
+
+      Register("NAV_HEADING", 0);
+      Register("NAV_SPEED", 0);
+      Register("NAV_DEPTH", 0);
+      Register("NAV_PITCH", 0);
+      Register("DESIRED_HEADING", 0);
+      Register("DESIRED_SPEED", 0);
+      Register("DESIRED_DEPTH", 0);
+      Register("PID_VERBOSE", 0);
+      Register("SPEED_FACTOR", 0);
+      Register("MOOS_MANUAL_OVERIDE", 0);
+      Register("MOOS_MANUAL_OVERRIDE", 0);
+      Register(MSG_ReadConfig,0);
+}
+bool MotionControler::buildReport()
+{
+  double frequency = pengine.getFrequency();
+  double delta_freq =  100.0*(setFrequency - frequency) / setFrequency;
+  if(abs(delta_freq) > frequency_delta)
+    faultCode = 2;
+
+  m_msgs << "Frequency Delta  : " << frequency  << endl;
+  m_msgs << "PID has_control  : " << boolToString(pengine.hasControl()) << endl;
+  m_msgs << "Config File Path : " << configFilePath << endl;
+  m_msgs << "S : H : D        : | " << intToString(pengine.hasSpdCtrl())+ " | " 
+  << intToString(pengine.hasHdgCtrl())+" | " 
+  << intToString(pengine.hasDphCtrl())+" |" << endl;
+
+  RepList["to BS"] = colVar;
+  // RepList["PID"] = pengine.getReport()["W"];
+  string rep = Json::writeString(RepJsBuilder, RepList);
+  m_msgs << rep << endl;
+
+  return(true);
+}
+
+void MotionControler::postPenginePostings()
+{
+    vector<VarDataPair> m_postings = pengine.getPostings();
+    for(unsigned int i=0; i<m_postings.size(); i++)
+    {
+        VarDataPair pair = m_postings[i];
+        string var = pair.get_var();
+        if(pair.is_string() && pair.get_sdata_set())
+        Notify(var, pair.get_sdata());
+        else if(!pair.is_string() && pair.get_ddata_set())
+        Notify(var, pair.get_ddata());
+    }
+    pengine.clearPostings();
+}
+void MotionControler::postPengineResults()
+{
+  bool all_stop = true;
+  if(pengine.hasControl())
+      all_stop = false;
+
+  if(all_stop) 
+  {
+    if(allstop_posted)
+      return;
+    Notify("DESIRED_RUDDER", 0.0);
+    Notify("DESIRED_THRUST", 0.0);
+    // if(pengine.hasDphCtrl())
+    Notify("DESIRED_ELEVATOR", 0.0);
+    postColVarToBS(0,0,0);
+    allstop_posted = true;
+  }
+  else 
+  {
+    int T=0,S=0,R=0;
+    if(pengine.hasHdgCtrl())
+    {
+      Notify("DESIRED_RUDDER", pengine.getDesiredRudder());
+      R = (int)pengine.getDesiredRudder();
+    }
+    if(pengine.hasSpdCtrl())
+    {
+      Notify("DESIRED_THRUST", pengine.getDesiredThrust());
+      T = (int)pengine.getDesiredThrust();
+    }
+      
+    if(pengine.hasDphCtrl())
+    {
+      Notify("DESIRED_ELEVATOR", pengine.getDesiredElevator());
+      S = pengine.getDesiredElevator();
+    }
+    postColVarToBS(T,S,R);
+    allstop_posted = false;
+  }
+}
+void MotionControler::postCharStatus()
+{
+  if(!verbose)
+    return;
+
+  if(pengine.hasControl())
+    cout << "$" << flush;
+  else 
+    cout << "*" << flush;
+}
+void MotionControler::postColVarToBS(int T, int S, int R)
+{
+  string s_msg;
+  colVar["mode"] = 2;
+  colVar["thrust"] = T;
+  colVar["rudder"] = R;
+  colVar["elevator"] = S;
+  s_msg = Json::writeString(RepJsBuilder,colVar);
+  Notify(MSG_TO_BS, s_msg);
+}
--- a/src/pMotionControler/MotionControler.hpp
+++ b/src/pMotionControler/MotionControler.hpp
@@ -0,0 +1,66 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-12 15:57:27
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-07 11:33:04
+ * @FilePath: /moos-ivp-pi/src/pMotionControler/MotionControler.hpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#ifndef TESTAPP
+#define TESEAPP
+#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
+#include <iostream>
+#include"pidControl.hpp"
+#include<vector>
+// #include"MOOS/libMOOS/Comms/XPCUdpSocket.h"
+using namespace std;
+class MotionControler : public AppCastingMOOSApp
+{
+  public:
+  MotionControler(){};
+  ~MotionControler(){};
+  bool OnNewMail(MOOSMSG_LIST &NewMail);
+  bool Iterate();
+  bool OnConnectToServer();
+  bool OnStartUp();
+  void registerVariables();
+  bool buildReport();
+  
+  void postPenginePostings();
+  void postPengineResults();
+  void postColVarToBS(int T, int S, int R);
+  void postCharStatus();
+
+  const string MSG_FALUT = "uMotion_fault_fb";
+  const string MSG_ReadConfig = "uMotion_config_cmd";
+  const string MSG_TO_BS = "uMotion_control_cmd";
+
+  private:
+  bool ignore_nav_yaw;
+  bool allstop_posted;
+  bool verbose;
+  bool override;
+  double ok_skew = 2;
+  int faultCode = 0;
+  double setFrequency;
+  double frequency_delta;
+  
+  Json::Value RepList;
+  Json::StreamWriterBuilder RepJsBuilder;
+  Json::Value colVar;
+
+  pidControl pengine;
+  string configFilePath;
+  int e;
+
+
+  
+};
+
+#endif
+
+//1.读取参数配置错误 faultCode = 10~
+//2.信息过时错误 faultCode=1
+//3.频率相差过大错误 faultCode=2
--- a/src/pMotionControler/a.moos
+++ b/src/pMotionControler/a.moos
@@ -0,0 +1,47 @@
+ServerHost = localhost
+ServerPort = 9000
+
+ProcessConfig = pMotionControler
+{
+  AppTick    = 5
+  CommsTick  = 5
+
+  verbose       = true
+  depth_control = true
+
+  // SIM_INSTABILITY = 20
+
+  // Yaw PID controller
+  yaw_pid_kp		 = 10
+  yaw_pid_kd		 = 0.0
+  yaw_pid_ki		 = 0.01
+  yaw_pid_integral_limit = 10
+
+  // Speed PID controller
+  speed_pid_kp		 = 20.0
+  speed_pid_kd		 = 0.0
+  speed_pid_ki		 = 1
+  speed_pid_integral_limit = 100
+
+  maxpitch = 15
+  maxelevator = 30
+
+  pitch_pid_kp = 1.5
+  pitch_pid_kd = 0
+  pitch_pid_ki = 1.0
+  pitch_pid_integral_limit = 0
+
+  z_to_pitch_pid_kp = 0.12
+  z_to_pitch_pid_kd = 0
+  z_to_pitch_pid_ki = 0.004
+  z_to_pitch_pid_integral_limit = 0.05
+  
+
+  //MAXIMUMS MAXRUDDER
+  maxrudder    = 30
+  maxthrust    = 1525
+
+  // A non-zero SPEED_FACTOR overrides use of SPEED_PID
+  // Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
+  speed_factor = 0
+}
--- a/src/pMotionControler/alpha.bhv
+++ b/src/pMotionControler/alpha.bhv
@@ -0,0 +1,181 @@
+//--------    FILE: alpha.bhv   -------------
+initialize RUN = false
+initialize TaskNum=t1
+initialize SendTask=false
+//--------------模式判断------------------------
+set MODE = ACTIVE{
+  RUN=true
+} INACTIVE
+
+set MODE = T1{
+  MODE=ACTIVE
+  TaskNum = t1
+}
+
+
+//----------路径点任务----------------------------
+Behavior = BHV_Waypoint
+{
+  name      = waypt_survey
+  pwt       = 100 //优先权重
+  condition = MODE==T1
+
+  //endflag   = START=false
+  endflag = END_WayPoint=true
+
+	configflag = CRUISE_SPD = $[SPEED]
+	//configflag = OSPOS = $[OSX],$[OSY]
+
+	activeflag = INFO=$[OWNSHIP]
+	activeflag = INFO=$[BHVNAME]
+	activeflag = INFO=$[BHVTYPE]
+
+	//cycleflag = CINFO=$[OSX],$[OSY]
+
+  wptflag = CurrentPointComplete=true
+	wptflag = PREV=$(PX),$(PY)
+	wptflag = NEXT=$(NX),$(NY)
+	wptflag = TEST=$(X),$(Y)
+	wptflag = OSPOS=$(OSX),$(OSY)
+	//wptflag_on_start = true
+  
+
+  updates    = WPT_UPDATE
+  //perpetual  = true
+
+  templating = spawn
+
+	//    speed_alt = 1.2
+  //use_alt_speed = true
+                lead = 8
+         lead_damper = 1
+       lead_to_start = false
+               speed = 1   // meters per second
+        capture_line = true
+      capture_radius = 5.0
+         slip_radius = 15.0
+	efficiency_measure = all
+
+               polygon = 60,-40
+               order = normal
+             //repeat  = 3
+
+    visual_hints = nextpt_color=yellow
+    visual_hints = nextpt_vertex_size=8
+    visual_hints = nextpt_lcolor=gray70
+    visual_hints = vertex_color=dodger_blue, edge_color=white
+    visual_hints = vertex_size=5, edge_size=1
+}
+
+
+//--------------定深任务------------------
+Behavior=BHV_ConstantDepth
+{
+  name = const_depth
+  pwt = 100
+  //condition = DEPLOY = true
+  condition = MODE==T1
+  duration = no-time-limit
+  updates = DEPTH_UPDATE
+  depth = 0
+}
+
+//--------------定向任务--------------------
+
+Behavior=BHV_ConstantHeading
+{
+  name       = const_heading
+  pwt        = 100
+  //condition  = START_TURN = true
+  //condition  = DEPLOY = true
+  condition = MODE==T3
+  perpetual  = true
+
+  activeflag = TURN = started
+
+  //endflag    = TURN = done
+  //endflag    = RETURN = true
+  //endflag    = START_TURN = false
+  endflag    = START=false
+
+          heading = 225
+  complete_thresh = 5
+       duration = no-time-limit
+ }
+
+//--------------定速任务--------------------
+Behavior=BHV_ConstantSpeed
+{
+  name       = const_speed
+  pwt        = 1000
+  condition = MODE==T1
+  perpetual  = true
+  updates    = SPEED_UPDATE
+  //endflag    = START=false
+
+  speed = 5
+
+  duration = no-time-limit
+  //peakwidth = 0.5
+  //basewidth = 0.5
+
+}
+//----------------安全模式-----------------------
+//----------------计时器---------------------
+Behavior = BHV_Timer
+{ 
+  name      = mtime
+  condition = MODE==T1
+  pwt = 100
+  templating = spawn
+	//duration_status = MSTATUS
+	//duration = 10
+  endflag = TIME_OUT=true
+
+  updates      = TIMER_UPDATES        
+  
+  //perpetual  = true
+}
+//-------------最大深度限制--------------------------
+Behavior = BHV_MaxDepth
+{
+  name = maxdepth
+  pwt  = 200
+  condition = MODE==ACTIVE
+  updates = MAXDEEP_UPDATES
+  max_depth = 20
+  tolerance = 0
+  duration = no-time-limit
+}
+//--------------安全区域设置-----------------------
+
+ Behavior = BHV_OpRegion
+ {
+   // General Behavior Parameters
+   // ---------------------------
+   name         = op_region                 // example
+   pwt          = 300                       // default
+   condition = MODE==T5
+   updates      = OPREGION_UPDATES          // example
+
+   // Parameters specific to this behavior
+   // ------------------------------------
+                  max_time = 20              // default (seconds)
+                 max_depth = 25              // default (meters)
+              min_altitude = 0              // default (meters)
+                 reset_var = OPREGION_RESET // example
+        trigger_entry_time = 1              // default (seconds)
+         trigger_exit_time = 0.5            // default (seconds) 
+
+                   polygon = pts={-80,-00:-30,-175:150,-100:95,25}
+
+    breached_altitude_flag = TaskFault = AltitudeOut
+       breached_depth_flag = TaskFault = DepthOut
+        breached_poly_flag = TaskFault = RegionOut
+        breached_time_flag = TaskFault = TimeOut
+
+              visual_hints = vertex_color = brown  // default
+              visual_hints = vertex_size  = 3      // default
+              visual_hints = edge_color   = aqua   // default
+              visual_hints = edge_size    = 1      // default
+ }
--- a/src/pMotionControler/alpha.moos
+++ b/src/pMotionControler/alpha.moos
@@ -0,0 +1,285 @@
+//-------------------------------------------------
+// NAME: M. Benjamin, MIT CSAIL
+// FILE: alpha.moos
+//-------------------------------------------------
+
+ServerHost   = localhost
+ServerPort   = 9000
+Community    = alpha
+MOOSTimeWarp = 1
+
+// Forest Lake
+LatOrigin  = 43.825300 
+LongOrigin = -70.330400 
+
+// MIT Sailing Pavilion (use this one)
+// LatOrigin  = 42.358456 
+// LongOrigin = -71.087589
+
+//------------------------------------------
+// Antler configuration  block
+ProcessConfig = ANTLER
+{
+  MSBetweenLaunches = 200
+
+  Run = MOOSDB        @ NewConsole = false
+  Run = pLogger       @ NewConsole = false
+  //Run = uSimMarineV22 @ NewConsole = false
+  //Run = pMarinePIDV22 @ NewConsole = false
+  Run = pHelmIvP      @ NewConsole = false
+  Run = pMarineViewer	@ NewConsole = false
+  Run = uProcessWatch	@ NewConsole = false
+  Run = pNodeReporter	@ NewConsole = false
+  Run = pRealm	      @ NewConsole = false
+  Run = pTaskManger   @ NewConsole = false
+  Run = pMotionControler   @ NewConsole = false
+  Run = pEmulator @ NewConsole = true
+  //Run = uTimerScript  @ NewConsole = false
+}
+ProcessConfig = pTaskManger
+{
+  AppTick   = 8
+  CommsTick = 8
+
+  planConfigPath  = /home/zjk/Desktop/project/moos-ivp-extend/PlanConfigure.json
+}
+ProcessConfig = pEmulator
+{
+  AppTick   = 5
+  CommsTick = 5
+  matlab_host = 192.168.0.11
+  matlab_port = 8085
+  local_port = 8080
+  prefix = NAV
+
+  start_x = 10
+  start_y = 9
+  start_z = 1
+  start_heading = 30
+}
+
+//------------------------------------------
+// pLogger config block
+
+ProcessConfig = pLogger
+{
+  AppTick   = 8
+  CommsTick = 8
+
+  AsyncLog = 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
+  //Log = BHV_SETTINGS @ 0 NOSYNC
+  Log = OPREGION_RESET @ 0 NOSYNC
+
+  LogAuxSrc = true
+  WildCardLogging = true
+  WildCardOmitPattern = *_STATUS
+  WildCardOmitPattern = DB_VARSUMMARY
+  WildCardOmitPattern = DB_RWSUMMARY
+  WildCardExclusionLog = true
+}
+
+//------------------------------------------
+// uProcessWatch config block
+
+ProcessConfig = uProcessWatch
+{
+  AppTick   = 4
+  CommsTick = 4
+
+  watch_all = true
+	nowatch   = uPokeDB*
+	nowatch   = uQueryDB*
+	nowatch   = uXMS*
+	nowatch   = uMAC*
+}
+
+//------------------------------------------
+// uSimMarineV22 config block
+
+ProcessConfig = uSimMarineV22
+{
+  AppTick  	= 4
+  CommsTick	= 4
+
+  start_pos = x=0, y=-20, heading=180, speed=5
+
+  prefix        = NAV
+
+  turn_rate     = 40
+  thrust_map    = 0:0, 20:1, 40:2, 60:3, 80:4, 100:5
+  //thrust_reflect = true
+
+  buoyancy_rate = 0.075
+  max_depth_rate = 5
+  max_depth_rate_speed = 2.0
+  default_water_depth = 400
+}
+
+//------------------------------------------
+// pHelmIvP config block
+
+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
+  
+  
+}
+
+//------------------------------------------
+// pMarinePID config block
+
+ProcessConfig = pMotionControler
+{
+  AppTick    = 10
+  CommsTick  = 10
+
+  verbose       = true
+  depth_control = true
+
+  // SIM_INSTABILITY = 20
+
+  // Yaw PID controller
+  yaw_pid_kp		 = 3.0
+  yaw_pid_kd		 = 0.0
+  yaw_pid_ki		 = 0.01
+  yaw_pid_integral_limit = 5.0
+
+  // Speed PID controller
+  speed_pid_kp		 = 50.0
+  speed_pid_kd		 = 0.0
+  speed_pid_ki		 = 10.0
+  speed_pid_integral_limit = 500.0
+
+  maxpitch = 10
+  maxelevator = 30
+
+  pitch_pid_kp = 10.0
+  pitch_pid_kd = 0
+  pitch_pid_ki = 0.1
+  pitch_pid_integral_limit = 5.0
+
+  z_to_pitch_pid_kp = 10.0
+  z_to_pitch_pid_kd = 0
+  z_to_pitch_pid_ki = 0.1
+  z_to_pitch_pid_integral_limit = 1.0
+  
+
+  //MAXIMUMS MAXRUDDER
+  maxrudder    = 35
+  maxthrust    = 1525
+
+  // A non-zero SPEED_FACTOR overrides use of SPEED_PID
+  // Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
+  speed_factor = 0
+}
+
+//------------------------------------------
+// pMarineViewer config block
+
+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 # uMission_action_cmd={"taskName":"east_waypt_survey","action":"start"}
+  //button_one = MOOS_MANUAL_OVERRIDE=false
+  button_two = STOP # START=false
+  //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
+}
+
+//------------------------------------------
+// pNodeReporter config block
+
+ProcessConfig = pNodeReporter
+{
+  AppTick    = 2
+  CommsTick	 = 2
+
+  //platform_type   = kayak
+  //更改显示形状为uuv
+  platform_type = UUV 
+  platform_color  = red
+  platform_length = 4
+}
+
+ProcessConfig = uTimerScript
+{
+  AppTick = 4
+  CommsTick = 4
+
+  condition = DEPLOY = true
+  randvar = varname = RND_DEPTH, min=20, max=80, key=at_reset
+  event = var = DEPTH_UPDATE, val=depth=$[RND_DEPTH], time=120
+  reset_max = nolimit reset_time = all-posted
+}
--- a/src/pMotionControler/main.cpp
+++ b/src/pMotionControler/main.cpp
@@ -0,0 +1,61 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-12 09:52:06
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-10-24 15:11:21
+ * @FilePath: /moos-ivp-extend/src/pMotionControler/main.cpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#include<iostream>
+using namespace std;
+#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
+// #include "VarDataPair.h"
+#include"MotionControler.hpp"
+#include <iostream>
+#include<string>
+#include "MBUtils.h"
+#include "ColorParse.h"
+// #include "MBUtils.h"
+// #include "ColorParse.h"
+
+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();
+      cout << " Version : 0.01" << endl;
+    else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
+      // showExampleConfigAndExit();
+      cout << " example : NULL" << endl;
+    else if((argi=="-h") || (argi == "--help") || (argi=="-help"))
+      // showHelpAndExit();
+      cout << " Low leave Control for UUV " << endl;
+    else if((argi=="-i") || (argi == "--interface"))
+      // showInterfaceAndExit();
+      cout << " UUV Motion Control " << endl;
+    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 << "pMarinePIDV22 launching as " << run_command << endl;
+  cout << termColor() << endl;
+
+  MotionControler marinePID;
+  marinePID.Run(run_command.c_str(), mission_file.c_str(), argc, argv);
+
+  return(0);
+}
--- a/src/pMotionControler/pidControl.cpp
+++ b/src/pMotionControler/pidControl.cpp
@@ -0,0 +1,305 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-16 15:14:15
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-03 11:30:38
+ * @FilePath: /moos-ivp-pi/src/pMotionControler/pidControl.cpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#include"pidControl.hpp"
+pidControl::pidControl()
+{
+    pidClear(pid_depth);
+    pidClear(pid_heading);
+    pidClear(pid_pitch);
+    pidClear(pid_speed);
+    setCurDepth(0,0);
+    setDesDepth(0,0);
+    setCurHeading(0,0);
+    setDesHeading(0,0);
+    setCurPitch(0,0);
+    setDesPitch(0,0);
+    setCurSpeed(0,0);
+    setDesSpeed(0,0);
+    const_thrust = 0;
+}
+int pidControl::step()
+{
+    if(!setDesiredValues())
+    {
+        //这里不对时间进行重置，保持故障状态
+        pidClear(pid_depth);
+        pidClear(pid_heading);
+        pidClear(pid_pitch);
+        pidClear(pid_speed);
+        return -1; //Falut
+    }
+    if(has_override)
+    {
+        //由手动移交的控制权，需要对时间进行重置，防止进入故障状态
+        pidClear(pid_depth);
+        pidClear(pid_heading);
+        pidClear(pid_pitch);
+        pidClear(pid_speed);
+        setCurDepth(0,0);
+        setDesDepth(0,0);
+        setCurHeading(0,0);
+        setDesHeading(0,0);
+        setCurPitch(0,0);
+        setDesPitch(0,0);
+        setCurSpeed(0,0);
+        setDesSpeed(0,0);
+        RepList["Waring"] = int(has_override);
+        return 1; //Ready
+    }
+    RepList["Waring"] = int(has_override);
+    string rpt = "";
+    if(has_speedCtrl)
+    {
+        if(const_thrust == 0)
+        {
+            double speed_error = desired_speed.value - current_speed.value;
+            desired_thrust.value = pidStep(speed_error, pid_speed);
+            desired_thrust.time = current_time;
+            //Limit(desired_thrust.value, max_thrust, 0);
+
+            rpt = "PID_SPEED: ";
+            rpt += " (Want):" + doubleToString(desired_speed.value);
+            rpt += " (Curr):" + doubleToString(current_speed.value);
+            rpt += " (Diff):" + doubleToString(speed_error);
+            rpt += " (Delt):" + doubleToString(pid_speed.delta_output);
+            rpt += " THRUST:" + doubleToString(desired_thrust.value);
+            addPosting("PID_SPD_DEBUG", rpt);
+            RepList["spd_pid"]["Want"] = doubleToString(desired_speed.value,2);
+            RepList["spd_pid"]["Curr"] = doubleToString(current_speed.value,2);
+            RepList["spd_pid"]["Diff"] = doubleToString(speed_error,2);
+            RepList["spd_pid"]["Delt"] = doubleToString(pid_speed.delta_output,2);
+            RepList["spd_pid"]["THRUST"] = doubleToString(desired_thrust.value,2);
+        }
+        else
+        {
+            desired_thrust.value = const_thrust;
+            desired_thrust.time = current_time;
+        }
+
+    }
+    if(desired_thrust.value <= dead_zone)
+    {
+        desired_rudder.value = 0;
+        desired_rudder.time = current_time;
+        desired_elevator.value = 0;
+        desired_elevator.time = current_time;
+        return true;
+    }
+
+    if(has_headCtrl)
+    {   
+        double heading_error = desired_heading.value - current_heading.value;
+        heading_error = angle180(heading_error);
+        desired_rudder.value = pidStep(heading_error, pid_heading);
+        desired_rudder.time = current_time;
+        
+        //Limit(desired_rudder.value, max_rudder, -max_rudder);
+        // Limit();
+
+        rpt = "PID_COURSE: ";
+        rpt += " (Want):" + doubleToString(desired_heading.value);
+        rpt += " (Curr):" + doubleToString(current_heading.value);
+        rpt += " (Diff):" + doubleToString(heading_error);
+        rpt += " (Delt):" + doubleToString(pid_heading.delta_output);
+        rpt += " RUDDER:" + doubleToString(desired_rudder.value);
+        addPosting("PID_HDG_DEBUG", rpt);
+        RepList["hdg_pid"]["Want"] = doubleToString(desired_heading.value,2);
+        RepList["hdg_pid"]["Curr"] = doubleToString(current_heading.value,2);
+        RepList["hdg_pid"]["Diff"] = doubleToString(heading_error,2);
+        RepList["hdg_pid"]["Delt"] = doubleToString(pid_heading.delta_output,2);
+        RepList["hdg_pid"]["RUDDER"] = doubleToString(desired_rudder.value,2);
+    }
+    if(has_depthCtrl)
+    {
+        double depth_error = desired_depth.value - current_depth.value;
+        desired_pitch.value = -pidStep(depth_error, pid_depth);
+        desired_pitch.time = current_time;
+        //Limit(desired_pitch.value, max_pitch, -max_pitch);
+        double pitch_error = desired_pitch.value - current_pitch.value;
+        desired_elevator.value = pidStep(pitch_error, pid_pitch);
+        desired_elevator.time = current_time;
+        //Limit(desired_elevator.value, max_elevator, -max_elevator);
+        
+        rpt = "PID_DEPTH: ";
+        rpt += " (Want):" + doubleToString(desired_depth.value);
+        rpt += " (Curr):" + doubleToString(current_depth.value);
+        rpt += " (Diff):" + doubleToString(depth_error);
+        rpt += " ELEVATOR:" + doubleToString(desired_elevator.value);
+        addPosting("PID_DEP_DEBUG", rpt);
+        RepList["dep_pid"]["Want"] = doubleToString(desired_depth.value,2);
+        RepList["dep_pid"]["Curr"] = doubleToString(current_depth.value,2);
+        RepList["dep_pid"]["Diff"] = doubleToString(depth_error,2);
+        RepList["dep_pid"]["Delt"] = doubleToString(pid_depth.delta_output,2);
+
+        RepList["pth_pid"]["Want"] = doubleToString(desired_pitch.value,2);
+        RepList["pth_pid"]["Curr"] = doubleToString(current_pitch.value,2);
+        RepList["pth_pid"]["Diff"] = doubleToString(pitch_error,2);
+        RepList["pth_pid"]["Delt"] = doubleToString(pid_pitch.delta_output,2);
+        RepList["pth_pid"]["ELEVATOR"] = doubleToString(desired_elevator.value,2);
+    }
+    // Limit();
+    return 0;
+}
+
+bool pidControl::setParam(double p, double i, double d, double limitDelta, double max, double min, pidInc &pid)
+{
+    // pid.kp = p + i + d;
+    // pid.ki = -p - 2*d;
+    // pid.kd = d;
+    pid.kp = p;
+    pid.ki = i;
+    pid.kd = d;
+    pid.limit_delta = limitDelta;
+    pid.output = 0;
+    pid.max_out = max;
+    pid.min_out = min;
+    return true;
+}
+
+double pidControl::pidStep(double error, pidInc &pid)
+{
+    double deltaOutPut = 0;
+    pid.error_0 = error;
+    deltaOutPut 
+    = pid.kp * (pid.error_0 - pid.error_1                 ) 
+    + pid.ki * (pid.error_0                               ) 
+    + pid.kd * (pid.error_0 -2.0*pid.error_1 + pid.error_2);
+    // RepList["W"]["d"] = deltaOutPut;
+    // RepList["W"]["e1"] = pid.kp;
+    // RepList["W"]["e2"] = pid.ki;
+    // RepList["W"]["e3"] = pid.kd;
+    Limit(deltaOutPut, pid.limit_delta, -pid.limit_delta);
+    pid.output += deltaOutPut;
+    Limit(pid.output, pid.max_out, pid.min_out);
+    pid.delta_output = deltaOutPut;
+    pid.error_2 = pid.error_1;
+    pid.error_1 = pid.error_0;
+    return pid.output;
+}
+// double  pidControl::picStep_p(double error, pidInc &pid)
+// {
+//     pid.delta_output = 
+// }
+bool pidControl::overrived(string svar)
+{
+    Controler::overrived(svar);
+    return true;
+}
+//TODO: 快速检测当前参数是具有与当前控制器相关
+bool pidControl::hasConfigSettings() const
+{
+    if(config_params.size() == 0)
+        return(false);
+
+    list<string>::const_iterator p;
+    for(p=config_params.begin(); p!=config_params.end(); p++) 
+    {
+        string line = *p;
+        string param = tolower(biteStringX(line, '='));
+
+        // if(param == "yaw_pid_kp")
+        //     return(true);
+        // else if(param == "yaw_pid_kd")
+        //     return(true);
+        // else if(param == "yaw_pid_ki")
+        //     return(true);
+        // else if(param == "yaw_pid_integral_limit")
+        //     return(true);
+        // else if(param == "yaw_pid_ki_limit")
+        //     return(true);
+        return true;
+    }
+
+    return(false);
+}
+
+bool pidControl::setParam(char n, double param, pidInc *pid_t)
+{
+    if(pid_t == NULL)
+        return false;
+    
+    switch (n)
+    {
+    case 'p':
+        pid_t->kp = param;
+        break;
+    case 'i':
+        pid_t->ki = param;
+        break;
+    case 'd':
+        pid_t->kd = param;
+        break;
+    default:
+        return false;
+        break;
+    }
+
+    return true;
+
+}
+int pidControl::setParam(string filePath)
+{
+    Json::Value paramList = getConfig(filePath);
+    if(paramList.empty())
+        return 1;
+    if(paramList["speed"].empty()       || paramList["pitch"].empty()
+       || paramList["depth"].empty()    || paramList["pitch"].empty()
+       || paramList["speedCol"].empty() ||  paramList["depthCol"].empty()
+       || paramList["HeadingCol"].empty())
+        return 2; //重要参数不全
+    Json::Value param = paramList["speed"];
+    if(!setParam(param, pid_speed))
+        return 3;
+    RepList["speed-param"] = param;
+    param = paramList["heading"];
+    if(!setParam(param, pid_heading))
+        return 4;
+    RepList["heading-param"] = param;
+    param = paramList["pitch"];
+    if(!setParam(param, pid_pitch))
+        return 5;
+    RepList["pitch-param"] = param;
+    param = paramList["depth"];
+    if(!setParam(param, pid_depth))
+        return 6;
+    RepList["depth-param"] = param;
+    setSpeedControl(paramList["speedCol"].asBool());
+    setDepthControl(paramList["depthCol"].asBool());
+    setHeadingControl(paramList["HeadingCol"].asBool());
+    //次要参数
+    if(!paramList["dead_zone"].empty())
+    {
+        dead_zone = paramList["dead_zone"].asDouble();
+        RepList["dead_zone"] = dead_zone;
+    }
+    if(!paramList["const_thrust"].empty())
+    {
+        const_thrust = paramList["const_thrust"].asDouble();
+        RepList["const_thrust"] = const_thrust;
+    }
+    return 0;
+}
+bool pidControl::setParam(Json::Value param, pidInc &pid)
+{
+    if(param.empty())
+        return false;
+    if(param["Kp"].empty() || param["Ki"].empty() || param["Kd"].empty())
+        return false;
+    pid.kp = param["Kp"].asDouble();
+    pid.ki = param["Ki"].asDouble();
+    pid.kd = param["Kd"].asDouble();
+    if(param["LimitDelta"].empty() || param["MaxOut"].empty() || param["MinOut"].empty())
+        return false;
+    pid.limit_delta = param["LimitDelta"].asDouble();
+    pid.max_out = param["MaxOut"].asDouble();
+    pid.min_out = param["MinOut"].asDouble();
+    return true;
+}
--- a/src/pMotionControler/pidControl.hpp
+++ b/src/pMotionControler/pidControl.hpp
@@ -0,0 +1,77 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-16 15:14:26
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-02 18:03:14
+ * @FilePath: /moos-ivp-pi/src/pMotionControler/pidControl.hpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#ifndef _PIDCONTROL_H
+#define _PIDCONTROL_H
+#include"Controler.hpp"
+
+typedef struct
+{
+    double kp;
+    double ki;
+    double kd;
+
+    double error_0;
+    double error_1;
+    double error_2;
+
+    double limit_delta;
+    double max_out;
+    double min_out;
+
+    double delta_output;
+    double output;
+} pidInc;
+
+
+class pidControl : public Controler
+{
+private:
+    /* data */
+    pidInc pid_speed;
+    pidInc pid_heading;
+    pidInc pid_pitch;
+    pidInc pid_depth;
+
+    double current_error;
+    double last_error;
+    vector<double> Error;
+    int Error_capacity;
+
+public:
+    pidControl(/* args */);
+    ~pidControl(){};
+
+    int step();
+    
+    bool setParam(double p, double i, double d, double limitDelta, double max, double min, pidInc &pid);
+    bool setParam(char n, double param, pidInc *pid_t);
+    int setParam(string filePath);
+    bool setParam(Json::Value param, pidInc &pid);
+
+    bool hasConfigSettings() const;
+    
+    double pidStep(double error, pidInc &pid);
+    //double picStep_p(double error, pidInc &pid);
+
+    bool overrived(string svar);
+
+    inline void pidClear(pidInc &pid)
+    {
+        pid.error_0 = 0;
+        pid.error_1 = 0;
+        pid.error_2 = 0;
+        pid.delta_output = 0;
+        pid.output = 0;
+    }
+
+};
+
+#endif
--- a/src/pMotionControler/simMat.moos
+++ b/src/pMotionControler/simMat.moos
@@ -0,0 +1,260 @@
+//-------------------------------------------------
+// NAME: M. Benjamin, MIT CSAIL
+// FILE: alpha.moos
+//-------------------------------------------------
+
+ServerHost   = localhost
+ServerPort   = 9000
+Community    = alpha
+MOOSTimeWarp = 1
+
+// Forest Lake
+LatOrigin  = 43.825300 
+LongOrigin = -70.330400 
+
+// MIT Sailing Pavilion (use this one)
+// LatOrigin  = 42.358456 
+// LongOrigin = -71.087589
+
+//------------------------------------------
+// Antler configuration  block
+ProcessConfig = ANTLER
+{
+  MSBetweenLaunches = 200
+
+  Run = MOOSDB        @ NewConsole = false
+  Run = pMarineViewer	@ NewConsole = false
+  Run = uProcessWatch	@ NewConsole = false
+  Run = pNodeReporter	@ NewConsole = false
+  Run = pEmulator @ NewConsole = false
+  //Run = pLogger       @ NewConsole = false
+  Run = pMotionControler   @ NewConsole = false
+  Run = pTaskManger   @ NewConsole = false
+  Run = pHelmIvP      @ NewConsole = false
+}
+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 = pTaskManger
+{
+  AppTick   = 8
+  CommsTick = 8
+}
+ProcessConfig = pEmulator
+{
+  AppTick   = 5
+  CommsTick = 5
+  matlab_host = 192.168.0.11
+  matlab_port = 8085
+  local_port = 8080
+  prefix = NAV
+
+  start_x = 10
+  start_y = 9
+  start_z = 1
+  start_heading = 30
+}
+
+ProcessConfig = pLogger
+{
+  AppTick   = 8
+  CommsTick = 8
+
+  AsyncLog = 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
+  //Log = BHV_SETTINGS @ 0 NOSYNC
+  Log = OPREGION_RESET @ 0 NOSYNC
+
+  LogAuxSrc = true
+  WildCardLogging = true
+  WildCardOmitPattern = *_STATUS
+  WildCardOmitPattern = DB_VARSUMMARY
+  WildCardOmitPattern = DB_RWSUMMARY
+  WildCardExclusionLog = true
+}
+
+//------------------------------------------
+// uProcessWatch config block
+
+ProcessConfig = uProcessWatch
+{
+  AppTick   = 4
+  CommsTick = 4
+
+  watch_all = true
+	nowatch   = uPokeDB*
+	nowatch   = uQueryDB*
+	nowatch   = uXMS*
+	nowatch   = uMAC*
+}
+
+//------------------------------------------
+// uSimMarineV22 config block
+//------------------------------------------
+// pHelmIvP config block
+
+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
+  
+  
+}
+
+//------------------------------------------
+// pMarinePID config block
+
+ProcessConfig = pMotionControler
+{
+  AppTick    = 5
+  CommsTick  = 5
+
+  verbose       = true
+  depth_control = true
+
+  // SIM_INSTABILITY = 20
+
+  // Yaw PID controller
+  yaw_pid_kp		 = 10
+  yaw_pid_kd		 = 0.0
+  yaw_pid_ki		 = 0.01
+  yaw_pid_integral_limit = 10
+
+  // Speed PID controller
+  speed_pid_kp		 = 20.0
+  speed_pid_kd		 = 0.0
+  speed_pid_ki		 = 1
+  speed_pid_integral_limit = 100
+
+  maxpitch = 15
+  maxelevator = 30
+
+  pitch_pid_kp = 1.5
+  pitch_pid_kd = 0
+  pitch_pid_ki = 1.0
+  pitch_pid_integral_limit = 0
+
+  z_to_pitch_pid_kp = 0.12
+  z_to_pitch_pid_kd = 0
+  z_to_pitch_pid_ki = 0.004
+  z_to_pitch_pid_integral_limit = 0.05
+  
+
+  //MAXIMUMS MAXRUDDER
+  maxrudder    = 30
+  maxthrust    = 1525
+
+  // A non-zero SPEED_FACTOR overrides use of SPEED_PID
+  // Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
+  speed_factor = 0
+}
+
+//------------------------------------------
+// pMarineViewer config block
+
+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 = MOOS_MANUAL_OVERRIDE=false
+  button_two = STOP # START=false
+  //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
+}
+
+//------------------------------------------
+// pNodeReporter config block
+
+ProcessConfig = pNodeReporter
+{
+  AppTick    = 2
+  CommsTick	 = 2
+
+  //platform_type   = kayak
+  //更改显示形状为uuv
+  platform_type = UUV 
+  platform_color  = red
+  platform_length = 4
+}
Author	SHA1	Message	Date
zjk	dbf35f6ba3	0626提交	2025-06-26 21:00:14 +08:00
zjk	8bad51bc5b	上传了model	2025-06-18 23:25:52 +08:00
zjk	f1e7d7b511	修复了航向的问题	2025-06-18 23:10:16 +08:00
zjk	44dd1f0470	更新了simulink控制器代码	2025-06-18 22:30:06 +08:00
zjk	f170b741da	修复了bug	2025-06-18 11:15:01 +08:00
zjk	8b84d4c48f	集成了simulink的生成代码	2025-06-17 23:55:57 +08:00
zjk	1124f4ce69	调整了控制指令发送的位置	2025-06-16 21:33:31 +08:00
zjk	c1afe465eb	可以使用	2025-06-16 18:27:52 +08:00
zjk	930545b427	补充md设计方案	2025-06-16 12:18:49 +08:00
zjk	f7c8f108b3	init	2025-06-16 11:36:21 +08:00