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

8.21调试完成

Browse Source
This commit is contained in:
zhaojingkui
2025-08-12 18:51:50 +08:00
parent fce40a81d0
commit cb25840296
4 changed files with 84 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
missions/auv150/alpha_vehicle.moos
View File

@@ -67,11 +67,11 @@ ProcessConfig = pHelmIvP
AppTick = 4
CommsTick = 4
Behaviors = gly_1.bhv
Behaviors = gly_2.bhv
Verbose = quiet
Domain = course:0:359:360
Domain = speed:0:4:21
Domain = depth:0:100:101
Domain = speed:0:2.5:21
Domain = depth:0:10:101
//IVP_BEHAVIOR_DIR = ../../lib

120
missions/auv150/gly_2.bhv Normal file → Executable file
View File

@@ -1,60 +1,38 @@
//-------- FILE: gly_1.bhv -------------
//-------- FILE: gly_2.bhv -------------
initialize DEPLOY = false
initialize RETURN = false
set MODE = ACTIVE {
DEPLOY = true
} INACTIVE
set MODE = Const {
MODE = ACTIVE
RETURN != true
}
set MODE = RETURNING {
MODE = ACTIVE
RETURN = true
}
//----------------------------------------------
Behavior = BHV_Waypoint
Behavior = BHV_ConstantHeading
{
name = waypt_survey
pwt = 100
condition = RETURN = false
condition = DEPLOY = true
//endflag = RETURN = true
endflag = MISSION = complete
// General Behavior Parameters
name = const_hdg // example
pwt = 100 // default
condition = MODE == Const
updates = CONST_HDG_UPDATES // example
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
// Parameters specific to this behavior
basewidth = 10 // default
duration = 100
heading = 270
heading_mismatch_var = HDG_DIFF // example
peakwidth = 10 // default
summitdelta = 25 // default
}
//----------------------------------------------
@@ -62,13 +40,47 @@ Behavior = BHV_ConstantDepth
{
name = bhv_const_depth
pwt = 100
condition = DEPLOY = true
perpetual = true
duration = 100
condition = MODE == Const
updates = DEPTH_VALUE
depth = 2
duration = no-time-limit
depth = 10
peakwidth = 8
basewidth = 12
summitdelta = 10
}
//----------------------------------------------
Behavior = BHV_ConstantSpeed
{
// General Behavior Parameters
name = const_spd_transit // example
pwt = 100 // default
condition = MODE==Const
updates = CONST_SPD_UPDATES // example
// Parameters specific to this behavior
basewidth = 0.2 // default
duration = 100
speed = 0.5
speed_mismatch_var = SPEED_DIFF // example
peakwidth = 0 // default
summitdelta = 0 // default
}
//----------------------------------------------
Behavior = BHV_Waypoint
{
name = waypt_return
priority = 100
perpetual = true
updates = RETURN_UPDATES
condition = MODE==RETURNING
endflag = RETURN = false
speed = 1
radius = 3.0
nm_radius = 15.0
point = x=-50, y=-100
}

44
src/pAUV150/AUV150.cpp
View File

@@ -128,7 +128,7 @@ AUV150::AUV150() : m_sqlite("AUV150.db")
if (!m_sqlite.createTable()) {
std::cerr << "Failed to create database tables" << std::endl;
}
m_TcpSocket = new XPCTcpSocket(8150L);
m_TcpSocket = new XPCTcpSocket(7007L);
m_real_read_freq = 0;
m_real_write_freq = 0;
m_counter = 0;
@@ -259,39 +259,6 @@ bool AUV150::Iterate()
m_sqlite.insertData(m_CommandFrame);
m_sqlite.insertData(m_FeedbackFrame);
// 发送状态
// ExtU rtU;
// External outputs
// ExtY rtY;
// Notify all ExtU variables
// string sU;
// sU.append("u=" + to_string(m_control.rtU.u));
// sU.append(",v=" + to_string(m_control.rtU.v));
// sU.append(",w=" + to_string(m_control.rtU.w));
// sU.append(",phi=" + to_string(m_control.rtU.phi));
// sU.append(",theta=" + to_string(m_control.rtU.theta));
// sU.append(",psi=" + to_string(m_control.rtU.psi));
// sU.append(",x=" + to_string(m_control.rtU.x));
// sU.append(",y=" + to_string(m_control.rtU.y));
// sU.append(",z=" + to_string(m_control.rtU.z));
// sU.append(",dx=" + to_string(m_control.rtU.dx));
// sU.append(",dy=" + to_string(m_control.rtU.dy));
// sU.append(",dz=" + to_string(m_control.rtU.dz));
// sU.append(",speed_cmd=" + to_string(m_control.rtU.speed_cmd));
// sU.append(",heading_cmd=" + to_string(m_control.rtU.heading_cmd));
// sU.append(",depth_cmd=" + to_string(m_control.rtU.depth_cmd));
// Notify("rtU", sU);
// // Notify all ExtY variables
// string sY;
// sY.append("MainThrusterSpeedCmd=" + to_string(m_control.rtY.MainThrusterSpeedCmd));
// sY.append(",DirectUpperRudderServoAngleCmd=" + to_string(m_control.rtY.DirectUpperRudderServoAngleCmd));
// sY.append(",DirectLowerRudderServoAngleCmd=" + to_string(m_control.rtY.DirectLowerRudderServoAngleCmd));
// sY.append(",DirectLeftRudderServoAngleCmd=" + to_string(m_control.rtY.DirectLeftRudderServoAngleCmd));
// sY.append(",DirectRightRudderServoAngleCmd=" + to_string(m_control.rtY.DirectRightRudderServoAngleCmd));
// sY.append(",heading_error=" + to_string(m_control.rtY.heading_error));
// sY.append(",pitch_error=" + to_string(m_control.rtY.pitch_error));
// sY.append(",depth_error=" + to_string(m_control.rtY.depth_error));
// Notify("rtY", sY);
// Notify individual variables for easier parsing
Notify("rtU_u", m_control.rtU.u);
@@ -319,7 +286,6 @@ bool AUV150::Iterate()
Notify("rtY_pitch_error", m_control.rtY.pitch_error);
Notify("rtY_depth_error", m_control.rtY.depth_error);
AppCastingMOOSApp::PostReport();
return(true);
}
@@ -478,10 +444,10 @@ bool AUV150::ListenLoop()
m_control.rtU.y = m_status.y;
m_control.rtU.z = m_status.z;
m_control.step();
m_CommandFrame.rudderUp = encodeDoubleToUint8WithSign(m_control.rtY.DirectUpperRudderServoAngleCmd,30);
m_CommandFrame.rudderDown = encodeDoubleToUint8WithSign(m_control.rtY.DirectLowerRudderServoAngleCmd,30);
m_CommandFrame.rudderLeft = encodeDoubleToUint8WithSign(m_control.rtY.DirectLeftRudderServoAngleCmd,30);
m_CommandFrame.rudderRight = encodeDoubleToUint8WithSign(m_control.rtY.DirectRightRudderServoAngleCmd,30);
m_CommandFrame.rudderUp = encodeDoubleToUint8WithSign(-m_control.rtY.DirectUpperRudderServoAngleCmd,30);
m_CommandFrame.rudderDown = encodeDoubleToUint8WithSign(-m_control.rtY.DirectLowerRudderServoAngleCmd,30);
m_CommandFrame.rudderLeft = encodeDoubleToUint8WithSign(-m_control.rtY.DirectLeftRudderServoAngleCmd,30);
m_CommandFrame.rudderRight = encodeDoubleToUint8WithSign(-m_control.rtY.DirectRightRudderServoAngleCmd,30);
m_CommandFrame.mainThruster = encodeDoubleToUint8WithSign(m_control.rtY.MainThrusterSpeedCmd,100);
m_status.depth_error = m_control.rtY.depth_error;
m_status.heading_error = m_control.rtY.heading_error;

12
src/pAUV150/Controler/Controler_data.cpp
View File

@@ -30,9 +30,9 @@ Controler::P Controler::rtP{
// '<S46>/Proportional Gain'
{
1.0,
0.01,
0.01,
10.0,
0.0,
0.0,
10.0
},
@@ -44,9 +44,9 @@ Controler::P Controler::rtP{
// '<S100>/Proportional Gain'
{
10.0,
1.0,
5.0,
0.5,
0.0,
0.0,
10.0
}
};