From 095b7559af83449bb292bd7beba464ee6ae2b47a Mon Sep 17 00:00:00 2001
From: zjk <1553836110@qq.com>
Date: Sun, 29 Jun 2025 11:21:58 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86=E6=8E=A7=E5=88=B6?=
 =?UTF-8?q?=E5=99=A8?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 missions/alder/alder.moos                 |   4 +-
 src/pAUV150/AUV150.cpp                    |   4 +-
 src/pAUV150/Controler/Controler.cpp       | 808 +++++++++++++++-------
 src/pAUV150/Controler/Controler.h         | 430 ++++++------
 src/pAUV150/Controler/Controler_data.cpp  |  46 +-
 src/pAUV150/Controler/Controler_private.h |   4 +-
 src/pAUV150/Controler/Controler_types.h   |   4 +-
 src/pAUV150/Controler/rtGetNaN.cpp        |   4 +-
 src/pAUV150/Controler/rtGetNaN.h          |   4 +-
 src/pAUV150/Controler/rt_nonfinite.cpp    |   4 +-
 src/pAUV150/Controler/rt_nonfinite.h      |   4 +-
 src/pAUV150/Controler/rtmodel.h           |  44 ++
 src/pAUV150/Controler/rtwtypes.h          |   4 +-
 13 files changed, 891 insertions(+), 473 deletions(-)
 create mode 100644 src/pAUV150/Controler/rtmodel.h

diff --git a/missions/alder/alder.moos b/missions/alder/alder.moos
index 71f7cec..fa71112 100644
--- a/missions/alder/alder.moos
+++ b/missions/alder/alder.moos
@@ -21,7 +21,7 @@ ProcessConfig = ANTLER
   Run = pMarineViewer	@ NewConsole = false
   Run = uProcessWatch @ NewConsole = false
   Run = pHelmIvP      @ NewConsole = false
-  Run = pAUV150       @ NewConsole = true
+  Run = pAUV150       @ NewConsole = false
   //Run = pMotionControler @ NewConsole = false
 }
 
@@ -164,7 +164,7 @@ ProcessConfig = pAUV150
 
   //server_host = 10.127.0.18
   //server_host = 127.0.0.1
-  server_host = 172.18.96.1
+  server_host = 172.24.224.1
   LatOrigin  = 43.825300 
   LongOrigin = -70.330400 
 
diff --git a/src/pAUV150/AUV150.cpp b/src/pAUV150/AUV150.cpp
index e948d17..f77f9ec 100644
--- a/src/pAUV150/AUV150.cpp
+++ b/src/pAUV150/AUV150.cpp
@@ -13,7 +13,7 @@
 #include <string>
 #include <iostream>
 
-#define DEBUG_
+// #define DEBUG_
 
 using namespace std;
 
@@ -145,7 +145,7 @@ bool AUV150::Iterate()
         m_geodesy.LatLong2LocalGrid(m_status.insLatitude, m_status.insLongitude, m_pos_x, m_pos_y);
         postStatusUpdate("NAV");
     #endif
-    // AppCastingMOOSApp::PostReport();
+    AppCastingMOOSApp::PostReport();
     return(true);
 }
 
diff --git a/src/pAUV150/Controler/Controler.cpp b/src/pAUV150/Controler/Controler.cpp
index 218793f..4580e04 100644
--- a/src/pAUV150/Controler/Controler.cpp
+++ b/src/pAUV150/Controler/Controler.cpp
@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.77
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// C/C++ source code generated on : Thu Jun 26 20:56:30 2025
 //
 // Target selection: ert.tlc
 // Embedded hardware selection: Intel->x86-64 (Linux 64)
@@ -27,25 +27,365 @@
 void Controler::step()
 {
   real_T mod_error;
+  real_T rtb_Delay1;
+  real_T rtb_Delay1_d;
+  real_T rtb_Delay5;
+  real_T rtb_Delay6;
+  real_T rtb_Delay6_c;
+  real_T rtb_Delay7;
+  real_T rtb_Delay7_c;
   real_T rtb_FilterCoefficient;
-  real_T rtb_FilterCoefficient_a;
-  real_T rtb_FilterCoefficient_lc;
-  real_T rtb_Filter_b;
+  real_T rtb_FilterCoefficient_g;
+  real_T rtb_FilterCoefficient_l;
+  real_T rtb_Filter_k;
   real_T rtb_IntegralGain;
   real_T rtb_Integrator_d;
   real_T rtb_Switch;
   real_T rtb_depth_error;
+  real_T rtb_pitch_error;
+  real_T rtb_pos_i1;
+  real_T rtb_pos_i1_k;
+  real_T rtb_pos_i2;
+  real_T rtb_pos_i2_l;
+  real_T rtb_pos_i3;
+  real_T rtb_ref_i1;
+  real_T rtb_ref_i1_p;
+  real_T rtb_ref_i2;
+  real_T rtb_ref_i2_d;
+  real_T rtb_ref_i3;
   int8_T rtb_Switch1;
   int8_T tmp;
+  int8_T tmp_0;
   boolean_T rtb_RelationalOperator;
   boolean_T rtb_fixforDTpropagationissue;
 
   // Outputs for Atomic SubSystem: '<Root>/Controler'
+  // Delay: '<S6>/pos_i-1'
+  rtb_pos_i1 = rtDW.pos_i1_DSTATE;
+
+  // Delay: '<S6>/pos_i-2'
+  rtb_pos_i2 = rtDW.pos_i2_DSTATE;
+
+  // Delay: '<S6>/pos_i-3'
+  rtb_pos_i3 = rtDW.pos_i3_DSTATE;
+
+  // Delay: '<S6>/ref_i-1'
+  rtb_ref_i1 = rtDW.ref_i1_DSTATE;
+
+  // Delay: '<S6>/ref_i-2'
+  rtb_ref_i2 = rtDW.ref_i2_DSTATE;
+
+  // Delay: '<S6>/ref_i-3'
+  rtb_ref_i3 = rtDW.ref_i3_DSTATE;
+
+  // Delay: '<S170>/Delay1'
+  rtb_Delay1 = rtDW.Delay1_DSTATE;
+
+  // Delay: '<S170>/Delay6'
+  rtb_Delay6 = rtDW.Delay6_DSTATE;
+
+  // Delay: '<S170>/Delay7'
+  rtb_Delay7 = rtDW.Delay7_DSTATE;
+
+  // Delay: '<S170>/Delay5'
+  rtb_Delay5 = rtDW.Delay5_DSTATE;
+
+  // Sum: '<S1>/Sum1' incorporates:
+  //   Delay: '<S170>/Delay1'
+  //   Delay: '<S170>/Delay10'
+  //   Delay: '<S170>/Delay2'
+  //   Delay: '<S170>/Delay3'
+  //   Delay: '<S170>/Delay4'
+  //   Delay: '<S170>/Delay5'
+  //   Delay: '<S170>/Delay6'
+  //   Delay: '<S170>/Delay7'
+  //   Delay: '<S170>/Delay8'
+  //   Delay: '<S170>/Delay9'
+  //   Delay: '<S6>/pos_i-1'
+  //   Delay: '<S6>/pos_i-2'
+  //   Delay: '<S6>/pos_i-3'
+  //   Delay: '<S6>/pos_i-4'
+  //   Delay: '<S6>/ref_i-1'
+  //   Delay: '<S6>/ref_i-2'
+  //   Delay: '<S6>/ref_i-3'
+  //   Delay: '<S6>/ref_i-4'
+  //   Gain: '<S170>/Gain1'
+  //   Gain: '<S170>/Gain10'
+  //   Gain: '<S170>/Gain11'
+  //   Gain: '<S170>/Gain2'
+  //   Gain: '<S170>/Gain3'
+  //   Gain: '<S170>/Gain4'
+  //   Gain: '<S170>/Gain5'
+  //   Gain: '<S170>/Gain6'
+  //   Gain: '<S170>/Gain7'
+  //   Gain: '<S170>/Gain9'
+  //   Gain: '<S6>/Compensation Gain'
+  //   Gain: '<S6>/Gain'
+  //   Gain: '<S6>/Gain1'
+  //   Gain: '<S6>/Gain10'
+  //   Gain: '<S6>/Gain11'
+  //   Gain: '<S6>/Gain12'
+  //   Gain: '<S6>/Gain13'
+  //   Gain: '<S6>/Gain14'
+  //   Gain: '<S6>/Gain15'
+  //   Gain: '<S6>/Gain16'
+  //   Gain: '<S6>/Gain17'
+  //   Gain: '<S6>/Gain18'
+  //   Gain: '<S6>/Gain19'
+  //   Gain: '<S6>/Gain2'
+  //   Gain: '<S6>/Gain3'
+  //   Gain: '<S6>/Gain4'
+  //   Gain: '<S6>/Gain5'
+  //   Gain: '<S6>/Gain6'
+  //   Gain: '<S6>/Gain7'
+  //   Gain: '<S6>/Gain8'
+  //   Gain: '<S6>/Gain9'
+  //   Inport: '<Root>/depth_cmd'
+  //   Inport: '<Root>/z'
+  //   Sum: '<S170>/Add2'
+  //   Sum: '<S1>/Add'
+  //   Sum: '<S6>/Add'
+  //   Sum: '<S6>/Add1'
+  //   Sum: '<S6>/predict_error_(i+2)'
+
+  rtb_depth_error = rtU.z - ((rtU.depth_cmd - ((((((((((((((((((((rtU.z -
+    4.96546716 * rtDW.Delay1_DSTATE) - -9.86401854 * rtDW.Delay6_DSTATE) -
+    9.7928555 * rtDW.Delay7_DSTATE) - -4.85552402 * rtDW.Delay5_DSTATE) -
+    0.9612199 * rtDW.Delay8_DSTATE) - -0.0031985800000000021 *
+    rtDW.Delay10_DSTATE) - 0.0095957400000000071 * rtDW.Delay9_DSTATE) -
+    -0.0031985800000000073 * rtDW.Delay4_DSTATE) - -0.0095957399999999984 *
+    rtDW.Delay3_DSTATE) - 0.0063971600000000007 * rtDW.Delay2_DSTATE) +
+    (((((((((4.96546716 * rtU.z + -9.86401854 * rtDW.pos_i1_DSTATE) + 9.7928555 *
+    rtDW.pos_i2_DSTATE) + -4.85552402 * rtDW.pos_i3_DSTATE) + 0.9612199 *
+          rtDW.pos_i4_DSTATE) + 0.0063971600000000007 * rtU.depth_cmd) +
+        -0.0095957399999999984 * rtDW.ref_i1_DSTATE) + -0.0031985800000000073 *
+       rtDW.ref_i2_DSTATE) + 0.0095957400000000071 * rtDW.ref_i3_DSTATE) +
+     -0.0031985800000000021 * rtDW.ref_i4_DSTATE)) * 4.96546716 + -9.86401854 *
+    rtU.z) + 9.7928555 * rtDW.pos_i1_DSTATE) + -4.85552402 * rtDW.pos_i2_DSTATE)
+    + 0.9612199 * rtDW.pos_i3_DSTATE) + 0.0063971600000000007 * rtU.depth_cmd) +
+    -0.0095957399999999984 * rtU.depth_cmd) + -0.0031985800000000073 *
+    rtDW.ref_i1_DSTATE) + 0.0095957400000000071 * rtDW.ref_i2_DSTATE) +
+    -0.0031985800000000021 * rtDW.ref_i3_DSTATE)) * rtP.gamma_depth +
+    rtU.depth_cmd);
+
+  // Gain: '<S101>/Filter Coefficient' incorporates:
+  //   DiscreteIntegrator: '<S93>/Filter'
+  //   Gain: '<S91>/Derivative Gain'
+  //   Sum: '<S93>/SumD'
+
+  rtb_FilterCoefficient = (rtP.pid_depth.D * rtb_depth_error -
+    rtDW.Filter_DSTATE) * rtP.pid_depth.N;
+
+  // Sum: '<S107>/Sum' incorporates:
+  //   DiscreteIntegrator: '<S98>/Integrator'
+  //   Gain: '<S103>/Proportional Gain'
+
+  rtb_Filter_k = (rtP.pid_depth.P * rtb_depth_error + rtDW.Integrator_DSTATE) +
+    rtb_FilterCoefficient;
+
+  // Saturate: '<S105>/Saturation'
+  if (rtb_Filter_k > 30.0) {
+    rtb_pitch_error = 30.0;
+  } else if (rtb_Filter_k < -30.0) {
+    rtb_pitch_error = -30.0;
+  } else {
+    rtb_pitch_error = rtb_Filter_k;
+  }
+
+  // Sum: '<S1>/Sum2' incorporates:
+  //   Inport: '<Root>/theta'
+  //   Saturate: '<S105>/Saturation'
+
+  rtb_pitch_error -= rtU.theta;
+
+  // Gain: '<S155>/Filter Coefficient' incorporates:
+  //   DiscreteIntegrator: '<S147>/Filter'
+  //   Gain: '<S145>/Derivative Gain'
+  //   Sum: '<S147>/SumD'
+
+  rtb_FilterCoefficient_g = (rtP.pid_pitch.D * rtb_pitch_error -
+    rtDW.Filter_DSTATE_l) * rtP.pid_pitch.N;
+
+  // Sum: '<S161>/Sum' incorporates:
+  //   DiscreteIntegrator: '<S152>/Integrator'
+  //   Gain: '<S157>/Proportional Gain'
+
+  rtb_Integrator_d = (rtP.pid_pitch.P * rtb_pitch_error +
+                      rtDW.Integrator_DSTATE_f) + rtb_FilterCoefficient_g;
+
+  // Saturate: '<S159>/Saturation' incorporates:
+  //   DeadZone: '<S144>/DeadZone'
+
+  if (rtb_Integrator_d > 35.0) {
+    // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
+    rtY.DirectLeftRudderServoAngleCmd = 35.0;
+    rtb_Integrator_d -= 35.0;
+  } else {
+    if (rtb_Integrator_d < -35.0) {
+      // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
+      rtY.DirectLeftRudderServoAngleCmd = -35.0;
+    } else {
+      // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
+      rtY.DirectLeftRudderServoAngleCmd = rtb_Integrator_d;
+    }
+
+    if (rtb_Integrator_d >= -35.0) {
+      rtb_Integrator_d = 0.0;
+    } else {
+      rtb_Integrator_d -= -35.0;
+    }
+  }
+
+  // End of Saturate: '<S159>/Saturation'
+
+  // RelationalOperator: '<S142>/Relational Operator' incorporates:
+  //   Constant: '<S142>/Clamping_zero'
+
+  rtb_RelationalOperator = (rtb_Integrator_d != 0.0);
+
+  // RelationalOperator: '<S142>/fix for DT propagation issue' incorporates:
+  //   Constant: '<S142>/Clamping_zero'
+
+  rtb_fixforDTpropagationissue = (rtb_Integrator_d > 0.0);
+
+  // Gain: '<S149>/Integral Gain'
+  rtb_Integrator_d = rtP.pid_pitch.I * rtb_pitch_error;
+
+  // Switch: '<S142>/Switch1' incorporates:
+  //   Constant: '<S142>/Constant'
+  //   Constant: '<S142>/Constant2'
+
+  if (rtb_fixforDTpropagationissue) {
+    tmp = 1;
+  } else {
+    tmp = -1;
+  }
+
+  // Switch: '<S142>/Switch2' incorporates:
+  //   Constant: '<S142>/Clamping_zero'
+  //   Constant: '<S142>/Constant3'
+  //   Constant: '<S142>/Constant4'
+  //   RelationalOperator: '<S142>/fix for DT propagation issue1'
+
+  if (rtb_Integrator_d > 0.0) {
+    tmp_0 = 1;
+  } else {
+    tmp_0 = -1;
+  }
+
+  // Switch: '<S142>/Switch' incorporates:
+  //   Constant: '<S142>/Constant1'
+  //   Logic: '<S142>/AND3'
+  //   RelationalOperator: '<S142>/Equal1'
+  //   Switch: '<S142>/Switch1'
+  //   Switch: '<S142>/Switch2'
+
+  if (rtb_RelationalOperator && (tmp == tmp_0)) {
+    rtb_Switch = 0.0;
+  } else {
+    rtb_Switch = rtb_Integrator_d;
+  }
+
+  // End of Switch: '<S142>/Switch'
+
+  // DeadZone: '<S90>/DeadZone'
+  if (rtb_Filter_k > 30.0) {
+    rtb_Filter_k -= 30.0;
+  } else if (rtb_Filter_k >= -30.0) {
+    rtb_Filter_k = 0.0;
+  } else {
+    rtb_Filter_k -= -30.0;
+  }
+
+  // End of DeadZone: '<S90>/DeadZone'
+
+  // Gain: '<S95>/Integral Gain'
+  rtb_Integrator_d = rtP.pid_depth.I * rtb_depth_error;
+
+  // Delay: '<S7>/pos_i-1'
+  rtb_pos_i1_k = rtDW.pos_i1_DSTATE_f;
+
+  // Delay: '<S7>/pos_i-2'
+  rtb_pos_i2_l = rtDW.pos_i2_DSTATE_n;
+
+  // Delay: '<S7>/ref_i-1'
+  rtb_ref_i1_p = rtDW.ref_i1_DSTATE_c;
+
+  // Delay: '<S7>/ref_i-2'
+  rtb_ref_i2_d = rtDW.ref_i2_DSTATE_m;
+
+  // Delay: '<S171>/Delay1'
+  rtb_Delay1_d = rtDW.Delay1_DSTATE_o;
+
+  // Delay: '<S171>/Delay6'
+  rtb_Delay6_c = rtDW.Delay6_DSTATE_k;
+
+  // Delay: '<S171>/Delay7'
+  rtb_Delay7_c = rtDW.Delay7_DSTATE_o;
+
   // MATLAB Function: '<S1>/MATLAB Function' incorporates:
+  //   Delay: '<S171>/Delay1'
+  //   Delay: '<S171>/Delay2'
+  //   Delay: '<S171>/Delay3'
+  //   Delay: '<S171>/Delay4'
+  //   Delay: '<S171>/Delay5'
+  //   Delay: '<S171>/Delay6'
+  //   Delay: '<S171>/Delay7'
+  //   Delay: '<S171>/Delay9'
+  //   Delay: '<S7>/pos_i-1'
+  //   Delay: '<S7>/pos_i-2'
+  //   Delay: '<S7>/pos_i-3'
+  //   Delay: '<S7>/ref_i-1'
+  //   Delay: '<S7>/ref_i-2'
+  //   Delay: '<S7>/ref_i-3'
+  //   Gain: '<S171>/Gain1'
+  //   Gain: '<S171>/Gain10'
+  //   Gain: '<S171>/Gain11'
+  //   Gain: '<S171>/Gain2'
+  //   Gain: '<S171>/Gain3'
+  //   Gain: '<S171>/Gain4'
+  //   Gain: '<S171>/Gain7'
+  //   Gain: '<S171>/Gain9'
+  //   Gain: '<S7>/Compensation Gain'
+  //   Gain: '<S7>/Gain'
+  //   Gain: '<S7>/Gain1'
+  //   Gain: '<S7>/Gain10'
+  //   Gain: '<S7>/Gain11'
+  //   Gain: '<S7>/Gain12'
+  //   Gain: '<S7>/Gain14'
+  //   Gain: '<S7>/Gain15'
+  //   Gain: '<S7>/Gain16'
+  //   Gain: '<S7>/Gain2'
+  //   Gain: '<S7>/Gain3'
+  //   Gain: '<S7>/Gain4'
+  //   Gain: '<S7>/Gain5'
+  //   Gain: '<S7>/Gain6'
+  //   Gain: '<S7>/Gain7'
+  //   Gain: '<S7>/Gain8'
+  //   Gain: '<S7>/Gain9'
   //   Inport: '<Root>/heading_cmd'
   //   Inport: '<Root>/psi'
+  //   Sum: '<S171>/Add2'
+  //   Sum: '<S1>/Add1'
+  //   Sum: '<S7>/Add'
+  //   Sum: '<S7>/Add1'
+  //   Sum: '<S7>/predict_error_(i+2)'
 
-  mod_error = rtU.heading_cmd - rtU.psi;
+  mod_error = ((rtU.heading_cmd - ((((((((((((((((rtU.psi - 3.828503 *
+    rtDW.Delay1_DSTATE_o) - -5.5210193 * rtDW.Delay6_DSTATE_k) - 3.5572363 *
+    rtDW.Delay7_DSTATE_o) - -0.86472 * rtDW.Delay5_DSTATE_d) -
+    -7.589415207398531E-19 * rtDW.Delay9_DSTATE_m) - -0.0032962999999999985 *
+    rtDW.Delay4_DSTATE_a) - 0.0072993 * rtDW.Delay3_DSTATE_f) -
+    -0.0040030000000000005 * rtDW.Delay2_DSTATE_g) + (((((((3.828503 * rtU.psi +
+    -5.5210193 * rtDW.pos_i1_DSTATE_f) + 3.5572363 * rtDW.pos_i2_DSTATE_n) +
+    -0.86472 * rtDW.pos_i3_DSTATE_b) + -0.0040030000000000005 * rtU.heading_cmd)
+    + 0.0072993 * rtDW.ref_i1_DSTATE_c) + -0.0032962999999999985 *
+    rtDW.ref_i2_DSTATE_m) + -7.589415207398531E-19 * rtDW.ref_i3_DSTATE_i)) *
+    3.828503 + -5.5210193 * rtU.psi) + 3.5572363 * rtDW.pos_i1_DSTATE_f) +
+    -0.86472 * rtDW.pos_i2_DSTATE_n) + -0.0040030000000000005 * rtU.heading_cmd)
+    + 0.0072993 * rtU.heading_cmd) + -0.0032962999999999985 *
+    rtDW.ref_i1_DSTATE_c) + -7.589415207398531E-19 * rtDW.ref_i2_DSTATE_m)) *
+               rtP.gamma_heading + rtU.heading_cmd) - rtU.psi;
   if (!std::isnan(mod_error)) {
     if (std::isinf(mod_error)) {
       mod_error = (rtNaN);
@@ -67,23 +407,23 @@ void Controler::step()
 
   // End of MATLAB Function: '<S1>/MATLAB Function'
 
-  // Gain: '<S45>/Filter Coefficient' incorporates:
-  //   DiscreteIntegrator: '<S37>/Filter'
-  //   Gain: '<S35>/Derivative Gain'
-  //   Sum: '<S37>/SumD'
+  // Gain: '<S47>/Filter Coefficient' incorporates:
+  //   DiscreteIntegrator: '<S39>/Filter'
+  //   Gain: '<S37>/Derivative Gain'
+  //   Sum: '<S39>/SumD'
 
-  rtb_FilterCoefficient = (rtP.pid_heading.D * mod_error - rtDW.Filter_DSTATE) *
-    rtP.pid_heading.N;
+  rtb_FilterCoefficient_l = (rtP.pid_heading.D * mod_error -
+    rtDW.Filter_DSTATE_m) * rtP.pid_heading.N;
 
-  // Sum: '<S51>/Sum' incorporates:
-  //   DiscreteIntegrator: '<S42>/Integrator'
-  //   Gain: '<S47>/Proportional Gain'
+  // Sum: '<S53>/Sum' incorporates:
+  //   DiscreteIntegrator: '<S44>/Integrator'
+  //   Gain: '<S49>/Proportional Gain'
 
-  rtb_IntegralGain = (rtP.pid_heading.P * mod_error + rtDW.Integrator_DSTATE) +
-    rtb_FilterCoefficient;
+  rtb_IntegralGain = (rtP.pid_heading.P * mod_error + rtDW.Integrator_DSTATE_b)
+    + rtb_FilterCoefficient_l;
 
-  // Saturate: '<S49>/Saturation' incorporates:
-  //   DeadZone: '<S34>/DeadZone'
+  // Saturate: '<S51>/Saturation' incorporates:
+  //   DeadZone: '<S36>/DeadZone'
 
   if (rtb_IntegralGain > 35.0) {
     // Outport: '<Root>/DirectUpperRudderServoAngleCmd'
@@ -105,271 +445,257 @@ void Controler::step()
     }
   }
 
-  // End of Saturate: '<S49>/Saturation'
+  // End of Saturate: '<S51>/Saturation'
 
-  // RelationalOperator: '<S32>/Relational Operator' incorporates:
-  //   Constant: '<S32>/Clamping_zero'
+  // RelationalOperator: '<S34>/Relational Operator' incorporates:
+  //   Constant: '<S34>/Clamping_zero'
 
   rtb_RelationalOperator = (rtb_IntegralGain != 0.0);
 
-  // RelationalOperator: '<S32>/fix for DT propagation issue' incorporates:
-  //   Constant: '<S32>/Clamping_zero'
+  // Switch: '<S34>/Switch1' incorporates:
+  //   Constant: '<S34>/Clamping_zero'
+  //   Constant: '<S34>/Constant'
+  //   Constant: '<S34>/Constant2'
+  //   RelationalOperator: '<S34>/fix for DT propagation issue'
 
-  rtb_fixforDTpropagationissue = (rtb_IntegralGain > 0.0);
+  if (rtb_IntegralGain > 0.0) {
+    rtb_Switch1 = 1;
+  } else {
+    rtb_Switch1 = -1;
+  }
 
-  // Gain: '<S39>/Integral Gain'
+  // End of Switch: '<S34>/Switch1'
+
+  // Gain: '<S41>/Integral Gain'
   rtb_IntegralGain = rtP.pid_heading.I * mod_error;
 
-  // Switch: '<S32>/Switch1' incorporates:
-  //   Constant: '<S32>/Constant'
-  //   Constant: '<S32>/Constant2'
-
-  if (rtb_fixforDTpropagationissue) {
-    tmp = 1;
-  } else {
-    tmp = -1;
-  }
-
-  // Switch: '<S32>/Switch2' incorporates:
-  //   Constant: '<S32>/Clamping_zero'
-  //   Constant: '<S32>/Constant3'
-  //   Constant: '<S32>/Constant4'
-  //   RelationalOperator: '<S32>/fix for DT propagation issue1'
-
-  if (rtb_IntegralGain > 0.0) {
-    rtb_Switch1 = 1;
-  } else {
-    rtb_Switch1 = -1;
-  }
-
-  // Switch: '<S32>/Switch' incorporates:
-  //   Constant: '<S32>/Constant1'
-  //   Logic: '<S32>/AND3'
-  //   RelationalOperator: '<S32>/Equal1'
-  //   Switch: '<S32>/Switch1'
-  //   Switch: '<S32>/Switch2'
-
-  if (rtb_RelationalOperator && (tmp == rtb_Switch1)) {
-    rtb_Switch = 0.0;
-  } else {
-    rtb_Switch = rtb_IntegralGain;
-  }
-
-  // End of Switch: '<S32>/Switch'
-
-  // Sum: '<S1>/Sum1' incorporates:
-  //   Inport: '<Root>/depth_cmd'
+  // Update for Delay: '<S6>/pos_i-1' incorporates:
   //   Inport: '<Root>/z'
 
-  rtb_depth_error = rtU.z - rtU.depth_cmd;
+  rtDW.pos_i1_DSTATE = rtU.z;
 
-  // Gain: '<S93>/Integral Gain'
-  rtb_IntegralGain = rtP.pid_depth.I * rtb_depth_error;
+  // Update for Delay: '<S6>/pos_i-2'
+  rtDW.pos_i2_DSTATE = rtb_pos_i1;
 
-  // Gain: '<S99>/Filter Coefficient' incorporates:
-  //   DiscreteIntegrator: '<S91>/Filter'
-  //   Gain: '<S89>/Derivative Gain'
-  //   Sum: '<S91>/SumD'
+  // Update for Delay: '<S6>/pos_i-3'
+  rtDW.pos_i3_DSTATE = rtb_pos_i2;
 
-  rtb_FilterCoefficient_lc = (rtP.pid_depth.D * rtb_depth_error -
-    rtDW.Filter_DSTATE_m) * rtP.pid_depth.N;
+  // Update for Delay: '<S6>/pos_i-4'
+  rtDW.pos_i4_DSTATE = rtb_pos_i3;
 
-  // Sum: '<S105>/Sum' incorporates:
-  //   DiscreteIntegrator: '<S96>/Integrator'
-  //   Gain: '<S101>/Proportional Gain'
+  // Update for Delay: '<S6>/ref_i-1' incorporates:
+  //   Inport: '<Root>/depth_cmd'
 
-  rtb_Filter_b = (rtP.pid_depth.P * rtb_depth_error + rtDW.Integrator_DSTATE_o)
-    + rtb_FilterCoefficient_lc;
+  rtDW.ref_i1_DSTATE = rtU.depth_cmd;
 
-  // DeadZone: '<S88>/DeadZone'
-  if (rtb_Filter_b > 30.0) {
-    rtb_Integrator_d = rtb_Filter_b - 30.0;
+  // Update for Delay: '<S6>/ref_i-2'
+  rtDW.ref_i2_DSTATE = rtb_ref_i1;
 
-    // Switch: '<S86>/Switch1' incorporates:
-    //   Constant: '<S86>/Constant'
+  // Update for Delay: '<S6>/ref_i-3'
+  rtDW.ref_i3_DSTATE = rtb_ref_i2;
 
+  // Update for Delay: '<S6>/ref_i-4'
+  rtDW.ref_i4_DSTATE = rtb_ref_i3;
+
+  // Update for Delay: '<S170>/Delay1' incorporates:
+  //   Inport: '<Root>/z'
+
+  rtDW.Delay1_DSTATE = rtU.z;
+
+  // Update for Delay: '<S170>/Delay6'
+  rtDW.Delay6_DSTATE = rtb_Delay1;
+
+  // Update for Delay: '<S170>/Delay7'
+  rtDW.Delay7_DSTATE = rtb_Delay6;
+
+  // Update for Delay: '<S170>/Delay5'
+  rtDW.Delay5_DSTATE = rtb_Delay7;
+
+  // Update for Delay: '<S170>/Delay8'
+  rtDW.Delay8_DSTATE = rtb_Delay5;
+
+  // Update for Delay: '<S170>/Delay10' incorporates:
+  //   Delay: '<S170>/Delay9'
+
+  rtDW.Delay10_DSTATE = rtDW.Delay9_DSTATE;
+
+  // Update for Delay: '<S170>/Delay9' incorporates:
+  //   Delay: '<S170>/Delay4'
+
+  rtDW.Delay9_DSTATE = rtDW.Delay4_DSTATE;
+
+  // Update for Delay: '<S170>/Delay4' incorporates:
+  //   Delay: '<S170>/Delay3'
+
+  rtDW.Delay4_DSTATE = rtDW.Delay3_DSTATE;
+
+  // Update for Delay: '<S170>/Delay3' incorporates:
+  //   Delay: '<S170>/Delay2'
+
+  rtDW.Delay3_DSTATE = rtDW.Delay2_DSTATE;
+
+  // Update for Delay: '<S170>/Delay2' incorporates:
+  //   Inport: '<Root>/depth_cmd'
+
+  rtDW.Delay2_DSTATE = rtU.depth_cmd;
+
+  // Switch: '<S88>/Switch1' incorporates:
+  //   Constant: '<S88>/Clamping_zero'
+  //   Constant: '<S88>/Constant'
+  //   Constant: '<S88>/Constant2'
+  //   RelationalOperator: '<S88>/fix for DT propagation issue'
+
+  if (rtb_Filter_k > 0.0) {
     tmp = 1;
   } else {
-    if (rtb_Filter_b >= -30.0) {
-      rtb_Integrator_d = 0.0;
-    } else {
-      rtb_Integrator_d = rtb_Filter_b - -30.0;
-    }
-
-    // Switch: '<S86>/Switch1' incorporates:
-    //   Constant: '<S86>/Constant2'
-
     tmp = -1;
   }
 
-  // End of DeadZone: '<S88>/DeadZone'
+  // Switch: '<S88>/Switch2' incorporates:
+  //   Constant: '<S88>/Clamping_zero'
+  //   Constant: '<S88>/Constant3'
+  //   Constant: '<S88>/Constant4'
+  //   RelationalOperator: '<S88>/fix for DT propagation issue1'
 
-  // Switch: '<S86>/Switch2' incorporates:
-  //   Constant: '<S86>/Clamping_zero'
-  //   Constant: '<S86>/Constant3'
-  //   Constant: '<S86>/Constant4'
-  //   RelationalOperator: '<S86>/fix for DT propagation issue1'
-
-  if (rtb_IntegralGain > 0.0) {
-    rtb_Switch1 = 1;
+  if (rtb_Integrator_d > 0.0) {
+    tmp_0 = 1;
   } else {
-    rtb_Switch1 = -1;
+    tmp_0 = -1;
   }
 
-  // Switch: '<S86>/Switch' incorporates:
-  //   Constant: '<S86>/Clamping_zero'
-  //   Constant: '<S86>/Constant1'
-  //   Logic: '<S86>/AND3'
-  //   RelationalOperator: '<S86>/Equal1'
-  //   RelationalOperator: '<S86>/Relational Operator'
-  //   Switch: '<S86>/Switch1'
-  //   Switch: '<S86>/Switch2'
+  // Switch: '<S88>/Switch' incorporates:
+  //   Constant: '<S88>/Clamping_zero'
+  //   Constant: '<S88>/Constant1'
+  //   Logic: '<S88>/AND3'
+  //   RelationalOperator: '<S88>/Equal1'
+  //   RelationalOperator: '<S88>/Relational Operator'
+  //   Switch: '<S88>/Switch1'
+  //   Switch: '<S88>/Switch2'
 
-  if ((rtb_Integrator_d != 0.0) && (tmp == rtb_Switch1)) {
+  if ((rtb_Filter_k != 0.0) && (tmp == tmp_0)) {
     rtb_Integrator_d = 0.0;
-  } else {
-    rtb_Integrator_d = rtb_IntegralGain;
   }
 
-  // End of Switch: '<S86>/Switch'
+  // Update for DiscreteIntegrator: '<S98>/Integrator' incorporates:
+  //   Switch: '<S88>/Switch'
 
-  // Saturate: '<S103>/Saturation'
-  if (rtb_Filter_b > 30.0) {
-    rtb_Filter_b = 30.0;
-  } else if (rtb_Filter_b < -30.0) {
-    rtb_Filter_b = -30.0;
+  rtDW.Integrator_DSTATE += rtb_Integrator_d;
+  if (rtDW.Integrator_DSTATE > 10.0) {
+    rtDW.Integrator_DSTATE = 10.0;
+  } else if (rtDW.Integrator_DSTATE < -10.0) {
+    rtDW.Integrator_DSTATE = -10.0;
   }
 
-  // Sum: '<S1>/Sum2' incorporates:
-  //   Inport: '<Root>/theta'
-  //   Saturate: '<S103>/Saturation'
+  // End of Update for DiscreteIntegrator: '<S98>/Integrator'
 
-  rtb_Filter_b -= rtU.theta;
-
-  // Gain: '<S153>/Filter Coefficient' incorporates:
-  //   DiscreteIntegrator: '<S145>/Filter'
-  //   Gain: '<S143>/Derivative Gain'
-  //   Sum: '<S145>/SumD'
-
-  rtb_FilterCoefficient_a = (rtP.pid_pitch.D * rtb_Filter_b -
-    rtDW.Filter_DSTATE_l) * rtP.pid_pitch.N;
-
-  // Sum: '<S159>/Sum' incorporates:
-  //   DiscreteIntegrator: '<S150>/Integrator'
-  //   Gain: '<S155>/Proportional Gain'
-
-  rtb_IntegralGain = (rtP.pid_pitch.P * rtb_Filter_b + rtDW.Integrator_DSTATE_f)
-    + rtb_FilterCoefficient_a;
-
-  // Saturate: '<S157>/Saturation' incorporates:
-  //   DeadZone: '<S142>/DeadZone'
-
-  if (rtb_IntegralGain > 35.0) {
-    // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
-    rtY.DirectLeftRudderServoAngleCmd = 35.0;
-    rtb_IntegralGain -= 35.0;
-  } else {
-    if (rtb_IntegralGain < -35.0) {
-      // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
-      rtY.DirectLeftRudderServoAngleCmd = -35.0;
-    } else {
-      // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
-      rtY.DirectLeftRudderServoAngleCmd = rtb_IntegralGain;
-    }
-
-    if (rtb_IntegralGain >= -35.0) {
-      rtb_IntegralGain = 0.0;
-    } else {
-      rtb_IntegralGain -= -35.0;
-    }
-  }
-
-  // End of Saturate: '<S157>/Saturation'
-
-  // RelationalOperator: '<S140>/Relational Operator' incorporates:
-  //   Constant: '<S140>/Clamping_zero'
-
-  rtb_RelationalOperator = (rtb_IntegralGain != 0.0);
-
-  // Switch: '<S140>/Switch1' incorporates:
-  //   Constant: '<S140>/Clamping_zero'
-  //   Constant: '<S140>/Constant'
-  //   Constant: '<S140>/Constant2'
-  //   RelationalOperator: '<S140>/fix for DT propagation issue'
-
-  if (rtb_IntegralGain > 0.0) {
-    rtb_Switch1 = 1;
-  } else {
-    rtb_Switch1 = -1;
-  }
-
-  // End of Switch: '<S140>/Switch1'
-
-  // Gain: '<S147>/Integral Gain'
-  rtb_IntegralGain = rtP.pid_pitch.I * rtb_Filter_b;
-
-  // Update for DiscreteIntegrator: '<S42>/Integrator'
-  rtDW.Integrator_DSTATE += rtb_Switch;
-  if (rtDW.Integrator_DSTATE > 15.0) {
-    rtDW.Integrator_DSTATE = 15.0;
-  } else if (rtDW.Integrator_DSTATE < -15.0) {
-    rtDW.Integrator_DSTATE = -15.0;
-  }
-
-  // End of Update for DiscreteIntegrator: '<S42>/Integrator'
-
-  // Update for DiscreteIntegrator: '<S37>/Filter'
+  // Update for DiscreteIntegrator: '<S93>/Filter'
   rtDW.Filter_DSTATE += 0.1 * rtb_FilterCoefficient;
 
-  // Update for DiscreteIntegrator: '<S96>/Integrator'
-  rtDW.Integrator_DSTATE_o += rtb_Integrator_d;
-  if (rtDW.Integrator_DSTATE_o > 10.0) {
-    rtDW.Integrator_DSTATE_o = 10.0;
-  } else if (rtDW.Integrator_DSTATE_o < -10.0) {
-    rtDW.Integrator_DSTATE_o = -10.0;
-  }
-
-  // End of Update for DiscreteIntegrator: '<S96>/Integrator'
-
-  // Update for DiscreteIntegrator: '<S91>/Filter'
-  rtDW.Filter_DSTATE_m += 0.1 * rtb_FilterCoefficient_lc;
-
-  // Switch: '<S140>/Switch2' incorporates:
-  //   Constant: '<S140>/Clamping_zero'
-  //   Constant: '<S140>/Constant3'
-  //   Constant: '<S140>/Constant4'
-  //   RelationalOperator: '<S140>/fix for DT propagation issue1'
-
-  if (rtb_IntegralGain > 0.0) {
-    tmp = 1;
-  } else {
-    tmp = -1;
-  }
-
-  // Switch: '<S140>/Switch' incorporates:
-  //   Constant: '<S140>/Constant1'
-  //   Logic: '<S140>/AND3'
-  //   RelationalOperator: '<S140>/Equal1'
-  //   Switch: '<S140>/Switch2'
-
-  if (rtb_RelationalOperator && (rtb_Switch1 == tmp)) {
-    rtb_IntegralGain = 0.0;
-  }
-
-  // Update for DiscreteIntegrator: '<S150>/Integrator' incorporates:
-  //   Switch: '<S140>/Switch'
-
-  rtDW.Integrator_DSTATE_f += rtb_IntegralGain;
+  // Update for DiscreteIntegrator: '<S152>/Integrator'
+  rtDW.Integrator_DSTATE_f += rtb_Switch;
   if (rtDW.Integrator_DSTATE_f > 15.0) {
     rtDW.Integrator_DSTATE_f = 15.0;
   } else if (rtDW.Integrator_DSTATE_f < -15.0) {
     rtDW.Integrator_DSTATE_f = -15.0;
   }
 
-  // End of Update for DiscreteIntegrator: '<S150>/Integrator'
+  // End of Update for DiscreteIntegrator: '<S152>/Integrator'
 
-  // Update for DiscreteIntegrator: '<S145>/Filter'
-  rtDW.Filter_DSTATE_l += 0.1 * rtb_FilterCoefficient_a;
+  // Update for DiscreteIntegrator: '<S147>/Filter'
+  rtDW.Filter_DSTATE_l += 0.1 * rtb_FilterCoefficient_g;
+
+  // Update for Delay: '<S7>/pos_i-1' incorporates:
+  //   Inport: '<Root>/psi'
+
+  rtDW.pos_i1_DSTATE_f = rtU.psi;
+
+  // Update for Delay: '<S7>/pos_i-2'
+  rtDW.pos_i2_DSTATE_n = rtb_pos_i1_k;
+
+  // Update for Delay: '<S7>/pos_i-3'
+  rtDW.pos_i3_DSTATE_b = rtb_pos_i2_l;
+
+  // Update for Delay: '<S7>/ref_i-1' incorporates:
+  //   Inport: '<Root>/heading_cmd'
+
+  rtDW.ref_i1_DSTATE_c = rtU.heading_cmd;
+
+  // Update for Delay: '<S7>/ref_i-2'
+  rtDW.ref_i2_DSTATE_m = rtb_ref_i1_p;
+
+  // Update for Delay: '<S7>/ref_i-3'
+  rtDW.ref_i3_DSTATE_i = rtb_ref_i2_d;
+
+  // Update for Delay: '<S171>/Delay1' incorporates:
+  //   Inport: '<Root>/psi'
+
+  rtDW.Delay1_DSTATE_o = rtU.psi;
+
+  // Update for Delay: '<S171>/Delay6'
+  rtDW.Delay6_DSTATE_k = rtb_Delay1_d;
+
+  // Update for Delay: '<S171>/Delay7'
+  rtDW.Delay7_DSTATE_o = rtb_Delay6_c;
+
+  // Update for Delay: '<S171>/Delay5'
+  rtDW.Delay5_DSTATE_d = rtb_Delay7_c;
+
+  // Update for Delay: '<S171>/Delay9' incorporates:
+  //   Delay: '<S171>/Delay4'
+
+  rtDW.Delay9_DSTATE_m = rtDW.Delay4_DSTATE_a;
+
+  // Update for Delay: '<S171>/Delay4' incorporates:
+  //   Delay: '<S171>/Delay3'
+
+  rtDW.Delay4_DSTATE_a = rtDW.Delay3_DSTATE_f;
+
+  // Update for Delay: '<S171>/Delay3' incorporates:
+  //   Delay: '<S171>/Delay2'
+
+  rtDW.Delay3_DSTATE_f = rtDW.Delay2_DSTATE_g;
+
+  // Update for Delay: '<S171>/Delay2' incorporates:
+  //   Inport: '<Root>/heading_cmd'
+
+  rtDW.Delay2_DSTATE_g = rtU.heading_cmd;
+
+  // Switch: '<S34>/Switch2' incorporates:
+  //   Constant: '<S34>/Clamping_zero'
+  //   Constant: '<S34>/Constant3'
+  //   Constant: '<S34>/Constant4'
+  //   RelationalOperator: '<S34>/fix for DT propagation issue1'
+
+  if (rtb_IntegralGain > 0.0) {
+    tmp = 1;
+  } else {
+    tmp = -1;
+  }
+
+  // Switch: '<S34>/Switch' incorporates:
+  //   Constant: '<S34>/Constant1'
+  //   Logic: '<S34>/AND3'
+  //   RelationalOperator: '<S34>/Equal1'
+  //   Switch: '<S34>/Switch2'
+
+  if (rtb_RelationalOperator && (rtb_Switch1 == tmp)) {
+    rtb_IntegralGain = 0.0;
+  }
+
+  // Update for DiscreteIntegrator: '<S44>/Integrator' incorporates:
+  //   Switch: '<S34>/Switch'
+
+  rtDW.Integrator_DSTATE_b += rtb_IntegralGain;
+  if (rtDW.Integrator_DSTATE_b > 15.0) {
+    rtDW.Integrator_DSTATE_b = 15.0;
+  } else if (rtDW.Integrator_DSTATE_b < -15.0) {
+    rtDW.Integrator_DSTATE_b = -15.0;
+  }
+
+  // End of Update for DiscreteIntegrator: '<S44>/Integrator'
+
+  // Update for DiscreteIntegrator: '<S39>/Filter'
+  rtDW.Filter_DSTATE_m += 0.1 * rtb_FilterCoefficient_l;
 
   // End of Outputs for SubSystem: '<Root>/Controler'
 
@@ -392,7 +718,7 @@ void Controler::step()
   rtY.heading_error = mod_error;
 
   // Outport: '<Root>/pitch_error'
-  rtY.pitch_error = rtb_Filter_b;
+  rtY.pitch_error = rtb_pitch_error;
 
   // Outport: '<Root>/depth_error'
   rtY.depth_error = rtb_depth_error;
diff --git a/src/pAUV150/Controler/Controler.h b/src/pAUV150/Controler/Controler.h
index bae67cf..d8a9292 100644
--- a/src/pAUV150/Controler/Controler.h
+++ b/src/pAUV150/Controler/Controler.h
@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.77
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// C/C++ source code generated on : Thu Jun 26 20:56:30 2025
 //
 // Target selection: ert.tlc
 // Embedded hardware selection: Intel->x86-64 (Linux 64)
@@ -32,6 +32,8 @@ extern "C"
 
 }
 
+#define Controler_M                    (rtM)
+
 // Class declaration for model Controler
 class Controler final
 {
@@ -39,12 +41,44 @@ class Controler final
  public:
   // Block signals and states (default storage) for system '<Root>'
   struct DW {
-    real_T Integrator_DSTATE;          // '<S42>/Integrator'
-    real_T Filter_DSTATE;              // '<S37>/Filter'
-    real_T Integrator_DSTATE_o;        // '<S96>/Integrator'
-    real_T Filter_DSTATE_m;            // '<S91>/Filter'
-    real_T Integrator_DSTATE_f;        // '<S150>/Integrator'
-    real_T Filter_DSTATE_l;            // '<S145>/Filter'
+    real_T pos_i1_DSTATE;              // '<S6>/pos_i-1'
+    real_T pos_i2_DSTATE;              // '<S6>/pos_i-2'
+    real_T pos_i3_DSTATE;              // '<S6>/pos_i-3'
+    real_T pos_i4_DSTATE;              // '<S6>/pos_i-4'
+    real_T ref_i1_DSTATE;              // '<S6>/ref_i-1'
+    real_T ref_i2_DSTATE;              // '<S6>/ref_i-2'
+    real_T ref_i3_DSTATE;              // '<S6>/ref_i-3'
+    real_T ref_i4_DSTATE;              // '<S6>/ref_i-4'
+    real_T Delay1_DSTATE;              // '<S170>/Delay1'
+    real_T Delay6_DSTATE;              // '<S170>/Delay6'
+    real_T Delay7_DSTATE;              // '<S170>/Delay7'
+    real_T Delay5_DSTATE;              // '<S170>/Delay5'
+    real_T Delay8_DSTATE;              // '<S170>/Delay8'
+    real_T Delay10_DSTATE;             // '<S170>/Delay10'
+    real_T Delay9_DSTATE;              // '<S170>/Delay9'
+    real_T Delay4_DSTATE;              // '<S170>/Delay4'
+    real_T Delay3_DSTATE;              // '<S170>/Delay3'
+    real_T Delay2_DSTATE;              // '<S170>/Delay2'
+    real_T Integrator_DSTATE;          // '<S98>/Integrator'
+    real_T Filter_DSTATE;              // '<S93>/Filter'
+    real_T Integrator_DSTATE_f;        // '<S152>/Integrator'
+    real_T Filter_DSTATE_l;            // '<S147>/Filter'
+    real_T pos_i1_DSTATE_f;            // '<S7>/pos_i-1'
+    real_T pos_i2_DSTATE_n;            // '<S7>/pos_i-2'
+    real_T pos_i3_DSTATE_b;            // '<S7>/pos_i-3'
+    real_T ref_i1_DSTATE_c;            // '<S7>/ref_i-1'
+    real_T ref_i2_DSTATE_m;            // '<S7>/ref_i-2'
+    real_T ref_i3_DSTATE_i;            // '<S7>/ref_i-3'
+    real_T Delay1_DSTATE_o;            // '<S171>/Delay1'
+    real_T Delay6_DSTATE_k;            // '<S171>/Delay6'
+    real_T Delay7_DSTATE_o;            // '<S171>/Delay7'
+    real_T Delay5_DSTATE_d;            // '<S171>/Delay5'
+    real_T Delay9_DSTATE_m;            // '<S171>/Delay9'
+    real_T Delay4_DSTATE_a;            // '<S171>/Delay4'
+    real_T Delay3_DSTATE_f;            // '<S171>/Delay3'
+    real_T Delay2_DSTATE_g;            // '<S171>/Delay2'
+    real_T Integrator_DSTATE_b;        // '<S44>/Integrator'
+    real_T Filter_DSTATE_m;            // '<S39>/Filter'
   };
 
   // External inputs (root inport signals with default storage)
@@ -86,24 +120,30 @@ class Controler final
   struct P {
     struct_n3jlyiLoC0PV1TieyhpPwD pid_depth;// Variable: pid_depth
                                                //  Referenced by:
-                                               //    '<S89>/Derivative Gain'
-                                               //    '<S93>/Integral Gain'
-                                               //    '<S99>/Filter Coefficient'
-                                               //    '<S101>/Proportional Gain'
+                                               //    '<S91>/Derivative Gain'
+                                               //    '<S95>/Integral Gain'
+                                               //    '<S101>/Filter Coefficient'
+                                               //    '<S103>/Proportional Gain'
 
     struct_n3jlyiLoC0PV1TieyhpPwD pid_heading;// Variable: pid_heading
                                                  //  Referenced by:
-                                                 //    '<S35>/Derivative Gain'
-                                                 //    '<S39>/Integral Gain'
-                                                 //    '<S45>/Filter Coefficient'
-                                                 //    '<S47>/Proportional Gain'
+                                                 //    '<S37>/Derivative Gain'
+                                                 //    '<S41>/Integral Gain'
+                                                 //    '<S47>/Filter Coefficient'
+                                                 //    '<S49>/Proportional Gain'
 
     struct_n3jlyiLoC0PV1TieyhpPwD pid_pitch;// Variable: pid_pitch
                                                //  Referenced by:
-                                               //    '<S143>/Derivative Gain'
-                                               //    '<S147>/Integral Gain'
-                                               //    '<S153>/Filter Coefficient'
-                                               //    '<S155>/Proportional Gain'
+                                               //    '<S145>/Derivative Gain'
+                                               //    '<S149>/Integral Gain'
+                                               //    '<S155>/Filter Coefficient'
+                                               //    '<S157>/Proportional Gain'
+
+    real_T gamma_depth;                // Variable: gamma_depth
+                                          //  Referenced by: '<S6>/Compensation Gain'
+
+    real_T gamma_heading;              // Variable: gamma_heading
+                                          //  Referenced by: '<S7>/Compensation Gain'
 
   };
 
@@ -159,12 +199,6 @@ class Controler final
   RT_MODEL rtM;
 };
 
-//-
-//  These blocks were eliminated from the model due to optimizations:
-//
-//  Block '<S1>/Scope' : Unused code path elimination
-
-
 //-
 //  The generated code includes comments that allow you to trace directly
 //  back to the appropriate location in the model.  The basic format
@@ -177,179 +211,183 @@ class Controler final
 //  MATLAB hilite_system command to trace the generated code back
 //  to the parent model.  For example,
 //
-//  hilite_system('SimModel0618/Controler')    - opens subsystem SimModel0618/Controler
-//  hilite_system('SimModel0618/Controler/Kp') - opens and selects block Kp
+//  hilite_system('SimModel0621/Controler')    - opens subsystem SimModel0621/Controler
+//  hilite_system('SimModel0621/Controler/Kp') - opens and selects block Kp
 //
 //  Here is the system hierarchy for this model
 //
-//  '<Root>' : 'SimModel0618'
-//  '<S1>'   : 'SimModel0618/Controler'
-//  '<S2>'   : 'SimModel0618/Controler/MATLAB Function'
-//  '<S3>'   : 'SimModel0618/Controler/PID Controller'
-//  '<S4>'   : 'SimModel0618/Controler/PID Controller1'
-//  '<S5>'   : 'SimModel0618/Controler/PID Controller2'
-//  '<S6>'   : 'SimModel0618/Controler/PID Controller/Anti-windup'
-//  '<S7>'   : 'SimModel0618/Controler/PID Controller/D Gain'
-//  '<S8>'   : 'SimModel0618/Controler/PID Controller/External Derivative'
-//  '<S9>'   : 'SimModel0618/Controler/PID Controller/Filter'
-//  '<S10>'  : 'SimModel0618/Controler/PID Controller/Filter ICs'
-//  '<S11>'  : 'SimModel0618/Controler/PID Controller/I Gain'
-//  '<S12>'  : 'SimModel0618/Controler/PID Controller/Ideal P Gain'
-//  '<S13>'  : 'SimModel0618/Controler/PID Controller/Ideal P Gain Fdbk'
-//  '<S14>'  : 'SimModel0618/Controler/PID Controller/Integrator'
-//  '<S15>'  : 'SimModel0618/Controler/PID Controller/Integrator ICs'
-//  '<S16>'  : 'SimModel0618/Controler/PID Controller/N Copy'
-//  '<S17>'  : 'SimModel0618/Controler/PID Controller/N Gain'
-//  '<S18>'  : 'SimModel0618/Controler/PID Controller/P Copy'
-//  '<S19>'  : 'SimModel0618/Controler/PID Controller/Parallel P Gain'
-//  '<S20>'  : 'SimModel0618/Controler/PID Controller/Reset Signal'
-//  '<S21>'  : 'SimModel0618/Controler/PID Controller/Saturation'
-//  '<S22>'  : 'SimModel0618/Controler/PID Controller/Saturation Fdbk'
-//  '<S23>'  : 'SimModel0618/Controler/PID Controller/Sum'
-//  '<S24>'  : 'SimModel0618/Controler/PID Controller/Sum Fdbk'
-//  '<S25>'  : 'SimModel0618/Controler/PID Controller/Tracking Mode'
-//  '<S26>'  : 'SimModel0618/Controler/PID Controller/Tracking Mode Sum'
-//  '<S27>'  : 'SimModel0618/Controler/PID Controller/Tsamp - Integral'
-//  '<S28>'  : 'SimModel0618/Controler/PID Controller/Tsamp - Ngain'
-//  '<S29>'  : 'SimModel0618/Controler/PID Controller/postSat Signal'
-//  '<S30>'  : 'SimModel0618/Controler/PID Controller/preInt Signal'
-//  '<S31>'  : 'SimModel0618/Controler/PID Controller/preSat Signal'
-//  '<S32>'  : 'SimModel0618/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel'
-//  '<S33>'  : 'SimModel0618/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel/Dead Zone'
-//  '<S34>'  : 'SimModel0618/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
-//  '<S35>'  : 'SimModel0618/Controler/PID Controller/D Gain/Internal Parameters'
-//  '<S36>'  : 'SimModel0618/Controler/PID Controller/External Derivative/Error'
-//  '<S37>'  : 'SimModel0618/Controler/PID Controller/Filter/Disc. Forward Euler Filter'
-//  '<S38>'  : 'SimModel0618/Controler/PID Controller/Filter ICs/Internal IC - Filter'
-//  '<S39>'  : 'SimModel0618/Controler/PID Controller/I Gain/Internal Parameters'
-//  '<S40>'  : 'SimModel0618/Controler/PID Controller/Ideal P Gain/Passthrough'
-//  '<S41>'  : 'SimModel0618/Controler/PID Controller/Ideal P Gain Fdbk/Disabled'
-//  '<S42>'  : 'SimModel0618/Controler/PID Controller/Integrator/Discrete'
-//  '<S43>'  : 'SimModel0618/Controler/PID Controller/Integrator ICs/Internal IC'
-//  '<S44>'  : 'SimModel0618/Controler/PID Controller/N Copy/Disabled'
-//  '<S45>'  : 'SimModel0618/Controler/PID Controller/N Gain/Internal Parameters'
-//  '<S46>'  : 'SimModel0618/Controler/PID Controller/P Copy/Disabled'
-//  '<S47>'  : 'SimModel0618/Controler/PID Controller/Parallel P Gain/Internal Parameters'
-//  '<S48>'  : 'SimModel0618/Controler/PID Controller/Reset Signal/Disabled'
-//  '<S49>'  : 'SimModel0618/Controler/PID Controller/Saturation/Enabled'
-//  '<S50>'  : 'SimModel0618/Controler/PID Controller/Saturation Fdbk/Disabled'
-//  '<S51>'  : 'SimModel0618/Controler/PID Controller/Sum/Sum_PID'
-//  '<S52>'  : 'SimModel0618/Controler/PID Controller/Sum Fdbk/Disabled'
-//  '<S53>'  : 'SimModel0618/Controler/PID Controller/Tracking Mode/Disabled'
-//  '<S54>'  : 'SimModel0618/Controler/PID Controller/Tracking Mode Sum/Passthrough'
-//  '<S55>'  : 'SimModel0618/Controler/PID Controller/Tsamp - Integral/TsSignalSpecification'
-//  '<S56>'  : 'SimModel0618/Controler/PID Controller/Tsamp - Ngain/Passthrough'
-//  '<S57>'  : 'SimModel0618/Controler/PID Controller/postSat Signal/Forward_Path'
-//  '<S58>'  : 'SimModel0618/Controler/PID Controller/preInt Signal/Internal PreInt'
-//  '<S59>'  : 'SimModel0618/Controler/PID Controller/preSat Signal/Forward_Path'
-//  '<S60>'  : 'SimModel0618/Controler/PID Controller1/Anti-windup'
-//  '<S61>'  : 'SimModel0618/Controler/PID Controller1/D Gain'
-//  '<S62>'  : 'SimModel0618/Controler/PID Controller1/External Derivative'
-//  '<S63>'  : 'SimModel0618/Controler/PID Controller1/Filter'
-//  '<S64>'  : 'SimModel0618/Controler/PID Controller1/Filter ICs'
-//  '<S65>'  : 'SimModel0618/Controler/PID Controller1/I Gain'
-//  '<S66>'  : 'SimModel0618/Controler/PID Controller1/Ideal P Gain'
-//  '<S67>'  : 'SimModel0618/Controler/PID Controller1/Ideal P Gain Fdbk'
-//  '<S68>'  : 'SimModel0618/Controler/PID Controller1/Integrator'
-//  '<S69>'  : 'SimModel0618/Controler/PID Controller1/Integrator ICs'
-//  '<S70>'  : 'SimModel0618/Controler/PID Controller1/N Copy'
-//  '<S71>'  : 'SimModel0618/Controler/PID Controller1/N Gain'
-//  '<S72>'  : 'SimModel0618/Controler/PID Controller1/P Copy'
-//  '<S73>'  : 'SimModel0618/Controler/PID Controller1/Parallel P Gain'
-//  '<S74>'  : 'SimModel0618/Controler/PID Controller1/Reset Signal'
-//  '<S75>'  : 'SimModel0618/Controler/PID Controller1/Saturation'
-//  '<S76>'  : 'SimModel0618/Controler/PID Controller1/Saturation Fdbk'
-//  '<S77>'  : 'SimModel0618/Controler/PID Controller1/Sum'
-//  '<S78>'  : 'SimModel0618/Controler/PID Controller1/Sum Fdbk'
-//  '<S79>'  : 'SimModel0618/Controler/PID Controller1/Tracking Mode'
-//  '<S80>'  : 'SimModel0618/Controler/PID Controller1/Tracking Mode Sum'
-//  '<S81>'  : 'SimModel0618/Controler/PID Controller1/Tsamp - Integral'
-//  '<S82>'  : 'SimModel0618/Controler/PID Controller1/Tsamp - Ngain'
-//  '<S83>'  : 'SimModel0618/Controler/PID Controller1/postSat Signal'
-//  '<S84>'  : 'SimModel0618/Controler/PID Controller1/preInt Signal'
-//  '<S85>'  : 'SimModel0618/Controler/PID Controller1/preSat Signal'
-//  '<S86>'  : 'SimModel0618/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel'
-//  '<S87>'  : 'SimModel0618/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone'
-//  '<S88>'  : 'SimModel0618/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
-//  '<S89>'  : 'SimModel0618/Controler/PID Controller1/D Gain/Internal Parameters'
-//  '<S90>'  : 'SimModel0618/Controler/PID Controller1/External Derivative/Error'
-//  '<S91>'  : 'SimModel0618/Controler/PID Controller1/Filter/Disc. Forward Euler Filter'
-//  '<S92>'  : 'SimModel0618/Controler/PID Controller1/Filter ICs/Internal IC - Filter'
-//  '<S93>'  : 'SimModel0618/Controler/PID Controller1/I Gain/Internal Parameters'
-//  '<S94>'  : 'SimModel0618/Controler/PID Controller1/Ideal P Gain/Passthrough'
-//  '<S95>'  : 'SimModel0618/Controler/PID Controller1/Ideal P Gain Fdbk/Disabled'
-//  '<S96>'  : 'SimModel0618/Controler/PID Controller1/Integrator/Discrete'
-//  '<S97>'  : 'SimModel0618/Controler/PID Controller1/Integrator ICs/Internal IC'
-//  '<S98>'  : 'SimModel0618/Controler/PID Controller1/N Copy/Disabled'
-//  '<S99>'  : 'SimModel0618/Controler/PID Controller1/N Gain/Internal Parameters'
-//  '<S100>' : 'SimModel0618/Controler/PID Controller1/P Copy/Disabled'
-//  '<S101>' : 'SimModel0618/Controler/PID Controller1/Parallel P Gain/Internal Parameters'
-//  '<S102>' : 'SimModel0618/Controler/PID Controller1/Reset Signal/Disabled'
-//  '<S103>' : 'SimModel0618/Controler/PID Controller1/Saturation/Enabled'
-//  '<S104>' : 'SimModel0618/Controler/PID Controller1/Saturation Fdbk/Disabled'
-//  '<S105>' : 'SimModel0618/Controler/PID Controller1/Sum/Sum_PID'
-//  '<S106>' : 'SimModel0618/Controler/PID Controller1/Sum Fdbk/Disabled'
-//  '<S107>' : 'SimModel0618/Controler/PID Controller1/Tracking Mode/Disabled'
-//  '<S108>' : 'SimModel0618/Controler/PID Controller1/Tracking Mode Sum/Passthrough'
-//  '<S109>' : 'SimModel0618/Controler/PID Controller1/Tsamp - Integral/TsSignalSpecification'
-//  '<S110>' : 'SimModel0618/Controler/PID Controller1/Tsamp - Ngain/Passthrough'
-//  '<S111>' : 'SimModel0618/Controler/PID Controller1/postSat Signal/Forward_Path'
-//  '<S112>' : 'SimModel0618/Controler/PID Controller1/preInt Signal/Internal PreInt'
-//  '<S113>' : 'SimModel0618/Controler/PID Controller1/preSat Signal/Forward_Path'
-//  '<S114>' : 'SimModel0618/Controler/PID Controller2/Anti-windup'
-//  '<S115>' : 'SimModel0618/Controler/PID Controller2/D Gain'
-//  '<S116>' : 'SimModel0618/Controler/PID Controller2/External Derivative'
-//  '<S117>' : 'SimModel0618/Controler/PID Controller2/Filter'
-//  '<S118>' : 'SimModel0618/Controler/PID Controller2/Filter ICs'
-//  '<S119>' : 'SimModel0618/Controler/PID Controller2/I Gain'
-//  '<S120>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain'
-//  '<S121>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain Fdbk'
-//  '<S122>' : 'SimModel0618/Controler/PID Controller2/Integrator'
-//  '<S123>' : 'SimModel0618/Controler/PID Controller2/Integrator ICs'
-//  '<S124>' : 'SimModel0618/Controler/PID Controller2/N Copy'
-//  '<S125>' : 'SimModel0618/Controler/PID Controller2/N Gain'
-//  '<S126>' : 'SimModel0618/Controler/PID Controller2/P Copy'
-//  '<S127>' : 'SimModel0618/Controler/PID Controller2/Parallel P Gain'
-//  '<S128>' : 'SimModel0618/Controler/PID Controller2/Reset Signal'
-//  '<S129>' : 'SimModel0618/Controler/PID Controller2/Saturation'
-//  '<S130>' : 'SimModel0618/Controler/PID Controller2/Saturation Fdbk'
-//  '<S131>' : 'SimModel0618/Controler/PID Controller2/Sum'
-//  '<S132>' : 'SimModel0618/Controler/PID Controller2/Sum Fdbk'
-//  '<S133>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode'
-//  '<S134>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode Sum'
-//  '<S135>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Integral'
-//  '<S136>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Ngain'
-//  '<S137>' : 'SimModel0618/Controler/PID Controller2/postSat Signal'
-//  '<S138>' : 'SimModel0618/Controler/PID Controller2/preInt Signal'
-//  '<S139>' : 'SimModel0618/Controler/PID Controller2/preSat Signal'
-//  '<S140>' : 'SimModel0618/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel'
-//  '<S141>' : 'SimModel0618/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel/Dead Zone'
-//  '<S142>' : 'SimModel0618/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
-//  '<S143>' : 'SimModel0618/Controler/PID Controller2/D Gain/Internal Parameters'
-//  '<S144>' : 'SimModel0618/Controler/PID Controller2/External Derivative/Error'
-//  '<S145>' : 'SimModel0618/Controler/PID Controller2/Filter/Disc. Forward Euler Filter'
-//  '<S146>' : 'SimModel0618/Controler/PID Controller2/Filter ICs/Internal IC - Filter'
-//  '<S147>' : 'SimModel0618/Controler/PID Controller2/I Gain/Internal Parameters'
-//  '<S148>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain/Passthrough'
-//  '<S149>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain Fdbk/Disabled'
-//  '<S150>' : 'SimModel0618/Controler/PID Controller2/Integrator/Discrete'
-//  '<S151>' : 'SimModel0618/Controler/PID Controller2/Integrator ICs/Internal IC'
-//  '<S152>' : 'SimModel0618/Controler/PID Controller2/N Copy/Disabled'
-//  '<S153>' : 'SimModel0618/Controler/PID Controller2/N Gain/Internal Parameters'
-//  '<S154>' : 'SimModel0618/Controler/PID Controller2/P Copy/Disabled'
-//  '<S155>' : 'SimModel0618/Controler/PID Controller2/Parallel P Gain/Internal Parameters'
-//  '<S156>' : 'SimModel0618/Controler/PID Controller2/Reset Signal/Disabled'
-//  '<S157>' : 'SimModel0618/Controler/PID Controller2/Saturation/Enabled'
-//  '<S158>' : 'SimModel0618/Controler/PID Controller2/Saturation Fdbk/Disabled'
-//  '<S159>' : 'SimModel0618/Controler/PID Controller2/Sum/Sum_PID'
-//  '<S160>' : 'SimModel0618/Controler/PID Controller2/Sum Fdbk/Disabled'
-//  '<S161>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode/Disabled'
-//  '<S162>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode Sum/Passthrough'
-//  '<S163>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Integral/TsSignalSpecification'
-//  '<S164>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Ngain/Passthrough'
-//  '<S165>' : 'SimModel0618/Controler/PID Controller2/postSat Signal/Forward_Path'
-//  '<S166>' : 'SimModel0618/Controler/PID Controller2/preInt Signal/Internal PreInt'
-//  '<S167>' : 'SimModel0618/Controler/PID Controller2/preSat Signal/Forward_Path'
+//  '<Root>' : 'SimModel0621'
+//  '<S1>'   : 'SimModel0621/Controler'
+//  '<S2>'   : 'SimModel0621/Controler/MATLAB Function'
+//  '<S3>'   : 'SimModel0621/Controler/PID Controller'
+//  '<S4>'   : 'SimModel0621/Controler/PID Controller1'
+//  '<S5>'   : 'SimModel0621/Controler/PID Controller2'
+//  '<S6>'   : 'SimModel0621/Controler/Pred_Depth'
+//  '<S7>'   : 'SimModel0621/Controler/Pred_Steering'
+//  '<S8>'   : 'SimModel0621/Controler/PID Controller/Anti-windup'
+//  '<S9>'   : 'SimModel0621/Controler/PID Controller/D Gain'
+//  '<S10>'  : 'SimModel0621/Controler/PID Controller/External Derivative'
+//  '<S11>'  : 'SimModel0621/Controler/PID Controller/Filter'
+//  '<S12>'  : 'SimModel0621/Controler/PID Controller/Filter ICs'
+//  '<S13>'  : 'SimModel0621/Controler/PID Controller/I Gain'
+//  '<S14>'  : 'SimModel0621/Controler/PID Controller/Ideal P Gain'
+//  '<S15>'  : 'SimModel0621/Controler/PID Controller/Ideal P Gain Fdbk'
+//  '<S16>'  : 'SimModel0621/Controler/PID Controller/Integrator'
+//  '<S17>'  : 'SimModel0621/Controler/PID Controller/Integrator ICs'
+//  '<S18>'  : 'SimModel0621/Controler/PID Controller/N Copy'
+//  '<S19>'  : 'SimModel0621/Controler/PID Controller/N Gain'
+//  '<S20>'  : 'SimModel0621/Controler/PID Controller/P Copy'
+//  '<S21>'  : 'SimModel0621/Controler/PID Controller/Parallel P Gain'
+//  '<S22>'  : 'SimModel0621/Controler/PID Controller/Reset Signal'
+//  '<S23>'  : 'SimModel0621/Controler/PID Controller/Saturation'
+//  '<S24>'  : 'SimModel0621/Controler/PID Controller/Saturation Fdbk'
+//  '<S25>'  : 'SimModel0621/Controler/PID Controller/Sum'
+//  '<S26>'  : 'SimModel0621/Controler/PID Controller/Sum Fdbk'
+//  '<S27>'  : 'SimModel0621/Controler/PID Controller/Tracking Mode'
+//  '<S28>'  : 'SimModel0621/Controler/PID Controller/Tracking Mode Sum'
+//  '<S29>'  : 'SimModel0621/Controler/PID Controller/Tsamp - Integral'
+//  '<S30>'  : 'SimModel0621/Controler/PID Controller/Tsamp - Ngain'
+//  '<S31>'  : 'SimModel0621/Controler/PID Controller/postSat Signal'
+//  '<S32>'  : 'SimModel0621/Controler/PID Controller/preInt Signal'
+//  '<S33>'  : 'SimModel0621/Controler/PID Controller/preSat Signal'
+//  '<S34>'  : 'SimModel0621/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel'
+//  '<S35>'  : 'SimModel0621/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel/Dead Zone'
+//  '<S36>'  : 'SimModel0621/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
+//  '<S37>'  : 'SimModel0621/Controler/PID Controller/D Gain/Internal Parameters'
+//  '<S38>'  : 'SimModel0621/Controler/PID Controller/External Derivative/Error'
+//  '<S39>'  : 'SimModel0621/Controler/PID Controller/Filter/Disc. Forward Euler Filter'
+//  '<S40>'  : 'SimModel0621/Controler/PID Controller/Filter ICs/Internal IC - Filter'
+//  '<S41>'  : 'SimModel0621/Controler/PID Controller/I Gain/Internal Parameters'
+//  '<S42>'  : 'SimModel0621/Controler/PID Controller/Ideal P Gain/Passthrough'
+//  '<S43>'  : 'SimModel0621/Controler/PID Controller/Ideal P Gain Fdbk/Disabled'
+//  '<S44>'  : 'SimModel0621/Controler/PID Controller/Integrator/Discrete'
+//  '<S45>'  : 'SimModel0621/Controler/PID Controller/Integrator ICs/Internal IC'
+//  '<S46>'  : 'SimModel0621/Controler/PID Controller/N Copy/Disabled'
+//  '<S47>'  : 'SimModel0621/Controler/PID Controller/N Gain/Internal Parameters'
+//  '<S48>'  : 'SimModel0621/Controler/PID Controller/P Copy/Disabled'
+//  '<S49>'  : 'SimModel0621/Controler/PID Controller/Parallel P Gain/Internal Parameters'
+//  '<S50>'  : 'SimModel0621/Controler/PID Controller/Reset Signal/Disabled'
+//  '<S51>'  : 'SimModel0621/Controler/PID Controller/Saturation/Enabled'
+//  '<S52>'  : 'SimModel0621/Controler/PID Controller/Saturation Fdbk/Disabled'
+//  '<S53>'  : 'SimModel0621/Controler/PID Controller/Sum/Sum_PID'
+//  '<S54>'  : 'SimModel0621/Controler/PID Controller/Sum Fdbk/Disabled'
+//  '<S55>'  : 'SimModel0621/Controler/PID Controller/Tracking Mode/Disabled'
+//  '<S56>'  : 'SimModel0621/Controler/PID Controller/Tracking Mode Sum/Passthrough'
+//  '<S57>'  : 'SimModel0621/Controler/PID Controller/Tsamp - Integral/TsSignalSpecification'
+//  '<S58>'  : 'SimModel0621/Controler/PID Controller/Tsamp - Ngain/Passthrough'
+//  '<S59>'  : 'SimModel0621/Controler/PID Controller/postSat Signal/Forward_Path'
+//  '<S60>'  : 'SimModel0621/Controler/PID Controller/preInt Signal/Internal PreInt'
+//  '<S61>'  : 'SimModel0621/Controler/PID Controller/preSat Signal/Forward_Path'
+//  '<S62>'  : 'SimModel0621/Controler/PID Controller1/Anti-windup'
+//  '<S63>'  : 'SimModel0621/Controler/PID Controller1/D Gain'
+//  '<S64>'  : 'SimModel0621/Controler/PID Controller1/External Derivative'
+//  '<S65>'  : 'SimModel0621/Controler/PID Controller1/Filter'
+//  '<S66>'  : 'SimModel0621/Controler/PID Controller1/Filter ICs'
+//  '<S67>'  : 'SimModel0621/Controler/PID Controller1/I Gain'
+//  '<S68>'  : 'SimModel0621/Controler/PID Controller1/Ideal P Gain'
+//  '<S69>'  : 'SimModel0621/Controler/PID Controller1/Ideal P Gain Fdbk'
+//  '<S70>'  : 'SimModel0621/Controler/PID Controller1/Integrator'
+//  '<S71>'  : 'SimModel0621/Controler/PID Controller1/Integrator ICs'
+//  '<S72>'  : 'SimModel0621/Controler/PID Controller1/N Copy'
+//  '<S73>'  : 'SimModel0621/Controler/PID Controller1/N Gain'
+//  '<S74>'  : 'SimModel0621/Controler/PID Controller1/P Copy'
+//  '<S75>'  : 'SimModel0621/Controler/PID Controller1/Parallel P Gain'
+//  '<S76>'  : 'SimModel0621/Controler/PID Controller1/Reset Signal'
+//  '<S77>'  : 'SimModel0621/Controler/PID Controller1/Saturation'
+//  '<S78>'  : 'SimModel0621/Controler/PID Controller1/Saturation Fdbk'
+//  '<S79>'  : 'SimModel0621/Controler/PID Controller1/Sum'
+//  '<S80>'  : 'SimModel0621/Controler/PID Controller1/Sum Fdbk'
+//  '<S81>'  : 'SimModel0621/Controler/PID Controller1/Tracking Mode'
+//  '<S82>'  : 'SimModel0621/Controler/PID Controller1/Tracking Mode Sum'
+//  '<S83>'  : 'SimModel0621/Controler/PID Controller1/Tsamp - Integral'
+//  '<S84>'  : 'SimModel0621/Controler/PID Controller1/Tsamp - Ngain'
+//  '<S85>'  : 'SimModel0621/Controler/PID Controller1/postSat Signal'
+//  '<S86>'  : 'SimModel0621/Controler/PID Controller1/preInt Signal'
+//  '<S87>'  : 'SimModel0621/Controler/PID Controller1/preSat Signal'
+//  '<S88>'  : 'SimModel0621/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel'
+//  '<S89>'  : 'SimModel0621/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone'
+//  '<S90>'  : 'SimModel0621/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
+//  '<S91>'  : 'SimModel0621/Controler/PID Controller1/D Gain/Internal Parameters'
+//  '<S92>'  : 'SimModel0621/Controler/PID Controller1/External Derivative/Error'
+//  '<S93>'  : 'SimModel0621/Controler/PID Controller1/Filter/Disc. Forward Euler Filter'
+//  '<S94>'  : 'SimModel0621/Controler/PID Controller1/Filter ICs/Internal IC - Filter'
+//  '<S95>'  : 'SimModel0621/Controler/PID Controller1/I Gain/Internal Parameters'
+//  '<S96>'  : 'SimModel0621/Controler/PID Controller1/Ideal P Gain/Passthrough'
+//  '<S97>'  : 'SimModel0621/Controler/PID Controller1/Ideal P Gain Fdbk/Disabled'
+//  '<S98>'  : 'SimModel0621/Controler/PID Controller1/Integrator/Discrete'
+//  '<S99>'  : 'SimModel0621/Controler/PID Controller1/Integrator ICs/Internal IC'
+//  '<S100>' : 'SimModel0621/Controler/PID Controller1/N Copy/Disabled'
+//  '<S101>' : 'SimModel0621/Controler/PID Controller1/N Gain/Internal Parameters'
+//  '<S102>' : 'SimModel0621/Controler/PID Controller1/P Copy/Disabled'
+//  '<S103>' : 'SimModel0621/Controler/PID Controller1/Parallel P Gain/Internal Parameters'
+//  '<S104>' : 'SimModel0621/Controler/PID Controller1/Reset Signal/Disabled'
+//  '<S105>' : 'SimModel0621/Controler/PID Controller1/Saturation/Enabled'
+//  '<S106>' : 'SimModel0621/Controler/PID Controller1/Saturation Fdbk/Disabled'
+//  '<S107>' : 'SimModel0621/Controler/PID Controller1/Sum/Sum_PID'
+//  '<S108>' : 'SimModel0621/Controler/PID Controller1/Sum Fdbk/Disabled'
+//  '<S109>' : 'SimModel0621/Controler/PID Controller1/Tracking Mode/Disabled'
+//  '<S110>' : 'SimModel0621/Controler/PID Controller1/Tracking Mode Sum/Passthrough'
+//  '<S111>' : 'SimModel0621/Controler/PID Controller1/Tsamp - Integral/TsSignalSpecification'
+//  '<S112>' : 'SimModel0621/Controler/PID Controller1/Tsamp - Ngain/Passthrough'
+//  '<S113>' : 'SimModel0621/Controler/PID Controller1/postSat Signal/Forward_Path'
+//  '<S114>' : 'SimModel0621/Controler/PID Controller1/preInt Signal/Internal PreInt'
+//  '<S115>' : 'SimModel0621/Controler/PID Controller1/preSat Signal/Forward_Path'
+//  '<S116>' : 'SimModel0621/Controler/PID Controller2/Anti-windup'
+//  '<S117>' : 'SimModel0621/Controler/PID Controller2/D Gain'
+//  '<S118>' : 'SimModel0621/Controler/PID Controller2/External Derivative'
+//  '<S119>' : 'SimModel0621/Controler/PID Controller2/Filter'
+//  '<S120>' : 'SimModel0621/Controler/PID Controller2/Filter ICs'
+//  '<S121>' : 'SimModel0621/Controler/PID Controller2/I Gain'
+//  '<S122>' : 'SimModel0621/Controler/PID Controller2/Ideal P Gain'
+//  '<S123>' : 'SimModel0621/Controler/PID Controller2/Ideal P Gain Fdbk'
+//  '<S124>' : 'SimModel0621/Controler/PID Controller2/Integrator'
+//  '<S125>' : 'SimModel0621/Controler/PID Controller2/Integrator ICs'
+//  '<S126>' : 'SimModel0621/Controler/PID Controller2/N Copy'
+//  '<S127>' : 'SimModel0621/Controler/PID Controller2/N Gain'
+//  '<S128>' : 'SimModel0621/Controler/PID Controller2/P Copy'
+//  '<S129>' : 'SimModel0621/Controler/PID Controller2/Parallel P Gain'
+//  '<S130>' : 'SimModel0621/Controler/PID Controller2/Reset Signal'
+//  '<S131>' : 'SimModel0621/Controler/PID Controller2/Saturation'
+//  '<S132>' : 'SimModel0621/Controler/PID Controller2/Saturation Fdbk'
+//  '<S133>' : 'SimModel0621/Controler/PID Controller2/Sum'
+//  '<S134>' : 'SimModel0621/Controler/PID Controller2/Sum Fdbk'
+//  '<S135>' : 'SimModel0621/Controler/PID Controller2/Tracking Mode'
+//  '<S136>' : 'SimModel0621/Controler/PID Controller2/Tracking Mode Sum'
+//  '<S137>' : 'SimModel0621/Controler/PID Controller2/Tsamp - Integral'
+//  '<S138>' : 'SimModel0621/Controler/PID Controller2/Tsamp - Ngain'
+//  '<S139>' : 'SimModel0621/Controler/PID Controller2/postSat Signal'
+//  '<S140>' : 'SimModel0621/Controler/PID Controller2/preInt Signal'
+//  '<S141>' : 'SimModel0621/Controler/PID Controller2/preSat Signal'
+//  '<S142>' : 'SimModel0621/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel'
+//  '<S143>' : 'SimModel0621/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel/Dead Zone'
+//  '<S144>' : 'SimModel0621/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
+//  '<S145>' : 'SimModel0621/Controler/PID Controller2/D Gain/Internal Parameters'
+//  '<S146>' : 'SimModel0621/Controler/PID Controller2/External Derivative/Error'
+//  '<S147>' : 'SimModel0621/Controler/PID Controller2/Filter/Disc. Forward Euler Filter'
+//  '<S148>' : 'SimModel0621/Controler/PID Controller2/Filter ICs/Internal IC - Filter'
+//  '<S149>' : 'SimModel0621/Controler/PID Controller2/I Gain/Internal Parameters'
+//  '<S150>' : 'SimModel0621/Controler/PID Controller2/Ideal P Gain/Passthrough'
+//  '<S151>' : 'SimModel0621/Controler/PID Controller2/Ideal P Gain Fdbk/Disabled'
+//  '<S152>' : 'SimModel0621/Controler/PID Controller2/Integrator/Discrete'
+//  '<S153>' : 'SimModel0621/Controler/PID Controller2/Integrator ICs/Internal IC'
+//  '<S154>' : 'SimModel0621/Controler/PID Controller2/N Copy/Disabled'
+//  '<S155>' : 'SimModel0621/Controler/PID Controller2/N Gain/Internal Parameters'
+//  '<S156>' : 'SimModel0621/Controler/PID Controller2/P Copy/Disabled'
+//  '<S157>' : 'SimModel0621/Controler/PID Controller2/Parallel P Gain/Internal Parameters'
+//  '<S158>' : 'SimModel0621/Controler/PID Controller2/Reset Signal/Disabled'
+//  '<S159>' : 'SimModel0621/Controler/PID Controller2/Saturation/Enabled'
+//  '<S160>' : 'SimModel0621/Controler/PID Controller2/Saturation Fdbk/Disabled'
+//  '<S161>' : 'SimModel0621/Controler/PID Controller2/Sum/Sum_PID'
+//  '<S162>' : 'SimModel0621/Controler/PID Controller2/Sum Fdbk/Disabled'
+//  '<S163>' : 'SimModel0621/Controler/PID Controller2/Tracking Mode/Disabled'
+//  '<S164>' : 'SimModel0621/Controler/PID Controller2/Tracking Mode Sum/Passthrough'
+//  '<S165>' : 'SimModel0621/Controler/PID Controller2/Tsamp - Integral/TsSignalSpecification'
+//  '<S166>' : 'SimModel0621/Controler/PID Controller2/Tsamp - Ngain/Passthrough'
+//  '<S167>' : 'SimModel0621/Controler/PID Controller2/postSat Signal/Forward_Path'
+//  '<S168>' : 'SimModel0621/Controler/PID Controller2/preInt Signal/Internal PreInt'
+//  '<S169>' : 'SimModel0621/Controler/PID Controller2/preSat Signal/Forward_Path'
+//  '<S170>' : 'SimModel0621/Controler/Pred_Depth/gamma'
+//  '<S171>' : 'SimModel0621/Controler/Pred_Steering/gamma'
 
 #endif                                 // Controler_h_
 
diff --git a/src/pAUV150/Controler/Controler_data.cpp b/src/pAUV150/Controler/Controler_data.cpp
index 499169c..7609911 100644
--- a/src/pAUV150/Controler/Controler_data.cpp
+++ b/src/pAUV150/Controler/Controler_data.cpp
@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.77
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// C/C++ source code generated on : Thu Jun 26 20:56:30 2025
 //
 // Target selection: ert.tlc
 // Embedded hardware selection: Intel->x86-64 (Linux 64)
@@ -24,45 +24,55 @@
 Controler::P Controler::rtP{
   // Variable: pid_depth
   //  Referenced by:
-  //    '<S89>/Derivative Gain'
-  //    '<S93>/Integral Gain'
-  //    '<S99>/Filter Coefficient'
-  //    '<S101>/Proportional Gain'
+  //    '<S91>/Derivative Gain'
+  //    '<S95>/Integral Gain'
+  //    '<S101>/Filter Coefficient'
+  //    '<S103>/Proportional Gain'
 
   {
     2.0,
-    0.0,
-    0.0,
+    0.5,
+    0.1,
     10.0
   },
 
   // Variable: pid_heading
   //  Referenced by:
-  //    '<S35>/Derivative Gain'
-  //    '<S39>/Integral Gain'
-  //    '<S45>/Filter Coefficient'
-  //    '<S47>/Proportional Gain'
+  //    '<S37>/Derivative Gain'
+  //    '<S41>/Integral Gain'
+  //    '<S47>/Filter Coefficient'
+  //    '<S49>/Proportional Gain'
 
   {
     1,
-    0.1,
+    0.8,
     0.01,
     10.0
   },
 
   // Variable: pid_pitch
   //  Referenced by:
-  //    '<S143>/Derivative Gain'
-  //    '<S147>/Integral Gain'
-  //    '<S153>/Filter Coefficient'
-  //    '<S155>/Proportional Gain'
+  //    '<S145>/Derivative Gain'
+  //    '<S149>/Integral Gain'
+  //    '<S155>/Filter Coefficient'
+  //    '<S157>/Proportional Gain'
 
   {
     0.5,
     0.0,
     0.1,
     10.0
-  }
+  },
+
+  // Variable: gamma_depth
+  //  Referenced by: '<S6>/Compensation Gain'
+
+  5.0,
+
+  // Variable: gamma_heading
+  //  Referenced by: '<S7>/Compensation Gain'
+
+  1.0
 };
 
 //
diff --git a/src/pAUV150/Controler/Controler_private.h b/src/pAUV150/Controler/Controler_private.h
index bafc1c3..3da9a0a 100644
--- a/src/pAUV150/Controler/Controler_private.h
+++ b/src/pAUV150/Controler/Controler_private.h
@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.77
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// C/C++ source code generated on : Thu Jun 26 20:56:30 2025
 //
 // Target selection: ert.tlc
 // Embedded hardware selection: Intel->x86-64 (Linux 64)
diff --git a/src/pAUV150/Controler/Controler_types.h b/src/pAUV150/Controler/Controler_types.h
index f871d4e..0a3df34 100644
--- a/src/pAUV150/Controler/Controler_types.h
+++ b/src/pAUV150/Controler/Controler_types.h
@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.77
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// C/C++ source code generated on : Thu Jun 26 20:56:30 2025
 //
 // Target selection: ert.tlc
 // Embedded hardware selection: Intel->x86-64 (Linux 64)
diff --git a/src/pAUV150/Controler/rtGetNaN.cpp b/src/pAUV150/Controler/rtGetNaN.cpp
index a6924f2..52704a1 100644
--- a/src/pAUV150/Controler/rtGetNaN.cpp
+++ b/src/pAUV150/Controler/rtGetNaN.cpp
@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.77
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// C/C++ source code generated on : Thu Jun 26 20:56:30 2025
 //
 // Target selection: ert.tlc
 // Embedded hardware selection: Intel->x86-64 (Linux 64)
diff --git a/src/pAUV150/Controler/rtGetNaN.h b/src/pAUV150/Controler/rtGetNaN.h
index c8a9487..8d070a1 100644
--- a/src/pAUV150/Controler/rtGetNaN.h
+++ b/src/pAUV150/Controler/rtGetNaN.h
@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.77
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// C/C++ source code generated on : Thu Jun 26 20:56:30 2025
 //
 // Target selection: ert.tlc
 // Embedded hardware selection: Intel->x86-64 (Linux 64)
diff --git a/src/pAUV150/Controler/rt_nonfinite.cpp b/src/pAUV150/Controler/rt_nonfinite.cpp
index 0e33e04..8d33e43 100644
--- a/src/pAUV150/Controler/rt_nonfinite.cpp
+++ b/src/pAUV150/Controler/rt_nonfinite.cpp
@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.77
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// C/C++ source code generated on : Thu Jun 26 20:56:30 2025
 //
 // Target selection: ert.tlc
 // Embedded hardware selection: Intel->x86-64 (Linux 64)
diff --git a/src/pAUV150/Controler/rt_nonfinite.h b/src/pAUV150/Controler/rt_nonfinite.h
index f9a5bc6..77e59dc 100644
--- a/src/pAUV150/Controler/rt_nonfinite.h
+++ b/src/pAUV150/Controler/rt_nonfinite.h
@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.77
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// C/C++ source code generated on : Thu Jun 26 20:56:30 2025
 //
 // Target selection: ert.tlc
 // Embedded hardware selection: Intel->x86-64 (Linux 64)
diff --git a/src/pAUV150/Controler/rtmodel.h b/src/pAUV150/Controler/rtmodel.h
new file mode 100644
index 0000000..817ce1a
--- /dev/null
+++ b/src/pAUV150/Controler/rtmodel.h
@@ -0,0 +1,44 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+//
+// File: rtmodel.h
+//
+// Code generated for Simulink model 'Controler'.
+//
+// Model version                  : 5.77
+// Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
+// C/C++ source code generated on : Thu Jun 26 20:56:30 2025
+//
+// Target selection: ert.tlc
+// Embedded hardware selection: Intel->x86-64 (Linux 64)
+// Code generation objectives:
+//    1. Execution efficiency
+//    2. RAM efficiency
+// Validation result: Not run
+//
+#ifndef rtmodel_h_
+#define rtmodel_h_
+#include "Controler.h"
+#define MODEL_CLASSNAME                Controler
+#define MODEL_STEPNAME                 step
+
+//
+//  ROOT_IO_FORMAT: 0 (Individual arguments)
+//  ROOT_IO_FORMAT: 1 (Structure reference)
+//  ROOT_IO_FORMAT: 2 (Part of model data structure)
+
+#define ROOT_IO_FORMAT                 1
+
+// Macros generated for backwards compatibility
+#ifndef rtmGetStopRequested
+#define rtmGetStopRequested(rtm)       ((void*) 0)
+#endif
+#endif                                 // rtmodel_h_
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//
diff --git a/src/pAUV150/Controler/rtwtypes.h b/src/pAUV150/Controler/rtwtypes.h
index d228e9d..c607d90 100644
--- a/src/pAUV150/Controler/rtwtypes.h
+++ b/src/pAUV150/Controler/rtwtypes.h
@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'Controler'.
 //
-// Model version                  : 5.56
+// Model version                  : 5.77
 // Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
-// C/C++ source code generated on : Wed Jun 18 22:52:45 2025
+// C/C++ source code generated on : Thu Jun 26 20:56:30 2025
 //
 // Target selection: ert.tlc
 // Embedded hardware selection: Intel->x86-64 (Linux 64)