8.21调试完成

missions/auv150/alpha_vehicle.moos

@@ -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     = speed:0:2.5:21
-  Domain     = depth:0:100:101
+  Domain     = depth:0:10:101
 
   //IVP_BEHAVIOR_DIR = ../../lib
 

missions/auv150/gly_2.bhv

@@ -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
+// General Behavior Parameters
-  pwt       = 100
+name = const_hdg // example
-  condition = RETURN = false
+pwt = 100 // default
-  condition = DEPLOY = true
+condition  = MODE == Const
-  //endflag   = RETURN = true
+updates = CONST_HDG_UPDATES // example
-  endflag    = MISSION = complete
 
-	configflag = CRUISE_SPD = $[SPEED]
+// Parameters specific to this behavior
-	//configflag = OSPOS = $[OSX],$[OSY]
+basewidth = 10 // default
-
+duration = 100
-	activeflag = INFO=$[OWNSHIP]
+heading = 270
-	activeflag = INFO=$[BHVNAME]
+heading_mismatch_var = HDG_DIFF // example
-	activeflag = INFO=$[BHVTYPE]
+peakwidth = 10 // default
-
+summitdelta = 25 // default
-	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
 }
 
 //----------------------------------------------
@@ -62,13 +40,47 @@ Behavior = BHV_ConstantDepth
 {
   name      = bhv_const_depth
   pwt       = 100
-  condition = DEPLOY = true
+  duration  = 100
-  perpetual  = true
+  condition = MODE == Const
   updates   = DEPTH_VALUE
 
-        depth = 2
+        depth = 10
-    duration  = no-time-limit
     peakwidth = 8
     basewidth = 12
-    summitdelta = 10
+  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
+}
+

src/pAUV150/AUV150.cpp

@@ -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.rudderUp = encodeDoubleToUint8WithSign(-m_control.rtY.DirectUpperRudderServoAngleCmd,30); 
-                        m_CommandFrame.rudderDown = encodeDoubleToUint8WithSign(m_control.rtY.DirectLowerRudderServoAngleCmd,30);
+                        m_CommandFrame.rudderDown = encodeDoubleToUint8WithSign(-m_control.rtY.DirectLowerRudderServoAngleCmd,30);
-                        m_CommandFrame.rudderLeft = encodeDoubleToUint8WithSign(m_control.rtY.DirectLeftRudderServoAngleCmd,30);
+                        m_CommandFrame.rudderLeft = encodeDoubleToUint8WithSign(-m_control.rtY.DirectLeftRudderServoAngleCmd,30);
-                        m_CommandFrame.rudderRight = encodeDoubleToUint8WithSign(m_control.rtY.DirectRightRudderServoAngleCmd,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;

src/pAUV150/Controler/Controler_data.cpp

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