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

修复了航向的问题

Browse Source
This commit is contained in:
zjk
2025-06-18 23:10:16 +08:00
parent 44dd1f0470
commit f1e7d7b511
15 changed files with 1834 additions and 387 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
doc/p.md Normal file
View File

@@ -0,0 +1,25 @@
# 参数记录
{
1,
0.1,
0.01,
10.0
},
// Variable: pid_pitch
// Referenced by:
// '<S142>/Derivative Gain'
// '<S146>/Integral Gain'
// '<S152>/Filter Coefficient'
// '<S154>/Proportional Gain'
{
0.5,
0.1,
0.1,
10.0
}
};

1218
missions/alder/XLOG_SHORESIDE_18_6_2025_____23_04_26/XLOG_SHORESIDE_18_6_2025_____23_04_26.slog Normal file
View File

File diff suppressed because it is too large Load Diff

2
missions/alder/alder.moos
View File

@@ -16,7 +16,7 @@ ProcessConfig = ANTLER
Run = MOOSDB @ NewConsole = false Run = MOOSDB @ NewConsole = false
//Run = uSimMarineV22 @ NewConsole = false //Run = uSimMarineV22 @ NewConsole = false
Run = pNodeReporter @ NewConsole = false Run = pNodeReporter @ NewConsole = false
//Run = pLogger @ NewConsole = false Run = pLogger @ NewConsole = false
//Run = pMarinePIDV22 @ NewConsole = false //Run = pMarinePIDV22 @ NewConsole = false
Run = pMarineViewer @ NewConsole = false Run = pMarineViewer @ NewConsole = false
Run = uProcessWatch @ NewConsole = false Run = uProcessWatch @ NewConsole = false

8
src/pAUV150/AUV150.cpp
View File

@@ -326,6 +326,9 @@ bool AUV150::ListenLoop()
m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd + 35.0f; m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd + 35.0f;
m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd + 35.0f; m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd + 35.0f;
m_CommandFrame.mainThruster = m_control.rtY.MainThrusterSpeedCmd; m_CommandFrame.mainThruster = m_control.rtY.MainThrusterSpeedCmd;
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;
// } // }
// 发送消息 // 发送消息
try try
@@ -663,4 +666,9 @@ void AUV150::postStatusUpdate(std::string prefix)
if(m_status.insAltitude > 0) { if(m_status.insAltitude > 0) {
Notify(prefix+"_ALTITUDE", m_status.insAltitude, m_curr_time); Notify(prefix+"_ALTITUDE", m_status.insAltitude, m_curr_time);
} }
// 发布误差信息
Notify(prefix+"_HEADING_ERROR", m_status.heading_error, m_curr_time);
Notify(prefix+"_PITCH_ERROR", m_status.pitch_error, m_curr_time);
Notify(prefix+"_DEPTH_ERROR", m_status.depth_error, m_curr_time);
} }

4
src/pAUV150/AUV150.h
View File

@@ -116,6 +116,10 @@ struct AUV150_Status
double x; double x;
double y; double y;
double z; double z;
double heading_error;
double pitch_error;
double depth_error;
}; };
class AUV150 : public AppCastingMOOSApp class AUV150 : public AppCastingMOOSApp

439
src/pAUV150/Controler/Controler.cpp
View File

@@ -7,9 +7,9 @@
// //
// Code generated for Simulink model 'Controler'. // Code generated for Simulink model 'Controler'.
// //
// Model version : 5.37 // Model version : 5.56
// Simulink Coder version : 24.2 (R2024b) 21-Jun-2024 // Simulink Coder version : 24.2 (R2024b) 21-Jun-2024
// C/C++ source code generated on : Wed Jun 18 17:26:02 2025 // C/C++ source code generated on : Wed Jun 18 22:52:45 2025
// //
// Target selection: ert.tlc // Target selection: ert.tlc
// Embedded hardware selection: Intel->x86-64 (Linux 64) // Embedded hardware selection: Intel->x86-64 (Linux 64)
@@ -19,308 +19,356 @@
// Validation result: Not run // Validation result: Not run
// //
#include "Controler.h" #include "Controler.h"
#include <cmath>
#include "rtwtypes.h" #include "rtwtypes.h"
#include "cmath"
// Model step function // Model step function
void Controler::step() void Controler::step()
{ {
real_T rtb_DeadZone; real_T mod_error;
real_T rtb_Filter;
real_T rtb_FilterCoefficient; real_T rtb_FilterCoefficient;
real_T rtb_FilterCoefficient_a; real_T rtb_FilterCoefficient_a;
real_T rtb_FilterCoefficient_lc; real_T rtb_FilterCoefficient_lc;
real_T rtb_Filter_k; real_T rtb_Filter_b;
real_T rtb_Integrator_l; real_T rtb_IntegralGain;
real_T rtb_Integrator_p; real_T rtb_Integrator_d;
real_T rtb_Switch;
real_T rtb_depth_error;
int8_T rtb_Switch1;
int8_T tmp; int8_T tmp;
int8_T tmp_0;
boolean_T rtb_RelationalOperator; boolean_T rtb_RelationalOperator;
boolean_T rtb_fixforDTpropagationissue;
// Outputs for Atomic SubSystem: '<Root>/Controler' // Outputs for Atomic SubSystem: '<Root>/Controler'
// Sum: '<S1>/Sum' incorporates: // MATLAB Function: '<S1>/MATLAB Function' incorporates:
// Inport: '<Root>/heading_cmd' // Inport: '<Root>/heading_cmd'
// Inport: '<Root>/psi' // Inport: '<Root>/psi'
rtb_Filter = rtU.heading_cmd - rtU.psi; mod_error = rtU.heading_cmd - rtU.psi;
if (!std::isnan(mod_error)) {
// Gain: '<S38>/Integral Gain' if (std::isinf(mod_error)) {
rtb_Integrator_l = rtP.pid_heading.I * rtb_Filter; mod_error = (rtNaN);
} else if (mod_error == 0.0) {
// Gain: '<S44>/Filter Coefficient' incorporates: mod_error = 0.0;
// DiscreteIntegrator: '<S36>/Filter'
// Gain: '<S34>/Derivative Gain'
// Sum: '<S36>/SumD'
rtb_FilterCoefficient = (rtP.pid_heading.D * rtb_Filter - rtDW.Filter_DSTATE) *
rtP.pid_heading.N;
// Sum: '<S50>/Sum' incorporates:
// DiscreteIntegrator: '<S41>/Integrator'
// Gain: '<S46>/Proportional Gain'
rtb_Filter = (rtP.pid_heading.P * rtb_Filter + rtDW.Integrator_DSTATE) +
rtb_FilterCoefficient;
// DeadZone: '<S33>/DeadZone' incorporates:
// Saturate: '<S48>/Saturation'
if (rtb_Filter > 35.0) {
rtb_Filter_k = rtb_Filter - 35.0;
// Switch: '<S31>/Switch1' incorporates:
// Constant: '<S31>/Constant'
tmp = 1;
// Outport: '<Root>/DirectUpperRudderServoAngleCmd'
rtY.DirectUpperRudderServoAngleCmd = 35.0;
} else { } else {
if (rtb_Filter >= -35.0) { mod_error = std::fmod(mod_error, 360.0);
rtb_Filter_k = 0.0; if (mod_error == 0.0) {
} else { mod_error = 0.0;
rtb_Filter_k = rtb_Filter - -35.0; } else if (mod_error < 0.0) {
mod_error += 360.0;
}
}
} }
// Switch: '<S31>/Switch1' incorporates: if (mod_error >= 180.0) {
// Constant: '<S31>/Constant2' mod_error -= 360.0;
}
tmp = -1; // End of MATLAB Function: '<S1>/MATLAB Function'
if (rtb_Filter < -35.0) {
// Gain: '<S45>/Filter Coefficient' incorporates:
// DiscreteIntegrator: '<S37>/Filter'
// Gain: '<S35>/Derivative Gain'
// Sum: '<S37>/SumD'
rtb_FilterCoefficient = (rtP.pid_heading.D * mod_error - rtDW.Filter_DSTATE) *
rtP.pid_heading.N;
// Sum: '<S51>/Sum' incorporates:
// DiscreteIntegrator: '<S42>/Integrator'
// Gain: '<S47>/Proportional Gain'
rtb_IntegralGain = (rtP.pid_heading.P * mod_error + rtDW.Integrator_DSTATE) +
rtb_FilterCoefficient;
// Saturate: '<S49>/Saturation' incorporates:
// DeadZone: '<S34>/DeadZone'
if (rtb_IntegralGain > 35.0) {
// Outport: '<Root>/DirectUpperRudderServoAngleCmd'
rtY.DirectUpperRudderServoAngleCmd = 35.0;
rtb_IntegralGain -= 35.0;
} else {
if (rtb_IntegralGain < -35.0) {
// Outport: '<Root>/DirectUpperRudderServoAngleCmd' // Outport: '<Root>/DirectUpperRudderServoAngleCmd'
rtY.DirectUpperRudderServoAngleCmd = -35.0; rtY.DirectUpperRudderServoAngleCmd = -35.0;
} else { } else {
// Outport: '<Root>/DirectUpperRudderServoAngleCmd' // Outport: '<Root>/DirectUpperRudderServoAngleCmd'
rtY.DirectUpperRudderServoAngleCmd = rtb_Filter; rtY.DirectUpperRudderServoAngleCmd = rtb_IntegralGain;
}
if (rtb_IntegralGain >= -35.0) {
rtb_IntegralGain = 0.0;
} else {
rtb_IntegralGain -= -35.0;
} }
} }
// End of DeadZone: '<S33>/DeadZone' // End of Saturate: '<S49>/Saturation'
// RelationalOperator: '<S31>/Relational Operator' incorporates: // RelationalOperator: '<S32>/Relational Operator' incorporates:
// Constant: '<S31>/Clamping_zero' // Constant: '<S32>/Clamping_zero'
rtb_RelationalOperator = (rtb_Filter_k != 0.0); rtb_RelationalOperator = (rtb_IntegralGain != 0.0);
// RelationalOperator: '<S32>/fix for DT propagation issue' incorporates:
// Constant: '<S32>/Clamping_zero'
rtb_fixforDTpropagationissue = (rtb_IntegralGain > 0.0);
// Gain: '<S39>/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: // Sum: '<S1>/Sum1' incorporates:
// Inport: '<Root>/depth_cmd' // Inport: '<Root>/depth_cmd'
// Inport: '<Root>/z' // Inport: '<Root>/z'
rtb_Filter_k = rtU.z - rtU.depth_cmd; rtb_depth_error = rtU.z - rtU.depth_cmd;
// Gain: '<S92>/Integral Gain' // Gain: '<S93>/Integral Gain'
rtb_Filter = 0.0 * rtb_Filter_k; rtb_IntegralGain = rtP.pid_depth.I * rtb_depth_error;
// Gain: '<S98>/Filter Coefficient' incorporates: // Gain: '<S99>/Filter Coefficient' incorporates:
// DiscreteIntegrator: '<S90>/Filter' // DiscreteIntegrator: '<S91>/Filter'
// Gain: '<S88>/Derivative Gain' // Gain: '<S89>/Derivative Gain'
// Sum: '<S90>/SumD' // Sum: '<S91>/SumD'
rtb_FilterCoefficient_lc = (0.0 * rtb_Filter_k - rtDW.Filter_DSTATE_m) * 10.0; rtb_FilterCoefficient_lc = (rtP.pid_depth.D * rtb_depth_error -
rtDW.Filter_DSTATE_m) * rtP.pid_depth.N;
// Sum: '<S104>/Sum' incorporates: // Sum: '<S105>/Sum' incorporates:
// DiscreteIntegrator: '<S95>/Integrator' // DiscreteIntegrator: '<S96>/Integrator'
// Gain: '<S100>/Proportional Gain' // Gain: '<S101>/Proportional Gain'
rtb_Filter_k = (2.0 * rtb_Filter_k + rtDW.Integrator_DSTATE_o) + rtb_Filter_b = (rtP.pid_depth.P * rtb_depth_error + rtDW.Integrator_DSTATE_o)
rtb_FilterCoefficient_lc; + rtb_FilterCoefficient_lc;
// DeadZone: '<S87>/DeadZone' incorporates: // DeadZone: '<S88>/DeadZone'
// Saturate: '<S102>/Saturation' if (rtb_Filter_b > 30.0) {
rtb_Integrator_d = rtb_Filter_b - 30.0;
if (rtb_Filter_k > 30.0) { // Switch: '<S86>/Switch1' incorporates:
rtb_Integrator_p = rtb_Filter_k - 30.0; // Constant: '<S86>/Constant'
rtb_Filter_k = 30.0;
tmp = 1;
} else { } else {
if (rtb_Filter_k >= -30.0) { if (rtb_Filter_b >= -30.0) {
rtb_Integrator_p = 0.0; rtb_Integrator_d = 0.0;
} else { } else {
rtb_Integrator_p = rtb_Filter_k - -30.0; rtb_Integrator_d = rtb_Filter_b - -30.0;
} }
if (rtb_Filter_k < -30.0) { // Switch: '<S86>/Switch1' incorporates:
rtb_Filter_k = -30.0; // Constant: '<S86>/Constant2'
}
tmp = -1;
} }
// End of DeadZone: '<S87>/DeadZone' // End of DeadZone: '<S88>/DeadZone'
// 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;
} else {
rtb_Switch1 = -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'
if ((rtb_Integrator_d != 0.0) && (tmp == rtb_Switch1)) {
rtb_Integrator_d = 0.0;
} else {
rtb_Integrator_d = rtb_IntegralGain;
}
// End of Switch: '<S86>/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;
}
// Sum: '<S1>/Sum2' incorporates: // Sum: '<S1>/Sum2' incorporates:
// Inport: '<Root>/theta' // Inport: '<Root>/theta'
// Saturate: '<S102>/Saturation' // Saturate: '<S103>/Saturation'
rtb_Filter_k -= rtU.theta; rtb_Filter_b -= rtU.theta;
// Gain: '<S152>/Filter Coefficient' incorporates: // Gain: '<S153>/Filter Coefficient' incorporates:
// DiscreteIntegrator: '<S144>/Filter' // DiscreteIntegrator: '<S145>/Filter'
// Gain: '<S142>/Derivative Gain' // Gain: '<S143>/Derivative Gain'
// Sum: '<S144>/SumD' // Sum: '<S145>/SumD'
rtb_FilterCoefficient_a = (rtP.pid_pitch.D * rtb_Filter_k - rtb_FilterCoefficient_a = (rtP.pid_pitch.D * rtb_Filter_b -
rtDW.Filter_DSTATE_l) * rtP.pid_pitch.N; rtDW.Filter_DSTATE_l) * rtP.pid_pitch.N;
// Sum: '<S158>/Sum' incorporates: // Sum: '<S159>/Sum' incorporates:
// DiscreteIntegrator: '<S149>/Integrator' // DiscreteIntegrator: '<S150>/Integrator'
// Gain: '<S154>/Proportional Gain' // Gain: '<S155>/Proportional Gain'
rtb_DeadZone = (rtP.pid_pitch.P * rtb_Filter_k + rtDW.Integrator_DSTATE_f) + rtb_IntegralGain = (rtP.pid_pitch.P * rtb_Filter_b + rtDW.Integrator_DSTATE_f)
rtb_FilterCoefficient_a; + rtb_FilterCoefficient_a;
// Saturate: '<S156>/Saturation' incorporates: // Saturate: '<S157>/Saturation' incorporates:
// DeadZone: '<S141>/DeadZone' // DeadZone: '<S142>/DeadZone'
if (rtb_DeadZone > 35.0) { if (rtb_IntegralGain > 35.0) {
// Outport: '<Root>/DirectLeftRudderServoAngleCmd' // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
rtY.DirectLeftRudderServoAngleCmd = 35.0; rtY.DirectLeftRudderServoAngleCmd = 35.0;
rtb_DeadZone -= 35.0; rtb_IntegralGain -= 35.0;
} else { } else {
if (rtb_DeadZone < -35.0) { if (rtb_IntegralGain < -35.0) {
// Outport: '<Root>/DirectLeftRudderServoAngleCmd' // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
rtY.DirectLeftRudderServoAngleCmd = -35.0; rtY.DirectLeftRudderServoAngleCmd = -35.0;
} else { } else {
// Outport: '<Root>/DirectLeftRudderServoAngleCmd' // Outport: '<Root>/DirectLeftRudderServoAngleCmd'
rtY.DirectLeftRudderServoAngleCmd = rtb_DeadZone; rtY.DirectLeftRudderServoAngleCmd = rtb_IntegralGain;
} }
if (rtb_DeadZone >= -35.0) { if (rtb_IntegralGain >= -35.0) {
rtb_DeadZone = 0.0; rtb_IntegralGain = 0.0;
} else { } else {
rtb_DeadZone -= -35.0; rtb_IntegralGain -= -35.0;
} }
} }
// End of Saturate: '<S156>/Saturation' // End of Saturate: '<S157>/Saturation'
// Gain: '<S146>/Integral Gain' // RelationalOperator: '<S140>/Relational Operator' incorporates:
rtb_Filter_k *= rtP.pid_pitch.I; // Constant: '<S140>/Clamping_zero'
// Switch: '<S31>/Switch2' incorporates: rtb_RelationalOperator = (rtb_IntegralGain != 0.0);
// Constant: '<S31>/Clamping_zero'
// Constant: '<S31>/Constant3'
// Constant: '<S31>/Constant4'
// RelationalOperator: '<S31>/fix for DT propagation issue1'
if (rtb_Integrator_l > 0.0) { // Switch: '<S140>/Switch1' incorporates:
tmp_0 = 1; // 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 { } else {
tmp_0 = -1; rtb_Switch1 = -1;
} }
// Switch: '<S31>/Switch' incorporates: // End of Switch: '<S140>/Switch1'
// Constant: '<S31>/Constant1'
// Logic: '<S31>/AND3'
// RelationalOperator: '<S31>/Equal1'
// Switch: '<S31>/Switch1'
// Switch: '<S31>/Switch2'
if (rtb_RelationalOperator && (tmp == tmp_0)) { // Gain: '<S147>/Integral Gain'
rtb_Integrator_l = 0.0; rtb_IntegralGain = rtP.pid_pitch.I * rtb_Filter_b;
}
// Update for DiscreteIntegrator: '<S41>/Integrator' incorporates: // Update for DiscreteIntegrator: '<S42>/Integrator'
// Switch: '<S31>/Switch' rtDW.Integrator_DSTATE += rtb_Switch;
rtDW.Integrator_DSTATE += rtb_Integrator_l;
if (rtDW.Integrator_DSTATE > 15.0) { if (rtDW.Integrator_DSTATE > 15.0) {
rtDW.Integrator_DSTATE = 15.0; rtDW.Integrator_DSTATE = 15.0;
} else if (rtDW.Integrator_DSTATE < -15.0) { } else if (rtDW.Integrator_DSTATE < -15.0) {
rtDW.Integrator_DSTATE = -15.0; rtDW.Integrator_DSTATE = -15.0;
} }
// End of Update for DiscreteIntegrator: '<S41>/Integrator' // End of Update for DiscreteIntegrator: '<S42>/Integrator'
// Update for DiscreteIntegrator: '<S36>/Filter' // Update for DiscreteIntegrator: '<S37>/Filter'
rtDW.Filter_DSTATE += 0.1 * rtb_FilterCoefficient; rtDW.Filter_DSTATE += 0.1 * rtb_FilterCoefficient;
// Switch: '<S85>/Switch1' incorporates: // Update for DiscreteIntegrator: '<S96>/Integrator'
// Constant: '<S85>/Clamping_zero' rtDW.Integrator_DSTATE_o += rtb_Integrator_d;
// Constant: '<S85>/Constant'
// Constant: '<S85>/Constant2'
// RelationalOperator: '<S85>/fix for DT propagation issue'
if (rtb_Integrator_p > 0.0) {
tmp = 1;
} else {
tmp = -1;
}
// Switch: '<S85>/Switch' incorporates:
// Constant: '<S85>/Clamping_zero'
// Constant: '<S85>/Constant1'
// Logic: '<S85>/AND3'
// RelationalOperator: '<S85>/Equal1'
// RelationalOperator: '<S85>/Relational Operator'
// Switch: '<S85>/Switch1'
// Switch: '<S85>/Switch2'
if ((rtb_Integrator_p != 0.0) && (tmp == -1)) {
rtb_Filter = 0.0;
}
// Update for DiscreteIntegrator: '<S95>/Integrator' incorporates:
// Switch: '<S85>/Switch'
rtDW.Integrator_DSTATE_o += rtb_Filter;
if (rtDW.Integrator_DSTATE_o > 10.0) { if (rtDW.Integrator_DSTATE_o > 10.0) {
rtDW.Integrator_DSTATE_o = 10.0; rtDW.Integrator_DSTATE_o = 10.0;
} else if (rtDW.Integrator_DSTATE_o < -10.0) { } else if (rtDW.Integrator_DSTATE_o < -10.0) {
rtDW.Integrator_DSTATE_o = -10.0; rtDW.Integrator_DSTATE_o = -10.0;
} }
// End of Update for DiscreteIntegrator: '<S95>/Integrator' // End of Update for DiscreteIntegrator: '<S96>/Integrator'
// Update for DiscreteIntegrator: '<S90>/Filter' // Update for DiscreteIntegrator: '<S91>/Filter'
rtDW.Filter_DSTATE_m += 0.1 * rtb_FilterCoefficient_lc; rtDW.Filter_DSTATE_m += 0.1 * rtb_FilterCoefficient_lc;
// Switch: '<S139>/Switch1' incorporates: // Switch: '<S140>/Switch2' incorporates:
// Constant: '<S139>/Clamping_zero' // Constant: '<S140>/Clamping_zero'
// Constant: '<S139>/Constant' // Constant: '<S140>/Constant3'
// Constant: '<S139>/Constant2' // Constant: '<S140>/Constant4'
// RelationalOperator: '<S139>/fix for DT propagation issue' // RelationalOperator: '<S140>/fix for DT propagation issue1'
if (rtb_DeadZone > 0.0) { if (rtb_IntegralGain > 0.0) {
tmp = 1; tmp = 1;
} else { } else {
tmp = -1; tmp = -1;
} }
// Switch: '<S139>/Switch2' incorporates: // Switch: '<S140>/Switch' incorporates:
// Constant: '<S139>/Clamping_zero' // Constant: '<S140>/Constant1'
// Constant: '<S139>/Constant3' // Logic: '<S140>/AND3'
// Constant: '<S139>/Constant4' // RelationalOperator: '<S140>/Equal1'
// RelationalOperator: '<S139>/fix for DT propagation issue1' // Switch: '<S140>/Switch2'
if (rtb_Filter_k > 0.0) { if (rtb_RelationalOperator && (rtb_Switch1 == tmp)) {
tmp_0 = 1; rtb_IntegralGain = 0.0;
} else {
tmp_0 = -1;
} }
// Switch: '<S139>/Switch' incorporates: // Update for DiscreteIntegrator: '<S150>/Integrator' incorporates:
// Constant: '<S139>/Clamping_zero' // Switch: '<S140>/Switch'
// Constant: '<S139>/Constant1'
// Logic: '<S139>/AND3'
// RelationalOperator: '<S139>/Equal1'
// RelationalOperator: '<S139>/Relational Operator'
// Switch: '<S139>/Switch1'
// Switch: '<S139>/Switch2'
if ((rtb_DeadZone != 0.0) && (tmp == tmp_0)) { rtDW.Integrator_DSTATE_f += rtb_IntegralGain;
rtb_Filter_k = 0.0;
}
// Update for DiscreteIntegrator: '<S149>/Integrator' incorporates:
// Switch: '<S139>/Switch'
rtDW.Integrator_DSTATE_f += rtb_Filter_k;
if (rtDW.Integrator_DSTATE_f > 15.0) { if (rtDW.Integrator_DSTATE_f > 15.0) {
rtDW.Integrator_DSTATE_f = 15.0; rtDW.Integrator_DSTATE_f = 15.0;
} else if (rtDW.Integrator_DSTATE_f < -15.0) { } else if (rtDW.Integrator_DSTATE_f < -15.0) {
rtDW.Integrator_DSTATE_f = -15.0; rtDW.Integrator_DSTATE_f = -15.0;
} }
// End of Update for DiscreteIntegrator: '<S149>/Integrator' // End of Update for DiscreteIntegrator: '<S150>/Integrator'
// Update for DiscreteIntegrator: '<S144>/Filter' // Update for DiscreteIntegrator: '<S145>/Filter'
rtDW.Filter_DSTATE_l += 0.1 * rtb_FilterCoefficient_a; rtDW.Filter_DSTATE_l += 0.1 * rtb_FilterCoefficient_a;
// End of Outputs for SubSystem: '<Root>/Controler' // End of Outputs for SubSystem: '<Root>/Controler'
@@ -339,6 +387,15 @@ void Controler::step()
// Constant: '<S1>/Constant4' // Constant: '<S1>/Constant4'
rtY.DirectRightRudderServoAngleCmd = 1.0; rtY.DirectRightRudderServoAngleCmd = 1.0;
// Outport: '<Root>/heading_error'
rtY.heading_error = mod_error;
// Outport: '<Root>/pitch_error'
rtY.pitch_error = rtb_Filter_b;
// Outport: '<Root>/depth_error'
rtY.depth_error = rtb_depth_error;
} }
// Model initialize function // Model initialize function

382
src/pAUV150/Controler/Controler.h
View File

@@ -7,9 +7,9 @@
// //
// Code generated for Simulink model 'Controler'. // Code generated for Simulink model 'Controler'.
// //
// Model version : 5.37 // Model version : 5.56
// Simulink Coder version : 24.2 (R2024b) 21-Jun-2024 // Simulink Coder version : 24.2 (R2024b) 21-Jun-2024
// C/C++ source code generated on : Wed Jun 18 17:26:02 2025 // C/C++ source code generated on : Wed Jun 18 22:52:45 2025
// //
// Target selection: ert.tlc // Target selection: ert.tlc
// Embedded hardware selection: Intel->x86-64 (Linux 64) // Embedded hardware selection: Intel->x86-64 (Linux 64)
@@ -25,6 +25,13 @@
#include "rt_nonfinite.h" #include "rt_nonfinite.h"
#include "Controler_types.h" #include "Controler_types.h"
extern "C"
{
#include "rtGetNaN.h"
}
// Class declaration for model Controler // Class declaration for model Controler
class Controler final class Controler final
{ {
@@ -32,12 +39,12 @@ class Controler final
public: public:
// Block signals and states (default storage) for system '<Root>' // Block signals and states (default storage) for system '<Root>'
struct DW { struct DW {
real_T Integrator_DSTATE; // '<S41>/Integrator' real_T Integrator_DSTATE; // '<S42>/Integrator'
real_T Filter_DSTATE; // '<S36>/Filter' real_T Filter_DSTATE; // '<S37>/Filter'
real_T Integrator_DSTATE_o; // '<S95>/Integrator' real_T Integrator_DSTATE_o; // '<S96>/Integrator'
real_T Filter_DSTATE_m; // '<S90>/Filter' real_T Filter_DSTATE_m; // '<S91>/Filter'
real_T Integrator_DSTATE_f; // '<S149>/Integrator' real_T Integrator_DSTATE_f; // '<S150>/Integrator'
real_T Filter_DSTATE_l; // '<S144>/Filter' real_T Filter_DSTATE_l; // '<S145>/Filter'
}; };
// External inputs (root inport signals with default storage) // External inputs (root inport signals with default storage)
@@ -70,23 +77,33 @@ class Controler final
// '<Root>/DirectLeftRudderServoAngleCmd' // '<Root>/DirectLeftRudderServoAngleCmd'
real_T DirectRightRudderServoAngleCmd; real_T DirectRightRudderServoAngleCmd;
// '<Root>/DirectRightRudderServoAngleCmd' // '<Root>/DirectRightRudderServoAngleCmd'
real_T heading_error; // '<Root>/heading_error'
real_T pitch_error; // '<Root>/pitch_error'
real_T depth_error; // '<Root>/depth_error'
}; };
// Parameters (default storage) // Parameters (default storage)
struct P { struct P {
struct_n3jlyiLoC0PV1TieyhpPwD pid_depth;// Variable: pid_depth
// Referenced by:
// '<S89>/Derivative Gain'
// '<S93>/Integral Gain'
// '<S99>/Filter Coefficient'
// '<S101>/Proportional Gain'
struct_n3jlyiLoC0PV1TieyhpPwD pid_heading;// Variable: pid_heading struct_n3jlyiLoC0PV1TieyhpPwD pid_heading;// Variable: pid_heading
// Referenced by: // Referenced by:
// '<S34>/Derivative Gain' // '<S35>/Derivative Gain'
// '<S38>/Integral Gain' // '<S39>/Integral Gain'
// '<S44>/Filter Coefficient' // '<S45>/Filter Coefficient'
// '<S46>/Proportional Gain' // '<S47>/Proportional Gain'
struct_n3jlyiLoC0PV1TieyhpPwD pid_pitch;// Variable: pid_pitch struct_n3jlyiLoC0PV1TieyhpPwD pid_pitch;// Variable: pid_pitch
// Referenced by: // Referenced by:
// '<S142>/Derivative Gain' // '<S143>/Derivative Gain'
// '<S146>/Integral Gain' // '<S147>/Integral Gain'
// '<S152>/Filter Coefficient' // '<S153>/Filter Coefficient'
// '<S154>/Proportional Gain' // '<S155>/Proportional Gain'
}; };
@@ -145,7 +162,7 @@ class Controler final
//- //-
// These blocks were eliminated from the model due to optimizations: // These blocks were eliminated from the model due to optimizations:
// //
// Block '<S1>/Scope1' : Unused code path elimination // Block '<S1>/Scope' : Unused code path elimination
//- //-
@@ -167,171 +184,172 @@ class Controler final
// //
// '<Root>' : 'SimModel0618' // '<Root>' : 'SimModel0618'
// '<S1>' : 'SimModel0618/Controler' // '<S1>' : 'SimModel0618/Controler'
// '<S2>' : 'SimModel0618/Controler/PID Controller' // '<S2>' : 'SimModel0618/Controler/MATLAB Function'
// '<S3>' : 'SimModel0618/Controler/PID Controller1' // '<S3>' : 'SimModel0618/Controler/PID Controller'
// '<S4>' : 'SimModel0618/Controler/PID Controller2' // '<S4>' : 'SimModel0618/Controler/PID Controller1'
// '<S5>' : 'SimModel0618/Controler/PID Controller/Anti-windup' // '<S5>' : 'SimModel0618/Controler/PID Controller2'
// '<S6>' : 'SimModel0618/Controler/PID Controller/D Gain' // '<S6>' : 'SimModel0618/Controler/PID Controller/Anti-windup'
// '<S7>' : 'SimModel0618/Controler/PID Controller/External Derivative' // '<S7>' : 'SimModel0618/Controler/PID Controller/D Gain'
// '<S8>' : 'SimModel0618/Controler/PID Controller/Filter' // '<S8>' : 'SimModel0618/Controler/PID Controller/External Derivative'
// '<S9>' : 'SimModel0618/Controler/PID Controller/Filter ICs' // '<S9>' : 'SimModel0618/Controler/PID Controller/Filter'
// '<S10>' : 'SimModel0618/Controler/PID Controller/I Gain' // '<S10>' : 'SimModel0618/Controler/PID Controller/Filter ICs'
// '<S11>' : 'SimModel0618/Controler/PID Controller/Ideal P Gain' // '<S11>' : 'SimModel0618/Controler/PID Controller/I Gain'
// '<S12>' : 'SimModel0618/Controler/PID Controller/Ideal P Gain Fdbk' // '<S12>' : 'SimModel0618/Controler/PID Controller/Ideal P Gain'
// '<S13>' : 'SimModel0618/Controler/PID Controller/Integrator' // '<S13>' : 'SimModel0618/Controler/PID Controller/Ideal P Gain Fdbk'
// '<S14>' : 'SimModel0618/Controler/PID Controller/Integrator ICs' // '<S14>' : 'SimModel0618/Controler/PID Controller/Integrator'
// '<S15>' : 'SimModel0618/Controler/PID Controller/N Copy' // '<S15>' : 'SimModel0618/Controler/PID Controller/Integrator ICs'
// '<S16>' : 'SimModel0618/Controler/PID Controller/N Gain' // '<S16>' : 'SimModel0618/Controler/PID Controller/N Copy'
// '<S17>' : 'SimModel0618/Controler/PID Controller/P Copy' // '<S17>' : 'SimModel0618/Controler/PID Controller/N Gain'
// '<S18>' : 'SimModel0618/Controler/PID Controller/Parallel P Gain' // '<S18>' : 'SimModel0618/Controler/PID Controller/P Copy'
// '<S19>' : 'SimModel0618/Controler/PID Controller/Reset Signal' // '<S19>' : 'SimModel0618/Controler/PID Controller/Parallel P Gain'
// '<S20>' : 'SimModel0618/Controler/PID Controller/Saturation' // '<S20>' : 'SimModel0618/Controler/PID Controller/Reset Signal'
// '<S21>' : 'SimModel0618/Controler/PID Controller/Saturation Fdbk' // '<S21>' : 'SimModel0618/Controler/PID Controller/Saturation'
// '<S22>' : 'SimModel0618/Controler/PID Controller/Sum' // '<S22>' : 'SimModel0618/Controler/PID Controller/Saturation Fdbk'
// '<S23>' : 'SimModel0618/Controler/PID Controller/Sum Fdbk' // '<S23>' : 'SimModel0618/Controler/PID Controller/Sum'
// '<S24>' : 'SimModel0618/Controler/PID Controller/Tracking Mode' // '<S24>' : 'SimModel0618/Controler/PID Controller/Sum Fdbk'
// '<S25>' : 'SimModel0618/Controler/PID Controller/Tracking Mode Sum' // '<S25>' : 'SimModel0618/Controler/PID Controller/Tracking Mode'
// '<S26>' : 'SimModel0618/Controler/PID Controller/Tsamp - Integral' // '<S26>' : 'SimModel0618/Controler/PID Controller/Tracking Mode Sum'
// '<S27>' : 'SimModel0618/Controler/PID Controller/Tsamp - Ngain' // '<S27>' : 'SimModel0618/Controler/PID Controller/Tsamp - Integral'
// '<S28>' : 'SimModel0618/Controler/PID Controller/postSat Signal' // '<S28>' : 'SimModel0618/Controler/PID Controller/Tsamp - Ngain'
// '<S29>' : 'SimModel0618/Controler/PID Controller/preInt Signal' // '<S29>' : 'SimModel0618/Controler/PID Controller/postSat Signal'
// '<S30>' : 'SimModel0618/Controler/PID Controller/preSat Signal' // '<S30>' : 'SimModel0618/Controler/PID Controller/preInt Signal'
// '<S31>' : 'SimModel0618/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel' // '<S31>' : 'SimModel0618/Controler/PID Controller/preSat Signal'
// '<S32>' : 'SimModel0618/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel/Dead Zone' // '<S32>' : 'SimModel0618/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel'
// '<S33>' : 'SimModel0618/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled' // '<S33>' : 'SimModel0618/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel/Dead Zone'
// '<S34>' : 'SimModel0618/Controler/PID Controller/D Gain/Internal Parameters' // '<S34>' : 'SimModel0618/Controler/PID Controller/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
// '<S35>' : 'SimModel0618/Controler/PID Controller/External Derivative/Error' // '<S35>' : 'SimModel0618/Controler/PID Controller/D Gain/Internal Parameters'
// '<S36>' : 'SimModel0618/Controler/PID Controller/Filter/Disc. Forward Euler Filter' // '<S36>' : 'SimModel0618/Controler/PID Controller/External Derivative/Error'
// '<S37>' : 'SimModel0618/Controler/PID Controller/Filter ICs/Internal IC - Filter' // '<S37>' : 'SimModel0618/Controler/PID Controller/Filter/Disc. Forward Euler Filter'
// '<S38>' : 'SimModel0618/Controler/PID Controller/I Gain/Internal Parameters' // '<S38>' : 'SimModel0618/Controler/PID Controller/Filter ICs/Internal IC - Filter'
// '<S39>' : 'SimModel0618/Controler/PID Controller/Ideal P Gain/Passthrough' // '<S39>' : 'SimModel0618/Controler/PID Controller/I Gain/Internal Parameters'
// '<S40>' : 'SimModel0618/Controler/PID Controller/Ideal P Gain Fdbk/Disabled' // '<S40>' : 'SimModel0618/Controler/PID Controller/Ideal P Gain/Passthrough'
// '<S41>' : 'SimModel0618/Controler/PID Controller/Integrator/Discrete' // '<S41>' : 'SimModel0618/Controler/PID Controller/Ideal P Gain Fdbk/Disabled'
// '<S42>' : 'SimModel0618/Controler/PID Controller/Integrator ICs/Internal IC' // '<S42>' : 'SimModel0618/Controler/PID Controller/Integrator/Discrete'
// '<S43>' : 'SimModel0618/Controler/PID Controller/N Copy/Disabled' // '<S43>' : 'SimModel0618/Controler/PID Controller/Integrator ICs/Internal IC'
// '<S44>' : 'SimModel0618/Controler/PID Controller/N Gain/Internal Parameters' // '<S44>' : 'SimModel0618/Controler/PID Controller/N Copy/Disabled'
// '<S45>' : 'SimModel0618/Controler/PID Controller/P Copy/Disabled' // '<S45>' : 'SimModel0618/Controler/PID Controller/N Gain/Internal Parameters'
// '<S46>' : 'SimModel0618/Controler/PID Controller/Parallel P Gain/Internal Parameters' // '<S46>' : 'SimModel0618/Controler/PID Controller/P Copy/Disabled'
// '<S47>' : 'SimModel0618/Controler/PID Controller/Reset Signal/Disabled' // '<S47>' : 'SimModel0618/Controler/PID Controller/Parallel P Gain/Internal Parameters'
// '<S48>' : 'SimModel0618/Controler/PID Controller/Saturation/Enabled' // '<S48>' : 'SimModel0618/Controler/PID Controller/Reset Signal/Disabled'
// '<S49>' : 'SimModel0618/Controler/PID Controller/Saturation Fdbk/Disabled' // '<S49>' : 'SimModel0618/Controler/PID Controller/Saturation/Enabled'
// '<S50>' : 'SimModel0618/Controler/PID Controller/Sum/Sum_PID' // '<S50>' : 'SimModel0618/Controler/PID Controller/Saturation Fdbk/Disabled'
// '<S51>' : 'SimModel0618/Controler/PID Controller/Sum Fdbk/Disabled' // '<S51>' : 'SimModel0618/Controler/PID Controller/Sum/Sum_PID'
// '<S52>' : 'SimModel0618/Controler/PID Controller/Tracking Mode/Disabled' // '<S52>' : 'SimModel0618/Controler/PID Controller/Sum Fdbk/Disabled'
// '<S53>' : 'SimModel0618/Controler/PID Controller/Tracking Mode Sum/Passthrough' // '<S53>' : 'SimModel0618/Controler/PID Controller/Tracking Mode/Disabled'
// '<S54>' : 'SimModel0618/Controler/PID Controller/Tsamp - Integral/TsSignalSpecification' // '<S54>' : 'SimModel0618/Controler/PID Controller/Tracking Mode Sum/Passthrough'
// '<S55>' : 'SimModel0618/Controler/PID Controller/Tsamp - Ngain/Passthrough' // '<S55>' : 'SimModel0618/Controler/PID Controller/Tsamp - Integral/TsSignalSpecification'
// '<S56>' : 'SimModel0618/Controler/PID Controller/postSat Signal/Forward_Path' // '<S56>' : 'SimModel0618/Controler/PID Controller/Tsamp - Ngain/Passthrough'
// '<S57>' : 'SimModel0618/Controler/PID Controller/preInt Signal/Internal PreInt' // '<S57>' : 'SimModel0618/Controler/PID Controller/postSat Signal/Forward_Path'
// '<S58>' : 'SimModel0618/Controler/PID Controller/preSat Signal/Forward_Path' // '<S58>' : 'SimModel0618/Controler/PID Controller/preInt Signal/Internal PreInt'
// '<S59>' : 'SimModel0618/Controler/PID Controller1/Anti-windup' // '<S59>' : 'SimModel0618/Controler/PID Controller/preSat Signal/Forward_Path'
// '<S60>' : 'SimModel0618/Controler/PID Controller1/D Gain' // '<S60>' : 'SimModel0618/Controler/PID Controller1/Anti-windup'
// '<S61>' : 'SimModel0618/Controler/PID Controller1/External Derivative' // '<S61>' : 'SimModel0618/Controler/PID Controller1/D Gain'
// '<S62>' : 'SimModel0618/Controler/PID Controller1/Filter' // '<S62>' : 'SimModel0618/Controler/PID Controller1/External Derivative'
// '<S63>' : 'SimModel0618/Controler/PID Controller1/Filter ICs' // '<S63>' : 'SimModel0618/Controler/PID Controller1/Filter'
// '<S64>' : 'SimModel0618/Controler/PID Controller1/I Gain' // '<S64>' : 'SimModel0618/Controler/PID Controller1/Filter ICs'
// '<S65>' : 'SimModel0618/Controler/PID Controller1/Ideal P Gain' // '<S65>' : 'SimModel0618/Controler/PID Controller1/I Gain'
// '<S66>' : 'SimModel0618/Controler/PID Controller1/Ideal P Gain Fdbk' // '<S66>' : 'SimModel0618/Controler/PID Controller1/Ideal P Gain'
// '<S67>' : 'SimModel0618/Controler/PID Controller1/Integrator' // '<S67>' : 'SimModel0618/Controler/PID Controller1/Ideal P Gain Fdbk'
// '<S68>' : 'SimModel0618/Controler/PID Controller1/Integrator ICs' // '<S68>' : 'SimModel0618/Controler/PID Controller1/Integrator'
// '<S69>' : 'SimModel0618/Controler/PID Controller1/N Copy' // '<S69>' : 'SimModel0618/Controler/PID Controller1/Integrator ICs'
// '<S70>' : 'SimModel0618/Controler/PID Controller1/N Gain' // '<S70>' : 'SimModel0618/Controler/PID Controller1/N Copy'
// '<S71>' : 'SimModel0618/Controler/PID Controller1/P Copy' // '<S71>' : 'SimModel0618/Controler/PID Controller1/N Gain'
// '<S72>' : 'SimModel0618/Controler/PID Controller1/Parallel P Gain' // '<S72>' : 'SimModel0618/Controler/PID Controller1/P Copy'
// '<S73>' : 'SimModel0618/Controler/PID Controller1/Reset Signal' // '<S73>' : 'SimModel0618/Controler/PID Controller1/Parallel P Gain'
// '<S74>' : 'SimModel0618/Controler/PID Controller1/Saturation' // '<S74>' : 'SimModel0618/Controler/PID Controller1/Reset Signal'
// '<S75>' : 'SimModel0618/Controler/PID Controller1/Saturation Fdbk' // '<S75>' : 'SimModel0618/Controler/PID Controller1/Saturation'
// '<S76>' : 'SimModel0618/Controler/PID Controller1/Sum' // '<S76>' : 'SimModel0618/Controler/PID Controller1/Saturation Fdbk'
// '<S77>' : 'SimModel0618/Controler/PID Controller1/Sum Fdbk' // '<S77>' : 'SimModel0618/Controler/PID Controller1/Sum'
// '<S78>' : 'SimModel0618/Controler/PID Controller1/Tracking Mode' // '<S78>' : 'SimModel0618/Controler/PID Controller1/Sum Fdbk'
// '<S79>' : 'SimModel0618/Controler/PID Controller1/Tracking Mode Sum' // '<S79>' : 'SimModel0618/Controler/PID Controller1/Tracking Mode'
// '<S80>' : 'SimModel0618/Controler/PID Controller1/Tsamp - Integral' // '<S80>' : 'SimModel0618/Controler/PID Controller1/Tracking Mode Sum'
// '<S81>' : 'SimModel0618/Controler/PID Controller1/Tsamp - Ngain' // '<S81>' : 'SimModel0618/Controler/PID Controller1/Tsamp - Integral'
// '<S82>' : 'SimModel0618/Controler/PID Controller1/postSat Signal' // '<S82>' : 'SimModel0618/Controler/PID Controller1/Tsamp - Ngain'
// '<S83>' : 'SimModel0618/Controler/PID Controller1/preInt Signal' // '<S83>' : 'SimModel0618/Controler/PID Controller1/postSat Signal'
// '<S84>' : 'SimModel0618/Controler/PID Controller1/preSat Signal' // '<S84>' : 'SimModel0618/Controler/PID Controller1/preInt Signal'
// '<S85>' : 'SimModel0618/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel' // '<S85>' : 'SimModel0618/Controler/PID Controller1/preSat Signal'
// '<S86>' : 'SimModel0618/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone' // '<S86>' : 'SimModel0618/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel'
// '<S87>' : 'SimModel0618/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled' // '<S87>' : 'SimModel0618/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone'
// '<S88>' : 'SimModel0618/Controler/PID Controller1/D Gain/Internal Parameters' // '<S88>' : 'SimModel0618/Controler/PID Controller1/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
// '<S89>' : 'SimModel0618/Controler/PID Controller1/External Derivative/Error' // '<S89>' : 'SimModel0618/Controler/PID Controller1/D Gain/Internal Parameters'
// '<S90>' : 'SimModel0618/Controler/PID Controller1/Filter/Disc. Forward Euler Filter' // '<S90>' : 'SimModel0618/Controler/PID Controller1/External Derivative/Error'
// '<S91>' : 'SimModel0618/Controler/PID Controller1/Filter ICs/Internal IC - Filter' // '<S91>' : 'SimModel0618/Controler/PID Controller1/Filter/Disc. Forward Euler Filter'
// '<S92>' : 'SimModel0618/Controler/PID Controller1/I Gain/Internal Parameters' // '<S92>' : 'SimModel0618/Controler/PID Controller1/Filter ICs/Internal IC - Filter'
// '<S93>' : 'SimModel0618/Controler/PID Controller1/Ideal P Gain/Passthrough' // '<S93>' : 'SimModel0618/Controler/PID Controller1/I Gain/Internal Parameters'
// '<S94>' : 'SimModel0618/Controler/PID Controller1/Ideal P Gain Fdbk/Disabled' // '<S94>' : 'SimModel0618/Controler/PID Controller1/Ideal P Gain/Passthrough'
// '<S95>' : 'SimModel0618/Controler/PID Controller1/Integrator/Discrete' // '<S95>' : 'SimModel0618/Controler/PID Controller1/Ideal P Gain Fdbk/Disabled'
// '<S96>' : 'SimModel0618/Controler/PID Controller1/Integrator ICs/Internal IC' // '<S96>' : 'SimModel0618/Controler/PID Controller1/Integrator/Discrete'
// '<S97>' : 'SimModel0618/Controler/PID Controller1/N Copy/Disabled' // '<S97>' : 'SimModel0618/Controler/PID Controller1/Integrator ICs/Internal IC'
// '<S98>' : 'SimModel0618/Controler/PID Controller1/N Gain/Internal Parameters' // '<S98>' : 'SimModel0618/Controler/PID Controller1/N Copy/Disabled'
// '<S99>' : 'SimModel0618/Controler/PID Controller1/P Copy/Disabled' // '<S99>' : 'SimModel0618/Controler/PID Controller1/N Gain/Internal Parameters'
// '<S100>' : 'SimModel0618/Controler/PID Controller1/Parallel P Gain/Internal Parameters' // '<S100>' : 'SimModel0618/Controler/PID Controller1/P Copy/Disabled'
// '<S101>' : 'SimModel0618/Controler/PID Controller1/Reset Signal/Disabled' // '<S101>' : 'SimModel0618/Controler/PID Controller1/Parallel P Gain/Internal Parameters'
// '<S102>' : 'SimModel0618/Controler/PID Controller1/Saturation/Enabled' // '<S102>' : 'SimModel0618/Controler/PID Controller1/Reset Signal/Disabled'
// '<S103>' : 'SimModel0618/Controler/PID Controller1/Saturation Fdbk/Disabled' // '<S103>' : 'SimModel0618/Controler/PID Controller1/Saturation/Enabled'
// '<S104>' : 'SimModel0618/Controler/PID Controller1/Sum/Sum_PID' // '<S104>' : 'SimModel0618/Controler/PID Controller1/Saturation Fdbk/Disabled'
// '<S105>' : 'SimModel0618/Controler/PID Controller1/Sum Fdbk/Disabled' // '<S105>' : 'SimModel0618/Controler/PID Controller1/Sum/Sum_PID'
// '<S106>' : 'SimModel0618/Controler/PID Controller1/Tracking Mode/Disabled' // '<S106>' : 'SimModel0618/Controler/PID Controller1/Sum Fdbk/Disabled'
// '<S107>' : 'SimModel0618/Controler/PID Controller1/Tracking Mode Sum/Passthrough' // '<S107>' : 'SimModel0618/Controler/PID Controller1/Tracking Mode/Disabled'
// '<S108>' : 'SimModel0618/Controler/PID Controller1/Tsamp - Integral/TsSignalSpecification' // '<S108>' : 'SimModel0618/Controler/PID Controller1/Tracking Mode Sum/Passthrough'
// '<S109>' : 'SimModel0618/Controler/PID Controller1/Tsamp - Ngain/Passthrough' // '<S109>' : 'SimModel0618/Controler/PID Controller1/Tsamp - Integral/TsSignalSpecification'
// '<S110>' : 'SimModel0618/Controler/PID Controller1/postSat Signal/Forward_Path' // '<S110>' : 'SimModel0618/Controler/PID Controller1/Tsamp - Ngain/Passthrough'
// '<S111>' : 'SimModel0618/Controler/PID Controller1/preInt Signal/Internal PreInt' // '<S111>' : 'SimModel0618/Controler/PID Controller1/postSat Signal/Forward_Path'
// '<S112>' : 'SimModel0618/Controler/PID Controller1/preSat Signal/Forward_Path' // '<S112>' : 'SimModel0618/Controler/PID Controller1/preInt Signal/Internal PreInt'
// '<S113>' : 'SimModel0618/Controler/PID Controller2/Anti-windup' // '<S113>' : 'SimModel0618/Controler/PID Controller1/preSat Signal/Forward_Path'
// '<S114>' : 'SimModel0618/Controler/PID Controller2/D Gain' // '<S114>' : 'SimModel0618/Controler/PID Controller2/Anti-windup'
// '<S115>' : 'SimModel0618/Controler/PID Controller2/External Derivative' // '<S115>' : 'SimModel0618/Controler/PID Controller2/D Gain'
// '<S116>' : 'SimModel0618/Controler/PID Controller2/Filter' // '<S116>' : 'SimModel0618/Controler/PID Controller2/External Derivative'
// '<S117>' : 'SimModel0618/Controler/PID Controller2/Filter ICs' // '<S117>' : 'SimModel0618/Controler/PID Controller2/Filter'
// '<S118>' : 'SimModel0618/Controler/PID Controller2/I Gain' // '<S118>' : 'SimModel0618/Controler/PID Controller2/Filter ICs'
// '<S119>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain' // '<S119>' : 'SimModel0618/Controler/PID Controller2/I Gain'
// '<S120>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain Fdbk' // '<S120>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain'
// '<S121>' : 'SimModel0618/Controler/PID Controller2/Integrator' // '<S121>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain Fdbk'
// '<S122>' : 'SimModel0618/Controler/PID Controller2/Integrator ICs' // '<S122>' : 'SimModel0618/Controler/PID Controller2/Integrator'
// '<S123>' : 'SimModel0618/Controler/PID Controller2/N Copy' // '<S123>' : 'SimModel0618/Controler/PID Controller2/Integrator ICs'
// '<S124>' : 'SimModel0618/Controler/PID Controller2/N Gain' // '<S124>' : 'SimModel0618/Controler/PID Controller2/N Copy'
// '<S125>' : 'SimModel0618/Controler/PID Controller2/P Copy' // '<S125>' : 'SimModel0618/Controler/PID Controller2/N Gain'
// '<S126>' : 'SimModel0618/Controler/PID Controller2/Parallel P Gain' // '<S126>' : 'SimModel0618/Controler/PID Controller2/P Copy'
// '<S127>' : 'SimModel0618/Controler/PID Controller2/Reset Signal' // '<S127>' : 'SimModel0618/Controler/PID Controller2/Parallel P Gain'
// '<S128>' : 'SimModel0618/Controler/PID Controller2/Saturation' // '<S128>' : 'SimModel0618/Controler/PID Controller2/Reset Signal'
// '<S129>' : 'SimModel0618/Controler/PID Controller2/Saturation Fdbk' // '<S129>' : 'SimModel0618/Controler/PID Controller2/Saturation'
// '<S130>' : 'SimModel0618/Controler/PID Controller2/Sum' // '<S130>' : 'SimModel0618/Controler/PID Controller2/Saturation Fdbk'
// '<S131>' : 'SimModel0618/Controler/PID Controller2/Sum Fdbk' // '<S131>' : 'SimModel0618/Controler/PID Controller2/Sum'
// '<S132>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode' // '<S132>' : 'SimModel0618/Controler/PID Controller2/Sum Fdbk'
// '<S133>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode Sum' // '<S133>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode'
// '<S134>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Integral' // '<S134>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode Sum'
// '<S135>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Ngain' // '<S135>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Integral'
// '<S136>' : 'SimModel0618/Controler/PID Controller2/postSat Signal' // '<S136>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Ngain'
// '<S137>' : 'SimModel0618/Controler/PID Controller2/preInt Signal' // '<S137>' : 'SimModel0618/Controler/PID Controller2/postSat Signal'
// '<S138>' : 'SimModel0618/Controler/PID Controller2/preSat Signal' // '<S138>' : 'SimModel0618/Controler/PID Controller2/preInt Signal'
// '<S139>' : 'SimModel0618/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel' // '<S139>' : 'SimModel0618/Controler/PID Controller2/preSat Signal'
// '<S140>' : 'SimModel0618/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel/Dead Zone' // '<S140>' : 'SimModel0618/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel'
// '<S141>' : 'SimModel0618/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled' // '<S141>' : 'SimModel0618/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel/Dead Zone'
// '<S142>' : 'SimModel0618/Controler/PID Controller2/D Gain/Internal Parameters' // '<S142>' : 'SimModel0618/Controler/PID Controller2/Anti-windup/Disc. Clamping Parallel/Dead Zone/Enabled'
// '<S143>' : 'SimModel0618/Controler/PID Controller2/External Derivative/Error' // '<S143>' : 'SimModel0618/Controler/PID Controller2/D Gain/Internal Parameters'
// '<S144>' : 'SimModel0618/Controler/PID Controller2/Filter/Disc. Forward Euler Filter' // '<S144>' : 'SimModel0618/Controler/PID Controller2/External Derivative/Error'
// '<S145>' : 'SimModel0618/Controler/PID Controller2/Filter ICs/Internal IC - Filter' // '<S145>' : 'SimModel0618/Controler/PID Controller2/Filter/Disc. Forward Euler Filter'
// '<S146>' : 'SimModel0618/Controler/PID Controller2/I Gain/Internal Parameters' // '<S146>' : 'SimModel0618/Controler/PID Controller2/Filter ICs/Internal IC - Filter'
// '<S147>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain/Passthrough' // '<S147>' : 'SimModel0618/Controler/PID Controller2/I Gain/Internal Parameters'
// '<S148>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain Fdbk/Disabled' // '<S148>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain/Passthrough'
// '<S149>' : 'SimModel0618/Controler/PID Controller2/Integrator/Discrete' // '<S149>' : 'SimModel0618/Controler/PID Controller2/Ideal P Gain Fdbk/Disabled'
// '<S150>' : 'SimModel0618/Controler/PID Controller2/Integrator ICs/Internal IC' // '<S150>' : 'SimModel0618/Controler/PID Controller2/Integrator/Discrete'
// '<S151>' : 'SimModel0618/Controler/PID Controller2/N Copy/Disabled' // '<S151>' : 'SimModel0618/Controler/PID Controller2/Integrator ICs/Internal IC'
// '<S152>' : 'SimModel0618/Controler/PID Controller2/N Gain/Internal Parameters' // '<S152>' : 'SimModel0618/Controler/PID Controller2/N Copy/Disabled'
// '<S153>' : 'SimModel0618/Controler/PID Controller2/P Copy/Disabled' // '<S153>' : 'SimModel0618/Controler/PID Controller2/N Gain/Internal Parameters'
// '<S154>' : 'SimModel0618/Controler/PID Controller2/Parallel P Gain/Internal Parameters' // '<S154>' : 'SimModel0618/Controler/PID Controller2/P Copy/Disabled'
// '<S155>' : 'SimModel0618/Controler/PID Controller2/Reset Signal/Disabled' // '<S155>' : 'SimModel0618/Controler/PID Controller2/Parallel P Gain/Internal Parameters'
// '<S156>' : 'SimModel0618/Controler/PID Controller2/Saturation/Enabled' // '<S156>' : 'SimModel0618/Controler/PID Controller2/Reset Signal/Disabled'
// '<S157>' : 'SimModel0618/Controler/PID Controller2/Saturation Fdbk/Disabled' // '<S157>' : 'SimModel0618/Controler/PID Controller2/Saturation/Enabled'
// '<S158>' : 'SimModel0618/Controler/PID Controller2/Sum/Sum_PID' // '<S158>' : 'SimModel0618/Controler/PID Controller2/Saturation Fdbk/Disabled'
// '<S159>' : 'SimModel0618/Controler/PID Controller2/Sum Fdbk/Disabled' // '<S159>' : 'SimModel0618/Controler/PID Controller2/Sum/Sum_PID'
// '<S160>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode/Disabled' // '<S160>' : 'SimModel0618/Controler/PID Controller2/Sum Fdbk/Disabled'
// '<S161>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode Sum/Passthrough' // '<S161>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode/Disabled'
// '<S162>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Integral/TsSignalSpecification' // '<S162>' : 'SimModel0618/Controler/PID Controller2/Tracking Mode Sum/Passthrough'
// '<S163>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Ngain/Passthrough' // '<S163>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Integral/TsSignalSpecification'
// '<S164>' : 'SimModel0618/Controler/PID Controller2/postSat Signal/Forward_Path' // '<S164>' : 'SimModel0618/Controler/PID Controller2/Tsamp - Ngain/Passthrough'
// '<S165>' : 'SimModel0618/Controler/PID Controller2/preInt Signal/Internal PreInt' // '<S165>' : 'SimModel0618/Controler/PID Controller2/postSat Signal/Forward_Path'
// '<S166>' : 'SimModel0618/Controler/PID Controller2/preSat Signal/Forward_Path' // '<S166>' : 'SimModel0618/Controler/PID Controller2/preInt Signal/Internal PreInt'
// '<S167>' : 'SimModel0618/Controler/PID Controller2/preSat Signal/Forward_Path'
#endif // Controler_h_ #endif // Controler_h_

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

@@ -7,9 +7,9 @@
// //
// Code generated for Simulink model 'Controler'. // Code generated for Simulink model 'Controler'.
// //
// Model version : 5.37 // Model version : 5.56
// Simulink Coder version : 24.2 (R2024b) 21-Jun-2024 // Simulink Coder version : 24.2 (R2024b) 21-Jun-2024
// C/C++ source code generated on : Wed Jun 18 17:26:02 2025 // C/C++ source code generated on : Wed Jun 18 22:52:45 2025
// //
// Target selection: ert.tlc // Target selection: ert.tlc
// Embedded hardware selection: Intel->x86-64 (Linux 64) // Embedded hardware selection: Intel->x86-64 (Linux 64)
@@ -22,12 +22,26 @@
// Block parameters (default storage) // Block parameters (default storage)
Controler::P Controler::rtP{ Controler::P Controler::rtP{
// Variable: pid_depth
// Referenced by:
// '<S89>/Derivative Gain'
// '<S93>/Integral Gain'
// '<S99>/Filter Coefficient'
// '<S101>/Proportional Gain'
{
2.0,
0.0,
0.0,
10.0
},
// Variable: pid_heading // Variable: pid_heading
// Referenced by: // Referenced by:
// '<S34>/Derivative Gain' // '<S35>/Derivative Gain'
// '<S38>/Integral Gain' // '<S39>/Integral Gain'
// '<S44>/Filter Coefficient' // '<S45>/Filter Coefficient'
// '<S46>/Proportional Gain' // '<S47>/Proportional Gain'
{ {
1, 1,
@@ -38,14 +52,14 @@ Controler::P Controler::rtP{
// Variable: pid_pitch // Variable: pid_pitch
// Referenced by: // Referenced by:
// '<S142>/Derivative Gain' // '<S143>/Derivative Gain'
// '<S146>/Integral Gain' // '<S147>/Integral Gain'
// '<S152>/Filter Coefficient' // '<S153>/Filter Coefficient'
// '<S154>/Proportional Gain' // '<S155>/Proportional Gain'
{ {
0.5, 0.5,
0.1, 0.0,
0.1, 0.1,
10.0 10.0
} }

4
src/pAUV150/Controler/Controler_private.h
View File

@@ -7,9 +7,9 @@
// //
// Code generated for Simulink model 'Controler'. // Code generated for Simulink model 'Controler'.
// //
// Model version : 5.37 // Model version : 5.56
// Simulink Coder version : 24.2 (R2024b) 21-Jun-2024 // Simulink Coder version : 24.2 (R2024b) 21-Jun-2024
// C/C++ source code generated on : Wed Jun 18 17:26:02 2025 // C/C++ source code generated on : Wed Jun 18 22:52:45 2025
// //
// Target selection: ert.tlc // Target selection: ert.tlc
// Embedded hardware selection: Intel->x86-64 (Linux 64) // Embedded hardware selection: Intel->x86-64 (Linux 64)

4
src/pAUV150/Controler/Controler_types.h
View File

@@ -7,9 +7,9 @@
// //
// Code generated for Simulink model 'Controler'. // Code generated for Simulink model 'Controler'.
// //
// Model version : 5.37 // Model version : 5.56
// Simulink Coder version : 24.2 (R2024b) 21-Jun-2024 // Simulink Coder version : 24.2 (R2024b) 21-Jun-2024
// C/C++ source code generated on : Wed Jun 18 17:26:02 2025 // C/C++ source code generated on : Wed Jun 18 22:52:45 2025
// //
// Target selection: ert.tlc // Target selection: ert.tlc
// Embedded hardware selection: Intel->x86-64 (Linux 64) // Embedded hardware selection: Intel->x86-64 (Linux 64)

50
src/pAUV150/Controler/rtGetNaN.cpp Normal file
View File

@@ -0,0 +1,50 @@
//
// 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: rtGetNaN.cpp
//
// Code generated for Simulink model 'Controler'.
//
// Model version : 5.56
// Simulink Coder version : 24.2 (R2024b) 21-Jun-2024
// C/C++ source code generated on : Wed Jun 18 22:52:45 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
//
#include "rtwtypes.h"
extern "C"
{
#include "rtGetNaN.h"
}
extern "C"
{
// Return rtNaN needed by the generated code.
real_T rtGetNaN(void)
{
return rtNaN;
}
// Return rtNaNF needed by the generated code.
real32_T rtGetNaNF(void)
{
return rtNaNF;
}
}
//
// File trailer for generated code.
//
// [EOF]
//

53
src/pAUV150/Controler/rtGetNaN.h Normal file
View File

@@ -0,0 +1,53 @@
//
// 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: rtGetNaN.h
//
// Code generated for Simulink model 'Controler'.
//
// Model version : 5.56
// Simulink Coder version : 24.2 (R2024b) 21-Jun-2024
// C/C++ source code generated on : Wed Jun 18 22:52:45 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 rtGetNaN_h_
#define rtGetNaN_h_
extern "C"
{
#include "rt_nonfinite.h"
}
#include "rtwtypes.h"
#ifdef __cplusplus
extern "C"
{
#endif
extern real_T rtGetNaN(void);
extern real32_T rtGetNaNF(void);
#ifdef __cplusplus
} // extern "C"
#endif
#endif // rtGetNaN_h_
//
// File trailer for generated code.
//
// [EOF]
//

4
src/pAUV150/Controler/rt_nonfinite.cpp
View File

@@ -7,9 +7,9 @@
// //
// Code generated for Simulink model 'Controler'. // Code generated for Simulink model 'Controler'.
// //
// Model version : 5.37 // Model version : 5.56
// Simulink Coder version : 24.2 (R2024b) 21-Jun-2024 // Simulink Coder version : 24.2 (R2024b) 21-Jun-2024
// C/C++ source code generated on : Wed Jun 18 17:26:02 2025 // C/C++ source code generated on : Wed Jun 18 22:52:45 2025
// //
// Target selection: ert.tlc // Target selection: ert.tlc
// Embedded hardware selection: Intel->x86-64 (Linux 64) // Embedded hardware selection: Intel->x86-64 (Linux 64)

4
src/pAUV150/Controler/rt_nonfinite.h
View File

@@ -7,9 +7,9 @@
// //
// Code generated for Simulink model 'Controler'. // Code generated for Simulink model 'Controler'.
// //
// Model version : 5.37 // Model version : 5.56
// Simulink Coder version : 24.2 (R2024b) 21-Jun-2024 // Simulink Coder version : 24.2 (R2024b) 21-Jun-2024
// C/C++ source code generated on : Wed Jun 18 17:26:02 2025 // C/C++ source code generated on : Wed Jun 18 22:52:45 2025
// //
// Target selection: ert.tlc // Target selection: ert.tlc
// Embedded hardware selection: Intel->x86-64 (Linux 64) // Embedded hardware selection: Intel->x86-64 (Linux 64)

4
src/pAUV150/Controler/rtwtypes.h
View File

@@ -7,9 +7,9 @@
// //
// Code generated for Simulink model 'Controler'. // Code generated for Simulink model 'Controler'.
// //
// Model version : 5.37 // Model version : 5.56
// Simulink Coder version : 24.2 (R2024b) 21-Jun-2024 // Simulink Coder version : 24.2 (R2024b) 21-Jun-2024
// C/C++ source code generated on : Wed Jun 18 17:26:02 2025 // C/C++ source code generated on : Wed Jun 18 22:52:45 2025
// //
// Target selection: ert.tlc // Target selection: ert.tlc
// Embedded hardware selection: Intel->x86-64 (Linux 64) // Embedded hardware selection: Intel->x86-64 (Linux 64)