paul cox / Mbed 2 deprecated Nucleo_pmsmfoc

An example of importing Embedded Coder code into the mbed IDE. Currently doesn't connect IO to PWM, ADC, and Encoder, instead provides random inputs and measures execution time.

Dependencies:   mbed-dsp mbed Nucleo_pmsmfoc

Dependents:   Nucleo_pmsmfoc

rtwdemo_pmsmfoc.cpp@0:70d27fec6d71, 2014-10-11 (annotated)

Committer:
paulcox
Date:
Sat Oct 11 08:39:22 2014 +0000
Revision:
0:70d27fec6d71
Child:
2:bbc155b0b886
functioning pmsm algorithm with simulated currents and profiling

Who changed what in which revision?

UserRevisionLine numberNew contents of line
paulcox 0:70d27fec6d71 1 /*
paulcox 0:70d27fec6d71 2 * File: rtwdemo_pmsmfoc.c
paulcox 0:70d27fec6d71 3 *
paulcox 0:70d27fec6d71 4 * Code generated for Simulink model 'rtwdemo_pmsmfoc'.
paulcox 0:70d27fec6d71 5 *
paulcox 0:70d27fec6d71 6 * Model version : 1.2949
paulcox 0:70d27fec6d71 7 * Simulink Coder version : 8.7 (R2014b) 11-Aug-2014
paulcox 0:70d27fec6d71 8 * C/C++ source code generated on : Sat Oct 11 00:06:05 2014
paulcox 0:70d27fec6d71 9 *
paulcox 0:70d27fec6d71 10 * Target selection: ert.tlc
paulcox 0:70d27fec6d71 11 * Embedded hardware selection: Generic->32-bit Embedded Processor
paulcox 0:70d27fec6d71 12 * Code generation objective: Execution efficiency
paulcox 0:70d27fec6d71 13 * Validation result: Not run
paulcox 0:70d27fec6d71 14 */
paulcox 0:70d27fec6d71 15
paulcox 0:70d27fec6d71 16 #include "rtwdemo_pmsmfoc.h"
paulcox 0:70d27fec6d71 17
paulcox 0:70d27fec6d71 18 /* Named constants for Chart: '<S1>/Controller_Mode_Scheduler' */
paulcox 0:70d27fec6d71 19 #define IN_Motor_Control ((uint8_T)1U)
paulcox 0:70d27fec6d71 20 #define IN_Motor_On ((uint8_T)1U)
paulcox 0:70d27fec6d71 21 #define IN_NO_ACTIVE_CHILD ((uint8_T)0U)
paulcox 0:70d27fec6d71 22 #define IN_Position_Control ((uint8_T)1U)
paulcox 0:70d27fec6d71 23 #define IN_Ramp_To_Stop ((uint8_T)2U)
paulcox 0:70d27fec6d71 24 #define IN_Stand_By ((uint8_T)2U)
paulcox 0:70d27fec6d71 25 #define IN_Startup_Open_Loop_Control ((uint8_T)2U)
paulcox 0:70d27fec6d71 26 #define IN_Torque_Control ((uint8_T)3U)
paulcox 0:70d27fec6d71 27 #define IN_Velocity_Control ((uint8_T)4U)
paulcox 0:70d27fec6d71 28
paulcox 0:70d27fec6d71 29 /* Named constants for Chart: '<S49>/Wait_For_Valid_Position' */
paulcox 0:70d27fec6d71 30 #define IN_Position_Valid ((uint8_T)1U)
paulcox 0:70d27fec6d71 31 #define IN_Wait_For_Valid_Position ((uint8_T)2U)
paulcox 0:70d27fec6d71 32 #define IN_Wait_For_Valid_Velocity ((uint8_T)3U)
paulcox 0:70d27fec6d71 33 #define WaitForValidVelocityTicks (124)
paulcox 0:70d27fec6d71 34
paulcox 0:70d27fec6d71 35 /* Exported block signals */
paulcox 0:70d27fec6d71 36 real32_T phase_currents[2]; /* '<S48>/Product' */
paulcox 0:70d27fec6d71 37 real32_T rotor_position; /* '<S49>/Add' */
paulcox 0:70d27fec6d71 38 real32_T velocity_measured; /* '<S55>/Scale_Output' */
paulcox 0:70d27fec6d71 39 real32_T d_current_error; /* '<S12>/Sum2' */
paulcox 0:70d27fec6d71 40 real32_T q_current_command; /* '<S5>/Lo_to_Hi_Rate_Transition1' */
paulcox 0:70d27fec6d71 41 real32_T q_current_measured; /* '<S22>/Add' */
paulcox 0:70d27fec6d71 42 real32_T q_current_error; /* '<S12>/Sum' */
paulcox 0:70d27fec6d71 43 real32_T phase_voltages[3]; /* '<S27>/Select_Sector' */
paulcox 0:70d27fec6d71 44 real32_T velocity_error; /* '<S11>/Sum2' */
paulcox 0:70d27fec6d71 45 EnumControllerMode controller_mode; /* '<S1>/Controller_Mode_Scheduler' */
paulcox 0:70d27fec6d71 46
paulcox 0:70d27fec6d71 47 /* Exported block parameters */
paulcox 0:70d27fec6d71 48 CTRLPARAMS_STRUCT ctrlParams = {
paulcox 0:70d27fec6d71 49 10.0F,
paulcox 0:70d27fec6d71 50 10000.0F,
paulcox 0:70d27fec6d71 51 0.005F,
paulcox 0:70d27fec6d71 52 0.015F,
paulcox 0:70d27fec6d71 53 0.1F,
paulcox 0:70d27fec6d71 54 0.6F,
paulcox 0:70d27fec6d71 55 1.0F,
paulcox 0:70d27fec6d71 56 0.2F,
paulcox 0:70d27fec6d71 57 20.0F,
paulcox 0:70d27fec6d71 58 2252.25F,
paulcox 0:70d27fec6d71 59 0.00488400506F,
paulcox 0:70d27fec6d71 60 -0.0F,
paulcox 0:70d27fec6d71 61 4.0F
paulcox 0:70d27fec6d71 62 } ; /* Variable: ctrlParams
paulcox 0:70d27fec6d71 63 * Referenced by:
paulcox 0:70d27fec6d71 64 * '<S1>/Controller_Mode_Scheduler'
paulcox 0:70d27fec6d71 65 * '<S48>/ADC_Driver_Units_To_Amps'
paulcox 0:70d27fec6d71 66 * '<S48>/ADC_Zero_Offset'
paulcox 0:70d27fec6d71 67 * '<S49>/Offset_Between_Encoder_Zero_And_Mechanical_Zero'
paulcox 0:70d27fec6d71 68 * '<S14>/number_of_pole_pairs'
paulcox 0:70d27fec6d71 69 * '<S42>/Integral Gain'
paulcox 0:70d27fec6d71 70 * '<S42>/Proportional Gain'
paulcox 0:70d27fec6d71 71 * '<S45>/Integral Gain'
paulcox 0:70d27fec6d71 72 * '<S45>/Proportional Gain'
paulcox 0:70d27fec6d71 73 * '<S63>/Startup_Acceleration_Constant'
paulcox 0:70d27fec6d71 74 * '<S17>/Integral Gain'
paulcox 0:70d27fec6d71 75 * '<S17>/Proportional Gain'
paulcox 0:70d27fec6d71 76 * '<S18>/Integral Gain'
paulcox 0:70d27fec6d71 77 * '<S18>/Proportional Gain'
paulcox 0:70d27fec6d71 78 */
paulcox 0:70d27fec6d71 79
paulcox 0:70d27fec6d71 80 /* Constant parameters (auto storage) */
paulcox 0:70d27fec6d71 81 const ConstParam ConstP = {
paulcox 0:70d27fec6d71 82 /* Computed Parameter: Lookup_Table_table
paulcox 0:70d27fec6d71 83 * Referenced by: '<S28>/Lookup_Table'
paulcox 0:70d27fec6d71 84 */
paulcox 0:70d27fec6d71 85 { 2U, 2U, 6U, 1U, 4U, 3U, 5U }
paulcox 0:70d27fec6d71 86 };
paulcox 0:70d27fec6d71 87
paulcox 0:70d27fec6d71 88 /* Block signals and states (auto storage) */
paulcox 0:70d27fec6d71 89 D_Work DWork;
paulcox 0:70d27fec6d71 90
paulcox 0:70d27fec6d71 91 /* Real-time model */
paulcox 0:70d27fec6d71 92 RT_MODEL M_;
paulcox 0:70d27fec6d71 93 RT_MODEL *const M = &M_;
paulcox 0:70d27fec6d71 94 extern real32_T rt_roundf(real32_T u);
paulcox 0:70d27fec6d71 95 extern real32_T rt_modf(real32_T u0, real32_T u1);
paulcox 0:70d27fec6d71 96 static void rate_scheduler(void);
paulcox 0:70d27fec6d71 97
paulcox 0:70d27fec6d71 98 /*
paulcox 0:70d27fec6d71 99 * This function updates active task flag for each subrate.
paulcox 0:70d27fec6d71 100 * The function is called at model base rate, hence the
paulcox 0:70d27fec6d71 101 * generated code self-manages all its subrates.
paulcox 0:70d27fec6d71 102 */
paulcox 0:70d27fec6d71 103 static void rate_scheduler(void)
paulcox 0:70d27fec6d71 104 {
paulcox 0:70d27fec6d71 105 /* Compute which subrates run during the next base time step. Subrates
paulcox 0:70d27fec6d71 106 * are an integer multiple of the base rate counter. Therefore, the subtask
paulcox 0:70d27fec6d71 107 * counter is reset when it reaches its limit (zero means run).
paulcox 0:70d27fec6d71 108 */
paulcox 0:70d27fec6d71 109 (M->Timing.TaskCounters.TID[1])++;
paulcox 0:70d27fec6d71 110 if ((M->Timing.TaskCounters.TID[1]) > 124) {/* Sample time: [0.005s, 0.0s] */
paulcox 0:70d27fec6d71 111 M->Timing.TaskCounters.TID[1] = 0;
paulcox 0:70d27fec6d71 112 }
paulcox 0:70d27fec6d71 113 }
paulcox 0:70d27fec6d71 114
paulcox 0:70d27fec6d71 115 real32_T rt_roundf(real32_T u)
paulcox 0:70d27fec6d71 116 {
paulcox 0:70d27fec6d71 117 real32_T y;
paulcox 0:70d27fec6d71 118 if (((real32_T)fabs(u)) < 8.388608E+6F) {
paulcox 0:70d27fec6d71 119 if (u >= 0.5F) {
paulcox 0:70d27fec6d71 120 y = (real32_T)floor(u + 0.5F);
paulcox 0:70d27fec6d71 121 } else if (u > -0.5F) {
paulcox 0:70d27fec6d71 122 y = 0.0F;
paulcox 0:70d27fec6d71 123 } else {
paulcox 0:70d27fec6d71 124 y = (real32_T)ceil(u - 0.5F);
paulcox 0:70d27fec6d71 125 }
paulcox 0:70d27fec6d71 126 } else {
paulcox 0:70d27fec6d71 127 y = u;
paulcox 0:70d27fec6d71 128 }
paulcox 0:70d27fec6d71 129
paulcox 0:70d27fec6d71 130 return y;
paulcox 0:70d27fec6d71 131 }
paulcox 0:70d27fec6d71 132
paulcox 0:70d27fec6d71 133 real32_T rt_modf(real32_T u0, real32_T u1)
paulcox 0:70d27fec6d71 134 {
paulcox 0:70d27fec6d71 135 real32_T y;
paulcox 0:70d27fec6d71 136 real32_T tmp;
paulcox 0:70d27fec6d71 137 if (u1 == 0.0F) {
paulcox 0:70d27fec6d71 138 y = u0;
paulcox 0:70d27fec6d71 139 } else {
paulcox 0:70d27fec6d71 140 tmp = u0 / u1;
paulcox 0:70d27fec6d71 141 if (u1 <= ((real32_T)floor(u1))) {
paulcox 0:70d27fec6d71 142 y = u0 - (((real32_T)floor(tmp)) * u1);
paulcox 0:70d27fec6d71 143 } else if (((real32_T)fabs(tmp - rt_roundf(tmp))) <= (FLT_EPSILON *
paulcox 0:70d27fec6d71 144 ((real32_T)fabs(tmp)))) {
paulcox 0:70d27fec6d71 145 y = 0.0F;
paulcox 0:70d27fec6d71 146 } else {
paulcox 0:70d27fec6d71 147 y = (tmp - ((real32_T)floor(tmp))) * u1;
paulcox 0:70d27fec6d71 148 }
paulcox 0:70d27fec6d71 149 }
paulcox 0:70d27fec6d71 150
paulcox 0:70d27fec6d71 151 return y;
paulcox 0:70d27fec6d71 152 }
paulcox 0:70d27fec6d71 153
paulcox 0:70d27fec6d71 154 /* Model step function */
paulcox 0:70d27fec6d71 155 extern "C" EnumErrorType Controller(uint16_T motor_on, EnumCommandType command_type,
paulcox 0:70d27fec6d71 156 real32_T current_request, SENSORS_STRUCT *sensors, uint16_T pwm_compare[3])
paulcox 0:70d27fec6d71 157 {
paulcox 0:70d27fec6d71 158 real32_T sin_coefficient;
paulcox 0:70d27fec6d71 159 int32_T Wrap_To_Pi;
paulcox 0:70d27fec6d71 160 real32_T electrical_angle;
paulcox 0:70d27fec6d71 161 real32_T cos_coefficient;
paulcox 0:70d27fec6d71 162 real32_T SignDeltaU_b;
paulcox 0:70d27fec6d71 163 int16_T Enum_To_Int;
paulcox 0:70d27fec6d71 164 uint8_T FixPtRelationalOperator;
paulcox 0:70d27fec6d71 165 real32_T SignDeltaU;
paulcox 0:70d27fec6d71 166 real32_T Gain1;
paulcox 0:70d27fec6d71 167 int8_T rtPrevAction;
paulcox 0:70d27fec6d71 168 real32_T alpha_voltage;
paulcox 0:70d27fec6d71 169 real32_T SignPreIntegrator_f;
paulcox 0:70d27fec6d71 170 real32_T IntegralGain_j;
paulcox 0:70d27fec6d71 171 boolean_T RelationalOperator;
paulcox 0:70d27fec6d71 172 boolean_T NotEqual_b;
paulcox 0:70d27fec6d71 173 real32_T Switch_fr;
paulcox 0:70d27fec6d71 174 real32_T Sectors_2_and_5_idx_1;
paulcox 0:70d27fec6d71 175 real32_T Sectors_2_and_5_idx_2;
paulcox 0:70d27fec6d71 176 int16_T u0;
paulcox 0:70d27fec6d71 177
paulcox 0:70d27fec6d71 178 /* specified return value */
paulcox 0:70d27fec6d71 179 EnumErrorType error;
paulcox 0:70d27fec6d71 180
paulcox 0:70d27fec6d71 181 /* Product: '<S48>/Product' incorporates:
paulcox 0:70d27fec6d71 182 * Constant: '<S48>/ADC_Driver_Units_To_Amps'
paulcox 0:70d27fec6d71 183 * Constant: '<S48>/ADC_Zero_Offset'
paulcox 0:70d27fec6d71 184 * Inport: '<Root>/sensors'
paulcox 0:70d27fec6d71 185 * Sum: '<S48>/Add'
paulcox 0:70d27fec6d71 186 */
paulcox 0:70d27fec6d71 187 phase_currents[0] = (((real32_T)sensors->adc_phase_currents[0]) -
paulcox 0:70d27fec6d71 188 ctrlParams.AdcZeroOffsetDriverUnits) *
paulcox 0:70d27fec6d71 189 ctrlParams.AdcDriverUnitsToAmps;
paulcox 0:70d27fec6d71 190 phase_currents[1] = (((real32_T)sensors->adc_phase_currents[1]) -
paulcox 0:70d27fec6d71 191 ctrlParams.AdcZeroOffsetDriverUnits) *
paulcox 0:70d27fec6d71 192 ctrlParams.AdcDriverUnitsToAmps;
paulcox 0:70d27fec6d71 193
paulcox 0:70d27fec6d71 194 /* Chart: '<S49>/Wait_For_Valid_Position' incorporates:
paulcox 0:70d27fec6d71 195 * Inport: '<Root>/sensors'
paulcox 0:70d27fec6d71 196 */
paulcox 0:70d27fec6d71 197 /* Gateway: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
paulcox 0:70d27fec6d71 198 if (DWork.temporalCounter_i1 < 127U) {
paulcox 0:70d27fec6d71 199 DWork.temporalCounter_i1++;
paulcox 0:70d27fec6d71 200 }
paulcox 0:70d27fec6d71 201
paulcox 0:70d27fec6d71 202 /* During: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
paulcox 0:70d27fec6d71 203 if (DWork.is_active_c2_rtwdemo_pmsmfoc == 0U) {
paulcox 0:70d27fec6d71 204 /* Entry: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
paulcox 0:70d27fec6d71 205 DWork.is_active_c2_rtwdemo_pmsmfoc = 1U;
paulcox 0:70d27fec6d71 206
paulcox 0:70d27fec6d71 207 /* Entry Internal: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
paulcox 0:70d27fec6d71 208 /* Transition: '<S52>:113' */
paulcox 0:70d27fec6d71 209 if (sensors->encoder_valid == 0) {
paulcox 0:70d27fec6d71 210 /* Transition: '<S52>:114' */
paulcox 0:70d27fec6d71 211 /* Transition: '<S52>:115' */
paulcox 0:70d27fec6d71 212 DWork.is_c2_rtwdemo_pmsmfoc = IN_Wait_For_Valid_Position;
paulcox 0:70d27fec6d71 213 } else {
paulcox 0:70d27fec6d71 214 /* Transition: '<S52>:120' */
paulcox 0:70d27fec6d71 215 /* Transition: '<S52>:119' */
paulcox 0:70d27fec6d71 216 DWork.is_c2_rtwdemo_pmsmfoc = IN_Position_Valid;
paulcox 0:70d27fec6d71 217 DWork.Position_Valid = 1U;
paulcox 0:70d27fec6d71 218 }
paulcox 0:70d27fec6d71 219 } else {
paulcox 0:70d27fec6d71 220 switch (DWork.is_c2_rtwdemo_pmsmfoc) {
paulcox 0:70d27fec6d71 221 case IN_Position_Valid:
paulcox 0:70d27fec6d71 222 /* During 'Position_Valid': '<S52>:25' */
paulcox 0:70d27fec6d71 223 break;
paulcox 0:70d27fec6d71 224
paulcox 0:70d27fec6d71 225 case IN_Wait_For_Valid_Position:
paulcox 0:70d27fec6d71 226 /* During 'Wait_For_Valid_Position': '<S52>:99' */
paulcox 0:70d27fec6d71 227 if (sensors->encoder_valid != 0) {
paulcox 0:70d27fec6d71 228 /* Transition: '<S52>:100' */
paulcox 0:70d27fec6d71 229 DWork.is_c2_rtwdemo_pmsmfoc = IN_Wait_For_Valid_Velocity;
paulcox 0:70d27fec6d71 230 DWork.temporalCounter_i1 = 0U;
paulcox 0:70d27fec6d71 231 }
paulcox 0:70d27fec6d71 232 break;
paulcox 0:70d27fec6d71 233
paulcox 0:70d27fec6d71 234 default:
paulcox 0:70d27fec6d71 235 /* During 'Wait_For_Valid_Velocity': '<S52>:101' */
paulcox 0:70d27fec6d71 236 if (DWork.temporalCounter_i1 >= WaitForValidVelocityTicks) {
paulcox 0:70d27fec6d71 237 /* Transition: '<S52>:102' */
paulcox 0:70d27fec6d71 238 DWork.is_c2_rtwdemo_pmsmfoc = IN_Position_Valid;
paulcox 0:70d27fec6d71 239 DWork.Position_Valid = 1U;
paulcox 0:70d27fec6d71 240 }
paulcox 0:70d27fec6d71 241 break;
paulcox 0:70d27fec6d71 242 }
paulcox 0:70d27fec6d71 243 }
paulcox 0:70d27fec6d71 244
paulcox 0:70d27fec6d71 245 /* End of Chart: '<S49>/Wait_For_Valid_Position' */
paulcox 0:70d27fec6d71 246
paulcox 0:70d27fec6d71 247 /* Sum: '<S49>/Add' incorporates:
paulcox 0:70d27fec6d71 248 * Constant: '<S49>/Offset_Between_Encoder_Zero_And_Mechanical_Zero'
paulcox 0:70d27fec6d71 249 * Gain: '<S49>/radians_per_counts'
paulcox 0:70d27fec6d71 250 * Inport: '<Root>/sensors'
paulcox 0:70d27fec6d71 251 */
paulcox 0:70d27fec6d71 252 rotor_position = (0.000785398181F * ((real32_T)sensors->encoder_counter)) +
paulcox 0:70d27fec6d71 253 ctrlParams.EncoderToMechanicalZeroOffsetRads;
paulcox 0:70d27fec6d71 254 if (M->Timing.TaskCounters.TID[1] == 0) {
paulcox 0:70d27fec6d71 255 /* Gain: '<S55>/Wrap_To_Pi' incorporates:
paulcox 0:70d27fec6d71 256 * DataTypeConversion: '<S55>/Convert_to_Uint32'
paulcox 0:70d27fec6d71 257 * Gain: '<S55>/Scale_Input'
paulcox 0:70d27fec6d71 258 */
paulcox 0:70d27fec6d71 259 Wrap_To_Pi = (((int32_T)(1.70891312E+8F * rotor_position)) << 2);
paulcox 0:70d27fec6d71 260
paulcox 0:70d27fec6d71 261 /* Gain: '<S55>/Scale_Output' incorporates:
paulcox 0:70d27fec6d71 262 * DataTypeConversion: '<S55>/Difference_to_Single'
paulcox 0:70d27fec6d71 263 * Delay: '<S55>/Position_Delay'
paulcox 0:70d27fec6d71 264 * Sum: '<S55>/Difference_Wrap'
paulcox 0:70d27fec6d71 265 */
paulcox 0:70d27fec6d71 266 velocity_measured = ((real32_T)(Wrap_To_Pi - DWork.Position_Delay_DSTATE)) *
paulcox 0:70d27fec6d71 267 2.92583621E-7F;
paulcox 0:70d27fec6d71 268
paulcox 0:70d27fec6d71 269 /* Chart: '<S1>/Controller_Mode_Scheduler' incorporates:
paulcox 0:70d27fec6d71 270 * Inport: '<Root>/command_type'
paulcox 0:70d27fec6d71 271 * Inport: '<Root>/command_value'
paulcox 0:70d27fec6d71 272 * Inport: '<Root>/motor_on'
paulcox 0:70d27fec6d71 273 */
paulcox 0:70d27fec6d71 274 /* Gateway: Mode_Scheduler/Controller_Mode_Scheduler */
paulcox 0:70d27fec6d71 275 /* During: Mode_Scheduler/Controller_Mode_Scheduler */
paulcox 0:70d27fec6d71 276 if (DWork.is_c1_rtwdemo_pmsmfoc == IN_Motor_On) {
paulcox 0:70d27fec6d71 277 /* During 'Motor_On': '<S4>:338' */
paulcox 0:70d27fec6d71 278 if (DWork.error_l != 0) {
paulcox 0:70d27fec6d71 279 /* Transition: '<S4>:339' */
paulcox 0:70d27fec6d71 280 /* Transition: '<S4>:353' */
paulcox 0:70d27fec6d71 281 /* Exit Internal 'Motor_On': '<S4>:338' */
paulcox 0:70d27fec6d71 282 /* Exit Internal 'Motor_Control': '<S4>:344' */
paulcox 0:70d27fec6d71 283 DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD;
paulcox 0:70d27fec6d71 284 DWork.is_Motor_On = IN_NO_ACTIVE_CHILD;
paulcox 0:70d27fec6d71 285 DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By;
paulcox 0:70d27fec6d71 286
paulcox 0:70d27fec6d71 287 /* Entry 'Stand_By': '<S4>:154' */
paulcox 0:70d27fec6d71 288 controller_mode = StandBy;
paulcox 0:70d27fec6d71 289 } else if (DWork.is_Motor_On == IN_Motor_Control) {
paulcox 0:70d27fec6d71 290 /* During 'Motor_Control': '<S4>:344' */
paulcox 0:70d27fec6d71 291 if (!(motor_on != 0)) {
paulcox 0:70d27fec6d71 292 /* Transition: '<S4>:282' */
paulcox 0:70d27fec6d71 293 /* Exit Internal 'Motor_Control': '<S4>:344' */
paulcox 0:70d27fec6d71 294 DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD;
paulcox 0:70d27fec6d71 295 DWork.is_Motor_On = IN_Ramp_To_Stop;
paulcox 0:70d27fec6d71 296
paulcox 0:70d27fec6d71 297 /* Entry 'Ramp_To_Stop': '<S4>:270' */
paulcox 0:70d27fec6d71 298 controller_mode = VelocityControl;
paulcox 0:70d27fec6d71 299 DWork.velocity_command = 0.0F;
paulcox 0:70d27fec6d71 300 DWork.torque_command = 0.0F;
paulcox 0:70d27fec6d71 301 } else {
paulcox 0:70d27fec6d71 302 switch (DWork.is_Motor_Control) {
paulcox 0:70d27fec6d71 303 case IN_Position_Control:
paulcox 0:70d27fec6d71 304 /* During 'Position_Control': '<S4>:226' */
paulcox 0:70d27fec6d71 305 DWork.position_command = current_request;
paulcox 0:70d27fec6d71 306 break;
paulcox 0:70d27fec6d71 307
paulcox 0:70d27fec6d71 308 case IN_Startup_Open_Loop_Control:
paulcox 0:70d27fec6d71 309 /* During 'Startup_Open_Loop_Control': '<S4>:103' */
paulcox 0:70d27fec6d71 310 if (DWork.Position_Valid != 0) {
paulcox 0:70d27fec6d71 311 /* Transition: '<S4>:157' */
paulcox 0:70d27fec6d71 312 /* Transition: '<S4>:233' */
paulcox 0:70d27fec6d71 313 switch (command_type) {
paulcox 0:70d27fec6d71 314 case Velocity:
paulcox 0:70d27fec6d71 315 /* Transition: '<S4>:162' */
paulcox 0:70d27fec6d71 316 DWork.is_Motor_Control = IN_Velocity_Control;
paulcox 0:70d27fec6d71 317
paulcox 0:70d27fec6d71 318 /* Entry 'Velocity_Control': '<S4>:108' */
paulcox 0:70d27fec6d71 319 controller_mode = VelocityControl;
paulcox 0:70d27fec6d71 320 DWork.velocity_command = current_request;
paulcox 0:70d27fec6d71 321 break;
paulcox 0:70d27fec6d71 322
paulcox 0:70d27fec6d71 323 case Position:
paulcox 0:70d27fec6d71 324 /* Transition: '<S4>:235' */
paulcox 0:70d27fec6d71 325 /* Transition: '<S4>:234' */
paulcox 0:70d27fec6d71 326 DWork.is_Motor_Control = IN_Position_Control;
paulcox 0:70d27fec6d71 327
paulcox 0:70d27fec6d71 328 /* Entry 'Position_Control': '<S4>:226' */
paulcox 0:70d27fec6d71 329 controller_mode = PositionControl;
paulcox 0:70d27fec6d71 330 DWork.position_command = current_request;
paulcox 0:70d27fec6d71 331 break;
paulcox 0:70d27fec6d71 332
paulcox 0:70d27fec6d71 333 default:
paulcox 0:70d27fec6d71 334 /* Transition: '<S4>:237' */
paulcox 0:70d27fec6d71 335 /* Transition: '<S4>:158' */
paulcox 0:70d27fec6d71 336 /* [command_type==Torque] */
paulcox 0:70d27fec6d71 337 DWork.is_Motor_Control = IN_Torque_Control;
paulcox 0:70d27fec6d71 338
paulcox 0:70d27fec6d71 339 /* Entry 'Torque_Control': '<S4>:220' */
paulcox 0:70d27fec6d71 340 controller_mode = TorqueControl;
paulcox 0:70d27fec6d71 341 DWork.torque_command = current_request;
paulcox 0:70d27fec6d71 342 break;
paulcox 0:70d27fec6d71 343 }
paulcox 0:70d27fec6d71 344 }
paulcox 0:70d27fec6d71 345 break;
paulcox 0:70d27fec6d71 346
paulcox 0:70d27fec6d71 347 case IN_Torque_Control:
paulcox 0:70d27fec6d71 348 /* During 'Torque_Control': '<S4>:220' */
paulcox 0:70d27fec6d71 349 DWork.torque_command = current_request;
paulcox 0:70d27fec6d71 350 break;
paulcox 0:70d27fec6d71 351
paulcox 0:70d27fec6d71 352 default:
paulcox 0:70d27fec6d71 353 /* During 'Velocity_Control': '<S4>:108' */
paulcox 0:70d27fec6d71 354 DWork.velocity_command = current_request;
paulcox 0:70d27fec6d71 355 break;
paulcox 0:70d27fec6d71 356 }
paulcox 0:70d27fec6d71 357 }
paulcox 0:70d27fec6d71 358 } else {
paulcox 0:70d27fec6d71 359 /* During 'Ramp_To_Stop': '<S4>:270' */
paulcox 0:70d27fec6d71 360 if (((real32_T)fabs(velocity_measured)) < ctrlParams.RampToStopVelocity)
paulcox 0:70d27fec6d71 361 {
paulcox 0:70d27fec6d71 362 /* Transition: '<S4>:169' */
paulcox 0:70d27fec6d71 363 /* Transition: '<S4>:355' */
paulcox 0:70d27fec6d71 364 DWork.is_Motor_On = IN_NO_ACTIVE_CHILD;
paulcox 0:70d27fec6d71 365 DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By;
paulcox 0:70d27fec6d71 366
paulcox 0:70d27fec6d71 367 /* Entry 'Stand_By': '<S4>:154' */
paulcox 0:70d27fec6d71 368 controller_mode = StandBy;
paulcox 0:70d27fec6d71 369 }
paulcox 0:70d27fec6d71 370 }
paulcox 0:70d27fec6d71 371 } else {
paulcox 0:70d27fec6d71 372 /* During 'Stand_By': '<S4>:154' */
paulcox 0:70d27fec6d71 373 if ((motor_on != 0) && (!(DWork.error_l != 0))) {
paulcox 0:70d27fec6d71 374 /* Transition: '<S4>:164' */
paulcox 0:70d27fec6d71 375 if (!(DWork.Position_Valid != 0)) {
paulcox 0:70d27fec6d71 376 /* Transition: '<S4>:133' */
paulcox 0:70d27fec6d71 377 DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
paulcox 0:70d27fec6d71 378 DWork.is_Motor_On = IN_Motor_Control;
paulcox 0:70d27fec6d71 379 DWork.is_Motor_Control = IN_Startup_Open_Loop_Control;
paulcox 0:70d27fec6d71 380
paulcox 0:70d27fec6d71 381 /* Entry 'Startup_Open_Loop_Control': '<S4>:103' */
paulcox 0:70d27fec6d71 382 controller_mode = Startup;
paulcox 0:70d27fec6d71 383 DWork.torque_command = ctrlParams.StartupCurrent;
paulcox 0:70d27fec6d71 384 } else {
paulcox 0:70d27fec6d71 385 /* Transition: '<S4>:137' */
paulcox 0:70d27fec6d71 386 /* Transition: '<S4>:233' */
paulcox 0:70d27fec6d71 387 switch (command_type) {
paulcox 0:70d27fec6d71 388 case Velocity:
paulcox 0:70d27fec6d71 389 /* Transition: '<S4>:162' */
paulcox 0:70d27fec6d71 390 DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
paulcox 0:70d27fec6d71 391 DWork.is_Motor_On = IN_Motor_Control;
paulcox 0:70d27fec6d71 392 DWork.is_Motor_Control = IN_Velocity_Control;
paulcox 0:70d27fec6d71 393
paulcox 0:70d27fec6d71 394 /* Entry 'Velocity_Control': '<S4>:108' */
paulcox 0:70d27fec6d71 395 controller_mode = VelocityControl;
paulcox 0:70d27fec6d71 396 DWork.velocity_command = current_request;
paulcox 0:70d27fec6d71 397 break;
paulcox 0:70d27fec6d71 398
paulcox 0:70d27fec6d71 399 case Position:
paulcox 0:70d27fec6d71 400 /* Transition: '<S4>:235' */
paulcox 0:70d27fec6d71 401 /* Transition: '<S4>:234' */
paulcox 0:70d27fec6d71 402 DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
paulcox 0:70d27fec6d71 403 DWork.is_Motor_On = IN_Motor_Control;
paulcox 0:70d27fec6d71 404 DWork.is_Motor_Control = IN_Position_Control;
paulcox 0:70d27fec6d71 405
paulcox 0:70d27fec6d71 406 /* Entry 'Position_Control': '<S4>:226' */
paulcox 0:70d27fec6d71 407 controller_mode = PositionControl;
paulcox 0:70d27fec6d71 408 DWork.position_command = current_request;
paulcox 0:70d27fec6d71 409 break;
paulcox 0:70d27fec6d71 410
paulcox 0:70d27fec6d71 411 default:
paulcox 0:70d27fec6d71 412 /* Transition: '<S4>:237' */
paulcox 0:70d27fec6d71 413 /* Transition: '<S4>:158' */
paulcox 0:70d27fec6d71 414 /* [command_type==Torque] */
paulcox 0:70d27fec6d71 415 DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
paulcox 0:70d27fec6d71 416 DWork.is_Motor_On = IN_Motor_Control;
paulcox 0:70d27fec6d71 417 DWork.is_Motor_Control = IN_Torque_Control;
paulcox 0:70d27fec6d71 418
paulcox 0:70d27fec6d71 419 /* Entry 'Torque_Control': '<S4>:220' */
paulcox 0:70d27fec6d71 420 controller_mode = TorqueControl;
paulcox 0:70d27fec6d71 421 DWork.torque_command = current_request;
paulcox 0:70d27fec6d71 422 break;
paulcox 0:70d27fec6d71 423 }
paulcox 0:70d27fec6d71 424 }
paulcox 0:70d27fec6d71 425 }
paulcox 0:70d27fec6d71 426 }
paulcox 0:70d27fec6d71 427
paulcox 0:70d27fec6d71 428 /* End of Chart: '<S1>/Controller_Mode_Scheduler' */
paulcox 0:70d27fec6d71 429
paulcox 0:70d27fec6d71 430 /* RelationalOperator: '<S51>/Relational Operator' incorporates:
paulcox 0:70d27fec6d71 431 * Constant: '<S62>/Constant'
paulcox 0:70d27fec6d71 432 */
paulcox 0:70d27fec6d71 433 DWork.RelationalOperator_a = (controller_mode == Startup);
paulcox 0:70d27fec6d71 434 }
paulcox 0:70d27fec6d71 435
paulcox 0:70d27fec6d71 436 /* Outputs for Enabled SubSystem: '<S51>/Open Loop Position' incorporates:
paulcox 0:70d27fec6d71 437 * EnablePort: '<S63>/Enable'
paulcox 0:70d27fec6d71 438 */
paulcox 0:70d27fec6d71 439 if (DWork.RelationalOperator_a) {
paulcox 0:70d27fec6d71 440 if (!DWork.OpenLoopPosition_MODE) {
paulcox 0:70d27fec6d71 441 /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Position' */
paulcox 0:70d27fec6d71 442 DWork.Integrate_To_Position_DSTATE = 0.0F;
paulcox 0:70d27fec6d71 443
paulcox 0:70d27fec6d71 444 /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' */
paulcox 0:70d27fec6d71 445 DWork.Integrate_To_Velocity_DSTATE = 0.0F;
paulcox 0:70d27fec6d71 446 DWork.OpenLoopPosition_MODE = true;
paulcox 0:70d27fec6d71 447 }
paulcox 0:70d27fec6d71 448
paulcox 0:70d27fec6d71 449 /* DiscreteIntegrator: '<S63>/Integrate_To_Position' */
paulcox 0:70d27fec6d71 450 DWork.position = DWork.Integrate_To_Position_DSTATE;
paulcox 0:70d27fec6d71 451
paulcox 0:70d27fec6d71 452 /* Update for DiscreteIntegrator: '<S63>/Integrate_To_Position' incorporates:
paulcox 0:70d27fec6d71 453 * DiscreteIntegrator: '<S63>/Integrate_To_Velocity'
paulcox 0:70d27fec6d71 454 */
paulcox 0:70d27fec6d71 455 DWork.Integrate_To_Position_DSTATE += 4.0E-5F *
paulcox 0:70d27fec6d71 456 DWork.Integrate_To_Velocity_DSTATE;
paulcox 0:70d27fec6d71 457
paulcox 0:70d27fec6d71 458 /* Update for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' incorporates:
paulcox 0:70d27fec6d71 459 * Constant: '<S63>/Startup_Acceleration_Constant'
paulcox 0:70d27fec6d71 460 */
paulcox 0:70d27fec6d71 461 DWork.Integrate_To_Velocity_DSTATE += 4.0E-5F *
paulcox 0:70d27fec6d71 462 ctrlParams.StartupAcceleration;
paulcox 0:70d27fec6d71 463 } else {
paulcox 0:70d27fec6d71 464 if (DWork.OpenLoopPosition_MODE) {
paulcox 0:70d27fec6d71 465 DWork.OpenLoopPosition_MODE = false;
paulcox 0:70d27fec6d71 466 }
paulcox 0:70d27fec6d71 467 }
paulcox 0:70d27fec6d71 468
paulcox 0:70d27fec6d71 469 /* End of Outputs for SubSystem: '<S51>/Open Loop Position' */
paulcox 0:70d27fec6d71 470
paulcox 0:70d27fec6d71 471 /* Switch: '<S51>/Switch' */
paulcox 0:70d27fec6d71 472 if (DWork.RelationalOperator_a) {
paulcox 0:70d27fec6d71 473 Switch_fr = DWork.position;
paulcox 0:70d27fec6d71 474 } else {
paulcox 0:70d27fec6d71 475 Switch_fr = rotor_position;
paulcox 0:70d27fec6d71 476 }
paulcox 0:70d27fec6d71 477
paulcox 0:70d27fec6d71 478 /* End of Switch: '<S51>/Switch' */
paulcox 0:70d27fec6d71 479
paulcox 0:70d27fec6d71 480 /* Gain: '<S14>/number_of_pole_pairs' */
paulcox 0:70d27fec6d71 481 electrical_angle = ctrlParams.PmsmPolePairs * Switch_fr;
paulcox 0:70d27fec6d71 482
paulcox 0:70d27fec6d71 483 /* Trigonometry: '<S14>/sine_cosine' */
paulcox 0:70d27fec6d71 484 sin_coefficient = (real32_T)sin(electrical_angle);
paulcox 0:70d27fec6d71 485 cos_coefficient = (real32_T)cos(electrical_angle);
paulcox 0:70d27fec6d71 486
paulcox 0:70d27fec6d71 487 /* Gain: '<S21>/Beta_Gain' incorporates:
paulcox 0:70d27fec6d71 488 * Gain: '<S21>/B_Gain'
paulcox 0:70d27fec6d71 489 * Sum: '<S21>/Add'
paulcox 0:70d27fec6d71 490 */
paulcox 0:70d27fec6d71 491 SignDeltaU_b = ((2.0F * phase_currents[1]) + phase_currents[0]) * 0.577350259F;
paulcox 0:70d27fec6d71 492
paulcox 0:70d27fec6d71 493 /* Sum: '<S12>/Sum2' incorporates:
paulcox 0:70d27fec6d71 494 * Constant: '<S12>/d_current_command (A)'
paulcox 0:70d27fec6d71 495 * Product: '<S22>/Product2'
paulcox 0:70d27fec6d71 496 * Product: '<S22>/Product3'
paulcox 0:70d27fec6d71 497 * Sum: '<S22>/Add1'
paulcox 0:70d27fec6d71 498 */
paulcox 0:70d27fec6d71 499 d_current_error = 0.0F - ((phase_currents[0] * cos_coefficient) +
paulcox 0:70d27fec6d71 500 (SignDeltaU_b * sin_coefficient));
paulcox 0:70d27fec6d71 501
paulcox 0:70d27fec6d71 502 /* DataTypeConversion: '<S8>/Enum_To_Int' */
paulcox 0:70d27fec6d71 503 Enum_To_Int = (int16_T)controller_mode;
paulcox 0:70d27fec6d71 504
paulcox 0:70d27fec6d71 505 /* RelationalOperator: '<S13>/FixPt Relational Operator' incorporates:
paulcox 0:70d27fec6d71 506 * UnitDelay: '<S13>/Delay Input1'
paulcox 0:70d27fec6d71 507 */
paulcox 0:70d27fec6d71 508 FixPtRelationalOperator = (uint8_T)(Enum_To_Int != DWork.DelayInput1_DSTATE);
paulcox 0:70d27fec6d71 509
paulcox 0:70d27fec6d71 510 /* DiscreteIntegrator: '<S17>/Integrator' */
paulcox 0:70d27fec6d71 511 if ((FixPtRelationalOperator != 0) || (DWork.Integrator_PrevResetState != 0))
paulcox 0:70d27fec6d71 512 {
paulcox 0:70d27fec6d71 513 DWork.Integrator_DSTATE = 0.0F;
paulcox 0:70d27fec6d71 514 }
paulcox 0:70d27fec6d71 515
paulcox 0:70d27fec6d71 516 /* Sum: '<S17>/Sum' incorporates:
paulcox 0:70d27fec6d71 517 * DiscreteIntegrator: '<S17>/Integrator'
paulcox 0:70d27fec6d71 518 * Gain: '<S17>/Proportional Gain'
paulcox 0:70d27fec6d71 519 */
paulcox 0:70d27fec6d71 520 SignDeltaU = (ctrlParams.Current_P * d_current_error) +
paulcox 0:70d27fec6d71 521 DWork.Integrator_DSTATE;
paulcox 0:70d27fec6d71 522
paulcox 0:70d27fec6d71 523 /* Saturate: '<S17>/Saturate' */
paulcox 0:70d27fec6d71 524 if (SignDeltaU > 12.0F) {
paulcox 0:70d27fec6d71 525 Gain1 = 12.0F;
paulcox 0:70d27fec6d71 526 } else if (SignDeltaU < -12.0F) {
paulcox 0:70d27fec6d71 527 Gain1 = -12.0F;
paulcox 0:70d27fec6d71 528 } else {
paulcox 0:70d27fec6d71 529 Gain1 = SignDeltaU;
paulcox 0:70d27fec6d71 530 }
paulcox 0:70d27fec6d71 531
paulcox 0:70d27fec6d71 532 /* End of Saturate: '<S17>/Saturate' */
paulcox 0:70d27fec6d71 533 if (M->Timing.TaskCounters.TID[1] == 0) {
paulcox 0:70d27fec6d71 534 /* SwitchCase: '<S5>/Switch Case' incorporates:
paulcox 0:70d27fec6d71 535 * Inport: '<S10>/torque_command'
paulcox 0:70d27fec6d71 536 */
paulcox 0:70d27fec6d71 537 rtPrevAction = DWork.SwitchCase_ActiveSubsystem;
paulcox 0:70d27fec6d71 538 switch (controller_mode) {
paulcox 0:70d27fec6d71 539 case VelocityControl:
paulcox 0:70d27fec6d71 540 DWork.SwitchCase_ActiveSubsystem = 0;
paulcox 0:70d27fec6d71 541 break;
paulcox 0:70d27fec6d71 542
paulcox 0:70d27fec6d71 543 case PositionControl:
paulcox 0:70d27fec6d71 544 DWork.SwitchCase_ActiveSubsystem = 1;
paulcox 0:70d27fec6d71 545 break;
paulcox 0:70d27fec6d71 546
paulcox 0:70d27fec6d71 547 default:
paulcox 0:70d27fec6d71 548 DWork.SwitchCase_ActiveSubsystem = 2;
paulcox 0:70d27fec6d71 549 break;
paulcox 0:70d27fec6d71 550 }
paulcox 0:70d27fec6d71 551
paulcox 0:70d27fec6d71 552 switch (DWork.SwitchCase_ActiveSubsystem) {
paulcox 0:70d27fec6d71 553 case 0:
paulcox 0:70d27fec6d71 554 if (DWork.SwitchCase_ActiveSubsystem != rtPrevAction) {
paulcox 0:70d27fec6d71 555 /* InitializeConditions for IfAction SubSystem: '<S5>/Velocity_Control' incorporates:
paulcox 0:70d27fec6d71 556 * InitializeConditions for ActionPort: '<S11>/Action Port'
paulcox 0:70d27fec6d71 557 */
paulcox 0:70d27fec6d71 558 /* InitializeConditions for SwitchCase: '<S5>/Switch Case' incorporates:
paulcox 0:70d27fec6d71 559 * InitializeConditions for DiscreteIntegrator: '<S45>/Integrator'
paulcox 0:70d27fec6d71 560 */
paulcox 0:70d27fec6d71 561 DWork.Integrator_DSTATE_f = 0.0F;
paulcox 0:70d27fec6d71 562
paulcox 0:70d27fec6d71 563 /* End of InitializeConditions for SubSystem: '<S5>/Velocity_Control' */
paulcox 0:70d27fec6d71 564 }
paulcox 0:70d27fec6d71 565
paulcox 0:70d27fec6d71 566 /* Outputs for IfAction SubSystem: '<S5>/Velocity_Control' incorporates:
paulcox 0:70d27fec6d71 567 * ActionPort: '<S11>/Action Port'
paulcox 0:70d27fec6d71 568 */
paulcox 0:70d27fec6d71 569 /* Sum: '<S11>/Sum2' */
paulcox 0:70d27fec6d71 570 velocity_error = DWork.velocity_command - velocity_measured;
paulcox 0:70d27fec6d71 571
paulcox 0:70d27fec6d71 572 /* Sum: '<S45>/Sum' incorporates:
paulcox 0:70d27fec6d71 573 * DiscreteIntegrator: '<S45>/Integrator'
paulcox 0:70d27fec6d71 574 * Gain: '<S45>/Proportional Gain'
paulcox 0:70d27fec6d71 575 */
paulcox 0:70d27fec6d71 576 electrical_angle = (ctrlParams.Velocity_P * velocity_error) +
paulcox 0:70d27fec6d71 577 DWork.Integrator_DSTATE_f;
paulcox 0:70d27fec6d71 578
paulcox 0:70d27fec6d71 579 /* Saturate: '<S45>/Saturate' */
paulcox 0:70d27fec6d71 580 if (electrical_angle > 2.0F) {
paulcox 0:70d27fec6d71 581 /* SignalConversion: '<S11>/Isolate_For_Merge' */
paulcox 0:70d27fec6d71 582 DWork.Merge = 2.0F;
paulcox 0:70d27fec6d71 583 } else if (electrical_angle < -2.0F) {
paulcox 0:70d27fec6d71 584 /* SignalConversion: '<S11>/Isolate_For_Merge' */
paulcox 0:70d27fec6d71 585 DWork.Merge = -2.0F;
paulcox 0:70d27fec6d71 586 } else {
paulcox 0:70d27fec6d71 587 /* SignalConversion: '<S11>/Isolate_For_Merge' */
paulcox 0:70d27fec6d71 588 DWork.Merge = electrical_angle;
paulcox 0:70d27fec6d71 589 }
paulcox 0:70d27fec6d71 590
paulcox 0:70d27fec6d71 591 /* End of Saturate: '<S45>/Saturate' */
paulcox 0:70d27fec6d71 592
paulcox 0:70d27fec6d71 593 /* DeadZone: '<S46>/DeadZone' */
paulcox 0:70d27fec6d71 594 if (electrical_angle > 2.0F) {
paulcox 0:70d27fec6d71 595 electrical_angle -= 2.0F;
paulcox 0:70d27fec6d71 596 } else if (electrical_angle >= -2.0F) {
paulcox 0:70d27fec6d71 597 electrical_angle = 0.0F;
paulcox 0:70d27fec6d71 598 } else {
paulcox 0:70d27fec6d71 599 electrical_angle -= -2.0F;
paulcox 0:70d27fec6d71 600 }
paulcox 0:70d27fec6d71 601
paulcox 0:70d27fec6d71 602 /* End of DeadZone: '<S46>/DeadZone' */
paulcox 0:70d27fec6d71 603
paulcox 0:70d27fec6d71 604 /* RelationalOperator: '<S46>/NotEqual' */
paulcox 0:70d27fec6d71 605 NotEqual_b = (0.0F != electrical_angle);
paulcox 0:70d27fec6d71 606
paulcox 0:70d27fec6d71 607 /* Signum: '<S46>/SignDeltaU' */
paulcox 0:70d27fec6d71 608 if (electrical_angle < 0.0F) {
paulcox 0:70d27fec6d71 609 electrical_angle = -1.0F;
paulcox 0:70d27fec6d71 610 } else {
paulcox 0:70d27fec6d71 611 if (electrical_angle > 0.0F) {
paulcox 0:70d27fec6d71 612 electrical_angle = 1.0F;
paulcox 0:70d27fec6d71 613 }
paulcox 0:70d27fec6d71 614 }
paulcox 0:70d27fec6d71 615
paulcox 0:70d27fec6d71 616 /* End of Signum: '<S46>/SignDeltaU' */
paulcox 0:70d27fec6d71 617
paulcox 0:70d27fec6d71 618 /* Gain: '<S45>/Integral Gain' */
paulcox 0:70d27fec6d71 619 Switch_fr = ctrlParams.Velocity_I * velocity_error;
paulcox 0:70d27fec6d71 620
paulcox 0:70d27fec6d71 621 /* DataTypeConversion: '<S46>/DataTypeConv1' */
paulcox 0:70d27fec6d71 622 if (electrical_angle < 128.0F) {
paulcox 0:70d27fec6d71 623 rtPrevAction = (int8_T)electrical_angle;
paulcox 0:70d27fec6d71 624 } else {
paulcox 0:70d27fec6d71 625 rtPrevAction = MAX_int8_T;
paulcox 0:70d27fec6d71 626 }
paulcox 0:70d27fec6d71 627
paulcox 0:70d27fec6d71 628 /* End of DataTypeConversion: '<S46>/DataTypeConv1' */
paulcox 0:70d27fec6d71 629
paulcox 0:70d27fec6d71 630 /* Signum: '<S46>/SignPreIntegrator' */
paulcox 0:70d27fec6d71 631 if (Switch_fr < 0.0F) {
paulcox 0:70d27fec6d71 632 electrical_angle = -1.0F;
paulcox 0:70d27fec6d71 633 } else if (Switch_fr > 0.0F) {
paulcox 0:70d27fec6d71 634 electrical_angle = 1.0F;
paulcox 0:70d27fec6d71 635 } else {
paulcox 0:70d27fec6d71 636 electrical_angle = Switch_fr;
paulcox 0:70d27fec6d71 637 }
paulcox 0:70d27fec6d71 638
paulcox 0:70d27fec6d71 639 /* Switch: '<S45>/Switch' incorporates:
paulcox 0:70d27fec6d71 640 * Constant: '<S45>/Constant'
paulcox 0:70d27fec6d71 641 * DataTypeConversion: '<S46>/DataTypeConv2'
paulcox 0:70d27fec6d71 642 * Logic: '<S46>/AND'
paulcox 0:70d27fec6d71 643 * RelationalOperator: '<S46>/Equal'
paulcox 0:70d27fec6d71 644 * Signum: '<S46>/SignPreIntegrator'
paulcox 0:70d27fec6d71 645 */
paulcox 0:70d27fec6d71 646 if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
paulcox 0:70d27fec6d71 647 Switch_fr = 0.0F;
paulcox 0:70d27fec6d71 648 }
paulcox 0:70d27fec6d71 649
paulcox 0:70d27fec6d71 650 /* End of Switch: '<S45>/Switch' */
paulcox 0:70d27fec6d71 651
paulcox 0:70d27fec6d71 652 /* Update for DiscreteIntegrator: '<S45>/Integrator' */
paulcox 0:70d27fec6d71 653 DWork.Integrator_DSTATE_f += 0.005F * Switch_fr;
paulcox 0:70d27fec6d71 654
paulcox 0:70d27fec6d71 655 /* End of Outputs for SubSystem: '<S5>/Velocity_Control' */
paulcox 0:70d27fec6d71 656 break;
paulcox 0:70d27fec6d71 657
paulcox 0:70d27fec6d71 658 case 1:
paulcox 0:70d27fec6d71 659 if (DWork.SwitchCase_ActiveSubsystem != rtPrevAction) {
paulcox 0:70d27fec6d71 660 /* InitializeConditions for IfAction SubSystem: '<S5>/Position_Control' incorporates:
paulcox 0:70d27fec6d71 661 * InitializeConditions for ActionPort: '<S9>/Action Port'
paulcox 0:70d27fec6d71 662 */
paulcox 0:70d27fec6d71 663 /* InitializeConditions for SwitchCase: '<S5>/Switch Case' incorporates:
paulcox 0:70d27fec6d71 664 * InitializeConditions for DiscreteIntegrator: '<S42>/Integrator'
paulcox 0:70d27fec6d71 665 */
paulcox 0:70d27fec6d71 666 DWork.Integrator_DSTATE_lc = 0.0F;
paulcox 0:70d27fec6d71 667
paulcox 0:70d27fec6d71 668 /* End of InitializeConditions for SubSystem: '<S5>/Position_Control' */
paulcox 0:70d27fec6d71 669 }
paulcox 0:70d27fec6d71 670
paulcox 0:70d27fec6d71 671 /* Outputs for IfAction SubSystem: '<S5>/Position_Control' incorporates:
paulcox 0:70d27fec6d71 672 * ActionPort: '<S9>/Action Port'
paulcox 0:70d27fec6d71 673 */
paulcox 0:70d27fec6d71 674 /* Sum: '<S9>/Sum2' */
paulcox 0:70d27fec6d71 675 Switch_fr = DWork.position_command - Switch_fr;
paulcox 0:70d27fec6d71 676
paulcox 0:70d27fec6d71 677 /* Switch: '<S43>/Select_Angle' incorporates:
paulcox 0:70d27fec6d71 678 * Constant: '<S43>/Neg_Pi_Constant'
paulcox 0:70d27fec6d71 679 * Constant: '<S43>/Pi_Constant_1'
paulcox 0:70d27fec6d71 680 * Constant: '<S43>/Pi_Constant_2'
paulcox 0:70d27fec6d71 681 * Constant: '<S43>/Pi_Constant_3'
paulcox 0:70d27fec6d71 682 * Constant: '<S43>/Two_Pi_Constant'
paulcox 0:70d27fec6d71 683 * Logic: '<S43>/OR'
paulcox 0:70d27fec6d71 684 * Math: '<S43>/Modulus'
paulcox 0:70d27fec6d71 685 * RelationalOperator: '<S43>/Greater_Than'
paulcox 0:70d27fec6d71 686 * RelationalOperator: '<S43>/Less_Than'
paulcox 0:70d27fec6d71 687 * Sum: '<S43>/Add'
paulcox 0:70d27fec6d71 688 * Sum: '<S43>/Subtract'
paulcox 0:70d27fec6d71 689 */
paulcox 0:70d27fec6d71 690 if ((Switch_fr < -1.57079637F) || (Switch_fr >= 1.57079637F)) {
paulcox 0:70d27fec6d71 691 Switch_fr = rt_modf(Switch_fr + 1.57079637F, 3.14159274F) - 1.57079637F;
paulcox 0:70d27fec6d71 692 }
paulcox 0:70d27fec6d71 693
paulcox 0:70d27fec6d71 694 /* End of Switch: '<S43>/Select_Angle' */
paulcox 0:70d27fec6d71 695
paulcox 0:70d27fec6d71 696 /* Sum: '<S42>/Sum' incorporates:
paulcox 0:70d27fec6d71 697 * DiscreteIntegrator: '<S42>/Integrator'
paulcox 0:70d27fec6d71 698 * Gain: '<S42>/Proportional Gain'
paulcox 0:70d27fec6d71 699 */
paulcox 0:70d27fec6d71 700 electrical_angle = (ctrlParams.Position_P * Switch_fr) +
paulcox 0:70d27fec6d71 701 DWork.Integrator_DSTATE_lc;
paulcox 0:70d27fec6d71 702
paulcox 0:70d27fec6d71 703 /* Saturate: '<S42>/Saturate' */
paulcox 0:70d27fec6d71 704 if (electrical_angle > 2.0F) {
paulcox 0:70d27fec6d71 705 /* SignalConversion: '<S9>/Isolate_For_Merge' */
paulcox 0:70d27fec6d71 706 DWork.Merge = 2.0F;
paulcox 0:70d27fec6d71 707 } else if (electrical_angle < -2.0F) {
paulcox 0:70d27fec6d71 708 /* SignalConversion: '<S9>/Isolate_For_Merge' */
paulcox 0:70d27fec6d71 709 DWork.Merge = -2.0F;
paulcox 0:70d27fec6d71 710 } else {
paulcox 0:70d27fec6d71 711 /* SignalConversion: '<S9>/Isolate_For_Merge' */
paulcox 0:70d27fec6d71 712 DWork.Merge = electrical_angle;
paulcox 0:70d27fec6d71 713 }
paulcox 0:70d27fec6d71 714
paulcox 0:70d27fec6d71 715 /* End of Saturate: '<S42>/Saturate' */
paulcox 0:70d27fec6d71 716
paulcox 0:70d27fec6d71 717 /* DeadZone: '<S44>/DeadZone' */
paulcox 0:70d27fec6d71 718 if (electrical_angle > 2.0F) {
paulcox 0:70d27fec6d71 719 electrical_angle -= 2.0F;
paulcox 0:70d27fec6d71 720 } else if (electrical_angle >= -2.0F) {
paulcox 0:70d27fec6d71 721 electrical_angle = 0.0F;
paulcox 0:70d27fec6d71 722 } else {
paulcox 0:70d27fec6d71 723 electrical_angle -= -2.0F;
paulcox 0:70d27fec6d71 724 }
paulcox 0:70d27fec6d71 725
paulcox 0:70d27fec6d71 726 /* End of DeadZone: '<S44>/DeadZone' */
paulcox 0:70d27fec6d71 727
paulcox 0:70d27fec6d71 728 /* RelationalOperator: '<S44>/NotEqual' */
paulcox 0:70d27fec6d71 729 NotEqual_b = (0.0F != electrical_angle);
paulcox 0:70d27fec6d71 730
paulcox 0:70d27fec6d71 731 /* Signum: '<S44>/SignDeltaU' */
paulcox 0:70d27fec6d71 732 if (electrical_angle < 0.0F) {
paulcox 0:70d27fec6d71 733 electrical_angle = -1.0F;
paulcox 0:70d27fec6d71 734 } else {
paulcox 0:70d27fec6d71 735 if (electrical_angle > 0.0F) {
paulcox 0:70d27fec6d71 736 electrical_angle = 1.0F;
paulcox 0:70d27fec6d71 737 }
paulcox 0:70d27fec6d71 738 }
paulcox 0:70d27fec6d71 739
paulcox 0:70d27fec6d71 740 /* End of Signum: '<S44>/SignDeltaU' */
paulcox 0:70d27fec6d71 741
paulcox 0:70d27fec6d71 742 /* Gain: '<S42>/Integral Gain' */
paulcox 0:70d27fec6d71 743 Switch_fr *= ctrlParams.Position_I;
paulcox 0:70d27fec6d71 744
paulcox 0:70d27fec6d71 745 /* DataTypeConversion: '<S44>/DataTypeConv1' */
paulcox 0:70d27fec6d71 746 if (electrical_angle < 128.0F) {
paulcox 0:70d27fec6d71 747 rtPrevAction = (int8_T)electrical_angle;
paulcox 0:70d27fec6d71 748 } else {
paulcox 0:70d27fec6d71 749 rtPrevAction = MAX_int8_T;
paulcox 0:70d27fec6d71 750 }
paulcox 0:70d27fec6d71 751
paulcox 0:70d27fec6d71 752 /* End of DataTypeConversion: '<S44>/DataTypeConv1' */
paulcox 0:70d27fec6d71 753
paulcox 0:70d27fec6d71 754 /* Signum: '<S44>/SignPreIntegrator' */
paulcox 0:70d27fec6d71 755 if (Switch_fr < 0.0F) {
paulcox 0:70d27fec6d71 756 electrical_angle = -1.0F;
paulcox 0:70d27fec6d71 757 } else if (Switch_fr > 0.0F) {
paulcox 0:70d27fec6d71 758 electrical_angle = 1.0F;
paulcox 0:70d27fec6d71 759 } else {
paulcox 0:70d27fec6d71 760 electrical_angle = Switch_fr;
paulcox 0:70d27fec6d71 761 }
paulcox 0:70d27fec6d71 762
paulcox 0:70d27fec6d71 763 /* Switch: '<S42>/Switch' incorporates:
paulcox 0:70d27fec6d71 764 * Constant: '<S42>/Constant'
paulcox 0:70d27fec6d71 765 * DataTypeConversion: '<S44>/DataTypeConv2'
paulcox 0:70d27fec6d71 766 * Logic: '<S44>/AND'
paulcox 0:70d27fec6d71 767 * RelationalOperator: '<S44>/Equal'
paulcox 0:70d27fec6d71 768 * Signum: '<S44>/SignPreIntegrator'
paulcox 0:70d27fec6d71 769 */
paulcox 0:70d27fec6d71 770 if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
paulcox 0:70d27fec6d71 771 Switch_fr = 0.0F;
paulcox 0:70d27fec6d71 772 }
paulcox 0:70d27fec6d71 773
paulcox 0:70d27fec6d71 774 /* End of Switch: '<S42>/Switch' */
paulcox 0:70d27fec6d71 775
paulcox 0:70d27fec6d71 776 /* Update for DiscreteIntegrator: '<S42>/Integrator' */
paulcox 0:70d27fec6d71 777 DWork.Integrator_DSTATE_lc += 0.005F * Switch_fr;
paulcox 0:70d27fec6d71 778
paulcox 0:70d27fec6d71 779 /* End of Outputs for SubSystem: '<S5>/Position_Control' */
paulcox 0:70d27fec6d71 780 break;
paulcox 0:70d27fec6d71 781
paulcox 0:70d27fec6d71 782 case 2:
paulcox 0:70d27fec6d71 783 /* Outputs for IfAction SubSystem: '<S5>/Torque_Control' incorporates:
paulcox 0:70d27fec6d71 784 * ActionPort: '<S10>/Action Port'
paulcox 0:70d27fec6d71 785 */
paulcox 0:70d27fec6d71 786 DWork.Merge = DWork.torque_command;
paulcox 0:70d27fec6d71 787
paulcox 0:70d27fec6d71 788 /* End of Outputs for SubSystem: '<S5>/Torque_Control' */
paulcox 0:70d27fec6d71 789 break;
paulcox 0:70d27fec6d71 790 }
paulcox 0:70d27fec6d71 791
paulcox 0:70d27fec6d71 792 /* End of SwitchCase: '<S5>/Switch Case' */
paulcox 0:70d27fec6d71 793 }
paulcox 0:70d27fec6d71 794
paulcox 0:70d27fec6d71 795 /* RateTransition: '<S5>/Lo_to_Hi_Rate_Transition1' */
paulcox 0:70d27fec6d71 796 q_current_command = DWork.Merge;
paulcox 0:70d27fec6d71 797
paulcox 0:70d27fec6d71 798 /* Sum: '<S22>/Add' incorporates:
paulcox 0:70d27fec6d71 799 * Product: '<S22>/Product'
paulcox 0:70d27fec6d71 800 * Product: '<S22>/Product1'
paulcox 0:70d27fec6d71 801 */
paulcox 0:70d27fec6d71 802 q_current_measured = (SignDeltaU_b * cos_coefficient) - (phase_currents[0] *
paulcox 0:70d27fec6d71 803 sin_coefficient);
paulcox 0:70d27fec6d71 804
paulcox 0:70d27fec6d71 805 /* Sum: '<S12>/Sum' */
paulcox 0:70d27fec6d71 806 q_current_error = q_current_command - q_current_measured;
paulcox 0:70d27fec6d71 807
paulcox 0:70d27fec6d71 808 /* DiscreteIntegrator: '<S18>/Integrator' */
paulcox 0:70d27fec6d71 809 if ((FixPtRelationalOperator != 0) || (DWork.Integrator_PrevResetState_c != 0))
paulcox 0:70d27fec6d71 810 {
paulcox 0:70d27fec6d71 811 DWork.Integrator_DSTATE_l = 0.0F;
paulcox 0:70d27fec6d71 812 }
paulcox 0:70d27fec6d71 813
paulcox 0:70d27fec6d71 814 /* Sum: '<S18>/Sum' incorporates:
paulcox 0:70d27fec6d71 815 * DiscreteIntegrator: '<S18>/Integrator'
paulcox 0:70d27fec6d71 816 * Gain: '<S18>/Proportional Gain'
paulcox 0:70d27fec6d71 817 */
paulcox 0:70d27fec6d71 818 SignDeltaU_b = (ctrlParams.Current_P * q_current_error) +
paulcox 0:70d27fec6d71 819 DWork.Integrator_DSTATE_l;
paulcox 0:70d27fec6d71 820
paulcox 0:70d27fec6d71 821 /* Saturate: '<S18>/Saturate' */
paulcox 0:70d27fec6d71 822 if (SignDeltaU_b > 12.0F) {
paulcox 0:70d27fec6d71 823 alpha_voltage = 12.0F;
paulcox 0:70d27fec6d71 824 } else if (SignDeltaU_b < -12.0F) {
paulcox 0:70d27fec6d71 825 alpha_voltage = -12.0F;
paulcox 0:70d27fec6d71 826 } else {
paulcox 0:70d27fec6d71 827 alpha_voltage = SignDeltaU_b;
paulcox 0:70d27fec6d71 828 }
paulcox 0:70d27fec6d71 829
paulcox 0:70d27fec6d71 830 /* End of Saturate: '<S18>/Saturate' */
paulcox 0:70d27fec6d71 831
paulcox 0:70d27fec6d71 832 /* Sum: '<S24>/Add' incorporates:
paulcox 0:70d27fec6d71 833 * Product: '<S24>/Product'
paulcox 0:70d27fec6d71 834 * Product: '<S24>/Product1'
paulcox 0:70d27fec6d71 835 */
paulcox 0:70d27fec6d71 836 SignPreIntegrator_f = (Gain1 * sin_coefficient) + (alpha_voltage *
paulcox 0:70d27fec6d71 837 cos_coefficient);
paulcox 0:70d27fec6d71 838
paulcox 0:70d27fec6d71 839 /* Gain: '<S29>/Gain' */
paulcox 0:70d27fec6d71 840 IntegralGain_j = 0.5F * SignPreIntegrator_f;
paulcox 0:70d27fec6d71 841
paulcox 0:70d27fec6d71 842 /* Sum: '<S24>/Add1' incorporates:
paulcox 0:70d27fec6d71 843 * Product: '<S24>/Product2'
paulcox 0:70d27fec6d71 844 * Product: '<S24>/Product3'
paulcox 0:70d27fec6d71 845 */
paulcox 0:70d27fec6d71 846 alpha_voltage = (Gain1 * cos_coefficient) - (alpha_voltage * sin_coefficient);
paulcox 0:70d27fec6d71 847
paulcox 0:70d27fec6d71 848 /* Gain: '<S29>/Gain1' */
paulcox 0:70d27fec6d71 849 Gain1 = 0.866025388F * alpha_voltage;
paulcox 0:70d27fec6d71 850
paulcox 0:70d27fec6d71 851 /* Gain: '<S30>/Space_Vector_Gain' incorporates:
paulcox 0:70d27fec6d71 852 * Gain: '<S30>/Alpha_Gain'
paulcox 0:70d27fec6d71 853 * Sum: '<S30>/Add'
paulcox 0:70d27fec6d71 854 */
paulcox 0:70d27fec6d71 855 electrical_angle = (((1.73205078F * alpha_voltage) + 33.941124F) +
paulcox 0:70d27fec6d71 856 SignPreIntegrator_f) * 0.353553385F;
paulcox 0:70d27fec6d71 857
paulcox 0:70d27fec6d71 858 /* Gain: '<S33>/Va_Gain' incorporates:
paulcox 0:70d27fec6d71 859 * Gain: '<S33>/Alpha_Gain'
paulcox 0:70d27fec6d71 860 * Gain: '<S33>/Beta_Gain'
paulcox 0:70d27fec6d71 861 * Sum: '<S33>/Add'
paulcox 0:70d27fec6d71 862 */
paulcox 0:70d27fec6d71 863 Switch_fr = ((33.941124F - (1.73205078F * alpha_voltage)) + (3.0F *
paulcox 0:70d27fec6d71 864 SignPreIntegrator_f)) * 0.353553385F;
paulcox 0:70d27fec6d71 865
paulcox 0:70d27fec6d71 866 /* Gain: '<S36>/Space_Vector_Gain' incorporates:
paulcox 0:70d27fec6d71 867 * Gain: '<S36>/Alpha_Gain'
paulcox 0:70d27fec6d71 868 * Sum: '<S36>/Add'
paulcox 0:70d27fec6d71 869 */
paulcox 0:70d27fec6d71 870 cos_coefficient = ((33.941124F - (1.73205078F * alpha_voltage)) -
paulcox 0:70d27fec6d71 871 SignPreIntegrator_f) * 0.353553385F;
paulcox 0:70d27fec6d71 872
paulcox 0:70d27fec6d71 873 /* Gain: '<S31>/Space_Vector_Gain' incorporates:
paulcox 0:70d27fec6d71 874 * Constant: '<S27>/Bus_Voltage'
paulcox 0:70d27fec6d71 875 * Gain: '<S31>/Alpha_Gain'
paulcox 0:70d27fec6d71 876 * Sum: '<S31>/Add'
paulcox 0:70d27fec6d71 877 */
paulcox 0:70d27fec6d71 878 sin_coefficient = ((2.44948983F * alpha_voltage) + 24.0F) * 0.5F;
paulcox 0:70d27fec6d71 879
paulcox 0:70d27fec6d71 880 /* Gain: '<S34>/Va_Gain' incorporates:
paulcox 0:70d27fec6d71 881 * Constant: '<S27>/Bus_Voltage'
paulcox 0:70d27fec6d71 882 * Gain: '<S34>/Beta_Gain'
paulcox 0:70d27fec6d71 883 * Sum: '<S34>/Add'
paulcox 0:70d27fec6d71 884 */
paulcox 0:70d27fec6d71 885 Sectors_2_and_5_idx_1 = ((1.41421354F * SignPreIntegrator_f) + 24.0F) * 0.5F;
paulcox 0:70d27fec6d71 886
paulcox 0:70d27fec6d71 887 /* Gain: '<S37>/Space_Vector_Gain' incorporates:
paulcox 0:70d27fec6d71 888 * Constant: '<S27>/Bus_Voltage'
paulcox 0:70d27fec6d71 889 * Gain: '<S37>/Beta_Gain'
paulcox 0:70d27fec6d71 890 * Sum: '<S37>/Add'
paulcox 0:70d27fec6d71 891 */
paulcox 0:70d27fec6d71 892 Sectors_2_and_5_idx_2 = (24.0F - (1.41421354F * SignPreIntegrator_f)) * 0.5F;
paulcox 0:70d27fec6d71 893
paulcox 0:70d27fec6d71 894 /* Gain: '<S32>/Space_Vector_Gain' incorporates:
paulcox 0:70d27fec6d71 895 * Gain: '<S32>/Alpha_Gain'
paulcox 0:70d27fec6d71 896 * Sum: '<S32>/Add'
paulcox 0:70d27fec6d71 897 */
paulcox 0:70d27fec6d71 898 phase_voltages[0] = (((1.73205078F * alpha_voltage) + 33.941124F) -
paulcox 0:70d27fec6d71 899 SignPreIntegrator_f) * 0.353553385F;
paulcox 0:70d27fec6d71 900
paulcox 0:70d27fec6d71 901 /* Gain: '<S35>/Va_Gain' incorporates:
paulcox 0:70d27fec6d71 902 * Gain: '<S35>/Alpha_Gain'
paulcox 0:70d27fec6d71 903 * Sum: '<S35>/Add'
paulcox 0:70d27fec6d71 904 */
paulcox 0:70d27fec6d71 905 phase_voltages[1] = ((33.941124F - (1.73205078F * alpha_voltage)) +
paulcox 0:70d27fec6d71 906 SignPreIntegrator_f) * 0.353553385F;
paulcox 0:70d27fec6d71 907
paulcox 0:70d27fec6d71 908 /* Gain: '<S38>/Space_Vector_Gain' incorporates:
paulcox 0:70d27fec6d71 909 * Gain: '<S38>/Alpha_Gain'
paulcox 0:70d27fec6d71 910 * Gain: '<S38>/Beta_Gain'
paulcox 0:70d27fec6d71 911 * Sum: '<S38>/Add'
paulcox 0:70d27fec6d71 912 */
paulcox 0:70d27fec6d71 913 phase_voltages[2] = ((33.941124F - (1.73205078F * alpha_voltage)) - (3.0F *
paulcox 0:70d27fec6d71 914 SignPreIntegrator_f)) * 0.353553385F;
paulcox 0:70d27fec6d71 915
paulcox 0:70d27fec6d71 916 /* LookupNDDirect: '<S28>/Lookup_Table' incorporates:
paulcox 0:70d27fec6d71 917 * Constant: '<S39>/Constant'
paulcox 0:70d27fec6d71 918 * Constant: '<S40>/Constant'
paulcox 0:70d27fec6d71 919 * Constant: '<S41>/Constant'
paulcox 0:70d27fec6d71 920 * Gain: '<S28>/Sector_Gain_VB'
paulcox 0:70d27fec6d71 921 * Gain: '<S28>/Sector_Gain_VC'
paulcox 0:70d27fec6d71 922 * RelationalOperator: '<S39>/Compare'
paulcox 0:70d27fec6d71 923 * RelationalOperator: '<S40>/Compare'
paulcox 0:70d27fec6d71 924 * RelationalOperator: '<S41>/Compare'
paulcox 0:70d27fec6d71 925 * Sum: '<S28>/Calculate_Phase_Advanced_Sector'
paulcox 0:70d27fec6d71 926 * Sum: '<S29>/Add'
paulcox 0:70d27fec6d71 927 * Sum: '<S29>/Add1'
paulcox 0:70d27fec6d71 928 *
paulcox 0:70d27fec6d71 929 * About '<S28>/Lookup_Table':
paulcox 0:70d27fec6d71 930 * 1-dimensional Direct Look-Up returning a Scalar
paulcox 0:70d27fec6d71 931 */
paulcox 0:70d27fec6d71 932 u0 = (int16_T)(((((Gain1 - IntegralGain_j) > 0.0F) << 1) +
paulcox 0:70d27fec6d71 933 (SignPreIntegrator_f > 0.0F)) + ((((0.0F - IntegralGain_j) -
paulcox 0:70d27fec6d71 934 Gain1) > 0.0F) << 2));
paulcox 0:70d27fec6d71 935 if (u0 > 6) {
paulcox 0:70d27fec6d71 936 u0 = 6;
paulcox 0:70d27fec6d71 937 }
paulcox 0:70d27fec6d71 938
paulcox 0:70d27fec6d71 939 /* MultiPortSwitch: '<S27>/Select_Sector' incorporates:
paulcox 0:70d27fec6d71 940 * LookupNDDirect: '<S28>/Lookup_Table'
paulcox 0:70d27fec6d71 941 *
paulcox 0:70d27fec6d71 942 * About '<S28>/Lookup_Table':
paulcox 0:70d27fec6d71 943 * 1-dimensional Direct Look-Up returning a Scalar
paulcox 0:70d27fec6d71 944 */
paulcox 0:70d27fec6d71 945 switch (ConstP.Lookup_Table_table[u0]) {
paulcox 0:70d27fec6d71 946 case 1:
paulcox 0:70d27fec6d71 947 phase_voltages[0] = electrical_angle;
paulcox 0:70d27fec6d71 948 phase_voltages[1] = Switch_fr;
paulcox 0:70d27fec6d71 949 phase_voltages[2] = cos_coefficient;
paulcox 0:70d27fec6d71 950 break;
paulcox 0:70d27fec6d71 951
paulcox 0:70d27fec6d71 952 case 2:
paulcox 0:70d27fec6d71 953 phase_voltages[0] = sin_coefficient;
paulcox 0:70d27fec6d71 954 phase_voltages[1] = Sectors_2_and_5_idx_1;
paulcox 0:70d27fec6d71 955 phase_voltages[2] = Sectors_2_and_5_idx_2;
paulcox 0:70d27fec6d71 956 break;
paulcox 0:70d27fec6d71 957
paulcox 0:70d27fec6d71 958 case 3:
paulcox 0:70d27fec6d71 959 break;
paulcox 0:70d27fec6d71 960
paulcox 0:70d27fec6d71 961 case 4:
paulcox 0:70d27fec6d71 962 phase_voltages[0] = electrical_angle;
paulcox 0:70d27fec6d71 963 phase_voltages[1] = Switch_fr;
paulcox 0:70d27fec6d71 964 phase_voltages[2] = cos_coefficient;
paulcox 0:70d27fec6d71 965 break;
paulcox 0:70d27fec6d71 966
paulcox 0:70d27fec6d71 967 case 5:
paulcox 0:70d27fec6d71 968 phase_voltages[0] = sin_coefficient;
paulcox 0:70d27fec6d71 969 phase_voltages[1] = Sectors_2_and_5_idx_1;
paulcox 0:70d27fec6d71 970 phase_voltages[2] = Sectors_2_and_5_idx_2;
paulcox 0:70d27fec6d71 971 break;
paulcox 0:70d27fec6d71 972 }
paulcox 0:70d27fec6d71 973
paulcox 0:70d27fec6d71 974 /* End of MultiPortSwitch: '<S27>/Select_Sector' */
paulcox 0:70d27fec6d71 975
paulcox 0:70d27fec6d71 976 /* Switch: '<S6>/Switch' */
paulcox 0:70d27fec6d71 977 if (DWork.Lo_to_Hi_Rate_Transition3_Buffe) {
paulcox 0:70d27fec6d71 978 /* Outport: '<Root>/pwm_compare' */
paulcox 0:70d27fec6d71 979 pwm_compare[0] = 1500U;
paulcox 0:70d27fec6d71 980 pwm_compare[1] = 1500U;
paulcox 0:70d27fec6d71 981 pwm_compare[2] = 1500U;
paulcox 0:70d27fec6d71 982 } else {
paulcox 0:70d27fec6d71 983 /* Gain: '<S6>/Voltage to PWM Compare Units' */
paulcox 0:70d27fec6d71 984 electrical_angle = 125.0F * phase_voltages[0];
paulcox 0:70d27fec6d71 985
paulcox 0:70d27fec6d71 986 /* Saturate: '<S6>/Saturation' */
paulcox 0:70d27fec6d71 987 if (electrical_angle > 2999.0F) {
paulcox 0:70d27fec6d71 988 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 989 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 990 */
paulcox 0:70d27fec6d71 991 pwm_compare[0] = 2999U;
paulcox 0:70d27fec6d71 992 } else if (electrical_angle < 0.0F) {
paulcox 0:70d27fec6d71 993 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 994 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 995 */
paulcox 0:70d27fec6d71 996 pwm_compare[0] = 0U;
paulcox 0:70d27fec6d71 997 } else {
paulcox 0:70d27fec6d71 998 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 999 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 1000 */
paulcox 0:70d27fec6d71 1001 pwm_compare[0] = (uint16_T)electrical_angle;
paulcox 0:70d27fec6d71 1002 }
paulcox 0:70d27fec6d71 1003
paulcox 0:70d27fec6d71 1004 /* Gain: '<S6>/Voltage to PWM Compare Units' */
paulcox 0:70d27fec6d71 1005 electrical_angle = 125.0F * phase_voltages[1];
paulcox 0:70d27fec6d71 1006
paulcox 0:70d27fec6d71 1007 /* Saturate: '<S6>/Saturation' */
paulcox 0:70d27fec6d71 1008 if (electrical_angle > 2999.0F) {
paulcox 0:70d27fec6d71 1009 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 1010 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 1011 */
paulcox 0:70d27fec6d71 1012 pwm_compare[1] = 2999U;
paulcox 0:70d27fec6d71 1013 } else if (electrical_angle < 0.0F) {
paulcox 0:70d27fec6d71 1014 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 1015 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 1016 */
paulcox 0:70d27fec6d71 1017 pwm_compare[1] = 0U;
paulcox 0:70d27fec6d71 1018 } else {
paulcox 0:70d27fec6d71 1019 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 1020 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 1021 */
paulcox 0:70d27fec6d71 1022 pwm_compare[1] = (uint16_T)electrical_angle;
paulcox 0:70d27fec6d71 1023 }
paulcox 0:70d27fec6d71 1024
paulcox 0:70d27fec6d71 1025 /* Gain: '<S6>/Voltage to PWM Compare Units' */
paulcox 0:70d27fec6d71 1026 electrical_angle = 125.0F * phase_voltages[2];
paulcox 0:70d27fec6d71 1027
paulcox 0:70d27fec6d71 1028 /* Saturate: '<S6>/Saturation' */
paulcox 0:70d27fec6d71 1029 if (electrical_angle > 2999.0F) {
paulcox 0:70d27fec6d71 1030 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 1031 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 1032 */
paulcox 0:70d27fec6d71 1033 pwm_compare[2] = 2999U;
paulcox 0:70d27fec6d71 1034 } else if (electrical_angle < 0.0F) {
paulcox 0:70d27fec6d71 1035 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 1036 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 1037 */
paulcox 0:70d27fec6d71 1038 pwm_compare[2] = 0U;
paulcox 0:70d27fec6d71 1039 } else {
paulcox 0:70d27fec6d71 1040 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 1041 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 1042 */
paulcox 0:70d27fec6d71 1043 pwm_compare[2] = (uint16_T)electrical_angle;
paulcox 0:70d27fec6d71 1044 }
paulcox 0:70d27fec6d71 1045 }
paulcox 0:70d27fec6d71 1046
paulcox 0:70d27fec6d71 1047 /* End of Switch: '<S6>/Switch' */
paulcox 0:70d27fec6d71 1048
paulcox 0:70d27fec6d71 1049 /* DeadZone: '<S19>/DeadZone' */
paulcox 0:70d27fec6d71 1050 if (SignDeltaU > 12.0F) {
paulcox 0:70d27fec6d71 1051 SignDeltaU -= 12.0F;
paulcox 0:70d27fec6d71 1052 } else if (SignDeltaU >= -12.0F) {
paulcox 0:70d27fec6d71 1053 SignDeltaU = 0.0F;
paulcox 0:70d27fec6d71 1054 } else {
paulcox 0:70d27fec6d71 1055 SignDeltaU -= -12.0F;
paulcox 0:70d27fec6d71 1056 }
paulcox 0:70d27fec6d71 1057
paulcox 0:70d27fec6d71 1058 /* End of DeadZone: '<S19>/DeadZone' */
paulcox 0:70d27fec6d71 1059
paulcox 0:70d27fec6d71 1060 /* RelationalOperator: '<S19>/NotEqual' */
paulcox 0:70d27fec6d71 1061 NotEqual_b = (0.0F != SignDeltaU);
paulcox 0:70d27fec6d71 1062
paulcox 0:70d27fec6d71 1063 /* Signum: '<S19>/SignDeltaU' */
paulcox 0:70d27fec6d71 1064 if (SignDeltaU < 0.0F) {
paulcox 0:70d27fec6d71 1065 SignDeltaU = -1.0F;
paulcox 0:70d27fec6d71 1066 } else {
paulcox 0:70d27fec6d71 1067 if (SignDeltaU > 0.0F) {
paulcox 0:70d27fec6d71 1068 SignDeltaU = 1.0F;
paulcox 0:70d27fec6d71 1069 }
paulcox 0:70d27fec6d71 1070 }
paulcox 0:70d27fec6d71 1071
paulcox 0:70d27fec6d71 1072 /* End of Signum: '<S19>/SignDeltaU' */
paulcox 0:70d27fec6d71 1073
paulcox 0:70d27fec6d71 1074 /* Gain: '<S17>/Integral Gain' */
paulcox 0:70d27fec6d71 1075 IntegralGain_j = ctrlParams.Current_I * d_current_error;
paulcox 0:70d27fec6d71 1076
paulcox 0:70d27fec6d71 1077 /* DataTypeConversion: '<S19>/DataTypeConv1' */
paulcox 0:70d27fec6d71 1078 if (SignDeltaU < 128.0F) {
paulcox 0:70d27fec6d71 1079 rtPrevAction = (int8_T)SignDeltaU;
paulcox 0:70d27fec6d71 1080 } else {
paulcox 0:70d27fec6d71 1081 rtPrevAction = MAX_int8_T;
paulcox 0:70d27fec6d71 1082 }
paulcox 0:70d27fec6d71 1083
paulcox 0:70d27fec6d71 1084 /* End of DataTypeConversion: '<S19>/DataTypeConv1' */
paulcox 0:70d27fec6d71 1085
paulcox 0:70d27fec6d71 1086 /* Signum: '<S19>/SignPreIntegrator' */
paulcox 0:70d27fec6d71 1087 if (IntegralGain_j < 0.0F) {
paulcox 0:70d27fec6d71 1088 electrical_angle = -1.0F;
paulcox 0:70d27fec6d71 1089 } else if (IntegralGain_j > 0.0F) {
paulcox 0:70d27fec6d71 1090 electrical_angle = 1.0F;
paulcox 0:70d27fec6d71 1091 } else {
paulcox 0:70d27fec6d71 1092 electrical_angle = IntegralGain_j;
paulcox 0:70d27fec6d71 1093 }
paulcox 0:70d27fec6d71 1094
paulcox 0:70d27fec6d71 1095 /* Switch: '<S17>/Switch' incorporates:
paulcox 0:70d27fec6d71 1096 * Constant: '<S17>/Constant'
paulcox 0:70d27fec6d71 1097 * DataTypeConversion: '<S19>/DataTypeConv2'
paulcox 0:70d27fec6d71 1098 * Logic: '<S19>/AND'
paulcox 0:70d27fec6d71 1099 * RelationalOperator: '<S19>/Equal'
paulcox 0:70d27fec6d71 1100 * Signum: '<S19>/SignPreIntegrator'
paulcox 0:70d27fec6d71 1101 */
paulcox 0:70d27fec6d71 1102 if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
paulcox 0:70d27fec6d71 1103 electrical_angle = 0.0F;
paulcox 0:70d27fec6d71 1104 } else {
paulcox 0:70d27fec6d71 1105 electrical_angle = IntegralGain_j;
paulcox 0:70d27fec6d71 1106 }
paulcox 0:70d27fec6d71 1107
paulcox 0:70d27fec6d71 1108 /* End of Switch: '<S17>/Switch' */
paulcox 0:70d27fec6d71 1109
paulcox 0:70d27fec6d71 1110 /* DeadZone: '<S20>/DeadZone' */
paulcox 0:70d27fec6d71 1111 if (SignDeltaU_b > 12.0F) {
paulcox 0:70d27fec6d71 1112 SignDeltaU_b -= 12.0F;
paulcox 0:70d27fec6d71 1113 } else if (SignDeltaU_b >= -12.0F) {
paulcox 0:70d27fec6d71 1114 SignDeltaU_b = 0.0F;
paulcox 0:70d27fec6d71 1115 } else {
paulcox 0:70d27fec6d71 1116 SignDeltaU_b -= -12.0F;
paulcox 0:70d27fec6d71 1117 }
paulcox 0:70d27fec6d71 1118
paulcox 0:70d27fec6d71 1119 /* End of DeadZone: '<S20>/DeadZone' */
paulcox 0:70d27fec6d71 1120
paulcox 0:70d27fec6d71 1121 /* RelationalOperator: '<S20>/NotEqual' */
paulcox 0:70d27fec6d71 1122 NotEqual_b = (0.0F != SignDeltaU_b);
paulcox 0:70d27fec6d71 1123
paulcox 0:70d27fec6d71 1124 /* Signum: '<S20>/SignDeltaU' */
paulcox 0:70d27fec6d71 1125 if (SignDeltaU_b < 0.0F) {
paulcox 0:70d27fec6d71 1126 SignDeltaU_b = -1.0F;
paulcox 0:70d27fec6d71 1127 } else {
paulcox 0:70d27fec6d71 1128 if (SignDeltaU_b > 0.0F) {
paulcox 0:70d27fec6d71 1129 SignDeltaU_b = 1.0F;
paulcox 0:70d27fec6d71 1130 }
paulcox 0:70d27fec6d71 1131 }
paulcox 0:70d27fec6d71 1132
paulcox 0:70d27fec6d71 1133 /* End of Signum: '<S20>/SignDeltaU' */
paulcox 0:70d27fec6d71 1134
paulcox 0:70d27fec6d71 1135 /* Gain: '<S18>/Integral Gain' */
paulcox 0:70d27fec6d71 1136 IntegralGain_j = ctrlParams.Current_I * q_current_error;
paulcox 0:70d27fec6d71 1137 if (M->Timing.TaskCounters.TID[1] == 0) {
paulcox 0:70d27fec6d71 1138 /* RelationalOperator: '<S6>/Relational Operator' incorporates:
paulcox 0:70d27fec6d71 1139 * Constant: '<S47>/Constant'
paulcox 0:70d27fec6d71 1140 */
paulcox 0:70d27fec6d71 1141 RelationalOperator = (controller_mode == StandBy);
paulcox 0:70d27fec6d71 1142
paulcox 0:70d27fec6d71 1143 /* Outputs for Enabled SubSystem: '<S58>/Generate_Error' incorporates:
paulcox 0:70d27fec6d71 1144 * EnablePort: '<S60>/Enable'
paulcox 0:70d27fec6d71 1145 */
paulcox 0:70d27fec6d71 1146 /* Logic: '<S58>/AND' incorporates:
paulcox 0:70d27fec6d71 1147 * Abs: '<S58>/Velocity_Abs'
paulcox 0:70d27fec6d71 1148 * Constant: '<S58>/Max_Valid_Velocity_Change'
paulcox 0:70d27fec6d71 1149 * Constant: '<S59>/Constant'
paulcox 0:70d27fec6d71 1150 * Delay: '<S58>/Velocity_Delay'
paulcox 0:70d27fec6d71 1151 * RelationalOperator: '<S58>/Excessive_Velocity_Change'
paulcox 0:70d27fec6d71 1152 * RelationalOperator: '<S58>/Relational_Operator'
paulcox 0:70d27fec6d71 1153 * Sum: '<S58>/Velocity_Difference'
paulcox 0:70d27fec6d71 1154 */
paulcox 0:70d27fec6d71 1155 if ((controller_mode == VelocityControl) && (((real32_T)fabs
paulcox 0:70d27fec6d71 1156 (velocity_measured - DWork.Velocity_Delay_DSTATE)) >= 628.318542F)) {
paulcox 0:70d27fec6d71 1157 /* DataStoreWrite: '<S60>/Data_Store_Write' incorporates:
paulcox 0:70d27fec6d71 1158 * Constant: '<S61>/Constant'
paulcox 0:70d27fec6d71 1159 */
paulcox 0:70d27fec6d71 1160 DWork.error_l = MeasuredVelocityError;
paulcox 0:70d27fec6d71 1161 }
paulcox 0:70d27fec6d71 1162
paulcox 0:70d27fec6d71 1163 /* End of Logic: '<S58>/AND' */
paulcox 0:70d27fec6d71 1164 /* End of Outputs for SubSystem: '<S58>/Generate_Error' */
paulcox 0:70d27fec6d71 1165 }
paulcox 0:70d27fec6d71 1166
paulcox 0:70d27fec6d71 1167 /* Outport: '<Root>/error' incorporates:
paulcox 0:70d27fec6d71 1168 * DataStoreRead: '<Root>/Data Store Read'
paulcox 0:70d27fec6d71 1169 */
paulcox 0:70d27fec6d71 1170 error = DWork.error_l;
paulcox 0:70d27fec6d71 1171
paulcox 0:70d27fec6d71 1172 /* Update for RateTransition: '<S6>/Lo_to_Hi_Rate_Transition3' */
paulcox 0:70d27fec6d71 1173 if (M->Timing.TaskCounters.TID[1] == 0) {
paulcox 0:70d27fec6d71 1174 DWork.Lo_to_Hi_Rate_Transition3_Buffe = RelationalOperator;
paulcox 0:70d27fec6d71 1175
paulcox 0:70d27fec6d71 1176 /* Update for Delay: '<S55>/Position_Delay' */
paulcox 0:70d27fec6d71 1177 DWork.Position_Delay_DSTATE = Wrap_To_Pi;
paulcox 0:70d27fec6d71 1178 }
paulcox 0:70d27fec6d71 1179
paulcox 0:70d27fec6d71 1180 /* End of Update for RateTransition: '<S6>/Lo_to_Hi_Rate_Transition3' */
paulcox 0:70d27fec6d71 1181
paulcox 0:70d27fec6d71 1182 /* Update for UnitDelay: '<S13>/Delay Input1' */
paulcox 0:70d27fec6d71 1183 DWork.DelayInput1_DSTATE = Enum_To_Int;
paulcox 0:70d27fec6d71 1184
paulcox 0:70d27fec6d71 1185 /* Update for DiscreteIntegrator: '<S17>/Integrator' */
paulcox 0:70d27fec6d71 1186 if (FixPtRelationalOperator == 0) {
paulcox 0:70d27fec6d71 1187 DWork.Integrator_DSTATE += 4.0E-5F * electrical_angle;
paulcox 0:70d27fec6d71 1188 }
paulcox 0:70d27fec6d71 1189
paulcox 0:70d27fec6d71 1190 if (FixPtRelationalOperator > 0) {
paulcox 0:70d27fec6d71 1191 DWork.Integrator_PrevResetState = 1;
paulcox 0:70d27fec6d71 1192 } else {
paulcox 0:70d27fec6d71 1193 DWork.Integrator_PrevResetState = 0;
paulcox 0:70d27fec6d71 1194 }
paulcox 0:70d27fec6d71 1195
paulcox 0:70d27fec6d71 1196 /* End of Update for DiscreteIntegrator: '<S17>/Integrator' */
paulcox 0:70d27fec6d71 1197
paulcox 0:70d27fec6d71 1198 /* Update for DiscreteIntegrator: '<S18>/Integrator' */
paulcox 0:70d27fec6d71 1199 if (FixPtRelationalOperator == 0) {
paulcox 0:70d27fec6d71 1200 /* DataTypeConversion: '<S20>/DataTypeConv1' */
paulcox 0:70d27fec6d71 1201 if (SignDeltaU_b < 128.0F) {
paulcox 0:70d27fec6d71 1202 rtPrevAction = (int8_T)SignDeltaU_b;
paulcox 0:70d27fec6d71 1203 } else {
paulcox 0:70d27fec6d71 1204 rtPrevAction = MAX_int8_T;
paulcox 0:70d27fec6d71 1205 }
paulcox 0:70d27fec6d71 1206
paulcox 0:70d27fec6d71 1207 /* End of DataTypeConversion: '<S20>/DataTypeConv1' */
paulcox 0:70d27fec6d71 1208
paulcox 0:70d27fec6d71 1209 /* Signum: '<S20>/SignPreIntegrator' */
paulcox 0:70d27fec6d71 1210 if (IntegralGain_j < 0.0F) {
paulcox 0:70d27fec6d71 1211 electrical_angle = -1.0F;
paulcox 0:70d27fec6d71 1212 } else if (IntegralGain_j > 0.0F) {
paulcox 0:70d27fec6d71 1213 electrical_angle = 1.0F;
paulcox 0:70d27fec6d71 1214 } else {
paulcox 0:70d27fec6d71 1215 electrical_angle = IntegralGain_j;
paulcox 0:70d27fec6d71 1216 }
paulcox 0:70d27fec6d71 1217
paulcox 0:70d27fec6d71 1218 /* Switch: '<S18>/Switch' incorporates:
paulcox 0:70d27fec6d71 1219 * Constant: '<S18>/Constant'
paulcox 0:70d27fec6d71 1220 * DataTypeConversion: '<S20>/DataTypeConv2'
paulcox 0:70d27fec6d71 1221 * Logic: '<S20>/AND'
paulcox 0:70d27fec6d71 1222 * RelationalOperator: '<S20>/Equal'
paulcox 0:70d27fec6d71 1223 * Signum: '<S20>/SignPreIntegrator'
paulcox 0:70d27fec6d71 1224 */
paulcox 0:70d27fec6d71 1225 if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
paulcox 0:70d27fec6d71 1226 IntegralGain_j = 0.0F;
paulcox 0:70d27fec6d71 1227 }
paulcox 0:70d27fec6d71 1228
paulcox 0:70d27fec6d71 1229 /* End of Switch: '<S18>/Switch' */
paulcox 0:70d27fec6d71 1230 DWork.Integrator_DSTATE_l += 4.0E-5F * IntegralGain_j;
paulcox 0:70d27fec6d71 1231 }
paulcox 0:70d27fec6d71 1232
paulcox 0:70d27fec6d71 1233 if (FixPtRelationalOperator > 0) {
paulcox 0:70d27fec6d71 1234 DWork.Integrator_PrevResetState_c = 1;
paulcox 0:70d27fec6d71 1235 } else {
paulcox 0:70d27fec6d71 1236 DWork.Integrator_PrevResetState_c = 0;
paulcox 0:70d27fec6d71 1237 }
paulcox 0:70d27fec6d71 1238
paulcox 0:70d27fec6d71 1239 /* End of Update for DiscreteIntegrator: '<S18>/Integrator' */
paulcox 0:70d27fec6d71 1240 if (M->Timing.TaskCounters.TID[1] == 0) {
paulcox 0:70d27fec6d71 1241 /* Update for Delay: '<S58>/Velocity_Delay' */
paulcox 0:70d27fec6d71 1242 DWork.Velocity_Delay_DSTATE = velocity_measured;
paulcox 0:70d27fec6d71 1243 }
paulcox 0:70d27fec6d71 1244
paulcox 0:70d27fec6d71 1245 rate_scheduler();
paulcox 0:70d27fec6d71 1246 return error;
paulcox 0:70d27fec6d71 1247 }
paulcox 0:70d27fec6d71 1248
paulcox 0:70d27fec6d71 1249 /* Model initialize function */
paulcox 0:70d27fec6d71 1250 extern "C" void Controller_Init(void)
paulcox 0:70d27fec6d71 1251 {
paulcox 0:70d27fec6d71 1252 /* Registration code */
paulcox 0:70d27fec6d71 1253
paulcox 0:70d27fec6d71 1254 /* initialize real-time model */
paulcox 0:70d27fec6d71 1255 (void) memset((void *)M, 0,
paulcox 0:70d27fec6d71 1256 sizeof(RT_MODEL));
paulcox 0:70d27fec6d71 1257
paulcox 0:70d27fec6d71 1258 /* block I/O */
paulcox 0:70d27fec6d71 1259
paulcox 0:70d27fec6d71 1260 /* exported global signals */
paulcox 0:70d27fec6d71 1261 phase_currents[0] = 0.0F;
paulcox 0:70d27fec6d71 1262 phase_currents[1] = 0.0F;
paulcox 0:70d27fec6d71 1263 rotor_position = 0.0F;
paulcox 0:70d27fec6d71 1264 velocity_measured = 0.0F;
paulcox 0:70d27fec6d71 1265 d_current_error = 0.0F;
paulcox 0:70d27fec6d71 1266 q_current_command = 0.0F;
paulcox 0:70d27fec6d71 1267 q_current_measured = 0.0F;
paulcox 0:70d27fec6d71 1268 q_current_error = 0.0F;
paulcox 0:70d27fec6d71 1269 phase_voltages[0] = 0.0F;
paulcox 0:70d27fec6d71 1270 phase_voltages[1] = 0.0F;
paulcox 0:70d27fec6d71 1271 phase_voltages[2] = 0.0F;
paulcox 0:70d27fec6d71 1272 velocity_error = 0.0F;
paulcox 0:70d27fec6d71 1273 controller_mode = StandBy;
paulcox 0:70d27fec6d71 1274
paulcox 0:70d27fec6d71 1275 /* states (dwork) */
paulcox 0:70d27fec6d71 1276 (void) memset((void *)&DWork, 0,
paulcox 0:70d27fec6d71 1277 sizeof(D_Work));
paulcox 0:70d27fec6d71 1278
paulcox 0:70d27fec6d71 1279 /* InitializeConditions for Enabled SubSystem: '<S51>/Open Loop Position' */
paulcox 0:70d27fec6d71 1280 /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Position' */
paulcox 0:70d27fec6d71 1281 DWork.Integrate_To_Position_DSTATE = 0.0F;
paulcox 0:70d27fec6d71 1282
paulcox 0:70d27fec6d71 1283 /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' */
paulcox 0:70d27fec6d71 1284 DWork.Integrate_To_Velocity_DSTATE = 0.0F;
paulcox 0:70d27fec6d71 1285
paulcox 0:70d27fec6d71 1286 /* End of InitializeConditions for SubSystem: '<S51>/Open Loop Position' */
paulcox 0:70d27fec6d71 1287
paulcox 0:70d27fec6d71 1288 /* Start for SwitchCase: '<S5>/Switch Case' */
paulcox 0:70d27fec6d71 1289 DWork.SwitchCase_ActiveSubsystem = -1;
paulcox 0:70d27fec6d71 1290
paulcox 0:70d27fec6d71 1291 /* InitializeConditions for IfAction SubSystem: '<S5>/Velocity_Control' */
paulcox 0:70d27fec6d71 1292 /* InitializeConditions for DiscreteIntegrator: '<S45>/Integrator' */
paulcox 0:70d27fec6d71 1293 DWork.Integrator_DSTATE_f = 0.0F;
paulcox 0:70d27fec6d71 1294
paulcox 0:70d27fec6d71 1295 /* End of InitializeConditions for SubSystem: '<S5>/Velocity_Control' */
paulcox 0:70d27fec6d71 1296
paulcox 0:70d27fec6d71 1297 /* InitializeConditions for IfAction SubSystem: '<S5>/Position_Control' */
paulcox 0:70d27fec6d71 1298 /* InitializeConditions for DiscreteIntegrator: '<S42>/Integrator' */
paulcox 0:70d27fec6d71 1299 DWork.Integrator_DSTATE_lc = 0.0F;
paulcox 0:70d27fec6d71 1300
paulcox 0:70d27fec6d71 1301 /* End of InitializeConditions for SubSystem: '<S5>/Position_Control' */
paulcox 0:70d27fec6d71 1302
paulcox 0:70d27fec6d71 1303 /* InitializeConditions for Chart: '<S49>/Wait_For_Valid_Position' */
paulcox 0:70d27fec6d71 1304 DWork.temporalCounter_i1 = 0U;
paulcox 0:70d27fec6d71 1305 DWork.is_active_c2_rtwdemo_pmsmfoc = 0U;
paulcox 0:70d27fec6d71 1306 DWork.is_c2_rtwdemo_pmsmfoc = IN_NO_ACTIVE_CHILD;
paulcox 0:70d27fec6d71 1307 DWork.Position_Valid = 0U;
paulcox 0:70d27fec6d71 1308
paulcox 0:70d27fec6d71 1309 /* InitializeConditions for Chart: '<S1>/Controller_Mode_Scheduler' */
paulcox 0:70d27fec6d71 1310 DWork.is_Motor_On = IN_NO_ACTIVE_CHILD;
paulcox 0:70d27fec6d71 1311 DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD;
paulcox 0:70d27fec6d71 1312 DWork.velocity_command = 0.0F;
paulcox 0:70d27fec6d71 1313 DWork.position_command = 0.0F;
paulcox 0:70d27fec6d71 1314 DWork.torque_command = 0.0F;
paulcox 0:70d27fec6d71 1315
paulcox 0:70d27fec6d71 1316 /* Entry: Mode_Scheduler/Controller_Mode_Scheduler */
paulcox 0:70d27fec6d71 1317 /* Entry Internal: Mode_Scheduler/Controller_Mode_Scheduler */
paulcox 0:70d27fec6d71 1318 /* Transition: '<S4>:9' */
paulcox 0:70d27fec6d71 1319 DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By;
paulcox 0:70d27fec6d71 1320
paulcox 0:70d27fec6d71 1321 /* Entry 'Stand_By': '<S4>:154' */
paulcox 0:70d27fec6d71 1322 controller_mode = StandBy;
paulcox 0:70d27fec6d71 1323
paulcox 0:70d27fec6d71 1324 /* InitializeConditions for DiscreteIntegrator: '<S17>/Integrator' */
paulcox 0:70d27fec6d71 1325 DWork.Integrator_PrevResetState = 0;
paulcox 0:70d27fec6d71 1326
paulcox 0:70d27fec6d71 1327 /* InitializeConditions for DiscreteIntegrator: '<S18>/Integrator' */
paulcox 0:70d27fec6d71 1328 DWork.Integrator_PrevResetState_c = 0;
paulcox 0:70d27fec6d71 1329 }
paulcox 0:70d27fec6d71 1330
paulcox 0:70d27fec6d71 1331 /*
paulcox 0:70d27fec6d71 1332 * File trailer for generated code.
paulcox 0:70d27fec6d71 1333 *
paulcox 0:70d27fec6d71 1334 * [EOF]
paulcox 0:70d27fec6d71 1335 */