MC100/Simulink/PMSM_Controller_ert_rtw/PMSM_Controller.c

/*
 * File: PMSM_Controller.c
 *
 * Code generated for Simulink model 'PMSM_Controller'.
 *
 * Model version                  : 1.1301
 * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
 * C/C++ source code generated on : Mon Apr 18 19:32:18 2022
 *
 * Target selection: ert.tlc
 * Embedded hardware selection: ARM Compatible->ARM Cortex-M
 * Code generation objectives:
 *    1. Execution efficiency
 *    2. RAM efficiency
 * Validation result: Not run
 */

#include "PMSM_Controller.h"

/* Named constants for Chart: '<S4>/Control_Mode_Manager' */
#define IN_ACTIVE                      ((uint8_T)1U)
#define IN_NO_ACTIVE_CHILD             ((uint8_T)0U)
#define IN_OPEN                        ((uint8_T)2U)
#define IN_SPEED_MODE                  ((uint8_T)1U)
#define IN_TORQUE_MODE                 ((uint8_T)2U)
#define OPEN_MODE                      ((uint8_T)0U)
#define SPD_MODE                       ((uint8_T)1U)
#define TRQ_MODE                       ((uint8_T)2U)
#ifndef UCHAR_MAX
#include <limits.h>
#endif

#if ( UCHAR_MAX != (0xFFU) ) || ( SCHAR_MAX != (0x7F) )
#error Code was generated for compiler with different sized uchar/char. \
Consider adjusting Test hardware word size settings on the \
Hardware Implementation pane to match your compiler word sizes as \
defined in limits.h of the compiler. Alternatively, you can \
select the Test hardware is the same as production hardware option and \
select the Enable portable word sizes option on the Code Generation > \
Verification pane for ERT based targets, which will disable the \
preprocessor word size checks.
#endif

#if ( USHRT_MAX != (0xFFFFU) ) || ( SHRT_MAX != (0x7FFF) )
#error Code was generated for compiler with different sized ushort/short. \
Consider adjusting Test hardware word size settings on the \
Hardware Implementation pane to match your compiler word sizes as \
defined in limits.h of the compiler. Alternatively, you can \
select the Test hardware is the same as production hardware option and \
select the Enable portable word sizes option on the Code Generation > \
Verification pane for ERT based targets, which will disable the \
preprocessor word size checks.
#endif

#if ( UINT_MAX != (0xFFFFFFFFU) ) || ( INT_MAX != (0x7FFFFFFF) )
#error Code was generated for compiler with different sized uint/int. \
Consider adjusting Test hardware word size settings on the \
Hardware Implementation pane to match your compiler word sizes as \
defined in limits.h of the compiler. Alternatively, you can \
select the Test hardware is the same as production hardware option and \
select the Enable portable word sizes option on the Code Generation > \
Verification pane for ERT based targets, which will disable the \
preprocessor word size checks.
#endif

#if ( ULONG_MAX != (0xFFFFFFFFU) ) || ( LONG_MAX != (0x7FFFFFFF) )
#error Code was generated for compiler with different sized ulong/long. \
Consider adjusting Test hardware word size settings on the \
Hardware Implementation pane to match your compiler word sizes as \
defined in limits.h of the compiler. Alternatively, you can \
select the Test hardware is the same as production hardware option and \
select the Enable portable word sizes option on the Code Generation > \
Verification pane for ERT based targets, which will disable the \
preprocessor word size checks.
#endif

/* Skipping ulong_long/long_long check: insufficient preprocessor integer range. */
extern int16_T rt_sqrt_Us16En12_Ys16E_cQn1iwAF(int16_T u);
static uint16_T plook_u16s16_evencka(int16_T u, int16_T bp0, uint16_T bpSpace,
  uint32_T maxIndex);
static void Counter_Init(DW_Counter *localDW, uint16_T rtp_z_cntInit);
static uint16_T Counter(uint16_T rtu_inc, uint16_T rtu_max, boolean_T rtu_rst,
  DW_Counter *localDW);
static boolean_T either_edge(boolean_T rtu_u, DW_either_edge *localDW);
static void Debounce_Filter_Init(DW_Debounce_Filter *localDW);
static void Debounce_Filter(boolean_T rtu_u, uint16_T rtu_tAcv, uint16_T
  rtu_tDeacv, boolean_T *rty_y, DW_Debounce_Filter *localDW);
static void Low_Pass_Filter(const int16_T rtu_u[2], uint16_T rtu_coef, int16_T
  rty_y[2], DW_Low_Pass_Filter *localDW);
static void PI_backCalc_fixdt_Init(DW_PI_backCalc_fixdt *localDW);
static void PI_backCalc_fixdt(int16_T rtu_err, int16_T rtu_P, int16_T rtu_I,
  int16_T rtu_Kb, int16_T rtu_satMax, int16_T rtu_satMin, int16_T
  rtu_ext_limProt, uint8_T rtu_reset, int16_T *rty_pi_out, const
  ConstB_PI_backCalc_fixdt *localC, DW_PI_backCalc_fixdt *localDW,
  ZCE_PI_backCalc_fixdt *localZCE);
static void pi_speed_Init(DW_pi_speed *localDW);
static int16_T pi_speed(int16_T rtu_err, int16_T rtu_P, int16_T rtu_I, int16_T
  rtu_Kb, int16_T rtu_satMax, int16_T rtu_satMin, int16_T rtu_ext_limProt,
  int16_T rtu_init, uint8_T rtu_reset, DW_pi_speed *localDW, ZCE_pi_speed
  *localZCE);
static uint16_T plook_u16s16_evencka(int16_T u, int16_T bp0, uint16_T bpSpace,
  uint32_T maxIndex)
{
  uint16_T bpIndex;

  /* Prelookup - Index only
     Index Search method: 'even'
     Extrapolation method: 'Clip'
     Use previous index: 'off'
     Use last breakpoint for index at or above upper limit: 'on'
     Remove protection against out-of-range input in generated code: 'off'
   */
  if (u <= bp0) {
    bpIndex = 0U;
  } else {
    bpIndex = (uint16_T)((uint32_T)(uint16_T)(u - bp0) / bpSpace);
    if (bpIndex < maxIndex) {
    } else {
      bpIndex = (uint16_T)maxIndex;
    }
  }

  return bpIndex;
}

/*
 * System initialize for atomic system:
 *    '<S40>/Counter'
 *    '<S39>/Counter'
 */
static void Counter_Init(DW_Counter *localDW, uint16_T rtp_z_cntInit)
{
  /* InitializeConditions for UnitDelay: '<S45>/UnitDelay' */
  localDW->UnitDelay_DSTATE = rtp_z_cntInit;
}

/*
 * Output and update for atomic system:
 *    '<S40>/Counter'
 *    '<S39>/Counter'
 */
static uint16_T Counter(uint16_T rtu_inc, uint16_T rtu_max, boolean_T rtu_rst,
  DW_Counter *localDW)
{
  uint16_T rty_cnt_0;
  uint16_T rtu_rst_0;

  /* Switch: '<S45>/Switch1' incorporates:
   *  Constant: '<S45>/Constant23'
   *  UnitDelay: '<S45>/UnitDelay'
   */
  if (rtu_rst) {
    rtu_rst_0 = 0U;
  } else {
    rtu_rst_0 = localDW->UnitDelay_DSTATE;
  }

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

  /* Sum: '<S44>/Sum1' */
  rty_cnt_0 = (uint16_T)((uint32_T)rtu_inc + rtu_rst_0);

  /* MinMax: '<S44>/MinMax' */
  if (rty_cnt_0 < rtu_max) {
    /* Update for UnitDelay: '<S45>/UnitDelay' */
    localDW->UnitDelay_DSTATE = rty_cnt_0;
  } else {
    /* Update for UnitDelay: '<S45>/UnitDelay' */
    localDW->UnitDelay_DSTATE = rtu_max;
  }

  /* End of MinMax: '<S44>/MinMax' */
  return rty_cnt_0;
}

/*
 * Output and update for atomic system:
 *    '<S36>/either_edge'
 *    '<S35>/either_edge'
 */
static boolean_T either_edge(boolean_T rtu_u, DW_either_edge *localDW)
{
  boolean_T rty_y_0;

  /* RelationalOperator: '<S41>/Relational Operator' incorporates:
   *  UnitDelay: '<S41>/UnitDelay'
   */
  rty_y_0 = (rtu_u != localDW->UnitDelay_DSTATE);

  /* Update for UnitDelay: '<S41>/UnitDelay' */
  localDW->UnitDelay_DSTATE = rtu_u;
  return rty_y_0;
}

/* System initialize for atomic system: '<S35>/Debounce_Filter' */
static void Debounce_Filter_Init(DW_Debounce_Filter *localDW)
{
  /* SystemInitialize for IfAction SubSystem: '<S36>/Qualification' */
  /* SystemInitialize for Atomic SubSystem: '<S40>/Counter' */
  Counter_Init(&localDW->Counter_f, 0);

  /* End of SystemInitialize for SubSystem: '<S40>/Counter' */
  /* End of SystemInitialize for SubSystem: '<S36>/Qualification' */

  /* SystemInitialize for IfAction SubSystem: '<S36>/Dequalification' */
  /* SystemInitialize for Atomic SubSystem: '<S39>/Counter' */
  Counter_Init(&localDW->Counter_d, 0);

  /* End of SystemInitialize for SubSystem: '<S39>/Counter' */
  /* End of SystemInitialize for SubSystem: '<S36>/Dequalification' */
}

/* Output and update for atomic system: '<S35>/Debounce_Filter' */
static void Debounce_Filter(boolean_T rtu_u, uint16_T rtu_tAcv, uint16_T
  rtu_tDeacv, boolean_T *rty_y, DW_Debounce_Filter *localDW)
{
  uint16_T rtb_Sum1_jb;
  boolean_T rtb_RelationalOperator_e;

  /* Outputs for Atomic SubSystem: '<S36>/either_edge' */
  rtb_RelationalOperator_e = either_edge(rtu_u, &localDW->either_edge_j);

  /* End of Outputs for SubSystem: '<S36>/either_edge' */

  /* If: '<S36>/If2' incorporates:
   *  Constant: '<S39>/Constant6'
   *  Constant: '<S40>/Constant6'
   *  Inport: '<S38>/yPrev'
   *  Logic: '<S36>/Logical Operator1'
   *  Logic: '<S36>/Logical Operator2'
   *  Logic: '<S36>/Logical Operator3'
   *  Logic: '<S36>/Logical Operator4'
   *  UnitDelay: '<S36>/UnitDelay'
   */
  if (rtu_u && (!localDW->UnitDelay_DSTATE)) {
    /* Outputs for IfAction SubSystem: '<S36>/Qualification' incorporates:
     *  ActionPort: '<S40>/Action Port'
     */
    /* Outputs for Atomic SubSystem: '<S40>/Counter' */
    rtb_Sum1_jb = Counter(1, rtu_tAcv, rtb_RelationalOperator_e,
                          &localDW->Counter_f);

    /* End of Outputs for SubSystem: '<S40>/Counter' */

    /* Switch: '<S40>/Switch2' incorporates:
     *  Constant: '<S40>/Constant6'
     *  RelationalOperator: '<S40>/Relational Operator2'
     */
    *rty_y = ((rtb_Sum1_jb > rtu_tAcv) || localDW->UnitDelay_DSTATE);

    /* End of Outputs for SubSystem: '<S36>/Qualification' */
  } else if ((!rtu_u) && localDW->UnitDelay_DSTATE) {
    /* Outputs for IfAction SubSystem: '<S36>/Dequalification' incorporates:
     *  ActionPort: '<S39>/Action Port'
     */
    /* Outputs for Atomic SubSystem: '<S39>/Counter' */
    rtb_Sum1_jb = Counter(1, rtu_tDeacv, rtb_RelationalOperator_e,
                          &localDW->Counter_d);

    /* End of Outputs for SubSystem: '<S39>/Counter' */

    /* Switch: '<S39>/Switch2' incorporates:
     *  Constant: '<S39>/Constant6'
     *  RelationalOperator: '<S39>/Relational Operator2'
     */
    *rty_y = ((rtb_Sum1_jb <= rtu_tDeacv) && localDW->UnitDelay_DSTATE);

    /* End of Outputs for SubSystem: '<S36>/Dequalification' */
  } else {
    /* Outputs for IfAction SubSystem: '<S36>/Default' incorporates:
     *  ActionPort: '<S38>/Action Port'
     */
    *rty_y = localDW->UnitDelay_DSTATE;

    /* End of Outputs for SubSystem: '<S36>/Default' */
  }

  /* End of If: '<S36>/If2' */

  /* Update for UnitDelay: '<S36>/UnitDelay' */
  localDW->UnitDelay_DSTATE = *rty_y;
}

/* Output and update for atomic system: '<S46>/Low_Pass_Filter' */
static void Low_Pass_Filter(const int16_T rtu_u[2], uint16_T rtu_coef, int16_T
  rty_y[2], DW_Low_Pass_Filter *localDW)
{
  int32_T tmp;

  /* Sum: '<S56>/Sum2' incorporates:
   *  UnitDelay: '<S56>/UnitDelay1'
   */
  tmp = rtu_u[0] - localDW->UnitDelay1_DSTATE[0];
  if (tmp > 32767) {
    tmp = 32767;
  } else {
    if (tmp < -32768) {
      tmp = -32768;
    }
  }

  /* Product: '<S56>/Divide3' incorporates:
   *  Sum: '<S56>/Sum2'
   */
  rty_y[0] = (int16_T)((rtu_coef * tmp) >> 16);

  /* Sum: '<S56>/Sum3' incorporates:
   *  UnitDelay: '<S56>/UnitDelay1'
   */
  rty_y[0] += localDW->UnitDelay1_DSTATE[0];

  /* Update for UnitDelay: '<S56>/UnitDelay1' incorporates:
   *  Sum: '<S56>/Sum3'
   */
  localDW->UnitDelay1_DSTATE[0] = rty_y[0];

  /* Sum: '<S56>/Sum2' incorporates:
   *  UnitDelay: '<S56>/UnitDelay1'
   */
  tmp = rtu_u[1] - localDW->UnitDelay1_DSTATE[1];
  if (tmp > 32767) {
    tmp = 32767;
  } else {
    if (tmp < -32768) {
      tmp = -32768;
    }
  }

  /* Product: '<S56>/Divide3' incorporates:
   *  Sum: '<S56>/Sum2'
   */
  rty_y[1] = (int16_T)((rtu_coef * tmp) >> 16);

  /* Sum: '<S56>/Sum3' incorporates:
   *  UnitDelay: '<S56>/UnitDelay1'
   */
  rty_y[1] += localDW->UnitDelay1_DSTATE[1];

  /* Update for UnitDelay: '<S56>/UnitDelay1' incorporates:
   *  Sum: '<S56>/Sum3'
   */
  localDW->UnitDelay1_DSTATE[1] = rty_y[1];
}

/*
 * System initialize for atomic system:
 *    '<S61>/PI_iq'
 *    '<S60>/PI_id'
 */
static void PI_backCalc_fixdt_Init(DW_PI_backCalc_fixdt *localDW)
{
  /* InitializeConditions for Delay: '<S68>/Resettable Delay' */
  localDW->icLoad = 1U;
}

/*
 * Output and update for atomic system:
 *    '<S61>/PI_iq'
 *    '<S60>/PI_id'
 */
static void PI_backCalc_fixdt(int16_T rtu_err, int16_T rtu_P, int16_T rtu_I,
  int16_T rtu_Kb, int16_T rtu_satMax, int16_T rtu_satMin, int16_T
  rtu_ext_limProt, uint8_T rtu_reset, int16_T *rty_pi_out, const
  ConstB_PI_backCalc_fixdt *localC, DW_PI_backCalc_fixdt *localDW,
  ZCE_PI_backCalc_fixdt *localZCE)
{
  int64_T tmp;
  int32_T rtb_Divide4_h;
  int32_T rtb_Sum1_ae;

  /* Product: '<S66>/Divide4' */
  rtb_Divide4_h = (rtu_err * rtu_P) >> 6;

  /* Delay: '<S68>/Resettable Delay' incorporates:
   *  DataTypeConversion: '<S68>/Data Type Conversion2'
   */
  if ((rtu_reset > 0) && (localZCE->ResettableDelay_Reset_ZCE_p != POS_ZCSIG)) {
    localDW->icLoad = 1U;
  }

  localZCE->ResettableDelay_Reset_ZCE_p = (ZCSigState)(rtu_reset > 0);
  if (localDW->icLoad != 0) {
    localDW->ResettableDelay_DSTATE = localC->DataTypeConversion2;
  }

  /* Product: '<S66>/Divide1' incorporates:
   *  Product: '<S66>/Divide4'
   */
  tmp = ((int64_T)rtb_Divide4_h * rtu_I) >> 10;
  if (tmp > 2147483647LL) {
    tmp = 2147483647LL;
  } else {
    if (tmp < -2147483648LL) {
      tmp = -2147483648LL;
    }
  }

  /* Sum: '<S66>/Sum2' incorporates:
   *  Product: '<S66>/Divide1'
   *  UnitDelay: '<S66>/UnitDelay'
   */
  tmp = (((int64_T)rtu_ext_limProt << 3) + (int32_T)tmp) +
    localDW->UnitDelay_DSTATE;
  if (tmp > 2147483647LL) {
    tmp = 2147483647LL;
  } else {
    if (tmp < -2147483648LL) {
      tmp = -2147483648LL;
    }
  }

  /* Sum: '<S68>/Sum1' incorporates:
   *  Delay: '<S68>/Resettable Delay'
   *  Sum: '<S66>/Sum2'
   */
  rtb_Sum1_ae = ((int32_T)tmp >> 2) + localDW->ResettableDelay_DSTATE;

  /* Sum: '<S66>/Sum6' incorporates:
   *  DataTypeConversion: '<S68>/Data Type Conversion1'
   *  Product: '<S66>/Divide4'
   *  Sum: '<S68>/Sum1'
   */
  tmp = ((int64_T)(rtb_Sum1_ae >> 2) << 4) + rtb_Divide4_h;
  if (tmp > 2147483647LL) {
    tmp = 2147483647LL;
  } else {
    if (tmp < -2147483648LL) {
      tmp = -2147483648LL;
    }
  }

  /* Switch: '<S69>/Switch2' incorporates:
   *  RelationalOperator: '<S69>/LowerRelop1'
   *  RelationalOperator: '<S69>/UpperRelop'
   *  Sum: '<S66>/Sum6'
   *  Switch: '<S69>/Switch'
   */
  if ((int32_T)tmp > (rtu_satMax << 4)) {
    *rty_pi_out = rtu_satMax;
  } else if ((int32_T)tmp < (rtu_satMin << 4)) {
    /* Switch: '<S69>/Switch' */
    *rty_pi_out = rtu_satMin;
  } else {
    *rty_pi_out = (int16_T)((int32_T)tmp >> 4);
  }

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

  /* Update for UnitDelay: '<S66>/UnitDelay' incorporates:
   *  Product: '<S66>/Divide2'
   *  Sum: '<S66>/Sum3'
   *  Sum: '<S66>/Sum6'
   */
  localDW->UnitDelay_DSTATE = (int32_T)(((int64_T)((*rty_pi_out << 4) - (int32_T)
    tmp) * rtu_Kb) >> 10);

  /* Update for Delay: '<S68>/Resettable Delay' incorporates:
   *  Sum: '<S68>/Sum1'
   */
  localDW->icLoad = 0U;
  localDW->ResettableDelay_DSTATE = rtb_Sum1_ae;
}

/* System initialize for atomic system: '<S90>/pi_speed' */
static void pi_speed_Init(DW_pi_speed *localDW)
{
  /* InitializeConditions for Delay: '<S94>/Resettable Delay' */
  localDW->icLoad = 1U;
}

/* Output and update for atomic system: '<S90>/pi_speed' */
static int16_T pi_speed(int16_T rtu_err, int16_T rtu_P, int16_T rtu_I, int16_T
  rtu_Kb, int16_T rtu_satMax, int16_T rtu_satMin, int16_T rtu_ext_limProt,
  int16_T rtu_init, uint8_T rtu_reset, DW_pi_speed *localDW, ZCE_pi_speed
  *localZCE)
{
  int16_T rty_pi_out_0;
  int64_T tmp;
  int32_T rtb_Divide4_g;
  int32_T rtb_Sum1_c;

  /* Product: '<S93>/Divide4' */
  rtb_Divide4_g = (rtu_err * rtu_P) >> 1;

  /* Delay: '<S94>/Resettable Delay' incorporates:
   *  DataTypeConversion: '<S94>/Data Type Conversion2'
   */
  if ((rtu_reset > 0) && (localZCE->ResettableDelay_Reset_ZCE != POS_ZCSIG)) {
    localDW->icLoad = 1U;
  }

  localZCE->ResettableDelay_Reset_ZCE = (ZCSigState)(rtu_reset > 0);
  if (localDW->icLoad != 0) {
    localDW->ResettableDelay_DSTATE = rtu_init << 2;
  }

  /* Product: '<S93>/Divide1' incorporates:
   *  Product: '<S93>/Divide4'
   */
  tmp = ((int64_T)rtb_Divide4_g * rtu_I) >> 10;
  if (tmp > 2147483647LL) {
    tmp = 2147483647LL;
  } else {
    if (tmp < -2147483648LL) {
      tmp = -2147483648LL;
    }
  }

  /* Sum: '<S93>/Sum2' incorporates:
   *  Product: '<S93>/Divide1'
   *  UnitDelay: '<S93>/UnitDelay'
   */
  tmp = (((int64_T)(int32_T)tmp + rtu_ext_limProt) + ((int64_T)
          localDW->UnitDelay_DSTATE << 2)) >> 2;
  if (tmp > 2147483647LL) {
    tmp = 2147483647LL;
  } else {
    if (tmp < -2147483648LL) {
      tmp = -2147483648LL;
    }
  }

  /* Sum: '<S94>/Sum1' incorporates:
   *  Delay: '<S94>/Resettable Delay'
   *  Sum: '<S93>/Sum2'
   */
  rtb_Sum1_c = (int32_T)tmp + localDW->ResettableDelay_DSTATE;

  /* Sum: '<S93>/Sum6' incorporates:
   *  DataTypeConversion: '<S94>/Data Type Conversion1'
   *  Product: '<S93>/Divide4'
   *  Sum: '<S94>/Sum1'
   */
  tmp = ((int64_T)(rtb_Sum1_c >> 2) << 4) + rtb_Divide4_g;
  if (tmp > 2147483647LL) {
    tmp = 2147483647LL;
  } else {
    if (tmp < -2147483648LL) {
      tmp = -2147483648LL;
    }
  }

  /* Switch: '<S95>/Switch2' incorporates:
   *  RelationalOperator: '<S95>/LowerRelop1'
   *  RelationalOperator: '<S95>/UpperRelop'
   *  Sum: '<S93>/Sum6'
   *  Switch: '<S95>/Switch'
   */
  if ((int32_T)tmp > (rtu_satMax << 4)) {
    rty_pi_out_0 = rtu_satMax;
  } else if ((int32_T)tmp < (rtu_satMin << 4)) {
    /* Switch: '<S95>/Switch' */
    rty_pi_out_0 = rtu_satMin;
  } else {
    rty_pi_out_0 = (int16_T)((int32_T)tmp >> 4);
  }

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

  /* Update for UnitDelay: '<S93>/UnitDelay' incorporates:
   *  Product: '<S93>/Divide2'
   *  Sum: '<S93>/Sum3'
   *  Sum: '<S93>/Sum6'
   */
  localDW->UnitDelay_DSTATE = (int32_T)(((int64_T)((rty_pi_out_0 << 4) -
    (int32_T)tmp) * rtu_Kb) >> 12);

  /* Update for Delay: '<S94>/Resettable Delay' incorporates:
   *  Sum: '<S94>/Sum1'
   */
  localDW->icLoad = 0U;
  localDW->ResettableDelay_DSTATE = rtb_Sum1_c;
  return rty_pi_out_0;
}

int16_T rt_sqrt_Us16En12_Ys16E_cQn1iwAF(int16_T u)
{
  int32_T iBit;
  int32_T tmp03_u;
  int16_T shiftMask;
  int16_T tmp01_y;
  int16_T y;

  /* Fixed-Point Sqrt Computation by the bisection method. */
  if (u > 0) {
    y = 0;
    shiftMask = 16384;
    tmp03_u = u << 12;
    for (iBit = 0; iBit < 15; iBit++) {
      tmp01_y = (int16_T)(y | shiftMask);
      if (tmp01_y * tmp01_y <= tmp03_u) {
        y = tmp01_y;
      }

      shiftMask = (int16_T)((uint32_T)shiftMask >> 1U);
    }
  } else {
    y = 0;
  }

  return y;
}

/* Model step function */
void PMSM_Controller_step(RT_MODEL *const rtM)
{
  DW *rtDW = rtM->dwork;
  PrevZCX *rtPrevZCX = rtM->prevZCSigState;
  ExtU *rtU = (ExtU *) rtM->inputs;
  ExtY *rtY = (ExtY *) rtM->outputs;
  int64_T tmp;
  int32_T rtb_Gain;
  int32_T tmp_0;
  int32_T tmp_1;
  int32_T tmp_2;
  uint32_T qY;
  uint32_T tmp_3;
  int16_T rtb_DataTypeConversion[2];
  int16_T rtb_TmpSignalConversionAtLow_Pa[2];
  int16_T rtb_Divide1_m;
  int16_T rtb_Sign;
  int16_T rtb_Switch2_l;
  int16_T rtb_Switch3_c;
  int16_T rtb_Switch_np;
  uint16_T rtb_Abs2;
  uint16_T rtb_LogicalOperator3;
  uint16_T rtb_Switch2;
  int8_T UnitDelay3;
  int8_T rtb_Sum2;
  int8_T rtb_Sum2_tmp;
  uint8_T rtb_Add_cr;
  uint8_T rtb_DataTypeConversion_m;
  uint8_T rtb_Sum;
  uint8_T rtb_UnitDelay_bc;
  uint8_T rtb_z_ctrlMod;
  boolean_T rtb_Equal_k;
  boolean_T rtb_LogicalOperator2;
  boolean_T rtb_LogicalOperator4;
  boolean_T rtb_LogicalOperator_p;
  boolean_T rtb_RelationalOperator4_f;
  boolean_T rtb_n_commDeacv;

  /* Outputs for Atomic SubSystem: '<Root>/PMSM_Controller' */
  /* Sum: '<S7>/Sum3' incorporates:
   *  UnitDelay: '<S7>/UnitDelay1'
   */
  qY = rtDW->UnitDelay1_DSTATE + /*MW:OvSatOk*/ 1U;
  if (rtDW->UnitDelay1_DSTATE + 1U < 1U) {
    qY = MAX_uint32_T;
  }

  /* RelationalOperator: '<S2>/Equal' incorporates:
   *  Constant: '<S2>/Constant1'
   *  Math: '<S2>/Rem'
   *  Sum: '<S7>/Sum3'
   */
  rtb_Equal_k = (qY % 20U == 0U);

  /* Logic: '<S9>/Edge_Detect' incorporates:
   *  Delay: '<S9>/Delay'
   *  Delay: '<S9>/Delay1'
   *  Delay: '<S9>/Delay2'
   *  Inport: '<Root>/hall_a'
   *  Inport: '<Root>/hall_b'
   *  Inport: '<Root>/hall_c'
   */
  rtb_LogicalOperator_p = (boolean_T)((rtU->hall_a != 0) ^ (rtDW->Delay_DSTATE
    != 0) ^ (rtU->hall_b != 0) ^ (rtDW->Delay1_DSTATE != 0) ^ (rtU->hall_c != 0))
    ^ (rtDW->Delay2_DSTATE != 0);

  /* Sum: '<S11>/Add' incorporates:
   *  Gain: '<S11>/Gain'
   *  Gain: '<S11>/Gain1'
   *  Inport: '<Root>/hall_a'
   *  Inport: '<Root>/hall_b'
   *  Inport: '<Root>/hall_c'
   */
  rtb_Add_cr = (uint8_T)((uint32_T)(uint8_T)((uint32_T)(uint8_T)(rtU->hall_c <<
    2) + (uint8_T)(rtU->hall_b << 1)) + rtU->hall_a);

  /* If: '<S3>/If2' incorporates:
   *  If: '<S14>/If2'
   *  Inport: '<S19>/z_counterRawPrev'
   *  UnitDelay: '<S14>/UnitDelay3'
   */
  if (rtb_LogicalOperator_p) {
    /* Outputs for IfAction SubSystem: '<S3>/Direction_Detection' incorporates:
     *  ActionPort: '<S8>/Action Port'
     */
    /* UnitDelay: '<S8>/UnitDelay3' */
    UnitDelay3 = rtDW->Switch2_i;

    /* End of Outputs for SubSystem: '<S3>/Direction_Detection' */

    /* Selector: '<S11>/Selector' incorporates:
     *  Constant: '<S11>/vec_hallToPos'
     */
    rtb_Sum2_tmp = rtConstP.vec_hallToPos_Value[rtb_Add_cr];

    /* Outputs for IfAction SubSystem: '<S3>/Direction_Detection' incorporates:
     *  ActionPort: '<S8>/Action Port'
     */
    /* Sum: '<S8>/Sum2' incorporates:
     *  Constant: '<S11>/vec_hallToPos'
     *  Selector: '<S11>/Selector'
     *  UnitDelay: '<S8>/UnitDelay2'
     */
    rtb_Sum2 = (int8_T)(rtb_Sum2_tmp - rtDW->UnitDelay2_DSTATE_j);

    /* Switch: '<S8>/Switch2' incorporates:
     *  Constant: '<S8>/Constant20'
     *  Constant: '<S8>/Constant8'
     *  Logic: '<S8>/Logical Operator3'
     *  RelationalOperator: '<S8>/Relational Operator1'
     *  RelationalOperator: '<S8>/Relational Operator6'
     */
    if ((rtb_Sum2 == 1) || (rtb_Sum2 == -5)) {
      /* Switch: '<S8>/Switch2' incorporates:
       *  Constant: '<S8>/Constant24'
       */
      rtDW->Switch2_i = 1;
    } else {
      /* Switch: '<S8>/Switch2' incorporates:
       *  Constant: '<S8>/Constant23'
       */
      rtDW->Switch2_i = -1;
    }

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

    /* Update for UnitDelay: '<S8>/UnitDelay2' */
    rtDW->UnitDelay2_DSTATE_j = rtb_Sum2_tmp;

    /* End of Outputs for SubSystem: '<S3>/Direction_Detection' */

    /* Outputs for IfAction SubSystem: '<S14>/Raw_Motor_Speed_Estimation' incorporates:
     *  ActionPort: '<S19>/Action Port'
     */
    /* RelationalOperator: '<S19>/Relational Operator4' */
    rtb_RelationalOperator4_f = (rtDW->Switch2_i != UnitDelay3);
    rtDW->z_counterRawPrev = rtDW->UnitDelay3_DSTATE;

    /* Switch: '<S19>/Switch3' incorporates:
     *  Constant: '<S19>/Constant4'
     *  Inport: '<S19>/z_counterRawPrev'
     *  Logic: '<S19>/Logical Operator1'
     *  Switch: '<S19>/Switch2'
     *  UnitDelay: '<S14>/UnitDelay3'
     *  UnitDelay: '<S19>/UnitDelay1'
     */
    if (rtb_RelationalOperator4_f && rtDW->UnitDelay1_DSTATE_i) {
      rtb_Switch3_c = 0;
    } else {
      if (rtb_RelationalOperator4_f) {
        /* Switch: '<S19>/Switch2' incorporates:
         *  UnitDelay: '<S14>/UnitDelay4'
         */
        rtb_Switch2 = (uint16_T)(rtDW->UnitDelay4_DSTATE << 1);
      } else {
        /* Product: '<S19>/Divide13' incorporates:
         *  Sum: '<S19>/Sum13'
         *  Switch: '<S19>/Switch2'
         *  UnitDelay: '<S19>/UnitDelay2'
         *  UnitDelay: '<S19>/UnitDelay3'
         *  UnitDelay: '<S19>/UnitDelay5'
         */
        tmp_3 = 4000000U / (((rtDW->UnitDelay2_DSTATE +
                              rtDW->UnitDelay3_DSTATE_l) +
                             rtDW->UnitDelay5_DSTATE) + rtDW->z_counterRawPrev);
        if (tmp_3 > 65535U) {
          tmp_3 = 65535U;
        }

        /* Switch: '<S19>/Switch2' incorporates:
         *  Product: '<S19>/Divide13'
         */
        rtb_Switch2 = (uint16_T)((uint16_T)tmp_3 << 1);
      }

      rtb_Switch3_c = (int16_T)rtb_Switch2;
    }

    /* End of Switch: '<S19>/Switch3' */

    /* Product: '<S19>/Divide11' incorporates:
     *  Switch: '<S19>/Switch3'
     */
    rtDW->Divide11 = (int16_T)((rtb_Switch3_c * rtDW->Switch2_i) >> 1);

    /* Update for UnitDelay: '<S19>/UnitDelay1' */
    rtDW->UnitDelay1_DSTATE_i = rtb_RelationalOperator4_f;

    /* Update for UnitDelay: '<S19>/UnitDelay2' incorporates:
     *  UnitDelay: '<S19>/UnitDelay3'
     */
    rtDW->UnitDelay2_DSTATE = rtDW->UnitDelay3_DSTATE_l;

    /* Update for UnitDelay: '<S19>/UnitDelay3' incorporates:
     *  UnitDelay: '<S19>/UnitDelay5'
     */
    rtDW->UnitDelay3_DSTATE_l = rtDW->UnitDelay5_DSTATE;

    /* Update for UnitDelay: '<S19>/UnitDelay5' */
    rtDW->UnitDelay5_DSTATE = rtDW->z_counterRawPrev;

    /* End of Outputs for SubSystem: '<S14>/Raw_Motor_Speed_Estimation' */
  }

  /* End of If: '<S3>/If2' */

  /* Switch: '<S10>/Switch3' incorporates:
   *  Constant: '<S10>/Constant16'
   *  Constant: '<S10>/Constant2'
   *  Constant: '<S11>/vec_hallToPos'
   *  RelationalOperator: '<S10>/Relational Operator7'
   *  Selector: '<S11>/Selector'
   *  Sum: '<S10>/Sum1'
   */
  if (rtDW->Switch2_i == 1) {
    rtb_Sum2 = rtConstP.vec_hallToPos_Value[rtb_Add_cr];
  } else {
    rtb_Sum2 = (int8_T)(rtConstP.vec_hallToPos_Value[rtb_Add_cr] + 1);
  }

  /* End of Switch: '<S10>/Switch3' */

  /* MinMax: '<S10>/MinMax' incorporates:
   *  Inport: '<Root>/hw_count'
   */
  if (rtU->hw_count < rtDW->z_counterRawPrev) {
    tmp_3 = rtU->hw_count;
  } else {
    tmp_3 = rtDW->z_counterRawPrev;
  }

  /* End of MinMax: '<S10>/MinMax' */

  /* Sum: '<S10>/Sum3' incorporates:
   *  Product: '<S10>/Divide1'
   *  Product: '<S10>/Divide3'
   */
  rtb_Switch3_c = (int16_T)(((int16_T)((int16_T)(((uint64_T)tmp_3 << 14) /
    rtDW->z_counterRawPrev) * rtDW->Switch2_i) + (rtb_Sum2 << 14)) >> 2);

  /* MinMax: '<S10>/MinMax1' incorporates:
   *  Constant: '<S10>/Constant1'
   *  Sum: '<S10>/Sum3'
   *  Switch: '<S10>/Switch2'
   */
  if (rtb_Switch3_c <= 0) {
    rtb_Switch3_c = 0;
  }

  /* End of MinMax: '<S10>/MinMax1' */

  /* Sum: '<S15>/Add2' incorporates:
   *  Constant: '<S15>/Constant2'
   *  Product: '<S10>/Divide2'
   */
  rtb_Switch3_c = (int16_T)((((15 * rtb_Switch3_c) >> 4) + 3840) >> 2);

  /* DataTypeConversion: '<S15>/Data Type Conversion' incorporates:
   *  Sum: '<S15>/Add2'
   */
  rtb_Divide1_m = (int16_T)(rtb_Switch3_c >> 4);

  /* If: '<S15>/If' incorporates:
   *  Constant: '<S15>/Constant3'
   *  Inport: '<S16>/In1'
   *  Merge: '<S15>/Merge'
   *  Sum: '<S15>/Add'
   *  Sum: '<S15>/Add2'
   */
  if (rtb_Divide1_m >= 360) {
    /* Outputs for IfAction SubSystem: '<S15>/If Action Subsystem' incorporates:
     *  ActionPort: '<S16>/Action Port'
     */
    rtb_Switch3_c = (int16_T)(rtb_Switch3_c - 5760);

    /* End of Outputs for SubSystem: '<S15>/If Action Subsystem' */
  }

  /* End of If: '<S15>/If' */

  /* If: '<S3>/If' incorporates:
   *  Inport: '<Root>/foc_calibrate'
   */
  if ((rtU->foc_calibrate == 0) || (rtU->foc_calibrate == 2)) {
    /* Outputs for IfAction SubSystem: '<S3>/If Action Subsystem' incorporates:
     *  ActionPort: '<S12>/Action Port'
     */
    /* Merge: '<S3>/Merge' incorporates:
     *  Inport: '<S12>/In1'
     *  Merge: '<S15>/Merge'
     */
    rtDW->Merge_i = rtb_Switch3_c;

    /* End of Outputs for SubSystem: '<S3>/If Action Subsystem' */
  } else {
    if (rtU->foc_calibrate == 1) {
      /* Outputs for IfAction SubSystem: '<S3>/If Action Subsystem1' incorporates:
       *  ActionPort: '<S13>/Action Port'
       */
      /* Merge: '<S3>/Merge' incorporates:
       *  Inport: '<Root>/open_theta'
       *  Inport: '<S13>/In1'
       */
      rtDW->Merge_i = rtU->open_theta;

      /* End of Outputs for SubSystem: '<S3>/If Action Subsystem1' */
    }
  }

  /* End of If: '<S3>/If' */

  /* Switch: '<S14>/Switch2' incorporates:
   *  Constant: '<S14>/Constant4'
   *  Inport: '<Root>/hw_count'
   *  Product: '<S19>/Divide11'
   *  RelationalOperator: '<S14>/Relational Operator2'
   */
  if (rtU->hw_count >= 400000U) {
    rtb_Switch2_l = 0;
  } else {
    rtb_Switch2_l = rtDW->Divide11;
  }

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

  /* Abs: '<S14>/Abs5' incorporates:
   *  Switch: '<S14>/Switch2'
   */
  if (rtb_Switch2_l < 0) {
    rtb_Switch2 = (uint16_T)-rtb_Switch2_l;
  } else {
    rtb_Switch2 = (uint16_T)rtb_Switch2_l;
  }

  /* End of Abs: '<S14>/Abs5' */

  /* If: '<S14>/If1' */
  if (rtb_LogicalOperator_p) {
    /* Outputs for IfAction SubSystem: '<S14>/Subsystem' incorporates:
     *  ActionPort: '<S20>/Action Port'
     */
    /* Relay: '<S20>/n_commDeacv' incorporates:
     *  Abs: '<S14>/Abs5'
     */
    rtDW->n_commDeacv_Mode = ((rtb_Switch2 >= 60) || ((rtb_Switch2 > 30) &&
      rtDW->n_commDeacv_Mode));

    /* RelationalOperator: '<S22>/Compare' incorporates:
     *  Constant: '<S22>/Constant'
     *  Relay: '<S20>/n_commDeacv'
     *  Sum: '<S20>/Sum13'
     *  UnitDelay: '<S20>/UnitDelay2'
     *  UnitDelay: '<S20>/UnitDelay3'
     *  UnitDelay: '<S20>/UnitDelay5'
     */
    rtDW->Compare = ((uint16_T)((uint32_T)(uint16_T)((uint32_T)(uint16_T)
      ((uint32_T)rtDW->UnitDelay2_DSTATE_f + rtDW->UnitDelay3_DSTATE_lh) +
      rtDW->UnitDelay5_DSTATE_f) + rtDW->n_commDeacv_Mode) >= 4);

    /* Update for UnitDelay: '<S20>/UnitDelay2' incorporates:
     *  UnitDelay: '<S20>/UnitDelay3'
     */
    rtDW->UnitDelay2_DSTATE_f = rtDW->UnitDelay3_DSTATE_lh;

    /* Update for UnitDelay: '<S20>/UnitDelay3' incorporates:
     *  UnitDelay: '<S20>/UnitDelay5'
     */
    rtDW->UnitDelay3_DSTATE_lh = rtDW->UnitDelay5_DSTATE_f;

    /* Update for UnitDelay: '<S20>/UnitDelay5' incorporates:
     *  Logic: '<S20>/Logical Operator3'
     *  Relay: '<S20>/n_commDeacv'
     */
    rtDW->UnitDelay5_DSTATE_f = rtDW->n_commDeacv_Mode;

    /* End of Outputs for SubSystem: '<S14>/Subsystem' */
  }

  /* End of If: '<S14>/If1' */

  /* Abs: '<S4>/Abs2' incorporates:
   *  Switch: '<S14>/Switch2'
   */
  if (rtb_Switch2_l < 0) {
    rtb_Abs2 = (uint16_T)((uint32_T)-rtb_Switch2_l >> 1);
  } else {
    rtb_Abs2 = (uint16_T)((uint32_T)rtb_Switch2_l >> 1);
  }

  /* End of Abs: '<S4>/Abs2' */

  /* S-Function (sfix_bitop): '<S4>/Bitwise Operator2' incorporates:
   *  Inport: '<Root>/foc_calibrate'
   */
  rtb_LogicalOperator3 = (uint16_T)(rtU->foc_calibrate & 1);

  /* UnitDelay: '<S35>/UnitDelay' */
  rtb_UnitDelay_bc = rtDW->UnitDelay_DSTATE_j;

  /* Outport: '<Root>/VqPrev' incorporates:
   *  UnitDelay: '<S6>/UnitDelay2'
   */
  rtY->VqPrev = rtDW->UnitDelay2_DSTATE_p;

  /* Switch: '<S35>/Switch3' incorporates:
   *  Abs: '<S14>/Abs5'
   *  Abs: '<S35>/Abs4'
   *  Constant: '<S35>/CTRL_COMM4'
   *  Inport: '<Root>/b_motEna'
   *  Logic: '<S35>/Logical Operator1'
   *  RelationalOperator: '<S14>/Relational Operator9'
   *  RelationalOperator: '<S35>/Relational Operator7'
   *  S-Function (sfix_bitop): '<S35>/Bitwise Operator1'
   *  UnitDelay: '<S6>/UnitDelay2'
   */
  if ((rtb_UnitDelay_bc & 4U) != 0U) {
    rtb_LogicalOperator_p = true;
  } else {
    if (rtDW->UnitDelay2_DSTATE_p < 0) {
      /* Abs: '<S35>/Abs4' incorporates:
       *  UnitDelay: '<S6>/UnitDelay2'
       */
      rtb_Switch_np = (int16_T)-rtDW->UnitDelay2_DSTATE_p;
    } else {
      /* Abs: '<S35>/Abs4' incorporates:
       *  UnitDelay: '<S6>/UnitDelay2'
       */
      rtb_Switch_np = rtDW->UnitDelay2_DSTATE_p;
    }

    rtb_LogicalOperator_p = (rtU->b_motEna && (rtb_Switch2 < 6) &&
      (rtb_Switch_np > 960));
  }

  /* End of Switch: '<S35>/Switch3' */

  /* Sum: '<S35>/Sum' incorporates:
   *  Constant: '<S35>/CTRL_COMM'
   *  Constant: '<S35>/CTRL_COMM1'
   *  DataTypeConversion: '<S35>/Data Type Conversion3'
   *  Gain: '<S35>/g_Hb'
   *  Gain: '<S35>/g_Hb1'
   *  RelationalOperator: '<S35>/Relational Operator1'
   *  RelationalOperator: '<S35>/Relational Operator3'
   */
  rtb_Sum = (uint8_T)(((uint32_T)((rtb_Add_cr == 7) << 1) + (rtb_Add_cr == 0)) +
                      (rtb_LogicalOperator_p << 2));

  /* Outputs for Atomic SubSystem: '<S35>/Debounce_Filter' */
  /* RelationalOperator: '<S35>/Relational Operator2' incorporates:
   *  Constant: '<S35>/CTRL_COMM2'
   *  Constant: '<S35>/t_errDequal'
   *  Constant: '<S35>/t_errQual'
   */
  Debounce_Filter(rtb_Sum != 0, 1600, 12000, &rtb_RelationalOperator4_f,
                  &rtDW->Debounce_Filter_i);

  /* End of Outputs for SubSystem: '<S35>/Debounce_Filter' */

  /* Logic: '<S24>/Logical Operator12' incorporates:
   *  Inport: '<Root>/b_motEna'
   *  Logic: '<S24>/Logical Operator7'
   */
  rtb_n_commDeacv = ((!rtb_RelationalOperator4_f) && rtU->b_motEna);

  /* Logic: '<S24>/Logical Operator4' incorporates:
   *  Constant: '<S24>/constant8'
   *  Inport: '<Root>/n_ctrlModReq'
   *  Logic: '<S24>/Logical Operator11'
   *  Logic: '<S24>/Logical Operator8'
   *  RelationalOperator: '<S24>/Relational Operator10'
   */
  rtb_LogicalOperator4 = ((rtb_LogicalOperator3 != 0) || (!rtDW->Compare) ||
    (!rtb_n_commDeacv) || (rtU->n_ctrlModReq == 0));

  /* Relay: '<S24>/n_SpeedCtrl' */
  rtDW->n_SpeedCtrl_Mode = ((rtb_Abs2 >= 300) || ((rtb_Abs2 > 200) &&
    rtDW->n_SpeedCtrl_Mode));
  rtb_LogicalOperator_p = rtDW->n_SpeedCtrl_Mode;

  /* Logic: '<S24>/Logical Operator10' incorporates:
   *  Inport: '<Root>/b_cruiseEna'
   */
  rtb_LogicalOperator_p = (rtb_LogicalOperator_p && rtU->b_cruiseEna);

  /* Logic: '<S24>/Logical Operator2' incorporates:
   *  Constant: '<S24>/constant'
   *  Inport: '<Root>/n_ctrlModReq'
   *  Logic: '<S24>/Logical Operator5'
   *  RelationalOperator: '<S24>/Relational Operator4'
   */
  rtb_LogicalOperator2 = ((rtU->n_ctrlModReq == 2) && (!rtb_LogicalOperator_p));

  /* Logic: '<S24>/Logical Operator1' incorporates:
   *  Constant: '<S24>/constant1'
   *  Inport: '<Root>/n_ctrlModReq'
   *  RelationalOperator: '<S24>/Relational Operator1'
   */
  rtb_LogicalOperator_p = ((rtU->n_ctrlModReq == 1) || rtb_LogicalOperator_p);

  /* Chart: '<S4>/Control_Mode_Manager' incorporates:
   *  Logic: '<S24>/Logical Operator3'
   *  Logic: '<S24>/Logical Operator6'
   *  Logic: '<S24>/Logical Operator9'
   */
  if (rtDW->is_active_c5_PMSM_Controller == 0U) {
    rtDW->is_active_c5_PMSM_Controller = 1U;
    rtDW->is_c5_PMSM_Controller = IN_OPEN;
    rtb_z_ctrlMod = OPEN_MODE;
  } else if (rtDW->is_c5_PMSM_Controller == 1) {
    if (rtb_LogicalOperator4) {
      rtDW->is_ACTIVE = IN_NO_ACTIVE_CHILD;
      rtDW->is_c5_PMSM_Controller = IN_OPEN;
      rtb_z_ctrlMod = OPEN_MODE;
    } else if (rtDW->is_ACTIVE == 1) {
      rtb_z_ctrlMod = SPD_MODE;
      if (!rtb_LogicalOperator_p) {
        if (rtb_LogicalOperator2) {
          rtDW->is_ACTIVE = IN_TORQUE_MODE;
          rtb_z_ctrlMod = TRQ_MODE;
        } else {
          rtDW->is_ACTIVE = IN_SPEED_MODE;
        }
      }
    } else {
      /* case IN_TORQUE_MODE: */
      rtb_z_ctrlMod = TRQ_MODE;
      if (!rtb_LogicalOperator2) {
        rtDW->is_ACTIVE = IN_SPEED_MODE;
        rtb_z_ctrlMod = SPD_MODE;
      }
    }
  } else {
    /* case IN_OPEN: */
    rtb_z_ctrlMod = OPEN_MODE;
    if ((!rtb_LogicalOperator4) && (rtb_LogicalOperator2 ||
         rtb_LogicalOperator_p)) {
      rtDW->is_c5_PMSM_Controller = IN_ACTIVE;
      if (rtb_LogicalOperator2) {
        rtDW->is_ACTIVE = IN_TORQUE_MODE;
        rtb_z_ctrlMod = TRQ_MODE;
      } else {
        rtDW->is_ACTIVE = IN_SPEED_MODE;
        rtb_z_ctrlMod = SPD_MODE;
      }
    }
  }

  /* End of Chart: '<S4>/Control_Mode_Manager' */

  /* Switch: '<S25>/Switch' incorporates:
   *  Constant: '<S25>/Constant3'
   *  Inport: '<Root>/speed_target'
   */
  if (rtU->speed_target > 30) {
    /* Switch: '<S25>/Switch1' incorporates:
     *  Constant: '<S25>/Constant1'
     *  DataTypeConversion: '<S25>/Data Type Conversion'
     *  Switch: '<S25>/Switch'
     */
    if (rtb_n_commDeacv) {
      rtb_Switch_np = rtU->speed_target;
    } else {
      rtb_Switch_np = 0;
    }

    /* End of Switch: '<S25>/Switch1' */
  } else {
    rtb_Switch_np = 0;
  }

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

  /* Switch: '<S25>/Switch3' incorporates:
   *  Constant: '<S25>/Constant4'
   *  DataTypeConversion: '<S25>/Data Type Conversion2'
   *  Inport: '<Root>/vdq_open_target'
   */
  if (rtb_n_commDeacv) {
    rtb_Divide1_m = rtU->vdq_open_target[1];
  } else {
    rtb_Divide1_m = 0;
  }

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

  /* If: '<S26>/If' incorporates:
   *  DataTypeConversion: '<S26>/Data Type Conversion1'
   *  DataTypeConversion: '<S26>/Data Type Conversion2'
   *  Inport: '<Root>/current_target'
   *  Inport: '<S27>/vq_in'
   *  Inport: '<S30>/r_currTgt'
   *  Switch: '<S25>/Switch3'
   */
  if (rtb_LogicalOperator3 == 1) {
    /* Switch: '<S25>/Switch2' incorporates:
     *  Constant: '<S25>/Constant2'
     *  DataTypeConversion: '<S25>/Data Type Conversion1'
     *  Inport: '<Root>/vdq_open_target'
     *  Inport: '<S27>/vd_in'
     */
    if (rtb_n_commDeacv) {
      /* Outputs for IfAction SubSystem: '<S26>/If Action Subsystem' incorporates:
       *  ActionPort: '<S27>/Action Port'
       */
      rtDW->Merge[0] = rtU->vdq_open_target[0];

      /* End of Outputs for SubSystem: '<S26>/If Action Subsystem' */
    } else {
      /* Outputs for IfAction SubSystem: '<S26>/If Action Subsystem' incorporates:
       *  ActionPort: '<S27>/Action Port'
       */
      rtDW->Merge[0] = 0;

      /* End of Outputs for SubSystem: '<S26>/If Action Subsystem' */
    }

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

    /* Outputs for IfAction SubSystem: '<S26>/If Action Subsystem' incorporates:
     *  ActionPort: '<S27>/Action Port'
     */
    rtDW->Merge[1] = rtb_Divide1_m;

    /* End of Outputs for SubSystem: '<S26>/If Action Subsystem' */
  } else if ((rtb_z_ctrlMod == 0) && rtb_Equal_k) {
    /* Outputs for IfAction SubSystem: '<S26>/open_mode' incorporates:
     *  ActionPort: '<S29>/Action Port'
     */
    /* RelationalOperator: '<S29>/Equal1' incorporates:
     *  Switch: '<S25>/Switch3'
     *  UnitDelay: '<S29>/Unit Delay'
     */
    rtb_LogicalOperator_p = (rtDW->UnitDelay_DSTATE != rtb_Divide1_m);

    /* If: '<S31>/If' */
    if (rtb_LogicalOperator_p) {
      /* Outputs for IfAction SubSystem: '<S31>/Subsystem' incorporates:
       *  ActionPort: '<S33>/Action Port'
       */
      /* Sum: '<S33>/Add' incorporates:
       *  Signum: '<S33>/Sign'
       *  Switch: '<S25>/Switch3'
       *  UnitDelay: '<S6>/UnitDelay2'
       */
      rtb_Sign = (int16_T)((rtb_Divide1_m - rtDW->UnitDelay2_DSTATE_p) >> 2);

      /* Signum: '<S33>/Sign' */
      if (rtb_Sign < 0) {
        rtb_Sign = -1;
      } else {
        rtb_Sign = (int16_T)(rtb_Sign > 0);
      }

      /* End of Signum: '<S33>/Sign' */

      /* Product: '<S33>/Divide' incorporates:
       *  Constant: '<S29>/Constant5'
       */
      rtDW->Divide = (int16_T)(rtb_Sign * 6);

      /* Switch: '<S33>/Switch' incorporates:
       *  Switch: '<S33>/Switch1'
       */
      if (rtb_Sign > 0) {
        /* Switch: '<S33>/Switch' incorporates:
         *  Switch: '<S25>/Switch3'
         */
        rtDW->Switch_i = rtb_Divide1_m;

        /* Switch: '<S33>/Switch1' incorporates:
         *  UnitDelay: '<S6>/UnitDelay2'
         */
        rtDW->Switch1 = rtDW->UnitDelay2_DSTATE_p;
      } else {
        /* Switch: '<S33>/Switch' incorporates:
         *  UnitDelay: '<S6>/UnitDelay2'
         */
        rtDW->Switch_i = rtDW->UnitDelay2_DSTATE_p;

        /* Switch: '<S33>/Switch1' incorporates:
         *  Switch: '<S25>/Switch3'
         */
        rtDW->Switch1 = rtb_Divide1_m;
      }

      /* End of Switch: '<S33>/Switch' */
      /* End of Outputs for SubSystem: '<S31>/Subsystem' */

      /* Switch: '<S34>/Switch1' incorporates:
       *  UnitDelay: '<S6>/UnitDelay2'
       */
      rtb_Sign = rtDW->UnitDelay2_DSTATE_p;
    } else {
      /* Switch: '<S34>/Switch1' incorporates:
       *  UnitDelay: '<S34>/UnitDelay'
       */
      rtb_Sign = rtDW->UnitDelay_DSTATE_f;
    }

    /* End of If: '<S31>/If' */

    /* Sum: '<S31>/Add2' incorporates:
     *  Product: '<S33>/Divide'
     */
    tmp_2 = ((rtb_Sign << 1) + rtDW->Divide) >> 1;
    if (tmp_2 > 32767) {
      tmp_2 = 32767;
    } else {
      if (tmp_2 < -32768) {
        tmp_2 = -32768;
      }
    }

    /* Switch: '<S29>/Switch' incorporates:
     *  Switch: '<S25>/Switch'
     */
    if (rtb_Switch_np > 0) {
      /* Switch: '<S32>/Switch2' incorporates:
       *  RelationalOperator: '<S32>/LowerRelop1'
       *  RelationalOperator: '<S32>/UpperRelop'
       *  Sum: '<S31>/Add2'
       *  Switch: '<S32>/Switch'
       *  Switch: '<S33>/Switch'
       *  Switch: '<S33>/Switch1'
       */
      if ((int16_T)tmp_2 > rtDW->Switch_i) {
        /* Merge: '<S26>/Merge' incorporates:
         *  Switch: '<S29>/Switch'
         */
        rtDW->Merge[1] = rtDW->Switch_i;
      } else if ((int16_T)tmp_2 < rtDW->Switch1) {
        /* Merge: '<S26>/Merge' incorporates:
         *  Switch: '<S29>/Switch'
         *  Switch: '<S32>/Switch'
         *  Switch: '<S33>/Switch1'
         */
        rtDW->Merge[1] = rtDW->Switch1;
      } else {
        /* Merge: '<S26>/Merge' incorporates:
         *  Switch: '<S29>/Switch'
         */
        rtDW->Merge[1] = (int16_T)tmp_2;
      }

      /* End of Switch: '<S32>/Switch2' */
    } else {
      /* Merge: '<S26>/Merge' incorporates:
       *  Constant: '<S29>/Constant1'
       */
      rtDW->Merge[1] = 0;
    }

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

    /* Merge: '<S26>/Merge' incorporates:
     *  Constant: '<S29>/Constant3'
     *  SignalConversion generated from: '<S29>/open_voltage'
     */
    rtDW->Merge[0] = 0;

    /* Update for UnitDelay: '<S29>/Unit Delay' incorporates:
     *  Switch: '<S25>/Switch3'
     */
    rtDW->UnitDelay_DSTATE = rtb_Divide1_m;

    /* Switch: '<S34>/Switch2' */
    if (rtb_LogicalOperator_p) {
      /* Update for UnitDelay: '<S34>/UnitDelay' incorporates:
       *  UnitDelay: '<S6>/UnitDelay2'
       */
      rtDW->UnitDelay_DSTATE_f = rtDW->UnitDelay2_DSTATE_p;
    } else {
      /* Update for UnitDelay: '<S34>/UnitDelay' incorporates:
       *  Sum: '<S31>/Add2'
       */
      rtDW->UnitDelay_DSTATE_f = (int16_T)tmp_2;
    }

    /* End of Switch: '<S34>/Switch2' */
    /* End of Outputs for SubSystem: '<S26>/open_mode' */
  } else if (rtb_z_ctrlMod == 2) {
    /* Outputs for IfAction SubSystem: '<S26>/torque_mode' incorporates:
     *  ActionPort: '<S30>/Action Port'
     */
    rtDW->r_currTgt = rtU->current_target;

    /* Merge: '<S26>/Merge1' incorporates:
     *  Inport: '<Root>/current_target'
     *  Inport: '<S30>/r_currTgt'
     *  Inport: '<S30>/r_spdTgt'
     *  Switch: '<S25>/Switch'
     */
    rtDW->Merge1 = rtb_Switch_np;

    /* End of Outputs for SubSystem: '<S26>/torque_mode' */
  } else {
    /* Outputs for IfAction SubSystem: '<S26>/If Action Subsystem1' incorporates:
     *  ActionPort: '<S28>/Action Port'
     */
    /* Merge: '<S26>/Merge1' incorporates:
     *  Inport: '<S28>/In1'
     *  Switch: '<S25>/Switch'
     */
    rtDW->Merge1 = rtb_Switch_np;

    /* End of Outputs for SubSystem: '<S26>/If Action Subsystem1' */
  }

  /* End of If: '<S26>/If' */

  /* Outputs for Atomic SubSystem: '<S35>/either_edge' */
  rtb_LogicalOperator_p = either_edge(rtb_RelationalOperator4_f,
    &rtDW->either_edge_f);

  /* End of Outputs for SubSystem: '<S35>/either_edge' */

  /* Switch: '<S35>/Switch1' */
  if (rtb_LogicalOperator_p) {
    rtb_UnitDelay_bc = rtb_Sum;
  }

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

  /* Gain: '<S52>/Multiply' incorporates:
   *  DataTypeConversion: '<S56>/Data Type Conversion'
   *  Inport: '<Root>/adc_a'
   *  Inport: '<Root>/adc_b'
   */
  tmp_2 = (12351 * rtU->adc_a) >> 11;
  if (tmp_2 > 32767) {
    tmp_2 = 32767;
  } else {
    if (tmp_2 < -32768) {
      tmp_2 = -32768;
    }
  }

  rtb_DataTypeConversion[0] = (int16_T)tmp_2;
  rtb_Gain = (12351 * rtU->adc_b) >> 11;
  if (rtb_Gain > 32767) {
    rtb_Gain = 32767;
  } else {
    if (rtb_Gain < -32768) {
      rtb_Gain = -32768;
    }
  }

  rtb_DataTypeConversion[1] = (int16_T)rtb_Gain;

  /* Sum: '<S46>/Add' incorporates:
   *  Gain: '<S52>/Multiply'
   */
  tmp_0 = (int16_T)tmp_2 + (int16_T)rtb_Gain;
  if (tmp_0 > 32767) {
    tmp_0 = 32767;
  } else {
    if (tmp_0 < -32768) {
      tmp_0 = -32768;
    }
  }

  /* Sum: '<S46>/Add1' incorporates:
   *  Sum: '<S46>/Add'
   */
  tmp_1 = -tmp_0;
  if (-tmp_0 > 32767) {
    tmp_1 = 32767;
  }

  /* Sum: '<S55>/Add3' incorporates:
   *  Gain: '<S52>/Multiply'
   *  Sum: '<S46>/Add1'
   */
  tmp_0 = (int16_T)rtb_Gain + (int16_T)tmp_1;
  if (tmp_0 > 32767) {
    tmp_0 = 32767;
  } else {
    if (tmp_0 < -32768) {
      tmp_0 = -32768;
    }
  }

  /* Sum: '<S55>/Add' incorporates:
   *  Gain: '<S52>/Multiply'
   *  Sum: '<S55>/Add3'
   */
  tmp_2 = (((int16_T)tmp_2 << 1) - tmp_0) >> 1;
  if (tmp_2 > 32767) {
    tmp_2 = 32767;
  } else {
    if (tmp_2 < -32768) {
      tmp_2 = -32768;
    }
  }

  /* Gain: '<S55>/Gain1' incorporates:
   *  Product: '<S57>/Divide1'
   *  Sum: '<S55>/Add'
   */
  rtb_Divide1_m = (int16_T)((21845 * tmp_2) >> 15);

  /* Gain: '<S55>/Gain2' incorporates:
   *  Gain: '<S52>/Multiply'
   *  Sum: '<S46>/Add1'
   *  Sum: '<S55>/Add2'
   */
  tmp_2 = ((((int16_T)rtb_Gain - (int16_T)tmp_1) >> 1) * 18919) >> 14;
  if (tmp_2 > 32767) {
    tmp_2 = 32767;
  } else {
    if (tmp_2 < -32768) {
      tmp_2 = -32768;
    }
  }

  /* PreLookup: '<S58>/a_elecAngle_XA' incorporates:
   *  Merge: '<S3>/Merge'
   */
  rtb_LogicalOperator3 = plook_u16s16_evencka(rtDW->Merge_i, 0, 4U, 1440U);

  /* Sum: '<S57>/Sum1' incorporates:
   *  Gain: '<S55>/Gain2'
   *  Interpolation_n-D: '<S58>/r_cos_M1'
   *  Interpolation_n-D: '<S58>/r_sin_M1'
   *  Product: '<S57>/Divide1'
   *  Product: '<S57>/Divide2'
   *  Product: '<S57>/Divide3'
   */
  rtb_Gain = (int16_T)((rtb_Divide1_m * rtConstP.pooled12[rtb_LogicalOperator3])
                       >> 14) + (int16_T)(((int16_T)tmp_2 *
    rtConstP.pooled11[rtb_LogicalOperator3]) >> 14);
  if (rtb_Gain > 32767) {
    rtb_Gain = 32767;
  } else {
    if (rtb_Gain < -32768) {
      rtb_Gain = -32768;
    }
  }

  /* SignalConversion generated from: '<S46>/Low_Pass_Filter' incorporates:
   *  Sum: '<S57>/Sum1'
   */
  rtb_TmpSignalConversionAtLow_Pa[0] = (int16_T)rtb_Gain;

  /* Sum: '<S57>/Sum6' incorporates:
   *  Gain: '<S55>/Gain2'
   *  Interpolation_n-D: '<S58>/r_cos_M1'
   *  Interpolation_n-D: '<S58>/r_sin_M1'
   *  Product: '<S57>/Divide1'
   *  Product: '<S57>/Divide4'
   */
  tmp_2 = (int16_T)(((int16_T)tmp_2 * rtConstP.pooled12[rtb_LogicalOperator3]) >>
                    14) - (int16_T)((rtb_Divide1_m *
    rtConstP.pooled11[rtb_LogicalOperator3]) >> 14);
  if (tmp_2 > 32767) {
    tmp_2 = 32767;
  } else {
    if (tmp_2 < -32768) {
      tmp_2 = -32768;
    }
  }

  /* SignalConversion generated from: '<S46>/Low_Pass_Filter' incorporates:
   *  Sum: '<S57>/Sum6'
   */
  rtb_TmpSignalConversionAtLow_Pa[1] = (int16_T)tmp_2;

  /* Outputs for Atomic SubSystem: '<S46>/Low_Pass_Filter' */
  /* Constant: '<S46>/Constant' */
  Low_Pass_Filter(rtb_TmpSignalConversionAtLow_Pa, 26214, rtb_DataTypeConversion,
                  &rtDW->Low_Pass_Filter_d);

  /* End of Outputs for SubSystem: '<S46>/Low_Pass_Filter' */

  /* Switch: '<S54>/Switch2' */
  rtb_Sum = (uint8_T)(rtb_z_ctrlMod != 0);

  /* DataTypeConversion: '<S48>/Data Type Conversion' incorporates:
   *  Logic: '<S48>/Logical Operator'
   *  RelationalOperator: '<S48>/Equal'
   *  UnitDelay: '<S48>/Unit Delay'
   */
  rtb_DataTypeConversion_m = (uint8_T)((rtb_Sum != 0) &&
    (rtDW->UnitDelay_DSTATE_b != rtb_Sum));

  /* If: '<S53>/If' incorporates:
   *  Logic: '<S54>/Logical Operator'
   */
  if ((rtb_Sum != 0) && rtb_Equal_k) {
    /* Outputs for IfAction SubSystem: '<S53>/Subsystem' incorporates:
     *  ActionPort: '<S81>/Action Port'
     */
    /* If: '<S84>/If' incorporates:
     *  Constant: '<S47>/Constant1'
     *  Constant: '<S90>/Constant1'
     *  Constant: '<S90>/Constant11'
     *  Constant: '<S90>/Constant2'
     *  Constant: '<S90>/Constant4'
     *  DataTypeConversion: '<S81>/Data Type Conversion'
     *  Gain: '<S81>/Gain'
     *  RelationalOperator: '<S81>/Equal'
     *  Sum: '<S90>/Add2'
     *  Switch: '<S14>/Switch2'
     *  Switch: '<S95>/Switch2'
     *  UnitDelay: '<S81>/Unit Delay'
     *  UnitDelay: '<S81>/Unit Delay1'
     */
    if (rtb_z_ctrlMod == 1) {
      /* Outputs for IfAction SubSystem: '<S84>/speed_mode' incorporates:
       *  ActionPort: '<S90>/Action Port'
       */
      /* Switch: '<S92>/Switch2' incorporates:
       *  Inport: '<Root>/speed_limit'
       *  Merge: '<S26>/Merge1'
       *  RelationalOperator: '<S92>/LowerRelop1'
       *  RelationalOperator: '<S92>/UpperRelop'
       *  Switch: '<S92>/Switch'
       *  Switch: '<S95>/Switch2'
       */
      if (rtDW->Merge1 > rtU->speed_limit) {
        rtb_Divide1_m = rtU->speed_limit;
      } else if (rtDW->Merge1 < 0) {
        /* Switch: '<S92>/Switch' incorporates:
         *  Constant: '<S90>/Constant5'
         *  Switch: '<S95>/Switch2'
         */
        rtb_Divide1_m = 0;
      } else {
        rtb_Divide1_m = rtDW->Merge1;
      }

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

      /* Outputs for Atomic SubSystem: '<S90>/pi_speed' */
      rtb_Divide1_m = pi_speed((int16_T)(rtb_Divide1_m - rtb_Switch2_l), 3174,
        10, 51, 7680, -7680, 0, (int16_T)(-rtDW->UnitDelay1_DSTATE_g >> 1),
        (uint8_T)(rtDW->UnitDelay_DSTATE_p != 1), &rtDW->pi_speed_d,
        &rtPrevZCX->pi_speed_d);

      /* End of Outputs for SubSystem: '<S90>/pi_speed' */

      /* Merge: '<S84>/Merge' incorporates:
       *  Constant: '<S47>/Constant1'
       *  Constant: '<S90>/Constant1'
       *  Constant: '<S90>/Constant11'
       *  Constant: '<S90>/Constant2'
       *  Constant: '<S90>/Constant4'
       *  DataTypeConversion: '<S81>/Data Type Conversion'
       *  Gain: '<S81>/Gain'
       *  RelationalOperator: '<S81>/Equal'
       *  SignalConversion generated from: '<S90>/idq_target'
       *  Sum: '<S90>/Add2'
       *  Switch: '<S14>/Switch2'
       *  Switch: '<S95>/Switch2'
       *  UnitDelay: '<S81>/Unit Delay'
       *  UnitDelay: '<S81>/Unit Delay1'
       */
      rtDW->Merge_f = rtb_Divide1_m;

      /* End of Outputs for SubSystem: '<S84>/speed_mode' */
    } else {
      if (rtb_z_ctrlMod == 2) {
        /* Outputs for IfAction SubSystem: '<S84>/torque_mode' incorporates:
         *  ActionPort: '<S91>/Action Port'
         */
        /* Switch: '<S91>/Switch' incorporates:
         *  Inport: '<S30>/r_currTgt'
         *  Switch: '<S14>/Switch2'
         *  Switch: '<S91>/Switch1'
         */
        if (rtDW->r_currTgt >= 0) {
          /* Product: '<S91>/Divide' incorporates:
           *  Merge: '<S26>/Merge1'
           *  Sum: '<S91>/Sum2'
           *  Switch: '<S14>/Switch2'
           */
          tmp_2 = (int16_T)(rtDW->Merge1 - rtb_Switch2_l) * 192;
          if (tmp_2 > 32767) {
            tmp_2 = 32767;
          } else {
            if (tmp_2 < -32768) {
              tmp_2 = -32768;
            }
          }

          rtb_Divide1_m = (int16_T)tmp_2;

          /* End of Product: '<S91>/Divide' */

          /* Product: '<S91>/Divide1' incorporates:
           *  Merge: '<S26>/Merge1'
           *  Sum: '<S91>/Sum3'
           *  Switch: '<S14>/Switch2'
           */
          tmp_2 = ((int16_T)(rtb_Switch2_l - rtDW->Merge1) * 51) >> 4;
          if (tmp_2 > 32767) {
            tmp_2 = 32767;
          } else {
            if (tmp_2 < -32768) {
              tmp_2 = -32768;
            }
          }

          /* MinMax: '<S91>/Max' incorporates:
           *  Product: '<S91>/Divide'
           *  Product: '<S91>/Divide1'
           */
          if (rtb_Divide1_m > (int16_T)tmp_2) {
            rtb_Sign = rtb_Divide1_m;
          } else {
            rtb_Sign = (int16_T)tmp_2;
          }

          /* End of MinMax: '<S91>/Max' */

          /* MinMax: '<S91>/Max3' incorporates:
           *  Constant: '<S47>/Constant1'
           *  MinMax: '<S91>/Max'
           */
          if (7680 < rtb_Sign) {
            rtb_Sign = 7680;
          }

          /* End of MinMax: '<S91>/Max3' */

          /* Switch: '<S96>/Switch2' incorporates:
           *  MinMax: '<S91>/Max1'
           *  MinMax: '<S91>/Max3'
           *  Product: '<S91>/Divide'
           *  Product: '<S91>/Divide1'
           *  RelationalOperator: '<S96>/LowerRelop1'
           */
          if (rtDW->r_currTgt > rtb_Sign) {
            /* Merge: '<S84>/Merge' */
            rtDW->Merge_f = rtb_Sign;
          } else {
            if (rtb_Divide1_m >= (int16_T)tmp_2) {
              /* MinMax: '<S91>/Max1' incorporates:
               *  Product: '<S91>/Divide1'
               */
              rtb_Divide1_m = (int16_T)tmp_2;
            }

            /* MinMax: '<S91>/Max2' incorporates:
             *  Gain: '<S47>/Gain1'
             *  MinMax: '<S91>/Max1'
             */
            if (rtb_Divide1_m <= -7680) {
              rtb_Divide1_m = -7680;
            }

            /* End of MinMax: '<S91>/Max2' */

            /* Switch: '<S96>/Switch' incorporates:
             *  MinMax: '<S91>/Max2'
             *  RelationalOperator: '<S96>/UpperRelop'
             */
            if (rtDW->r_currTgt < rtb_Divide1_m) {
              /* Merge: '<S84>/Merge' */
              rtDW->Merge_f = rtb_Divide1_m;
            } else {
              /* Merge: '<S84>/Merge' */
              rtDW->Merge_f = rtDW->r_currTgt;
            }

            /* End of Switch: '<S96>/Switch' */
          }

          /* End of Switch: '<S96>/Switch2' */
        } else if (rtb_Switch2_l > 80) {
          /* Switch: '<S91>/Switch1' incorporates:
           *  Merge: '<S84>/Merge'
           */
          rtDW->Merge_f = rtDW->r_currTgt;
        } else {
          /* Merge: '<S84>/Merge' incorporates:
           *  Constant: '<S91>/Constant5'
           *  Switch: '<S91>/Switch1'
           */
          rtDW->Merge_f = 0;
        }

        /* End of Switch: '<S91>/Switch' */
        /* End of Outputs for SubSystem: '<S84>/torque_mode' */
      }
    }

    /* End of If: '<S84>/If' */

    /* If: '<S83>/If' incorporates:
     *  Inport: '<Root>/foc_calibrate'
     *  S-Function (sfix_bitop): '<S83>/Bitwise Operator1'
     */
    if ((rtU->foc_calibrate & 2U) == 0U) {
      /* Outputs for IfAction SubSystem: '<S83>/MTPA_Calc' incorporates:
       *  ActionPort: '<S87>/Action Port'
       */
      /* Merge: '<S83>/Merge' incorporates:
       *  Constant: '<S87>/Constant3'
       *  Switch: '<S87>/Switch'
       */
      rtDW->Merge_c[0] = 0;
      rtDW->Merge_c[1] = rtDW->Merge_f;

      /* End of Outputs for SubSystem: '<S83>/MTPA_Calc' */
    } else {
      /* Outputs for IfAction SubSystem: '<S83>/MTPA_Cali' incorporates:
       *  ActionPort: '<S88>/Action Port'
       */
      /* PreLookup: '<S89>/a_elecAngle_XA' incorporates:
       *  Inport: '<Root>/open_theta'
       */
      rtb_Abs2 = plook_u16s16_evencka(rtU->open_theta, 0, 4U, 1440U);

      /* Product: '<S88>/Divide' incorporates:
       *  Gain: '<S88>/Gain'
       *  Interpolation_n-D: '<S89>/r_sin_M1'
       *  Merge: '<S84>/Merge'
       */
      rtDW->Merge_c[0] = (int16_T)(((int64_T)(-32768 *
        rtConstP.pooled11[rtb_Abs2]) * rtDW->Merge_f) >> 29);

      /* Product: '<S88>/Divide1' incorporates:
       *  Interpolation_n-D: '<S89>/r_cos_M1'
       *  Merge: '<S84>/Merge'
       */
      rtDW->Merge_c[1] = (int16_T)((rtConstP.pooled12[rtb_Abs2] * rtDW->Merge_f)
        >> 14);

      /* End of Outputs for SubSystem: '<S83>/MTPA_Cali' */
    }

    /* End of If: '<S83>/If' */

    /* Switch: '<S82>/Switch' incorporates:
     *  Merge: '<S83>/Merge'
     */
    rtDW->Switch[0] = rtDW->Merge_c[0];
    rtDW->Switch[1] = rtDW->Merge_c[1];

    /* Update for UnitDelay: '<S81>/Unit Delay' */
    rtDW->UnitDelay_DSTATE_p = rtb_z_ctrlMod;

    /* Update for UnitDelay: '<S81>/Unit Delay1' incorporates:
     *  Merge: '<S84>/Merge'
     */
    rtDW->UnitDelay1_DSTATE_g = rtDW->Merge_f;

    /* End of Outputs for SubSystem: '<S53>/Subsystem' */
  }

  /* End of If: '<S53>/If' */

  /* If: '<S48>/If' incorporates:
   *  Constant: '<S60>/Constant3'
   *  Constant: '<S60>/Constant4'
   *  Constant: '<S60>/Constant6'
   *  Constant: '<S60>/Constant9'
   *  Constant: '<S61>/Constant1'
   *  Constant: '<S61>/Constant7'
   *  Constant: '<S61>/Constant8'
   *  Constant: '<S61>/Constant9'
   *  Gain: '<S47>/Gain3'
   *  Gain: '<S47>/Gain5'
   *  If: '<S48>/If1'
   *  Inport: '<Root>/vbus_voltage'
   *  Sum: '<S60>/Add'
   *  Sum: '<S61>/Add1'
   *  Switch: '<S63>/Switch2'
   *  Switch: '<S67>/Switch2'
   */
  if (rtb_Sum == 1) {
    /* Outputs for IfAction SubSystem: '<S48>/iq_ctrl' incorporates:
     *  ActionPort: '<S61>/Action Port'
     */
    /* Switch: '<S67>/Switch2' incorporates:
     *  Constant: '<S47>/Constant1'
     *  RelationalOperator: '<S67>/LowerRelop1'
     *  RelationalOperator: '<S67>/UpperRelop'
     *  Switch: '<S67>/Switch'
     */
    if (rtDW->Switch[1] > 7680) {
      rtb_Divide1_m = 7680;
    } else if (rtDW->Switch[1] < -7680) {
      /* Switch: '<S67>/Switch' incorporates:
       *  Gain: '<S47>/Gain1'
       *  Switch: '<S67>/Switch2'
       */
      rtb_Divide1_m = -7680;
    } else {
      rtb_Divide1_m = rtDW->Switch[1];
    }

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

    /* Outputs for Atomic SubSystem: '<S61>/PI_iq' */
    PI_backCalc_fixdt((int16_T)(rtb_Divide1_m - rtb_DataTypeConversion[1]), 2048,
                      26, 1024, rtU->vbus_voltage, (int16_T)-rtU->vbus_voltage,
                      0, rtb_DataTypeConversion_m, &rtDW->Switch2_d,
                      &rtConstB.PI_iq, &rtDW->PI_iq, &rtPrevZCX->PI_iq);

    /* End of Outputs for SubSystem: '<S61>/PI_iq' */
    /* End of Outputs for SubSystem: '<S48>/iq_ctrl' */

    /* Outputs for IfAction SubSystem: '<S48>/id_ctrl' incorporates:
     *  ActionPort: '<S60>/Action Port'
     */
    /* Switch: '<S63>/Switch2' incorporates:
     *  Constant: '<S47>/Constant1'
     *  Constant: '<S61>/Constant1'
     *  Constant: '<S61>/Constant7'
     *  Constant: '<S61>/Constant8'
     *  Constant: '<S61>/Constant9'
     *  Gain: '<S47>/Gain5'
     *  Inport: '<Root>/vbus_voltage'
     *  RelationalOperator: '<S63>/LowerRelop1'
     *  RelationalOperator: '<S63>/UpperRelop'
     *  Sum: '<S61>/Add1'
     *  Switch: '<S63>/Switch'
     *  Switch: '<S67>/Switch2'
     */
    if (rtDW->Switch[0] > 7680) {
      rtb_Divide1_m = 7680;
    } else if (rtDW->Switch[0] < -7680) {
      /* Switch: '<S63>/Switch' incorporates:
       *  Gain: '<S47>/Gain4'
       *  Switch: '<S63>/Switch2'
       */
      rtb_Divide1_m = -7680;
    } else {
      rtb_Divide1_m = rtDW->Switch[0];
    }

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

    /* Outputs for Atomic SubSystem: '<S60>/PI_id' */
    PI_backCalc_fixdt((int16_T)(rtb_Divide1_m - rtb_DataTypeConversion[0]), 4096,
                      51, 1024, rtU->vbus_voltage, (int16_T)-rtU->vbus_voltage,
                      0, rtb_DataTypeConversion_m, &rtDW->Switch2,
                      &rtConstB.PI_id, &rtDW->PI_id, &rtPrevZCX->PI_id);

    /* End of Outputs for SubSystem: '<S60>/PI_id' */
    /* End of Outputs for SubSystem: '<S48>/id_ctrl' */
  }

  /* End of If: '<S48>/If' */

  /* Switch: '<S6>/Switch' incorporates:
   *  Merge: '<S26>/Merge'
   */
  if (rtb_z_ctrlMod != 0) {
    rtb_TmpSignalConversionAtLow_Pa[0] = rtDW->Switch2;
    rtb_TmpSignalConversionAtLow_Pa[1] = rtDW->Switch2_d;
  } else {
    rtb_TmpSignalConversionAtLow_Pa[0] = rtDW->Merge[0];
    rtb_TmpSignalConversionAtLow_Pa[1] = rtDW->Merge[1];
  }

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

  /* Gain: '<S51>/Gain' incorporates:
   *  Inport: '<Root>/vbus_voltage'
   *  Product: '<S49>/Divide1'
   */
  rtb_Divide1_m = (int16_T)((15565 * rtU->vbus_voltage) >> 14);

  /* Product: '<S51>/Divide' incorporates:
   *  Math: '<S51>/Math Function'
   *  Math: '<S51>/Math Function1'
   *  Product: '<S49>/Divide1'
   *  Sum: '<S51>/Sum of Elements'
   *  Switch: '<S6>/Switch'
   */
  tmp = ((int64_T)(((rtb_TmpSignalConversionAtLow_Pa[0] *
                     rtb_TmpSignalConversionAtLow_Pa[0]) >> 6) +
                   ((rtb_TmpSignalConversionAtLow_Pa[1] *
                     rtb_TmpSignalConversionAtLow_Pa[1]) >> 6)) << 12) /
    ((rtb_Divide1_m * rtb_Divide1_m) >> 6);
  if (tmp > 32767LL) {
    tmp = 32767LL;
  } else {
    if (tmp < -32768LL) {
      tmp = -32768LL;
    }
  }

  /* Sqrt: '<S51>/Sqrt' incorporates:
   *  Product: '<S51>/Divide'
   */
  rtb_Divide1_m = rt_sqrt_Us16En12_Ys16E_cQn1iwAF((int16_T)tmp);

  /* Switch: '<S51>/Switch' incorporates:
   *  Product: '<S51>/Divide1'
   *  RelationalOperator: '<S79>/Compare'
   *  Sqrt: '<S51>/Sqrt'
   *  Switch: '<S6>/Switch'
   */
  if (rtb_Divide1_m > 4096) {
    /* Product: '<S51>/Divide1' incorporates:
     *  Switch: '<S6>/Switch'
     */
    tmp_2 = (rtb_TmpSignalConversionAtLow_Pa[0] << 12) / rtb_Divide1_m;
    rtb_TmpSignalConversionAtLow_Pa[0] = (int16_T)tmp_2;

    /* Product: '<S51>/Divide1' incorporates:
     *  Switch: '<S51>/Switch'
     *  Switch: '<S6>/Switch'
     */
    tmp_2 = (rtb_TmpSignalConversionAtLow_Pa[1] << 12) / rtb_Divide1_m;
    rtb_TmpSignalConversionAtLow_Pa[1] = (int16_T)tmp_2;
  }

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

  /* Sum: '<S49>/Sum1' incorporates:
   *  Interpolation_n-D: '<S58>/r_cos_M1'
   *  Interpolation_n-D: '<S58>/r_sin_M1'
   *  Product: '<S49>/Divide2'
   *  Product: '<S49>/Divide3'
   */
  tmp_2 = (int16_T)((rtb_TmpSignalConversionAtLow_Pa[0] *
                     rtConstP.pooled11[rtb_LogicalOperator3]) >> 14) + (int16_T)
    ((rtb_TmpSignalConversionAtLow_Pa[1] *
      rtConstP.pooled12[rtb_LogicalOperator3]) >> 14);
  if (tmp_2 > 32767) {
    tmp_2 = 32767;
  } else {
    if (tmp_2 < -32768) {
      tmp_2 = -32768;
    }
  }

  rtb_Divide1_m = (int16_T)tmp_2;

  /* End of Sum: '<S49>/Sum1' */

  /* Sum: '<S49>/Sum6' incorporates:
   *  Interpolation_n-D: '<S58>/r_cos_M1'
   *  Interpolation_n-D: '<S58>/r_sin_M1'
   *  Product: '<S49>/Divide1'
   *  Product: '<S49>/Divide4'
   */
  tmp_2 = (int16_T)((rtb_TmpSignalConversionAtLow_Pa[0] *
                     rtConstP.pooled12[rtb_LogicalOperator3]) >> 14) - (int16_T)
    ((rtb_TmpSignalConversionAtLow_Pa[1] *
      rtConstP.pooled11[rtb_LogicalOperator3]) >> 14);
  if (tmp_2 > 32767) {
    tmp_2 = 32767;
  } else {
    if (tmp_2 < -32768) {
      tmp_2 = -32768;
    }
  }

  /* Product: '<S70>/Divide7' incorporates:
   *  Constant: '<S70>/Constant3'
   *  Sum: '<S49>/Sum1'
   */
  rtb_Sign = (int16_T)((2365 * rtb_Divide1_m) >> 12);

  /* MATLAB Function: '<S70>/sector_select' incorporates:
   *  Product: '<S70>/Divide7'
   *  Sum: '<S49>/Sum1'
   *  Sum: '<S49>/Sum6'
   */
  if (rtb_Divide1_m >= 0) {
    if ((int16_T)tmp_2 >= 0) {
      if (rtb_Sign > (int16_T)tmp_2) {
        /* DataTypeConversion: '<S70>/Data Type Conversion' */
        rtb_DataTypeConversion_m = 2U;
      } else {
        /* DataTypeConversion: '<S70>/Data Type Conversion' */
        rtb_DataTypeConversion_m = 1U;
      }
    } else if (-rtb_Sign > (int16_T)tmp_2) {
      /* DataTypeConversion: '<S70>/Data Type Conversion' */
      rtb_DataTypeConversion_m = 3U;
    } else {
      /* DataTypeConversion: '<S70>/Data Type Conversion' */
      rtb_DataTypeConversion_m = 2U;
    }
  } else if ((int16_T)tmp_2 >= 0) {
    if (-rtb_Sign > (int16_T)tmp_2) {
      /* DataTypeConversion: '<S70>/Data Type Conversion' */
      rtb_DataTypeConversion_m = 5U;
    } else {
      /* DataTypeConversion: '<S70>/Data Type Conversion' */
      rtb_DataTypeConversion_m = 6U;
    }
  } else if (rtb_Sign > (int16_T)tmp_2) {
    /* DataTypeConversion: '<S70>/Data Type Conversion' */
    rtb_DataTypeConversion_m = 4U;
  } else {
    /* DataTypeConversion: '<S70>/Data Type Conversion' */
    rtb_DataTypeConversion_m = 5U;
  }

  /* End of MATLAB Function: '<S70>/sector_select' */

  /* Gain: '<S70>/Gain' incorporates:
   *  Inport: '<Root>/vbus_voltage'
   */
  rtb_Gain = 18919 * rtU->vbus_voltage;

  /* Product: '<S70>/Divide' incorporates:
   *  Gain: '<S70>/Gain'
   *  Sum: '<S49>/Sum6'
   */
  rtb_Switch_np = (int16_T)(((int64_T)(int16_T)tmp_2 << 26) / rtb_Gain);

  /* Product: '<S70>/Divide1' incorporates:
   *  Gain: '<S70>/Gain'
   *  Sum: '<S49>/Sum1'
   */
  rtb_Divide1_m = (int16_T)(((int64_T)rtb_Divide1_m << 26) / rtb_Gain);

  /* MultiPortSwitch: '<S71>/Multiport Switch' incorporates:
   *  DataTypeConversion: '<S70>/Data Type Conversion1'
   */
  switch (rtb_DataTypeConversion_m) {
   case 1:
    /* Product: '<S73>/Divide3' incorporates:
     *  Product: '<S70>/Divide1'
     *  Product: '<S73>/Divide2'
     */
    rtb_Sign = (int16_T)(((int16_T)((rtb_Divide1_m * 9459) >> 13) * 375) >> 9);

    /* Product: '<S73>/Divide1' incorporates:
     *  Constant: '<S73>/Constant'
     *  Product: '<S70>/Divide'
     *  Product: '<S70>/Divide1'
     *  Product: '<S73>/Divide'
     *  Sum: '<S73>/Add'
     */
    rtb_Divide1_m = (int16_T)(((int16_T)(rtb_Switch_np - ((rtb_Divide1_m * 9459)
      >> 14)) * 375) >> 9);

    /* Product: '<S73>/Divide4' incorporates:
     *  Sum: '<S73>/Add1'
     *  Sum: '<S73>/Add2'
     */
    rtb_Switch_np = (int16_T)((int16_T)(3000 - (int16_T)(rtb_Divide1_m +
      rtb_Sign)) >> 1);

    /* Sum: '<S73>/Add3' */
    rtb_Sign += rtb_Switch_np;

    /* Outport: '<Root>/PWM' incorporates:
     *  Sum: '<S73>/Add4'
     */
    rtY->PWM[0] = (int16_T)(rtb_Sign + rtb_Divide1_m);
    rtY->PWM[1] = rtb_Sign;
    rtY->PWM[2] = rtb_Switch_np;
    break;

   case 2:
    /* Product: '<S74>/Divide1' incorporates:
     *  Constant: '<S74>/Constant'
     *  Product: '<S70>/Divide'
     *  Product: '<S70>/Divide1'
     *  Product: '<S74>/Divide'
     *  Sum: '<S74>/Add'
     */
    rtb_Sign = (int16_T)(((int16_T)(((rtb_Divide1_m * 9459) >> 14) +
      rtb_Switch_np) * 375) >> 9);

    /* Product: '<S74>/Divide3' incorporates:
     *  Constant: '<S74>/Constant'
     *  Product: '<S70>/Divide'
     *  Product: '<S70>/Divide1'
     *  Product: '<S74>/Divide2'
     *  Sum: '<S74>/Add5'
     */
    rtb_Divide1_m = (int16_T)(((int16_T)(((rtb_Divide1_m * 9459) >> 14) -
      rtb_Switch_np) * 375) >> 9);

    /* Product: '<S74>/Divide4' incorporates:
     *  Sum: '<S74>/Add1'
     *  Sum: '<S74>/Add2'
     */
    rtb_Switch_np = (int16_T)((int16_T)(3000 - (int16_T)(rtb_Divide1_m +
      rtb_Sign)) >> 1);

    /* Sum: '<S74>/Add3' */
    rtb_Sign += rtb_Switch_np;

    /* Outport: '<Root>/PWM' incorporates:
     *  Sum: '<S74>/Add4'
     */
    rtY->PWM[0] = rtb_Sign;
    rtY->PWM[1] = (int16_T)(rtb_Sign + rtb_Divide1_m);
    rtY->PWM[2] = rtb_Switch_np;
    break;

   case 3:
    /* Product: '<S75>/Divide1' incorporates:
     *  Constant: '<S75>/Constant'
     *  Product: '<S70>/Divide'
     *  Product: '<S70>/Divide1'
     *  Product: '<S75>/Divide'
     *  Sum: '<S75>/Add'
     */
    rtb_Switch_np = (int16_T)(((int16_T)(-rtb_Switch_np - ((rtb_Divide1_m * 9459)
      >> 14)) * 375) >> 9);

    /* Product: '<S75>/Divide3' incorporates:
     *  Product: '<S70>/Divide1'
     *  Product: '<S75>/Divide2'
     */
    rtb_Divide1_m = (int16_T)(((int16_T)((rtb_Divide1_m * 9459) >> 13) * 375) >>
      9);

    /* Product: '<S75>/Divide4' incorporates:
     *  Sum: '<S75>/Add1'
     *  Sum: '<S75>/Add2'
     */
    rtb_Sign = (int16_T)((int16_T)(3000 - (int16_T)(rtb_Divide1_m +
      rtb_Switch_np)) >> 1);

    /* Sum: '<S75>/Add3' */
    rtb_Switch_np += rtb_Sign;

    /* Outport: '<Root>/PWM' incorporates:
     *  Sum: '<S75>/Add4'
     */
    rtY->PWM[0] = rtb_Sign;
    rtY->PWM[1] = (int16_T)(rtb_Switch_np + rtb_Divide1_m);
    rtY->PWM[2] = rtb_Switch_np;
    break;

   case 4:
    /* Product: '<S76>/Divide1' incorporates:
     *  Constant: '<S76>/Constant'
     *  Product: '<S70>/Divide'
     *  Product: '<S70>/Divide1'
     *  Product: '<S76>/Divide'
     *  Sum: '<S76>/Add'
     */
    rtb_Switch_np = (int16_T)(((int16_T)(((rtb_Divide1_m * 9459) >> 14) -
      rtb_Switch_np) * 375) >> 9);

    /* Product: '<S76>/Divide3' incorporates:
     *  Product: '<S70>/Divide1'
     *  Product: '<S76>/Divide2'
     *  Sum: '<S76>/Add5'
     */
    rtb_Divide1_m = (int16_T)(((int16_T)(-((int16_T)((rtb_Divide1_m * 9459) >>
      13) << 2) >> 2) * 375) >> 9);

    /* Product: '<S76>/Divide4' incorporates:
     *  Sum: '<S76>/Add1'
     *  Sum: '<S76>/Add2'
     */
    rtb_Sign = (int16_T)((int16_T)(3000 - (int16_T)(rtb_Divide1_m +
      rtb_Switch_np)) >> 1);

    /* Sum: '<S76>/Add3' */
    rtb_Switch_np += rtb_Sign;

    /* Outport: '<Root>/PWM' incorporates:
     *  Sum: '<S76>/Add4'
     */
    rtY->PWM[0] = rtb_Sign;
    rtY->PWM[1] = rtb_Switch_np;
    rtY->PWM[2] = (int16_T)(rtb_Switch_np + rtb_Divide1_m);
    break;

   case 5:
    /* Product: '<S77>/Divide3' incorporates:
     *  Constant: '<S77>/Constant'
     *  Product: '<S70>/Divide'
     *  Product: '<S70>/Divide1'
     *  Product: '<S77>/Divide2'
     *  Sum: '<S77>/Add5'
     */
    rtb_Sign = (int16_T)(((int16_T)(rtb_Switch_np - ((rtb_Divide1_m * 9459) >>
      14)) * 375) >> 9);

    /* Product: '<S77>/Divide1' incorporates:
     *  Constant: '<S77>/Constant'
     *  Product: '<S70>/Divide'
     *  Product: '<S70>/Divide1'
     *  Product: '<S77>/Divide'
     *  Sum: '<S77>/Add'
     */
    rtb_Divide1_m = (int16_T)(((int16_T)(-rtb_Switch_np - ((rtb_Divide1_m * 9459)
      >> 14)) * 375) >> 9);

    /* Product: '<S77>/Divide4' incorporates:
     *  Sum: '<S77>/Add1'
     *  Sum: '<S77>/Add2'
     */
    rtb_Switch_np = (int16_T)((int16_T)(3000 - (int16_T)(rtb_Divide1_m +
      rtb_Sign)) >> 1);

    /* Sum: '<S77>/Add3' */
    rtb_Sign += rtb_Switch_np;

    /* Outport: '<Root>/PWM' incorporates:
     *  Sum: '<S77>/Add4'
     */
    rtY->PWM[0] = rtb_Sign;
    rtY->PWM[1] = rtb_Switch_np;
    rtY->PWM[2] = (int16_T)(rtb_Sign + rtb_Divide1_m);
    break;

   default:
    /* Product: '<S78>/Divide3' incorporates:
     *  Product: '<S70>/Divide1'
     *  Product: '<S78>/Divide2'
     *  Sum: '<S78>/Add5'
     */
    rtb_Sign = (int16_T)(((int16_T)(-((int16_T)((rtb_Divide1_m * 9459) >> 13) <<
      2) >> 2) * 375) >> 9);

    /* Product: '<S78>/Divide1' incorporates:
     *  Constant: '<S78>/Constant'
     *  Product: '<S70>/Divide'
     *  Product: '<S70>/Divide1'
     *  Product: '<S78>/Divide'
     *  Sum: '<S78>/Add'
     */
    rtb_Divide1_m = (int16_T)(((int16_T)(((rtb_Divide1_m * 9459) >> 14) +
      rtb_Switch_np) * 375) >> 9);

    /* Product: '<S78>/Divide4' incorporates:
     *  Sum: '<S78>/Add1'
     *  Sum: '<S78>/Add2'
     */
    rtb_Switch_np = (int16_T)((int16_T)(3000 - (int16_T)(rtb_Divide1_m +
      rtb_Sign)) >> 1);

    /* Sum: '<S78>/Add3' */
    rtb_Sign += rtb_Switch_np;

    /* Outport: '<Root>/PWM' incorporates:
     *  Sum: '<S78>/Add4'
     */
    rtY->PWM[0] = (int16_T)(rtb_Sign + rtb_Divide1_m);
    rtY->PWM[1] = rtb_Switch_np;
    rtY->PWM[2] = rtb_Sign;
    break;
  }

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

  /* Outport: '<Root>/VdPrev' incorporates:
   *  UnitDelay: '<S6>/UnitDelay1'
   */
  rtY->VdPrev = rtDW->UnitDelay1_DSTATE_f;

  /* Update for UnitDelay: '<S7>/UnitDelay1' incorporates:
   *  Sum: '<S7>/Sum3'
   */
  rtDW->UnitDelay1_DSTATE = qY;

  /* Update for Delay: '<S9>/Delay' incorporates:
   *  Inport: '<Root>/hall_a'
   */
  rtDW->Delay_DSTATE = rtU->hall_a;

  /* Update for Delay: '<S9>/Delay1' incorporates:
   *  Inport: '<Root>/hall_b'
   */
  rtDW->Delay1_DSTATE = rtU->hall_b;

  /* Update for Delay: '<S9>/Delay2' incorporates:
   *  Inport: '<Root>/hall_c'
   */
  rtDW->Delay2_DSTATE = rtU->hall_c;

  /* Update for UnitDelay: '<S14>/UnitDelay3' incorporates:
   *  Inport: '<Root>/hw_count'
   */
  rtDW->UnitDelay3_DSTATE = rtU->hw_count;

  /* Update for UnitDelay: '<S14>/UnitDelay4' incorporates:
   *  Abs: '<S14>/Abs5'
   */
  rtDW->UnitDelay4_DSTATE = rtb_Switch2;

  /* Update for UnitDelay: '<S35>/UnitDelay' */
  rtDW->UnitDelay_DSTATE_j = rtb_UnitDelay_bc;

  /* Update for UnitDelay: '<S6>/UnitDelay2' */
  rtDW->UnitDelay2_DSTATE_p = rtb_TmpSignalConversionAtLow_Pa[1];

  /* Update for UnitDelay: '<S48>/Unit Delay' */
  rtDW->UnitDelay_DSTATE_b = rtb_Sum;

  /* Update for UnitDelay: '<S6>/UnitDelay1' */
  rtDW->UnitDelay1_DSTATE_f = rtb_TmpSignalConversionAtLow_Pa[0];

  /* End of Outputs for SubSystem: '<Root>/PMSM_Controller' */

  /* Outport: '<Root>/sector' */
  rtY->sector = rtb_DataTypeConversion_m;

  /* Outport: '<Root>/n_MotError' */
  rtY->n_MotError = rtb_UnitDelay_bc;

  /* Outport: '<Root>/id' */
  rtY->id = rtb_DataTypeConversion[0];

  /* Outport: '<Root>/iq' */
  rtY->iq = rtb_DataTypeConversion[1];

  /* Outport: '<Root>/angle' incorporates:
   *  Merge: '<S3>/Merge'
   */
  rtY->angle = rtDW->Merge_i;

  /* Outport: '<Root>/rpm' incorporates:
   *  Switch: '<S14>/Switch2'
   */
  rtY->rpm = rtb_Switch2_l;

  /* Outport: '<Root>/hall_angle' incorporates:
   *  Merge: '<S15>/Merge'
   */
  rtY->hall_angle = rtb_Switch3_c;

  /* Outport: '<Root>/hall_state' */
  rtY->hall_state = rtb_Add_cr;

  /* Outport: '<Root>/running_mode' */
  rtY->running_mode = rtb_z_ctrlMod;
}

/* Model initialize function */
void PMSM_Controller_initialize(RT_MODEL *const rtM)
{
  DW *rtDW = rtM->dwork;
  PrevZCX *rtPrevZCX = rtM->prevZCSigState;
  ExtY *rtY = (ExtY *) rtM->outputs;
  rtPrevZCX->pi_speed_d.ResettableDelay_Reset_ZCE = POS_ZCSIG;
  rtPrevZCX->PI_id.ResettableDelay_Reset_ZCE_p = POS_ZCSIG;
  rtPrevZCX->PI_iq.ResettableDelay_Reset_ZCE_p = POS_ZCSIG;

  /* SystemInitialize for Atomic SubSystem: '<Root>/PMSM_Controller' */
  /* SystemInitialize for IfAction SubSystem: '<S14>/Raw_Motor_Speed_Estimation' */
  /* InitializeConditions for UnitDelay: '<S19>/UnitDelay2' */
  rtDW->UnitDelay2_DSTATE = 200000U;

  /* SystemInitialize for Outport: '<S19>/z_counter' incorporates:
   *  Inport: '<S19>/z_counterRawPrev'
   */
  rtDW->z_counterRawPrev = 200000U;

  /* End of SystemInitialize for SubSystem: '<S14>/Raw_Motor_Speed_Estimation' */

  /* SystemInitialize for Atomic SubSystem: '<S35>/Debounce_Filter' */
  Debounce_Filter_Init(&rtDW->Debounce_Filter_i);

  /* End of SystemInitialize for SubSystem: '<S35>/Debounce_Filter' */

  /* SystemInitialize for IfAction SubSystem: '<S53>/Subsystem' */
  /* SystemInitialize for IfAction SubSystem: '<S84>/speed_mode' */
  /* SystemInitialize for Atomic SubSystem: '<S90>/pi_speed' */
  pi_speed_Init(&rtDW->pi_speed_d);

  /* End of SystemInitialize for SubSystem: '<S90>/pi_speed' */
  /* End of SystemInitialize for SubSystem: '<S84>/speed_mode' */
  /* End of SystemInitialize for SubSystem: '<S53>/Subsystem' */

  /* SystemInitialize for IfAction SubSystem: '<S48>/iq_ctrl' */
  /* SystemInitialize for Atomic SubSystem: '<S61>/PI_iq' */
  PI_backCalc_fixdt_Init(&rtDW->PI_iq);

  /* End of SystemInitialize for SubSystem: '<S61>/PI_iq' */
  /* End of SystemInitialize for SubSystem: '<S48>/iq_ctrl' */

  /* SystemInitialize for IfAction SubSystem: '<S48>/id_ctrl' */
  /* SystemInitialize for Atomic SubSystem: '<S60>/PI_id' */
  PI_backCalc_fixdt_Init(&rtDW->PI_id);

  /* End of SystemInitialize for SubSystem: '<S60>/PI_id' */
  /* End of SystemInitialize for SubSystem: '<S48>/id_ctrl' */
  /* End of SystemInitialize for SubSystem: '<Root>/PMSM_Controller' */

  /* SystemInitialize for Outport: '<Root>/angle' incorporates:
   *  Merge: '<S3>/Merge'
   */
  rtY->angle = rtDW->Merge_i;
}

/*
 * File trailer for generated code.
 *
 * [EOF]
 */