instaspin_foc
proj_lab11.c
Go to the documentation of this file.
1 // --COPYRIGHT--,BSD
2 // Copyright (c) 2015, Texas Instruments Incorporated
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions
7 // are met:
8 //
9 // * Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // * Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // * Neither the name of Texas Instruments Incorporated nor the names of
17 // its contributors may be used to endorse or promote products derived
18 // from this software without specific prior written permission.
19 //
20 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
22 // THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23 // PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
24 // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25 // EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26 // PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
27 // OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
28 // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
29 // OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
30 // EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 // --/COPYRIGHT
36 
38 
39 
45 
46 // **************************************************************************
47 // the includes
48 
49 // system includes
50 #include <math.h>
51 #include "main.h"
52 
53 #ifdef FLASH
54 #pragma CODE_SECTION(mainISR,"ramfuncs");
55 #endif
56 
57 // Include header files used in the main function
58 
59 // **************************************************************************
60 // the defines
61 
62 // **************************************************************************
63 // the globals
64 
68 
72 
74 
78 uint16_t pidCntSpeed;
80 
85 
88 
89 #ifdef F2802xF
90 #pragma DATA_SECTION(halHandle,"rom_accessed_data");
91 #endif
93 
95 HAL_PwmData_t gPwmData = {_IQ(0.0),_IQ(0.0),_IQ(0.0)};
96 
100 
102 MATH_vec3 gOffsets_I_pu = {_IQ(0.0),_IQ(0.0),_IQ(0.0)};
103 
105 MATH_vec3 gOffsets_V_pu = {_IQ(0.0),_IQ(0.0),_IQ(0.0)};
106 
108 MATH_vec2 gIdq_ref_pu = {_IQ(0.0),_IQ(0.0)};
109 
111 MATH_vec2 gVdq_out_pu = {_IQ(0.0),_IQ(0.0)};
112 
114 MATH_vec2 gIdq_pu = {_IQ(0.0),_IQ(0.0)};
115 
117 #ifdef F2802xF
118 #pragma DATA_SECTION(gUserParams,"rom_accessed_data");
119 #endif
121 
123 
127 #ifdef FLASH
128 // Used for running BackGround in flash, and ISR in RAM
129 extern uint16_t *RamfuncsLoadStart, *RamfuncsLoadEnd, *RamfuncsRunStart;
130 
131 #ifdef F2802xF
132 extern uint16_t *econst_start, *econst_end, *econst_ram_load;
133 extern uint16_t *switch_start, *switch_end, *switch_ram_load;
134 #endif
135 
136 #endif
137 
138 #ifdef DRV8301_SPI
139 // Watch window interface to the 8301 SPI
140 DRV_SPI_8301_Vars_t gDrvSpi8301Vars;
141 #endif
142 
143 #ifdef DRV8305_SPI
144 // Watch window interface to the 8305 SPI
145 DRV_SPI_8305_Vars_t gDrvSpi8305Vars;
146 #endif
147 
149 
151 
153 
155 
157  / (USER_IQ_FULL_SCALE_FREQ_Hz * 60.0));
158 
160  / 1000.0);
161 
162 // **************************************************************************
163 // the functions
164 void main(void)
165 {
166  // IMPORTANT NOTE: If you are not familiar with MotorWare coding guidelines
167  // please refer to the following document:
168  // C:/ti/motorware/motorware_1_01_00_1x/docs/motorware_coding_standards.pdf
169 
170  // Only used if running from FLASH
171  // Note that the variable FLASH is defined by the project
172 
173  #ifdef FLASH
174  // Copy time critical code and Flash setup code to RAM
175  // The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
176  // symbols are created by the linker. Refer to the linker files.
177  memCopy((uint16_t *)&RamfuncsLoadStart,(uint16_t *)&RamfuncsLoadEnd,
178  (uint16_t *)&RamfuncsRunStart);
179 
180  #ifdef F2802xF
181  //copy .econst to unsecure RAM
182  if(*econst_end - *econst_start)
183  {
184  memCopy((uint16_t *)&econst_start,(uint16_t *)&econst_end,
185  (uint16_t *)&econst_ram_load);
186  }
187 
188  //copy .switch ot unsecure RAM
189  if(*switch_end - *switch_start)
190  {
191  memCopy((uint16_t *)&switch_start,(uint16_t *)&switch_end,
192  (uint16_t *)&switch_ram_load);
193  }
194  #endif
195 
196  #endif
197 
198  // initialize the Hardware Abstraction Layer (HAL)
199  // halHandle will be used throughout the code to interface with the HAL
200  // (set parameters, get and set functions, etc) halHandle is required since
201  // this is how all objects are interfaced, and it allows interface with
202  // multiple objects by simply passing a different handle. The use of
203  // handles is explained in this document:
204  // C:/ti/motorware/motorware_1_01_00_1x/docs/motorware_coding_standards.pdf
205  halHandle = HAL_init(&hal,sizeof(hal));
206 
207  // check for errors in user parameters
208  USER_checkForErrors(&gUserParams);
209 
210  // store user parameter error in global variable
211  gMotorVars.UserErrorCode = USER_getErrorCode(&gUserParams);
212 
213  // do not allow code execution if there is a user parameter error. If there
214  // is an error, the code will be stuck in this forever loop
215  if(gMotorVars.UserErrorCode != USER_ErrorCode_NoError)
216  {
217  for(;;)
218  {
219  gMotorVars.Flag_enableSys = false;
220  }
221  }
222 
223  // initialize the Clarke modules
224  // Clarke handle initialization for current signals
225  clarkeHandle_I = CLARKE_init(&clarke_I,sizeof(clarke_I));
226  // Clarke handle initialization for voltage signals
227  clarkeHandle_V = CLARKE_init(&clarke_V,sizeof(clarke_V));
228 
229  // initialize the estimator
230  estHandle = EST_init((void *)USER_EST_HANDLE_ADDRESS,0x200);
231 
232  // initialize the user parameters
233  // This function initializes all values of structure gUserParams with
234  // values defined in user.h. The values in gUserParams will be then used by
235  // the hardware abstraction layer (HAL) to configure peripherals such as
236  // PWM, ADC, interrupts, etc.
237  USER_setParams(&gUserParams);
238 
239  // set the hardware abstraction layer parameters
240  // This function initializes all peripherals through a Hardware Abstraction
241  // Layer (HAL). It uses all values stored in gUserParams.
242  HAL_setParams(halHandle,&gUserParams);
243 
244  #ifdef FAST_ROM_V1p6
245  {
246  // These function calls are used to initialize the estimator with ROM
247  // function calls. It needs the specific address where the controller
248  // object is declared by the ROM code.
249  CTRL_Handle ctrlHandle = CTRL_init((void *)USER_CTRL_HANDLE_ADDRESS
250  ,0x200);
251  CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
252 
253  // this sets the estimator handle (part of the controller object) to
254  // the same value initialized above by the EST_init() function call.
255  // This is done so the next function implemented in ROM, can
256  // successfully initialize the estimator as part of the controller
257  // object.
258  obj->estHandle = estHandle;
259 
260  // initialize the estimator through the controller. These three
261  // function calls are needed for the F2806xF/M implementation of
262  // InstaSPIN.
263  CTRL_setParams(ctrlHandle,&gUserParams);
264  CTRL_setUserMotorParams(ctrlHandle);
265  CTRL_setupEstIdleState(ctrlHandle);
266  }
267  #else
268  {
269  // initialize the estimator. These two function calls are needed for
270  // the F2802xF implementation of InstaSPIN using the estimator handle
271  // initialized by EST_init(), these two function calls configure the
272  // estimator, and they set the estimator in a proper state prior to
273  // spinning a motor.
274  EST_setEstParams(estHandle,&gUserParams);
276  }
277  #endif
278 
279  // disable Rs recalculation
281 
282  // set the number of current sensors
283  setupClarke_I(clarkeHandle_I,USER_NUM_CURRENT_SENSORS);
284 
285  // set the number of voltage sensors
286  setupClarke_V(clarkeHandle_V,USER_NUM_VOLTAGE_SENSORS);
287 
288  // set the pre-determined current and voltage feeback offset values
289  gOffsets_I_pu.value[0] = _IQ(I_A_offset);
290  gOffsets_I_pu.value[1] = _IQ(I_B_offset);
291  gOffsets_I_pu.value[2] = _IQ(I_C_offset);
292  gOffsets_V_pu.value[0] = _IQ(V_A_offset);
293  gOffsets_V_pu.value[1] = _IQ(V_B_offset);
294  gOffsets_V_pu.value[2] = _IQ(V_C_offset);
295 
296  // initialize the PID controllers
297  {
298  // This equation defines the relationship between per unit current and
299  // real-world current. The resulting value in per units (pu) is then
300  // used to configure the controllers
301  _iq maxCurrent_pu = _IQ(USER_MOTOR_MAX_CURRENT
303 
304  // This equation uses the scaled maximum voltage vector, which is
305  // already in per units, hence there is no need to include the #define
306  // for USER_IQ_FULL_SCALE_VOLTAGE_V
307  _iq maxVoltage_pu = _IQ(USER_MAX_VS_MAG_PU * USER_VD_SF);
308 
309  float_t fullScaleCurrent = USER_IQ_FULL_SCALE_CURRENT_A;
310  float_t fullScaleVoltage = USER_IQ_FULL_SCALE_VOLTAGE_V;
311  float_t IsrPeriod_sec = 1.0 / USER_ISR_FREQ_Hz;
312  float_t Ls_d = USER_MOTOR_Ls_d;
313  float_t Ls_q = USER_MOTOR_Ls_q;
314  float_t Rs = USER_MOTOR_Rs;
315 
316  // This lab assumes that motor parameters are known, and it does not
317  // perform motor ID, so the R/L parameters are known and defined in
318  // user.h
319  float_t RoverLs_d = Rs / Ls_d;
320  float_t RoverLs_q = Rs / Ls_q;
321 
322  // For the current controller, Kp = Ls*bandwidth(rad/sec) But in order
323  // to be used, it must be converted to per unit values by multiplying
324  // by fullScaleCurrent and then dividing by fullScaleVoltage. From the
325  // statement below, we see that the bandwidth in rad/sec is equal to
326  // 0.25/IsrPeriod_sec, which is equal to USER_ISR_FREQ_HZ/4. This means
327  // that by setting Kp as described below, the bandwidth in Hz is
328  // USER_ISR_FREQ_HZ/(8*pi).
329  _iq Kp_Id = _IQ((0.25 * Ls_d * fullScaleCurrent) / (IsrPeriod_sec
330  * fullScaleVoltage));
331 
332  // In order to achieve pole/zero cancellation (which reduces the
333  // closed-loop transfer function from a second-order system to a
334  // first-order system), Ki must equal Rs/Ls. Since the output of the
335  // Ki gain stage is integrated by a DIGITAL integrator, the integrator
336  // input must be scaled by 1/IsrPeriod_sec. That's just the way
337  // digital integrators work. But, since IsrPeriod_sec is a constant,
338  // we can save an additional multiplication operation by lumping this
339  // term with the Ki value.
340  _iq Ki_Id = _IQ(RoverLs_d * IsrPeriod_sec);
341 
342  // Now do the same thing for Kp for the q-axis current controller.
343  // If the motor is not an IPM motor, Ld and Lq are the same, which
344  // means that Kp_Iq = Kp_Id
345  _iq Kp_Iq = _IQ((0.25 * Ls_q * fullScaleCurrent) / (IsrPeriod_sec
346  * fullScaleVoltage));
347 
348  // Do the same thing for Ki for the q-axis current controller. If the
349  // motor is not an IPM motor, Ld and Lq are the same, which means that
350  // Ki_Iq = Ki_Id.
351  _iq Ki_Iq = _IQ(RoverLs_q * IsrPeriod_sec);
352 
353  // There are three PI controllers; one speed controller and two current
354  // controllers. Each PI controller has two coefficients; Kp and Ki.
355  // So you have a total of six coefficients that must be defined.
356  // This is for the speed controller
357  pidHandle[0] = PID_init(&pid[0],sizeof(pid[0]));
358  // This is for the Id current controller
359  pidHandle[1] = PID_init(&pid[1],sizeof(pid[1]));
360  // This is for the Iq current controller
361  pidHandle[2] = PID_init(&pid[2],sizeof(pid[2]));
362 
363  // The following instructions load the parameters for the speed PI
364  // controller.
365  PID_setGains(pidHandle[0],_IQ(1.0),_IQ(0.01),_IQ(0.0));
366 
367  // The current limit is performed by the limits placed on the speed PI
368  // controller output. In the following statement, the speed
369  // controller's largest negative current is set to -maxCurrent_pu, and
370  // the largest positive current is set to maxCurrent_pu.
371  PID_setMinMax(pidHandle[0],-maxCurrent_pu,maxCurrent_pu);
372  PID_setUi(pidHandle[0],_IQ(0.0)); // Set the initial condition value
373  // for the integrator output to 0
374 
375  pidCntSpeed = 0; // Set the counter for decimating the speed
376  // controller to 0
377 
378  // The following instructions load the parameters for the d-axis
379  // current controller.
380  // P term = Kp_Id, I term = Ki_Id, D term = 0
381  PID_setGains(pidHandle[1],Kp_Id,Ki_Id,_IQ(0.0));
382 
383  // Largest negative voltage = -maxVoltage_pu, largest positive
384  // voltage = maxVoltage_pu
385  PID_setMinMax(pidHandle[1],-maxVoltage_pu,maxVoltage_pu);
386 
387  // Set the initial condition value for the integrator output to 0
388  PID_setUi(pidHandle[1],_IQ(0.0));
389 
390  // The following instructions load the parameters for the q-axis
391  // current controller.
392  // P term = Kp_Iq, I term = Ki_Iq, D term = 0
393  PID_setGains(pidHandle[2],Kp_Iq,Ki_Iq,_IQ(0.0));
394 
395  // The largest negative voltage = 0 and the largest positive
396  // voltage = 0. But these limits are updated every single ISR before
397  // actually executing the Iq controller. The limits depend on how much
398  // voltage is left over after the Id controller executes. So having an
399  // initial value of 0 does not affect Iq current controller execution.
400  PID_setMinMax(pidHandle[2],_IQ(0.0),_IQ(0.0));
401 
402  // Set the initial condition value for the integrator output to 0
403  PID_setUi(pidHandle[2],_IQ(0.0));
404  }
405 
406  // initialize the speed reference in kilo RPM where base speed is
407  // USER_IQ_FULL_SCALE_FREQ_Hz.
408  // Set 10 Hz electrical frequency as initial value, so the kRPM value would
409  // be: 10 * 60 / motor pole pairs / 1000.
410  gMotorVars.SpeedRef_krpm = _IQmpy(_IQ(10.0),gSpeed_hz_to_krpm_sf);
411 
412  // initialize the inverse Park module
413  iparkHandle = IPARK_init(&ipark,sizeof(ipark));
414 
415  // initialize the space vector generator module
416  svgenHandle = SVGEN_init(&svgen,sizeof(svgen));
417 
418  // setup faults
419  HAL_setupFaults(halHandle);
420 
421  // initialize the interrupt vector table
422  HAL_initIntVectorTable(halHandle);
423 
424  // enable the ADC interrupts
425  HAL_enableAdcInts(halHandle);
426 
427  // enable global interrupts
428  HAL_enableGlobalInts(halHandle);
429 
430  // enable debug interrupts
431  HAL_enableDebugInt(halHandle);
432 
433  // disable the PWM
434  HAL_disablePwm(halHandle);
435 
436  // compute scaling factors for flux and torque calculations
441 
442  // enable the system by default
443  gMotorVars.Flag_enableSys = true;
444 
445  #ifdef DRV8301_SPI
446  // turn on the DRV8301 if present
447  HAL_enableDrv(halHandle);
448  // initialize the DRV8301 interface
449  HAL_setupDrvSpi(halHandle,&gDrvSpi8301Vars);
450  #endif
451 
452  #ifdef DRV8305_SPI
453  // turn on the DRV8305 if present
454  HAL_enableDrv(halHandle);
455  // initialize the DRV8305 interface
456  HAL_setupDrvSpi(halHandle,&gDrvSpi8305Vars);
457  #endif
458 
459  // Begin the background loop
460  for(;;)
461  {
462  // Waiting for enable system flag to be set
463  while(!(gMotorVars.Flag_enableSys));
464 
465  // loop while the enable system flag is true
466  while(gMotorVars.Flag_enableSys)
467  {
468  // If Flag_enableSys is set AND Flag_Run_Identify is set THEN
469  // enable PWMs and set the speed reference
470  if(gMotorVars.Flag_Run_Identify)
471  {
472  // update estimator state
474 
475  #ifdef FAST_ROM_V1p6
476  // call this function to fix 1p6. This is only used for
477  // F2806xF/M implementation of InstaSPIN (version 1.6 of
478  // ROM), since the inductance calculation is not done
479  // correctly in ROM, so this function fixes that ROM bug.
481  #endif
482 
483  // enable the PWM
484  HAL_enablePwm(halHandle);
485  }
486  else // Flag_enableSys is set AND Flag_Run_Identify is not set
487  {
488  // set estimator to Idle
490 
491  // disable the PWM
492  HAL_disablePwm(halHandle);
493 
494  // clear integrator outputs
495  PID_setUi(pidHandle[0],_IQ(0.0));
496  PID_setUi(pidHandle[1],_IQ(0.0));
497  PID_setUi(pidHandle[2],_IQ(0.0));
498 
499  // clear Id and Iq references
500  gIdq_ref_pu.value[0] = _IQ(0.0);
501  gIdq_ref_pu.value[1] = _IQ(0.0);
502  }
503 
504  // update the global variables
506 
507  // enable/disable the forced angle
509  gMotorVars.Flag_enableForceAngle);
510 
511  // set target speed
512  gMotorVars.SpeedRef_pu = _IQmpy(gMotorVars.SpeedRef_krpm,
514 
515  #ifdef DRV8301_SPI
516  HAL_writeDrvData(halHandle,&gDrvSpi8301Vars);
517 
518  HAL_readDrvData(halHandle,&gDrvSpi8301Vars);
519  #endif
520  #ifdef DRV8305_SPI
521  HAL_writeDrvData(halHandle,&gDrvSpi8305Vars);
522 
523  HAL_readDrvData(halHandle,&gDrvSpi8305Vars);
524  #endif
525 
526  } // end of while(gFlag_enableSys) loop
527 
528  // disable the PWM
529  HAL_disablePwm(halHandle);
530 
531  gMotorVars.Flag_Run_Identify = false;
532  } // end of for(;;) loop
533 } // end of main() function
534 
535 
537 interrupt void mainISR(void)
538 {
539  // Declaration of local variables
540  _iq angle_pu = _IQ(0.0);
541  _iq speed_pu = _IQ(0.0);
542  _iq oneOverDcBus;
543  MATH_vec2 Iab_pu;
544  MATH_vec2 Vab_pu;
545  MATH_vec2 phasor;
546 
547  // acknowledge the ADC interrupt
548  HAL_acqAdcInt(halHandle,ADC_IntNumber_1);
549 
550  // convert the ADC data
551  HAL_readAdcDataWithOffsets(halHandle,&gAdcData);
552 
553  // remove offsets
554  gAdcData.I.value[0] = gAdcData.I.value[0] - gOffsets_I_pu.value[0];
555  gAdcData.I.value[1] = gAdcData.I.value[1] - gOffsets_I_pu.value[1];
556  gAdcData.I.value[2] = gAdcData.I.value[2] - gOffsets_I_pu.value[2];
557  gAdcData.V.value[0] = gAdcData.V.value[0] - gOffsets_V_pu.value[0];
558  gAdcData.V.value[1] = gAdcData.V.value[1] - gOffsets_V_pu.value[1];
559  gAdcData.V.value[2] = gAdcData.V.value[2] - gOffsets_V_pu.value[2];
560 
561  // run Clarke transform on current. Three values are passed, two values
562  // are returned.
563  CLARKE_run(clarkeHandle_I,&gAdcData.I,&Iab_pu);
564 
565  // run Clarke transform on voltage. Three values are passed, two values
566  // are returned.
567  CLARKE_run(clarkeHandle_V,&gAdcData.V,&Vab_pu);
568 
569  // run the estimator
570  // The speed reference is needed so that the proper sign of the forced
571  // angle is calculated. When the estimator does not do motor ID as in this
572  // lab, only the sign of the speed reference is used
573  EST_run(estHandle,&Iab_pu,&Vab_pu,gAdcData.dcBus,gMotorVars.SpeedRef_pu);
574 
575  // generate the motor electrical angle
576  angle_pu = EST_getAngle_pu(estHandle);
577  speed_pu = EST_getFm_pu(estHandle);
578 
579  // get Idq from estimator to avoid sin and cos, and a Park transform,
580  // which saves CPU cycles
581  EST_getIdq_pu(estHandle,&gIdq_pu);
582 
583  // run the appropriate controller
584  if(gMotorVars.Flag_Run_Identify)
585  {
586  // Declaration of local variables.
587  _iq refValue;
588  _iq fbackValue;
589  _iq outMax_pu;
590 
591  // when appropriate, run the PID speed controller
592  // This mechanism provides the decimation for the speed loop.
594  {
595  // Reset the Speed PID execution counter.
596  pidCntSpeed = 0;
597 
598  // The next instruction executes the PI speed controller and places
599  // its output in Idq_ref_pu.value[1], which is the input reference
600  // value for the q-axis current controller.
601  PID_run_spd(pidHandle[0],gMotorVars.SpeedRef_pu,speed_pu,
602  &(gIdq_ref_pu.value[1]));
603  }
604  else
605  {
606  // increment counter
607  pidCntSpeed++;
608  }
609 
610  // Get the reference value for the d-axis current controller.
611  refValue = gIdq_ref_pu.value[0];
612 
613  // Get the actual value of Id
614  fbackValue = gIdq_pu.value[0];
615 
616  // The next instruction executes the PI current controller for the
617  // d axis and places its output in Vdq_pu.value[0], which is the
618  // control voltage along the d-axis (Vd)
619  PID_run(pidHandle[1],refValue,fbackValue,&(gVdq_out_pu.value[0]));
620 
621  // get the Iq reference value
622  refValue = gIdq_ref_pu.value[1];
623 
624  // get the actual value of Iq
625  fbackValue = gIdq_pu.value[1];
626 
627  // The voltage limits on the output of the q-axis current controller
628  // are dynamic, and are dependent on the output voltage from the d-axis
629  // current controller. In other words, the d-axis current controller
630  // gets first dibs on the available voltage, and the q-axis current
631  // controller gets what's left over. That is why the d-axis current
632  // controller executes first. The next instruction calculates the
633  // maximum limits for this voltage as:
634  // Vq_min_max = +/- sqrt(Vbus^2 - Vd^2)
636  - _IQmpy(gVdq_out_pu.value[0],gVdq_out_pu.value[0]));
637 
638  // Set the limits to +/- outMax_pu
639  PID_setMinMax(pidHandle[2],-outMax_pu,outMax_pu);
640 
641  // The next instruction executes the PI current controller for the
642  // q axis and places its output in Vdq_pu.value[1], which is the
643  // control voltage vector along the q-axis (Vq)
644  PID_run(pidHandle[2],refValue,fbackValue,&(gVdq_out_pu.value[1]));
645 
646  // The voltage vector is now calculated and ready to be applied to the
647  // motor in the form of three PWM signals. However, even though the
648  // voltages may be supplied to the PWM module now, they won't be
649  // applied to the motor until the next PWM cycle. By this point, the
650  // motor will have moved away from the angle that the voltage vector
651  // was calculated for, by an amount which is proportional to the
652  // sampling frequency and the speed of the motor. For steady-state
653  // speeds, we can calculate this angle delay and compensate for it.
654  angle_pu = angleDelayComp(speed_pu,angle_pu);
655 
656  // compute the sine and cosine phasor values which are part of the inverse
657  // Park transform calculations. Once these values are computed,
658  // they are copied into the IPARK module, which then uses them to
659  // transform the voltages from DQ to Alpha/Beta reference frames.
660  phasor.value[0] = _IQcosPU(angle_pu);
661  phasor.value[1] = _IQsinPU(angle_pu);
662 
663  // set the phasor in the inverse Park transform
664  IPARK_setPhasor(iparkHandle,&phasor);
665 
666  // Run the inverse Park module. This converts the voltage vector from
667  // synchronous frame values to stationary frame values.
668  IPARK_run(iparkHandle,&gVdq_out_pu,&Vab_pu);
669 
670  // These 3 statements compensate for variations in the DC bus by adjusting the
671  // PWM duty cycle. The goal is to achieve the same volt-second product
672  // regardless of the DC bus value. To do this, we must divide the desired voltage
673  // values by the DC bus value. Or...it is easier to multiply by 1/(DC bus value).
674  oneOverDcBus = EST_getOneOverDcBus_pu(estHandle);
675  Vab_pu.value[0] = _IQmpy(Vab_pu.value[0],oneOverDcBus);
676  Vab_pu.value[1] = _IQmpy(Vab_pu.value[1],oneOverDcBus);
677 
678  // Now run the space vector generator (SVGEN) module.
679  // There is no need to do an inverse CLARKE transform, as this is
680  // handled in the SVGEN_run function.
681  SVGEN_run(svgenHandle,&Vab_pu,&(gPwmData.Tabc));
682  }
683  else // gMotorVars.Flag_Run_Identify = 0
684  {
685  // disable the PWM
686  HAL_disablePwm(halHandle);
687 
688  // Set the PWMs to 50% duty cycle
689  gPwmData.Tabc.value[0] = _IQ(0.0);
690  gPwmData.Tabc.value[1] = _IQ(0.0);
691  gPwmData.Tabc.value[2] = _IQ(0.0);
692  }
693 
694  // write to the PWM compare registers, and then we are done!
695  HAL_writePwmData(halHandle,&gPwmData);
696 
697  return;
698 } // end of mainISR() function
699 
700 
704 _iq angleDelayComp(const _iq fm_pu,const _iq angleUncomp_pu)
705 {
706  _iq angleDelta_pu = _IQmpy(fm_pu,_IQ(USER_IQ_FULL_SCALE_FREQ_Hz
707  / (USER_PWM_FREQ_kHz*1000.0)));
708  _iq angleCompFactor = _IQ(1.0 + (float_t)USER_NUM_PWM_TICKS_PER_ISR_TICK
709  * 0.5);
710  _iq angleDeltaComp_pu = _IQmpy(angleDelta_pu,angleCompFactor);
711  uint32_t angleMask = ((uint32_t)0xFFFFFFFF >> (32 - GLOBAL_Q));
712  _iq angleComp_pu;
713  _iq angleTmp_pu;
714 
715  // increment the angle
716  angleTmp_pu = angleUncomp_pu + angleDeltaComp_pu;
717 
718  // mask the angle for wrap around
719  // note: must account for the sign of the angle
720  angleComp_pu = _IQabs(angleTmp_pu) & angleMask;
721 
722  // account for sign
723  if(angleTmp_pu < _IQ(0.0))
724  {
725  angleComp_pu = -angleComp_pu;
726  }
727 
728  return(angleComp_pu);
729 } // end of angleDelayComp() function
730 
731 
737 {
738  float_t fullScaleInductance = USER_IQ_FULL_SCALE_VOLTAGE_V
741  float_t Ls_coarse_max = _IQ30toF(EST_getLs_coarse_max_pu(handle));
742  int_least8_t lShift = ceil(log(USER_MOTOR_Ls_d / (Ls_coarse_max
743  * fullScaleInductance)) / log(2.0));
744  uint_least8_t Ls_qFmt = 30 - lShift;
745  float_t L_max = fullScaleInductance * pow(2.0,lShift);
746  _iq Ls_d_pu = _IQ30(USER_MOTOR_Ls_d / L_max);
747  _iq Ls_q_pu = _IQ30(USER_MOTOR_Ls_q / L_max);
748 
749  // store the results
750  EST_setLs_d_pu(handle,Ls_d_pu);
751  EST_setLs_q_pu(handle,Ls_q_pu);
752  EST_setLs_qFmt(handle,Ls_qFmt);
753 
754  return;
755 } // end of softwareUpdate1p6() function
756 
757 
761 void setupClarke_I(CLARKE_Handle handle,const uint_least8_t numCurrentSensors)
762 {
763  _iq alpha_sf,beta_sf;
764 
765  // initialize the Clarke transform module for current
766  if(numCurrentSensors == 3)
767  {
768  alpha_sf = _IQ(MATH_ONE_OVER_THREE);
769  beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
770  }
771  else if(numCurrentSensors == 2)
772  {
773  alpha_sf = _IQ(1.0);
774  beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
775  }
776  else
777  {
778  alpha_sf = _IQ(0.0);
779  beta_sf = _IQ(0.0);
780  }
781 
782  // set the parameters
783  CLARKE_setScaleFactors(handle,alpha_sf,beta_sf);
784  CLARKE_setNumSensors(handle,numCurrentSensors);
785 
786  return;
787 } // end of setupClarke_I() function
788 
789 
793 void setupClarke_V(CLARKE_Handle handle,const uint_least8_t numVoltageSensors)
794 {
795  _iq alpha_sf,beta_sf;
796 
797  // initialize the Clarke transform module for voltage
798  if(numVoltageSensors == 3)
799  {
800  alpha_sf = _IQ(MATH_ONE_OVER_THREE);
801  beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
802  }
803  else
804  {
805  alpha_sf = _IQ(0.0);
806  beta_sf = _IQ(0.0);
807  }
808 
809  // In other words, the only acceptable number of voltage sensors is three.
810  // set the parameters
811  CLARKE_setScaleFactors(handle,alpha_sf,beta_sf);
812  CLARKE_setNumSensors(handle,numVoltageSensors);
813 
814  return;
815 } // end of setupClarke_V() function
816 
817 
821 {
822  // get the speed estimate
823  gMotorVars.Speed_krpm = EST_getSpeed_krpm(handle);
824 
825  // get the torque estimate
826  {
827  _iq Flux_pu = EST_getFlux_pu(handle);
828  _iq Id_pu = PID_getFbackValue(pidHandle[1]);
829  _iq Iq_pu = PID_getFbackValue(pidHandle[2]);
830  _iq Ld_minus_Lq_pu = _IQ30toIQ(EST_getLs_d_pu(handle)
831  - EST_getLs_q_pu(handle));
832 
833  // Reactance Torque
834  _iq Torque_Flux_Iq_Nm = _IQmpy(_IQmpy(Flux_pu,Iq_pu),
836 
837  // Reluctance Torque
838  _iq Torque_Ls_Id_Iq_Nm = _IQmpy(_IQmpy(_IQmpy(Ld_minus_Lq_pu,Id_pu),
840 
841  // Total torque is sum of reactance torque and reluctance torque
842  _iq Torque_Nm = Torque_Flux_Iq_Nm + Torque_Ls_Id_Iq_Nm;
843 
844  gMotorVars.Torque_Nm = Torque_Nm;
845  }
846 
847  // get the magnetizing current
848  gMotorVars.MagnCurr_A = EST_getIdRated(handle);
849 
850  // get the rotor resistance
851  gMotorVars.Rr_Ohm = EST_getRr_Ohm(handle);
852 
853  // get the stator resistance
854  gMotorVars.Rs_Ohm = EST_getRs_Ohm(handle);
855 
856  // get the stator inductance in the direct coordinate direction
857  gMotorVars.Lsd_H = EST_getLs_d_H(handle);
858 
859  // get the stator inductance in the quadrature coordinate direction
860  gMotorVars.Lsq_H = EST_getLs_q_H(handle);
861 
862  // get the flux in V/Hz in floating point
863  gMotorVars.Flux_VpHz = EST_getFlux_VpHz(handle);
864 
865  // get the flux in Wb in fixed point
866  gMotorVars.Flux_Wb = _IQmpy(EST_getFlux_pu(handle),gFlux_pu_to_Wb_sf);
867 
868  // get the estimator state
869  gMotorVars.EstState = EST_getState(handle);
870 
871  // Get the DC buss voltage
872  gMotorVars.VdcBus_kV = _IQmpy(gAdcData.dcBus,
874 
875  // read Vd and Vq vectors per units
876  gMotorVars.Vd = gVdq_out_pu.value[0];
877  gMotorVars.Vq = gVdq_out_pu.value[1];
878 
879  // calculate vector Vs in per units: (Vs = sqrt(Vd^2 + Vq^2))
880  gMotorVars.Vs = _IQsqrt(_IQmpy(gMotorVars.Vd,gMotorVars.Vd)
881  + _IQmpy(gMotorVars.Vq,gMotorVars.Vq));
882 
883  // read Id and Iq vectors in amps
884  gMotorVars.Id_A = _IQmpy(gIdq_pu.value[0],
886  gMotorVars.Iq_A = _IQmpy(gIdq_pu.value[1],
888 
889  // calculate vector Is in amps: (Is_A = sqrt(Id_A^2 + Iq_A^2))
890  gMotorVars.Is_A = _IQsqrt(_IQmpy(gMotorVars.Id_A,gMotorVars.Id_A)
891  + _IQmpy(gMotorVars.Iq_A,gMotorVars.Iq_A));
892 
893  return;
894 } // end of updateGlobalVariables() function
895 
896 
898 // end of file
899 
900 
float_t EST_getFlux_VpHz(EST_Handle handle)
#define USER_NUM_CURRENT_SENSORS
Defines the number of current sensors used.
Definition: user.h:116
void EST_setFlag_enableForceAngle(EST_Handle handle, const bool state)
IPARK_Handle IPARK_init(void *pMemory, const size_t numBytes)
_iq EST_getLs_coarse_max_pu(EST_Handle handle)
void updateGlobalVariables(EST_Handle handle)
Update the global variables (gMotorVars).
Definition: proj_lab11.c:820
#define _IQsinPU(A)
void HAL_enableGlobalInts(HAL_Handle handle)
void HAL_enableAdcInts(HAL_Handle handle)
USER_ErrorCode_e UserErrorCode
Definition: main.h:150
float_t Lsq_H
Definition: main.h:172
#define USER_MOTOR_Ls_d
Definition: user.h:401
static void HAL_readAdcDataWithOffsets(HAL_Handle handle, HAL_AdcData_t *pAdcData)
_iq USER_computeFlux_pu_to_VpHz_sf(void)
Computes the scale factor needed to convert from per unit to V/Hz.
interrupt void mainISR(void)
The main ISR that implements the motor control.
Definition: proj_lab11.c:537
#define _IQ(A)
_iq Iq_A
Definition: main.h:195
static void PID_setUi(PID_Handle handle, const _iq Ui)
bool Flag_Run_Identify
Definition: main.h:137
_iq EST_getSpeed_krpm(EST_Handle handle)
USER_Params gUserParams
The user parameters.
Definition: proj_lab11.c:120
void EST_setupEstIdleState(EST_Handle handle)
HAL_AdcData_t gAdcData
Defines the ADC data.
Definition: proj_lab11.c:99
_iq gTorque_Ls_Id_Iq_pu_to_Nm_sf
Definition: proj_lab11.c:152
IPARK_Obj ipark
the inverse Park transform object
Definition: proj_lab11.c:84
static void HAL_writePwmData(HAL_Handle handle, HAL_PwmData_t *pPwmData)
_iq value[3]
struct _EST_Obj_ * EST_Handle
void EST_setIdle(EST_Handle handle)
void HAL_writeDrvData(HAL_Handle handle, DRV_SPI_8301_Vars_t *Spi_8301_Vars)
#define USER_VD_SF
Defines the direct voltage (Vd) scale factor.
Definition: user.h:165
void setupClarke_V(CLARKE_Handle handle, const uint_least8_t numVoltageSensors)
Setup the Clarke transform for either 2 or 3 sensors.
Definition: proj_lab11.c:793
PID_Obj pid[3]
Definition: proj_lab11.c:75
static void HAL_acqAdcInt(HAL_Handle handle, const ADC_IntNumber_e intNumber)
#define USER_IQ_FULL_SCALE_VOLTAGE_V
Defines full scale value for the IQ30 variable of Voltage inside the system.
Definition: user.h:88
#define _IQcosPU(A)
void softwareUpdate1p6(EST_Handle handle)
Call this function to fix 1p6. This is only used for F2806xF/M.
Definition: proj_lab11.c:736
#define USER_MOTOR_NUM_POLE_PAIRS
Definition: user.h:398
void memCopy(uint16_t *srcStartAddr, uint16_t *srcEndAddr, uint16_t *dstAddr)
EST_Handle estHandle
bool Flag_enableForceAngle
Definition: main.h:139
#define USER_PWM_FREQ_kHz
Defines the Pulse Width Modulation (PWM) frequency, kHz.
Definition: user.h:147
EST_Handle estHandle
the handle for the estimator
Definition: proj_lab11.c:73
static void PID_setMinMax(PID_Handle handle, const _iq outMin, const _iq outMax)
static void SVGEN_run(SVGEN_Handle handle, const MATH_vec2 *pVab, MATH_vec3 *pT)
HAL_PwmData_t gPwmData
Defines the PWM data.
Definition: proj_lab11.c:95
static _iq PID_getFbackValue(PID_Handle handle)
#define V_B_offset
Definition: user.h:133
long _iq
#define MATH_ONE_OVER_THREE
void USER_setParams(USER_Params *pUserParams)
Sets the user parameter values.
void EST_setLs_qFmt(EST_Handle handle, const uint_least8_t Ls_qFmt)
_iq gSpeed_krpm_to_pu_sf
Definition: proj_lab11.c:156
EST_State_e EstState
Definition: main.h:148
CLARKE_Obj clarke_I
the current Clarke transform object
Definition: proj_lab11.c:67
float_t Rs_Ohm
Definition: main.h:169
#define V_A_offset
ADC voltage offsets for A, B, and C phases.
Definition: user.h:132
CLARKE_Handle clarkeHandle_V
Definition: proj_lab11.c:69
_iq EST_getLs_d_pu(EST_Handle handle)
#define I_A_offset
ADC current offsets for A, B, and C phases.
Definition: user.h:125
static void CLARKE_setScaleFactors(CLARKE_Handle handle, const _iq alpha_sf, const _iq beta_sf)
Defines the structures, global initialization, and functions used in MAIN.
#define USER_MOTOR_Rs
Definition: user.h:400
CTRL_Handle CTRL_init(void *pMemory, const size_t numBytes)
void HAL_setupFaults(HAL_Handle handle)
EST_Handle EST_init(void *pMemory, const size_t numBytes)
float_t EST_getLs_d_H(EST_Handle handle)
void EST_setFlag_enableRsRecalc(EST_Handle handle, const bool state)
MATH_vec3 gOffsets_I_pu
the offsets for the current feedback
Definition: proj_lab11.c:102
MATH_vec3 Tabc
static void PID_setGains(PID_Handle handle, const _iq Kp, const _iq Ki, const _iq Kd)
static void HAL_enablePwm(HAL_Handle handle)
#define USER_MAX_VS_MAG_PU
Defines the maximum Voltage vector (Vs) magnitude allowed. This value sets the maximum magnitude for ...
Definition: user.h:157
void CTRL_setParams(CTRL_Handle handle, USER_Params *pUserParams)
#define _IQmpy(A, B)
USER_ErrorCode_e USER_getErrorCode(USER_Params *pUserParams)
Gets the error code in the user parameters.
void HAL_enableDrv(HAL_Handle handle)
_iq value[2]
_iq VdcBus_kV
Definition: main.h:192
_iq Torque_Nm
Definition: main.h:165
HAL_Handle halHandle
The hal handle.
Definition: proj_lab11.c:92
_iq SpeedRef_krpm
Definition: main.h:157
_iq angleDelayComp(const _iq fm_pu, const _iq angleUncomp_pu)
The angleDelayComp function compensates for the delay introduced.
Definition: proj_lab11.c:704
_iq EST_getOneOverDcBus_pu(EST_Handle handle)
_iq EST_getFlux_pu(EST_Handle handle)
void HAL_setupDrvSpi(HAL_Handle handle, DRV_SPI_8301_Vars_t *Spi_8301_Vars)
float_t EST_getLs_q_H(EST_Handle handle)
#define USER_EST_HANDLE_ADDRESS
Defines the address of estimator handle.
Definition: user.h:161
void HAL_enableDebugInt(HAL_Handle handle)
void CTRL_setupEstIdleState(CTRL_Handle handle)
#define USER_NUM_CTRL_TICKS_PER_SPEED_TICK
Defines the number of controller clock ticks per speed controller clock tick.
Definition: user.h:206
CLARKE_Handle CLARKE_init(void *pMemory, const size_t numBytes)
volatile MOTOR_Vars_t gMotorVars
Definition: proj_lab11.c:122
void CTRL_setUserMotorParams(CTRL_Handle handle)
#define I_C_offset
Definition: user.h:127
float_t Flux_VpHz
Definition: main.h:173
#define _IQ30toIQ(A)
static void IPARK_run(IPARK_Handle handle, const MATH_vec2 *pInVec, MATH_vec2 *pOutVec)
EST_State_e EST_getState(EST_Handle handle)
SVGEN_Handle svgenHandle
the handle for the space vector generator
Definition: proj_lab11.c:86
#define I_B_offset
Definition: user.h:126
CLARKE_Handle clarkeHandle_I
Definition: proj_lab11.c:65
_iq EST_getFm_pu(EST_Handle handle)
#define MOTOR_Vars_INIT
Initialization values of global variables.
Definition: main.h:76
#define _IQabs(A)
#define V_C_offset
Definition: user.h:134
_iq SpeedRef_pu
Definition: main.h:156
_iq USER_computeTorque_Flux_Iq_pu_to_Nm_sf(void)
Computes the scale factor needed to convert from torque created by flux and Iq, from per unit to Nm...
_iq Speed_krpm
Definition: main.h:160
float_t EST_getIdRated(EST_Handle handle)
_iq gFlux_pu_to_Wb_sf
Definition: proj_lab11.c:148
_iq Is_A
Definition: main.h:196
_iq EST_getLs_q_pu(EST_Handle handle)
static void CLARKE_setNumSensors(CLARKE_Handle handle, const uint_least8_t numSensors)
float_t Rr_Ohm
Definition: main.h:168
float_t EST_getRs_Ohm(EST_Handle handle)
float_t Lsd_H
Definition: main.h:171
#define _IQ30(A)
#define USER_IQ_FULL_SCALE_FREQ_Hz
CURRENTS AND VOLTAGES.
Definition: user.h:78
MATH_vec2 gIdq_pu
measured values
Definition: proj_lab11.c:114
SVGEN_Obj svgen
the space vector generator object
Definition: proj_lab11.c:87
#define USER_ISR_FREQ_Hz
Defines the Interrupt Service Routine (ISR) frequency, Hz.
Definition: user.h:173
static void IPARK_setPhasor(IPARK_Handle handle, const MATH_vec2 *pPhasor)
MATH_vec2 gIdq_ref_pu
Iq references.
Definition: proj_lab11.c:108
_iq EST_getAngle_pu(EST_Handle handle)
static void HAL_disablePwm(HAL_Handle handle)
MATH_vec3 gOffsets_V_pu
the offsets for the voltage feedback
Definition: proj_lab11.c:105
void main(void)
Definition: proj_lab11.c:164
PID_Handle PID_init(void *pMemory, const size_t numBytes)
HAL_Handle HAL_init(void *pMemory, const size_t numBytes)
uint16_t pidCntSpeed
Definition: proj_lab11.c:79
static void CLARKE_run(CLARKE_Handle handle, const MATH_vec3 *pInVec, MATH_vec2 *pOutVec)
_iq gTorque_Flux_Iq_pu_to_Nm_sf
Definition: proj_lab11.c:154
#define _IQsqrt(A)
MATH_vec2 gVdq_out_pu
Vd and Vq from the current controllers.
Definition: proj_lab11.c:111
#define USER_VOLTAGE_FILTER_POLE_rps
Defines the analog voltage filter pole location, rad/s.
Definition: user.h:305
CLARKE_Obj clarke_V
the voltage Clarke transform object
Definition: proj_lab11.c:71
#define USER_NUM_VOLTAGE_SENSORS
Defines the number of voltage (phase) sensors.
Definition: user.h:120
void EST_setLs_q_pu(EST_Handle handle, const _iq Ls_q_pu)
#define USER_IQ_FULL_SCALE_CURRENT_A
Defines the full scale current for the IQ variables, A.
Definition: user.h:102
float_t MagnCurr_A
Definition: main.h:167
HAL_Obj hal
_iq gSpeed_hz_to_krpm_sf
Definition: proj_lab11.c:159
static void PID_run_spd(PID_Handle handle, const _iq refValue, const _iq fbackValue, _iq *pOutValue)
CTRL_Handle ctrlHandle
The controller handle.
bool Flag_enableSys
Definition: main.h:136
_iq Id_A
Definition: main.h:194
#define MATH_ONE_OVER_SQRT_THREE
static void HAL_initIntVectorTable(HAL_Handle handle)
#define USER_MOTOR_Ls_q
Definition: user.h:402
bool EST_updateState(EST_Handle handle, const _iq Id_target_pu)
float_t EST_getRr_Ohm(EST_Handle handle)
void HAL_setParams(HAL_Handle handle, const USER_Params *pUserParams)
void HAL_readDrvData(HAL_Handle handle, DRV_SPI_8301_Vars_t *Spi_8301_Vars)
#define USER_MOTOR_MAX_CURRENT
Definition: user.h:407
void setupClarke_I(CLARKE_Handle handle, const uint_least8_t numCurrentSensors)
Setup the Clarke transform for either 2 or 3 sensors.
Definition: proj_lab11.c:761
void EST_getIdq_pu(EST_Handle handle, MATH_vec2 *pIdq_pu)
float _IQ30toF(long A)
void USER_checkForErrors(USER_Params *pUserParams)
Checks for errors in the user parameter values.
_iq gFlux_pu_to_VpHz_sf
Definition: proj_lab11.c:150
SVGEN_Handle SVGEN_init(void *pMemory, const size_t numBytes)
void EST_setEstParams(EST_Handle handle, USER_Params *pUserParams)
_iq Flux_Wb
Definition: main.h:164
#define GLOBAL_Q
_iq USER_computeTorque_Ls_Id_Iq_pu_to_Nm_sf(void)
Computes the scale factor needed to convert from torque created by Ld, Lq, Id and Iq...
PID_Handle pidHandle[3]
Definition: proj_lab11.c:77
_iq USER_computeFlux_pu_to_Wb_sf(void)
Computes the scale factor needed to convert from per unit to Wb.
IPARK_Handle iparkHandle
Definition: proj_lab11.c:82
void EST_setLs_d_pu(EST_Handle handle, const _iq Ls_d_pu)
float float_t
static void PID_run(PID_Handle handle, const _iq refValue, const _iq fbackValue, _iq *pOutValue)
void EST_run(EST_Handle handle, const MATH_vec2 *pIab_pu, const MATH_vec2 *pVab_pu, const _iq dcBus_pu, const _iq speed_ref_pu)
#define USER_NUM_PWM_TICKS_PER_ISR_TICK
DECIMATION.
Definition: user.h:184