/*
 * control_vectorial.cpp
 *
 * 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.
 *
 * Code generation for model "control_vectorial".
 *
 * Model version              : 1.33
 * Simulink Coder version : 24.2 (R2024b) 21-Jun-2024
 * C++ source code generated on : Mon Jul 28 10:28:03 2025
 *
 * Target selection: speedgoat.tlc
 * Note: GRT includes extra infrastructure and instrumentation for prototyping
 * Embedded hardware selection: Intel->x86-64 (Linux 64)
 * Code generation objectives: Unspecified
 * Validation result: Not run
 */

#include "control_vectorial.h"
#include "control_vectorial_cal.h"
#include "rtwtypes.h"
#include "rte_control_vectorial_parameters.h"
#include <cmath>

extern "C"
{

#include "rt_nonfinite.h"

}

#include "control_vectorial_private.h"
#include "zero_crossing_types.h"
#include <cstring>
#include "rt_defines.h"
#include <cfloat>

/* Block signals (default storage) */
B_control_vectorial_T control_vectorial_B;

/* Continuous states */
X_control_vectorial_T control_vectorial_X;

/* Disabled State Vector */
XDis_control_vectorial_T control_vectorial_XDis;

/* Block states (default storage) */
DW_control_vectorial_T control_vectorial_DW;

/* Previous zero-crossings (trigger) states */
PrevZCX_control_vectorial_T control_vectorial_PrevZCX;

/* Real-time model */
RT_MODEL_control_vectorial_T control_vectorial_M_ = RT_MODEL_control_vectorial_T
  ();
RT_MODEL_control_vectorial_T *const control_vectorial_M = &control_vectorial_M_;
real_T look1_pbinlxpw(real_T u0, const real_T bp0[], const real_T table[],
                      uint32_T prevIndex[], uint32_T maxIndex)
{
  real_T frac;
  real_T yL_0d0;
  uint32_T bpIdx;

  /* Column-major Lookup 1-D
     Search method: 'binary'
     Use previous index: 'on'
     Interpolation method: 'Linear point-slope'
     Extrapolation method: 'Linear'
     Use last breakpoint for index at or above upper limit: 'off'
     Remove protection against out-of-range input in generated code: 'off'
   */
  /* Prelookup - Index and Fraction
     Index Search method: 'binary'
     Extrapolation method: 'Linear'
     Use previous index: 'on'
     Use last breakpoint for index at or above upper limit: 'off'
     Remove protection against out-of-range input in generated code: 'off'
   */
  if (u0 <= bp0[0U]) {
    bpIdx = 0U;
    frac = (u0 - bp0[0U]) / (bp0[1U] - bp0[0U]);
  } else if (u0 < bp0[maxIndex]) {
    uint32_T found;
    uint32_T iLeft;
    uint32_T iRght;

    /* Binary Search using Previous Index */
    bpIdx = prevIndex[0U];
    iLeft = 0U;
    iRght = maxIndex;
    found = 0U;
    while (found == 0U) {
      if (u0 < bp0[bpIdx]) {
        iRght = bpIdx - 1U;
        bpIdx = ((bpIdx + iLeft) - 1U) >> 1U;
      } else if (u0 < bp0[bpIdx + 1U]) {
        found = 1U;
      } else {
        iLeft = bpIdx + 1U;
        bpIdx = ((bpIdx + iRght) + 1U) >> 1U;
      }
    }

    frac = (u0 - bp0[bpIdx]) / (bp0[bpIdx + 1U] - bp0[bpIdx]);
  } else {
    bpIdx = maxIndex - 1U;
    frac = (u0 - bp0[maxIndex - 1U]) / (bp0[maxIndex] - bp0[maxIndex - 1U]);
  }

  prevIndex[0U] = bpIdx;

  /* Column-major Interpolation 1-D
     Interpolation method: 'Linear point-slope'
     Use last breakpoint for index at or above upper limit: 'off'
     Overflow mode: 'portable wrapping'
   */
  yL_0d0 = table[bpIdx];
  return (table[bpIdx + 1U] - yL_0d0) * frac + yL_0d0;
}

uint32_T plook_binx(real_T u, const real_T bp[], uint32_T maxIndex, real_T
                    *fraction)
{
  uint32_T bpIndex;

  /* Prelookup - Index and Fraction
     Index Search method: 'binary'
     Extrapolation method: 'Linear'
     Use previous index: 'off'
     Use last breakpoint for index at or above upper limit: 'off'
     Remove protection against out-of-range input in generated code: 'off'
   */
  if (u <= bp[0U]) {
    bpIndex = 0U;
    *fraction = (u - bp[0U]) / (bp[1U] - bp[0U]);
  } else if (u < bp[maxIndex]) {
    bpIndex = binsearch_u32d(u, bp, maxIndex >> 1U, maxIndex);
    *fraction = (u - bp[bpIndex]) / (bp[bpIndex + 1U] - bp[bpIndex]);
  } else {
    bpIndex = maxIndex - 1U;
    *fraction = (u - bp[maxIndex - 1U]) / (bp[maxIndex] - bp[maxIndex - 1U]);
  }

  return bpIndex;
}

real_T intrp3d_l_pw(const uint32_T bpIndex[], const real_T frac[], const real_T
                    table[], const uint32_T stride[])
{
  real_T yL_0d0;
  real_T yL_1d;
  real_T yL_2d;
  uint32_T offset_0d;
  uint32_T offset_2d;

  /* Column-major Interpolation 3-D
     Interpolation method: 'Linear point-slope'
     Use last breakpoint for index at or above upper limit: 'off'
     Overflow mode: 'portable wrapping'
   */
  offset_2d = (bpIndex[2U] * stride[2U] + bpIndex[1U] * stride[1U]) + bpIndex[0U];
  yL_0d0 = table[offset_2d];
  yL_1d = (table[offset_2d + 1U] - yL_0d0) * frac[0U] + yL_0d0;
  offset_0d = offset_2d + stride[1U];
  yL_0d0 = table[offset_0d];
  yL_2d = (((table[offset_0d + 1U] - yL_0d0) * frac[0U] + yL_0d0) - yL_1d) *
    frac[1U] + yL_1d;
  offset_2d += stride[2U];
  yL_0d0 = table[offset_2d];
  yL_1d = (table[offset_2d + 1U] - yL_0d0) * frac[0U] + yL_0d0;
  offset_0d = offset_2d + stride[1U];
  yL_0d0 = table[offset_0d];
  return (((((table[offset_0d + 1U] - yL_0d0) * frac[0U] + yL_0d0) - yL_1d) *
           frac[1U] + yL_1d) - yL_2d) * frac[2U] + yL_2d;
}

uint32_T binsearch_u32d(real_T u, const real_T bp[], uint32_T startIndex,
  uint32_T maxIndex)
{
  uint32_T bpIdx;
  uint32_T bpIndex;
  uint32_T iRght;

  /* Binary Search */
  bpIdx = startIndex;
  bpIndex = 0U;
  iRght = maxIndex;
  while (iRght - bpIndex > 1U) {
    if (u < bp[bpIdx]) {
      iRght = bpIdx;
    } else {
      bpIndex = bpIdx;
    }

    bpIdx = (iRght + bpIndex) >> 1U;
  }

  return bpIndex;
}

/*
 * This function updates continuous states using the ODE3 fixed-step
 * solver algorithm
 */
static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si )
{
  /* Solver Matrices */
  static const real_T rt_ODE3_A[3] = {
    1.0/2.0, 3.0/4.0, 1.0
  };

  static const real_T rt_ODE3_B[3][3] = {
    { 1.0/2.0, 0.0, 0.0 },

    { 0.0, 3.0/4.0, 0.0 },

    { 2.0/9.0, 1.0/3.0, 4.0/9.0 }
  };

  time_T t = rtsiGetT(si);
  time_T tnew = rtsiGetSolverStopTime(si);
  time_T h = rtsiGetStepSize(si);
  real_T *x = rtsiGetContStates(si);
  ODE3_IntgData *id = static_cast<ODE3_IntgData *>(rtsiGetSolverData(si));
  real_T *y = id->y;
  real_T *f0 = id->f[0];
  real_T *f1 = id->f[1];
  real_T *f2 = id->f[2];
  real_T hB[3];
  int_T i;
  int_T nXc = 5;
  rtsiSetSimTimeStep(si,MINOR_TIME_STEP);

  /* Save the state values at time t in y, we'll use x as ynew. */
  (void) std::memcpy(y, x,
                     static_cast<uint_T>(nXc)*sizeof(real_T));

  /* Assumes that rtsiSetT and ModelOutputs are up-to-date */
  /* f0 = f(t,y) */
  rtsiSetdX(si, f0);
  control_vectorial_derivatives();

  /* f(:,2) = feval(odefile, t + hA(1), y + f*hB(:,1), args(:)(*)); */
  hB[0] = h * rt_ODE3_B[0][0];
  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0]);
  }

  rtsiSetT(si, t + h*rt_ODE3_A[0]);
  rtsiSetdX(si, f1);
  control_vectorial_step0();
  control_vectorial_derivatives();

  /* f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*)); */
  for (i = 0; i <= 1; i++) {
    hB[i] = h * rt_ODE3_B[1][i];
  }

  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1]);
  }

  rtsiSetT(si, t + h*rt_ODE3_A[1]);
  rtsiSetdX(si, f2);
  control_vectorial_step0();
  control_vectorial_derivatives();

  /* tnew = t + hA(3);
     ynew = y + f*hB(:,3); */
  for (i = 0; i <= 2; i++) {
    hB[i] = h * rt_ODE3_B[2][i];
  }

  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2]);
  }

  rtsiSetT(si, tnew);
  rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
}

real_T rt_hypotd_snf(real_T u0, real_T u1)
{
  real_T a;
  real_T b;
  real_T y;
  a = std::abs(u0);
  b = std::abs(u1);
  if (a < b) {
    a /= b;
    y = std::sqrt(a * a + 1.0) * b;
  } else if (a > b) {
    b /= a;
    y = std::sqrt(b * b + 1.0) * a;
  } else if (rtIsNaN(b)) {
    y = (rtNaN);
  } else {
    y = a * 1.4142135623730951;
  }

  return y;
}

real_T rt_atan2d_snf(real_T u0, real_T u1)
{
  real_T y;
  if (rtIsNaN(u0) || rtIsNaN(u1)) {
    y = (rtNaN);
  } else if (rtIsInf(u0) && rtIsInf(u1)) {
    int32_T tmp;
    int32_T tmp_0;
    if (u1 > 0.0) {
      tmp = 1;
    } else {
      tmp = -1;
    }

    if (u0 > 0.0) {
      tmp_0 = 1;
    } else {
      tmp_0 = -1;
    }

    y = std::atan2(static_cast<real_T>(tmp_0), static_cast<real_T>(tmp));
  } else if (u1 == 0.0) {
    if (u0 > 0.0) {
      y = RT_PI / 2.0;
    } else if (u0 < 0.0) {
      y = -(RT_PI / 2.0);
    } else {
      y = 0.0;
    }
  } else {
    y = std::atan2(u0, u1);
  }

  return y;
}

real_T rt_remd_snf(real_T u0, real_T u1)
{
  real_T y;
  if (rtIsNaN(u0) || rtIsNaN(u1) || rtIsInf(u0)) {
    y = (rtNaN);
  } else if (rtIsInf(u1)) {
    y = u0;
  } else {
    if (u1 < 0.0) {
      y = std::ceil(u1);
    } else {
      y = std::floor(u1);
    }

    if ((u1 != 0.0) && (u1 != y)) {
      real_T q;
      q = std::abs(u0 / u1);
      if (!(std::abs(q - std::floor(q + 0.5)) > DBL_EPSILON * q)) {
        y = 0.0 * u0;
      } else {
        y = std::fmod(u0, u1);
      }
    } else {
      y = std::fmod(u0, u1);
    }
  }

  return y;
}

/* Model step function for TID0 */
void control_vectorial_step0(void)     /* Sample time: [0.0s, 0.0s] */
{
  if (rtmIsMajorTimeStep(control_vectorial_M)) {
    /* set solver stop time */
    if (!(control_vectorial_M->Timing.clockTick0+1)) {
      rtsiSetSolverStopTime(&control_vectorial_M->solverInfo,
                            ((control_vectorial_M->Timing.clockTickH0 + 1) *
        control_vectorial_M->Timing.stepSize0 * 4294967296.0));
    } else {
      rtsiSetSolverStopTime(&control_vectorial_M->solverInfo,
                            ((control_vectorial_M->Timing.clockTick0 + 1) *
        control_vectorial_M->Timing.stepSize0 +
        control_vectorial_M->Timing.clockTickH0 *
        control_vectorial_M->Timing.stepSize0 * 4294967296.0));
    }

    /* Update the flag to indicate when data transfers from
     *  Sample time: [5.0E-5s, 0.0s] to Sample time: [0.0001s, 0.0s]  */
    control_vectorial_M->Timing.perTaskSampleHits[5] =
      (control_vectorial_M->Timing.RateInteraction.TID1_2 == 0);
    (control_vectorial_M->Timing.RateInteraction.TID1_2)++;
    if ((control_vectorial_M->Timing.RateInteraction.TID1_2) > 1) {
      control_vectorial_M->Timing.RateInteraction.TID1_2 = 0;
    }
  }                                    /* end MajorTimeStep */

  /* Update absolute time of base rate at minor time step */
  if (rtmIsMinorTimeStep(control_vectorial_M)) {
    control_vectorial_M->Timing.t[0] = rtsiGetT(&control_vectorial_M->solverInfo);
  }

  {
    real_T fractions[3];
    real_T fractions_0[3];
    real_T Abs1_tmp;
    real_T Bias;
    real_T Bias_tmp;
    real_T TrigonometricFunction1_tmp;
    real_T TrigonometricFunction_tmp;
    real_T tmp_0;
    real_T tmp_1;
    real_T tmp_2;
    real_T tmp_3;
    real_T tmp_4;
    real_T tmp_5;
    real_T tmp_6;
    real_T u0;
    real_T u2;
    uint32_T bpIndices[3];
    uint32_T bpIndices_0[3];
    uint32_T bpIdx;
    boolean_T tmp;
    boolean_T zcEvent;

    /* Gain: '<S22>/1//wb' */
    tmp_6 = *get_wb();

    /* Switch: '<S37>/Switch' incorporates:
     *  Constant: '<S3>/Constant1'
     */
    tmp_5 = *get_Vmax();

    /* Sum: '<S25>/Sum1' incorporates:
     *  Constant: '<S3>/Constant2'
     */
    tmp_4 = *get_T_max();

    /* Gain: '<S29>/ki' */
    tmp_3 = *get_ki_iq();

    /* Gain: '<S32>/Vdc factor' incorporates:
     *  Constant: '<S3>/Constant'
     */
    tmp_2 = *get_Vdc();

    /* Product: '<S32>/Product' incorporates:
     *  Constant: '<S32>/Constant'
     */
    tmp_1 = *get_Potencia();

    /* Gain: '<S3>/p' */
    tmp_0 = *get_p();

    /* Reset subsysRan breadcrumbs */
    srClearBC(control_vectorial_DW.TriggeredSubsystem_SubsysRanBC);

    /* Reset subsysRan breadcrumbs */
    srClearBC(control_vectorial_DW.NEGATIVEEdge_SubsysRanBC);

    /* Reset subsysRan breadcrumbs */
    srClearBC(control_vectorial_DW.POSITIVEEdge_SubsysRanBC);

    /* Outputs for Enabled SubSystem: '<S5>/NEGATIVE Edge' incorporates:
     *  EnablePort: '<S7>/Enable'
     */
    /* RateTransition generated from: '<S21>/Filtro Pasa Baja1' */
    tmp = rtmIsMajorTimeStep(control_vectorial_M);

    /* End of Outputs for SubSystem: '<S5>/NEGATIVE Edge' */
    if (tmp) {
      /* S-Function (sg_fpga_di_sf_a2): '<Root>/Encoder' */

      /* Level2 S-Function Block: '<Root>/Encoder' (sg_fpga_di_sf_a2) */
      {
        SimStruct *rts = control_vectorial_M->childSfunctions[0];
        sfcnOutputs(rts,0);
      }
    }

    /* DataTypeConversion: '<S5>/Data Type Conversion2' */
    control_vectorial_B.DataTypeConversion2 = (control_vectorial_B.Encoder_o1 !=
      0.0);
    if (tmp) {
      /* Memory: '<S5>/Memory' */
      control_vectorial_B.Memory = control_vectorial_DW.Memory_PreviousInput;

      /* MultiPortSwitch: '<S5>/Multiport Switch' incorporates:
       *  Constant: '<S5>/Constant1'
       */
      switch (static_cast<int32_T>(control_vectorial_cal->EdgeDetector_model)) {
       case 1:
        /* MultiPortSwitch: '<S5>/Multiport Switch' incorporates:
         *  Constant: '<S5>/pos. edge'
         */
        control_vectorial_B.MultiportSwitch[0] =
          control_vectorial_cal->posedge_Value[0];
        control_vectorial_B.MultiportSwitch[1] =
          control_vectorial_cal->posedge_Value[1];
        break;

       case 2:
        /* MultiPortSwitch: '<S5>/Multiport Switch' incorporates:
         *  Constant: '<S5>/neg. edge'
         */
        control_vectorial_B.MultiportSwitch[0] =
          control_vectorial_cal->negedge_Value[0];
        control_vectorial_B.MultiportSwitch[1] =
          control_vectorial_cal->negedge_Value[1];
        break;

       default:
        /* MultiPortSwitch: '<S5>/Multiport Switch' incorporates:
         *  Constant: '<S5>/either edge'
         */
        control_vectorial_B.MultiportSwitch[0] =
          control_vectorial_cal->eitheredge_Value[0];
        control_vectorial_B.MultiportSwitch[1] =
          control_vectorial_cal->eitheredge_Value[1];
        break;
      }

      /* End of MultiPortSwitch: '<S5>/Multiport Switch' */

      /* Outputs for Enabled SubSystem: '<S5>/POSITIVE Edge' incorporates:
       *  EnablePort: '<S8>/Enable'
       */
      if (rtsiIsModeUpdateTimeStep(&control_vectorial_M->solverInfo)) {
        if (control_vectorial_B.MultiportSwitch[0] > 0.0) {
          if (!control_vectorial_DW.POSITIVEEdge_MODE) {
            control_vectorial_DW.POSITIVEEdge_MODE = true;
          }
        } else if (control_vectorial_DW.POSITIVEEdge_MODE) {
          control_vectorial_DW.POSITIVEEdge_MODE = false;
        }
      }

      /* End of Outputs for SubSystem: '<S5>/POSITIVE Edge' */
    }

    /* Outputs for Enabled SubSystem: '<S5>/POSITIVE Edge' incorporates:
     *  EnablePort: '<S8>/Enable'
     */
    if (control_vectorial_DW.POSITIVEEdge_MODE) {
      /* RelationalOperator: '<S8>/Relational Operator1' */
      control_vectorial_B.RelationalOperator1 = (static_cast<int32_T>
        (control_vectorial_B.Memory) < static_cast<int32_T>
        (control_vectorial_B.DataTypeConversion2));
      if (rtsiIsModeUpdateTimeStep(&control_vectorial_M->solverInfo)) {
        srUpdateBC(control_vectorial_DW.POSITIVEEdge_SubsysRanBC);
      }
    }

    /* End of Outputs for SubSystem: '<S5>/POSITIVE Edge' */

    /* Outputs for Enabled SubSystem: '<S5>/NEGATIVE Edge' incorporates:
     *  EnablePort: '<S7>/Enable'
     */
    if (tmp && rtsiIsModeUpdateTimeStep(&control_vectorial_M->solverInfo)) {
      if (control_vectorial_B.MultiportSwitch[1] > 0.0) {
        if (!control_vectorial_DW.NEGATIVEEdge_MODE) {
          control_vectorial_DW.NEGATIVEEdge_MODE = true;
        }
      } else if (control_vectorial_DW.NEGATIVEEdge_MODE) {
        control_vectorial_DW.NEGATIVEEdge_MODE = false;
      }
    }

    if (control_vectorial_DW.NEGATIVEEdge_MODE) {
      /* RelationalOperator: '<S7>/Relational Operator1' */
      control_vectorial_B.RelationalOperator1_m = (static_cast<int32_T>
        (control_vectorial_B.Memory) > static_cast<int32_T>
        (control_vectorial_B.DataTypeConversion2));
      if (rtsiIsModeUpdateTimeStep(&control_vectorial_M->solverInfo)) {
        srUpdateBC(control_vectorial_DW.NEGATIVEEdge_SubsysRanBC);
      }
    }

    /* End of Outputs for SubSystem: '<S5>/NEGATIVE Edge' */

    /* Logic: '<S5>/Logical Operator1' */
    control_vectorial_B.LogicalOperator1 =
      (control_vectorial_B.RelationalOperator1 ||
       control_vectorial_B.RelationalOperator1_m);

    /* DataTypeConversion: '<S1>/Data Type Conversion' */
    control_vectorial_B.DataTypeConversion =
      control_vectorial_B.LogicalOperator1;
    if (tmp) {
      /* Delay: '<S1>/Delay4' */
      control_vectorial_B.Delay4 = control_vectorial_DW.Delay4_DSTATE;

      /* Delay: '<S1>/Delay3' */
      control_vectorial_B.Delay3 = control_vectorial_DW.Delay3_DSTATE[0];

      /* Switch: '<S1>/Switch1' */
      if (control_vectorial_B.Delay3 > *get_n_lim_direccion()) {
        /* Switch: '<S1>/Switch1' */
        control_vectorial_B.Switch1 = control_vectorial_B.Delay4;
      } else {
        /* Switch: '<S1>/Switch1' */
        control_vectorial_B.Switch1 = control_vectorial_B.Encoder_o2;
      }

      /* End of Switch: '<S1>/Switch1' */

      /* Delay: '<S1>/Delay1' */
      control_vectorial_B.Delay1 = control_vectorial_DW.Delay1_DSTATE;
    }

    /* Switch: '<S1>/Switch2' incorporates:
     *  Constant: '<Root>/Constant1'
     *  Switch: '<S1>/Switch'
     */
    if (control_vectorial_cal->Constant1_Value_k >
        control_vectorial_cal->Switch2_Threshold) {
      /* Switch: '<S1>/Switch2' incorporates:
       *  Constant: '<S1>/Constant'
       */
      control_vectorial_B.Switch2 = control_vectorial_cal->Constant_Value;
    } else {
      if (control_vectorial_B.Switch1 > control_vectorial_cal->Switch_Threshold)
      {
        /* Switch: '<S1>/Switch' */
        control_vectorial_B.Switch_a = control_vectorial_B.DataTypeConversion;
      } else {
        /* Gain: '<S1>/Gain1' incorporates:
         *  Switch: '<S1>/Switch'
         */
        control_vectorial_B.Gain1_f = control_vectorial_cal->Gain1_Gain *
          control_vectorial_B.DataTypeConversion;

        /* Switch: '<S1>/Switch' */
        control_vectorial_B.Switch_a = control_vectorial_B.Gain1_f;
      }

      /* Sum: '<S1>/Sum' */
      control_vectorial_B.Sum_a = control_vectorial_B.Switch_a +
        control_vectorial_B.Delay1;

      /* Switch: '<S1>/Switch2' */
      control_vectorial_B.Switch2 = control_vectorial_B.Sum_a;
    }

    /* End of Switch: '<S1>/Switch2' */

    /* Gain: '<S1>/CTE Encoder' */
    control_vectorial_B.CTEEncoder = *get_cte_encoder() *
      control_vectorial_B.Switch2;

    /* Gain: '<S3>/p' */
    control_vectorial_B.Theta_r = tmp_0 * control_vectorial_B.CTEEncoder;
    if (tmp) {
      /* Delay: '<S1>/Delay2' */
      control_vectorial_B.Delay2 = control_vectorial_DW.Delay2_DSTATE[0];

      /* DataTypeConversion: '<S31>/Conversion' incorporates:
       *  Constant: '<S23>/idRef'
       */
      control_vectorial_B.Conversion = control_vectorial_cal->idRef_Value;
    }

    /* Sum: '<S1>/Sum1' */
    control_vectorial_B.Sum1 = control_vectorial_B.CTEEncoder -
      control_vectorial_B.Delay2;

    /* Gain: '<S1>/Gain Velocidad' */
    control_vectorial_B.GainVelocidad = *get_gain_velocidad() *
      control_vectorial_B.Sum1;

    /* Gain: '<S3>/p1' */
    control_vectorial_B.p1 = tmp_0 * control_vectorial_B.GainVelocidad;

    /* Gain: '<S22>/1//wb' */
    Bias = 1.0 / tmp_6;

    /* Gain: '<S22>/1//wb' */
    control_vectorial_B.uwb = Bias * control_vectorial_B.p1;

    /* Gain: '<S29>/-1' */
    control_vectorial_B.u = control_vectorial_cal->u_Gain *
      control_vectorial_B.uwb;

    /* Trigonometry: '<S28>/Trigonometric Function' incorporates:
     *  Switch: '<Root>/Switch'
     *  Trigonometry: '<S41>/Trigonometric Function'
     */
    TrigonometricFunction_tmp = std::sin(control_vectorial_B.Theta_r);

    /* Trigonometry: '<S28>/Trigonometric Function' */
    control_vectorial_B.TrigonometricFunction = TrigonometricFunction_tmp;

    /* Gain: '<S28>/Gain' */
    control_vectorial_B.Gain = control_vectorial_cal->Gain_Gain_h *
      control_vectorial_B.TrigonometricFunction;

    /* Trigonometry: '<S28>/Trigonometric Function1' incorporates:
     *  Switch: '<Root>/Switch'
     *  Trigonometry: '<S41>/Trigonometric Function1'
     */
    TrigonometricFunction1_tmp = std::cos(control_vectorial_B.Theta_r);

    /* Trigonometry: '<S28>/Trigonometric Function1' */
    control_vectorial_B.TrigonometricFunction1 = TrigonometricFunction1_tmp;

    /* SignalConversion generated from: '<S28>/Dot Product' */
    control_vectorial_B.TmpSignalConversionAtDotProduct[0] =
      control_vectorial_B.Gain;
    control_vectorial_B.TmpSignalConversionAtDotProduct[1] =
      control_vectorial_B.TrigonometricFunction1;

    /* TransferFcn: '<S21>/Filtro Pasa Baja' */
    control_vectorial_B.FiltroPasaBaja = control_vectorial_cal->FiltroPasaBaja_C
      * control_vectorial_X.FiltroPasaBaja_CSTATE;

    /* TransferFcn: '<S21>/Filtro Pasa Baja1' */
    control_vectorial_B.FiltroPasaBaja1 =
      control_vectorial_cal->FiltroPasaBaja1_C *
      control_vectorial_X.FiltroPasaBaja1_CSTATE;

    /* Gain: '<S27>/Gain1' */
    control_vectorial_B.Gain1 = control_vectorial_cal->Gain1_Gain_n *
      control_vectorial_B.FiltroPasaBaja1;

    /* TransferFcn: '<S21>/Filtro Pasa Baja2' */
    control_vectorial_B.FiltroPasaBaja2 =
      control_vectorial_cal->FiltroPasaBaja2_C *
      control_vectorial_X.FiltroPasaBaja2_CSTATE;

    /* Gain: '<S27>/Gain2' */
    control_vectorial_B.Gain2 = control_vectorial_cal->Gain2_Gain_e *
      control_vectorial_B.FiltroPasaBaja2;

    /* Sum: '<S27>/Sum1' */
    control_vectorial_B.Sum1_d = (control_vectorial_B.FiltroPasaBaja -
      control_vectorial_B.Gain1) - control_vectorial_B.Gain2;

    /* Gain: '<S27>/Gain' */
    control_vectorial_B.Gain_b = control_vectorial_cal->Gain_Gain_f *
      control_vectorial_B.Sum1_d;

    /* Sum: '<S27>/Sum2' */
    control_vectorial_B.Sum2 = control_vectorial_B.FiltroPasaBaja1 -
      control_vectorial_B.FiltroPasaBaja2;

    /* Gain: '<S27>/Gain4' */
    control_vectorial_B.Gain4 = control_vectorial_cal->Gain4_Gain *
      control_vectorial_B.Sum2;

    /* SignalConversion generated from: '<S28>/Dot Product' */
    control_vectorial_B.TmpSignalConversionAtDotProdu_e[0] =
      control_vectorial_B.Gain_b;
    control_vectorial_B.TmpSignalConversionAtDotProdu_e[1] =
      control_vectorial_B.Gain4;

    /* DotProduct: '<S28>/Dot Product' */
    Bias = control_vectorial_B.TmpSignalConversionAtDotProduct[0];
    u0 = control_vectorial_B.TmpSignalConversionAtDotProdu_e[0];
    u2 = Bias * u0;
    Bias = control_vectorial_B.TmpSignalConversionAtDotProduct[1];
    u0 = control_vectorial_B.TmpSignalConversionAtDotProdu_e[1];
    u2 += Bias * u0;

    /* DotProduct: '<S28>/Dot Product' */
    control_vectorial_B.DotProduct = u2;

    /* Gain: '<S3>/iq->iqn' incorporates:
     *  Gain: '<S3>/id->idn'
     *  Gain: '<S3>/id->idn1'
     *  Gain: '<S3>/iq->iqn1'
     */
    Bias_tmp = *get_Lq() / 0.0466027;
    Bias = Bias_tmp;

    /* Gain: '<S3>/iq->iqn' */
    control_vectorial_B.iqiqn = Bias * control_vectorial_B.DotProduct;

    /* Product: '<S29>/Divide5' */
    control_vectorial_B.Divide5 = control_vectorial_B.u *
      control_vectorial_B.iqiqn;

    /* Abs: '<S23>/Abs1' incorporates:
     *  Abs: '<S1>/Abs1'
     */
    Abs1_tmp = std::abs(control_vectorial_B.GainVelocidad);

    /* Abs: '<S23>/Abs1' */
    control_vectorial_B.Abs1 = Abs1_tmp;

    /* Saturate: '<S32>/Saturation' */
    Bias = tmp_1 / tmp_4;
    u0 = control_vectorial_B.Abs1;
    u2 = control_vectorial_cal->Saturation_UpperSat;
    if (u0 > u2) {
      /* Saturate: '<S32>/Saturation' */
      control_vectorial_B.Saturation = u2;
    } else if (u0 < Bias) {
      /* Saturate: '<S32>/Saturation' */
      control_vectorial_B.Saturation = Bias;
    } else {
      /* Saturate: '<S32>/Saturation' */
      control_vectorial_B.Saturation = u0;
    }

    /* End of Saturate: '<S32>/Saturation' */

    /* Product: '<S32>/Product' incorporates:
     *  Constant: '<S32>/Constant'
     */
    control_vectorial_B.Product = 1.0 / control_vectorial_B.Saturation * tmp_1;

    /* MinMax: '<S32>/MinMax' incorporates:
     *  Constant: '<S32>/Constant1'
     */
    u0 = control_vectorial_B.Product;
    Bias = tmp_4;
    if ((u0 <= Bias) || rtIsNaN(Bias)) {
      Bias = u0;
    }

    /* MinMax: '<S32>/MinMax' */
    control_vectorial_B.MinMax = Bias;
    if (tmp) {
      /* Gain: '<S32>/Vdc factor' */
      Bias = 1.0 / tmp_2;

      /* Gain: '<S32>/Vdc factor' incorporates:
       *  Constant: '<S3>/Constant'
       */
      control_vectorial_B.Vdcfactor = Bias * tmp_2;

      /* Gain: '<Root>/Gain' incorporates:
       *  Constant: '<Root>/Constant'
       */
      control_vectorial_B.Gain_n = control_vectorial_cal->Gain_Gain_p *
        control_vectorial_cal->Constant_Value_d;

      /* DiscreteTransferFcn: '<S25>/Zero-cancellation  omega' */
      control_vectorial_B.Zerocancellationomega = control_vectorial_B.Gain_n *
        control_vectorial_cal->Zerocancellationomega_NumCoef;
      control_vectorial_B.Zerocancellationomega /=
        control_vectorial_cal->Zerocancellationomega_DenCoef;

      /* UnitDelay: '<S39>/Unit Delay' */
      control_vectorial_B.UnitDelay = control_vectorial_DW.UnitDelay_DSTATE;

      /* Gain: '<S39>/Kaw' */
      control_vectorial_B.Kaw = control_vectorial_cal->VelocityController_Kaw_w *
        control_vectorial_B.UnitDelay;
    }

    /* Product: '<S32>/Product1' */
    control_vectorial_B.Product1 = control_vectorial_B.MinMax *
      control_vectorial_B.Vdcfactor;

    /* Sum: '<S25>/Sum' */
    control_vectorial_B.Sum = control_vectorial_B.Zerocancellationomega -
      control_vectorial_B.GainVelocidad;

    /* Gain: '<S39>/Kp' */
    control_vectorial_B.Kp = control_vectorial_cal->VelocityController_Kp_w *
      control_vectorial_B.Sum;

    /* Gain: '<S39>/Ki' */
    control_vectorial_B.Ki = control_vectorial_cal->VelocityController_Ki_w *
      control_vectorial_B.Sum;

    /* Sum: '<S39>/Sum1' */
    control_vectorial_B.Sum1_g = control_vectorial_B.Ki +
      control_vectorial_B.Kaw;
    if (tmp) {
      /* DiscreteIntegrator: '<S39>/Discrete-Time Integrator' */
      control_vectorial_B.DiscreteTimeIntegrator =
        control_vectorial_cal->DiscreteTimeIntegrator_gainval *
        control_vectorial_B.Sum1_g +
        control_vectorial_DW.DiscreteTimeIntegrator_DSTATE;

      /* RelationalOperator: '<S38>/Compare' incorporates:
       *  Constant: '<S25>/Constant'
       *  Constant: '<S38>/Constant'
       */
      control_vectorial_B.Compare =
        (control_vectorial_cal->VelocityController_CtrlType <
         control_vectorial_cal->CompareToConstant_const);
    }

    /* Sum: '<S39>/Sum' */
    control_vectorial_B.Sum_o = control_vectorial_B.Kp +
      control_vectorial_B.DiscreteTimeIntegrator;

    /* Switch: '<S25>/Switch' */
    if (control_vectorial_B.Compare) {
      /* Switch: '<S25>/Switch' */
      control_vectorial_B.Switch = control_vectorial_B.Sum_o;
    } else {
      /* Sum: '<S25>/Sum2' */
      control_vectorial_B.Sum2_h = control_vectorial_B.Sum_o -
        control_vectorial_B.GainVelocidad;

      /* Gain: '<S25>/Kv_w' */
      control_vectorial_B.Kv_w = control_vectorial_cal->Kv_w_Gain *
        control_vectorial_B.Sum2_h;

      /* Switch: '<S25>/Switch' */
      control_vectorial_B.Switch = control_vectorial_B.Kv_w;
    }

    /* End of Switch: '<S25>/Switch' */

    /* RelationalOperator: '<S33>/LowerRelop1' */
    control_vectorial_B.LowerRelop1 = (control_vectorial_B.Switch >
      control_vectorial_B.Product1);

    /* Gain: '<S32>/Sign Change' */
    control_vectorial_B.SignChange = control_vectorial_cal->SignChange_Gain *
      control_vectorial_B.Product1;

    /* RelationalOperator: '<S33>/UpperRelop' */
    control_vectorial_B.UpperRelop = (control_vectorial_B.Switch <
      control_vectorial_B.SignChange);

    /* Switch: '<S33>/Switch' */
    if (control_vectorial_B.UpperRelop) {
      /* Switch: '<S33>/Switch' */
      control_vectorial_B.Switch_o = control_vectorial_B.SignChange;
    } else {
      /* Switch: '<S33>/Switch' */
      control_vectorial_B.Switch_o = control_vectorial_B.Switch;
    }

    /* End of Switch: '<S33>/Switch' */

    /* Switch: '<S33>/Switch2' */
    if (control_vectorial_B.LowerRelop1) {
      /* Switch: '<S33>/Switch2' */
      control_vectorial_B.Switch2_e = control_vectorial_B.Product1;
    } else {
      /* Switch: '<S33>/Switch2' */
      control_vectorial_B.Switch2_e = control_vectorial_B.Switch_o;
    }

    /* End of Switch: '<S33>/Switch2' */

    /* Switch: '<S23>/Switch' incorporates:
     *  Constant: '<S23>/Constant'
     */
    if (control_vectorial_cal->Constant_Value_k >=
        control_vectorial_cal->Switch_Threshold_g) {
      /* Gain: '<S23>/Tq--> iqRef' */
      Bias = 2.0 / (3.0 * tmp_0 *
                    control_vectorial_cal->PMSMCurrentReferenceGenerator_p);

      /* Gain: '<S23>/Tq--> iqRef' */
      control_vectorial_B.TqiqRef = Bias * control_vectorial_B.Switch2_e;

      /* Switch: '<S23>/Switch' */
      control_vectorial_B.Switch_m[0] = control_vectorial_B.Conversion;
      control_vectorial_B.Switch_m[1] = control_vectorial_B.TqiqRef;
    } else {
      /* Gain: '<S23>/rad//s->rpm' */
      control_vectorial_B.radsrpm = control_vectorial_cal->radsrpm_Gain *
        control_vectorial_B.Abs1;

      /* Lookup_n-D: '<S23>/Lookup Table id reference' incorporates:
       *  Constant: '<S3>/Constant'
       *  Gain: '<S23>/rad//s->rpm'
       *  Switch: '<S33>/Switch2'
       */
      bpIdx = plook_binx(control_vectorial_B.radsrpm,
                         control_vectorial_cal->LookupTableidreference_bp01Data,
                         1U, &Bias);
      fractions[0U] = Bias;
      bpIndices[0U] = bpIdx;
      bpIdx = plook_binx(control_vectorial_B.Switch2_e,
                         control_vectorial_cal->LookupTableidreference_bp02Data,
                         2U, &Bias);
      fractions[1U] = Bias;
      bpIndices[1U] = bpIdx;
      bpIdx = plook_binx(tmp_2,
                         control_vectorial_cal->LookupTableidreference_bp03Data,
                         1U, &Bias);
      fractions[2U] = Bias;
      bpIndices[2U] = bpIdx;

      /* Lookup_n-D: '<S23>/Lookup Table id reference' */
      control_vectorial_B.LookupTableidreference = intrp3d_l_pw(bpIndices,
        fractions, control_vectorial_cal->LookupTableidreference_tableDat,
        control_vectorial_cal->LookupTableidreference_dimSizes);

      /* Lookup_n-D: '<S23>/Lookup Table iq reference' incorporates:
       *  Constant: '<S3>/Constant'
       *  Gain: '<S23>/rad//s->rpm'
       *  Switch: '<S33>/Switch2'
       */
      bpIdx = plook_binx(control_vectorial_B.radsrpm,
                         control_vectorial_cal->LookupTableiqreference_bp01Data,
                         1U, &Bias);
      fractions_0[0U] = Bias;
      bpIndices_0[0U] = bpIdx;
      bpIdx = plook_binx(control_vectorial_B.Switch2_e,
                         control_vectorial_cal->LookupTableiqreference_bp02Data,
                         2U, &Bias);
      fractions_0[1U] = Bias;
      bpIndices_0[1U] = bpIdx;
      bpIdx = plook_binx(tmp_2,
                         control_vectorial_cal->LookupTableiqreference_bp03Data,
                         1U, &Bias);
      fractions_0[2U] = Bias;
      bpIndices_0[2U] = bpIdx;

      /* Lookup_n-D: '<S23>/Lookup Table iq reference' */
      control_vectorial_B.LookupTableiqreference = intrp3d_l_pw(bpIndices_0,
        fractions_0, control_vectorial_cal->LookupTableiqreference_tableDat,
        control_vectorial_cal->LookupTableiqreference_dimSizes);

      /* Switch: '<S23>/Switch' */
      control_vectorial_B.Switch_m[0] =
        control_vectorial_B.LookupTableidreference;
      control_vectorial_B.Switch_m[1] =
        control_vectorial_B.LookupTableiqreference;
    }

    /* End of Switch: '<S23>/Switch' */

    /* Fcn: '<S34>/x->r' */
    control_vectorial_B.xr = rt_hypotd_snf(control_vectorial_B.Switch_m[0],
      control_vectorial_B.Switch_m[1]);

    /* Fcn: '<S34>/x->theta' */
    control_vectorial_B.xtheta = rt_atan2d_snf(control_vectorial_B.Switch_m[1],
      control_vectorial_B.Switch_m[0]);

    /* Sum: '<S24>/Sum1' incorporates:
     *  Constant: '<S24>/Constant'
     */
    control_vectorial_B.Sum1_gv = control_vectorial_cal->Constant_Value_h -
      control_vectorial_B.xtheta;
    if (tmp) {
      /* Gain: '<S3>/vdqn-> vdq' */
      Bias = tmp_6 * 0.0466027;

      /* UnitDelay: '<S3>/Unit Delay' */
      u0 = control_vectorial_DW.UnitDelay_DSTATE_j[0];
      control_vectorial_B.UnitDelay_g[0] = u0;

      /* Gain: '<S3>/vdqn-> vdq' */
      u0 *= Bias;
      control_vectorial_B.vdqnvdq[0] = u0;

      /* Math: '<S35>/Math Function' */
      control_vectorial_B.MathFunction[0] = u0 * u0;

      /* UnitDelay: '<S3>/Unit Delay' */
      u0 = control_vectorial_DW.UnitDelay_DSTATE_j[1];
      control_vectorial_B.UnitDelay_g[1] = u0;

      /* Gain: '<S3>/vdqn-> vdq' */
      u0 *= Bias;
      control_vectorial_B.vdqnvdq[1] = u0;

      /* Math: '<S35>/Math Function' */
      control_vectorial_B.MathFunction[1] = u0 * u0;

      /* Sum: '<S35>/Sum of Elements' */
      Bias = control_vectorial_B.MathFunction[0];
      Bias += control_vectorial_B.MathFunction[1];

      /* Sum: '<S35>/Sum of Elements' */
      control_vectorial_B.SumofElements = Bias;

      /* Sqrt: '<S35>/Sqrt' */
      control_vectorial_B.Sqrt = std::sqrt(control_vectorial_B.SumofElements);

      /* Switch: '<S37>/Switch' incorporates:
       *  Constant: '<S3>/Constant1'
       */
      if (tmp_5 > control_vectorial_cal->Switch_Threshold_e) {
        /* Switch: '<S37>/Switch' */
        control_vectorial_B.Switch_p = tmp_5;
      } else {
        /* Switch: '<S37>/Switch' incorporates:
         *  Constant: '<S37>/Constant1'
         */
        control_vectorial_B.Switch_p = control_vectorial_cal->Constant1_Value;
      }

      /* Product: '<S35>/Product' */
      control_vectorial_B.Product_c = control_vectorial_B.Sqrt /
        control_vectorial_B.Switch_p;

      /* Sum: '<S24>/Sum' incorporates:
       *  Constant: '<S24>/Modulation index threshold'
       */
      control_vectorial_B.Sum_d =
        control_vectorial_cal->PMSMFieldWeakeningController1_M -
        control_vectorial_B.Product_c;

      /* Gain: '<S24>/Gain' */
      control_vectorial_B.Gain_a =
        control_vectorial_cal->PMSMFieldWeakeningController1_o *
        control_vectorial_B.Sum_d;

      /* UnitDelay: '<S24>/Unit Delay' */
      control_vectorial_B.UnitDelay_o = control_vectorial_DW.UnitDelay_DSTATE_p;

      /* Gain: '<S24>/Gain1' */
      control_vectorial_B.Gain1_k =
        control_vectorial_cal->PMSMFieldWeakeningController1_K *
        control_vectorial_B.UnitDelay_o;

      /* Sum: '<S24>/Sum4' */
      control_vectorial_B.Sum4 = control_vectorial_B.Gain_a +
        control_vectorial_B.Gain1_k;

      /* DiscreteIntegrator: '<S24>/Discrete-Time Integrator' */
      control_vectorial_B.DiscreteTimeIntegrator_c =
        control_vectorial_cal->DiscreteTimeIntegrator_gainva_k *
        control_vectorial_B.Sum4 +
        control_vectorial_DW.DiscreteTimeIntegrator_DSTATE_h;

      /* Saturate: '<S24>/Saturation' */
      u0 = control_vectorial_B.DiscreteTimeIntegrator_c;
      Bias = control_vectorial_cal->Saturation_LowerSat;
      u2 = control_vectorial_cal->Saturation_UpperSat_g;
      if (u0 > u2) {
        /* Saturate: '<S24>/Saturation' */
        control_vectorial_B.Saturation_n = u2;
      } else if (u0 < Bias) {
        /* Saturate: '<S24>/Saturation' */
        control_vectorial_B.Saturation_n = Bias;
      } else {
        /* Saturate: '<S24>/Saturation' */
        control_vectorial_B.Saturation_n = u0;
      }

      /* End of Saturate: '<S24>/Saturation' */
    }

    /* Product: '<S24>/Product' */
    control_vectorial_B.Product_l = control_vectorial_B.Sum1_gv *
      control_vectorial_B.Saturation_n;

    /* Sum: '<S24>/Sum2' incorporates:
     *  Constant: '<S24>/Constant1'
     */
    control_vectorial_B.Sum2_e = control_vectorial_cal->Constant1_Value_h -
      control_vectorial_B.Product_l;

    /* Fcn: '<S36>/r->x' */
    control_vectorial_B.rx = control_vectorial_B.xr * std::cos
      (control_vectorial_B.Sum2_e);

    /* Gain: '<S3>/id->idn1' */
    Bias = Bias_tmp;

    /* Gain: '<S3>/id->idn1' */
    control_vectorial_B.ididn1 = Bias * control_vectorial_B.rx;

    /* SignalConversion generated from: '<S28>/Dot Product1' */
    control_vectorial_B.TmpSignalConversionAtDotProdu_g[0] =
      control_vectorial_B.TrigonometricFunction1;
    control_vectorial_B.TmpSignalConversionAtDotProdu_g[1] =
      control_vectorial_B.TrigonometricFunction;

    /* DotProduct: '<S28>/Dot Product1' */
    Bias = control_vectorial_B.TmpSignalConversionAtDotProdu_g[0];
    u0 = control_vectorial_B.TmpSignalConversionAtDotProdu_e[0];
    u2 = Bias * u0;
    Bias = control_vectorial_B.TmpSignalConversionAtDotProdu_g[1];
    u0 = control_vectorial_B.TmpSignalConversionAtDotProdu_e[1];
    u2 += Bias * u0;

    /* DotProduct: '<S28>/Dot Product1' */
    control_vectorial_B.DotProduct1 = u2;

    /* Gain: '<S3>/id->idn' */
    Bias = Bias_tmp;

    /* Gain: '<S3>/id->idn' */
    control_vectorial_B.ididn = Bias * control_vectorial_B.DotProduct1;

    /* Sum: '<S29>/Sum' */
    control_vectorial_B.Sum_j = control_vectorial_B.ididn1 -
      control_vectorial_B.ididn;

    /* Gain: '<S29>/kp' */
    control_vectorial_B.kp = *get_kp_id() * control_vectorial_B.Sum_j;

    /* Integrator: '<S29>/Integrator' */
    control_vectorial_B.Integrator = control_vectorial_X.Integrator_CSTATE;

    /* Gain: '<S29>/ki' */
    control_vectorial_B.ki = tmp_3 * control_vectorial_B.Integrator;

    /* Sum: '<S29>/Sum1' */
    control_vectorial_B.Sum1_k = control_vectorial_B.kp + control_vectorial_B.ki;

    /* Sum: '<S29>/Sum2' */
    control_vectorial_B.Sum2_m = control_vectorial_B.Divide5 +
      control_vectorial_B.Sum1_k;

    /* Fcn: '<S36>/theta->y' */
    control_vectorial_B.thetay = control_vectorial_B.xr * std::sin
      (control_vectorial_B.Sum2_e);

    /* Gain: '<S3>/iq->iqn1' */
    control_vectorial_B.iqiqn1 = Bias * control_vectorial_B.thetay;

    /* Sum: '<S30>/Sum4' */
    control_vectorial_B.Sum4_f = control_vectorial_B.iqiqn1 -
      control_vectorial_B.iqiqn;

    /* Gain: '<S30>/kp' */
    control_vectorial_B.kp_h = *get_kp_iq() * control_vectorial_B.Sum4_f;

    /* Integrator: '<S30>/Integrator' */
    control_vectorial_B.Integrator_h = control_vectorial_X.Integrator_CSTATE_k;

    /* Gain: '<S30>/ki' */
    control_vectorial_B.ki_g = tmp_3 * control_vectorial_B.Integrator_h;

    /* Sum: '<S30>/Sum1' */
    control_vectorial_B.Sum1_i = control_vectorial_B.kp_h +
      control_vectorial_B.ki_g;

    /* Gain: '<S30>/k' */
    control_vectorial_B.k_d = *get_k() * control_vectorial_B.ididn;

    /* Product: '<S30>/Divide5' */
    control_vectorial_B.Divide5_l = control_vectorial_B.uwb *
      control_vectorial_B.k_d;

    /* Sum: '<S30>/Sum2' */
    control_vectorial_B.Sum2_i = control_vectorial_B.uwb +
      control_vectorial_B.Divide5_l;

    /* Sum: '<S30>/Sum3' */
    control_vectorial_B.Sum3 = control_vectorial_B.Sum1_i +
      control_vectorial_B.Sum2_i;

    /* Switch: '<Root>/Switch' incorporates:
     *  Constant: '<Root>/Constant5'
     */
    if (control_vectorial_cal->Constant5_Value >
        control_vectorial_cal->Switch_Threshold_l) {
      /* Switch: '<Root>/Switch' incorporates:
       *  Constant: '<Root>/Constant2'
       *  Constant: '<Root>/Constant3'
       *  Constant: '<Root>/Constant4'
       */
      control_vectorial_B.Switch_k[0] = *get_V_alinear();
      control_vectorial_B.Switch_k[1] = control_vectorial_cal->Constant2_Value;
      control_vectorial_B.Switch_k[2] = control_vectorial_cal->Constant3_Value;
    } else {
      /* SignalConversion generated from: '<S41>/Dot Product' */
      control_vectorial_B.TmpSignalConversionAtDotProd_ex[0] =
        control_vectorial_B.Sum2_m;
      control_vectorial_B.TmpSignalConversionAtDotProd_ex[1] =
        control_vectorial_B.Sum3;

      /* Trigonometry: '<S41>/Trigonometric Function' */
      control_vectorial_B.TrigonometricFunction_a = TrigonometricFunction_tmp;

      /* Gain: '<S41>/Gain1' */
      control_vectorial_B.Gain1_p = control_vectorial_cal->Gain1_Gain_b *
        control_vectorial_B.TrigonometricFunction_a;

      /* Trigonometry: '<S41>/Trigonometric Function1' */
      control_vectorial_B.TrigonometricFunction1_h = TrigonometricFunction1_tmp;

      /* SignalConversion generated from: '<S41>/Dot Product1' */
      control_vectorial_B.TmpSignalConversionAtDotProd_gl[0] =
        control_vectorial_B.TrigonometricFunction1_h;
      control_vectorial_B.TmpSignalConversionAtDotProd_gl[1] =
        control_vectorial_B.Gain1_p;

      /* SignalConversion generated from: '<S41>/Dot Product' */
      control_vectorial_B.TmpSignalConversionAtDotProdu_j[0] =
        control_vectorial_B.TrigonometricFunction_a;
      control_vectorial_B.TmpSignalConversionAtDotProdu_j[1] =
        control_vectorial_B.TrigonometricFunction1_h;

      /* DotProduct: '<S41>/Dot Product1' */
      Bias = control_vectorial_B.TmpSignalConversionAtDotProd_gl[0];
      TrigonometricFunction_tmp =
        control_vectorial_B.TmpSignalConversionAtDotProd_ex[0];
      u0 = Bias * TrigonometricFunction_tmp;

      /* DotProduct: '<S41>/Dot Product' */
      Bias = control_vectorial_B.TmpSignalConversionAtDotProdu_j[0];
      u2 = Bias * TrigonometricFunction_tmp;

      /* DotProduct: '<S41>/Dot Product1' */
      Bias = control_vectorial_B.TmpSignalConversionAtDotProd_gl[1];
      TrigonometricFunction_tmp =
        control_vectorial_B.TmpSignalConversionAtDotProd_ex[1];
      u0 += Bias * TrigonometricFunction_tmp;

      /* DotProduct: '<S41>/Dot Product' */
      Bias = control_vectorial_B.TmpSignalConversionAtDotProdu_j[1];
      u2 += Bias * TrigonometricFunction_tmp;

      /* DotProduct: '<S41>/Dot Product1' */
      control_vectorial_B.DotProduct1_n = u0;

      /* Gain: '<S40>/Gain1' */
      control_vectorial_B.Gain1_i = control_vectorial_cal->Gain1_Gain_o *
        control_vectorial_B.DotProduct1_n;

      /* Gain: '<S40>/Gain2' */
      control_vectorial_B.Gain2_e = control_vectorial_cal->Gain2_Gain *
        control_vectorial_B.DotProduct1_n;

      /* DotProduct: '<S41>/Dot Product' */
      control_vectorial_B.DotProduct_o = u2;

      /* Gain: '<S40>/Gain' */
      control_vectorial_B.Gain_j = control_vectorial_cal->Gain_Gain *
        control_vectorial_B.DotProduct_o;

      /* Sum: '<S40>/Sum' */
      control_vectorial_B.Sum_g = control_vectorial_B.Gain_j -
        control_vectorial_B.Gain2_e;

      /* Gain: '<S40>/Gain3' */
      control_vectorial_B.Gain3 = control_vectorial_cal->Gain3_Gain *
        control_vectorial_B.DotProduct1_n;

      /* Sum: '<S40>/Sum1' */
      control_vectorial_B.Sum1_ds = (0.0 - control_vectorial_B.Gain_j) -
        control_vectorial_B.Gain3;

      /* Switch: '<Root>/Switch' */
      control_vectorial_B.Switch_k[0] = control_vectorial_B.Gain1_i;
      control_vectorial_B.Switch_k[1] = control_vectorial_B.Sum_g;
      control_vectorial_B.Switch_k[2] = control_vectorial_B.Sum1_ds;
    }

    if (tmp) {
      /* DigitalClock: '<S19>/Digital Clock' */
      control_vectorial_B.DigitalClock = control_vectorial_M->Timing.t[1];

      /* Sum: '<S19>/Add1' incorporates:
       *  Constant: '<S19>/Constant3'
       */
      control_vectorial_B.Add1 = control_vectorial_B.DigitalClock +
        control_vectorial_cal->Constant3_Value_m;

      /* Math: '<S19>/Math Function' incorporates:
       *  Constant: '<S19>/Constant1'
       */
      control_vectorial_B.MathFunction_l = rt_remd_snf(control_vectorial_B.Add1,
        control_vectorial_cal->Constant1_Value_e);

      /* Gain: '<S19>/1\ib1' */
      control_vectorial_B.uib1 = control_vectorial_cal->uib1_Gain *
        control_vectorial_B.MathFunction_l;

      /* Lookup_n-D: '<S19>/1-D Lookup Table' incorporates:
       *  Gain: '<S19>/1\ib1'
       */
      control_vectorial_B.uDLookupTable = look1_pbinlxpw
        (control_vectorial_B.uib1, control_vectorial_cal->uDLookupTable_bp01Data,
         control_vectorial_cal->uDLookupTable_tableData,
         &control_vectorial_DW.m_bpIndex, 2U);

      /* Sum: '<S19>/Add3' incorporates:
       *  Constant: '<S19>/Constant2'
       */
      control_vectorial_B.Add3 = control_vectorial_B.uDLookupTable -
        control_vectorial_cal->Constant2_Value_p;

      /* Sum: '<S10>/Add3' incorporates:
       *  Constant: '<S2>/Constant10'
       */
      control_vectorial_B.Add3_g =
        control_vectorial_cal->PWMGenerator2Level_MinMax[1] -
        control_vectorial_cal->PWMGenerator2Level_MinMax[0];

      /* Gain: '<S10>/Gain1' */
      control_vectorial_B.Gain1_k2 = control_vectorial_cal->Gain1_Gain_h *
        control_vectorial_B.Add3_g;

      /* Product: '<S10>/MUL1' */
      control_vectorial_B.MUL1 = control_vectorial_B.Add3 *
        control_vectorial_B.Gain1_k2;

      /* Sum: '<S10>/Add4' incorporates:
       *  Constant: '<S2>/Constant10'
       */
      control_vectorial_B.Add4 =
        (control_vectorial_cal->PWMGenerator2Level_MinMax[0] +
         control_vectorial_B.MUL1) + control_vectorial_B.Gain1_k2;

      /* RelationalOperator: '<S14>/Relational Operator2' */
      zcEvent = (control_vectorial_B.Switch_k[0] >= control_vectorial_B.Add4);
      control_vectorial_B.RelationalOperator2[0] = zcEvent;

      /* Logic: '<S2>/Logical Operator4' */
      control_vectorial_B.LogicalOperator4[0] = !zcEvent;

      /* RelationalOperator: '<S14>/Relational Operator2' */
      zcEvent = (control_vectorial_B.Switch_k[1] >= control_vectorial_B.Add4);
      control_vectorial_B.RelationalOperator2[1] = zcEvent;

      /* Logic: '<S2>/Logical Operator4' */
      control_vectorial_B.LogicalOperator4[1] = !zcEvent;

      /* RelationalOperator: '<S14>/Relational Operator2' */
      zcEvent = (control_vectorial_B.Switch_k[2] >= control_vectorial_B.Add4);
      control_vectorial_B.RelationalOperator2[2] = zcEvent;

      /* Logic: '<S2>/Logical Operator4' */
      control_vectorial_B.LogicalOperator4[2] = !zcEvent;

      /* DataTypeConversion: '<S2>/Data Type Conversion' */
      control_vectorial_B.DataTypeConversion_b[0] =
        control_vectorial_B.RelationalOperator2[0];
      control_vectorial_B.DataTypeConversion_b[1] =
        control_vectorial_B.LogicalOperator4[0];
      control_vectorial_B.DataTypeConversion_b[2] =
        control_vectorial_B.RelationalOperator2[1];
      control_vectorial_B.DataTypeConversion_b[3] =
        control_vectorial_B.LogicalOperator4[1];
      control_vectorial_B.DataTypeConversion_b[4] =
        control_vectorial_B.RelationalOperator2[2];
      control_vectorial_B.DataTypeConversion_b[5] =
        control_vectorial_B.LogicalOperator4[2];

      /* S-Function (sg_fpga_do_sf_a2): '<Root>/Digital output' */

      /* Level2 S-Function Block: '<Root>/Digital output' (sg_fpga_do_sf_a2) */
      {
        SimStruct *rts = control_vectorial_M->childSfunctions[1];
        sfcnOutputs(rts,0);
      }
    }

    /* Abs: '<S1>/Abs1' */
    control_vectorial_B.Abs1_c = Abs1_tmp;

    /* Clock: '<S6>/Clock' */
    control_vectorial_B.Clock = control_vectorial_M->Timing.t[0];
    if (tmp) {
      /* Outputs for Triggered SubSystem: '<S6>/Triggered Subsystem' incorporates:
       *  TriggerPort: '<S9>/Trigger'
       */
      if (rtsiIsModeUpdateTimeStep(&control_vectorial_M->solverInfo)) {
        zcEvent = (control_vectorial_B.LogicalOperator1 &&
                   (control_vectorial_PrevZCX.TriggeredSubsystem_Trig_ZCE !=
                    POS_ZCSIG));
        if (zcEvent) {
          /* SignalConversion generated from: '<S9>/In1' */
          control_vectorial_B.In1 = control_vectorial_B.Clock;
          control_vectorial_DW.TriggeredSubsystem_SubsysRanBC = 4;
        }

        control_vectorial_PrevZCX.TriggeredSubsystem_Trig_ZCE =
          control_vectorial_B.LogicalOperator1;
      }

      /* End of Outputs for SubSystem: '<S6>/Triggered Subsystem' */

      /* Sum: '<S6>/Sum' incorporates:
       *  Constant: '<S6>/Constant'
       */
      control_vectorial_B.Sum_ds = control_vectorial_B.In1 +
        control_vectorial_cal->Constant_Value_p;
    }

    /* RelationalOperator: '<S6>/Relational Operator' */
    control_vectorial_B.RelationalOperator = (control_vectorial_B.Sum_ds >
      control_vectorial_B.Clock);

    /* RateTransition generated from: '<S21>/Filtro Pasa Baja1' */
    if (tmp) {
      /* RateTransition generated from: '<S21>/Filtro Pasa Baja2' incorporates:
       *  RateTransition generated from: '<S21>/Filtro Pasa Baja'
       */
      zcEvent = (control_vectorial_M->Timing.RateInteraction.TID1_2 == 1);
      if (zcEvent) {
        control_vectorial_DW.TmpRTBAtFiltroPasaBaja1Inport_a = static_cast<
          int8_T>(control_vectorial_DW.TmpRTBAtFiltroPasaBaja1Inport_a == 0);
      }

      /* RateTransition generated from: '<S21>/Filtro Pasa Baja1' */
      control_vectorial_B.TmpRTBAtFiltroPasaBaja1Inport1 =
        control_vectorial_DW.TmpRTBAtFiltroPasaBaja1Inport1_[control_vectorial_DW.TmpRTBAtFiltroPasaBaja1Inport_a];

      /* RateTransition generated from: '<S21>/Filtro Pasa Baja2' */
      if (zcEvent) {
        control_vectorial_DW.TmpRTBAtFiltroPasaBaja2Inport_l =
          static_cast<int8_T>
          (control_vectorial_DW.TmpRTBAtFiltroPasaBaja2Inport_l == 0);
      }

      /* RateTransition generated from: '<S21>/Filtro Pasa Baja2' */
      control_vectorial_B.TmpRTBAtFiltroPasaBaja2Inport1 =
        control_vectorial_DW.TmpRTBAtFiltroPasaBaja2Inport1_[control_vectorial_DW.TmpRTBAtFiltroPasaBaja2Inport_l];

      /* RateTransition generated from: '<S21>/Filtro Pasa Baja' */
      if (zcEvent) {
        control_vectorial_DW.TmpRTBAtFiltroPasaBajaInport1_R =
          static_cast<int8_T>
          (control_vectorial_DW.TmpRTBAtFiltroPasaBajaInport1_R == 0);
      }

      /* RateTransition generated from: '<S21>/Filtro Pasa Baja' */
      control_vectorial_B.TmpRTBAtFiltroPasaBajaInport1 =
        control_vectorial_DW.TmpRTBAtFiltroPasaBajaInport1_B[control_vectorial_DW.TmpRTBAtFiltroPasaBajaInport1_R];
    }

    /* Abs: '<S29>/Abs' */
    control_vectorial_B.Abs = std::abs(control_vectorial_B.Sum_j);

    /* Switch: '<S29>/Switch' */
    if (control_vectorial_B.Abs >= control_vectorial_cal->Switch_Threshold_d) {
      /* Switch: '<S29>/Switch' incorporates:
       *  Constant: '<S29>/C1'
       */
      control_vectorial_B.Switch_h = control_vectorial_cal->C1_Value;
    } else {
      /* Switch: '<S29>/Switch' */
      control_vectorial_B.Switch_h = control_vectorial_B.Sum_j;
    }

    /* End of Switch: '<S29>/Switch' */

    /* Abs: '<S30>/Abs' */
    control_vectorial_B.Abs_e = std::abs(control_vectorial_B.Sum4_f);

    /* Switch: '<S30>/Switch' */
    if (control_vectorial_B.Abs_e >= control_vectorial_cal->Switch_Threshold_ed)
    {
      /* Switch: '<S30>/Switch' incorporates:
       *  Constant: '<S30>/C1'
       */
      control_vectorial_B.Switch_e = control_vectorial_cal->C1_Value_p;
    } else {
      /* Switch: '<S30>/Switch' */
      control_vectorial_B.Switch_e = control_vectorial_B.Sum4_f;
    }

    /* End of Switch: '<S30>/Switch' */

    /* Sum: '<S25>/Sum1' incorporates:
     *  Constant: '<S3>/Constant2'
     */
    control_vectorial_B.Sum1_h = tmp_4 - control_vectorial_B.Switch;
    if (tmp) {
      /* Sum: '<S24>/Sum3' */
      control_vectorial_B.Sum3_k = control_vectorial_B.Saturation_n -
        control_vectorial_B.DiscreteTimeIntegrator_c;
    }

    /* user code (Output function Trailer for TID0) */
    {
    }
  }

  if (rtmIsMajorTimeStep(control_vectorial_M)) {
    int32_T idxDelay;
    if (rtmIsMajorTimeStep(control_vectorial_M)) {
      /* Update for Memory: '<S5>/Memory' */
      control_vectorial_DW.Memory_PreviousInput =
        control_vectorial_B.DataTypeConversion2;

      /* Update for Delay: '<S1>/Delay4' */
      control_vectorial_DW.Delay4_DSTATE = control_vectorial_B.Switch1;

      /* Update for Delay: '<S1>/Delay1' */
      control_vectorial_DW.Delay1_DSTATE = control_vectorial_B.Switch2;
      for (idxDelay = 0; idxDelay < 199; idxDelay++) {
        /* Update for Delay: '<S1>/Delay3' */
        control_vectorial_DW.Delay3_DSTATE[idxDelay] =
          control_vectorial_DW.Delay3_DSTATE[idxDelay + 1];

        /* Update for Delay: '<S1>/Delay2' */
        control_vectorial_DW.Delay2_DSTATE[idxDelay] =
          control_vectorial_DW.Delay2_DSTATE[idxDelay + 1];
      }

      /* Update for Delay: '<S1>/Delay3' */
      control_vectorial_DW.Delay3_DSTATE[199] = control_vectorial_B.Abs1_c;

      /* Update for Delay: '<S1>/Delay2' */
      control_vectorial_DW.Delay2_DSTATE[199] = control_vectorial_B.CTEEncoder;

      /* Update for UnitDelay: '<S39>/Unit Delay' */
      control_vectorial_DW.UnitDelay_DSTATE = control_vectorial_B.Sum1_h;

      /* Update for DiscreteIntegrator: '<S39>/Discrete-Time Integrator' */
      control_vectorial_DW.DiscreteTimeIntegrator_DSTATE =
        control_vectorial_B.DiscreteTimeIntegrator;
      control_vectorial_DW.DiscreteTimeIntegrator_PrevRese = 0;

      /* Update for UnitDelay: '<S3>/Unit Delay' */
      control_vectorial_DW.UnitDelay_DSTATE_j[0] = control_vectorial_B.Sum2_m;
      control_vectorial_DW.UnitDelay_DSTATE_j[1] = control_vectorial_B.Sum3;

      /* Update for UnitDelay: '<S24>/Unit Delay' */
      control_vectorial_DW.UnitDelay_DSTATE_p = control_vectorial_B.Sum3_k;

      /* Update for DiscreteIntegrator: '<S24>/Discrete-Time Integrator' */
      control_vectorial_DW.DiscreteTimeIntegrator_DSTATE_h =
        control_vectorial_B.DiscreteTimeIntegrator_c;
    }
  }                                    /* end MajorTimeStep */

  if (rtmIsMajorTimeStep(control_vectorial_M)) {
    rt_ertODEUpdateContinuousStates(&control_vectorial_M->solverInfo);

    /* Update absolute time */
    /* The "clockTick0" counts the number of times the code of this task has
     * been executed. The absolute time is the multiplication of "clockTick0"
     * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
     * overflow during the application lifespan selected.
     * Timer of this task consists of two 32 bit unsigned integers.
     * The two integers represent the low bits Timing.clockTick0 and the high bits
     * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
     */
    if (!(++control_vectorial_M->Timing.clockTick0)) {
      ++control_vectorial_M->Timing.clockTickH0;
    }

    control_vectorial_M->Timing.t[0] = rtsiGetSolverStopTime
      (&control_vectorial_M->solverInfo);

    /* Update absolute time */
    /* The "clockTick1" counts the number of times the code of this task has
     * been executed. The absolute time is the multiplication of "clockTick1"
     * and "Timing.stepSize1". Size of "clockTick1" ensures timer will not
     * overflow during the application lifespan selected.
     * Timer of this task consists of two 32 bit unsigned integers.
     * The two integers represent the low bits Timing.clockTick1 and the high bits
     * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment.
     */
    if (!(++control_vectorial_M->Timing.clockTick1)) {
      ++control_vectorial_M->Timing.clockTickH1;
    }

    control_vectorial_M->Timing.t[1] = control_vectorial_M->Timing.clockTick1 *
      control_vectorial_M->Timing.stepSize1 +
      control_vectorial_M->Timing.clockTickH1 *
      control_vectorial_M->Timing.stepSize1 * 4294967296.0;
  }                                    /* end MajorTimeStep */
}

/* Derivatives for root system: '<Root>' */
void control_vectorial_derivatives(void)
{
  XDot_control_vectorial_T *_rtXdot;
  _rtXdot = ((XDot_control_vectorial_T *) control_vectorial_M->derivs);

  /* Derivatives for TransferFcn: '<S21>/Filtro Pasa Baja' */
  _rtXdot->FiltroPasaBaja_CSTATE = control_vectorial_cal->FiltroPasaBaja_A *
    control_vectorial_X.FiltroPasaBaja_CSTATE;
  _rtXdot->FiltroPasaBaja_CSTATE +=
    control_vectorial_B.TmpRTBAtFiltroPasaBajaInport1;

  /* Derivatives for TransferFcn: '<S21>/Filtro Pasa Baja1' */
  _rtXdot->FiltroPasaBaja1_CSTATE = control_vectorial_cal->FiltroPasaBaja1_A *
    control_vectorial_X.FiltroPasaBaja1_CSTATE;
  _rtXdot->FiltroPasaBaja1_CSTATE +=
    control_vectorial_B.TmpRTBAtFiltroPasaBaja1Inport1;

  /* Derivatives for TransferFcn: '<S21>/Filtro Pasa Baja2' */
  _rtXdot->FiltroPasaBaja2_CSTATE = control_vectorial_cal->FiltroPasaBaja2_A *
    control_vectorial_X.FiltroPasaBaja2_CSTATE;
  _rtXdot->FiltroPasaBaja2_CSTATE +=
    control_vectorial_B.TmpRTBAtFiltroPasaBaja2Inport1;

  /* Derivatives for Integrator: '<S29>/Integrator' */
  _rtXdot->Integrator_CSTATE = control_vectorial_B.Switch_h;

  /* Derivatives for Integrator: '<S30>/Integrator' */
  _rtXdot->Integrator_CSTATE_k = control_vectorial_B.Switch_e;
}

/* Model step function for TID2 */
void control_vectorial_step2(void)     /* Sample time: [0.0001s, 0.0s] */
{
  /* S-Function (sg_fpga_IO397_ad): '<Root>/Corrientes y Par' */

  /* Level2 S-Function Block: '<Root>/Corrientes y Par' (sg_fpga_IO397_ad) */
  {
    SimStruct *rts = control_vectorial_M->childSfunctions[2];
    sfcnOutputs(rts,2);
  }

  /* RateTransition generated from: '<S21>/Filtro Pasa Baja1' */
  control_vectorial_DW.TmpRTBAtFiltroPasaBaja1Inport_j = static_cast<int8_T>
    (control_vectorial_DW.TmpRTBAtFiltroPasaBaja1Inport_j == 0);
  control_vectorial_DW.TmpRTBAtFiltroPasaBaja1Inport1_[control_vectorial_DW.TmpRTBAtFiltroPasaBaja1Inport_j]
    = control_vectorial_B.CorrientesyPar_o2;

  /* RateTransition generated from: '<S21>/Filtro Pasa Baja2' */
  control_vectorial_DW.TmpRTBAtFiltroPasaBaja2Inport_p = static_cast<int8_T>
    (control_vectorial_DW.TmpRTBAtFiltroPasaBaja2Inport_p == 0);
  control_vectorial_DW.TmpRTBAtFiltroPasaBaja2Inport1_[control_vectorial_DW.TmpRTBAtFiltroPasaBaja2Inport_p]
    = control_vectorial_B.CorrientesyPar_o3;

  /* RateTransition generated from: '<S21>/Filtro Pasa Baja' */
  control_vectorial_DW.TmpRTBAtFiltroPasaBajaInport1_W = static_cast<int8_T>
    (control_vectorial_DW.TmpRTBAtFiltroPasaBajaInport1_W == 0);
  control_vectorial_DW.TmpRTBAtFiltroPasaBajaInport1_B[control_vectorial_DW.TmpRTBAtFiltroPasaBajaInport1_W]
    = control_vectorial_B.CorrientesyPar_o1;

  /* Update absolute time */
  /* The "clockTick2" counts the number of times the code of this task has
   * been executed. The absolute time is the multiplication of "clockTick2"
   * and "Timing.stepSize2". Size of "clockTick2" ensures timer will not
   * overflow during the application lifespan selected.
   * Timer of this task consists of two 32 bit unsigned integers.
   * The two integers represent the low bits Timing.clockTick2 and the high bits
   * Timing.clockTickH2. When the low bit overflows to 0, the high bits increment.
   */
  if (!(++control_vectorial_M->Timing.clockTick2)) {
    ++control_vectorial_M->Timing.clockTickH2;
  }

  control_vectorial_M->Timing.t[2] = control_vectorial_M->Timing.clockTick2 *
    control_vectorial_M->Timing.stepSize2 +
    control_vectorial_M->Timing.clockTickH2 *
    control_vectorial_M->Timing.stepSize2 * 4294967296.0;
}

/* Model initialize function */
void control_vectorial_initialize(void)
{
  /* Registration code */

  /* initialize non-finites */
  rt_InitInfAndNaN(sizeof(real_T));

  /* non-finite (run-time) assignments */
  control_vectorial_cal->Saturation_UpperSat = rtInf;

  /* Set task counter limit used by the static main program */
  (control_vectorial_M)->Timing.TaskCounters.cLimit[0] = 1;
  (control_vectorial_M)->Timing.TaskCounters.cLimit[1] = 1;
  (control_vectorial_M)->Timing.TaskCounters.cLimit[2] = 2;

  {
    /* Setup solver object */
    rtsiSetSimTimeStepPtr(&control_vectorial_M->solverInfo,
                          &control_vectorial_M->Timing.simTimeStep);
    rtsiSetTPtr(&control_vectorial_M->solverInfo, &rtmGetTPtr
                (control_vectorial_M));
    rtsiSetStepSizePtr(&control_vectorial_M->solverInfo,
                       &control_vectorial_M->Timing.stepSize0);
    rtsiSetdXPtr(&control_vectorial_M->solverInfo, &control_vectorial_M->derivs);
    rtsiSetContStatesPtr(&control_vectorial_M->solverInfo, (real_T **)
                         &control_vectorial_M->contStates);
    rtsiSetNumContStatesPtr(&control_vectorial_M->solverInfo,
      &control_vectorial_M->Sizes.numContStates);
    rtsiSetNumPeriodicContStatesPtr(&control_vectorial_M->solverInfo,
      &control_vectorial_M->Sizes.numPeriodicContStates);
    rtsiSetPeriodicContStateIndicesPtr(&control_vectorial_M->solverInfo,
      &control_vectorial_M->periodicContStateIndices);
    rtsiSetPeriodicContStateRangesPtr(&control_vectorial_M->solverInfo,
      &control_vectorial_M->periodicContStateRanges);
    rtsiSetContStateDisabledPtr(&control_vectorial_M->solverInfo, (boolean_T**)
      &control_vectorial_M->contStateDisabled);
    rtsiSetErrorStatusPtr(&control_vectorial_M->solverInfo, (&rtmGetErrorStatus
      (control_vectorial_M)));
    rtsiSetRTModelPtr(&control_vectorial_M->solverInfo, control_vectorial_M);
  }

  rtsiSetSimTimeStep(&control_vectorial_M->solverInfo, MAJOR_TIME_STEP);
  rtsiSetIsMinorTimeStepWithModeChange(&control_vectorial_M->solverInfo, false);
  rtsiSetIsContModeFrozen(&control_vectorial_M->solverInfo, false);
  control_vectorial_M->intgData.y = control_vectorial_M->odeY;
  control_vectorial_M->intgData.f[0] = control_vectorial_M->odeF[0];
  control_vectorial_M->intgData.f[1] = control_vectorial_M->odeF[1];
  control_vectorial_M->intgData.f[2] = control_vectorial_M->odeF[2];
  control_vectorial_M->contStates = ((X_control_vectorial_T *)
    &control_vectorial_X);
  control_vectorial_M->contStateDisabled = ((XDis_control_vectorial_T *)
    &control_vectorial_XDis);
  control_vectorial_M->Timing.tStart = (0.0);
  rtsiSetSolverData(&control_vectorial_M->solverInfo, static_cast<void *>
                    (&control_vectorial_M->intgData));
  rtsiSetSolverName(&control_vectorial_M->solverInfo,"ode3");
  control_vectorial_M->solverInfoPtr = (&control_vectorial_M->solverInfo);

  /* Initialize timing info */
  {
    int_T *mdlTsMap = control_vectorial_M->Timing.sampleTimeTaskIDArray;
    mdlTsMap[0] = 0;
    mdlTsMap[1] = 1;
    mdlTsMap[2] = 2;
    control_vectorial_M->Timing.sampleTimeTaskIDPtr = (&mdlTsMap[0]);
    control_vectorial_M->Timing.sampleTimes =
      (&control_vectorial_M->Timing.sampleTimesArray[0]);
    control_vectorial_M->Timing.offsetTimes =
      (&control_vectorial_M->Timing.offsetTimesArray[0]);

    /* task periods */
    control_vectorial_M->Timing.sampleTimes[0] = (0.0);
    control_vectorial_M->Timing.sampleTimes[1] = (5.0E-5);
    control_vectorial_M->Timing.sampleTimes[2] = (0.0001);

    /* task offsets */
    control_vectorial_M->Timing.offsetTimes[0] = (0.0);
    control_vectorial_M->Timing.offsetTimes[1] = (0.0);
    control_vectorial_M->Timing.offsetTimes[2] = (0.0);
  }

  rtmSetTPtr(control_vectorial_M, &control_vectorial_M->Timing.tArray[0]);

  {
    int_T *mdlSampleHits = control_vectorial_M->Timing.sampleHitArray;
    int_T *mdlPerTaskSampleHits =
      control_vectorial_M->Timing.perTaskSampleHitsArray;
    control_vectorial_M->Timing.perTaskSampleHits = (&mdlPerTaskSampleHits[0]);
    mdlSampleHits[0] = 1;
    control_vectorial_M->Timing.sampleHits = (&mdlSampleHits[0]);
  }

  rtmSetTFinal(control_vectorial_M, -1);
  control_vectorial_M->Timing.stepSize0 = 5.0E-5;
  control_vectorial_M->Timing.stepSize1 = 5.0E-5;
  control_vectorial_M->Timing.stepSize2 = 0.0001;
  control_vectorial_M->solverInfoPtr = (&control_vectorial_M->solverInfo);
  control_vectorial_M->Timing.stepSize = (5.0E-5);
  rtsiSetFixedStepSize(&control_vectorial_M->solverInfo, 5.0E-5);
  rtsiSetSolverMode(&control_vectorial_M->solverInfo, SOLVER_MODE_MULTITASKING);

  /* block I/O */
  (void) std::memset((static_cast<void *>(&control_vectorial_B)), 0,
                     sizeof(B_control_vectorial_T));

  /* states (continuous) */
  {
    (void) std::memset(static_cast<void *>(&control_vectorial_X), 0,
                       sizeof(X_control_vectorial_T));
  }

  /* disabled states */
  {
    (void) std::memset(static_cast<void *>(&control_vectorial_XDis), 0,
                       sizeof(XDis_control_vectorial_T));
  }

  /* states (dwork) */
  (void) std::memset(static_cast<void *>(&control_vectorial_DW), 0,
                     sizeof(DW_control_vectorial_T));

  /* child S-Function registration */
  {
    RTWSfcnInfo *sfcnInfo = &control_vectorial_M->NonInlinedSFcns.sfcnInfo;
    control_vectorial_M->sfcnInfo = (sfcnInfo);
    rtssSetErrorStatusPtr(sfcnInfo, (&rtmGetErrorStatus(control_vectorial_M)));
    control_vectorial_M->Sizes.numSampTimes = (3);
    rtssSetNumRootSampTimesPtr(sfcnInfo,
      &control_vectorial_M->Sizes.numSampTimes);
    control_vectorial_M->NonInlinedSFcns.taskTimePtrs[0] = (&rtmGetTPtr
      (control_vectorial_M)[0]);
    control_vectorial_M->NonInlinedSFcns.taskTimePtrs[1] = (&rtmGetTPtr
      (control_vectorial_M)[1]);
    control_vectorial_M->NonInlinedSFcns.taskTimePtrs[2] = (&rtmGetTPtr
      (control_vectorial_M)[2]);
    rtssSetTPtrPtr(sfcnInfo,control_vectorial_M->NonInlinedSFcns.taskTimePtrs);
    rtssSetTStartPtr(sfcnInfo, &rtmGetTStart(control_vectorial_M));
    rtssSetTFinalPtr(sfcnInfo, &rtmGetTFinal(control_vectorial_M));
    rtssSetTimeOfLastOutputPtr(sfcnInfo, &rtmGetTimeOfLastOutput
      (control_vectorial_M));
    rtssSetStepSizePtr(sfcnInfo, &control_vectorial_M->Timing.stepSize);
    rtssSetStopRequestedPtr(sfcnInfo, &rtmGetStopRequested(control_vectorial_M));
    rtssSetDerivCacheNeedsResetPtr(sfcnInfo,
      &control_vectorial_M->derivCacheNeedsReset);
    rtssSetZCCacheNeedsResetPtr(sfcnInfo,
      &control_vectorial_M->zCCacheNeedsReset);
    rtssSetContTimeOutputInconsistentWithStateAtMajorStepPtr(sfcnInfo,
      &control_vectorial_M->CTOutputIncnstWithState);
    rtssSetSampleHitsPtr(sfcnInfo, &control_vectorial_M->Timing.sampleHits);
    rtssSetPerTaskSampleHitsPtr(sfcnInfo,
      &control_vectorial_M->Timing.perTaskSampleHits);
    rtssSetSimModePtr(sfcnInfo, &control_vectorial_M->simMode);
    rtssSetSolverInfoPtr(sfcnInfo, &control_vectorial_M->solverInfoPtr);
  }

  control_vectorial_M->Sizes.numSFcns = (3);

  /* register each child */
  {
    (void) std::memset(static_cast<void *>
                       (&control_vectorial_M->NonInlinedSFcns.childSFunctions[0]),
                       0,
                       3*sizeof(SimStruct));
    control_vectorial_M->childSfunctions =
      (&control_vectorial_M->NonInlinedSFcns.childSFunctionPtrs[0]);
    control_vectorial_M->childSfunctions[0] =
      (&control_vectorial_M->NonInlinedSFcns.childSFunctions[0]);
    control_vectorial_M->childSfunctions[1] =
      (&control_vectorial_M->NonInlinedSFcns.childSFunctions[1]);
    control_vectorial_M->childSfunctions[2] =
      (&control_vectorial_M->NonInlinedSFcns.childSFunctions[2]);

    /* Level2 S-Function Block: control_vectorial/<Root>/Encoder (sg_fpga_di_sf_a2) */
    {
      SimStruct *rts = control_vectorial_M->childSfunctions[0];

      /* timing info */
      time_T *sfcnPeriod = control_vectorial_M->NonInlinedSFcns.Sfcn0.sfcnPeriod;
      time_T *sfcnOffset = control_vectorial_M->NonInlinedSFcns.Sfcn0.sfcnOffset;
      int_T *sfcnTsMap = control_vectorial_M->NonInlinedSFcns.Sfcn0.sfcnTsMap;
      (void) std::memset(static_cast<void*>(sfcnPeriod), 0,
                         sizeof(time_T)*1);
      (void) std::memset(static_cast<void*>(sfcnOffset), 0,
                         sizeof(time_T)*1);
      ssSetSampleTimePtr(rts, &sfcnPeriod[0]);
      ssSetOffsetTimePtr(rts, &sfcnOffset[0]);
      ssSetSampleTimeTaskIDPtr(rts, sfcnTsMap);

      {
        ssSetBlkInfo2Ptr(rts, &control_vectorial_M->NonInlinedSFcns.blkInfo2[0]);
      }

      _ssSetBlkInfo2PortInfo2Ptr(rts,
        &control_vectorial_M->NonInlinedSFcns.inputOutputPortInfo2[0]);

      /* Set up the mdlInfo pointer */
      ssSetRTWSfcnInfo(rts, control_vectorial_M->sfcnInfo);

      /* Allocate memory of model methods 2 */
      {
        ssSetModelMethods2(rts, &control_vectorial_M->NonInlinedSFcns.methods2[0]);
      }

      /* Allocate memory of model methods 3 */
      {
        ssSetModelMethods3(rts, &control_vectorial_M->NonInlinedSFcns.methods3[0]);
      }

      /* Allocate memory of model methods 4 */
      {
        ssSetModelMethods4(rts, &control_vectorial_M->NonInlinedSFcns.methods4[0]);
      }

      /* Allocate memory for states auxilliary information */
      {
        ssSetStatesInfo2(rts, &control_vectorial_M->NonInlinedSFcns.statesInfo2
                         [0]);
        ssSetPeriodicStatesInfo(rts,
          &control_vectorial_M->NonInlinedSFcns.periodicStatesInfo[0]);
      }

      /* outputs */
      {
        ssSetPortInfoForOutputs(rts,
          &control_vectorial_M->NonInlinedSFcns.Sfcn0.outputPortInfo[0]);
        ssSetPortInfoForOutputs(rts,
          &control_vectorial_M->NonInlinedSFcns.Sfcn0.outputPortInfo[0]);
        _ssSetNumOutputPorts(rts, 2);
        _ssSetPortInfo2ForOutputUnits(rts,
          &control_vectorial_M->NonInlinedSFcns.Sfcn0.outputPortUnits[0]);
        ssSetOutputPortUnit(rts, 0, 0);
        ssSetOutputPortUnit(rts, 1, 0);
        _ssSetPortInfo2ForOutputCoSimAttribute(rts,
          &control_vectorial_M->NonInlinedSFcns.Sfcn0.outputPortCoSimAttribute[0]);
        ssSetOutputPortIsContinuousQuantity(rts, 0, 0);
        ssSetOutputPortIsContinuousQuantity(rts, 1, 0);

        /* port 0 */
        {
          _ssSetOutputPortNumDimensions(rts, 0, 1);
          ssSetOutputPortWidthAsInt(rts, 0, 1);
          ssSetOutputPortSignal(rts, 0, ((real_T *)
            &control_vectorial_B.Encoder_o1));
        }

        /* port 1 */
        {
          _ssSetOutputPortNumDimensions(rts, 1, 1);
          ssSetOutputPortWidthAsInt(rts, 1, 1);
          ssSetOutputPortSignal(rts, 1, ((real_T *)
            &control_vectorial_B.Encoder_o2));
        }
      }

      /* path info */
      ssSetModelName(rts, "Encoder");
      ssSetPath(rts, "control_vectorial/Encoder");
      ssSetRTModel(rts,control_vectorial_M);
      ssSetParentSS(rts, (NULL));
      ssSetRootSS(rts, rts);
      ssSetVersion(rts, SIMSTRUCT_VERSION_LEVEL2);

      /* parameters */
      {
        mxArray **sfcnParams = (mxArray **)
          &control_vectorial_M->NonInlinedSFcns.Sfcn0.params;
        ssSetSFcnParamsCount(rts, 4);
        ssSetSFcnParamsPtr(rts, &sfcnParams[0]);
        ssSetSFcnParam(rts, 0, (mxArray*)control_vectorial_cal->Encoder_P1_Size);
        ssSetSFcnParam(rts, 1, (mxArray*)control_vectorial_cal->Encoder_P2_Size);
        ssSetSFcnParam(rts, 2, (mxArray*)control_vectorial_cal->Encoder_P3_Size);
        ssSetSFcnParam(rts, 3, (mxArray*)control_vectorial_cal->Encoder_P4_Size);
      }

      /* work vectors */
      ssSetPWork(rts, (void **) &control_vectorial_DW.Encoder_PWORK[0]);

      {
        struct _ssDWorkRecord *dWorkRecord = (struct _ssDWorkRecord *)
          &control_vectorial_M->NonInlinedSFcns.Sfcn0.dWork;
        struct _ssDWorkAuxRecord *dWorkAuxRecord = (struct _ssDWorkAuxRecord *)
          &control_vectorial_M->NonInlinedSFcns.Sfcn0.dWorkAux;
        ssSetSFcnDWork(rts, dWorkRecord);
        ssSetSFcnDWorkAux(rts, dWorkAuxRecord);
        ssSetNumDWorkAsInt(rts, 1);

        /* PWORK */
        ssSetDWorkWidthAsInt(rts, 0, 2);
        ssSetDWorkDataType(rts, 0,SS_POINTER);
        ssSetDWorkComplexSignal(rts, 0, 0);
        ssSetDWork(rts, 0, &control_vectorial_DW.Encoder_PWORK[0]);
      }

      /* registration */
      sg_fpga_di_sf_a2(rts);
      sfcnInitializeSizes(rts);
      sfcnInitializeSampleTimes(rts);

      /* adjust sample time */
      ssSetSampleTime(rts, 0, 5.0E-5);
      ssSetOffsetTime(rts, 0, 0.0);
      sfcnTsMap[0] = 1;

      /* set compiled values of dynamic vector attributes */
      ssSetNumNonsampledZCsAsInt(rts, 0);

      /* Update connectivity flags for each port */
      _ssSetOutputPortConnected(rts, 0, 1);
      _ssSetOutputPortConnected(rts, 1, 1);
      _ssSetOutputPortBeingMerged(rts, 0, 0);
      _ssSetOutputPortBeingMerged(rts, 1, 0);

      /* Update the BufferDstPort flags for each input port */
    }

    /* Level2 S-Function Block: control_vectorial/<Root>/Digital output (sg_fpga_do_sf_a2) */
    {
      SimStruct *rts = control_vectorial_M->childSfunctions[1];

      /* timing info */
      time_T *sfcnPeriod = control_vectorial_M->NonInlinedSFcns.Sfcn1.sfcnPeriod;
      time_T *sfcnOffset = control_vectorial_M->NonInlinedSFcns.Sfcn1.sfcnOffset;
      int_T *sfcnTsMap = control_vectorial_M->NonInlinedSFcns.Sfcn1.sfcnTsMap;
      (void) std::memset(static_cast<void*>(sfcnPeriod), 0,
                         sizeof(time_T)*1);
      (void) std::memset(static_cast<void*>(sfcnOffset), 0,
                         sizeof(time_T)*1);
      ssSetSampleTimePtr(rts, &sfcnPeriod[0]);
      ssSetOffsetTimePtr(rts, &sfcnOffset[0]);
      ssSetSampleTimeTaskIDPtr(rts, sfcnTsMap);

      {
        ssSetBlkInfo2Ptr(rts, &control_vectorial_M->NonInlinedSFcns.blkInfo2[1]);
      }

      _ssSetBlkInfo2PortInfo2Ptr(rts,
        &control_vectorial_M->NonInlinedSFcns.inputOutputPortInfo2[1]);

      /* Set up the mdlInfo pointer */
      ssSetRTWSfcnInfo(rts, control_vectorial_M->sfcnInfo);

      /* Allocate memory of model methods 2 */
      {
        ssSetModelMethods2(rts, &control_vectorial_M->NonInlinedSFcns.methods2[1]);
      }

      /* Allocate memory of model methods 3 */
      {
        ssSetModelMethods3(rts, &control_vectorial_M->NonInlinedSFcns.methods3[1]);
      }

      /* Allocate memory of model methods 4 */
      {
        ssSetModelMethods4(rts, &control_vectorial_M->NonInlinedSFcns.methods4[1]);
      }

      /* Allocate memory for states auxilliary information */
      {
        ssSetStatesInfo2(rts, &control_vectorial_M->NonInlinedSFcns.statesInfo2
                         [1]);
        ssSetPeriodicStatesInfo(rts,
          &control_vectorial_M->NonInlinedSFcns.periodicStatesInfo[1]);
      }

      /* inputs */
      {
        _ssSetNumInputPorts(rts, 6);
        ssSetPortInfoForInputs(rts,
          &control_vectorial_M->NonInlinedSFcns.Sfcn1.inputPortInfo[0]);
        ssSetPortInfoForInputs(rts,
          &control_vectorial_M->NonInlinedSFcns.Sfcn1.inputPortInfo[0]);
        _ssSetPortInfo2ForInputUnits(rts,
          &control_vectorial_M->NonInlinedSFcns.Sfcn1.inputPortUnits[0]);
        ssSetInputPortUnit(rts, 0, 0);
        ssSetInputPortUnit(rts, 1, 0);
        ssSetInputPortUnit(rts, 2, 0);
        ssSetInputPortUnit(rts, 3, 0);
        ssSetInputPortUnit(rts, 4, 0);
        ssSetInputPortUnit(rts, 5, 0);
        _ssSetPortInfo2ForInputCoSimAttribute(rts,
          &control_vectorial_M->NonInlinedSFcns.Sfcn1.inputPortCoSimAttribute[0]);
        ssSetInputPortIsContinuousQuantity(rts, 0, 0);
        ssSetInputPortIsContinuousQuantity(rts, 1, 0);
        ssSetInputPortIsContinuousQuantity(rts, 2, 0);
        ssSetInputPortIsContinuousQuantity(rts, 3, 0);
        ssSetInputPortIsContinuousQuantity(rts, 4, 0);
        ssSetInputPortIsContinuousQuantity(rts, 5, 0);

        /* port 0 */
        {
          ssSetInputPortRequiredContiguous(rts, 0, 1);
          ssSetInputPortSignal(rts, 0,
                               &control_vectorial_B.DataTypeConversion_b[0]);
          _ssSetInputPortNumDimensions(rts, 0, 1);
          ssSetInputPortWidthAsInt(rts, 0, 1);
        }

        /* port 1 */
        {
          ssSetInputPortRequiredContiguous(rts, 1, 1);
          ssSetInputPortSignal(rts, 1,
                               &control_vectorial_B.DataTypeConversion_b[1]);
          _ssSetInputPortNumDimensions(rts, 1, 1);
          ssSetInputPortWidthAsInt(rts, 1, 1);
        }

        /* port 2 */
        {
          ssSetInputPortRequiredContiguous(rts, 2, 1);
          ssSetInputPortSignal(rts, 2,
                               &control_vectorial_B.DataTypeConversion_b[2]);
          _ssSetInputPortNumDimensions(rts, 2, 1);
          ssSetInputPortWidthAsInt(rts, 2, 1);
        }

        /* port 3 */
        {
          ssSetInputPortRequiredContiguous(rts, 3, 1);
          ssSetInputPortSignal(rts, 3,
                               &control_vectorial_B.DataTypeConversion_b[3]);
          _ssSetInputPortNumDimensions(rts, 3, 1);
          ssSetInputPortWidthAsInt(rts, 3, 1);
        }

        /* port 4 */
        {
          ssSetInputPortRequiredContiguous(rts, 4, 1);
          ssSetInputPortSignal(rts, 4,
                               &control_vectorial_B.DataTypeConversion_b[4]);
          _ssSetInputPortNumDimensions(rts, 4, 1);
          ssSetInputPortWidthAsInt(rts, 4, 1);
        }

        /* port 5 */
        {
          ssSetInputPortRequiredContiguous(rts, 5, 1);
          ssSetInputPortSignal(rts, 5,
                               &control_vectorial_B.DataTypeConversion_b[5]);
          _ssSetInputPortNumDimensions(rts, 5, 1);
          ssSetInputPortWidthAsInt(rts, 5, 1);
        }
      }

      /* path info */
      ssSetModelName(rts, "Digital output");
      ssSetPath(rts, "control_vectorial/Digital output");
      ssSetRTModel(rts,control_vectorial_M);
      ssSetParentSS(rts, (NULL));
      ssSetRootSS(rts, rts);
      ssSetVersion(rts, SIMSTRUCT_VERSION_LEVEL2);

      /* parameters */
      {
        mxArray **sfcnParams = (mxArray **)
          &control_vectorial_M->NonInlinedSFcns.Sfcn1.params;
        ssSetSFcnParamsCount(rts, 6);
        ssSetSFcnParamsPtr(rts, &sfcnParams[0]);
        ssSetSFcnParam(rts, 0, (mxArray*)
                       control_vectorial_cal->Digitaloutput_P1_Size);
        ssSetSFcnParam(rts, 1, (mxArray*)
                       control_vectorial_cal->Digitaloutput_P2_Size);
        ssSetSFcnParam(rts, 2, (mxArray*)
                       control_vectorial_cal->Digitaloutput_P3_Size);
        ssSetSFcnParam(rts, 3, (mxArray*)
                       control_vectorial_cal->Digitaloutput_P4_Size);
        ssSetSFcnParam(rts, 4, (mxArray*)
                       control_vectorial_cal->Digitaloutput_P5_Size);
        ssSetSFcnParam(rts, 5, (mxArray*)
                       control_vectorial_cal->Digitaloutput_P6_Size);
      }

      /* work vectors */
      ssSetIWork(rts, (int_T *) &control_vectorial_DW.Digitaloutput_IWORK);
      ssSetPWork(rts, (void **) &control_vectorial_DW.Digitaloutput_PWORK[0]);

      {
        struct _ssDWorkRecord *dWorkRecord = (struct _ssDWorkRecord *)
          &control_vectorial_M->NonInlinedSFcns.Sfcn1.dWork;
        struct _ssDWorkAuxRecord *dWorkAuxRecord = (struct _ssDWorkAuxRecord *)
          &control_vectorial_M->NonInlinedSFcns.Sfcn1.dWorkAux;
        ssSetSFcnDWork(rts, dWorkRecord);
        ssSetSFcnDWorkAux(rts, dWorkAuxRecord);
        ssSetNumDWorkAsInt(rts, 2);

        /* IWORK */
        ssSetDWorkWidthAsInt(rts, 0, 1);
        ssSetDWorkDataType(rts, 0,SS_INTEGER);
        ssSetDWorkComplexSignal(rts, 0, 0);
        ssSetDWork(rts, 0, &control_vectorial_DW.Digitaloutput_IWORK);

        /* PWORK */
        ssSetDWorkWidthAsInt(rts, 1, 2);
        ssSetDWorkDataType(rts, 1,SS_POINTER);
        ssSetDWorkComplexSignal(rts, 1, 0);
        ssSetDWork(rts, 1, &control_vectorial_DW.Digitaloutput_PWORK[0]);
      }

      /* registration */
      sg_fpga_do_sf_a2(rts);
      sfcnInitializeSizes(rts);
      sfcnInitializeSampleTimes(rts);

      /* adjust sample time */
      ssSetSampleTime(rts, 0, 5.0E-5);
      ssSetOffsetTime(rts, 0, 0.0);
      sfcnTsMap[0] = 1;

      /* set compiled values of dynamic vector attributes */
      ssSetNumNonsampledZCsAsInt(rts, 0);

      /* Update connectivity flags for each port */
      _ssSetInputPortConnected(rts, 0, 1);
      _ssSetInputPortConnected(rts, 1, 1);
      _ssSetInputPortConnected(rts, 2, 1);
      _ssSetInputPortConnected(rts, 3, 1);
      _ssSetInputPortConnected(rts, 4, 1);
      _ssSetInputPortConnected(rts, 5, 1);

      /* Update the BufferDstPort flags for each input port */
      ssSetInputPortBufferDstPort(rts, 0, -1);
      ssSetInputPortBufferDstPort(rts, 1, -1);
      ssSetInputPortBufferDstPort(rts, 2, -1);
      ssSetInputPortBufferDstPort(rts, 3, -1);
      ssSetInputPortBufferDstPort(rts, 4, -1);
      ssSetInputPortBufferDstPort(rts, 5, -1);
    }

    /* Level2 S-Function Block: control_vectorial/<Root>/Corrientes y Par (sg_fpga_IO397_ad) */
    {
      SimStruct *rts = control_vectorial_M->childSfunctions[2];

      /* timing info */
      time_T *sfcnPeriod = control_vectorial_M->NonInlinedSFcns.Sfcn2.sfcnPeriod;
      time_T *sfcnOffset = control_vectorial_M->NonInlinedSFcns.Sfcn2.sfcnOffset;
      int_T *sfcnTsMap = control_vectorial_M->NonInlinedSFcns.Sfcn2.sfcnTsMap;
      (void) std::memset(static_cast<void*>(sfcnPeriod), 0,
                         sizeof(time_T)*1);
      (void) std::memset(static_cast<void*>(sfcnOffset), 0,
                         sizeof(time_T)*1);
      ssSetSampleTimePtr(rts, &sfcnPeriod[0]);
      ssSetOffsetTimePtr(rts, &sfcnOffset[0]);
      ssSetSampleTimeTaskIDPtr(rts, sfcnTsMap);

      {
        ssSetBlkInfo2Ptr(rts, &control_vectorial_M->NonInlinedSFcns.blkInfo2[2]);
      }

      _ssSetBlkInfo2PortInfo2Ptr(rts,
        &control_vectorial_M->NonInlinedSFcns.inputOutputPortInfo2[2]);

      /* Set up the mdlInfo pointer */
      ssSetRTWSfcnInfo(rts, control_vectorial_M->sfcnInfo);

      /* Allocate memory of model methods 2 */
      {
        ssSetModelMethods2(rts, &control_vectorial_M->NonInlinedSFcns.methods2[2]);
      }

      /* Allocate memory of model methods 3 */
      {
        ssSetModelMethods3(rts, &control_vectorial_M->NonInlinedSFcns.methods3[2]);
      }

      /* Allocate memory of model methods 4 */
      {
        ssSetModelMethods4(rts, &control_vectorial_M->NonInlinedSFcns.methods4[2]);
      }

      /* Allocate memory for states auxilliary information */
      {
        ssSetStatesInfo2(rts, &control_vectorial_M->NonInlinedSFcns.statesInfo2
                         [2]);
        ssSetPeriodicStatesInfo(rts,
          &control_vectorial_M->NonInlinedSFcns.periodicStatesInfo[2]);
      }

      /* outputs */
      {
        ssSetPortInfoForOutputs(rts,
          &control_vectorial_M->NonInlinedSFcns.Sfcn2.outputPortInfo[0]);
        ssSetPortInfoForOutputs(rts,
          &control_vectorial_M->NonInlinedSFcns.Sfcn2.outputPortInfo[0]);
        _ssSetNumOutputPorts(rts, 3);
        _ssSetPortInfo2ForOutputUnits(rts,
          &control_vectorial_M->NonInlinedSFcns.Sfcn2.outputPortUnits[0]);
        ssSetOutputPortUnit(rts, 0, 0);
        ssSetOutputPortUnit(rts, 1, 0);
        ssSetOutputPortUnit(rts, 2, 0);
        _ssSetPortInfo2ForOutputCoSimAttribute(rts,
          &control_vectorial_M->NonInlinedSFcns.Sfcn2.outputPortCoSimAttribute[0]);
        ssSetOutputPortIsContinuousQuantity(rts, 0, 0);
        ssSetOutputPortIsContinuousQuantity(rts, 1, 0);
        ssSetOutputPortIsContinuousQuantity(rts, 2, 0);

        /* port 0 */
        {
          _ssSetOutputPortNumDimensions(rts, 0, 1);
          ssSetOutputPortWidthAsInt(rts, 0, 1);
          ssSetOutputPortSignal(rts, 0, ((real_T *)
            &control_vectorial_B.CorrientesyPar_o1));
        }

        /* port 1 */
        {
          _ssSetOutputPortNumDimensions(rts, 1, 1);
          ssSetOutputPortWidthAsInt(rts, 1, 1);
          ssSetOutputPortSignal(rts, 1, ((real_T *)
            &control_vectorial_B.CorrientesyPar_o2));
        }

        /* port 2 */
        {
          _ssSetOutputPortNumDimensions(rts, 2, 1);
          ssSetOutputPortWidthAsInt(rts, 2, 1);
          ssSetOutputPortSignal(rts, 2, ((real_T *)
            &control_vectorial_B.CorrientesyPar_o3));
        }
      }

      /* path info */
      ssSetModelName(rts, "Corrientes y Par");
      ssSetPath(rts, "control_vectorial/Corrientes y Par");
      ssSetRTModel(rts,control_vectorial_M);
      ssSetParentSS(rts, (NULL));
      ssSetRootSS(rts, rts);
      ssSetVersion(rts, SIMSTRUCT_VERSION_LEVEL2);

      /* parameters */
      {
        mxArray **sfcnParams = (mxArray **)
          &control_vectorial_M->NonInlinedSFcns.Sfcn2.params;
        ssSetSFcnParamsCount(rts, 9);
        ssSetSFcnParamsPtr(rts, &sfcnParams[0]);
        ssSetSFcnParam(rts, 0, (mxArray*)
                       control_vectorial_cal->CorrientesyPar_P1_Size);
        ssSetSFcnParam(rts, 1, (mxArray*)
                       control_vectorial_cal->CorrientesyPar_P2_Size);
        ssSetSFcnParam(rts, 2, (mxArray*)
                       control_vectorial_cal->CorrientesyPar_P3_Size);
        ssSetSFcnParam(rts, 3, (mxArray*)
                       control_vectorial_cal->CorrientesyPar_P4_Size);
        ssSetSFcnParam(rts, 4, (mxArray*)
                       control_vectorial_cal->CorrientesyPar_P5_Size);
        ssSetSFcnParam(rts, 5, (mxArray*)
                       control_vectorial_cal->CorrientesyPar_P6_Size);
        ssSetSFcnParam(rts, 6, (mxArray*)
                       control_vectorial_cal->CorrientesyPar_P7_Size);
        ssSetSFcnParam(rts, 7, (mxArray*)
                       control_vectorial_cal->CorrientesyPar_P8_Size);
        ssSetSFcnParam(rts, 8, (mxArray*)
                       control_vectorial_cal->CorrientesyPar_P9_Size);
      }

      /* work vectors */
      ssSetPWork(rts, (void **) &control_vectorial_DW.CorrientesyPar_PWORK[0]);

      {
        struct _ssDWorkRecord *dWorkRecord = (struct _ssDWorkRecord *)
          &control_vectorial_M->NonInlinedSFcns.Sfcn2.dWork;
        struct _ssDWorkAuxRecord *dWorkAuxRecord = (struct _ssDWorkAuxRecord *)
          &control_vectorial_M->NonInlinedSFcns.Sfcn2.dWorkAux;
        ssSetSFcnDWork(rts, dWorkRecord);
        ssSetSFcnDWorkAux(rts, dWorkAuxRecord);
        ssSetNumDWorkAsInt(rts, 1);

        /* PWORK */
        ssSetDWorkWidthAsInt(rts, 0, 3);
        ssSetDWorkDataType(rts, 0,SS_POINTER);
        ssSetDWorkComplexSignal(rts, 0, 0);
        ssSetDWork(rts, 0, &control_vectorial_DW.CorrientesyPar_PWORK[0]);
      }

      /* registration */
      sg_fpga_IO397_ad(rts);
      sfcnInitializeSizes(rts);
      sfcnInitializeSampleTimes(rts);

      /* adjust sample time */
      ssSetSampleTime(rts, 0, 0.0001);
      ssSetOffsetTime(rts, 0, 0.0);
      sfcnTsMap[0] = 2;

      /* set compiled values of dynamic vector attributes */
      ssSetNumNonsampledZCsAsInt(rts, 0);

      /* Update connectivity flags for each port */
      _ssSetOutputPortConnected(rts, 0, 1);
      _ssSetOutputPortConnected(rts, 1, 1);
      _ssSetOutputPortConnected(rts, 2, 1);
      _ssSetOutputPortBeingMerged(rts, 0, 0);
      _ssSetOutputPortBeingMerged(rts, 1, 0);
      _ssSetOutputPortBeingMerged(rts, 2, 0);

      /* Update the BufferDstPort flags for each input port */
    }
  }

  /* Start for S-Function (sg_fpga_di_sf_a2): '<Root>/Encoder' */
  /* Level2 S-Function Block: '<Root>/Encoder' (sg_fpga_di_sf_a2) */
  {
    SimStruct *rts = control_vectorial_M->childSfunctions[0];
    sfcnStart(rts);
    if (ssGetErrorStatus(rts) != (NULL))
      return;
  }

  /* Start for S-Function (sg_fpga_do_sf_a2): '<Root>/Digital output' */
  /* Level2 S-Function Block: '<Root>/Digital output' (sg_fpga_do_sf_a2) */
  {
    SimStruct *rts = control_vectorial_M->childSfunctions[1];
    sfcnStart(rts);
    if (ssGetErrorStatus(rts) != (NULL))
      return;
  }

  /* Start for RateTransition generated from: '<S21>/Filtro Pasa Baja1' */
  control_vectorial_B.TmpRTBAtFiltroPasaBaja1Inport1 =
    control_vectorial_cal->TmpRTBAtFiltroPasaBaja1Inport1_;

  /* Start for RateTransition generated from: '<S21>/Filtro Pasa Baja2' */
  control_vectorial_B.TmpRTBAtFiltroPasaBaja2Inport1 =
    control_vectorial_cal->TmpRTBAtFiltroPasaBaja2Inport1_;

  /* Start for RateTransition generated from: '<S21>/Filtro Pasa Baja' */
  control_vectorial_B.TmpRTBAtFiltroPasaBajaInport1 =
    control_vectorial_cal->TmpRTBAtFiltroPasaBajaInport1_I;

  /* Start for S-Function (sg_fpga_IO397_ad): '<Root>/Corrientes y Par' */
  /* Level2 S-Function Block: '<Root>/Corrientes y Par' (sg_fpga_IO397_ad) */
  {
    SimStruct *rts = control_vectorial_M->childSfunctions[2];
    sfcnStart(rts);
    if (ssGetErrorStatus(rts) != (NULL))
      return;
  }

  control_vectorial_PrevZCX.TriggeredSubsystem_Trig_ZCE = POS_ZCSIG;

  {
    int32_T i;

    /* InitializeConditions for Memory: '<S5>/Memory' */
    control_vectorial_DW.Memory_PreviousInput =
      control_vectorial_cal->EdgeDetector_ic;

    /* InitializeConditions for Delay: '<S1>/Delay4' */
    control_vectorial_DW.Delay4_DSTATE =
      control_vectorial_cal->Delay4_InitialCondition;

    /* InitializeConditions for Delay: '<S1>/Delay1' */
    control_vectorial_DW.Delay1_DSTATE =
      control_vectorial_cal->Delay1_InitialCondition;
    for (i = 0; i < 200; i++) {
      /* InitializeConditions for Delay: '<S1>/Delay3' */
      control_vectorial_DW.Delay3_DSTATE[i] =
        control_vectorial_cal->Delay3_InitialCondition;

      /* InitializeConditions for Delay: '<S1>/Delay2' */
      control_vectorial_DW.Delay2_DSTATE[i] =
        control_vectorial_cal->Delay2_InitialCondition;
    }

    /* InitializeConditions for TransferFcn: '<S21>/Filtro Pasa Baja' */
    control_vectorial_X.FiltroPasaBaja_CSTATE = 0.0;

    /* InitializeConditions for TransferFcn: '<S21>/Filtro Pasa Baja1' */
    control_vectorial_X.FiltroPasaBaja1_CSTATE = 0.0;

    /* InitializeConditions for TransferFcn: '<S21>/Filtro Pasa Baja2' */
    control_vectorial_X.FiltroPasaBaja2_CSTATE = 0.0;

    /* InitializeConditions for UnitDelay: '<S39>/Unit Delay' */
    control_vectorial_DW.UnitDelay_DSTATE =
      control_vectorial_cal->UnitDelay_InitialCondition;

    /* InitializeConditions for DiscreteIntegrator: '<S39>/Discrete-Time Integrator' */
    control_vectorial_DW.DiscreteTimeIntegrator_DSTATE =
      control_vectorial_cal->DiscretePIController_x0;
    control_vectorial_DW.DiscreteTimeIntegrator_PrevRese = 2;

    /* InitializeConditions for UnitDelay: '<S3>/Unit Delay' */
    control_vectorial_DW.UnitDelay_DSTATE_j[0] =
      control_vectorial_cal->UnitDelay_InitialCondition_i;
    control_vectorial_DW.UnitDelay_DSTATE_j[1] =
      control_vectorial_cal->UnitDelay_InitialCondition_i;

    /* InitializeConditions for UnitDelay: '<S24>/Unit Delay' */
    control_vectorial_DW.UnitDelay_DSTATE_p =
      control_vectorial_cal->UnitDelay_InitialCondition_g;

    /* InitializeConditions for DiscreteIntegrator: '<S24>/Discrete-Time Integrator' */
    control_vectorial_DW.DiscreteTimeIntegrator_DSTATE_h =
      control_vectorial_cal->DiscreteTimeIntegrator_IC;

    /* InitializeConditions for Integrator: '<S29>/Integrator' */
    control_vectorial_X.Integrator_CSTATE = control_vectorial_cal->Integrator_IC;

    /* InitializeConditions for Integrator: '<S30>/Integrator' */
    control_vectorial_X.Integrator_CSTATE_k =
      control_vectorial_cal->Integrator_IC_l;

    /* InitializeConditions for RateTransition generated from: '<S21>/Filtro Pasa Baja1' */
    control_vectorial_DW.TmpRTBAtFiltroPasaBaja1Inport1_[0] =
      control_vectorial_cal->TmpRTBAtFiltroPasaBaja1Inport1_;
    control_vectorial_DW.TmpRTBAtFiltroPasaBaja1Inport_j = 0;
    control_vectorial_DW.TmpRTBAtFiltroPasaBaja1Inport_a = 1;

    /* InitializeConditions for RateTransition generated from: '<S21>/Filtro Pasa Baja2' */
    control_vectorial_DW.TmpRTBAtFiltroPasaBaja2Inport1_[0] =
      control_vectorial_cal->TmpRTBAtFiltroPasaBaja2Inport1_;
    control_vectorial_DW.TmpRTBAtFiltroPasaBaja2Inport_p = 0;
    control_vectorial_DW.TmpRTBAtFiltroPasaBaja2Inport_l = 1;

    /* InitializeConditions for RateTransition generated from: '<S21>/Filtro Pasa Baja' */
    control_vectorial_DW.TmpRTBAtFiltroPasaBajaInport1_B[0] =
      control_vectorial_cal->TmpRTBAtFiltroPasaBajaInport1_I;
    control_vectorial_DW.TmpRTBAtFiltroPasaBajaInport1_W = 0;
    control_vectorial_DW.TmpRTBAtFiltroPasaBajaInport1_R = 1;

    /* SystemInitialize for Triggered SubSystem: '<S6>/Triggered Subsystem' */
    /* SystemInitialize for SignalConversion generated from: '<S9>/In1' incorporates:
     *  Outport: '<S9>/Out1'
     */
    control_vectorial_B.In1 = control_vectorial_cal->Out1_Y0;

    /* End of SystemInitialize for SubSystem: '<S6>/Triggered Subsystem' */

    /* SystemInitialize for Enabled SubSystem: '<S5>/NEGATIVE Edge' */
    /* SystemInitialize for RelationalOperator: '<S7>/Relational Operator1' incorporates:
     *  Outport: '<S7>/OUT'
     */
    control_vectorial_B.RelationalOperator1_m = control_vectorial_cal->OUT_Y0;

    /* End of SystemInitialize for SubSystem: '<S5>/NEGATIVE Edge' */

    /* SystemInitialize for Enabled SubSystem: '<S5>/POSITIVE Edge' */
    /* SystemInitialize for RelationalOperator: '<S8>/Relational Operator1' incorporates:
     *  Outport: '<S8>/OUT'
     */
    control_vectorial_B.RelationalOperator1 = control_vectorial_cal->OUT_Y0_l;

    /* End of SystemInitialize for SubSystem: '<S5>/POSITIVE Edge' */
  }
}

/* Model terminate function */
void control_vectorial_terminate(void)
{
  /* Terminate for S-Function (sg_fpga_di_sf_a2): '<Root>/Encoder' */
  /* Level2 S-Function Block: '<Root>/Encoder' (sg_fpga_di_sf_a2) */
  {
    SimStruct *rts = control_vectorial_M->childSfunctions[0];
    sfcnTerminate(rts);
  }

  /* Terminate for S-Function (sg_fpga_do_sf_a2): '<Root>/Digital output' */
  /* Level2 S-Function Block: '<Root>/Digital output' (sg_fpga_do_sf_a2) */
  {
    SimStruct *rts = control_vectorial_M->childSfunctions[1];
    sfcnTerminate(rts);
  }

  /* Terminate for S-Function (sg_fpga_IO397_ad): '<Root>/Corrientes y Par' */
  /* Level2 S-Function Block: '<Root>/Corrientes y Par' (sg_fpga_IO397_ad) */
  {
    SimStruct *rts = control_vectorial_M->childSfunctions[2];
    sfcnTerminate(rts);
  }

  /* user code (Terminate function Trailer) */
  {
    freeFPGAModuleSgLib((uint32_t)1);
  }
}
