diff --git a/.marscode/deviceInfo.json b/.marscode/deviceInfo.json new file mode 100644 index 0000000..72b4baa --- /dev/null +++ b/.marscode/deviceInfo.json @@ -0,0 +1,3 @@ +{ + "deviceId": "61b790edcb3293a5008d89c980a4ef768a223b90be5d0ae36098ddb5379d3c93" +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index a988b70..71a202d 100644 --- a/.vscode/settings.json +++ b/.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" } } \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 915ae68..a1e260a 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/missions/alpha/alpha_shoreside.moos b/missions/alpha/alpha_shoreside.moos index 5ce3247..d18a09e 100644 --- a/missions/alpha/alpha_shoreside.moos +++ b/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=DEPLOY, route=192.168.0.101:8082 - output = src_name=RETURN, route=192.168.0.101:8082 - output = src_name=MOOS_MANUAL_OVERIDE, 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.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 } //------------------------------------------ @@ -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 } //------------------------------------------ diff --git a/missions/alpha/alpha_vehicle.moos b/missions/alpha/alpha_vehicle.moos index 2cbe55f..b9135b3 100644 --- a/missions/alpha/alpha_vehicle.moos +++ b/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 diff --git a/missions/auv150/alpha.bhv b/missions/auv150/alpha.bhv new file mode 100644 index 0000000..c63240f --- /dev/null +++ b/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 + +} + diff --git a/missions/auv150/alpha_shoreside.moos b/missions/auv150/alpha_shoreside.moos new file mode 100644 index 0000000..bedef73 --- /dev/null +++ b/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 +} \ No newline at end of file diff --git a/missions/auv150/alpha_vehicle.moos b/missions/auv150/alpha_vehicle.moos new file mode 100644 index 0000000..dbaba8d --- /dev/null +++ b/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 +} \ No newline at end of file diff --git a/missions/auv150/gly_1.bhv b/missions/auv150/gly_1.bhv new file mode 100644 index 0000000..1c1865f --- /dev/null +++ b/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 +} \ No newline at end of file diff --git a/missions/auv150/gly_2.bhv b/missions/auv150/gly_2.bhv new file mode 100644 index 0000000..1c1865f --- /dev/null +++ b/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 +} \ No newline at end of file diff --git a/missions/auv150/kill.sh b/missions/auv150/kill.sh new file mode 100755 index 0000000..9b9ef34 --- /dev/null +++ b/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." \ No newline at end of file diff --git a/missions/auv150/launch.sh b/missions/auv150/launch.sh new file mode 100755 index 0000000..bcdc766 --- /dev/null +++ b/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 -- -$$ diff --git a/src/pAUV150/AUV150.cpp b/src/pAUV150/AUV150.cpp index 393eadf..72b5bce 100644 --- a/src/pAUV150/AUV150.cpp +++ b/src/pAUV150/AUV150.cpp @@ -12,7 +12,7 @@ #include #include #include - +#include // #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) { - // 负数：取补码并除以100后取负 - return -((~value + 1) & 0x7FFF); + if (value >= 32768) { + return -((value-32768)); } // 正数 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") @@ -169,10 +199,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); } @@ -249,11 +298,11 @@ bool AUV150::OnStartUp() m_OriginLongitude = stod(value); handled = true; } - // else if(param == "controlfrequency") - // { - // m_controlCycle = 1.0 / stod(value); - // handled = true; - // } + else if(param == "controlfrequency") + { + m_controlCycle = 1.0 / stod(value); + 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(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,25 +407,28 @@ 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); m_CommandFrame.rudderRight = encodeDoubleToUint8WithSign(m_control.rtY.DirectRightRudderServoAngleCmd,30); - m_CommandFrame.mainThruster = encodeDoubleToUint8WithSign(m_control.rtY.MainThrusterSpeedCmd,100); + 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 + { + 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.rudderDown-35.0f, 1) + "deg", - doubleToString(m_CommandFrame.rudderLeft-35.0f, 1) + "deg", - doubleToString(m_CommandFrame.rudderRight-35.0f, 1) + "deg", + doubleToString(m_CommandFrame.rudderUp, 1) + "deg", + doubleToString(m_CommandFrame.rudderDown, 1) + "deg", + doubleToString(m_CommandFrame.rudderLeft, 1) + "deg", + doubleToString(m_CommandFrame.rudderRight, 1) + "deg", } ); // m_msgs << "---------------------------------\n"; @@ -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.insLatitude = feedbackFrame.insLatitude / 1e5; + m_status.insLongitude = feedbackFrame.insLongitude / 1e6; + m_status.insLatitude = feedbackFrame.insLatitude / 1e6; m_status.insAltitude = feedbackFrame.insAltitude / 100.0f; m_status.dvlVelX = parseInt16WithSign(feedbackFrame.dvlVelX) / 100.0f; m_status.dvlVelY = parseInt16WithSign(feedbackFrame.dvlVelY) / 100.0f; @@ -712,4 +774,4 @@ void AUV150::postStatusUpdate(std::string prefix) Notify(prefix+"_DESIRED_RUDDER",m_status.desired_rudder); Notify(prefix+"_DESIRED_EVEVATOR",m_status.desired_elevator); Notify(prefix+"_DESIRED_THRUST",m_status.desired_thrust); -} +} \ No newline at end of file diff --git a/src/pAUV150/AUV150.h b/src/pAUV150/AUV150.h index 37a19bf..618ba77 100644 --- a/src/pAUV150/AUV150.h +++ b/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°，惯导 - int16_t roll; // 9. 横滚角 @14 int16 单位:0.01° -18000~18000 对应 -180~180°，惯导 - int16_t velocityEast; // 10. 东向速度 @16 int16 单位:0.01m/s -10000~10000 对应 -100~100m/s，惯导 - int16_t velocityNorth; // 11. 北向速度 @18 int16 单位:0.01m/s -10000~10000 对应 -100~100m/s，惯导 - int16_t velocityDown; // 12. 垂向速度 @20 int16 单位:0.01m/s -10000~10000 对应 -100~100m/s，惯导 + uint16_t pitch; // 8. 俯仰角 @12 int16 单位:0.01° -18000~18000 对应 -180~180°，惯导 + uint16_t roll; // 9. 横滚角 @14 int16 单位:0.01° -18000~18000 对应 -180~180°，惯导 + uint16_t velocityEast; // 10. 东向速度 @16 int16 单位:0.01m/s -10000~10000 对应 -100~100m/s，惯导 + uint16_t velocityNorth; // 11. 北向速度 @18 int16 单位:0.01m/s -10000~10000 对应 -100~100m/s，惯导 + uint16_t velocityDown; // 12. 垂向速度 @20 int16 单位:0.01m/s -10000~10000 对应 -100~100m/s，惯导 uint32_t insLongitude; // 13. 惯导：经度 @22 Uint32 单位:°，精度:360/2^32 -180~180，惯导 uint32_t insLatitude; // 14. 惯导：纬度 @26 Uint32 单位:°，精度:180/2^32 -90~90，惯导 - int16_t insAltitude; // 15. 惯导：高度 @30 int16 单位:0.01m 无符号，惯导 - int16_t dvlVelX; // 16. DVL:横向速度 @32 int16 单位:0.01m/s DVL - int16_t dvlVelY; // 17. DVL:纵向速度 @34 int16 单位:0.01m/s DVL - int16_t dvlVelZ; // 18. DVL:天向速度 @36 int16 单位:0.01m/s DVL - int16_t thrusterRPM; // 19. 主推进器实际转速(x1) @38 int16 单位:rpm -4000~4000，推进器 + uint16_t insAltitude; // 15. 惯导：高度 @30 int16 单位:0.01m 无符号，惯导 + uint16_t dvlVelX; // 16. DVL:横向速度 @32 int16 单位:0.01m/s DVL + uint16_t dvlVelY; // 17. DVL:纵向速度 @34 int16 单位:0.01m/s DVL + uint16_t dvlVelZ; // 18. DVL:天向速度 @36 int16 单位:0.01m/s DVL + uint16_t thrusterRPM; // 19. 主推进器实际转速(x1) @38 int16 单位:rpm -4000~4000，推进器 uint8_t ledSwitch; // 20. 灯的开关 @40 Uint8 单位:— 0x00: 关，0x01: 开，灯 uint16_t batteryVoltage; // 21. 电池电压反馈 @41 Uint16 单位:mV 约24000，电池状态 uint8_t batteryLevel; // 22. 电池电量反馈 @43 Uint8 单位:% 0~100，电池状态 @@ -163,6 +163,10 @@ public: // 状态发布函数 void postStatusUpdate(std::string prefix); + //DEBUG + std::vector m_debug; + std::vector m_debug_str; + private: // State variables XPCTcpSocket* m_TcpSocket; CMOOSThread WritingThread_; diff --git a/src/pAUV150/Controler/rtwtypes.h b/src/pAUV150/Controler/rtwtypes.h index 3382459..8d4a12d 100644 --- a/src/pAUV150/Controler/rtwtypes.h +++ b/src/pAUV150/Controler/rtwtypes.h @@ -103,4 +103,4 @@ typedef void * pointer_T; // File trailer for generated code. // // [EOF] -// +// \ No newline at end of file