Diff: S_con1.cpp

Revision:

0:75d9a6364691

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/S_con1.cpp	Mon Apr 13 19:08:23 2020 +0000
@@ -0,0 +1,406 @@
+//
+// File: S_con1.cpp
+//
+// Code generated for Simulink model 'S_con1'.
+//
+// Model version                  : 1.1
+// Simulink Coder version         : 9.1 (R2019a) 23-Nov-2018
+// C/C++ source code generated on : Mon Feb  3 20:42:16 2020
+//
+// Target selection: ert.tlc
+// Embedded hardware selection: ARM Compatible->ARM 7
+// Code generation objectives:
+//    1. Execution efficiency
+//    2. RAM efficiency
+// Validation result: Not run
+//
+#include "S_con1.h"
+
+static void MATLABFunction(real_T rtu_pwm, real_T *rty_y);
+
+//
+// Output and update for atomic system:
+//    '<Root>/MATLAB Function'
+//    '<Root>/MATLAB Function2'
+//    '<Root>/MATLAB Function4'
+//
+static void MATLABFunction(real_T rtu_pwm, real_T *rty_y)
+{
+  *rty_y = (rtu_pwm - 1500.0) / 1000.0;
+}
+
+// Model step function
+void S_con1ModelClass::step()
+{
+  real_T A[4];
+  static const real_T a[16] = { 0.25, 0.25, 0.25, 0.25, 0.0, 2.193, 0.0, -2.193,
+    -2.193, 0.0, 2.193, 0.0, 4.2591, -4.2591, 4.2591, -4.2591 };
+
+  real_T rtb_FilterCoefficient_b;
+  real_T rtb_FilterCoefficient_p;
+  real_T rtb_Integrator_nd;
+  real_T rtb_Integrator_m;
+  real_T rtb_Filter_d;
+  real_T rtb_FilterCoefficient_e;
+  real_T rtb_Add3;
+  real_T rtb_FilterCoefficient_o;
+  int32_T i;
+  real_T rtb_VectorConcatenate_idx_0;
+  real_T rtb_Gain1_idx_1;
+  real_T rtb_VectorConcatenate_idx_1;
+  real_T rtb_VectorConcatenate_idx_2;
+  real_T rtb_VectorConcatenate_idx_3;
+  real_T rtb_TmpSignalConversionAtSFun_0;
+  real_T rtb_TmpSignalConversionAtSFun_1;
+  real_T rtb_angle_des_idx_0;
+
+  // MATLAB Function: '<Root>/MATLAB Function' incorporates:
+  //   Inport: '<Root>/pitch'
+
+  MATLABFunction(rtU.pitch, &rtb_Integrator_m);
+
+  // MATLAB Function: '<Root>/MATLAB Function2' incorporates:
+  //   Inport: '<Root>/roll'
+
+  MATLABFunction(rtU.roll, &rtb_Filter_d);
+
+  // MATLAB Function: '<Root>/MATLAB Function4' incorporates:
+  //   Inport: '<Root>/yaw'
+
+  MATLABFunction(rtU.yaw, &rtb_Add3);
+
+  // Outputs for Atomic SubSystem: '<Root>/small_angle_control'
+  // Gain: '<S7>/Gain1'
+  rtb_Gain1_idx_1 = 0.0066666666666666671 * rtb_Filter_d;
+
+  // Sum: '<S15>/Add1' incorporates:
+  //   Gain: '<S7>/Gain1'
+  //   Inport: '<Root>/vx'
+
+  rtb_Integrator_m = 0.0066666666666666671 * rtb_Integrator_m - rtU.vx;
+
+  // Gain: '<S137>/Filter Coefficient' incorporates:
+  //   DiscreteIntegrator: '<S129>/Filter'
+  //   Gain: '<S128>/Derivative Gain'
+  //   Sum: '<S129>/SumD'
+
+  rtb_Filter_d = (1.68 * rtb_Integrator_m - rtDW.Filter_DSTATE) * 100.0;
+
+  // Sum: '<S15>/Add2' incorporates:
+  //   Inport: '<Root>/vy'
+
+  rtb_Gain1_idx_1 -= rtU.vy;
+
+  // Gain: '<S269>/Filter Coefficient' incorporates:
+  //   DiscreteIntegrator: '<S261>/Filter'
+  //   Gain: '<S260>/Derivative Gain'
+  //   Sum: '<S261>/SumD'
+
+  rtb_FilterCoefficient_b = (1.6832 * rtb_Gain1_idx_1 - rtDW.Filter_DSTATE_p) *
+    206.264558996617;
+
+  // SignalConversion: '<S14>/TmpSignal ConversionAt SFunction Inport1' incorporates:
+  //   DiscreteIntegrator: '<S134>/Integrator'
+  //   DiscreteIntegrator: '<S266>/Integrator'
+  //   Gain: '<S139>/Proportional Gain'
+  //   Gain: '<S271>/Proportional Gain'
+  //   MATLAB Function: '<S7>/MATLAB Function4'
+  //   Sum: '<S143>/Sum'
+  //   Sum: '<S275>/Sum'
+
+  rtb_TmpSignalConversionAtSFun_0 = (0.57 * rtb_Integrator_m +
+    rtDW.Integrator_DSTATE) + rtb_Filter_d;
+  rtb_TmpSignalConversionAtSFun_1 = (0.57 * rtb_Gain1_idx_1 +
+    rtDW.Integrator_DSTATE_c) + rtb_FilterCoefficient_b;
+
+  // MATLAB Function: '<S7>/MATLAB Function4'
+  rtb_FilterCoefficient_p = std::cos(rtb_Add3);
+  rtb_VectorConcatenate_idx_1 = std::sin(rtb_Add3);
+  rtb_angle_des_idx_0 = (rtb_TmpSignalConversionAtSFun_0 *
+    rtb_VectorConcatenate_idx_1 - rtb_TmpSignalConversionAtSFun_1 *
+    rtb_FilterCoefficient_p) * 0.1019367991845056;
+  rtb_TmpSignalConversionAtSFun_1 = (rtb_TmpSignalConversionAtSFun_0 *
+    rtb_FilterCoefficient_p + rtb_TmpSignalConversionAtSFun_1 *
+    rtb_VectorConcatenate_idx_1) * 0.1019367991845056;
+
+  // Saturate: '<S7>/Saturation'
+  rtb_VectorConcatenate_idx_1 = rtb_angle_des_idx_0;
+  if (rtb_angle_des_idx_0 > 0.26179938779914941) {
+    rtb_VectorConcatenate_idx_1 = 0.26179938779914941;
+  } else {
+    if (rtb_angle_des_idx_0 < -0.26179938779914941) {
+      rtb_VectorConcatenate_idx_1 = -0.26179938779914941;
+    }
+  }
+
+  rtb_angle_des_idx_0 = rtb_VectorConcatenate_idx_1;
+  rtb_VectorConcatenate_idx_1 = rtb_TmpSignalConversionAtSFun_1;
+  if (rtb_TmpSignalConversionAtSFun_1 > 0.26179938779914941) {
+    rtb_VectorConcatenate_idx_1 = 0.26179938779914941;
+  } else {
+    if (rtb_TmpSignalConversionAtSFun_1 < -0.26179938779914941) {
+      rtb_VectorConcatenate_idx_1 = -0.26179938779914941;
+    }
+  }
+
+  rtb_TmpSignalConversionAtSFun_1 = rtb_VectorConcatenate_idx_1;
+
+  // MATLAB Function: '<S7>/MATLAB Function4'
+  rtb_VectorConcatenate_idx_1 = rtb_Add3;
+
+  // Saturate: '<S7>/Saturation' incorporates:
+  //   MATLAB Function: '<S7>/MATLAB Function4'
+
+  if (rtb_Add3 > 0.26179938779914941) {
+    rtb_VectorConcatenate_idx_1 = 0.26179938779914941;
+  } else {
+    if (rtb_Add3 < -0.26179938779914941) {
+      rtb_VectorConcatenate_idx_1 = -0.26179938779914941;
+    }
+  }
+
+  // Sum: '<S16>/Add1' incorporates:
+  //   Inport: '<Root>/phi'
+
+  rtb_angle_des_idx_0 -= rtU.phi;
+
+  // Gain: '<S181>/Filter Coefficient' incorporates:
+  //   DiscreteIntegrator: '<S173>/Filter'
+  //   Gain: '<S172>/Derivative Gain'
+  //   Sum: '<S173>/SumD'
+
+  rtb_TmpSignalConversionAtSFun_0 = (0.171115754916436 * rtb_angle_des_idx_0 -
+    rtDW.Filter_DSTATE_b) * 176.087006843524;
+
+  // Sum: '<S16>/Add2' incorporates:
+  //   Inport: '<Root>/theta'
+
+  rtb_TmpSignalConversionAtSFun_1 -= rtU.theta;
+
+  // Gain: '<S49>/Filter Coefficient' incorporates:
+  //   DiscreteIntegrator: '<S41>/Filter'
+  //   Gain: '<S40>/Derivative Gain'
+  //   Sum: '<S41>/SumD'
+
+  rtb_FilterCoefficient_p = (0.171115754916436 * rtb_TmpSignalConversionAtSFun_1
+    - rtDW.Filter_DSTATE_a) * 176.087006843524;
+
+  // Sum: '<S16>/Add3' incorporates:
+  //   Inport: '<Root>/psy'
+
+  rtb_Integrator_nd = rtb_VectorConcatenate_idx_1 - rtU.psy;
+
+  // Gain: '<S93>/Filter Coefficient' incorporates:
+  //   DiscreteIntegrator: '<S85>/Filter'
+  //   Gain: '<S84>/Derivative Gain'
+  //   Sum: '<S85>/SumD'
+
+  rtb_FilterCoefficient_e = (0.0490884878345462 * rtb_Integrator_nd -
+    rtDW.Filter_DSTATE_ai) * 270.43836739932;
+
+  // SignalConversion: '<S7>/ConcatBufferAtVector ConcatenateIn2' incorporates:
+  //   DiscreteIntegrator: '<S178>/Integrator'
+  //   DiscreteIntegrator: '<S46>/Integrator'
+  //   DiscreteIntegrator: '<S90>/Integrator'
+  //   Gain: '<S183>/Proportional Gain'
+  //   Gain: '<S51>/Proportional Gain'
+  //   Gain: '<S95>/Proportional Gain'
+  //   Sum: '<S187>/Sum'
+  //   Sum: '<S55>/Sum'
+  //   Sum: '<S99>/Sum'
+
+  rtb_VectorConcatenate_idx_1 = (0.421511657975665 * rtb_angle_des_idx_0 +
+    rtDW.Integrator_DSTATE_p) + rtb_TmpSignalConversionAtSFun_0;
+  rtb_VectorConcatenate_idx_2 = (0.421511657975665 *
+    rtb_TmpSignalConversionAtSFun_1 + rtDW.Integrator_DSTATE_b) +
+    rtb_FilterCoefficient_p;
+  rtb_VectorConcatenate_idx_3 = (0.00309186213155581 * rtb_Integrator_nd +
+    rtDW.Integrator_DSTATE_m) + rtb_FilterCoefficient_e;
+
+  // Sum: '<S15>/Add3' incorporates:
+  //   Gain: '<S7>/Gain1'
+  //   Inport: '<Root>/altitude'
+  //   Inport: '<Root>/r_z'
+  //   MATLAB Function: '<Root>/MATLAB Function3'
+
+  rtb_Add3 = (rtU.altitude - 1000.0) * 2.0 / 1000.0 * 0.0066666666666666671 -
+    rtU.r_z;
+
+  // Gain: '<S225>/Filter Coefficient' incorporates:
+  //   DiscreteIntegrator: '<S217>/Filter'
+  //   Gain: '<S216>/Derivative Gain'
+  //   Sum: '<S217>/SumD'
+
+  rtb_FilterCoefficient_o = (3.68 * rtb_Add3 - rtDW.Filter_DSTATE_o) *
+    19.9677854964375;
+
+  // Sum: '<S7>/Add' incorporates:
+  //   DiscreteIntegrator: '<S222>/Integrator'
+  //   Gain: '<S227>/Proportional Gain'
+  //   Sum: '<S231>/Sum'
+
+  rtb_VectorConcatenate_idx_0 = ((3.1623 * rtb_Add3 + rtDW.Integrator_DSTATE_bz)
+    + rtb_FilterCoefficient_o) + 7.3575;
+
+  // Update for DiscreteIntegrator: '<S134>/Integrator' incorporates:
+  //   Gain: '<S131>/Integral Gain'
+
+  rtDW.Integrator_DSTATE += 0.04922 * rtb_Integrator_m * 0.0023;
+
+  // Update for DiscreteIntegrator: '<S129>/Filter'
+  rtDW.Filter_DSTATE += 0.0023 * rtb_Filter_d;
+
+  // Update for DiscreteIntegrator: '<S266>/Integrator' incorporates:
+  //   Gain: '<S263>/Integral Gain'
+
+  rtDW.Integrator_DSTATE_c += 0.0492289691165195 * rtb_Gain1_idx_1 * 0.0023;
+
+  // Update for DiscreteIntegrator: '<S261>/Filter'
+  rtDW.Filter_DSTATE_p += 0.0023 * rtb_FilterCoefficient_b;
+
+  // Update for DiscreteIntegrator: '<S178>/Integrator' incorporates:
+  //   Gain: '<S175>/Integral Gain'
+
+  rtDW.Integrator_DSTATE_p += 0.0 * rtb_angle_des_idx_0 * 0.0023;
+
+  // Update for DiscreteIntegrator: '<S173>/Filter'
+  rtDW.Filter_DSTATE_b += 0.0023 * rtb_TmpSignalConversionAtSFun_0;
+
+  // Update for DiscreteIntegrator: '<S46>/Integrator' incorporates:
+  //   Gain: '<S43>/Integral Gain'
+
+  rtDW.Integrator_DSTATE_b += 0.0 * rtb_TmpSignalConversionAtSFun_1 * 0.0023;
+
+  // Update for DiscreteIntegrator: '<S41>/Filter'
+  rtDW.Filter_DSTATE_a += 0.0023 * rtb_FilterCoefficient_p;
+
+  // Update for DiscreteIntegrator: '<S90>/Integrator' incorporates:
+  //   Gain: '<S87>/Integral Gain'
+
+  rtDW.Integrator_DSTATE_m += 0.0 * rtb_Integrator_nd * 0.0023;
+
+  // Update for DiscreteIntegrator: '<S85>/Filter'
+  rtDW.Filter_DSTATE_ai += 0.0023 * rtb_FilterCoefficient_e;
+
+  // Update for DiscreteIntegrator: '<S222>/Integrator' incorporates:
+  //   Gain: '<S219>/Integral Gain'
+
+  rtDW.Integrator_DSTATE_bz += 0.0506661578399485 * rtb_Add3 * 0.0023;
+
+  // Update for DiscreteIntegrator: '<S217>/Filter'
+  rtDW.Filter_DSTATE_o += 0.0023 * rtb_FilterCoefficient_o;
+
+  // End of Outputs for SubSystem: '<Root>/small_angle_control'
+
+  // MATLAB Function: '<Root>/MATLAB Function1'
+  for (i = 0; i < 4; i++) {
+    rtb_Integrator_m = a[i + 12] * rtb_VectorConcatenate_idx_3 + (a[i + 8] *
+      rtb_VectorConcatenate_idx_2 + (a[i + 4] * rtb_VectorConcatenate_idx_1 +
+      a[i] * rtb_VectorConcatenate_idx_0));
+    A[i] = rtb_Integrator_m;
+  }
+
+  if (A[0] < 0.0) {
+    A[0] = 0.0;
+  }
+
+  if (A[1] < 0.0) {
+    A[1] = 0.0;
+  }
+
+  if (A[2] < 0.0) {
+    A[2] = 0.0;
+  }
+
+  if (A[3] < 0.0) {
+    A[3] = 0.0;
+  }
+
+  // MATLAB Function: '<Root>/rpm2duty' incorporates:
+  //   MATLAB Function: '<Root>/MATLAB Function1'
+
+  rtb_Integrator_m = (3485.3574033876184 * std::sqrt(A[0]) * 0.1865 + 800.0) /
+    20000.0;
+  rtb_Filter_d = (3485.3574033876184 * std::sqrt(A[1]) * 0.1865 + 800.0) /
+    20000.0;
+  rtb_Add3 = (3485.3574033876184 * std::sqrt(A[2]) * 0.1865 + 800.0) / 20000.0;
+  rtb_Gain1_idx_1 = (3485.3574033876184 * std::sqrt(A[3]) * 0.1865 + 800.0) /
+    20000.0;
+
+  // Saturate: '<Root>/Saturation'
+  if (rtb_Integrator_m > 0.1) {
+    // Outport: '<Root>/M1'
+    rtY.M1 = 0.1;
+  } else if (rtb_Integrator_m < 0.05) {
+    // Outport: '<Root>/M1'
+    rtY.M1 = 0.05;
+  } else {
+    // Outport: '<Root>/M1'
+    rtY.M1 = rtb_Integrator_m;
+  }
+
+  if (rtb_Filter_d > 0.1) {
+    // Outport: '<Root>/M2'
+    rtY.M2 = 0.1;
+  } else if (rtb_Filter_d < 0.05) {
+    // Outport: '<Root>/M2'
+    rtY.M2 = 0.05;
+  } else {
+    // Outport: '<Root>/M2'
+    rtY.M2 = rtb_Filter_d;
+  }
+
+  if (rtb_Add3 > 0.1) {
+    // Outport: '<Root>/M3'
+    rtY.M3 = 0.1;
+  } else if (rtb_Add3 < 0.05) {
+    // Outport: '<Root>/M3'
+    rtY.M3 = 0.05;
+  } else {
+    // Outport: '<Root>/M3'
+    rtY.M3 = rtb_Add3;
+  }
+
+  if (rtb_Gain1_idx_1 > 0.1) {
+    // Outport: '<Root>/M4'
+    rtY.M4 = 0.1;
+  } else if (rtb_Gain1_idx_1 < 0.05) {
+    // Outport: '<Root>/M4'
+    rtY.M4 = 0.05;
+  } else {
+    // Outport: '<Root>/M4'
+    rtY.M4 = rtb_Gain1_idx_1;
+  }
+
+  // End of Saturate: '<Root>/Saturation'
+}
+
+// Model initialize function
+void S_con1ModelClass::initialize()
+{
+  // (no initialization code required)
+}
+
+// Constructor
+S_con1ModelClass::S_con1ModelClass()
+{
+  // Currently there is no constructor body generated.
+}
+
+// Destructor
+S_con1ModelClass::~S_con1ModelClass()
+{
+  // Currently there is no destructor body generated.
+}
+
+// Real-Time Model get method
+RT_MODEL * S_con1ModelClass::getRTM()
+{
+  return (&rtM);
+}
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//