8.21调试完成

Reviewed-on: #1

.gitignore

@@ -17,6 +17,7 @@
 *.so
 *.dylib
 *.dll
+*.db
 
 # Fortran module files
 *.mod

.marscode/deviceInfo.json

@@ -0,0 +1,3 @@
+{
+  "deviceId": "61b790edcb3293a5008d89c980a4ef768a223b90be5d0ae36098ddb5379d3c93"
+}

.vscode/settings.json

@@ -51,6 +51,10 @@
         "sstream": "cpp",
         "stdexcept": "cpp",
         "cinttypes": "cpp",
-        "typeinfo": "cpp"
+        "typeinfo": "cpp",
+        "cassert": "cpp",
+        "cerrno": "cpp",
+        "cfloat": "cpp",
+        "climits": "cpp"
     }
 }

CMakeLists.txt

@@ -10,6 +10,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.0)
 PROJECT( IVP_EXTEND )
 
 set (CMAKE_CXX_STANDARD 11)
+# Resolved conflict: using relative path for MOOSIVP_SOURCE_TREE_BASE
 set (MOOSIVP_SOURCE_TREE_BASE "/home/zjk/project/lib/moos-ivp")
 
 #=======================================================================
@@ -102,4 +103,4 @@ ENDIF( ${WIN32} )
 #=======================================================================
 # Add Subdirectories
 #=======================================================================
-ADD_SUBDIRECTORY( src )
+ADD_SUBDIRECTORY( src )

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米所有的精度，精度比较低，或许可以使用航位推算结合无迹卡尔曼滤波弥补并提高这个精度

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
+  }
+};
+
+

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
+}

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 = 192.168.137.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
+}

missions/alpha/alpha.bhv

@@ -0,0 +1,94 @@
+//--------    FILE: alpha.bhv   -------------
+
+initialize   DEPLOY = false
+initialize   RETURN = false
+	
+//----------------------------------------------
+Behavior = BHV_Waypoint
+{ 
+  name      = waypt_survey
+  pwt       = 100
+  condition = RETURN = false
+  condition = DEPLOY = true
+  endflag   = RETURN = true
+
+	configflag = CRUISE_SPD = $[SPEED]
+	//configflag = OSPOS = $[OSX],$[OSY]
+
+	activeflag = INFO=$[OWNSHIP]
+	activeflag = INFO=$[BHVNAME]
+	activeflag = INFO=$[BHVTYPE]
+
+	cycleflag = CINFO=$[OSX],$[OSY]
+
+	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
+
+	    speed_alt = 1.2
+  use_alt_speed = true
+                lead = 8
+         lead_damper = 1
+       lead_to_start = true
+               speed = 12   // meters per second
+        capture_line = true
+      capture_radius = 5.0
+         slip_radius = 15.0
+	efficiency_measure = all
+
+  //           points = pts={107.5,-53.5:112,-43.8:112,-46.1:111.7,-49.3:108.3,-52.7:107.7,-53.3}
+	           polygon = 60,-40 : 60,-160 : 150,-160 : 180,-100 : 150,-40
+
+               order = normal
+             //repeat  = 100000
+             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_Waypoint
+{
+  name       = waypt_return
+  pwt        = 100
+  condition  = RETURN = true
+  condition  = DEPLOY = true
+  condition  =  ((DEPLoy = true) or (alpha = one)) or (bravo = two)
+  perpetual  = true
+  updates    = RETURN_UPDATE
+  endflag    = RETURN = false
+  endflag    = DEPLOY = false
+  endflag    = MISSION = complete
+	
+           speed = 2.0
+  capture_radius = 2.0
+     slip_radius = 8.0
+          points = 0,-20
+ }
+
+//----------------------------------------------
+Behavior=BHV_ConstantSpeed
+{
+  name       = const_speed
+  pwt        = 200
+  condition  = SPD=true
+  condition  = DEPLOY = true
+  perpetual  = true
+  updates    = SPEED_UPDATE
+  endflag    = SPD = false
+
+     speed = 0.5
+		 duration = 10
+		 duration_reset = CONST_SPD_RESET=true
+	
+}
+

missions/alpha/alpha.moos

@@ -0,0 +1,233 @@
+// Alder mission file
+ServerHost = localhost
+ServerPort = 9000
+Community  = alder
+MOOSTimeWarp = 1
+
+LatOrigin  = 43.825300 
+LongOrigin = -70.330400
+
+//------------------------------------------
+// Antler config block
+ProcessConfig = ANTLER
+{
+  MSBetweenLaunches = 200
+
+  Run = MOOSDB        @ NewConsole = false
+  //Run = uSimMarineV22	@ NewConsole = false
+  Run = pNodeReporter	@ NewConsole = false
+  //Run = pLogger       @ NewConsole = false
+  Run = pMarineViewer	@ NewConsole = false
+  Run = uProcessWatch @ NewConsole = false
+  Run = pHelmIvP      @ NewConsole = false
+  Run = pAUV150       @ NewConsole = false
+  Run = pShare        @ NewConsole = false
+}
+
+//------------------------------------------
+// pShare config block
+
+ProcessConfig = pShare
+{
+  AppTick    = 10
+  CommsTick  = 10
+  //UUV 信息
+  //output = src_name = NODE_REPORT*, route = 192.168.137.7:8085
+  //App 信息
+  //output = src_name = APPCAST*, route = 192.168.137.7:8085
+  //路径点信息
+  //output = src_name = VIEW*, route = 192.168.137.7:8085
+
+  //output = src_name = *, route = 192.168.137.7:8085
+  //调试端输入端口
+  input = route = localhost:8082
+}
+
+//------------------------------------------
+// uSimMarineV22 config block
+
+ProcessConfig = uSimMarineV22
+{
+  AppTick	  = 10
+  CommsTick	= 10
+
+  START_X       = 0
+  START_Y       = 0
+  START_SPEED   = 0
+  START_HEADING = 180
+  PREFIX        = NAV
+}
+
+//------------------------------------------
+// uProcessWatch config block
+
+ProcessConfig = uProcessWatch
+{
+  AppTick   = 4
+  CommsTick = 4
+
+  summary_wait = 5
+
+  nowatch   = uXMS*
+  nowatch   = uMAC*
+  nowatch   = uPokeDB*
+  nowatch   = uQueryDB*
+  nowatch   = uTermCommand*
+  watch_all = true
+}
+
+//------------------------------------------
+// pHelmIvP config block
+
+ProcessConfig = pHelmIvP
+{
+  AppTick    = 4
+  CommsTick  = 4
+
+  Behaviors  = gly_1.bhv
+  Verbose    = quiet
+  Domain     = course:0:359:360
+  Domain     = speed:0:4:21
+  Domain     = depth:0:100:101
+
+  //IVP_BEHAVIOR_DIR = ../../lib
+
+  ok_skew = any
+  start_in_drive = false
+}
+
+//------------------------------------------
+// pMarinePIDV22 config block
+
+ProcessConfig = pMarinePIDV22
+{
+  AppTick    = 10
+  CommsTick  = 10
+
+  VERBOSE       = true
+  DEPTH_CONTROL = false
+
+  // Yaw PID controller
+  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
+  SPEED_PID_KD		 = 0.0
+  SPEED_PID_KI		 = 0.0
+  SPEED_PID_INTEGRAL_LIMIT = 0.07
+
+  //MAXIMUMS
+  MAXRUDDER    = 35
+  MAXTHRUST    = 35
+
+  // A non-zero SPEED_FACTOR overrides use of SPEED_PID
+  // Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
+  SPEED_FACTOR = 20
+}
+
+//------------------------------------------
+// pMarineViewer config block
+
+ProcessConfig = pMarineViewer
+{
+  AppTick    = 4
+  CommsTick  = 4
+
+  TIFF_FILE            = forrest19.tif
+  set_pan_x            = -90
+  set_pan_y            = -280
+  zoom                 = 1
+  vehicle_shape_scale  = 1.5
+  hash_delta           = 50
+  hash_shade           = 0.4
+  hash_viewable        = true
+  VName = abc
+  scope = ODOMETRY_DIST
+
+   // Appcast configuration
+  appcast_height       = 75
+  appcast_width        = 30
+  appcast_viewable     = true
+  appcast_color_scheme = indigo
+  nodes_font_size      = medium
+  procs_font_size      = medium
+  appcast_font_size    = small
+
+  BUTTON_ONE = DEPLOY # DEPLOY=true
+  BUTTON_ONE = MOOS_MANUAL_OVERIDE=false # RETURN=false
+  BUTTON_TWO = RETURN # RETURN=true
+}
+
+//------------------------------------------
+// pNodeReporter config block
+
+ProcessConfig = pNodeReporter
+{
+  AppTick	  = 2
+  CommsTick	= 2
+
+  platform_length = 2              // meters
+  platform_beam   = 2             // meters
+  platform_type   = auv
+  platform_color  = dodger_blue
+  //vessel_type = auv
+}
+
+//------------------------------------------
+// pAUV150 config block
+
+ProcessConfig = pAUV150
+{
+  AppTick   = 10
+  CommsTick = 10
+
+  //server_host = 10.127.0.18
+  //server_host = 127.0.0.1
+  //server_host = 192.168.137.1
+  server_host = 192.168.220.1
+  //server_host = 192.168.43.131
+  //LatOrigin  = 43.825300 
+  //LongOrigin = -70.330400
+
+  LatOrigin  = 43.825750 
+  LongOrigin = -70.329158
+
+  ControlFrequency = 10
+}
+
+//------------------------------------------
+// pMotionControler config block
+
+ProcessConfig = pMotionControler
+{
+  AppTick    = 10
+  CommsTick  = 10
+  config_file = /home/zjk/project/moos-ivp-extend/missions/alder/ControlParam.json
+}
+
+//------------------------------------------
+// pLogger config block
+
+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
+}

missions/alpha/alpha_shoreside.moos

@@ -0,0 +1,121 @@
+// Alder mission file
+ServerHost = localhost
+ServerPort = 9000
+Community  = alpha
+MOOSTimeWarp = 1
+
+LatOrigin  = 43.825300 
+LongOrigin = -70.330400 
+
+//------------------------------------------
+// Antler config block
+ProcessConfig = ANTLER
+{
+  MSBetweenLaunches = 200
+
+  Run = MOOSDB        @ NewConsole = false
+  //Run = pLogger     @ NewConsole = false
+  Run = pMarineViewer	@ NewConsole = false
+  //Run = uProcessWatch @ NewConsole = false
+  RUn = pShare        @ NewConsole = false
+}
+
+//------------------------------------------
+// pShare config block
+
+ProcessConfig = pShare
+{
+  AppTick    = 10
+  CommsTick  = 10
+
+  input = route =  localhost:8085
+  output = src_name=APPCAST_REQ, route=192.168.1.200:8082
+  
+  //输出有两个端口，8081和8082，选择用1或者2
+  //发送消息看以下格式
+  //output = src_name=Y, dest_name=B, route=host:port
+  output = src_name=uMission_action_cmd,route=192.168.1.200:8082
+  output = src_name=DEPLOY, route=192.168.1.200:8082
+  output = src_name=RETURN, route=192.168.1.200:8082
+  output = src_name=MOOS_MANUAL_OVERIDE, route=192.168.1.200:8082
+  output = src_name=PZ, route=192.168.1.200:8082
+  output = src_name=LED, route=192.168.1.200:8082
+}
+
+//------------------------------------------
+// uProcessWatch config block
+
+ProcessConfig = uProcessWatch
+{
+  AppTick   = 4
+  CommsTick = 4
+
+  summary_wait = 5
+
+  nowatch   = uXMS*
+  nowatch   = uMAC*
+  nowatch   = uPokeDB*
+  nowatch   = uQueryDB*
+  nowatch   = uTermCommand*
+  watch_all = true
+}
+
+//------------------------------------------
+// pMarineViewer config block
+
+ProcessConfig = pMarineViewer
+{
+  AppTick    = 4
+  CommsTick  = 4
+
+  TIFF_FILE            = forrest19.tif
+  set_pan_x            = -90
+  set_pan_y            = -280
+  zoom                 = 1
+  vehicle_shape_scale  = 1.5
+  hash_delta           = 50
+  hash_shade           = 0.4
+  hash_viewable        = true
+
+  scope = ODOMETRY_DIST
+
+   // Appcast configuration
+  appcast_height       = 75
+  appcast_width        = 30
+  appcast_viewable     = true
+  appcast_color_scheme = indigo
+  nodes_font_size      = medium
+  procs_font_size      = medium
+  appcast_font_size    = small
+
+  BUTTON_ONE = DEPLOY # DEPLOY=true
+  BUTTON_ONE = MOOS_MANUAL_OVERIDE=false # RETURN=false
+  BUTTON_TWO = RETURN # RETURN=true
+  BUTTON_Three = PZ # PZ=true
+
+  BUTTON_Four = LED # LED=on
+}
+
+//------------------------------------------
+// pLogger config block
+
+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
+}

missions/alpha/alpha_vehicle.moos

@@ -0,0 +1,139 @@
+// Alder mission file
+ServerHost = localhost
+ServerPort = 9000
+Community  = auv150
+MOOSTimeWarp = 1
+
+LatOrigin  = 43.825300 
+LongOrigin = -70.330400 
+
+//------------------------------------------
+// Antler config block
+ProcessConfig = ANTLER
+{
+  MSBetweenLaunches = 200
+
+  Run = MOOSDB        @ NewConsole = false
+  Run = pNodeReporter	@ NewConsole = false
+  //Run = pLogger     @ NewConsole = false
+  Run = uProcessWatch @ NewConsole = false
+  Run = pHelmIvP      @ NewConsole = false
+  Run = pAUV150       @ NewConsole = false
+  Run = pShare        @ NewConsole = false
+}
+
+//------------------------------------------
+// pShare config block
+
+ProcessConfig = pShare
+{
+  AppTick    = 10
+  CommsTick  = 10
+  //UUV 信息
+  //output = src_name = NODE_REPORT*, route = 192.168.137.7:8085
+  //App 信息
+  //output = src_name = APPCAST*, route = 192.168.137.7:8085
+  //路径点信息
+  //output = src_name = VIEW*, route = 192.168.137.7:8085
+
+  output = src_name = *, route = 192.168.1.100:8085
+  //调试端输入端口
+  input = route = localhost:8082
+}
+
+//------------------------------------------
+// uProcessWatch config block
+
+ProcessConfig = uProcessWatch
+{
+  AppTick   = 4
+  CommsTick = 4
+
+  summary_wait = 5
+
+  nowatch   = uXMS*
+  nowatch   = uMAC*
+  nowatch   = uPokeDB*
+  nowatch   = uQueryDB*
+  nowatch   = uTermCommand*
+  watch_all = true
+}
+
+//------------------------------------------
+// pHelmIvP config block
+
+ProcessConfig = pHelmIvP
+{
+  AppTick    = 4
+  CommsTick  = 4
+
+  Behaviors  = gly_1.bhv
+  Verbose    = quiet
+  Domain     = course:0:359:360
+  Domain     = speed:0:4:21
+  Domain     = depth:0:100:101
+
+  //IVP_BEHAVIOR_DIR = ../../lib
+
+  ok_skew = any
+  start_in_drive = false
+}
+
+//------------------------------------------
+// pNodeReporter config block
+
+ProcessConfig = pNodeReporter
+{
+  AppTick	  = 2
+  CommsTick	= 2
+
+  platform_length = 2              // meters
+  //platform_beam   = 0.2             // meters
+  platform_type   = auv
+  platform_color  = dodger_blue
+  //vessel_type = auv
+}
+
+//------------------------------------------
+// pAUV150 config block
+
+ProcessConfig = pAUV150
+{
+  AppTick   = 10
+  CommsTick = 10
+
+  //server_host = 10.127.0.18
+  //server_host = 127.0.0.1
+  server_host = 192.168.1.88
+  //LatOrigin  = 43.825300 
+  //LongOrigin = -70.330400
+
+  LatOrigin  = 43.825750 
+  LongOrigin = -70.329158 
+
+  ControlFrequency = 10
+}
+
+//------------------------------------------
+// pLogger config block
+
+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
+}

missions/alpha/gly_1.bhv

@@ -0,0 +1,74 @@
+//--------    FILE: gly_1.bhv   -------------
+
+initialize   DEPLOY = false
+initialize   RETURN = false
+	
+//----------------------------------------------
+Behavior = BHV_Waypoint
+{ 
+  name      = waypt_survey
+  pwt       = 100
+  condition = RETURN = false
+  condition = DEPLOY = true
+  //endflag   = RETURN = true
+  endflag    = MISSION = complete
+
+	configflag = CRUISE_SPD = $[SPEED]
+	//configflag = OSPOS = $[OSX],$[OSY]
+
+	activeflag = INFO=$[OWNSHIP]
+	activeflag = INFO=$[BHVNAME]
+	activeflag = INFO=$[BHVTYPE]
+
+	cycleflag = CINFO=$[OSX],$[OSY]
+
+	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
+
+	         speed_alt = 1.2
+       use_alt_speed = true
+                lead = 8
+         lead_damper = 1
+       lead_to_start = true
+               speed = 2   // meters per second
+        capture_line = true
+      capture_radius = 5.0
+         slip_radius = 15.0
+	efficiency_measure = all
+
+  //          points = pts={107.5,-53.5:112,-43.8:112,-46.1:111.7,-49.3:108.3,-52.7:107.7,-53.3}
+	//         polygon = -50,-100: -50,-300: 25, -300: 25, -100: 100, -100: 100, -300: 175, -300: 175, -100
+           polygon = -50,-100:   0,-300: 50, -287.5: 0, -87.5: 50, -75: 100, -275: 150, -262.5: 100, -62.5
+  //         polygon = -50,-100: -50,-170.71: 0,-220.71: 70.71,-220.71: 120.71,-170.71: 120.71,-100: 70.71,-50: 0,-50: -50,-100
+  //         polygon = -50,-100: -50,-300
+               order = normal
+             repeat  = 0
+
+    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      = bhv_const_depth
+  pwt       = 100
+  condition = DEPLOY = true
+  perpetual  = true
+  updates   = DEPTH_VALUE
+
+        depth = 2
+    duration  = no-time-limit
+    peakwidth = 8
+    basewidth = 12
+    summitdelta = 10
+}

missions/auv150/alpha.bhv

@@ -0,0 +1,94 @@
+//--------    FILE: alpha.bhv   -------------
+
+initialize   DEPLOY = false
+initialize   RETURN = false
+	
+//----------------------------------------------
+Behavior = BHV_Waypoint
+{ 
+  name      = waypt_survey
+  pwt       = 100
+  condition = RETURN = false
+  condition = DEPLOY = true
+  endflag   = RETURN = true
+
+	configflag = CRUISE_SPD = $[SPEED]
+	//configflag = OSPOS = $[OSX],$[OSY]
+
+	activeflag = INFO=$[OWNSHIP]
+	activeflag = INFO=$[BHVNAME]
+	activeflag = INFO=$[BHVTYPE]
+
+	cycleflag = CINFO=$[OSX],$[OSY]
+
+	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
+
+	    speed_alt = 1.2
+  use_alt_speed = true
+                lead = 8
+         lead_damper = 1
+       lead_to_start = true
+               speed = 12   // meters per second
+        capture_line = true
+      capture_radius = 5.0
+         slip_radius = 15.0
+	efficiency_measure = all
+
+  //           points = pts={107.5,-53.5:112,-43.8:112,-46.1:111.7,-49.3:108.3,-52.7:107.7,-53.3}
+	           polygon = 60,-40 : 60,-160 : 150,-160 : 180,-100 : 150,-40
+
+               order = normal
+             //repeat  = 100000
+             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_Waypoint
+{
+  name       = waypt_return
+  pwt        = 100
+  condition  = RETURN = true
+  condition  = DEPLOY = true
+  condition  =  ((DEPLoy = true) or (alpha = one)) or (bravo = two)
+  perpetual  = true
+  updates    = RETURN_UPDATE
+  endflag    = RETURN = false
+  endflag    = DEPLOY = false
+  endflag    = MISSION = complete
+	
+           speed = 2.0
+  capture_radius = 2.0
+     slip_radius = 8.0
+          points = 0,-20
+ }
+
+//----------------------------------------------
+Behavior=BHV_ConstantSpeed
+{
+  name       = const_speed
+  pwt        = 200
+  condition  = SPD=true
+  condition  = DEPLOY = true
+  perpetual  = true
+  updates    = SPEED_UPDATE
+  endflag    = SPD = false
+
+     speed = 0.5
+		 duration = 10
+		 duration_reset = CONST_SPD_RESET=true
+	
+}
+

missions/auv150/alpha_shoreside.moos

@@ -0,0 +1,149 @@
+// Alder mission file
+ServerHost = localhost
+ServerPort = 9000
+Community  = alpha
+MOOSTimeWarp = 1
+
+LatOrigin  = 43.825300 
+LongOrigin = -70.330400 
+
+//------------------------------------------
+// Antler config block
+ProcessConfig = ANTLER
+{
+  MSBetweenLaunches = 200
+
+  Run = MOOSDB        @ NewConsole = false
+  //Run = pLogger     @ NewConsole = false
+  Run = pMarineViewer	@ NewConsole = false
+  //Run = uProcessWatch @ NewConsole = false
+  RUn = pShare        @ NewConsole = false
+}
+
+//------------------------------------------
+// pShare config block
+
+ProcessConfig = pShare
+{
+  AppTick    = 10
+  CommsTick  = 10
+
+  input = route =  localhost:8085
+  output = src_name=APPCAST_REQ, route=192.168.1.200:8082
+  
+  //发送消息看以下格式
+  //output = src_name=Y, dest_name=B, route=host:port
+  output = src_name=uMission_action_cmd,route=192.168.1.200:8082
+  output = src_name=DEPLOY, route=192.168.1.200:8082
+  output = src_name=RETURN, route=192.168.1.200:8082
+  output = src_name=MOOS_MANUAL_OVERIDE, route=192.168.1.200:8082
+  output = src_name=PZ, route=192.168.1.200:8082
+  output = src_name=LED, route=192.168.1.200:8082
+  output = src_name = MOOS_MANUAL_OVERIDE*, route=192.168.1.200:8082
+}
+
+//------------------------------------------
+// uProcessWatch config block
+
+ProcessConfig = uProcessWatch
+{
+  AppTick   = 4
+  CommsTick = 4
+
+  summary_wait = 5
+
+  nowatch   = uXMS*
+  nowatch   = uMAC*
+  nowatch   = uPokeDB*
+  nowatch   = uQueryDB*
+  nowatch   = uTermCommand*
+  watch_all = true
+}
+
+//------------------------------------------
+// pMarineViewer config block
+
+ProcessConfig = pMarineViewer
+{
+  AppTick    = 4
+  CommsTick  = 4
+
+  TIFF_FILE            = forrest19.tif
+  set_pan_x            = -90
+  set_pan_y            = -280
+  zoom                 = 1
+  vehicle_shape_scale  = 1.5
+  hash_delta           = 50
+  hash_shade           = 0.4
+  hash_viewable        = true
+
+  scope = rtU_speed_cmd
+  scope = rtU_u
+  scope = rtU_v
+  scope = rtU_w
+  scope = rtU_phi
+  scope = rtU_theta
+  scope = rtU_psi
+  scope = rtU_x
+  scope = rtU_y
+  scope = rtU_z
+  scope = rtU_dx
+  scope = rtU_dy
+  scope = rtU_dz
+  scope = rtU_heading_cmd
+  scope = rtU_depth_cmd
+  scope = rtY_MainThrusterSpeedCmd
+  scope = rtY_DirectUpperRudderServoAngleCmd
+  scope = rtY_DirectLowerRudderServoAngleCmd
+  scope = rtY_DirectLeftRudderServoAngleCmd
+  scope = rtY_DirectRightRudderServoAngleCmd
+  scope = rtY_heading_error
+  scope = rtY_pitch_error
+  scope = rtY_depth_error
+
+   // Appcast configuration
+  appcast_height       = 75
+  appcast_width        = 30
+  appcast_viewable     = true
+  appcast_color_scheme = indigo
+  nodes_font_size      = medium
+  procs_font_size      = medium
+  appcast_font_size    = small
+
+  BUTTON_ONE = DEPLOY # DEPLOY=true
+  BUTTON_ONE = MOOS_MANUAL_OVERIDE=false # RETURN=false
+
+  BUTTON_TWO = RETURN # RETURN=true
+
+  BUTTON_Three = PZ # PZ=true
+
+  BUTTON_Four = LED # LED=on
+  BUTTON_Five = MANUAL_OVERIDE # MOOS_MANUAL_OVERIDE=true
+
+  //cmd = label=MANUAL_FALSE, var=MOOS_MANUAL_OVERIDE,  sval=false,  receivers=auv150
+  //cmd = label=MANUAL_TRUE, var=MOOS_MANUAL_OVERIDE,  sval=true,  receivers=auv150
+}
+
+//------------------------------------------
+// pLogger config block
+
+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
+}

missions/auv150/alpha_vehicle.moos

@@ -0,0 +1,134 @@
+// Alder mission file
+ServerHost = localhost
+ServerPort = 9000
+Community  = auv150
+MOOSTimeWarp = 1
+
+LatOrigin = $(LatOrigin)
+LongOrigin = $(LongOrigin)
+
+//------------------------------------------
+// Antler config block
+ProcessConfig = ANTLER
+{
+  MSBetweenLaunches = 200
+
+  Run = MOOSDB        @ NewConsole = false
+  Run = pNodeReporter	@ NewConsole = false
+  //Run = pLogger     @ NewConsole = false
+  Run = uProcessWatch @ NewConsole = false
+  Run = pHelmIvP      @ NewConsole = false
+  Run = pAUV150       @ NewConsole = false
+  Run = pShare        @ NewConsole = false
+}
+
+//------------------------------------------
+// pShare config block
+
+ProcessConfig = pShare
+{
+  AppTick    = 10
+  CommsTick  = 10
+  //UUV 信息
+  //output = src_name = NODE_REPORT*, route = 192.168.137.7:8085
+  //App 信息
+  //output = src_name = APPCAST*, route = 192.168.137.7:8085
+  //路径点信息
+  //output = src_name = VIEW*, route = 192.168.137.7:8085
+
+  output = src_name = *, route = 192.168.1.100:8085
+  //调试端输入端口
+  input = route = localhost:8082
+}
+
+//------------------------------------------
+// uProcessWatch config block
+
+ProcessConfig = uProcessWatch
+{
+  AppTick   = 4
+  CommsTick = 4
+
+  summary_wait = 5
+
+  nowatch   = uXMS*
+  nowatch   = uMAC*
+  nowatch   = uPokeDB*
+  nowatch   = uQueryDB*
+  nowatch   = uTermCommand*
+  watch_all = true
+}
+
+//------------------------------------------
+// pHelmIvP config block
+
+ProcessConfig = pHelmIvP
+{
+  AppTick    = 4
+  CommsTick  = 4
+
+  Behaviors  = gly_2.bhv
+  Verbose    = quiet
+  Domain     = course:0:359:360
+  Domain     = speed:0:2.5:21
+  Domain     = depth:0:10:101
+
+  //IVP_BEHAVIOR_DIR = ../../lib
+
+  ok_skew = any
+  start_in_drive = false
+}
+
+//------------------------------------------
+// pNodeReporter config block
+
+ProcessConfig = pNodeReporter
+{
+  AppTick	  = 2
+  CommsTick	= 2
+
+  platform_length = 2              // meters
+  //platform_beam   = 0.2             // meters
+  platform_type   = auv
+  platform_color  = dodger_blue
+  //vessel_type = auv
+}
+
+//------------------------------------------
+// pAUV150 config block
+
+ProcessConfig = pAUV150
+{
+  AppTick   = 10
+  CommsTick = 10
+
+  server_host = $(server_host)
+  LatOrigin = $(LatOrigin)
+  LongOrigin = $(LongOrigin)
+
+  ControlFrequency = 10
+}
+
+//------------------------------------------
+// pLogger config block
+
+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
+}

missions/auv150/gly_1.bhv

@@ -0,0 +1,74 @@
+//--------    FILE: gly_1.bhv   -------------
+
+initialize   DEPLOY = false
+initialize   RETURN = false
+	
+//----------------------------------------------
+Behavior = BHV_Waypoint
+{ 
+  name      = waypt_survey
+  pwt       = 100
+  condition = RETURN = false
+  condition = DEPLOY = true
+  //endflag   = RETURN = true
+  endflag    = MISSION = complete
+
+	configflag = CRUISE_SPD = $[SPEED]
+	//configflag = OSPOS = $[OSX],$[OSY]
+
+	activeflag = INFO=$[OWNSHIP]
+	activeflag = INFO=$[BHVNAME]
+	activeflag = INFO=$[BHVTYPE]
+
+	cycleflag = CINFO=$[OSX],$[OSY]
+
+	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
+
+	         speed_alt = 1.2
+       use_alt_speed = true
+                lead = 8
+         lead_damper = 1
+       lead_to_start = true
+               speed = 2  // meters per second
+        capture_line = true
+      capture_radius = 5.0
+         slip_radius = 15.0
+	efficiency_measure = all
+
+  //          points = pts={107.5,-53.5:112,-43.8:112,-46.1:111.7,-49.3:108.3,-52.7:107.7,-53.3}
+	//         polygon = -50,-100: -50,-300: 25, -300: 25, -100: 100, -100: 100, -300: 175, -300: 175, -100
+           polygon = -50,-100:   0,-300: 50, -287.5: 0, -87.5: 50, -75: 100, -275: 150, -262.5: 100, -62.5
+  //         polygon = -50,-100: -50,-170.71: 0,-220.71: 70.71,-220.71: 120.71,-170.71: 120.71,-100: 70.71,-50: 0,-50: -50,-100
+  //         polygon = -50,-100: -50,-300
+               order = normal
+             repeat  = 0
+
+    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      = bhv_const_depth
+  pwt       = 100
+  condition = DEPLOY = true
+  perpetual  = true
+  updates   = DEPTH_VALUE
+
+        depth = 2
+    duration  = no-time-limit
+    peakwidth = 8
+    basewidth = 12
+    summitdelta = 10
+}

missions/auv150/gly_2.bhv

@@ -0,0 +1,86 @@
+//--------    FILE: gly_2.bhv   -------------
+
+initialize   DEPLOY = false
+initialize   RETURN = false
+
+set MODE = ACTIVE {
+  DEPLOY = true
+} INACTIVE
+
+set MODE = Const {
+  MODE = ACTIVE
+  RETURN != true
+}
+
+set MODE = RETURNING {
+  MODE = ACTIVE
+  RETURN = true
+}
+
+//----------------------------------------------
+Behavior = BHV_ConstantHeading
+{
+// General Behavior Parameters
+name = const_hdg // example
+pwt = 100 // default
+condition  = MODE == Const
+updates = CONST_HDG_UPDATES // example
+
+// Parameters specific to this behavior
+basewidth = 10 // default
+duration = 100
+heading = 270
+heading_mismatch_var = HDG_DIFF // example
+peakwidth = 10 // default
+summitdelta = 25 // default
+}
+
+//----------------------------------------------
+Behavior = BHV_ConstantDepth
+{
+  name      = bhv_const_depth
+  pwt       = 100
+  duration  = 100
+  condition = MODE == Const
+  updates   = DEPTH_VALUE
+
+        depth = 10
+    peakwidth = 8
+    basewidth = 12
+  summitdelta = 10
+}
+
+//----------------------------------------------
+Behavior = BHV_ConstantSpeed
+{
+// General Behavior Parameters
+name = const_spd_transit // example
+pwt = 100 // default
+condition = MODE==Const
+updates = CONST_SPD_UPDATES // example
+
+// Parameters specific to this behavior
+basewidth = 0.2 // default
+duration = 100
+speed = 0.5
+speed_mismatch_var = SPEED_DIFF // example
+peakwidth = 0 // default
+summitdelta = 0 // default
+}
+
+//----------------------------------------------
+Behavior = BHV_Waypoint
+{
+  name      = waypt_return
+  priority  = 100
+  perpetual = true
+  updates   = RETURN_UPDATES
+  condition = MODE==RETURNING
+  endflag   = RETURN = false
+
+      speed = 1
+     radius = 3.0
+  nm_radius = 15.0
+      point = x=-50, y=-100
+}
+

missions/auv150/kill.sh

@@ -0,0 +1,39 @@
+#!/bin/bash -e
+
+# 查找并杀死由launch.sh启动的进程
+# 假设launch.sh启动的进程包含特定的标识符，比如mission名称
+
+# 定义进程标识符（根据实际launch.sh启动的程序进行调整）
+#!/bin/bash -e
+
+# Kill all processes launched by launch.sh
+
+echo "Killing AUV150 mission processes..."
+
+# Kill processes by name patterns
+pkill -f "pAntler.*targ_vehicle.moos" 2>/dev/null || true
+pkill -f "uMAC.*targ_vehicle.moos" 2>/dev/null || true
+pkill -f "pNodeReporter" 2>/dev/null || true
+pkill -f "uProcessWatch" 2>/dev/null || true
+pkill -f "pHelmIvP" 2>/dev/null || true
+pkill -f "pAUV150" 2>/dev/null || true
+pkill -f "pShare" 2>/dev/null || true
+pkill -f "pMarineViewer" 2>/dev/null || true
+pkill -f "MOOSDB" 2>/dev/null || true
+
+# Wait a moment for processes to terminate
+echo "Waiting for processes to terminate..."
+sleep 2
+
+# Force kill any remaining processes
+pkill -9 -f "pAntler.*targ_vehicle.moos" 2>/dev/null || true
+pkill -9 -f "uMAC.*targ_vehicle.moos" 2>/dev/null || true
+pkill -9 -f "pNodeReporter" 2>/dev/null || true
+pkill -9 -f "uProcessWatch" 2>/dev/null || true
+pkill -9 -f "pHelmIvP" 2>/dev/null || true
+pkill -9 -f "pAUV150" 2>/dev/null || true
+pkill -9 -f "pShare" 2>/dev/null || true
+pkill -9 -f "pMarineViewer" 2>/dev/null || true
+pkill -9 -f "MOOSDB" 2>/dev/null || true
+
+echo "All AUV150 processes have been terminated."

missions/auv150/launch.sh

@@ -0,0 +1,68 @@
+#!/bin/bash -e
+
+#----------------------------------------------------------
+#  Script: launch.sh
+#  Author: Michael Benjamin
+#  LastEd: May 17th 2019
+#----------------------------------------------------------
+#  Part 1: Set global var defaults
+#----------------------------------------------------------
+# 注意书写格式，不能有空格
+TIME_WARP=1
+JUST_MAKE="no"
+
+LatOrigin="39.0935" 
+LongOrigin="117.5361" 
+
+
+server_host="192.168.1.88"
+#----------------------------------------------------------
+#  Part 2: Check for and handle command-line arguments
+#----------------------------------------------------------
+for ARGI; do
+    if [ "${ARGI}" = "--help" -o "${ARGI}" = "-h" ]; then
+	echo "launch.sh [SWITCHES] [time_warp]    "
+	echo "  --help, -h           Show this help message            " 
+	echo "  --just_make, -j      Just create targ files, no launch " 
+	echo "  --fast, -f           Init positions for fast encounter " 
+	exit 0;
+    elif [ "${ARGI//[^0-9]/}" = "$ARGI" -a "$TIME_WARP" = 1 ]; then 
+        TIME_WARP=$ARGI
+    elif [ "${ARGI}" = "--just_make" -o "${ARGI}" = "-j" ] ; then
+	JUST_MAKE="yes"
+    elif [ "${ARGI}" = "--fast" -o "${ARGI}" = "-f" ] ; then
+	START_POS1="170,-80,270"         
+	START_POS2="-30,-80,90"        
+	LOITER_POS1="x=0,y=-95"
+	LOITER_POS2="x=125,y=-65"
+    else 
+        echo "launch.sh Bad arg:" $ARGI " Exiting with code: 1"
+        exit 1
+    fi
+done
+
+#----------------------------------------------------------
+#  Part 3: Create the .moos and .bhv files. 
+#----------------------------------------------------------
+nsplug alpha_vehicle.moos targ_vehicle.moos -i -f WARP=$TIME_WARP \
+       LatOrigin=$LatOrigin   \
+       LongOrigin=$LongOrigin  \
+       server_host=$server_host
+
+# nsplug meta_vehicle.bhv targ_gilda.bhv -i -f VNAME=$VNAME2    \
+#        START_POS=$START_POS2 LOITER_POS=$LOITER_POS2       
+
+if [ ${JUST_MAKE} = "yes" ] ; then
+    echo "Files assembled; nothing launched; exiting per request."
+    exit 0
+fi
+
+#----------------------------------------------------------
+#  Part 4: Launch the processes
+#----------------------------------------------------------
+echo "Launching Shoreside MOOS Community. WARP is" $TIME_WARP
+pAntler targ_vehicle.moos >& /dev/null &
+
+uMAC targ_vehicle.moos
+
+kill -- -$$

scripts/clear.sh

@@ -0,0 +1,28 @@
+#!/bin/bash
+
+# 配置变量
+REMOTE_USER="auv"
+REMOTE_HOST="192.168.1.200"
+REMOTE_PATH="/home/auv/software/AUV_150/missions/auv150"
+
+# 定义远程文件路径
+REMOTE_FILE="$REMOTE_PATH/AUV150.db"
+
+# 检查是否安装了ssh
+echo "检查ssh命令..."
+if ! command -v ssh &> /dev/null; then
+    echo "错误: 未找到ssh命令，请安装openssh客户端"
+    exit 1
+fi
+
+# 删除远程文件
+echo "正在删除远程主机 $REMOTE_HOST 上的文件 $REMOTE_FILE..."
+ssh "$REMOTE_USER@$REMOTE_HOST" "rm -f $REMOTE_FILE"
+
+# 检查删除是否成功
+if [ $? -eq 0 ]; then
+    echo "远程文件删除成功!"
+else
+    echo "远程文件删除失败!"
+    exit 1
+fi

scripts/download.sh

@@ -0,0 +1,38 @@
+#!/bin/bash
+
+# 配置变量
+REMOTE_USER="auv"
+REMOTE_HOST="192.168.1.200"
+REMOTE_PATH="/home/auv/software/AUV_150/missions/auv150"
+LOCAL_DATA_DIR="../data"
+
+# 创建本地data目录（如果不存在）
+mkdir -p "$LOCAL_DATA_DIR"
+
+# 获取当前时间戳
+TIMESTAMP=$(date +"%Y%m%d_%H%M%S")
+
+# 定义远程文件和本地文件路径
+REMOTE_FILE="$REMOTE_PATH/AUV150.db"
+LOCAL_FILE="$LOCAL_DATA_DIR/AUV150_$TIMESTAMP.db"
+
+# 检查是否安装了scp
+echo "检查scp命令..."
+if ! command -v scp &> /dev/null; then
+    echo "错误: 未找到scp命令，请安装openssh客户端"
+    exit 1
+fi
+
+# 下载文件
+echo "正在从 $REMOTE_HOST 下载 $REMOTE_FILE..."
+scp "$REMOTE_USER@$REMOTE_HOST:$REMOTE_FILE" "$LOCAL_FILE"
+
+# 检查下载是否成功
+if [ $? -eq 0 ]; then
+    echo "文件下载成功!"
+    echo "本地文件路径: $LOCAL_FILE"
+    echo "文件大小: $(ls -lh "$LOCAL_FILE" | awk '{print $5}')"
+else
+    echo "文件下载失败!"
+    exit 1
+fi

scripts/update.sh

@@ -0,0 +1,22 @@
+#!/bin/bash
+set -e
+
+# 配置变量
+REMOTE_USER="auv"
+REMOTE_HOST="192.168.1.200"
+REMOTE_PATH="/home/auv/software/AUV_150"
+BUILD_SCRIPT="build.sh"  # 假设build脚本名称为build.sh
+
+# 同步src目录到远程主机
+echo "开始同步src目录..."
+rsync -avz --delete -e ssh ../src/ ${REMOTE_USER}@${REMOTE_HOST}:${REMOTE_PATH}/src/
+
+# 同步missions目录到远程主机
+echo "开始同步missions目录..."
+rsync -avz --delete -e ssh ../missions/ ${REMOTE_USER}@${REMOTE_HOST}:${REMOTE_PATH}/missions/
+
+# 在远程服务器上运行build脚本
+echo "在远程服务器上运行build脚本..."
+ssh ${REMOTE_USER}@${REMOTE_HOST} "cd ${REMOTE_PATH} && ./${BUILD_SCRIPT}"
+
+echo "同步完成并已执行build脚本！"

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
 ##############################################################################

src/pAUV150/AUV150.cpp

@@ -12,10 +12,101 @@
 #include <iomanip>
 #include <string>
 #include <iostream>
-
+#include <arpa/inet.h>
 // #define DEBUG_
 
 using namespace std;
+// until函数
+/**
+ * @brief 解析int16数值，除以100并根据高位判断正负
+ * @param value 待解析的int16数值
+ * @return 解析后的浮点数值
+ */
+float parseInt16WithSign(uint16_t value) {
+    // 检查最高位是否为1(负数)
+    if (value >= 32768) {
+        return -((value-32768));
+    }
+    // 正数
+    return value;
+}
+/**
+ * @brief 将double编码为uint8，第一位为符号位
+ * @param value 要编码的double值
+ * @param limit 限幅值，输入将被限制在[-limit, limit]范围内
+ * @return 编码后的uint8值，第一位为符号位(0正1负)，后7位为数值
+ */
+uint8_t encodeDoubleToUint8WithSign(double value, double limit) {
+    // 限幅处理
+    if (value > limit) value = limit;
+    if (value < -limit) value = -limit;
+    
+    // 直接取整数值
+    uint8_t magnitude = static_cast<uint8_t>(round(fabs(value)));
+    
+    // 确保不会超过127
+    magnitude = magnitude > 127 ? 127 : magnitude;
+    
+    // 设置符号位
+    if (value < 0) {
+        magnitude |= 0x80; // 设置最高位为1表示负数
+    }
+    
+    return magnitude;
+}
+static uint8_t calculateCRC(const FeedbackFrame_150AUV& frame) {
+    const uint8_t* start = reinterpret_cast<const uint8_t*>(&frame.navModeFb);
+    const uint8_t* end = reinterpret_cast<const uint8_t*>(&frame.dvlStatus) + sizeof(frame.dvlStatus);
+    
+    uint8_t crc = 0;
+    for (const uint8_t* p = start; p < end; ++p) {
+        crc += *p;
+    }
+    return crc;
+}
+static uint8_t calculateCRC(const CommandFrame_150AUV& frame) {
+    const uint8_t* start = reinterpret_cast<const uint8_t*>(&frame.navMode);
+    const uint8_t* end = reinterpret_cast<const uint8_t*>(&frame.payloadCtrl) + sizeof(frame.payloadCtrl);
+    
+    uint8_t crc = 0;
+    for (const uint8_t* p = start; p < end; ++p) {
+        crc += *p;
+    }
+    return crc;
+}
+void convertToHostByteOrder(FeedbackFrame_150AUV& frame) {
+    frame.frameHeader = ntohs(frame.frameHeader);
+    frame.counter = ntohs(frame.counter);
+    frame.altimeterHeight = ntohs(frame.altimeterHeight);
+    frame.depthSensor = ntohs(frame.depthSensor);
+    frame.trueHeading = ntohs(frame.trueHeading);
+    frame.pitch = ntohs(frame.pitch);
+    frame.roll = ntohs(frame.roll);
+    frame.velocityEast = ntohs(frame.velocityEast);
+    frame.velocityNorth = ntohs(frame.velocityNorth);
+    frame.velocityDown = ntohs(frame.velocityDown);
+    frame.insLongitude = ntohl(frame.insLongitude);
+    frame.insLatitude = ntohl(frame.insLatitude);
+    frame.insAltitude = ntohs(frame.insAltitude);
+    frame.dvlVelX = ntohs(frame.dvlVelX);
+    frame.dvlVelY = ntohs(frame.dvlVelY);
+    frame.dvlVelZ = ntohs(frame.dvlVelZ);
+    frame.thrusterRPM = ntohs(frame.thrusterRPM);
+    frame.batteryVoltage = ntohs(frame.batteryVoltage);
+    frame.batteryTemp = ntohs(frame.batteryTemp);
+    frame.leakStatus = ntohl(frame.leakStatus);
+    frame.frameTail = ntohs(frame.frameTail);
+}
+void convertCommandFrameToHostByteOrder(CommandFrame_150AUV& frame) {
+    frame.frameHeader = ntohs(frame.frameHeader);
+    frame.counter = ntohs(frame.counter);
+    frame.heading = ntohs(frame.heading);
+    frame.depth = ntohs(frame.depth);
+    frame.frameTail = ntohs(frame.frameTail);
+}
+
+// 添加静态成员定义
+Controler AUV150::m_control;
 
 static bool TCP_ReadLoop(void *p)
 {
@@ -32,9 +123,12 @@ static bool TCP_WriteLoop(void *p)
 //---------------------------------------------------------
 // Constructor()
 
-AUV150::AUV150()
+AUV150::AUV150() : m_sqlite("AUV150.db")
 {
-    m_TcpSocket = new XPCTcpSocket(8150L);
+    if (!m_sqlite.createTable()) {
+        std::cerr << "Failed to create database tables" << std::endl;
+    }
+    m_TcpSocket = new XPCTcpSocket(7007L);
     m_real_read_freq = 0;
     m_real_write_freq = 0;
     m_counter = 0;
@@ -49,6 +143,9 @@ AUV150::AUV150()
     m_desiredVarTime = 0;
     m_controlTime = 0;
     m_samplingTime = 0;
+
+    m_overrived = true; //手动控制
+    m_control.initialize();
 }
 
 //---------------------------------------------------------
@@ -74,7 +171,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 +180,52 @@ 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") || (key == "MOOS_MANUAL_OVERRID_AUV150"))
     {
-        m_CommandFrame.rudderUp = msg.GetDouble();
-        m_CommandFrame.rudderDown = msg.GetDouble();
-        m_desiredVarTime = msg.GetTime();
+        string sval = msg.GetString();
+        m_debug.push_back(m_overrived);
+        m_debug_str.push_back(sval);
+        if(sval == "false")
+        {
+            m_overrived = false;
+        }
+        else
+        {
+            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 == "PZ")
+    {
+        m_debug_str.push_back("PZ");
+        m_CommandFrame.payloadCtrl = 1;
+        MOOSPause(1000);
+        m_CommandFrame.payloadCtrl = 0;
+    }
+    else if(key == "LED")
+    {
+        m_debug_str.push_back("LED" + msg.GetString());
+        if(m_CommandFrame.led == 1)
+        {
+            m_CommandFrame.led = 0x00;
+        }
+        else
+        {
+            m_CommandFrame.led = 0x01;
+        }
+    }
+    else if(key != "APPCAST_REQ") 
        reportRunWarning("Unhandled Mail: " + key);
    }
 	
@@ -132,6 +255,37 @@ bool AUV150::Iterate()
         m_geodesy.LatLong2LocalGrid(m_status.insLatitude, m_status.insLongitude, m_pos_x, m_pos_y);
         postStatusUpdate("NAV");
     #endif
+    m_sqlite.insertData(m_status);
+    m_sqlite.insertData(m_CommandFrame);
+    m_sqlite.insertData(m_FeedbackFrame);
+    // 发送状态
+
+    // Notify individual variables for easier parsing
+    Notify("rtU_u", m_control.rtU.u);
+    Notify("rtU_v", m_control.rtU.v);
+    Notify("rtU_w", m_control.rtU.w);
+    Notify("rtU_phi", m_control.rtU.phi);
+    Notify("rtU_theta", m_control.rtU.theta);
+    Notify("rtU_psi", m_control.rtU.psi);
+    Notify("rtU_x", m_control.rtU.x);
+    Notify("rtU_y", m_control.rtU.y);
+    Notify("rtU_z", m_control.rtU.z);
+    Notify("rtU_dx", m_control.rtU.dx);
+    Notify("rtU_dy", m_control.rtU.dy);
+    Notify("rtU_dz", m_control.rtU.dz);
+    Notify("rtU_speed_cmd", m_control.rtU.speed_cmd);
+    Notify("rtU_heading_cmd", m_control.rtU.heading_cmd);
+    Notify("rtU_depth_cmd", m_control.rtU.depth_cmd);
+
+    Notify("rtY_MainThrusterSpeedCmd", m_control.rtY.MainThrusterSpeedCmd);
+    Notify("rtY_DirectUpperRudderServoAngleCmd", m_control.rtY.DirectUpperRudderServoAngleCmd);
+    Notify("rtY_DirectLowerRudderServoAngleCmd", m_control.rtY.DirectLowerRudderServoAngleCmd);
+    Notify("rtY_DirectLeftRudderServoAngleCmd", m_control.rtY.DirectLeftRudderServoAngleCmd);
+    Notify("rtY_DirectRightRudderServoAngleCmd", m_control.rtY.DirectRightRudderServoAngleCmd);
+    Notify("rtY_heading_error", m_control.rtY.heading_error);
+    Notify("rtY_pitch_error", m_control.rtY.pitch_error);
+    Notify("rtY_depth_error", m_control.rtY.depth_error);
+
     AppCastingMOOSApp::PostReport();
     return(true);
 }
@@ -202,13 +356,17 @@ 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_HEADING", 0);
+  Register("DESIRED_SPEED", 0);
+  Register("DESIRED_DEPTH", 0);
+  Register("MOOS_MANUAL_OVERIDE", 0);
+  Register("MOOS_MANUAL_OVERRIDE", 0);
+  Register("MOOS_MANUAL_OVERIDE*", 0);
+  Register("MOOS_MANUAL_OVERRIDE*", 0);
+  Register("PZ", 0);
+  Register("LED", 0);
 }
 
-
 bool AUV150::Start()
 {
     //初始化TcpSocket
@@ -247,10 +405,12 @@ 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),10);
                 if(count > 0) {
                     std::cout << "Received " << count << " bytes" << std::endl;
                     FeedbackFrame_150AUV *p = reinterpret_cast<FeedbackFrame_150AUV *>(buffer);
+                    convertToHostByteOrder(*p);
+                    m_FeedbackFrame = *p;
                     // 处理接受到的数据
                     if(p->frameHeader != 0xEBA1)
                     {
@@ -258,30 +418,85 @@ bool AUV150::ListenLoop()
                         m_faultCodes.insert(0x11);
                         continue;
                     }
+                    if(p->crc != calculateCRC(*p))
+                    {
+                        std::cout << "Received data is not a valid frame" << std::endl;
+                        m_faultCodes.insert(0x14); // 0x14 校验错误
+                        continue;
+                    }
                     // 解析数据
                     m_counter = p->counter;
                     updateStatus(*p);
                     postStatusUpdate("NAV");
-
-                    m_samplingTime = MOOSTime();
-                    message_count++;
-                    double elapsed_time = m_samplingTime - start_time;
-                    if(elapsed_time >= 1.0) {  // 每秒更新一次频率
-                        m_real_read_freq = message_count / elapsed_time;
-                        message_count = 0;
-                        start_time = MOOSTime();
+                    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 = encodeDoubleToUint8WithSign(-m_control.rtY.DirectUpperRudderServoAngleCmd,30); 
+                        m_CommandFrame.rudderDown = encodeDoubleToUint8WithSign(-m_control.rtY.DirectLowerRudderServoAngleCmd,30);
+                        m_CommandFrame.rudderLeft = encodeDoubleToUint8WithSign(-m_control.rtY.DirectLeftRudderServoAngleCmd,30);
+                        m_CommandFrame.rudderRight = encodeDoubleToUint8WithSign(-m_control.rtY.DirectRightRudderServoAngleCmd,30);
+                        m_CommandFrame.mainThruster = encodeDoubleToUint8WithSign(m_control.rtY.MainThrusterSpeedCmd,100);  
+                        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;
                     }
-                    //正常情况下清除错误码
-                    m_faultCodes.erase(0x11);
-                    m_faultCodes.erase(0x12);
-                    m_faultCodes.erase(0x13);
-                }
-                else
-                {
-                    m_faultCodes.insert(0x12); //无数据接收
-                }
+                    else
+                    {
+                        m_CommandFrame.rudderUp = 0; 
+                        m_CommandFrame.rudderDown = 0;
+                        m_CommandFrame.rudderLeft = 0;
+                        m_CommandFrame.rudderRight = 0;
+                        m_CommandFrame.mainThruster = 0;   
+                    }
+            }
+            try
+            {   
+                m_CommandFrame.crc = calculateCRC(m_CommandFrame);
+
+                convertCommandFrameToHostByteOrder(m_CommandFrame);
+                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;
+            if(elapsed_time >= 1.0) {  // 每秒更新一次频率
+                m_real_read_freq = message_count / elapsed_time;
+                message_count = 0;
+                start_time = MOOSTime();
+            }
+            //正常情况下清除错误码
+            m_faultCodes.erase(0x11);
+            m_faultCodes.erase(0x12);
+            m_faultCodes.erase(0x13);
+        }
+        else
+        {
+            m_faultCodes.insert(0x12); //无数据接收
+        }
         }
         catch (XPCException & e)
         {
@@ -295,38 +510,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;
@@ -338,7 +523,7 @@ bool AUV150::WriteLoop()
                     delete m_TcpSocket;
                 }
                 // m_TcpSocket->vCloseSocket();
-                m_TcpSocket = new XPCTcpSocket(8150L);
+                m_TcpSocket = new XPCTcpSocket(7007L);
                 m_TcpSocket->vSetRecieveBuf(m_nReceiveBufferSizeKB * 1024);
                 m_TcpSocket->vSetSendBuf(m_nSendBufferSizeKB * 1024);
                 m_TcpSocket->vConnect(m_serverHost.c_str());
@@ -353,7 +538,6 @@ bool AUV150::WriteLoop()
     return true;
 }
 
-
 bool AUV150::buildReport()
 {
     auto displayWidth = [&](const std::string& str) {
@@ -425,18 +609,34 @@ 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;
+    for (const auto& debug_value : m_debug)
+    {
+        m_msgs << "debug_value: " << debug_value << std::endl;
+    }
+    m_debug.clear();
+    for (const auto& debug_str : m_debug_str)
+    {
+        m_msgs << debug_str << std::endl;
+    }
+    m_debug_str.clear();
     // =============== Navigation Info ===============
       printTable(
-      {"Connect", "Read", "Writ" ,"Conter" ,"Server ip","Latitude", "Longitude","Gap"},
+      {"Connect", "Fre","Cont" ,"ip"},
       {
           (isConnected() ? "Yes" : "No"),
-          doubleToString(getRealReadFreq(), 2) + " Hz",
-          doubleToString(getRealWriteFreq(), 2) + " Hz",
+          doubleToString(getRealReadFreq(), 4) + " Hz",
           ulintToString(getCounter()),
           m_serverHost,
-          doubleToString(m_OriginLatitude, 5) + "deg",
-          doubleToString(m_OriginLongitude, 5) + "deg",
-          doubleToString(m_controlGap, 5) + "s"
+        //   doubleToString(m_OriginLatitude, 5) + "deg",
+        //   doubleToString(m_OriginLongitude, 5) + "deg",
+        //   doubleToString(m_controlGap, 5) + "s"
       }
     );
     m_msgs << "Fault Codes: ";
@@ -445,9 +645,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 +662,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 +674,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 +685,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, 1) + "deg",
+            doubleToString(m_CommandFrame.rudderDown, 1) + "deg",
+            doubleToString(m_CommandFrame.rudderLeft, 1) + "deg",
+            doubleToString(m_CommandFrame.rudderRight, 1) + "deg",
         }
     );
     // m_msgs << "---------------------------------\n";
@@ -499,16 +701,16 @@ bool AUV150::buildReport()
     printTable(
         {"Battery Voltage", "Battery Level", "Battery Temperature"},
         {
-            doubleToString(m_status.batteryVoltage / 1000.0, 2) + "V",
+            doubleToString(m_status.batteryVoltage, 2) + "V",
             uintToString(m_status.batteryLevel) + "%",
-            doubleToString(m_status.batteryTemp / 10.0, 1) + "°C"
+            doubleToString(m_status.batteryTemp, 1) + "°C"
         }
     );
     m_msgs << "---------------------------------\n";
 
     // =============== System Status ===============
     printTable(
-        {"Light", "Leak", "Power" , "Emergency power" , "Payload", "DVL Status","Thruster Status"},
+        {"Light", "Leak", "CtlFlg" , "Emergency power" , "PZ", "DVL","Thruster Status"},
         {
             m_status.ledSwitch ? "ON" : "OFF",
             m_status.leakStatus ? "LEAK" : "NORMAL",
@@ -527,25 +729,27 @@ 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 / 10.0f;
+
+    // m_status.pitch = feedbackFrame.pitch / 10.0f;
+    m_status.pitch = parseInt16WithSign(feedbackFrame.pitch)/10.0f;
+    m_status.roll = parseInt16WithSign(feedbackFrame.roll)/10.0f;
+    m_status.velocityEast = parseInt16WithSign(feedbackFrame.velocityEast)/100.0f;
+    m_status.velocityNorth = parseInt16WithSign(feedbackFrame.velocityNorth)/100.0f;
+    m_status.velocityDown = parseInt16WithSign(feedbackFrame.velocityDown)/100.0f;
+    m_status.insLongitude = feedbackFrame.insLongitude / 1e6; 
+    m_status.insLatitude = feedbackFrame.insLatitude / 1e6;
+    m_status.insAltitude = feedbackFrame.insAltitude / 100.0f;
+    m_status.dvlVelX = parseInt16WithSign(feedbackFrame.dvlVelX) / 100.0f;
+    m_status.dvlVelY = parseInt16WithSign(feedbackFrame.dvlVelY) / 100.0f;
+    m_status.dvlVelZ = parseInt16WithSign(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,7 +759,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 +801,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);
 }

src/pAUV150/AUV150.h

@@ -14,6 +14,9 @@
 #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 "sqlit3/SQLite.h"
 
 #include <iostream>
 #include <string>
@@ -34,7 +37,7 @@ struct CommandFrame_150AUV {
     uint8_t  rudderDown;      // 9. 直操下舵机角度指令 @12 Uint8 单位:占空比 首位符号位，-30~30 (°)
     uint8_t  rudderLeft;      // 10. 直操左舵机角度指令 @13 Uint8 单位:占空比 首位符号位，-30~30 (°)
     uint8_t  rudderRight;     // 11. 直操右舵机角度指令 @14 Uint8 单位:占空比 首位符号位，-30~30 (°)
-    uint8_t  led;             // 12. LED灯      @15  Uint8   单位:—        0x00: 关，0x01: 开
+    uint8_t  led = 0x01;             // 12. LED灯      @15  Uint8   单位:—        0x00: 关，0x01: 开
     uint8_t  dvlSwitch;       // 13. 传感器开关:DVL开关 @16 Uint8 单位:—   0xFF: 默认，0x00: 关，0x01: 开
     uint8_t  payloadCtrl;     // 14. 控制抛载  @17  Uint8   单位:—        0xFF: 默认，0x01: 执行抛载
     uint8_t  crc;             // 15. CRC校验位 @18  Uint8   单位:—        对“航行模式”到“控制抛载”区间进行求和校验
@@ -50,24 +53,24 @@ struct FeedbackFrame_150AUV {
     uint16_t altimeterHeight;// 5. 高度计：高度 @6   Uint16  单位:0.01m        无符号，0~100 对应 0~1.00m，取小值
     uint16_t depthSensor;    // 6. 深度计：深度 @8   Uint16  单位:0.01m        无符号，0~600 对应 0~6.00m
     uint16_t trueHeading;    // 7. 真航向 @10  Uint16  单位:0.01°        无符号，0~36000 对应 0~360°，惯导无符号
-    int16_t  pitch;          // 8. 俯仰角 @12  int16   单位:0.01°        -18000~18000 对应 -180~180°，惯导
-    int16_t  roll;           // 9. 横滚角 @14  int16   单位:0.01°        -18000~18000 对应 -180~180°，惯导
-    int16_t  velocityEast;   // 10. 东向速度 @16 int16   单位:0.01m/s     -10000~10000 对应 -100~100m/s，惯导
-    int16_t  velocityNorth;  // 11. 北向速度 @18 int16   单位:0.01m/s     -10000~10000 对应 -100~100m/s，惯导
-    int16_t  velocityDown;   // 12. 垂向速度 @20 int16   单位:0.01m/s     -10000~10000 对应 -100~100m/s，惯导
+    uint16_t  pitch;          // 8. 俯仰角 @12  int16   单位:0.01°        -18000~18000 对应 -180~180°，惯导
+    uint16_t  roll;           // 9. 横滚角 @14  int16   单位:0.01°        -18000~18000 对应 -180~180°，惯导
+    uint16_t  velocityEast;   // 10. 东向速度 @16 int16   单位:0.01m/s     -10000~10000 对应 -100~100m/s，惯导
+    uint16_t  velocityNorth;  // 11. 北向速度 @18 int16   单位:0.01m/s     -10000~10000 对应 -100~100m/s，惯导
+    uint16_t  velocityDown;   // 12. 垂向速度 @20 int16   单位:0.01m/s     -10000~10000 对应 -100~100m/s，惯导
     uint32_t insLongitude;   // 13. 惯导：经度 @22 Uint32  单位:°，精度:360/2^32 -180~180，惯导
     uint32_t insLatitude;    // 14. 惯导：纬度 @26 Uint32  单位:°，精度:180/2^32 -90~90，惯导
-    int16_t  insAltitude;    // 15. 惯导：高度 @30 int16   单位:0.01m        无符号，惯导
-    int16_t  dvlVelX;        // 16. DVL:横向速度 @32 int16   单位:0.01m/s     DVL
-    int16_t  dvlVelY;        // 17. DVL:纵向速度 @34 int16   单位:0.01m/s     DVL
-    int16_t  dvlVelZ;        // 18. DVL:天向速度 @36 int16   单位:0.01m/s     DVL
-    int16_t  thrusterRPM;    // 19. 主推进器实际转速(x1) @38 int16 单位:rpm      -4000~4000，推进器
+    uint16_t  insAltitude;    // 15. 惯导：高度 @30 int16   单位:0.01m        无符号，惯导
+    uint16_t  dvlVelX;        // 16. DVL:横向速度 @32 int16   单位:0.01m/s     DVL
+    uint16_t  dvlVelY;        // 17. DVL:纵向速度 @34 int16   单位:0.01m/s     DVL
+    uint16_t  dvlVelZ;        // 18. DVL:天向速度 @36 int16   单位:0.01m/s     DVL
+    uint16_t  thrusterRPM;    // 19. 主推进器实际转速(x1) @38 int16 单位:rpm      -4000~4000，推进器
     uint8_t  ledSwitch;      // 20. 灯的开关 @40   Uint8   单位:—           0x00: 关，0x01: 开，灯
     uint16_t batteryVoltage; // 21. 电池电压反馈 @41 Uint16  单位:mV         约24000，电池状态
     uint8_t  batteryLevel;   // 22. 电池电量反馈 @43 Uint8   单位:%          0~100，电池状态
     uint16_t batteryTemp;    // 23. 电池温度反馈 @44 Uint16  单位:0.1°C       0~1000 对应 0~100.0°C，电池状态
     uint32_t leakStatus;     // 24. 漏水传感器反馈 @46 Uint32  单位:—         漏水传感器状态
-    uint8_t  powerModule;    // 25. 电源模块状态反馈 @50 Uint8   单位:—         电源模块状态
+    uint8_t  powerModule;    // 25. 操作标志 @50 Uint8   单位:—         操作标志 1 允许控制 0 不允许控制
     uint8_t  backupPower;    // 26. 应急电源模块状态反馈 @51 Uint8   单位:—     应急电源模块状态
     uint8_t  thrusterStatus; // 27. 推进器状态反馈 @52 Uint8   单位:—         推进器状态
     uint8_t  reserved;       // 30. 预留 @55   Uint8   单位:—         
@@ -110,6 +113,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
@@ -149,6 +164,10 @@ public:
     // 状态发布函数
     void postStatusUpdate(std::string prefix);
 
+    //DEBUG
+    std::vector<uint16_T> m_debug;
+    std::vector<std::string> m_debug_str;
+
 private: // State variables
   XPCTcpSocket* m_TcpSocket;
   CMOOSThread WritingThread_; 
@@ -163,6 +182,9 @@ private: // State variables
   std::set<unsigned char> m_faultCodes;
   unsigned long int m_counter;
 
+  SQLite m_sqlite;
+  
+
   //导航相关
   CMOOSGeodesy m_geodesy;
   bool m_geo_ok;
@@ -179,6 +201,11 @@ private: // State variables
 
   double m_controlTime;
   double m_samplingTime;
+
+  bool m_overrived;
+  
+  //控制器
+  static Controler m_control;
 };
 
 #endif

src/pAUV150/CMakeLists.txt

@@ -12,10 +12,25 @@ 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
+  sqlit3/SQLite.cpp
+  sqlit3/sqlite3.c
+  ${SRC_FILES}
+)
+
+# 添加头文件目录
+include_directories(
+  ${MOOSGEODESY_INCLUDE_DIRS}
+  ${CMAKE_CURRENT_SOURCE_DIR}/Controler
+  ${CMAKE_CURRENT_SOURCE_DIR}/sqlit3
 )
 
 ADD_EXECUTABLE(pAUV150 ${SRC})
@@ -23,8 +38,8 @@ ADD_EXECUTABLE(pAUV150 ${SRC})
 TARGET_LINK_LIBRARIES(pAUV150
    ${MOOS_LIBRARIES}
    ${MOOSGEODESY_LIBRARIES}
+   geometry
    apputil
    mbutil
    m
    pthread)
-

src/pAUV150/Controler/Controler.cpp

@@ -0,0 +1,338 @@
+//
+// 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.115
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Tue Aug 12 11:26:01 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_DeadZone;
+  real_T rtb_FilterCoefficient;
+  real_T rtb_FilterCoefficient_a;
+  real_T rtb_IntegralGain;
+  real_T rtb_Integrator_l;
+  real_T rtb_Sum;
+  real_T rtb_Switch;
+  real_T rtb_depth_error;
+  int8_T tmp;
+  int8_T tmp_0;
+  boolean_T rtb_RelationalOperator;
+  boolean_T rtb_fixforDTpropagationissue;
+
+  // Outputs for Atomic SubSystem: '<Root>/Controler'
+  // MATLAB Function: '<S1>/MATLAB Function1' incorporates:
+  //   Inport: '<Root>/heading_cmd'
+  //   Inport: '<Root>/psi'
+
+  mod_error = rtU.psi - rtU.heading_cmd;
+  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 Function1'
+
+  // Gain: '<S98>/Filter Coefficient' incorporates:
+  //   DiscreteIntegrator: '<S90>/Filter'
+  //   Gain: '<S88>/Derivative Gain'
+  //   Sum: '<S90>/SumD'
+
+  rtb_FilterCoefficient = (rtP.pid_heading.D * mod_error - rtDW.Filter_DSTATE) *
+    rtP.pid_heading.N;
+
+  // Sum: '<S104>/Sum' incorporates:
+  //   DiscreteIntegrator: '<S95>/Integrator'
+  //   Gain: '<S100>/Proportional Gain'
+
+  rtb_Integrator_l = (rtP.pid_heading.P * mod_error + rtDW.Integrator_DSTATE) +
+    rtb_FilterCoefficient;
+
+  // DeadZone: '<S87>/DeadZone'
+  if (rtb_Integrator_l > 35.0) {
+    rtb_IntegralGain = rtb_Integrator_l - 35.0;
+  } else if (rtb_Integrator_l >= -35.0) {
+    rtb_IntegralGain = 0.0;
+  } else {
+    rtb_IntegralGain = rtb_Integrator_l - -35.0;
+  }
+
+  // End of DeadZone: '<S87>/DeadZone'
+
+  // RelationalOperator: '<S85>/Relational Operator' incorporates:
+  //   Constant: '<S85>/Clamping_zero'
+
+  rtb_RelationalOperator = (rtb_IntegralGain != 0.0);
+
+  // RelationalOperator: '<S85>/fix for DT propagation issue' incorporates:
+  //   Constant: '<S85>/Clamping_zero'
+
+  rtb_fixforDTpropagationissue = (rtb_IntegralGain > 0.0);
+
+  // Gain: '<S92>/Integral Gain'
+  rtb_IntegralGain = rtP.pid_heading.I * mod_error;
+
+  // Switch: '<S85>/Switch1' incorporates:
+  //   Constant: '<S85>/Constant'
+  //   Constant: '<S85>/Constant2'
+
+  if (rtb_fixforDTpropagationissue) {
+    tmp = 1;
+  } else {
+    tmp = -1;
+  }
+
+  // Switch: '<S85>/Switch2' incorporates:
+  //   Constant: '<S85>/Clamping_zero'
+  //   Constant: '<S85>/Constant3'
+  //   Constant: '<S85>/Constant4'
+  //   RelationalOperator: '<S85>/fix for DT propagation issue1'
+
+  if (rtb_IntegralGain > 0.0) {
+    tmp_0 = 1;
+  } else {
+    tmp_0 = -1;
+  }
+
+  // Switch: '<S85>/Switch' incorporates:
+  //   Constant: '<S85>/Constant1'
+  //   Logic: '<S85>/AND3'
+  //   RelationalOperator: '<S85>/Equal1'
+  //   Switch: '<S85>/Switch1'
+  //   Switch: '<S85>/Switch2'
+
+  if (rtb_RelationalOperator && (tmp == tmp_0)) {
+    rtb_Switch = 0.0;
+  } else {
+    rtb_Switch = rtb_IntegralGain;
+  }
+
+  // End of Switch: '<S85>/Switch'
+
+  // Saturate: '<S102>/Saturation'
+  if (rtb_Integrator_l > 35.0) {
+    rtb_Integrator_l = 35.0;
+  } else if (rtb_Integrator_l < -35.0) {
+    rtb_Integrator_l = -35.0;
+  }
+
+  // End of Saturate: '<S102>/Saturation'
+
+  // Sum: '<S1>/Sum1' incorporates:
+  //   Inport: '<Root>/depth_cmd'
+  //   Inport: '<Root>/z'
+
+  rtb_depth_error = rtU.depth_cmd - rtU.z;
+
+  // Gain: '<S38>/Integral Gain'
+  rtb_IntegralGain = rtP.pid_depth.I * rtb_depth_error;
+
+  // Gain: '<S44>/Filter Coefficient' incorporates:
+  //   DiscreteIntegrator: '<S36>/Filter'
+  //   Gain: '<S34>/Derivative Gain'
+  //   Sum: '<S36>/SumD'
+
+  rtb_FilterCoefficient_a = (rtP.pid_depth.D * rtb_depth_error -
+    rtDW.Filter_DSTATE_m) * rtP.pid_depth.N;
+
+  // Sum: '<S50>/Sum' incorporates:
+  //   DiscreteIntegrator: '<S41>/Integrator'
+  //   Gain: '<S46>/Proportional Gain'
+
+  rtb_Sum = (rtP.pid_depth.P * rtb_depth_error + rtDW.Integrator_DSTATE_o) +
+    rtb_FilterCoefficient_a;
+
+  // DeadZone: '<S33>/DeadZone' incorporates:
+  //   Saturate: '<S48>/Saturation'
+
+  if (rtb_Sum > 30.0) {
+    rtb_DeadZone = rtb_Sum - 30.0;
+    rtb_Sum = 30.0;
+  } else {
+    if (rtb_Sum >= -30.0) {
+      rtb_DeadZone = 0.0;
+    } else {
+      rtb_DeadZone = rtb_Sum - -30.0;
+    }
+
+    if (rtb_Sum < -30.0) {
+      rtb_Sum = -30.0;
+    }
+  }
+
+  // End of DeadZone: '<S33>/DeadZone'
+
+  // Update for DiscreteIntegrator: '<S95>/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: '<S95>/Integrator'
+
+  // Update for DiscreteIntegrator: '<S90>/Filter'
+  rtDW.Filter_DSTATE += 0.1 * rtb_FilterCoefficient;
+
+  // Switch: '<S31>/Switch1' incorporates:
+  //   Constant: '<S31>/Clamping_zero'
+  //   Constant: '<S31>/Constant'
+  //   Constant: '<S31>/Constant2'
+  //   RelationalOperator: '<S31>/fix for DT propagation issue'
+
+  if (rtb_DeadZone > 0.0) {
+    tmp = 1;
+  } else {
+    tmp = -1;
+  }
+
+  // Switch: '<S31>/Switch2' incorporates:
+  //   Constant: '<S31>/Clamping_zero'
+  //   Constant: '<S31>/Constant3'
+  //   Constant: '<S31>/Constant4'
+  //   RelationalOperator: '<S31>/fix for DT propagation issue1'
+
+  if (rtb_IntegralGain > 0.0) {
+    tmp_0 = 1;
+  } else {
+    tmp_0 = -1;
+  }
+
+  // Switch: '<S31>/Switch' incorporates:
+  //   Constant: '<S31>/Clamping_zero'
+  //   Constant: '<S31>/Constant1'
+  //   Logic: '<S31>/AND3'
+  //   RelationalOperator: '<S31>/Equal1'
+  //   RelationalOperator: '<S31>/Relational Operator'
+  //   Switch: '<S31>/Switch1'
+  //   Switch: '<S31>/Switch2'
+
+  if ((rtb_DeadZone != 0.0) && (tmp == tmp_0)) {
+    rtb_IntegralGain = 0.0;
+  }
+
+  // Update for DiscreteIntegrator: '<S41>/Integrator' incorporates:
+  //   Switch: '<S31>/Switch'
+
+  rtDW.Integrator_DSTATE_o += rtb_IntegralGain;
+  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: '<S41>/Integrator'
+
+  // Update for DiscreteIntegrator: '<S36>/Filter'
+  rtDW.Filter_DSTATE_m += 0.1 * rtb_FilterCoefficient_a;
+
+  // Outport: '<Root>/MainThrusterSpeedCmd' incorporates:
+  //   Constant: '<S1>/Constant1'
+  //   Inport: '<Root>/speed_cmd'
+  //   Product: '<S1>/Divide'
+
+  rtY.MainThrusterSpeedCmd = rtU.speed_cmd / 0.025;
+
+  // End of Outputs for SubSystem: '<Root>/Controler'
+
+  // Outport: '<Root>/DirectUpperRudderServoAngleCmd'
+  rtY.DirectUpperRudderServoAngleCmd = rtb_Integrator_l;
+
+  // Outport: '<Root>/DirectLowerRudderServoAngleCmd'
+  rtY.DirectLowerRudderServoAngleCmd = rtb_Integrator_l;
+
+  // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
+  rtY.DirectLeftRudderServoAngleCmd = rtb_Sum;
+
+  // Outport: '<Root>/DirectRightRudderServoAngleCmd'
+  rtY.DirectRightRudderServoAngleCmd = rtb_Sum;
+
+  // Outport: '<Root>/heading_error'
+  rtY.heading_error = mod_error;
+
+  // Outport: '<Root>/pitch_error' incorporates:
+  //   Constant: '<S1>/Constant'
+
+  rtY.pitch_error = 0.0;
+
+  // 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]
+//

src/pAUV150/Controler/Controler.h

@@ -0,0 +1,297 @@
+//
+// 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.115
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Tue Aug 12 11:26:01 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;          // '<S95>/Integrator'
+    real_T Filter_DSTATE;              // '<S90>/Filter'
+    real_T Integrator_DSTATE_o;        // '<S41>/Integrator'
+    real_T Filter_DSTATE_m;            // '<S36>/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:
+                                               //    '<S34>/Derivative Gain'
+                                               //    '<S38>/Integral Gain'
+                                               //    '<S44>/Filter Coefficient'
+                                               //    '<S46>/Proportional Gain'
+
+    struct_n3jlyiLoC0PV1TieyhpPwD pid_heading;// Variable: pid_heading
+                                                 //  Referenced by:
+                                                 //    '<S88>/Derivative Gain'
+                                                 //    '<S92>/Integral Gain'
+                                                 //    '<S98>/Filter Coefficient'
+                                                 //    '<S100>/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>/Gain' : Eliminated nontunable gain of 1
+//  Block '<S1>/Gain1' : Eliminated nontunable gain of 1
+
+
+//-
+//  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('SimModel0621_S4/Controler')    - opens subsystem SimModel0621_S4/Controler
+//  hilite_system('SimModel0621_S4/Controler/Kp') - opens and selects block Kp
+//
+//  Here is the system hierarchy for this model
+//
+//  '<Root>' : 'SimModel0621_S4'
+//  '<S1>'   : 'SimModel0621_S4/Controler'
+//  '<S2>'   : 'SimModel0621_S4/Controler/MATLAB Function1'
+//  '<S3>'   : 'SimModel0621_S4/Controler/PID Controller1'
+//  '<S4>'   : 'SimModel0621_S4/Controler/PID Controller3'
+//  '<S5>'   : 'SimModel0621_S4/Controler/PID Controller1/Anti-windup'
+//  '<S6>'   : 'SimModel0621_S4/Controler/PID Controller1/D Gain'
+//  '<S7>'   : 'SimModel0621_S4/Controler/PID Controller1/External Derivative'
+//  '<S8>'   : 'SimModel0621_S4/Controler/PID Controller1/Filter'
+//  '<S9>'   : 'SimModel0621_S4/Controler/PID Controller1/Filter ICs'
+//  '<S10>'  : 'SimModel0621_S4/Controler/PID Controller1/I Gain'
+//  '<S11>'  : 'SimModel0621_S4/Controler/PID Controller1/Ideal P Gain'
+//  '<S12>'  : 'SimModel0621_S4/Controler/PID Controller1/Ideal P Gain Fdbk'
+//  '<S13>'  : 'SimModel0621_S4/Controler/PID Controller1/Integrator'
+//  '<S14>'  : 'SimModel0621_S4/Controler/PID Controller1/Integrator ICs'
+//  '<S15>'  : 'SimModel0621_S4/Controler/PID Controller1/N Copy'
+//  '<S16>'  : 'SimModel0621_S4/Controler/PID Controller1/N Gain'
+//  '<S17>'  : 'SimModel0621_S4/Controler/PID Controller1/P Copy'
+//  '<S18>'  : 'SimModel0621_S4/Controler/PID Controller1/Parallel P Gain'
+//  '<S19>'  : 'SimModel0621_S4/Controler/PID Controller1/Reset Signal'
+//  '<S20>'  : 'SimModel0621_S4/Controler/PID Controller1/Saturation'
+//  '<S21>'  : 'SimModel0621_S4/Controler/PID Controller1/Saturation Fdbk'
+//  '<S22>'  : 'SimModel0621_S4/Controler/PID Controller1/Sum'
+//  '<S23>'  : 'SimModel0621_S4/Controler/PID Controller1/Sum Fdbk'
+//  '<S24>'  : 'SimModel0621_S4/Controler/PID Controller1/Tracking Mode'
+//  '<S25>'  : 'SimModel0621_S4/Controler/PID Controller1/Tracking Mode Sum'
+//  '<S26>'  : 'SimModel0621_S4/Controler/PID Controller1/Tsamp - Integral'
+//  '<S27>'  : 'SimModel0621_S4/Controler/PID Controller1/Tsamp - Ngain'
+//  '<S28>'  : 'SimModel0621_S4/Controler/PID Controller1/postSat Signal'
+//  '<S29>'  : 'SimModel0621_S4/Controler/PID Controller1/preInt Signal'
+//  '<S30>'  : 'SimModel0621_S4/Controler/PID Controller1/preSat Signal'
+//  '<S31>'  : 'SimModel0621_S4/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel'
+//  '<S32>'  : 'SimModel0621_S4/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone'
+//  '<S33>'  : 'SimModel0621_S4/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
+//  '<S34>'  : 'SimModel0621_S4/Controler/PID Controller1/D Gain/Internal Parameters'
+//  '<S35>'  : 'SimModel0621_S4/Controler/PID Controller1/External Derivative/Error'
+//  '<S36>'  : 'SimModel0621_S4/Controler/PID Controller1/Filter/Disc. Forward Euler Filter'
+//  '<S37>'  : 'SimModel0621_S4/Controler/PID Controller1/Filter ICs/Internal IC - Filter'
+//  '<S38>'  : 'SimModel0621_S4/Controler/PID Controller1/I Gain/Internal Parameters'
+//  '<S39>'  : 'SimModel0621_S4/Controler/PID Controller1/Ideal P Gain/Passthrough'
+//  '<S40>'  : 'SimModel0621_S4/Controler/PID Controller1/Ideal P Gain Fdbk/Disabled'
+//  '<S41>'  : 'SimModel0621_S4/Controler/PID Controller1/Integrator/Discrete'
+//  '<S42>'  : 'SimModel0621_S4/Controler/PID Controller1/Integrator ICs/Internal IC'
+//  '<S43>'  : 'SimModel0621_S4/Controler/PID Controller1/N Copy/Disabled'
+//  '<S44>'  : 'SimModel0621_S4/Controler/PID Controller1/N Gain/Internal Parameters'
+//  '<S45>'  : 'SimModel0621_S4/Controler/PID Controller1/P Copy/Disabled'
+//  '<S46>'  : 'SimModel0621_S4/Controler/PID Controller1/Parallel P Gain/Internal Parameters'
+//  '<S47>'  : 'SimModel0621_S4/Controler/PID Controller1/Reset Signal/Disabled'
+//  '<S48>'  : 'SimModel0621_S4/Controler/PID Controller1/Saturation/Enabled'
+//  '<S49>'  : 'SimModel0621_S4/Controler/PID Controller1/Saturation Fdbk/Disabled'
+//  '<S50>'  : 'SimModel0621_S4/Controler/PID Controller1/Sum/Sum_PID'
+//  '<S51>'  : 'SimModel0621_S4/Controler/PID Controller1/Sum Fdbk/Disabled'
+//  '<S52>'  : 'SimModel0621_S4/Controler/PID Controller1/Tracking Mode/Disabled'
+//  '<S53>'  : 'SimModel0621_S4/Controler/PID Controller1/Tracking Mode Sum/Passthrough'
+//  '<S54>'  : 'SimModel0621_S4/Controler/PID Controller1/Tsamp - Integral/TsSignalSpecification'
+//  '<S55>'  : 'SimModel0621_S4/Controler/PID Controller1/Tsamp - Ngain/Passthrough'
+//  '<S56>'  : 'SimModel0621_S4/Controler/PID Controller1/postSat Signal/Forward_Path'
+//  '<S57>'  : 'SimModel0621_S4/Controler/PID Controller1/preInt Signal/Internal PreInt'
+//  '<S58>'  : 'SimModel0621_S4/Controler/PID Controller1/preSat Signal/Forward_Path'
+//  '<S59>'  : 'SimModel0621_S4/Controler/PID Controller3/Anti-windup'
+//  '<S60>'  : 'SimModel0621_S4/Controler/PID Controller3/D Gain'
+//  '<S61>'  : 'SimModel0621_S4/Controler/PID Controller3/External Derivative'
+//  '<S62>'  : 'SimModel0621_S4/Controler/PID Controller3/Filter'
+//  '<S63>'  : 'SimModel0621_S4/Controler/PID Controller3/Filter ICs'
+//  '<S64>'  : 'SimModel0621_S4/Controler/PID Controller3/I Gain'
+//  '<S65>'  : 'SimModel0621_S4/Controler/PID Controller3/Ideal P Gain'
+//  '<S66>'  : 'SimModel0621_S4/Controler/PID Controller3/Ideal P Gain Fdbk'
+//  '<S67>'  : 'SimModel0621_S4/Controler/PID Controller3/Integrator'
+//  '<S68>'  : 'SimModel0621_S4/Controler/PID Controller3/Integrator ICs'
+//  '<S69>'  : 'SimModel0621_S4/Controler/PID Controller3/N Copy'
+//  '<S70>'  : 'SimModel0621_S4/Controler/PID Controller3/N Gain'
+//  '<S71>'  : 'SimModel0621_S4/Controler/PID Controller3/P Copy'
+//  '<S72>'  : 'SimModel0621_S4/Controler/PID Controller3/Parallel P Gain'
+//  '<S73>'  : 'SimModel0621_S4/Controler/PID Controller3/Reset Signal'
+//  '<S74>'  : 'SimModel0621_S4/Controler/PID Controller3/Saturation'
+//  '<S75>'  : 'SimModel0621_S4/Controler/PID Controller3/Saturation Fdbk'
+//  '<S76>'  : 'SimModel0621_S4/Controler/PID Controller3/Sum'
+//  '<S77>'  : 'SimModel0621_S4/Controler/PID Controller3/Sum Fdbk'
+//  '<S78>'  : 'SimModel0621_S4/Controler/PID Controller3/Tracking Mode'
+//  '<S79>'  : 'SimModel0621_S4/Controler/PID Controller3/Tracking Mode Sum'
+//  '<S80>'  : 'SimModel0621_S4/Controler/PID Controller3/Tsamp - Integral'
+//  '<S81>'  : 'SimModel0621_S4/Controler/PID Controller3/Tsamp - Ngain'
+//  '<S82>'  : 'SimModel0621_S4/Controler/PID Controller3/postSat Signal'
+//  '<S83>'  : 'SimModel0621_S4/Controler/PID Controller3/preInt Signal'
+//  '<S84>'  : 'SimModel0621_S4/Controler/PID Controller3/preSat Signal'
+//  '<S85>'  : 'SimModel0621_S4/Controler/PID Controller3/Anti-windup/Disc. Clamping Parallel'
+//  '<S86>'  : 'SimModel0621_S4/Controler/PID Controller3/Anti-windup/Disc. Clamping Parallel/Dead Zone'
+//  '<S87>'  : 'SimModel0621_S4/Controler/PID Controller3/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
+//  '<S88>'  : 'SimModel0621_S4/Controler/PID Controller3/D Gain/Internal Parameters'
+//  '<S89>'  : 'SimModel0621_S4/Controler/PID Controller3/External Derivative/Error'
+//  '<S90>'  : 'SimModel0621_S4/Controler/PID Controller3/Filter/Disc. Forward Euler Filter'
+//  '<S91>'  : 'SimModel0621_S4/Controler/PID Controller3/Filter ICs/Internal IC - Filter'
+//  '<S92>'  : 'SimModel0621_S4/Controler/PID Controller3/I Gain/Internal Parameters'
+//  '<S93>'  : 'SimModel0621_S4/Controler/PID Controller3/Ideal P Gain/Passthrough'
+//  '<S94>'  : 'SimModel0621_S4/Controler/PID Controller3/Ideal P Gain Fdbk/Disabled'
+//  '<S95>'  : 'SimModel0621_S4/Controler/PID Controller3/Integrator/Discrete'
+//  '<S96>'  : 'SimModel0621_S4/Controler/PID Controller3/Integrator ICs/Internal IC'
+//  '<S97>'  : 'SimModel0621_S4/Controler/PID Controller3/N Copy/Disabled'
+//  '<S98>'  : 'SimModel0621_S4/Controler/PID Controller3/N Gain/Internal Parameters'
+//  '<S99>'  : 'SimModel0621_S4/Controler/PID Controller3/P Copy/Disabled'
+//  '<S100>' : 'SimModel0621_S4/Controler/PID Controller3/Parallel P Gain/Internal Parameters'
+//  '<S101>' : 'SimModel0621_S4/Controler/PID Controller3/Reset Signal/Disabled'
+//  '<S102>' : 'SimModel0621_S4/Controler/PID Controller3/Saturation/Enabled'
+//  '<S103>' : 'SimModel0621_S4/Controler/PID Controller3/Saturation Fdbk/Disabled'
+//  '<S104>' : 'SimModel0621_S4/Controler/PID Controller3/Sum/Sum_PID'
+//  '<S105>' : 'SimModel0621_S4/Controler/PID Controller3/Sum Fdbk/Disabled'
+//  '<S106>' : 'SimModel0621_S4/Controler/PID Controller3/Tracking Mode/Disabled'
+//  '<S107>' : 'SimModel0621_S4/Controler/PID Controller3/Tracking Mode Sum/Passthrough'
+//  '<S108>' : 'SimModel0621_S4/Controler/PID Controller3/Tsamp - Integral/TsSignalSpecification'
+//  '<S109>' : 'SimModel0621_S4/Controler/PID Controller3/Tsamp - Ngain/Passthrough'
+//  '<S110>' : 'SimModel0621_S4/Controler/PID Controller3/postSat Signal/Forward_Path'
+//  '<S111>' : 'SimModel0621_S4/Controler/PID Controller3/preInt Signal/Internal PreInt'
+//  '<S112>' : 'SimModel0621_S4/Controler/PID Controller3/preSat Signal/Forward_Path'
+
+#endif                                 // Controler_h_
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//

src/pAUV150/Controler/Controler_data.cpp

@@ -0,0 +1,58 @@
+//
+// 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.115
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Tue Aug 12 11:26:01 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:
+  //    '<S34>/Derivative Gain'
+  //    '<S38>/Integral Gain'
+  //    '<S44>/Filter Coefficient'
+  //    '<S46>/Proportional Gain'
+
+  {
+    10.0,
+    0.0,
+    0.0,
+    10.0
+  },
+
+  // Variable: pid_heading
+  //  Referenced by:
+  //    '<S88>/Derivative Gain'
+  //    '<S92>/Integral Gain'
+  //    '<S98>/Filter Coefficient'
+  //    '<S100>/Proportional Gain'
+
+  {
+    0.5,
+    0.0,
+    0.0,
+    10.0
+  }
+};
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//

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.115
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Tue Aug 12 11:26:01 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]
+//

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.115
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Tue Aug 12 11:26:01 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]
+//

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.115
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Tue Aug 12 11:26:01 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]
+//

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.115
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Tue Aug 12 11:26:01 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]
+//

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.115
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Tue Aug 12 11:26:01 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]
+//

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.115
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Tue Aug 12 11:26:01 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]
+//

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.115
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Tue Aug 12 11:26:01 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]
+//

src/pAUV150/sqlit3/SQLite.cpp

@@ -0,0 +1,250 @@
+#include "SQLite.h"
+#include "../AUV150.h"
+#include <string>
+#include <iostream>
+#include <sstream>
+
+SQLite::SQLite(const std::string& dbPath) {
+    try {
+        int rc = sqlite3_open(dbPath.c_str(), &m_db);
+        if (rc != SQLITE_OK) {
+            std::cerr << "Can't open database: " << sqlite3_errmsg(m_db) << std::endl;
+            m_db = nullptr;
+        } else {
+            std::cout << "Opened database successfully: " << dbPath << std::endl;
+        }
+        m_dbPath = dbPath;
+    } catch (const std::exception& e) {
+        std::cerr << "SQLite constructor exception: " << e.what() << std::endl;
+        m_db = nullptr;
+    }
+}
+
+SQLite::~SQLite()
+{
+    if (m_db) sqlite3_close(m_db);
+}
+
+bool SQLite::execSQL(const std::string& sql) {
+    if (!m_db) {
+        std::cerr << "Database not opened, cannot execute SQL" << std::endl;
+        return false;
+    }
+    
+    char* errMsg = nullptr;
+    int rc = sqlite3_exec(m_db, sql.c_str(), nullptr, nullptr, &errMsg);
+    if (rc != SQLITE_OK) {
+        std::cerr << "SQL error (" << rc << "): " << (errMsg ? errMsg : "unknown error") << std::endl;
+        std::cerr << "Failed SQL: " << sql << std::endl;
+        if (errMsg) sqlite3_free(errMsg);
+        return false;
+    }
+    return true;
+}
+
+bool SQLite::createTable() {
+    if (!m_db) {
+        std::cerr << "Database not opened, cannot create tables" << std::endl;
+        return false;
+    }
+
+    std::cout << "Creating database tables in: " << m_dbPath << std::endl;
+
+    std::string cmdTableSQL = R"(
+        CREATE TABLE IF NOT EXISTS CommandFrame (
+            id INTEGER PRIMARY KEY AUTOINCREMENT,
+            timestamp DATETIME DEFAULT CURRENT_TIMESTAMP,
+            frameHeader INTEGER,
+            counter INTEGER,
+            dataLength INTEGER,
+            navMode INTEGER,
+            mainThruster INTEGER,
+            heading INTEGER,
+            depth INTEGER,
+            rudderUp INTEGER,
+            rudderDown INTEGER,
+            rudderLeft INTEGER,
+            rudderRight INTEGER,
+            led INTEGER,
+            dvlSwitch INTEGER,
+            payloadCtrl INTEGER,
+            crc INTEGER,
+            frameTail INTEGER
+        );
+    )";
+
+    std::string feedbackTableSQL = R"(
+        CREATE TABLE IF NOT EXISTS FeedbackFrame (
+            id INTEGER PRIMARY KEY AUTOINCREMENT,
+            timestamp DATETIME DEFAULT CURRENT_TIMESTAMP,
+            frameHeader INTEGER,
+            counter INTEGER,
+            dataLength INTEGER,
+            navModeFb INTEGER,
+            altimeterHeight REAL,
+            depthSensor REAL,
+            trueHeading REAL,
+            pitch REAL,
+            roll REAL,
+            velocityEast REAL,
+            velocityNorth REAL,
+            velocityDown REAL,
+            insLongitude REAL,
+            insLatitude REAL,
+            insAltitude REAL,
+            dvlVelX REAL,
+            dvlVelY REAL,
+            dvlVelZ REAL,
+            thrusterRPM REAL,
+            ledSwitch INTEGER,
+            batteryVoltage REAL,
+            batteryLevel INTEGER,
+            batteryTemp REAL,
+            leakStatus INTEGER,
+            powerModule INTEGER,
+            backupPower INTEGER,
+            thrusterStatus INTEGER,
+            reserved INTEGER,
+            payloadStatus INTEGER,
+            dvlStatus INTEGER,
+            crc INTEGER,
+            frameTail INTEGER
+        );
+    )";
+
+    std::string statusTableSQL = R"(
+        CREATE TABLE IF NOT EXISTS AUVStatus (
+            id INTEGER PRIMARY KEY AUTOINCREMENT,
+            timestamp DATETIME DEFAULT CURRENT_TIMESTAMP,
+            navModeFb INTEGER,
+            altimeterHeight REAL,
+            depthSensor REAL,
+            trueHeading REAL,
+            pitch REAL,
+            roll REAL,
+            velocityEast REAL,
+            velocityNorth REAL,
+            velocityDown REAL,
+            insLongitude REAL,
+            insLatitude REAL,
+            insAltitude REAL,
+            dvlVelX REAL,
+            dvlVelY REAL,
+            dvlVelZ REAL,
+            thrusterRPM REAL,
+            ledSwitch INTEGER,
+            batteryVoltage REAL,
+            batteryLevel INTEGER,
+            batteryTemp REAL,
+            leakStatus INTEGER,
+            powerModule INTEGER,
+            backupPower INTEGER,
+            thrusterStatus INTEGER,
+            reserved INTEGER,
+            payloadStatus INTEGER,
+            dvlStatus INTEGER,
+            x REAL,
+            y REAL,
+            z REAL,
+            heading_error REAL,
+            pitch_error REAL,
+            depth_error REAL,
+            desired_rudder REAL,
+            desired_elevator REAL,
+            desired_thrust REAL
+        );
+    )";
+
+    return execSQL(cmdTableSQL) && 
+           execSQL(feedbackTableSQL) && 
+           execSQL(statusTableSQL);
+}
+
+bool SQLite::insertData(CommandFrame_150AUV data) {
+    if (!m_db) {
+        std::cerr << "Database not opened, cannot insert CommandFrame data" << std::endl;
+        return false;
+    }
+
+    std::cout << "Inserting CommandFrame data into database" << std::endl;
+    std::stringstream ss;
+    ss << "INSERT INTO CommandFrame ("
+       << "frameHeader, counter, dataLength, navMode, mainThruster, "
+       << "heading, depth, rudderUp, rudderDown, rudderLeft, rudderRight, "
+       << "led, dvlSwitch, payloadCtrl, crc, frameTail) VALUES ("
+       << data.frameHeader << ", " << data.counter << ", " << (int)data.dataLength << ", "
+       << (int)data.navMode << ", " << (int)data.mainThruster << ", "
+       << data.heading << ", " << data.depth << ", " << (int)data.rudderUp << ", "
+       << (int)data.rudderDown << ", " << (int)data.rudderLeft << ", " << (int)data.rudderRight << ", "
+       << (int)data.led << ", " << (int)data.dvlSwitch << ", " << (int)data.payloadCtrl << ", "
+       << (int)data.crc << ", " << data.frameTail << ");";
+
+    return execSQL(ss.str());
+}
+
+bool SQLite::insertData(FeedbackFrame_150AUV data) {
+    if (!m_db) {
+        std::cerr << "Database not opened, cannot insert FeedbackFrame data" << std::endl;
+        return false;
+    }
+
+    std::cout << "Inserting FeedbackFrame data into database" << std::endl;
+    std::stringstream ss;
+    ss << "INSERT INTO FeedbackFrame ("
+       << "frameHeader, counter, dataLength, navModeFb, altimeterHeight, "
+       << "depthSensor, trueHeading, pitch, roll, velocityEast, velocityNorth, "
+       << "velocityDown, insLongitude, insLatitude, insAltitude, dvlVelX, "
+       << "dvlVelY, dvlVelZ, thrusterRPM, ledSwitch, batteryVoltage, "
+       << "batteryLevel, batteryTemp, leakStatus, powerModule, backupPower, "
+       << "thrusterStatus, reserved, payloadStatus, dvlStatus, crc, frameTail) VALUES ("
+       << data.frameHeader << ", " << data.counter << ", " << (int)data.dataLength << ", "
+       << (int)data.navModeFb << ", " << data.altimeterHeight/100.0 << ", "
+       << data.depthSensor/100.0 << ", " << data.trueHeading/100.0 << ", "
+       << (short)data.pitch/100.0 << ", " << (short)data.roll/100.0 << ", "
+       << (short)data.velocityEast/100.0 << ", " << (short)data.velocityNorth/100.0 << ", "
+       << (short)data.velocityDown/100.0 << ", " << data.insLongitude/1e7 << ", "
+       << data.insLatitude/1e7 << ", " << (short)data.insAltitude/100.0 << ", "
+       << (short)data.dvlVelX/100.0 << ", " << (short)data.dvlVelY/100.0 << ", "
+       << (short)data.dvlVelZ/100.0 << ", " << (short)data.thrusterRPM << ", "
+       << (int)data.ledSwitch << ", " << data.batteryVoltage/1000.0 << ", "
+       << (int)data.batteryLevel << ", " << data.batteryTemp/10.0 << ", "
+       << data.leakStatus << ", " << (int)data.powerModule << ", "
+       << (int)data.backupPower << ", " << (int)data.thrusterStatus << ", "
+       << (int)data.reserved << ", " << (int)data.payloadStatus << ", "
+       << (int)data.dvlStatus << ", " << (int)data.crc << ", " << data.frameTail << ");";
+
+    return execSQL(ss.str());
+}
+
+bool SQLite::insertData(AUV150_Status data) {
+    if (!m_db) {
+        std::cerr << "Database not opened, cannot insert AUVStatus data" << std::endl;
+        return false;
+    }
+
+    std::cout << "Inserting AUVStatus data into database" << std::endl;
+    std::stringstream ss;
+    ss << "INSERT INTO AUVStatus ("
+       << "navModeFb, altimeterHeight, depthSensor, trueHeading, pitch, roll, "
+       << "velocityEast, velocityNorth, velocityDown, insLongitude, insLatitude, "
+       << "insAltitude, dvlVelX, dvlVelY, dvlVelZ, thrusterRPM, ledSwitch, "
+       << "batteryVoltage, batteryLevel, batteryTemp, leakStatus, powerModule, "
+       << "backupPower, thrusterStatus, reserved, payloadStatus, dvlStatus, "
+       << "x, y, z, heading_error, pitch_error, depth_error, desired_rudder, "
+       << "desired_elevator, desired_thrust) VALUES ("
+       << (int)data.navModeFb << ", " << data.altimeterHeight << ", " << data.depthSensor << ", "
+       << data.trueHeading << ", " << data.pitch << ", " << data.roll << ", "
+       << data.velocityEast << ", " << data.velocityNorth << ", " << data.velocityDown << ", "
+       << data.insLongitude << ", " << data.insLatitude << ", " << data.insAltitude << ", "
+       << data.dvlVelX << ", " << data.dvlVelY << ", " << data.dvlVelZ << ", "
+       << data.thrusterRPM << ", " << (int)data.ledSwitch << ", " << data.batteryVoltage << ", "
+       << (int)data.batteryLevel << ", " << data.batteryTemp << ", " << data.leakStatus << ", "
+       << (int)data.powerModule << ", " << (int)data.backupPower << ", "
+       << (int)data.thrusterStatus << ", " << (int)data.reserved << ", "
+       << (int)data.payloadStatus << ", " << (int)data.dvlStatus << ", "
+       << data.x << ", " << data.y << ", " << data.z << ", " << data.heading_error << ", "
+       << data.pitch_error << ", " << data.depth_error << ", " << data.desired_rudder << ", "
+       << data.desired_elevator << ", " << data.desired_thrust << ");";
+
+    return execSQL(ss.str());
+}

src/pAUV150/sqlit3/SQLite.h

@@ -0,0 +1,31 @@
+#ifndef SQLITE_H
+#define SQLITE_H
+#include "sqlite3.h"
+#include <string>
+
+// Forward declarations
+struct CommandFrame_150AUV;
+struct FeedbackFrame_150AUV;
+struct AUV150_Status;
+
+class SQLite
+{
+private:
+    sqlite3* m_db;
+    bool execSQL(const std::string& sql);
+public:
+    std::string m_dbPath;
+    SQLite(const std::string& dbName);
+    ~SQLite();
+
+    //创建表
+    bool createTable();
+
+    // 插入数据函数
+    bool insertData(CommandFrame_150AUV data);
+    bool insertData(FeedbackFrame_150AUV data);
+    bool insertData(AUV150_Status data);
+};
+
+
+#endif

src/pAUV150/sqlit3/sqlite3ext.h

@@ -0,0 +1,719 @@
+/*
+** 2006 June 7
+**
+** The author disclaims copyright to this source code.  In place of
+** a legal notice, here is a blessing:
+**
+**    May you do good and not evil.
+**    May you find forgiveness for yourself and forgive others.
+**    May you share freely, never taking more than you give.
+**
+*************************************************************************
+** This header file defines the SQLite interface for use by
+** shared libraries that want to be imported as extensions into
+** an SQLite instance.  Shared libraries that intend to be loaded
+** as extensions by SQLite should #include this file instead of 
+** sqlite3.h.
+*/
+#ifndef SQLITE3EXT_H
+#define SQLITE3EXT_H
+#include "sqlite3.h"
+
+/*
+** The following structure holds pointers to all of the SQLite API
+** routines.
+**
+** WARNING:  In order to maintain backwards compatibility, add new
+** interfaces to the end of this structure only.  If you insert new
+** interfaces in the middle of this structure, then older different
+** versions of SQLite will not be able to load each other's shared
+** libraries!
+*/
+struct sqlite3_api_routines {
+  void * (*aggregate_context)(sqlite3_context*,int nBytes);
+  int  (*aggregate_count)(sqlite3_context*);
+  int  (*bind_blob)(sqlite3_stmt*,int,const void*,int n,void(*)(void*));
+  int  (*bind_double)(sqlite3_stmt*,int,double);
+  int  (*bind_int)(sqlite3_stmt*,int,int);
+  int  (*bind_int64)(sqlite3_stmt*,int,sqlite_int64);
+  int  (*bind_null)(sqlite3_stmt*,int);
+  int  (*bind_parameter_count)(sqlite3_stmt*);
+  int  (*bind_parameter_index)(sqlite3_stmt*,const char*zName);
+  const char * (*bind_parameter_name)(sqlite3_stmt*,int);
+  int  (*bind_text)(sqlite3_stmt*,int,const char*,int n,void(*)(void*));
+  int  (*bind_text16)(sqlite3_stmt*,int,const void*,int,void(*)(void*));
+  int  (*bind_value)(sqlite3_stmt*,int,const sqlite3_value*);
+  int  (*busy_handler)(sqlite3*,int(*)(void*,int),void*);
+  int  (*busy_timeout)(sqlite3*,int ms);
+  int  (*changes)(sqlite3*);
+  int  (*close)(sqlite3*);
+  int  (*collation_needed)(sqlite3*,void*,void(*)(void*,sqlite3*,
+                           int eTextRep,const char*));
+  int  (*collation_needed16)(sqlite3*,void*,void(*)(void*,sqlite3*,
+                             int eTextRep,const void*));
+  const void * (*column_blob)(sqlite3_stmt*,int iCol);
+  int  (*column_bytes)(sqlite3_stmt*,int iCol);
+  int  (*column_bytes16)(sqlite3_stmt*,int iCol);
+  int  (*column_count)(sqlite3_stmt*pStmt);
+  const char * (*column_database_name)(sqlite3_stmt*,int);
+  const void * (*column_database_name16)(sqlite3_stmt*,int);
+  const char * (*column_decltype)(sqlite3_stmt*,int i);
+  const void * (*column_decltype16)(sqlite3_stmt*,int);
+  double  (*column_double)(sqlite3_stmt*,int iCol);
+  int  (*column_int)(sqlite3_stmt*,int iCol);
+  sqlite_int64  (*column_int64)(sqlite3_stmt*,int iCol);
+  const char * (*column_name)(sqlite3_stmt*,int);
+  const void * (*column_name16)(sqlite3_stmt*,int);
+  const char * (*column_origin_name)(sqlite3_stmt*,int);
+  const void * (*column_origin_name16)(sqlite3_stmt*,int);
+  const char * (*column_table_name)(sqlite3_stmt*,int);
+  const void * (*column_table_name16)(sqlite3_stmt*,int);
+  const unsigned char * (*column_text)(sqlite3_stmt*,int iCol);
+  const void * (*column_text16)(sqlite3_stmt*,int iCol);
+  int  (*column_type)(sqlite3_stmt*,int iCol);
+  sqlite3_value* (*column_value)(sqlite3_stmt*,int iCol);
+  void * (*commit_hook)(sqlite3*,int(*)(void*),void*);
+  int  (*complete)(const char*sql);
+  int  (*complete16)(const void*sql);
+  int  (*create_collation)(sqlite3*,const char*,int,void*,
+                           int(*)(void*,int,const void*,int,const void*));
+  int  (*create_collation16)(sqlite3*,const void*,int,void*,
+                             int(*)(void*,int,const void*,int,const void*));
+  int  (*create_function)(sqlite3*,const char*,int,int,void*,
+                          void (*xFunc)(sqlite3_context*,int,sqlite3_value**),
+                          void (*xStep)(sqlite3_context*,int,sqlite3_value**),
+                          void (*xFinal)(sqlite3_context*));
+  int  (*create_function16)(sqlite3*,const void*,int,int,void*,
+                            void (*xFunc)(sqlite3_context*,int,sqlite3_value**),
+                            void (*xStep)(sqlite3_context*,int,sqlite3_value**),
+                            void (*xFinal)(sqlite3_context*));
+  int (*create_module)(sqlite3*,const char*,const sqlite3_module*,void*);
+  int  (*data_count)(sqlite3_stmt*pStmt);
+  sqlite3 * (*db_handle)(sqlite3_stmt*);
+  int (*declare_vtab)(sqlite3*,const char*);
+  int  (*enable_shared_cache)(int);
+  int  (*errcode)(sqlite3*db);
+  const char * (*errmsg)(sqlite3*);
+  const void * (*errmsg16)(sqlite3*);
+  int  (*exec)(sqlite3*,const char*,sqlite3_callback,void*,char**);
+  int  (*expired)(sqlite3_stmt*);
+  int  (*finalize)(sqlite3_stmt*pStmt);
+  void  (*free)(void*);
+  void  (*free_table)(char**result);
+  int  (*get_autocommit)(sqlite3*);
+  void * (*get_auxdata)(sqlite3_context*,int);
+  int  (*get_table)(sqlite3*,const char*,char***,int*,int*,char**);
+  int  (*global_recover)(void);
+  void  (*interruptx)(sqlite3*);
+  sqlite_int64  (*last_insert_rowid)(sqlite3*);
+  const char * (*libversion)(void);
+  int  (*libversion_number)(void);
+  void *(*malloc)(int);
+  char * (*mprintf)(const char*,...);
+  int  (*open)(const char*,sqlite3**);
+  int  (*open16)(const void*,sqlite3**);
+  int  (*prepare)(sqlite3*,const char*,int,sqlite3_stmt**,const char**);
+  int  (*prepare16)(sqlite3*,const void*,int,sqlite3_stmt**,const void**);
+  void * (*profile)(sqlite3*,void(*)(void*,const char*,sqlite_uint64),void*);
+  void  (*progress_handler)(sqlite3*,int,int(*)(void*),void*);
+  void *(*realloc)(void*,int);
+  int  (*reset)(sqlite3_stmt*pStmt);
+  void  (*result_blob)(sqlite3_context*,const void*,int,void(*)(void*));
+  void  (*result_double)(sqlite3_context*,double);
+  void  (*result_error)(sqlite3_context*,const char*,int);
+  void  (*result_error16)(sqlite3_context*,const void*,int);
+  void  (*result_int)(sqlite3_context*,int);
+  void  (*result_int64)(sqlite3_context*,sqlite_int64);
+  void  (*result_null)(sqlite3_context*);
+  void  (*result_text)(sqlite3_context*,const char*,int,void(*)(void*));
+  void  (*result_text16)(sqlite3_context*,const void*,int,void(*)(void*));
+  void  (*result_text16be)(sqlite3_context*,const void*,int,void(*)(void*));
+  void  (*result_text16le)(sqlite3_context*,const void*,int,void(*)(void*));
+  void  (*result_value)(sqlite3_context*,sqlite3_value*);
+  void * (*rollback_hook)(sqlite3*,void(*)(void*),void*);
+  int  (*set_authorizer)(sqlite3*,int(*)(void*,int,const char*,const char*,
+                         const char*,const char*),void*);
+  void  (*set_auxdata)(sqlite3_context*,int,void*,void (*)(void*));
+  char * (*xsnprintf)(int,char*,const char*,...);
+  int  (*step)(sqlite3_stmt*);
+  int  (*table_column_metadata)(sqlite3*,const char*,const char*,const char*,
+                                char const**,char const**,int*,int*,int*);
+  void  (*thread_cleanup)(void);
+  int  (*total_changes)(sqlite3*);
+  void * (*trace)(sqlite3*,void(*xTrace)(void*,const char*),void*);
+  int  (*transfer_bindings)(sqlite3_stmt*,sqlite3_stmt*);
+  void * (*update_hook)(sqlite3*,void(*)(void*,int ,char const*,char const*,
+                                         sqlite_int64),void*);
+  void * (*user_data)(sqlite3_context*);
+  const void * (*value_blob)(sqlite3_value*);
+  int  (*value_bytes)(sqlite3_value*);
+  int  (*value_bytes16)(sqlite3_value*);
+  double  (*value_double)(sqlite3_value*);
+  int  (*value_int)(sqlite3_value*);
+  sqlite_int64  (*value_int64)(sqlite3_value*);
+  int  (*value_numeric_type)(sqlite3_value*);
+  const unsigned char * (*value_text)(sqlite3_value*);
+  const void * (*value_text16)(sqlite3_value*);
+  const void * (*value_text16be)(sqlite3_value*);
+  const void * (*value_text16le)(sqlite3_value*);
+  int  (*value_type)(sqlite3_value*);
+  char *(*vmprintf)(const char*,va_list);
+  /* Added ??? */
+  int (*overload_function)(sqlite3*, const char *zFuncName, int nArg);
+  /* Added by 3.3.13 */
+  int (*prepare_v2)(sqlite3*,const char*,int,sqlite3_stmt**,const char**);
+  int (*prepare16_v2)(sqlite3*,const void*,int,sqlite3_stmt**,const void**);
+  int (*clear_bindings)(sqlite3_stmt*);
+  /* Added by 3.4.1 */
+  int (*create_module_v2)(sqlite3*,const char*,const sqlite3_module*,void*,
+                          void (*xDestroy)(void *));
+  /* Added by 3.5.0 */
+  int (*bind_zeroblob)(sqlite3_stmt*,int,int);
+  int (*blob_bytes)(sqlite3_blob*);
+  int (*blob_close)(sqlite3_blob*);
+  int (*blob_open)(sqlite3*,const char*,const char*,const char*,sqlite3_int64,
+                   int,sqlite3_blob**);
+  int (*blob_read)(sqlite3_blob*,void*,int,int);
+  int (*blob_write)(sqlite3_blob*,const void*,int,int);
+  int (*create_collation_v2)(sqlite3*,const char*,int,void*,
+                             int(*)(void*,int,const void*,int,const void*),
+                             void(*)(void*));
+  int (*file_control)(sqlite3*,const char*,int,void*);
+  sqlite3_int64 (*memory_highwater)(int);
+  sqlite3_int64 (*memory_used)(void);
+  sqlite3_mutex *(*mutex_alloc)(int);
+  void (*mutex_enter)(sqlite3_mutex*);
+  void (*mutex_free)(sqlite3_mutex*);
+  void (*mutex_leave)(sqlite3_mutex*);
+  int (*mutex_try)(sqlite3_mutex*);
+  int (*open_v2)(const char*,sqlite3**,int,const char*);
+  int (*release_memory)(int);
+  void (*result_error_nomem)(sqlite3_context*);
+  void (*result_error_toobig)(sqlite3_context*);
+  int (*sleep)(int);
+  void (*soft_heap_limit)(int);
+  sqlite3_vfs *(*vfs_find)(const char*);
+  int (*vfs_register)(sqlite3_vfs*,int);
+  int (*vfs_unregister)(sqlite3_vfs*);
+  int (*xthreadsafe)(void);
+  void (*result_zeroblob)(sqlite3_context*,int);
+  void (*result_error_code)(sqlite3_context*,int);
+  int (*test_control)(int, ...);
+  void (*randomness)(int,void*);
+  sqlite3 *(*context_db_handle)(sqlite3_context*);
+  int (*extended_result_codes)(sqlite3*,int);
+  int (*limit)(sqlite3*,int,int);
+  sqlite3_stmt *(*next_stmt)(sqlite3*,sqlite3_stmt*);
+  const char *(*sql)(sqlite3_stmt*);
+  int (*status)(int,int*,int*,int);
+  int (*backup_finish)(sqlite3_backup*);
+  sqlite3_backup *(*backup_init)(sqlite3*,const char*,sqlite3*,const char*);
+  int (*backup_pagecount)(sqlite3_backup*);
+  int (*backup_remaining)(sqlite3_backup*);
+  int (*backup_step)(sqlite3_backup*,int);
+  const char *(*compileoption_get)(int);
+  int (*compileoption_used)(const char*);
+  int (*create_function_v2)(sqlite3*,const char*,int,int,void*,
+                            void (*xFunc)(sqlite3_context*,int,sqlite3_value**),
+                            void (*xStep)(sqlite3_context*,int,sqlite3_value**),
+                            void (*xFinal)(sqlite3_context*),
+                            void(*xDestroy)(void*));
+  int (*db_config)(sqlite3*,int,...);
+  sqlite3_mutex *(*db_mutex)(sqlite3*);
+  int (*db_status)(sqlite3*,int,int*,int*,int);
+  int (*extended_errcode)(sqlite3*);
+  void (*log)(int,const char*,...);
+  sqlite3_int64 (*soft_heap_limit64)(sqlite3_int64);
+  const char *(*sourceid)(void);
+  int (*stmt_status)(sqlite3_stmt*,int,int);
+  int (*strnicmp)(const char*,const char*,int);
+  int (*unlock_notify)(sqlite3*,void(*)(void**,int),void*);
+  int (*wal_autocheckpoint)(sqlite3*,int);
+  int (*wal_checkpoint)(sqlite3*,const char*);
+  void *(*wal_hook)(sqlite3*,int(*)(void*,sqlite3*,const char*,int),void*);
+  int (*blob_reopen)(sqlite3_blob*,sqlite3_int64);
+  int (*vtab_config)(sqlite3*,int op,...);
+  int (*vtab_on_conflict)(sqlite3*);
+  /* Version 3.7.16 and later */
+  int (*close_v2)(sqlite3*);
+  const char *(*db_filename)(sqlite3*,const char*);
+  int (*db_readonly)(sqlite3*,const char*);
+  int (*db_release_memory)(sqlite3*);
+  const char *(*errstr)(int);
+  int (*stmt_busy)(sqlite3_stmt*);
+  int (*stmt_readonly)(sqlite3_stmt*);
+  int (*stricmp)(const char*,const char*);
+  int (*uri_boolean)(const char*,const char*,int);
+  sqlite3_int64 (*uri_int64)(const char*,const char*,sqlite3_int64);
+  const char *(*uri_parameter)(const char*,const char*);
+  char *(*xvsnprintf)(int,char*,const char*,va_list);
+  int (*wal_checkpoint_v2)(sqlite3*,const char*,int,int*,int*);
+  /* Version 3.8.7 and later */
+  int (*auto_extension)(void(*)(void));
+  int (*bind_blob64)(sqlite3_stmt*,int,const void*,sqlite3_uint64,
+                     void(*)(void*));
+  int (*bind_text64)(sqlite3_stmt*,int,const char*,sqlite3_uint64,
+                      void(*)(void*),unsigned char);
+  int (*cancel_auto_extension)(void(*)(void));
+  int (*load_extension)(sqlite3*,const char*,const char*,char**);
+  void *(*malloc64)(sqlite3_uint64);
+  sqlite3_uint64 (*msize)(void*);
+  void *(*realloc64)(void*,sqlite3_uint64);
+  void (*reset_auto_extension)(void);
+  void (*result_blob64)(sqlite3_context*,const void*,sqlite3_uint64,
+                        void(*)(void*));
+  void (*result_text64)(sqlite3_context*,const char*,sqlite3_uint64,
+                         void(*)(void*), unsigned char);
+  int (*strglob)(const char*,const char*);
+  /* Version 3.8.11 and later */
+  sqlite3_value *(*value_dup)(const sqlite3_value*);
+  void (*value_free)(sqlite3_value*);
+  int (*result_zeroblob64)(sqlite3_context*,sqlite3_uint64);
+  int (*bind_zeroblob64)(sqlite3_stmt*, int, sqlite3_uint64);
+  /* Version 3.9.0 and later */
+  unsigned int (*value_subtype)(sqlite3_value*);
+  void (*result_subtype)(sqlite3_context*,unsigned int);
+  /* Version 3.10.0 and later */
+  int (*status64)(int,sqlite3_int64*,sqlite3_int64*,int);
+  int (*strlike)(const char*,const char*,unsigned int);
+  int (*db_cacheflush)(sqlite3*);
+  /* Version 3.12.0 and later */
+  int (*system_errno)(sqlite3*);
+  /* Version 3.14.0 and later */
+  int (*trace_v2)(sqlite3*,unsigned,int(*)(unsigned,void*,void*,void*),void*);
+  char *(*expanded_sql)(sqlite3_stmt*);
+  /* Version 3.18.0 and later */
+  void (*set_last_insert_rowid)(sqlite3*,sqlite3_int64);
+  /* Version 3.20.0 and later */
+  int (*prepare_v3)(sqlite3*,const char*,int,unsigned int,
+                    sqlite3_stmt**,const char**);
+  int (*prepare16_v3)(sqlite3*,const void*,int,unsigned int,
+                      sqlite3_stmt**,const void**);
+  int (*bind_pointer)(sqlite3_stmt*,int,void*,const char*,void(*)(void*));
+  void (*result_pointer)(sqlite3_context*,void*,const char*,void(*)(void*));
+  void *(*value_pointer)(sqlite3_value*,const char*);
+  int (*vtab_nochange)(sqlite3_context*);
+  int (*value_nochange)(sqlite3_value*);
+  const char *(*vtab_collation)(sqlite3_index_info*,int);
+  /* Version 3.24.0 and later */
+  int (*keyword_count)(void);
+  int (*keyword_name)(int,const char**,int*);
+  int (*keyword_check)(const char*,int);
+  sqlite3_str *(*str_new)(sqlite3*);
+  char *(*str_finish)(sqlite3_str*);
+  void (*str_appendf)(sqlite3_str*, const char *zFormat, ...);
+  void (*str_vappendf)(sqlite3_str*, const char *zFormat, va_list);
+  void (*str_append)(sqlite3_str*, const char *zIn, int N);
+  void (*str_appendall)(sqlite3_str*, const char *zIn);
+  void (*str_appendchar)(sqlite3_str*, int N, char C);
+  void (*str_reset)(sqlite3_str*);
+  int (*str_errcode)(sqlite3_str*);
+  int (*str_length)(sqlite3_str*);
+  char *(*str_value)(sqlite3_str*);
+  /* Version 3.25.0 and later */
+  int (*create_window_function)(sqlite3*,const char*,int,int,void*,
+                            void (*xStep)(sqlite3_context*,int,sqlite3_value**),
+                            void (*xFinal)(sqlite3_context*),
+                            void (*xValue)(sqlite3_context*),
+                            void (*xInv)(sqlite3_context*,int,sqlite3_value**),
+                            void(*xDestroy)(void*));
+  /* Version 3.26.0 and later */
+  const char *(*normalized_sql)(sqlite3_stmt*);
+  /* Version 3.28.0 and later */
+  int (*stmt_isexplain)(sqlite3_stmt*);
+  int (*value_frombind)(sqlite3_value*);
+  /* Version 3.30.0 and later */
+  int (*drop_modules)(sqlite3*,const char**);
+  /* Version 3.31.0 and later */
+  sqlite3_int64 (*hard_heap_limit64)(sqlite3_int64);
+  const char *(*uri_key)(const char*,int);
+  const char *(*filename_database)(const char*);
+  const char *(*filename_journal)(const char*);
+  const char *(*filename_wal)(const char*);
+  /* Version 3.32.0 and later */
+  const char *(*create_filename)(const char*,const char*,const char*,
+                           int,const char**);
+  void (*free_filename)(const char*);
+  sqlite3_file *(*database_file_object)(const char*);
+  /* Version 3.34.0 and later */
+  int (*txn_state)(sqlite3*,const char*);
+  /* Version 3.36.1 and later */
+  sqlite3_int64 (*changes64)(sqlite3*);
+  sqlite3_int64 (*total_changes64)(sqlite3*);
+  /* Version 3.37.0 and later */
+  int (*autovacuum_pages)(sqlite3*,
+     unsigned int(*)(void*,const char*,unsigned int,unsigned int,unsigned int),
+     void*, void(*)(void*));
+  /* Version 3.38.0 and later */
+  int (*error_offset)(sqlite3*);
+  int (*vtab_rhs_value)(sqlite3_index_info*,int,sqlite3_value**);
+  int (*vtab_distinct)(sqlite3_index_info*);
+  int (*vtab_in)(sqlite3_index_info*,int,int);
+  int (*vtab_in_first)(sqlite3_value*,sqlite3_value**);
+  int (*vtab_in_next)(sqlite3_value*,sqlite3_value**);
+  /* Version 3.39.0 and later */
+  int (*deserialize)(sqlite3*,const char*,unsigned char*,
+                     sqlite3_int64,sqlite3_int64,unsigned);
+  unsigned char *(*serialize)(sqlite3*,const char *,sqlite3_int64*,
+                              unsigned int);
+  const char *(*db_name)(sqlite3*,int);
+  /* Version 3.40.0 and later */
+  int (*value_encoding)(sqlite3_value*);
+  /* Version 3.41.0 and later */
+  int (*is_interrupted)(sqlite3*);
+  /* Version 3.43.0 and later */
+  int (*stmt_explain)(sqlite3_stmt*,int);
+  /* Version 3.44.0 and later */
+  void *(*get_clientdata)(sqlite3*,const char*);
+  int (*set_clientdata)(sqlite3*, const char*, void*, void(*)(void*));
+};
+
+/*
+** This is the function signature used for all extension entry points.  It
+** is also defined in the file "loadext.c".
+*/
+typedef int (*sqlite3_loadext_entry)(
+  sqlite3 *db,                       /* Handle to the database. */
+  char **pzErrMsg,                   /* Used to set error string on failure. */
+  const sqlite3_api_routines *pThunk /* Extension API function pointers. */
+);
+
+/*
+** The following macros redefine the API routines so that they are
+** redirected through the global sqlite3_api structure.
+**
+** This header file is also used by the loadext.c source file
+** (part of the main SQLite library - not an extension) so that
+** it can get access to the sqlite3_api_routines structure
+** definition.  But the main library does not want to redefine
+** the API.  So the redefinition macros are only valid if the
+** SQLITE_CORE macros is undefined.
+*/
+#if !defined(SQLITE_CORE) && !defined(SQLITE_OMIT_LOAD_EXTENSION)
+#define sqlite3_aggregate_context      sqlite3_api->aggregate_context
+#ifndef SQLITE_OMIT_DEPRECATED
+#define sqlite3_aggregate_count        sqlite3_api->aggregate_count
+#endif
+#define sqlite3_bind_blob              sqlite3_api->bind_blob
+#define sqlite3_bind_double            sqlite3_api->bind_double
+#define sqlite3_bind_int               sqlite3_api->bind_int
+#define sqlite3_bind_int64             sqlite3_api->bind_int64
+#define sqlite3_bind_null              sqlite3_api->bind_null
+#define sqlite3_bind_parameter_count   sqlite3_api->bind_parameter_count
+#define sqlite3_bind_parameter_index   sqlite3_api->bind_parameter_index
+#define sqlite3_bind_parameter_name    sqlite3_api->bind_parameter_name
+#define sqlite3_bind_text              sqlite3_api->bind_text
+#define sqlite3_bind_text16            sqlite3_api->bind_text16
+#define sqlite3_bind_value             sqlite3_api->bind_value
+#define sqlite3_busy_handler           sqlite3_api->busy_handler
+#define sqlite3_busy_timeout           sqlite3_api->busy_timeout
+#define sqlite3_changes                sqlite3_api->changes
+#define sqlite3_close                  sqlite3_api->close
+#define sqlite3_collation_needed       sqlite3_api->collation_needed
+#define sqlite3_collation_needed16     sqlite3_api->collation_needed16
+#define sqlite3_column_blob            sqlite3_api->column_blob
+#define sqlite3_column_bytes           sqlite3_api->column_bytes
+#define sqlite3_column_bytes16         sqlite3_api->column_bytes16
+#define sqlite3_column_count           sqlite3_api->column_count
+#define sqlite3_column_database_name   sqlite3_api->column_database_name
+#define sqlite3_column_database_name16 sqlite3_api->column_database_name16
+#define sqlite3_column_decltype        sqlite3_api->column_decltype
+#define sqlite3_column_decltype16      sqlite3_api->column_decltype16
+#define sqlite3_column_double          sqlite3_api->column_double
+#define sqlite3_column_int             sqlite3_api->column_int
+#define sqlite3_column_int64           sqlite3_api->column_int64
+#define sqlite3_column_name            sqlite3_api->column_name
+#define sqlite3_column_name16          sqlite3_api->column_name16
+#define sqlite3_column_origin_name     sqlite3_api->column_origin_name
+#define sqlite3_column_origin_name16   sqlite3_api->column_origin_name16
+#define sqlite3_column_table_name      sqlite3_api->column_table_name
+#define sqlite3_column_table_name16    sqlite3_api->column_table_name16
+#define sqlite3_column_text            sqlite3_api->column_text
+#define sqlite3_column_text16          sqlite3_api->column_text16
+#define sqlite3_column_type            sqlite3_api->column_type
+#define sqlite3_column_value           sqlite3_api->column_value
+#define sqlite3_commit_hook            sqlite3_api->commit_hook
+#define sqlite3_complete               sqlite3_api->complete
+#define sqlite3_complete16             sqlite3_api->complete16
+#define sqlite3_create_collation       sqlite3_api->create_collation
+#define sqlite3_create_collation16     sqlite3_api->create_collation16
+#define sqlite3_create_function        sqlite3_api->create_function
+#define sqlite3_create_function16      sqlite3_api->create_function16
+#define sqlite3_create_module          sqlite3_api->create_module
+#define sqlite3_create_module_v2       sqlite3_api->create_module_v2
+#define sqlite3_data_count             sqlite3_api->data_count
+#define sqlite3_db_handle              sqlite3_api->db_handle
+#define sqlite3_declare_vtab           sqlite3_api->declare_vtab
+#define sqlite3_enable_shared_cache    sqlite3_api->enable_shared_cache
+#define sqlite3_errcode                sqlite3_api->errcode
+#define sqlite3_errmsg                 sqlite3_api->errmsg
+#define sqlite3_errmsg16               sqlite3_api->errmsg16
+#define sqlite3_exec                   sqlite3_api->exec
+#ifndef SQLITE_OMIT_DEPRECATED
+#define sqlite3_expired                sqlite3_api->expired
+#endif
+#define sqlite3_finalize               sqlite3_api->finalize
+#define sqlite3_free                   sqlite3_api->free
+#define sqlite3_free_table             sqlite3_api->free_table
+#define sqlite3_get_autocommit         sqlite3_api->get_autocommit
+#define sqlite3_get_auxdata            sqlite3_api->get_auxdata
+#define sqlite3_get_table              sqlite3_api->get_table
+#ifndef SQLITE_OMIT_DEPRECATED
+#define sqlite3_global_recover         sqlite3_api->global_recover
+#endif
+#define sqlite3_interrupt              sqlite3_api->interruptx
+#define sqlite3_last_insert_rowid      sqlite3_api->last_insert_rowid
+#define sqlite3_libversion             sqlite3_api->libversion
+#define sqlite3_libversion_number      sqlite3_api->libversion_number
+#define sqlite3_malloc                 sqlite3_api->malloc
+#define sqlite3_mprintf                sqlite3_api->mprintf
+#define sqlite3_open                   sqlite3_api->open
+#define sqlite3_open16                 sqlite3_api->open16
+#define sqlite3_prepare                sqlite3_api->prepare
+#define sqlite3_prepare16              sqlite3_api->prepare16
+#define sqlite3_prepare_v2             sqlite3_api->prepare_v2
+#define sqlite3_prepare16_v2           sqlite3_api->prepare16_v2
+#define sqlite3_profile                sqlite3_api->profile
+#define sqlite3_progress_handler       sqlite3_api->progress_handler
+#define sqlite3_realloc                sqlite3_api->realloc
+#define sqlite3_reset                  sqlite3_api->reset
+#define sqlite3_result_blob            sqlite3_api->result_blob
+#define sqlite3_result_double          sqlite3_api->result_double
+#define sqlite3_result_error           sqlite3_api->result_error
+#define sqlite3_result_error16         sqlite3_api->result_error16
+#define sqlite3_result_int             sqlite3_api->result_int
+#define sqlite3_result_int64           sqlite3_api->result_int64
+#define sqlite3_result_null            sqlite3_api->result_null
+#define sqlite3_result_text            sqlite3_api->result_text
+#define sqlite3_result_text16          sqlite3_api->result_text16
+#define sqlite3_result_text16be        sqlite3_api->result_text16be
+#define sqlite3_result_text16le        sqlite3_api->result_text16le
+#define sqlite3_result_value           sqlite3_api->result_value
+#define sqlite3_rollback_hook          sqlite3_api->rollback_hook
+#define sqlite3_set_authorizer         sqlite3_api->set_authorizer
+#define sqlite3_set_auxdata            sqlite3_api->set_auxdata
+#define sqlite3_snprintf               sqlite3_api->xsnprintf
+#define sqlite3_step                   sqlite3_api->step
+#define sqlite3_table_column_metadata  sqlite3_api->table_column_metadata
+#define sqlite3_thread_cleanup         sqlite3_api->thread_cleanup
+#define sqlite3_total_changes          sqlite3_api->total_changes
+#define sqlite3_trace                  sqlite3_api->trace
+#ifndef SQLITE_OMIT_DEPRECATED
+#define sqlite3_transfer_bindings      sqlite3_api->transfer_bindings
+#endif
+#define sqlite3_update_hook            sqlite3_api->update_hook
+#define sqlite3_user_data              sqlite3_api->user_data
+#define sqlite3_value_blob             sqlite3_api->value_blob
+#define sqlite3_value_bytes            sqlite3_api->value_bytes
+#define sqlite3_value_bytes16          sqlite3_api->value_bytes16
+#define sqlite3_value_double           sqlite3_api->value_double
+#define sqlite3_value_int              sqlite3_api->value_int
+#define sqlite3_value_int64            sqlite3_api->value_int64
+#define sqlite3_value_numeric_type     sqlite3_api->value_numeric_type
+#define sqlite3_value_text             sqlite3_api->value_text
+#define sqlite3_value_text16           sqlite3_api->value_text16
+#define sqlite3_value_text16be         sqlite3_api->value_text16be
+#define sqlite3_value_text16le         sqlite3_api->value_text16le
+#define sqlite3_value_type             sqlite3_api->value_type
+#define sqlite3_vmprintf               sqlite3_api->vmprintf
+#define sqlite3_vsnprintf              sqlite3_api->xvsnprintf
+#define sqlite3_overload_function      sqlite3_api->overload_function
+#define sqlite3_prepare_v2             sqlite3_api->prepare_v2
+#define sqlite3_prepare16_v2           sqlite3_api->prepare16_v2
+#define sqlite3_clear_bindings         sqlite3_api->clear_bindings
+#define sqlite3_bind_zeroblob          sqlite3_api->bind_zeroblob
+#define sqlite3_blob_bytes             sqlite3_api->blob_bytes
+#define sqlite3_blob_close             sqlite3_api->blob_close
+#define sqlite3_blob_open              sqlite3_api->blob_open
+#define sqlite3_blob_read              sqlite3_api->blob_read
+#define sqlite3_blob_write             sqlite3_api->blob_write
+#define sqlite3_create_collation_v2    sqlite3_api->create_collation_v2
+#define sqlite3_file_control           sqlite3_api->file_control
+#define sqlite3_memory_highwater       sqlite3_api->memory_highwater
+#define sqlite3_memory_used            sqlite3_api->memory_used
+#define sqlite3_mutex_alloc            sqlite3_api->mutex_alloc
+#define sqlite3_mutex_enter            sqlite3_api->mutex_enter
+#define sqlite3_mutex_free             sqlite3_api->mutex_free
+#define sqlite3_mutex_leave            sqlite3_api->mutex_leave
+#define sqlite3_mutex_try              sqlite3_api->mutex_try
+#define sqlite3_open_v2                sqlite3_api->open_v2
+#define sqlite3_release_memory         sqlite3_api->release_memory
+#define sqlite3_result_error_nomem     sqlite3_api->result_error_nomem
+#define sqlite3_result_error_toobig    sqlite3_api->result_error_toobig
+#define sqlite3_sleep                  sqlite3_api->sleep
+#define sqlite3_soft_heap_limit        sqlite3_api->soft_heap_limit
+#define sqlite3_vfs_find               sqlite3_api->vfs_find
+#define sqlite3_vfs_register           sqlite3_api->vfs_register
+#define sqlite3_vfs_unregister         sqlite3_api->vfs_unregister
+#define sqlite3_threadsafe             sqlite3_api->xthreadsafe
+#define sqlite3_result_zeroblob        sqlite3_api->result_zeroblob
+#define sqlite3_result_error_code      sqlite3_api->result_error_code
+#define sqlite3_test_control           sqlite3_api->test_control
+#define sqlite3_randomness             sqlite3_api->randomness
+#define sqlite3_context_db_handle      sqlite3_api->context_db_handle
+#define sqlite3_extended_result_codes  sqlite3_api->extended_result_codes
+#define sqlite3_limit                  sqlite3_api->limit
+#define sqlite3_next_stmt              sqlite3_api->next_stmt
+#define sqlite3_sql                    sqlite3_api->sql
+#define sqlite3_status                 sqlite3_api->status
+#define sqlite3_backup_finish          sqlite3_api->backup_finish
+#define sqlite3_backup_init            sqlite3_api->backup_init
+#define sqlite3_backup_pagecount       sqlite3_api->backup_pagecount
+#define sqlite3_backup_remaining       sqlite3_api->backup_remaining
+#define sqlite3_backup_step            sqlite3_api->backup_step
+#define sqlite3_compileoption_get      sqlite3_api->compileoption_get
+#define sqlite3_compileoption_used     sqlite3_api->compileoption_used
+#define sqlite3_create_function_v2     sqlite3_api->create_function_v2
+#define sqlite3_db_config              sqlite3_api->db_config
+#define sqlite3_db_mutex               sqlite3_api->db_mutex
+#define sqlite3_db_status              sqlite3_api->db_status
+#define sqlite3_extended_errcode       sqlite3_api->extended_errcode
+#define sqlite3_log                    sqlite3_api->log
+#define sqlite3_soft_heap_limit64      sqlite3_api->soft_heap_limit64
+#define sqlite3_sourceid               sqlite3_api->sourceid
+#define sqlite3_stmt_status            sqlite3_api->stmt_status
+#define sqlite3_strnicmp               sqlite3_api->strnicmp
+#define sqlite3_unlock_notify          sqlite3_api->unlock_notify
+#define sqlite3_wal_autocheckpoint     sqlite3_api->wal_autocheckpoint
+#define sqlite3_wal_checkpoint         sqlite3_api->wal_checkpoint
+#define sqlite3_wal_hook               sqlite3_api->wal_hook
+#define sqlite3_blob_reopen            sqlite3_api->blob_reopen
+#define sqlite3_vtab_config            sqlite3_api->vtab_config
+#define sqlite3_vtab_on_conflict       sqlite3_api->vtab_on_conflict
+/* Version 3.7.16 and later */
+#define sqlite3_close_v2               sqlite3_api->close_v2
+#define sqlite3_db_filename            sqlite3_api->db_filename
+#define sqlite3_db_readonly            sqlite3_api->db_readonly
+#define sqlite3_db_release_memory      sqlite3_api->db_release_memory
+#define sqlite3_errstr                 sqlite3_api->errstr
+#define sqlite3_stmt_busy              sqlite3_api->stmt_busy
+#define sqlite3_stmt_readonly          sqlite3_api->stmt_readonly
+#define sqlite3_stricmp                sqlite3_api->stricmp
+#define sqlite3_uri_boolean            sqlite3_api->uri_boolean
+#define sqlite3_uri_int64              sqlite3_api->uri_int64
+#define sqlite3_uri_parameter          sqlite3_api->uri_parameter
+#define sqlite3_uri_vsnprintf          sqlite3_api->xvsnprintf
+#define sqlite3_wal_checkpoint_v2      sqlite3_api->wal_checkpoint_v2
+/* Version 3.8.7 and later */
+#define sqlite3_auto_extension         sqlite3_api->auto_extension
+#define sqlite3_bind_blob64            sqlite3_api->bind_blob64
+#define sqlite3_bind_text64            sqlite3_api->bind_text64
+#define sqlite3_cancel_auto_extension  sqlite3_api->cancel_auto_extension
+#define sqlite3_load_extension         sqlite3_api->load_extension
+#define sqlite3_malloc64               sqlite3_api->malloc64
+#define sqlite3_msize                  sqlite3_api->msize
+#define sqlite3_realloc64              sqlite3_api->realloc64
+#define sqlite3_reset_auto_extension   sqlite3_api->reset_auto_extension
+#define sqlite3_result_blob64          sqlite3_api->result_blob64
+#define sqlite3_result_text64          sqlite3_api->result_text64
+#define sqlite3_strglob                sqlite3_api->strglob
+/* Version 3.8.11 and later */
+#define sqlite3_value_dup              sqlite3_api->value_dup
+#define sqlite3_value_free             sqlite3_api->value_free
+#define sqlite3_result_zeroblob64      sqlite3_api->result_zeroblob64
+#define sqlite3_bind_zeroblob64        sqlite3_api->bind_zeroblob64
+/* Version 3.9.0 and later */
+#define sqlite3_value_subtype          sqlite3_api->value_subtype
+#define sqlite3_result_subtype         sqlite3_api->result_subtype
+/* Version 3.10.0 and later */
+#define sqlite3_status64               sqlite3_api->status64
+#define sqlite3_strlike                sqlite3_api->strlike
+#define sqlite3_db_cacheflush          sqlite3_api->db_cacheflush
+/* Version 3.12.0 and later */
+#define sqlite3_system_errno           sqlite3_api->system_errno
+/* Version 3.14.0 and later */
+#define sqlite3_trace_v2               sqlite3_api->trace_v2
+#define sqlite3_expanded_sql           sqlite3_api->expanded_sql
+/* Version 3.18.0 and later */
+#define sqlite3_set_last_insert_rowid  sqlite3_api->set_last_insert_rowid
+/* Version 3.20.0 and later */
+#define sqlite3_prepare_v3             sqlite3_api->prepare_v3
+#define sqlite3_prepare16_v3           sqlite3_api->prepare16_v3
+#define sqlite3_bind_pointer           sqlite3_api->bind_pointer
+#define sqlite3_result_pointer         sqlite3_api->result_pointer
+#define sqlite3_value_pointer          sqlite3_api->value_pointer
+/* Version 3.22.0 and later */
+#define sqlite3_vtab_nochange          sqlite3_api->vtab_nochange
+#define sqlite3_value_nochange         sqlite3_api->value_nochange
+#define sqlite3_vtab_collation         sqlite3_api->vtab_collation
+/* Version 3.24.0 and later */
+#define sqlite3_keyword_count          sqlite3_api->keyword_count
+#define sqlite3_keyword_name           sqlite3_api->keyword_name
+#define sqlite3_keyword_check          sqlite3_api->keyword_check
+#define sqlite3_str_new                sqlite3_api->str_new
+#define sqlite3_str_finish             sqlite3_api->str_finish
+#define sqlite3_str_appendf            sqlite3_api->str_appendf
+#define sqlite3_str_vappendf           sqlite3_api->str_vappendf
+#define sqlite3_str_append             sqlite3_api->str_append
+#define sqlite3_str_appendall          sqlite3_api->str_appendall
+#define sqlite3_str_appendchar         sqlite3_api->str_appendchar
+#define sqlite3_str_reset              sqlite3_api->str_reset
+#define sqlite3_str_errcode            sqlite3_api->str_errcode
+#define sqlite3_str_length             sqlite3_api->str_length
+#define sqlite3_str_value              sqlite3_api->str_value
+/* Version 3.25.0 and later */
+#define sqlite3_create_window_function sqlite3_api->create_window_function
+/* Version 3.26.0 and later */
+#define sqlite3_normalized_sql         sqlite3_api->normalized_sql
+/* Version 3.28.0 and later */
+#define sqlite3_stmt_isexplain         sqlite3_api->stmt_isexplain
+#define sqlite3_value_frombind         sqlite3_api->value_frombind
+/* Version 3.30.0 and later */
+#define sqlite3_drop_modules           sqlite3_api->drop_modules
+/* Version 3.31.0 and later */
+#define sqlite3_hard_heap_limit64      sqlite3_api->hard_heap_limit64
+#define sqlite3_uri_key                sqlite3_api->uri_key
+#define sqlite3_filename_database      sqlite3_api->filename_database
+#define sqlite3_filename_journal       sqlite3_api->filename_journal
+#define sqlite3_filename_wal           sqlite3_api->filename_wal
+/* Version 3.32.0 and later */
+#define sqlite3_create_filename        sqlite3_api->create_filename
+#define sqlite3_free_filename          sqlite3_api->free_filename
+#define sqlite3_database_file_object   sqlite3_api->database_file_object
+/* Version 3.34.0 and later */
+#define sqlite3_txn_state              sqlite3_api->txn_state
+/* Version 3.36.1 and later */
+#define sqlite3_changes64              sqlite3_api->changes64
+#define sqlite3_total_changes64        sqlite3_api->total_changes64
+/* Version 3.37.0 and later */
+#define sqlite3_autovacuum_pages       sqlite3_api->autovacuum_pages
+/* Version 3.38.0 and later */
+#define sqlite3_error_offset           sqlite3_api->error_offset
+#define sqlite3_vtab_rhs_value         sqlite3_api->vtab_rhs_value
+#define sqlite3_vtab_distinct          sqlite3_api->vtab_distinct
+#define sqlite3_vtab_in                sqlite3_api->vtab_in
+#define sqlite3_vtab_in_first          sqlite3_api->vtab_in_first
+#define sqlite3_vtab_in_next           sqlite3_api->vtab_in_next
+/* Version 3.39.0 and later */
+#ifndef SQLITE_OMIT_DESERIALIZE
+#define sqlite3_deserialize            sqlite3_api->deserialize
+#define sqlite3_serialize              sqlite3_api->serialize
+#endif
+#define sqlite3_db_name                sqlite3_api->db_name
+/* Version 3.40.0 and later */
+#define sqlite3_value_encoding         sqlite3_api->value_encoding
+/* Version 3.41.0 and later */
+#define sqlite3_is_interrupted         sqlite3_api->is_interrupted
+/* Version 3.43.0 and later */
+#define sqlite3_stmt_explain           sqlite3_api->stmt_explain
+/* Version 3.44.0 and later */
+#define sqlite3_get_clientdata         sqlite3_api->get_clientdata
+#define sqlite3_set_clientdata         sqlite3_api->set_clientdata
+#endif /* !defined(SQLITE_CORE) && !defined(SQLITE_OMIT_LOAD_EXTENSION) */
+
+#if !defined(SQLITE_CORE) && !defined(SQLITE_OMIT_LOAD_EXTENSION)
+  /* This case when the file really is being compiled as a loadable 
+  ** extension */
+# define SQLITE_EXTENSION_INIT1     const sqlite3_api_routines *sqlite3_api=0;
+# define SQLITE_EXTENSION_INIT2(v)  sqlite3_api=v;
+# define SQLITE_EXTENSION_INIT3     \
+    extern const sqlite3_api_routines *sqlite3_api;
+#else
+  /* This case when the file is being statically linked into the 
+  ** application */
+# define SQLITE_EXTENSION_INIT1     /*no-op*/
+# define SQLITE_EXTENSION_INIT2(v)  (void)v; /* unused parameter */
+# define SQLITE_EXTENSION_INIT3     /*no-op*/
+#endif
+
+#endif /* SQLITE3EXT_H */

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
+   )
+

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
+}

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;
+}

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

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);
+}

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

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
+}

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
+ }

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
+}

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);
+}

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;
+}

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

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
+}