matlab code gen
Embed:
(wiki syntax)
Show/hide line numbers
S_con1.cpp
00001 // 00002 // File: S_con1.cpp 00003 // 00004 // Code generated for Simulink model 'S_con1'. 00005 // 00006 // Model version : 1.1 00007 // Simulink Coder version : 9.1 (R2019a) 23-Nov-2018 00008 // C/C++ source code generated on : Mon Feb 3 20:42:16 2020 00009 // 00010 // Target selection: ert.tlc 00011 // Embedded hardware selection: ARM Compatible->ARM 7 00012 // Code generation objectives: 00013 // 1. Execution efficiency 00014 // 2. RAM efficiency 00015 // Validation result: Not run 00016 // 00017 #include "S_con1.h" 00018 00019 static void MATLABFunction(real_T rtu_pwm, real_T *rty_y); 00020 00021 // 00022 // Output and update for atomic system: 00023 // '<Root>/MATLAB Function' 00024 // '<Root>/MATLAB Function2' 00025 // '<Root>/MATLAB Function4' 00026 // 00027 static void MATLABFunction(real_T rtu_pwm, real_T *rty_y) 00028 { 00029 *rty_y = (rtu_pwm - 1500.0) / 1000.0; 00030 } 00031 00032 // Model step function 00033 void S_con1ModelClass::step() 00034 { 00035 real_T A[4]; 00036 static const real_T a[16] = { 0.25, 0.25, 0.25, 0.25, 0.0, 2.193, 0.0, -2.193, 00037 -2.193, 0.0, 2.193, 0.0, 4.2591, -4.2591, 4.2591, -4.2591 }; 00038 00039 real_T rtb_FilterCoefficient_b; 00040 real_T rtb_FilterCoefficient_p; 00041 real_T rtb_Integrator_nd; 00042 real_T rtb_Integrator_m; 00043 real_T rtb_Filter_d; 00044 real_T rtb_FilterCoefficient_e; 00045 real_T rtb_Add3; 00046 real_T rtb_FilterCoefficient_o; 00047 int32_T i; 00048 real_T rtb_VectorConcatenate_idx_0; 00049 real_T rtb_Gain1_idx_1; 00050 real_T rtb_VectorConcatenate_idx_1; 00051 real_T rtb_VectorConcatenate_idx_2; 00052 real_T rtb_VectorConcatenate_idx_3; 00053 real_T rtb_TmpSignalConversionAtSFun_0; 00054 real_T rtb_TmpSignalConversionAtSFun_1; 00055 real_T rtb_angle_des_idx_0; 00056 00057 // MATLAB Function: '<Root>/MATLAB Function' incorporates: 00058 // Inport: '<Root>/pitch' 00059 00060 MATLABFunction(rtU.pitch, &rtb_Integrator_m); 00061 00062 // MATLAB Function: '<Root>/MATLAB Function2' incorporates: 00063 // Inport: '<Root>/roll' 00064 00065 MATLABFunction(rtU.roll, &rtb_Filter_d); 00066 00067 // MATLAB Function: '<Root>/MATLAB Function4' incorporates: 00068 // Inport: '<Root>/yaw' 00069 00070 MATLABFunction(rtU.yaw, &rtb_Add3); 00071 00072 // Outputs for Atomic SubSystem: '<Root>/small_angle_control' 00073 // Gain: '<S7>/Gain1' 00074 rtb_Gain1_idx_1 = 0.0066666666666666671 * rtb_Filter_d; 00075 00076 // Sum: '<S15>/Add1' incorporates: 00077 // Gain: '<S7>/Gain1' 00078 // Inport: '<Root>/vx' 00079 00080 rtb_Integrator_m = 0.0066666666666666671 * rtb_Integrator_m - rtU.vx; 00081 00082 // Gain: '<S137>/Filter Coefficient' incorporates: 00083 // DiscreteIntegrator: '<S129>/Filter' 00084 // Gain: '<S128>/Derivative Gain' 00085 // Sum: '<S129>/SumD' 00086 00087 rtb_Filter_d = (1.68 * rtb_Integrator_m - rtDW.Filter_DSTATE) * 100.0; 00088 00089 // Sum: '<S15>/Add2' incorporates: 00090 // Inport: '<Root>/vy' 00091 00092 rtb_Gain1_idx_1 -= rtU.vy; 00093 00094 // Gain: '<S269>/Filter Coefficient' incorporates: 00095 // DiscreteIntegrator: '<S261>/Filter' 00096 // Gain: '<S260>/Derivative Gain' 00097 // Sum: '<S261>/SumD' 00098 00099 rtb_FilterCoefficient_b = (1.6832 * rtb_Gain1_idx_1 - rtDW.Filter_DSTATE_p) * 00100 206.264558996617; 00101 00102 // SignalConversion: '<S14>/TmpSignal ConversionAt SFunction Inport1' incorporates: 00103 // DiscreteIntegrator: '<S134>/Integrator' 00104 // DiscreteIntegrator: '<S266>/Integrator' 00105 // Gain: '<S139>/Proportional Gain' 00106 // Gain: '<S271>/Proportional Gain' 00107 // MATLAB Function: '<S7>/MATLAB Function4' 00108 // Sum: '<S143>/Sum' 00109 // Sum: '<S275>/Sum' 00110 00111 rtb_TmpSignalConversionAtSFun_0 = (0.57 * rtb_Integrator_m + 00112 rtDW.Integrator_DSTATE) + rtb_Filter_d; 00113 rtb_TmpSignalConversionAtSFun_1 = (0.57 * rtb_Gain1_idx_1 + 00114 rtDW.Integrator_DSTATE_c) + rtb_FilterCoefficient_b; 00115 00116 // MATLAB Function: '<S7>/MATLAB Function4' 00117 rtb_FilterCoefficient_p = std::cos(rtb_Add3); 00118 rtb_VectorConcatenate_idx_1 = std::sin(rtb_Add3); 00119 rtb_angle_des_idx_0 = (rtb_TmpSignalConversionAtSFun_0 * 00120 rtb_VectorConcatenate_idx_1 - rtb_TmpSignalConversionAtSFun_1 * 00121 rtb_FilterCoefficient_p) * 0.1019367991845056; 00122 rtb_TmpSignalConversionAtSFun_1 = (rtb_TmpSignalConversionAtSFun_0 * 00123 rtb_FilterCoefficient_p + rtb_TmpSignalConversionAtSFun_1 * 00124 rtb_VectorConcatenate_idx_1) * 0.1019367991845056; 00125 00126 // Saturate: '<S7>/Saturation' 00127 rtb_VectorConcatenate_idx_1 = rtb_angle_des_idx_0; 00128 if (rtb_angle_des_idx_0 > 0.26179938779914941) { 00129 rtb_VectorConcatenate_idx_1 = 0.26179938779914941; 00130 } else { 00131 if (rtb_angle_des_idx_0 < -0.26179938779914941) { 00132 rtb_VectorConcatenate_idx_1 = -0.26179938779914941; 00133 } 00134 } 00135 00136 rtb_angle_des_idx_0 = rtb_VectorConcatenate_idx_1; 00137 rtb_VectorConcatenate_idx_1 = rtb_TmpSignalConversionAtSFun_1; 00138 if (rtb_TmpSignalConversionAtSFun_1 > 0.26179938779914941) { 00139 rtb_VectorConcatenate_idx_1 = 0.26179938779914941; 00140 } else { 00141 if (rtb_TmpSignalConversionAtSFun_1 < -0.26179938779914941) { 00142 rtb_VectorConcatenate_idx_1 = -0.26179938779914941; 00143 } 00144 } 00145 00146 rtb_TmpSignalConversionAtSFun_1 = rtb_VectorConcatenate_idx_1; 00147 00148 // MATLAB Function: '<S7>/MATLAB Function4' 00149 rtb_VectorConcatenate_idx_1 = rtb_Add3; 00150 00151 // Saturate: '<S7>/Saturation' incorporates: 00152 // MATLAB Function: '<S7>/MATLAB Function4' 00153 00154 if (rtb_Add3 > 0.26179938779914941) { 00155 rtb_VectorConcatenate_idx_1 = 0.26179938779914941; 00156 } else { 00157 if (rtb_Add3 < -0.26179938779914941) { 00158 rtb_VectorConcatenate_idx_1 = -0.26179938779914941; 00159 } 00160 } 00161 00162 // Sum: '<S16>/Add1' incorporates: 00163 // Inport: '<Root>/phi' 00164 00165 rtb_angle_des_idx_0 -= rtU.phi; 00166 00167 // Gain: '<S181>/Filter Coefficient' incorporates: 00168 // DiscreteIntegrator: '<S173>/Filter' 00169 // Gain: '<S172>/Derivative Gain' 00170 // Sum: '<S173>/SumD' 00171 00172 rtb_TmpSignalConversionAtSFun_0 = (0.171115754916436 * rtb_angle_des_idx_0 - 00173 rtDW.Filter_DSTATE_b) * 176.087006843524; 00174 00175 // Sum: '<S16>/Add2' incorporates: 00176 // Inport: '<Root>/theta' 00177 00178 rtb_TmpSignalConversionAtSFun_1 -= rtU.theta; 00179 00180 // Gain: '<S49>/Filter Coefficient' incorporates: 00181 // DiscreteIntegrator: '<S41>/Filter' 00182 // Gain: '<S40>/Derivative Gain' 00183 // Sum: '<S41>/SumD' 00184 00185 rtb_FilterCoefficient_p = (0.171115754916436 * rtb_TmpSignalConversionAtSFun_1 00186 - rtDW.Filter_DSTATE_a) * 176.087006843524; 00187 00188 // Sum: '<S16>/Add3' incorporates: 00189 // Inport: '<Root>/psy' 00190 00191 rtb_Integrator_nd = rtb_VectorConcatenate_idx_1 - rtU.psy; 00192 00193 // Gain: '<S93>/Filter Coefficient' incorporates: 00194 // DiscreteIntegrator: '<S85>/Filter' 00195 // Gain: '<S84>/Derivative Gain' 00196 // Sum: '<S85>/SumD' 00197 00198 rtb_FilterCoefficient_e = (0.0490884878345462 * rtb_Integrator_nd - 00199 rtDW.Filter_DSTATE_ai) * 270.43836739932; 00200 00201 // SignalConversion: '<S7>/ConcatBufferAtVector ConcatenateIn2' incorporates: 00202 // DiscreteIntegrator: '<S178>/Integrator' 00203 // DiscreteIntegrator: '<S46>/Integrator' 00204 // DiscreteIntegrator: '<S90>/Integrator' 00205 // Gain: '<S183>/Proportional Gain' 00206 // Gain: '<S51>/Proportional Gain' 00207 // Gain: '<S95>/Proportional Gain' 00208 // Sum: '<S187>/Sum' 00209 // Sum: '<S55>/Sum' 00210 // Sum: '<S99>/Sum' 00211 00212 rtb_VectorConcatenate_idx_1 = (0.421511657975665 * rtb_angle_des_idx_0 + 00213 rtDW.Integrator_DSTATE_p) + rtb_TmpSignalConversionAtSFun_0; 00214 rtb_VectorConcatenate_idx_2 = (0.421511657975665 * 00215 rtb_TmpSignalConversionAtSFun_1 + rtDW.Integrator_DSTATE_b) + 00216 rtb_FilterCoefficient_p; 00217 rtb_VectorConcatenate_idx_3 = (0.00309186213155581 * rtb_Integrator_nd + 00218 rtDW.Integrator_DSTATE_m) + rtb_FilterCoefficient_e; 00219 00220 // Sum: '<S15>/Add3' incorporates: 00221 // Gain: '<S7>/Gain1' 00222 // Inport: '<Root>/altitude' 00223 // Inport: '<Root>/r_z' 00224 // MATLAB Function: '<Root>/MATLAB Function3' 00225 00226 rtb_Add3 = (rtU.altitude - 1000.0) * 2.0 / 1000.0 * 0.0066666666666666671 - 00227 rtU.r_z; 00228 00229 // Gain: '<S225>/Filter Coefficient' incorporates: 00230 // DiscreteIntegrator: '<S217>/Filter' 00231 // Gain: '<S216>/Derivative Gain' 00232 // Sum: '<S217>/SumD' 00233 00234 rtb_FilterCoefficient_o = (3.68 * rtb_Add3 - rtDW.Filter_DSTATE_o) * 00235 19.9677854964375; 00236 00237 // Sum: '<S7>/Add' incorporates: 00238 // DiscreteIntegrator: '<S222>/Integrator' 00239 // Gain: '<S227>/Proportional Gain' 00240 // Sum: '<S231>/Sum' 00241 00242 rtb_VectorConcatenate_idx_0 = ((3.1623 * rtb_Add3 + rtDW.Integrator_DSTATE_bz) 00243 + rtb_FilterCoefficient_o) + 7.3575; 00244 00245 // Update for DiscreteIntegrator: '<S134>/Integrator' incorporates: 00246 // Gain: '<S131>/Integral Gain' 00247 00248 rtDW.Integrator_DSTATE += 0.04922 * rtb_Integrator_m * 0.0023; 00249 00250 // Update for DiscreteIntegrator: '<S129>/Filter' 00251 rtDW.Filter_DSTATE += 0.0023 * rtb_Filter_d; 00252 00253 // Update for DiscreteIntegrator: '<S266>/Integrator' incorporates: 00254 // Gain: '<S263>/Integral Gain' 00255 00256 rtDW.Integrator_DSTATE_c += 0.0492289691165195 * rtb_Gain1_idx_1 * 0.0023; 00257 00258 // Update for DiscreteIntegrator: '<S261>/Filter' 00259 rtDW.Filter_DSTATE_p += 0.0023 * rtb_FilterCoefficient_b; 00260 00261 // Update for DiscreteIntegrator: '<S178>/Integrator' incorporates: 00262 // Gain: '<S175>/Integral Gain' 00263 00264 rtDW.Integrator_DSTATE_p += 0.0 * rtb_angle_des_idx_0 * 0.0023; 00265 00266 // Update for DiscreteIntegrator: '<S173>/Filter' 00267 rtDW.Filter_DSTATE_b += 0.0023 * rtb_TmpSignalConversionAtSFun_0; 00268 00269 // Update for DiscreteIntegrator: '<S46>/Integrator' incorporates: 00270 // Gain: '<S43>/Integral Gain' 00271 00272 rtDW.Integrator_DSTATE_b += 0.0 * rtb_TmpSignalConversionAtSFun_1 * 0.0023; 00273 00274 // Update for DiscreteIntegrator: '<S41>/Filter' 00275 rtDW.Filter_DSTATE_a += 0.0023 * rtb_FilterCoefficient_p; 00276 00277 // Update for DiscreteIntegrator: '<S90>/Integrator' incorporates: 00278 // Gain: '<S87>/Integral Gain' 00279 00280 rtDW.Integrator_DSTATE_m += 0.0 * rtb_Integrator_nd * 0.0023; 00281 00282 // Update for DiscreteIntegrator: '<S85>/Filter' 00283 rtDW.Filter_DSTATE_ai += 0.0023 * rtb_FilterCoefficient_e; 00284 00285 // Update for DiscreteIntegrator: '<S222>/Integrator' incorporates: 00286 // Gain: '<S219>/Integral Gain' 00287 00288 rtDW.Integrator_DSTATE_bz += 0.0506661578399485 * rtb_Add3 * 0.0023; 00289 00290 // Update for DiscreteIntegrator: '<S217>/Filter' 00291 rtDW.Filter_DSTATE_o += 0.0023 * rtb_FilterCoefficient_o; 00292 00293 // End of Outputs for SubSystem: '<Root>/small_angle_control' 00294 00295 // MATLAB Function: '<Root>/MATLAB Function1' 00296 for (i = 0; i < 4; i++) { 00297 rtb_Integrator_m = a[i + 12] * rtb_VectorConcatenate_idx_3 + (a[i + 8] * 00298 rtb_VectorConcatenate_idx_2 + (a[i + 4] * rtb_VectorConcatenate_idx_1 + 00299 a[i] * rtb_VectorConcatenate_idx_0)); 00300 A[i] = rtb_Integrator_m; 00301 } 00302 00303 if (A[0] < 0.0) { 00304 A[0] = 0.0; 00305 } 00306 00307 if (A[1] < 0.0) { 00308 A[1] = 0.0; 00309 } 00310 00311 if (A[2] < 0.0) { 00312 A[2] = 0.0; 00313 } 00314 00315 if (A[3] < 0.0) { 00316 A[3] = 0.0; 00317 } 00318 00319 // MATLAB Function: '<Root>/rpm2duty' incorporates: 00320 // MATLAB Function: '<Root>/MATLAB Function1' 00321 00322 rtb_Integrator_m = (3485.3574033876184 * std::sqrt(A[0]) * 0.1865 + 800.0) / 00323 20000.0; 00324 rtb_Filter_d = (3485.3574033876184 * std::sqrt(A[1]) * 0.1865 + 800.0) / 00325 20000.0; 00326 rtb_Add3 = (3485.3574033876184 * std::sqrt(A[2]) * 0.1865 + 800.0) / 20000.0; 00327 rtb_Gain1_idx_1 = (3485.3574033876184 * std::sqrt(A[3]) * 0.1865 + 800.0) / 00328 20000.0; 00329 00330 // Saturate: '<Root>/Saturation' 00331 if (rtb_Integrator_m > 0.1) { 00332 // Outport: '<Root>/M1' 00333 rtY.M1 = 0.1; 00334 } else if (rtb_Integrator_m < 0.05) { 00335 // Outport: '<Root>/M1' 00336 rtY.M1 = 0.05; 00337 } else { 00338 // Outport: '<Root>/M1' 00339 rtY.M1 = rtb_Integrator_m; 00340 } 00341 00342 if (rtb_Filter_d > 0.1) { 00343 // Outport: '<Root>/M2' 00344 rtY.M2 = 0.1; 00345 } else if (rtb_Filter_d < 0.05) { 00346 // Outport: '<Root>/M2' 00347 rtY.M2 = 0.05; 00348 } else { 00349 // Outport: '<Root>/M2' 00350 rtY.M2 = rtb_Filter_d; 00351 } 00352 00353 if (rtb_Add3 > 0.1) { 00354 // Outport: '<Root>/M3' 00355 rtY.M3 = 0.1; 00356 } else if (rtb_Add3 < 0.05) { 00357 // Outport: '<Root>/M3' 00358 rtY.M3 = 0.05; 00359 } else { 00360 // Outport: '<Root>/M3' 00361 rtY.M3 = rtb_Add3; 00362 } 00363 00364 if (rtb_Gain1_idx_1 > 0.1) { 00365 // Outport: '<Root>/M4' 00366 rtY.M4 = 0.1; 00367 } else if (rtb_Gain1_idx_1 < 0.05) { 00368 // Outport: '<Root>/M4' 00369 rtY.M4 = 0.05; 00370 } else { 00371 // Outport: '<Root>/M4' 00372 rtY.M4 = rtb_Gain1_idx_1; 00373 } 00374 00375 // End of Saturate: '<Root>/Saturation' 00376 } 00377 00378 // Model initialize function 00379 void S_con1ModelClass::initialize() 00380 { 00381 // (no initialization code required) 00382 } 00383 00384 // Constructor 00385 S_con1ModelClass::S_con1ModelClass() 00386 { 00387 // Currently there is no constructor body generated. 00388 } 00389 00390 // Destructor 00391 S_con1ModelClass::~S_con1ModelClass() 00392 { 00393 // Currently there is no destructor body generated. 00394 } 00395 00396 // Real-Time Model get method 00397 RT_MODEL * S_con1ModelClass::getRTM() 00398 { 00399 return (&rtM); 00400 } 00401 00402 // 00403 // File trailer for generated code. 00404 // 00405 // [EOF] 00406 //
Generated on Wed Jul 13 2022 10:53:59 by
1.7.2