/*

 * File: PMSM_Controller.c

 *

 * Code generated for Simulink model 'PMSM_Controller'.

 *

 * Model version                  : 1.1277

 * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020

 * C/C++ source code generated on : Wed Apr 13 16:49:14 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 int32_T rt_sqrt_Us32En6_Ys32En_dnD5ZXjs(int32_T u);

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 int32_T div_nde_s32_floor(int32_T numerator, int32_T denominator);

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,

  uint8_T rtu_reset, const ConstB_pi_speed *localC, 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;

}



static int32_T div_nde_s32_floor(int32_T numerator, int32_T denominator)

{

  return (((numerator < 0) != (denominator < 0)) && (numerator % denominator !=

           0) ? -1 : 0) + numerator / denominator;

}



/*

 * System initialize for atomic system:

 *    '<S37>/Counter'

 *    '<S36>/Counter'

 */

static void Counter_Init(DW_Counter *localDW, uint16_T rtp_z_cntInit)

{

  /* InitializeConditions for UnitDelay: '<S42>/UnitDelay' */

  localDW->UnitDelay_DSTATE = rtp_z_cntInit;

}



/*

 * Output and update for atomic system:

 *    '<S37>/Counter'

 *    '<S36>/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: '<S42>/Switch1' incorporates:

   *  Constant: '<S42>/Constant23'

   *  UnitDelay: '<S42>/UnitDelay'

   */

  if (rtu_rst) {

    rtu_rst_0 = 0U;

  } else {

    rtu_rst_0 = localDW->UnitDelay_DSTATE;

  }



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



  /* Sum: '<S41>/Sum1' */

  rty_cnt_0 = (uint16_T)((uint32_T)rtu_inc + rtu_rst_0);



  /* MinMax: '<S41>/MinMax' */

  if (rty_cnt_0 < rtu_max) {

    /* Update for UnitDelay: '<S42>/UnitDelay' */

    localDW->UnitDelay_DSTATE = rty_cnt_0;

  } else {

    /* Update for UnitDelay: '<S42>/UnitDelay' */

    localDW->UnitDelay_DSTATE = rtu_max;

  }



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

  return rty_cnt_0;

}



/*

 * Output and update for atomic system:

 *    '<S33>/either_edge'

 *    '<S32>/either_edge'

 */

static boolean_T either_edge(boolean_T rtu_u, DW_either_edge *localDW)

{

  boolean_T rty_y_0;



  /* RelationalOperator: '<S38>/Relational Operator' incorporates:

   *  UnitDelay: '<S38>/UnitDelay'

   */

  rty_y_0 = (rtu_u != localDW->UnitDelay_DSTATE);



  /* Update for UnitDelay: '<S38>/UnitDelay' */

  localDW->UnitDelay_DSTATE = rtu_u;

  return rty_y_0;

}



/* System initialize for atomic system: '<S32>/Debounce_Filter' */

static void Debounce_Filter_Init(DW_Debounce_Filter *localDW)

{

  /* SystemInitialize for IfAction SubSystem: '<S33>/Qualification' */

  /* SystemInitialize for Atomic SubSystem: '<S37>/Counter' */

  Counter_Init(&localDW->Counter_f, 0);



  /* End of SystemInitialize for SubSystem: '<S37>/Counter' */

  /* End of SystemInitialize for SubSystem: '<S33>/Qualification' */



  /* SystemInitialize for IfAction SubSystem: '<S33>/Dequalification' */

  /* SystemInitialize for Atomic SubSystem: '<S36>/Counter' */

  Counter_Init(&localDW->Counter_d, 0);



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

  /* End of SystemInitialize for SubSystem: '<S33>/Dequalification' */

}



/* Output and update for atomic system: '<S32>/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: '<S33>/either_edge' */

  rtb_RelationalOperator_e = either_edge(rtu_u, &localDW->either_edge_j);



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



  /* If: '<S33>/If2' incorporates:

   *  Constant: '<S36>/Constant6'

   *  Constant: '<S37>/Constant6'

   *  Inport: '<S35>/yPrev'

   *  Logic: '<S33>/Logical Operator1'

   *  Logic: '<S33>/Logical Operator2'

   *  Logic: '<S33>/Logical Operator3'

   *  Logic: '<S33>/Logical Operator4'

   *  UnitDelay: '<S33>/UnitDelay'

   */

  if (rtu_u && (!localDW->UnitDelay_DSTATE)) {

    /* Outputs for IfAction SubSystem: '<S33>/Qualification' incorporates:

     *  ActionPort: '<S37>/Action Port'

     */

    /* Outputs for Atomic SubSystem: '<S37>/Counter' */

    rtb_Sum1_jb = Counter(1, rtu_tAcv, rtb_RelationalOperator_e,

                          &localDW->Counter_f);



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



    /* Switch: '<S37>/Switch2' incorporates:

     *  Constant: '<S37>/Constant6'

     *  RelationalOperator: '<S37>/Relational Operator2'

     */

    *rty_y = ((rtb_Sum1_jb > rtu_tAcv) || localDW->UnitDelay_DSTATE);



    /* End of Outputs for SubSystem: '<S33>/Qualification' */

  } else if ((!rtu_u) && localDW->UnitDelay_DSTATE) {

    /* Outputs for IfAction SubSystem: '<S33>/Dequalification' incorporates:

     *  ActionPort: '<S36>/Action Port'

     */

    /* Outputs for Atomic SubSystem: '<S36>/Counter' */

    rtb_Sum1_jb = Counter(1, rtu_tDeacv, rtb_RelationalOperator_e,

                          &localDW->Counter_d);



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



    /* Switch: '<S36>/Switch2' incorporates:

     *  Constant: '<S36>/Constant6'

     *  RelationalOperator: '<S36>/Relational Operator2'

     */

    *rty_y = ((rtb_Sum1_jb <= rtu_tDeacv) && localDW->UnitDelay_DSTATE);



    /* End of Outputs for SubSystem: '<S33>/Dequalification' */

  } else {

    /* Outputs for IfAction SubSystem: '<S33>/Default' incorporates:

     *  ActionPort: '<S35>/Action Port'

     */

    *rty_y = localDW->UnitDelay_DSTATE;



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

  }



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



  /* Update for UnitDelay: '<S33>/UnitDelay' */

  localDW->UnitDelay_DSTATE = *rty_y;

}



/* Output and update for atomic system: '<S43>/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: '<S53>/Sum2' incorporates:

   *  UnitDelay: '<S53>/UnitDelay1'

   */

  tmp = rtu_u[0] - localDW->UnitDelay1_DSTATE[0];

  if (tmp > 32767) {

    tmp = 32767;

  } else {

    if (tmp < -32768) {

      tmp = -32768;

    }

  }



  /* Product: '<S53>/Divide3' incorporates:

   *  Sum: '<S53>/Sum2'

   */

  rty_y[0] = (int16_T)((rtu_coef * tmp) >> 16);



  /* Sum: '<S53>/Sum3' incorporates:

   *  UnitDelay: '<S53>/UnitDelay1'

   */

  rty_y[0] += localDW->UnitDelay1_DSTATE[0];



  /* Update for UnitDelay: '<S53>/UnitDelay1' incorporates:

   *  Sum: '<S53>/Sum3'

   */

  localDW->UnitDelay1_DSTATE[0] = rty_y[0];



  /* Sum: '<S53>/Sum2' incorporates:

   *  UnitDelay: '<S53>/UnitDelay1'

   */

  tmp = rtu_u[1] - localDW->UnitDelay1_DSTATE[1];

  if (tmp > 32767) {

    tmp = 32767;

  } else {

    if (tmp < -32768) {

      tmp = -32768;

    }

  }



  /* Product: '<S53>/Divide3' incorporates:

   *  Sum: '<S53>/Sum2'

   */

  rty_y[1] = (int16_T)((rtu_coef * tmp) >> 16);



  /* Sum: '<S53>/Sum3' incorporates:

   *  UnitDelay: '<S53>/UnitDelay1'

   */

  rty_y[1] += localDW->UnitDelay1_DSTATE[1];



  /* Update for UnitDelay: '<S53>/UnitDelay1' incorporates:

   *  Sum: '<S53>/Sum3'

   */

  localDW->UnitDelay1_DSTATE[1] = rty_y[1];

}



/*

 * System initialize for atomic system:

 *    '<S57>/PI_iq'

 *    '<S56>/PI_id'

 */

static void PI_backCalc_fixdt_Init(DW_PI_backCalc_fixdt *localDW)

{

  /* InitializeConditions for Delay: '<S64>/Resettable Delay' */

  localDW->icLoad = 1U;

}



/*

 * Output and update for atomic system:

 *    '<S57>/PI_iq'

 *    '<S56>/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: '<S62>/Divide4' */

  rtb_Divide4_h = (rtu_err * rtu_P) >> 6;



  /* Delay: '<S64>/Resettable Delay' incorporates:

   *  DataTypeConversion: '<S64>/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: '<S62>/Divide1' incorporates:

   *  Product: '<S62>/Divide4'

   */

  tmp = ((int64_T)rtb_Divide4_h * rtu_I) >> 10;

  if (tmp > 2147483647LL) {

    tmp = 2147483647LL;

  } else {

    if (tmp < -2147483648LL) {

      tmp = -2147483648LL;

    }

  }



  /* Sum: '<S62>/Sum2' incorporates:

   *  Product: '<S62>/Divide1'

   *  UnitDelay: '<S62>/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: '<S64>/Sum1' incorporates:

   *  Delay: '<S64>/Resettable Delay'

   *  Sum: '<S62>/Sum2'

   */

  rtb_Sum1_ae = ((int32_T)tmp >> 2) + localDW->ResettableDelay_DSTATE;



  /* Sum: '<S62>/Sum6' incorporates:

   *  DataTypeConversion: '<S64>/Data Type Conversion1'

   *  Product: '<S62>/Divide4'

   *  Sum: '<S64>/Sum1'

   */

  tmp = ((int64_T)(rtb_Sum1_ae >> 2) << 4) + rtb_Divide4_h;

  if (tmp > 2147483647LL) {

    tmp = 2147483647LL;

  } else {

    if (tmp < -2147483648LL) {

      tmp = -2147483648LL;

    }

  }



  /* Switch: '<S65>/Switch2' incorporates:

   *  RelationalOperator: '<S65>/LowerRelop1'

   *  RelationalOperator: '<S65>/UpperRelop'

   *  Sum: '<S62>/Sum6'

   *  Switch: '<S65>/Switch'

   */

  if ((int32_T)tmp > (rtu_satMax << 4)) {

    *rty_pi_out = rtu_satMax;

  } else if ((int32_T)tmp < (rtu_satMin << 4)) {

    /* Switch: '<S65>/Switch' */

    *rty_pi_out = rtu_satMin;

  } else {

    *rty_pi_out = (int16_T)((int32_T)tmp >> 4);

  }



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



  /* Update for UnitDelay: '<S62>/UnitDelay' incorporates:

   *  Product: '<S62>/Divide2'

   *  Sum: '<S62>/Sum3'

   *  Sum: '<S62>/Sum6'

   */

  localDW->UnitDelay_DSTATE = (int32_T)(((int64_T)((*rty_pi_out << 4) - (int32_T)

    tmp) * rtu_Kb) >> 10);



  /* Update for Delay: '<S64>/Resettable Delay' incorporates:

   *  Sum: '<S64>/Sum1'

   */

  localDW->icLoad = 0U;

  localDW->ResettableDelay_DSTATE = rtb_Sum1_ae;

}



/* System initialize for atomic system: '<S82>/pi_speed' */

static void pi_speed_Init(DW_pi_speed *localDW)

{

  /* InitializeConditions for Delay: '<S86>/Resettable Delay' */

  localDW->icLoad = 1U;

}



/* Output and update for atomic system: '<S82>/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,

  uint8_T rtu_reset, const ConstB_pi_speed *localC, 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: '<S85>/Divide4' */

  rtb_Divide4_g = (rtu_err * rtu_P) >> 2;



  /* Delay: '<S86>/Resettable Delay' incorporates:

   *  DataTypeConversion: '<S86>/Data Type Conversion2'

   */

  if ((rtu_reset > 0) && (localZCE->ResettableDelay_Reset_ZCE_a != POS_ZCSIG)) {

    localDW->icLoad = 1U;

  }



  localZCE->ResettableDelay_Reset_ZCE_a = (ZCSigState)(rtu_reset > 0);

  if (localDW->icLoad != 0) {

    localDW->ResettableDelay_DSTATE = localC->DataTypeConversion2;

  }



  /* Product: '<S85>/Divide1' incorporates:

   *  Product: '<S85>/Divide4'

   */

  tmp = ((int64_T)rtb_Divide4_g * rtu_I) >> 10;

  if (tmp > 2147483647LL) {

    tmp = 2147483647LL;

  } else {

    if (tmp < -2147483648LL) {

      tmp = -2147483648LL;

    }

  }



  /* Sum: '<S85>/Sum2' incorporates:

   *  Product: '<S85>/Divide1'

   *  UnitDelay: '<S85>/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: '<S86>/Sum1' incorporates:

   *  Delay: '<S86>/Resettable Delay'

   *  Sum: '<S85>/Sum2'

   */

  rtb_Sum1_c = (int32_T)tmp + localDW->ResettableDelay_DSTATE;



  /* Sum: '<S85>/Sum6' incorporates:

   *  DataTypeConversion: '<S86>/Data Type Conversion1'

   *  Product: '<S85>/Divide4'

   *  Sum: '<S86>/Sum1'

   */

  tmp = ((int64_T)(rtb_Sum1_c >> 2) << 4) + rtb_Divide4_g;

  if (tmp > 2147483647LL) {

    tmp = 2147483647LL;

  } else {

    if (tmp < -2147483648LL) {

      tmp = -2147483648LL;

    }

  }



  /* Switch: '<S87>/Switch2' incorporates:

   *  RelationalOperator: '<S87>/LowerRelop1'

   *  RelationalOperator: '<S87>/UpperRelop'

   *  Sum: '<S85>/Sum6'

   *  Switch: '<S87>/Switch'

   */

  if ((int32_T)tmp > (rtu_satMax << 4)) {

    rty_pi_out_0 = rtu_satMax;

  } else if ((int32_T)tmp < (rtu_satMin << 4)) {

    /* Switch: '<S87>/Switch' */

    rty_pi_out_0 = rtu_satMin;

  } else {

    rty_pi_out_0 = (int16_T)((int32_T)tmp >> 4);

  }



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



  /* Update for UnitDelay: '<S85>/UnitDelay' incorporates:

   *  Product: '<S85>/Divide2'

   *  Sum: '<S85>/Sum3'

   *  Sum: '<S85>/Sum6'

   */

  localDW->UnitDelay_DSTATE = (int32_T)(((int64_T)((rty_pi_out_0 << 4) -

    (int32_T)tmp) * rtu_Kb) >> 12);



  /* Update for Delay: '<S86>/Resettable Delay' incorporates:

   *  Sum: '<S86>/Sum1'

   */

  localDW->icLoad = 0U;

  localDW->ResettableDelay_DSTATE = rtb_Sum1_c;

  return rty_pi_out_0;

}



int32_T rt_sqrt_Us32En6_Ys32En_dnD5ZXjs(int32_T u)

{

  int64_T tmp03_u;

  int32_T iBit;

  int32_T shiftMask;

  int32_T tmp01_y;

  int32_T y;



  /* Fixed-Point Sqrt Computation by the bisection method. */

  if (u > 0) {

    y = 0;

    shiftMask = 1073741824;

    tmp03_u = (int64_T)u << 6;

    for (iBit = 0; iBit < 31; iBit++) {

      tmp01_y = y | shiftMask;

      if ((int64_T)tmp01_y * tmp01_y <= tmp03_u) {

        y = tmp01_y;

      }



      shiftMask = (int32_T)((uint32_T)shiftMask >> 1U);

    }

  } else {

    y = 0;

  }



  return y;

}



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 rtb_Saturation;

  int32_T rtb_Sum1;

  int32_T tmp_0;

  int32_T tmp_2;

  uint32_T qY;

  uint32_T tmp_1;

  int16_T rtb_DataTypeConversion_k1[2];

  int16_T rtb_Switch_m[2];

  int16_T rtb_MultiportSwitch_idx_0;

  int16_T rtb_MultiportSwitch_idx_1;

  int16_T rtb_Sign;

  int16_T rtb_Sum;

  int16_T rtb_Switch2_c;

  int16_T rtb_Switch3_c;

  int16_T rtb_Switch_dr;

  int16_T rtb_Switch_oi;

  int16_T rtb_r_cos_M1;

  uint16_T rtb_LogicalOperator3;

  uint16_T rtb_Switch2_j;

  int8_T UnitDelay3;

  int8_T rtb_Sum2;

  int8_T rtb_Sum2_tmp;

  uint8_T rtb_Add_cr;

  uint8_T rtb_DataTypeConversion1_c;

  uint8_T rtb_DataTypeConversion_i;

  uint8_T rtb_Switch2_fu;

  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: '<S12>/If2'

   *  Inport: '<S17>/z_counterRawPrev'

   *  UnitDelay: '<S12>/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: '<S12>/Raw_Motor_Speed_Estimation' incorporates:

     *  ActionPort: '<S17>/Action Port'

     */

    /* RelationalOperator: '<S17>/Relational Operator4' */

    rtb_RelationalOperator4_f = (rtDW->Switch2_i != UnitDelay3);

    rtDW->z_counterRawPrev = rtDW->UnitDelay3_DSTATE;



    /* Switch: '<S17>/Switch3' incorporates:

     *  Constant: '<S17>/Constant4'

     *  Inport: '<S17>/z_counterRawPrev'

     *  Logic: '<S17>/Logical Operator1'

     *  Switch: '<S17>/Switch2'

     *  UnitDelay: '<S12>/UnitDelay3'

     *  UnitDelay: '<S17>/UnitDelay1'

     */

    if (rtb_RelationalOperator4_f && rtDW->UnitDelay1_DSTATE_i) {

      rtb_Switch3_c = 0;

    } else {

      if (rtb_RelationalOperator4_f) {

        /* Switch: '<S17>/Switch2' incorporates:

         *  UnitDelay: '<S12>/UnitDelay4'

         */

        rtb_Switch2_j = rtDW->UnitDelay4_DSTATE;

      } else {

        /* Product: '<S17>/Divide13' incorporates:

         *  Sum: '<S17>/Sum13'

         *  Switch: '<S17>/Switch2'

         *  UnitDelay: '<S17>/UnitDelay2'

         *  UnitDelay: '<S17>/UnitDelay3'

         *  UnitDelay: '<S17>/UnitDelay5'

         */

        tmp_1 = 8000000U / (((rtDW->UnitDelay2_DSTATE +

                              rtDW->UnitDelay3_DSTATE_l) +

                             rtDW->UnitDelay5_DSTATE) + rtDW->z_counterRawPrev);

        if (tmp_1 > 65535U) {

          tmp_1 = 65535U;

        }



        /* Switch: '<S17>/Switch2' incorporates:

         *  Product: '<S17>/Divide13'

         */

        rtb_Switch2_j = (uint16_T)tmp_1;

      }



      rtb_Switch3_c = (int16_T)rtb_Switch2_j;

    }



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



    /* Product: '<S17>/Divide11' incorporates:

     *  Switch: '<S17>/Switch3'

     */

    rtDW->Divide11 = (int16_T)(rtb_Switch3_c * rtDW->Switch2_i);



    /* Update for UnitDelay: '<S17>/UnitDelay1' */

    rtDW->UnitDelay1_DSTATE_i = rtb_RelationalOperator4_f;



    /* Update for UnitDelay: '<S17>/UnitDelay2' incorporates:

     *  UnitDelay: '<S17>/UnitDelay3'

     */

    rtDW->UnitDelay2_DSTATE = rtDW->UnitDelay3_DSTATE_l;



    /* Update for UnitDelay: '<S17>/UnitDelay3' incorporates:

     *  UnitDelay: '<S17>/UnitDelay5'

     */

    rtDW->UnitDelay3_DSTATE_l = rtDW->UnitDelay5_DSTATE;



    /* Update for UnitDelay: '<S17>/UnitDelay5' */

    rtDW->UnitDelay5_DSTATE = rtDW->z_counterRawPrev;



    /* End of Outputs for SubSystem: '<S12>/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_1 = rtU->hw_count;

  } else {

    tmp_1 = 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_1 << 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: '<S13>/Add2' incorporates:

   *  Constant: '<S13>/Constant2'

   *  Product: '<S10>/Divide2'

   */

  rtb_Switch3_c = (int16_T)((((15 * rtb_Switch3_c) >> 4) + 3840) >> 2);



  /* If: '<S13>/If' incorporates:

   *  Constant: '<S13>/Constant3'

   *  DataTypeConversion: '<S13>/Data Type Conversion'

   *  Inport: '<S14>/In1'

   *  Merge: '<S13>/Merge'

   *  Sum: '<S13>/Add'

   *  Sum: '<S13>/Add2'

   */

  if ((int16_T)(rtb_Switch3_c >> 4) >= 360) {

    /* Outputs for IfAction SubSystem: '<S13>/If Action Subsystem' incorporates:

     *  ActionPort: '<S14>/Action Port'

     */

    rtb_Switch3_c = (int16_T)(rtb_Switch3_c - 5760);



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

  }



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



  /* Switch: '<S12>/Switch2' incorporates:

   *  Constant: '<S12>/Constant4'

   *  Inport: '<Root>/hw_count'

   *  Product: '<S17>/Divide11'

   *  RelationalOperator: '<S12>/Relational Operator2'

   */

  if (rtU->hw_count >= 400000U) {

    rtb_Switch2_c = 0;

  } else {

    rtb_Switch2_c = rtDW->Divide11;

  }



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



  /* Abs: '<S12>/Abs5' incorporates:

   *  Switch: '<S12>/Switch2'

   */

  if (rtb_Switch2_c < 0) {

    rtb_Switch2_j = (uint16_T)-rtb_Switch2_c;

  } else {

    rtb_Switch2_j = (uint16_T)rtb_Switch2_c;

  }



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



  /* If: '<S12>/If1' */

  if (rtb_LogicalOperator_p) {

    /* Outputs for IfAction SubSystem: '<S12>/Subsystem' incorporates:

     *  ActionPort: '<S18>/Action Port'

     */

    /* Relay: '<S18>/n_commDeacv' incorporates:

     *  Abs: '<S12>/Abs5'

     */

    rtDW->n_commDeacv_Mode = ((rtb_Switch2_j >= 120) || ((rtb_Switch2_j > 60) &&

      rtDW->n_commDeacv_Mode));



    /* RelationalOperator: '<S20>/Compare' incorporates:

     *  Constant: '<S20>/Constant'

     *  Relay: '<S18>/n_commDeacv'

     *  Sum: '<S18>/Sum13'

     *  UnitDelay: '<S18>/UnitDelay2'

     *  UnitDelay: '<S18>/UnitDelay3'

     *  UnitDelay: '<S18>/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: '<S18>/UnitDelay2' incorporates:

     *  UnitDelay: '<S18>/UnitDelay3'

     */

    rtDW->UnitDelay2_DSTATE_f = rtDW->UnitDelay3_DSTATE_lh;



    /* Update for UnitDelay: '<S18>/UnitDelay3' incorporates:

     *  UnitDelay: '<S18>/UnitDelay5'

     */

    rtDW->UnitDelay3_DSTATE_lh = rtDW->UnitDelay5_DSTATE_f;



    /* Update for UnitDelay: '<S18>/UnitDelay5' incorporates:

     *  Logic: '<S18>/Logical Operator3'

     *  Relay: '<S18>/n_commDeacv'

     */

    rtDW->UnitDelay5_DSTATE_f = rtDW->n_commDeacv_Mode;



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

  }



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



  /* S-Function (sfix_bitop): '<S3>/Bitwise Operator1' incorporates:

   *  Inport: '<Root>/foc_calibrate'

   *  S-Function (sfix_bitop): '<S4>/Bitwise Operator2'

   */

  tmp_2 = rtU->foc_calibrate & 1;



  /* Switch: '<S3>/Switch' incorporates:

   *  Inport: '<Root>/open_theta'

   *  Merge: '<S13>/Merge'

   *  S-Function (sfix_bitop): '<S3>/Bitwise Operator1'

   */

  if (tmp_2 > 0) {

    rtb_r_cos_M1 = (int16_T)(rtU->open_theta << 4);

  } else {

    rtb_r_cos_M1 = rtb_Switch3_c;

  }



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



  /* Sum: '<S3>/Sum' incorporates:

   *  Inport: '<Root>/foc_calibrate'

   *  Inport: '<Root>/open_theta'

   *  Product: '<S3>/Divide'

   *  S-Function (sfix_bitop): '<S3>/Bitwise Operator2'

   */

  rtb_Sum = (int16_T)((int16_T)((int16_T)((rtU->foc_calibrate & 2) *

    rtU->open_theta) << 4) + rtb_r_cos_M1);



  /* Abs: '<S4>/Abs2' incorporates:

   *  Switch: '<S12>/Switch2'

   */

  if (rtb_Switch2_c < 0) {

    rtb_LogicalOperator3 = (uint16_T)((uint32_T)-rtb_Switch2_c >> 2);

  } else {

    rtb_LogicalOperator3 = (uint16_T)((uint32_T)rtb_Switch2_c >> 2);

  }



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



  /* UnitDelay: '<S32>/UnitDelay' */

  rtb_UnitDelay_bc = rtDW->UnitDelay_DSTATE_j;



  /* Outport: '<Root>/VqPrev' incorporates:

   *  UnitDelay: '<S6>/UnitDelay2'

   */

  rtY->VqPrev = rtDW->UnitDelay2_DSTATE_p;



  /* Switch: '<S32>/Switch3' incorporates:

   *  Abs: '<S12>/Abs5'

   *  Abs: '<S32>/Abs4'

   *  Constant: '<S32>/CTRL_COMM4'

   *  Inport: '<Root>/b_motEna'

   *  Logic: '<S32>/Logical Operator1'

   *  RelationalOperator: '<S12>/Relational Operator9'

   *  RelationalOperator: '<S32>/Relational Operator7'

   *  S-Function (sfix_bitop): '<S32>/Bitwise Operator1'

   *  UnitDelay: '<S6>/UnitDelay2'

   */

  if ((rtb_UnitDelay_bc & 4U) != 0U) {

    rtb_LogicalOperator_p = true;

  } else {

    if (rtDW->UnitDelay2_DSTATE_p < 0) {

      /* Abs: '<S32>/Abs4' incorporates:

       *  UnitDelay: '<S6>/UnitDelay2'

       */

      rtb_r_cos_M1 = (int16_T)-rtDW->UnitDelay2_DSTATE_p;

    } else {

      /* Abs: '<S32>/Abs4' incorporates:

       *  UnitDelay: '<S6>/UnitDelay2'

       */

      rtb_r_cos_M1 = rtDW->UnitDelay2_DSTATE_p;

    }



    rtb_LogicalOperator_p = (rtU->b_motEna && (rtb_Switch2_j < 12) &&

      (rtb_r_cos_M1 > 960));

  }



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



  /* Sum: '<S32>/Sum' incorporates:

   *  Constant: '<S32>/CTRL_COMM'

   *  Constant: '<S32>/CTRL_COMM1'

   *  DataTypeConversion: '<S32>/Data Type Conversion3'

   *  Gain: '<S32>/g_Hb'

   *  Gain: '<S32>/g_Hb1'

   *  RelationalOperator: '<S32>/Relational Operator1'

   *  RelationalOperator: '<S32>/Relational Operator3'

   */

  rtb_DataTypeConversion1_c = (uint8_T)(((uint32_T)((rtb_Add_cr == 7) << 1) +

    (rtb_Add_cr == 0)) + (rtb_LogicalOperator_p << 2));



  /* Outputs for Atomic SubSystem: '<S32>/Debounce_Filter' */

  /* RelationalOperator: '<S32>/Relational Operator2' incorporates:

   *  Constant: '<S32>/CTRL_COMM2'

   *  Constant: '<S32>/t_errDequal'

   *  Constant: '<S32>/t_errQual'

   */

  Debounce_Filter(rtb_DataTypeConversion1_c != 0, 1600, 12000,

                  &rtb_RelationalOperator4_f, &rtDW->Debounce_Filter_i);



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



  /* Logic: '<S22>/Logical Operator12' incorporates:

   *  Inport: '<Root>/b_motEna'

   *  Logic: '<S22>/Logical Operator7'

   */

  rtb_n_commDeacv = ((!rtb_RelationalOperator4_f) && rtU->b_motEna);



  /* Logic: '<S22>/Logical Operator4' incorporates:

   *  Constant: '<S22>/constant8'

   *  Inport: '<Root>/n_ctrlModReq'

   *  Logic: '<S22>/Logical Operator11'

   *  Logic: '<S22>/Logical Operator8'

   *  RelationalOperator: '<S22>/Relational Operator10'

   *  S-Function (sfix_bitop): '<S4>/Bitwise Operator2'

   */

  rtb_LogicalOperator4 = (((uint16_T)tmp_2 != 0) || (!rtDW->Compare) ||

    (!rtb_n_commDeacv) || (rtU->n_ctrlModReq == 0));



  /* Relay: '<S22>/n_SpeedCtrl' */

  rtDW->n_SpeedCtrl_Mode = ((rtb_LogicalOperator3 >= 300) ||

    ((rtb_LogicalOperator3 > 200) && rtDW->n_SpeedCtrl_Mode));

  rtb_LogicalOperator_p = rtDW->n_SpeedCtrl_Mode;



  /* Logic: '<S22>/Logical Operator10' incorporates:

   *  Inport: '<Root>/b_cruiseEna'

   */

  rtb_LogicalOperator_p = (rtb_LogicalOperator_p && rtU->b_cruiseEna);



  /* Logic: '<S22>/Logical Operator2' incorporates:

   *  Constant: '<S22>/constant'

   *  Inport: '<Root>/n_ctrlModReq'

   *  Logic: '<S22>/Logical Operator5'

   *  RelationalOperator: '<S22>/Relational Operator4'

   */

  rtb_LogicalOperator2 = ((rtU->n_ctrlModReq == 2) && (!rtb_LogicalOperator_p));



  /* Logic: '<S22>/Logical Operator1' incorporates:

   *  Constant: '<S22>/constant1'

   *  Inport: '<Root>/n_ctrlModReq'

   *  RelationalOperator: '<S22>/Relational Operator1'

   */

  rtb_LogicalOperator_p = ((rtU->n_ctrlModReq == 1) || rtb_LogicalOperator_p);



  /* Chart: '<S4>/Control_Mode_Manager' incorporates:

   *  Logic: '<S22>/Logical Operator3'

   *  Logic: '<S22>/Logical Operator6'

   *  Logic: '<S22>/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: '<S23>/Switch' incorporates:

   *  Constant: '<S23>/Constant3'

   *  Inport: '<Root>/input_target'

   */

  if (rtU->input_target > 60) {

    /* Switch: '<S23>/Switch1' incorporates:

     *  Constant: '<S23>/Constant1'

     *  DataTypeConversion: '<S23>/Data Type Conversion'

     *  Switch: '<S23>/Switch'

     */

    if (rtb_n_commDeacv) {

      rtb_Switch_oi = rtU->input_target;

    } else {

      rtb_Switch_oi = 0;

    }



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

  } else {

    rtb_Switch_oi = 0;

  }



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



  /* Switch: '<S23>/Switch3' incorporates:

   *  Constant: '<S23>/Constant4'

   *  DataTypeConversion: '<S23>/Data Type Conversion2'

   *  Inport: '<Root>/vdq_open_target'

   */

  if (rtb_n_commDeacv) {

    rtb_r_cos_M1 = rtU->vdq_open_target[1];

  } else {

    rtb_r_cos_M1 = 0;

  }



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



  /* If: '<S24>/If' incorporates:

   *  DataTypeConversion: '<S24>/Data Type Conversion1'

   *  Inport: '<S25>/vq_in'

   *  S-Function (sfix_bitop): '<S4>/Bitwise Operator2'

   *  Switch: '<S23>/Switch3'

   */

  if ((uint16_T)tmp_2 == 1) {

    /* Switch: '<S23>/Switch2' incorporates:

     *  Constant: '<S23>/Constant2'

     *  DataTypeConversion: '<S23>/Data Type Conversion1'

     *  Inport: '<Root>/vdq_open_target'

     *  Inport: '<S25>/vd_in'

     */

    if (rtb_n_commDeacv) {

      /* Outputs for IfAction SubSystem: '<S24>/If Action Subsystem' incorporates:

       *  ActionPort: '<S25>/Action Port'

       */

      rtDW->Merge[0] = rtU->vdq_open_target[0];



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

    } else {

      /* Outputs for IfAction SubSystem: '<S24>/If Action Subsystem' incorporates:

       *  ActionPort: '<S25>/Action Port'

       */

      rtDW->Merge[0] = 0;



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

    }



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



    /* Outputs for IfAction SubSystem: '<S24>/If Action Subsystem' incorporates:

     *  ActionPort: '<S25>/Action Port'

     */

    rtDW->Merge[1] = rtb_r_cos_M1;



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

  } else if ((rtb_z_ctrlMod == 0) && rtb_Equal_k) {

    /* Outputs for IfAction SubSystem: '<S24>/open_mode' incorporates:

     *  ActionPort: '<S26>/Action Port'

     */

    /* RelationalOperator: '<S26>/Equal1' incorporates:

     *  Switch: '<S23>/Switch3'

     *  UnitDelay: '<S26>/Unit Delay'

     */

    rtb_LogicalOperator_p = (rtDW->UnitDelay_DSTATE_i != rtb_r_cos_M1);



    /* If: '<S28>/If' */

    if (rtb_LogicalOperator_p) {

      /* Outputs for IfAction SubSystem: '<S28>/Subsystem' incorporates:

       *  ActionPort: '<S30>/Action Port'

       */

      /* Sum: '<S30>/Add' incorporates:

       *  Signum: '<S30>/Sign'

       *  Switch: '<S23>/Switch3'

       *  UnitDelay: '<S6>/UnitDelay2'

       */

      rtb_Sign = (int16_T)((rtb_r_cos_M1 - rtDW->UnitDelay2_DSTATE_p) >> 2);



      /* Signum: '<S30>/Sign' */

      if (rtb_Sign < 0) {

        rtb_Sign = -1;

      } else {

        rtb_Sign = (int16_T)(rtb_Sign > 0);

      }



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



      /* Product: '<S30>/Divide' incorporates:

       *  Constant: '<S26>/Constant5'

       */

      rtDW->Divide = (int16_T)(rtb_Sign * 6);



      /* Switch: '<S30>/Switch' incorporates:

       *  Switch: '<S30>/Switch1'

       */

      if (rtb_Sign > 0) {

        /* Switch: '<S30>/Switch' incorporates:

         *  Switch: '<S23>/Switch3'

         */

        rtDW->Switch = rtb_r_cos_M1;



        /* Switch: '<S30>/Switch1' incorporates:

         *  UnitDelay: '<S6>/UnitDelay2'

         */

        rtDW->Switch1 = rtDW->UnitDelay2_DSTATE_p;

      } else {

        /* Switch: '<S30>/Switch' incorporates:

         *  UnitDelay: '<S6>/UnitDelay2'

         */

        rtDW->Switch = rtDW->UnitDelay2_DSTATE_p;



        /* Switch: '<S30>/Switch1' incorporates:

         *  Switch: '<S23>/Switch3'

         */

        rtDW->Switch1 = rtb_r_cos_M1;

      }



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

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



      /* Switch: '<S31>/Switch1' incorporates:

       *  UnitDelay: '<S6>/UnitDelay2'

       */

      rtb_Sign = rtDW->UnitDelay2_DSTATE_p;

    } else {

      /* Switch: '<S31>/Switch1' incorporates:

       *  UnitDelay: '<S31>/UnitDelay'

       */

      rtb_Sign = rtDW->UnitDelay_DSTATE_d;

    }



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



    /* Sum: '<S28>/Add2' incorporates:

     *  Product: '<S30>/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: '<S26>/Switch' incorporates:

     *  Switch: '<S23>/Switch'

     */

    if (rtb_Switch_oi > 0) {

      /* Switch: '<S29>/Switch2' incorporates:

       *  RelationalOperator: '<S29>/LowerRelop1'

       *  RelationalOperator: '<S29>/UpperRelop'

       *  Sum: '<S28>/Add2'

       *  Switch: '<S29>/Switch'

       *  Switch: '<S30>/Switch'

       *  Switch: '<S30>/Switch1'

       */

      if ((int16_T)tmp_2 > rtDW->Switch) {

        /* Merge: '<S24>/Merge' incorporates:

         *  Switch: '<S26>/Switch'

         */

        rtDW->Merge[1] = rtDW->Switch;

      } else if ((int16_T)tmp_2 < rtDW->Switch1) {

        /* Merge: '<S24>/Merge' incorporates:

         *  Switch: '<S26>/Switch'

         *  Switch: '<S29>/Switch'

         *  Switch: '<S30>/Switch1'

         */

        rtDW->Merge[1] = rtDW->Switch1;

      } else {

        /* Merge: '<S24>/Merge' incorporates:

         *  Switch: '<S26>/Switch'

         */

        rtDW->Merge[1] = (int16_T)tmp_2;

      }



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

    } else {

      /* Merge: '<S24>/Merge' incorporates:

       *  Constant: '<S26>/Constant1'

       */

      rtDW->Merge[1] = 0;

    }



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



    /* Merge: '<S24>/Merge' incorporates:

     *  Constant: '<S26>/Constant3'

     *  SignalConversion generated from: '<S26>/open_voltage'

     */

    rtDW->Merge[0] = 0;



    /* Update for UnitDelay: '<S26>/Unit Delay' incorporates:

     *  Switch: '<S23>/Switch3'

     */

    rtDW->UnitDelay_DSTATE_i = rtb_r_cos_M1;



    /* Switch: '<S31>/Switch2' */

    if (rtb_LogicalOperator_p) {

      /* Update for UnitDelay: '<S31>/UnitDelay' incorporates:

       *  UnitDelay: '<S6>/UnitDelay2'

       */

      rtDW->UnitDelay_DSTATE_d = rtDW->UnitDelay2_DSTATE_p;

    } else {

      /* Update for UnitDelay: '<S31>/UnitDelay' incorporates:

       *  Sum: '<S28>/Add2'

       */

      rtDW->UnitDelay_DSTATE_d = (int16_T)tmp_2;

    }



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

    /* End of Outputs for SubSystem: '<S24>/open_mode' */

  } else {

    if (rtb_z_ctrlMod == 2) {

      /* Outputs for IfAction SubSystem: '<S24>/torque_mode' incorporates:

       *  ActionPort: '<S27>/Action Port'

       */

      /* Switch: '<S27>/Switch' incorporates:

       *  Constant: '<S27>/Constant'

       *  Inport: '<Root>/foc_calibrate'

       *  Inport: '<Root>/i_dc_limit'

       *  Inport: '<Root>/speed_limit'

       *  Logic: '<S27>/Logical Operator'

       *  Product: '<S27>/Divide4'

       *  S-Function (sfix_bitop): '<S4>/Bitwise Operator1'

       */

      if ((rtU->foc_calibrate & 2U) == 0U) {

        rtb_LogicalOperator3 = (uint16_T)((rtU->i_dc_limit << 8) /

          rtU->speed_limit);

      } else {

        rtb_LogicalOperator3 = 4096U;

      }



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



      /* Product: '<S27>/Divide1' incorporates:

       *  Switch: '<S23>/Switch'

       *  Switch: '<S27>/Switch'

       */

      tmp_2 = (rtb_Switch_oi * rtb_LogicalOperator3) >> 8;

      if (tmp_2 > 32767) {

        tmp_2 = 32767;

      } else {

        if (tmp_2 < -32768) {

          tmp_2 = -32768;

        }

      }



      /* Product: '<S27>/Divide1' */

      rtDW->Divide1 = (int16_T)tmp_2;



      /* End of Outputs for SubSystem: '<S24>/torque_mode' */

    }

  }



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



  /* Outputs for Atomic SubSystem: '<S32>/either_edge' */

  rtb_LogicalOperator_p = either_edge(rtb_RelationalOperator4_f,

    &rtDW->either_edge_f);



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



  /* Switch: '<S32>/Switch1' */

  if (rtb_LogicalOperator_p) {

    rtb_UnitDelay_bc = rtb_DataTypeConversion1_c;

  }



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



  /* Gain: '<S49>/Multiply' incorporates:

   *  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;

    }

  }



  tmp_0 = (12351 * rtU->adc_b) >> 11;

  if (tmp_0 > 32767) {

    tmp_0 = 32767;

  } else {

    if (tmp_0 < -32768) {

      tmp_0 = -32768;

    }

  }



  /* Sum: '<S43>/Add' incorporates:

   *  Gain: '<S49>/Multiply'

   */

  rtb_Sum1 = (int16_T)tmp_2 + (int16_T)tmp_0;

  if (rtb_Sum1 > 32767) {

    rtb_Sum1 = 32767;

  } else {

    if (rtb_Sum1 < -32768) {

      rtb_Sum1 = -32768;

    }

  }



  /* Sum: '<S43>/Add1' incorporates:

   *  Sum: '<S43>/Add'

   */

  rtb_Saturation = -rtb_Sum1;

  if (-rtb_Sum1 > 32767) {

    rtb_Saturation = 32767;

  }



  /* Sum: '<S52>/Add3' incorporates:

   *  Gain: '<S49>/Multiply'

   *  Sum: '<S43>/Add1'

   */

  rtb_Sum1 = (int16_T)tmp_0 + (int16_T)rtb_Saturation;

  if (rtb_Sum1 > 32767) {

    rtb_Sum1 = 32767;

  } else {

    if (rtb_Sum1 < -32768) {

      rtb_Sum1 = -32768;

    }

  }



  /* Sum: '<S52>/Add' incorporates:

   *  Gain: '<S49>/Multiply'

   *  Sum: '<S52>/Add3'

   */

  tmp_2 = (((int16_T)tmp_2 << 1) - rtb_Sum1) >> 1;

  if (tmp_2 > 32767) {

    tmp_2 = 32767;

  } else {

    if (tmp_2 < -32768) {

      tmp_2 = -32768;

    }

  }



  /* Gain: '<S52>/Gain1' incorporates:

   *  Product: '<S54>/Divide1'

   *  Sum: '<S52>/Add'

   */

  rtb_Sign = (int16_T)((21845 * tmp_2) >> 15);



  /* Gain: '<S52>/Gain2' incorporates:

   *  Gain: '<S49>/Multiply'

   *  Sum: '<S43>/Add1'

   *  Sum: '<S52>/Add2'

   */

  tmp_2 = ((int16_T)(((int16_T)tmp_0 - (int16_T)rtb_Saturation) >> 1) * 18919) >>

    14;

  if (tmp_2 > 32767) {

    tmp_2 = 32767;

  } else {

    if (tmp_2 < -32768) {

      tmp_2 = -32768;

    }

  }



  /* PreLookup: '<S55>/a_elecAngle_XA' incorporates:

   *  Sum: '<S3>/Sum'

   */

  rtb_LogicalOperator3 = plook_u16s16_evencka(rtb_Sum, 0, 4U, 1440U);



  /* Sum: '<S54>/Sum1' incorporates:

   *  Gain: '<S52>/Gain2'

   *  Interpolation_n-D: '<S55>/r_cos_M1'

   *  Interpolation_n-D: '<S55>/r_sin_M1'

   *  Product: '<S54>/Divide1'

   *  Product: '<S54>/Divide2'

   *  Product: '<S54>/Divide3'

   */

  tmp_0 = (int16_T)((rtb_Sign * rtConstP.r_cos_M1_Table[rtb_LogicalOperator3]) >>

                    14) + (int16_T)(((int16_T)tmp_2 *

    rtConstP.r_sin_M1_Table[rtb_LogicalOperator3]) >> 14);

  if (tmp_0 > 32767) {

    tmp_0 = 32767;

  } else {

    if (tmp_0 < -32768) {

      tmp_0 = -32768;

    }

  }



  /* SignalConversion generated from: '<S43>/Low_Pass_Filter' incorporates:

   *  Sum: '<S54>/Sum1'

   */

  rtb_Switch_m[0] = (int16_T)tmp_0;



  /* Sum: '<S54>/Sum6' incorporates:

   *  Gain: '<S52>/Gain2'

   *  Interpolation_n-D: '<S55>/r_cos_M1'

   *  Interpolation_n-D: '<S55>/r_sin_M1'

   *  Product: '<S54>/Divide1'

   *  Product: '<S54>/Divide4'

   */

  tmp_2 = (int16_T)(((int16_T)tmp_2 *

                     rtConstP.r_cos_M1_Table[rtb_LogicalOperator3]) >> 14) -

    (int16_T)((rtb_Sign * rtConstP.r_sin_M1_Table[rtb_LogicalOperator3]) >> 14);

  if (tmp_2 > 32767) {

    tmp_2 = 32767;

  } else {

    if (tmp_2 < -32768) {

      tmp_2 = -32768;

    }

  }



  /* SignalConversion generated from: '<S43>/Low_Pass_Filter' incorporates:

   *  Sum: '<S54>/Sum6'

   */

  rtb_Switch_m[1] = (int16_T)tmp_2;



  /* Outputs for Atomic SubSystem: '<S43>/Low_Pass_Filter' */

  /* Constant: '<S43>/Constant' */

  Low_Pass_Filter(rtb_Switch_m, 26214, rtb_DataTypeConversion_k1,

                  &rtDW->Low_Pass_Filter_d);



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



  /* Switch: '<S51>/Switch2' */

  rtb_Switch2_fu = (uint8_T)(rtb_z_ctrlMod != 0);



  /* DataTypeConversion: '<S45>/Data Type Conversion' incorporates:

   *  Logic: '<S45>/Logical Operator'

   *  RelationalOperator: '<S45>/Equal'

   *  UnitDelay: '<S45>/Unit Delay'

   */

  rtb_DataTypeConversion_i = (uint8_T)((rtb_Switch2_fu != 0) &&

    (rtDW->UnitDelay_DSTATE_b != rtb_Switch2_fu));



  /* Delay: '<S81>/Resettable Delay' incorporates:

   *  DataTypeConversion: '<S81>/Data Type Conversion2'

   */

  if ((rtb_Switch2_fu > 0) && (rtPrevZCX->ResettableDelay_Reset_ZCE != 1)) {

    rtDW->icLoad = 1U;

  }



  rtPrevZCX->ResettableDelay_Reset_ZCE = (ZCSigState)(rtb_Switch2_fu > 0);

  if (rtDW->icLoad != 0) {

    rtDW->ResettableDelay_DSTATE = 0;

  }



  /* Sum: '<S81>/Sum1' incorporates:

   *  Constant: '<S78>/Constant3'

   *  Delay: '<S81>/Resettable Delay'

   *  Gain: '<S78>/Gain'

   *  Gain: '<S78>/Gain1'

   *  Sum: '<S78>/Sum'

   *  Sum: '<S78>/Sum4'

   *  UnitDelay: '<S50>/Unit Delay'

   *  UnitDelay: '<S78>/Unit Delay'

   */

  rtb_Sum1 = ((((int16_T)((15565 - (rtDW->UnitDelay_DSTATE_e << 2)) >> 2) * 6711)

               >> 31) + (int32_T)((1374389535LL * rtDW->UnitDelay_DSTATE) >> 37))

    + rtDW->ResettableDelay_DSTATE;



  /* Saturate: '<S78>/Saturation' incorporates:

   *  Sum: '<S81>/Sum1'

   */

  if (rtb_Sum1 > 0) {

    rtb_Saturation = 0;

  } else if (rtb_Sum1 < -1920) {

    rtb_Saturation = -1920;

  } else {

    rtb_Saturation = rtb_Sum1;

  }



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



  /* DataTypeConversion: '<S51>/Data Type Conversion1' incorporates:

   *  Logic: '<S51>/Logical Operator'

   */

  rtb_DataTypeConversion1_c = (uint8_T)((rtb_Switch2_fu != 0) && rtb_Equal_k);



  /* If: '<S80>/If' incorporates:

   *  Constant: '<S82>/Constant1'

   *  Constant: '<S82>/Constant11'

   *  Constant: '<S82>/Constant2'

   *  Constant: '<S82>/Constant4'

   *  Gain: '<S44>/Gain1'

   *  Inport: '<Root>/i_dc_limit'

   *  Sum: '<S82>/Add2'

   *  Switch: '<S12>/Switch2'

   *  Switch: '<S87>/Switch2'

   */

  if ((rtb_DataTypeConversion1_c > 0) && (rtb_z_ctrlMod == 1)) {

    /* Outputs for IfAction SubSystem: '<S80>/speed_mode' incorporates:

     *  ActionPort: '<S82>/Action Port'

     */

    /* Switch: '<S84>/Switch2' incorporates:

     *  Inport: '<Root>/speed_limit'

     *  RelationalOperator: '<S84>/LowerRelop1'

     *  RelationalOperator: '<S84>/UpperRelop'

     *  Switch: '<S23>/Switch'

     *  Switch: '<S84>/Switch'

     *  Switch: '<S87>/Switch2'

     */

    if (rtb_Switch_oi > rtU->speed_limit) {

      rtb_Switch_oi = rtU->speed_limit;

    } else {

      if (rtb_Switch_oi < 0) {

        /* Switch: '<S84>/Switch' incorporates:

         *  Constant: '<S82>/Constant5'

         *  Switch: '<S87>/Switch2'

         */

        rtb_Switch_oi = 0;

      }

    }



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



    /* Outputs for Atomic SubSystem: '<S82>/pi_speed' */

    rtb_Switch_oi = pi_speed((int16_T)(rtb_Switch_oi - rtb_Switch2_c), 3174, 10,

      20, rtU->i_dc_limit, (int16_T)-rtU->i_dc_limit, 0, rtb_Switch2_fu,

      &rtConstB.pi_speed_d, &rtDW->pi_speed_d, &rtPrevZCX->pi_speed_d);



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



    /* Merge: '<S80>/Merge' incorporates:

     *  Constant: '<S82>/Constant1'

     *  Constant: '<S82>/Constant11'

     *  Constant: '<S82>/Constant2'

     *  Constant: '<S82>/Constant4'

     *  Gain: '<S44>/Gain1'

     *  Inport: '<Root>/i_dc_limit'

     *  SignalConversion generated from: '<S82>/idq_target'

     *  Sum: '<S82>/Add2'

     *  Switch: '<S12>/Switch2'

     *  Switch: '<S87>/Switch2'

     */

    rtDW->Merge_f = rtb_Switch_oi;



    /* End of Outputs for SubSystem: '<S80>/speed_mode' */

  } else {

    if ((rtb_DataTypeConversion1_c > 0) && (rtb_z_ctrlMod == 2)) {

      /* Outputs for IfAction SubSystem: '<S80>/torque_mode' incorporates:

       *  ActionPort: '<S83>/Action Port'

       */

      /* Product: '<S83>/Divide' incorporates:

       *  Constant: '<S83>/Constant2'

       *  Sum: '<S83>/Sum2'

       *  Switch: '<S12>/Switch2'

       *  Switch: '<S23>/Switch'

       */

      tmp_2 = ((int16_T)(rtb_Switch_oi - rtb_Switch2_c) * 819) >> 6;

      if (tmp_2 > 32767) {

        tmp_2 = 32767;

      } else {

        if (tmp_2 < -32768) {

          tmp_2 = -32768;

        }

      }



      /* Product: '<S83>/Divide1' incorporates:

       *  Sum: '<S83>/Sum3'

       *  Switch: '<S12>/Switch2'

       *  Switch: '<S23>/Switch'

       */

      tmp_0 = ((int16_T)(rtb_Switch2_c - rtb_Switch_oi) * -51) >> 5;

      if (tmp_0 > 32767) {

        tmp_0 = 32767;

      } else {

        if (tmp_0 < -32768) {

          tmp_0 = -32768;

        }

      }



      rtb_Switch_oi = (int16_T)tmp_0;



      /* End of Product: '<S83>/Divide1' */



      /* MinMax: '<S83>/Max' incorporates:

       *  Product: '<S83>/Divide'

       *  Product: '<S83>/Divide1'

       */

      if ((int16_T)tmp_2 > rtb_Switch_oi) {

        rtb_r_cos_M1 = (int16_T)tmp_2;

      } else {

        rtb_r_cos_M1 = rtb_Switch_oi;

      }



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



      /* MinMax: '<S83>/Max3' incorporates:

       *  Inport: '<Root>/i_dc_limit'

       *  MinMax: '<S83>/Max'

       *  Switch: '<S88>/Switch2'

       */

      if (rtU->i_dc_limit < rtb_r_cos_M1) {

        rtb_r_cos_M1 = rtU->i_dc_limit;

      }



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



      /* Switch: '<S88>/Switch2' incorporates:

       *  Product: '<S27>/Divide1'

       *  RelationalOperator: '<S88>/LowerRelop1'

       */

      if (rtDW->Divide1 <= rtb_r_cos_M1) {

        /* MinMax: '<S83>/Max1' incorporates:

         *  Product: '<S83>/Divide'

         *  Product: '<S83>/Divide1'

         */

        if ((int16_T)tmp_2 < rtb_Switch_oi) {

          rtb_Switch_oi = (int16_T)tmp_2;

        }



        /* End of MinMax: '<S83>/Max1' */



        /* MinMax: '<S83>/Max2' incorporates:

         *  Gain: '<S44>/Gain1'

         *  Inport: '<Root>/i_dc_limit'

         *  MinMax: '<S83>/Max1'

         */

        if (rtb_Switch_oi <= (int16_T)-rtU->i_dc_limit) {

          rtb_Switch_oi = (int16_T)-rtU->i_dc_limit;

        }



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



        /* Switch: '<S88>/Switch' incorporates:

         *  MinMax: '<S83>/Max2'

         *  RelationalOperator: '<S88>/UpperRelop'

         */

        if (rtDW->Divide1 < rtb_Switch_oi) {

          rtb_r_cos_M1 = rtb_Switch_oi;

        } else {

          rtb_r_cos_M1 = rtDW->Divide1;

        }



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

      }



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



      /* Merge: '<S80>/Merge' incorporates:

       *  SignalConversion generated from: '<S83>/idq_target'

       *  Switch: '<S88>/Switch2'

       */

      rtDW->Merge_f = rtb_r_cos_M1;



      /* End of Outputs for SubSystem: '<S80>/torque_mode' */

    }

  }



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



  /* Math: '<S78>/Math Function2' incorporates:

   *  Saturate: '<S78>/Saturation'

   *  Sum: '<S78>/Sum1'

   */

  tmp = ((int64_T)rtb_Saturation * rtb_Saturation) >> 6;

  if (tmp > 2147483647LL) {

    tmp = 2147483647LL;

  } else {

    if (tmp < -2147483648LL) {

      tmp = -2147483648LL;

    }

  }



  /* Sqrt: '<S78>/Sqrt' incorporates:

   *  Gain: '<S66>/Gain'

   *  Math: '<S78>/Math Function1'

   *  Math: '<S78>/Math Function2'

   *  Sum: '<S78>/Sum2'

   *  Switch: '<S79>/Switch'

   */

  rtb_Gain = rt_sqrt_Us32En6_Ys32En_dnD5ZXjs(((rtDW->Merge_f * rtDW->Merge_f) >>

    6) - (int32_T)tmp);



  /* If: '<S45>/If' incorporates:

   *  Constant: '<S56>/Constant3'

   *  Constant: '<S56>/Constant4'

   *  Constant: '<S56>/Constant6'

   *  Constant: '<S56>/Constant9'

   *  Constant: '<S57>/Constant1'

   *  Constant: '<S57>/Constant7'

   *  Constant: '<S57>/Constant8'

   *  Constant: '<S57>/Constant9'

   *  Gain: '<S44>/Gain3'

   *  Gain: '<S44>/Gain5'

   *  If: '<S45>/If1'

   *  Inport: '<Root>/vbus_voltage'

   *  Sum: '<S56>/Add'

   *  Sum: '<S57>/Add1'

   *  Switch: '<S59>/Switch2'

   *  Switch: '<S63>/Switch2'

   */

  if (rtb_Switch2_fu == 1) {

    /* Outputs for IfAction SubSystem: '<S45>/iq_ctrl' incorporates:

     *  ActionPort: '<S57>/Action Port'

     */

    /* Switch: '<S63>/Switch2' incorporates:

     *  DataTypeConversion: '<S78>/Data Type Conversion'

     *  Gain: '<S44>/Gain1'

     *  Inport: '<Root>/i_dc_limit'

     *  RelationalOperator: '<S63>/LowerRelop1'

     *  RelationalOperator: '<S63>/UpperRelop'

     *  Switch: '<S63>/Switch'

     */

    if ((int16_T)rtb_Gain > rtU->i_dc_limit) {

      rtb_Switch_oi = rtU->i_dc_limit;

    } else if ((int16_T)rtb_Gain < (int16_T)-rtU->i_dc_limit) {

      /* Switch: '<S63>/Switch' incorporates:

       *  Gain: '<S44>/Gain1'

       *  Switch: '<S63>/Switch2'

       */

      rtb_Switch_oi = (int16_T)-rtU->i_dc_limit;

    } else {

      rtb_Switch_oi = (int16_T)rtb_Gain;

    }



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



    /* Outputs for Atomic SubSystem: '<S57>/PI_iq' */

    PI_backCalc_fixdt((int16_T)(rtb_Switch_oi - rtb_DataTypeConversion_k1[1]),

                      4096, 51, 1024, rtU->vbus_voltage, (int16_T)

                      -rtU->vbus_voltage, 0, rtb_DataTypeConversion_i,

                      &rtDW->Switch2_d, &rtConstB.PI_iq, &rtDW->PI_iq,

                      &rtPrevZCX->PI_iq);



    /* End of Outputs for SubSystem: '<S57>/PI_iq' */

    /* End of Outputs for SubSystem: '<S45>/iq_ctrl' */



    /* Outputs for IfAction SubSystem: '<S45>/id_ctrl' incorporates:

     *  ActionPort: '<S56>/Action Port'

     */

    /* Switch: '<S59>/Switch2' incorporates:

     *  Constant: '<S57>/Constant1'

     *  Constant: '<S57>/Constant7'

     *  Constant: '<S57>/Constant8'

     *  Constant: '<S57>/Constant9'

     *  DataTypeConversion: '<S78>/Data Type Conversion'

     *  Gain: '<S44>/Gain4'

     *  Gain: '<S44>/Gain5'

     *  Inport: '<Root>/i_dc_limit'

     *  Inport: '<Root>/vbus_voltage'

     *  RelationalOperator: '<S59>/LowerRelop1'

     *  RelationalOperator: '<S59>/UpperRelop'

     *  Saturate: '<S78>/Saturation'

     *  Sum: '<S57>/Add1'

     *  Sum: '<S78>/Sum1'

     *  Switch: '<S59>/Switch'

     *  Switch: '<S63>/Switch2'

     */

    if ((int16_T)rtb_Saturation > rtU->i_dc_limit) {

      rtb_Switch_oi = rtU->i_dc_limit;

    } else if ((int16_T)rtb_Saturation < (int16_T)-rtU->i_dc_limit) {

      /* Switch: '<S59>/Switch' incorporates:

       *  Gain: '<S44>/Gain4'

       *  Switch: '<S59>/Switch2'

       */

      rtb_Switch_oi = (int16_T)-rtU->i_dc_limit;

    } else {

      rtb_Switch_oi = (int16_T)rtb_Saturation;

    }



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



    /* Outputs for Atomic SubSystem: '<S56>/PI_id' */

    PI_backCalc_fixdt((int16_T)(rtb_Switch_oi - rtb_DataTypeConversion_k1[0]),

                      4096, 51, 1024, rtU->vbus_voltage, (int16_T)

                      -rtU->vbus_voltage, 0, rtb_DataTypeConversion_i,

                      &rtDW->Switch2, &rtConstB.PI_id, &rtDW->PI_id,

                      &rtPrevZCX->PI_id);



    /* End of Outputs for SubSystem: '<S56>/PI_id' */

    /* End of Outputs for SubSystem: '<S45>/id_ctrl' */

  }



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



  /* Switch: '<S6>/Switch' incorporates:

   *  Merge: '<S24>/Merge'

   */

  if (rtb_z_ctrlMod != 0) {

    rtb_Switch_m[0] = rtDW->Switch2;

    rtb_Switch_m[1] = rtDW->Switch2_d;

  } else {

    rtb_Switch_m[0] = rtDW->Merge[0];

    rtb_Switch_m[1] = rtDW->Merge[1];

  }



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



  /* Gain: '<S48>/Gain' incorporates:

   *  Inport: '<Root>/vbus_voltage'

   *  Product: '<S77>/Product'

   */

  rtb_Switch_oi = (int16_T)((15565 * rtU->vbus_voltage) >> 14);



  /* Product: '<S48>/Divide' incorporates:

   *  Math: '<S48>/Math Function'

   *  Math: '<S48>/Math Function1'

   *  Product: '<S77>/Product'

   *  Sum: '<S48>/Sum of Elements'

   *  Switch: '<S6>/Switch'

   */

  tmp = ((int64_T)(((rtb_Switch_m[0] * rtb_Switch_m[0]) >> 6) + ((rtb_Switch_m[1]

            * rtb_Switch_m[1]) >> 6)) << 12) / ((rtb_Switch_oi * rtb_Switch_oi) >>

    6);

  if (tmp > 32767LL) {

    tmp = 32767LL;

  } else {

    if (tmp < -32768LL) {

      tmp = -32768LL;

    }

  }



  /* Sqrt: '<S48>/Sqrt' incorporates:

   *  Product: '<S48>/Divide'

   */

  rtb_Switch_oi = rt_sqrt_Us16En12_Ys16E_cQn1iwAF((int16_T)tmp);



  /* Switch: '<S48>/Switch' incorporates:

   *  RelationalOperator: '<S76>/Compare'

   *  Sqrt: '<S48>/Sqrt'

   */

  if (rtb_Switch_oi > 4096) {

    /* Switch: '<S48>/Switch' incorporates:

     *  Product: '<S48>/Divide1'

     *  Switch: '<S6>/Switch'

     */

    rtb_Switch_m[0] = (int16_T)div_nde_s32_floor(rtb_Switch_m[0] << 12,

      rtb_Switch_oi);

    rtb_Switch_m[1] = (int16_T)div_nde_s32_floor(rtb_Switch_m[1] << 12,

      rtb_Switch_oi);

  }



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



  /* Sum: '<S46>/Sum1' incorporates:

   *  Interpolation_n-D: '<S55>/r_cos_M1'

   *  Interpolation_n-D: '<S55>/r_sin_M1'

   *  Product: '<S46>/Divide2'

   *  Product: '<S46>/Divide3'

   */

  tmp_2 = (int16_T)((rtb_Switch_m[0] *

                     rtConstP.r_sin_M1_Table[rtb_LogicalOperator3]) >> 14) +

    (int16_T)((rtb_Switch_m[1] * rtConstP.r_cos_M1_Table[rtb_LogicalOperator3]) >>

              14);

  if (tmp_2 > 32767) {

    tmp_2 = 32767;

  } else {

    if (tmp_2 < -32768) {

      tmp_2 = -32768;

    }

  }



  /* Sum: '<S46>/Sum6' incorporates:

   *  Interpolation_n-D: '<S55>/r_cos_M1'

   *  Interpolation_n-D: '<S55>/r_sin_M1'

   *  Product: '<S46>/Divide1'

   *  Product: '<S46>/Divide4'

   */

  tmp_0 = (int16_T)((rtb_Switch_m[0] *

                     rtConstP.r_cos_M1_Table[rtb_LogicalOperator3]) >> 14) -

    (int16_T)((rtb_Switch_m[1] * rtConstP.r_sin_M1_Table[rtb_LogicalOperator3]) >>

              14);

  if (tmp_0 > 32767) {

    tmp_0 = 32767;

  } else {

    if (tmp_0 < -32768) {

      tmp_0 = -32768;

    }

  }



  /* Product: '<S66>/Divide7' incorporates:

   *  Constant: '<S66>/Constant3'

   *  Sum: '<S46>/Sum1'

   */

  rtb_Switch_dr = (int16_T)((2365 * (int16_T)tmp_2) >> 12);



  /* MATLAB Function: '<S66>/sector_select' incorporates:

   *  Product: '<S66>/Divide7'

   *  Sum: '<S46>/Sum1'

   *  Sum: '<S46>/Sum6'

   */

  if ((int16_T)tmp_2 >= 0) {

    if ((int16_T)tmp_0 >= 0) {

      if (rtb_Switch_dr > (int16_T)tmp_0) {

        /* DataTypeConversion: '<S66>/Data Type Conversion' */

        rtb_DataTypeConversion1_c = 2U;

      } else {

        /* DataTypeConversion: '<S66>/Data Type Conversion' */

        rtb_DataTypeConversion1_c = 1U;

      }

    } else if (-rtb_Switch_dr > (int16_T)tmp_0) {

      /* DataTypeConversion: '<S66>/Data Type Conversion' */

      rtb_DataTypeConversion1_c = 3U;

    } else {

      /* DataTypeConversion: '<S66>/Data Type Conversion' */

      rtb_DataTypeConversion1_c = 2U;

    }

  } else if ((int16_T)tmp_0 >= 0) {

    if (-rtb_Switch_dr > (int16_T)tmp_0) {

      /* DataTypeConversion: '<S66>/Data Type Conversion' */

      rtb_DataTypeConversion1_c = 5U;

    } else {

      /* DataTypeConversion: '<S66>/Data Type Conversion' */

      rtb_DataTypeConversion1_c = 6U;

    }

  } else if (rtb_Switch_dr > (int16_T)tmp_0) {

    /* DataTypeConversion: '<S66>/Data Type Conversion' */

    rtb_DataTypeConversion1_c = 4U;

  } else {

    /* DataTypeConversion: '<S66>/Data Type Conversion' */

    rtb_DataTypeConversion1_c = 5U;

  }



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



  /* Gain: '<S66>/Gain' incorporates:

   *  Inport: '<Root>/vbus_voltage'

   */

  rtb_Gain = 18919 * rtU->vbus_voltage;



  /* Product: '<S66>/Divide' incorporates:

   *  Gain: '<S66>/Gain'

   *  Sum: '<S46>/Sum6'

   */

  rtb_Sign = (int16_T)(((int64_T)(int16_T)tmp_0 << 26) / rtb_Gain);



  /* Product: '<S66>/Divide1' incorporates:

   *  Gain: '<S66>/Gain'

   *  Sum: '<S46>/Sum1'

   */

  rtb_r_cos_M1 = (int16_T)(((int64_T)(int16_T)tmp_2 << 26) / rtb_Gain);



  /* MultiPortSwitch: '<S68>/Multiport Switch' incorporates:

   *  DataTypeConversion: '<S66>/Data Type Conversion1'

   *  Sum: '<S70>/Add4'

   *  Sum: '<S71>/Add4'

   *  Sum: '<S72>/Add4'

   *  Sum: '<S73>/Add4'

   *  Sum: '<S74>/Add4'

   *  Sum: '<S75>/Add4'

   */

  switch (rtb_DataTypeConversion1_c) {

   case 1:

    /* Product: '<S70>/Divide3' incorporates:

     *  Product: '<S66>/Divide1'

     *  Product: '<S70>/Divide2'

     */

    rtb_Switch_dr = (int16_T)(((int16_T)((rtb_r_cos_M1 * 9459) >> 13) * 375) >>

      9);



    /* Product: '<S70>/Divide1' incorporates:

     *  Constant: '<S70>/Constant'

     *  Product: '<S66>/Divide'

     *  Product: '<S66>/Divide1'

     *  Product: '<S70>/Divide'

     *  Sum: '<S70>/Add'

     */

    rtb_r_cos_M1 = (int16_T)(((int16_T)(rtb_Sign - ((rtb_r_cos_M1 * 9459) >> 14))

      * 375) >> 9);



    /* Product: '<S70>/Divide4' incorporates:

     *  Sum: '<S70>/Add1'

     *  Sum: '<S70>/Add2'

     */

    rtb_Sign = (int16_T)((int16_T)(3000 - (int16_T)(rtb_r_cos_M1 + rtb_Switch_dr))

                         >> 1);



    /* Sum: '<S70>/Add3' */

    rtb_Switch_dr += rtb_Sign;

    rtb_MultiportSwitch_idx_0 = (int16_T)(rtb_Switch_dr + rtb_r_cos_M1);

    rtb_MultiportSwitch_idx_1 = rtb_Switch_dr;

    rtb_Switch_dr = rtb_Sign;

    break;



   case 2:

    /* Product: '<S71>/Divide1' incorporates:

     *  Constant: '<S71>/Constant'

     *  Product: '<S66>/Divide'

     *  Product: '<S66>/Divide1'

     *  Product: '<S71>/Divide'

     *  Sum: '<S71>/Add'

     */

    rtb_Switch_dr = (int16_T)(((int16_T)(((rtb_r_cos_M1 * 9459) >> 14) +

      rtb_Sign) * 375) >> 9);



    /* Product: '<S71>/Divide3' incorporates:

     *  Constant: '<S71>/Constant'

     *  Product: '<S66>/Divide'

     *  Product: '<S66>/Divide1'

     *  Product: '<S71>/Divide2'

     *  Sum: '<S71>/Add5'

     */

    rtb_r_cos_M1 = (int16_T)(((int16_T)(((rtb_r_cos_M1 * 9459) >> 14) - rtb_Sign)

      * 375) >> 9);



    /* Product: '<S71>/Divide4' incorporates:

     *  Sum: '<S71>/Add1'

     *  Sum: '<S71>/Add2'

     */

    rtb_Sign = (int16_T)((int16_T)(3000 - (int16_T)(rtb_r_cos_M1 + rtb_Switch_dr))

                         >> 1);



    /* Sum: '<S71>/Add3' */

    rtb_Switch_dr += rtb_Sign;

    rtb_MultiportSwitch_idx_0 = rtb_Switch_dr;

    rtb_MultiportSwitch_idx_1 = (int16_T)(rtb_Switch_dr + rtb_r_cos_M1);

    rtb_Switch_dr = rtb_Sign;

    break;



   case 3:

    /* Product: '<S72>/Divide1' incorporates:

     *  Constant: '<S72>/Constant'

     *  Product: '<S66>/Divide'

     *  Product: '<S66>/Divide1'

     *  Product: '<S72>/Divide'

     *  Sum: '<S72>/Add'

     */

    rtb_Sign = (int16_T)(((int16_T)(-rtb_Sign - ((rtb_r_cos_M1 * 9459) >> 14)) *

                          375) >> 9);



    /* Product: '<S72>/Divide3' incorporates:

     *  Product: '<S66>/Divide1'

     *  Product: '<S72>/Divide2'

     */

    rtb_r_cos_M1 = (int16_T)(((int16_T)((rtb_r_cos_M1 * 9459) >> 13) * 375) >> 9);



    /* Product: '<S72>/Divide4' incorporates:

     *  Sum: '<S72>/Add1'

     *  Sum: '<S72>/Add2'

     */

    rtb_Switch_dr = (int16_T)((int16_T)(3000 - (int16_T)(rtb_r_cos_M1 + rtb_Sign))

      >> 1);



    /* Sum: '<S72>/Add3' */

    rtb_Sign += rtb_Switch_dr;

    rtb_MultiportSwitch_idx_0 = rtb_Switch_dr;

    rtb_MultiportSwitch_idx_1 = (int16_T)(rtb_Sign + rtb_r_cos_M1);

    rtb_Switch_dr = rtb_Sign;

    break;



   case 4:

    /* Product: '<S73>/Divide1' incorporates:

     *  Constant: '<S73>/Constant'

     *  Product: '<S66>/Divide'

     *  Product: '<S66>/Divide1'

     *  Product: '<S73>/Divide'

     *  Sum: '<S73>/Add'

     */

    rtb_Sign = (int16_T)(((int16_T)(((rtb_r_cos_M1 * 9459) >> 14) - rtb_Sign) *

                          375) >> 9);



    /* Product: '<S73>/Divide3' incorporates:

     *  Product: '<S66>/Divide1'

     *  Product: '<S73>/Divide2'

     *  Sum: '<S73>/Add5'

     */

    rtb_r_cos_M1 = (int16_T)(((int16_T)(-((int16_T)((rtb_r_cos_M1 * 9459) >> 13)

      << 2) >> 2) * 375) >> 9);



    /* Product: '<S73>/Divide4' incorporates:

     *  Sum: '<S73>/Add1'

     *  Sum: '<S73>/Add2'

     */

    rtb_Switch_dr = (int16_T)((int16_T)(3000 - (int16_T)(rtb_r_cos_M1 + rtb_Sign))

      >> 1);



    /* Sum: '<S73>/Add3' */

    rtb_Sign += rtb_Switch_dr;

    rtb_MultiportSwitch_idx_0 = rtb_Switch_dr;

    rtb_MultiportSwitch_idx_1 = rtb_Sign;

    rtb_Switch_dr = (int16_T)(rtb_Sign + rtb_r_cos_M1);

    break;



   case 5:

    /* Product: '<S74>/Divide3' incorporates:

     *  Constant: '<S74>/Constant'

     *  Product: '<S66>/Divide'

     *  Product: '<S66>/Divide1'

     *  Product: '<S74>/Divide2'

     *  Sum: '<S74>/Add5'

     */

    rtb_Switch_dr = (int16_T)(((int16_T)(rtb_Sign - ((rtb_r_cos_M1 * 9459) >> 14))

      * 375) >> 9);



    /* Product: '<S74>/Divide1' incorporates:

     *  Constant: '<S74>/Constant'

     *  Product: '<S66>/Divide'

     *  Product: '<S66>/Divide1'

     *  Product: '<S74>/Divide'

     *  Sum: '<S74>/Add'

     */

    rtb_r_cos_M1 = (int16_T)(((int16_T)(-rtb_Sign - ((rtb_r_cos_M1 * 9459) >> 14))

      * 375) >> 9);



    /* Product: '<S74>/Divide4' incorporates:

     *  Sum: '<S74>/Add1'

     *  Sum: '<S74>/Add2'

     */

    rtb_Sign = (int16_T)((int16_T)(3000 - (int16_T)(rtb_r_cos_M1 + rtb_Switch_dr))

                         >> 1);



    /* Sum: '<S74>/Add3' */

    rtb_Switch_dr += rtb_Sign;

    rtb_MultiportSwitch_idx_0 = rtb_Switch_dr;

    rtb_MultiportSwitch_idx_1 = rtb_Sign;

    rtb_Switch_dr += rtb_r_cos_M1;

    break;



   default:

    /* Product: '<S75>/Divide3' incorporates:

     *  Product: '<S66>/Divide1'

     *  Product: '<S75>/Divide2'

     *  Sum: '<S75>/Add5'

     */

    rtb_Switch_dr = (int16_T)(((int16_T)(-((int16_T)((rtb_r_cos_M1 * 9459) >> 13)

      << 2) >> 2) * 375) >> 9);



    /* Product: '<S75>/Divide1' incorporates:

     *  Constant: '<S75>/Constant'

     *  Product: '<S66>/Divide'

     *  Product: '<S66>/Divide1'

     *  Product: '<S75>/Divide'

     *  Sum: '<S75>/Add'

     */

    rtb_r_cos_M1 = (int16_T)(((int16_T)(((rtb_r_cos_M1 * 9459) >> 14) + rtb_Sign)

      * 375) >> 9);



    /* Product: '<S75>/Divide4' incorporates:

     *  Sum: '<S75>/Add1'

     *  Sum: '<S75>/Add2'

     */

    rtb_Sign = (int16_T)((int16_T)(3000 - (int16_T)(rtb_r_cos_M1 + rtb_Switch_dr))

                         >> 1);



    /* Sum: '<S75>/Add3' */

    rtb_Switch_dr += rtb_Sign;

    rtb_MultiportSwitch_idx_0 = (int16_T)(rtb_Switch_dr + rtb_r_cos_M1);

    rtb_MultiportSwitch_idx_1 = rtb_Sign;

    break;

  }



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



  /* Outport: '<Root>/VdPrev' incorporates:

   *  UnitDelay: '<S6>/UnitDelay1'

   */

  rtY->VdPrev = rtDW->UnitDelay1_DSTATE_f;



  /* Sum: '<S77>/Add1' incorporates:

   *  Constant: '<S77>/Filter_Constant'

   *  Constant: '<S77>/One'

   *  Product: '<S77>/Product'

   *  Product: '<S77>/Product1'

   *  Sqrt: '<S48>/Sqrt'

   *  UnitDelay: '<S77>/Unit Delay'

   */

  rtb_Switch_oi = (int16_T)(((rtb_Switch_oi * 41) >> 12) + ((4055 *

    rtDW->UnitDelay_DSTATE_c) >> 12));



  /* 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: '<S12>/UnitDelay3' incorporates:

   *  Inport: '<Root>/hw_count'

   */

  rtDW->UnitDelay3_DSTATE = rtU->hw_count;



  /* Update for UnitDelay: '<S12>/UnitDelay4' incorporates:

   *  Abs: '<S12>/Abs5'

   */

  rtDW->UnitDelay4_DSTATE = rtb_Switch2_j;



  /* Update for UnitDelay: '<S32>/UnitDelay' */

  rtDW->UnitDelay_DSTATE_j = rtb_UnitDelay_bc;



  /* Update for UnitDelay: '<S6>/UnitDelay2' */

  rtDW->UnitDelay2_DSTATE_p = rtb_Switch_m[1];



  /* Update for UnitDelay: '<S45>/Unit Delay' */

  rtDW->UnitDelay_DSTATE_b = rtb_Switch2_fu;



  /* Update for UnitDelay: '<S78>/Unit Delay' incorporates:

   *  Saturate: '<S78>/Saturation'

   *  Sum: '<S78>/Sum3'

   *  Sum: '<S81>/Sum1'

   */

  rtDW->UnitDelay_DSTATE = rtb_Saturation - rtb_Sum1;



  /* Update for UnitDelay: '<S50>/Unit Delay' incorporates:

   *  Sum: '<S77>/Add1'

   */

  rtDW->UnitDelay_DSTATE_e = rtb_Switch_oi;



  /* Update for Delay: '<S81>/Resettable Delay' incorporates:

   *  Sum: '<S81>/Sum1'

   */

  rtDW->icLoad = 0U;

  rtDW->ResettableDelay_DSTATE = rtb_Sum1;



  /* Update for UnitDelay: '<S6>/UnitDelay1' */

  rtDW->UnitDelay1_DSTATE_f = rtb_Switch_m[0];



  /* Update for UnitDelay: '<S77>/Unit Delay' incorporates:

   *  Sum: '<S77>/Add1'

   */

  rtDW->UnitDelay_DSTATE_c = rtb_Switch_oi;



  /* Switch: '<S67>/Switch2' incorporates:

   *  RelationalOperator: '<S67>/LowerRelop1'

   *  RelationalOperator: '<S67>/UpperRelop'

   *  Switch: '<S67>/Switch'

   */

  if (rtb_MultiportSwitch_idx_0 > 3000) {

    /* Outport: '<Root>/PWM' incorporates:

     *  Constant: '<S66>/Constant1'

     */

    rtY->PWM[0] = 3000U;

  } else if (rtb_MultiportSwitch_idx_0 < 0) {

    /* Switch: '<S67>/Switch' incorporates:

     *  Constant: '<S66>/Constant5'

     *  Outport: '<Root>/PWM'

     */

    rtY->PWM[0] = 0U;

  } else {

    /* Outport: '<Root>/PWM' incorporates:

     *  Switch: '<S67>/Switch'

     */

    rtY->PWM[0] = (uint16_T)rtb_MultiportSwitch_idx_0;

  }



  if (rtb_MultiportSwitch_idx_1 > 3000) {

    /* Outport: '<Root>/PWM' incorporates:

     *  Constant: '<S66>/Constant1'

     */

    rtY->PWM[1] = 3000U;

  } else if (rtb_MultiportSwitch_idx_1 < 0) {

    /* Switch: '<S67>/Switch' incorporates:

     *  Constant: '<S66>/Constant5'

     *  Outport: '<Root>/PWM'

     */

    rtY->PWM[1] = 0U;

  } else {

    /* Outport: '<Root>/PWM' incorporates:

     *  Switch: '<S67>/Switch'

     */

    rtY->PWM[1] = (uint16_T)rtb_MultiportSwitch_idx_1;

  }



  if (rtb_Switch_dr > 3000) {

    /* Outport: '<Root>/PWM' incorporates:

     *  Constant: '<S66>/Constant1'

     */

    rtY->PWM[2] = 3000U;

  } else if (rtb_Switch_dr < 0) {

    /* Switch: '<S67>/Switch' incorporates:

     *  Constant: '<S66>/Constant5'

     *  Outport: '<Root>/PWM'

     */

    rtY->PWM[2] = 0U;

  } else {

    /* Outport: '<Root>/PWM' incorporates:

     *  Switch: '<S67>/Switch'

     */

    rtY->PWM[2] = (uint16_T)rtb_Switch_dr;

  }



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

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



  /* Outport: '<Root>/sector' */

  rtY->sector = rtb_DataTypeConversion1_c;



  /* Outport: '<Root>/n_MotError' */

  rtY->n_MotError = rtb_UnitDelay_bc;



  /* Outport: '<Root>/iq' */

  rtY->iq = rtb_DataTypeConversion_k1[1];



  /* Outport: '<Root>/id' */

  rtY->id = rtb_DataTypeConversion_k1[0];



  /* Outport: '<Root>/angle' incorporates:

   *  Sum: '<S3>/Sum'

   */

  rtY->angle = rtb_Sum;



  /* Outport: '<Root>/rpm' incorporates:

   *  Switch: '<S12>/Switch2'

   */

  rtY->rpm = rtb_Switch2_c;



  /* Outport: '<Root>/hall_angle' incorporates:

   *  Merge: '<S13>/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;

  rtPrevZCX->ResettableDelay_Reset_ZCE = POS_ZCSIG;

  rtPrevZCX->pi_speed_d.ResettableDelay_Reset_ZCE_a = 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' */

  /* InitializeConditions for Delay: '<S81>/Resettable Delay' */

  rtDW->icLoad = 1U;



  /* SystemInitialize for IfAction SubSystem: '<S12>/Raw_Motor_Speed_Estimation' */

  /* InitializeConditions for UnitDelay: '<S17>/UnitDelay2' */

  rtDW->UnitDelay2_DSTATE = 200000U;



  /* SystemInitialize for Outport: '<S17>/z_counter' incorporates:

   *  Inport: '<S17>/z_counterRawPrev'

   */

  rtDW->z_counterRawPrev = 200000U;



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



  /* SystemInitialize for Atomic SubSystem: '<S32>/Debounce_Filter' */

  Debounce_Filter_Init(&rtDW->Debounce_Filter_i);



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



  /* SystemInitialize for IfAction SubSystem: '<S80>/speed_mode' */

  /* SystemInitialize for Atomic SubSystem: '<S82>/pi_speed' */

  pi_speed_Init(&rtDW->pi_speed_d);



  /* End of SystemInitialize for SubSystem: '<S82>/pi_speed' */

  /* End of SystemInitialize for SubSystem: '<S80>/speed_mode' */



  /* SystemInitialize for IfAction SubSystem: '<S45>/iq_ctrl' */

  /* SystemInitialize for Atomic SubSystem: '<S57>/PI_iq' */

  PI_backCalc_fixdt_Init(&rtDW->PI_iq);



  /* End of SystemInitialize for SubSystem: '<S57>/PI_iq' */

  /* End of SystemInitialize for SubSystem: '<S45>/iq_ctrl' */



  /* SystemInitialize for IfAction SubSystem: '<S45>/id_ctrl' */

  /* SystemInitialize for Atomic SubSystem: '<S56>/PI_id' */

  PI_backCalc_fixdt_Init(&rtDW->PI_id);



  /* End of SystemInitialize for SubSystem: '<S56>/PI_id' */

  /* End of SystemInitialize for SubSystem: '<S45>/id_ctrl' */

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

}



/*

 * File trailer for generated code.

 *

 * [EOF]

 */