File content as of revision 0:70d27fec6d71:

/*
 * File: rtwdemo_pmsmfoc.c
 *
 * Code generated for Simulink model 'rtwdemo_pmsmfoc'.
 *
 * Model version                  : 1.2949
 * Simulink Coder version         : 8.7 (R2014b) 11-Aug-2014
 * C/C++ source code generated on : Sat Oct 11 00:06:05 2014
 *
 * Target selection: ert.tlc
 * Embedded hardware selection: Generic->32-bit Embedded Processor
 * Code generation objective: Execution efficiency
 * Validation result: Not run
 */

#include "rtwdemo_pmsmfoc.h"

/* Named constants for Chart: '<S1>/Controller_Mode_Scheduler' */
#define IN_Motor_Control               ((uint8_T)1U)
#define IN_Motor_On                    ((uint8_T)1U)
#define IN_NO_ACTIVE_CHILD             ((uint8_T)0U)
#define IN_Position_Control            ((uint8_T)1U)
#define IN_Ramp_To_Stop                ((uint8_T)2U)
#define IN_Stand_By                    ((uint8_T)2U)
#define IN_Startup_Open_Loop_Control   ((uint8_T)2U)
#define IN_Torque_Control              ((uint8_T)3U)
#define IN_Velocity_Control            ((uint8_T)4U)

/* Named constants for Chart: '<S49>/Wait_For_Valid_Position' */
#define IN_Position_Valid              ((uint8_T)1U)
#define IN_Wait_For_Valid_Position     ((uint8_T)2U)
#define IN_Wait_For_Valid_Velocity     ((uint8_T)3U)
#define WaitForValidVelocityTicks      (124)

/* Exported block signals */
real32_T phase_currents[2];            /* '<S48>/Product' */
real32_T rotor_position;               /* '<S49>/Add' */
real32_T velocity_measured;            /* '<S55>/Scale_Output' */
real32_T d_current_error;              /* '<S12>/Sum2' */
real32_T q_current_command;            /* '<S5>/Lo_to_Hi_Rate_Transition1' */
real32_T q_current_measured;           /* '<S22>/Add' */
real32_T q_current_error;              /* '<S12>/Sum' */
real32_T phase_voltages[3];            /* '<S27>/Select_Sector' */
real32_T velocity_error;               /* '<S11>/Sum2' */
EnumControllerMode controller_mode;    /* '<S1>/Controller_Mode_Scheduler' */

/* Exported block parameters */
CTRLPARAMS_STRUCT ctrlParams = {
  10.0F,
  10000.0F,
  0.005F,
  0.015F,
  0.1F,
  0.6F,
  1.0F,
  0.2F,
  20.0F,
  2252.25F,
  0.00488400506F,
  -0.0F,
  4.0F
} ;                                    /* Variable: ctrlParams
                                        * Referenced by:
                                        *   '<S1>/Controller_Mode_Scheduler'
                                        *   '<S48>/ADC_Driver_Units_To_Amps'
                                        *   '<S48>/ADC_Zero_Offset'
                                        *   '<S49>/Offset_Between_Encoder_Zero_And_Mechanical_Zero'
                                        *   '<S14>/number_of_pole_pairs'
                                        *   '<S42>/Integral Gain'
                                        *   '<S42>/Proportional Gain'
                                        *   '<S45>/Integral Gain'
                                        *   '<S45>/Proportional Gain'
                                        *   '<S63>/Startup_Acceleration_Constant'
                                        *   '<S17>/Integral Gain'
                                        *   '<S17>/Proportional Gain'
                                        *   '<S18>/Integral Gain'
                                        *   '<S18>/Proportional Gain'
                                        */

/* Constant parameters (auto storage) */
const ConstParam ConstP = {
  /* Computed Parameter: Lookup_Table_table
   * Referenced by: '<S28>/Lookup_Table'
   */
  { 2U, 2U, 6U, 1U, 4U, 3U, 5U }
};

/* Block signals and states (auto storage) */
D_Work DWork;

/* Real-time model */
RT_MODEL M_;
RT_MODEL *const M = &M_;
extern real32_T rt_roundf(real32_T u);
extern real32_T rt_modf(real32_T u0, real32_T u1);
static void rate_scheduler(void);

/*
 *   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(void)
{
  /* 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).
   */
  (M->Timing.TaskCounters.TID[1])++;
  if ((M->Timing.TaskCounters.TID[1]) > 124) {/* Sample time: [0.005s, 0.0s] */
    M->Timing.TaskCounters.TID[1] = 0;
  }
}

real32_T rt_roundf(real32_T u)
{
  real32_T y;
  if (((real32_T)fabs(u)) < 8.388608E+6F) {
    if (u >= 0.5F) {
      y = (real32_T)floor(u + 0.5F);
    } else if (u > -0.5F) {
      y = 0.0F;
    } else {
      y = (real32_T)ceil(u - 0.5F);
    }
  } else {
    y = u;
  }

  return y;
}

real32_T rt_modf(real32_T u0, real32_T u1)
{
  real32_T y;
  real32_T tmp;
  if (u1 == 0.0F) {
    y = u0;
  } else {
    tmp = u0 / u1;
    if (u1 <= ((real32_T)floor(u1))) {
      y = u0 - (((real32_T)floor(tmp)) * u1);
    } else if (((real32_T)fabs(tmp - rt_roundf(tmp))) <= (FLT_EPSILON *
                ((real32_T)fabs(tmp)))) {
      y = 0.0F;
    } else {
      y = (tmp - ((real32_T)floor(tmp))) * u1;
    }
  }

  return y;
}

/* Model step function */
extern "C" EnumErrorType Controller(uint16_T motor_on, EnumCommandType command_type,
  real32_T current_request, SENSORS_STRUCT *sensors, uint16_T pwm_compare[3])
{
  real32_T sin_coefficient;
  int32_T Wrap_To_Pi;
  real32_T electrical_angle;
  real32_T cos_coefficient;
  real32_T SignDeltaU_b;
  int16_T Enum_To_Int;
  uint8_T FixPtRelationalOperator;
  real32_T SignDeltaU;
  real32_T Gain1;
  int8_T rtPrevAction;
  real32_T alpha_voltage;
  real32_T SignPreIntegrator_f;
  real32_T IntegralGain_j;
  boolean_T RelationalOperator;
  boolean_T NotEqual_b;
  real32_T Switch_fr;
  real32_T Sectors_2_and_5_idx_1;
  real32_T Sectors_2_and_5_idx_2;
  int16_T u0;

  /* specified return value */
  EnumErrorType error;

  /* Product: '<S48>/Product' incorporates:
   *  Constant: '<S48>/ADC_Driver_Units_To_Amps'
   *  Constant: '<S48>/ADC_Zero_Offset'
   *  Inport: '<Root>/sensors'
   *  Sum: '<S48>/Add'
   */
  phase_currents[0] = (((real32_T)sensors->adc_phase_currents[0]) -
                       ctrlParams.AdcZeroOffsetDriverUnits) *
    ctrlParams.AdcDriverUnitsToAmps;
  phase_currents[1] = (((real32_T)sensors->adc_phase_currents[1]) -
                       ctrlParams.AdcZeroOffsetDriverUnits) *
    ctrlParams.AdcDriverUnitsToAmps;

  /* Chart: '<S49>/Wait_For_Valid_Position' incorporates:
   *  Inport: '<Root>/sensors'
   */
  /* Gateway: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
  if (DWork.temporalCounter_i1 < 127U) {
    DWork.temporalCounter_i1++;
  }

  /* During: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
  if (DWork.is_active_c2_rtwdemo_pmsmfoc == 0U) {
    /* Entry: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
    DWork.is_active_c2_rtwdemo_pmsmfoc = 1U;

    /* Entry Internal: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
    /* Transition: '<S52>:113' */
    if (sensors->encoder_valid == 0) {
      /* Transition: '<S52>:114' */
      /* Transition: '<S52>:115' */
      DWork.is_c2_rtwdemo_pmsmfoc = IN_Wait_For_Valid_Position;
    } else {
      /* Transition: '<S52>:120' */
      /* Transition: '<S52>:119' */
      DWork.is_c2_rtwdemo_pmsmfoc = IN_Position_Valid;
      DWork.Position_Valid = 1U;
    }
  } else {
    switch (DWork.is_c2_rtwdemo_pmsmfoc) {
     case IN_Position_Valid:
      /* During 'Position_Valid': '<S52>:25' */
      break;

     case IN_Wait_For_Valid_Position:
      /* During 'Wait_For_Valid_Position': '<S52>:99' */
      if (sensors->encoder_valid != 0) {
        /* Transition: '<S52>:100' */
        DWork.is_c2_rtwdemo_pmsmfoc = IN_Wait_For_Valid_Velocity;
        DWork.temporalCounter_i1 = 0U;
      }
      break;

     default:
      /* During 'Wait_For_Valid_Velocity': '<S52>:101' */
      if (DWork.temporalCounter_i1 >= WaitForValidVelocityTicks) {
        /* Transition: '<S52>:102' */
        DWork.is_c2_rtwdemo_pmsmfoc = IN_Position_Valid;
        DWork.Position_Valid = 1U;
      }
      break;
    }
  }

  /* End of Chart: '<S49>/Wait_For_Valid_Position' */

  /* Sum: '<S49>/Add' incorporates:
   *  Constant: '<S49>/Offset_Between_Encoder_Zero_And_Mechanical_Zero'
   *  Gain: '<S49>/radians_per_counts'
   *  Inport: '<Root>/sensors'
   */
  rotor_position = (0.000785398181F * ((real32_T)sensors->encoder_counter)) +
    ctrlParams.EncoderToMechanicalZeroOffsetRads;
  if (M->Timing.TaskCounters.TID[1] == 0) {
    /* Gain: '<S55>/Wrap_To_Pi' incorporates:
     *  DataTypeConversion: '<S55>/Convert_to_Uint32'
     *  Gain: '<S55>/Scale_Input'
     */
    Wrap_To_Pi = (((int32_T)(1.70891312E+8F * rotor_position)) << 2);

    /* Gain: '<S55>/Scale_Output' incorporates:
     *  DataTypeConversion: '<S55>/Difference_to_Single'
     *  Delay: '<S55>/Position_Delay'
     *  Sum: '<S55>/Difference_Wrap'
     */
    velocity_measured = ((real32_T)(Wrap_To_Pi - DWork.Position_Delay_DSTATE)) *
      2.92583621E-7F;

    /* Chart: '<S1>/Controller_Mode_Scheduler' incorporates:
     *  Inport: '<Root>/command_type'
     *  Inport: '<Root>/command_value'
     *  Inport: '<Root>/motor_on'
     */
    /* Gateway: Mode_Scheduler/Controller_Mode_Scheduler */
    /* During: Mode_Scheduler/Controller_Mode_Scheduler */
    if (DWork.is_c1_rtwdemo_pmsmfoc == IN_Motor_On) {
      /* During 'Motor_On': '<S4>:338' */
      if (DWork.error_l != 0) {
        /* Transition: '<S4>:339' */
        /* Transition: '<S4>:353' */
        /* Exit Internal 'Motor_On': '<S4>:338' */
        /* Exit Internal 'Motor_Control': '<S4>:344' */
        DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD;
        DWork.is_Motor_On = IN_NO_ACTIVE_CHILD;
        DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By;

        /* Entry 'Stand_By': '<S4>:154' */
        controller_mode = StandBy;
      } else if (DWork.is_Motor_On == IN_Motor_Control) {
        /* During 'Motor_Control': '<S4>:344' */
        if (!(motor_on != 0)) {
          /* Transition: '<S4>:282' */
          /* Exit Internal 'Motor_Control': '<S4>:344' */
          DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD;
          DWork.is_Motor_On = IN_Ramp_To_Stop;

          /* Entry 'Ramp_To_Stop': '<S4>:270' */
          controller_mode = VelocityControl;
          DWork.velocity_command = 0.0F;
          DWork.torque_command = 0.0F;
        } else {
          switch (DWork.is_Motor_Control) {
           case IN_Position_Control:
            /* During 'Position_Control': '<S4>:226' */
            DWork.position_command = current_request;
            break;

           case IN_Startup_Open_Loop_Control:
            /* During 'Startup_Open_Loop_Control': '<S4>:103' */
            if (DWork.Position_Valid != 0) {
              /* Transition: '<S4>:157' */
              /* Transition: '<S4>:233' */
              switch (command_type) {
               case Velocity:
                /* Transition: '<S4>:162' */
                DWork.is_Motor_Control = IN_Velocity_Control;

                /* Entry 'Velocity_Control': '<S4>:108' */
                controller_mode = VelocityControl;
                DWork.velocity_command = current_request;
                break;

               case Position:
                /* Transition: '<S4>:235' */
                /* Transition: '<S4>:234' */
                DWork.is_Motor_Control = IN_Position_Control;

                /* Entry 'Position_Control': '<S4>:226' */
                controller_mode = PositionControl;
                DWork.position_command = current_request;
                break;

               default:
                /* Transition: '<S4>:237' */
                /* Transition: '<S4>:158' */
                /*  [command_type==Torque] */
                DWork.is_Motor_Control = IN_Torque_Control;

                /* Entry 'Torque_Control': '<S4>:220' */
                controller_mode = TorqueControl;
                DWork.torque_command = current_request;
                break;
              }
            }
            break;

           case IN_Torque_Control:
            /* During 'Torque_Control': '<S4>:220' */
            DWork.torque_command = current_request;
            break;

           default:
            /* During 'Velocity_Control': '<S4>:108' */
            DWork.velocity_command = current_request;
            break;
          }
        }
      } else {
        /* During 'Ramp_To_Stop': '<S4>:270' */
        if (((real32_T)fabs(velocity_measured)) < ctrlParams.RampToStopVelocity)
        {
          /* Transition: '<S4>:169' */
          /* Transition: '<S4>:355' */
          DWork.is_Motor_On = IN_NO_ACTIVE_CHILD;
          DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By;

          /* Entry 'Stand_By': '<S4>:154' */
          controller_mode = StandBy;
        }
      }
    } else {
      /* During 'Stand_By': '<S4>:154' */
      if ((motor_on != 0) && (!(DWork.error_l != 0))) {
        /* Transition: '<S4>:164' */
        if (!(DWork.Position_Valid != 0)) {
          /* Transition: '<S4>:133' */
          DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
          DWork.is_Motor_On = IN_Motor_Control;
          DWork.is_Motor_Control = IN_Startup_Open_Loop_Control;

          /* Entry 'Startup_Open_Loop_Control': '<S4>:103' */
          controller_mode = Startup;
          DWork.torque_command = ctrlParams.StartupCurrent;
        } else {
          /* Transition: '<S4>:137' */
          /* Transition: '<S4>:233' */
          switch (command_type) {
           case Velocity:
            /* Transition: '<S4>:162' */
            DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
            DWork.is_Motor_On = IN_Motor_Control;
            DWork.is_Motor_Control = IN_Velocity_Control;

            /* Entry 'Velocity_Control': '<S4>:108' */
            controller_mode = VelocityControl;
            DWork.velocity_command = current_request;
            break;

           case Position:
            /* Transition: '<S4>:235' */
            /* Transition: '<S4>:234' */
            DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
            DWork.is_Motor_On = IN_Motor_Control;
            DWork.is_Motor_Control = IN_Position_Control;

            /* Entry 'Position_Control': '<S4>:226' */
            controller_mode = PositionControl;
            DWork.position_command = current_request;
            break;

           default:
            /* Transition: '<S4>:237' */
            /* Transition: '<S4>:158' */
            /*  [command_type==Torque] */
            DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
            DWork.is_Motor_On = IN_Motor_Control;
            DWork.is_Motor_Control = IN_Torque_Control;

            /* Entry 'Torque_Control': '<S4>:220' */
            controller_mode = TorqueControl;
            DWork.torque_command = current_request;
            break;
          }
        }
      }
    }

    /* End of Chart: '<S1>/Controller_Mode_Scheduler' */

    /* RelationalOperator: '<S51>/Relational Operator' incorporates:
     *  Constant: '<S62>/Constant'
     */
    DWork.RelationalOperator_a = (controller_mode == Startup);
  }

  /* Outputs for Enabled SubSystem: '<S51>/Open Loop Position' incorporates:
   *  EnablePort: '<S63>/Enable'
   */
  if (DWork.RelationalOperator_a) {
    if (!DWork.OpenLoopPosition_MODE) {
      /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Position' */
      DWork.Integrate_To_Position_DSTATE = 0.0F;

      /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' */
      DWork.Integrate_To_Velocity_DSTATE = 0.0F;
      DWork.OpenLoopPosition_MODE = true;
    }

    /* DiscreteIntegrator: '<S63>/Integrate_To_Position' */
    DWork.position = DWork.Integrate_To_Position_DSTATE;

    /* Update for DiscreteIntegrator: '<S63>/Integrate_To_Position' incorporates:
     *  DiscreteIntegrator: '<S63>/Integrate_To_Velocity'
     */
    DWork.Integrate_To_Position_DSTATE += 4.0E-5F *
      DWork.Integrate_To_Velocity_DSTATE;

    /* Update for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' incorporates:
     *  Constant: '<S63>/Startup_Acceleration_Constant'
     */
    DWork.Integrate_To_Velocity_DSTATE += 4.0E-5F *
      ctrlParams.StartupAcceleration;
  } else {
    if (DWork.OpenLoopPosition_MODE) {
      DWork.OpenLoopPosition_MODE = false;
    }
  }

  /* End of Outputs for SubSystem: '<S51>/Open Loop Position' */

  /* Switch: '<S51>/Switch' */
  if (DWork.RelationalOperator_a) {
    Switch_fr = DWork.position;
  } else {
    Switch_fr = rotor_position;
  }

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

  /* Gain: '<S14>/number_of_pole_pairs' */
  electrical_angle = ctrlParams.PmsmPolePairs * Switch_fr;

  /* Trigonometry: '<S14>/sine_cosine' */
  sin_coefficient = (real32_T)sin(electrical_angle);
  cos_coefficient = (real32_T)cos(electrical_angle);

  /* Gain: '<S21>/Beta_Gain' incorporates:
   *  Gain: '<S21>/B_Gain'
   *  Sum: '<S21>/Add'
   */
  SignDeltaU_b = ((2.0F * phase_currents[1]) + phase_currents[0]) * 0.577350259F;

  /* Sum: '<S12>/Sum2' incorporates:
   *  Constant: '<S12>/d_current_command (A)'
   *  Product: '<S22>/Product2'
   *  Product: '<S22>/Product3'
   *  Sum: '<S22>/Add1'
   */
  d_current_error = 0.0F - ((phase_currents[0] * cos_coefficient) +
    (SignDeltaU_b * sin_coefficient));

  /* DataTypeConversion: '<S8>/Enum_To_Int' */
  Enum_To_Int = (int16_T)controller_mode;

  /* RelationalOperator: '<S13>/FixPt Relational Operator' incorporates:
   *  UnitDelay: '<S13>/Delay Input1'
   */
  FixPtRelationalOperator = (uint8_T)(Enum_To_Int != DWork.DelayInput1_DSTATE);

  /* DiscreteIntegrator: '<S17>/Integrator' */
  if ((FixPtRelationalOperator != 0) || (DWork.Integrator_PrevResetState != 0))
  {
    DWork.Integrator_DSTATE = 0.0F;
  }

  /* Sum: '<S17>/Sum' incorporates:
   *  DiscreteIntegrator: '<S17>/Integrator'
   *  Gain: '<S17>/Proportional Gain'
   */
  SignDeltaU = (ctrlParams.Current_P * d_current_error) +
    DWork.Integrator_DSTATE;

  /* Saturate: '<S17>/Saturate' */
  if (SignDeltaU > 12.0F) {
    Gain1 = 12.0F;
  } else if (SignDeltaU < -12.0F) {
    Gain1 = -12.0F;
  } else {
    Gain1 = SignDeltaU;
  }

  /* End of Saturate: '<S17>/Saturate' */
  if (M->Timing.TaskCounters.TID[1] == 0) {
    /* SwitchCase: '<S5>/Switch Case' incorporates:
     *  Inport: '<S10>/torque_command'
     */
    rtPrevAction = DWork.SwitchCase_ActiveSubsystem;
    switch (controller_mode) {
     case VelocityControl:
      DWork.SwitchCase_ActiveSubsystem = 0;
      break;

     case PositionControl:
      DWork.SwitchCase_ActiveSubsystem = 1;
      break;

     default:
      DWork.SwitchCase_ActiveSubsystem = 2;
      break;
    }

    switch (DWork.SwitchCase_ActiveSubsystem) {
     case 0:
      if (DWork.SwitchCase_ActiveSubsystem != rtPrevAction) {
        /* InitializeConditions for IfAction SubSystem: '<S5>/Velocity_Control' incorporates:
         *  InitializeConditions for ActionPort: '<S11>/Action Port'
         */
        /* InitializeConditions for SwitchCase: '<S5>/Switch Case' incorporates:
         *  InitializeConditions for DiscreteIntegrator: '<S45>/Integrator'
         */
        DWork.Integrator_DSTATE_f = 0.0F;

        /* End of InitializeConditions for SubSystem: '<S5>/Velocity_Control' */
      }

      /* Outputs for IfAction SubSystem: '<S5>/Velocity_Control' incorporates:
       *  ActionPort: '<S11>/Action Port'
       */
      /* Sum: '<S11>/Sum2' */
      velocity_error = DWork.velocity_command - velocity_measured;

      /* Sum: '<S45>/Sum' incorporates:
       *  DiscreteIntegrator: '<S45>/Integrator'
       *  Gain: '<S45>/Proportional Gain'
       */
      electrical_angle = (ctrlParams.Velocity_P * velocity_error) +
        DWork.Integrator_DSTATE_f;

      /* Saturate: '<S45>/Saturate' */
      if (electrical_angle > 2.0F) {
        /* SignalConversion: '<S11>/Isolate_For_Merge' */
        DWork.Merge = 2.0F;
      } else if (electrical_angle < -2.0F) {
        /* SignalConversion: '<S11>/Isolate_For_Merge' */
        DWork.Merge = -2.0F;
      } else {
        /* SignalConversion: '<S11>/Isolate_For_Merge' */
        DWork.Merge = electrical_angle;
      }

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

      /* DeadZone: '<S46>/DeadZone' */
      if (electrical_angle > 2.0F) {
        electrical_angle -= 2.0F;
      } else if (electrical_angle >= -2.0F) {
        electrical_angle = 0.0F;
      } else {
        electrical_angle -= -2.0F;
      }

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

      /* RelationalOperator: '<S46>/NotEqual' */
      NotEqual_b = (0.0F != electrical_angle);

      /* Signum: '<S46>/SignDeltaU' */
      if (electrical_angle < 0.0F) {
        electrical_angle = -1.0F;
      } else {
        if (electrical_angle > 0.0F) {
          electrical_angle = 1.0F;
        }
      }

      /* End of Signum: '<S46>/SignDeltaU' */

      /* Gain: '<S45>/Integral Gain' */
      Switch_fr = ctrlParams.Velocity_I * velocity_error;

      /* DataTypeConversion: '<S46>/DataTypeConv1' */
      if (electrical_angle < 128.0F) {
        rtPrevAction = (int8_T)electrical_angle;
      } else {
        rtPrevAction = MAX_int8_T;
      }

      /* End of DataTypeConversion: '<S46>/DataTypeConv1' */

      /* Signum: '<S46>/SignPreIntegrator' */
      if (Switch_fr < 0.0F) {
        electrical_angle = -1.0F;
      } else if (Switch_fr > 0.0F) {
        electrical_angle = 1.0F;
      } else {
        electrical_angle = Switch_fr;
      }

      /* Switch: '<S45>/Switch' incorporates:
       *  Constant: '<S45>/Constant'
       *  DataTypeConversion: '<S46>/DataTypeConv2'
       *  Logic: '<S46>/AND'
       *  RelationalOperator: '<S46>/Equal'
       *  Signum: '<S46>/SignPreIntegrator'
       */
      if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
        Switch_fr = 0.0F;
      }

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

      /* Update for DiscreteIntegrator: '<S45>/Integrator' */
      DWork.Integrator_DSTATE_f += 0.005F * Switch_fr;

      /* End of Outputs for SubSystem: '<S5>/Velocity_Control' */
      break;

     case 1:
      if (DWork.SwitchCase_ActiveSubsystem != rtPrevAction) {
        /* InitializeConditions for IfAction SubSystem: '<S5>/Position_Control' incorporates:
         *  InitializeConditions for ActionPort: '<S9>/Action Port'
         */
        /* InitializeConditions for SwitchCase: '<S5>/Switch Case' incorporates:
         *  InitializeConditions for DiscreteIntegrator: '<S42>/Integrator'
         */
        DWork.Integrator_DSTATE_lc = 0.0F;

        /* End of InitializeConditions for SubSystem: '<S5>/Position_Control' */
      }

      /* Outputs for IfAction SubSystem: '<S5>/Position_Control' incorporates:
       *  ActionPort: '<S9>/Action Port'
       */
      /* Sum: '<S9>/Sum2' */
      Switch_fr = DWork.position_command - Switch_fr;

      /* Switch: '<S43>/Select_Angle' incorporates:
       *  Constant: '<S43>/Neg_Pi_Constant'
       *  Constant: '<S43>/Pi_Constant_1'
       *  Constant: '<S43>/Pi_Constant_2'
       *  Constant: '<S43>/Pi_Constant_3'
       *  Constant: '<S43>/Two_Pi_Constant'
       *  Logic: '<S43>/OR'
       *  Math: '<S43>/Modulus'
       *  RelationalOperator: '<S43>/Greater_Than'
       *  RelationalOperator: '<S43>/Less_Than'
       *  Sum: '<S43>/Add'
       *  Sum: '<S43>/Subtract'
       */
      if ((Switch_fr < -1.57079637F) || (Switch_fr >= 1.57079637F)) {
        Switch_fr = rt_modf(Switch_fr + 1.57079637F, 3.14159274F) - 1.57079637F;
      }

      /* End of Switch: '<S43>/Select_Angle' */

      /* Sum: '<S42>/Sum' incorporates:
       *  DiscreteIntegrator: '<S42>/Integrator'
       *  Gain: '<S42>/Proportional Gain'
       */
      electrical_angle = (ctrlParams.Position_P * Switch_fr) +
        DWork.Integrator_DSTATE_lc;

      /* Saturate: '<S42>/Saturate' */
      if (electrical_angle > 2.0F) {
        /* SignalConversion: '<S9>/Isolate_For_Merge' */
        DWork.Merge = 2.0F;
      } else if (electrical_angle < -2.0F) {
        /* SignalConversion: '<S9>/Isolate_For_Merge' */
        DWork.Merge = -2.0F;
      } else {
        /* SignalConversion: '<S9>/Isolate_For_Merge' */
        DWork.Merge = electrical_angle;
      }

      /* End of Saturate: '<S42>/Saturate' */

      /* DeadZone: '<S44>/DeadZone' */
      if (electrical_angle > 2.0F) {
        electrical_angle -= 2.0F;
      } else if (electrical_angle >= -2.0F) {
        electrical_angle = 0.0F;
      } else {
        electrical_angle -= -2.0F;
      }

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

      /* RelationalOperator: '<S44>/NotEqual' */
      NotEqual_b = (0.0F != electrical_angle);

      /* Signum: '<S44>/SignDeltaU' */
      if (electrical_angle < 0.0F) {
        electrical_angle = -1.0F;
      } else {
        if (electrical_angle > 0.0F) {
          electrical_angle = 1.0F;
        }
      }

      /* End of Signum: '<S44>/SignDeltaU' */

      /* Gain: '<S42>/Integral Gain' */
      Switch_fr *= ctrlParams.Position_I;

      /* DataTypeConversion: '<S44>/DataTypeConv1' */
      if (electrical_angle < 128.0F) {
        rtPrevAction = (int8_T)electrical_angle;
      } else {
        rtPrevAction = MAX_int8_T;
      }

      /* End of DataTypeConversion: '<S44>/DataTypeConv1' */

      /* Signum: '<S44>/SignPreIntegrator' */
      if (Switch_fr < 0.0F) {
        electrical_angle = -1.0F;
      } else if (Switch_fr > 0.0F) {
        electrical_angle = 1.0F;
      } else {
        electrical_angle = Switch_fr;
      }

      /* Switch: '<S42>/Switch' incorporates:
       *  Constant: '<S42>/Constant'
       *  DataTypeConversion: '<S44>/DataTypeConv2'
       *  Logic: '<S44>/AND'
       *  RelationalOperator: '<S44>/Equal'
       *  Signum: '<S44>/SignPreIntegrator'
       */
      if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
        Switch_fr = 0.0F;
      }

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

      /* Update for DiscreteIntegrator: '<S42>/Integrator' */
      DWork.Integrator_DSTATE_lc += 0.005F * Switch_fr;

      /* End of Outputs for SubSystem: '<S5>/Position_Control' */
      break;

     case 2:
      /* Outputs for IfAction SubSystem: '<S5>/Torque_Control' incorporates:
       *  ActionPort: '<S10>/Action Port'
       */
      DWork.Merge = DWork.torque_command;

      /* End of Outputs for SubSystem: '<S5>/Torque_Control' */
      break;
    }

    /* End of SwitchCase: '<S5>/Switch Case' */
  }

  /* RateTransition: '<S5>/Lo_to_Hi_Rate_Transition1' */
  q_current_command = DWork.Merge;

  /* Sum: '<S22>/Add' incorporates:
   *  Product: '<S22>/Product'
   *  Product: '<S22>/Product1'
   */
  q_current_measured = (SignDeltaU_b * cos_coefficient) - (phase_currents[0] *
    sin_coefficient);

  /* Sum: '<S12>/Sum' */
  q_current_error = q_current_command - q_current_measured;

  /* DiscreteIntegrator: '<S18>/Integrator' */
  if ((FixPtRelationalOperator != 0) || (DWork.Integrator_PrevResetState_c != 0))
  {
    DWork.Integrator_DSTATE_l = 0.0F;
  }

  /* Sum: '<S18>/Sum' incorporates:
   *  DiscreteIntegrator: '<S18>/Integrator'
   *  Gain: '<S18>/Proportional Gain'
   */
  SignDeltaU_b = (ctrlParams.Current_P * q_current_error) +
    DWork.Integrator_DSTATE_l;

  /* Saturate: '<S18>/Saturate' */
  if (SignDeltaU_b > 12.0F) {
    alpha_voltage = 12.0F;
  } else if (SignDeltaU_b < -12.0F) {
    alpha_voltage = -12.0F;
  } else {
    alpha_voltage = SignDeltaU_b;
  }

  /* End of Saturate: '<S18>/Saturate' */

  /* Sum: '<S24>/Add' incorporates:
   *  Product: '<S24>/Product'
   *  Product: '<S24>/Product1'
   */
  SignPreIntegrator_f = (Gain1 * sin_coefficient) + (alpha_voltage *
    cos_coefficient);

  /* Gain: '<S29>/Gain' */
  IntegralGain_j = 0.5F * SignPreIntegrator_f;

  /* Sum: '<S24>/Add1' incorporates:
   *  Product: '<S24>/Product2'
   *  Product: '<S24>/Product3'
   */
  alpha_voltage = (Gain1 * cos_coefficient) - (alpha_voltage * sin_coefficient);

  /* Gain: '<S29>/Gain1' */
  Gain1 = 0.866025388F * alpha_voltage;

  /* Gain: '<S30>/Space_Vector_Gain' incorporates:
   *  Gain: '<S30>/Alpha_Gain'
   *  Sum: '<S30>/Add'
   */
  electrical_angle = (((1.73205078F * alpha_voltage) + 33.941124F) +
                      SignPreIntegrator_f) * 0.353553385F;

  /* Gain: '<S33>/Va_Gain' incorporates:
   *  Gain: '<S33>/Alpha_Gain'
   *  Gain: '<S33>/Beta_Gain'
   *  Sum: '<S33>/Add'
   */
  Switch_fr = ((33.941124F - (1.73205078F * alpha_voltage)) + (3.0F *
    SignPreIntegrator_f)) * 0.353553385F;

  /* Gain: '<S36>/Space_Vector_Gain' incorporates:
   *  Gain: '<S36>/Alpha_Gain'
   *  Sum: '<S36>/Add'
   */
  cos_coefficient = ((33.941124F - (1.73205078F * alpha_voltage)) -
                     SignPreIntegrator_f) * 0.353553385F;

  /* Gain: '<S31>/Space_Vector_Gain' incorporates:
   *  Constant: '<S27>/Bus_Voltage'
   *  Gain: '<S31>/Alpha_Gain'
   *  Sum: '<S31>/Add'
   */
  sin_coefficient = ((2.44948983F * alpha_voltage) + 24.0F) * 0.5F;

  /* Gain: '<S34>/Va_Gain' incorporates:
   *  Constant: '<S27>/Bus_Voltage'
   *  Gain: '<S34>/Beta_Gain'
   *  Sum: '<S34>/Add'
   */
  Sectors_2_and_5_idx_1 = ((1.41421354F * SignPreIntegrator_f) + 24.0F) * 0.5F;

  /* Gain: '<S37>/Space_Vector_Gain' incorporates:
   *  Constant: '<S27>/Bus_Voltage'
   *  Gain: '<S37>/Beta_Gain'
   *  Sum: '<S37>/Add'
   */
  Sectors_2_and_5_idx_2 = (24.0F - (1.41421354F * SignPreIntegrator_f)) * 0.5F;

  /* Gain: '<S32>/Space_Vector_Gain' incorporates:
   *  Gain: '<S32>/Alpha_Gain'
   *  Sum: '<S32>/Add'
   */
  phase_voltages[0] = (((1.73205078F * alpha_voltage) + 33.941124F) -
                       SignPreIntegrator_f) * 0.353553385F;

  /* Gain: '<S35>/Va_Gain' incorporates:
   *  Gain: '<S35>/Alpha_Gain'
   *  Sum: '<S35>/Add'
   */
  phase_voltages[1] = ((33.941124F - (1.73205078F * alpha_voltage)) +
                       SignPreIntegrator_f) * 0.353553385F;

  /* Gain: '<S38>/Space_Vector_Gain' incorporates:
   *  Gain: '<S38>/Alpha_Gain'
   *  Gain: '<S38>/Beta_Gain'
   *  Sum: '<S38>/Add'
   */
  phase_voltages[2] = ((33.941124F - (1.73205078F * alpha_voltage)) - (3.0F *
    SignPreIntegrator_f)) * 0.353553385F;

  /* LookupNDDirect: '<S28>/Lookup_Table' incorporates:
   *  Constant: '<S39>/Constant'
   *  Constant: '<S40>/Constant'
   *  Constant: '<S41>/Constant'
   *  Gain: '<S28>/Sector_Gain_VB'
   *  Gain: '<S28>/Sector_Gain_VC'
   *  RelationalOperator: '<S39>/Compare'
   *  RelationalOperator: '<S40>/Compare'
   *  RelationalOperator: '<S41>/Compare'
   *  Sum: '<S28>/Calculate_Phase_Advanced_Sector'
   *  Sum: '<S29>/Add'
   *  Sum: '<S29>/Add1'
   *
   * About '<S28>/Lookup_Table':
   *  1-dimensional Direct Look-Up returning a Scalar
   */
  u0 = (int16_T)(((((Gain1 - IntegralGain_j) > 0.0F) << 1) +
                  (SignPreIntegrator_f > 0.0F)) + ((((0.0F - IntegralGain_j) -
    Gain1) > 0.0F) << 2));
  if (u0 > 6) {
    u0 = 6;
  }

  /* MultiPortSwitch: '<S27>/Select_Sector' incorporates:
   *  LookupNDDirect: '<S28>/Lookup_Table'
   *
   * About '<S28>/Lookup_Table':
   *  1-dimensional Direct Look-Up returning a Scalar
   */
  switch (ConstP.Lookup_Table_table[u0]) {
   case 1:
    phase_voltages[0] = electrical_angle;
    phase_voltages[1] = Switch_fr;
    phase_voltages[2] = cos_coefficient;
    break;

   case 2:
    phase_voltages[0] = sin_coefficient;
    phase_voltages[1] = Sectors_2_and_5_idx_1;
    phase_voltages[2] = Sectors_2_and_5_idx_2;
    break;

   case 3:
    break;

   case 4:
    phase_voltages[0] = electrical_angle;
    phase_voltages[1] = Switch_fr;
    phase_voltages[2] = cos_coefficient;
    break;

   case 5:
    phase_voltages[0] = sin_coefficient;
    phase_voltages[1] = Sectors_2_and_5_idx_1;
    phase_voltages[2] = Sectors_2_and_5_idx_2;
    break;
  }

  /* End of MultiPortSwitch: '<S27>/Select_Sector' */

  /* Switch: '<S6>/Switch' */
  if (DWork.Lo_to_Hi_Rate_Transition3_Buffe) {
    /* Outport: '<Root>/pwm_compare' */
    pwm_compare[0] = 1500U;
    pwm_compare[1] = 1500U;
    pwm_compare[2] = 1500U;
  } else {
    /* Gain: '<S6>/Voltage to PWM Compare Units' */
    electrical_angle = 125.0F * phase_voltages[0];

    /* Saturate: '<S6>/Saturation' */
    if (electrical_angle > 2999.0F) {
      /* Outport: '<Root>/pwm_compare' incorporates:
       *  DataTypeConversion: '<S6>/Quantize'
       */
      pwm_compare[0] = 2999U;
    } else if (electrical_angle < 0.0F) {
      /* Outport: '<Root>/pwm_compare' incorporates:
       *  DataTypeConversion: '<S6>/Quantize'
       */
      pwm_compare[0] = 0U;
    } else {
      /* Outport: '<Root>/pwm_compare' incorporates:
       *  DataTypeConversion: '<S6>/Quantize'
       */
      pwm_compare[0] = (uint16_T)electrical_angle;
    }

    /* Gain: '<S6>/Voltage to PWM Compare Units' */
    electrical_angle = 125.0F * phase_voltages[1];

    /* Saturate: '<S6>/Saturation' */
    if (electrical_angle > 2999.0F) {
      /* Outport: '<Root>/pwm_compare' incorporates:
       *  DataTypeConversion: '<S6>/Quantize'
       */
      pwm_compare[1] = 2999U;
    } else if (electrical_angle < 0.0F) {
      /* Outport: '<Root>/pwm_compare' incorporates:
       *  DataTypeConversion: '<S6>/Quantize'
       */
      pwm_compare[1] = 0U;
    } else {
      /* Outport: '<Root>/pwm_compare' incorporates:
       *  DataTypeConversion: '<S6>/Quantize'
       */
      pwm_compare[1] = (uint16_T)electrical_angle;
    }

    /* Gain: '<S6>/Voltage to PWM Compare Units' */
    electrical_angle = 125.0F * phase_voltages[2];

    /* Saturate: '<S6>/Saturation' */
    if (electrical_angle > 2999.0F) {
      /* Outport: '<Root>/pwm_compare' incorporates:
       *  DataTypeConversion: '<S6>/Quantize'
       */
      pwm_compare[2] = 2999U;
    } else if (electrical_angle < 0.0F) {
      /* Outport: '<Root>/pwm_compare' incorporates:
       *  DataTypeConversion: '<S6>/Quantize'
       */
      pwm_compare[2] = 0U;
    } else {
      /* Outport: '<Root>/pwm_compare' incorporates:
       *  DataTypeConversion: '<S6>/Quantize'
       */
      pwm_compare[2] = (uint16_T)electrical_angle;
    }
  }

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

  /* DeadZone: '<S19>/DeadZone' */
  if (SignDeltaU > 12.0F) {
    SignDeltaU -= 12.0F;
  } else if (SignDeltaU >= -12.0F) {
    SignDeltaU = 0.0F;
  } else {
    SignDeltaU -= -12.0F;
  }

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

  /* RelationalOperator: '<S19>/NotEqual' */
  NotEqual_b = (0.0F != SignDeltaU);

  /* Signum: '<S19>/SignDeltaU' */
  if (SignDeltaU < 0.0F) {
    SignDeltaU = -1.0F;
  } else {
    if (SignDeltaU > 0.0F) {
      SignDeltaU = 1.0F;
    }
  }

  /* End of Signum: '<S19>/SignDeltaU' */

  /* Gain: '<S17>/Integral Gain' */
  IntegralGain_j = ctrlParams.Current_I * d_current_error;

  /* DataTypeConversion: '<S19>/DataTypeConv1' */
  if (SignDeltaU < 128.0F) {
    rtPrevAction = (int8_T)SignDeltaU;
  } else {
    rtPrevAction = MAX_int8_T;
  }

  /* End of DataTypeConversion: '<S19>/DataTypeConv1' */

  /* Signum: '<S19>/SignPreIntegrator' */
  if (IntegralGain_j < 0.0F) {
    electrical_angle = -1.0F;
  } else if (IntegralGain_j > 0.0F) {
    electrical_angle = 1.0F;
  } else {
    electrical_angle = IntegralGain_j;
  }

  /* Switch: '<S17>/Switch' incorporates:
   *  Constant: '<S17>/Constant'
   *  DataTypeConversion: '<S19>/DataTypeConv2'
   *  Logic: '<S19>/AND'
   *  RelationalOperator: '<S19>/Equal'
   *  Signum: '<S19>/SignPreIntegrator'
   */
  if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
    electrical_angle = 0.0F;
  } else {
    electrical_angle = IntegralGain_j;
  }

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

  /* DeadZone: '<S20>/DeadZone' */
  if (SignDeltaU_b > 12.0F) {
    SignDeltaU_b -= 12.0F;
  } else if (SignDeltaU_b >= -12.0F) {
    SignDeltaU_b = 0.0F;
  } else {
    SignDeltaU_b -= -12.0F;
  }

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

  /* RelationalOperator: '<S20>/NotEqual' */
  NotEqual_b = (0.0F != SignDeltaU_b);

  /* Signum: '<S20>/SignDeltaU' */
  if (SignDeltaU_b < 0.0F) {
    SignDeltaU_b = -1.0F;
  } else {
    if (SignDeltaU_b > 0.0F) {
      SignDeltaU_b = 1.0F;
    }
  }

  /* End of Signum: '<S20>/SignDeltaU' */

  /* Gain: '<S18>/Integral Gain' */
  IntegralGain_j = ctrlParams.Current_I * q_current_error;
  if (M->Timing.TaskCounters.TID[1] == 0) {
    /* RelationalOperator: '<S6>/Relational Operator' incorporates:
     *  Constant: '<S47>/Constant'
     */
    RelationalOperator = (controller_mode == StandBy);

    /* Outputs for Enabled SubSystem: '<S58>/Generate_Error' incorporates:
     *  EnablePort: '<S60>/Enable'
     */
    /* Logic: '<S58>/AND' incorporates:
     *  Abs: '<S58>/Velocity_Abs'
     *  Constant: '<S58>/Max_Valid_Velocity_Change'
     *  Constant: '<S59>/Constant'
     *  Delay: '<S58>/Velocity_Delay'
     *  RelationalOperator: '<S58>/Excessive_Velocity_Change'
     *  RelationalOperator: '<S58>/Relational_Operator'
     *  Sum: '<S58>/Velocity_Difference'
     */
    if ((controller_mode == VelocityControl) && (((real32_T)fabs
          (velocity_measured - DWork.Velocity_Delay_DSTATE)) >= 628.318542F)) {
      /* DataStoreWrite: '<S60>/Data_Store_Write' incorporates:
       *  Constant: '<S61>/Constant'
       */
      DWork.error_l = MeasuredVelocityError;
    }

    /* End of Logic: '<S58>/AND' */
    /* End of Outputs for SubSystem: '<S58>/Generate_Error' */
  }

  /* Outport: '<Root>/error' incorporates:
   *  DataStoreRead: '<Root>/Data Store Read'
   */
  error = DWork.error_l;

  /* Update for RateTransition: '<S6>/Lo_to_Hi_Rate_Transition3' */
  if (M->Timing.TaskCounters.TID[1] == 0) {
    DWork.Lo_to_Hi_Rate_Transition3_Buffe = RelationalOperator;

    /* Update for Delay: '<S55>/Position_Delay' */
    DWork.Position_Delay_DSTATE = Wrap_To_Pi;
  }

  /* End of Update for RateTransition: '<S6>/Lo_to_Hi_Rate_Transition3' */

  /* Update for UnitDelay: '<S13>/Delay Input1' */
  DWork.DelayInput1_DSTATE = Enum_To_Int;

  /* Update for DiscreteIntegrator: '<S17>/Integrator' */
  if (FixPtRelationalOperator == 0) {
    DWork.Integrator_DSTATE += 4.0E-5F * electrical_angle;
  }

  if (FixPtRelationalOperator > 0) {
    DWork.Integrator_PrevResetState = 1;
  } else {
    DWork.Integrator_PrevResetState = 0;
  }

  /* End of Update for DiscreteIntegrator: '<S17>/Integrator' */

  /* Update for DiscreteIntegrator: '<S18>/Integrator' */
  if (FixPtRelationalOperator == 0) {
    /* DataTypeConversion: '<S20>/DataTypeConv1' */
    if (SignDeltaU_b < 128.0F) {
      rtPrevAction = (int8_T)SignDeltaU_b;
    } else {
      rtPrevAction = MAX_int8_T;
    }

    /* End of DataTypeConversion: '<S20>/DataTypeConv1' */

    /* Signum: '<S20>/SignPreIntegrator' */
    if (IntegralGain_j < 0.0F) {
      electrical_angle = -1.0F;
    } else if (IntegralGain_j > 0.0F) {
      electrical_angle = 1.0F;
    } else {
      electrical_angle = IntegralGain_j;
    }

    /* Switch: '<S18>/Switch' incorporates:
     *  Constant: '<S18>/Constant'
     *  DataTypeConversion: '<S20>/DataTypeConv2'
     *  Logic: '<S20>/AND'
     *  RelationalOperator: '<S20>/Equal'
     *  Signum: '<S20>/SignPreIntegrator'
     */
    if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
      IntegralGain_j = 0.0F;
    }

    /* End of Switch: '<S18>/Switch' */
    DWork.Integrator_DSTATE_l += 4.0E-5F * IntegralGain_j;
  }

  if (FixPtRelationalOperator > 0) {
    DWork.Integrator_PrevResetState_c = 1;
  } else {
    DWork.Integrator_PrevResetState_c = 0;
  }

  /* End of Update for DiscreteIntegrator: '<S18>/Integrator' */
  if (M->Timing.TaskCounters.TID[1] == 0) {
    /* Update for Delay: '<S58>/Velocity_Delay' */
    DWork.Velocity_Delay_DSTATE = velocity_measured;
  }

  rate_scheduler();
  return error;
}

/* Model initialize function */
extern "C" void Controller_Init(void)
{
  /* Registration code */

  /* initialize real-time model */
  (void) memset((void *)M, 0,
                sizeof(RT_MODEL));

  /* block I/O */

  /* exported global signals */
  phase_currents[0] = 0.0F;
  phase_currents[1] = 0.0F;
  rotor_position = 0.0F;
  velocity_measured = 0.0F;
  d_current_error = 0.0F;
  q_current_command = 0.0F;
  q_current_measured = 0.0F;
  q_current_error = 0.0F;
  phase_voltages[0] = 0.0F;
  phase_voltages[1] = 0.0F;
  phase_voltages[2] = 0.0F;
  velocity_error = 0.0F;
  controller_mode = StandBy;

  /* states (dwork) */
  (void) memset((void *)&DWork, 0,
                sizeof(D_Work));

  /* InitializeConditions for Enabled SubSystem: '<S51>/Open Loop Position' */
  /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Position' */
  DWork.Integrate_To_Position_DSTATE = 0.0F;

  /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' */
  DWork.Integrate_To_Velocity_DSTATE = 0.0F;

  /* End of InitializeConditions for SubSystem: '<S51>/Open Loop Position' */

  /* Start for SwitchCase: '<S5>/Switch Case' */
  DWork.SwitchCase_ActiveSubsystem = -1;

  /* InitializeConditions for IfAction SubSystem: '<S5>/Velocity_Control' */
  /* InitializeConditions for DiscreteIntegrator: '<S45>/Integrator' */
  DWork.Integrator_DSTATE_f = 0.0F;

  /* End of InitializeConditions for SubSystem: '<S5>/Velocity_Control' */

  /* InitializeConditions for IfAction SubSystem: '<S5>/Position_Control' */
  /* InitializeConditions for DiscreteIntegrator: '<S42>/Integrator' */
  DWork.Integrator_DSTATE_lc = 0.0F;

  /* End of InitializeConditions for SubSystem: '<S5>/Position_Control' */

  /* InitializeConditions for Chart: '<S49>/Wait_For_Valid_Position' */
  DWork.temporalCounter_i1 = 0U;
  DWork.is_active_c2_rtwdemo_pmsmfoc = 0U;
  DWork.is_c2_rtwdemo_pmsmfoc = IN_NO_ACTIVE_CHILD;
  DWork.Position_Valid = 0U;

  /* InitializeConditions for Chart: '<S1>/Controller_Mode_Scheduler' */
  DWork.is_Motor_On = IN_NO_ACTIVE_CHILD;
  DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD;
  DWork.velocity_command = 0.0F;
  DWork.position_command = 0.0F;
  DWork.torque_command = 0.0F;

  /* Entry: Mode_Scheduler/Controller_Mode_Scheduler */
  /* Entry Internal: Mode_Scheduler/Controller_Mode_Scheduler */
  /* Transition: '<S4>:9' */
  DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By;

  /* Entry 'Stand_By': '<S4>:154' */
  controller_mode = StandBy;

  /* InitializeConditions for DiscreteIntegrator: '<S17>/Integrator' */
  DWork.Integrator_PrevResetState = 0;

  /* InitializeConditions for DiscreteIntegrator: '<S18>/Integrator' */
  DWork.Integrator_PrevResetState_c = 0;
}

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