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,7 +10,8 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.0)
 PROJECT( IVP_EXTEND )
 
 set (CMAKE_CXX_STANDARD 11)
-set (MOOSIVP_SOURCE_TREE_BASE "/home/zjk/software/moos-ivp-main")
+# Resolved conflict: using relative path for MOOSIVP_SOURCE_TREE_BASE
+set (MOOSIVP_SOURCE_TREE_BASE "/home/zjk/project/lib/moos-ivp")
 
 #=======================================================================
 # Set the output directories for the binary and library files
@@ -102,4 +103,4 @@ ENDIF( ${WIN32} )
 #=======================================================================
 # Add Subdirectories
 #=======================================================================
-ADD_SUBDIRECTORY( src )
+ADD_SUBDIRECTORY( src )

missions/alder/alder.moos

@@ -164,7 +164,7 @@ ProcessConfig = pAUV150
 
   //server_host = 10.127.0.18
   //server_host = 127.0.0.1
-  server_host = 172.18.96.1
+  server_host = 192.168.137.1
   LatOrigin  = 43.825300 
   LongOrigin = -70.330400 
 

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/pAUV150/AUV150.cpp

@@ -12,10 +12,98 @@
 #include <iomanip>
 #include <string>
 #include <iostream>
-
+#include <arpa/inet.h>
-#define DEBUG_
+// #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;
@@ -35,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;
@@ -89,18 +180,18 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
     bool   mdbl  = msg.IsDouble();
     bool   mstr  = msg.IsString();
 #endif
-    if((key == "MOOS_MANUAL_OVERIDE") || (key == "MOOS_MANUAL_OVERRIDE"))
+    if((key == "MOOS_MANUAL_OVERIDE") || (key == "MOOS_MANUAL_OVERRIDE") || (key == "MOOS_MANUAL_OVERRID_AUV150"))
     {
         string sval = msg.GetString();
+        m_debug.push_back(m_overrived);
+        m_debug_str.push_back(sval);
         if(sval == "false")
         {
-            std::cout << "======================" << std::endl;
+            m_overrived = false;
-            m_overrived == false;
         }
         else
         {
-            std::cout << "=============//=========" << std::endl;
+            m_overrived = true;
-            m_overrived == true;
         }
     }
     else if(key == "DESIRED_HEADING")
@@ -111,10 +202,29 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
     {
         m_control.rtU.speed_cmd = msg.GetDouble();
     }
-    else if(key == "DESIRED_DEPTH") 
+    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);
    }
@@ -145,7 +255,38 @@ bool AUV150::Iterate()
         m_geodesy.LatLong2LocalGrid(m_status.insLatitude, m_status.insLongitude, m_pos_x, m_pos_y);
         postStatusUpdate("NAV");
     #endif
-    // AppCastingMOOSApp::PostReport();
+    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);
 }
 
@@ -215,17 +356,17 @@ bool AUV150::OnStartUp()
 void AUV150::registerVariables()
 {
   AppCastingMOOSApp::RegisterVariables();
-//   Register("DESIRED_RUDDER",0);
-//   Register("DESIRED_THRUST",0);
-//   Register("DESIRED_ELEVATOR",0);
   Register("DESIRED_HEADING", 0);
   Register("DESIRED_SPEED", 0);
   Register("DESIRED_DEPTH", 0);
   Register("MOOS_MANUAL_OVERIDE", 0);
   Register("MOOS_MANUAL_OVERRIDE", 0);
+  Register("MOOS_MANUAL_OVERIDE*", 0);
+  Register("MOOS_MANUAL_OVERRIDE*", 0);
+  Register("PZ", 0);
+  Register("LED", 0);
 }
 
-
 bool AUV150::Start()
 {
     //初始化TcpSocket
@@ -264,10 +405,12 @@ bool AUV150::ListenLoop()
         {
             if((m_TcpSocket!=NULL) && m_bConnected)
             {
-                int count = m_TcpSocket->iReadMessageWithTimeOut(buffer, sizeof(FeedbackFrame_150AUV),3);
+                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)
                     {
@@ -275,12 +418,18 @@ 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");
-                    // if(!m_overrived)
+                    if(!m_overrived)
-                    // {
+                    {
                         // 计算控制量
                         m_control.rtU.u = m_status.dvlVelX;
                         m_control.rtU.v = m_status.dvlVelY;
@@ -295,49 +444,59 @@ bool AUV150::ListenLoop()
                         m_control.rtU.y = m_status.y;
                         m_control.rtU.z = m_status.z;
                         m_control.step();
-                        m_CommandFrame.rudderUp = m_control.rtY.DirectUpperRudderServoAngleCmd + 35.0f; 
+                        m_CommandFrame.rudderUp = encodeDoubleToUint8WithSign(-m_control.rtY.DirectUpperRudderServoAngleCmd,30); 
-                        m_CommandFrame.rudderDown = m_control.rtY.DirectLowerRudderServoAngleCmd + 35.0f;
+                        m_CommandFrame.rudderDown = encodeDoubleToUint8WithSign(-m_control.rtY.DirectLowerRudderServoAngleCmd,30);
-                        m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd + 35.0f;
+                        m_CommandFrame.rudderLeft = encodeDoubleToUint8WithSign(-m_control.rtY.DirectLeftRudderServoAngleCmd,30);
-                        m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd + 35.0f;
+                        m_CommandFrame.rudderRight = encodeDoubleToUint8WithSign(-m_control.rtY.DirectRightRudderServoAngleCmd,30);
-                        m_CommandFrame.mainThruster = m_control.rtY.MainThrusterSpeedCmd;
+                        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;
-                    // }
+                    }
-                    // 发送消息 
+                    else
-                    try
                     {
-                        m_TcpSocket->iSendMessage(&m_CommandFrame, sizeof(CommandFrame_150AUV));
+                        m_CommandFrame.rudderUp = 0; 
-                        m_controlGap =  MOOSTime() - m_desiredVarTime;
+                        m_CommandFrame.rudderDown = 0;
-                        m_controlTime = MOOSTime();
+                        m_CommandFrame.rudderLeft = 0;
-                        m_bConnected = true;
+                        m_CommandFrame.rudderRight = 0;
+                        m_CommandFrame.mainThruster = 0;   
                     }
-                    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); //无数据接收
-                }
             }
+            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)
         {
@@ -364,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());
@@ -379,7 +538,6 @@ bool AUV150::WriteLoop()
     return true;
 }
 
-
 bool AUV150::buildReport()
 {
     auto displayWidth = [&](const std::string& str) {
@@ -458,17 +616,27 @@ bool AUV150::buildReport()
     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", "Fre","Cont" ,"ip","OLat", "OLon","Gap"},
+      {"Connect", "Fre","Cont" ,"ip"},
       {
           (isConnected() ? "Yes" : "No"),
           doubleToString(getRealReadFreq(), 4) + " Hz",
           ulintToString(getCounter()),
           m_serverHost,
-          doubleToString(m_OriginLatitude, 5) + "deg",
+        //   doubleToString(m_OriginLatitude, 5) + "deg",
-          doubleToString(m_OriginLongitude, 5) + "deg",
+        //   doubleToString(m_OriginLongitude, 5) + "deg",
-          doubleToString(m_controlGap, 5) + "s"
+        //   doubleToString(m_controlGap, 5) + "s"
       }
     );
     m_msgs << "Fault Codes: ";
@@ -521,10 +689,10 @@ bool AUV150::buildReport()
         {
             doubleToString(m_status.thrusterRPM, 1) + "rpm",
             doubleToString(m_CommandFrame.mainThruster, 1) + "%",
-            doubleToString(m_CommandFrame.rudderUp-35.0f, 1) + "deg",
+            doubleToString(m_CommandFrame.rudderUp, 1) + "deg",
-            doubleToString(m_CommandFrame.rudderDown-35.0f, 1) + "deg",
+            doubleToString(m_CommandFrame.rudderDown, 1) + "deg",
-            doubleToString(m_CommandFrame.rudderLeft-35.0f, 1) + "deg",
+            doubleToString(m_CommandFrame.rudderLeft, 1) + "deg",
-            doubleToString(m_CommandFrame.rudderRight-35.0f, 1) + "deg",
+            doubleToString(m_CommandFrame.rudderRight, 1) + "deg",
         }
     );
     // m_msgs << "---------------------------------\n";
@@ -533,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" , "PZ", "DVL","Thruster Status"},
+        {"Light", "Leak", "CtlFlg" , "Emergency power" , "PZ", "DVL","Thruster Status"},
         {
             m_status.ledSwitch ? "ON" : "OFF",
             m_status.leakStatus ? "LEAK" : "NORMAL",
@@ -563,19 +731,20 @@ bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame)
     m_status.navModeFb = feedbackFrame.navModeFb;
     m_status.altimeterHeight = feedbackFrame.altimeterHeight / 100.0f;
     m_status.depthSensor = feedbackFrame.depthSensor / 100.0f;
-    m_status.trueHeading = feedbackFrame.trueHeading / 100.0f;
+    m_status.trueHeading = feedbackFrame.trueHeading / 10.0f;
 
-    m_status.pitch = feedbackFrame.pitch / 100.0f;
+    // m_status.pitch = feedbackFrame.pitch / 10.0f;
-    m_status.roll = feedbackFrame.roll / 100.0f;
+    m_status.pitch = parseInt16WithSign(feedbackFrame.pitch)/10.0f;
-    m_status.velocityEast = feedbackFrame.velocityEast / 100.0f;
+    m_status.roll = parseInt16WithSign(feedbackFrame.roll)/10.0f;
-    m_status.velocityNorth = feedbackFrame.velocityNorth / 100.0f;
+    m_status.velocityEast = parseInt16WithSign(feedbackFrame.velocityEast)/100.0f;
-    m_status.velocityDown = feedbackFrame.velocityDown / 100.0f;
+    m_status.velocityNorth = parseInt16WithSign(feedbackFrame.velocityNorth)/100.0f;
-    m_status.insLongitude = feedbackFrame.insLongitude / 1e5 - 180.0f; 
+    m_status.velocityDown = parseInt16WithSign(feedbackFrame.velocityDown)/100.0f;
-    m_status.insLatitude = feedbackFrame.insLatitude / 1e5 - 90.0f;
+    m_status.insLongitude = feedbackFrame.insLongitude / 1e6; 
+    m_status.insLatitude = feedbackFrame.insLatitude / 1e6;
     m_status.insAltitude = feedbackFrame.insAltitude / 100.0f;
-    m_status.dvlVelX = feedbackFrame.dvlVelX / 100.0f;
+    m_status.dvlVelX = parseInt16WithSign(feedbackFrame.dvlVelX) / 100.0f;
-    m_status.dvlVelY = feedbackFrame.dvlVelY / 100.0f;
+    m_status.dvlVelY = parseInt16WithSign(feedbackFrame.dvlVelY) / 100.0f;
-    m_status.dvlVelZ = feedbackFrame.dvlVelZ / 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.0f;
@@ -594,7 +763,6 @@ bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame)
     m_status.x = m_pos_x;
     m_status.y = m_pos_y;
     m_status.z = m_status.depthSensor;
-
     return true;
 }
 

src/pAUV150/AUV150.h

@@ -16,6 +16,7 @@
 #include "MOOS/libMOOSGeodesy/MOOSGeodesy.h"
 #include "Controler/Controler.h"
 #include "AngleUtils.h"
+#include "sqlit3/SQLite.h"
 
 #include <iostream>
 #include <string>
@@ -36,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   单位:—        对“航行模式”到“控制抛载”区间进行求和校验
@@ -52,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°，惯导
+    uint16_t  pitch;          // 8. 俯仰角 @12  int16   单位:0.01°        -18000~18000 对应 -180~180°，惯导
-    int16_t  roll;           // 9. 横滚角 @14  int16   单位:0.01°        -18000~18000 对应 -180~180°，惯导
+    uint16_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，惯导
+    uint16_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，惯导
+    uint16_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  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        无符号，惯导
+    uint16_t  insAltitude;    // 15. 惯导：高度 @30 int16   单位:0.01m        无符号，惯导
-    int16_t  dvlVelX;        // 16. DVL:横向速度 @32 int16   单位:0.01m/s     DVL
+    uint16_t  dvlVelX;        // 16. DVL:横向速度 @32 int16   单位:0.01m/s     DVL
-    int16_t  dvlVelY;        // 17. DVL:纵向速度 @34 int16   单位:0.01m/s     DVL
+    uint16_t  dvlVelY;        // 17. DVL:纵向速度 @34 int16   单位:0.01m/s     DVL
-    int16_t  dvlVelZ;        // 18. DVL:天向速度 @36 int16   单位:0.01m/s     DVL
+    uint16_t  dvlVelZ;        // 18. DVL:天向速度 @36 int16   单位:0.01m/s     DVL
-    int16_t  thrusterRPM;    // 19. 主推进器实际转速(x1) @38 int16 单位:rpm      -4000~4000，推进器
+    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   单位:—         
@@ -163,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_; 
@@ -177,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;

src/pAUV150/CMakeLists.txt

@@ -21,6 +21,8 @@ SET(SRC
   AUV150.cpp
   AUV150_Info.cpp
   main.cpp
+  sqlit3/SQLite.cpp
+  sqlit3/sqlite3.c
   ${SRC_FILES}
 )
 
@@ -28,6 +30,7 @@ SET(SRC
 include_directories(
   ${MOOSGEODESY_INCLUDE_DIRS}
   ${CMAKE_CURRENT_SOURCE_DIR}/Controler
+  ${CMAKE_CURRENT_SOURCE_DIR}/sqlit3
 )
 
 ADD_EXECUTABLE(pAUV150 ${SRC})
@@ -40,4 +43,3 @@ TARGET_LINK_LIBRARIES(pAUV150
    mbutil
    m
    pthread)
-

src/pAUV150/Controler/Controler.cpp

@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.115
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// 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)
@@ -27,25 +27,25 @@
 void Controler::step()
 {
   real_T mod_error;
+  real_T rtb_DeadZone;
   real_T rtb_FilterCoefficient;
   real_T rtb_FilterCoefficient_a;
-  real_T rtb_FilterCoefficient_lc;
-  real_T rtb_Filter_b;
   real_T rtb_IntegralGain;
-  real_T rtb_Integrator_d;
+  real_T rtb_Integrator_l;
+  real_T rtb_Sum;
   real_T rtb_Switch;
   real_T rtb_depth_error;
-  int8_T rtb_Switch1;
   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 Function' incorporates:
+  // MATLAB Function: '<S1>/MATLAB Function1' incorporates:
   //   Inport: '<Root>/heading_cmd'
   //   Inport: '<Root>/psi'
 
-  mod_error = rtU.heading_cmd - rtU.psi;
+  mod_error = rtU.psi - rtU.heading_cmd;
   if (!std::isnan(mod_error)) {
     if (std::isinf(mod_error)) {
       mod_error = (rtNaN);
@@ -65,64 +65,50 @@ void Controler::step()
     mod_error -= 360.0;
   }
 
-  // End of MATLAB Function: '<S1>/MATLAB Function'
+  // End of MATLAB Function: '<S1>/MATLAB Function1'
 
-  // Gain: '<S45>/Filter Coefficient' incorporates:
+  // Gain: '<S98>/Filter Coefficient' incorporates:
-  //   DiscreteIntegrator: '<S37>/Filter'
+  //   DiscreteIntegrator: '<S90>/Filter'
-  //   Gain: '<S35>/Derivative Gain'
+  //   Gain: '<S88>/Derivative Gain'
-  //   Sum: '<S37>/SumD'
+  //   Sum: '<S90>/SumD'
 
   rtb_FilterCoefficient = (rtP.pid_heading.D * mod_error - rtDW.Filter_DSTATE) *
     rtP.pid_heading.N;
 
-  // Sum: '<S51>/Sum' incorporates:
+  // Sum: '<S104>/Sum' incorporates:
-  //   DiscreteIntegrator: '<S42>/Integrator'
+  //   DiscreteIntegrator: '<S95>/Integrator'
-  //   Gain: '<S47>/Proportional Gain'
+  //   Gain: '<S100>/Proportional Gain'
 
-  rtb_IntegralGain = (rtP.pid_heading.P * mod_error + rtDW.Integrator_DSTATE) +
+  rtb_Integrator_l = (rtP.pid_heading.P * mod_error + rtDW.Integrator_DSTATE) +
     rtb_FilterCoefficient;
 
-  // Saturate: '<S49>/Saturation' incorporates:
+  // DeadZone: '<S87>/DeadZone'
-  //   DeadZone: '<S34>/DeadZone'
+  if (rtb_Integrator_l > 35.0) {
-
+    rtb_IntegralGain = rtb_Integrator_l - 35.0;
-  if (rtb_IntegralGain > 35.0) {
+  } else if (rtb_Integrator_l >= -35.0) {
-    // Outport: '<Root>/DirectUpperRudderServoAngleCmd'
+    rtb_IntegralGain = 0.0;
-    rtY.DirectUpperRudderServoAngleCmd = 35.0;
-    rtb_IntegralGain -= 35.0;
   } else {
-    if (rtb_IntegralGain < -35.0) {
+    rtb_IntegralGain = rtb_Integrator_l - -35.0;
-      // Outport: '<Root>/DirectUpperRudderServoAngleCmd'
-      rtY.DirectUpperRudderServoAngleCmd = -35.0;
-    } else {
-      // Outport: '<Root>/DirectUpperRudderServoAngleCmd'
-      rtY.DirectUpperRudderServoAngleCmd = rtb_IntegralGain;
-    }
-
-    if (rtb_IntegralGain >= -35.0) {
-      rtb_IntegralGain = 0.0;
-    } else {
-      rtb_IntegralGain -= -35.0;
-    }
   }
 
-  // End of Saturate: '<S49>/Saturation'
+  // End of DeadZone: '<S87>/DeadZone'
 
-  // RelationalOperator: '<S32>/Relational Operator' incorporates:
+  // RelationalOperator: '<S85>/Relational Operator' incorporates:
-  //   Constant: '<S32>/Clamping_zero'
+  //   Constant: '<S85>/Clamping_zero'
 
   rtb_RelationalOperator = (rtb_IntegralGain != 0.0);
 
-  // RelationalOperator: '<S32>/fix for DT propagation issue' incorporates:
+  // RelationalOperator: '<S85>/fix for DT propagation issue' incorporates:
-  //   Constant: '<S32>/Clamping_zero'
+  //   Constant: '<S85>/Clamping_zero'
 
   rtb_fixforDTpropagationissue = (rtb_IntegralGain > 0.0);
 
-  // Gain: '<S39>/Integral Gain'
+  // Gain: '<S92>/Integral Gain'
   rtb_IntegralGain = rtP.pid_heading.I * mod_error;
 
-  // Switch: '<S32>/Switch1' incorporates:
+  // Switch: '<S85>/Switch1' incorporates:
-  //   Constant: '<S32>/Constant'
+  //   Constant: '<S85>/Constant'
-  //   Constant: '<S32>/Constant2'
+  //   Constant: '<S85>/Constant2'
 
   if (rtb_fixforDTpropagationissue) {
     tmp = 1;
@@ -130,185 +116,87 @@ void Controler::step()
     tmp = -1;
   }
 
-  // Switch: '<S32>/Switch2' incorporates:
+  // Switch: '<S85>/Switch2' incorporates:
-  //   Constant: '<S32>/Clamping_zero'
+  //   Constant: '<S85>/Clamping_zero'
-  //   Constant: '<S32>/Constant3'
+  //   Constant: '<S85>/Constant3'
-  //   Constant: '<S32>/Constant4'
+  //   Constant: '<S85>/Constant4'
-  //   RelationalOperator: '<S32>/fix for DT propagation issue1'
+  //   RelationalOperator: '<S85>/fix for DT propagation issue1'
 
   if (rtb_IntegralGain > 0.0) {
-    rtb_Switch1 = 1;
+    tmp_0 = 1;
   } else {
-    rtb_Switch1 = -1;
+    tmp_0 = -1;
   }
 
-  // Switch: '<S32>/Switch' incorporates:
+  // Switch: '<S85>/Switch' incorporates:
-  //   Constant: '<S32>/Constant1'
+  //   Constant: '<S85>/Constant1'
-  //   Logic: '<S32>/AND3'
+  //   Logic: '<S85>/AND3'
-  //   RelationalOperator: '<S32>/Equal1'
+  //   RelationalOperator: '<S85>/Equal1'
-  //   Switch: '<S32>/Switch1'
+  //   Switch: '<S85>/Switch1'
-  //   Switch: '<S32>/Switch2'
+  //   Switch: '<S85>/Switch2'
 
-  if (rtb_RelationalOperator && (tmp == rtb_Switch1)) {
+  if (rtb_RelationalOperator && (tmp == tmp_0)) {
     rtb_Switch = 0.0;
   } else {
     rtb_Switch = rtb_IntegralGain;
   }
 
-  // End of Switch: '<S32>/Switch'
+  // 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.z - rtU.depth_cmd;
+  rtb_depth_error = rtU.depth_cmd - rtU.z;
 
-  // Gain: '<S93>/Integral Gain'
+  // Gain: '<S38>/Integral Gain'
   rtb_IntegralGain = rtP.pid_depth.I * rtb_depth_error;
 
-  // Gain: '<S99>/Filter Coefficient' incorporates:
+  // Gain: '<S44>/Filter Coefficient' incorporates:
-  //   DiscreteIntegrator: '<S91>/Filter'
+  //   DiscreteIntegrator: '<S36>/Filter'
-  //   Gain: '<S89>/Derivative Gain'
+  //   Gain: '<S34>/Derivative Gain'
-  //   Sum: '<S91>/SumD'
+  //   Sum: '<S36>/SumD'
 
-  rtb_FilterCoefficient_lc = (rtP.pid_depth.D * rtb_depth_error -
+  rtb_FilterCoefficient_a = (rtP.pid_depth.D * rtb_depth_error -
     rtDW.Filter_DSTATE_m) * rtP.pid_depth.N;
 
-  // Sum: '<S105>/Sum' incorporates:
+  // Sum: '<S50>/Sum' incorporates:
-  //   DiscreteIntegrator: '<S96>/Integrator'
+  //   DiscreteIntegrator: '<S41>/Integrator'
-  //   Gain: '<S101>/Proportional Gain'
+  //   Gain: '<S46>/Proportional Gain'
 
-  rtb_Filter_b = (rtP.pid_depth.P * rtb_depth_error + rtDW.Integrator_DSTATE_o)
+  rtb_Sum = (rtP.pid_depth.P * rtb_depth_error + rtDW.Integrator_DSTATE_o) +
-    + rtb_FilterCoefficient_lc;
+    rtb_FilterCoefficient_a;
 
-  // DeadZone: '<S88>/DeadZone'
+  // DeadZone: '<S33>/DeadZone' incorporates:
-  if (rtb_Filter_b > 30.0) {
+  //   Saturate: '<S48>/Saturation'
-    rtb_Integrator_d = rtb_Filter_b - 30.0;
 
-    // Switch: '<S86>/Switch1' incorporates:
+  if (rtb_Sum > 30.0) {
-    //   Constant: '<S86>/Constant'
+    rtb_DeadZone = rtb_Sum - 30.0;
-
+    rtb_Sum = 30.0;
-    tmp = 1;
   } else {
-    if (rtb_Filter_b >= -30.0) {
+    if (rtb_Sum >= -30.0) {
-      rtb_Integrator_d = 0.0;
+      rtb_DeadZone = 0.0;
     } else {
-      rtb_Integrator_d = rtb_Filter_b - -30.0;
+      rtb_DeadZone = rtb_Sum - -30.0;
     }
 
-    // Switch: '<S86>/Switch1' incorporates:
+    if (rtb_Sum < -30.0) {
-    //   Constant: '<S86>/Constant2'
+      rtb_Sum = -30.0;
-
-    tmp = -1;
-  }
-
-  // End of DeadZone: '<S88>/DeadZone'
-
-  // Switch: '<S86>/Switch2' incorporates:
-  //   Constant: '<S86>/Clamping_zero'
-  //   Constant: '<S86>/Constant3'
-  //   Constant: '<S86>/Constant4'
-  //   RelationalOperator: '<S86>/fix for DT propagation issue1'
-
-  if (rtb_IntegralGain > 0.0) {
-    rtb_Switch1 = 1;
-  } else {
-    rtb_Switch1 = -1;
-  }
-
-  // Switch: '<S86>/Switch' incorporates:
-  //   Constant: '<S86>/Clamping_zero'
-  //   Constant: '<S86>/Constant1'
-  //   Logic: '<S86>/AND3'
-  //   RelationalOperator: '<S86>/Equal1'
-  //   RelationalOperator: '<S86>/Relational Operator'
-  //   Switch: '<S86>/Switch1'
-  //   Switch: '<S86>/Switch2'
-
-  if ((rtb_Integrator_d != 0.0) && (tmp == rtb_Switch1)) {
-    rtb_Integrator_d = 0.0;
-  } else {
-    rtb_Integrator_d = rtb_IntegralGain;
-  }
-
-  // End of Switch: '<S86>/Switch'
-
-  // Saturate: '<S103>/Saturation'
-  if (rtb_Filter_b > 30.0) {
-    rtb_Filter_b = 30.0;
-  } else if (rtb_Filter_b < -30.0) {
-    rtb_Filter_b = -30.0;
-  }
-
-  // Sum: '<S1>/Sum2' incorporates:
-  //   Inport: '<Root>/theta'
-  //   Saturate: '<S103>/Saturation'
-
-  rtb_Filter_b -= rtU.theta;
-
-  // Gain: '<S153>/Filter Coefficient' incorporates:
-  //   DiscreteIntegrator: '<S145>/Filter'
-  //   Gain: '<S143>/Derivative Gain'
-  //   Sum: '<S145>/SumD'
-
-  rtb_FilterCoefficient_a = (rtP.pid_pitch.D * rtb_Filter_b -
-    rtDW.Filter_DSTATE_l) * rtP.pid_pitch.N;
-
-  // Sum: '<S159>/Sum' incorporates:
-  //   DiscreteIntegrator: '<S150>/Integrator'
-  //   Gain: '<S155>/Proportional Gain'
-
-  rtb_IntegralGain = (rtP.pid_pitch.P * rtb_Filter_b + rtDW.Integrator_DSTATE_f)
-    + rtb_FilterCoefficient_a;
-
-  // Saturate: '<S157>/Saturation' incorporates:
-  //   DeadZone: '<S142>/DeadZone'
-
-  if (rtb_IntegralGain > 35.0) {
-    // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
-    rtY.DirectLeftRudderServoAngleCmd = 35.0;
-    rtb_IntegralGain -= 35.0;
-  } else {
-    if (rtb_IntegralGain < -35.0) {
-      // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
-      rtY.DirectLeftRudderServoAngleCmd = -35.0;
-    } else {
-      // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
-      rtY.DirectLeftRudderServoAngleCmd = rtb_IntegralGain;
-    }
-
-    if (rtb_IntegralGain >= -35.0) {
-      rtb_IntegralGain = 0.0;
-    } else {
-      rtb_IntegralGain -= -35.0;
     }
   }
 
-  // End of Saturate: '<S157>/Saturation'
+  // End of DeadZone: '<S33>/DeadZone'
 
-  // RelationalOperator: '<S140>/Relational Operator' incorporates:
+  // Update for DiscreteIntegrator: '<S95>/Integrator'
-  //   Constant: '<S140>/Clamping_zero'
-
-  rtb_RelationalOperator = (rtb_IntegralGain != 0.0);
-
-  // Switch: '<S140>/Switch1' incorporates:
-  //   Constant: '<S140>/Clamping_zero'
-  //   Constant: '<S140>/Constant'
-  //   Constant: '<S140>/Constant2'
-  //   RelationalOperator: '<S140>/fix for DT propagation issue'
-
-  if (rtb_IntegralGain > 0.0) {
-    rtb_Switch1 = 1;
-  } else {
-    rtb_Switch1 = -1;
-  }
-
-  // End of Switch: '<S140>/Switch1'
-
-  // Gain: '<S147>/Integral Gain'
-  rtb_IntegralGain = rtP.pid_pitch.I * rtb_Filter_b;
-
-  // Update for DiscreteIntegrator: '<S42>/Integrator'
   rtDW.Integrator_DSTATE += rtb_Switch;
   if (rtDW.Integrator_DSTATE > 15.0) {
     rtDW.Integrator_DSTATE = 15.0;
@@ -316,83 +204,91 @@ void Controler::step()
     rtDW.Integrator_DSTATE = -15.0;
   }
 
-  // End of Update for DiscreteIntegrator: '<S42>/Integrator'
+  // End of Update for DiscreteIntegrator: '<S95>/Integrator'
 
-  // Update for DiscreteIntegrator: '<S37>/Filter'
+  // Update for DiscreteIntegrator: '<S90>/Filter'
   rtDW.Filter_DSTATE += 0.1 * rtb_FilterCoefficient;
 
-  // Update for DiscreteIntegrator: '<S96>/Integrator'
+  // Switch: '<S31>/Switch1' incorporates:
-  rtDW.Integrator_DSTATE_o += rtb_Integrator_d;
+  //   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: '<S96>/Integrator'
+  // End of Update for DiscreteIntegrator: '<S41>/Integrator'
 
-  // Update for DiscreteIntegrator: '<S91>/Filter'
+  // Update for DiscreteIntegrator: '<S36>/Filter'
-  rtDW.Filter_DSTATE_m += 0.1 * rtb_FilterCoefficient_lc;
+  rtDW.Filter_DSTATE_m += 0.1 * rtb_FilterCoefficient_a;
 
-  // Switch: '<S140>/Switch2' incorporates:
+  // Outport: '<Root>/MainThrusterSpeedCmd' incorporates:
-  //   Constant: '<S140>/Clamping_zero'
+  //   Constant: '<S1>/Constant1'
-  //   Constant: '<S140>/Constant3'
+  //   Inport: '<Root>/speed_cmd'
-  //   Constant: '<S140>/Constant4'
+  //   Product: '<S1>/Divide'
-  //   RelationalOperator: '<S140>/fix for DT propagation issue1'
 
-  if (rtb_IntegralGain > 0.0) {
+  rtY.MainThrusterSpeedCmd = rtU.speed_cmd / 0.025;
-    tmp = 1;
-  } else {
-    tmp = -1;
-  }
-
-  // Switch: '<S140>/Switch' incorporates:
-  //   Constant: '<S140>/Constant1'
-  //   Logic: '<S140>/AND3'
-  //   RelationalOperator: '<S140>/Equal1'
-  //   Switch: '<S140>/Switch2'
-
-  if (rtb_RelationalOperator && (rtb_Switch1 == tmp)) {
-    rtb_IntegralGain = 0.0;
-  }
-
-  // Update for DiscreteIntegrator: '<S150>/Integrator' incorporates:
-  //   Switch: '<S140>/Switch'
-
-  rtDW.Integrator_DSTATE_f += rtb_IntegralGain;
-  if (rtDW.Integrator_DSTATE_f > 15.0) {
-    rtDW.Integrator_DSTATE_f = 15.0;
-  } else if (rtDW.Integrator_DSTATE_f < -15.0) {
-    rtDW.Integrator_DSTATE_f = -15.0;
-  }
-
-  // End of Update for DiscreteIntegrator: '<S150>/Integrator'
-
-  // Update for DiscreteIntegrator: '<S145>/Filter'
-  rtDW.Filter_DSTATE_l += 0.1 * rtb_FilterCoefficient_a;
 
   // End of Outputs for SubSystem: '<Root>/Controler'
 
-  // Outport: '<Root>/MainThrusterSpeedCmd' incorporates:
+  // Outport: '<Root>/DirectUpperRudderServoAngleCmd'
-  //   Constant: '<S1>/Constant'
+  rtY.DirectUpperRudderServoAngleCmd = rtb_Integrator_l;
 
-  rtY.MainThrusterSpeedCmd = 100.0;
+  // Outport: '<Root>/DirectLowerRudderServoAngleCmd'
+  rtY.DirectLowerRudderServoAngleCmd = rtb_Integrator_l;
 
-  // Outport: '<Root>/DirectLowerRudderServoAngleCmd' incorporates:
+  // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
-  //   Constant: '<S1>/Constant2'
+  rtY.DirectLeftRudderServoAngleCmd = rtb_Sum;
 
-  rtY.DirectLowerRudderServoAngleCmd = 1.0;
+  // Outport: '<Root>/DirectRightRudderServoAngleCmd'
-
+  rtY.DirectRightRudderServoAngleCmd = rtb_Sum;
-  // Outport: '<Root>/DirectRightRudderServoAngleCmd' incorporates:
-  //   Constant: '<S1>/Constant4'
-
-  rtY.DirectRightRudderServoAngleCmd = 1.0;
 
   // Outport: '<Root>/heading_error'
   rtY.heading_error = mod_error;
 
-  // Outport: '<Root>/pitch_error'
+  // Outport: '<Root>/pitch_error' incorporates:
-  rtY.pitch_error = rtb_Filter_b;
+  //   Constant: '<S1>/Constant'
+
+  rtY.pitch_error = 0.0;
 
   // Outport: '<Root>/depth_error'
   rtY.depth_error = rtb_depth_error;

src/pAUV150/Controler/Controler.h

@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.115
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// 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)
@@ -39,12 +39,10 @@ class Controler final
  public:
   // Block signals and states (default storage) for system '<Root>'
   struct DW {
-    real_T Integrator_DSTATE;          // '<S42>/Integrator'
+    real_T Integrator_DSTATE;          // '<S95>/Integrator'
-    real_T Filter_DSTATE;              // '<S37>/Filter'
+    real_T Filter_DSTATE;              // '<S90>/Filter'
-    real_T Integrator_DSTATE_o;        // '<S96>/Integrator'
+    real_T Integrator_DSTATE_o;        // '<S41>/Integrator'
-    real_T Filter_DSTATE_m;            // '<S91>/Filter'
+    real_T Filter_DSTATE_m;            // '<S36>/Filter'
-    real_T Integrator_DSTATE_f;        // '<S150>/Integrator'
-    real_T Filter_DSTATE_l;            // '<S145>/Filter'
   };
 
   // External inputs (root inport signals with default storage)
@@ -86,24 +84,17 @@ class Controler final
   struct P {
     struct_n3jlyiLoC0PV1TieyhpPwD pid_depth;// Variable: pid_depth
                                                //  Referenced by:
-                                               //    '<S89>/Derivative Gain'
+                                               //    '<S34>/Derivative Gain'
-                                               //    '<S93>/Integral Gain'
+                                               //    '<S38>/Integral Gain'
-                                               //    '<S99>/Filter Coefficient'
+                                               //    '<S44>/Filter Coefficient'
-                                               //    '<S101>/Proportional Gain'
+                                               //    '<S46>/Proportional Gain'
 
     struct_n3jlyiLoC0PV1TieyhpPwD pid_heading;// Variable: pid_heading
                                                  //  Referenced by:
-                                                 //    '<S35>/Derivative Gain'
+                                                 //    '<S88>/Derivative Gain'
-                                                 //    '<S39>/Integral Gain'
+                                                 //    '<S92>/Integral Gain'
-                                                 //    '<S45>/Filter Coefficient'
+                                                 //    '<S98>/Filter Coefficient'
-                                                 //    '<S47>/Proportional Gain'
+                                                 //    '<S100>/Proportional Gain'
-
-    struct_n3jlyiLoC0PV1TieyhpPwD pid_pitch;// Variable: pid_pitch
-                                               //  Referenced by:
-                                               //    '<S143>/Derivative Gain'
-                                               //    '<S147>/Integral Gain'
-                                               //    '<S153>/Filter Coefficient'
-                                               //    '<S155>/Proportional Gain'
 
   };
 
@@ -162,7 +153,8 @@ class Controler final
 //-
 //  These blocks were eliminated from the model due to optimizations:
 //
-//  Block '<S1>/Scope' : Unused code path elimination
+//  Block '<S1>/Gain' : Eliminated nontunable gain of 1
+//  Block '<S1>/Gain1' : Eliminated nontunable gain of 1
 
 
 //-
@@ -177,179 +169,124 @@ class Controler final
 //  MATLAB hilite_system command to trace the generated code back
 //  to the parent model.  For example,
 //
-//  hilite_system('SimModel0618/Controler')    - opens subsystem SimModel0618/Controler
+//  hilite_system('SimModel0621_S4/Controler')    - opens subsystem SimModel0621_S4/Controler
-//  hilite_system('SimModel0618/Controler/Kp') - opens and selects block Kp
+//  hilite_system('SimModel0621_S4/Controler/Kp') - opens and selects block Kp
 //
 //  Here is the system hierarchy for this model
 //
-//  '<Root>' : 'SimModel0618'
+//  '<Root>' : 'SimModel0621_S4'
-//  '<S1>'   : 'SimModel0618/Controler'
+//  '<S1>'   : 'SimModel0621_S4/Controler'
-//  '<S2>'   : 'SimModel0618/Controler/MATLAB Function'
+//  '<S2>'   : 'SimModel0621_S4/Controler/MATLAB Function1'
-//  '<S3>'   : 'SimModel0618/Controler/PID Controller'
+//  '<S3>'   : 'SimModel0621_S4/Controler/PID Controller1'
-//  '<S4>'   : 'SimModel0618/Controler/PID Controller1'
+//  '<S4>'   : 'SimModel0621_S4/Controler/PID Controller3'
-//  '<S5>'   : 'SimModel0618/Controler/PID Controller2'
+//  '<S5>'   : 'SimModel0621_S4/Controler/PID Controller1/Anti-windup'
-//  '<S6>'   : 'SimModel0618/Controler/PID Controller/Anti-windup'
+//  '<S6>'   : 'SimModel0621_S4/Controler/PID Controller1/D Gain'
-//  '<S7>'   : 'SimModel0618/Controler/PID Controller/D Gain'
+//  '<S7>'   : 'SimModel0621_S4/Controler/PID Controller1/External Derivative'
-//  '<S8>'   : 'SimModel0618/Controler/PID Controller/External Derivative'
+//  '<S8>'   : 'SimModel0621_S4/Controler/PID Controller1/Filter'
-//  '<S9>'   : 'SimModel0618/Controler/PID Controller/Filter'
+//  '<S9>'   : 'SimModel0621_S4/Controler/PID Controller1/Filter ICs'
-//  '<S10>'  : 'SimModel0618/Controler/PID Controller/Filter ICs'
+//  '<S10>'  : 'SimModel0621_S4/Controler/PID Controller1/I Gain'
-//  '<S11>'  : 'SimModel0618/Controler/PID Controller/I Gain'
+//  '<S11>'  : 'SimModel0621_S4/Controler/PID Controller1/Ideal P Gain'
-//  '<S12>'  : 'SimModel0618/Controler/PID Controller/Ideal P Gain'
+//  '<S12>'  : 'SimModel0621_S4/Controler/PID Controller1/Ideal P Gain Fdbk'
-//  '<S13>'  : 'SimModel0618/Controler/PID Controller/Ideal P Gain Fdbk'
+//  '<S13>'  : 'SimModel0621_S4/Controler/PID Controller1/Integrator'
-//  '<S14>'  : 'SimModel0618/Controler/PID Controller/Integrator'
+//  '<S14>'  : 'SimModel0621_S4/Controler/PID Controller1/Integrator ICs'
-//  '<S15>'  : 'SimModel0618/Controler/PID Controller/Integrator ICs'
+//  '<S15>'  : 'SimModel0621_S4/Controler/PID Controller1/N Copy'
-//  '<S16>'  : 'SimModel0618/Controler/PID Controller/N Copy'
+//  '<S16>'  : 'SimModel0621_S4/Controler/PID Controller1/N Gain'
-//  '<S17>'  : 'SimModel0618/Controler/PID Controller/N Gain'
+//  '<S17>'  : 'SimModel0621_S4/Controler/PID Controller1/P Copy'
-//  '<S18>'  : 'SimModel0618/Controler/PID Controller/P Copy'
+//  '<S18>'  : 'SimModel0621_S4/Controler/PID Controller1/Parallel P Gain'
-//  '<S19>'  : 'SimModel0618/Controler/PID Controller/Parallel P Gain'
+//  '<S19>'  : 'SimModel0621_S4/Controler/PID Controller1/Reset Signal'
-//  '<S20>'  : 'SimModel0618/Controler/PID Controller/Reset Signal'
+//  '<S20>'  : 'SimModel0621_S4/Controler/PID Controller1/Saturation'
-//  '<S21>'  : 'SimModel0618/Controler/PID Controller/Saturation'
+//  '<S21>'  : 'SimModel0621_S4/Controler/PID Controller1/Saturation Fdbk'
-//  '<S22>'  : 'SimModel0618/Controler/PID Controller/Saturation Fdbk'
+//  '<S22>'  : 'SimModel0621_S4/Controler/PID Controller1/Sum'
-//  '<S23>'  : 'SimModel0618/Controler/PID Controller/Sum'
+//  '<S23>'  : 'SimModel0621_S4/Controler/PID Controller1/Sum Fdbk'
-//  '<S24>'  : 'SimModel0618/Controler/PID Controller/Sum Fdbk'
+//  '<S24>'  : 'SimModel0621_S4/Controler/PID Controller1/Tracking Mode'
-//  '<S25>'  : 'SimModel0618/Controler/PID Controller/Tracking Mode'
+//  '<S25>'  : 'SimModel0621_S4/Controler/PID Controller1/Tracking Mode Sum'
-//  '<S26>'  : 'SimModel0618/Controler/PID Controller/Tracking Mode Sum'
+//  '<S26>'  : 'SimModel0621_S4/Controler/PID Controller1/Tsamp - Integral'
-//  '<S27>'  : 'SimModel0618/Controler/PID Controller/Tsamp - Integral'
+//  '<S27>'  : 'SimModel0621_S4/Controler/PID Controller1/Tsamp - Ngain'
-//  '<S28>'  : 'SimModel0618/Controler/PID Controller/Tsamp - Ngain'
+//  '<S28>'  : 'SimModel0621_S4/Controler/PID Controller1/postSat Signal'
-//  '<S29>'  : 'SimModel0618/Controler/PID Controller/postSat Signal'
+//  '<S29>'  : 'SimModel0621_S4/Controler/PID Controller1/preInt Signal'
-//  '<S30>'  : 'SimModel0618/Controler/PID Controller/preInt Signal'
+//  '<S30>'  : 'SimModel0621_S4/Controler/PID Controller1/preSat Signal'
-//  '<S31>'  : 'SimModel0618/Controler/PID Controller/preSat Signal'
+//  '<S31>'  : 'SimModel0621_S4/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel'
-//  '<S32>'  : 'SimModel0618/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel'
+//  '<S32>'  : 'SimModel0621_S4/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone'
-//  '<S33>'  : 'SimModel0618/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel/Dead Zone'
+//  '<S33>'  : 'SimModel0621_S4/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
-//  '<S34>'  : 'SimModel0618/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
+//  '<S34>'  : 'SimModel0621_S4/Controler/PID Controller1/D Gain/Internal Parameters'
-//  '<S35>'  : 'SimModel0618/Controler/PID Controller/D Gain/Internal Parameters'
+//  '<S35>'  : 'SimModel0621_S4/Controler/PID Controller1/External Derivative/Error'
-//  '<S36>'  : 'SimModel0618/Controler/PID Controller/External Derivative/Error'
+//  '<S36>'  : 'SimModel0621_S4/Controler/PID Controller1/Filter/Disc. Forward Euler Filter'
-//  '<S37>'  : 'SimModel0618/Controler/PID Controller/Filter/Disc. Forward Euler Filter'
+//  '<S37>'  : 'SimModel0621_S4/Controler/PID Controller1/Filter ICs/Internal IC - Filter'
-//  '<S38>'  : 'SimModel0618/Controler/PID Controller/Filter ICs/Internal IC - Filter'
+//  '<S38>'  : 'SimModel0621_S4/Controler/PID Controller1/I Gain/Internal Parameters'
-//  '<S39>'  : 'SimModel0618/Controler/PID Controller/I Gain/Internal Parameters'
+//  '<S39>'  : 'SimModel0621_S4/Controler/PID Controller1/Ideal P Gain/Passthrough'
-//  '<S40>'  : 'SimModel0618/Controler/PID Controller/Ideal P Gain/Passthrough'
+//  '<S40>'  : 'SimModel0621_S4/Controler/PID Controller1/Ideal P Gain Fdbk/Disabled'
-//  '<S41>'  : 'SimModel0618/Controler/PID Controller/Ideal P Gain Fdbk/Disabled'
+//  '<S41>'  : 'SimModel0621_S4/Controler/PID Controller1/Integrator/Discrete'
-//  '<S42>'  : 'SimModel0618/Controler/PID Controller/Integrator/Discrete'
+//  '<S42>'  : 'SimModel0621_S4/Controler/PID Controller1/Integrator ICs/Internal IC'
-//  '<S43>'  : 'SimModel0618/Controler/PID Controller/Integrator ICs/Internal IC'
+//  '<S43>'  : 'SimModel0621_S4/Controler/PID Controller1/N Copy/Disabled'
-//  '<S44>'  : 'SimModel0618/Controler/PID Controller/N Copy/Disabled'
+//  '<S44>'  : 'SimModel0621_S4/Controler/PID Controller1/N Gain/Internal Parameters'
-//  '<S45>'  : 'SimModel0618/Controler/PID Controller/N Gain/Internal Parameters'
+//  '<S45>'  : 'SimModel0621_S4/Controler/PID Controller1/P Copy/Disabled'
-//  '<S46>'  : 'SimModel0618/Controler/PID Controller/P Copy/Disabled'
+//  '<S46>'  : 'SimModel0621_S4/Controler/PID Controller1/Parallel P Gain/Internal Parameters'
-//  '<S47>'  : 'SimModel0618/Controler/PID Controller/Parallel P Gain/Internal Parameters'
+//  '<S47>'  : 'SimModel0621_S4/Controler/PID Controller1/Reset Signal/Disabled'
-//  '<S48>'  : 'SimModel0618/Controler/PID Controller/Reset Signal/Disabled'
+//  '<S48>'  : 'SimModel0621_S4/Controler/PID Controller1/Saturation/Enabled'
-//  '<S49>'  : 'SimModel0618/Controler/PID Controller/Saturation/Enabled'
+//  '<S49>'  : 'SimModel0621_S4/Controler/PID Controller1/Saturation Fdbk/Disabled'
-//  '<S50>'  : 'SimModel0618/Controler/PID Controller/Saturation Fdbk/Disabled'
+//  '<S50>'  : 'SimModel0621_S4/Controler/PID Controller1/Sum/Sum_PID'
-//  '<S51>'  : 'SimModel0618/Controler/PID Controller/Sum/Sum_PID'
+//  '<S51>'  : 'SimModel0621_S4/Controler/PID Controller1/Sum Fdbk/Disabled'
-//  '<S52>'  : 'SimModel0618/Controler/PID Controller/Sum Fdbk/Disabled'
+//  '<S52>'  : 'SimModel0621_S4/Controler/PID Controller1/Tracking Mode/Disabled'
-//  '<S53>'  : 'SimModel0618/Controler/PID Controller/Tracking Mode/Disabled'
+//  '<S53>'  : 'SimModel0621_S4/Controler/PID Controller1/Tracking Mode Sum/Passthrough'
-//  '<S54>'  : 'SimModel0618/Controler/PID Controller/Tracking Mode Sum/Passthrough'
+//  '<S54>'  : 'SimModel0621_S4/Controler/PID Controller1/Tsamp - Integral/TsSignalSpecification'
-//  '<S55>'  : 'SimModel0618/Controler/PID Controller/Tsamp - Integral/TsSignalSpecification'
+//  '<S55>'  : 'SimModel0621_S4/Controler/PID Controller1/Tsamp - Ngain/Passthrough'
-//  '<S56>'  : 'SimModel0618/Controler/PID Controller/Tsamp - Ngain/Passthrough'
+//  '<S56>'  : 'SimModel0621_S4/Controler/PID Controller1/postSat Signal/Forward_Path'
-//  '<S57>'  : 'SimModel0618/Controler/PID Controller/postSat Signal/Forward_Path'
+//  '<S57>'  : 'SimModel0621_S4/Controler/PID Controller1/preInt Signal/Internal PreInt'
-//  '<S58>'  : 'SimModel0618/Controler/PID Controller/preInt Signal/Internal PreInt'
+//  '<S58>'  : 'SimModel0621_S4/Controler/PID Controller1/preSat Signal/Forward_Path'
-//  '<S59>'  : 'SimModel0618/Controler/PID Controller/preSat Signal/Forward_Path'
+//  '<S59>'  : 'SimModel0621_S4/Controler/PID Controller3/Anti-windup'
-//  '<S60>'  : 'SimModel0618/Controler/PID Controller1/Anti-windup'
+//  '<S60>'  : 'SimModel0621_S4/Controler/PID Controller3/D Gain'
-//  '<S61>'  : 'SimModel0618/Controler/PID Controller1/D Gain'
+//  '<S61>'  : 'SimModel0621_S4/Controler/PID Controller3/External Derivative'
-//  '<S62>'  : 'SimModel0618/Controler/PID Controller1/External Derivative'
+//  '<S62>'  : 'SimModel0621_S4/Controler/PID Controller3/Filter'
-//  '<S63>'  : 'SimModel0618/Controler/PID Controller1/Filter'
+//  '<S63>'  : 'SimModel0621_S4/Controler/PID Controller3/Filter ICs'
-//  '<S64>'  : 'SimModel0618/Controler/PID Controller1/Filter ICs'
+//  '<S64>'  : 'SimModel0621_S4/Controler/PID Controller3/I Gain'
-//  '<S65>'  : 'SimModel0618/Controler/PID Controller1/I Gain'
+//  '<S65>'  : 'SimModel0621_S4/Controler/PID Controller3/Ideal P Gain'
-//  '<S66>'  : 'SimModel0618/Controler/PID Controller1/Ideal P Gain'
+//  '<S66>'  : 'SimModel0621_S4/Controler/PID Controller3/Ideal P Gain Fdbk'
-//  '<S67>'  : 'SimModel0618/Controler/PID Controller1/Ideal P Gain Fdbk'
+//  '<S67>'  : 'SimModel0621_S4/Controler/PID Controller3/Integrator'
-//  '<S68>'  : 'SimModel0618/Controler/PID Controller1/Integrator'
+//  '<S68>'  : 'SimModel0621_S4/Controler/PID Controller3/Integrator ICs'
-//  '<S69>'  : 'SimModel0618/Controler/PID Controller1/Integrator ICs'
+//  '<S69>'  : 'SimModel0621_S4/Controler/PID Controller3/N Copy'
-//  '<S70>'  : 'SimModel0618/Controler/PID Controller1/N Copy'
+//  '<S70>'  : 'SimModel0621_S4/Controler/PID Controller3/N Gain'
-//  '<S71>'  : 'SimModel0618/Controler/PID Controller1/N Gain'
+//  '<S71>'  : 'SimModel0621_S4/Controler/PID Controller3/P Copy'
-//  '<S72>'  : 'SimModel0618/Controler/PID Controller1/P Copy'
+//  '<S72>'  : 'SimModel0621_S4/Controler/PID Controller3/Parallel P Gain'
-//  '<S73>'  : 'SimModel0618/Controler/PID Controller1/Parallel P Gain'
+//  '<S73>'  : 'SimModel0621_S4/Controler/PID Controller3/Reset Signal'
-//  '<S74>'  : 'SimModel0618/Controler/PID Controller1/Reset Signal'
+//  '<S74>'  : 'SimModel0621_S4/Controler/PID Controller3/Saturation'
-//  '<S75>'  : 'SimModel0618/Controler/PID Controller1/Saturation'
+//  '<S75>'  : 'SimModel0621_S4/Controler/PID Controller3/Saturation Fdbk'
-//  '<S76>'  : 'SimModel0618/Controler/PID Controller1/Saturation Fdbk'
+//  '<S76>'  : 'SimModel0621_S4/Controler/PID Controller3/Sum'
-//  '<S77>'  : 'SimModel0618/Controler/PID Controller1/Sum'
+//  '<S77>'  : 'SimModel0621_S4/Controler/PID Controller3/Sum Fdbk'
-//  '<S78>'  : 'SimModel0618/Controler/PID Controller1/Sum Fdbk'
+//  '<S78>'  : 'SimModel0621_S4/Controler/PID Controller3/Tracking Mode'
-//  '<S79>'  : 'SimModel0618/Controler/PID Controller1/Tracking Mode'
+//  '<S79>'  : 'SimModel0621_S4/Controler/PID Controller3/Tracking Mode Sum'
-//  '<S80>'  : 'SimModel0618/Controler/PID Controller1/Tracking Mode Sum'
+//  '<S80>'  : 'SimModel0621_S4/Controler/PID Controller3/Tsamp - Integral'
-//  '<S81>'  : 'SimModel0618/Controler/PID Controller1/Tsamp - Integral'
+//  '<S81>'  : 'SimModel0621_S4/Controler/PID Controller3/Tsamp - Ngain'
-//  '<S82>'  : 'SimModel0618/Controler/PID Controller1/Tsamp - Ngain'
+//  '<S82>'  : 'SimModel0621_S4/Controler/PID Controller3/postSat Signal'
-//  '<S83>'  : 'SimModel0618/Controler/PID Controller1/postSat Signal'
+//  '<S83>'  : 'SimModel0621_S4/Controler/PID Controller3/preInt Signal'
-//  '<S84>'  : 'SimModel0618/Controler/PID Controller1/preInt Signal'
+//  '<S84>'  : 'SimModel0621_S4/Controler/PID Controller3/preSat Signal'
-//  '<S85>'  : 'SimModel0618/Controler/PID Controller1/preSat Signal'
+//  '<S85>'  : 'SimModel0621_S4/Controler/PID Controller3/Anti-windup/Disc. Clamping Parallel'
-//  '<S86>'  : 'SimModel0618/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel'
+//  '<S86>'  : 'SimModel0621_S4/Controler/PID Controller3/Anti-windup/Disc. Clamping Parallel/Dead Zone'
-//  '<S87>'  : 'SimModel0618/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone'
+//  '<S87>'  : 'SimModel0621_S4/Controler/PID Controller3/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
-//  '<S88>'  : 'SimModel0618/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
+//  '<S88>'  : 'SimModel0621_S4/Controler/PID Controller3/D Gain/Internal Parameters'
-//  '<S89>'  : 'SimModel0618/Controler/PID Controller1/D Gain/Internal Parameters'
+//  '<S89>'  : 'SimModel0621_S4/Controler/PID Controller3/External Derivative/Error'
-//  '<S90>'  : 'SimModel0618/Controler/PID Controller1/External Derivative/Error'
+//  '<S90>'  : 'SimModel0621_S4/Controler/PID Controller3/Filter/Disc. Forward Euler Filter'
-//  '<S91>'  : 'SimModel0618/Controler/PID Controller1/Filter/Disc. Forward Euler Filter'
+//  '<S91>'  : 'SimModel0621_S4/Controler/PID Controller3/Filter ICs/Internal IC - Filter'
-//  '<S92>'  : 'SimModel0618/Controler/PID Controller1/Filter ICs/Internal IC - Filter'
+//  '<S92>'  : 'SimModel0621_S4/Controler/PID Controller3/I Gain/Internal Parameters'
-//  '<S93>'  : 'SimModel0618/Controler/PID Controller1/I Gain/Internal Parameters'
+//  '<S93>'  : 'SimModel0621_S4/Controler/PID Controller3/Ideal P Gain/Passthrough'
-//  '<S94>'  : 'SimModel0618/Controler/PID Controller1/Ideal P Gain/Passthrough'
+//  '<S94>'  : 'SimModel0621_S4/Controler/PID Controller3/Ideal P Gain Fdbk/Disabled'
-//  '<S95>'  : 'SimModel0618/Controler/PID Controller1/Ideal P Gain Fdbk/Disabled'
+//  '<S95>'  : 'SimModel0621_S4/Controler/PID Controller3/Integrator/Discrete'
-//  '<S96>'  : 'SimModel0618/Controler/PID Controller1/Integrator/Discrete'
+//  '<S96>'  : 'SimModel0621_S4/Controler/PID Controller3/Integrator ICs/Internal IC'
-//  '<S97>'  : 'SimModel0618/Controler/PID Controller1/Integrator ICs/Internal IC'
+//  '<S97>'  : 'SimModel0621_S4/Controler/PID Controller3/N Copy/Disabled'
-//  '<S98>'  : 'SimModel0618/Controler/PID Controller1/N Copy/Disabled'
+//  '<S98>'  : 'SimModel0621_S4/Controler/PID Controller3/N Gain/Internal Parameters'
-//  '<S99>'  : 'SimModel0618/Controler/PID Controller1/N Gain/Internal Parameters'
+//  '<S99>'  : 'SimModel0621_S4/Controler/PID Controller3/P Copy/Disabled'
-//  '<S100>' : 'SimModel0618/Controler/PID Controller1/P Copy/Disabled'
+//  '<S100>' : 'SimModel0621_S4/Controler/PID Controller3/Parallel P Gain/Internal Parameters'
-//  '<S101>' : 'SimModel0618/Controler/PID Controller1/Parallel P Gain/Internal Parameters'
+//  '<S101>' : 'SimModel0621_S4/Controler/PID Controller3/Reset Signal/Disabled'
-//  '<S102>' : 'SimModel0618/Controler/PID Controller1/Reset Signal/Disabled'
+//  '<S102>' : 'SimModel0621_S4/Controler/PID Controller3/Saturation/Enabled'
-//  '<S103>' : 'SimModel0618/Controler/PID Controller1/Saturation/Enabled'
+//  '<S103>' : 'SimModel0621_S4/Controler/PID Controller3/Saturation Fdbk/Disabled'
-//  '<S104>' : 'SimModel0618/Controler/PID Controller1/Saturation Fdbk/Disabled'
+//  '<S104>' : 'SimModel0621_S4/Controler/PID Controller3/Sum/Sum_PID'
-//  '<S105>' : 'SimModel0618/Controler/PID Controller1/Sum/Sum_PID'
+//  '<S105>' : 'SimModel0621_S4/Controler/PID Controller3/Sum Fdbk/Disabled'
-//  '<S106>' : 'SimModel0618/Controler/PID Controller1/Sum Fdbk/Disabled'
+//  '<S106>' : 'SimModel0621_S4/Controler/PID Controller3/Tracking Mode/Disabled'
-//  '<S107>' : 'SimModel0618/Controler/PID Controller1/Tracking Mode/Disabled'
+//  '<S107>' : 'SimModel0621_S4/Controler/PID Controller3/Tracking Mode Sum/Passthrough'
-//  '<S108>' : 'SimModel0618/Controler/PID Controller1/Tracking Mode Sum/Passthrough'
+//  '<S108>' : 'SimModel0621_S4/Controler/PID Controller3/Tsamp - Integral/TsSignalSpecification'
-//  '<S109>' : 'SimModel0618/Controler/PID Controller1/Tsamp - Integral/TsSignalSpecification'
+//  '<S109>' : 'SimModel0621_S4/Controler/PID Controller3/Tsamp - Ngain/Passthrough'
-//  '<S110>' : 'SimModel0618/Controler/PID Controller1/Tsamp - Ngain/Passthrough'
+//  '<S110>' : 'SimModel0621_S4/Controler/PID Controller3/postSat Signal/Forward_Path'
-//  '<S111>' : 'SimModel0618/Controler/PID Controller1/postSat Signal/Forward_Path'
+//  '<S111>' : 'SimModel0621_S4/Controler/PID Controller3/preInt Signal/Internal PreInt'
-//  '<S112>' : 'SimModel0618/Controler/PID Controller1/preInt Signal/Internal PreInt'
+//  '<S112>' : 'SimModel0621_S4/Controler/PID Controller3/preSat Signal/Forward_Path'
-//  '<S113>' : 'SimModel0618/Controler/PID Controller1/preSat Signal/Forward_Path'
-//  '<S114>' : 'SimModel0618/Controler/PID Controller2/Anti-windup'
-//  '<S115>' : 'SimModel0618/Controler/PID Controller2/D Gain'
-//  '<S116>' : 'SimModel0618/Controler/PID Controller2/External Derivative'
-//  '<S117>' : 'SimModel0618/Controler/PID Controller2/Filter'
-//  '<S118>' : 'SimModel0618/Controler/PID Controller2/Filter ICs'
-//  '<S119>' : 'SimModel0618/Controler/PID Controller2/I Gain'
-//  '<S120>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain'
-//  '<S121>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain Fdbk'
-//  '<S122>' : 'SimModel0618/Controler/PID Controller2/Integrator'
-//  '<S123>' : 'SimModel0618/Controler/PID Controller2/Integrator ICs'
-//  '<S124>' : 'SimModel0618/Controler/PID Controller2/N Copy'
-//  '<S125>' : 'SimModel0618/Controler/PID Controller2/N Gain'
-//  '<S126>' : 'SimModel0618/Controler/PID Controller2/P Copy'
-//  '<S127>' : 'SimModel0618/Controler/PID Controller2/Parallel P Gain'
-//  '<S128>' : 'SimModel0618/Controler/PID Controller2/Reset Signal'
-//  '<S129>' : 'SimModel0618/Controler/PID Controller2/Saturation'
-//  '<S130>' : 'SimModel0618/Controler/PID Controller2/Saturation Fdbk'
-//  '<S131>' : 'SimModel0618/Controler/PID Controller2/Sum'
-//  '<S132>' : 'SimModel0618/Controler/PID Controller2/Sum Fdbk'
-//  '<S133>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode'
-//  '<S134>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode Sum'
-//  '<S135>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Integral'
-//  '<S136>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Ngain'
-//  '<S137>' : 'SimModel0618/Controler/PID Controller2/postSat Signal'
-//  '<S138>' : 'SimModel0618/Controler/PID Controller2/preInt Signal'
-//  '<S139>' : 'SimModel0618/Controler/PID Controller2/preSat Signal'
-//  '<S140>' : 'SimModel0618/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel'
-//  '<S141>' : 'SimModel0618/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel/Dead Zone'
-//  '<S142>' : 'SimModel0618/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
-//  '<S143>' : 'SimModel0618/Controler/PID Controller2/D Gain/Internal Parameters'
-//  '<S144>' : 'SimModel0618/Controler/PID Controller2/External Derivative/Error'
-//  '<S145>' : 'SimModel0618/Controler/PID Controller2/Filter/Disc. Forward Euler Filter'
-//  '<S146>' : 'SimModel0618/Controler/PID Controller2/Filter ICs/Internal IC - Filter'
-//  '<S147>' : 'SimModel0618/Controler/PID Controller2/I Gain/Internal Parameters'
-//  '<S148>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain/Passthrough'
-//  '<S149>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain Fdbk/Disabled'
-//  '<S150>' : 'SimModel0618/Controler/PID Controller2/Integrator/Discrete'
-//  '<S151>' : 'SimModel0618/Controler/PID Controller2/Integrator ICs/Internal IC'
-//  '<S152>' : 'SimModel0618/Controler/PID Controller2/N Copy/Disabled'
-//  '<S153>' : 'SimModel0618/Controler/PID Controller2/N Gain/Internal Parameters'
-//  '<S154>' : 'SimModel0618/Controler/PID Controller2/P Copy/Disabled'
-//  '<S155>' : 'SimModel0618/Controler/PID Controller2/Parallel P Gain/Internal Parameters'
-//  '<S156>' : 'SimModel0618/Controler/PID Controller2/Reset Signal/Disabled'
-//  '<S157>' : 'SimModel0618/Controler/PID Controller2/Saturation/Enabled'
-//  '<S158>' : 'SimModel0618/Controler/PID Controller2/Saturation Fdbk/Disabled'
-//  '<S159>' : 'SimModel0618/Controler/PID Controller2/Sum/Sum_PID'
-//  '<S160>' : 'SimModel0618/Controler/PID Controller2/Sum Fdbk/Disabled'
-//  '<S161>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode/Disabled'
-//  '<S162>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode Sum/Passthrough'
-//  '<S163>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Integral/TsSignalSpecification'
-//  '<S164>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Ngain/Passthrough'
-//  '<S165>' : 'SimModel0618/Controler/PID Controller2/postSat Signal/Forward_Path'
-//  '<S166>' : 'SimModel0618/Controler/PID Controller2/preInt Signal/Internal PreInt'
-//  '<S167>' : 'SimModel0618/Controler/PID Controller2/preSat Signal/Forward_Path'
 
 #endif                                 // Controler_h_
 

src/pAUV150/Controler/Controler_data.cpp

@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.115
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// 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)
@@ -24,13 +24,13 @@
 Controler::P Controler::rtP{
   // Variable: pid_depth
   //  Referenced by:
-  //    '<S89>/Derivative Gain'
+  //    '<S34>/Derivative Gain'
-  //    '<S93>/Integral Gain'
+  //    '<S38>/Integral Gain'
-  //    '<S99>/Filter Coefficient'
+  //    '<S44>/Filter Coefficient'
-  //    '<S101>/Proportional Gain'
+  //    '<S46>/Proportional Gain'
 
   {
-    2.0,
+    10.0,
     0.0,
     0.0,
     10.0
@@ -38,29 +38,15 @@ Controler::P Controler::rtP{
 
   // Variable: pid_heading
   //  Referenced by:
-  //    '<S35>/Derivative Gain'
+  //    '<S88>/Derivative Gain'
-  //    '<S39>/Integral Gain'
+  //    '<S92>/Integral Gain'
-  //    '<S45>/Filter Coefficient'
+  //    '<S98>/Filter Coefficient'
-  //    '<S47>/Proportional Gain'
+  //    '<S100>/Proportional Gain'
-
-  {
-    1,
-    0.1,
-    0.01,
-    10.0
-  },
-
-  // Variable: pid_pitch
-  //  Referenced by:
-  //    '<S143>/Derivative Gain'
-  //    '<S147>/Integral Gain'
-  //    '<S153>/Filter Coefficient'
-  //    '<S155>/Proportional Gain'
 
   {
     0.5,
     0.0,
-    0.1,
+    0.0,
     10.0
   }
 };

src/pAUV150/Controler/Controler_private.h

@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.115
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// 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)

src/pAUV150/Controler/Controler_types.h

@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.115
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// 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)

src/pAUV150/Controler/rtGetNaN.cpp

@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.115
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// 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)

src/pAUV150/Controler/rtGetNaN.h

@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.115
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// 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)

src/pAUV150/Controler/rt_nonfinite.cpp

@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.115
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// 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)

src/pAUV150/Controler/rt_nonfinite.h

@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.115
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// 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)

src/pAUV150/Controler/rtwtypes.h

@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.115
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// 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)

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 */