This thread has been locked.

If you have a related question, please click the "Ask a related question" button in the top right corner. The newly created question will be automatically linked to this question.

How to use PWMDAC module in Motorware?

Other Parts Discussed in Thread: MOTORWARE, CONTROLSUITE, DRV8312, DRV8301

Hello,

I want to use PWMDAC module in Motorware in order to monitor internal signal values in my project, such as: svgen, or ramp signals, by an external oscilloscope.

In ControlSuite, I simply use PWM_MACRO in the main_ISR() as following:

// ------------------------------------------------------------------------------
//    Connect inputs of the PWMDAC module 
// ------------------------------------------------------------------------------	
	pwmdac1.MfuncC1 = svgen1.Ta; 
    pwmdac1.MfuncC2 = svgen1.Tb; 
    PWMDAC_MACRO(6,pwmdac1)	  						// PWMDAC 6A, 6B
    
    pwmdac1.MfuncC1 = svgen1.Tc; 
    pwmdac1.MfuncC2 = svgen1.Tb-svgen1.Tc; 
	PWMDAC_MACRO(7,pwmdac1)		  				// PWMDAC 7A, 7B  

In my project, I am using PWM6A, 6B and PWM7A, 7B as a debugging pins when connecting them to an external oscilloscope. And now I want the same function in Motorware system. I know that the Motorware provides the following settings:

#ifdef DEBUG
  // initialize PWM DAC handles
  obj->pwmDacHandle[0] = PWMDAC_init((void *)PWM_ePWM6_BASE_ADDR,sizeof(PWM_Obj)); //Using 6A
  obj->pwmDacHandle[1] = PWMDAC_init((void *)PWM_ePWM7_BASE_ADDR,sizeof(PWM_Obj)); //Using 7A
#endif //DEBUG

and call the setting function for the parameters of this module:

#ifdef DEBUG
  // setup the PWM DACs
  HAL_setupPwmDacs(handle);
#endif

and the content of the setupPwmDacs() function as following:

void HAL_setupPwmDacs(HAL_Handle handle)
{
  HAL_Obj *obj = (HAL_Obj *)handle;
  uint16_t halfPeriod_cycles = 512;       // 3000->10kHz, 1500->20kHz, 1000-> 30kHz, 500->60kHz
  uint_least8_t    cnt;


  for(cnt=0;cnt<3;cnt++)
    {
      // initialize the Time-Base Control Register (TBCTL)
      PWMDAC_setCounterMode(obj->pwmDacHandle[cnt],PWM_CounterMode_UpDown);
      PWMDAC_disableCounterLoad(obj->pwmDacHandle[cnt]);
      PWMDAC_setPeriodLoad(obj->pwmDacHandle[cnt],PWM_PeriodLoad_Immediate);
      PWMDAC_setSyncMode(obj->pwmDacHandle[cnt],PWM_SyncMode_EPWMxSYNC);
      PWMDAC_setHighSpeedClkDiv(obj->pwmDacHandle[cnt],PWM_HspClkDiv_by_1);
      PWMDAC_setClkDiv(obj->pwmDacHandle[cnt],PWM_ClkDiv_by_1);
      PWMDAC_setPhaseDir(obj->pwmDacHandle[cnt],PWM_PhaseDir_CountUp);
      PWMDAC_setRunMode(obj->pwmDacHandle[cnt],PWM_RunMode_FreeRun);

      // initialize the Timer-Based Phase Register (TBPHS)
      PWMDAC_setPhase(obj->pwmDacHandle[cnt],0);

      // setup the Time-Base Counter Register (TBCTR)
      PWMDAC_setCount(obj->pwmDacHandle[cnt],0);

      // Initialize the Time-Base Period Register (TBPRD)
      // set to zero initially
      PWMDAC_setPeriod(obj->pwmDacHandle[cnt],0);

      // initialize the Counter-Compare Control Register (CMPCTL)
      PWMDAC_setLoadMode_CmpA(obj->pwmDacHandle[cnt],PWM_LoadMode_Zero);
      PWMDAC_setLoadMode_CmpB(obj->pwmDacHandle[cnt],PWM_LoadMode_Zero);
      PWMDAC_setShadowMode_CmpA(obj->pwmDacHandle[cnt],PWM_ShadowMode_Shadow);
      PWMDAC_setShadowMode_CmpB(obj->pwmDacHandle[cnt],PWM_ShadowMode_Shadow);

      // Initialize the Action-Qualifier Output A Register (AQCTLA) 
      PWMDAC_setActionQual_CntUp_CmpA_PwmA(obj->pwmDacHandle[cnt],PWM_ActionQual_Clear);
      PWMDAC_setActionQual_CntDown_CmpA_PwmA(obj->pwmDacHandle[cnt],PWM_ActionQual_Set);

      // account for EPWM6B
      if(cnt == 0)
        {
          PWMDAC_setActionQual_CntUp_CmpB_PwmB(obj->pwmDacHandle[cnt],PWM_ActionQual_Clear);
          PWMDAC_setActionQual_CntDown_CmpB_PwmB(obj->pwmDacHandle[cnt],PWM_ActionQual_Set);
        }

      // Initialize the Dead-Band Control Register (DBCTL) 
      PWMDAC_disableDeadBand(obj->pwmDacHandle[cnt]);

      // Initialize the PWM-Chopper Control Register (PCCTL)
      PWMDAC_disableChopping(obj->pwmDacHandle[cnt]);

      // Initialize the Trip-Zone Control Register (TZSEL)
      PWMDAC_disableTripZones(obj->pwmDacHandle[cnt]);

      // Initialize the Trip-Zone Control Register (TZCTL)
      PWMDAC_setTripZoneState_TZA(obj->pwmDacHandle[cnt],PWM_TripZoneState_HighImp);
      PWMDAC_setTripZoneState_TZB(obj->pwmDacHandle[cnt],PWM_TripZoneState_HighImp);
      PWMDAC_setTripZoneState_DCAEVT1(obj->pwmDacHandle[cnt],PWM_TripZoneState_HighImp);
      PWMDAC_setTripZoneState_DCAEVT2(obj->pwmDacHandle[cnt],PWM_TripZoneState_HighImp);
      PWMDAC_setTripZoneState_DCBEVT1(obj->pwmDacHandle[cnt],PWM_TripZoneState_HighImp);
    }

  // since the PWM is configured as an up/down counter, the period register is set to one-half 
  // of the desired PWM period
  PWMDAC_setPeriod(obj->pwmDacHandle[PWMDAC_Number_1],halfPeriod_cycles);
  PWMDAC_setPeriod(obj->pwmDacHandle[PWMDAC_Number_2],halfPeriod_cycles);
  PWMDAC_setPeriod(obj->pwmDacHandle[PWMDAC_Number_3],halfPeriod_cycles);

  return;
}  // end of HAL_setupPwmDacs() function

The questions are:

1. How to use this module in the main_ISR() function, for example, to update an internal signal in to PWM6A, 6B and PWM7A, 7B?

2. When I see the HAL_setupPwmDacs(), three channels was setup. But in my project, there were only two channels (PWM6 and PWM7), so how will this setup effect to the third channel?

3. How can I nominalize the signals before writing them to this module?

Many thanks in advance,

Tran

  • Hi,

    I just updated three files to make this work on a DRV8312 kit with 6x device running lab 3a. Find the files attached:

    2570.proj_lab03a.c
    /* --COPYRIGHT--,BSD
     * Copyright (c) 2012, Texas Instruments Incorporated
     * All rights reserved.
     *
     * Redistribution and use in source and binary forms, with or without
     * modification, are permitted provided that the following conditions
     * are met:
     *
     * *  Redistributions of source code must retain the above copyright
     *    notice, this list of conditions and the following disclaimer.
     *
     * *  Redistributions in binary form must reproduce the above copyright
     *    notice, this list of conditions and the following disclaimer in the
     *    documentation and/or other materials provided with the distribution.
     *
     * *  Neither the name of Texas Instruments Incorporated nor the names of
     *    its contributors may be used to endorse or promote products derived
     *    from this software without specific prior written permission.
     *
     * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
     * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
     * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
     * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
     * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
     * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
     * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
     * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
     * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
     * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
     * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
     * --/COPYRIGHT--*/
    //! \file   solutions/instaspin_foc/src/proj_lab03a.c
    //! \brief Using your own motor parameters from user.h, skipping Motor ID 
    //!
    //! (C) Copyright 2011, Texas Instruments, Inc.
    
    //! \defgroup PROJ_LAB03a PROJ_LAB03a
    //@{
    
    //! \defgroup PROJ_LAB03a_OVERVIEW Project Overview
    //!
    //! Saving your motor parameters and loading from user.h
    //!
    
    // **************************************************************************
    // the includes
    
    // system includes
    #include <math.h>
    #include "main.h"
    
    #ifdef FLASH
    #pragma CODE_SECTION(mainISR,"ramfuncs");
    #endif
    
    // Include header files used in the main function
    
    
    // **************************************************************************
    // the defines
    
    #define LED_BLINK_FREQ_Hz   5
    
    
    // **************************************************************************
    // the globals
    
    uint_least16_t gCounter_updateGlobals = 0;
    
    bool Flag_Latch_softwareUpdate = true;
    
    CTRL_Handle ctrlHandle;
    
    #ifdef F2802xF
    #pragma DATA_SECTION(halHandle,"rom_accessed_data");
    #endif
    HAL_Handle halHandle;
    
    #ifdef F2802xF
    #pragma DATA_SECTION(gUserParams,"rom_accessed_data");
    #endif
    USER_Params gUserParams;
    
    HAL_PwmData_t gPwmData = {_IQ(0.0), _IQ(0.0), _IQ(0.0)};
    
    HAL_DacData_t gDacData = {_IQ(0.0), _IQ(0.0), _IQ(0.0), _IQ(0.0)};
    
    HAL_AdcData_t gAdcData;
    
    _iq gMaxCurrentSlope = _IQ(0.0);
    
    #ifdef FAST_ROM_V1p6
    CTRL_Obj *controller_obj;
    #else
    #ifdef F2802xF
    #pragma DATA_SECTION(ctrl,"rom_accessed_data");
    #endif
    CTRL_Obj ctrl;				//v1p7 format
    #endif
    
    uint16_t gLEDcnt = 0;
    
    volatile MOTOR_Vars_t gMotorVars = MOTOR_Vars_INIT;
    
    #ifdef FLASH
    // Used for running BackGround in flash, and ISR in RAM
    extern uint16_t *RamfuncsLoadStart, *RamfuncsLoadEnd, *RamfuncsRunStart;
    
    #ifdef F2802xF
    extern uint16_t *econst_start, *econst_end, *econst_ram_load;
    extern uint16_t *switch_start, *switch_end, *switch_ram_load;
    #endif
    
    #endif
    
    
    #ifdef DRV8301_SPI
    // Watch window interface to the 8301 SPI
    DRV_SPI_8301_Vars_t gDrvSpi8301Vars;
    #endif
    
    #ifdef DRV8305_SPI
    // Watch window interface to the 8305 SPI
    DRV_SPI_8305_Vars_t gDrvSpi8305Vars;
    #endif
    
    _iq gFlux_pu_to_Wb_sf;
    
    _iq gFlux_pu_to_VpHz_sf;
    
    _iq gTorque_Ls_Id_Iq_pu_to_Nm_sf;
    
    _iq gTorque_Flux_Iq_pu_to_Nm_sf;
    
    // **************************************************************************
    // the functions
    
    void main(void)
    {
      uint_least8_t estNumber = 0;
    
    #ifdef FAST_ROM_V1p6
      uint_least8_t ctrlNumber = 0;
    #endif
    
      // Only used if running from FLASH
      // Note that the variable FLASH is defined by the project
      #ifdef FLASH
      // Copy time critical code and Flash setup code to RAM
      // The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
      // symbols are created by the linker. Refer to the linker files.
      memCopy((uint16_t *)&RamfuncsLoadStart,(uint16_t *)&RamfuncsLoadEnd,(uint16_t *)&RamfuncsRunStart);
    
      #ifdef F2802xF
        //copy .econst to unsecure RAM
        if(*econst_end - *econst_start)
          {
            memCopy((uint16_t *)&econst_start,(uint16_t *)&econst_end,(uint16_t *)&econst_ram_load);
          }
    
        //copy .switch ot unsecure RAM
        if(*switch_end - *switch_start)
          {
            memCopy((uint16_t *)&switch_start,(uint16_t *)&switch_end,(uint16_t *)&switch_ram_load);
          }
      #endif
    
      #endif
    
      // initialize the hardware abstraction layer
      halHandle = HAL_init(&hal,sizeof(hal));
    
    
      // check for errors in user parameters
      USER_checkForErrors(&gUserParams);
    
    
      // store user parameter error in global variable
      gMotorVars.UserErrorCode = USER_getErrorCode(&gUserParams);
    
    
      // do not allow code execution if there is a user parameter error
      if(gMotorVars.UserErrorCode != USER_ErrorCode_NoError)
        {
          for(;;)
            {
              gMotorVars.Flag_enableSys = false;
            }
        }
    
    
      // initialize the user parameters
      USER_setParams(&gUserParams);
    
    
      // set the hardware abstraction layer parameters
      HAL_setParams(halHandle,&gUserParams);
    
    
      // initialize the controller
    #ifdef FAST_ROM_V1p6
      ctrlHandle = CTRL_initCtrl(ctrlNumber, estNumber);  		//v1p6 format (06xF and 06xM devices)
      controller_obj = (CTRL_Obj *)ctrlHandle;
    #else
      ctrlHandle = CTRL_initCtrl(estNumber,&ctrl,sizeof(ctrl));	//v1p7 format default
    #endif
    
    
      {
        CTRL_Version version;
    
        // get the version number
        CTRL_getVersion(ctrlHandle,&version);
    
        gMotorVars.CtrlVersion = version;
      }
    
    
      // set the default controller parameters
      CTRL_setParams(ctrlHandle,&gUserParams);
    
    
      // setup faults
      HAL_setupFaults(halHandle);
    
    
      // initialize the interrupt vector table
      HAL_initIntVectorTable(halHandle);
    
    
      // enable the ADC interrupts
      HAL_enableAdcInts(halHandle);
    
    
      // enable global interrupts
      HAL_enableGlobalInts(halHandle);
    
    
      // enable debug interrupts
      HAL_enableDebugInt(halHandle);
    
    
      // disable the PWM
      HAL_disablePwm(halHandle);
    
    
    #ifdef DRV8301_SPI
      // turn on the DRV8301 if present
      HAL_enableDrv(halHandle);
      // initialize the DRV8301 interface
      HAL_setupDrvSpi(halHandle,&gDrvSpi8301Vars);
    #endif
    
    #ifdef DRV8305_SPI
      // turn on the DRV8305 if present
      HAL_enableDrv(halHandle);
      // initialize the DRV8305 interface
      HAL_setupDrvSpi(halHandle,&gDrvSpi8305Vars);
    #endif
    
      // enable DC bus compensation
      CTRL_setFlag_enableDcBusComp(ctrlHandle, true);
    
    
      // compute scaling factors for flux and torque calculations
      gFlux_pu_to_Wb_sf = USER_computeFlux_pu_to_Wb_sf();
      gFlux_pu_to_VpHz_sf = USER_computeFlux_pu_to_VpHz_sf();
      gTorque_Ls_Id_Iq_pu_to_Nm_sf = USER_computeTorque_Ls_Id_Iq_pu_to_Nm_sf();
      gTorque_Flux_Iq_pu_to_Nm_sf = USER_computeTorque_Flux_Iq_pu_to_Nm_sf();
    
    
      for(;;)
      {
        // Waiting for enable system flag to be set
        while(!(gMotorVars.Flag_enableSys));
    
        // loop while the enable system flag is true
        while(gMotorVars.Flag_enableSys)
          {
            CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
    
            // increment counters
            gCounter_updateGlobals++;
    
            // enable/disable the use of motor parameters being loaded from user.h
            CTRL_setFlag_enableUserMotorParams(ctrlHandle,gMotorVars.Flag_enableUserParams);
    
    
            if(CTRL_isError(ctrlHandle))
              {
                // set the enable controller flag to false
                CTRL_setFlag_enableCtrl(ctrlHandle,false);
    
                // set the enable system flag to false
                gMotorVars.Flag_enableSys = false;
    
                // disable the PWM
                HAL_disablePwm(halHandle);
              }
            else
              {
                // update the controller state
                bool flag_ctrlStateChanged = CTRL_updateState(ctrlHandle);
    
                // enable or disable the control
                CTRL_setFlag_enableCtrl(ctrlHandle, gMotorVars.Flag_Run_Identify);
    
                if(flag_ctrlStateChanged)
                  {
                    CTRL_State_e ctrlState = CTRL_getState(ctrlHandle);
    
                    if(ctrlState == CTRL_State_OffLine)
                      {
                        // enable the PWM
                        HAL_enablePwm(halHandle);
                      }
                    else if(ctrlState == CTRL_State_OnLine)
                      {
                        // update the ADC bias values
                        HAL_updateAdcBias(halHandle);
    
                        // Return the bias value for currents
                        gMotorVars.I_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Current,0);
                        gMotorVars.I_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Current,1);
                        gMotorVars.I_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Current,2);
    
                        // Return the bias value for voltages
                        gMotorVars.V_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Voltage,0);
                        gMotorVars.V_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Voltage,1);
                        gMotorVars.V_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Voltage,2);
    
                        // enable the PWM
                        HAL_enablePwm(halHandle);
                      }
                    else if(ctrlState == CTRL_State_Idle)
                      {
                        // disable the PWM
                        HAL_disablePwm(halHandle);
                        gMotorVars.Flag_Run_Identify = false;
                      }
    
                    if((CTRL_getFlag_enableUserMotorParams(ctrlHandle) == true) &&
                      (ctrlState > CTRL_State_Idle) &&
                      (gMotorVars.CtrlVersion.minor == 6))
                      {
                        // call this function to fix 1p6
                        USER_softwareUpdate1p6(ctrlHandle);
                      }
    
                  }
              }
    
    
            if(EST_isMotorIdentified(obj->estHandle))
              {
                // set the current ramp
                EST_setMaxCurrentSlope_pu(obj->estHandle,gMaxCurrentSlope);
                gMotorVars.Flag_MotorIdentified = true;
    
                // set the speed reference
                CTRL_setSpd_ref_krpm(ctrlHandle,gMotorVars.SpeedRef_krpm);
    
                // set the speed acceleration
                CTRL_setMaxAccel_pu(ctrlHandle,_IQmpy(MAX_ACCEL_KRPMPS_SF,gMotorVars.MaxAccel_krpmps));
    
                if(Flag_Latch_softwareUpdate)
                {
                  Flag_Latch_softwareUpdate = false;
    
                  USER_calcPIgains(ctrlHandle);
                }
    
              }
            else
              {
                Flag_Latch_softwareUpdate = true;
    
                // the estimator sets the maximum current slope during identification
                gMaxCurrentSlope = EST_getMaxCurrentSlope_pu(obj->estHandle);
              }
    
    
            // when appropriate, update the global variables
            if(gCounter_updateGlobals >= NUM_MAIN_TICKS_FOR_GLOBAL_VARIABLE_UPDATE)
              {
                // reset the counter
                gCounter_updateGlobals = 0;
    
                updateGlobalVariables_motor(ctrlHandle);
              }
    
            // enable/disable the forced angle
            EST_setFlag_enableForceAngle(obj->estHandle,gMotorVars.Flag_enableForceAngle);
    
            // enable or disable power warp
            CTRL_setFlag_enablePowerWarp(ctrlHandle,gMotorVars.Flag_enablePowerWarp);
    
    #ifdef DRV8301_SPI
            HAL_writeDrvData(halHandle,&gDrvSpi8301Vars);
    
            HAL_readDrvData(halHandle,&gDrvSpi8301Vars);
    #endif
    #ifdef DRV8305_SPI
            HAL_writeDrvData(halHandle,&gDrvSpi8305Vars);
    
            HAL_readDrvData(halHandle,&gDrvSpi8305Vars);
    #endif
          } // end of while(gFlag_enableSys) loop
    
    
        // disable the PWM
        HAL_disablePwm(halHandle);
    
        // set the default controller parameters (Reset the control to re-identify the motor)
        CTRL_setParams(ctrlHandle,&gUserParams);
        gMotorVars.Flag_Run_Identify = false;
    
      } // end of for(;;) loop
    
    } // end of main() function
    
    
    interrupt void mainISR(void)
    {
      // toggle status LED
      if(gLEDcnt++ > (uint_least32_t)(USER_ISR_FREQ_Hz / LED_BLINK_FREQ_Hz))
      {
        HAL_toggleLed(halHandle,(GPIO_Number_e)HAL_Gpio_LED2);
        gLEDcnt = 0;
      }
    
    
      // acknowledge the ADC interrupt
      HAL_acqAdcInt(halHandle,ADC_IntNumber_1);
    
    
      // convert the ADC data
      HAL_readAdcData(halHandle,&gAdcData);
    
    
      // run the controller
      CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData);
    
    
      // write the PWM compare values
      HAL_writePwmData(halHandle,&gPwmData);
    
      gDacData.value[0] = gPwmData.Tabc.value[0];
      gDacData.value[1] = gPwmData.Tabc.value[1];
      gDacData.value[2] = gPwmData.Tabc.value[2];
    
      // write the PWM compare values for DAC
      HAL_writeDacData(halHandle,&gDacData);
    
      // setup the controller
      CTRL_setup(ctrlHandle);
    
    
      return;
    } // end of mainISR() function
    
    
    void updateGlobalVariables_motor(CTRL_Handle handle)
    {
      CTRL_Obj *obj = (CTRL_Obj *)handle;
    
      // get the speed estimate
      gMotorVars.Speed_krpm = EST_getSpeed_krpm(obj->estHandle);
    
      // get the real time speed reference coming out of the speed trajectory generator
      gMotorVars.SpeedTraj_krpm = _IQmpy(CTRL_getSpd_int_ref_pu(handle),EST_get_pu_to_krpm_sf(obj->estHandle));
    
      // get the torque estimate
      gMotorVars.Torque_Nm = USER_computeTorque_Nm(handle, gTorque_Flux_Iq_pu_to_Nm_sf, gTorque_Ls_Id_Iq_pu_to_Nm_sf);
    
      // get the magnetizing current
      gMotorVars.MagnCurr_A = EST_getIdRated(obj->estHandle);
    
      // get the rotor resistance
      gMotorVars.Rr_Ohm = EST_getRr_Ohm(obj->estHandle);
    
      // get the stator resistance
      gMotorVars.Rs_Ohm = EST_getRs_Ohm(obj->estHandle);
    
      // get the stator inductance in the direct coordinate direction
      gMotorVars.Lsd_H = EST_getLs_d_H(obj->estHandle);
    
      // get the stator inductance in the quadrature coordinate direction
      gMotorVars.Lsq_H = EST_getLs_q_H(obj->estHandle);
    
      // get the flux in V/Hz in floating point
      gMotorVars.Flux_VpHz = EST_getFlux_VpHz(obj->estHandle);
    
      // get the flux in Wb in fixed point
      gMotorVars.Flux_Wb = USER_computeFlux(handle, gFlux_pu_to_Wb_sf);
    
      // get the controller state
      gMotorVars.CtrlState = CTRL_getState(handle);
    
      // get the estimator state
      gMotorVars.EstState = EST_getState(obj->estHandle);
    
      // Get the DC buss voltage
      gMotorVars.VdcBus_kV = _IQmpy(gAdcData.dcBus,_IQ(USER_IQ_FULL_SCALE_VOLTAGE_V/1000.0));
    
      return;
    } // end of updateGlobalVariables_motor() function
    
    
    //@} //defgroup
    // end of file
    
    
    
    

    6303.hal.c
    /* --COPYRIGHT--,BSD
     * Copyright (c) 2012, Texas Instruments Incorporated
     * All rights reserved.
     *
     * Redistribution and use in source and binary forms, with or without
     * modification, are permitted provided that the following conditions
     * are met:
     *
     * *  Redistributions of source code must retain the above copyright
     *    notice, this list of conditions and the following disclaimer.
     *
     * *  Redistributions in binary form must reproduce the above copyright
     *    notice, this list of conditions and the following disclaimer in the
     *    documentation and/or other materials provided with the distribution.
     *
     * *  Neither the name of Texas Instruments Incorporated nor the names of
     *    its contributors may be used to endorse or promote products derived
     *    from this software without specific prior written permission.
     *
     * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
     * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
     * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
     * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
     * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
     * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
     * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
     * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
     * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
     * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
     * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
     * --/COPYRIGHT--*/
    //! \file   solutions/instaspin_foc/boards/drv8312kit_revD/f28x/f2806xF/src/hal.c
    //! \brief Contains the various functions related to the HAL object (everything outside the CTRL system) 
    //!
    //! (C) Copyright 2011, Texas Instruments, Inc.
    
    
    // **************************************************************************
    // the includes
    
    // drivers
    
    // modules
    
    // platforms
    #include "hal.h"
    #include "user.h"
    #include "hal_obj.h"
    
    #ifdef FLASH
    #pragma CODE_SECTION(HAL_setupFlash,"ramfuncs");
    #endif
    
    // **************************************************************************
    // the defines
    
    
    // **************************************************************************
    // the globals
    
    HAL_Obj hal;
    
    
    // **************************************************************************
    // the functions
    
    void HAL_cal(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      // enable the ADC clock
      CLK_enableAdcClock(obj->clkHandle);
    
    
      // Run the Device_cal() function
      // This function copies the ADC and oscillator calibration values from TI reserved
      // OTP into the appropriate trim registers
      // This boot ROM automatically calls this function to calibrate the interal 
      // oscillators and ADC with device specific calibration data.
      // If the boot ROM is bypassed by Code Composer Studio during the development process,
      // then the calibration must be initialized by the application
      ENABLE_PROTECTED_REGISTER_WRITE_MODE;
      (*Device_cal)();
      DISABLE_PROTECTED_REGISTER_WRITE_MODE;
    
      // run offsets calibration in user's memory
      HAL_AdcOffsetSelfCal(handle);
    
      // run oscillator compensation
      HAL_OscTempComp(handle);
    
      // disable the ADC clock
      CLK_disableAdcClock(obj->clkHandle);
    
      return;
    } // end of HAL_cal() function
    
    
    void HAL_OscTempComp(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
      uint16_t Temperature;
    
      // disable the ADCs
      ADC_disable(obj->adcHandle);
    
      // power up the bandgap circuit
      ADC_enableBandGap(obj->adcHandle);
    
      // set the ADC voltage reference source to internal
      ADC_setVoltRefSrc(obj->adcHandle,ADC_VoltageRefSrc_Int);
    
      // enable the ADC reference buffers
      ADC_enableRefBuffers(obj->adcHandle);
    
      // Set main clock scaling factor (max45MHz clock for the ADC module)
      ADC_setDivideSelect(obj->adcHandle,ADC_DivideSelect_ClkIn_by_2);
    
      // power up the ADCs
      ADC_powerUp(obj->adcHandle);
    
      // enable the ADCs
      ADC_enable(obj->adcHandle);
    
      // enable non-overlap mode
      ADC_enableNoOverlapMode(obj->adcHandle);
    
      // connect channel A5 internally to the temperature sensor
      ADC_setTempSensorSrc(obj->adcHandle, ADC_TempSensorSrc_Int);
    
      // set SOC0 channel select to ADCINA5
      ADC_setSocChanNumber(obj->adcHandle, ADC_SocNumber_0, ADC_SocChanNumber_A5);
    
      // set SOC0 acquisition period to 26 ADCCLK
      ADC_setSocSampleDelay(obj->adcHandle, ADC_SocNumber_0, ADC_SocSampleDelay_64_cycles);
    
      // connect ADCINT1 to EOC0
      ADC_setIntSrc(obj->adcHandle, ADC_IntNumber_1, ADC_IntSrc_EOC0);
    
      // clear ADCINT1 flag
      ADC_clearIntFlag(obj->adcHandle, ADC_IntNumber_1);
    
      // enable ADCINT1
      ADC_enableInt(obj->adcHandle, ADC_IntNumber_1);
    
      // force start of conversion on SOC0
      ADC_setSocFrc(obj->adcHandle, ADC_SocFrc_0);
    
      // wait for end of conversion
      while (ADC_getIntFlag(obj->adcHandle, ADC_IntNumber_1) == 0){}
    
      // clear ADCINT1 flag
      ADC_clearIntFlag(obj->adcHandle, ADC_IntNumber_1);
    
      Temperature = ADC_readResult(obj->adcHandle, ADC_ResultNumber_0);
    
      HAL_osc1Comp(handle, Temperature);
    
      HAL_osc2Comp(handle, Temperature);
    
      return;
    } // end of HAL_OscTempComp() function
    
    
    void HAL_osc1Comp(HAL_Handle handle, const int16_t sensorSample)
    {
    	int16_t compOscFineTrim;
    	HAL_Obj *obj = (HAL_Obj *)handle;
    
    	ENABLE_PROTECTED_REGISTER_WRITE_MODE;
    
        compOscFineTrim = ((sensorSample - getRefTempOffset())*(int32_t)getOsc1FineTrimSlope()
                          + OSC_POSTRIM_OFF + FP_ROUND )/FP_SCALE + getOsc1FineTrimOffset() - OSC_POSTRIM;
    
        if(compOscFineTrim > 31)
          {
            compOscFineTrim = 31;
          }
    	else if(compOscFineTrim < -31)
          {
            compOscFineTrim = -31;
          }
    
        OSC_setTrim(obj->oscHandle, OSC_Number_1, HAL_getOscTrimValue(getOsc1CoarseTrim(), compOscFineTrim));
    
        DISABLE_PROTECTED_REGISTER_WRITE_MODE;
    
        return;
    } // end of HAL_osc1Comp() function
    
    
    void HAL_osc2Comp(HAL_Handle handle, const int16_t sensorSample)
    {
    	int16_t compOscFineTrim;
    	HAL_Obj *obj = (HAL_Obj *)handle;
    
    	ENABLE_PROTECTED_REGISTER_WRITE_MODE;
    
        compOscFineTrim = ((sensorSample - getRefTempOffset())*(int32_t)getOsc2FineTrimSlope()
                          + OSC_POSTRIM_OFF + FP_ROUND )/FP_SCALE + getOsc2FineTrimOffset() - OSC_POSTRIM;
    
        if(compOscFineTrim > 31)
          {
            compOscFineTrim = 31;
          }
    	else if(compOscFineTrim < -31)
          {
            compOscFineTrim = -31;
          }
    
        OSC_setTrim(obj->oscHandle, OSC_Number_2, HAL_getOscTrimValue(getOsc2CoarseTrim(), compOscFineTrim));
    
        DISABLE_PROTECTED_REGISTER_WRITE_MODE;
    
        return;
    } // end of HAL_osc2Comp() function
    
    
    uint16_t HAL_getOscTrimValue(int16_t coarse, int16_t fine)
    {
      uint16_t regValue = 0;
    
      if(fine < 0)
        {
          regValue = ((-fine) | 0x20) << 9;
        }
      else
        {
          regValue = fine << 9;
        }
    
      if(coarse < 0)
        {
          regValue |= ((-coarse) | 0x80);
        }
      else
        {
          regValue |= coarse;
        }
    
      return regValue;
    } // end of HAL_getOscTrimValue() function
    
    
    void HAL_AdcOffsetSelfCal(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
      uint16_t AdcConvMean;
    
      // disable the ADCs
      ADC_disable(obj->adcHandle);
    
      // power up the bandgap circuit
      ADC_enableBandGap(obj->adcHandle);
    
      // set the ADC voltage reference source to internal
      ADC_setVoltRefSrc(obj->adcHandle,ADC_VoltageRefSrc_Int);
    
      // enable the ADC reference buffers
      ADC_enableRefBuffers(obj->adcHandle);
    
      // Set main clock scaling factor (max45MHz clock for the ADC module)
      ADC_setDivideSelect(obj->adcHandle,ADC_DivideSelect_ClkIn_by_2);
    
      // power up the ADCs
      ADC_powerUp(obj->adcHandle);
    
      // enable the ADCs
      ADC_enable(obj->adcHandle);
    
      //Select VREFLO internal connection on B5
      ADC_enableVoltRefLoConv(obj->adcHandle);
    
      //Select channel B5 for all SOC
      HAL_AdcCalChanSelect(handle, ADC_SocChanNumber_B5);
    
      //Apply artificial offset (+80) to account for a negative offset that may reside in the ADC core
      ADC_setOffTrim(obj->adcHandle, 80);
    
      //Capture ADC conversion on VREFLO
      AdcConvMean = HAL_AdcCalConversion(handle);
    
      //Set offtrim register with new value (i.e remove artical offset (+80) and create a two's compliment of the offset error)
      ADC_setOffTrim(obj->adcHandle, 80 - AdcConvMean);
    
      //Select external ADCIN5 input pin on B5
      ADC_disableVoltRefLoConv(obj->adcHandle);
    
      return;
    } // end of HAL_AdcOffsetSelfCal() function
    
    
    void HAL_AdcCalChanSelect(HAL_Handle handle, const ADC_SocChanNumber_e chanNumber)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_0,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_1,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_2,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_3,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_4,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_5,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_6,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_7,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_8,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_9,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_10,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_11,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_12,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_13,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_14,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_15,chanNumber);
    
      return;
    } // end of HAL_AdcCalChanSelect() function
    
    
    uint16_t HAL_AdcCalConversion(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
      uint16_t index, SampleSize, Mean;
      uint32_t Sum;
      ADC_SocSampleDelay_e ACQPS_Value;
    
      index       = 0;     //initialize index to 0
      SampleSize  = 256;   //set sample size to 256 (**NOTE: Sample size must be multiples of 2^x where is an integer >= 4)
      Sum         = 0;     //set sum to 0
      Mean        = 999;   //initialize mean to known value
    
      //Set the ADC sample window to the desired value (Sample window = ACQPS + 1)
      ACQPS_Value = ADC_SocSampleDelay_7_cycles;
    
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_0,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_1,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_2,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_3,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_4,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_5,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_6,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_7,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_8,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_9,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_10,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_11,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_12,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_13,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_14,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_15,ACQPS_Value);
    
      // Enabled ADCINT1 and ADCINT2
      ADC_enableInt(obj->adcHandle, ADC_IntNumber_1);
      ADC_enableInt(obj->adcHandle, ADC_IntNumber_2);
    
      // Disable continuous sampling for ADCINT1 and ADCINT2
      ADC_setIntMode(obj->adcHandle, ADC_IntNumber_1, ADC_IntMode_EOC);
      ADC_setIntMode(obj->adcHandle, ADC_IntNumber_2, ADC_IntMode_EOC);
    
      //ADCINTs trigger at end of conversion
      ADC_setIntPulseGenMode(obj->adcHandle, ADC_IntPulseGenMode_Prior);
    
      // Setup ADCINT1 and ADCINT2 trigger source
      ADC_setIntSrc(obj->adcHandle, ADC_IntNumber_1, ADC_IntSrc_EOC6);
      ADC_setIntSrc(obj->adcHandle, ADC_IntNumber_2, ADC_IntSrc_EOC14);
    
      // Setup each SOC's ADCINT trigger source
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_0, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_1, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_2, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_3, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_4, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_5, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_6, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_7, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_8, ADC_Int1TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_9, ADC_Int1TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_10, ADC_Int1TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_11, ADC_Int1TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_12, ADC_Int1TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_13, ADC_Int1TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_14, ADC_Int1TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_15, ADC_Int1TriggersSOC);
    
      // Delay before converting ADC channels
      usDelay(ADC_DELAY_usec);
    
      ADC_setSocFrcWord(obj->adcHandle, 0x00FF);
    
      while( index < SampleSize )
        {
          //Wait for ADCINT1 to trigger, then add ADCRESULT0-7 registers to sum
          while (ADC_getIntFlag(obj->adcHandle, ADC_IntNumber_1) == 0){}
    
          //Must clear ADCINT1 flag since INT1CONT = 0
          ADC_clearIntFlag(obj->adcHandle, ADC_IntNumber_1);
    
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_0);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_1);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_2);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_3);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_4);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_5);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_6);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_7);
    
          //Wait for ADCINT2 to trigger, then add ADCRESULT8-15 registers to sum
          while (ADC_getIntFlag(obj->adcHandle, ADC_IntNumber_2) == 0){}
    
          //Must clear ADCINT2 flag since INT2CONT = 0
          ADC_clearIntFlag(obj->adcHandle, ADC_IntNumber_2);
    
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_8);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_9);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_10);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_11);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_12);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_13);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_14);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_15);
    
          index+=16;
    
      } // end data collection
    
      //Disable ADCINT1 and ADCINT2 to STOP the ping-pong sampling
      ADC_disableInt(obj->adcHandle, ADC_IntNumber_1);
      ADC_disableInt(obj->adcHandle, ADC_IntNumber_2);
    
      //Calculate average ADC sample value
      Mean = Sum / SampleSize;
    
      // Clear start of conversion trigger
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_0, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_1, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_2, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_3, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_4, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_5, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_6, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_7, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_8, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_9, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_10, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_11, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_12, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_13, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_14, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_15, ADC_NoIntTriggersSOC);
    
      //return the average
      return(Mean);
    } // end of HAL_AdcCalConversion() function
    
    
    void HAL_disableWdog(HAL_Handle halHandle)
    {
      HAL_Obj *hal = (HAL_Obj *)halHandle;
    
    
      WDOG_disable(hal->wdogHandle);
    
    
      return;
    } // end of HAL_disableWdog() function
    
    
    void HAL_disableGlobalInts(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      CPU_disableGlobalInts(obj->cpuHandle);
    
      return;
    } // end of HAL_disableGlobalInts() function
    
    
    void HAL_enableAdcInts(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      // enable the PIE interrupts associated with the ADC interrupts
      PIE_enableAdcInt(obj->pieHandle,ADC_IntNumber_1);
    
    
      // enable the ADC interrupts
      ADC_enableInt(obj->adcHandle,ADC_IntNumber_1);
    
    
      // enable the cpu interrupt for ADC interrupts
      CPU_enableInt(obj->cpuHandle,CPU_IntNumber_10);
    
      return;
    } // end of HAL_enableAdcInts() function
    
    
    void HAL_enableDebugInt(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      CPU_enableDebugInt(obj->cpuHandle);
    
      return;
    } // end of HAL_enableDebugInt() function
    
    
    void HAL_enableGlobalInts(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      CPU_enableGlobalInts(obj->cpuHandle);
    
      return;
    } // end of HAL_enableGlobalInts() function
    
    
    void HAL_enablePwmInt(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      PIE_enablePwmInt(obj->pieHandle,PWM_Number_1);
    
    
      // enable the interrupt
      PWM_enableInt(obj->pwmHandle[PWM_Number_1]);
    
    
      // enable the cpu interrupt for EPWM1_INT
      CPU_enableInt(obj->cpuHandle,CPU_IntNumber_3);
    
      return;
    } // end of HAL_enablePwmInt() function
    
    
    void HAL_setupFaults(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
      uint_least8_t cnt;
    
    
      // Configure Trip Mechanism for the Motor control software
      // -Cycle by cycle trip on CPU halt
      // -One shot fault trip zone
      // These trips need to be repeated for EPWM1 ,2 & 3
      for(cnt=0;cnt<3;cnt++)
        {
          PWM_enableTripZoneSrc(obj->pwmHandle[cnt],PWM_TripZoneSrc_CycleByCycle_TZ6_NOT);
    
          PWM_enableTripZoneSrc(obj->pwmHandle[cnt],PWM_TripZoneSrc_OneShot_TZ3_NOT);
    
          PWM_enableTripZoneSrc(obj->pwmHandle[cnt],PWM_TripZoneSrc_OneShot_TZ2_NOT);
    
          // What do we want the OST/CBC events to do?
          // TZA events can force EPWMxA
          // TZB events can force EPWMxB
    
          PWM_setTripZoneState_TZA(obj->pwmHandle[cnt],PWM_TripZoneState_EPWM_Low);
          PWM_setTripZoneState_TZB(obj->pwmHandle[cnt],PWM_TripZoneState_EPWM_Low);
        }
    
      return;
    } // end of HAL_setupFaults() function
    
    
    HAL_Handle HAL_init(void *pMemory,const size_t numBytes)
    {
      uint_least8_t cnt;
      HAL_Handle handle;
      HAL_Obj *obj;
    
    
      if(numBytes < sizeof(HAL_Obj))
        return((HAL_Handle)NULL);
    
    
      // assign the handle
      handle = (HAL_Handle)pMemory;
    
    
      // assign the object
      obj = (HAL_Obj *)handle;
    
    
      // initialize the watchdog driver
      obj->wdogHandle = WDOG_init((void *)WDOG_BASE_ADDR,sizeof(WDOG_Obj));
    
    
      // disable watchdog
      HAL_disableWdog(handle);
    
    
      // initialize the ADC
      obj->adcHandle = ADC_init((void *)ADC_BASE_ADDR,sizeof(ADC_Obj));
    
    
      // initialize the clock handle
      obj->clkHandle = CLK_init((void *)CLK_BASE_ADDR,sizeof(CLK_Obj));
    
    
      // initialize the CPU handle
      obj->cpuHandle = CPU_init(&cpu,sizeof(cpu));
    
    
      // initialize the FLASH handle
      obj->flashHandle = FLASH_init((void *)FLASH_BASE_ADDR,sizeof(FLASH_Obj));
    
    
      // initialize the GPIO handle
      obj->gpioHandle = GPIO_init((void *)GPIO_BASE_ADDR,sizeof(GPIO_Obj));
    
    
      // initialize the current offset estimator handles
      for(cnt=0;cnt<USER_NUM_CURRENT_SENSORS;cnt++)
        {
          obj->offsetHandle_I[cnt] = OFFSET_init(&obj->offset_I[cnt],sizeof(obj->offset_I[cnt]));
        }
    
    
      // initialize the voltage offset estimator handles
      for(cnt=0;cnt<USER_NUM_VOLTAGE_SENSORS;cnt++)
        {
          obj->offsetHandle_V[cnt] = OFFSET_init(&obj->offset_V[cnt],sizeof(obj->offset_V[cnt]));
        }
    
    
      // initialize the oscillator handle
      obj->oscHandle = OSC_init((void *)OSC_BASE_ADDR,sizeof(OSC_Obj));
    
    
      // initialize the PIE handle
      obj->pieHandle = PIE_init((void *)PIE_BASE_ADDR,sizeof(PIE_Obj));
    
    
      // initialize the PLL handle
      obj->pllHandle = PLL_init((void *)PLL_BASE_ADDR,sizeof(PLL_Obj));
    
    
      // initialize the SPIA handle
      obj->spiAHandle = SPI_init((void *)SPIA_BASE_ADDR,sizeof(SPI_Obj));
    
    
    
      // initialize PWM handles
      obj->pwmHandle[0] = PWM_init((void *)PWM_ePWM1_BASE_ADDR,sizeof(PWM_Obj));
      obj->pwmHandle[1] = PWM_init((void *)PWM_ePWM2_BASE_ADDR,sizeof(PWM_Obj));
      obj->pwmHandle[2] = PWM_init((void *)PWM_ePWM3_BASE_ADDR,sizeof(PWM_Obj));
    
    
      // initialize PWM DAC handles
      obj->pwmDacHandle[0] = PWMDAC_init((void *)PWM_ePWM6_BASE_ADDR,sizeof(PWM_Obj));
      obj->pwmDacHandle[1] = PWMDAC_init((void *)PWM_ePWM5_BASE_ADDR,sizeof(PWM_Obj));
      obj->pwmDacHandle[2] = PWMDAC_init((void *)PWM_ePWM4_BASE_ADDR,sizeof(PWM_Obj));
    
    
      // initialize power handle
      obj->pwrHandle = PWR_init((void *)PWR_BASE_ADDR,sizeof(PWR_Obj));
    
    
      // initialize timer handles
      obj->timerHandle[0] = TIMER_init((void *)TIMER0_BASE_ADDR,sizeof(TIMER_Obj));
      obj->timerHandle[1] = TIMER_init((void *)TIMER1_BASE_ADDR,sizeof(TIMER_Obj));
      obj->timerHandle[2] = TIMER_init((void *)TIMER2_BASE_ADDR,sizeof(TIMER_Obj));
    
    #ifdef QEP
      // initialize QEP driver
      obj->qepHandle[0] = QEP_init((void*)QEP1_BASE_ADDR,sizeof(QEP_Obj));
    #endif
    
      return(handle);
    } // end of HAL_init() function
    
    
    void HAL_setParams(HAL_Handle handle,const USER_Params *pUserParams)
    {
      uint_least8_t cnt;
      HAL_Obj *obj = (HAL_Obj *)handle;
      _iq beta_lp_pu = _IQ(pUserParams->offsetPole_rps/(float_t)pUserParams->ctrlFreq_Hz);
    
    
      HAL_setNumCurrentSensors(handle,pUserParams->numCurrentSensors);
      HAL_setNumVoltageSensors(handle,pUserParams->numVoltageSensors);
    
    
      for(cnt=0;cnt<HAL_getNumCurrentSensors(handle);cnt++)
        {
          HAL_setOffsetBeta_lp_pu(handle,HAL_SensorType_Current,cnt,beta_lp_pu);
          HAL_setOffsetInitCond(handle,HAL_SensorType_Current,cnt,_IQ(0.0));
          HAL_setOffsetValue(handle,HAL_SensorType_Current,cnt,_IQ(0.0));
        }
    
    
      for(cnt=0;cnt<HAL_getNumVoltageSensors(handle);cnt++)
        {
          HAL_setOffsetBeta_lp_pu(handle,HAL_SensorType_Voltage,cnt,beta_lp_pu);
          HAL_setOffsetInitCond(handle,HAL_SensorType_Voltage,cnt,_IQ(0.0));
          HAL_setOffsetValue(handle,HAL_SensorType_Voltage,cnt,_IQ(0.0));
        }
    
    
      // disable global interrupts
      CPU_disableGlobalInts(obj->cpuHandle);
    
    
      // disable cpu interrupts
      CPU_disableInts(obj->cpuHandle);
    
    
      // clear cpu interrupt flags
      CPU_clearIntFlags(obj->cpuHandle);
    
    
      // setup the clocks
      HAL_setupClks(handle);
    
    
      // Setup the PLL
      HAL_setupPll(handle,PLL_ClkFreq_90_MHz);
    
    
      // setup the PIE
      HAL_setupPie(handle);
    
    
      // run the device calibration
      HAL_cal(handle);
    
    
      // setup the peripheral clocks
      HAL_setupPeripheralClks(handle);
    
    
      // setup the GPIOs
      HAL_setupGpios(handle);
    
    
      // setup the flash
      HAL_setupFlash(handle);
    
    
      // setup the ADCs
      HAL_setupAdcs(handle);
    
    
      // setup the PWMs
      HAL_setupPwms(handle,
                    (float_t)pUserParams->systemFreq_MHz,
                    pUserParams->pwmPeriod_usec,
                    USER_NUM_PWM_TICKS_PER_ISR_TICK);
    
    #ifdef QEP
      // setup the QEP
      HAL_setupQEP(handle,HAL_Qep_QEP1);
    #endif
    
    
      // setup the spiA
      HAL_setupSpiA(handle);
    
    
      // setup the PWM DACs
      HAL_setupPwmDacs(handle);
    
    
      // setup the timers
      HAL_setupTimers(handle,
                      (float_t)pUserParams->systemFreq_MHz);
    
    
      // set the default current bias
     {
       uint_least8_t cnt;
       _iq bias = _IQ12mpy(ADC_dataBias,_IQ(pUserParams->current_sf));
       
       for(cnt=0;cnt<HAL_getNumCurrentSensors(handle);cnt++)
         {
           HAL_setBias(handle,HAL_SensorType_Current,cnt,bias);
         }
     }
    
    
      //  set the current scale factor
     {
       _iq current_sf = _IQ(pUserParams->current_sf);
    
      HAL_setCurrentScaleFactor(handle,current_sf);
     }
    
    
      // set the default voltage bias
     {
       uint_least8_t cnt;
       _iq bias = _IQ(0.0);
       
       for(cnt=0;cnt<HAL_getNumVoltageSensors(handle);cnt++)
         {
           HAL_setBias(handle,HAL_SensorType_Voltage,cnt,bias);
         }
     }
    
    
      //  set the voltage scale factor
     {
       _iq voltage_sf = _IQ(pUserParams->voltage_sf);
    
      HAL_setVoltageScaleFactor(handle,voltage_sf);
     }
    
     return;
    } // end of HAL_setParams() function
    
    
    void HAL_setupAdcs(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      // disable the ADCs
      ADC_disable(obj->adcHandle);
    
    
      // power up the bandgap circuit
      ADC_enableBandGap(obj->adcHandle);
    
    
      // set the ADC voltage reference source to internal 
      ADC_setVoltRefSrc(obj->adcHandle,ADC_VoltageRefSrc_Int);
    
    
      // enable the ADC reference buffers
      ADC_enableRefBuffers(obj->adcHandle);
    
    
      // Set main clock scaling factor (max45MHz clock for the ADC module)
      ADC_setDivideSelect(obj->adcHandle,ADC_DivideSelect_ClkIn_by_2);
    
    
      // power up the ADCs
      ADC_powerUp(obj->adcHandle);
    
    
      // enable the ADCs
      ADC_enable(obj->adcHandle);
    
    
      // set the ADC interrupt pulse generation to prior
      ADC_setIntPulseGenMode(obj->adcHandle,ADC_IntPulseGenMode_Prior);
    
    
      // set the temperature sensor source to external
      ADC_setTempSensorSrc(obj->adcHandle,ADC_TempSensorSrc_Ext);
    
    
      // configure the interrupt sources
      ADC_disableInt(obj->adcHandle,ADC_IntNumber_1);
      ADC_setIntMode(obj->adcHandle,ADC_IntNumber_1,ADC_IntMode_ClearFlag);
      ADC_setIntSrc(obj->adcHandle,ADC_IntNumber_1,ADC_IntSrc_EOC7);
    
    
      // configure the SOCs for drv8312kit_revD
      // EXT IA-FB
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_0,ADC_SocChanNumber_A1);
      ADC_setSocTrigSrc(obj->adcHandle,ADC_SocNumber_0,ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_0,ADC_SocSampleDelay_9_cycles);
    
      // EXT IA-FB
      // Duplicate conversion due to ADC Initial Conversion bug (SPRZ342)
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_1,ADC_SocChanNumber_A1);
      ADC_setSocTrigSrc(obj->adcHandle,ADC_SocNumber_1,ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_1,ADC_SocSampleDelay_9_cycles);
    
      // EXT IB-FB
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_2,ADC_SocChanNumber_B5);
      ADC_setSocTrigSrc(obj->adcHandle,ADC_SocNumber_2,ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_2,ADC_SocSampleDelay_9_cycles);
    
      // EXT IC-FB
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_3,ADC_SocChanNumber_A5);
      ADC_setSocTrigSrc(obj->adcHandle,ADC_SocNumber_3,ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_3,ADC_SocSampleDelay_9_cycles);
    
      // ADC-Vhb1
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_4,ADC_SocChanNumber_B7);
      ADC_setSocTrigSrc(obj->adcHandle,ADC_SocNumber_4,ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_4,ADC_SocSampleDelay_9_cycles);
    
      // ADC-Vhb2
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_5,ADC_SocChanNumber_A7);
      ADC_setSocTrigSrc(obj->adcHandle,ADC_SocNumber_5,ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_5,ADC_SocSampleDelay_9_cycles);
    
      // ADC-Vhb3
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_6,ADC_SocChanNumber_B4);
      ADC_setSocTrigSrc(obj->adcHandle,ADC_SocNumber_6,ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_6,ADC_SocSampleDelay_9_cycles);
    
      // VDCBUS
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_7,ADC_SocChanNumber_B2);
      ADC_setSocTrigSrc(obj->adcHandle,ADC_SocNumber_7,ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_7,ADC_SocSampleDelay_9_cycles);
    
      return;
    } // end of HAL_setupAdcs() function
    
    
    void HAL_setupClks(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      // enable internal oscillator 1
      CLK_enableOsc1(obj->clkHandle);
    
      // set the oscillator source
      CLK_setOscSrc(obj->clkHandle,CLK_OscSrc_Internal);
    
      // disable the external clock in
      CLK_disableClkIn(obj->clkHandle);
    
      // disable the crystal oscillator
      CLK_disableCrystalOsc(obj->clkHandle);
    
      // disable oscillator 2
      CLK_disableOsc2(obj->clkHandle);
    
      // set the low speed clock prescaler
      CLK_setLowSpdPreScaler(obj->clkHandle,CLK_LowSpdPreScaler_SysClkOut_by_1);
    
      // set the clock out prescaler
      CLK_setClkOutPreScaler(obj->clkHandle,CLK_ClkOutPreScaler_SysClkOut_by_1);
    
      return;
    } // end of HAL_setupClks() function
    
    
    void HAL_setupFlash(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      FLASH_enablePipelineMode(obj->flashHandle);
    
      FLASH_setNumPagedReadWaitStates(obj->flashHandle,FLASH_NumPagedWaitStates_3);
    
      FLASH_setNumRandomReadWaitStates(obj->flashHandle,FLASH_NumRandomWaitStates_3);
    
      FLASH_setOtpWaitStates(obj->flashHandle,FLASH_NumOtpWaitStates_5);
    
      FLASH_setStandbyWaitCount(obj->flashHandle,FLASH_STANDBY_WAIT_COUNT_DEFAULT);
    
      FLASH_setActiveWaitCount(obj->flashHandle,FLASH_ACTIVE_WAIT_COUNT_DEFAULT);
    
      return;
    } // HAL_setupFlash() function
    
    
    void HAL_setupGpios(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      // PWM1
      GPIO_setMode(obj->gpioHandle,GPIO_Number_0,GPIO_0_Mode_EPWM1A);
    
      // PWM2
      GPIO_setMode(obj->gpioHandle,GPIO_Number_1,GPIO_1_Mode_EPWM1B);
    
      // PWM3
      GPIO_setMode(obj->gpioHandle,GPIO_Number_2,GPIO_2_Mode_EPWM2A);
    
      // PWM4
      GPIO_setMode(obj->gpioHandle,GPIO_Number_3,GPIO_3_Mode_EPWM2B);
    
      // PWM5
      GPIO_setMode(obj->gpioHandle,GPIO_Number_4,GPIO_4_Mode_EPWM3A);
    
      // PWM6
      GPIO_setMode(obj->gpioHandle,GPIO_Number_5,GPIO_5_Mode_EPWM3B);
    
      // PWM-DAC4
      GPIO_setMode(obj->gpioHandle,GPIO_Number_6,GPIO_6_Mode_EPWM4A);
    
      // Input
      GPIO_setMode(obj->gpioHandle,GPIO_Number_7,GPIO_7_Mode_GeneralPurpose);
    
      // PWM-DAC3
      GPIO_setMode(obj->gpioHandle,GPIO_Number_8,GPIO_8_Mode_EPWM5A);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_9,GPIO_9_Mode_GeneralPurpose);
    
      // PWM-DAC1
      GPIO_setMode(obj->gpioHandle,GPIO_Number_10,GPIO_10_Mode_EPWM6A);
    
      // PWM-DAC2
      GPIO_setMode(obj->gpioHandle,GPIO_Number_11,GPIO_11_Mode_EPWM6B);
    
      // FAULTn
      GPIO_setMode(obj->gpioHandle,GPIO_Number_12,GPIO_12_Mode_GeneralPurpose);
      GPIO_setLow(obj->gpioHandle,GPIO_Number_12);
      GPIO_setDirection(obj->gpioHandle,GPIO_Number_12,GPIO_Direction_Output);
    
      // OCTWn
      GPIO_setMode(obj->gpioHandle,GPIO_Number_13,GPIO_13_Mode_TZ2_NOT);
    
      // FAULTn
      GPIO_setMode(obj->gpioHandle,GPIO_Number_14,GPIO_14_Mode_TZ3_NOT);
    
      // LED2
      GPIO_setMode(obj->gpioHandle,GPIO_Number_15,GPIO_15_Mode_GeneralPurpose);
    
      // Set Qualification Period for GPIO16-23, 22*2*(1/90MHz) = 0.48us
      GPIO_setQualificationPeriod(obj->gpioHandle,GPIO_Number_16,22);
    
      // SPI-SIMO
      GPIO_setMode(obj->gpioHandle,GPIO_Number_16,GPIO_16_Mode_SPISIMOA);
    
      // SPI-SOMI
      GPIO_setMode(obj->gpioHandle,GPIO_Number_17,GPIO_17_Mode_SPISOMIA);
    
      // SPI-CLK
      GPIO_setMode(obj->gpioHandle,GPIO_Number_18,GPIO_18_Mode_SPICLKA);
    
      // SPI-STE
      GPIO_setMode(obj->gpioHandle,GPIO_Number_19,GPIO_19_Mode_SPISTEA_NOT);
    
    #ifdef QEP
      // EQEPA
      GPIO_setMode(obj->gpioHandle,GPIO_Number_20,GPIO_20_Mode_EQEP1A);
      GPIO_setQualification(obj->gpioHandle,GPIO_Number_20,GPIO_Qual_Sample_3);
    
      // EQEPB
      GPIO_setMode(obj->gpioHandle,GPIO_Number_21,GPIO_21_Mode_EQEP1B);
      GPIO_setQualification(obj->gpioHandle,GPIO_Number_21,GPIO_Qual_Sample_3);
    
      // STATUS
      GPIO_setMode(obj->gpioHandle,GPIO_Number_22,GPIO_22_Mode_GeneralPurpose);
    
      // EQEP1I
      GPIO_setMode(obj->gpioHandle,GPIO_Number_23,GPIO_23_Mode_EQEP1I);
      GPIO_setQualification(obj->gpioHandle,GPIO_Number_23,GPIO_Qual_Sample_3);
    #else
      // EQEPA
      GPIO_setMode(obj->gpioHandle,GPIO_Number_20,GPIO_20_Mode_GeneralPurpose);
    
      // EQEPB
      GPIO_setMode(obj->gpioHandle,GPIO_Number_21,GPIO_21_Mode_GeneralPurpose);
    
      // STATUS
      GPIO_setMode(obj->gpioHandle,GPIO_Number_22,GPIO_22_Mode_GeneralPurpose);
    
      // EQEP1I
      GPIO_setMode(obj->gpioHandle,GPIO_Number_23,GPIO_23_Mode_GeneralPurpose);
    #endif
    
      // SPI SIMO B
      GPIO_setMode(obj->gpioHandle,GPIO_Number_24,GPIO_24_Mode_GeneralPurpose);
    
      // SPI SOMI B
      GPIO_setMode(obj->gpioHandle,GPIO_Number_25,GPIO_25_Mode_GeneralPurpose);
    
      // SPI CLK B
      GPIO_setMode(obj->gpioHandle,GPIO_Number_26,GPIO_26_Mode_GeneralPurpose);
    
      // SPI CSn B
      GPIO_setMode(obj->gpioHandle,GPIO_Number_27,GPIO_27_Mode_GeneralPurpose);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_28,GPIO_28_Mode_GeneralPurpose);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_29,GPIO_29_Mode_GeneralPurpose);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_30,GPIO_30_Mode_GeneralPurpose);
    
      // ControlCARD LED2
      GPIO_setMode(obj->gpioHandle,GPIO_Number_31,GPIO_31_Mode_GeneralPurpose);
      GPIO_setLow(obj->gpioHandle,GPIO_Number_31);
      GPIO_setDirection(obj->gpioHandle,GPIO_Number_31,GPIO_Direction_Output);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_32,GPIO_32_Mode_GeneralPurpose);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_33,GPIO_33_Mode_GeneralPurpose);
    
      // ControlCARD LED3
      GPIO_setMode(obj->gpioHandle,GPIO_Number_34,GPIO_34_Mode_GeneralPurpose);
      GPIO_setLow(obj->gpioHandle,GPIO_Number_34);
      GPIO_setDirection(obj->gpioHandle,GPIO_Number_34,GPIO_Direction_Output);
    
      // JTAG
      GPIO_setMode(obj->gpioHandle,GPIO_Number_35,GPIO_35_Mode_JTAG_TDI);
      GPIO_setMode(obj->gpioHandle,GPIO_Number_36,GPIO_36_Mode_JTAG_TMS);
      GPIO_setMode(obj->gpioHandle,GPIO_Number_37,GPIO_37_Mode_JTAG_TDO);
      GPIO_setMode(obj->gpioHandle,GPIO_Number_38,GPIO_38_Mode_JTAG_TCK);
    
      // DRV8301 Enable
      GPIO_setMode(obj->gpioHandle,GPIO_Number_39,GPIO_39_Mode_GeneralPurpose);
    
      // CAP1
      GPIO_setMode(obj->gpioHandle,GPIO_Number_40,GPIO_40_Mode_GeneralPurpose);
    
      // CAP2
      GPIO_setMode(obj->gpioHandle,GPIO_Number_41,GPIO_41_Mode_GeneralPurpose);
    
      // CAP3
      GPIO_setMode(obj->gpioHandle,GPIO_Number_42,GPIO_42_Mode_GeneralPurpose);
    
      // DC_CAL
      GPIO_setMode(obj->gpioHandle,GPIO_Number_43,GPIO_43_Mode_GeneralPurpose);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_44,GPIO_44_Mode_GeneralPurpose);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_50,GPIO_50_Mode_GeneralPurpose);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_51,GPIO_51_Mode_GeneralPurpose);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_52,GPIO_52_Mode_GeneralPurpose);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_53,GPIO_53_Mode_GeneralPurpose);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_54,GPIO_54_Mode_GeneralPurpose);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_55,GPIO_55_Mode_GeneralPurpose);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_56,GPIO_56_Mode_GeneralPurpose);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_57,GPIO_57_Mode_GeneralPurpose);
    
      // No Connection
      GPIO_setMode(obj->gpioHandle,GPIO_Number_58,GPIO_58_Mode_GeneralPurpose);
    
      return;
    }  // end of HAL_setupGpios() function
    
    
    void HAL_setupPie(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      PIE_disable(obj->pieHandle);
    
      PIE_disableAllInts(obj->pieHandle);
    
      PIE_clearAllInts(obj->pieHandle);
    
      PIE_clearAllFlags(obj->pieHandle);
    
      PIE_setDefaultIntVectorTable(obj->pieHandle);
    
      PIE_enable(obj->pieHandle);
    
      return;
    } // end of HAL_setupPie() function
    
    
    void HAL_setupPeripheralClks(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      CLK_enableAdcClock(obj->clkHandle);
    
      CLK_enableCompClock(obj->clkHandle,CLK_CompNumber_1);
      CLK_enableCompClock(obj->clkHandle,CLK_CompNumber_2);
      CLK_enableCompClock(obj->clkHandle,CLK_CompNumber_3);
    
      CLK_enableEcap1Clock(obj->clkHandle);
    
      CLK_disableEcanaClock(obj->clkHandle);
    
    #ifdef QEP
      CLK_enableEqep1Clock(obj->clkHandle);
      CLK_disableEqep2Clock(obj->clkHandle);
    #endif
    
      CLK_enablePwmClock(obj->clkHandle,PWM_Number_1);
      CLK_enablePwmClock(obj->clkHandle,PWM_Number_2);
      CLK_enablePwmClock(obj->clkHandle,PWM_Number_3);
      CLK_enablePwmClock(obj->clkHandle,PWM_Number_4);
      CLK_enablePwmClock(obj->clkHandle,PWM_Number_5);
      CLK_enablePwmClock(obj->clkHandle,PWM_Number_6);
      CLK_enablePwmClock(obj->clkHandle,PWM_Number_7);
    
      CLK_disableHrPwmClock(obj->clkHandle);
    
      CLK_disableI2cClock(obj->clkHandle);
    
      CLK_disableLinAClock(obj->clkHandle);
    
      CLK_disableClaClock(obj->clkHandle);
    
      CLK_enableSciaClock(obj->clkHandle);
    
      CLK_enableSpiaClock(obj->clkHandle);
      CLK_disableSpibClock(obj->clkHandle);
      
      CLK_enableTbClockSync(obj->clkHandle);
    
      return;
    } // end of HAL_setupPeripheralClks() function
    
    
    void HAL_setupPll(HAL_Handle handle,const PLL_ClkFreq_e clkFreq)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      // make sure PLL is not running in limp mode
      if(PLL_getClkStatus(obj->pllHandle) != PLL_ClkStatus_Normal)
        {
          // reset the clock detect
          PLL_resetClkDetect(obj->pllHandle);
    
          // ???????
          asm("        ESTOP0");
        }
    
    
      // Divide Select must be ClkIn/4 before the clock rate can be changed
      if(PLL_getDivideSelect(obj->pllHandle) != PLL_DivideSelect_ClkIn_by_4)
        {
          PLL_setDivideSelect(obj->pllHandle,PLL_DivideSelect_ClkIn_by_4);
        }
    
    
      if(PLL_getClkFreq(obj->pllHandle) != clkFreq)
        {
          // disable the clock detect
          PLL_disableClkDetect(obj->pllHandle);
    
          // set the clock rate
          PLL_setClkFreq(obj->pllHandle,clkFreq);
        }
    
    
      // wait until locked
      while(PLL_getLockStatus(obj->pllHandle) != PLL_LockStatus_Done) {}
    
    
      // enable the clock detect
      PLL_enableClkDetect(obj->pllHandle);
    
    
      // set divide select to ClkIn/2 to get desired clock rate
      // NOTE: clock must be locked before setting this register
      PLL_setDivideSelect(obj->pllHandle,PLL_DivideSelect_ClkIn_by_2);
    
      return;
    } // end of HAL_setupPll() function
    
    
    void HAL_setupPwms(HAL_Handle handle,
                       const float_t systemFreq_MHz,
                       const float_t pwmPeriod_usec,
                       const uint_least16_t numPwmTicksPerIsrTick)
    {
      HAL_Obj   *obj = (HAL_Obj *)handle;
      uint16_t   halfPeriod_cycles = (uint16_t)(systemFreq_MHz*pwmPeriod_usec) >> 1;
      uint_least8_t    cnt;
    
    
      // turns off the outputs of the EPWM peripherals which will put the power switches
      // into a high impedance state.
      PWM_setOneShotTrip(obj->pwmHandle[PWM_Number_1]);
      PWM_setOneShotTrip(obj->pwmHandle[PWM_Number_2]);
      PWM_setOneShotTrip(obj->pwmHandle[PWM_Number_3]);
    
      for(cnt=0;cnt<3;cnt++)
        {
          // setup the Time-Base Control Register (TBCTL)
          PWM_setCounterMode(obj->pwmHandle[cnt],PWM_CounterMode_UpDown);
          PWM_disableCounterLoad(obj->pwmHandle[cnt]);
          PWM_setPeriodLoad(obj->pwmHandle[cnt],PWM_PeriodLoad_Immediate);
          PWM_setSyncMode(obj->pwmHandle[cnt],PWM_SyncMode_EPWMxSYNC);
          PWM_setHighSpeedClkDiv(obj->pwmHandle[cnt],PWM_HspClkDiv_by_1);
          PWM_setClkDiv(obj->pwmHandle[cnt],PWM_ClkDiv_by_1);
          PWM_setPhaseDir(obj->pwmHandle[cnt],PWM_PhaseDir_CountUp);
          PWM_setRunMode(obj->pwmHandle[cnt],PWM_RunMode_FreeRun);
    
          // setup the Timer-Based Phase Register (TBPHS)
          PWM_setPhase(obj->pwmHandle[cnt],0);
    
          // setup the Time-Base Counter Register (TBCTR)
          PWM_setCount(obj->pwmHandle[cnt],0);
    
          // setup the Time-Base Period Register (TBPRD)
          // set to zero initially
          PWM_setPeriod(obj->pwmHandle[cnt],0);
    
          // setup the Counter-Compare Control Register (CMPCTL)
          PWM_setLoadMode_CmpA(obj->pwmHandle[cnt],PWM_LoadMode_Zero);
          PWM_setLoadMode_CmpB(obj->pwmHandle[cnt],PWM_LoadMode_Zero);
          PWM_setShadowMode_CmpA(obj->pwmHandle[cnt],PWM_ShadowMode_Shadow);
          PWM_setShadowMode_CmpB(obj->pwmHandle[cnt],PWM_ShadowMode_Immediate);
    
          // setup the Action-Qualifier Output A Register (AQCTLA) 
          PWM_setActionQual_CntUp_CmpA_PwmA(obj->pwmHandle[cnt],PWM_ActionQual_Set);
          PWM_setActionQual_CntDown_CmpA_PwmA(obj->pwmHandle[cnt],PWM_ActionQual_Clear);
    
          // setup the Action-qualifier Continuous Software Force Register (AQCSFRC)
          PWM_setActionQualContSWForce_PwmB(obj->pwmHandle[cnt],PWM_ActionQualContSWForce_Set);
    
          // setup the Dead-Band Generator Control Register (DBCTL)
          PWM_setDeadBandOutputMode(obj->pwmHandle[cnt],PWM_DeadBandOutputMode_Bypass);
    
          // setup the PWM-Chopper Control Register (PCCTL)
          PWM_disableChopping(obj->pwmHandle[cnt]);
    
          // setup the Trip Zone Select Register (TZSEL)
          PWM_disableTripZones(obj->pwmHandle[cnt]);
        }
    
    
      // setup the Event Trigger Selection Register (ETSEL)
      PWM_disableInt(obj->pwmHandle[PWM_Number_1]);
      PWM_setSocAPulseSrc(obj->pwmHandle[PWM_Number_1],PWM_SocPulseSrc_CounterEqualZero);
      PWM_enableSocAPulse(obj->pwmHandle[PWM_Number_1]);
      
    
      // setup the Event Trigger Prescale Register (ETPS)
      if(numPwmTicksPerIsrTick == 3)
        {
          PWM_setIntPeriod(obj->pwmHandle[PWM_Number_1],PWM_IntPeriod_ThirdEvent);
          PWM_setSocAPeriod(obj->pwmHandle[PWM_Number_1],PWM_SocPeriod_ThirdEvent);
        }
      else if(numPwmTicksPerIsrTick == 2)
        {
          PWM_setIntPeriod(obj->pwmHandle[PWM_Number_1],PWM_IntPeriod_SecondEvent);
          PWM_setSocAPeriod(obj->pwmHandle[PWM_Number_1],PWM_SocPeriod_SecondEvent);
        }
      else
        {
          PWM_setIntPeriod(obj->pwmHandle[PWM_Number_1],PWM_IntPeriod_FirstEvent);
          PWM_setSocAPeriod(obj->pwmHandle[PWM_Number_1],PWM_SocPeriod_FirstEvent);
        }
    
    
      // setup the Event Trigger Clear Register (ETCLR)
      PWM_clearIntFlag(obj->pwmHandle[PWM_Number_1]);
      PWM_clearSocAFlag(obj->pwmHandle[PWM_Number_1]);
    
      // first step to synchronize the pwms
      CLK_disableTbClockSync(obj->clkHandle);
    
      // since the PWM is configured as an up/down counter, the period register is set to one-half 
      // of the desired PWM period
      PWM_setPeriod(obj->pwmHandle[PWM_Number_1],halfPeriod_cycles);
      PWM_setPeriod(obj->pwmHandle[PWM_Number_2],halfPeriod_cycles);
      PWM_setPeriod(obj->pwmHandle[PWM_Number_3],halfPeriod_cycles);
    
      // last step to synchronize the pwms
      CLK_enableTbClockSync(obj->clkHandle);
    
      return;
    }  // end of HAL_setupPwms() function
    
    #ifdef QEP
    void HAL_setupQEP(HAL_Handle handle,HAL_QepSelect_e qep)
    {
      HAL_Obj   *obj = (HAL_Obj *)handle;
    
    
      // hold the counter in reset
      QEP_reset_counter(obj->qepHandle[qep]);
    
      // set the QPOSINIT register
      QEP_set_posn_init_count(obj->qepHandle[qep], 0);
    
      // disable all interrupts
      QEP_disable_all_interrupts(obj->qepHandle[qep]);
    
      // clear the interrupt flags
      QEP_clear_all_interrupt_flags(obj->qepHandle[qep]);
    
      // clear the position counter
      QEP_clear_posn_counter(obj->qepHandle[qep]);
    
      // setup the max position
      QEP_set_max_posn_count(obj->qepHandle[qep], (4*USER_MOTOR_ENCODER_LINES)-1);
    
      // setup the QDECCTL register
      QEP_set_QEP_source(obj->qepHandle[qep], QEP_Qsrc_Quad_Count_Mode);
      QEP_disable_sync_out(obj->qepHandle[qep]);
      QEP_set_swap_quad_inputs(obj->qepHandle[qep], QEP_Swap_Not_Swapped);
      QEP_disable_gate_index(obj->qepHandle[qep]);
      QEP_set_ext_clock_rate(obj->qepHandle[qep], QEP_Xcr_2x_Res);
      QEP_set_A_polarity(obj->qepHandle[qep], QEP_Qap_No_Effect);
      QEP_set_B_polarity(obj->qepHandle[qep], QEP_Qbp_No_Effect);
      QEP_set_index_polarity(obj->qepHandle[qep], QEP_Qip_No_Effect);
    
      // setup the QEPCTL register
      QEP_set_emu_control(obj->qepHandle[qep], QEPCTL_Freesoft_Unaffected_Halt);
      QEP_set_posn_count_reset_mode(obj->qepHandle[qep], QEPCTL_Pcrm_Max_Reset);
      QEP_set_strobe_event_init(obj->qepHandle[qep], QEPCTL_Sei_Nothing);
      QEP_set_index_event_init(obj->qepHandle[qep], QEPCTL_Iei_Nothing);
      QEP_set_index_event_latch(obj->qepHandle[qep], QEPCTL_Iel_Rising_Edge);
      QEP_set_soft_init(obj->qepHandle[qep], QEPCTL_Swi_Nothing);
      QEP_disable_unit_timer(obj->qepHandle[qep]);
      QEP_disable_watchdog(obj->qepHandle[qep]);
    
      // setup the QPOSCTL register
      QEP_disable_posn_compare(obj->qepHandle[qep]);
    
      // setup the QCAPCTL register
      QEP_disable_capture(obj->qepHandle[qep]);
    
      // renable the position counter
      QEP_enable_counter(obj->qepHandle[qep]);
    
    
      return;
    }
    #endif
    
    
    void HAL_setupSpiA(HAL_Handle handle)
    {
      HAL_Obj   *obj = (HAL_Obj *)handle;
    
      SPI_reset(obj->spiAHandle);
      SPI_setClkPolarity(obj->spiAHandle,SPI_ClkPolarity_OutputRisingEdge_InputFallingEdge);
      SPI_disableLoopBack(obj->spiAHandle);
      SPI_setCharLength(obj->spiAHandle,SPI_CharLength_16_Bits);
    
      SPI_setMode(obj->spiAHandle,SPI_Mode_Slave);
      SPI_setClkPhase(obj->spiAHandle,SPI_ClkPhase_Delayed);
      SPI_enableTx(obj->spiAHandle);
    
      SPI_enableChannels(obj->spiAHandle);
      SPI_enableTxFifoEnh(obj->spiAHandle);
      SPI_enableTxFifo(obj->spiAHandle);
      SPI_setTxDelay(obj->spiAHandle,0);
      SPI_clearTxFifoInt(obj->spiAHandle);
      SPI_enableRxFifo(obj->spiAHandle);
    
    //not needed for slave mode  SPI_setBaudRate(obj->spiAHandle,(SPI_BaudRate_e)(0x0003));
      SPI_setSuspend(obj->spiAHandle,SPI_TxSuspend_free);
      SPI_enable(obj->spiAHandle);
    
      return;
    }  // end of HAL_setupSpiA() function
    
    void HAL_setupPwmDacs(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
      uint16_t halfPeriod_cycles = 512;       // 3000->10kHz, 1500->20kHz, 1000-> 30kHz, 500->60kHz
      uint_least8_t    cnt;
    
    
      for(cnt=0;cnt<3;cnt++)
        {
          // initialize the Time-Base Control Register (TBCTL)
          PWMDAC_setCounterMode(obj->pwmDacHandle[cnt],PWM_CounterMode_UpDown);
          PWMDAC_disableCounterLoad(obj->pwmDacHandle[cnt]);
          PWMDAC_setPeriodLoad(obj->pwmDacHandle[cnt],PWM_PeriodLoad_Immediate);
          PWMDAC_setSyncMode(obj->pwmDacHandle[cnt],PWM_SyncMode_EPWMxSYNC);
          PWMDAC_setHighSpeedClkDiv(obj->pwmDacHandle[cnt],PWM_HspClkDiv_by_1);
          PWMDAC_setClkDiv(obj->pwmDacHandle[cnt],PWM_ClkDiv_by_1);
          PWMDAC_setPhaseDir(obj->pwmDacHandle[cnt],PWM_PhaseDir_CountUp);
          PWMDAC_setRunMode(obj->pwmDacHandle[cnt],PWM_RunMode_FreeRun);
    
          // initialize the Timer-Based Phase Register (TBPHS)
          PWMDAC_setPhase(obj->pwmDacHandle[cnt],0);
    
          // setup the Time-Base Counter Register (TBCTR)
          PWMDAC_setCount(obj->pwmDacHandle[cnt],0);
    
          // Initialize the Time-Base Period Register (TBPRD)
          // set to zero initially
          PWMDAC_setPeriod(obj->pwmDacHandle[cnt],0);
    
          // initialize the Counter-Compare Control Register (CMPCTL)
          PWMDAC_setLoadMode_CmpA(obj->pwmDacHandle[cnt],PWM_LoadMode_Zero);
          PWMDAC_setLoadMode_CmpB(obj->pwmDacHandle[cnt],PWM_LoadMode_Zero);
          PWMDAC_setShadowMode_CmpA(obj->pwmDacHandle[cnt],PWM_ShadowMode_Shadow);
          PWMDAC_setShadowMode_CmpB(obj->pwmDacHandle[cnt],PWM_ShadowMode_Shadow);
    
          // Initialize the Action-Qualifier Output A Register (AQCTLA) 
          PWMDAC_setActionQual_CntUp_CmpA_PwmA(obj->pwmDacHandle[cnt],PWM_ActionQual_Clear);
          PWMDAC_setActionQual_CntDown_CmpA_PwmA(obj->pwmDacHandle[cnt],PWM_ActionQual_Set);
    
          // account for EPWM6B
          if(cnt == 0)
            {
              PWMDAC_setActionQual_CntUp_CmpB_PwmB(obj->pwmDacHandle[cnt],PWM_ActionQual_Clear);
              PWMDAC_setActionQual_CntDown_CmpB_PwmB(obj->pwmDacHandle[cnt],PWM_ActionQual_Set);
            }
    
          // Initialize the Dead-Band Control Register (DBCTL) 
          PWMDAC_disableDeadBand(obj->pwmDacHandle[cnt]);
    
          // Initialize the PWM-Chopper Control Register (PCCTL)
          PWMDAC_disableChopping(obj->pwmDacHandle[cnt]);
    
          // Initialize the Trip-Zone Control Register (TZSEL)
          PWMDAC_disableTripZones(obj->pwmDacHandle[cnt]);
    
          // Initialize the Trip-Zone Control Register (TZCTL)
          PWMDAC_setTripZoneState_TZA(obj->pwmDacHandle[cnt],PWM_TripZoneState_HighImp);
          PWMDAC_setTripZoneState_TZB(obj->pwmDacHandle[cnt],PWM_TripZoneState_HighImp);
          PWMDAC_setTripZoneState_DCAEVT1(obj->pwmDacHandle[cnt],PWM_TripZoneState_HighImp);
          PWMDAC_setTripZoneState_DCAEVT2(obj->pwmDacHandle[cnt],PWM_TripZoneState_HighImp);
          PWMDAC_setTripZoneState_DCBEVT1(obj->pwmDacHandle[cnt],PWM_TripZoneState_HighImp);
        }
    
      // since the PWM is configured as an up/down counter, the period register is set to one-half 
      // of the desired PWM period
      PWMDAC_setPeriod(obj->pwmDacHandle[PWMDAC_Number_1],halfPeriod_cycles);
      PWMDAC_setPeriod(obj->pwmDacHandle[PWMDAC_Number_2],halfPeriod_cycles);
      PWMDAC_setPeriod(obj->pwmDacHandle[PWMDAC_Number_3],halfPeriod_cycles);
    
      return;
    }  // end of HAL_setupPwmDacs() function
    
    
    void HAL_setupTimers(HAL_Handle handle,const float_t systemFreq_MHz)
    {
      HAL_Obj  *obj = (HAL_Obj *)handle;
      uint32_t  timerPeriod_cnts = (uint32_t)(systemFreq_MHz * (float_t)1000000.0) - 1;
    
      // use timer 0 for frequency diagnostics
      TIMER_setDecimationFactor(obj->timerHandle[0],0);
      TIMER_setEmulationMode(obj->timerHandle[0],TIMER_EmulationMode_RunFree);
      TIMER_setPeriod(obj->timerHandle[0],timerPeriod_cnts);
      TIMER_setPreScaler(obj->timerHandle[0],0);
    
      // use timer 1 for CPU usage diagnostics
      TIMER_setDecimationFactor(obj->timerHandle[1],0);
      TIMER_setEmulationMode(obj->timerHandle[1],TIMER_EmulationMode_RunFree);
      TIMER_setPeriod(obj->timerHandle[1],timerPeriod_cnts);
      TIMER_setPreScaler(obj->timerHandle[1],0);
    
      return;
    }  // end of HAL_setupTimers() function
    
    // end of file
    

    0246.hal.h

    They are based on MW15. Please run a diff from a fresh MW15 and let me know if you have any questions.

    -Jorge

  • By the way, in this example, the inputs to the DAC write is from _IQ(-0.5) to _IQ(0.5), so please scale it appropriately.

  • Dear Jorge,

    Thank you for your guiding. However, I am a beginner in developing Motor Control in real (for example, with TI's C2000). I feel confuse about Scalling Technique when converting a floating algorithm from Matlab/Simulink or Plecs Simulation. If you have any document to explain in detail how to convert a floating equation into a fix-point equation and do it with IQmath, please share with me.

    For example, I wrote a C-scrip to do SVM algorithm in Simulink as following:

    #include <float.h>
    #include	<math.h>
    
    
    #define V_DC       Input(0)
    #define V_ALPHA    Input(1)
    #define V_BETA     Input(2)
    
    #define S_A        Output(0)
    #define S_B        Output(1)
    #define S_C        Output(2)
    #define SECTOR     Output(3)
    #define TAU_A      Output(4)
    #define TAU_B      Output(5)
    #define TAU_0      Output(6)
    
    
    #define TS         ParamRealData(0,0)  //sample period
    #define PATTERN    ParamRealData(1,0)
    #define OUTPUT_OFF ParamRealData(2, 0)
    #define OUTPUT_ON  ParamRealData(2, 1)
    
    #define max(x, y)	(x) > (y) ? (x) : (y)
    #define min(x, y)	(x) < (y) ? (x) : (y)
    #define PI			3.14159265358979323846
    
    #define MAX_TIMES	7
    double  sw_time[MAX_TIMES];
    int     sw_sequ[MAX_TIMES];
    int     sector, sw_index; 
    
    //Switching matrix for optimal SV modulation
    int sw_matrix_opt[6][3][3] = //[level idx][row idx][column idx]
    { //sector 0
     {{1,0,0},  //Va
      {1,1,0},  //Vb
      {1,1,1}}, //Vo
      
     {{1,1,0}, 
      {0,1,0},
      {0,0,0}}, 
    
     {{0,1,0},  
      {0,1,1},
      {1,1,1}}, 
    
     {{0,1,1},
      {0,0,1},
      {0,0,0}}, 
    
     {{0,0,1},
      {1,0,1},
      {1,1,1}},
    
     {{1,0,1},
      {1,0,0},
      {0,0,0}}};
    
    int sw_matrix_sym[6][4][3] = //[level idx][row idx][column idx]
    { //sector 0
     {{0,0,0},  //Vo'
      {1,0,0},  //Va
      {1,1,0},  //Vb
      {1,1,1}}, //Vo
      
     {{1,1,1},
      {1,1,0}, 
      {0,1,0},
      {0,0,0}}, 
    
     {{0,0,0},
      {0,1,0},  
      {0,1,1},
      {1,1,1}}, 
    
     {{1,1,1},
      {0,1,1},
      {0,0,1},
      {0,0,0}}, 
    
     {{0,0,0},
      {0,0,1},
      {1,0,1},
      {1,1,1}},
    
     {{1,1,1},
      {1,0,1},
      {1,0,0},
      {0,0,0}}};
    
    NextSampleHit = DBL_MAX;
    
    if (IsSampleHit(0))
    {
    
       double v_mag, v_ang;
       double alpha, sector_offset, tau_a, tau_b, tau_0;
    
    
       //Calculate magnitude and angle of reference vector Vs
       v_mag = sqrt(V_ALPHA*V_ALPHA + V_BETA*V_BETA);
       v_ang = fmod( atan2(V_BETA, V_ALPHA)+2.*PI, 2.*PI );
    
       //Sector detection - sector 1 middle at PhaseA ref peak
       sector = ((unsigned) floor(3./PI*v_ang)) % 6; // 0..5
       SECTOR = (double) sector + 1.;
    
       //Calculate sector angle, alpha
       sector_offset = PI/3. * (double) (sector);
       alpha = v_ang - sector_offset;
       
       //Limitation of the voltage magnitude
       v_mag = min(v_mag, 2./3.*V_DC/(cos(alpha) + 1./sqrt(3.)*sin(alpha)) );
    
       //Calculate relative vector on times
       tau_a = min( sqrt(3.)*v_mag/V_DC*sin(PI/3.-alpha), 1.);
       tau_b = min( sqrt(3.)*v_mag/V_DC*sin(alpha), 1.);
       tau_0 = max( 1.-tau_a-tau_b, 0.);
       TAU_A = tau_a;
       TAU_B = tau_b;
       TAU_0 = tau_0;
    
       if (PATTERN == 1) //optimal
       {
          if (sector%2 == 0)
          {
             sw_sequ[0] = 2;
             sw_time[0] = CurrentTime + TS*tau_0/2.;
             sw_sequ[1] = 1;
             sw_time[1] = CurrentTime + TS*(tau_0+tau_b)/2.;
             sw_sequ[2] = 0;
             sw_time[2] = CurrentTime + TS*(1 - (tau_0+tau_b)/2.);
             sw_sequ[3] = 1;
             sw_time[3] = CurrentTime + TS*(1. - tau_0/2.);
             sw_sequ[4] = 2;
             sw_time[4] = DBL_MAX;
          }
          else
          {
             sw_sequ[0] = 0;
             sw_time[0] = CurrentTime + TS*(1 - (tau_0+tau_b))/2.;
             sw_sequ[1] = 1;
             sw_time[1] = CurrentTime + TS*(1 - tau_0)/2.;
             sw_sequ[2] = 2;
             sw_time[2] = CurrentTime + TS*(1 + tau_0)/2.;
             sw_sequ[3] = 1;
             sw_time[3] = CurrentTime + TS*(1. + (tau_0+tau_b))/2.;
             sw_sequ[4] = 0;
             sw_time[4] = DBL_MAX;
          }
       }
       else if (PATTERN == 2) //symmetrical
       {
          if (sector%2 == 0)
          {
             sw_sequ[0] = 3;
             sw_time[0] = CurrentTime + TS*tau_0/4.;
             sw_sequ[1] = 2;
             sw_time[1] = CurrentTime + TS*(tau_0/2.+tau_b)/2.;
             sw_sequ[2] = 1;
             sw_time[2] = CurrentTime + TS*(tau_0/2.+tau_b+tau_a)/2.;
             sw_sequ[3] = 0;
             sw_time[3] = CurrentTime + TS*(1 - (tau_0/2.+tau_b+tau_a)/2.);
             sw_sequ[4] = 1;
             sw_time[4] = CurrentTime + TS*(1 - (tau_0/2.+tau_b)/2.);
             sw_sequ[5] = 2;
             sw_time[5] = CurrentTime + TS*(1. - tau_0/4.);
             sw_sequ[6] = 3;
             sw_time[6] = DBL_MAX;
          }
          else
          {
             sw_sequ[0] = 0;
             sw_time[0] = CurrentTime + TS*tau_0/4.;
             sw_sequ[1] = 1;
             sw_time[1] = CurrentTime + TS*(tau_0/2.+tau_a)/2.;
             sw_sequ[2] = 2;
             sw_time[2] = CurrentTime + TS*(tau_0/2.+tau_b+tau_a)/2.;
             sw_sequ[3] = 3;
             sw_time[3] = CurrentTime + TS*(1 - (tau_0/2.+tau_b+tau_a)/2.);
             sw_sequ[4] = 2;
             sw_time[4] = CurrentTime + TS*(1 - (tau_0/2.+tau_a)/2.);
             sw_sequ[5] = 1;
             sw_time[5] = CurrentTime + TS*(1. - tau_0/4.);
             sw_sequ[6] = 0;
             sw_time[6] = DBL_MAX;
          }
       }
       sw_index = 0;
    }
    
    if (IsSampleHit(0) ||
        (IsSampleHit(1) && CurrentTime >= NextSampleHit))
    {
       int sequence;
       
       do
       {
          sequence = sw_sequ[sw_index];
          NextSampleHit = sw_time[sw_index++];
       }
       while (NextSampleHit <= CurrentTime);
    
       //Outputs for legA,B,C
       if (PATTERN == 1)
       {
         S_A = sw_matrix_opt[sector][sequence][0] ? OUTPUT_ON : OUTPUT_OFF; 
         S_B = sw_matrix_opt[sector][sequence][1] ? OUTPUT_ON : OUTPUT_OFF;
         S_C = sw_matrix_opt[sector][sequence][2] ? OUTPUT_ON : OUTPUT_OFF;
       }
       else if (PATTERN == 2)
       {
         S_A = sw_matrix_sym[sector][sequence][0] ? OUTPUT_ON : OUTPUT_OFF; 
         S_B = sw_matrix_sym[sector][sequence][1] ? OUTPUT_ON : OUTPUT_OFF;
         S_C = sw_matrix_sym[sector][sequence][2] ? OUTPUT_ON : OUTPUT_OFF;
       }
    }
    

    Many thank you!

    Tran,

  • Dear Jorge,

    I checked your solution with MW15, but I got fail result for testing with the following code:

    #ifdef DEBUG
      // ------------------------------------------------------------------------------
      //    Connect inputs of the PWMDAC module
      // ------------------------------------------------------------------------------
      gDacData.value[0] = gPwmData.Tabc.value[0];
      gDacData.value[1] = gPwmData.Tabc.value[1];
      gDacData.value[2] = gPwmData.Tabc.value[1] - gPwmData.Tabc.value[2];
      gDacData.value[3] = _IQ(0.0);
    
      // write the data to DAC module
      //HAL_writeDacData(halHandle, &gDacData);
    #endif // DEBUG

    The result that I got is:

    Have you checked and the result is OK? In my opinion, the input of DacData is in _IQ15 format rather than _IQ, because the PWMDAC_setup() ends of the code lines:

    void HAL_setupPwmDacs(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
      uint16_t halfPeriod_cycles = 512;       // 3000->10kHz, 1500->20kHz, 1000-> 30kHz, 500->60kHz
      uint_least8_t    cnt;
    ....
    
      // since the PWM is configured as an up/down counter, the period register is set to one-half 
      // of the desired PWM period
      PWMDAC_setPeriod(obj->pwmDacHandle[PWMDAC_Number_1],halfPeriod_cycles);
      PWMDAC_setPeriod(obj->pwmDacHandle[PWMDAC_Number_2],halfPeriod_cycles);
      PWMDAC_setPeriod(obj->pwmDacHandle[PWMDAC_Number_3],halfPeriod_cycles);
    
      return;
    }  // end of HAL_setupPwmDacs() function
    

    NOTICE THAT: I used PWM6A, 6B and PWM7A, 7B as PWMDAC output for debugging purpose. So that I setup as following:

    //! \Brief Initializes all Hardware Drives that is used in the project.
    HAL_Handle HAL_init(void *pMemory,const size_t numBytes)
    {
      uint_least8_t cnt;
      HAL_Handle handle;
      HAL_Obj *obj;
    
      volatile int tmp;
    .
    .
    .
    #ifdef DEBUG
      // initialize PWM DAC handles
      obj->pwmDacHandle[0] = PWMDAC_init((void *)PWM_ePWM6_BASE_ADDR,sizeof(PWM_Obj)); //Using PWM6A/B
      obj->pwmDacHandle[1] = PWMDAC_init((void *)PWM_ePWM7_BASE_ADDR,sizeof(PWM_Obj)); //Using PWM7A/B
      //obj->pwmDacHandle[2] = PWMDAC_init((void *)PWM_ePWM5_BASE_ADDR,sizeof(PWM_Obj)); //Using PWM5A/B
    #endif //DEBUG
    .
    .
    .
      return(handle);
    } // end of HAL_init() function
    }
    
    
    void HAL_setParams(HAL_Handle handle,const USER_Params *pUserParams)
    {
      uint_least8_t cnt;
      HAL_Obj *obj = (HAL_Obj *)handle;
      _iq beta_lp_pu = _IQ(pUserParams->offsetPole_rps/(float_t)pUserParams->ctrlFreq_Hz);
    
    .
    .
    .
    #ifdef DEBUG
      // setup the PWM DACs
      HAL_setupPwmDacs(handle);
    #endif
    .
    .
     return;
    } // end of HAL_setParams() function
    

  • Unfortunately I don't have a document to share for that. However, if you are doing your own algorithm, you can do it in floating point if you are using a 28069M, since it does have a floating point unit, so you can use your algorithm how you are designing it in Simulink.

    If you are not using a device with floating point, you need to scale everything to per units. To do that, you need to normalize all inputs and outputs. Depending on what the inputs represent, for example current, you need to keep a scale factor internally, so that all inputs representing current are multiplied by your current scale factor, and the resulting value is in per units, usually from _IQ(-1.0) to _IQ(1.0). I might be able to  help more if you ask a more specific question about this.

    -Jorge

  • Yes, I tested the code I sent you on a DRV8312 board with a 28069M device.

    I can't test your exact configuration because I don't have a board that connects PWMDACs to those outputs, but let me send you what I think it would work.

    Notice that in my attachments I also updated this function: HAL_writeDacData() which now it scales a value from _IQ(-0.5) to _IQ(0.5), and it outputs a PWMDAC value from 0.0V to 3.3V so you can see it on the scope.

    If you are using 6A, 6B, 7A and 7B, first, configure those pins as PWM:

      // PWM-DAC1
      GPIO_setMode(obj->gpioHandle,GPIO_Number_10,GPIO_10_Mode_EPWM6A);
    
      // PWM-DAC2
      GPIO_setMode(obj->gpioHandle,GPIO_Number_11,GPIO_11_Mode_EPWM6B);
    
      // PWM-DAC3 NOTE: EPWM7A can also be in pin 40 and pin 58
      GPIO_setMode(obj->gpioHandle,GPIO_Number_30,GPIO_30_Mode_EPWM7A);
    
      // PWM-DAC4 NOTE: EPWM7B can also be in pin 44
      GPIO_setMode(obj->gpioHandle,GPIO_Number_41,GPIO_41_Mode_EPWM7B);

    You need to use this function to write PWMDAC in your case:

    //! \brief     Writes DAC data to the PWM comparators for DAC (digital-to-analog conversion) output
    //! \param[in] handle    The hardware abstraction layer (HAL) handle
    //! \param[in] pDacData  The pointer to the DAC data
    static inline void HAL_writeDacData(HAL_Handle handle,HAL_DacData_t *pDacData)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
      PWM_Obj *pwm;
      _iq period;
      _iq pwmData_sat_dc;
      _iq value;
      uint16_t value_sat;
    
      pwm = (PWM_Obj *)obj->pwmDacHandle[0];
      period = (_iq)pwm->TBPRD;
      pwmData_sat_dc = _IQsat(pDacData->value[0],_IQ(0.5),_IQ(-0.5)) + _IQ(0.5);
      value = _IQmpy(pwmData_sat_dc, period);
      value_sat = (uint16_t)_IQsat(value, period, _IQ(0.0));
      // write the PWM data
      PWMDAC_write_CmpA(obj->pwmDacHandle[0],value_sat);
    
      pwmData_sat_dc = _IQsat(pDacData->value[1],_IQ(0.5),_IQ(-0.5)) + _IQ(0.5);
      value = _IQmpy(pwmData_sat_dc, period);
      value_sat = (uint16_t)_IQsat(value, period, _IQ(0.0));
      // write the PWM data
      PWMDAC_write_CmpB(obj->pwmDacHandle[0],value_sat);
    
      pwm = (PWM_Obj *)obj->pwmDacHandle[1];
      period = (_iq)pwm->TBPRD;
      pwmData_sat_dc = _IQsat(pDacData->value[2],_IQ(0.5),_IQ(-0.5)) + _IQ(0.5);
      value = _IQmpy(pwmData_sat_dc, period);
      value_sat = (uint16_t)_IQsat(value, period, _IQ(0.0));
      // write the PWM data
      PWMDAC_write_CmpA(obj->pwmDacHandle[1],value_sat);
    
      pwmData_sat_dc = _IQsat(pDacData->value[3],_IQ(0.5),_IQ(-0.5)) + _IQ(0.5);
      value = _IQmpy(pwmData_sat_dc, period);
      value_sat = (uint16_t)_IQsat(value, period, _IQ(0.0));
      // write the PWM data
      PWMDAC_write_CmpB(obj->pwmDacHandle[1],value_sat);
    
      return;
    } // end of HAL_writeDacData() function

    In hal.c, under function HAL_init(), change this section, where the handles of the PWMDAC are initialized:

      // initialize PWM DAC handles
      obj->pwmDacHandle[0] = PWMDAC_init((void *)PWM_ePWM6_BASE_ADDR,sizeof(PWM_Obj));
      obj->pwmDacHandle[1] = PWMDAC_init((void *)PWM_ePWM7_BASE_ADDR,sizeof(PWM_Obj));
      obj->pwmDacHandle[2] = (PWM_Handle)NULL;

    On top of your main file, add this global structure which you already have I think, to make sure it is initialized with zeros:

    HAL_DacData_t gDacData = {_IQ(0.0), _IQ(0.0), _IQ(0.0), _IQ(0.0)};

    Then in the ISR, add the values you want to monitor, from the example above:

      gDacData.value[0] = gPwmData.Tabc.value[0];
      gDacData.value[1] = gPwmData.Tabc.value[1];
      gDacData.value[2] = gPwmData.Tabc.value[1] - gPwmData.Tabc.value[2]; // This could create a value greater than 0.5! this might saturate the output of the DAC.
      gDacData.value[3] = _IQ(0.0);

    And finally, call the write function:

      // write the data to DAC module
      HAL_writeDacData(halHandle, &gDacData);

    -Jorge

  • Dear Jorge,

    Thank your for your reply. However, when I run your HAL_writeDacData() in my project, the DSP be constantly reset!!! While with the sample code are not.

    Beside I looked deeply at the sample code of Lab11 Motorware, the HAL_writeDacData() function is written as:

    //! \brief     Writes DAC data to the PWM comparators for DAC (digital-to-analog conversion) output
    //! \param[in] handle    The hardware abstraction layer (HAL) handle
    //! \param[in] pDacData  The pointer to the DAC data
    static inline void HAL_writeDacData(HAL_Handle handle,HAL_DacData_t *pDacData)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
      // convert values from _IQ to _IQ15
      int16_t dacValue_1 = (int16_t)_IQtoIQ15(pDacData->value[0]);
      int16_t dacValue_2 = (int16_t)_IQtoIQ15(pDacData->value[1]);
      int16_t dacValue_3 = (int16_t)_IQtoIQ15(pDacData->value[2]);
      int16_t dacValue_4 = (int16_t)_IQtoIQ15(pDacData->value[3]);
    
      // write the DAC data
      PWMDAC_write_CmpA(obj->pwmDacHandle[PWMDAC_Number_1],dacValue_1);
      PWMDAC_write_CmpB(obj->pwmDacHandle[PWMDAC_Number_1],dacValue_2);
      PWMDAC_write_CmpA(obj->pwmDacHandle[PWMDAC_Number_2],dacValue_3);
      PWMDAC_write_CmpA(obj->pwmDacHandle[PWMDAC_Number_3],dacValue_4);
    
      return;
    } // end of HAL_writeDacData() function

    Notice that I copy this section of code from the Lab11 for F28027F + drv8031_revB kit.

    But when I put the data in this function for testing with my own board based on F28054F, I got a wrong result too. I designed my own kit and test it successfully when implementing it with ControlSuite's PWMDAC_MACRO().

    This is the figure of my own kit and the its schematic:

    And the schematic at: TMS320F28055 ControlCard-V2 - PAGE02.pdf

  • The HAL_writeDacData() function needs to be updated as my previous post, otherwise it won't work.

    But let's take a step back. Let me ask you, what device and what boards do you need to get this working on? I will give you specific instructions on the target and board(s) combination to make sure it works on your system.

    -Jorge

  • Dear Jorge,

    I have fixed the problem with my own board when calling your HAL_writeDacData(). The reason is one of PWM pin on my board is toughed to GND, when I fixed this hardware error, the DSP is no longer reset anymore.

    But when I check the shape of Ta, Tb and Tc which is generated by

    SVGEN_run(svgenHandle,&Vab_pu,&(gPwmData.Tabc)); 

    then I got the strange result on my Oscilloscope, as following:

    This is the imagine of Ta & Tc when write to PWMDAC:

    Did you get the same result?

    Thank you for your feedback!

    Tran,

  • Could you send what you are writing to the DACs? From previous posts, you had this:

    gDacData.value[0] = gPwmData.Tabc.value[0];
    gDacData.value[1] = gPwmData.Tabc.value[1];
    gDacData.value[2] = gPwmData.Tabc.value[1] - gPwmData.Tabc.value[2];

    Sounds like what you want to monitor is:

    gDacData.value[0] = gPwmData.Tabc.value[0];
    gDacData.value[1] = gPwmData.Tabc.value[1];
    gDacData.value[2] = gPwmData.Tabc.value[2];

    Is you have the right writes, could you assign Tc to the first DAC to make sure the issue follows Tc and not the value of the third DAC? So try this:

    gDacData.value[0] = gPwmData.Tabc.value[2];
    gDacData.value[1] = gPwmData.Tabc.value[1];
    gDacData.value[2] = gPwmData.Tabc.value[0];

    And see what you get.

    I reviewed the HAL_writeDacData() implementation and I don't see any issues, so let's debug it.

    -Jorge

  • Hi Jorge,

    Thank you for your reply. I followed your way to debug my project, and this it the code section in the main_ISR() in which the HAL_writeDacData() is called:

    //! mainISR() function
    interrupt void mainISR(void)
    {
    	// Declaration of local variables
    	_iq 		angle_pu;					//The angle of space vector
    	MATH_vec2 	Vab_pu;
    	MATH_vec2 	phasor;
    	uint32_t 	timer1Cnt;
    
    	_iq dacValue_1, dacValue_2, dacValue_3, dacValue_4;
      // ------------------------------- FRAMEWORK ----------------------------------------
      // read the timer 1 value and update the CPU usage module
      timer1Cnt = HAL_readTimerCnt(halHandle,1);
      CPU_USAGE_updateCnts(cpu_usageHandle,timer1Cnt);
    
      // acknowledge the ADC interrupt
      HAL_acqAdcInt(halHandle,ADC_IntNumber_1);
    
    // =============================== LEVEL 1 ======================================
    //	  Checks target independent modules, duty cycle waveforms and PWM update
    //	  Keep the motors disconnected at this level!
    // ==============================================================================
    #if (BUILDLEVEL==LEVEL1)
      //
      gFlag_Vdq_Open_Loop = true;
    
      if(gFlag_Vdq_Open_Loop == true)
      {
    	 //notes: Test is OK by Duongtb (21-Nov-2015).
     	 CTRL_incrAngle(&gAngle_Open_Loop, gAngle_Delta_Open_Loop);
    
     	 // get an electrical angle for testing in BUILD_LEVEL_1 from the CTRL module
     	 angle_pu = gAngle_Open_Loop;
      }
      else
      {
    	  // run the ramp (saw-tooth) at 50Hz
    	  // notes: test is OK by Duongtb (18-Nov-2015).
    	  rampgen.freq = _IQ(USER_REF_FREQ_Hz/USER_IQ_FULL_SCALE_FREQ_Hz);
    	  RAMPGEN_run(rampgenHandle);
    
    	  // get an electrical angle for testing in BUILD_LEVEL_1 from the RAMPGEN module
    	  angle_pu = rampgen.output;
      }
    
      // Set the amplitude of the voltage.
      // Notice that, by changing the value of gVdq_Open_Loop.value[1] we will change
      // the amplitude of the voltage vector. The maximum value here is _IQ(1.333333).
      gVdq_out_pu.value[0] = _IQ(0.1); //gVdq_Open_Loop.value[0];
      gVdq_out_pu.value[1] = _IQ(0.5); //gVdq_Open_Loop.value[1];
    
      // compute the sine and cosine phasor values which are part of the inverse
      // Park transform calculations. Once these values are computed,
      // they are copied into the IPARK module, which then uses them to
      // transform the voltages from DQ to Alpha/Beta reference frames.
      phasor.value[0] = _IQcosPU(angle_pu);
      phasor.value[1] = _IQsinPU(angle_pu);
    
      // set the phasor in the inverse Park transform
      IPARK_setPhasor(iparkHandle,&phasor);
    
      // Run the inverse Park module.  This converts the voltage vector from
      // synchronous frame values to stationary frame values.
      IPARK_run(iparkHandle,&gVdq_out_pu,&Vab_pu);
    
      // Now run the space vector generator (SVGEN) module.
      // There is no need to do an inverse CLARKE transform, as this is
      // handled in the SVGEN_run function.
      SVGEN_run(svgenHandle,&Vab_pu,&(gPwmData.Tabc));
    
      dacValue_1 = gPwmData.Tabc.value[0];
      dacValue_2 = gPwmData.Tabc.value[1];
      dacValue_3 = gPwmData.Tabc.value[2];
      dacValue_4 = (_iq)((gPwmData.Tabc.value[0] - gPwmData.Tabc.value[1])>>1);	//Dividing by 2 to nominalize the output signal
    
      // write to the PWM compare registers, and then we are done!
      HAL_writePwmData(halHandle,&gPwmData);
    
    #ifdef DEBUG
      // ------------------------------------------------------------------------------
      //    Connect inputs of the PWMDAC module
      // ------------------------------------------------------------------------------
    /*  gDacData.value[0] = gPwmData.Tabc.value[1];
      gDacData.value[1] = gPwmData.Tabc.value[2];
      gDacData.value[2] = gPwmData.Tabc.value[2];
      gDacData.value[3] = angle_pu;*/
    
      gDacData.value[0] = dacValue_1;
      gDacData.value[1] = dacValue_2;
      gDacData.value[2] = dacValue_3;
      gDacData.value[3] = dacValue_4;
    
      // write the data to DAC module
      HAL_writeDacData(halHandle, &gDacData);
      //HAL_writeDacData_Jorge(halHandle, &gDacData);
      // ------------------------------------------------------------------------------
      //    Connect inputs of the DATALOG module
      // ------------------------------------------------------------------------------
    /*  DlogCh1 = (int16)_IQtoIQ15(svgen1.Ta);
      DlogCh2 = (int16)_IQtoIQ15(svgen1.Tb);
      DlogCh3 = (int16)_IQtoIQ15(svgen1.Tc);
      DlogCh4 = (int16)_IQtoIQ15(svgen1.Tb-svgen1.Tc);*/
    #endif // DEBUG
    
    #endif // (BUILDLEVEL==LEVEL1)
    
      return;
    }

    And this is the result of the above code:

    a) (Ta & Tc) or (Tb & Tc) (gDacData.value[0] vs gDacData.value[2]) - This is a wrong result.

    b) gDacdatavalue[2] and gDacData.value[3] - This is a true result!

    But when I write to gDacData.value[0], gDacData.value[1] and gDacData.value[2] with the same data, as the following code:

      dacValue_1 = gPwmData.Tabc.value[0];
      dacValue_2 = gPwmData.Tabc.value[1];
      dacValue_3 = gPwmData.Tabc.value[2];
      dacValue_4 = (_iq)((gPwmData.Tabc.value[0] - gPwmData.Tabc.value[1])>>1);	//Dividing by 2 to nominalize the output signal
    
      // write to the PWM compare registers, and then we are done!
      HAL_writePwmData(halHandle,&gPwmData);
    
    #ifdef DEBUG
      // ------------------------------------------------------------------------------
      //    Connect inputs of the PWMDAC module
      // ------------------------------------------------------------------------------
    /*  gDacData.value[0] = gPwmData.Tabc.value[1];
      gDacData.value[1] = gPwmData.Tabc.value[2];
      gDacData.value[2] = gPwmData.Tabc.value[2];
      gDacData.value[3] = angle_pu;*/
    
      gDacData.value[0] = dacValue_1;  // The same value for two remaining channels.
      gDacData.value[1] = dacValue_1;
      gDacData.value[2] = dacValue_1;
      gDacData.value[3] = dacValue_4;
    
      // write the data to DAC module
      HAL_writeDacData(halHandle, &gDacData);
      //HAL_writeDacData_Jorge(halHandle, &gDacData);
    
    #endif // DEBUG

    ... and the result is true for all three channels! It's amazing. The result is:

    So that I am in two minds that there are the corruption of gDacData or the HAL_writeDacData() function make the gDacData is corrupted!

    I can send the code for your test with your own kit at 

    Notice that you should copy it to the following path:  C:\ti\motorware\motorware_1_01_00_15\sw\solutions\instaspin_foc\boards\vfdkit_rev1\f28x\f2805xF\projects\ccs5\proj_vfd01a

    proj_vfd01a.rar

  • We are making "some" progress I guess :)

    I tried looking at your code, but there are a lot of files that didn't make it through, so let me ask you, could you send me the file where you implemented HAL_writeDacData()?

    Also, please send me where you initialize the hal handle. This is usually where you have HAL_init function.

    Also, send me the file where you implemented the function: HAL_setupPwmDacs()

    I checked my configuration, and I do see the correct results:

    Thanks

  • Dear Jorge,

    Thank you for continuing to follow this topic. I'm very happy when having a good man discuss about this issue.

    I attached the files that you required in the threat:

    vfd_hal.c
    /* --COPYRIGHT--,BSD
     * Copyright (c) 2015, VPEC Group.
     * All rights reserved.
     *
     * Project:			SimpleVFD
     *
     * Version: 		1.0
     *
     * Target:  		TMS320F2805x
     *
     * Type: 			Device Independent
     *
     * Updated:			28 Oct 2015
     *
     * Programmer:		Duongtb
     *
     * Email:			tran.binhduong@gmail.com
     *
     * --/COPYRIGHT--*/
    //! \file   solutions/instaspin_foc/boards/hvkit_rev1p1/f28x/f2805xF/src/hal.c
    //! \brief Contains the various functions related to the HAL object (everything outside the CTRL system) 
    //!
    //! (C) Copyright 2011, Texas Instruments, Inc.
    
    
    // **************************************************************************
    // the includes
    //! Include to use the Board-Support-Packet (BSP) Definitions
    #include "vfd_settings.h"
    //#include "..\BSP\bsp.h"
    #include "..\HAL\hal_addon.h"
    // drivers
    
    // modules
    
    // platforms
    #include "hal.h"
    #include "user.h"
    #include "hal_obj.h"
    
    
    #ifdef FLASH
    #pragma CODE_SECTION(HAL_setupFlash,"ramfuncs");
    #endif
    
    // **************************************************************************
    // the defines
    #define CPU_RATE   16.667L   // for a 60MHz CPU clock speed (SYSCLKOUT)
    //#define CPU_RATE   20.000L   // for a 50MHz CPU clock speed  (SYSCLKOUT)
    //#define CPU_RATE   25.000L   // for a 40MHz CPU clock speed  (SYSCLKOUT)
    //#define CPU_RATE   33.333L   // for a 30MHz CPU clock speed  (SYSCLKOUT)
    //#define CPU_RATE   41.667L   // for a 24MHz CPU clock speed  (SYSCLKOUT)
    //#define CPU_RATE   50.000L   // for a 20MHz CPU clock speed  (SYSCLKOUT)
    //#define CPU_RATE   66.667L   // for a 15MHz CPU clock speed  (SYSCLKOUT)
    //#define CPU_RATE  100.000L   // for a 10MHz CPU clock speed  (SYSCLKOUT)
    
    // DO NOT MODIFY THIS LINE.
    #define DELAY_US(A)  usDelay(((((long double) A * 1000.0L) / (long double)USER_SYSTEM_CPU_RATE) - 9.0L) / 5.0L)
    
    
    // **************************************************************************
    // the globals
    
    HAL_Obj hal;
    
    
    // **************************************************************************
    // the functions
    
    void HAL_cal(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      // enable the ADC clock
      CLK_enableAdcClock(obj->clkHandle);
    
    
      // Run the Device_cal() function
      // This function copies the ADC and oscillator calibration values from TI reserved
      // OTP into the appropriate trim registers
      // This boot ROM automatically calls this function to calibrate the interal 
      // oscillators and ADC with device specific calibration data.
      // If the boot ROM is bypassed by Code Composer Studio during the development process,
      // then the calibration must be initialized by the application
      ENABLE_PROTECTED_REGISTER_WRITE_MODE;
      (*Device_cal)();
      DISABLE_PROTECTED_REGISTER_WRITE_MODE;
    
      // run offsets calibration in user's memory
      HAL_AdcOffsetSelfCal(handle);
    
      // run oscillator compensation
      //HAL_OscTempComp(handle);
    
      // disable the ADC clock
      CLK_disableAdcClock(obj->clkHandle);
    
      return;
    } // end of HAL_cal() function
    
    
    void HAL_OscTempComp(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
      uint16_t Temperature;
    
      // disable the ADCs
      ADC_disable(obj->adcHandle);
    
      // power up the bandgap circuit
      ADC_enableBandGap(obj->adcHandle);
    
      // set the ADC voltage reference source to internal
      ADC_setVoltRefSrc(obj->adcHandle,ADC_VoltageRefSrc_Int);
    
      // enable the ADC reference buffers
      ADC_enableRefBuffers(obj->adcHandle);
    
      // Set main clock scaling factor (max45MHz clock for the ADC module)
      ADC_setDivideSelect(obj->adcHandle,ADC_DivideSelect_ClkIn_by_2);
    
      // power up the ADCs
      ADC_powerUp(obj->adcHandle);
    
      // enable the ADCs
      ADC_enable(obj->adcHandle);
    
      // enable non-overlap mode
      ADC_enableNoOverlapMode(obj->adcHandle);
    
      // connect channel A5 internally to the temperature sensor
      ADC_setTempSensorSrc(obj->adcHandle, ADC_TempSensorSrc_Int);
    
      // set SOC0 channel select to ADCINA5
      ADC_setSocChanNumber(obj->adcHandle, ADC_SocNumber_0, ADC_SocChanNumber_A5);
    
      // set SOC0 acquisition period to 26 ADCCLK
      ADC_setSocSampleDelay(obj->adcHandle, ADC_SocNumber_0, ADC_SocSampleDelay_64_cycles);
    
      // connect ADCINT1 to EOC0
      ADC_setIntSrc(obj->adcHandle, ADC_IntNumber_1, ADC_IntSrc_EOC0);
    
      // clear ADCINT1 flag
      ADC_clearIntFlag(obj->adcHandle, ADC_IntNumber_1);
    
      // enable ADCINT1
      ADC_enableInt(obj->adcHandle, ADC_IntNumber_1);
    
      // force start of conversion on SOC0
      ADC_setSocFrc(obj->adcHandle, ADC_SocFrc_0);
    
      // wait for end of conversion
      while (ADC_getIntFlag(obj->adcHandle, ADC_IntNumber_1) == 0){}
    
      // clear ADCINT1 flag
      ADC_clearIntFlag(obj->adcHandle, ADC_IntNumber_1);
    
      Temperature = ADC_readResult(obj->adcHandle, ADC_ResultNumber_0);
    
      HAL_osc1Comp(handle, Temperature);
    
      HAL_osc2Comp(handle, Temperature);
    
      return;
    } // end of HAL_OscTempComp() function
    
    
    void HAL_osc1Comp(HAL_Handle handle, const int16_t sensorSample)
    {
    	int16_t compOscFineTrim;
    	HAL_Obj *obj = (HAL_Obj *)handle;
    
    	ENABLE_PROTECTED_REGISTER_WRITE_MODE;
    
        compOscFineTrim = ((sensorSample - getRefTempOffset())*(int32_t)getOsc1FineTrimSlope()
                          + OSC_POSTRIM_OFF + FP_ROUND )/FP_SCALE + getOsc1FineTrimOffset() - OSC_POSTRIM;
    
        if(compOscFineTrim > 31)
          {
            compOscFineTrim = 31;
          }
    	else if(compOscFineTrim < -31)
          {
            compOscFineTrim = -31;
          }
    
        OSC_setTrim(obj->oscHandle, OSC_Number_1, HAL_getOscTrimValue(getOsc1CoarseTrim(), compOscFineTrim));
    
        DISABLE_PROTECTED_REGISTER_WRITE_MODE;
    
        return;
    } // end of HAL_osc1Comp() function
    
    
    void HAL_osc2Comp(HAL_Handle handle, const int16_t sensorSample)
    {
    	int16_t compOscFineTrim;
    	HAL_Obj *obj = (HAL_Obj *)handle;
    
    	ENABLE_PROTECTED_REGISTER_WRITE_MODE;
    
        compOscFineTrim = ((sensorSample - getRefTempOffset())*(int32_t)getOsc2FineTrimSlope()
                          + OSC_POSTRIM_OFF + FP_ROUND )/FP_SCALE + getOsc2FineTrimOffset() - OSC_POSTRIM;
    
        if(compOscFineTrim > 31)
          {
            compOscFineTrim = 31;
          }
    	else if(compOscFineTrim < -31)
          {
            compOscFineTrim = -31;
          }
    
        OSC_setTrim(obj->oscHandle, OSC_Number_2, HAL_getOscTrimValue(getOsc2CoarseTrim(), compOscFineTrim));
    
        DISABLE_PROTECTED_REGISTER_WRITE_MODE;
    
        return;
    } // end of HAL_osc2Comp() function
    
    
    uint16_t HAL_getOscTrimValue(int16_t coarse, int16_t fine)
    {
      uint16_t regValue = 0;
    
      if(fine < 0)
        {
          regValue = ((-fine) | 0x20) << 9;
        }
      else
        {
          regValue = fine << 9;
        }
    
      if(coarse < 0)
        {
          regValue |= ((-coarse) | 0x80);
        }
      else
        {
          regValue |= coarse;
        }
    
      return regValue;
    } // end of HAL_getOscTrimValue() function
    
    
    void HAL_AdcOffsetSelfCal(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
      uint16_t AdcConvMean;
    
      // disable the ADCs
      ADC_disable(obj->adcHandle);
    
      // power up the bandgap circuit
      ADC_enableBandGap(obj->adcHandle);
    
      // set the ADC voltage reference source to internal
      ADC_setVoltRefSrc(obj->adcHandle,ADC_VoltageRefSrc_Int);
    
      // enable the ADC reference buffers
      ADC_enableRefBuffers(obj->adcHandle);
    
      // power up the ADCs
      ADC_powerUp(obj->adcHandle);
    
      // enable the ADCs
      ADC_enable(obj->adcHandle);
    
      //Select VREFLO internal connection on B5
      ADC_enableVoltRefLoConv(obj->adcHandle);
    
      //Select channel B5 for all SOC
      HAL_AdcCalChanSelect(handle, ADC_SocChanNumber_B5);
    
      //Apply artificial offset (+80) to account for a negative offset that may reside in the ADC core
      ADC_setOffTrim(obj->adcHandle, 80);
    
      //Capture ADC conversion on VREFLO
      AdcConvMean = HAL_AdcCalConversion(handle);
    
      //Set offtrim register with new value (i.e remove artical offset (+80) and create a two's compliment of the offset error)
      ADC_setOffTrim(obj->adcHandle, 80 - AdcConvMean);
    
      //Select external ADCIN5 input pin on B5
      ADC_disableVoltRefLoConv(obj->adcHandle);
    
      return;
    } // end of HAL_AdcOffsetSelfCal() function
    
    
    void HAL_AdcCalChanSelect(HAL_Handle handle, const ADC_SocChanNumber_e chanNumber)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_0,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_1,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_2,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_3,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_4,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_5,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_6,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_7,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_8,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_9,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_10,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_11,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_12,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_13,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_14,chanNumber);
      ADC_setSocChanNumber(obj->adcHandle,ADC_SocNumber_15,chanNumber);
    
      return;
    } // end of HAL_AdcCalChanSelect() function
    
    
    uint16_t HAL_AdcCalConversion(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
      uint16_t index, SampleSize, Mean;
      uint32_t Sum;
      ADC_SocSampleDelay_e ACQPS_Value;
    
      index       = 0;     //initialize index to 0
      SampleSize  = 256;   //set sample size to 256 (**NOTE: Sample size must be multiples of 2^x where is an integer >= 4)
      Sum         = 0;     //set sum to 0
      Mean        = 999;   //initialize mean to known value
    
      //Set the ADC sample window to the desired value (Sample window = ACQPS + 1)
      ACQPS_Value = ADC_SocSampleDelay_7_cycles;
    
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_0,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_1,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_2,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_3,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_4,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_5,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_6,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_7,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_8,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_9,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_10,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_11,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_12,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_13,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_14,ACQPS_Value);
      ADC_setSocSampleDelay(obj->adcHandle,ADC_SocNumber_15,ACQPS_Value);
    
      // Enabled ADCINT1 and ADCINT2
      ADC_enableInt(obj->adcHandle, ADC_IntNumber_1);
      ADC_enableInt(obj->adcHandle, ADC_IntNumber_2);
    
      // Disable continuous sampling for ADCINT1 and ADCINT2
      ADC_setIntMode(obj->adcHandle, ADC_IntNumber_1, ADC_IntMode_EOC);
      ADC_setIntMode(obj->adcHandle, ADC_IntNumber_2, ADC_IntMode_EOC);
    
      //ADCINTs trigger at end of conversion
      ADC_setIntPulseGenMode(obj->adcHandle, ADC_IntPulseGenMode_Prior);
    
      // Setup ADCINT1 and ADCINT2 trigger source
      ADC_setIntSrc(obj->adcHandle, ADC_IntNumber_1, ADC_IntSrc_EOC6);
      ADC_setIntSrc(obj->adcHandle, ADC_IntNumber_2, ADC_IntSrc_EOC14);
    
      // Setup each SOC's ADCINT trigger source
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_0, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_1, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_2, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_3, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_4, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_5, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_6, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_7, ADC_Int2TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_8, ADC_Int1TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_9, ADC_Int1TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_10, ADC_Int1TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_11, ADC_Int1TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_12, ADC_Int1TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_13, ADC_Int1TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_14, ADC_Int1TriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_15, ADC_Int1TriggersSOC);
    
      // Delay before converting ADC channels
      usDelay(ADC_DELAY_usec);
    
      ADC_setSocFrcWord(obj->adcHandle, 0x00FF);
    
      while( index < SampleSize )
        {
          //Wait for ADCINT1 to trigger, then add ADCRESULT0-7 registers to sum
          while (ADC_getIntFlag(obj->adcHandle, ADC_IntNumber_1) == 0){}
    
          //Must clear ADCINT1 flag since INT1CONT = 0
          ADC_clearIntFlag(obj->adcHandle, ADC_IntNumber_1);
    
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_0);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_1);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_2);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_3);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_4);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_5);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_6);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_7);
    
          //Wait for ADCINT2 to trigger, then add ADCRESULT8-15 registers to sum
          while (ADC_getIntFlag(obj->adcHandle, ADC_IntNumber_2) == 0){}
    
          //Must clear ADCINT2 flag since INT2CONT = 0
          ADC_clearIntFlag(obj->adcHandle, ADC_IntNumber_2);
    
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_8);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_9);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_10);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_11);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_12);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_13);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_14);
          Sum += ADC_readResult(obj->adcHandle, ADC_ResultNumber_15);
    
          index+=16;
    
      } // end data collection
    
      //Disable ADCINT1 and ADCINT2 to STOP the ping-pong sampling
      ADC_disableInt(obj->adcHandle, ADC_IntNumber_1);
      ADC_disableInt(obj->adcHandle, ADC_IntNumber_2);
    
      //Calculate average ADC sample value
      Mean = Sum / SampleSize;
    
      // Clear start of conversion trigger
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_0, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_1, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_2, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_3, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_4, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_5, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_6, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_7, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_8, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_9, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_10, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_11, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_12, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_13, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_14, ADC_NoIntTriggersSOC);
      ADC_setupSocTrigSrc(obj->adcHandle, ADC_SocNumber_15, ADC_NoIntTriggersSOC);
    
      //return the average
      return(Mean);
    } // end of HAL_AdcCalConversion() function
    
    
    void HAL_disableWdog(HAL_Handle halHandle)
    {
      HAL_Obj *hal = (HAL_Obj *)halHandle;
    
    
      WDOG_disable(hal->wdogHandle);
    
    
      return;
    } // end of HAL_disableWdog() function
    
    
    void HAL_disableGlobalInts(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      CPU_disableGlobalInts(obj->cpuHandle);
    
      return;
    } // end of HAL_disableGlobalInts() function
    
    
    void HAL_enableAdcInts(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      // enable the PIE interrupts associated with the ADC interrupts
      PIE_enableAdcInt(obj->pieHandle,ADC_IntNumber_1);
    
    
      // enable the ADC interrupts
      ADC_enableInt(obj->adcHandle,ADC_IntNumber_1);
    
    
      // enable the cpu interrupt for ADC interrupts
      CPU_enableInt(obj->cpuHandle,CPU_IntNumber_10);
    
      return;
    } // end of HAL_enableAdcInts() function
    
    
    void HAL_enableDebugInt(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      CPU_enableDebugInt(obj->cpuHandle);
    
      return;
    } // end of HAL_enableDebugInt() function
    
    
    void HAL_enableGlobalInts(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      CPU_enableGlobalInts(obj->cpuHandle);
    
      return;
    } // end of HAL_enableGlobalInts() function
    
    
    void HAL_enablePwmInt(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      PIE_enablePwmInt(obj->pieHandle,PWM_Number_1);
    
    
      // enable the interrupt
      PWM_enableInt(obj->pwmHandle[PWM_Number_1]);
    
    
      // enable the cpu interrupt for EPWM1_INT
      CPU_enableInt(obj->cpuHandle,CPU_IntNumber_3);
    
      return;
    } // end of HAL_enablePwmInt() function
    
    
    void HAL_setupFaults(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
      uint_least8_t cnt;
    
    
      // Configure Trip Mechanism for the Motor control software
      // -Cycle by cycle trip on CPU halt
      // -One shot fault trip zone
      // These trips need to be repeated for EPWM1 ,2 & 3
      for(cnt=0;cnt<3;cnt++)
        {
          PWM_enableTripZoneSrc(obj->pwmHandle[cnt],PWM_TripZoneSrc_CycleByCycle_TZ6_NOT);
    
          PWM_enableTripZoneSrc(obj->pwmHandle[cnt],PWM_TripZoneSrc_OneShot_TZ1_NOT);
    
          // What do we want the OST/CBC events to do?
          // TZA events can force EPWMxA
          // TZB events can force EPWMxB
    
          PWM_setTripZoneState_TZA(obj->pwmHandle[cnt],PWM_TripZoneState_EPWM_Low);
          PWM_setTripZoneState_TZB(obj->pwmHandle[cnt],PWM_TripZoneState_EPWM_Low);
    
          // Clear faults from flip flop
          GPIO_setLow(obj->gpioHandle,GPIO_Number_9);
          GPIO_setHigh(obj->gpioHandle,GPIO_Number_9);
        }
    
      return;
    } // end of HAL_setupFaults() function
    
    //! \Brief Initializes all Hardware Drives that is used in the project.
    HAL_Handle HAL_init(void *pMemory,const size_t numBytes)
    {
      uint_least8_t cnt;
      HAL_Handle handle;
      HAL_Obj *obj;
    
      volatile int tmp;
    
      ENABLE_PROTECTED_REGISTER_WRITE_MODE;
    
      tmp = *(int *)0x003D7A18;
      tmp = *(int *)0x003D7A19;
      tmp = *(int *)0x003D7A1a;
      tmp = *(int *)0x003D7A1b;
      *(int *)0xB90 = 0xCEEC;
      *(int *)0xB91 = 0x4CA7;
      *(int *)0xB92 = 0xAAEB;
      *(int *)0xB93 = 0x7030;
    
      DISABLE_PROTECTED_REGISTER_WRITE_MODE;
    
    
      if(numBytes < sizeof(HAL_Obj))
        return((HAL_Handle)NULL);
    
    
      // assign the handle
      handle = (HAL_Handle)pMemory;
    
    
      // assign the object
      obj = (HAL_Obj *)handle;
    
    
      // initialize the watchdog driver
      obj->wdogHandle = WDOG_init((void *)WDOG_BASE_ADDR,sizeof(WDOG_Obj));
    
    
      // disable watchdog
      HAL_disableWdog(handle);
    
    
      // initialize the AFE
      obj->afeHandle = AFE_init((void *)AFE_BASE_ADDR,sizeof(AFE_Obj));
    
    
      // initialize the ADC
      obj->adcHandle = ADC_init((void *)ADC_BASE_ADDR,sizeof(ADC_Obj));
    
    
      // initialize the clock handle
      obj->clkHandle = CLK_init((void *)CLK_BASE_ADDR,sizeof(CLK_Obj));
    
    
      // initialize the CPU handle
      obj->cpuHandle = CPU_init(&cpu,sizeof(cpu));
    
    
      // initialize the FLASH handle
      obj->flashHandle = FLASH_init((void *)FLASH_BASE_ADDR,sizeof(FLASH_Obj));
    
    
      // initialize the GPIO handle
      obj->gpioHandle = GPIO_init((void *)GPIO_BASE_ADDR,sizeof(GPIO_Obj));
    
    
      // initialize the current offset estimator handles
      for(cnt=0;cnt<USER_NUM_CURRENT_SENSORS;cnt++)
        {
          obj->offsetHandle_I[cnt] = OFFSET_init(&obj->offset_I[cnt],sizeof(obj->offset_I[cnt]));
        }
    
    
      // initialize the voltage offset estimator handles
      for(cnt=0;cnt<USER_NUM_VOLTAGE_SENSORS;cnt++)
        {
          obj->offsetHandle_V[cnt] = OFFSET_init(&obj->offset_V[cnt],sizeof(obj->offset_V[cnt]));
        }
    
    
      // initialize the oscillator handle
      obj->oscHandle = OSC_init((void *)OSC_BASE_ADDR,sizeof(OSC_Obj));
    
    
      // initialize the PIE handle
      obj->pieHandle = PIE_init((void *)PIE_BASE_ADDR,sizeof(PIE_Obj));
    
    
      // initialize the PLL handle
      obj->pllHandle = PLL_init((void *)PLL_BASE_ADDR,sizeof(PLL_Obj));
    
    
      // initialize PWM handles
      obj->pwmHandle[0] = PWM_init((void *)PWM_ePWM1_BASE_ADDR,sizeof(PWM_Obj));
      obj->pwmHandle[1] = PWM_init((void *)PWM_ePWM2_BASE_ADDR,sizeof(PWM_Obj));
      obj->pwmHandle[2] = PWM_init((void *)PWM_ePWM3_BASE_ADDR,sizeof(PWM_Obj));
    
    #ifdef DEBUG
      // initialize PWM DAC handles
      obj->pwmDacHandle[0] = PWMDAC_init((void *)PWM_ePWM6_BASE_ADDR,sizeof(PWM_Obj)); //Using PWM6A/B
      obj->pwmDacHandle[1] = PWMDAC_init((void *)PWM_ePWM7_BASE_ADDR,sizeof(PWM_Obj)); //Using PWM7A/B
      //obj->pwmDacHandle[2] = PWMDAC_init((void *)PWM_ePWM5_BASE_ADDR,sizeof(PWM_Obj)); //Using PWM5A/B
      obj->pwmDacHandle[2] = (PWM_Handle)NULL;
    #endif //DEBUG
    
      // initialize power handle
      obj->pwrHandle = PWR_init((void *)PWR_BASE_ADDR,sizeof(PWR_Obj));
    
    
      // initialize timer handles
      obj->timerHandle[0] = TIMER_init((void *)TIMER0_BASE_ADDR,sizeof(TIMER_Obj));
      obj->timerHandle[1] = TIMER_init((void *)TIMER1_BASE_ADDR,sizeof(TIMER_Obj));
      obj->timerHandle[2] = TIMER_init((void *)TIMER2_BASE_ADDR,sizeof(TIMER_Obj));
    
    #ifdef QEP
      // initialize QEP driver
      obj->qepHandle[0] = QEP_init((void*)QEP1_BASE_ADDR,sizeof(QEP_Obj));
    #endif
    
      return(handle);
    } // end of HAL_init() function
    
    
    //! \Brief Setting up the parameters which is used in every module of HAL
    void HAL_setParams(HAL_Handle handle,const USER_Params *pUserParams)
    {
      uint_least8_t cnt;
      HAL_Obj *obj = (HAL_Obj *)handle;
      _iq beta_lp_pu = _IQ(pUserParams->offsetPole_rps/(float_t)pUserParams->ctrlFreq_Hz);
    
    
      HAL_setNumCurrentSensors(handle,pUserParams->numCurrentSensors);
      HAL_setNumVoltageSensors(handle,pUserParams->numVoltageSensors);
    
    
      for(cnt=0;cnt<HAL_getNumCurrentSensors(handle);cnt++)
        {
          HAL_setOffsetBeta_lp_pu(handle,HAL_SensorType_Current,cnt,beta_lp_pu);
          HAL_setOffsetInitCond(handle,HAL_SensorType_Current,cnt,_IQ(0.0));
          HAL_setOffsetValue(handle,HAL_SensorType_Current,cnt,_IQ(0.0));
        }
    
    
      for(cnt=0;cnt<HAL_getNumVoltageSensors(handle);cnt++)
        {
          HAL_setOffsetBeta_lp_pu(handle,HAL_SensorType_Voltage,cnt,beta_lp_pu);
          HAL_setOffsetInitCond(handle,HAL_SensorType_Voltage,cnt,_IQ(0.0));
          HAL_setOffsetValue(handle,HAL_SensorType_Voltage,cnt,_IQ(0.0));
        }
    
    
      // disable global interrupts
      CPU_disableGlobalInts(obj->cpuHandle);
    
    
      // disable cpu interrupts
      CPU_disableInts(obj->cpuHandle);
    
    
      // clear cpu interrupt flags
      CPU_clearIntFlags(obj->cpuHandle);
    
    
      // setup the clocks: Select OSC1 channel and using the internal Osc. source
      HAL_setupClks(handle);
    
    
      // Setup the PLL to configure SYSCLKOUT = 60MHz
      HAL_setupPll(handle,PLL_ClkFreq_60_MHz);
    
    
      // setup the PIE
      HAL_setupPie(handle);
    
    
      // run the device calibration
      HAL_cal(handle);
    
    
      // setup the peripheral clocks
      HAL_setupPeripheralClks(handle);
    
    
      // setup the GPIOs
      HAL_setupGpios(handle);
    
    
      // enable the appropriate pgas
      HAL_setupAfe(handle);
    
    
      // setup the flash
      HAL_setupFlash(handle);
    
    
      // setup the ADCs
      HAL_setupAdcs(handle);
    
    
      // setup the PWMs
      HAL_setupPwms(handle,
                    (float_t)pUserParams->systemFreq_MHz,
                    pUserParams->pwmPeriod_usec,
                    USER_NUM_PWM_TICKS_PER_ISR_TICK);
    
    #ifdef QEP
      // setup the QEP
      HAL_setupQEP(handle,HAL_Qep_QEP1);
    #endif
    
    #ifdef DEBUG
      // setup the PWM DACs
      HAL_setupPwmDacs(handle);
    #endif
    
      // setup the timers
      HAL_setupTimers(handle,
                      (float_t)pUserParams->systemFreq_MHz);
    
    
      // set the default current bias
      {
       uint_least8_t cnt;
       _iq bias = _IQ12mpy(ADC_dataBias,_IQ(pUserParams->current_sf));
       
       for(cnt=0;cnt<HAL_getNumCurrentSensors(handle);cnt++)
         {
           HAL_setBias(handle,HAL_SensorType_Current,cnt,bias);
         }
      }
    
    
      //  set the current scale factor
      {
       _iq current_sf = _IQ(pUserParams->current_sf);
    
      HAL_setCurrentScaleFactor(handle,current_sf);
      }
    
    
      // set the default voltage bias
      {
       uint_least8_t cnt;
       _iq bias = _IQ(0.0);
       
       for(cnt=0;cnt<HAL_getNumVoltageSensors(handle);cnt++)
         {
           HAL_setBias(handle,HAL_SensorType_Voltage,cnt,bias);
         }
      }
    
    
      //  set the voltage scale factor
     {
       _iq voltage_sf = _IQ(pUserParams->voltage_sf);
    
      HAL_setVoltageScaleFactor(handle,voltage_sf);
     }
    
     return;
    } // end of HAL_setParams() function
    
    
    void HAL_setupAfe(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      // enable the PGA amplifiers
      AFE_enablePGA(obj->afeHandle, (AFE_PGAEN_e)(AFE_AMPA1EN   \
                                                | AFE_AMPB1EN   \
                                                | AFE_AMPA3EN   \
                                                | AFE_AMPB7EN   \
                                                | AFE_AMPB6EN   \
                                                | AFE_AMPB4EN));
    
    //  // enable VrefOut
    //  AFE_enableVrefOut(obj->afeHandle);
    //
    //  // set the VrefOut voltage
    //  AFE_setVrefOut(obj->afeHandle,47);
    //
    //  // enable the DACs for the M1 comparators
    //  AFE_enableDAC(obj->afeHandle, (AFE_DACEN_e)(AFE_DAC1EN  \
    //                                            | AFE_DAC2EN));
    //
    //  // set the DAC output voltage
    //  AFE_setDacCtl(obj->afeHandle, AFE_DAC1, (31 + 7));
    //  AFE_setDacCtl(obj->afeHandle, AFE_DAC2, (31 - 8));
    //
    //  // enable the M1 comparators
    //  AFE_enableComp(obj->afeHandle, (AFE_COMPEN_e)(AFE_COMPA1EN    \
    //                                              | AFE_COMPA3EN    \
    //                                              | AFE_COMPB1EN));
    //
    //  // Bypass the M1 system digital filter, send both high and low of M1 comparator outputs
    //  // to the digital filter subsystem
    //  AFE_setA1CompSubsystem(obj->afeHandle, (AFE_CTRIPxxICTL_FIELDS_e)(AFE_CTRIPOUTBYP \
    //                                                                  | AFE_CTRIPBYP    \
    //                                                                  | AFE_COMPLINPEN  \
    //                                                                  | AFE_COMPHINPEN  \
    //                                                                  | AFE_COMPLPOL));
    //
    //  AFE_setB1CompSubsystem(obj->afeHandle, (AFE_CTRIPxxICTL_FIELDS_e)(AFE_CTRIPOUTBYP \
    //                                                                  | AFE_CTRIPBYP    \
    //                                                                  | AFE_COMPLINPEN  \
    //                                                                  | AFE_COMPHINPEN  \
    //                                                                  | AFE_COMPLPOL));
    //
    //  AFE_setA3CompSubsystem(obj->afeHandle, (AFE_CTRIPxxICTL_FIELDS_e)(AFE_CTRIPOUTBYP \
    //                                                                  | AFE_CTRIPBYP    \
    //                                                                  | AFE_COMPLINPEN  \
    //                                                                  | AFE_COMPHINPEN  \
    //                                                                  | AFE_COMPLPOL));
    //
    //  // Enable the M1 CTRIP output to the GPIO
    //  AFE_setM1CtripOut(obj->afeHandle, (AFE_CTRIPMxOCTL_FIELDS_e)(AFE_CTRIPA1OUTEN));
    
      return;
    }
    
    /*
     * **********************************************************************************
     * Function		: HAL_setupAdcs()
     *
     * Description	: Set up the ADC module according to requirements of the project
     *
     * Argument (s)	: HAL handle
     *
     * Return (s)	: none
     *
     * Note (s)		: This routine should be called one time at system initialization
     *
     * **********************************************************************************
    */
    void HAL_setupAdcs(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      // disable the ADCs. Always do this before going to set up ADC!
      ADC_disable(obj->adcHandle);
    
    
      // power up the bandgap circuit
      ADC_enableBandGap(obj->adcHandle);
    
    
      // set the ADC voltage reference source to internal 
      ADC_setVoltRefSrc(obj->adcHandle,ADC_VoltageRefSrc_Int);
    
    
      // enable the ADC reference buffers
      ADC_enableRefBuffers(obj->adcHandle);
    
    
      // power up the ADCs
      ADC_powerUp(obj->adcHandle);
    
    
      // enable the ADCs
      ADC_enable(obj->adcHandle);
    
    
      // set the ADC interrupt pulse generation to prior
      ADC_setIntPulseGenMode(obj->adcHandle,ADC_IntPulseGenMode_Prior);
    
    
      // set the temperature sensor source to external
      ADC_setTempSensorSrc(obj->adcHandle,ADC_TempSensorSrc_Ext);
    
    
      // configure the interrupt sources
      ADC_disableInt(obj->adcHandle,ADC_IntNumber_1);
      ADC_setIntMode(obj->adcHandle,ADC_IntNumber_1,ADC_IntMode_ClearFlag);
      ADC_setIntSrc(obj->adcHandle,ADC_IntNumber_1,ADC_IntSrc_EOC8);
    
      // configure the priority level for SOC
      //! Note: Added by Duongtb in 30-Oct-2015
      ADC_setSocPriorityNumber(obj->adcHandle, ADC_SocPriorityNumber_8);		//!> SOC0-8 is high priority, SOC9-15 round robin
      ADC_setPositionRRPointer(obj->adcHandle, ADC_PositionRRPointer_15);		//!> SOC15 last converted, SOC9 highest priority in the round robin group.
    
      //! Configure the SOCs for vfdkit_rev1p1 (the commercial version 1.0)
      //! Brief: Configure ADC to be triggered from EPWM1 Period event. Mapping channels to ADC Pins
      //! NOTE: The dummy reads are to account for first sample issue in Rev 0 silicon errata workaround (See more in sprz342f)
      //!       The following ADC channels is triggered every 1/10kHz = 100uS (See more in setting of EPWM1)
      ADC_setSocChanNumber(obj->adcHandle,	ADC_SocNumber_0,	ADC_I_PHU);						//!> ADC 1st priority: #I_Ph_U (Note: ADCRESULT0 is ignored)
      ADC_setSocTrigSrc(obj->adcHandle,		ADC_SocNumber_0,	ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,	ADC_SocNumber_0,	ADC_SocSampleDelay_7_cycles);
    
      ADC_setSocChanNumber(obj->adcHandle,	ADC_SocNumber_1,	ADC_I_PHU);						//!> ADC 2nd priority: ADCRESULT1 = #I_Ph_U
      ADC_setSocTrigSrc(obj->adcHandle,		ADC_SocNumber_1,	ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,	ADC_SocNumber_1,	ADC_SocSampleDelay_7_cycles);
    
      ADC_setSocChanNumber(obj->adcHandle,	ADC_SocNumber_2,	ADC_I_PHW);						//!> ADC 3rd priority: ADCRESULT2 = #I_Ph_W
      ADC_setSocTrigSrc(obj->adcHandle,		ADC_SocNumber_2,	ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,	ADC_SocNumber_2,	ADC_SocSampleDelay_7_cycles);
    
      ADC_setSocChanNumber(obj->adcHandle,	ADC_SocNumber_3,	ADC_I_DCBus);					//!> ADC 4th priority: ADCRESULT3 = #I_DCbus
      ADC_setSocTrigSrc(obj->adcHandle,		ADC_SocNumber_3,	ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,	ADC_SocNumber_3,	ADC_SocSampleDelay_7_cycles);
    
      ADC_setSocChanNumber(obj->adcHandle,	ADC_SocNumber_4,	ADC_V_DCBus);					//!> ADC 5th priority: ADCRESULT4 = #V_DCbus
      ADC_setSocTrigSrc(obj->adcHandle,		ADC_SocNumber_4,	ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,	ADC_SocNumber_4,	ADC_SocSampleDelay_7_cycles);
    
      ADC_setSocChanNumber(obj->adcHandle,	ADC_SocNumber_5,	ADC_V_BIAS);					//!> ADC 6th priority: ADCRESULT5 = #V_Bias (1.65V)
      ADC_setSocTrigSrc(obj->adcHandle,		ADC_SocNumber_5,	ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,	ADC_SocNumber_5,	ADC_SocSampleDelay_7_cycles);
    
      ADC_setSocChanNumber(obj->adcHandle,	ADC_SocNumber_6,	ADC_V_PHW);						//!> ADC 7th priority: ADCRESULT6 = #V_Ph_W
      ADC_setSocTrigSrc(obj->adcHandle,		ADC_SocNumber_6,	ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,	ADC_SocNumber_6,	ADC_SocSampleDelay_7_cycles);
    
      ADC_setSocChanNumber(obj->adcHandle,	ADC_SocNumber_7,	ADC_V_PHU);						//!> ADC 8th priority: ADCRESULT7 = #V_Ph_U
      ADC_setSocTrigSrc(obj->adcHandle,		ADC_SocNumber_7,	ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,	ADC_SocNumber_7,	ADC_SocSampleDelay_7_cycles);
    
      ADC_setSocChanNumber(obj->adcHandle,	ADC_SocNumber_8,	ADC_V_PHV);						//!> ADC 9th priority: ADCRESULT8 = #V_Ph_V
      ADC_setSocTrigSrc(obj->adcHandle,		ADC_SocNumber_8,	ADC_SocTrigSrc_EPWM1_ADCSOCA);
      ADC_setSocSampleDelay(obj->adcHandle,	ADC_SocNumber_8,	ADC_SocSampleDelay_7_cycles);
    
      //! \Note: The following ADC channels is triggered every 10mS (See more in setting of CPU_TIMER0)
      //!		 Round-robbin is selected for these ADC channels
      ADC_setSocChanNumber(obj->adcHandle,	ADC_SocNumber_9,	ADC_TEMP_BOARD);				//!> ADC 10th priority: ADCRESULT9 = #V_Ph_V
      ADC_setSocTrigSrc(obj->adcHandle,		ADC_SocNumber_9,	ADC_SocTrigSrc_CpuTimer_0);
      ADC_setSocSampleDelay(obj->adcHandle,	ADC_SocNumber_9,	ADC_SocSampleDelay_7_cycles);
    
      ADC_setSocChanNumber(obj->adcHandle,	ADC_SocNumber_10,	ADC_TEMP_IGBT);					//!> ADC 11th priority: ADCRESULT10 = #V_Ph_V
      ADC_setSocTrigSrc(obj->adcHandle,		ADC_SocNumber_10,	ADC_SocTrigSrc_CpuTimer_0);
      ADC_setSocSampleDelay(obj->adcHandle,	ADC_SocNumber_10,	ADC_SocSampleDelay_7_cycles);
    
      ADC_setSocChanNumber(obj->adcHandle,	ADC_SocNumber_10,	ADC_AI1);						//!> ADC 12th priority: ADCRESULT11 = #V_Ph_V
      ADC_setSocTrigSrc(obj->adcHandle,		ADC_SocNumber_10,	ADC_SocTrigSrc_CpuTimer_0);
      ADC_setSocSampleDelay(obj->adcHandle,	ADC_SocNumber_10,	ADC_SocSampleDelay_7_cycles);
    
      ADC_setSocChanNumber(obj->adcHandle,	ADC_SocNumber_10,	ADC_AI2);						//!> ADC 13th priority: ADCRESULT12 = #V_Ph_V
      ADC_setSocTrigSrc(obj->adcHandle,		ADC_SocNumber_10,	ADC_SocTrigSrc_CpuTimer_0);
      ADC_setSocSampleDelay(obj->adcHandle,	ADC_SocNumber_10,	ADC_SocSampleDelay_7_cycles);
    
      ADC_setSocChanNumber(obj->adcHandle,	ADC_SocNumber_10,	ADC_AI3);						//!> ADC 14th priority: ADCRESULT13 = #V_Ph_V
      ADC_setSocTrigSrc(obj->adcHandle,		ADC_SocNumber_10,	ADC_SocTrigSrc_CpuTimer_0);
      ADC_setSocSampleDelay(obj->adcHandle,	ADC_SocNumber_10,	ADC_SocSampleDelay_7_cycles);
    
      return;
    } // end of HAL_setupAdcs() function
    
    
    void HAL_setupClks(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      // enable internal oscillator 1
      CLK_enableOsc1(obj->clkHandle);
    
      // set the oscillator source
      CLK_setOscSrc(obj->clkHandle,CLK_OscSrc_Internal);
    
      // disable the external clock in
      CLK_disableClkIn(obj->clkHandle);
    
      // disable the crystal oscillator
      CLK_disableCrystalOsc(obj->clkHandle);
    
      // disable oscillator 2
      CLK_disableOsc2(obj->clkHandle);
    
      // set the low speed clock prescaler
      CLK_setLowSpdPreScaler(obj->clkHandle,CLK_LowSpdPreScaler_SysClkOut_by_1);
    
      // set the clock out prescaler
      CLK_setClkOutPreScaler(obj->clkHandle,CLK_ClkOutPreScaler_SysClkOut_by_1);
    
      return;
    } // end of HAL_setupClks() function
    
    // This function initializes the Flash Control registers
    //
    //                   CAUTION
    // This function MUST be executed out of RAM. Executing it
    // out of OTP/Flash will yield unpredictable results
    void HAL_setupFlash(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
      //                CAUTION
      //Minimum waitstates required for the flash operating
      //at a given CPU rate must be characterized by TI.
      //Refer to the datasheet for the latest information.
    
      FLASH_enablePipelineMode(obj->flashHandle);
    
      FLASH_setNumPagedReadWaitStates(obj->flashHandle,FLASH_NumPagedWaitStates_2);
    
      FLASH_setNumRandomReadWaitStates(obj->flashHandle,FLASH_NumRandomWaitStates_2);
    
      FLASH_setOtpWaitStates(obj->flashHandle,FLASH_NumOtpWaitStates_3);
    
      FLASH_setStandbyWaitCount(obj->flashHandle,FLASH_STANDBY_WAIT_COUNT_DEFAULT);
    
      FLASH_setActiveWaitCount(obj->flashHandle,FLASH_ACTIVE_WAIT_COUNT_DEFAULT);
    
      return;
    } // HAL_setupFlash() function
    
    
    void HAL_setupGpios(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      // PWM1
      GPIO_setMode(obj->gpioHandle,GPIO_Number_0,GPIO_0_Mode_EPWM1A);
    
      // PWM2
      GPIO_setMode(obj->gpioHandle,GPIO_Number_1,GPIO_1_Mode_EPWM1B);
    
      // PWM3
      GPIO_setMode(obj->gpioHandle,GPIO_Number_2,GPIO_2_Mode_EPWM2A);
    
      // PWM4
      GPIO_setMode(obj->gpioHandle,GPIO_Number_3,GPIO_3_Mode_EPWM2B);
    
      // PWM5
      GPIO_setMode(obj->gpioHandle,GPIO_Number_4,GPIO_4_Mode_EPWM3A);
    
      // PWM6
      GPIO_setMode(obj->gpioHandle,GPIO_Number_5,GPIO_5_Mode_EPWM3B);
    
      // GPIO06: PIN FUNCTION = #BRAKING_CTRL
      GPIO_setMode(obj->gpioHandle,GPIO_Number_6,GPIO_6_Mode_GeneralPurpose);
    
      // GPIO07: PIN FUNCTION = #FAN_CTRL
      GPIO_setMode(obj->gpioHandle,GPIO_Number_7,GPIO_7_Mode_GeneralPurpose);
    
      // GPIO-08: PIN FUNCTION = #RELAY1
      GPIO_setMode(obj->gpioHandle,GPIO_Number_8,GPIO_8_Mode_GeneralPurpose);
      GPIO_setHigh(obj->gpioHandle,GPIO_Number_8);
      GPIO_setDirection(obj->gpioHandle,GPIO_Number_8,GPIO_Direction_Output);
    
      // GPIO-09: PIN FUNCTION = #RELAY2
      GPIO_setMode(obj->gpioHandle,GPIO_Number_9,GPIO_9_Mode_GeneralPurpose);
      GPIO_setHigh(obj->gpioHandle,GPIO_Number_9);
      GPIO_setDirection(obj->gpioHandle,GPIO_Number_9,GPIO_Direction_Output);
    
    #ifdef DEBUG
      // GPIO-10: PIN FUNCTION = DAC-PWM-1
      GPIO_setMode(obj->gpioHandle,GPIO_Number_10,GPIO_10_Mode_EPWM6A);
    #else
      // GPIO-10: PIN FUNCTION = DIO#0
      GPIO_setMode(obj->gpioHandle,GPIO_Number_10,GPIO_10_Mode_GeneralPurpose);
      GPIO_setDirection(obj->gpioHandle,GPIO_Number_10,GPIO_Direction_Input);
    #endif //DEBUG
    
    #ifdef DEBUG
      // GPIO-11: PIN FUNCTION = DAC-PWM-2
      GPIO_setMode(obj->gpioHandle,GPIO_Number_11,GPIO_11_Mode_EPWM6B);
    #else
      // GPIO-11: PIN FUNCTION = DIO#0
      GPIO_setMode(obj->gpioHandle,GPIO_Number_11,GPIO_11_Mode_GeneralPurpose);
      GPIO_setDirection(obj->gpioHandle,GPIO_Number_11,GPIO_Direction_Input);
    #endif //DEBUG
    
      // GPIO-12 - PIN FUNCTION = TZ-1
      GPIO_setMode(obj->gpioHandle,GPIO_Number_12,GPIO_12_Mode_TZ1_NOT_CTRIPM1OUT);
    
      // GPIO-13 - PIN FUNCTION = --Spare--
      GPIO_setMode(obj->gpioHandle,GPIO_Number_13,GPIO_13_Mode_GeneralPurpose);
    
      // GPIO-14 - PIN FUNCTION = --Spare--
      GPIO_setMode(obj->gpioHandle,GPIO_Number_14,GPIO_14_Mode_GeneralPurpose);
    
      // GPIO-15 - PIN FUNCTION = --Spare--
      GPIO_setMode(obj->gpioHandle,GPIO_Number_15,GPIO_15_Mode_GeneralPurpose);
    
      // Set Qualification Period for GPIO16-23, 15*2*(1/60MHz) = 0.50us
      GPIO_setQualificationPeriod(obj->gpioHandle,GPIO_Number_16,15);
    
      // GPIO-16 - PIN FUNCTION = SPISIMOA (Connected to U27-MCP4922; an external DAC-SPI)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_16,GPIO_16_Mode_GeneralPurpose);
    
      // GPIO-17 - PIN FUNCTION = SPISOMIA (Connected to U27-MCP4922; an external DAC-SPI)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_17,GPIO_17_Mode_GeneralPurpose);
    
      // GPIO-18 - PIN FUNCTION = SPICLKA (Connected to U27-MCP4922; an external DAC-SPI)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_18,GPIO_18_Mode_GeneralPurpose);
    
      // GPIO-19 - PIN FUNCTION = SPISTEA (Connected to U27-MCP4922; an external DAC-SPI)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_19,GPIO_19_Mode_GeneralPurpose);
    
    #ifdef QEP
      // GPIO-20: PIN FUNCTION = EQEPA
      GPIO_setMode(obj->gpioHandle,GPIO_Number_20,GPIO_20_Mode_EQEP1A);
      GPIO_setQualification(obj->gpioHandle,GPIO_Number_20,GPIO_Qual_Sample_3);
    
      // GPIO-21: PIN FUNCTION = EQEPB
      GPIO_setMode(obj->gpioHandle,GPIO_Number_21,GPIO_21_Mode_EQEP1B);
      GPIO_setQualification(obj->gpioHandle,GPIO_Number_21,GPIO_Qual_Sample_3);
    
      // GPIO-22: PIN FUNCTION = #DC-BUS Charging (OUTPUT)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_22,GPIO_22_Mode_GeneralPurpose);
      GPIO_setLow(obj->gpioHandle,GPIO_Number_22);
      GPIO_setDirection(obj->gpioHandle,GPIO_Number_22,GPIO_Direction_Output);
    
      // GPIO-23: PIN FUNCTION = EQEP1I
      GPIO_setMode(obj->gpioHandle,GPIO_Number_23,GPIO_23_Mode_EQEP1I);
      GPIO_setQualification(obj->gpioHandle,GPIO_Number_23,GPIO_Qual_Sample_3);
    #else
      // GPIO-20: PIN FUNCTION = --Spare--
      GPIO_setMode(obj->gpioHandle,GPIO_Number_20,GPIO_20_Mode_GeneralPurpose);
    
      // GPIO-21: PIN FUNCTION = --Spare--
      GPIO_setMode(obj->gpioHandle,GPIO_Number_21,GPIO_21_Mode_GeneralPurpose);
    
      // GPIO-22: PIN FUNCTION = #DC-BUS Charging (OUTPUT)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_22,GPIO_22_Mode_GeneralPurpose);
      GPIO_setLow(obj->gpioHandle,GPIO_Number_22);
      GPIO_setDirection(obj->gpioHandle,GPIO_Number_22,GPIO_Direction_Output);
    
      // GPIO-23: PIN FUNCTION = --Spare--
      GPIO_setMode(obj->gpioHandle,GPIO_Number_23,GPIO_23_Mode_GeneralPurpose);
    #endif
    
      // GPIO-24: PIN FUNCTION = #DIO7 (Default = Input)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_24,GPIO_24_Mode_GeneralPurpose);
      GPIO_setDirection(obj->gpioHandle,GPIO_Number_22,GPIO_Direction_Input);
    
      // GPIO-25: PIN FUNCTION = #DIO-CS (Connected to U22-74HC157)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_25,GPIO_25_Mode_GeneralPurpose);
    
      // GPIO-26: PIN FUNCTION = #DAC-LD (Connected to U27-MCP4922; an external DAC-SPI)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_26,GPIO_26_Mode_GeneralPurpose);
    
      // GPIO-27: PIN FUNCTION = #CLR-FLT (The reset signal for system errors)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_27,GPIO_27_Mode_GeneralPurpose);
      GPIO_setLow(obj->gpioHandle,GPIO_Number_27);
      GPIO_setDirection(obj->gpioHandle,GPIO_Number_22,GPIO_Direction_Output);
    
    #ifdef DEBUG
      // GPIO-28: PIN FUNCTION = #SCI-RX-A
      GPIO_setMode(obj->gpioHandle,GPIO_Number_28,GPIO_28_Mode_SCIRXDA);
    
      // GPIO-29: PIN FUNCTION = #SCI-TX-A
      GPIO_setMode(obj->gpioHandle,GPIO_Number_29,GPIO_29_Mode_SCITXDA);
    #else
      // GPIO-28: PIN FUNCTION = --Spare--
      GPIO_setMode(obj->gpioHandle,GPIO_Number_28,GPIO_28_Mode_GeneralPurpose);
    
      // GPIO-29: PIN FUNCTION = --Spare--
      GPIO_setMode(obj->gpioHandle,GPIO_Number_29,GPIO_29_Mode_GeneralPurpose);
    #endif //DEBUG
    
      // GPIO-30: PIN FUNCTION = #CAN-RX
      GPIO_setMode(obj->gpioHandle,GPIO_Number_30,GPIO_30_Mode_CANRXA);
    
      // GPIO-31: PIN FUNCTION = #CAN-TX
      GPIO_setMode(obj->gpioHandle,GPIO_Number_31,GPIO_31_Mode_CANTXA);
    
      // GPIO-32: PIN FUNCTION = I2C-SDA (Connected to an external I2C port)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_32,GPIO_32_Mode_GeneralPurpose);
    
      // GPIO-33: PIN FUNCTION = I2C-SCL (Connected to an external I2C port)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_33,GPIO_33_Mode_GeneralPurpose);
    
      // GPIO34: PIN FUNCTION = LED_RUN (YELLOW or BLUE leds)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_34,GPIO_34_Mode_GeneralPurpose);
      GPIO_setLow(obj->gpioHandle,GPIO_Number_34);
      GPIO_setDirection(obj->gpioHandle,GPIO_Number_34,GPIO_Direction_Output);
    
      // JTAG
      GPIO_setMode(obj->gpioHandle,GPIO_Number_35,GPIO_35_Mode_JTAG_TDI);
      GPIO_setMode(obj->gpioHandle,GPIO_Number_36,GPIO_36_Mode_JTAG_TMS);
      GPIO_setMode(obj->gpioHandle,GPIO_Number_37,GPIO_37_Mode_JTAG_TDO);
      GPIO_setMode(obj->gpioHandle,GPIO_Number_38,GPIO_38_Mode_JTAG_TCK);
    
      // GPIO-39: PIN FUNCTION = LED_ERR (RED led)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_39,GPIO_39_Mode_GeneralPurpose);
      GPIO_setLow(obj->gpioHandle,GPIO_Number_39);
      GPIO_setDirection(obj->gpioHandle,GPIO_Number_39,GPIO_Direction_Output);
    
    #ifdef DEBUG
      // GPIO-40: PIN FUNCTION = #DAC-PWM3
      GPIO_setMode(obj->gpioHandle,GPIO_Number_40,GPIO_40_Mode_EPWM7A);
    #else
      // GPIO-40: PIN FUNCTION = #DIO2 (Default: Input)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_40,GPIO_40_Mode_GeneralPurpose);
      GPIO_setDirection(obj->gpioHandle,GPIO_Number_40,GPIO_Direction_Input);
    #endif //DEBUG
    
      //--------------------------------------------------------------------------------------
      //  GPIO-41 - PIN FUNCTION = --Not Available--
      //--------------------------------------------------------------------------------------
    
    #ifdef DEBUG
      // GPIO-42: PIN FUNCTION = #DAC-PWM4
      GPIO_setMode(obj->gpioHandle,GPIO_Number_42,GPIO_42_Mode_EPWM7B);
    #else
      // GPIO-42: PIN FUNCTION = #DIO4 (Default: Input)
      GPIO_setMode(obj->gpioHandle,GPIO_Number_42,GPIO_42_Mode_GeneralPurpose);
      GPIO_setDirection(obj->gpioHandle,GPIO_Number_42,GPIO_Direction_Input);
    #endif //DEBUG
    
      //--------------------------------------------------------------------------------------
      //  GPIO-43 - PIN FUNCTION = --Not Available--
      //--------------------------------------------------------------------------------------
    
      //--------------------------------------------------------------------------------------
      //  GPIO-44 - PIN FUNCTION = --Not Available--
      //--------------------------------------------------------------------------------------
    
      return;
    }  // end of HAL_setupGpios() function
    
    
    void HAL_setupPie(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      PIE_disable(obj->pieHandle);
    
      PIE_disableAllInts(obj->pieHandle);
    
      PIE_clearAllInts(obj->pieHandle);
    
      PIE_clearAllFlags(obj->pieHandle);
    
      PIE_setDefaultIntVectorTable(obj->pieHandle);
    
      PIE_enable(obj->pieHandle);
    
      return;
    } // end of HAL_setupPie() function
    
    // This function initializes the clocks to the peripheral modules.
    // First the high and low clock prescalers are set
    // Second the clocks are enabled to each peripheral.
    // To reduce power, leave clocks to unused peripherals disabled
    //
    // Note: If a peripherals clock is not enabled then you cannot
    // read or write to the registers for that peripheral
    void HAL_setupPeripheralClks(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      CLK_enableAdcClock(obj->clkHandle);
    
      CLK_enableEcap1Clock(obj->clkHandle);
    
      CLK_enableEqep1Clock(obj->clkHandle);
    
      CLK_enablePwmClock(obj->clkHandle,PWM_Number_1);
      CLK_enablePwmClock(obj->clkHandle,PWM_Number_2);
      CLK_enablePwmClock(obj->clkHandle,PWM_Number_3);
      CLK_enablePwmClock(obj->clkHandle,PWM_Number_4);
      CLK_enablePwmClock(obj->clkHandle,PWM_Number_5);
      CLK_enablePwmClock(obj->clkHandle,PWM_Number_6);
      CLK_enablePwmClock(obj->clkHandle,PWM_Number_7);
    
      CLK_disableI2cClock(obj->clkHandle);
    
      CLK_disableClaClock(obj->clkHandle);
    
      CLK_enableSciaClock(obj->clkHandle);
    
      CLK_enableSpiaClock(obj->clkHandle);
    
      CLK_enableTbClockSync(obj->clkHandle);
    
    //  CLK_enableAfeCompClock(obj->clkHandle,(CLK_ClkAfeCtrip_e)(CLK_PCLKCR4_CTRIPA1ENCLK | CLK_PCLKCR4_CTRIPA3ENCLK | CLK_PCLKCR4_CTRIPB1ENCLK));
    
      return;
    } // end of HAL_setupPeripheralClks() function
    
    
    void HAL_setupPll(HAL_Handle handle,const PLL_ClkFreq_e clkFreq)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
    
    
      // make sure PLL is not running in limp mode
      if(PLL_getClkStatus(obj->pllHandle) != PLL_ClkStatus_Normal)
        {
          // reset the clock detect
          PLL_resetClkDetect(obj->pllHandle);
    
          // Illegal operation TRAP for debug only to halt the processor here
          asm("        ESTOP0");
        }
    
    
      // Divide Select must be ClkIn/4 before the clock rate can be changed
      if(PLL_getDivideSelect(obj->pllHandle) != PLL_DivideSelect_ClkIn_by_4)
        {
          PLL_setDivideSelect(obj->pllHandle,PLL_DivideSelect_ClkIn_by_4);
        }
    
    
      if(PLL_getClkFreq(obj->pllHandle) != clkFreq)
        {
          // disable the clock detect
          PLL_disableClkDetect(obj->pllHandle);
    
          // set the clock rate
          PLL_setClkFreq(obj->pllHandle,clkFreq);
        }
    
    
      // wait until locked
      while(PLL_getLockStatus(obj->pllHandle) != PLL_LockStatus_Done) {}
    
    
      // enable the clock detect
      PLL_enableClkDetect(obj->pllHandle);
    
    
      // set divide select to ClkIn/2 to get desired clock rate
      // NOTE: clock must be locked before setting this register
      PLL_setDivideSelect(obj->pllHandle,PLL_DivideSelect_ClkIn_by_2);
    
      return;
    } // end of HAL_setupPll() function
    
    
    void HAL_setupPwms(HAL_Handle handle,
                       const float_t systemFreq_MHz,
                       const float_t pwmPeriod_usec,
                       const uint_least16_t numPwmTicksPerIsrTick)
    {
      HAL_Obj   *obj = (HAL_Obj *)handle;
      uint16_t   halfPeriod_cycles = (uint16_t)(systemFreq_MHz*pwmPeriod_usec) >> 1;
      uint_least8_t    cnt;
    
    
      // turns off the outputs of the EPWM peripherals which will put the power switches
      // into a high impedance state.
      PWM_setOneShotTrip(obj->pwmHandle[PWM_Number_1]);
      PWM_setOneShotTrip(obj->pwmHandle[PWM_Number_2]);
      PWM_setOneShotTrip(obj->pwmHandle[PWM_Number_3]);
    
    	/* Initializing ePWM1, ePWM2 and ePWM3 as driver for controlling 3-leg inverter *
    	 * NOTE: By default, ePWM1-3 were configured with TBCLK = SYSCLKOUT = 60MHz, and Symmetic PWM mode.
    	 * 	Because we will setup Fpwm = 10kHz with the Tdead_time = 2uS, so:
    	 * 	+ the value of TBPRD register is calculate: period = SYSCLKOUT/Fpwm = 60MHz/10kHz = 12000 CPUclk cycles
    	 * 	+ the value of DBRED, DBFED registers is: deadband = DBFED = DBRED = Td * SYSCLKOUT = 2uS x 60MHz = 120 CPUclk cycles
    	 */
      for(cnt=0;cnt<3;cnt++)
        {
          // setup the Time-Base Control Register (TBCTL)
          PWM_setCounterMode(obj->pwmHandle[cnt],PWM_CounterMode_UpDown);
          PWM_disableCounterLoad(obj->pwmHandle[cnt]);
          PWM_setPeriodLoad(obj->pwmHandle[cnt],PWM_PeriodLoad_Immediate);
          PWM_setSyncMode(obj->pwmHandle[cnt],PWM_SyncMode_EPWMxSYNC);
          PWM_setHighSpeedClkDiv(obj->pwmHandle[cnt],PWM_HspClkDiv_by_1);
          PWM_setClkDiv(obj->pwmHandle[cnt],PWM_ClkDiv_by_1);
          PWM_setPhaseDir(obj->pwmHandle[cnt],PWM_PhaseDir_CountUp);
          PWM_setRunMode(obj->pwmHandle[cnt],PWM_RunMode_FreeRun);
    
          // setup the Timer-Based Phase Register (TBPHS)
          PWM_setPhase(obj->pwmHandle[cnt],0);
    
          // setup the Time-Base Counter Register (TBCTR)
          PWM_setCount(obj->pwmHandle[cnt],0);
    
          // setup the Time-Base Period Register (TBPRD)
          // set to zero initially
          PWM_setPeriod(obj->pwmHandle[cnt],0);
    
          // setup the Counter-Compare Control Register (CMPCTL)
          PWM_setLoadMode_CmpA(obj->pwmHandle[cnt],PWM_LoadMode_Zero);
          PWM_setLoadMode_CmpB(obj->pwmHandle[cnt],PWM_LoadMode_Zero);
          PWM_setShadowMode_CmpA(obj->pwmHandle[cnt],PWM_ShadowMode_Shadow);
          PWM_setShadowMode_CmpB(obj->pwmHandle[cnt],PWM_ShadowMode_Immediate);
    
          // setup the Action-Qualifier Output A Register (AQCTLA) 
          PWM_setActionQual_CntUp_CmpA_PwmA(obj->pwmHandle[cnt],PWM_ActionQual_Set);
          PWM_setActionQual_CntDown_CmpA_PwmA(obj->pwmHandle[cnt],PWM_ActionQual_Clear);
    
          // setup the Dead-Band Generator Control Register (DBCTL)
          PWM_setDeadBandOutputMode(obj->pwmHandle[cnt],PWM_DeadBandOutputMode_EPWMxA_Rising_EPWMxB_Falling);
          PWM_setDeadBandPolarity(obj->pwmHandle[cnt],PWM_DeadBandPolarity_EPWMxB_Inverted);
    
          // setup the Dead-Band Rising Edge Delay Register (DBRED)
          PWM_setDeadBandRisingEdgeDelay(obj->pwmHandle[cnt],HAL_PWM_DBRED_CNT);
    
          // setup the Dead-Band Falling Edge Delay Register (DBFED)
          PWM_setDeadBandFallingEdgeDelay(obj->pwmHandle[cnt],HAL_PWM_DBFED_CNT);
    
          // setup the PWM-Chopper Control Register (PCCTL)
          PWM_disableChopping(obj->pwmHandle[cnt]);
    
          // setup the Trip Zone Select Register (TZSEL)
          PWM_disableTripZones(obj->pwmHandle[cnt]);
        }
    
    
      // setup the Event Trigger Selection Register (ETSEL)
      PWM_disableInt(obj->pwmHandle[PWM_Number_1]);
      PWM_setSocAPulseSrc(obj->pwmHandle[PWM_Number_1],PWM_SocPulseSrc_CounterEqualZero);
      PWM_enableSocAPulse(obj->pwmHandle[PWM_Number_1]);
      
    
      // setup the Event Trigger Prescale Register (ETPS)
      if(numPwmTicksPerIsrTick == 3)
        {
          PWM_setIntPeriod(obj->pwmHandle[PWM_Number_1],PWM_IntPeriod_ThirdEvent);
          PWM_setSocAPeriod(obj->pwmHandle[PWM_Number_1],PWM_SocPeriod_ThirdEvent);
        }
      else if(numPwmTicksPerIsrTick == 2)
        {
          PWM_setIntPeriod(obj->pwmHandle[PWM_Number_1],PWM_IntPeriod_SecondEvent);
          PWM_setSocAPeriod(obj->pwmHandle[PWM_Number_1],PWM_SocPeriod_SecondEvent);
        }
      else
        {
          PWM_setIntPeriod(obj->pwmHandle[PWM_Number_1],PWM_IntPeriod_FirstEvent);
          PWM_setSocAPeriod(obj->pwmHandle[PWM_Number_1],PWM_SocPeriod_FirstEvent);
        }
    
    
      // setup the Event Trigger Clear Register (ETCLR)
      PWM_clearIntFlag(obj->pwmHandle[PWM_Number_1]);
      PWM_clearSocAFlag(obj->pwmHandle[PWM_Number_1]);
    
      // first step to synchronize the pwms
      CLK_disableTbClockSync(obj->clkHandle);
    
      // since the PWM is configured as an up/down counter, the period register is set to one-half 
      // of the desired PWM period
      PWM_setPeriod(obj->pwmHandle[PWM_Number_1],halfPeriod_cycles);
      PWM_setPeriod(obj->pwmHandle[PWM_Number_2],halfPeriod_cycles);
      PWM_setPeriod(obj->pwmHandle[PWM_Number_3],halfPeriod_cycles);
    
      // last step to synchronize the pwms
      CLK_enableTbClockSync(obj->clkHandle);
    
      return;
    }  // end of HAL_setupPwms() function
    
    #ifdef QEP
    void HAL_setupQEP(HAL_Handle handle,HAL_QepSelect_e qep)
    {
      HAL_Obj   *obj = (HAL_Obj *)handle;
    
    
      // hold the counter in reset
      QEP_reset_counter(obj->qepHandle[qep]);
    
      // set the QPOSINIT register
      QEP_set_posn_init_count(obj->qepHandle[qep], 0);
    
      // disable all interrupts
      QEP_disable_all_interrupts(obj->qepHandle[qep]);
    
      // clear the interrupt flags
      QEP_clear_all_interrupt_flags(obj->qepHandle[qep]);
    
      // clear the position counter
      QEP_clear_posn_counter(obj->qepHandle[qep]);
    
      // setup the max position
      QEP_set_max_posn_count(obj->qepHandle[qep], (4*USER_MOTOR_ENCODER_LINES)-1);
    
      // setup the QDECCTL register
      QEP_set_QEP_source(obj->qepHandle[qep], QEP_Qsrc_Quad_Count_Mode);
      QEP_disable_sync_out(obj->qepHandle[qep]);
      QEP_set_swap_quad_inputs(obj->qepHandle[qep], QEP_Swap_Not_Swapped);
      QEP_disable_gate_index(obj->qepHandle[qep]);
      QEP_set_ext_clock_rate(obj->qepHandle[qep], QEP_Xcr_2x_Res);
      QEP_set_A_polarity(obj->qepHandle[qep], QEP_Qap_No_Effect);
      QEP_set_B_polarity(obj->qepHandle[qep], QEP_Qbp_No_Effect);
      QEP_set_index_polarity(obj->qepHandle[qep], QEP_Qip_No_Effect);
    
      // setup the QEPCTL register
      QEP_set_emu_control(obj->qepHandle[qep], QEPCTL_Freesoft_Unaffected_Halt);
      QEP_set_posn_count_reset_mode(obj->qepHandle[qep], QEPCTL_Pcrm_Max_Reset);
      QEP_set_strobe_event_init(obj->qepHandle[qep], QEPCTL_Sei_Nothing);
      QEP_set_index_event_init(obj->qepHandle[qep], QEPCTL_Iei_Nothing);
      QEP_set_index_event_latch(obj->qepHandle[qep], QEPCTL_Iel_Rising_Edge);
      QEP_set_soft_init(obj->qepHandle[qep], QEPCTL_Swi_Nothing);
      QEP_disable_unit_timer(obj->qepHandle[qep]);
      QEP_disable_watchdog(obj->qepHandle[qep]);
    
      // setup the QPOSCTL register
      QEP_disable_posn_compare(obj->qepHandle[qep]);
    
      // setup the QCAPCTL register
      QEP_disable_capture(obj->qepHandle[qep]);
    
      // renable the position counter
      QEP_enable_counter(obj->qepHandle[qep]);
    
    
      return;
    }
    #endif
    
    void HAL_setupPwmDacs(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
      uint16_t halfPeriod_cycles = 512;       // 3000->10kHz, 1500->20kHz, 1000-> 30kHz, 500->60kHz
      uint_least8_t    cnt;
    
    
      for(cnt=0;cnt<3;cnt++)
        {
          // initialize the Time-Base Control Register (TBCTL)
          PWMDAC_setCounterMode(obj->pwmDacHandle[cnt],PWM_CounterMode_UpDown);
          PWMDAC_disableCounterLoad(obj->pwmDacHandle[cnt]);
          PWMDAC_setPeriodLoad(obj->pwmDacHandle[cnt],PWM_PeriodLoad_Immediate);
          PWMDAC_setSyncMode(obj->pwmDacHandle[cnt],PWM_SyncMode_EPWMxSYNC);
          PWMDAC_setHighSpeedClkDiv(obj->pwmDacHandle[cnt],PWM_HspClkDiv_by_1);
          PWMDAC_setClkDiv(obj->pwmDacHandle[cnt],PWM_ClkDiv_by_1);
          PWMDAC_setPhaseDir(obj->pwmDacHandle[cnt],PWM_PhaseDir_CountUp);
          PWMDAC_setRunMode(obj->pwmDacHandle[cnt],PWM_RunMode_FreeRun);
    
          // initialize the Timer-Based Phase Register (TBPHS)
          PWMDAC_setPhase(obj->pwmDacHandle[cnt],0);
    
          // setup the Time-Base Counter Register (TBCTR)
          PWMDAC_setCount(obj->pwmDacHandle[cnt],0);
    
          // Initialize the Time-Base Period Register (TBPRD)
          // set to zero initially
          PWMDAC_setPeriod(obj->pwmDacHandle[cnt],0);
    
          // initialize the Counter-Compare Control Register (CMPCTL)
          PWMDAC_setLoadMode_CmpA(obj->pwmDacHandle[cnt],PWM_LoadMode_Zero);
          PWMDAC_setLoadMode_CmpB(obj->pwmDacHandle[cnt],PWM_LoadMode_Zero);
          PWMDAC_setShadowMode_CmpA(obj->pwmDacHandle[cnt],PWM_ShadowMode_Shadow);
          PWMDAC_setShadowMode_CmpB(obj->pwmDacHandle[cnt],PWM_ShadowMode_Shadow);
    
          // Initialize the Action-Qualifier Output A Register (AQCTLA) 
          PWMDAC_setActionQual_CntUp_CmpA_PwmA(obj->pwmDacHandle[cnt],PWM_ActionQual_Clear);
          PWMDAC_setActionQual_CntDown_CmpA_PwmA(obj->pwmDacHandle[cnt],PWM_ActionQual_Set);
    
          // account for EPWM6B
          if(cnt == 0)
            {
              PWMDAC_setActionQual_CntUp_CmpB_PwmB(obj->pwmDacHandle[cnt],PWM_ActionQual_Clear);
              PWMDAC_setActionQual_CntDown_CmpB_PwmB(obj->pwmDacHandle[cnt],PWM_ActionQual_Set);
            }
    
          // account for EPWM7B. Added by Duongtb (21-Nov-2015)
          if(cnt == 1)
          {
              PWMDAC_setActionQual_CntUp_CmpB_PwmB(obj->pwmDacHandle[cnt],PWM_ActionQual_Clear);
              PWMDAC_setActionQual_CntDown_CmpB_PwmB(obj->pwmDacHandle[cnt],PWM_ActionQual_Set);
          }
    
          // Initialize the Dead-Band Control Register (DBCTL) 
          PWMDAC_disableDeadBand(obj->pwmDacHandle[cnt]);
    
          // Initialize the PWM-Chopper Control Register (PCCTL)
          PWMDAC_disableChopping(obj->pwmDacHandle[cnt]);
    
          // Initialize the Trip-Zone Control Register (TZSEL)
          PWMDAC_disableTripZones(obj->pwmDacHandle[cnt]);
    
          // Initialize the Trip-Zone Control Register (TZCTL)
          PWMDAC_setTripZoneState_TZA(obj->pwmDacHandle[cnt],PWM_TripZoneState_HighImp);
          PWMDAC_setTripZoneState_TZB(obj->pwmDacHandle[cnt],PWM_TripZoneState_HighImp);
          PWMDAC_setTripZoneState_DCAEVT1(obj->pwmDacHandle[cnt],PWM_TripZoneState_HighImp);
          PWMDAC_setTripZoneState_DCAEVT2(obj->pwmDacHandle[cnt],PWM_TripZoneState_HighImp);
          PWMDAC_setTripZoneState_DCBEVT1(obj->pwmDacHandle[cnt],PWM_TripZoneState_HighImp);
        }
    
      // since the PWM is configured as an up/down counter, the period register is set to one-half 
      // of the desired PWM period
      PWMDAC_setPeriod(obj->pwmDacHandle[PWMDAC_Number_1],halfPeriod_cycles);
      PWMDAC_setPeriod(obj->pwmDacHandle[PWMDAC_Number_2],halfPeriod_cycles);
      PWMDAC_setPeriod(obj->pwmDacHandle[PWMDAC_Number_3],halfPeriod_cycles);
      //PWMDAC_setPeriod(obj->pwmDacHandle[PWMDAC_Number_4],halfPeriod_cycles);
    
      return;
    }  // end of HAL_setupPwmDacs() function
    
    /*
     * **********************************************************************************
     * Function		: HAL_setupTimers()
     *
     * Description	: Initialize the Timer module according to requirements of the project
     *
     * Argument (s)	: timerHandle and SysFreq_MHz
     *
     * Return (s)	: none
     *
     * Note (s)		: This routine should be called one time at system initialization
     *
     * **********************************************************************************
    */
    void HAL_setupTimers(HAL_Handle handle,const float_t systemFreq_MHz)
    {
      HAL_Obj  *obj = (HAL_Obj *)handle;
      uint32_t  timerPeriod_cnts;
    
    
      // \brief use timer 0 for frequency diagnostics
      // \notes Timer 0 is set up to overflow after every 1,000,000uS = 1 seconds
      timerPeriod_cnts = (uint32_t)(systemFreq_MHz * (float_t)USER_TIMER0_PERIOD_usec) - 1;
      TIMER_setDecimationFactor(obj->timerHandle[0],0);
      TIMER_setEmulationMode(obj->timerHandle[0],TIMER_EmulationMode_RunFree);
      TIMER_setPeriod(obj->timerHandle[0],timerPeriod_cnts);
      TIMER_setPreScaler(obj->timerHandle[0],0);
    
      // \brief use timer 1 for CPU usage diagnostics
      // \notes Timer 1 is set up to overflow after every 1,000,000uS = 1 seconds
      timerPeriod_cnts = (uint32_t)(systemFreq_MHz * (float_t)USER_TIMER1_PERIOD_usec) - 1;
      TIMER_setDecimationFactor(obj->timerHandle[1],0);
      TIMER_setEmulationMode(obj->timerHandle[1],TIMER_EmulationMode_RunFree);
      TIMER_setPeriod(obj->timerHandle[1],timerPeriod_cnts);
      TIMER_setPreScaler(obj->timerHandle[1],0);
    
      return;
    }  // end of HAL_setupTimers() function
    
    //! System delay function in ms
    void HAL_DelayMs(uint16_t time_ms)
    {
    	extern void usDelay(uint32_t Count);
     	uint16_t i;
    
     	for(i=0; i<time_ms; i++) {
     		DELAY_US(1000L);
     	}
    }
    
    // end of file
    

    vfd_main.c
    /* --COPYRIGHT--,BSD
     * Copyright (c) 2015, VPEC Group
     * All rights reserved.
     *
     * Project:			SimpleVFD
     *
     * Version: 		1.0
     *
     * Target:  		TMS320F2805x
     *
     * Type: 			Device Independent
     *
     * Updated:			28 Oct 2015
     *
     * Programmer:		Duongtb
     *
     * Email:			tran.binhduong@gmail.com
     *
     * --/COPYRIGHT--*/
    
    //! \file   solutions/instaspin_foc/src/vfd_main.c
    //! \brief  CPU and Inverter Set-up and introduction to interfacing to the ROM library
    //!
    //! (C) Copyright 2015, VPEC Group.
    
    //! \defgroup PROJ_VFD PROJ_VFD_REV01
    //@{
    
    //! \defgroup PROJ_VFD_OVERVIEW Project Overview
    //!
    //! CPU and Inverter Set-up and introduction to interfacing to the ROM library
    //!
    
    //
    // **************************************************************************
    // the includes
    
    // system includes
    #include <math.h>
    
    // modules
    #include "sw/modules/math/src/32b/math.h"
    #include "sw/modules/memCopy/src/memCopy.h"
    
    #include "sw/modules/ctrl/src/32b/ctrl.h"			//Added by Duongtb: 18-Nov-2015
    
    // drivers
    
    
    // platforms
    #include "vfd_main.h"
    #include "vfd_settings.h"
    
    
    // **************************************************************************
    // the defines
    
    #define LED_BLINK_FREQ_Hz   5
    
    // **************************************************************************
    // the globals
    uint_least32_t gLEDcnt = 0;      // Counter used to divide down the ISR rate for visually blinking an LED
    
    #ifdef FLASH
    // Used for running BackGround in flash, and ISR in RAM
    extern uint16_t *RamfuncsLoadStart, *RamfuncsLoadEnd, *RamfuncsRunStart;
    
    #ifdef F2802xF
    extern uint16_t *econst_start, *econst_end, *econst_ram_load;
    extern uint16_t *switch_start, *switch_end, *switch_ram_load;
    #endif
    
    #endif //FLASH
    
    // ---------------------------------- HAL OBJECTS -----------------------------------------
    #ifdef F2802xF
    #pragma DATA_SECTION(halHandle,"rom_accessed_data");
    #endif
    HAL_Handle halHandle;                         // Handle to the Inverter hardware abstraction layer
    
    HAL_PwmData_t gPwmData = {_IQ(0.0), _IQ(0.0), _IQ(0.0)};           // Contains PWM duty cycles in global Q format
    
    HAL_AdcData_t gAdcData = {_IQ(0.0), _IQ(0.0), _IQ(0.0),
    						  _IQ(0.0), _IQ(0.0), _IQ(0.0),
    						  _IQ(0.0)};     							// Contains Current and Voltage ADC readings in global Q format
    
    HAL_DacData_t gDacData = {0, 0, 0, 0};	// Contains PWMDAC data in global Q format
    
    // ---------------------------------- CONTROL OBJECTS --------------------------------------------
    CPU_USAGE_Handle  cpu_usageHandle;	//!< the handle for CPU_USAGE
    CPU_USAGE_Obj     cpu_usage;		//!< the object for CPU_USAGE
    
    CLARKE_Handle   clarkeHandle_I;  	//!< the handle for the current Clarke
                                     	//!< transform
    CLARKE_Obj      clarke_I;        	//!< the current Clarke transform object
    
    CLARKE_Handle   clarkeHandle_V;  	//!< the handle for the voltage Clarke
                                     	//!< transform
    CLARKE_Obj      clarke_V;        	//!< the voltage Clarke transform object
    
    EST_Handle      estHandle;       	//!< the handle for the estimator
    
    PID_Obj         pid[3];          	//!< three objects for PID controllers
                                     	//!< 0 - Speed, 1 - Id, 2 - Iq
    PID_Handle      pidHandle[3];    	//!< three handles for PID controllers
                                     	//!< 0 - Speed, 1 - Id, 2 - Iq
    uint16_t        pidCntSpeed;     	//!< count variable to decimate the execution
                                     	//!< of the speed PID controller
    
    IPARK_Handle    iparkHandle;     	//!< the handle for the inverse Park
                                     	//!< transform
    IPARK_Obj       ipark;           	//!< the inverse Park transform object
    
    SVGEN_Handle    svgenHandle;     	//!< the handle for the space vector generator
    SVGEN_Obj       svgen;           	//!< the space vector generator object
    
    RAMPGEN_Handle  rampgenHandle;	 	//!< the handle for ramp generator. Added by Duongtb, 13-Nov-2015
    RAMPGEN_Obj		rampgen;		 	//!< the ramp generator object. Added by Duongtb, 13-Nov-2015
    
    FILTER_FO_Handle  filterHandle[6];  //!< the handles for the 3-current and 3-voltage filters for offset calculation
    FILTER_FO_Obj     filter[6];        //!< the 3-current and 3-voltage filters for offset calculation
    
    // ---------------------------------- USER VARIABLES --------------------------------------
    #ifdef F2802xF
    #pragma DATA_SECTION(gUserParams,"rom_accessed_data");
    #endif
    USER_Params gUserParams;                      		  //!< Contains the user.h settings
    
    volatile MOTOR_Vars_t gMotorVars = MOTOR_Vars_INIT;   //!< the global motor
                                     	 	 	 	 	  //!< variables that are defined in main.h and
                                     	 	 	 	 	  //!< used for display in the debugger's watch
                                     	 	 	 	 	  //!< window
    
    MATH_vec3 		gOffsets_I_pu = {_IQ(0.0),_IQ(0.0),_IQ(0.0)};  //!< contains
                                     //!< the offsets for the current feedback
    
    MATH_vec3       gOffsets_V_pu = {_IQ(0.0),_IQ(0.0),_IQ(0.0)};  //!< contains
                                     //!< the offsets for the voltage feedback
    
    MATH_vec2       gIdq_ref_pu = {_IQ(0.0),_IQ(0.0)};  //!< contains the Id and
                                     //!< Iq references
    
    MATH_vec2       gVdq_out_pu = {_IQ(0.0),_IQ(0.0)};  //!< contains the output
                                     //!< Vd and Vq from the current controllers
    
    MATH_vec2       gIdq_pu = {_IQ(0.0),_IQ(0.0)};  //!< contains the Id and Iq
                                     //!< measured values
    
    _iq 	gFlux_pu_to_Wb_sf;
    
    _iq 	gFlux_pu_to_VpHz_sf;
    
    _iq 	gTorque_Ls_Id_Iq_pu_to_Nm_sf;
    
    _iq 	gTorque_Flux_Iq_pu_to_Nm_sf;
    
    _iq 	gSpeed_krpm_to_pu_sf = _IQ((float_t)USER_MOTOR_NUM_POLE_PAIRS * 1000.0
                / (USER_IQ_FULL_SCALE_FREQ_Hz * 60.0));
    
    _iq 	gSpeed_hz_to_krpm_sf = _IQ(60.0 / (float_t)USER_MOTOR_NUM_POLE_PAIRS
                / 1000.0);
    
    _iq gIdRef     = _IQ(0.1);			// Id reference (pu)
    _iq gIqRef     = _IQ(0.05);			// Iq reference (pu)
    _iq gSpeedRef  = _IQ(0.3);			// Speed reference (pu)
    
    _iq gMaxCurrentSlope = _IQ(0.0);
    // **************************************************************************
    // the functions
    // IMPORTANT NOTE: If you are not familiar with MotorWare coding guidelines
    // please refer to the following document:
    // C:/ti/motorware/motorware_1_01_00_1x/docs/motorware_coding_standards.pdf
    
    //----------------------------------------------------------------------------------
    // MAIN CODE - starts here
    //----------------------------------------------------------------------------------
    void main(void)
    {
      uint16_t cnt = 0;
    
      // ------------------------------- FRAMEWORK ----------------------------------------
      // Only used if running from FLASH
      // Note that the variable FLASH is defined by the project
      #ifdef FLASH
      // Copy time critical code and Flash setup code to RAM
      // The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
      // symbols are created by the linker. Refer to the linker files.
      memCopy((uint16_t *)&RamfuncsLoadStart,(uint16_t *)&RamfuncsLoadEnd,(uint16_t *)&RamfuncsRunStart);
    
      #ifdef F2802xF
        //copy .econst to unsecure RAM
        if(*econst_end - *econst_start)
          {
            memCopy((uint16_t *)&econst_start,(uint16_t *)&econst_end,(uint16_t *)&econst_ram_load);
          }
    
        //copy .switch ot unsecure RAM
        if(*switch_end - *switch_start)
          {
            memCopy((uint16_t *)&switch_start,(uint16_t *)&switch_end,(uint16_t *)&switch_ram_load);
          }
      #endif
    
      #endif //FLASH
    
      // initialize the hardware abstraction layer
      // halHandle will be used throughout the code to interface with the HAL
      // (set parameters, get and set functions, etc) halHandle is required since
      // this is how all objects are interfaced, and it allows interface with
      // multiple objects by simply passing a different handle. The use of
      // handles is explained in this document:
      // C:/ti/motorware/motorware_1_01_00_1x/docs/motorware_coding_standards.pdf
      halHandle = HAL_init(&hal,sizeof(hal));
    
      // ---------------------------------- USER SETTINGS ------------------------------------------
      // check for errors in user parameters
      USER_checkForErrors(&gUserParams);
    
      // store user parameter error in global variable
      gMotorVars.UserErrorCode = USER_getErrorCode(&gUserParams);
    
      //!> Warning: DO NOT allow code execution if there is a user parameter error
      //! \Checked by: Duongtb; 10-Nov-2015
      if(gMotorVars.UserErrorCode != USER_ErrorCode_NoError)
        {
          for(;;)
            {
              gMotorVars.Flag_enableSys = false;
            }
        }
    
    
      // initialize the user parameters
      //! Notes: You MUST call this procedure before calling the HAL_setParams.
      USER_setParams(&gUserParams);
    
      // set the hardware abstraction layer parameters
      HAL_setParams(halHandle,&gUserParams);
    
      // ---------------------------------- ESTIMATOR SETUP ------------------------------------------
      // initialize the estimator
      estHandle = EST_init((void *)USER_EST_HANDLE_ADDRESS,0x200);
    
    	#ifdef FAST_ROM_V1p6
    	{
    		// These function calls are used to initialize the estimator with ROM
    		// function calls. It needs the specific address where the controller
    		// object is declared by the ROM code.
    		CTRL_Handle ctrlHandle = CTRL_init((void *)USER_CTRL_HANDLE_ADDRESS,0x200);
    		CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
    
    		// this sets the estimator handle (part of the controller object) to
    		// the same value initialized above by the EST_init() function call.
    		// This is done so the next function implemented in ROM, can
    		// successfully initialize the estimator as part of the controller
    		// object.
    		obj->estHandle = estHandle;
    
    		// initialize the estimator through the controller. These three
    		// function calls are needed for the F2806xF/M implementation of
    		// InstaSPIN.
    		CTRL_setParams(ctrlHandle,&gUserParams);
    		CTRL_setUserMotorParams(ctrlHandle);
    		CTRL_setupEstIdleState(ctrlHandle);
    	}
    	#else //For FAST_ROM_V1p7 - Implemented in 2xF and 5xF/M devices
    	{
    		// initialize the estimator. These two function calls are needed for
    		// the F2802xF/F2805xF implementation of InstaSPIN using the estimator handle
    		// initialized by EST_init(), these two function calls configure the estimator,
    		// and they set the estimator in a proper state prior to spinning a motor.
    		EST_setEstParams(estHandle,&gUserParams);	//called from the EST_setEstParams.lib
    		EST_setupEstIdleState(estHandle);			//called from the EST_setupEstIdleState.lib
    	}
    	#endif
    
    	// disable Rs recalculation
    	EST_setFlag_enableRsRecalc(estHandle,false);
    
    	// ---------------------------------- CTRL INIT -------------------------------------
    	// initialize the RAMPGEN modules
    	rampgenHandle = RAMPGEN_init(&rampgen,sizeof(rampgen));
    
    	// setup the RAMPGEN module
    	RAMPGEN_setup(rampgenHandle,
    			(float_t)USER_IQ_FULL_SCALE_FREQ_Hz,	//set RAMPGEN with the base freq = 120Hz
    			(float_t)(1.0 / USER_ISR_FREQ_Hz),		//set the Samping Period in seconds
    			_IQ(1.0),								//set the gain = _IQ(1.0)
    			_IQ(0.0));								//set the offset = _IQ(0.0)
    
    	// initialize and setup the Clarke modules
    	{
    		// Clarke handle initialization for current signals
    		clarkeHandle_I = CLARKE_init(&clarke_I,sizeof(clarke_I));
    
    		// Clarke handle initialization for voltage signals
    		clarkeHandle_V = CLARKE_init(&clarke_V,sizeof(clarke_V));
    
    		// set the number of current sensors
    		APP_setupClarke_I(clarkeHandle_I,USER_NUM_CURRENT_SENSORS);
    
    		// set the number of voltage sensors
    		APP_setupClarke_V(clarkeHandle_V,USER_NUM_VOLTAGE_SENSORS);
    	}
    
    	// initialize and setup the Filter modules
    	{
    		for(cnt=0;cnt<6;cnt++)
    		{
    			filterHandle[cnt] = FILTER_FO_init(&filter[cnt],sizeof(filter[cnt]));
    		}
    
    		APP_setupFilters(&filterHandle[0]);
    
    		gMotorVars.Flag_enableOffsetcalc = false;
    	}
    
    	// set the pre-determined current and voltage feeback offset values
    	gOffsets_I_pu.value[0] = _IQ(I_A_offset);
    	gOffsets_I_pu.value[1] = _IQ(I_B_offset);
    	gOffsets_I_pu.value[2] = _IQ(I_C_offset);
    	gOffsets_V_pu.value[0] = _IQ(V_A_offset);
    	gOffsets_V_pu.value[1] = _IQ(V_B_offset);
    	gOffsets_V_pu.value[2] = _IQ(V_C_offset);
    
    	// initialize the PID controllers
    	{
    		// This equation defines the relationship between per unit current and
    		// real-world current. The resulting value in per units (pu) is then
    		// used to configure the controllers
    		_iq maxCurrent_pu = _IQ(USER_MOTOR_MAX_CURRENT/ USER_IQ_FULL_SCALE_CURRENT_A);
    
    		// This equation uses the scaled maximum voltage vector, which is
    		// already in per units, hence there is no need to include the #define
    		// for USER_IQ_FULL_SCALE_VOLTAGE_V
    		_iq maxVoltage_pu = _IQ(USER_MAX_VS_MAG_PU * USER_VD_SF);
    
    		float_t fullScaleCurrent = USER_IQ_FULL_SCALE_CURRENT_A;
    		float_t fullScaleVoltage = USER_IQ_FULL_SCALE_VOLTAGE_V;
    		float_t IsrPeriod_sec = 1.0 / USER_ISR_FREQ_Hz;
    		float_t Ls_d = USER_MOTOR_Ls_d;
    		float_t Ls_q = USER_MOTOR_Ls_q;
    		float_t Rs = USER_MOTOR_Rs;
    
    		// This project assumes that motor parameters are known, and it does not
    		// perform motor ID, so the R/L parameters are known and defined in user.h
    		float_t RoverLs_d = Rs / Ls_d;
    		float_t RoverLs_q = Rs / Ls_q;
    
    		// For the current controller, Kp = Ls*bandwidth(rad/sec)  But in order
    		// to be used, it must be converted to per unit values by multiplying
    		// by fullScaleCurrent and then dividing by fullScaleVoltage.  From the
    		// statement below, we see that the bandwidth in rad/sec is equal to
    		// 0.25/IsrPeriod_sec, which is equal to USER_ISR_FREQ_HZ/4. This means
    		// that by setting Kp as described below, the bandwidth in Hz is
    		// USER_ISR_FREQ_HZ/(8*pi).
    		_iq Kp_Id = _IQ((0.25 * Ls_d * fullScaleCurrent) / (IsrPeriod_sec
    					* fullScaleVoltage));
    
    		// In order to achieve pole/zero cancellation (which reduces the
    		// closed-loop transfer function from a second-order system to a
    		// first-order system), Ki must equal Rs/Ls.  Since the output of the
    		// Ki gain stage is integrated by a DIGITAL integrator, the integrator
    		// input must be scaled by 1/IsrPeriod_sec.  That's just the way
    		// digital integrators work.  But, since IsrPeriod_sec is a constant,
    		// we can save an additional multiplication operation by lumping this
    		// term with the Ki value.
    		_iq Ki_Id = _IQ(RoverLs_d * IsrPeriod_sec);
    
    		// Now do the same thing for Kp for the q-axis current controller.
    		// If the motor is not an IPM motor, Ld and Lq are the same, which
    		// means that Kp_Iq = Kp_Id
    		_iq Kp_Iq = _IQ((0.25 * Ls_q * fullScaleCurrent) / (IsrPeriod_sec
    					* fullScaleVoltage));
    
    		// Do the same thing for Ki for the q-axis current controller.  If the
    		// motor is not an IPM motor, Ld and Lq are the same, which means that
    		// Ki_Iq = Ki_Id.
    		_iq Ki_Iq = _IQ(RoverLs_q * IsrPeriod_sec);
    
    		// There are three PI controllers; one speed controller and two current
    		// controllers.  Each PI controller has two coefficients; Kp and Ki.
    		// So you have a total of six coefficients that must be defined.
    		// This is for the speed controller
    		pidHandle[0] = PID_init(&pid[0],sizeof(pid[0]));
    		// This is for the Id current controller
    		pidHandle[1] = PID_init(&pid[1],sizeof(pid[1]));
    		// This is for the Iq current controller
    		pidHandle[2] = PID_init(&pid[2],sizeof(pid[2]));
    
    		// The following instructions load the parameters for the speed PI
    		// controller.
    		PID_setGains(pidHandle[0],_IQ(1.0),_IQ(0.01),_IQ(0.0));
    
    		// The current limit is performed by the limits placed on the speed PI
    		// controller output.  In the following statement, the speed
    		// controller's largest negative current is set to -maxCurrent_pu, and
    		// the largest positive current is set to maxCurrent_pu.
    		PID_setMinMax(pidHandle[0],-maxCurrent_pu,maxCurrent_pu);
    		PID_setUi(pidHandle[0],_IQ(0.0));  // Set the initial condition value
    										   // for the integrator output to 0
    
    		pidCntSpeed = 0;  // Set the counter for decimating the speed
    						  // controller to 0
    
    		// The following instructions load the parameters for the d-axis
    		// current controller.
    		// P term = Kp_Id, I term = Ki_Id, D term = 0
    		PID_setGains(pidHandle[1],Kp_Id,Ki_Id,_IQ(0.0));
    
    		// Largest negative voltage = -maxVoltage_pu, largest positive
    		// voltage = maxVoltage_pu
    		PID_setMinMax(pidHandle[1],-maxVoltage_pu,maxVoltage_pu);
    
    		// Set the initial condition value for the integrator output to 0
    		PID_setUi(pidHandle[1],_IQ(0.0));
    
    		// The following instructions load the parameters for the q-axis
    		// current controller.
    		// P term = Kp_Iq, I term = Ki_Iq, D term = 0
    		PID_setGains(pidHandle[2],Kp_Iq,Ki_Iq,_IQ(0.0));
    
    		// The largest negative voltage = 0 and the largest positive
    		// voltage = 0.  But these limits are updated every single ISR before
    		// actually executing the Iq controller. The limits depend on how much
    		// voltage is left over after the Id controller executes. So having an
    		// initial value of 0 does not affect Iq current controller execution.
    		PID_setMinMax(pidHandle[2],_IQ(0.0),_IQ(0.0));
    
    		// Set the initial condition value for the integrator output to 0
    		PID_setUi(pidHandle[2],_IQ(0.0));
    	}
    
    	// initialize the speed reference in kilo RPM where base speed is
    	// USER_IQ_FULL_SCALE_FREQ_Hz.
    	// Set 10 Hz electrical frequency as initial value, so the kRPM value would
    	// be: 10 * 60 / motor pole pairs / 1000.
    	gMotorVars.SpeedRef_krpm = _IQmpy(_IQ(10.0),gSpeed_hz_to_krpm_sf);
    
    	// initialize the inverse Park module
    	iparkHandle = IPARK_init(&ipark,sizeof(ipark));
    
    	// initialize the space vector generator module
    	svgenHandle = SVGEN_init(&svgen,sizeof(svgen));
    
      // ------------------------------- FRAMEWORK ----------------------------------------
      // initialize the CPU usage module
      cpu_usageHandle = CPU_USAGE_init(&cpu_usage,sizeof(cpu_usage));
      CPU_USAGE_setParams(cpu_usageHandle,
    					 (uint32_t)USER_SYSTEM_FREQ_MHz * 1000000,     // timer period, cnts
    					 (uint32_t)USER_ISR_FREQ_Hz);                  // average over 1 second of ISRs
    
      // setup faults
      HAL_setupFaults(halHandle);
    
      // initialize the interrupt vector table
      HAL_initIntVectorTable(halHandle);
    
      // enable the ADC interrupts
      HAL_enableAdcInts(halHandle);
    
      // enable global interrupts
      HAL_enableGlobalInts(halHandle);
    
      // enable debug interrupts
      HAL_enableDebugInt(halHandle);
    
    #ifdef DEBUG
      {
    	//Turn all LEDs on
    	HAL_turnLedOn(halHandle,(GPIO_Number_e)HAL_Gpio_LED1);
    	HAL_turnLedOn(halHandle,(GPIO_Number_e)HAL_Gpio_LED2);
    
    	// enable the PWM
    	HAL_enablePwm(halHandle);
    
    	// delay 2 seconds
    	HAL_DelayMs(2000L);
    
    	//Turn all LEDs off
    	HAL_turnLedOff(halHandle,(GPIO_Number_e)HAL_Gpio_LED1);
    	HAL_turnLedOff(halHandle,(GPIO_Number_e)HAL_Gpio_LED2);
      }
    #endif //DEBUG
    
      // disable the PWM
      HAL_disablePwm(halHandle);
    
      // compute scaling factors for flux and torque calculations
      gFlux_pu_to_Wb_sf = USER_computeFlux_pu_to_Wb_sf();
      gFlux_pu_to_VpHz_sf = USER_computeFlux_pu_to_VpHz_sf();
      gTorque_Ls_Id_Iq_pu_to_Nm_sf = USER_computeTorque_Ls_Id_Iq_pu_to_Nm_sf();
      gTorque_Flux_Iq_pu_to_Nm_sf = USER_computeTorque_Flux_Iq_pu_to_Nm_sf();
    
    //===================================================================================
    //	BACKGROUND (BG) LOOP
    //===================================================================================
      // enable the system by default
      gMotorVars.Flag_enableSys = true;
    
      // Begin the background loop
      for(;;)
      {
          // Waiting for enable system flag to be set
          while(!(gMotorVars.Flag_enableSys));
    
          // loop while the enable system flag is true
          while(gMotorVars.Flag_enableSys)
          {
    		#if (BUILDLEVEL==LEVEL1)
    		  HAL_enablePwm(halHandle);
    		#endif //#if (BUILDLEVEL==LEVEL1)
    
    		#if (BUILDLEVEL!=LEVEL1)
        	  // If Flag_enableSys is set AND Flag_Run_Identify is set THEN
              // enable PWMs and set the speed reference
              if(gMotorVars.Flag_Run_Identify)
              {
                  // update estimator state
                  EST_updateState(estHandle,0);
    
                  #ifdef FAST_ROM_V1p6
                      // call this function to fix 1p6. This is only used for
                      // F2806xF/M implementation of InstaSPIN (version 1.6 of
                      // ROM), since the inductance calculation is not done
                      // correctly in ROM, so this function fixes that ROM bug.
                      APP_softwareUpdate1p6(estHandle);
                  #endif
    
                  // enable the PWM
                  HAL_enablePwm(halHandle);
              }
              else  // Flag_enableSys is set AND Flag_Run_Identify is not set
              {
                  // set estimator to Idle
                  EST_setIdle(estHandle);
    
                  // disable the PWM
                  HAL_disablePwm(halHandle);
    
                  // clear integrator outputs
                  PID_setUi(pidHandle[0],_IQ(0.0));
                  PID_setUi(pidHandle[1],_IQ(0.0));
                  PID_setUi(pidHandle[2],_IQ(0.0));
    
                  // clear Id and Iq references
                  gIdq_ref_pu.value[0] = _IQ(0.0);
                  gIdq_ref_pu.value[1] = _IQ(0.0);
              }
    
              // update the global variables
              APP_updateGlobalVariables(estHandle);
    
              // enable/disable the forced angle
              EST_setFlag_enableForceAngle(estHandle,
                      gMotorVars.Flag_enableForceAngle);
    
              // set target speed
              gMotorVars.SpeedRef_pu = _IQmpy(gMotorVars.SpeedRef_krpm,
                      gSpeed_krpm_to_pu_sf);
    		#endif //#if (BUILDLEVEL==LEVEL1)
    
          } // end of while(gFlag_enableSys) loop
    
          // disable the PWM
          HAL_disablePwm(halHandle);
    
          gMotorVars.Flag_Run_Identify = false;
      } // end of for(;;) loop
    
    } // end of main() function
    
    //! mainISR() function
    interrupt void mainISR(void)
    {
    	// Declaration of local variables
    	_iq 		angle_pu;					//The angle of space vector
    	MATH_vec2 	Vab_pu;
    	MATH_vec2 	phasor;
    	uint32_t 	timer1Cnt;
    
    	_iq dacValue_1, dacValue_2, dacValue_3, dacValue_4;
    
      // ------------------------------- FRAMEWORK ----------------------------------------
      // read the timer 1 value and update the CPU usage module
      timer1Cnt = HAL_readTimerCnt(halHandle,1);
      CPU_USAGE_updateCnts(cpu_usageHandle,timer1Cnt);
    
      // acknowledge the ADC interrupt
      HAL_acqAdcInt(halHandle,ADC_IntNumber_1);
    
    // =============================== LEVEL 1 ======================================
    //	  Checks target independent modules, duty cycle waveforms and PWM update
    //	  Keep the motors disconnected at this level!
    // ==============================================================================
    
    #if (BUILDLEVEL==LEVEL1)
    //! Build for LEVEL#1 is tested successfully on 21-Nov-2015
      gFlag_Vdq_Open_Loop = false;
    
      if(gFlag_Vdq_Open_Loop == true)
      {
    	 //notes: Test is OK by Duongtb (21-Nov-2015).
     	 CTRL_incrAngle(&gAngle_Open_Loop, gAngle_Delta_Open_Loop);
    
     	 // get an electrical angle for testing in BUILD_LEVEL_1 from the CTRL module
     	 angle_pu = gAngle_Open_Loop;
      }
      else
      {
    	  // run the ramp (saw-tooth) at USER_REF_FREQ_Hz = 25Hz
    	  // notes: test is OK by Duongtb (18-Nov-2015).
    	  rampgen.freq = _IQ(USER_REF_FREQ_Hz/USER_IQ_FULL_SCALE_FREQ_Hz);
    	  RAMPGEN_run(rampgenHandle);
    
    	  // get an electrical angle for testing in BUILD_LEVEL_1 from the RAMPGEN module
    	  angle_pu = rampgen.output;
      }
    
      // Set the amplitude of the voltage.
      // Notice that, by changing the value of gVdq_Open_Loop.value[1] we will change
      // the amplitude of the voltage vector. The maximum value here is _IQ(1.333333).
      gVdq_out_pu.value[0] = _IQ(0.1); //gVdq_Open_Loop.value[0];
      gVdq_out_pu.value[1] = _IQ(0.5); //gVdq_Open_Loop.value[1];
    
      // compute the sine and cosine phasor values which are part of the inverse
      // Park transform calculations. Once these values are computed,
      // they are copied into the IPARK module, which then uses them to
      // transform the voltages from DQ to Alpha/Beta reference frames.
      phasor.value[0] = _IQcosPU(angle_pu);
      phasor.value[1] = _IQsinPU(angle_pu);
    
      // set the phasor in the inverse Park transform
      IPARK_setPhasor(iparkHandle,&phasor);
    
      // Run the inverse Park module.  This converts the voltage vector from
      // synchronous frame values to stationary frame values.
      IPARK_run(iparkHandle,&gVdq_out_pu,&Vab_pu);
    
      // Now run the space vector generator (SVGEN) module.
      // There is no need to do an inverse CLARKE transform, as this is
      // handled in the SVGEN_run function.
      SVGEN_run(svgenHandle,&Vab_pu,&(gPwmData.Tabc));
    
      dacValue_1 = (_iq)(((angle_pu>>1) + _IQ(-0.25))<<1); //gPwmData.Tabc.value[0];
      dacValue_2 = gPwmData.Tabc.value[1];
      dacValue_3 = gPwmData.Tabc.value[2];
      dacValue_4 = (_iq)((gPwmData.Tabc.value[0] - gPwmData.Tabc.value[1])>>1);	//Dividing by 2 to nominalize the output signal
    
      // write to the PWM compare registers, and then we are done!
      HAL_writePwmData(halHandle,&gPwmData);
    
    #ifdef DEBUG
      // ------------------------------------------------------------------------------
      //    Connect inputs of the PWMDAC module
      // ------------------------------------------------------------------------------
    
      //! \notes: Testing result is OK when gDacData.value[0] = gDacData.value[1].
      //! \notes: In case of gDacData.value[0] <> gDacData.value[1], the result is wrong!!!
      //! \notes: Up to now I have not understood why???
      gDacData.value[0] = dacValue_1;
      gDacData.value[1] = dacValue_1;
      gDacData.value[2] = dacValue_3;
      gDacData.value[3] = dacValue_4;
    
      // write the data to DAC module
      HAL_writeDacData(halHandle, &gDacData);
      //HAL_writeDacData_Jorge(halHandle, &gDacData);
    
      // ------------------------------------------------------------------------------
      //    Connect inputs of the DATALOG module
      // ------------------------------------------------------------------------------
    /*  DlogCh1 = (int16)_IQtoIQ15(svgen1.Ta);
      DlogCh2 = (int16)_IQtoIQ15(svgen1.Tb);
      DlogCh3 = (int16)_IQtoIQ15(svgen1.Tc);
      DlogCh4 = (int16)_IQtoIQ15(svgen1.Tb-svgen1.Tc);*/
    #endif // DEBUG
    
    #endif // (BUILDLEVEL==LEVEL1)
    
    
    // =============================== LEVEL 2 ======================================
    //	  Level 2 verifies the analog-to-digital conversion, offset compensation,
    //    clarke/park transformations (CLARKE/PARK).
    // ==============================================================================
    #if (BUILDLEVEL==LEVEL2)
    	_iq speed_pu;
    	_iq oneOverDcBus;
    	MATH_vec2 Iab_pu;
    
      // ------------------------------------------------------------------------------
      //  Connect inputs of the RAMP GEN module and call the ramp generator macro
      // ------------------------------------------------------------------------------
      gFlag_Vdq_Open_Loop = false;
    
      if(gFlag_Vdq_Open_Loop == true)
      {
    	 //notes: Test is OK by Duongtb (21-Nov-2015).
    	 CTRL_incrAngle(&gAngle_Open_Loop, gAngle_Delta_Open_Loop);
    
    	 // get an electrical angle for testing in BUILD_LEVEL_1 from the CTRL module
    	 angle_pu = gAngle_Open_Loop;
      }
      else
      {
    	  // run the ramp (saw-tooth) at USER_REF_FREQ_Hz = 25Hz
    	  // notes: test is OK by Duongtb (18-Nov-2015).
    	  rampgen.freq = _IQ(USER_REF_FREQ_Hz/USER_IQ_FULL_SCALE_FREQ_Hz);
    	  RAMPGEN_run(rampgenHandle);
    
    	  // get an electrical angle for testing in BUILD_LEVEL_1 from the RAMPGEN module
    	  angle_pu = rampgen.output;
      }
    
      // ------------------------------------------------------------------------------
      //  Measure phase currents, subtract the offset and normalize from (-0.5,+0.5) to (-1,+1).
      //	Connect inputs of the CLARKE module and call the clarke transformation macro
      // ------------------------------------------------------------------------------
      // convert the ADC data
      //HAL_readAdcData(halHandle,&gAdcData);
      HAL_readAdcDataWithOffsets(halHandle,&gAdcData);
    
      // remove offsets
      gAdcData.I.value[0] = gAdcData.I.value[0] - gOffsets_I_pu.value[0];
      gAdcData.I.value[1] = gAdcData.I.value[1] - gOffsets_I_pu.value[1];
      gAdcData.I.value[2] = gAdcData.I.value[2] - gOffsets_I_pu.value[2];
      gAdcData.V.value[0] = gAdcData.V.value[0] - gOffsets_V_pu.value[0];
      gAdcData.V.value[1] = gAdcData.V.value[1] - gOffsets_V_pu.value[1];
      gAdcData.V.value[2] = gAdcData.V.value[2] - gOffsets_V_pu.value[2];
    
      // ADC processing and pwm result code goes here
    
      // run Clarke transform on current.  Three values are passed, two values
      // are returned.
      CLARKE_run(clarkeHandle_I,&gAdcData.I,&Iab_pu);
    
      // run Clarke transform on voltage.  Three values are passed, two values
      // are returned.
      CLARKE_run(clarkeHandle_V,&gAdcData.V,&Vab_pu);
    
      // run the estimator
      // The speed reference is needed so that the proper sign of the forced
      // angle is calculated. When the estimator does not do motor ID as in this
      // lab, only the sign of the speed reference is used
      EST_run(estHandle,&Iab_pu,&Vab_pu,gAdcData.dcBus,gMotorVars.SpeedRef_pu);
    
      // generate the motor electrical angle
      angle_pu = EST_getAngle_pu(estHandle);
      speed_pu = EST_getFm_pu(estHandle);
    
      // The voltage vector is now calculated and ready to be applied to the
      // motor in the form of three PWM signals.  However, even though the
      // voltages may be supplied to the PWM module now, they won't be
      // applied to the motor until the next PWM cycle. By this point, the
      // motor will have moved away from the angle that the voltage vector
      // was calculated for, by an amount which is proportional to the
      // sampling frequency and the speed of the motor.  For steady-state
      // speeds, we can calculate this angle delay and compensate for it.
      angle_pu = APP_angleDelayComp(speed_pu,angle_pu);
    
      // get Idq from estimator to avoid sin and cos, and a Park transform,
      // which saves CPU cycles
      EST_getIdq_pu(estHandle,&gIdq_pu);
    
      // write the PWM compare values
      HAL_writePwmData(halHandle,&gPwmData);
    #endif // (BUILDLEVEL==LEVEL2)
    
    // =============================== LEVEL 3 ======================================
    //	Level 3 verifies the dq-axis current regulation performed by PI and
    //	speed measurement modules
    // ==============================================================================
    #if (BUILDLEVEL==LEVEL3)
    
    #endif // (BUILDLEVEL==LEVEL3)
    
    
    // =============================== LEVEL 4 ======================================
    //	  Level 4 verifies the current model (CUR_MOD).
    // ==============================================================================
    #if (BUILDLEVEL==LEVEL4)
    
    #endif // (BUILDLEVEL==LEVEL4)
    
    // =============================== LEVEL 5 ======================================
    //	Level 5 verifies verifies the speed regulator performed by PI module.
    //	The system speed loop is closed by using the measured speed from capture
    //	signal as a feedback.
    // ==============================================================================
    #if (BUILDLEVEL==LEVEL5)
    
    #endif // (BUILDLEVEL==LEVEL5)
    
    // ------------------------------- FRAMEWORK ------------------------------------
      // read the timer 1 value and update the CPU usage module
      timer1Cnt = HAL_readTimerCnt(halHandle,1);
      CPU_USAGE_updateCnts(cpu_usageHandle,timer1Cnt);
    
      // run the CPU usage module
      CPU_USAGE_run(cpu_usageHandle);
    
    //-------------------------- END OF TESTING LEVELS ------------------------------
    #ifdef DEBUG
    	// toggle status LED in order to verify the ADC1 interrupt to programmer!
    	if(gLEDcnt++ > (uint_least32_t)(USER_ISR_FREQ_Hz / LED_BLINK_FREQ_Hz))
    	{
    	  HAL_toggleLed(halHandle,(GPIO_Number_e)HAL_Gpio_LED1);
    	  gLEDcnt = 0;
    	}
    #endif //DEBUG
    
      return;
    } // end of mainISR() function
    
    //! \brief     Setup the Clarke transform for either 2 or 3 sensors.
    //! \param[in] handle             The clarke (CLARKE) handle
    //! \param[in] numCurrentSensors  The number of current sensors
    void APP_setupClarke_I(CLARKE_Handle handle,const uint_least8_t numCurrentSensors)
    {
        _iq alpha_sf,beta_sf;
    
        // initialize the Clarke transform module for current
        if(numCurrentSensors == 3)
        {
            alpha_sf = _IQ(MATH_ONE_OVER_THREE);
            beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
        }
        else if(numCurrentSensors == 2)
        {
            alpha_sf = _IQ(1.0);
            beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
        }
        else
        {
            alpha_sf = _IQ(0.0);
            beta_sf = _IQ(0.0);
        }
    
        // set the parameters
        CLARKE_setScaleFactors(handle,alpha_sf,beta_sf);
        CLARKE_setNumSensors(handle,numCurrentSensors);
    
        return;
    } // end of setupClarke_I() function
    
    
    //! \brief     Setup the Clarke transform for either 2 or 3 sensors.
    //! \param[in] handle             The clarke (CLARKE) handle
    //! \param[in] numVoltageSensors  The number of voltage sensors
    void APP_setupClarke_V(CLARKE_Handle handle,const uint_least8_t numVoltageSensors)
    {
        _iq alpha_sf,beta_sf;
    
        // initialize the Clarke transform module for voltage
        if(numVoltageSensors == 3)
        {
            alpha_sf = _IQ(MATH_ONE_OVER_THREE);
            beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
        }
        else
        {
            alpha_sf = _IQ(0.0);
            beta_sf = _IQ(0.0);
        }
    
        // In other words, the only acceptable number of voltage sensors is three.
        // set the parameters
        CLARKE_setScaleFactors(handle,alpha_sf,beta_sf);
        CLARKE_setNumSensors(handle,numVoltageSensors);
    
        return;
    } // end of setupClarke_V() function
    
    //! \brief  The angleDelayComp function compensates for the delay introduced
    //! \brief  from the time when the system inputs are sampled to when the PWM
    //! \brief  voltages are applied to the motor windings.
    _iq APP_angleDelayComp(const _iq fm_pu,const _iq angleUncomp_pu)
    {
        _iq angleDelta_pu = _IQmpy(fm_pu,_IQ(USER_IQ_FULL_SCALE_FREQ_Hz
                    / (USER_PWM_FREQ_kHz*1000.0)));
        _iq angleCompFactor = _IQ(1.0 + (float_t)USER_NUM_PWM_TICKS_PER_ISR_TICK
                    * 0.5);
        _iq angleDeltaComp_pu = _IQmpy(angleDelta_pu,angleCompFactor);
        uint32_t angleMask = ((uint32_t)0xFFFFFFFF >> (32 - GLOBAL_Q));
        _iq angleComp_pu;
        _iq angleTmp_pu;
    
        // increment the angle
        angleTmp_pu = angleUncomp_pu + angleDeltaComp_pu;
    
        // mask the angle for wrap around
        // note: must account for the sign of the angle
        angleComp_pu = _IQabs(angleTmp_pu) & angleMask;
    
        // account for sign
        if(angleTmp_pu < _IQ(0.0))
        {
            angleComp_pu = -angleComp_pu;
        }
    
        return(angleComp_pu);
    } // end of angleDelayComp() function
    
    #ifdef FAST_ROM_V1p6
    //! \brief  Call this function to fix 1p6. This is only used for F2806xF/M
    //! \brief  implementation of InstaSPIN (version 1.6 of ROM) since the
    //! \brief  inductance calculation is not done correctly in ROM, so this
    //! \brief  function fixes that ROM bug.
    void APP_softwareUpdate1p6(EST_Handle handle)
    {
        float_t fullScaleInductance = USER_IQ_FULL_SCALE_VOLTAGE_V
                        / (USER_IQ_FULL_SCALE_CURRENT_A
                        * USER_VOLTAGE_FILTER_POLE_rps);
        float_t Ls_coarse_max = _IQ30toF(EST_getLs_coarse_max_pu(handle));
        int_least8_t lShift = ceil(log(USER_MOTOR_Ls_d / (Ls_coarse_max
                             * fullScaleInductance)) / log(2.0));
        uint_least8_t Ls_qFmt = 30 - lShift;
        float_t L_max = fullScaleInductance * pow(2.0,lShift);
        _iq Ls_d_pu = _IQ30(USER_MOTOR_Ls_d / L_max);
        _iq Ls_q_pu = _IQ30(USER_MOTOR_Ls_q / L_max);
    
        // store the results
        EST_setLs_d_pu(handle,Ls_d_pu);
        EST_setLs_q_pu(handle,Ls_q_pu);
        EST_setLs_qFmt(handle,Ls_qFmt);
    
        return;
    } // end of softwareUpdate1p6() function
    #endif //FASH_ROM_V1p6
    
    //! \brief     Update the global variables (gMotorVars).
    //! \param[in] handle  The estimator (EST) handle
    void APP_updateGlobalVariables(EST_Handle handle)
    {
        // get the speed estimate
        gMotorVars.Speed_krpm = EST_getSpeed_krpm(handle);
    
        // get the torque estimate
        {
            _iq Flux_pu = EST_getFlux_pu(handle);
            _iq Id_pu = PID_getFbackValue(pidHandle[1]);
            _iq Iq_pu = PID_getFbackValue(pidHandle[2]);
            _iq Ld_minus_Lq_pu = _IQ30toIQ(EST_getLs_d_pu(handle)
                        - EST_getLs_q_pu(handle));
    
            // Reactance Torque
            _iq Torque_Flux_Iq_Nm = _IQmpy(_IQmpy(Flux_pu,Iq_pu),
                        gTorque_Flux_Iq_pu_to_Nm_sf);
    
            // Reluctance Torque
            _iq Torque_Ls_Id_Iq_Nm = _IQmpy(_IQmpy(_IQmpy(Ld_minus_Lq_pu,Id_pu),
                        Iq_pu),gTorque_Ls_Id_Iq_pu_to_Nm_sf);
    
            // Total torque is sum of reactance torque and reluctance torque
            _iq Torque_Nm = Torque_Flux_Iq_Nm + Torque_Ls_Id_Iq_Nm;
    
            gMotorVars.Torque_Nm = Torque_Nm;
        }
    
        // get the magnetizing current
        gMotorVars.MagnCurr_A = EST_getIdRated(handle);
    
        // get the rotor resistance
        gMotorVars.Rr_Ohm = EST_getRr_Ohm(handle);
    
        // get the stator resistance
        gMotorVars.Rs_Ohm = EST_getRs_Ohm(handle);
    
        // get the stator inductance in the direct coordinate direction
        gMotorVars.Lsd_H = EST_getLs_d_H(handle);
    
        // get the stator inductance in the quadrature coordinate direction
        gMotorVars.Lsq_H = EST_getLs_q_H(handle);
    
        // get the flux in V/Hz in floating point
        gMotorVars.Flux_VpHz = EST_getFlux_VpHz(handle);
    
        // get the flux in Wb in fixed point
        gMotorVars.Flux_Wb = _IQmpy(EST_getFlux_pu(handle),gFlux_pu_to_Wb_sf);
    
        // get the estimator state
        gMotorVars.EstState = EST_getState(handle);
    
        // Get the DC buss voltage
        gMotorVars.VdcBus_kV = _IQmpy(gAdcData.dcBus,
                _IQ(USER_IQ_FULL_SCALE_VOLTAGE_V / 1000.0));
    
        // read Vd and Vq vectors per units
        gMotorVars.Vd = gVdq_out_pu.value[0];
        gMotorVars.Vq = gVdq_out_pu.value[1];
    
        // calculate vector Vs in per units: (Vs = sqrt(Vd^2 + Vq^2))
        gMotorVars.Vs = _IQsqrt(_IQmpy(gMotorVars.Vd,gMotorVars.Vd)
                + _IQmpy(gMotorVars.Vq,gMotorVars.Vq));
    
        // read Id and Iq vectors in amps
        gMotorVars.Id_A = _IQmpy(gIdq_pu.value[0],
                _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
        gMotorVars.Iq_A = _IQmpy(gIdq_pu.value[1],
                _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
    
        // calculate vector Is in amps:  (Is_A = sqrt(Id_A^2 + Iq_A^2))
        gMotorVars.Is_A = _IQsqrt(_IQmpy(gMotorVars.Id_A,gMotorVars.Id_A)
                + _IQmpy(gMotorVars.Iq_A,gMotorVars.Iq_A));
    
        return;
    } // end of updateGlobalVariables() function
    
    //! \brief     Setup the filters that used in the project.
    //! \param[in] handle  The Filter (FILTER) handle
    void APP_setupFilters(FILTER_FO_Handle *pHandle)
    {
    	//FILTER_FO_Obj *obj = (FILTER_FO_Obj *)handle;
    
    	uint16_t cnt = 0;
    	_iq 	 b0 = _IQ(gUserParams.offsetPole_rps/(float_t)gUserParams.ctrlFreq_Hz);
    	_iq		 a1 = (b0 - _IQ(1.0));
    	_iq		 b1 = _IQ(0.0);
    
    	for(cnt=0; cnt<6; cnt++)
    	{
    		//filterHandle[cnt] = FILTER_FO_init(&filter[cnt],sizeof(filter[cnt]));
    		FILTER_FO_setDenCoeffs(pHandle[cnt],a1);
    		FILTER_FO_setNumCoeffs(pHandle[cnt],b0,b1);
    		FILTER_FO_setInitialConditions(pHandle[cnt],_IQ(0.0),_IQ(0.0));
    	}
    
    	return;
    }
    
    //! \brief 	   Initializes and sets up the PID controllers which is used in the project
    //! \param[in] handle 	The PID controller handler
    void APP_setupPidControllers(void)
    {
    	// This equation defines the relationship between per unit current and
    	// real-world current. The resulting value in per units (pu) is then
    	// used to configure the controllers
    	_iq maxCurrent_pu = _IQ(USER_MOTOR_MAX_CURRENT/ USER_IQ_FULL_SCALE_CURRENT_A);
    
    	// This equation uses the scaled maximum voltage vector, which is
    	// already in per units, hence there is no need to include the #define
    	// for USER_IQ_FULL_SCALE_VOLTAGE_V
    	_iq maxVoltage_pu = _IQ(USER_MAX_VS_MAG_PU * USER_VD_SF);
    
    	float_t fullScaleCurrent = USER_IQ_FULL_SCALE_CURRENT_A;
    	float_t fullScaleVoltage = USER_IQ_FULL_SCALE_VOLTAGE_V;
    	float_t IsrPeriod_sec = 1.0 / USER_ISR_FREQ_Hz;
    	float_t Ls_d = USER_MOTOR_Ls_d;
    	float_t Ls_q = USER_MOTOR_Ls_q;
    	float_t Rs = USER_MOTOR_Rs;
    
    	// This project assumes that motor parameters are known, and it does not
    	// perform motor ID, so the R/L parameters are known and defined in user.h
    	float_t RoverLs_d = Rs / Ls_d;
    	float_t RoverLs_q = Rs / Ls_q;
    
    
    	// For the current controller, Kp = Ls*bandwidth(rad/sec)  But in order
    	// to be used, it must be converted to per unit values by multiplying
    	// by fullScaleCurrent and then dividing by fullScaleVoltage.  From the
    	// statement below, we see that the bandwidth in rad/sec is equal to
    	// 0.25/IsrPeriod_sec, which is equal to USER_ISR_FREQ_HZ/4. This means
    	// that by setting Kp as described below, the bandwidth in Hz is
    	// USER_ISR_FREQ_HZ/(8*pi).
    	_iq Kp_Id = _IQ((0.25 * Ls_d * fullScaleCurrent) / (IsrPeriod_sec
    				* fullScaleVoltage));
    
    	// In order to achieve pole/zero cancellation (which reduces the
    	// closed-loop transfer function from a second-order system to a
    	// first-order system), Ki must equal Rs/Ls.  Since the output of the
    	// Ki gain stage is integrated by a DIGITAL integrator, the integrator
    	// input must be scaled by 1/IsrPeriod_sec.  That's just the way
    	// digital integrators work.  But, since IsrPeriod_sec is a constant,
    	// we can save an additional multiplication operation by lumping this
    	// term with the Ki value.
    	_iq Ki_Id = _IQ(RoverLs_d * IsrPeriod_sec);
    
    	// Now do the same thing for Kp for the q-axis current controller.
    	// If the motor is not an IPM motor, Ld and Lq are the same, which
    	// means that Kp_Iq = Kp_Id
    	_iq Kp_Iq = _IQ((0.25 * Ls_q * fullScaleCurrent) / (IsrPeriod_sec
    				* fullScaleVoltage));
    
    	// Do the same thing for Ki for the q-axis current controller.  If the
    	// motor is not an IPM motor, Ld and Lq are the same, which means that
    	// Ki_Iq = Ki_Id.
    	_iq Ki_Iq = _IQ(RoverLs_q * IsrPeriod_sec);
    
    
    	// There are three PI controllers; one speed controller and two current
    	// controllers.  Each PI controller has two coefficients; Kp and Ki.
    	// So you have a total of six coefficients that must be defined.
    	// This is for the speed controller
    	pidHandle[0] = PID_init(&pid[0],sizeof(pid[0]));
    	// This is for the Id current controller
    	pidHandle[1] = PID_init(&pid[1],sizeof(pid[1]));
    	// This is for the Iq current controller
    	pidHandle[2] = PID_init(&pid[2],sizeof(pid[2]));
    
    	// The following instructions load the parameters for the speed PI
    	// controller.
    	PID_setGains(pidHandle[0],_IQ(1.0),_IQ(0.01),_IQ(0.0));
    
    	// The current limit is performed by the limits placed on the speed PI
    	// controller output.  In the following statement, the speed
    	// controller's largest negative current is set to -maxCurrent_pu, and
    	// the largest positive current is set to maxCurrent_pu.
    	PID_setMinMax(pidHandle[0],-maxCurrent_pu,maxCurrent_pu);
    	PID_setUi(pidHandle[0],_IQ(0.0));  // Set the initial condition value
    									   // for the integrator output to 0
    
    	pidCntSpeed = 0;  // Set the counter for decimating the speed
    					  // controller to 0
    
    	// The following instructions load the parameters for the d-axis
    	// current controller.
    	// P term = Kp_Id, I term = Ki_Id, D term = 0
    	PID_setGains(pidHandle[1],Kp_Id,Ki_Id,_IQ(0.0));
    
    	// Largest negative voltage = -maxVoltage_pu, largest positive
    	// voltage = maxVoltage_pu
    	PID_setMinMax(pidHandle[1],-maxVoltage_pu,maxVoltage_pu);
    
    	// Set the initial condition value for the integrator output to 0
    	PID_setUi(pidHandle[1],_IQ(0.0));
    
    	// The following instructions load the parameters for the q-axis
    	// current controller.
    	// P term = Kp_Iq, I term = Ki_Iq, D term = 0
    	PID_setGains(pidHandle[2],Kp_Iq,Ki_Iq,_IQ(0.0));
    
    	// The largest negative voltage = 0 and the largest positive
    	// voltage = 0.  But these limits are updated every single ISR before
    	// actually executing the Iq controller. The limits depend on how much
    	// voltage is left over after the Id controller executes. So having an
    	// initial value of 0 does not affect Iq current controller execution.
    	PID_setMinMax(pidHandle[2],_IQ(0.0),_IQ(0.0));
    
    	// Set the initial condition value for the integrator output to 0
    	PID_setUi(pidHandle[2],_IQ(0.0));
    
    	return;
    }
    //@} //defgroup
    // end of file
    
    
    
    
    

    vfd_main.h

    I am looking forward to hearing from you soon.

    With kind regards,

    Tran

  • Hi,

    In hal.c, let's remove the third count, since this is initialized to NULL, I am affraid this code is writing to uninitialized memory or even some registers unintentionally:

    void HAL_setupPwmDacs(HAL_Handle handle)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
      uint16_t halfPeriod_cycles = 512;       // 3000->10kHz, 1500->20kHz, 1000-> 30kHz, 500->60kHz
      uint_least8_t    cnt;
    
    
      for(cnt=0;cnt<2;cnt++)

    Let's also comment this out at the end of the function

      //PWMDAC_setPeriod(obj->pwmDacHandle[PWMDAC_Number_3],halfPeriod_cycles);
    
      return;
    }  // end of HAL_setupPwmDacs() function

    Could you also send me hal.h file? I want to double check the implementation of function: HAL_writeDacData

    Thanks,

    -Jorge

  • Dear Jorge,

    Here is the hal.h which you required, please download at: 

    7801.hal.h

    With kind regards,

    Tran,

  • I don't find anything wrong. By the way, I think this function is a bit more compact and easier to debug when it comes to writing DAC values:

    static inline void HAL_writeDacData(HAL_Handle handle,HAL_DacData_t *pDacData)
    {
      HAL_Obj *obj = (HAL_Obj *)handle;
      PWM_Obj *pwm = (PWM_Obj *)obj->pwmDacHandle[PWMDAC_Number_1];
      _iq period = (_iq)pwm->TBPRD;
      _iq pwmData_sat, pwmData_sat_dc, value;
      uint16_t value_sat[4];
      uint_least8_t cnt;
    
    
      for(cnt=0;cnt<4;cnt++)
        {
          pwmData_sat = _IQsat(pDacData->value[cnt],_IQ(0.5),_IQ(-0.5));
          pwmData_sat_dc = pwmData_sat + _IQ(0.5);
          value = _IQmpy(pwmData_sat_dc, period);
          value_sat[cnt] = (uint16_t)_IQsat(value, period, _IQ(0.0));
        }
    
    
      // write the DAC data
      PWMDAC_write_CmpA(obj->pwmDacHandle[PWMDAC_Number_1],value_sat[0]);
      PWMDAC_write_CmpB(obj->pwmDacHandle[PWMDAC_Number_1],value_sat[1]);
      PWMDAC_write_CmpA(obj->pwmDacHandle[PWMDAC_Number_2],value_sat[2]);
      PWMDAC_write_CmpB(obj->pwmDacHandle[PWMDAC_Number_2],value_sat[3]);
    
      return;
    } // end of HAL_writeDacData() function

    Can you please use that function? and let's write a known value to all four DACs so we can focus on the one(s) with the problem.

    -Jorge

  • Hi Jorge,

    That's a good idea, your compact function to implement HAL_writeDacData(). I tried and verify that it worked well. 

    For my problem, I have just found that there is a hardware error in my mainboard where I plug my controlCARD on it. When I fixed this mistake the results on Dac outputs are true.

    I verify that all the problem was resolved. Thank you for your following this topic with me. I am also sorry about this inconvenience.

    Hoping to see you in the other topics on our forum.

    Thanks again!

    Tran,

  • I am glad it worked! :)
  • Jorge:

    You make the following statement:
    "Then in the ISR, add the values you want to monitor, from the example above:"
    I have searched through the code and I cannot figure out how gPwmData.Tabc.value[] is mapped to the output of the SVPWM generator. Can you provide more detail? I would like to monitor more than just the SVPWM process. For instance the ramp generator, or the output of the PI loop, or the output of the feedback/Park inputs.

    Kurt Kloesel
  • Hi Kurt,

    The below space vector generator (SVGEN) module outputs the gPwmData.Tabc.value[], you can find the function define in svgen.h.
    SVGEN_run(svgenHandle,&Vab_pu,&(gPwmData.Tabc));

    And you can use PWMDAC to monitor any variables as the SVGEN output also.

    You can refer to below code if you was using lab11a+ in motorWare.

    _iq speedRef = TRAJ_getIntValue(trajHandle_spd);

    dacValue_1 = gVdq_out_pu.value[0]; // Id PI output
    dacValue_2 = gVdq_out_pu.value[1]; // Iq PI output
    dacValue_3 = gIdq_ref_pu.value[1]; // Speed PI output
    dacValue_4 = speedRef ; // Speed PI reference input

    gDacData.value[0] = dacValue_1;
    gDacData.value[1] = dacValue_2;
    gDacData.value[2] = dacValue_3;
    gDacData.value[3] = dacValue_4;

    // write the data to DAC module
    HAL_writeDacData(halHandle, &gDacData);

    Best Regards,
    Yanming
  • Thanks Yanming and Jorge, I was able to successfully use Jorge's files on a DRV8312 board
    to view the outputs of the SVM generator in Motorware motorware_1_01_00_16! So.....

    If we look at proj_lab3a.c and the Jorge version, labeled '2570.proj_lab03a.c'
    We see that the controller is run via the following:
    // STEP 1 - proj_lab3a.c
    // run the controller
    CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData);

    // STEP 2 - function within controller function
    // run the space Vector Generator (SVGEN) module
    SVGEN_run(obj->svgenHandle,CTRL_getVab_out_addr(handle),&(pPwmData->Tabc));

    Tabc is defined in hal_obj.h as a MATH_vec3, and in the ctrl.h function pT is associated with Tabc:

    // STEP 3 - Function defined in ctrl.h
    static inline void SVGEN_run(SVGEN_Handle handle,const MATH_vec2 *pVab,MATH_vec3 *pT)


    So if I want to look at other parts of the operation with a oscope using the pwmdac,
    for instance, the inputs and outputs of the clarke module,
    the CTRL_runOnLine or the CTRL_runOnLine_User functions both perform a "CLARKE_run" function

    // run Clarke transform on current
    CLARKE_run(obj->clarkeHandle_I,&pAdcData->I,CTRL_getIab_in_addr(handle));

    If we look at clarke.h the function definition passes MATH_vec3 and MATH_vec2 values,
    and both inputs I and V are defined in hal_obj.h as MATH_vec3
    and the output CTRL_getIab_in_addr is defined as a MATCH_vec2 in ctrl.h:

    static inline void CLARKE_run(CLARKE_Handle handle,const MATH_vec3 *pInVec,MATH_vec2 *pOutVec)

    Thus these can be examined by making the following statements:

    gDacData.value[0] = gAdcData.I.value[0]; //Jorge
    gDacData.value[1] = gAdcData.I.value[1]; //Jorge
    gDacData.value[2] = gAdcData.I.value[2]; //Jorge

    and commenting out the following statements:

    // gDacData.value[0] = gPwmData.Tabc.value[0]; //Jorge
    // gDacData.value[1] = gPwmData.Tabc.value[1]; //Jorge
    // gDacData.value[2] = gPwmData.Tabc.value[2]; //Jorge

    // write the PWM compare values
    HAL_writePwmData(halHandle,&gAdcData); // gAdcData is now the input instead of gPwmData

    Did I miss something? I have to go to the lab to compile this and try it out!




    Notes:
    // In the document Spruhj1f.pdf, page 24, Figure 1-2.i. the SVM module accepts two main inputs and outputs three.
    // the inputs are Vab, in this case a MATH_vec2 pointer is passed through the function named *pVab and
    // the outputs are a MATH_vec3 pointer is passed out called *pT
    // Vab or Valpha-beta was obtained from the inverse Park module
  • Yanming-
    As per your previous statements:

    _iq speedRef = TRAJ_getIntValue(trajHandle_spd);

    dacValue_1 = gVdq_out_pu.value[0]; // Id PI output
    dacValue_2 = gVdq_out_pu.value[1]; // Iq PI output
    dacValue_3 = gIdq_ref_pu.value[1]; // Speed PI output
    dacValue_4 = speedRef ; // Speed PI reference input

    These are inputs to the inverse Park module and the Idq PI module.
    These references are as I orginally requested, thanks. Therefore if I wanted to
    look at the inputs to the inverse Park module,
    the IPARK is run from the "CTRL_run" in the proj_lab03a.c
    and that in turn runs "CTRL_runOnLine" in ctrl.c and defined in ctrl.h
    // run the online controller
    CTRL_runOnLine(handle,pAdcData,pPwmData);
    The "CTRL_runOnLine" then runs the IPARK function

    // run the inverse Park module
    IPARK_run(obj->iparkHandle,CTRL_getVdq_out_addr(handle),CTRL_getVab_out_addr(handle));

    Of which both CTRL_getVdq_out_addr(handle) and CTRL_getVab_out_addr(handle) are defined as MATH_vec2
    in the ctrl.h file. Another way would be to look at the output of "PID_run" which is what you stated,
    "dacValue_2 = gVdq_out_pu.value[1]; // Iq PI output".
    My question is:
    Do I have to set HAL_writePwmData function to input the gVdq_out?

    Kurt
  • Jorge -

    I would like to transfer this code to the DRV8301 kit, however when I compare the original DRV8312 file hal.h to your modifided hal.h, the changes seem substantial. Are all your changes to hal.h related specifically to this issue of displaying PWMDAC?


    Kurt
  • Yanming,

    Thanks for the suggestion regarding lab11a, but I have invested a lot of time working on labs 2 through 5.  I have made significant modifications to labs 4 - 5 using CAN interfaces. I think I understand the significance of lab11a and why you feel you must encourage development in this area.  My intermediate position is; Jorge has gotten the pwmdac to output SVG signals, I made some progress getting other signals out of lab3a, If it ain't broke don't fix it.  Switching to 11 would impact me severely.  The  InstaSPIN labs are set up in a deliberate sequence and easily accommodate student learing.  I have given the InstaSPIN labs sequence to several of my students, and they seem to get through it OK.  The addition of pwmdac readouts would greatly enhance the learning experience.

    Thanks,

    Kurt Kloesel