/*

 * File: PMSM_Controller.c

 *

 * Code generated for Simulink model 'PMSM_Controller'.

 *

 * Model version                  : 1.1460

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

 * C/C++ source code generated on : Sat May 28 14:25:47 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_Us32En6_Ys16En_1bhh77n4(int32_T u);

extern int16_T rt_sqrt_Us32En10_Ys16E_7VJYwqF9(int32_T u);

extern uint16_T rt_sqrt_Uu16En14_Yu16E_WMwW1mku(uint16_T u);

uint16_T plook_u16s16_evencka(int16_T u, int16_T bp0, uint16_T bpSpace, uint32_T

  maxIndex);

int32_T div_nde_s32_floor(int32_T numerator, int32_T denominator);

extern void Low_Pass_Filter(const int16_T rtu_u[2], uint16_T rtu_coef, int16_T

  rty_y[2], DW_Low_Pass_Filter *localDW);

extern void PI_backCalc_fixdt_Init(DW_PI_backCalc_fixdt *localDW);

extern int32_T PI_backCalc_fixdt(int32_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_init,

  uint8_T rtu_reset, DW_PI_backCalc_fixdt *localDW, ZCE_PI_backCalc_fixdt

  *localZCE);

extern void PI_backCalc_fixdt_p_Init(DW_PI_backCalc_fixdt_i *localDW);

extern int32_T PI_backCalc_fixdt_o(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_init,

  uint8_T rtu_reset, DW_PI_backCalc_fixdt_i *localDW, ZCE_PI_backCalc_fixdt_e

  *localZCE);

extern void RateInit(int16_T rtu_initVal, int16_T rtu_target, int16_T rtu_step,

                     int16_T *rty_s_step, int16_T *rty_High, int16_T *rty_Low);

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;

}



int32_T div_nde_s32_floor(int32_T numerator, int32_T denominator)

{

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

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

}



/*

 * Output and update for atomic system:

 *    '<S48>/Low_Pass_Filter'

 *    '<S76>/Low_Pass_Filter'

 */

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



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

   *  UnitDelay: '<S56>/UnitDelay1'

   */

  rtb_Sum3_m = rtu_u[0] - (localDW->UnitDelay1_DSTATE[0] >> 16);

  if (rtb_Sum3_m > 32767) {

    rtb_Sum3_m = 32767;

  } else {

    if (rtb_Sum3_m < -32768) {

      rtb_Sum3_m = -32768;

    }

  }



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

   *  Product: '<S56>/Divide3'

   *  Sum: '<S56>/Sum2'

   *  UnitDelay: '<S56>/UnitDelay1'

   */

  rtb_Sum3_m = rtu_coef * rtb_Sum3_m + localDW->UnitDelay1_DSTATE[0];



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

  rty_y[0] = (int16_T)(rtb_Sum3_m >> 16);



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

  localDW->UnitDelay1_DSTATE[0] = rtb_Sum3_m;



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

   *  UnitDelay: '<S56>/UnitDelay1'

   */

  rtb_Sum3_m = rtu_u[1] - (localDW->UnitDelay1_DSTATE[1] >> 16);

  if (rtb_Sum3_m > 32767) {

    rtb_Sum3_m = 32767;

  } else {

    if (rtb_Sum3_m < -32768) {

      rtb_Sum3_m = -32768;

    }

  }



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

   *  Product: '<S56>/Divide3'

   *  Sum: '<S56>/Sum2'

   *  UnitDelay: '<S56>/UnitDelay1'

   */

  rtb_Sum3_m = rtu_coef * rtb_Sum3_m + localDW->UnitDelay1_DSTATE[1];



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

  rty_y[1] = (int16_T)(rtb_Sum3_m >> 16);



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

  localDW->UnitDelay1_DSTATE[1] = rtb_Sum3_m;

}



/* System initialize for atomic system: '<S87>/PI_Speed' */

void PI_backCalc_fixdt_Init(DW_PI_backCalc_fixdt *localDW)

{

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

  localDW->icLoad = 1U;

}



/* Output and update for atomic system: '<S87>/PI_Speed' */

int32_T PI_backCalc_fixdt(int32_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_init, uint8_T

  rtu_reset, DW_PI_backCalc_fixdt *localDW, ZCE_PI_backCalc_fixdt *localZCE)

{

  int32_T rty_pi_out_0;

  int64_T tmp;

  int64_T tmp_0;



  /* Product: '<S89>/Divide4' */

  tmp_0 = (int64_T)rtu_err * rtu_P;

  if (tmp_0 > 2147483647LL) {

    tmp_0 = 2147483647LL;

  } else {

    if (tmp_0 < -2147483648LL) {

      tmp_0 = -2147483648LL;

    }

  }



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

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

   */

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

    localDW->icLoad = 1U;

  }



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

  if (localDW->icLoad != 0) {

    localDW->ResettableDelay_DSTATE = rtu_init << 7;

  }



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

   *  Product: '<S89>/Divide4'

   */

  tmp = ((int64_T)(int32_T)tmp_0 * rtu_I) >> 14;

  if (tmp > 2147483647LL) {

    tmp = 2147483647LL;

  } else {

    if (tmp < -2147483648LL) {

      tmp = -2147483648LL;

    }

  }



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

   *  Product: '<S89>/Divide1'

   *  UnitDelay: '<S89>/UnitDelay'

   */

  tmp = (int64_T)(int32_T)tmp + localDW->UnitDelay_DSTATE;

  if (tmp > 2147483647LL) {

    tmp = 2147483647LL;

  } else {

    if (tmp < -2147483648LL) {

      tmp = -2147483648LL;

    }

  }



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

   *  Delay: '<S90>/Resettable Delay'

   *  Sum: '<S89>/Sum2'

   */

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

  if (tmp > 2147483647LL) {

    tmp = 2147483647LL;

  } else {

    if (tmp < -2147483648LL) {

      tmp = -2147483648LL;

    }

  }



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

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

   *  Product: '<S89>/Divide4'

   *  Sum: '<S90>/Sum1'

   */

  tmp_0 = (int64_T)((int32_T)tmp << 2) + (int32_T)tmp_0;

  if (tmp_0 > 2147483647LL) {

    tmp_0 = 2147483647LL;

  } else {

    if (tmp_0 < -2147483648LL) {

      tmp_0 = -2147483648LL;

    }

  }



  /* RelationalOperator: '<S91>/LowerRelop1' incorporates:

   *  Switch: '<S91>/Switch2'

   */

  rty_pi_out_0 = rtu_satMax << 9;



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

   *  RelationalOperator: '<S91>/LowerRelop1'

   *  Sum: '<S89>/Sum6'

   */

  if ((int32_T)tmp_0 <= rty_pi_out_0) {

    /* RelationalOperator: '<S91>/UpperRelop' incorporates:

     *  Switch: '<S91>/Switch'

     */

    rty_pi_out_0 = rtu_satMin << 9;



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

     *  RelationalOperator: '<S91>/UpperRelop'

     */

    if ((int32_T)tmp_0 >= rty_pi_out_0) {

      rty_pi_out_0 = (int32_T)tmp_0;

    }

  }



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

   *  Product: '<S89>/Divide2'

   *  Sum: '<S89>/Sum3'

   *  Sum: '<S89>/Sum6'

   */

  localDW->UnitDelay_DSTATE = (int32_T)(((int64_T)(rty_pi_out_0 - (int32_T)tmp_0)

    * rtu_Kb) >> 14);



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

   *  Sum: '<S90>/Sum1'

   */

  localDW->icLoad = 0U;

  localDW->ResettableDelay_DSTATE = (int32_T)tmp;

  return rty_pi_out_0;

}



/*

 * System initialize for atomic system:

 *    '<S95>/PI_backCalc_fixdt'

 *    '<S95>/PI_backCalc_fixdt1'

 */

void PI_backCalc_fixdt_p_Init(DW_PI_backCalc_fixdt_i *localDW)

{

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

  localDW->icLoad = 1U;

}



/*

 * Output and update for atomic system:

 *    '<S95>/PI_backCalc_fixdt'

 *    '<S95>/PI_backCalc_fixdt1'

 */

int32_T PI_backCalc_fixdt_o(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_init,

  uint8_T rtu_reset, DW_PI_backCalc_fixdt_i *localDW, ZCE_PI_backCalc_fixdt_e

  *localZCE)

{

  int32_T rty_pi_out_0;

  int64_T tmp;

  int64_T tmp_0;

  int32_T rtb_Divide4_n;



  /* Product: '<S100>/Divide4' */

  rtb_Divide4_n = (rtu_err * rtu_P) >> 1;



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

   *  DataTypeConversion: '<S102>/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 << 7;

  }



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

   *  Product: '<S100>/Divide4'

   */

  tmp_0 = ((int64_T)rtb_Divide4_n * rtu_I) >> 14;

  if (tmp_0 > 2147483647LL) {

    tmp_0 = 2147483647LL;

  } else {

    if (tmp_0 < -2147483648LL) {

      tmp_0 = -2147483648LL;

    }

  }



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

   *  Product: '<S100>/Divide1'

   *  UnitDelay: '<S100>/UnitDelay'

   */

  tmp_0 = (int64_T)(int32_T)tmp_0 + localDW->UnitDelay_DSTATE;

  if (tmp_0 > 2147483647LL) {

    tmp_0 = 2147483647LL;

  } else {

    if (tmp_0 < -2147483648LL) {

      tmp_0 = -2147483648LL;

    }

  }



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

   *  Delay: '<S102>/Resettable Delay'

   *  Sum: '<S100>/Sum2'

   */

  tmp_0 = (((int64_T)localDW->ResettableDelay_DSTATE << 2) + (int32_T)tmp_0) >>

    2;

  if (tmp_0 > 2147483647LL) {

    tmp_0 = 2147483647LL;

  } else {

    if (tmp_0 < -2147483648LL) {

      tmp_0 = -2147483648LL;

    }

  }



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

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

   *  Product: '<S100>/Divide4'

   *  Sum: '<S102>/Sum1'

   */

  tmp = (int64_T)((int32_T)tmp_0 << 2) + rtb_Divide4_n;

  if (tmp > 2147483647LL) {

    tmp = 2147483647LL;

  } else {

    if (tmp < -2147483648LL) {

      tmp = -2147483648LL;

    }

  }



  /* RelationalOperator: '<S103>/LowerRelop1' incorporates:

   *  Switch: '<S103>/Switch2'

   */

  rty_pi_out_0 = rtu_satMax << 9;



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

   *  RelationalOperator: '<S103>/LowerRelop1'

   *  Sum: '<S100>/Sum6'

   */

  if ((int32_T)tmp <= rty_pi_out_0) {

    /* RelationalOperator: '<S103>/UpperRelop' incorporates:

     *  Switch: '<S103>/Switch'

     */

    rty_pi_out_0 = rtu_satMin << 9;



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

     *  RelationalOperator: '<S103>/UpperRelop'

     */

    if ((int32_T)tmp >= rty_pi_out_0) {

      rty_pi_out_0 = (int32_T)tmp;

    }

  }



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

   *  Product: '<S100>/Divide2'

   *  Sum: '<S100>/Sum3'

   *  Sum: '<S100>/Sum6'

   */

  localDW->UnitDelay_DSTATE = (int32_T)(((int64_T)(rty_pi_out_0 - (int32_T)tmp) *

    rtu_Kb) >> 14);



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

   *  Sum: '<S102>/Sum1'

   */

  localDW->icLoad = 0U;

  localDW->ResettableDelay_DSTATE = (int32_T)tmp_0;

  return rty_pi_out_0;

}



/*

 * Output and update for action system:

 *    '<S108>/RateInit'

 *    '<S115>/RateInit'

 */

void RateInit(int16_T rtu_initVal, int16_T rtu_target, int16_T rtu_step, int16_T

              *rty_s_step, int16_T *rty_High, int16_T *rty_Low)

{

  int16_T rtb_Add_b;



  /* Sum: '<S109>/Add' */

  rtb_Add_b = (int16_T)((rtu_target - rtu_initVal) >> 1);



  /* Signum: '<S109>/Sign' incorporates:

   *  Sum: '<S109>/Add'

   */

  if (rtb_Add_b < 0) {

    rtb_Add_b = -1;

  } else {

    rtb_Add_b = (int16_T)(rtb_Add_b > 0);

  }



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



  /* Product: '<S109>/Divide' */

  *rty_s_step = (int16_T)(rtu_step * rtb_Add_b);



  /* MinMax: '<S109>/Max' */

  if (rtu_target > rtu_initVal) {

    *rty_High = rtu_target;

  } else {

    *rty_High = rtu_initVal;

  }



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



  /* MinMax: '<S109>/Max1' */

  if (rtu_initVal < rtu_target) {

    *rty_Low = rtu_initVal;

  } else {

    *rty_Low = rtu_target;

  }



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

}



int16_T rt_sqrt_Us32En6_Ys16En_1bhh77n4(int32_T u)

{

  int64_T tmp03_u;

  int32_T iBit;

  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 = (int64_T)u << 4;

    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;

}



int16_T rt_sqrt_Us32En10_Ys16E_7VJYwqF9(int32_T u)

{

  int32_T iBit;

  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;

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

      tmp01_y = (int16_T)(y | shiftMask);

      if (tmp01_y * tmp01_y <= u) {

        y = tmp01_y;

      }



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

    }

  } else {

    y = 0;

  }



  return y;

}



uint16_T rt_sqrt_Uu16En14_Yu16E_WMwW1mku(uint16_T u)

{

  int32_T iBit;

  uint32_T tmp03_u;

  uint16_T shiftMask;

  uint16_T tmp01_y;

  uint16_T y;



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

  if (u > 0) {

    y = 0U;

    shiftMask = 32768U;

    tmp03_u = (uint32_T)u << 14;

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

      tmp01_y = (uint16_T)(y | shiftMask);

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

        y = tmp01_y;

      }



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

    }

  } else {

    y = 0U;

  }



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

  int64_T tmp_2;

  uint64_T tmp_3;

  int32_T rtb_Divide_idx_0;

  int32_T rtb_Divide_idx_1;

  int32_T rtb_Gain_b0;

  int32_T rtb_Gain_p2;

  int32_T rtb_MathFunction2_p;

  int32_T rtb_Sum1_f;

  int32_T rtb_Switch3;

  int32_T rtb_Switch_np;

  int32_T tmp;

  int32_T tmp_0;

  uint32_T qY;

  uint32_T rtb_Switch2;

  int16_T rtb_TmpSignalConversionAtLow_Pa[2];

  int16_T rtb_UnitDelay1_m[2];

  int16_T rtb_Divide1_m;

  int16_T rtb_Divide3_k;

  int16_T rtb_Min;

  int16_T rtb_Sum1_a;

  int16_T rtb_Sum3_jm;

  int16_T rtb_Sum6_k;

  int16_T rtb_Sum6_p;

  int16_T rtb_Switch_f2_idx_0;

  int16_T rtb_Switch_f2_idx_1;

  int16_T rtb_r_cos_M1;

  uint16_T rtb_BitwiseOperator2;

  uint16_T rtb_LogicalOperator3;

  int8_T UnitDelay3;

  int8_T rtb_Sum2;

  int8_T rtb_Sum2_tmp;

  uint8_T rtb_Add_gf;

  uint8_T rtb_DataTypeConversion_j;

  uint8_T rtb_Sum_i;

  uint8_T rtb_UnitDelay;

  uint8_T rtb_z_ctrlMod;

  boolean_T rtb_Equal_k;

  boolean_T rtb_LogicalOperator12;

  boolean_T rtb_LogicalOperator2_h;

  boolean_T rtb_LogicalOperator4_e;

  boolean_T rtb_RelationalOperator4_f;

  boolean_T rtb_n_commDeacv;



  /* Outputs for Atomic SubSystem: '<Root>/PMSM_Controller' */

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

  rtb_UnitDelay1_m[0] = rtDW->UnitDelay1_DSTATE_f[0];

  rtb_UnitDelay1_m[1] = rtDW->UnitDelay1_DSTATE_f[1];



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

   *  Inport: '<Root>/FOC_Flags'

   */

  rtb_BitwiseOperator2 = (uint16_T)(rtU->FOC_Flags & 1);



  /* UnitDelay: '<S37>/UnitDelay' */

  rtb_UnitDelay = rtDW->UnitDelay_DSTATE_j;



  /* 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_Equal_k = (boolean_T)((rtU->hall_A != 0) ^ (rtDW->Delay_DSTATE_d != 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_gf = (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: '<S20>/z_counterRawPrev'

   *  UnitDelay: '<S14>/UnitDelay3'

   */

  if (rtb_Equal_k) {

    /* 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_gf];



    /* 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: '<S20>/Action Port'

     */

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

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

    rtDW->z_counterRawPrev = rtDW->UnitDelay3_DSTATE;



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

     *  Constant: '<S20>/Constant4'

     *  Inport: '<S20>/z_counterRawPrev'

     *  Logic: '<S20>/Logical Operator1'

     *  Switch: '<S20>/Switch2'

     *  UnitDelay: '<S14>/UnitDelay3'

     *  UnitDelay: '<S20>/UnitDelay1'

     */

    if (rtb_RelationalOperator4_f && rtDW->UnitDelay1_DSTATE_iv) {

      rtb_Switch3 = 0;

    } else {

      if (rtb_RelationalOperator4_f) {

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

         *  UnitDelay: '<S14>/UnitDelay4'

         */

        rtb_Switch2 = rtDW->UnitDelay4_DSTATE;

      } else {

        /* Sum: '<S20>/Sum13' incorporates:

         *  Switch: '<S20>/Switch2'

         *  UnitDelay: '<S20>/UnitDelay2'

         *  UnitDelay: '<S20>/UnitDelay3'

         *  UnitDelay: '<S20>/UnitDelay5'

         */

        tmp_3 = (((uint64_T)rtDW->UnitDelay2_DSTATE + rtDW->UnitDelay3_DSTATE_l)

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

        if (tmp_3 > 4294967295ULL) {

          tmp_3 = 4294967295ULL;

        }



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

         *  Constant: '<S20>/cf_speedCoef'

         *  Constant: '<S20>/cf_speedCoef1'

         *  Gain: '<S20>/g_Ha'

         *  Product: '<S20>/Divide'

         *  Sum: '<S20>/Sum13'

         *  Switch: '<S20>/Switch2'

         */

        tmp_3 = ((uint64_T)((10000000U / rtP.n_polePairs) << 2) << 4) /

          (uint32_T)tmp_3;

        if (tmp_3 > 4294967295ULL) {

          tmp_3 = 4294967295ULL;

        }



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

         *  Product: '<S20>/Divide13'

         */

        rtb_Switch2 = (uint32_T)tmp_3;

      }



      rtb_Switch3 = (int32_T)rtb_Switch2;

    }



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



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

     *  Switch: '<S20>/Switch3'

     */

    rtDW->Divide11 = rtb_Switch3 * rtDW->Switch2_i;



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

    rtDW->UnitDelay1_DSTATE_iv = rtb_RelationalOperator4_f;



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

     *  UnitDelay: '<S20>/UnitDelay3'

     */

    rtDW->UnitDelay2_DSTATE = rtDW->UnitDelay3_DSTATE_l;



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

     *  UnitDelay: '<S20>/UnitDelay5'

     */

    rtDW->UnitDelay3_DSTATE_l = rtDW->UnitDelay5_DSTATE;



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

    rtDW->UnitDelay5_DSTATE = rtDW->z_counterRawPrev;



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

  }



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



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

   *  Constant: '<S14>/Constant4'

   *  Constant: '<S14>/z_maxCntRst'

   *  Gain: '<S14>/Gain'

   *  Inport: '<Root>/us_Count'

   *  Product: '<S20>/Divide11'

   *  RelationalOperator: '<S14>/Relational Operator2'

   */

  if (rtU->us_Count >= (rtP.n_hall_count_ps << 1)) {

    rtb_Switch3 = 0;

  } else {

    rtb_Switch3 = rtDW->Divide11;

  }



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



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

   *  Switch: '<S14>/Switch2'

   */

  if (rtb_Switch3 < 0) {

    rtb_Switch2 = (uint32_T)-rtb_Switch3;

  } else {

    rtb_Switch2 = (uint32_T)rtb_Switch3;

  }



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



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

  if (rtb_Equal_k) {

    /* Outputs for IfAction SubSystem: '<S14>/AdvCtrlDetect' incorporates:

     *  ActionPort: '<S19>/Action Port'

     */

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

     *  Abs: '<S14>/Abs5'

     */

    rtDW->n_commDeacv_Mode = ((rtb_Switch2 >= 480U) || ((rtb_Switch2 > 240U) &&

      rtDW->n_commDeacv_Mode));



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

     *  Constant: '<S21>/Constant'

     *  Relay: '<S19>/n_commDeacv'

     *  Sum: '<S19>/Sum13'

     *  UnitDelay: '<S19>/UnitDelay2'

     *  UnitDelay: '<S19>/UnitDelay3'

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

     *  UnitDelay: '<S19>/UnitDelay3'

     */

    rtDW->UnitDelay2_DSTATE_f = rtDW->UnitDelay3_DSTATE_lh;



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

     *  UnitDelay: '<S19>/UnitDelay5'

     */

    rtDW->UnitDelay3_DSTATE_lh = rtDW->UnitDelay5_DSTATE_f;



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

     *  Logic: '<S19>/Logical Operator3'

     *  Relay: '<S19>/n_commDeacv'

     */

    rtDW->UnitDelay5_DSTATE_f = rtDW->n_commDeacv_Mode;



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

  }



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



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

   *  Abs: '<S14>/Abs5'

   *  Abs: '<S37>/Abs4'

   *  Constant: '<S37>/CTRL_COMM4'

   *  Inport: '<Root>/b_motEna'

   *  Logic: '<S37>/Logical Operator1'

   *  RelationalOperator: '<S14>/Relational Operator9'

   *  RelationalOperator: '<S37>/Relational Operator7'

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

   *  UnitDelay: '<S6>/UnitDelay1'

   */

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

    rtb_Equal_k = true;

  } else {

    if (rtDW->UnitDelay1_DSTATE_f[1] < 0) {

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

       *  UnitDelay: '<S6>/UnitDelay1'

       */

      rtb_Switch_f2_idx_1 = (int16_T)-rtDW->UnitDelay1_DSTATE_f[1];

    } else {

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

       *  UnitDelay: '<S6>/UnitDelay1'

       */

      rtb_Switch_f2_idx_1 = rtDW->UnitDelay1_DSTATE_f[1];

    }



    rtb_Equal_k = (rtU->b_motEna && (rtb_Switch2 < 48U) && (rtb_Switch_f2_idx_1 >

      9920));

  }



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



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

   *  Constant: '<S37>/CTRL_COMM'

   *  Constant: '<S37>/CTRL_COMM1'

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

   *  Gain: '<S37>/g_Hb'

   *  Gain: '<S37>/g_Hb1'

   *  RelationalOperator: '<S37>/Relational Operator1'

   *  RelationalOperator: '<S37>/Relational Operator3'

   */

  rtb_Sum_i = (uint8_T)(((uint32_T)((rtb_Add_gf == 7) << 1) + (rtb_Add_gf == 0))

                        + (rtb_Equal_k << 2));



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

   *  Constant: '<S37>/CTRL_COMM2'

   */

  rtb_RelationalOperator4_f = (rtb_Sum_i != 0);



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

   *  UnitDelay: '<S42>/UnitDelay'

   */

  rtb_n_commDeacv = (rtb_RelationalOperator4_f != rtDW->UnitDelay_DSTATE_nx);



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

   *  Inport: '<S40>/yPrev'

   *  Logic: '<S38>/Logical Operator1'

   *  Logic: '<S38>/Logical Operator2'

   *  Logic: '<S38>/Logical Operator3'

   *  Logic: '<S38>/Logical Operator4'

   *  UnitDelay: '<S38>/UnitDelay'

   */

  if (rtb_RelationalOperator4_f && (!rtDW->UnitDelay_DSTATE_k)) {

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

     *  ActionPort: '<S43>/Action Port'

     */

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

     *  Constant: '<S47>/Constant23'

     *  UnitDelay: '<S47>/UnitDelay'

     */

    if (rtb_n_commDeacv) {

      rtb_LogicalOperator3 = 0U;

    } else {

      rtb_LogicalOperator3 = rtDW->UnitDelay_DSTATE_p;

    }



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



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

     *  Constant: '<S37>/t_errQual'

     *  Constant: '<S43>/Constant6'

     *  RelationalOperator: '<S43>/Relational Operator2'

     *  Sum: '<S46>/Sum1'

     */

    rtb_n_commDeacv = (((uint16_T)(rtb_LogicalOperator3 + 1U) > 1600) ||

                       rtDW->UnitDelay_DSTATE_k);



    /* MinMax: '<S46>/MinMax' incorporates:

     *  Constant: '<S43>/Constant6'

     *  Sum: '<S46>/Sum1'

     */

    if ((uint16_T)(rtb_LogicalOperator3 + 1U) < 1600) {

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

      rtDW->UnitDelay_DSTATE_p = (uint16_T)(rtb_LogicalOperator3 + 1U);

    } else {

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

      rtDW->UnitDelay_DSTATE_p = 1600U;

    }



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

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

  } else if ((!rtb_RelationalOperator4_f) && rtDW->UnitDelay_DSTATE_k) {

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

     *  ActionPort: '<S41>/Action Port'

     */

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

     *  Constant: '<S45>/Constant23'

     *  UnitDelay: '<S45>/UnitDelay'

     */

    if (rtb_n_commDeacv) {

      rtb_LogicalOperator3 = 0U;

    } else {

      rtb_LogicalOperator3 = rtDW->UnitDelay_DSTATE_f;

    }



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



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

     *  Constant: '<S37>/t_errDequal'

     *  Constant: '<S41>/Constant6'

     *  RelationalOperator: '<S41>/Relational Operator2'

     *  Sum: '<S44>/Sum1'

     */

    rtb_n_commDeacv = (((uint16_T)(rtb_LogicalOperator3 + 1U) <= 12000) &&

                       rtDW->UnitDelay_DSTATE_k);



    /* MinMax: '<S44>/MinMax' incorporates:

     *  Constant: '<S41>/Constant6'

     *  Sum: '<S44>/Sum1'

     */

    if ((uint16_T)(rtb_LogicalOperator3 + 1U) < 12000) {

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

      rtDW->UnitDelay_DSTATE_f = (uint16_T)(rtb_LogicalOperator3 + 1U);

    } else {

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

      rtDW->UnitDelay_DSTATE_f = 12000U;

    }



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

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

  } else {

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

     *  ActionPort: '<S40>/Action Port'

     */

    rtb_n_commDeacv = rtDW->UnitDelay_DSTATE_k;



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

  }



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



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

   *  Inport: '<Root>/b_motEna'

   *  Logic: '<S25>/Logical Operator7'

   */

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



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

   *  Constant: '<S25>/constant8'

   *  Inport: '<Root>/n_ctrlModReq'

   *  Logic: '<S25>/Logical Operator11'

   *  Logic: '<S25>/Logical Operator8'

   *  RelationalOperator: '<S25>/Relational Operator10'

   */

  rtb_LogicalOperator4_e = ((rtb_BitwiseOperator2 != 0) || (!rtDW->Compare) || (

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



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

   *  Switch: '<S14>/Switch2'

   */

  if (rtb_Switch3 < 0) {

    rtb_LogicalOperator3 = (uint16_T)((uint32_T)-rtb_Switch3 >> 4);

  } else {

    rtb_LogicalOperator3 = (uint16_T)((uint32_T)rtb_Switch3 >> 4);

  }



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



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

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

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



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

   *  Inport: '<Root>/b_cruiseEna'

   *  Relay: '<S25>/n_SpeedCtrl'

   */

  rtb_Equal_k = (rtDW->n_SpeedCtrl_Mode && rtU->b_cruiseEna);



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

   *  Constant: '<S25>/constant'

   *  Inport: '<Root>/n_ctrlModReq'

   *  Logic: '<S25>/Logical Operator5'

   *  RelationalOperator: '<S25>/Relational Operator4'

   */

  rtb_LogicalOperator2_h = ((rtU->n_ctrlModReq == 2) && (!rtb_Equal_k));



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

   *  Constant: '<S25>/constant1'

   *  Inport: '<Root>/n_ctrlModReq'

   *  RelationalOperator: '<S25>/Relational Operator1'

   */

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



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

   *  Logic: '<S25>/Logical Operator3'

   *  Logic: '<S25>/Logical Operator6'

   *  Logic: '<S25>/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_e) {

      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_Equal_k) {

        if (rtb_LogicalOperator2_h) {

          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_h) {

        rtDW->is_ACTIVE = IN_SPEED_MODE;

        rtb_z_ctrlMod = SPD_MODE;

      }

    }

  } else {

    /* case IN_OPEN: */

    rtb_z_ctrlMod = OPEN_MODE;

    if ((!rtb_LogicalOperator4_e) && (rtb_LogicalOperator2_h || rtb_Equal_k)) {

      rtDW->is_c5_PMSM_Controller = IN_ACTIVE;

      if (rtb_LogicalOperator2_h) {

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



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

   *  Constant: '<S52>/Constant'

   *  Inport: '<Root>/adc_Pha'

   *  Inport: '<Root>/adc_Phb'

   */

  rtb_Divide_idx_0 = rtU->adc_Pha * rtP.f_adc_curr_ceof;

  rtb_Divide_idx_1 = rtU->adc_Phb * rtP.f_adc_curr_ceof;



  /* Sum: '<S48>/Add' */

  tmp_2 = (int64_T)rtb_Divide_idx_0 + rtb_Divide_idx_1;

  if (tmp_2 > 2147483647LL) {

    tmp_2 = 2147483647LL;

  } else {

    if (tmp_2 < -2147483648LL) {

      tmp_2 = -2147483648LL;

    }

  }



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

   *  Sum: '<S48>/Add'

   */

  tmp_2 = -(int64_T)(int32_T)tmp_2 >> 9;

  if (tmp_2 > 32767LL) {

    tmp_2 = 32767LL;

  } else {

    if (tmp_2 < -32768LL) {

      tmp_2 = -32768LL;

    }

  }



  /* Gain: '<S55>/Gain' */

  rtb_Divide_idx_0 >>= 8;

  if (rtb_Divide_idx_0 > 32767) {

    rtb_Divide_idx_0 = 32767;

  } else {

    if (rtb_Divide_idx_0 < -32768) {

      rtb_Divide_idx_0 = -32768;

    }

  }



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

   *  Sum: '<S48>/Add1'

   *  Sum: '<S55>/Add2'

   */

  tmp_2 = (int64_T)(int16_T)tmp_2 << 9;

  tmp_1 = (tmp_2 + rtb_Divide_idx_1) >> 8;

  if (tmp_1 > 32767LL) {

    tmp_1 = 32767LL;

  } else {

    if (tmp_1 < -32768LL) {

      tmp_1 = -32768LL;

    }

  }



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

   *  Gain: '<S55>/Gain'

   *  Sum: '<S55>/Add3'

   */

  rtb_Divide_idx_0 = ((rtb_Divide_idx_0 << 1) - (int16_T)tmp_1) >> 1;

  if (rtb_Divide_idx_0 > 32767) {

    rtb_Divide_idx_0 = 32767;

  } else {

    if (rtb_Divide_idx_0 < -32768) {

      rtb_Divide_idx_0 = -32768;

    }

  }



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

   *  Product: '<S57>/Divide1'

   *  Sum: '<S55>/Add'

   */

  rtb_Divide1_m = (int16_T)((21845 * rtb_Divide_idx_0) >> 16);



  /* 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_gf];

  } else {

    rtb_Sum2 = (int8_T)(rtConstP.vec_hallToPos_Value[rtb_Add_gf] + 1);

  }



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



  /* MinMax: '<S10>/MinMax' incorporates:

   *  Inport: '<Root>/us_Count'

   */

  if (rtU->us_Count < rtDW->z_counterRawPrev) {

    qY = rtU->us_Count;

  } else {

    qY = rtDW->z_counterRawPrev;

  }



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



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

   *  Product: '<S10>/Divide1'

   *  Product: '<S10>/Divide3'

   */

  rtb_Sum3_jm = (int16_T)(((int16_T)((int16_T)(((uint64_T)qY << 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_Sum3_jm <= 0) {

    rtb_Sum3_jm = 0;

  }



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



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

   *  Constant: '<S15>/Constant2'

   *  Product: '<S10>/Divide2'

   */

  rtb_Sum3_jm = (int16_T)((((15 * rtb_Sum3_jm) >> 4) + (rtP.i_hall_offset << 2))

    >> 2);



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

   *  Sum: '<S15>/Add2'

   */

  rtb_r_cos_M1 = (int16_T)(rtb_Sum3_jm >> 4);



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

   *  Constant: '<S15>/Constant1'

   *  Constant: '<S15>/Constant3'

   *  Inport: '<S16>/In1'

   *  Inport: '<S18>/In1'

   *  Merge: '<S15>/Merge'

   *  Sum: '<S15>/Add'

   *  Sum: '<S15>/Add1'

   *  Sum: '<S15>/Add2'

   */

  if (rtb_r_cos_M1 >= 360) {

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

     *  ActionPort: '<S16>/Action Port'

     */

    rtb_Sum3_jm = (int16_T)(rtb_Sum3_jm - 5760);



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

  } else {

    if (rtb_r_cos_M1 < 0) {

      /* Outputs for IfAction SubSystem: '<S15>/If Action Subsystem2' incorporates:

       *  ActionPort: '<S18>/Action Port'

       */

      rtb_Sum3_jm = (int16_T)(rtb_Sum3_jm + 5760);



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

    }

  }



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



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

   *  Inport: '<Root>/FOC_Flags'

   */

  if ((rtU->FOC_Flags == 0) || (rtU->FOC_Flags == 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_Sum3_jm;



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

  } else {

    if (rtU->FOC_Flags == 1) {

      /* Outputs for IfAction SubSystem: '<S3>/If Action Subsystem1' incorporates:

       *  ActionPort: '<S13>/Action Port'

       */

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

       *  Inport: '<Root>/theta_Open'

       *  Inport: '<S13>/In1'

       */

      rtDW->Merge_i = rtU->theta_Open;



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

    }

  }



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



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

   *  Merge: '<S3>/Merge'

   */

  rtb_LogicalOperator3 = plook_u16s16_evencka(rtDW->Merge_i, 0, 16U, 360U);



  /* Sum: '<S55>/Add2' */

  tmp_2 = (rtb_Divide_idx_1 - tmp_2) >> 9;

  if (tmp_2 > 32767LL) {

    tmp_2 = 32767LL;

  } else {

    if (tmp_2 < -32768LL) {

      tmp_2 = -32768LL;

    }

  }



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

   *  Sum: '<S55>/Add2'

   *  Sum: '<S57>/Sum6'

   */

  rtb_Sum6_p = (int16_T)((18919 * (int16_T)tmp_2) >> 15);



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

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

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

   *  Product: '<S57>/Divide1'

   *  Product: '<S57>/Divide2'

   *  Product: '<S57>/Divide3'

   *  Sum: '<S57>/Sum6'

   */

  rtb_Divide_idx_0 = ((rtb_Divide1_m * rtConstP.pooled9[rtb_LogicalOperator3]) >>

                      14) + (int16_T)((rtb_Sum6_p *

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

  if (rtb_Divide_idx_0 > 32767) {

    rtb_Divide_idx_0 = 32767;

  } else {

    if (rtb_Divide_idx_0 < -32768) {

      rtb_Divide_idx_0 = -32768;

    }

  }



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

   *  Sum: '<S57>/Sum1'

   */

  rtb_TmpSignalConversionAtLow_Pa[0] = (int16_T)rtb_Divide_idx_0;



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

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

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

   *  Product: '<S57>/Divide1'

   *  Product: '<S57>/Divide4'

   */

  rtb_Divide_idx_0 = (int16_T)((rtb_Sum6_p *

    rtConstP.pooled9[rtb_LogicalOperator3]) >> 14) - ((rtb_Divide1_m *

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

  if (rtb_Divide_idx_0 > 32767) {

    rtb_Divide_idx_0 = 32767;

  } else {

    if (rtb_Divide_idx_0 < -32768) {

      rtb_Divide_idx_0 = -32768;

    }

  }



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

   *  Sum: '<S57>/Sum6'

   */

  rtb_TmpSignalConversionAtLow_Pa[1] = (int16_T)rtb_Divide_idx_0;



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

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

   *  Outport: '<Root>/f_Idq'

   */

  Low_Pass_Filter(rtb_TmpSignalConversionAtLow_Pa, rtP.f_lpf_idq, rtY->f_Idq,

                  &rtDW->Low_Pass_Filter_d);



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



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

   *  Constant: '<S24>/Constant3'

   *  Inport: '<Root>/spd_Target'

   */

  if (rtU->spd_Target > 240) {

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

     *  Constant: '<S24>/Constant1'

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

     *  Switch: '<S24>/Switch'

     */

    if (rtb_LogicalOperator12) {

      rtb_Switch_np = rtU->spd_Target;

    } else {

      rtb_Switch_np = 0;

    }



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

  } else {

    rtb_Switch_np = 0;

  }



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



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

   *  Constant: '<S24>/Constant4'

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

   *  Inport: '<Root>/vdq_Open'

   */

  if (rtb_LogicalOperator12) {

    rtb_Sum6_p = rtU->vdq_Open[1];

  } else {

    rtb_Sum6_p = 0;

  }



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



  /* 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 % 40U == 0U);



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

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

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

   *  Inport: '<Root>/idq_Target'

   *  Inport: '<S27>/vq_in'

   *  Inport: '<S30>/r_currTgt'

   *  Switch: '<S24>/Switch3'

   */

  if (rtb_BitwiseOperator2 == 1) {

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

     *  Constant: '<S24>/Constant2'

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

     *  Inport: '<Root>/vdq_Open'

     *  Inport: '<S27>/vd_in'

     */

    if (rtb_LogicalOperator12) {

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

       *  ActionPort: '<S27>/Action Port'

       */

      rtDW->Merge[0] = rtU->vdq_Open[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: '<S24>/Switch2' */



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

     *  ActionPort: '<S27>/Action Port'

     */

    rtDW->Merge[1] = rtb_Sum6_p;



    /* 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: '<S31>/Relational Operator' incorporates:

     *  Switch: '<S24>/Switch3'

     *  UnitDelay: '<S31>/UnitDelay'

     */

    rtb_LogicalOperator12 = (rtb_Sum6_p != rtDW->UnitDelay_DSTATE_e);



    /* If: '<S32>/If' */

    if (rtb_LogicalOperator12) {

      /* Outputs for IfAction SubSystem: '<S32>/RateInit' incorporates:

       *  ActionPort: '<S33>/Action Port'

       */

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

       *  Switch: '<S24>/Switch3'

       *  UnitDelay: '<S6>/UnitDelay1'

       */

      rtb_Divide1_m = (int16_T)((rtb_Sum6_p - rtDW->UnitDelay1_DSTATE_f[1]) >> 1);



      /* Signum: '<S33>/Sign' incorporates:

       *  Sum: '<S33>/Add'

       */

      if (rtb_Divide1_m < 0) {

        rtb_Divide1_m = -1;

      } else {

        rtb_Divide1_m = (int16_T)(rtb_Divide1_m > 0);

      }



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



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

       *  Constant: '<S29>/Constant5'

       */

      rtDW->Divide = (int16_T)(rtP.dz_OpenStepVol * rtb_Divide1_m);



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

       *  Switch: '<S24>/Switch3'

       *  UnitDelay: '<S6>/UnitDelay1'

       */

      if (rtb_Sum6_p > rtDW->UnitDelay1_DSTATE_f[1]) {

        /* MinMax: '<S33>/Max' */

        rtDW->Max_p = rtb_Sum6_p;

      } else {

        /* MinMax: '<S33>/Max' */

        rtDW->Max_p = rtDW->UnitDelay1_DSTATE_f[1];

      }



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



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

       *  Switch: '<S24>/Switch3'

       *  UnitDelay: '<S6>/UnitDelay1'

       */

      if (rtDW->UnitDelay1_DSTATE_f[1] < rtb_Sum6_p) {

        /* MinMax: '<S33>/Max1' */

        rtDW->Max1_g = rtDW->UnitDelay1_DSTATE_f[1];

      } else {

        /* MinMax: '<S33>/Max1' */

        rtDW->Max1_g = rtb_Sum6_p;

      }



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

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



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

       *  UnitDelay: '<S6>/UnitDelay1'

       */

      rtb_r_cos_M1 = rtDW->UnitDelay1_DSTATE_f[1];

    } else {

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

       *  UnitDelay: '<S36>/UnitDelay'

       */

      rtb_r_cos_M1 = rtDW->UnitDelay_DSTATE_fv;

    }



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



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

     *  Constant: '<S32>/Constant'

     *  Product: '<S33>/Divide'

     *  RelationalOperator: '<S32>/Equal'

     *  Switch: '<S24>/Switch3'

     *  UnitDelay: '<S32>/Unit Delay'

     */

    if (rtb_Sum6_p != rtDW->UnitDelay_DSTATE_i) {

      rtb_Divide1_m = rtDW->Divide;

    } else {

      rtb_Divide1_m = 0;

    }



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



    /* Sum: '<S35>/Add2' */

    rtb_Divide_idx_0 = ((rtb_r_cos_M1 << 2) + rtb_Divide1_m) >> 2;

    if (rtb_Divide_idx_0 > 32767) {

      rtb_Divide_idx_0 = 32767;

    } else {

      if (rtb_Divide_idx_0 < -32768) {

        rtb_Divide_idx_0 = -32768;

      }

    }



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

     *  MinMax: '<S33>/Max'

     *  MinMax: '<S33>/Max1'

     *  RelationalOperator: '<S34>/LowerRelop1'

     *  RelationalOperator: '<S34>/UpperRelop'

     *  Sum: '<S35>/Add2'

     *  Switch: '<S34>/Switch'

     */

    if ((int16_T)rtb_Divide_idx_0 > rtDW->Max_p) {

      rtb_r_cos_M1 = rtDW->Max_p;

    } else if ((int16_T)rtb_Divide_idx_0 < rtDW->Max1_g) {

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

       *  MinMax: '<S33>/Max1'

       *  Switch: '<S34>/Switch2'

       */

      rtb_r_cos_M1 = rtDW->Max1_g;

    } else {

      rtb_r_cos_M1 = (int16_T)rtb_Divide_idx_0;

    }



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



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

     *  Constant: '<S29>/Constant3'

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

     */

    rtDW->Merge[0] = 0;



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

     *  Switch: '<S24>/Switch'

     */

    if (rtb_Switch_np > 0) {

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

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

       *  Switch: '<S34>/Switch2'

       */

      rtDW->Merge[1] = rtb_r_cos_M1;

    } else {

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

       *  Constant: '<S29>/Constant1'

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

       */

      rtDW->Merge[1] = 0;

    }



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



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

     *  Switch: '<S24>/Switch3'

     */

    rtDW->UnitDelay_DSTATE_e = rtb_Sum6_p;



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

    if (rtb_LogicalOperator12) {

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

       *  UnitDelay: '<S6>/UnitDelay1'

       */

      rtDW->UnitDelay_DSTATE_fv = rtDW->UnitDelay1_DSTATE_f[1];

    } else {

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

       *  Sum: '<S35>/Add2'

       */

      rtDW->UnitDelay_DSTATE_fv = (int16_T)rtb_Divide_idx_0;

    }



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



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

     *  Switch: '<S34>/Switch2'

     */

    rtDW->UnitDelay_DSTATE_i = rtb_r_cos_M1;



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



    /* Merge: '<S26>/Merge1' incorporates:

     *  Inport: '<Root>/idq_Target'

     *  Inport: '<S30>/r_currTgt'

     *  Inport: '<S30>/r_spdTgt'

     *  Switch: '<S24>/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: '<S24>/Switch'

     */

    rtDW->Merge1 = rtb_Switch_np;



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

  }



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



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

   *  Inport: '<Root>/spd_Limit'

   *  Merge: '<S26>/Merge1'

   *  RelationalOperator: '<S59>/LowerRelop1'

   *  RelationalOperator: '<S59>/UpperRelop'

   *  Switch: '<S59>/Switch'

   */

  if (rtDW->Merge1 > rtU->spd_Limit) {

    rtb_Switch_np = rtU->spd_Limit;

  } else if (rtDW->Merge1 < 0) {

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

     *  Constant: '<S49>/Constant'

     *  Switch: '<S59>/Switch2'

     */

    rtb_Switch_np = 0;

  } else {

    rtb_Switch_np = rtDW->Merge1;

  }



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



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

   *  Constant: '<S76>/Constant'

   *  Logic: '<S71>/Logical Operator'

   *  Switch: '<S71>/Switch2'

   */

  rtb_Sum2 = -1;

  if ((rtb_z_ctrlMod != 0) && rtb_Equal_k) {

    rtb_Sum2 = 0;



    /* Outputs for IfAction SubSystem: '<S53>/Do_Calc' incorporates:

     *  ActionPort: '<S70>/Action Port'

     */

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

    Low_Pass_Filter(rtb_UnitDelay1_m, rtP.f_lpf_vdq,

                    rtb_TmpSignalConversionAtLow_Pa, &rtDW->Low_Pass_Filter_h);



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



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

     *  Constant: '<S76>/Constant'

     *  RelationalOperator: '<S70>/Equal'

     *  UnitDelay: '<S70>/Unit Delay'

     */

    rtb_DataTypeConversion_j = (uint8_T)(rtDW->UnitDelay_DSTATE_p2 !=

      rtb_z_ctrlMod);



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

     *  Constant: '<S87>/Constant1'

     *  Constant: '<S87>/Constant11'

     *  Constant: '<S87>/Constant4'

     *  Gain: '<S70>/Gain'

     *  Sum: '<S87>/Sum1'

     *  Switch: '<S14>/Switch2'

     *  Switch: '<S59>/Switch2'

     *  UnitDelay: '<S70>/Unit Delay1'

     */

    if (rtb_z_ctrlMod == 1) {

      rtDW->If_ActiveSubsystem_h = 0;



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

       *  ActionPort: '<S87>/Action Port'

       */

      /* MinMax: '<S87>/Min' incorporates:

       *  Constant: '<S87>/Constant6'

       *  UnitDelay: '<S87>/Unit Delay'

       */

      if (rtP.i_dqMax < rtDW->UnitDelay_DSTATE_l) {

        rtb_Switch_f2_idx_1 = rtP.i_dqMax;

      } else {

        rtb_Switch_f2_idx_1 = rtDW->UnitDelay_DSTATE_l;

      }



      /* End of MinMax: '<S87>/Min' */



      /* MinMax: '<S87>/Min1' incorporates:

       *  Constant: '<S87>/Constant6'

       *  Gain: '<S87>/Gain'

       *  Gain: '<S87>/Gain1'

       *  UnitDelay: '<S87>/Unit Delay'

       */

      if ((int16_T)-rtDW->UnitDelay_DSTATE_l > (int16_T)-rtP.i_dqMax) {

        rtb_Min = (int16_T)-rtDW->UnitDelay_DSTATE_l;

      } else {

        rtb_Min = (int16_T)-rtP.i_dqMax;

      }



      /* End of MinMax: '<S87>/Min1' */



      /* Outputs for Atomic SubSystem: '<S87>/PI_Speed' */

      rtb_Sum1_f = PI_backCalc_fixdt(rtb_Switch_np - rtb_Switch3, rtP.cf_nKp,

        rtP.cf_nKi, rtP.cf_nKb, rtb_Switch_f2_idx_1, rtb_Min, (int16_T)

        ((rtP.cf_lastIqGain * rtDW->UnitDelay1_DSTATE_g) >> 15),

        rtb_DataTypeConversion_j, &rtDW->PI_Speed, &rtPrevZCX->PI_Speed);



      /* End of Outputs for SubSystem: '<S87>/PI_Speed' */



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

       *  Constant: '<S87>/Constant1'

       *  Constant: '<S87>/Constant11'

       *  Constant: '<S87>/Constant4'

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

       *  Gain: '<S70>/Gain'

       *  Sum: '<S87>/Sum1'

       *  Switch: '<S14>/Switch2'

       *  Switch: '<S59>/Switch2'

       *  Switch: '<S91>/Switch2'

       *  UnitDelay: '<S70>/Unit Delay1'

       */

      rtDW->Merge_f = (int16_T)(rtb_Sum1_f >> 9);



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

    } else {

      rtDW->If_ActiveSubsystem_h = 1;



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

       *  ActionPort: '<S88>/Action Port'

       */

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

       *  Switch: '<S14>/Switch2'

       *  Switch: '<S59>/Switch2'

       */

      rtb_Switch_np -= rtb_Switch3;



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

       *  Inport: '<S30>/r_currTgt'

       */

      if (rtDW->icLoad_p != 0) {

        rtDW->Delay_DSTATE = rtDW->r_currTgt;

      }



      /* MinMax: '<S88>/Min' incorporates:

       *  Delay: '<S88>/Delay'

       *  Inport: '<S30>/r_currTgt'

       */

      if (rtDW->r_currTgt < rtDW->Delay_DSTATE) {

        rtb_Min = rtDW->r_currTgt;

      } else {

        rtb_Min = rtDW->Delay_DSTATE;

      }



      /* End of MinMax: '<S88>/Min' */



      /* Outputs for Atomic SubSystem: '<S88>/PI_TrqSpdLim' */

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

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

       *  Inport: '<S30>/r_currTgt'

       */

      if ((rtb_DataTypeConversion_j > 0) &&

          (rtPrevZCX->ResettableDelay_Reset_ZCE_a != 1)) {

        rtDW->icLoad_k = 1U;

      }



      rtPrevZCX->ResettableDelay_Reset_ZCE_a = (ZCSigState)

        (rtb_DataTypeConversion_j > 0);

      if (rtDW->icLoad_k != 0) {

        rtDW->ResettableDelay_DSTATE_c = rtDW->r_currTgt << 7;

      }



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

       *  Constant: '<S88>/Constant1'

       *  Sum: '<S88>/Sum1'

       */

      tmp_2 = (int64_T)rtb_Switch_np * rtP.cf_TrqLimKi;

      if (tmp_2 > 2147483647LL) {

        tmp_2 = 2147483647LL;

      } else {

        if (tmp_2 < -2147483648LL) {

          tmp_2 = -2147483648LL;

        }

      }



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

       *  Product: '<S92>/Divide1'

       *  UnitDelay: '<S92>/Unit Delay'

       */

      if (((int32_T)tmp_2 < 0) && (rtDW->UnitDelay_DSTATE_n < MIN_int32_T

           - (int32_T)tmp_2)) {

        rtb_Divide_idx_0 = MIN_int32_T;

      } else if (((int32_T)tmp_2 > 0) && (rtDW->UnitDelay_DSTATE_n > MAX_int32_T

                  - (int32_T)tmp_2)) {

        rtb_Divide_idx_0 = MAX_int32_T;

      } else {

        rtb_Divide_idx_0 = (int32_T)tmp_2 + rtDW->UnitDelay_DSTATE_n;

      }



      /* End of Sum: '<S92>/Sum2' */



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

       *  Delay: '<S93>/Resettable Delay'

       */

      tmp_2 = (((int64_T)rtDW->ResettableDelay_DSTATE_c << 2) + rtb_Divide_idx_0)

        >> 2;

      if (tmp_2 > 2147483647LL) {

        tmp_2 = 2147483647LL;

      } else {

        if (tmp_2 < -2147483648LL) {

          tmp_2 = -2147483648LL;

        }

      }



      rtb_Sum1_f = (int32_T)tmp_2;



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



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

       *  Constant: '<S88>/Constant4'

       *  Sum: '<S88>/Sum1'

       */

      tmp_2 = (int64_T)rtb_Switch_np * rtP.cf_TrqLimKp;

      if (tmp_2 > 2147483647LL) {

        tmp_2 = 2147483647LL;

      } else {

        if (tmp_2 < -2147483648LL) {

          tmp_2 = -2147483648LL;

        }

      }



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

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

       *  Product: '<S92>/Divide4'

       *  Sum: '<S93>/Sum1'

       */

      tmp_2 = (int64_T)(rtb_Sum1_f << 2) + (int32_T)tmp_2;

      if (tmp_2 > 2147483647LL) {

        tmp_2 = 2147483647LL;

      } else {

        if (tmp_2 < -2147483648LL) {

          tmp_2 = -2147483648LL;

        }

      }



      rtb_Switch_np = (int32_T)tmp_2;



      /* End of Sum: '<S92>/Sum6' */



      /* RelationalOperator: '<S94>/LowerRelop1' incorporates:

       *  MinMax: '<S88>/Min'

       *  Switch: '<S94>/Switch2'

       */

      rtb_Divide_idx_0 = rtb_Min << 9;



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

       *  RelationalOperator: '<S94>/LowerRelop1'

       *  Sum: '<S92>/Sum6'

       */

      if (rtb_Switch_np > rtb_Divide_idx_0) {

        rtb_Gain_b0 = rtb_Divide_idx_0;

      } else {

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

         *  MinMax: '<S88>/Min'

         */

        rtb_Gain_b0 = -32768 * rtb_Min;



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

         *  Gain: '<S92>/Gain'

         *  RelationalOperator: '<S94>/UpperRelop'

         *  Switch: '<S94>/Switch2'

         */

        if (((int64_T)rtb_Switch_np << 6) < rtb_Gain_b0) {

          rtb_Gain_b0 >>= 6;

        } else {

          rtb_Gain_b0 = rtb_Switch_np;

        }



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

      }



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

       *  Constant: '<S88>/Constant2'

       *  Product: '<S92>/Divide2'

       *  Sum: '<S92>/Sum3'

       *  Sum: '<S92>/Sum6'

       *  Switch: '<S94>/Switch2'

       */

      rtDW->UnitDelay_DSTATE_n = (int32_T)(((int64_T)(rtb_Gain_b0 -

        rtb_Switch_np) * rtP.cf_TrqLimKb) >> 10);



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

       *  Sum: '<S93>/Sum1'

       */

      rtDW->icLoad_k = 0U;

      rtDW->ResettableDelay_DSTATE_c = rtb_Sum1_f;



      /* End of Outputs for SubSystem: '<S88>/PI_TrqSpdLim' */



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

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

       *  ManualSwitch: '<S88>/Manual Switch'

       *  Switch: '<S94>/Switch2'

       */

      rtDW->Merge_f = (int16_T)(rtb_Gain_b0 >> 9);



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

    }



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



    /* Outputs for IfAction SubSystem: '<S75>/MTPA_Calc' incorporates:

     *  ActionPort: '<S80>/Action Port'

     */

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

     *  Constant: '<S80>/Constant3'

     *  Merge: '<S75>/Merge'

     *  Switch: '<S80>/Switch'

     */

    rtDW->Merge_c[0] = 0;

    rtDW->Merge_c[1] = rtDW->Merge_f;



    /* End of Outputs for SubSystem: '<S75>/MTPA_Calc' */



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

     *  Constant: '<S74>/Constant3'

     *  UnitDelay: '<S74>/Unit Delay1'

     */

    rtb_Divide_idx_0 = (rtP.V_modulation - rtDW->UnitDelay1_DSTATE_c) >> 1;

    if (rtb_Divide_idx_0 < -32768) {

      rtb_Divide_idx_0 = -32768;

    }



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

     *  Constant: '<S74>/Constant4'

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

     */

    if ((rtb_DataTypeConversion_j > 0) &&

        (rtPrevZCX->ResettableDelay_Reset_ZCE_o != 1)) {

      rtDW->icLoad = 1U;

    }



    rtPrevZCX->ResettableDelay_Reset_ZCE_o = (ZCSigState)

      (rtb_DataTypeConversion_j > 0);

    if (rtDW->icLoad != 0) {

      rtDW->ResettableDelay_DSTATE = 0;

    }



    /* Signum: '<S74>/Sign' incorporates:

     *  Sum: '<S74>/Sum'

     */

    if ((int16_T)rtb_Divide_idx_0 < 0) {

      rtb_Switch_f2_idx_1 = -1;

    } else {

      rtb_Switch_f2_idx_1 = (int16_T)((int16_T)rtb_Divide_idx_0 > 0);

    }



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



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

     *  Constant: '<S74>/Constant2'

     *  Constant: '<S74>/Constant5'

     *  Delay: '<S78>/Resettable Delay'

     *  Product: '<S77>/Divide'

     *  Product: '<S77>/Divide1'

     *  Sum: '<S77>/Add'

     *  UnitDelay: '<S77>/Unit Delay'

     */

    rtb_Sum1_f = (((((rtP.cf_Fw_Kb * rtDW->UnitDelay_DSTATE) << 6) >> 12) +

                   rtb_Switch_f2_idx_1 * rtP.cf_Fw_Ki) >> 4) +

      rtDW->ResettableDelay_DSTATE;



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

     *  Constant: '<S77>/Constant6'

     *  RelationalOperator: '<S79>/LowerRelop1'

     *  Sum: '<S78>/Sum1'

     */

    if (rtb_Sum1_f > 0) {

      rtb_Divide_idx_1 = 0;

    } else {

      /* Gain: '<S74>/Gain1' */

      rtb_Switch_np = -32768 * rtDW->Merge_c[1];



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

       *  Constant: '<S74>/Constant6'

       *  Gain: '<S74>/Gain1'

       */

      rtb_Divide_idx_0 = rtP.id_fieldWeakMax << 15;

      if (rtb_Switch_np <= rtb_Divide_idx_0) {

        rtb_Switch_np = rtb_Divide_idx_0;

      }



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



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

       *  MinMax: '<S74>/Max'

       *  RelationalOperator: '<S79>/UpperRelop'

       *  Switch: '<S79>/Switch2'

       */

      if (((int64_T)rtb_Sum1_f << 14) < rtb_Switch_np) {

        rtb_Divide_idx_1 = rtb_Switch_np >> 14;

      } else {

        rtb_Divide_idx_1 = rtb_Sum1_f;

      }



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

    }



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



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

     *  Product: '<S76>/Divide1'

     *  Switch: '<S79>/Switch2'

     */

    rtb_Min = (int16_T)((rtb_Divide_idx_1 >> 1) + rtDW->Merge_c[0]);



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

    rtb_Switch_np = 1;

    rtb_Gain_b0 = 0;



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

     *  Math: '<S74>/Math Function1'

     *  Math: '<S74>/Math Function2'

     *  Merge: '<S75>/Merge'

     *  Product: '<S76>/Divide1'

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

     *  Sum: '<S74>/Sum2'

     */

    rtb_Switch_f2_idx_1 = rt_sqrt_Us32En6_Ys16En_1bhh77n4((((rtDW->Merge_c[0] *

      rtDW->Merge_c[0] + rtDW->Merge_c[1] * rtDW->Merge_c[1]) >> 1) - ((rtb_Min *

      rtb_Min) >> 1)) >> 3);



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

     *  Inport: '<Root>/iDC_Limit'

     *  Inport: '<Root>/vDC'

     *  Math: '<S86>/Math Function2'

     *  Product: '<S49>/Divide'

     *  Product: '<S76>/Divide'

     *  Switch: '<S74>/Switch'

     */

    rtb_MathFunction2_p = rtU->iDC_Limit * rtU->vDC - rtb_Min *

      rtb_TmpSignalConversionAtLow_Pa[0];



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

     *  Constant: '<S76>/Constant5'

     *  Gain: '<S76>/Gain'

     *  Math: '<S86>/Math Function2'

     */

    rtb_Divide_idx_0 = rtb_MathFunction2_p / (rtP.i_dqMax << 1);

    if (rtb_Divide_idx_0 > 32767) {

      rtb_Divide_idx_0 = 32767;

    } else {

      if (rtb_Divide_idx_0 < -32768) {

        rtb_Divide_idx_0 = -32768;

      }

    }



    /* MinMax: '<S76>/Min2' incorporates:

     *  Product: '<S76>/Divide3'

     */

    if (rtb_TmpSignalConversionAtLow_Pa[1] > (int16_T)rtb_Divide_idx_0) {

      rtb_Divide1_m = rtb_TmpSignalConversionAtLow_Pa[1];

    } else {

      rtb_Divide1_m = (int16_T)rtb_Divide_idx_0;

    }



    /* End of MinMax: '<S76>/Min2' */



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

     *  Math: '<S86>/Math Function2'

     */

    rtb_Divide_idx_0 = rtb_MathFunction2_p / rtb_Divide1_m;

    if (rtb_Divide_idx_0 > 32767) {

      rtb_Divide_idx_0 = 32767;

    } else {

      if (rtb_Divide_idx_0 < -32768) {

        rtb_Divide_idx_0 = -32768;

      }

    }



    /* Signum: '<S76>/Sign' incorporates:

     *  Sqrt: '<S74>/Sqrt'

     */

    if (rtb_Switch_f2_idx_1 < 0) {

      rtb_Sum6_p = -1;

    } else {

      rtb_Sum6_p = (int16_T)(rtb_Switch_f2_idx_1 > 0);

    }



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



    /* Product: '<S76>/Divide2' incorporates:

     *  Product: '<S76>/Divide1'

     */

    rtb_r_cos_M1 = (int16_T)((int16_T)rtb_Divide_idx_0 * rtb_Sum6_p);



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

     *  Constant: '<S76>/Constant2'

     *  Constant: '<S76>/Constant3'

     *  Gain: '<S76>/Gain1'

     *  Product: '<S76>/Divide2'

     *  RelationalOperator: '<S85>/LowerRelop1'

     *  RelationalOperator: '<S85>/UpperRelop'

     *  Switch: '<S85>/Switch'

     */

    if (rtb_r_cos_M1 > rtP.i_dqMax) {

      rtb_r_cos_M1 = rtP.i_dqMax;

    } else {

      if (rtb_r_cos_M1 < (int16_T)-rtP.i_dqMax) {

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

         *  Constant: '<S76>/Constant2'

         *  Gain: '<S76>/Gain1'

         *  Switch: '<S85>/Switch2'

         */

        rtb_r_cos_M1 = (int16_T)-rtP.i_dqMax;

      }

    }



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



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

     *  MinMax: '<S76>/Min1'

     *  Sqrt: '<S74>/Sqrt'

     *  Switch: '<S85>/Switch2'

     */

    if (rtb_Sum6_p > 0) {

      /* MinMax: '<S76>/Min' incorporates:

       *  Sqrt: '<S74>/Sqrt'

       *  Switch: '<S85>/Switch2'

       */

      if (rtb_r_cos_M1 < rtb_Switch_f2_idx_1) {

        /* Switch: '<S76>/Switch' */

        rtDW->Switch = rtb_r_cos_M1;

      } else {

        /* Switch: '<S76>/Switch' */

        rtDW->Switch = rtb_Switch_f2_idx_1;

      }



      /* End of MinMax: '<S76>/Min' */

    } else if (rtb_r_cos_M1 > rtb_Switch_f2_idx_1) {

      /* MinMax: '<S76>/Min1' incorporates:

       *  Switch: '<S76>/Switch'

       *  Switch: '<S85>/Switch2'

       */

      rtDW->Switch = rtb_r_cos_M1;

    } else {

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

       *  Sqrt: '<S74>/Sqrt'

       */

      rtDW->Switch = rtb_Switch_f2_idx_1;

    }



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



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

     *  Constant: '<S76>/Constant1'

     *  Constant: '<S76>/Constant2'

     *  Gain: '<S76>/Gain1'

     *  RelationalOperator: '<S84>/LowerRelop1'

     *  RelationalOperator: '<S84>/UpperRelop'

     *  Switch: '<S74>/Switch'

     *  Switch: '<S84>/Switch'

     */

    if (rtb_Min > rtP.i_dqMax) {

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

      rtDW->Switch2 = rtP.i_dqMax;

    } else if (rtb_Min < (int16_T)-rtP.i_dqMax) {

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

       *  Constant: '<S76>/Constant2'

       *  Gain: '<S76>/Gain1'

       *  Switch: '<S84>/Switch2'

       */

      rtDW->Switch2 = (int16_T)-rtP.i_dqMax;

    } else {

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

      rtDW->Switch2 = rtb_Min;

    }



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



    /* Sqrt: '<S86>/Sqrt1' incorporates:

     *  Math: '<S86>/Math Function2'

     *  Math: '<S86>/Math Function3'

     *  Product: '<S76>/Divide1'

     *  Sum: '<S86>/Add'

     *  Switch: '<S74>/Switch'

     */

    rtb_Min = rt_sqrt_Us32En10_Ys16E_7VJYwqF9(rtb_Min * rtb_Min + (int16_T)

      rtb_Divide_idx_0 * (int16_T)rtb_Divide_idx_0);



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

     *  Sum: '<S78>/Sum1'

     *  Switch: '<S79>/Switch2'

     */

    rtb_MathFunction2_p = rtb_Divide_idx_1 - rtb_Sum1_f;



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

  }



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

   *  Switch: '<S84>/Switch2'

   *  UnitDelay: '<S106>/UnitDelay'

   */

  rtb_Equal_k = (rtDW->Switch2 != rtDW->UnitDelay_DSTATE_h);



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

   *  Product: '<S60>/Divide1'

   *  Switch: '<S84>/Switch2'

   *  UnitDelay: '<S97>/Unit Delay1'

   */

  rtb_Sum6_p = (int16_T)(rtDW->Switch2 - rtDW->UnitDelay1_DSTATE_i);



  /* Abs: '<S97>/Abs' incorporates:

   *  Product: '<S60>/Divide1'

   */

  if (rtb_Sum6_p < 0) {

    rtb_Sum6_p = (int16_T)-rtb_Sum6_p;

  }



  /* End of Abs: '<S97>/Abs' */



  /* Outputs for Enabled SubSystem: '<S97>/Enabled Subsystem' incorporates:

   *  EnablePort: '<S107>/Enable'

   */

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

   *  Gain: '<S97>/Gain'

   *  Product: '<S60>/Divide1'

   *  UnitDelay: '<S97>/Unit Delay1'

   */

  if (rtb_Equal_k) {

    /* Outputs for IfAction SubSystem: '<S108>/RateInit' incorporates:

     *  ActionPort: '<S109>/Action Port'

     */

    RateInit(rtDW->UnitDelay1_DSTATE_i, rtDW->Switch2, (int16_T)((13107 *

               rtb_Sum6_p) >> 13), &rtDW->Divide_n, &rtDW->Max_g, &rtDW->Max1_j);



    /* End of Outputs for SubSystem: '<S108>/RateInit' */



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

     *  Gain: '<S97>/Gain'

     *  Product: '<S60>/Divide1'

     *  UnitDelay: '<S97>/Unit Delay1'

     */

    rtb_Divide1_m = rtDW->UnitDelay1_DSTATE_i;

  } else {

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

     *  UnitDelay: '<S112>/UnitDelay'

     */

    rtb_Divide1_m = rtDW->UnitDelay_DSTATE_b;

  }



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

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



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

   *  Constant: '<S108>/Constant'

   *  Product: '<S109>/Divide'

   *  RelationalOperator: '<S108>/Equal'

   *  Switch: '<S84>/Switch2'

   *  UnitDelay: '<S108>/Unit Delay'

   */

  if (rtDW->Switch2 != rtDW->UnitDelay_DSTATE_g) {

    rtb_Switch_f2_idx_1 = rtDW->Divide_n;

  } else {

    rtb_Switch_f2_idx_1 = 0;

  }



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



  /* Sum: '<S111>/Add2' */

  rtb_Divide_idx_0 = ((rtb_Divide1_m << 5) + rtb_Switch_f2_idx_1) >> 5;

  if (rtb_Divide_idx_0 > 32767) {

    rtb_Divide_idx_0 = 32767;

  } else {

    if (rtb_Divide_idx_0 < -32768) {

      rtb_Divide_idx_0 = -32768;

    }

  }



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

   *  MinMax: '<S109>/Max'

   *  MinMax: '<S109>/Max1'

   *  RelationalOperator: '<S110>/LowerRelop1'

   *  RelationalOperator: '<S110>/UpperRelop'

   *  Sum: '<S111>/Add2'

   *  Switch: '<S110>/Switch'

   */

  if ((int16_T)rtb_Divide_idx_0 > rtDW->Max_g) {

    rtb_Divide1_m = rtDW->Max_g;

  } else if ((int16_T)rtb_Divide_idx_0 < rtDW->Max1_j) {

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

     *  MinMax: '<S109>/Max1'

     *  Switch: '<S110>/Switch2'

     */

    rtb_Divide1_m = rtDW->Max1_j;

  } else {

    rtb_Divide1_m = (int16_T)rtb_Divide_idx_0;

  }



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



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

   *  Switch: '<S76>/Switch'

   *  UnitDelay: '<S113>/UnitDelay'

   */

  rtb_LogicalOperator12 = (rtDW->Switch != rtDW->UnitDelay_DSTATE_o);



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

   *  Product: '<S60>/Divide1'

   *  Switch: '<S76>/Switch'

   *  UnitDelay: '<S98>/Unit Delay1'

   */

  rtb_Sum6_p = (int16_T)(rtDW->Switch - rtDW->UnitDelay1_DSTATE_b);



  /* Abs: '<S98>/Abs' incorporates:

   *  Product: '<S60>/Divide1'

   */

  if (rtb_Sum6_p < 0) {

    rtb_Sum6_p = (int16_T)-rtb_Sum6_p;

  }



  /* End of Abs: '<S98>/Abs' */



  /* Outputs for Enabled SubSystem: '<S98>/Enabled Subsystem' incorporates:

   *  EnablePort: '<S114>/Enable'

   */

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

   *  Gain: '<S98>/Gain'

   *  Product: '<S60>/Divide1'

   *  UnitDelay: '<S98>/Unit Delay1'

   */

  if (rtb_LogicalOperator12) {

    /* Outputs for IfAction SubSystem: '<S115>/RateInit' incorporates:

     *  ActionPort: '<S116>/Action Port'

     */

    RateInit(rtDW->UnitDelay1_DSTATE_b, rtDW->Switch, (int16_T)((13107 *

               rtb_Sum6_p) >> 13), &rtDW->Divide_l, &rtDW->Max, &rtDW->Max1);



    /* End of Outputs for SubSystem: '<S115>/RateInit' */



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

     *  Gain: '<S98>/Gain'

     *  Product: '<S60>/Divide1'

     *  UnitDelay: '<S98>/Unit Delay1'

     */

    rtb_r_cos_M1 = rtDW->UnitDelay1_DSTATE_b;

  } else {

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

     *  UnitDelay: '<S119>/UnitDelay'

     */

    rtb_r_cos_M1 = rtDW->UnitDelay_DSTATE_d;

  }



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

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



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

   *  Constant: '<S115>/Constant'

   *  Product: '<S116>/Divide'

   *  RelationalOperator: '<S115>/Equal'

   *  Switch: '<S76>/Switch'

   *  UnitDelay: '<S115>/Unit Delay'

   */

  if (rtDW->Switch != rtDW->UnitDelay_DSTATE_a) {

    rtb_Switch_f2_idx_1 = rtDW->Divide_l;

  } else {

    rtb_Switch_f2_idx_1 = 0;

  }



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



  /* Sum: '<S118>/Add2' */

  rtb_Divide_idx_1 = ((rtb_r_cos_M1 << 5) + rtb_Switch_f2_idx_1) >> 5;

  if (rtb_Divide_idx_1 > 32767) {

    rtb_Divide_idx_1 = 32767;

  } else {

    if (rtb_Divide_idx_1 < -32768) {

      rtb_Divide_idx_1 = -32768;

    }

  }



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

   *  MinMax: '<S116>/Max'

   *  MinMax: '<S116>/Max1'

   *  RelationalOperator: '<S117>/LowerRelop1'

   *  RelationalOperator: '<S117>/UpperRelop'

   *  Sum: '<S118>/Add2'

   *  Switch: '<S117>/Switch'

   */

  if ((int16_T)rtb_Divide_idx_1 > rtDW->Max) {

    rtb_r_cos_M1 = rtDW->Max;

  } else if ((int16_T)rtb_Divide_idx_1 < rtDW->Max1) {

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

     *  MinMax: '<S116>/Max1'

     *  Switch: '<S117>/Switch2'

     */

    rtb_r_cos_M1 = rtDW->Max1;

  } else {

    rtb_r_cos_M1 = (int16_T)rtb_Divide_idx_1;

  }



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



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

   *  Logic: '<S54>/Logical Operator'

   *  RelationalOperator: '<S54>/Equal'

   *  UnitDelay: '<S54>/Unit Delay'

   */

  rtb_DataTypeConversion_j = (uint8_T)((rtb_z_ctrlMod != 0) &&

    (rtDW->UnitDelay_DSTATE_bm != rtb_z_ctrlMod));



  /* If: '<S54>/If1' incorporates:

   *  Constant: '<S95>/Constant1'

   *  Constant: '<S95>/Constant3'

   *  Constant: '<S95>/Constant4'

   *  Constant: '<S95>/Constant6'

   *  Constant: '<S95>/Constant7'

   *  Constant: '<S95>/Constant8'

   *  Gain: '<S95>/Gain1'

   *  Gain: '<S95>/Gain2'

   *  Inport: '<S96>/In1'

   *  Merge: '<S26>/Merge'

   *  Merge: '<S54>/Merge'

   *  Outport: '<Root>/f_Idq'

   *  Product: '<S95>/Divide'

   *  Sum: '<S95>/Sum'

   *  Sum: '<S95>/Sum1'

   *  Switch: '<S110>/Switch2'

   *  Switch: '<S117>/Switch2'

   *  UnitDelay: '<S6>/UnitDelay1'

   */

  if (rtb_z_ctrlMod != 0) {

    /* Outputs for IfAction SubSystem: '<S54>/CurrentLoop' incorporates:

     *  ActionPort: '<S95>/Action Port'

     */

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

     *  Constant: '<S95>/Constant2'

     *  Inport: '<Root>/vDC'

     */

    rtb_Sum6_p = (int16_T)((rtU->vDC * rtP.V_modulation) >> 14);



    /* Outputs for Atomic SubSystem: '<S95>/PI_backCalc_fixdt' */

    rtb_Switch_np = PI_backCalc_fixdt_o((int16_T)(rtb_Divide1_m - rtY->f_Idq[0]),

      rtP.cf_idKp, rtP.cf_idKi, rtP.cf_idKb, rtb_Sum6_p, (int16_T)-rtb_Sum6_p,

      rtDW->UnitDelay1_DSTATE_f[0], rtb_DataTypeConversion_j,

      &rtDW->PI_backCalc_fixdt_o3, &rtPrevZCX->PI_backCalc_fixdt_o3);



    /* End of Outputs for SubSystem: '<S95>/PI_backCalc_fixdt' */



    /* Outputs for Atomic SubSystem: '<S95>/PI_backCalc_fixdt1' */

    rtb_Gain_b0 = PI_backCalc_fixdt_o((int16_T)(rtb_r_cos_M1 - rtY->f_Idq[1]),

      rtP.cf_iqKp, rtP.cf_iqKi, rtP.cf_iqKb, rtb_Sum6_p, (int16_T)-rtb_Sum6_p,

      rtDW->UnitDelay1_DSTATE_f[1], rtb_DataTypeConversion_j,

      &rtDW->PI_backCalc_fixdt1, &rtPrevZCX->PI_backCalc_fixdt1);



    /* End of Outputs for SubSystem: '<S95>/PI_backCalc_fixdt1' */



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

     *  Constant: '<S95>/Constant1'

     *  Constant: '<S95>/Constant3'

     *  Constant: '<S95>/Constant4'

     *  Constant: '<S95>/Constant6'

     *  Constant: '<S95>/Constant7'

     *  Constant: '<S95>/Constant8'

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

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

     *  Gain: '<S95>/Gain1'

     *  Gain: '<S95>/Gain2'

     *  Merge: '<S54>/Merge'

     *  Outport: '<Root>/f_Idq'

     *  Product: '<S95>/Divide'

     *  Sum: '<S95>/Sum'

     *  Sum: '<S95>/Sum1'

     *  Switch: '<S103>/Switch2'

     *  Switch: '<S105>/Switch2'

     *  Switch: '<S110>/Switch2'

     *  Switch: '<S117>/Switch2'

     *  UnitDelay: '<S6>/UnitDelay1'

     */

    rtb_TmpSignalConversionAtLow_Pa[0] = (int16_T)(rtb_Switch_np >> 9);

    rtb_TmpSignalConversionAtLow_Pa[1] = (int16_T)(rtb_Gain_b0 >> 9);



    /* End of Outputs for SubSystem: '<S54>/CurrentLoop' */

  } else {

    /* Outputs for IfAction SubSystem: '<S54>/OpenLoop' incorporates:

     *  ActionPort: '<S96>/Action Port'

     */

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

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



    /* End of Outputs for SubSystem: '<S54>/OpenLoop' */

  }



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



  /* Product: '<S51>/Divide2' incorporates:

   *  Constant: '<S51>/Constant'

   *  Inport: '<Root>/vDC'

   *  Product: '<S60>/Divide1'

   */

  rtb_Sum6_p = (int16_T)div_nde_s32_floor(rtU->vDC << 14, rtP.V_modulation);



  /* Sum: '<S51>/Sum of Elements' incorporates:

   *  Math: '<S51>/Math Function'

   *  Merge: '<S54>/Merge'

   */

  tmp_2 = (int64_T)((rtb_TmpSignalConversionAtLow_Pa[0] *

                     rtb_TmpSignalConversionAtLow_Pa[0]) >> 4) +

    ((rtb_TmpSignalConversionAtLow_Pa[1] * rtb_TmpSignalConversionAtLow_Pa[1]) >>

     4);

  if (tmp_2 > 2147483647LL) {

    tmp_2 = 2147483647LL;

  } else {

    if (tmp_2 < -2147483648LL) {

      tmp_2 = -2147483648LL;

    }

  }



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

   *  Math: '<S51>/Math Function1'

   *  Product: '<S60>/Divide1'

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

   */

  tmp_2 = ((int64_T)(int32_T)tmp_2 << 14) / ((rtb_Sum6_p * rtb_Sum6_p) >> 4);

  if (tmp_2 < 0LL) {

    tmp_2 = 0LL;

  } else {

    if (tmp_2 > 65535LL) {

      tmp_2 = 65535LL;

    }

  }



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

   *  Product: '<S51>/Divide'

   */

  rtb_BitwiseOperator2 = rt_sqrt_Uu16En14_Yu16E_WMwW1mku((uint16_T)tmp_2);



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

   *  Merge: '<S54>/Merge'

   *  Sqrt: '<S51>/Sqrt'

   */

  if (rtb_BitwiseOperator2 > 16384) {

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

     *  Merge: '<S54>/Merge'

     *  MultiPortSwitch: '<S51>/Multiport Switch'

     *  Product: '<S51>/Divide1'

     */

    rtb_Switch_f2_idx_0 = (int16_T)((rtb_TmpSignalConversionAtLow_Pa[0] << 14) /

      rtb_BitwiseOperator2);

    rtb_Switch_f2_idx_1 = (int16_T)((rtb_TmpSignalConversionAtLow_Pa[1] << 14) /

      rtb_BitwiseOperator2);

  } else {

    rtb_Switch_f2_idx_0 = rtb_TmpSignalConversionAtLow_Pa[0];

    rtb_Switch_f2_idx_1 = rtb_TmpSignalConversionAtLow_Pa[1];

  }



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



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

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

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

   *  Product: '<S60>/Divide2'

   *  Product: '<S60>/Divide3'

   */

  tmp_0 = (int16_T)((rtb_Switch_f2_idx_0 * rtConstP.pooled8[rtb_LogicalOperator3])

                    >> 14) + (int16_T)((rtb_Switch_f2_idx_1 *

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

  if (tmp_0 > 32767) {

    tmp_0 = 32767;

  } else {

    if (tmp_0 < -32768) {

      tmp_0 = -32768;

    }

  }



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

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

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

   *  Product: '<S60>/Divide1'

   *  Product: '<S60>/Divide4'

   */

  tmp = (int16_T)((rtb_Switch_f2_idx_0 * rtConstP.pooled9[rtb_LogicalOperator3])

                  >> 14) - (int16_T)((rtb_Switch_f2_idx_1 *

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

  if (tmp > 32767) {

    tmp = 32767;

  } else {

    if (tmp < -32768) {

      tmp = -32768;

    }

  }



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

   *  Constant: '<S61>/Constant3'

   *  Sum: '<S60>/Sum1'

   */

  rtb_Sum6_p = (int16_T)((2365 * (int16_T)tmp_0) >> 11);



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

   *  Product: '<S61>/Divide7'

   *  Sum: '<S60>/Sum1'

   *  Sum: '<S60>/Sum6'

   */

  if ((int16_T)tmp_0 >= 0) {

    if ((int16_T)tmp >= 0) {

      if (rtb_Sum6_p > ((int16_T)tmp << 1)) {

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

        rtb_DataTypeConversion_j = 2U;

      } else {

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

        rtb_DataTypeConversion_j = 1U;

      }

    } else {

      rtb_Gain_p2 = -rtb_Sum6_p;

      if (-rtb_Sum6_p > 32767) {

        rtb_Gain_p2 = 32767;

      }



      if (rtb_Gain_p2 > ((int16_T)tmp << 1)) {

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

        rtb_DataTypeConversion_j = 3U;

      } else {

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

        rtb_DataTypeConversion_j = 2U;

      }

    }

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

    rtb_Gain_p2 = -rtb_Sum6_p;

    if (-rtb_Sum6_p > 32767) {

      rtb_Gain_p2 = 32767;

    }



    if (rtb_Gain_p2 > ((int16_T)tmp << 1)) {

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

      rtb_DataTypeConversion_j = 5U;

    } else {

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

      rtb_DataTypeConversion_j = 6U;

    }

  } else if (rtb_Sum6_p > ((int16_T)tmp << 1)) {

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

    rtb_DataTypeConversion_j = 4U;

  } else {

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

    rtb_DataTypeConversion_j = 5U;

  }



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



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

   *  Inport: '<Root>/vDC'

   */

  rtb_Gain_p2 = 18919 * rtU->vDC;



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

   *  Gain: '<S61>/Gain'

   *  Sum: '<S60>/Sum6'

   */

  rtb_Sum6_k = (int16_T)(((int64_T)(int16_T)tmp << 26) / rtb_Gain_p2);



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

   *  Gain: '<S61>/Gain'

   *  Sum: '<S60>/Sum1'

   */

  rtb_Sum1_a = (int16_T)(((int64_T)(int16_T)tmp_0 << 26) / rtb_Gain_p2);



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

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

   */

  switch (rtb_DataTypeConversion_j) {

   case 1:

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

     *  Constant: '<S61>/Constant1'

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

     *  Product: '<S61>/Divide1'

     *  Product: '<S64>/Divide2'

     */

    rtb_Divide3_k = (int16_T)(((int16_T)((rtb_Sum1_a * 9459) >> 13) * (int16_T)

      rtP.i_pwm_count) >> 12);



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

     *  Constant: '<S61>/Constant1'

     *  Constant: '<S64>/Constant'

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

     *  Product: '<S61>/Divide'

     *  Product: '<S61>/Divide1'

     *  Product: '<S64>/Divide'

     *  Sum: '<S64>/Add'

     */

    rtb_Sum1_a = (int16_T)(((int16_T)(rtb_Sum6_k - ((rtb_Sum1_a * 9459) >> 14)) *

      (int16_T)rtP.i_pwm_count) >> 12);



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

     *  Constant: '<S61>/Constant1'

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

     *  Sum: '<S64>/Add1'

     *  Sum: '<S64>/Add2'

     */

    rtb_Sum6_p = (int16_T)((int16_T)((int16_T)((int16_T)rtP.i_pwm_count -

      rtb_Sum1_a) - rtb_Divide3_k) >> 1);



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

    rtb_Sum6_k = (int16_T)(rtb_Sum6_p + rtb_Divide3_k);



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

     *  Sum: '<S64>/Add4'

     */

    rtY->pwm_Duty[0] = (int16_T)(rtb_Sum6_k + rtb_Sum1_a);

    rtY->pwm_Duty[1] = rtb_Sum6_k;

    rtY->pwm_Duty[2] = rtb_Sum6_p;

    break;



   case 2:

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

     *  Constant: '<S61>/Constant1'

     *  Constant: '<S65>/Constant'

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

     *  Product: '<S61>/Divide'

     *  Product: '<S61>/Divide1'

     *  Product: '<S65>/Divide'

     *  Sum: '<S65>/Add'

     */

    rtb_Divide3_k = (int16_T)(((int16_T)(((rtb_Sum1_a * 9459) >> 14) +

      rtb_Sum6_k) * (int16_T)rtP.i_pwm_count) >> 12);



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

     *  Constant: '<S61>/Constant1'

     *  Constant: '<S65>/Constant'

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

     *  Product: '<S61>/Divide'

     *  Product: '<S61>/Divide1'

     *  Product: '<S65>/Divide2'

     *  Sum: '<S65>/Add5'

     */

    rtb_Sum1_a = (int16_T)(((int16_T)(((rtb_Sum1_a * 9459) >> 14) - rtb_Sum6_k) *

      (int16_T)rtP.i_pwm_count) >> 12);



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

     *  Constant: '<S61>/Constant1'

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

     *  Sum: '<S65>/Add1'

     *  Sum: '<S65>/Add2'

     */

    rtb_Sum6_p = (int16_T)((int16_T)((int16_T)((int16_T)rtP.i_pwm_count -

      rtb_Sum1_a) - rtb_Divide3_k) >> 1);



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

    rtb_Sum6_k = (int16_T)(rtb_Sum6_p + rtb_Divide3_k);



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

     *  Sum: '<S65>/Add4'

     */

    rtY->pwm_Duty[0] = rtb_Sum6_k;

    rtY->pwm_Duty[1] = (int16_T)(rtb_Sum6_k + rtb_Sum1_a);

    rtY->pwm_Duty[2] = rtb_Sum6_p;

    break;



   case 3:

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

     *  Constant: '<S61>/Constant1'

     *  Constant: '<S66>/Constant'

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

     *  Product: '<S61>/Divide'

     *  Product: '<S61>/Divide1'

     *  Product: '<S66>/Divide'

     *  Sum: '<S66>/Add'

     */

    rtb_Sum6_k = (int16_T)(((int16_T)(-rtb_Sum6_k - ((rtb_Sum1_a * 9459) >> 14))

      * (int16_T)rtP.i_pwm_count) >> 12);



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

     *  Constant: '<S61>/Constant1'

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

     *  Product: '<S61>/Divide1'

     *  Product: '<S66>/Divide2'

     */

    rtb_Sum1_a = (int16_T)(((int16_T)((rtb_Sum1_a * 9459) >> 13) * (int16_T)

      rtP.i_pwm_count) >> 12);



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

     *  Constant: '<S61>/Constant1'

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

     *  Sum: '<S66>/Add1'

     *  Sum: '<S66>/Add2'

     */

    rtb_Sum6_p = (int16_T)((int16_T)((int16_T)((int16_T)rtP.i_pwm_count -

      rtb_Sum1_a) - rtb_Sum6_k) >> 1);



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

    rtb_Sum6_k += rtb_Sum6_p;



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

     *  Sum: '<S66>/Add4'

     */

    rtY->pwm_Duty[0] = rtb_Sum6_p;

    rtY->pwm_Duty[1] = (int16_T)(rtb_Sum6_k + rtb_Sum1_a);

    rtY->pwm_Duty[2] = rtb_Sum6_k;

    break;



   case 4:

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

     *  Constant: '<S61>/Constant1'

     *  Constant: '<S67>/Constant'

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

     *  Product: '<S61>/Divide'

     *  Product: '<S61>/Divide1'

     *  Product: '<S67>/Divide'

     *  Sum: '<S67>/Add'

     */

    rtb_Sum6_k = (int16_T)(((int16_T)(((rtb_Sum1_a * 9459) >> 14) - rtb_Sum6_k) *

      (int16_T)rtP.i_pwm_count) >> 12);



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

     *  Constant: '<S61>/Constant1'

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

     *  Product: '<S61>/Divide1'

     *  Product: '<S67>/Divide2'

     *  Sum: '<S67>/Add5'

     */

    rtb_Sum1_a = (int16_T)(((int16_T)(-((int16_T)((rtb_Sum1_a * 9459) >> 13) <<

      2) >> 2) * (int16_T)rtP.i_pwm_count) >> 12);



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

     *  Constant: '<S61>/Constant1'

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

     *  Sum: '<S67>/Add1'

     *  Sum: '<S67>/Add2'

     */

    rtb_Sum6_p = (int16_T)((int16_T)((int16_T)((int16_T)rtP.i_pwm_count -

      rtb_Sum1_a) - rtb_Sum6_k) >> 1);



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

    rtb_Sum6_k += rtb_Sum6_p;



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

     *  Sum: '<S67>/Add4'

     */

    rtY->pwm_Duty[0] = rtb_Sum6_p;

    rtY->pwm_Duty[1] = rtb_Sum6_k;

    rtY->pwm_Duty[2] = (int16_T)(rtb_Sum6_k + rtb_Sum1_a);

    break;



   case 5:

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

     *  Constant: '<S61>/Constant1'

     *  Constant: '<S68>/Constant'

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

     *  Product: '<S61>/Divide'

     *  Product: '<S61>/Divide1'

     *  Product: '<S68>/Divide2'

     *  Sum: '<S68>/Add5'

     */

    rtb_Divide3_k = (int16_T)(((int16_T)(rtb_Sum6_k - ((rtb_Sum1_a * 9459) >> 14))

      * (int16_T)rtP.i_pwm_count) >> 12);



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

     *  Constant: '<S61>/Constant1'

     *  Constant: '<S68>/Constant'

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

     *  Product: '<S61>/Divide'

     *  Product: '<S61>/Divide1'

     *  Product: '<S68>/Divide'

     *  Sum: '<S68>/Add'

     */

    rtb_Sum1_a = (int16_T)(((int16_T)(-rtb_Sum6_k - ((rtb_Sum1_a * 9459) >> 14))

      * (int16_T)rtP.i_pwm_count) >> 12);



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

     *  Constant: '<S61>/Constant1'

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

     *  Sum: '<S68>/Add1'

     *  Sum: '<S68>/Add2'

     */

    rtb_Sum6_p = (int16_T)((int16_T)((int16_T)((int16_T)rtP.i_pwm_count -

      rtb_Sum1_a) - rtb_Divide3_k) >> 1);



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

    rtb_Sum6_k = (int16_T)(rtb_Sum6_p + rtb_Divide3_k);



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

     *  Sum: '<S68>/Add4'

     */

    rtY->pwm_Duty[0] = rtb_Sum6_k;

    rtY->pwm_Duty[1] = rtb_Sum6_p;

    rtY->pwm_Duty[2] = (int16_T)(rtb_Sum6_k + rtb_Sum1_a);

    break;



   default:

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

     *  Constant: '<S61>/Constant1'

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

     *  Product: '<S61>/Divide1'

     *  Product: '<S69>/Divide2'

     *  Sum: '<S69>/Add5'

     */

    rtb_Divide3_k = (int16_T)(((int16_T)(-((int16_T)((rtb_Sum1_a * 9459) >> 13) <<

      2) >> 2) * (int16_T)rtP.i_pwm_count) >> 12);



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

     *  Constant: '<S61>/Constant1'

     *  Constant: '<S69>/Constant'

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

     *  Product: '<S61>/Divide'

     *  Product: '<S61>/Divide1'

     *  Product: '<S69>/Divide'

     *  Sum: '<S69>/Add'

     */

    rtb_Sum1_a = (int16_T)(((int16_T)(((rtb_Sum1_a * 9459) >> 14) + rtb_Sum6_k) *

      (int16_T)rtP.i_pwm_count) >> 12);



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

     *  Constant: '<S61>/Constant1'

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

     *  Sum: '<S69>/Add1'

     *  Sum: '<S69>/Add2'

     */

    rtb_Sum6_p = (int16_T)((int16_T)((int16_T)((int16_T)rtP.i_pwm_count -

      rtb_Sum1_a) - rtb_Divide3_k) >> 1);



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

    rtb_Sum6_k = (int16_T)(rtb_Sum6_p + rtb_Divide3_k);



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

     *  Sum: '<S69>/Add4'

     */

    rtY->pwm_Duty[0] = (int16_T)(rtb_Sum6_k + rtb_Sum1_a);

    rtY->pwm_Duty[1] = rtb_Sum6_p;

    rtY->pwm_Duty[2] = rtb_Sum6_k;

    break;

  }



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



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

  if (rtb_LogicalOperator12) {

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

     *  UnitDelay: '<S98>/Unit Delay1'

     */

    rtDW->UnitDelay_DSTATE_d = rtDW->UnitDelay1_DSTATE_b;

  } else {

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

     *  Sum: '<S118>/Add2'

     */

    rtDW->UnitDelay_DSTATE_d = (int16_T)rtb_Divide_idx_1;

  }



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



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

  if (rtb_Equal_k) {

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

     *  UnitDelay: '<S97>/Unit Delay1'

     */

    rtDW->UnitDelay_DSTATE_b = rtDW->UnitDelay1_DSTATE_i;

  } else {

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

     *  Sum: '<S111>/Add2'

     */

    rtDW->UnitDelay_DSTATE_b = (int16_T)rtb_Divide_idx_0;

  }



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



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

   *  RelationalOperator: '<S39>/Relational Operator'

   *  UnitDelay: '<S39>/UnitDelay'

   */

  if (rtb_n_commDeacv != rtDW->UnitDelay_DSTATE_bv) {

    rtb_UnitDelay = rtb_Sum_i;

  }



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



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

   *  Switch: '<S51>/Switch'

   */

  rtDW->UnitDelay1_DSTATE_f[0] = rtb_Switch_f2_idx_0;

  rtDW->UnitDelay1_DSTATE_f[1] = rtb_Switch_f2_idx_1;



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

  rtDW->UnitDelay_DSTATE_j = rtb_UnitDelay;



  /* Update for Delay: '<S9>/Delay' incorporates:

   *  Inport: '<Root>/hall_A'

   */

  rtDW->Delay_DSTATE_d = 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>/us_Count'

   */

  rtDW->UnitDelay3_DSTATE = rtU->us_Count;



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

   *  Abs: '<S14>/Abs5'

   */

  rtDW->UnitDelay4_DSTATE = rtb_Switch2;



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

  rtDW->UnitDelay_DSTATE_k = rtb_n_commDeacv;



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

  rtDW->UnitDelay_DSTATE_nx = rtb_RelationalOperator4_f;



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

   *  Sum: '<S7>/Sum3'

   */

  rtDW->UnitDelay1_DSTATE = qY;



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

  if (rtb_Sum2 == 0) {

    /* Update for IfAction SubSystem: '<S53>/Do_Calc' incorporates:

     *  ActionPort: '<S70>/Action Port'

     */

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

    rtDW->UnitDelay_DSTATE_p2 = rtb_z_ctrlMod;



    /* Update for UnitDelay: '<S70>/Unit Delay1' incorporates:

     *  Merge: '<S73>/Merge'

     */

    rtDW->UnitDelay1_DSTATE_g = rtDW->Merge_f;



    /* Update for If: '<S73>/If' */

    switch (rtDW->If_ActiveSubsystem_h) {

     case 0:

      /* Update for IfAction SubSystem: '<S73>/speed_mode' incorporates:

       *  ActionPort: '<S87>/Action Port'

       */

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

       *  Sqrt: '<S86>/Sqrt1'

       */

      rtDW->UnitDelay_DSTATE_l = rtb_Min;



      /* End of Update for SubSystem: '<S73>/speed_mode' */

      break;



     case 1:

      /* Update for IfAction SubSystem: '<S73>/torque_mode' incorporates:

       *  ActionPort: '<S88>/Action Port'

       */

      /* Update for Delay: '<S88>/Delay' incorporates:

       *  Sqrt: '<S86>/Sqrt1'

       */

      rtDW->icLoad_p = 0U;

      rtDW->Delay_DSTATE = rtb_Min;



      /* End of Update for SubSystem: '<S73>/torque_mode' */

      break;

    }



    /* End of Update for If: '<S73>/If' */



    /* Update for UnitDelay: '<S74>/Unit Delay1' incorporates:

     *  Sqrt: '<S51>/Sqrt'

     */

    rtDW->UnitDelay1_DSTATE_c = rtb_BitwiseOperator2;



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

     *  Sum: '<S77>/Sum'

     */

    rtDW->UnitDelay_DSTATE = rtb_MathFunction2_p;



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

     *  Sum: '<S78>/Sum1'

     */

    rtDW->icLoad = 0U;

    rtDW->ResettableDelay_DSTATE = rtb_Sum1_f;



    /* End of Update for SubSystem: '<S53>/Do_Calc' */

  }



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

   *  Switch: '<S84>/Switch2'

   */

  rtDW->UnitDelay_DSTATE_h = rtDW->Switch2;



  /* Update for UnitDelay: '<S97>/Unit Delay1' incorporates:

   *  Switch: '<S110>/Switch2'

   */

  rtDW->UnitDelay1_DSTATE_i = rtb_Divide1_m;



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

   *  Switch: '<S110>/Switch2'

   */

  rtDW->UnitDelay_DSTATE_g = rtb_Divide1_m;



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

   *  Switch: '<S76>/Switch'

   */

  rtDW->UnitDelay_DSTATE_o = rtDW->Switch;



  /* Update for UnitDelay: '<S98>/Unit Delay1' incorporates:

   *  Switch: '<S117>/Switch2'

   */

  rtDW->UnitDelay1_DSTATE_b = rtb_r_cos_M1;



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

   *  Switch: '<S117>/Switch2'

   */

  rtDW->UnitDelay_DSTATE_a = rtb_r_cos_M1;



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

  rtDW->UnitDelay_DSTATE_bm = rtb_z_ctrlMod;



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

  rtDW->UnitDelay_DSTATE_bv = rtb_n_commDeacv;



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



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

   *  UnitDelay: '<S6>/UnitDelay1'

   */

  rtY->f_Vdq[0] = rtb_UnitDelay1_m[0];

  rtY->f_Vdq[1] = rtb_UnitDelay1_m[1];



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

  rtY->n_Sector = rtb_DataTypeConversion_j;



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

  rtY->n_MotError = rtb_UnitDelay;



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

   *  Merge: '<S3>/Merge'

   */

  rtY->f_MotAngle = rtDW->Merge_i;



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

   *  Switch: '<S14>/Switch2'

   */

  rtY->f_MotRPM = rtb_Switch3;



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

   *  Merge: '<S15>/Merge'

   */

  rtY->f_hallAngle = rtb_Sum3_jm;



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

  rtY->n_hallStat = rtb_Add_gf;



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

  rtY->n_runingMode = 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->ResettableDelay_Reset_ZCE_a = POS_ZCSIG;

  rtPrevZCX->ResettableDelay_Reset_ZCE_o = POS_ZCSIG;

  rtPrevZCX->PI_backCalc_fixdt1.ResettableDelay_Reset_ZCE = POS_ZCSIG;

  rtPrevZCX->PI_backCalc_fixdt_o3.ResettableDelay_Reset_ZCE = POS_ZCSIG;

  rtPrevZCX->PI_Speed.ResettableDelay_Reset_ZCE_f = POS_ZCSIG;



  /* SystemInitialize for Atomic SubSystem: '<Root>/PMSM_Controller' */

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

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

  rtDW->UnitDelay2_DSTATE = rtP.n_hall_count_ps;



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

   *  Inport: '<S20>/z_counterRawPrev'

   */

  rtDW->z_counterRawPrev = rtP.n_hall_count_ps;



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



  /* SystemInitialize for IfAction SubSystem: '<S53>/Do_Calc' */

  /* Start for If: '<S73>/If' */

  rtDW->If_ActiveSubsystem_h = -1;



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

  rtDW->icLoad = 1U;



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

  /* SystemInitialize for Atomic SubSystem: '<S87>/PI_Speed' */

  PI_backCalc_fixdt_Init(&rtDW->PI_Speed);



  /* End of SystemInitialize for SubSystem: '<S87>/PI_Speed' */

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



  /* SystemInitialize for IfAction SubSystem: '<S73>/torque_mode' */

  /* InitializeConditions for Delay: '<S88>/Delay' */

  rtDW->icLoad_p = 1U;



  /* SystemInitialize for Atomic SubSystem: '<S88>/PI_TrqSpdLim' */

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

  rtDW->icLoad_k = 1U;



  /* End of SystemInitialize for SubSystem: '<S88>/PI_TrqSpdLim' */

  /* End of SystemInitialize for SubSystem: '<S73>/torque_mode' */

  /* End of SystemInitialize for SubSystem: '<S53>/Do_Calc' */



  /* SystemInitialize for IfAction SubSystem: '<S54>/CurrentLoop' */

  /* SystemInitialize for Atomic SubSystem: '<S95>/PI_backCalc_fixdt' */

  PI_backCalc_fixdt_p_Init(&rtDW->PI_backCalc_fixdt_o3);



  /* End of SystemInitialize for SubSystem: '<S95>/PI_backCalc_fixdt' */



  /* SystemInitialize for Atomic SubSystem: '<S95>/PI_backCalc_fixdt1' */

  PI_backCalc_fixdt_p_Init(&rtDW->PI_backCalc_fixdt1);



  /* End of SystemInitialize for SubSystem: '<S95>/PI_backCalc_fixdt1' */

  /* End of SystemInitialize for SubSystem: '<S54>/CurrentLoop' */

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



  /* SystemInitialize for Outport: '<Root>/f_MotAngle' incorporates:

   *  Merge: '<S3>/Merge'

   */

  rtY->f_MotAngle = rtDW->Merge_i;

}



/*

 * File trailer for generated code.

 *

 * [EOF]

 */