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