matlab code gen

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers S_con1.cpp Source File

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 //