MC100/Simulink/PMSM_Controller_ert_rtw/PMSM_Controller.c

/*
 * File: PMSM_Controller.c
 *
 * Code generated for Simulink model 'PMSM_Controller'.
 *
 * Model version                  : 1.885
 * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
 * C/C++ source code generated on : Sat Jan  8 15:44:11 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"
#include "PMSM_Controller_private.h"

static void rate_scheduler(RT_MODEL *const rtM);
uint16_T plook_u16u16_evencka(uint16_T u, uint16_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)((uint32_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;
}

/*
 *   This function updates active task flag for each subrate.
 * The function is called at model base rate, hence the
 * generated code self-manages all its subrates.
 */
static void rate_scheduler(RT_MODEL *const rtM)
{
  /* Compute which subrates run during the next base time step.  Subrates
   * are an integer multiple of the base rate counter.  Therefore, the subtask
   * counter is reset when it reaches its limit (zero means run).
   */
  (rtM->Timing.TaskCounters.TID[1])++;
  if ((rtM->Timing.TaskCounters.TID[1]) > 9) {/* Sample time: [6.2E-5s, 0.0s] */
    rtM->Timing.TaskCounters.TID[1] = 0;
  }
}

real_T rt_modd_snf(real_T u0, real_T u1)
{
  real_T q;
  real_T y;
  boolean_T yEq;
  y = u0;
  if (u1 == 0.0) {
    if (u0 == 0.0) {
      y = u1;
    }
  } else if (rtIsNaN(u0) || rtIsNaN(u1) || rtIsInf(u0)) {
    y = (rtNaN);
  } else if (u0 == 0.0) {
    y = 0.0 / u1;
  } else if (rtIsInf(u1)) {
    if ((u1 < 0.0) != (u0 < 0.0)) {
      y = u1;
    }
  } else {
    y = fmod(u0, u1);
    yEq = (y == 0.0);
    if ((!yEq) && (u1 > floor(u1))) {
      q = fabs(u0 / u1);
      yEq = !(fabs(q - floor(q + 0.5)) > DBL_EPSILON * q);
    }

    if (yEq) {
      y = u1 * 0.0;
    } else {
      if ((u0 < 0.0) != (u1 < 0.0)) {
        y += u1;
      }
    }
  }

  return y;
}

real_T rt_roundd_snf(real_T u)
{
  real_T y;
  if (fabs(u) < 4.503599627370496E+15) {
    if (u >= 0.5) {
      y = floor(u + 0.5);
    } else if (u > -0.5) {
      y = u * 0.0;
    } else {
      y = ceil(u - 0.5);
    }
  } else {
    y = u;
  }

  return y;
}

/* Model step function */
void PMSM_Controller_step(RT_MODEL *const rtM, int16_T rtU_Vdc, uint16_T rtU_Ts,
  int16_T rtU_adc_a, int16_T rtU_adc_b, real_T rtU_rotor_angle, int16_T
  rtU_rotor_speed, int16_T rtU_input_target, uint16_T rtY_pwm_counts[8], uint8_T
  *rtY_sector)
{
  DW *rtDW = rtM->dwork;
  int32_T rtb_Sum1_tmp;
  int32_T rtb_Sum3_idx_0;
  int32_T rtb_Sum3_idx_1;
  int32_T tmp;
  int32_T tmp_0;
  int16_T rtb_Divide;
  int16_T rtb_Divide4;
  int16_T rtb_Divide7;
  int16_T rtb_Multiply_idx_0;
  int16_T rtb_Multiply_idx_1;
  int16_T rtb_SignPreIntegrator;
  int16_T rtb_SignPreSat;
  int16_T rtb_SignPreSat_k;
  int16_T rtb_Sum1;
  int16_T rtb_Sum2;
  int16_T rtb_Sum3_l;
  uint16_T rtb_DataTypeConversion3;
  uint16_T rtb_MultiportSwitch_idx_0;
  uint16_T rtb_MultiportSwitch_idx_1;
  uint16_T rtb_o_samp_p1;
  uint8_T rtb_DataTypeConversion_a;
  boolean_T rtb_NotEqual;

  /* Gain: '<S1>/Multiply' incorporates:
   *  Inport: '<Root>/adc_a'
   *  Inport: '<Root>/adc_b'
   */
  rtb_Multiply_idx_0 = (int16_T)((19761 * rtU_adc_a) >> 17);
  rtb_Multiply_idx_1 = (int16_T)((19761 * rtU_adc_b) >> 17);

  /* Outputs for Atomic SubSystem: '<S1>/controller' */
  /* PreLookup: '<S23>/a_elecAngle_XA' incorporates:
   *  Constant: '<S1>/Constant1'
   *  Gain: '<S1>/Gain'
   *  Inport: '<Root>/rotor_angle'
   *  Math: '<S1>/Math Function'
   */
  rtb_o_samp_p1 = plook_u16u16_evencka((uint16_T)(57.295779513082323 *
    rt_modd_snf(rtU_rotor_angle, 6.2831853071795862)), 0U, 1U, 360U);

  /* Sum: '<S15>/Add1' incorporates:
   *  Product: '<S177>/Divide4'
   *  Sum: '<S15>/Add'
   */
  rtb_Divide4 = (int16_T)-(rtb_Multiply_idx_0 + rtb_Multiply_idx_1);

  /* Gain: '<S20>/Gain1' incorporates:
   *  Gain: '<S20>/Gain'
   *  Product: '<S177>/Divide'
   *  Product: '<S177>/Divide4'
   *  Sum: '<S20>/Add'
   *  Sum: '<S20>/Add1'
   */
  rtb_Divide = (int16_T)(((((rtb_Multiply_idx_0 << 1) - rtb_Multiply_idx_1) -
    rtb_Divide4) * 21845) >> 16);

  /* Gain: '<S20>/Gain2' incorporates:
   *  Product: '<S177>/Divide4'
   *  Sum: '<S20>/Add2'
   */
  rtb_Divide4 = (int16_T)(((rtb_Multiply_idx_1 - rtb_Divide4) * 18919) >> 15);

  /* Sum: '<S22>/Sum1' incorporates:
   *  Interpolation_n-D: '<S23>/r_cos_M1'
   *  Interpolation_n-D: '<S23>/r_sin_M1'
   *  Product: '<S177>/Divide'
   *  Product: '<S177>/Divide4'
   *  Product: '<S22>/Divide2'
   *  Product: '<S22>/Divide3'
   */
  rtb_Sum1_tmp = ((rtb_Divide * rtConstP.r_cos_M1_Table[rtb_o_samp_p1]) >> 14) +
    ((rtb_Divide4 * rtConstP.r_sin_M1_Table[rtb_o_samp_p1]) >> 14);
  if (rtb_Sum1_tmp > 32767) {
    rtb_Sum1_tmp = 32767;
  } else {
    if (rtb_Sum1_tmp < -32768) {
      rtb_Sum1_tmp = -32768;
    }
  }

  /* Sum: '<S21>/Sum2' incorporates:
   *  Sum: '<S22>/Sum1'
   *  UnitDelay: '<S21>/UnitDelay1'
   */
  rtb_Sum1_tmp -= rtDW->UnitDelay1_DSTATE[0] >> 16;
  if (rtb_Sum1_tmp > 32767) {
    rtb_Sum1_tmp = 32767;
  } else {
    if (rtb_Sum1_tmp < -32768) {
      rtb_Sum1_tmp = -32768;
    }
  }

  /* Sum: '<S21>/Sum3' incorporates:
   *  Constant: '<S15>/Constant'
   *  Product: '<S21>/Divide3'
   *  Sum: '<S21>/Sum2'
   *  UnitDelay: '<S21>/UnitDelay1'
   */
  rtb_Sum3_idx_0 = 13107 * rtb_Sum1_tmp + rtDW->UnitDelay1_DSTATE[0];

  /* Sum: '<S22>/Sum6' incorporates:
   *  Interpolation_n-D: '<S23>/r_cos_M1'
   *  Interpolation_n-D: '<S23>/r_sin_M1'
   *  Product: '<S177>/Divide'
   *  Product: '<S177>/Divide4'
   *  Product: '<S22>/Divide1'
   *  Product: '<S22>/Divide4'
   */
  rtb_Sum1_tmp = ((rtb_Divide4 * rtConstP.r_cos_M1_Table[rtb_o_samp_p1]) >> 14)
    - ((rtb_Divide * rtConstP.r_sin_M1_Table[rtb_o_samp_p1]) >> 14);
  if (rtb_Sum1_tmp > 32767) {
    rtb_Sum1_tmp = 32767;
  } else {
    if (rtb_Sum1_tmp < -32768) {
      rtb_Sum1_tmp = -32768;
    }
  }

  /* Sum: '<S21>/Sum2' incorporates:
   *  Sum: '<S22>/Sum6'
   *  UnitDelay: '<S21>/UnitDelay1'
   */
  rtb_Sum1_tmp -= rtDW->UnitDelay1_DSTATE[1] >> 16;
  if (rtb_Sum1_tmp > 32767) {
    rtb_Sum1_tmp = 32767;
  } else {
    if (rtb_Sum1_tmp < -32768) {
      rtb_Sum1_tmp = -32768;
    }
  }

  /* Sum: '<S21>/Sum3' incorporates:
   *  Constant: '<S15>/Constant'
   *  Product: '<S21>/Divide3'
   *  Sum: '<S21>/Sum2'
   *  UnitDelay: '<S21>/UnitDelay1'
   */
  rtb_Sum3_idx_1 = 13107 * rtb_Sum1_tmp + rtDW->UnitDelay1_DSTATE[1];

  /* DataTypeConversion: '<S21>/Data Type Conversion' incorporates:
   *  Sum: '<S21>/Sum3'
   */
  rtb_Multiply_idx_0 = (int16_T)(rtb_Sum3_idx_0 >> 16);

  /* Sum: '<S69>/Sum' incorporates:
   *  DiscreteIntegrator: '<S60>/Integrator'
   *  Gain: '<S65>/Proportional Gain'
   *  Signum: '<S51>/SignPreSat'
   *  Sum: '<S16>/Sum1'
   */
  rtb_Multiply_idx_1 = (int16_T)((((int16_T)(9 * (int16_T)-rtb_Multiply_idx_0) <<
    15) + (int32_T)((rtDW->Integrator_DSTATE * 1704243LL) >> 23)) >> 15);

  /* Saturate: '<S67>/Saturation' incorporates:
   *  Product: '<S177>/Divide'
   *  Signum: '<S51>/SignPreSat'
   */
  if (rtb_Multiply_idx_1 > 8960) {
    rtb_Divide = 8960;
  } else if (rtb_Multiply_idx_1 < -8960) {
    rtb_Divide = -8960;
  } else {
    rtb_Divide = rtb_Multiply_idx_1;
  }

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

  /* Sum: '<S16>/Sum3' incorporates:
   *  Inport: '<Root>/input_target'
   *  Inport: '<Root>/rotor_speed'
   */
  rtb_Sum3_l = (int16_T)(rtU_input_target - rtU_rotor_speed);

  /* Sum: '<S169>/Sum' incorporates:
   *  DiscreteIntegrator: '<S160>/Integrator'
   *  Gain: '<S165>/Proportional Gain'
   *  Signum: '<S151>/SignPreSat'
   */
  rtb_SignPreSat = (int16_T)((rtDW->Integrator_DSTATE_i * 26629 < 0 ? -1 : 0) +
    (rtb_Sum3_l << 4));

  /* Saturate: '<S167>/Saturation' incorporates:
   *  Signum: '<S151>/SignPreSat'
   */
  if (rtb_SignPreSat > 800) {
    rtb_Divide4 = 800;
  } else if (rtb_SignPreSat < -800) {
    rtb_Divide4 = -800;
  } else {
    rtb_Divide4 = rtb_SignPreSat;
  }

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

  /* Sum: '<S16>/Sum2' incorporates:
   *  DataTypeConversion: '<S21>/Data Type Conversion'
   *  Sum: '<S21>/Sum3'
   */
  rtb_Sum2 = (int16_T)(rtb_Divide4 - (int16_T)(rtb_Sum3_idx_1 >> 16));

  /* Sum: '<S119>/Sum' incorporates:
   *  DiscreteIntegrator: '<S110>/Integrator'
   *  Gain: '<S115>/Proportional Gain'
   *  Signum: '<S101>/SignPreSat'
   *  Sum: '<S16>/Sum2'
   */
  rtb_SignPreSat_k = (int16_T)((((int16_T)(10 * rtb_Sum2) << 15) + (int32_T)
    ((rtDW->Integrator_DSTATE_p * 1704243LL) >> 23)) >> 15);

  /* Saturate: '<S117>/Saturation' incorporates:
   *  Signum: '<S101>/SignPreSat'
   *  Signum: '<S51>/SignPreIntegrator'
   */
  if (rtb_SignPreSat_k > 8960) {
    rtb_SignPreIntegrator = 8960;
  } else if (rtb_SignPreSat_k < -8960) {
    rtb_SignPreIntegrator = -8960;
  } else {
    rtb_SignPreIntegrator = rtb_SignPreSat_k;
  }

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

  /* Sum: '<S18>/Sum1' incorporates:
   *  Interpolation_n-D: '<S23>/r_cos_M1'
   *  Interpolation_n-D: '<S23>/r_sin_M1'
   *  Product: '<S177>/Divide'
   *  Product: '<S18>/Divide2'
   *  Product: '<S18>/Divide3'
   *  Signum: '<S51>/SignPreIntegrator'
   */
  rtb_Sum1_tmp = ((rtb_Divide * rtConstP.r_sin_M1_Table[rtb_o_samp_p1]) >> 14) +
    ((rtb_SignPreIntegrator * rtConstP.r_cos_M1_Table[rtb_o_samp_p1]) >> 14);
  if (rtb_Sum1_tmp > 32767) {
    rtb_Sum1_tmp = 32767;
  } else {
    if (rtb_Sum1_tmp < -32768) {
      rtb_Sum1_tmp = -32768;
    }
  }

  /* Sum: '<S18>/Sum6' incorporates:
   *  Interpolation_n-D: '<S23>/r_cos_M1'
   *  Interpolation_n-D: '<S23>/r_sin_M1'
   *  Product: '<S177>/Divide'
   *  Product: '<S18>/Divide1'
   *  Product: '<S18>/Divide4'
   *  Signum: '<S51>/SignPreIntegrator'
   */
  tmp_0 = ((rtb_Divide * rtConstP.r_cos_M1_Table[rtb_o_samp_p1]) >> 14) -
    ((rtb_SignPreIntegrator * rtConstP.r_sin_M1_Table[rtb_o_samp_p1]) >> 14);
  if (tmp_0 > 32767) {
    tmp_0 = 32767;
  } else {
    if (tmp_0 < -32768) {
      tmp_0 = -32768;
    }
  }

  /* Product: '<S177>/Divide7' incorporates:
   *  Constant: '<S177>/Constant3'
   *  Sum: '<S18>/Sum1'
   */
  rtb_Divide7 = (int16_T)((2365 * (int16_T)rtb_Sum1_tmp) >> 10);

  /* MATLAB Function: '<S177>/sector_select' incorporates:
   *  Product: '<S177>/Divide7'
   *  Sum: '<S18>/Sum1'
   *  Sum: '<S18>/Sum6'
   */
  if ((int16_T)rtb_Sum1_tmp >= 0) {
    if ((int16_T)tmp_0 >= 0) {
      if (rtb_Divide7 > ((int16_T)tmp_0 << 2)) {
        /* DataTypeConversion: '<S177>/Data Type Conversion' */
        rtb_DataTypeConversion_a = 2U;
      } else {
        /* DataTypeConversion: '<S177>/Data Type Conversion' */
        rtb_DataTypeConversion_a = 1U;
      }
    } else {
      tmp = -rtb_Divide7;
      if (-rtb_Divide7 > 32767) {
        tmp = 32767;
      }

      if (tmp > ((int16_T)tmp_0 << 2)) {
        /* DataTypeConversion: '<S177>/Data Type Conversion' */
        rtb_DataTypeConversion_a = 3U;
      } else {
        /* DataTypeConversion: '<S177>/Data Type Conversion' */
        rtb_DataTypeConversion_a = 2U;
      }
    }
  } else if ((int16_T)tmp_0 >= 0) {
    tmp = -rtb_Divide7;
    if (-rtb_Divide7 > 32767) {
      tmp = 32767;
    }

    if (tmp > ((int16_T)tmp_0 << 2)) {
      /* DataTypeConversion: '<S177>/Data Type Conversion' */
      rtb_DataTypeConversion_a = 5U;
    } else {
      /* DataTypeConversion: '<S177>/Data Type Conversion' */
      rtb_DataTypeConversion_a = 6U;
    }
  } else if (rtb_Divide7 > ((int16_T)tmp_0 << 2)) {
    /* DataTypeConversion: '<S177>/Data Type Conversion' */
    rtb_DataTypeConversion_a = 4U;
  } else {
    /* DataTypeConversion: '<S177>/Data Type Conversion' */
    rtb_DataTypeConversion_a = 5U;
  }

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

  /* Product: '<S177>/Divide' incorporates:
   *  DataTypeConversion: '<S177>/Data Type Conversion6'
   *  Inport: '<Root>/Ts'
   *  Inport: '<Root>/Vdc'
   */
  rtb_Divide = (int16_T)div_nde_s32_floor((int16_T)rtU_Ts << 10, rtU_Vdc);

  /* Product: '<S177>/Divide8' incorporates:
   *  Constant: '<S177>/Constant'
   *  Product: '<S177>/Divide2'
   *  Product: '<S177>/Divide4'
   *  Sum: '<S18>/Sum1'
   */
  rtb_Sum1_tmp = 2365 * (int16_T)((3547 * (int16_T)rtb_Sum1_tmp) >> 10);

  /* Product: '<S177>/Divide1' incorporates:
   *  Product: '<S177>/Divide'
   *  Product: '<S177>/Divide8'
   */
  rtb_Sum1 = (int16_T)(((int16_T)(rtb_Sum1_tmp >> 11) * rtb_Divide) >> 12);

  /* Product: '<S177>/Divide3' incorporates:
   *  Constant: '<S177>/Constant1'
   *  Signum: '<S51>/SignPreIntegrator'
   *  Sum: '<S18>/Sum6'
   */
  rtb_SignPreIntegrator = (int16_T)((3547 * (int16_T)tmp_0) >> 10);

  /* Product: '<S177>/Divide4' */
  rtb_Divide4 = (int16_T)(rtb_Sum1_tmp >> 12);

  /* Product: '<S177>/Divide5' incorporates:
   *  Product: '<S177>/Divide'
   *  Product: '<S177>/Divide4'
   *  Signum: '<S51>/SignPreIntegrator'
   *  Sum: '<S177>/Add'
   */
  rtb_Divide7 = (int16_T)(((int16_T)(rtb_SignPreIntegrator + rtb_Divide4) *
    rtb_Divide) >> 12);

  /* Product: '<S177>/Divide6' incorporates:
   *  Product: '<S177>/Divide'
   *  Product: '<S177>/Divide4'
   *  Signum: '<S51>/SignPreIntegrator'
   *  Sum: '<S177>/Add1'
   */
  rtb_SignPreIntegrator = (int16_T)(((int16_T)(rtb_Divide4 -
    rtb_SignPreIntegrator) * rtb_Divide) >> 12);

  /* MATLAB Function: '<S177>/phase_time' incorporates:
   *  DataTypeConversion: '<S177>/Data Type Conversion1'
   *  DataTypeConversion: '<S177>/Data Type Conversion2'
   *  Inport: '<Root>/Ts'
   */
  switch ((int8_T)rtb_DataTypeConversion_a) {
   case 1:
    rtb_Sum1_tmp = -rtb_SignPreIntegrator;
    if (-rtb_SignPreIntegrator > 32767) {
      rtb_Sum1_tmp = 32767;
    }

    tmp_0 = (int16_T)rtU_Ts - (int16_T)rtb_Sum1_tmp;
    if (tmp_0 > 32767) {
      tmp_0 = 32767;
    } else {
      if (tmp_0 < -32768) {
        tmp_0 = -32768;
      }
    }

    tmp_0 -= rtb_Sum1;
    if (tmp_0 > 32767) {
      tmp_0 = 32767;
    } else {
      if (tmp_0 < -32768) {
        tmp_0 = -32768;
      }
    }

    rtb_Divide = (int16_T)rt_roundd_snf((real_T)tmp_0 / 4.0);
    rtb_Sum1 = (int16_T)((int32_T)rt_roundd_snf((real_T)rtb_Sum1 / 2.0) +
                         rtb_Divide);
    rtb_Sum1_tmp = (int32_T)rt_roundd_snf((real_T)(int16_T)rtb_Sum1_tmp / 2.0) +
      rtb_Sum1;
    if (rtb_Sum1_tmp > 32767) {
      rtb_Sum1_tmp = 32767;
    } else {
      if (rtb_Sum1_tmp < -32768) {
        rtb_Sum1_tmp = -32768;
      }
    }

    rtb_Divide7 = (int16_T)rtb_Sum1_tmp;
    break;

   case 2:
    rtb_Sum1_tmp = (int16_T)rtU_Ts - rtb_Divide7;
    if (rtb_Sum1_tmp > 32767) {
      rtb_Sum1_tmp = 32767;
    } else {
      if (rtb_Sum1_tmp < -32768) {
        rtb_Sum1_tmp = -32768;
      }
    }

    rtb_Sum1_tmp -= rtb_SignPreIntegrator;
    if (rtb_Sum1_tmp > 32767) {
      rtb_Sum1_tmp = 32767;
    } else {
      if (rtb_Sum1_tmp < -32768) {
        rtb_Sum1_tmp = -32768;
      }
    }

    rtb_Divide = (int16_T)rt_roundd_snf((real_T)rtb_Sum1_tmp / 4.0);
    rtb_Divide7 = (int16_T)((int32_T)rt_roundd_snf((real_T)rtb_Divide7 / 2.0) +
      rtb_Divide);
    rtb_Sum1_tmp = (int32_T)rt_roundd_snf((real_T)rtb_SignPreIntegrator / 2.0) +
      rtb_Divide7;
    if (rtb_Sum1_tmp > 32767) {
      rtb_Sum1_tmp = 32767;
    } else {
      if (rtb_Sum1_tmp < -32768) {
        rtb_Sum1_tmp = -32768;
      }
    }

    rtb_Sum1 = (int16_T)rtb_Sum1_tmp;
    break;

   case 3:
    rtb_Sum1_tmp = -rtb_Divide7;
    if (-rtb_Divide7 > 32767) {
      rtb_Sum1_tmp = 32767;
    }

    tmp_0 = (int16_T)rtU_Ts - rtb_Sum1;
    if (tmp_0 > 32767) {
      tmp_0 = 32767;
    } else {
      if (tmp_0 < -32768) {
        tmp_0 = -32768;
      }
    }

    tmp_0 -= (int16_T)rtb_Sum1_tmp;
    if (tmp_0 > 32767) {
      tmp_0 = 32767;
    } else {
      if (tmp_0 < -32768) {
        tmp_0 = -32768;
      }
    }

    rtb_Divide7 = (int16_T)rt_roundd_snf((real_T)tmp_0 / 4.0);
    rtb_Divide = (int16_T)((int32_T)rt_roundd_snf((real_T)(int16_T)rtb_Sum1_tmp /
      2.0) + rtb_Divide7);
    rtb_Sum1_tmp = (int32_T)rt_roundd_snf((real_T)rtb_Sum1 / 2.0) + rtb_Divide;
    if (rtb_Sum1_tmp > 32767) {
      rtb_Sum1_tmp = 32767;
    } else {
      if (rtb_Sum1_tmp < -32768) {
        rtb_Sum1_tmp = -32768;
      }
    }

    rtb_Sum1 = (int16_T)rtb_Sum1_tmp;
    break;

   case 4:
    rtb_Sum1_tmp = -rtb_Sum1;
    if (-rtb_Sum1 > 32767) {
      rtb_Sum1_tmp = 32767;
    }

    tmp_0 = (int16_T)rtU_Ts - (int16_T)rtb_Sum1_tmp;
    if (tmp_0 > 32767) {
      tmp_0 = 32767;
    } else {
      if (tmp_0 < -32768) {
        tmp_0 = -32768;
      }
    }

    tmp_0 -= rtb_SignPreIntegrator;
    if (tmp_0 > 32767) {
      tmp_0 = 32767;
    } else {
      if (tmp_0 < -32768) {
        tmp_0 = -32768;
      }
    }

    rtb_Divide7 = (int16_T)rt_roundd_snf((real_T)tmp_0 / 4.0);
    rtb_Sum1 = (int16_T)((int32_T)rt_roundd_snf((real_T)rtb_SignPreIntegrator /
      2.0) + rtb_Divide7);
    rtb_Sum1_tmp = (int32_T)rt_roundd_snf((real_T)(int16_T)rtb_Sum1_tmp / 2.0) +
      rtb_Sum1;
    if (rtb_Sum1_tmp > 32767) {
      rtb_Sum1_tmp = 32767;
    } else {
      if (rtb_Sum1_tmp < -32768) {
        rtb_Sum1_tmp = -32768;
      }
    }

    rtb_Divide = (int16_T)rtb_Sum1_tmp;
    break;

   case 5:
    rtb_Sum1_tmp = -rtb_Divide7;
    if (-rtb_Divide7 > 32767) {
      rtb_Sum1_tmp = 32767;
    }

    tmp_0 = -rtb_SignPreIntegrator;
    if (-rtb_SignPreIntegrator > 32767) {
      tmp_0 = 32767;
    }

    tmp = (int16_T)rtU_Ts - (int16_T)rtb_Sum1_tmp;
    if (tmp > 32767) {
      tmp = 32767;
    } else {
      if (tmp < -32768) {
        tmp = -32768;
      }
    }

    tmp -= (int16_T)tmp_0;
    if (tmp > 32767) {
      tmp = 32767;
    } else {
      if (tmp < -32768) {
        tmp = -32768;
      }
    }

    rtb_Sum1 = (int16_T)rt_roundd_snf((real_T)tmp / 4.0);
    rtb_Divide7 = (int16_T)((int32_T)rt_roundd_snf((real_T)(int16_T)tmp_0 / 2.0)
      + rtb_Sum1);
    rtb_Sum1_tmp = (int32_T)rt_roundd_snf((real_T)(int16_T)rtb_Sum1_tmp / 2.0) +
      rtb_Divide7;
    if (rtb_Sum1_tmp > 32767) {
      rtb_Sum1_tmp = 32767;
    } else {
      if (rtb_Sum1_tmp < -32768) {
        rtb_Sum1_tmp = -32768;
      }
    }

    rtb_Divide = (int16_T)rtb_Sum1_tmp;
    break;

   default:
    rtb_Sum1_tmp = -rtb_Sum1;
    if (-rtb_Sum1 > 32767) {
      rtb_Sum1_tmp = 32767;
    }

    tmp_0 = (int16_T)rtU_Ts - rtb_Divide7;
    if (tmp_0 > 32767) {
      tmp_0 = 32767;
    } else {
      if (tmp_0 < -32768) {
        tmp_0 = -32768;
      }
    }

    tmp_0 -= (int16_T)rtb_Sum1_tmp;
    if (tmp_0 > 32767) {
      tmp_0 = 32767;
    } else {
      if (tmp_0 < -32768) {
        tmp_0 = -32768;
      }
    }

    rtb_Sum1 = (int16_T)rt_roundd_snf((real_T)tmp_0 / 4.0);
    rtb_Divide = (int16_T)((int32_T)rt_roundd_snf((real_T)(int16_T)rtb_Sum1_tmp /
      2.0) + rtb_Sum1);
    rtb_Sum1_tmp = (int32_T)rt_roundd_snf((real_T)rtb_Divide7 / 2.0) +
      rtb_Divide;
    if (rtb_Sum1_tmp > 32767) {
      rtb_Sum1_tmp = 32767;
    } else {
      if (rtb_Sum1_tmp < -32768) {
        rtb_Sum1_tmp = -32768;
      }
    }

    rtb_Divide7 = (int16_T)rtb_Sum1_tmp;
    break;
  }

  /* DataTypeConversion: '<S177>/Data Type Conversion3' incorporates:
   *  MATLAB Function: '<S177>/phase_time'
   */
  rtb_DataTypeConversion3 = (uint16_T)rtb_Divide7;

  /* MultiPortSwitch: '<S178>/Multiport Switch' incorporates:
   *  DataTypeConversion: '<S177>/Data Type Conversion3'
   *  DataTypeConversion: '<S177>/Data Type Conversion4'
   *  DataTypeConversion: '<S177>/Data Type Conversion5'
   *  MATLAB Function: '<S177>/phase_time'
   */
  switch (rtb_DataTypeConversion_a) {
   case 1:
    rtb_MultiportSwitch_idx_0 = (uint16_T)rtb_Divide7;
    rtb_MultiportSwitch_idx_1 = (uint16_T)rtb_Sum1;
    break;

   case 2:
    rtb_MultiportSwitch_idx_0 = (uint16_T)rtb_Sum1;
    rtb_MultiportSwitch_idx_1 = (uint16_T)rtb_Divide7;
    break;

   case 3:
    rtb_MultiportSwitch_idx_0 = (uint16_T)rtb_Sum1;
    rtb_MultiportSwitch_idx_1 = (uint16_T)rtb_Divide;
    break;

   case 4:
    rtb_MultiportSwitch_idx_0 = (uint16_T)rtb_Divide;
    rtb_MultiportSwitch_idx_1 = (uint16_T)rtb_Sum1;
    break;

   case 5:
    rtb_MultiportSwitch_idx_0 = (uint16_T)rtb_Divide;
    rtb_MultiportSwitch_idx_1 = (uint16_T)rtb_Divide7;
    break;

   default:
    rtb_MultiportSwitch_idx_0 = (uint16_T)rtb_Divide7;
    rtb_MultiportSwitch_idx_1 = (uint16_T)rtb_Divide;
    break;
  }

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

  /* Outport: '<Root>/pwm_counts' incorporates:
   *  Constant: '<S178>/Constant'
   *  Constant: '<S178>/Constant1'
   *  Constant: '<S178>/Constant2'
   *  Constant: '<S178>/Constant3'
   *  MATLAB Function: '<S178>/Shunt_Three_Sample_Point'
   *  Outport: '<Root>/sector'
   */
  Shunt_Three_Sample_Point(rtb_MultiportSwitch_idx_0, rtb_MultiportSwitch_idx_1,
    rtb_DataTypeConversion_a, 4000, 10, 10, 10, &rtb_o_samp_p1, &rtY_pwm_counts
    [7], rtY_sector);

  /* DeadZone: '<S103>/DeadZone' incorporates:
   *  Signum: '<S101>/SignPreSat'
   */
  if (rtb_SignPreSat_k > 8960) {
    rtb_SignPreSat_k = (int16_T)(rtb_SignPreSat_k - 8960);
  } else if (rtb_SignPreSat_k >= -8960) {
    rtb_SignPreSat_k = 0;
  } else {
    rtb_SignPreSat_k = (int16_T)(rtb_SignPreSat_k - -8960);
  }

  /* End of DeadZone: '<S103>/DeadZone' */

  /* RelationalOperator: '<S101>/NotEqual' incorporates:
   *  Gain: '<S101>/ZeroGain'
   *  Signum: '<S101>/SignPreSat'
   */
  rtb_NotEqual = (0 != rtb_SignPreSat_k);

  /* Signum: '<S101>/SignPreSat' */
  if (rtb_SignPreSat_k < 0) {
    rtb_SignPreSat_k = -1;
  } else {
    rtb_SignPreSat_k = (int16_T)(rtb_SignPreSat_k > 0);
  }

  /* End of Signum: '<S101>/SignPreSat' */

  /* Gain: '<S107>/Integral Gain' incorporates:
   *  Gain: '<S57>/Integral Gain'
   *  Sum: '<S16>/Sum2'
   */
  rtb_Divide7 = (int16_T)(1080 * rtb_Sum2);

  /* Signum: '<S101>/SignPreIntegrator' incorporates:
   *  Gain: '<S57>/Integral Gain'
   */
  if (rtb_Divide7 < 0) {
    rtb_Divide4 = -1;
  } else {
    rtb_Divide4 = (int16_T)(rtb_Divide7 > 0);
  }

  /* End of Signum: '<S101>/SignPreIntegrator' */

  /* Switch: '<S101>/Switch' incorporates:
   *  Constant: '<S101>/Constant1'
   *  DataTypeConversion: '<S101>/DataTypeConv1'
   *  DataTypeConversion: '<S101>/DataTypeConv2'
   *  Gain: '<S57>/Integral Gain'
   *  Logic: '<S101>/AND3'
   *  RelationalOperator: '<S101>/Equal1'
   */
  if (rtb_NotEqual && (rtb_SignPreSat_k == rtb_Divide4)) {
    rtb_Sum2 = 0;
  } else {
    rtb_Sum2 = rtb_Divide7;
  }

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

  /* DeadZone: '<S153>/DeadZone' incorporates:
   *  Signum: '<S151>/SignPreSat'
   */
  if (rtb_SignPreSat > 800) {
    rtb_SignPreSat = (int16_T)(rtb_SignPreSat - 800);
  } else if (rtb_SignPreSat >= -800) {
    rtb_SignPreSat = 0;
  } else {
    rtb_SignPreSat = (int16_T)(rtb_SignPreSat - -800);
  }

  /* End of DeadZone: '<S153>/DeadZone' */

  /* RelationalOperator: '<S151>/NotEqual' incorporates:
   *  Gain: '<S151>/ZeroGain'
   *  Signum: '<S151>/SignPreSat'
   */
  rtb_NotEqual = (0 != rtb_SignPreSat);

  /* Signum: '<S151>/SignPreSat' */
  if (rtb_SignPreSat < 0) {
    rtb_SignPreSat = -1;
  } else {
    rtb_SignPreSat = (int16_T)(rtb_SignPreSat > 0);
  }

  /* End of Signum: '<S151>/SignPreSat' */

  /* Gain: '<S157>/Integral Gain' incorporates:
   *  Gain: '<S57>/Integral Gain'
   */
  rtb_Divide7 = (int16_T)((int16_T)(200 * rtb_Sum3_l) << 4);

  /* Signum: '<S151>/SignPreIntegrator' incorporates:
   *  Gain: '<S57>/Integral Gain'
   */
  if (rtb_Divide7 < 0) {
    rtb_Divide4 = -1;
  } else {
    rtb_Divide4 = (int16_T)(rtb_Divide7 > 0);
  }

  /* End of Signum: '<S151>/SignPreIntegrator' */

  /* Switch: '<S151>/Switch' incorporates:
   *  Constant: '<S151>/Constant1'
   *  DataTypeConversion: '<S151>/DataTypeConv1'
   *  DataTypeConversion: '<S151>/DataTypeConv2'
   *  Gain: '<S57>/Integral Gain'
   *  Logic: '<S151>/AND3'
   *  RelationalOperator: '<S151>/Equal1'
   */
  if (rtb_NotEqual && (rtb_SignPreSat == rtb_Divide4)) {
    rtb_Sum3_l = 0;
  } else {
    rtb_Sum3_l = rtb_Divide7;
  }

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

  /* DeadZone: '<S53>/DeadZone' incorporates:
   *  Signum: '<S51>/SignPreSat'
   */
  if (rtb_Multiply_idx_1 > 8960) {
    rtb_Multiply_idx_1 = (int16_T)(rtb_Multiply_idx_1 - 8960);
  } else if (rtb_Multiply_idx_1 >= -8960) {
    rtb_Multiply_idx_1 = 0;
  } else {
    rtb_Multiply_idx_1 = (int16_T)(rtb_Multiply_idx_1 - -8960);
  }

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

  /* RelationalOperator: '<S51>/NotEqual' incorporates:
   *  Gain: '<S51>/ZeroGain'
   *  Signum: '<S51>/SignPreSat'
   */
  rtb_NotEqual = (0 != rtb_Multiply_idx_1);

  /* Signum: '<S51>/SignPreSat' */
  if (rtb_Multiply_idx_1 < 0) {
    rtb_Multiply_idx_1 = -1;
  } else {
    rtb_Multiply_idx_1 = (int16_T)(rtb_Multiply_idx_1 > 0);
  }

  /* End of Signum: '<S51>/SignPreSat' */

  /* Gain: '<S57>/Integral Gain' incorporates:
   *  Sum: '<S16>/Sum1'
   */
  rtb_Divide7 = (int16_T)(1071 * (int16_T)-rtb_Multiply_idx_0);

  /* Update for UnitDelay: '<S21>/UnitDelay1' incorporates:
   *  Sum: '<S21>/Sum3'
   */
  rtDW->UnitDelay1_DSTATE[0] = rtb_Sum3_idx_0;
  rtDW->UnitDelay1_DSTATE[1] = rtb_Sum3_idx_1;

  /* Signum: '<S51>/SignPreIntegrator' incorporates:
   *  Gain: '<S57>/Integral Gain'
   */
  if (rtb_Divide7 < 0) {
    rtb_Divide4 = -1;
  } else {
    rtb_Divide4 = (int16_T)(rtb_Divide7 > 0);
  }

  /* End of Signum: '<S51>/SignPreIntegrator' */

  /* Switch: '<S51>/Switch' incorporates:
   *  Constant: '<S51>/Constant1'
   *  DataTypeConversion: '<S51>/DataTypeConv1'
   *  DataTypeConversion: '<S51>/DataTypeConv2'
   *  Logic: '<S51>/AND3'
   *  RelationalOperator: '<S51>/Equal1'
   */
  if (rtb_NotEqual && (rtb_Multiply_idx_1 == rtb_Divide4)) {
    rtb_Divide7 = 0;
  }

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

  /* Update for DiscreteIntegrator: '<S60>/Integrator' */
  rtDW->Integrator_DSTATE += rtb_Divide7;

  /* Update for DiscreteIntegrator: '<S160>/Integrator' incorporates:
   *  Switch: '<S151>/Switch'
   */
  rtDW->Integrator_DSTATE_i += rtb_Sum3_l;

  /* Update for DiscreteIntegrator: '<S110>/Integrator' incorporates:
   *  Switch: '<S101>/Switch'
   */
  rtDW->Integrator_DSTATE_p += rtb_Sum2;

  /* End of Outputs for SubSystem: '<S1>/controller' */

  /* Outport: '<Root>/pwm_counts' incorporates:
   *  DataTypeConversion: '<S177>/Data Type Conversion4'
   *  DataTypeConversion: '<S177>/Data Type Conversion5'
   *  MATLAB Function: '<S177>/phase_time'
   */
  rtY_pwm_counts[0] = rtb_DataTypeConversion3;

  /* Outputs for Atomic SubSystem: '<S1>/controller' */
  rtY_pwm_counts[1] = (uint16_T)rtb_Sum1;
  rtY_pwm_counts[2] = (uint16_T)rtb_Divide;

  /* End of Outputs for SubSystem: '<S1>/controller' */
  rtY_pwm_counts[3] = rtb_DataTypeConversion3;

  /* Outputs for Atomic SubSystem: '<S1>/controller' */
  rtY_pwm_counts[4] = (uint16_T)rtb_Sum1;
  rtY_pwm_counts[5] = (uint16_T)rtb_Divide;

  /* End of Outputs for SubSystem: '<S1>/controller' */
  rtY_pwm_counts[6] = rtb_o_samp_p1;
  rate_scheduler(rtM);
}

/* Model initialize function */
void PMSM_Controller_initialize(RT_MODEL *const rtM)
{
  /* Registration code */

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

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