zjk/AUV_150
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

8.12

Browse Source
This commit is contained in:
zhaojingkui
2025-08-12 10:42:30 +08:00
parent fb6874c1e5
commit 71b44dc6b0
15 changed files with 746 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.marscode/deviceInfo.json Normal file
View File

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

6
.vscode/settings.json vendored
View File

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

2
CMakeLists.txt
View File

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

15
missions/alpha/alpha_shoreside.moos
View File

@@ -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
}
//------------------------------------------

4
missions/alpha/alpha_vehicle.moos
View File

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

94
missions/auv150/alpha.bhv Normal file
View File

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

124
missions/auv150/alpha_shoreside.moos Normal file
View File

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

134
missions/auv150/alpha_vehicle.moos Normal file
View File

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

74
missions/auv150/gly_1.bhv Normal file
View File

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

74
missions/auv150/gly_2.bhv Normal file
View File

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

39
missions/auv150/kill.sh Executable file
View File

@@ -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."

68
missions/auv150/launch.sh Executable file
View File

@@ -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 -- -$$

136
src/pAUV150/AUV150.cpp
View File

@@ -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) {
// 负数：取补码并除以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")
@@ -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")
// {
// 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<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.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;

26
src/pAUV150/AUV150.h
View File

@@ -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<uint16_T> m_debug;
std::vector<std::string> m_debug_str;
private: // State variables
XPCTcpSocket* m_TcpSocket;
CMOOSThread WritingThread_;