8.12

.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

@@ -11,7 +11,7 @@ PROJECT( IVP_EXTEND )
 
 set (CMAKE_CXX_STANDARD 11)
 # Resolved conflict: using relative path for MOOSIVP_SOURCE_TREE_BASE
-set (MOOSIVP_SOURCE_TREE_BASE "../moos-ivp")
+set (MOOSIVP_SOURCE_TREE_BASE "/home/zjk/project/lib/moos-ivp")
 
 #=======================================================================
 # Set the output directories for the binary and library files

missions/alpha/alpha_shoreside.moos

@@ -29,15 +29,17 @@ ProcessConfig = pShare
   CommsTick  = 10
 
   input = route =  localhost:8085
-  output = src_name=APPCAST_REQ, route=192.168.0.101:8082
+  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.0.101:8082
+  output = src_name=uMission_action_cmd,route=192.168.1.200:8082
-  output = src_name=DEPLOY, route=192.168.0.101:8082
+  output = src_name=DEPLOY, route=192.168.1.200:8082
-  output = src_name=RETURN, route=192.168.0.101:8082
+  output = src_name=RETURN, route=192.168.1.200:8082
-  output = src_name=MOOS_MANUAL_OVERIDE, route=192.168.0.101: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
 }
 
 //------------------------------------------
@@ -89,6 +91,9 @@ ProcessConfig = pMarineViewer
   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
 }
 
 //------------------------------------------

missions/alpha/alpha_vehicle.moos

@@ -36,7 +36,7 @@ ProcessConfig = pShare
   //路径点信息
   //output = src_name = VIEW*, route = 192.168.137.7:8085
 
-  output = src_name = *, route = 192.168.137.7:8085
+  output = src_name = *, route = 192.168.1.100:8085
   //调试端输入端口
   input = route = localhost:8082
 }
@@ -104,7 +104,7 @@ ProcessConfig = pAUV150
 
   //server_host = 10.127.0.18
   //server_host = 127.0.0.1
-  server_host = 192.168.137.1
+  server_host = 192.168.1.88
   //LatOrigin  = 43.825300 
   //LongOrigin = -70.330400
 

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,124 @@
+// 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 = *, 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
+
+  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_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 = $(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,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/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 -- -$$

src/pAUV150/AUV150.cpp

@@ -12,7 +12,7 @@
 #include <iomanip>
 #include <string>
 #include <iostream>
-
+#include <arpa/inet.h>
 // #define DEBUG_
 
 using namespace std;
@@ -22,11 +22,10 @@ using namespace std;
  * @param value 待解析的int16数值
  * @return 解析后的浮点数值
  */
-float parseInt16WithSign(int16_t value) {
+float parseInt16WithSign(uint16_t value) {
     // 检查最高位是否为1(负数)
-    if (value & 0x8000) {
+    if (value >= 32768) {
-        // 负数：取补码并除以100后取负
+        return -((value-32768));
-        return -((~value + 1) & 0x7FFF);
     }
     // 正数
     return value;
@@ -75,6 +74,37 @@ static uint8_t calculateCRC(const CommandFrame_150AUV& frame) {
     }
     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;
 
@@ -150,15 +180,15 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
     if((key == "MOOS_MANUAL_OVERIDE") || (key == "MOOS_MANUAL_OVERRIDE"))
     {
         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")
@@ -173,6 +203,25 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
     {
         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);
    }
@@ -249,11 +298,11 @@ bool AUV150::OnStartUp()
         m_OriginLongitude = stod(value);
         handled = true;
     }
-    // else if(param == "controlfrequency")
+    else if(param == "controlfrequency")
-    // {
+    {
-    //     m_controlCycle = 1.0 / stod(value);
+        m_controlCycle = 1.0 / stod(value);
-    //     handled = true;
+        handled = true;
-    // }
+    }
     if(!handled)
       reportUnhandledConfigWarning(orig);
   }
@@ -278,9 +327,10 @@ void AUV150::registerVariables()
   Register("DESIRED_DEPTH", 0);
   Register("MOOS_MANUAL_OVERIDE", 0);
   Register("MOOS_MANUAL_OVERRIDE", 0);
+  Register("PZ", 0);
+  Register("LED", 0);
 }
 
-
 bool AUV150::Start()
 {
     //初始化TcpSocket
@@ -318,12 +368,12 @@ bool AUV150::ListenLoop()
         try
         {
             if((m_TcpSocket!=NULL) && m_bConnected)
-            // if((m_TcpSocket!=NULL))
             {
-                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);
                     // 处理接受到的数据
                     if(p->frameHeader != 0xEBA1)
                     {
@@ -341,8 +391,8 @@ bool AUV150::ListenLoop()
                     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;
@@ -357,11 +407,6 @@ 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.rudderDown = m_control.rtY.DirectLowerRudderServoAngleCmd + 35.0f;
-                        // m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd + 35.0f;
-                        // m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd + 35.0f;
-                        // m_CommandFrame.mainThruster = m_control.rtY.MainThrusterSpeedCmd;
                         m_CommandFrame.rudderUp = encodeDoubleToUint8WithSign(m_control.rtY.DirectUpperRudderServoAngleCmd,30); 
                         m_CommandFrame.rudderDown = encodeDoubleToUint8WithSign(m_control.rtY.DirectLowerRudderServoAngleCmd,30);
                         m_CommandFrame.rudderLeft = encodeDoubleToUint8WithSign(m_control.rtY.DirectLeftRudderServoAngleCmd,30);
@@ -370,12 +415,20 @@ bool AUV150::ListenLoop()
                         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
+                    {
+                        m_CommandFrame.rudderUp = 0; 
+                        m_CommandFrame.rudderDown = 0;
+                        m_CommandFrame.rudderLeft = 0;
+                        m_CommandFrame.rudderRight = 0;
+                        m_CommandFrame.mainThruster = 0;   
+                    }
+            }
             try
             {   
                 m_CommandFrame.crc = calculateCRC(m_CommandFrame);
+                convertCommandFrameToHostByteOrder(m_CommandFrame);
                 m_TcpSocket->iSendMessage(&m_CommandFrame, sizeof(CommandFrame_150AUV));
                 m_controlGap =  MOOSTime() - m_desiredVarTime;
                 m_controlTime = MOOSTime();
@@ -432,7 +485,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());
@@ -447,7 +500,6 @@ bool AUV150::WriteLoop()
     return true;
 }
 
-
 bool AUV150::buildReport()
 {
     auto displayWidth = [&](const std::string& str) {
@@ -526,6 +578,16 @@ 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"},
@@ -589,10 +651,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";
@@ -601,9 +663,9 @@ 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";
@@ -631,7 +693,7 @@ 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 / 10.0f;
     m_status.pitch = parseInt16WithSign(feedbackFrame.pitch)/10.0f;
@@ -639,8 +701,8 @@ bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame)
     m_status.velocityEast = parseInt16WithSign(feedbackFrame.velocityEast)/100.0f;
     m_status.velocityNorth = parseInt16WithSign(feedbackFrame.velocityNorth)/100.0f;
     m_status.velocityDown = parseInt16WithSign(feedbackFrame.velocityDown)/100.0f;
-    m_status.insLongitude = feedbackFrame.insLongitude / 1e5; 
+    m_status.insLongitude = feedbackFrame.insLongitude / 1e6; 
-    m_status.insLatitude = feedbackFrame.insLatitude / 1e5;
+    m_status.insLatitude = feedbackFrame.insLatitude / 1e6;
     m_status.insAltitude = feedbackFrame.insAltitude / 100.0f;
     m_status.dvlVelX = parseInt16WithSign(feedbackFrame.dvlVelX) / 100.0f;
     m_status.dvlVelY = parseInt16WithSign(feedbackFrame.dvlVelY) / 100.0f;

src/pAUV150/AUV150.h

@@ -36,7 +36,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,18 +52,18 @@ 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，电池状态
@@ -163,6 +163,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_;