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

36
real32_T phase_currents[2];            /* '<S48>/Product' */

37
real32_T rotor_position;               /* '<S49>/Add' */

38
real32_T velocity_measured;            /* '<S55>/Scale_Output' */

39
real32_T d_current_error;              /* '<S12>/Sum2' */

40
real32_T q_current_command;            /* '<S5>/Lo_to_Hi_Rate_Transition1' */

41
real32_T q_current_measured;           /* '<S22>/Add' */

42
real32_T q_current_error;              /* '<S12>/Sum' */

43
real32_T phase_voltages[3];            /* '<S27>/Select_Sector' */

44
real32_T velocity_error;               /* '<S11>/Sum2' */

45
EnumControllerMode controller_mode;    /* '<S1>/Controller_Mode_Scheduler' */

46


47
/* Exported block parameters */

48
CTRLPARAMS_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) */

81
const 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) */

89
D_Work DWork;

90


91
/* Real-time model */

92
RT_MODEL M_;

93
RT_MODEL *const M = &M_;

94
extern real32_T rt_roundf(real32_T u);

95
extern real32_T rt_modf(real32_T u0, real32_T u1);

96
static 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
 */

103
static 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


115
real32_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


133
real32_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 */

155
EnumErrorType 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 */

1250
void 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