//#############################################################################
// $Copyright:
// Copyright (C) 2017-2023 Texas Instruments Incorporated - http://www.ti.com/
// 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.
// $
//#############################################################################


//! \file   /solutions/tidm_02010_dmpfc/common/source/motor1_drive.c
//!
//! \brief  This project is used to control two motor and pfc with one MCU
//!         Support for dmpfc board with F28002x/F28003x/F280013x
//!

//
// include the related header files
//
#include "sys_settings.h"
#include "sys_main.h"

#pragma CODE_SECTION(motor1CtrlISR, "ctrlfuncs");

// the globals
float32_t angleFOCM1_rad;     //!< the rotor angle from FOC modules
#pragma DATA_SECTION(angleFOCM1_rad, "motor_data");

#if defined(MOTOR1_DCLINKSS)
//!< the handle for single-shunt current reconstruction
DCLINK_SS_Handle dclinkM1Handle;

//!< the single-shunt current reconstruction object
DCLINK_SS_Obj    dclinkM1;

#pragma DATA_SECTION(dclinkM1Handle, "motor_data");
#pragma DATA_SECTION(dclinkM1, "motor_data");
#endif  // MOTOR1_DCLINKSS

#ifdef MOTOR1_ESMO
//!< the handle for the esmo object
ESMO_Handle   esmoM1Handle;

//!< the esmo object
ESMO_Obj      esmoM1;
#pragma DATA_SECTION(esmoM1, "motor_data");

//!< the handle for the speedfr object
SPDFR_Handle spdfrM1Handle;

//!< the speedfr object
SPDFR_Obj spdfrM1;
#pragma DATA_SECTION(spdfrM1, "motor_data");

float32_t angleCompM1_rad;
float32_t anglePLLM1_rad;
float32_t angleSMOM1_rad;
float32_t speedPLLM1_Hz;
float32_t frswPosM1_sf;

#pragma DATA_SECTION(angleCompM1_rad, "motor_data");
#pragma DATA_SECTION(anglePLLM1_rad, "motor_data");
#pragma DATA_SECTION(angleSMOM1_rad, "motor_data");
#pragma DATA_SECTION(speedPLLM1_Hz, "motor_data");
#pragma DATA_SECTION(frswPosM1_sf, "motor_data");
#endif  // MOTOR1_ESMO

#if defined(MOTOR1_FAST)
EST_Handle    estM1Handle;     //!< the handle for the estimator
EST_InputData_t estInputDataM1;
EST_OutputData_t estOutputDataM1;

#pragma DATA_SECTION(estM1Handle, "motor_data");
#pragma DATA_SECTION(estInputDataM1, "motor_data");
#pragma DATA_SECTION(estOutputDataM1, "motor_data");

float32_t angleDeltaM1_rad;   //!< the rotor angle compensation value
float32_t angleESTM1_rad;     //!< the rotor angle from EST modules
float32_t speedESTM1_Hz;     //!< the speed from EST modules

#pragma DATA_SECTION(angleDeltaM1_rad, "motor_data");
#pragma DATA_SECTION(angleESTM1_rad, "motor_data");
#pragma DATA_SECTION(speedESTM1_Hz, "motor_data");
#endif  // MOTOR1_FAST

#if((DMC_BUILDLEVEL == DMC_LEVEL_2) || (DMC_BUILDLEVEL == DMC_LEVEL_3) || \
        defined(MOTOR1_ESMO))
//!< the handles for Angle Generate for open loop control
ANGLE_GEN_Handle angleGenM1Handle;

//!< the Angle Generate onject for open loop control
ANGLE_GEN_Obj    angleGenM1;
float32_t angleGenM1_rad;     //!< the rotor angle from Generator modules
float32_t speedRefM1_Hz;

#pragma DATA_SECTION(angleGenM1Handle, "motor_data");
#pragma DATA_SECTION(angleGenM1, "motor_data");
#pragma DATA_SECTION(angleGenM1_rad, "motor_data");
#pragma DATA_SECTION(speedRefM1_Hz, "motor_data");
#endif  // ((DMC_BUILDLEVEL <= DMC_LEVEL_3) || defined(MOTOR1_ESMO))

// Only compressor needs MTPA, FWC and SSIP functions
#if defined(MOTOR1_SSIPD)
SSIPD_Handle    ssipdHandle;
SSIPD_Obj       ssipd;

#pragma DATA_SECTION(ssipdHandle, "motor_data");
#pragma DATA_SECTION(ssipd, "motor_data");

// IPD is only for compressor
bool flagEnableIPD;
float32_t angleOffsetIPD_rad;
float32_t angleDetectIPD_rad;

#pragma DATA_SECTION(flagEnableIPD, "motor_data");
#pragma DATA_SECTION(angleOffsetIPD_rad, "motor_data");
#pragma DATA_SECTION(angleDetectIPD_rad, "motor_data");
#endif  // MOTOR1_SSIPD

#ifdef MOTOR1_MTPA  // MPTA and FWC are only for compressor
//!< the handle and object for the fwc PI controller
float32_t mtpaKconst;
float32_t LsOnline_d_H;
float32_t LsOnline_q_H;
float32_t fluxOnline_Wb;
float32_t angleMTPA_rad;

#pragma DATA_SECTION(mtpaKconst, "motor_data");
#pragma DATA_SECTION(LsOnline_d_H, "motor_data");
#pragma DATA_SECTION(LsOnline_q_H, "motor_data");
#pragma DATA_SECTION(fluxOnline_Wb, "motor_data");
#pragma DATA_SECTION(angleMTPA_rad, "motor_data");

//!< the handle for the Maximum torque per ampere (MTPA)
MTPA_Handle  mtpaHandle;

//!< the Maximum torque per ampere (MTPA) object
MTPA_Obj     mtpa;

#pragma DATA_SECTION(mtpaHandle, "motor_data");
#pragma DATA_SECTION(mtpa, "motor_data");
#endif  // MOTOR1_MTPA

#if defined(MOTOR1_FWC) // MPTA and FWC are only for compressor
//!< the handle and object for the fwc PI controller
float32_t Kp_fwc;
float32_t Ki_fwc;
float32_t angleFWCMax_rad;
float32_t angleFWC_rad;
float32_t VsRef_pu;
float32_t VsRef_V;

#pragma DATA_SECTION(Kp_fwc, "motor_data");
#pragma DATA_SECTION(Ki_fwc, "motor_data");
#pragma DATA_SECTION(angleFWCMax_rad, "motor_data");
#pragma DATA_SECTION(angleFWC_rad, "motor_data");
#pragma DATA_SECTION(VsRef_pu, "motor_data");
#pragma DATA_SECTION(VsRef_V, "motor_data");

PI_Handle    piHandle_fwc;
PI_Obj       pi_fwc;

#pragma DATA_SECTION(piHandle_fwc, "motor_data");
#pragma DATA_SECTION(pi_fwc, "motor_data");
#endif  // MOTOR1_FWC

#if defined(MOTOR1_VIBCOMPA)
VIB_COMP_Handle vibCompHandle;
VIB_COMP_Obj    vibComp;

float32_t       vibCompAlpha;
float32_t       vibCompGain;
int16_t         vibCompIndexDelta;
bool            vibCompFlagReset;
bool            vibCompFlagEnable;

#pragma DATA_SECTION(vibCompHandle, "vibc_data");
#pragma DATA_SECTION(vibComp, "vibc_data");

#pragma DATA_SECTION(vibCompAlpha, "vibc_data");
#pragma DATA_SECTION(vibCompGain, "vibc_data");
#pragma DATA_SECTION(vibCompIndexDelta, "vibc_data");
#pragma DATA_SECTION(vibCompFlagReset, "vibc_data");
#pragma DATA_SECTION(vibCompFlagEnable, "vibc_data");

#elif defined(MOTOR1_VIBCOMPT)
VIB_COMP_Handle vibCompHandle;
VIB_COMP_Obj    vibComp;

float32_t       compressorAngle;
float32_t       vibCompAlpha0;
float32_t       vibCompAlpha120;
float32_t       vibCompAlpha240;

#pragma DATA_SECTION(vibCompHandle, "vibc_data");
#pragma DATA_SECTION(vibComp, "vibc_data");

#pragma DATA_SECTION(compressorAngle, "vibc_data");
#pragma DATA_SECTION(vibCompAlpha0, "vibc_data");
#pragma DATA_SECTION(vibCompAlpha120, "vibc_data");
#pragma DATA_SECTION(vibCompAlpha240, "vibc_data");
#endif  // MOTOR1_VIBCOMPA || MOTOR1_VIBCOMPT


float32_t speedVarLow_Hz;
float32_t speedVarHigh_Hz;
float32_t speedVarSF;

#pragma DATA_SECTION(speedVarLow_Hz, "motor_data");
#pragma DATA_SECTION(speedVarHigh_Hz, "motor_data");
#pragma DATA_SECTION(speedVarSF, "motor_data");

float32_t Kp_spd_M1[3];
float32_t Ki_spd_M1[3];
#pragma DATA_SECTION(Kp_spd_M1, "motor_data");
#pragma DATA_SECTION(Ki_spd_M1, "motor_data");

float32_t Kp_Iq_M1[2];
float32_t Ki_Iq_M1[2];
#pragma DATA_SECTION(Kp_Iq_M1, "motor_data");
#pragma DATA_SECTION(Ki_Iq_M1, "motor_data");

float32_t angleCurrentM1_rad;
#pragma DATA_SECTION(angleCurrentM1_rad, "motor_data");

bool flagEnableFWCM1;
bool flagEnableMTPAM1;
bool flagUpdateMTPAParamsM1;

#pragma DATA_SECTION(flagEnableFWCM1, "motor_data");
#pragma DATA_SECTION(flagEnableMTPAM1, "motor_data");
#pragma DATA_SECTION(flagUpdateMTPAParamsM1, "motor_data");

uint16_t tripFaultFlag_M1;
uint16_t tripFaultCount_M1;

#pragma DATA_SECTION(tripFaultFlag_M1, "motor_data");
#pragma DATA_SECTION(tripFaultCount_M1, "motor_data");


__interrupt void motor1CtrlISR(void)
{


    motorVars[MTR_1].ISRCount++;

    // acknowledge the interrupt of motor 1
    HAL_ackMtr1ADCInt();

#ifdef NEST_INT_ENABLE
    HAL_enableMtr1NestInterrupt();
#endif  // NEST_INT_ENABLE

    // read the ADC data with offsets
    HAL_readMtr1ADCData(&adcData[MTR_1]);

#if defined(MOTOR1_DCLINKSS)
    // run single-shunt current reconstruction
    DCLINK_SS_runCurrentReconstruction(dclinkM1Handle,
                                       &adcData[MTR_1].Idc1_A,
                                       &adcData[MTR_1].Idc2_A);     // 4 sampling

    adcData[MTR_1].I_A.value[0] = DCLINK_SS_getIa(dclinkM1Handle);
    adcData[MTR_1].I_A.value[1] = DCLINK_SS_getIb(dclinkM1Handle);
    adcData[MTR_1].I_A.value[2] = DCLINK_SS_getIc(dclinkM1Handle);
#endif  // MOTOR1_DCLINKSS
#ifdef PFC_DISABLE
    HAL_readPFCADCData(&adcDataPFC);
    pfcVars.VdcBus_V = adcDataPFC.VdcBus * USER_PFC_ADC_FULL_SCALE_DC_VOLTAGE_V;
#endif  // PFC_DISABLE

#if defined(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_MOTOR1)
    sfraNoiseInj_pu = 0.0f;
    sfraNoiseId = 0.0f;
    sfraNoiseIq = 0.0f;
    sfraNoiseSpd = 0.0f;

    // SFRA injection, create SFRA Noise per 'sfraTestLoop'
    sfraNoiseInj_pu = SFRA_F32_inject(0.0f);

    if(sfraTestLoop == SFRA_TEST_MOTOR1_ID)
    {
        sfraNoiseId = sfraNoiseInj_pu * USER_M1_ADC_FULL_SCALE_CURRENT_A;
    }
    else if(sfraTestLoop == SFRA_TEST_MOTOR1_IQ)
    {
        sfraNoiseIq = sfraNoiseInj_pu * USER_M1_ADC_FULL_SCALE_CURRENT_A;
    }
    else if(sfraTestLoop == SFRA_TEST_MOTOR1_SPEED)
    {
        sfraNoiseSpd = sfraNoiseInj_pu * USER_MOTOR1_FREQ_MAX_Hz;
    }
#endif  // SFRA_ENABLE && SFRA_TEST_MOTOR1

#if defined(MOTOR1_DISABLE)
// This motor is disable
    // No any code here

// Runs FAST and eSMO in parallel
#elif defined(MOTOR1_FAST) && defined(MOTOR1_ESMO)
    MATH_Vec2 phasor;
    bool flagEnableSpeedCtrl = false;
    bool flagEnableCurrentCtrl = false;

    ANGLE_GEN_run(angleGenM1Handle, estInputDataM1.speed_ref_Hz);
    angleGenM1_rad = ANGLE_GEN_getAngle(angleGenM1Handle);

    // run Clarke transform on current
    CLARKE_run(clarkeHandle_I[MTR_1],
               &adcData[MTR_1].I_A, &estInputDataM1.Iab_A);

    // remove offsets
    adcData[MTR_1].V_V.value[0] -=
            adcData[MTR_1].offset_V_sf[0] * pfcVars.VdcBus_V;

    adcData[MTR_1].V_V.value[1] -=
            adcData[MTR_1].offset_V_sf[1] * pfcVars.VdcBus_V;

    adcData[MTR_1].V_V.value[2] -=
            adcData[MTR_1].offset_V_sf[2] * pfcVars.VdcBus_V;
    // run Clarke transform on voltage
    CLARKE_run(clarkeHandle_V[MTR_1],
               &adcData[MTR_1].V_V, &(estInputDataM1.Vab_V));

    // store the input data into a buffer
    estInputDataM1.dcBus_V = pfcVars.VdcBus_V;

    // run the FAST estimator
    EST_run(estM1Handle, &estInputDataM1, &estOutputDataM1);

    // compute angle with delay compensation
    angleDeltaM1_rad = userParams[MTR_1].angleDelayed_sf_sec *
                     estOutputDataM1.fm_lp_rps;

    angleESTM1_rad = MATH_incrAngle(estOutputDataM1.angle_rad, angleDeltaM1_rad);
    speedESTM1_Hz = EST_getFm_lp_Hz(estM1Handle);

    // run the eSMO
    ESMO_setSpeedRef(esmoM1Handle, estInputDataM1.speed_ref_Hz);
    ESMO_inline_run(esmoM1Handle, pfcVars.VdcBus_V,
                    &(pwmData[MTR_1].Vabc_pu), &(estInputDataM1.Iab_A));

    angleCompM1_rad = speedPLLM1_Hz * motorVars[MTR_1].angleDelayed_sf;
    anglePLLM1_rad = MATH_incrAngle(ESMO_getAnglePLL(esmoM1Handle), angleCompM1_rad);
    angleSMOM1_rad = ESMO_getAngleElec(esmoM1Handle);

    SPDFR_run(spdfrM1Handle, anglePLLM1_rad);
    speedPLLM1_Hz = SPDFR_getSpeedHz(spdfrM1Handle);

#if defined(_SIMPLE_FAST_LIB)
    // Identification
    if( (EST_getState(estM1Handle) == EST_STATE_RS) &&
            (EST_isEnabled(estM1Handle) == true))
#else    // !(_SIMPLE_FAST_LIB)
    // Identification
    if(((EST_isMotorIdentified(estM1Handle) == false) ||
            (EST_getState(estM1Handle) == EST_STATE_RS)) &&
            (EST_isEnabled(estM1Handle) == true))
#endif   // !(_SIMPLE_FAST_LIB)
    {
        Idq_ref_A[MTR_1].value[0] = 0.0f;
        motorVars[MTR_1].motorState = MOTOR_CTRL_RUN;

        // setup the trajectory generator
        EST_setupTrajState(estM1Handle,
                           Idq_ref_A[MTR_1].value[1],
                           motorVars[MTR_1].speedRef_Hz,
                           0.0);

        // run the trajectories
        EST_runTraj(estM1Handle);

        // store the input data into a buffer
#if !defined(_SIMPLE_FAST_LIB)
        estInputDataM1.speed_ref_Hz = EST_getIntValue_spd_Hz(estM1Handle);
#endif   // !(_SIMPLE_FAST_LIB)

        flagEnableSpeedCtrl = EST_doSpeedCtrl(estM1Handle);
        flagEnableCurrentCtrl = EST_doCurrentCtrl(estM1Handle);

        motorVars[MTR_1].IdRated_A = EST_getIntValue_Id_A(estM1Handle);

        angleESTM1_rad = estOutputDataM1.angle_rad;
    }
    else if(motorVars[MTR_1].flagMotorIdentified == true)
    {
        if(motorVars[MTR_1].flagRunIdentAndOnLine == true)
        {
            counterTrajSpeed[MTR_1]++;

            if(counterTrajSpeed[MTR_1] >= userParams[MTR_1].numIsrTicksPerTrajTick)
            {
                // clear counter
                counterTrajSpeed[MTR_1] = 0;

                // run a trajectory for speed reference,
                // so the reference changes with a ramp instead of a step
                TRAJ_run(trajHandle_spd[MTR_1]);
            }

            flagEnableSpeedCtrl = motorVars[MTR_1].flagEnableSpeedCtrl;
            flagEnableCurrentCtrl = true;
        }

        estInputDataM1.speed_ref_Hz = TRAJ_getIntValue(trajHandle_spd[MTR_1]);

#if !defined(_SIMPLE_FAST_LIB)
        // get Id reference for Rs OnLine
        motorVars[MTR_1].IdRated_A = EST_getIdRated_A(estM1Handle);
#else
        motorVars[MTR_1].IdRated_A = 0.0f;
#endif   // !(_SIMPLE_FAST_LIB)
    }

    motorVars[MTR_1].stateRunTimeCnt++;

#if(DMC_BUILDLEVEL > DMC_LEVEL_3)
    if(motorVars[MTR_1].estimatorMode == ESTIMATOR_MODE_FAST)
    {
        motorVars[MTR_1].speed_Hz = speedESTM1_Hz;

        if(motorVars[MTR_1].motorState >= MOTOR_CL_RUNNING)
        {
            angleFOCM1_rad = angleESTM1_rad;
        }
        else if(motorVars[MTR_1].motorState == MOTOR_OL_START)
        {
            angleFOCM1_rad = angleESTM1_rad;

            if(estInputDataM1.speed_ref_Hz >= motorVars[MTR_1].speedForce_Hz)
            {
                motorVars[MTR_1].motorState = MOTOR_CL_RUNNING;
                ESMO_setAnglePu(esmoM1Handle, angleFOCM1_rad);
            }
        }
        else if(motorVars[MTR_1].motorState == MOTOR_ALIGNMENT)
        {
            angleFOCM1_rad = 0.0f;
            flagEnableSpeedCtrl = false;

            motorVars[MTR_1].IsRef_A = 0.0f;
            Idq_ref_A[MTR_1].value[0] = motorVars[MTR_1].startCurrent_A;
            Idq_ref_A[MTR_1].value[1] = 0.0f;

            TRAJ_setIntValue(trajHandle_spd[MTR_1], 0.0f);

            if(motorVars[MTR_1].stateRunTimeCnt > motorVars[MTR_1].lockRotorTimeDelay)
            {
                motorVars[MTR_1].stateRunTimeCnt = 0;
                motorVars[MTR_1].motorState = MOTOR_OL_START;

                EST_setAngle_rad(estM1Handle, angleFOCM1_rad);
                PI_setUi(piHandle_spd[MTR_1], 0.0);

                ESMO_setAnglePu(esmoM1Handle, angleFOCM1_rad);
                ANGLE_GEN_setAngle(angleGenM1Handle, angleFOCM1_rad);
            }
        }
    }
    else if(motorVars[MTR_1].estimatorMode == ESTIMATOR_MODE_ESMO)
    {
        motorVars[MTR_1].speed_Hz = speedPLLM1_Hz;

        if(motorVars[MTR_1].motorState >= MOTOR_CL_RUNNING)
        {
            angleFOCM1_rad = anglePLLM1_rad;

            ESMO_updateKslide(esmoM1Handle);
        }
        else if(motorVars[MTR_1].motorState == MOTOR_OL_START)
        {
            angleFOCM1_rad = angleGenM1_rad;
            flagEnableSpeedCtrl = false;

            motorVars[MTR_1].IsRef_A = motorVars[MTR_1].startCurrent_A;
            Idq_ref_A[MTR_1].value[0] = 0.0f;
            Idq_ref_A[MTR_1].value[1] = motorVars[MTR_1].startCurrent_A;

            if(estInputDataM1.speed_ref_Hz >= motorVars[MTR_1].speedForce_Hz)
            {
                TRAJ_setIntValue(trajHandle_spd[MTR_1], estInputDataM1.speed_ref_Hz);

                if(motorVars[MTR_1].stateRunTimeCnt > motorVars[MTR_1].forceRunTimeDelay)
                {
                    motorVars[MTR_1].motorState = MOTOR_CL_RUNNING;

                    EST_setAngle_rad(estM1Handle, angleFOCM1_rad);
                    ESMO_setAnglePu(esmoM1Handle, angleFOCM1_rad);

                    PI_setUi(piHandle_spd[MTR_1], (frswPosM1_sf * Idq_ref_A[MTR_1].value[1]));
                }
            }
        }
        else if(motorVars[MTR_1].motorState == MOTOR_ALIGNMENT)
        {
            angleFOCM1_rad = 0.0f;
            flagEnableSpeedCtrl = false;

            motorVars[MTR_1].IsRef_A = 0.0f;
            Idq_ref_A[MTR_1].value[0] = motorVars[MTR_1].startCurrent_A;
            Idq_ref_A[MTR_1].value[1] = 0.0f;

            if(motorVars[MTR_1].stateRunTimeCnt > motorVars[MTR_1].lockRotorTimeDelay)
            {
                motorVars[MTR_1].motorState = MOTOR_OL_START;

                EST_setAngle_rad(estM1Handle, angleFOCM1_rad);
                ESMO_setAnglePu(esmoM1Handle, angleFOCM1_rad);
                ANGLE_GEN_setAngle(angleGenM1Handle, angleFOCM1_rad);
            }
        }
    }   // Check motor control state machine

    // Not flyingstart for compressor

    // compute the sin/cos phasor
    phasor.value[0] = __cos(angleFOCM1_rad);
    phasor.value[1] = __sin(angleFOCM1_rad);

    // set the phasor in the Park transform
    PARK_setPhasor(parkHandle_I[MTR_1], &phasor);

    // run the Park transform
    PARK_run(parkHandle_I[MTR_1], &(estInputDataM1.Iab_A),
             (MATH_vec2 *)&(Idq_in_A[MTR_1]));

    // run the speed controller
    if(flagEnableSpeedCtrl == true)
    {
        counterSpeed[MTR_1]++;

        if(counterSpeed[MTR_1] >= userParams[MTR_1].numCtrlTicksPerSpeedTick)
        {
            counterSpeed[MTR_1] = 0;

#if defined(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_MOTOR1)
            PI_run(piHandle_spd[MTR_1],
                   estInputDataM1.speed_ref_Hz + sfraNoiseSpd,
                   motorVars[MTR_1].speed_Hz,
                   (float32_t *)&motorVars[MTR_1].IsRef_A);
#else  // !(SFRA_ENABLE && SFRA_TEST_MOTOR1)
            PI_run(piHandle_spd[MTR_1],
                   estInputDataM1.speed_ref_Hz,
                   motorVars[MTR_1].speed_Hz,
                   (float32_t *)&motorVars[MTR_1].IsRef_A);
#endif  // !(SFRA_ENABLE && SFRA_TEST_MOTOR1)
        }
#if defined(MOTOR1_FWC) && defined(MOTOR1_MTPA)
        else if(counterSpeed[MTR_1] == 1)
        {
            MATH_Vec2 fwcPhasor;

            // get the current angle
            angleCurrentM1_rad =
                    (angleFWC_rad > angleMTPA_rad) ? angleFWC_rad : angleMTPA_rad;

            fwcPhasor.value[0] = __cos(angleCurrentM1_rad);
            fwcPhasor.value[1] = __sin(angleCurrentM1_rad);

            Idq_ref_A[MTR_1].value[0] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[0];
            Idq_ref_A[MTR_1].value[1] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[1];
        }
        else if(counterSpeed[MTR_1] == 2)
        {
            //
            // Compute the output and reference vector voltage
            motorVars[MTR_1] .Vs_V =
                    __sqrt((Vdq_out_V[MTR_1].value[0] * Vdq_out_V[MTR_1].value[0]) +
                           (Vdq_out_V[MTR_1].value[1] * Vdq_out_V[MTR_1].value[1]));

            VsRef_V = VsRef_pu * pfcVars.VdcBus_V;

        }
        else if(counterSpeed[MTR_1] == 3)   // FWC
        {
            if(flagEnableFWCM1 == true)
            {
                float32_t angleFWC;

                PI_run(piHandle_fwc,
                       VsRef_V, motorVars[MTR_1].Vs_V, (float32_t*)&angleFWC);
                angleFWC_rad = MATH_PI_OVER_TWO - angleFWC;
            }
            else
            {
                PI_setUi(piHandle_fwc, 0.0f);
                angleFWC_rad = MATH_PI_OVER_TWO;
            }
        }
        else if(counterSpeed[MTR_1] == 4)   // MTPA
        {
            if(flagEnableMTPAM1 == true)
            {
                angleMTPA_rad = MTPA_computeCurrentAngle(mtpaHandle, motorVars[MTR_1].IsRef_A);
            }
            else
            {
                angleMTPA_rad = MATH_PI_OVER_TWO;
            }
        }
    }
    else
    {
        PI_setUi(piHandle_fwc, 0.0f);
    }
#elif defined(MOTOR1_FWC)
        else if(counterSpeed[MTR_1] == 1)
        {
            MATH_Vec2 fwcPhasor;

            // get the current angle
            angleCurrentM1_rad = angleFWC_rad;

            fwcPhasor.value[0] = __cos(angleCurrentM1_rad);
            fwcPhasor.value[1] = __sin(angleCurrentM1_rad);

            Idq_ref_A[MTR_1].value[0] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[0];
            Idq_ref_A[MTR_1].value[1] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[1];
        }
        else if(counterSpeed[MTR_1] == 2)
        {
            //
            // Compute the output and reference vector voltage
            motorVars[MTR_1] .Vs_V =
                    __sqrt((Vdq_out_V[MTR_1].value[0] * Vdq_out_V[MTR_1].value[0]) +
                           (Vdq_out_V[MTR_1].value[1] * Vdq_out_V[MTR_1].value[1]));

            VsRef_V = VsRef_pu * pfcVars.VdcBus_V;

        }
        else if(counterSpeed[MTR_1] == 3)   // FWC
        {
            if(flagEnableFWCM1 == true)
            {
                float32_t angleFWC;

                PI_run(piHandle_fwc,
                       VsRef_V, motorVars[MTR_1].Vs_V, (float32_t*)&angleFWC);
                angleFWC_rad = MATH_PI_OVER_TWO - angleFWC;
            }
            else
            {
                PI_setUi(piHandle_fwc, 0.0f);
                angleFWC_rad = MATH_PI_OVER_TWO;
            }
        }
    }
    else
    {
        PI_setUi(piHandle_fwc, 0.0f);
    }
#elif defined(MOTOR1_MTPA)
        else if(counterSpeed[MTR_1] == 1)
        {
            MATH_Vec2 fwcPhasor;

            // get the current angle
            angleCurrentM1_rad = angleMTPA_rad;
            fwcPhasor.value[0] = __cos(angleCurrentM1_rad);
            fwcPhasor.value[1] = __sin(angleCurrentM1_rad);

            Idq_ref_A[MTR_1].value[0] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[0];
            Idq_ref_A[MTR_1].value[1] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[1];
        }
        else if(counterSpeed[MTR_1] == 4)   // MTPA
        {
            if(flagEnableMTPAM1 == true)
            {
                angleMTPA_rad = MTPA_computeCurrentAngle(mtpaHandle, motorVars[MTR_1].IsRef_A);
            }
            else
            {
                angleMTPA_rad = MATH_PI_OVER_TWO;
            }
        }
    }
#else   // !MOTOR1_MTPA && !MOTOR1_FWC
    }

    Idq_ref_A[MTR_1].value[1] = motorVars[MTR_1].IsRef_A;
#endif  // !MOTOR1_MTPA && !MOTOR1_FWC/

#if !defined(_SIMPLE_FAST_LIB)
    motorVars[MTR_1].IdqRef_A.value[0] = Idq_ref_A[MTR_1].value[0] + motorVars[MTR_1].IdRated_A;
#else  // _SIMPLE_FAST_LIB
    motorVars[MTR_1].IdqRef_A.value[0] = Idq_ref_A[MTR_1].value[0];
#endif  // _SIMPLE_FAST_LIB

#if defined(MOTOR1_VIBCOMPA)
    // get the Iq reference value plus vibration compensation
    motorVars[MTR_1].IdqRef_A.value[1] = Idq_ref_A[MTR_1].value[1] +
            VIB_COMP_run(vibCompHandle, angleFOCM1_rad, Idq_in_A[MTR_1].value[1]);
#elif defined(MOTOR1_VIBCOMPT)
    // This algorithm reduces speed ripple induced vibration by adjusting Iq depending on compressor angle
    // Compressor angle are in radians and compensation value are defined by vibCompAlpha0, vibCompAlpha120,vibCompAlpha240
    // Fine tune the angle range for compensation depending on compressors torque vs angle profile
    // Note that compressor may not align at 0 Mech degree at startup and hence based on speed ripple, angle may need to be updated
    compressorAngle = VIB_COMP_calcMechangle(vibCompHandle, angleFOCM1_rad);

    // adjust the Iq current for angle lower than 1.57rad by value of vibCompAlpha0
    if(compressorAngle < 1.57f)
    {
        motorVars[MTR_1].IdqRef_A.value[1] =
                Idq_ref_A[MTR_1].value[1] - vibCompAlpha0;
    }
    else if(compressorAngle >= 1.57f && compressorAngle <= 4.2f )
    {
        motorVars[MTR_1].IdqRef_A.value[1] =
                Idq_ref_A[MTR_1].value[1] + ((compressorAngle - 1.57f) * vibCompAlpha120);
    }
    else if(compressorAngle > 4.2f)
    {
        motorVars[MTR_1].IdqRef_A.value[1] =
                Idq_ref_A[MTR_1].value[1] - ((compressorAngle - 4.2f) * vibCompAlpha240);
    }
#else   // !MOTOR1_VIBCOMPA && !MOTOR1_VIBCOMPT
    motorVars[MTR_1].IdqRef_A.value[1] = Idq_ref_A[MTR_1].value[1];
#endif  // !MOTOR1_VIBCOMPA && !MOTOR1_VIBCOMPT

#ifdef MOTOR1_SSIPD
    if(SSIPD_getRunState(ssipdHandle) == true)
    {
        SSIPD_inine_run(ssipdHandle, &(estInputDataM1.Iab_A));

        Vdq_out_V[MTR_1].value[0] = 0.0f;
        Vdq_out_V[MTR_1].value[0] = SSIPD_getVolInject_V(ssipdHandle);
        angleFOCM1_rad = SSIPD_getAngleCmd_rad(ssipdHandle);

        TRAJ_setIntValue(trajHandle_spd[MTR_1], 0.0f);
    }
#endif  // MOTOR1_SSIPD

#endif  // (DMC_BUILDLEVEL > DMC_LEVEL_3)

#if !defined(_SIMPLE_FAST_LIB)
    // update Id reference for Rs OnLine
    EST_updateId_ref_A(estM1Handle, (float32_t *)(&motorVars[MTR_1].IdqRef_A.value[0]));
#endif  // !_SIMPLE_FAST_LIB

    // setup the space vector generator (SVGEN) module
    SVGEN_setup(svgenHandle[MTR_1], estOutputDataM1.oneOverDcBus_invV);

// Only runs FAST
#elif defined(MOTOR1_FAST)    // Only FAST
    MATH_Vec2 phasor;
    bool flagEnableSpeedCtrl = false;
    bool flagEnableCurrentCtrl = false;

#if ((DMC_BUILDLEVEL == DMC_LEVEL_2) || (DMC_BUILDLEVEL == DMC_LEVEL_3))
    ANGLE_GEN_run(angleGenM1Handle, estInputDataM1.speed_ref_Hz);
    angleGenM1_rad = ANGLE_GEN_getAngle(angleGenM1Handle);
#endif  // (DMC_BUILDLEVEL <= DMC_LEVEL_3)

    // run Clarke transform on current
    CLARKE_run(clarkeHandle_I[MTR_1],
               &adcData[MTR_1].I_A, &estInputDataM1.Iab_A);

    // remove offsets
    adcData[MTR_1].V_V.value[0] -=
            adcData[MTR_1].offset_V_sf[0] * pfcVars.VdcBus_V;

    adcData[MTR_1].V_V.value[1] -=
            adcData[MTR_1].offset_V_sf[1] * pfcVars.VdcBus_V;

    adcData[MTR_1].V_V.value[2] -=
            adcData[MTR_1].offset_V_sf[2] * pfcVars.VdcBus_V;
    // run Clarke transform on voltage
    CLARKE_run(clarkeHandle_V[MTR_1],
               &adcData[MTR_1].V_V, &(estInputDataM1.Vab_V));

    // store the input data into a buffer
    estInputDataM1.dcBus_V = pfcVars.VdcBus_V;

    // run the FAST estimator
    EST_run(estM1Handle, &estInputDataM1, &estOutputDataM1);

    // compute angle with delay compensation
    angleDeltaM1_rad = userParams[MTR_1].angleDelayed_sf_sec *
                     estOutputDataM1.fm_lp_rps;

    angleESTM1_rad = MATH_incrAngle(estOutputDataM1.angle_rad, angleDeltaM1_rad);
    speedESTM1_Hz = EST_getFm_lp_Hz(estM1Handle);

#if defined(_SIMPLE_FAST_LIB)
    // Identification
    if( (EST_getState(estM1Handle) == EST_STATE_RS) &&
            (EST_isEnabled(estM1Handle) == true))
#else    // !(_SIMPLE_FAST_LIB)
    // Identification
    if(((EST_isMotorIdentified(estM1Handle) == false) ||
            (EST_getState(estM1Handle) == EST_STATE_RS)) &&
            (EST_isEnabled(estM1Handle) == true))
#endif   // !(_SIMPLE_FAST_LIB)
    {
        Idq_ref_A[MTR_1].value[0] = 0.0f;
        motorVars[MTR_1].motorState = MOTOR_CTRL_RUN;

        // setup the trajectory generator
        EST_setupTrajState(estM1Handle,
                           Idq_ref_A[MTR_1].value[1],
                           motorVars[MTR_1].speedRef_Hz,
                           0.0f);

        // run the trajectories
        EST_runTraj(estM1Handle);

        // store the input data into a buffer
#if !defined(_SIMPLE_FAST_LIB)
        estInputDataM1.speed_ref_Hz = EST_getIntValue_spd_Hz(estM1Handle);
#endif   // !(_SIMPLE_FAST_LIB)

        flagEnableSpeedCtrl = EST_doSpeedCtrl(estM1Handle);
        flagEnableCurrentCtrl = EST_doCurrentCtrl(estM1Handle);

        motorVars[MTR_1].IdRated_A = EST_getIntValue_Id_A(estM1Handle);

        angleFOCM1_rad = estOutputDataM1.angle_rad;
    }
    else if(motorVars[MTR_1].flagMotorIdentified == true)
    {
        if(motorVars[MTR_1].flagRunIdentAndOnLine == true)
        {
            counterTrajSpeed[MTR_1]++;

            if(counterTrajSpeed[MTR_1] >= userParams[MTR_1].numIsrTicksPerTrajTick)
            {
                // clear counter
                counterTrajSpeed[MTR_1] = 0;

                // run a trajectory for speed reference,
                // so the reference changes with a ramp instead of a step
                TRAJ_run(trajHandle_spd[MTR_1]);
            }

            flagEnableSpeedCtrl = motorVars[MTR_1].flagEnableSpeedCtrl;
            flagEnableCurrentCtrl = true;
        }

        estInputDataM1.speed_ref_Hz = TRAJ_getIntValue(trajHandle_spd[MTR_1]);

#if !defined(_SIMPLE_FAST_LIB)
        // get Id reference for Rs OnLine
        motorVars[MTR_1].IdRated_A = EST_getIdRated_A(estM1Handle);
#else
        motorVars[MTR_1].IdRated_A = 0.0f;
#endif   // !(_SIMPLE_FAST_LIB)

        angleFOCM1_rad = angleESTM1_rad;
    }

    motorVars[MTR_1].speed_Hz = speedESTM1_Hz;

    motorVars[MTR_1].stateRunTimeCnt++;

    if(motorVars[MTR_1].motorState >= MOTOR_CL_RUNNING)
    {
        angleFOCM1_rad = angleESTM1_rad;
    }
    else if(motorVars[MTR_1].motorState == MOTOR_OL_START)
    {
        angleFOCM1_rad = angleESTM1_rad;

        if(fabsf(estInputDataM1.speed_ref_Hz) >= fabsf(motorVars[MTR_1].speedForce_Hz))
        {
            motorVars[MTR_1].motorState = MOTOR_CL_RUNNING;
        }
    }
    else if(motorVars[MTR_1].motorState == MOTOR_ALIGNMENT)
    {
        angleFOCM1_rad = 0.0f;
        flagEnableSpeedCtrl = false;

        motorVars[MTR_1].IsRef_A = 0.0f;
        Idq_ref_A[MTR_1].value[0] = motorVars[MTR_1].alignCurrent_A;
        Idq_ref_A[MTR_1].value[1] = 0.0f;

        TRAJ_setIntValue(trajHandle_spd[MTR_1], 0.0f);

        if(motorVars[MTR_1].stateRunTimeCnt > motorVars[MTR_1].lockRotorTimeDelay)
        {
            motorVars[MTR_1].stateRunTimeCnt = 0;
            motorVars[MTR_1].motorState = MOTOR_OL_START;
            EST_setAngle_rad(estM1Handle, angleFOCM1_rad);
            PI_setUi(piHandle_spd[MTR_1], 0.0);
        }
    }

    // compute the sin/cos phasor
    phasor.value[0] = __cos(angleFOCM1_rad);
    phasor.value[1] = __sin(angleFOCM1_rad);

    // set the phasor in the Park transform
    PARK_setPhasor(parkHandle_I[MTR_1], &phasor);

    // run the Park transform
    PARK_run(parkHandle_I[MTR_1], &(estInputDataM1.Iab_A),
             (MATH_vec2 *)&(Idq_in_A[MTR_1]));

#if(DMC_BUILDLEVEL >= DMC_LEVEL_4)
    // run the speed controller
    if(flagEnableSpeedCtrl == true)
    {
        counterSpeed[MTR_1]++;

        if(counterSpeed[MTR_1] >= userParams[MTR_1].numCtrlTicksPerSpeedTick)
        {
            counterSpeed[MTR_1] = 0;

#if defined(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_MOTOR1)
            PI_run(piHandle_spd[MTR_1],
                   estInputDataM1.speed_ref_Hz + sfraNoiseSpd,
                   motorVars[MTR_1].speed_Hz,
                   (float32_t *)&motorVars[MTR_1].IsRef_A);
#else  // !(SFRA_ENABLE && SFRA_TEST_MOTOR1)
            PI_run(piHandle_spd[MTR_1],
                   estInputDataM1.speed_ref_Hz,
                   motorVars[MTR_1].speed_Hz,
                   (float32_t *)&motorVars[MTR_1].IsRef_A);
#endif  // !(SFRA_ENABLE && SFRA_TEST_MOTOR1)

        }
#if defined(MOTOR1_FWC) && defined(MOTOR1_MTPA)
        else if(counterSpeed[MTR_1] == 1)
        {
            MATH_Vec2 fwcPhasor;

            // get the current angle
            angleCurrentM1_rad =
                    (angleFWC_rad > angleMTPA_rad) ? angleFWC_rad : angleMTPA_rad;

            fwcPhasor.value[0] = __cos(angleCurrentM1_rad);
            fwcPhasor.value[1] = __sin(angleCurrentM1_rad);

            if((flagEnableFWCM1 == true) || (flagEnableMTPAM1 == true))
            {
                Idq_ref_A[MTR_1].value[0] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[0];
            }

            Idq_ref_A[MTR_1].value[1] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[1];
        }
        else if(counterSpeed[MTR_1] == 2)
        {
            //
            // Compute the output and reference vector voltage
            motorVars[MTR_1] .Vs_V =
                    __sqrt((Vdq_out_V[MTR_1].value[0] * Vdq_out_V[MTR_1].value[0]) +
                           (Vdq_out_V[MTR_1].value[1] * Vdq_out_V[MTR_1].value[1]));

            VsRef_V = VsRef_pu * pfcVars.VdcBus_V;

        }
        else if(counterSpeed[MTR_1] == 3)   // FWC
        {
            if(flagEnableFWCM1 == true)
            {
                float32_t angleFWC;

                PI_run(piHandle_fwc,
                       VsRef_V, motorVars[MTR_1].Vs_V, (float32_t*)&angleFWC);
                angleFWC_rad = MATH_PI_OVER_TWO - angleFWC;
            }
            else
            {
                PI_setUi(piHandle_fwc, 0.0f);
                angleFWC_rad = MATH_PI_OVER_TWO;
            }
        }
        else if(counterSpeed[MTR_1] == 4)   // MTPA
        {
            if(flagEnableMTPAM1 == true)
            {
                angleMTPA_rad = MTPA_computeCurrentAngle(mtpaHandle, motorVars[MTR_1].IsRef_A);
            }
            else
            {
                angleMTPA_rad = MATH_PI_OVER_TWO;
            }
        }
    }
    else
    {
        PI_setUi(piHandle_fwc, 0.0f);
    }
#elif defined(MOTOR1_FWC)
        else if(counterSpeed[MTR_1] == 1)
        {
            MATH_Vec2 fwcPhasor;

            // get the current angle
            angleCurrentM1_rad = angleFWC_rad;

            fwcPhasor.value[0] = __cos(angleCurrentM1_rad);
            fwcPhasor.value[1] = __sin(angleCurrentM1_rad);

            if(flagEnableFWCM1 == true)
            {
                Idq_ref_A[MTR_1].value[0] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[0];
            }

            Idq_ref_A[MTR_1].value[1] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[1];
        }
        else if(counterSpeed[MTR_1] == 2)
        {
            //
            // Compute the output and reference vector voltage
            motorVars[MTR_1] .Vs_V =
                    __sqrt((Vdq_out_V[MTR_1].value[0] * Vdq_out_V[MTR_1].value[0]) +
                           (Vdq_out_V[MTR_1].value[1] * Vdq_out_V[MTR_1].value[1]));

            VsRef_V = VsRef_pu * pfcVars.VdcBus_V;

        }
        else if(counterSpeed[MTR_1] == 3)   // FWC
        {
            if(flagEnableFWCM1 == true)
            {
                float32_t angleFWC;

                PI_run(piHandle_fwc,
                       VsRef_V, motorVars[MTR_1].Vs_V, (float32_t*)&angleFWC);
                angleFWC_rad = MATH_PI_OVER_TWO - angleFWC;
            }
            else
            {
                PI_setUi(piHandle_fwc, 0.0f);
                angleFWC_rad = MATH_PI_OVER_TWO;
            }
        }
    }
    else
    {
        PI_setUi(piHandle_fwc, 0.0f);
    }
#elif defined(MOTOR1_MTPA)
        else if(counterSpeed[MTR_1] == 1)
        {
            MATH_Vec2 fwcPhasor;

            // get the current angle
            angleCurrentM1_rad = angleMTPA_rad;

            fwcPhasor.value[0] = __cos(angleCurrentM1_rad);
            fwcPhasor.value[1] = __sin(angleCurrentM1_rad);

            if(flagEnableMTPAM1 == true)
            {
                Idq_ref_A[MTR_1].value[0] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[0];
            }

            Idq_ref_A[MTR_1].value[1] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[1];
        }
        else if(counterSpeed[MTR_1] == 4)   // MTPA
        {
            if(flagEnableMTPAM1 == true)
            {
                angleMTPA_rad = MTPA_computeCurrentAngle(mtpaHandle, motorVars[MTR_1].IsRef_A);
            }
            else
            {
                angleMTPA_rad = MATH_PI_OVER_TWO;
            }
        }
    }
#else   // !MOTOR1_MTPA && !MOTOR1_FWC
    }

    Idq_ref_A[MTR_1].value[1] = motorVars[MTR_1].IsRef_A;
#endif  // !MOTOR1_MTPA && !MOTOR1_FWC/

    motorVars[MTR_1].IdqRef_A.value[0] = Idq_ref_A[MTR_1].value[0] +
                                         motorVars[MTR_1].IdRated_A;

#if defined(MOTOR1_VIBCOMPA)
    // get the Iq reference value plus vibration compensation
    motorVars[MTR_1].IdqRef_A.value[1] = Idq_ref_A[MTR_1].value[1] +
            VIB_COMP_run(vibCompHandle, angleFOCM1_rad, Idq_in_A[MTR_1].value[1]);

#elif defined(MOTOR1_VIBCOMPT)
    // This algorithm reduces speed ripple induced vibration by adjusting Iq depending on compressor angle
    // Compressor angle are in radians and compensation value are defined by vibCompAlpha0, vibCompAlpha120,vibCompAlpha240
    // Fine tune the angle range for compensation depending on compressors torque vs angle profile
    // Note that compressor may not align at 0 Mech degree at startup and hence based on speed ripple, angle may need to be updated
    compressorAngle = VIB_COMP_calcMechangle(vibCompHandle, angleFOCM1_rad);

    // adjust the Iq current for angle lower than 1.57rad by value of vibCompAlpha0
    if(compressorAngle < 1.57f)
    {
        motorVars[MTR_1].IdqRef_A.value[1] =
                Idq_ref_A[MTR_1].value[1] - vibCompAlpha0;
    }
    else if(compressorAngle >= 1.57f && compressorAngle <= 4.2f )
    {
        motorVars[MTR_1].IdqRef_A.value[1] =
                Idq_ref_A[MTR_1].value[1] + ((compressorAngle - 1.57f) * vibCompAlpha120);
    }
    else if(compressorAngle > 4.2f)
    {
        motorVars[MTR_1].IdqRef_A.value[1] =
                Idq_ref_A[MTR_1].value[1] - ((compressorAngle - 4.2f) * vibCompAlpha240);
    }
#else   // !MOTOR1_VIBCOMPA && !MOTOR1_VIBCOMPT
    motorVars[MTR_1].IdqRef_A.value[1] = Idq_ref_A[MTR_1].value[1];
#endif  // !MOTOR1_VIBCOMPA && !MOTOR1_VIBCOMPT

#ifdef MOTOR1_SSIPD
    if(SSIPD_getRunState(ssipdHandle) == true)
    {
        SSIPD_inine_run(ssipdHandle, &(estInputDataM1.Iab_A));

        Vdq_out_V[MTR_1].value[0] = 0.0f;
        Vdq_out_V[MTR_1].value[0] = SSIPD_getVolInject_V(ssipdHandle);
        angleFOCM1_rad = SSIPD_getAngleCmd_rad(ssipdHandle);

        TRAJ_setIntValue(trajHandle_spd[MTR_1], 0.0f);
    }
#endif  // MOTOR1_SSIPD
#endif  // (DMC_BUILDLEVEL > DMC_LEVEL_3)

#if !defined(_SIMPLE_FAST_LIB)
    // update Id reference for Rs OnLine
    EST_updateId_ref_A(estM1Handle, (float32_t *)(&motorVars[MTR_1].IdqRef_A.value[0]));
#endif  // !_SIMPLE_FAST_LIB

    // setup the space vector generator (SVGEN) module
    SVGEN_setup(svgenHandle[MTR_1], estOutputDataM1.oneOverDcBus_invV);

// Only runs eSMO
#elif defined(MOTOR1_ESMO)
    MATH_Vec2 Iab_A;
    MATH_Vec2 phasor;

    bool flagEnableSpeedCtrl = false;
    bool flagEnableCurrentCtrl = false;

    ANGLE_GEN_run(angleGenM1Handle, speedRefM1_Hz);
    angleGenM1_rad = ANGLE_GEN_getAngle(angleGenM1Handle);

    // run Clarke transform on current
    CLARKE_run(clarkeHandle_I[MTR_1],
               &adcData[MTR_1].I_A, &Iab_A);

    // store the input data into a buffer
    float32_t oneOverDcBus_invV = 1.0f / pfcVars.VdcBus_V;

    ESMO_setSpeedRef(esmoM1Handle, speedRefM1_Hz);
    ESMO_inline_run(esmoM1Handle, pfcVars.VdcBus_V,
                    &(pwmData[MTR_1].Vabc_pu), &Iab_A);

    angleCompM1_rad = speedPLLM1_Hz * motorVars[MTR_1].angleDelayed_sf;
    anglePLLM1_rad = MATH_incrAngle(ESMO_getAnglePLL(esmoM1Handle), angleCompM1_rad);
    angleSMOM1_rad = ESMO_getAngleElec(esmoM1Handle);

    SPDFR_run(spdfrM1Handle, anglePLLM1_rad);
    speedPLLM1_Hz = SPDFR_getSpeedHz(spdfrM1Handle);

    if(motorVars[MTR_1].flagRunIdentAndOnLine == true)
    {
        counterTrajSpeed[MTR_1]++;

        if(counterTrajSpeed[MTR_1] >= userParams[MTR_1].numIsrTicksPerTrajTick)
        {
            // clear counter
            counterTrajSpeed[MTR_1] = 0;

            // run a trajectory for speed reference,
            // so the reference changes with a ramp instead of a step
            TRAJ_run(trajHandle_spd[MTR_1]);
        }

        flagEnableSpeedCtrl = motorVars[MTR_1].flagEnableSpeedCtrl;
        flagEnableCurrentCtrl = true;
    }

    speedRefM1_Hz = TRAJ_getIntValue(trajHandle_spd[MTR_1]);
    motorVars[MTR_1].speed_Hz = speedPLLM1_Hz;

    motorVars[MTR_1].stateRunTimeCnt++;

#if(DMC_BUILDLEVEL > DMC_LEVEL_3)
    if(motorVars[MTR_1].motorState >= MOTOR_CL_RUNNING)
    {
        angleFOCM1_rad = anglePLLM1_rad;

        ESMO_updateKslide(esmoM1Handle);
    }
    else if(motorVars[MTR_1].motorState == MOTOR_OL_START)
    {
        angleFOCM1_rad = angleGenM1_rad;
        flagEnableSpeedCtrl = false;

        motorVars[MTR_1].IsRef_A = motorVars[MTR_1].startCurrent_A;
        Idq_ref_A[MTR_1].value[0] = 0.0f;
        Idq_ref_A[MTR_1].value[1] = motorVars[MTR_1].startCurrent_A;

        if(speedRefM1_Hz >= motorVars[MTR_1].speedForce_Hz)
        {
            TRAJ_setIntValue(trajHandle_spd[MTR_1], speedRefM1_Hz);

            if(motorVars[MTR_1].stateRunTimeCnt > motorVars[MTR_1].forceRunTimeDelay)
            {
                motorVars[MTR_1].motorState = MOTOR_CL_RUNNING;

                ESMO_setAnglePu(esmoM1Handle, angleFOCM1_rad);

                PI_setUi(piHandle_spd[MTR_1], (frswPosM1_sf * Idq_ref_A[MTR_1].value[1]));

                flagEnableSpeedCtrl = true;
            }
        }
    }
    else if(motorVars[MTR_1].motorState == MOTOR_ALIGNMENT)
    {
        angleFOCM1_rad = 0.0f;
        flagEnableSpeedCtrl = false;

        motorVars[MTR_1].IsRef_A = 0.0f;
        Idq_ref_A[MTR_1].value[0] = motorVars[MTR_1].startCurrent_A;
        Idq_ref_A[MTR_1].value[1] = 0.0f;

        if(motorVars[MTR_1].stateRunTimeCnt > motorVars[MTR_1].lockRotorTimeDelay)
        {
            motorVars[MTR_1].motorState = MOTOR_OL_START;

            ESMO_setAnglePu(esmoM1Handle, angleFOCM1_rad);
            ANGLE_GEN_setAngle(angleGenM1Handle, angleFOCM1_rad);
        }
    }

    // compute the sin/cos phasor
    phasor.value[0] = __cos(angleFOCM1_rad);
    phasor.value[1] = __sin(angleFOCM1_rad);

    // set the phasor in the Park transform
    PARK_setPhasor(parkHandle_I[MTR_1], &phasor);

    // run the Park transform
    PARK_run(parkHandle_I[MTR_1], &Iab_A, &Idq_in_A[MTR_1]);

    // run the speed controller
    if(flagEnableSpeedCtrl == true)
    {
        counterSpeed[MTR_1]++;

        if(counterSpeed[MTR_1] >= userParams[MTR_1].numCtrlTicksPerSpeedTick)
        {
            counterSpeed[MTR_1] = 0;

            PI_run(piHandle_spd[MTR_1],
                          speedRefM1_Hz,
                          motorVars[MTR_1].speed_Hz,
                          (float32_t *)&motorVars[MTR_1].IsRef_A);
        }
#if defined(MOTOR1_FWC) && defined(MOTOR1_MTPA)
        else if(counterSpeed[MTR_1] == 1)
        {
            MATH_Vec2 fwcPhasor;

            // get the current angle
            angleCurrentM1_rad =
                    (angleFWC_rad > angleMTPA_rad) ? angleFWC_rad : angleMTPA_rad;

            fwcPhasor.value[0] = __cos(angleCurrentM1_rad);
            fwcPhasor.value[1] = __sin(angleCurrentM1_rad);

            Idq_ref_A[MTR_1].value[0] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[0];
            Idq_ref_A[MTR_1].value[1] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[1];
        }
        else if(counterSpeed[MTR_1] == 2)
        {
            // Compute the output and reference vector voltage
            motorVars[MTR_1] .Vs_V =
                    __sqrt((Vdq_out_V[MTR_1].value[0] * Vdq_out_V[MTR_1].value[0]) +
                           (Vdq_out_V[MTR_1].value[1] * Vdq_out_V[MTR_1].value[1]));

            VsRef_V = VsRef_pu * pfcVars.VdcBus_V;

        }
        else if(counterSpeed[MTR_1] == 3)   // FWC
        {
            if(flagEnableFWCM1 == true)
            {
                float32_t angleFWC;

                PI_run(piHandle_fwc,
                       VsRef_V, motorVars[MTR_1].Vs_V, (float32_t*)&angleFWC);
                angleFWC_rad = MATH_PI_OVER_TWO - angleFWC;
            }
            else
            {
                PI_setUi(piHandle_fwc, 0.0f);
                angleFWC_rad = MATH_PI_OVER_TWO;
            }
        }
        else if(counterSpeed[MTR_1] == 4)   // MTPA
        {
            if(flagEnableMTPAM1 == true)
            {
                angleMTPA_rad = MTPA_computeCurrentAngle(mtpaHandle,
                                                         motorVars[MTR_1].IsRef_A);
            }
            else
            {
                angleMTPA_rad = MATH_PI_OVER_TWO;
            }
        }
    }
    else
    {
        PI_setUi(piHandle_fwc, 0.0f);
    }
#elif defined(MOTOR1_FWC)
        else if(counterSpeed[MTR_1] == 1)
        {
            MATH_Vec2 fwcPhasor;

            // get the current angle
            angleCurrentM1_rad = angleFWC_rad;

            fwcPhasor.value[0] = __cos(angleCurrentM1_rad);
            fwcPhasor.value[1] = __sin(angleCurrentM1_rad);

            Idq_ref_A[MTR_1].value[0] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[0];
            Idq_ref_A[MTR_1].value[1] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[1];
        }
        else if(counterSpeed[MTR_1] == 2)
        {
            //
            // Compute the output and reference vector voltage
            motorVars[MTR_1] .Vs_V =
                    __sqrt((Vdq_out_V[MTR_1].value[0] * Vdq_out_V[MTR_1].value[0]) +
                           (Vdq_out_V[MTR_1].value[1] * Vdq_out_V[MTR_1].value[1]));

            VsRef_V = VsRef_pu * pfcVars.VdcBus_V;

        }
        else if(counterSpeed[MTR_1] == 3)   // FWC
        {
            if(flagEnableFWCM1 == true)
            {
                float32_t angleFWC;

                PI_run(piHandle_fwc,
                       VsRef_V, motorVars[MTR_1].Vs_V, (float32_t*)&angleFWC);

                angleFWC_rad = MATH_PI_OVER_TWO - angleFWC;
            }
            else
            {
                PI_setUi(piHandle_fwc, 0.0f);
                angleFWC_rad = MATH_PI_OVER_TWO;
            }
        }
    }
    else
    {
        PI_setUi(piHandle_fwc, 0.0f);
    }
#elif defined(MOTOR1_MTPA)
        else if(counterSpeed[MTR_1] == 1)
        {
            MATH_Vec2 fwcPhasor;

            // get the current angle
            angleCurrentM1_rad = angleMTPA_rad;
            fwcPhasor.value[0] = __cos(angleCurrentM1_rad);
            fwcPhasor.value[1] = __sin(angleCurrentM1_rad);

            Idq_ref_A[MTR_1].value[0] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[0];
            Idq_ref_A[MTR_1].value[1] = motorVars[MTR_1].IsRef_A * fwcPhasor.value[1];
        }
        else if(counterSpeed[MTR_1] == 4)   // MTPA
        {
            if(flagEnableMTPAM1 == true)
            {
                angleMTPA_rad = MTPA_computeCurrentAngle(mtpaHandle, motorVars[MTR_1].IsRef_A);
            }
            else
            {
                angleMTPA_rad = MATH_PI_OVER_TWO;
            }
        }
    }
#else   // !MOTOR1_MTPA && !MOTOR1_FWC
    }

    Idq_ref_A[MTR_1].value[1] = motorVars[MTR_1].IsRef_A;
#endif  // !MOTOR1_MTPA && !MOTOR1_FWC/

    motorVars[MTR_1].IdqRef_A.value[0] = Idq_ref_A[MTR_1].value[0];

#if defined(MOTOR1_VIBCOMPA)
    if(motorVars[MTR_1].motorState == MOTOR_CTRL_RUN)
    {
        // get the Iq reference value plus vibration compensation
        motorVars[MTR_1].IdqRef_A.value[1] = Idq_ref_A[MTR_1].value[1] +
                VIB_COMP_run(vibCompHandle, angleFOCM1_rad, Idq_in_A[MTR_1].value[1]);
    }
    else
    {
        motorVars[MTR_1].IdqRef_A.value[1] = Idq_ref_A[MTR_1].value[1];
    }
#elif defined(MOTOR1_VIBCOMPT)
    if(motorVars[MTR_1].motorState == MOTOR_CTRL_RUN)
    {
        compressorAngle = VIB_COMP_calcMechangle(vibCompHandle, angleFOCM1_rad);

        if (compressorAngle < 1.57f)
        {
            motorVars[MTR_1].IdqRef_A.value[1] =
                    Idq_ref_A[MTR_1].value[1] - vibCompAlpha0;
        }
        else if (compressorAngle >= 1.57f && compressorAngle <= 4.2f)
        {
            motorVars[MTR_1].IdqRef_A.value[1] =
                    Idq_ref_A[MTR_1].value[1] + ((compressorAngle - 1.57f) * vibCompAlpha120);
        }
        else if (compressorAngle > 4.2f)
        {
            motorVars[MTR_1].IdqRef_A.value[1] =
                    Idq_ref_A[MTR_1].value[1] - ((compressorAngle - 4.2f) * vibCompAlpha240);
        }
    }
    else
    {
        motorVars[MTR_1].IdqRef_A.value[1] = Idq_ref_A[MTR_1].value[1];
    }
#else   // !MOTOR1_VIBCOMPA && !MOTOR1_VIBCOMPT
    motorVars[MTR_1].IdqRef_A.value[1] = Idq_ref_A[MTR_1].value[1];
#endif  // !MOTOR1_VIBCOMPA && !MOTOR1_VIBCOMPT

#ifdef MOTOR1_SSIPD
    if(SSIPD_getRunState(ssipdHandle) == true)
    {
        SSIPD_inine_run(ssipdHandle, &Iab_A);

        Vdq_out_V[MTR_1].value[0] = 0.0f;
        Vdq_out_V[MTR_1].value[0] = SSIPD_getVolInject_V(ssipdHandle);
        angleFOCM1_rad = SSIPD_getAngleCmd_rad(ssipdHandle);

        TRAJ_setIntValue(trajHandle_spd[MTR_1], 0.0f);
    }
#endif  // MOTOR1_SSIPD
#else   // (DMC_BUILDLEVEL <= DMC_LEVEL_3)
    if(motorVars[MTR_1].motorState == MOTOR_ALIGNMENT)
    {
        angleFOCM1_rad = 0.0f;
        flagEnableSpeedCtrl = false;

        motorVars[MTR_1].IsRef_A = 0.0f;
        Idq_ref_A[MTR_1].value[0] = motorVars[MTR_1].startCurrent_A;
        Idq_ref_A[MTR_1].value[1] = 0.0f;

        if(motorVars[MTR_1].stateRunTimeCnt > motorVars[MTR_1].lockRotorTimeDelay)
        {
            motorVars[MTR_1].motorState = MOTOR_CL_RUNNING;

            ESMO_setAnglePu(esmoM1Handle, angleFOCM1_rad);
            ANGLE_GEN_setAngle(angleGenM1Handle, angleFOCM1_rad);
        }
    }
#endif  // (DMC_BUILDLEVEL > DMC_LEVEL_3)

    // setup the space vector generator (SVGEN) module
    SVGEN_setup(svgenHandle[MTR_1], oneOverDcBus_invV);
#else   // !MOTOR1_ESMO && !MOTOR1_FAST
#error No select a right estimator for motor_1 control
#endif  // MOTOR1_ESMO || MOTOR1_FAST

#if !defined(MOTOR1_DISABLE)
//---------- Common Open loop for FAST or eSMO ---------------------------------
#if((DMC_BUILDLEVEL == DMC_LEVEL_2) || (DMC_BUILDLEVEL == DMC_LEVEL_3))
    angleFOCM1_rad = angleGenM1_rad;

    motorVars[MTR_1].IdqRef_A.value[0] = Idq_set_A[MTR_1].value[0];
    motorVars[MTR_1].IdqRef_A.value[1] = Idq_set_A[MTR_1].value[1];

    // compute the sin/cos phasor
    phasor.value[0] = __cos(angleFOCM1_rad);
    phasor.value[1] = __sin(angleFOCM1_rad);

    // set the phasor in the Park transform
    PARK_setPhasor(parkHandle_I[MTR_1], &phasor);

    // run the Park transform
#if defined(MOTOR1_FAST)
    PARK_run(parkHandle_I[MTR_1], &estInputDataM1.Iab_A, &Idq_in_A[MTR_1]);
#elif defined(MOTOR1_ESMO)
    PARK_run(parkHandle_I[MTR_1], &Iab_A, &Idq_in_A[MTR_1]);
#elif defined(MOTOR1_DISABLE)
    // This motor is disable
#else   // !MOTOR1_ESMO && !MOTOR1_FAST
#error No select a right estimator for motor_1 control
#endif  // MOTOR1_FAST || MOTOR1_ESMO
#endif // (DMC_BUILDLEVEL == DMC_LEVEL_2) | (DMC_BUILDLEVEL == DMC_LEVEL_3)

#if(DMC_BUILDLEVEL != DMC_LEVEL_4)
    if(flagEnableSpeedCtrl == true);    // Meanless, just for no warning
#endif  // !DMC_LEVEL_4

//---------- Common Current Loop for both FAST and eSMO ------------------------------
    if(flagEnableCurrentCtrl == true)
    {
        float32_t outMax_V;

        // Maximum voltage output
        userParams[MTR_1].maxVsMag_V =
                userParams[MTR_1].maxVsMag_pu * pfcVars.VdcBus_V;

        PI_setMinMax(piHandle_Id[MTR_1],
                     -userParams[MTR_1].maxVsMag_V, userParams[MTR_1].maxVsMag_V);

#if defined(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_MOTOR1)
        // run the Id controller
        PI_run(piHandle_Id[MTR_1],
               motorVars[MTR_1].IdqRef_A.value[0] + sfraNoiseId,
               Idq_in_A[MTR_1].value[0],
               &Vdq_out_V[MTR_1].value[0]);
#else  // !(SFRA_ENABLE && SFRA_TEST_MOTOR1)
        // run the Id controller
        PI_run(piHandle_Id[MTR_1],
               motorVars[MTR_1].IdqRef_A.value[0],
               Idq_in_A[MTR_1].value[0],
               &Vdq_out_V[MTR_1].value[0]);
#endif  // !(SFRA_ENABLE && SFRA_TEST_MOTOR1)

        // calculate Iq controller limits, and run Iq controller using fast RTS
        // function, callable assembly
        outMax_V = __sqrt((userParams[MTR_1].maxVsMag_V * userParams[MTR_1].maxVsMag_V) -
                          (Vdq_out_V[MTR_1].value[0] * Vdq_out_V[MTR_1].value[0]));

        PI_setMinMax(piHandle_Iq[MTR_1], -outMax_V, outMax_V);

#if defined(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_MOTOR1)
        PI_run(piHandle_Iq[MTR_1],
               motorVars[MTR_1].IdqRef_A.value[1] + sfraNoiseIq,
               Idq_in_A[MTR_1].value[1],
               &Vdq_out_V[MTR_1].value[1]);
#else  // !(SFRA_ENABLE && SFRA_TEST_MOTOR1)
        PI_run(piHandle_Iq[MTR_1],
               motorVars[MTR_1].IdqRef_A.value[1],
               Idq_in_A[MTR_1].value[1],
               &Vdq_out_V[MTR_1].value[1]);
#endif  // SFRA_ENABLE && SFRA_TEST_MOTOR1
    }   // flagEnableCurrentCtrl == true

//---------- v/f Open loop for FAST or eSMO ---------------------------------
#if(DMC_BUILDLEVEL == DMC_LEVEL_2)
#if defined(MOTOR1_FAST)
    VS_FREQ_run(VsFreqHandle[MTR_1], estInputDataM1.speed_ref_Hz);
#elif defined(MOTOR1_ESMO)
    VS_FREQ_run(VsFreqHandle[MTR_1], speedRefM1_Hz);
#else
#error No select a right estimator for motor_1 control
#endif  // MOTOR1_FAST || MOTOR1_ESMO

    Vdq_out_V[MTR_1].value[0] = VsFreq[MTR_1].Vdq_out.value[0];
    Vdq_out_V[MTR_1].value[1] = VsFreq[MTR_1].Vdq_out.value[1];
#endif // (DMC_BUILDLEVEL == DMC_LEVEL_2)

    // set the phasor in the inverse Park transform
    IPARK_setPhasor(iparkHandle_V[MTR_1], &phasor);

    // run the inverse Park module
    IPARK_run(iparkHandle_V[MTR_1],
              &Vdq_out_V[MTR_1], &Vab_out_V[MTR_1]);

    // run the space vector generator (SVGEN) module
    SVGEN_run(svgenHandle[MTR_1], &Vab_out_V[MTR_1], &(pwmData[MTR_1].Vabc_pu));

    // write the PWM compare values
    if(HAL_getPwmEnableStatus(halMtrHandle[MTR_1]) == false)
    {
        // clear PWM data
        pwmData[MTR_1].Vabc_pu.value[0] = 0.0;
        pwmData[MTR_1].Vabc_pu.value[1] = 0.0;
        pwmData[MTR_1].Vabc_pu.value[2] = 0.0;
    }

#if(DMC_BUILDLEVEL == DMC_LEVEL_1)
    // output 50%
    pwmData[MTR_1].Vabc_pu.value[0] = 0.0;
    pwmData[MTR_1].Vabc_pu.value[1] = 0.0;
    pwmData[MTR_1].Vabc_pu.value[2] = 0.0;
#endif  // (DMC_BUILDLEVEL == DMC_LEVEL_1)
#else   // MOTOR1_DISABLE
    // output 50%
    pwmData[MTR_1].Vabc_pu.value[0] = 0.0;
    pwmData[MTR_1].Vabc_pu.value[1] = 0.0;
    pwmData[MTR_1].Vabc_pu.value[2] = 0.0;
#endif  // MOTOR1_DISABLE

#ifdef MOTOR1_DCLINKSS
    // write the PWM compare values
    HAL_writePWMData(halMtrHandle[MTR_1], &pwmData[MTR_1]);

    // revise PWM compare(CMPA/B) values for shifting switching pattern
    // and, update SOC trigger point
    HAL_runSingleShuntCompensation(halMtrHandle[MTR_1], dclinkM1Handle,
                         &Vab_out_V[MTR_1], &pwmData[MTR_1], pfcVars.VdcBus_V);
#else   // !MOTOR1_DCLINKSS
    // write the PWM compare values
    HAL_writePWMData(halMtrHandle[MTR_1], &pwmData[MTR_1]);
#endif  // !MOTOR1_DCLINKSS

#if defined(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_MOTOR1)
    if(sfraTestLoop == SFRA_TEST_MOTOR1_SPEED)
    {
        sfraNoiseOut = motorVars[MTR_1].IsRef_A * (1.0f / USER_M1_ADC_FULL_SCALE_CURRENT_A);
        sfraNoiseFdb = motorVars[MTR_1].speed_Hz * (1.0f / USER_MOTOR1_FREQ_MAX_Hz);
    }
    else if(sfraTestLoop == SFRA_TEST_MOTOR1_ID)
    {
        sfraNoiseOut = Vdq_out_V[MTR_1].value[0] * (1.0f / USER_M1_ADC_FULL_SCALE_VOLTAGE_V);
        sfraNoiseFdb = Idq_in_A[MTR_1].value[0] * (1.0f / USER_M1_ADC_FULL_SCALE_CURRENT_A);
    }
    else if(sfraTestLoop == SFRA_TEST_MOTOR1_IQ)
    {
        sfraNoiseOut = Vdq_out_V[MTR_1].value[1] * (1.0f / USER_M1_ADC_FULL_SCALE_VOLTAGE_V);
        sfraNoiseFdb = Idq_in_A[MTR_1].value[1] * (1.0f / USER_M1_ADC_FULL_SCALE_CURRENT_A);
    }

    if(sfraCollectStart == true)
    {
        // Collect noise feedback from loop
        SFRA_COLLECT((float32_t*)&sfraNoiseOut, (float32_t*)&sfraNoiseFdb);
    }

    sfraCollectStart = true;       // enable SFRA data collection
#endif  // SFRA_ENABLE && SFRA_TEST_MOTOR1

    // Collect current and voltage data to calculate the RMS value
    collectRMSData(MTR_1);

#ifdef NEST_INT_ENABLE
    HAL_resetMtr1NestInterrupt();
#endif  // NEST_INT_ENABLE

    // The EPWMDAC only works on HVMTRPFC_REV1P1
#if defined(EPWMDAC_MODE) && defined(PWMDAC_MOTOR)
    // connect inputs of the PWMDAC module.
    HAL_writePWMDACData(halHandle, &pwmDACData);
#endif  // EPWMDAC_MODE

#if defined(DAC128S_ENABLE) && !defined(DAC_FASTUPDATE)
#if defined(DAC80504_SEL)
    DAC80504_writeData(dac128sHandle);
#else   // DAC128S805
    DAC128S_writeData(dac128sHandle);
#endif  // DAC128S805
#endif  // DAC128S_ENABLE

#ifdef DEBUG_MONITOR_EN
    if(motorVars[MTR_1].flagClearRecord == 1)
    {
        motorVars[MTR_1].speedMax_Hz = 0.0f;
        motorVars[MTR_1].speedMin_Hz = 1000.0f;
        motorVars[MTR_1].flagClearRecord = 0;
    }
    else
    {
        if(motorVars[MTR_1].speed_Hz > motorVars[MTR_1].speedMax_Hz)
        {
            motorVars[MTR_1].speedMax_Hz = motorVars[MTR_1].speed_Hz;
        }

        if(motorVars[MTR_1].speed_Hz < motorVars[MTR_1].speedMin_Hz)
        {
            motorVars[MTR_1].speedMin_Hz = motorVars[MTR_1].speed_Hz;
        }

        motorVars[MTR_1].speedDelta_Hz = motorVars[MTR_1].speedMax_Hz -
                motorVars[MTR_1].speedMin_Hz;
    }
#endif  // DEBUG_MONITOR_EN

    return;
} // end of motor1CtrlISR() function

void runMotor1Control(void)
{
    if(HAL_getPwmEnableStatus(halMtrHandle[MTR_1]) == true)
    {
        tripFaultFlag_M1 = HAL_getMtrTripFaults(halMtrHandle[MTR_1]);

        if(HAL_getMtrTripFaults(halMtrHandle[MTR_1]) != 0)
        {
            motorVars[MTR_1].faultMtrNow.bit.moduleOverCurrent = 1;

            tripFaultCount_M1++;
        }
    }

    motorVars[MTR_1].faultMtrNow.bit.overVoltage =
                                      pfcVars.faultPFCNow.bit.overVoltageDC;

    motorVars[MTR_1].faultMtrNow.bit.underVoltage =
                                      pfcVars.faultPFCNow.bit.underVoltageDC;

    motorVars[MTR_1].faultMtrPrev.all |= motorVars[MTR_1].faultMtrNow.all;

    motorVars[MTR_1].faultMtrUse.all = motorVars[MTR_1].faultMtrNow.all &
                                        motorVars[MTR_1].faultMtrMask.all;

    motorVars[MTR_1].speedAbs_Hz = fabsf(motorVars[MTR_1].speed_Hz);

    HAL_setMtrCMPSSDACValue(halMtrHandle[MTR_1],
                    motorVars[MTR_1].dacCMPValH, motorVars[MTR_1].dacCMPValL);

    if(motorVars[MTR_1].flagClearFaults == true)
    {
        HAL_clearMtrFaultStatus(halMtrHandle[MTR_1]);

        motorVars[MTR_1].faultMtrNow.all = 0;
        motorVars[MTR_1].flagClearFaults = false;
    }

    if(motorVars[MTR_1].flagEnableRunAndIdentify == true)
    {
        // Had some faults to stop the motor
        if(motorVars[MTR_1].faultMtrUse.all != 0)
        {
            if(motorVars[MTR_1].flagRunIdentAndOnLine == true)
            {
                // for debug
                motorVars[MTR_1].flagEnableRunAndIdentify = false;

                motorVars[MTR_1].flagRunIdentAndOnLine = false;
                motorVars[MTR_1].motorState = MOTOR_FAULT_STOP;

                motorCtrlVars[MTR_1].stopWaitTimeCnt =
                        motorCtrlVars[MTR_1].restartWaitTimeSet;

                motorCtrlVars[MTR_1].restartTimesCnt++;
            }
            else if(motorCtrlVars[MTR_1].stopWaitTimeCnt == 0)
            {
                if(motorCtrlVars[MTR_1].restartTimesCnt <
                        motorCtrlVars[MTR_1].restartTimesSet)
                {
                    motorVars[MTR_1].flagClearFaults = 1;
                }
                else
                {
                    motorVars[MTR_1].flagEnableRunAndIdentify = false;
                }
            }

            // disable the PWM for stopping the motor
            HAL_disableMTRPWM(halMtrHandle[MTR_1]);
        }
        else if((motorVars[MTR_1].flagRunIdentAndOnLine == false) &&
                (motorCtrlVars[MTR_1].stopWaitTimeCnt == 0))
        {
        #if defined(MOTOR1_SSIPD)
            if(flagEnableIPD == true)
            {
                if(SSIPD_getDoneStatus(ssipdHandle) == true)
                {
                    if(motorVars[MTR_1].speedRef_Hz > 0.0f)
                    {
                        angleDetectIPD_rad = SSIPD_getAngleOut_rad(ssipdHandle) -
                                               angleOffsetIPD_rad;

                    }
                    else
                    {
                        angleDetectIPD_rad = SSIPD_getAngleOut_rad(ssipdHandle) +
                                               angleOffsetIPD_rad;
                    }

                    if(angleDetectIPD_rad < 0.0f)
                    {
                        angleDetectIPD_rad += MATH_TWO_PI;
                    }
                    else if(angleDetectIPD_rad > MATH_TWO_PI)
                    {
                        angleDetectIPD_rad -= MATH_TWO_PI;
                    }

                    restartMotorControl(MTR_1);
                    angleCurrentM1_rad = MATH_PI_OVER_TWO;

                    #if defined(MOTOR1_FAST)
                    EST_setAngle_rad(estM1Handle, angleDetectIPD_rad);
                    #endif // MOTOR1_FAST

                    #if defined(MOTOR1_ESMO)
                    ESMO_resetParams(esmoM1Handle);
                    #endif  //MOTOR1_ESMO
                }
                else if(SSIPD_getRunState(ssipdHandle) == true)
                {
                    if(SSIPD_getFlagEnablePWM(ssipdHandle) == true)
                    {
                        if(HAL_getPwmEnableStatus(halMtrHandle[MTR_1]) == false)
                        {
                            // enable the PWM for motor_1
                            HAL_enableMtrPWM(halMtrHandle[MTR_1]);
                        }
                    }
                    else
                    {
                        if(HAL_getPwmEnableStatus(halMtrHandle[MTR_1]) == true)
                        {
                            // disable the PWM for stopping the motor
                            HAL_disableMTRPWM(halMtrHandle[MTR_1]);
                        }
                    }
                }
                else
                {
                    SSIPD_start(ssipdHandle);
                }
            }
            else
            {
                restartMotorControl(MTR_1);
                angleCurrentM1_rad = MATH_PI_OVER_TWO;

                #if defined(MOTOR1_ESMO)
                ESMO_resetParams(esmoM1Handle);
                #endif  //MOTOR1_ESMO
            }
        #else  // !MOTOR1_SSIPD
            restartMotorControl(MTR_1);
            #if defined(MOTOR1_ESMO)
            ESMO_resetParams(esmoM1Handle);
            #endif  //MOTOR1_ESMO
        #endif  // !MOTOR1_SSIPD

            flagEnableFWCM1 = false;
            flagEnableMTPAM1 = false;

            angleCurrentM1_rad = MATH_PI_OVER_TWO;

        #if defined(MOTOR1_FWC)
            angleFWC_rad = MATH_PI_OVER_TWO;
        #endif  // MOTOR1_FWC

        #ifdef MOTOR1_MTPA
            angleMTPA_rad = MATH_PI_OVER_TWO;
        #endif  // MOTOR1_MTPA
        }
    }
    else if(motorVars[MTR_1].flagRunIdentAndOnLine == true)
    {
        motorVars[MTR_1].flagRunIdentAndOnLine = false;

        motorCtrlVars[MTR_1].stopWaitTimeCnt =
                motorCtrlVars[MTR_1].stopWaitTimeSet;

        flagEnableFWCM1 = false;
        flagEnableMTPAM1 = false;

        angleCurrentM1_rad = MATH_PI_OVER_TWO;

#if defined(MOTOR1_VIBCOMPA)
        VIB_COMP_setFlag_enableOutput(vibCompHandle, false);
#endif  // MOTOR1_VIBCOMPA
    }
    else
    {
        #if defined(MOTOR1_SSIPD)
        // Reset
        if(SSIPD_getDoneStatus(ssipdHandle) == true)
        {
            SSIPD_reset(ssipdHandle);
        }
        #endif // MOTOR1_SSIPD
    }


    #if defined(MOTOR1_FAST) && !defined(_SIMPLE_FAST_LIB)
    EST_setFlag_enableRsOnLine(estM1Handle,
                               motorVars[MTR_1].flagEnableRsOnLine);
    #endif // MOTOR1_FAST && !_SIMPLE_FAST_LIB

    if(motorVars[MTR_1].flagRunIdentAndOnLine == true)
    {
        if(HAL_getPwmEnableStatus(halMtrHandle[MTR_1]) == false)
        {
            #if defined(MOTOR1_FAST)
            // enable the estimator
            EST_enable(estM1Handle);

            // enable the trajectory generator
            EST_enableTraj(estM1Handle);
            #endif // MOTOR1_FAST

            // enable the PWM for motor_1
            HAL_enableMtrPWM(halMtrHandle[MTR_1]);
        }

        if(motorVars[MTR_1].flagMotorIdentified == true)
        {
            #if defined(MOTOR2_DISABLE)
             // This motor is disable
            #elif defined(MOTOR1_FAST) && defined(MOTOR1_ESMO)
            // enable or disable force angle
            EST_setFlag_enableForceAngle(estM1Handle,
                                         motorVars[MTR_1].flagEnableForceAngle);

            EST_setFlag_enableRsRecalc(estM1Handle,
                                       motorVars[MTR_1].flagEnableRsRecalc);

            if(motorVars[MTR_1].estimatorMode != ESTIMATOR_MODE_FAST)
            {
                if(motorVars[MTR_1].motorState >= MOTOR_CL_RUNNING)
                {
                    TRAJ_setTargetValue(trajHandle_spd[MTR_1],
                                        motorVars[MTR_1].speedRef_Hz);
                }
                else
                {
                    TRAJ_setTargetValue(trajHandle_spd[MTR_1],
                                        motorVars[MTR_1].speedForce_Hz);
                }
            }
            else
            {
                TRAJ_setTargetValue(trajHandle_spd[MTR_1],
                                    motorVars[MTR_1].speedRef_Hz);
            }
            #elif defined(MOTOR1_FAST)
            // enable or disable force angle
            EST_setFlag_enableForceAngle(estM1Handle,
                                         motorVars[MTR_1].flagEnableForceAngle);

            EST_setFlag_enableRsRecalc(estM1Handle,
                                       motorVars[MTR_1].flagEnableRsRecalc);

            TRAJ_setTargetValue(trajHandle_spd[MTR_1],
                                motorVars[MTR_1].speedRef_Hz);

            #elif defined(MOTOR1_ESMO)
            if(motorVars[MTR_1].motorState >= MOTOR_CL_RUNNING)
            {
                TRAJ_setTargetValue(trajHandle_spd[MTR_1],
                                    motorVars[MTR_1].speedRef_Hz);
            }
            else
            {
                TRAJ_setTargetValue(trajHandle_spd[MTR_1],
                                    motorVars[MTR_1].speedForce_Hz);
            }
            #else   // !MOTOR1_ESMO && !MOTOR1_FAST
            #error No select a right estimator for motor_1 control
            #endif  // MOTOR1_ESMO || MOTOR1_FAST

            if(motorVars[MTR_1].motorState == MOTOR_CTRL_RUN)
            {
                TRAJ_setMaxDelta(trajHandle_spd[MTR_1],
                  (motorVars[MTR_1].accelerationMax_Hzps / userParams[MTR_1].ctrlFreq_Hz));

                PI_setMinMax(piHandle_spd[MTR_1],
                             -motorVars[MTR_1].maxCurrent_A, motorVars[MTR_1].maxCurrent_A);

#if defined(MOTOR1_VIBCOMPA)
                VIB_COMP_setFlag_enableOutput(vibCompHandle, vibCompFlagEnable);
#endif  // MOTOR1_VIBCOMPA
            }
            else
            {
                TRAJ_setMaxDelta(trajHandle_spd[MTR_1],
                  (motorVars[MTR_1].accelerationStart_Hzps / userParams[MTR_1].ctrlFreq_Hz));

                PI_setMinMax(piHandle_spd[MTR_1],
                             -motorVars[MTR_1].startCurrent_A, motorVars[MTR_1].startCurrent_A);
            }

            if(motorVars[MTR_1].motorState == MOTOR_CL_RUNNING)
            {
                if(motorVars[MTR_1].stateRunTimeCnt >= motorVars[MTR_1].fwcTimeDelay)
                {
                    flagEnableMTPAM1 = true;
                    flagEnableFWCM1 = true;
                    Idq_ref_A[MTR_1].value[0] = 0.0f;
                    motorVars[MTR_1].motorState = MOTOR_CTRL_RUN;

#if defined(MOTOR1_VIBCOMPA) || defined(MOTOR1_VIBCOMPT)
                    VIB_COMP_setAngleMechPoles(vibCompHandle, angleFOCM1_rad);
#endif  // MOTOR1_VIBCOMPA || MOTOR1_VIBCOMPT
                }
            }
        }
    }
#if defined(MOTOR1_SSIPD)
    else if(SSIPD_getRunState(ssipdHandle) == false)
#else
    else
#endif  // MOTOR1_SSIPD
    {
#if defined(MOTOR1_FAST)
            // disable the estimator
        EST_disable(estM1Handle);

        // disable the trajectory generator
        EST_disableTraj(estM1Handle);
#endif // MOTOR1_FAST

        if(motorVars[MTR_1].motorState != MOTOR_FAULT_STOP)
        {
            resetMotorControl(MTR_1);
            motorVars[MTR_1].motorState = MOTOR_NORM_STOP;
        }
    }

#if defined(MOTOR1_FAST)
#if !defined(_SIMPLE_FAST_LIB)
    // check the trajectory generator
    if(EST_isTrajError(estM1Handle) == true)
    {
        // disable the PWM for stopping the motor
        HAL_disableMTRPWM(halMtrHandle[MTR_1]);
    }
    else
    {
        // update the trajectory generator state
        EST_updateTrajState(estM1Handle);
    }
#else  // !(_SIMPLE_FAST_LIB)
    // update the trajectory generator state
    EST_updateTrajState(estM1Handle);
#endif  // !(_SIMPLE_FAST_LIB)

    // check the estimator
    if(EST_isError(estM1Handle) == true)
    {
        // disable the PWM for stopping the motor
        HAL_disableMTRPWM(halMtrHandle[MTR_1]);
    }
    else        // No any estimator error
    {
        bool flagEstStateChanged = false;

        float32_t Id_target_A = EST_getIntValue_Id_A(estM1Handle);

        if(motorVars[MTR_1].flagMotorIdentified == true)
        {
            flagEstStateChanged = EST_updateState(estM1Handle, 0.0f);
        }
        else
        {
            flagEstStateChanged = EST_updateState(estM1Handle, Id_target_A);
        }

        if(flagEstStateChanged == true)
        {
            // configure the trajectory generator
            EST_configureTraj(estM1Handle);

            if(motorVars[MTR_1].flagMotorIdentified == false)
            {
                // configure the controllers
                EST_configureTrajState(estM1Handle,  &userParams[MTR_1],
                                       piHandle_spd[MTR_1],
                                       piHandle_Id[MTR_1], piHandle_Iq[MTR_1]);
            }

#if !defined(_SIMPLE_FAST_LIB)
            if(userParams[MTR_1].flag_bypassMotorId == false)
            {
                if((EST_isMotorIdentified(estM1Handle) == true) &&
                        (EST_isIdle(estM1Handle) == true))
                {
                    motorVars[MTR_1].flagMotorIdentified = true;

                    // clear the flag
                    motorVars[MTR_1].flagRunIdentAndOnLine = false;
                    motorVars[MTR_1].flagEnableRunAndIdentify = false;
                }
            }
#endif  // !_SIMPLE_FAST_LIB
        }

        motorVars[MTR_1].flagMotorIdentified = EST_isMotorIdentified(estM1Handle);
    }
#else // !MOTOR1_FAST
    motorVars[MTR_1].flagMotorIdentified = true;
#endif // !MOTOR1_FAST

    if(motorVars[MTR_1].flagMotorIdentified == true)
    {
        if(motorVars[MTR_1].flagSetupController == true)
        {
            // update the controller
            updateControllers(MTR_1);
        }
        else
        {
            motorVars[MTR_1].flagSetupController = true;

            setupControllers(MTR_1);
        }

#if defined(MOTOR1_VIBCOMPA)
        vibCompCtrl(vibCompHandle);
#endif // MOTOR1_VIBCOMPA

    }

#if defined(MOTOR1_FAST)
#if !defined(_SIMPLE_FAST_LIB)
    // run Rs online
    runRsOnLine(estM1Handle, MTR_1);
#endif  // !(_SIMPLE_FAST_LIB)

    // update the global variables
    updateGlobalVariables(estM1Handle, MTR_1);
#else// !MOTOR1_FAST
    // update the global variables without using FAST
    updateGlobalVariablesNF(MTR_1);
#endif // !MOTOR1_FAST

    return;
}   // end of the runMotor1Control() function

// the control variables for motor 1
void initMotor1CtrlParameters(void)
{
    // initialize the user parameters
    USER_setParams_priv(&userParams[MTR_1]);

    // initialize the user parameters
    USER_setMotor1Params(&userParams[MTR_1]);

    // initialize the driver
    halMtrHandle[MTR_1] =
            HAL_MTR_init(&halMtr[MTR_1], sizeof(halMtr[MTR_1]), MTR_1);

    // set the driver parameters
    HAL_MTR_setParams(halMtrHandle[MTR_1], &userParams[MTR_1]);


    // set the current scale coefficient
    adcData[MTR_1].current_sf = userParams[MTR_1].current_sf * USER_M1_SIGN_CURRENT_SF;

    motorVars[MTR_1].flagEnableForceAngle = true;
    motorVars[MTR_1].flagEnableUserParams = true;
    motorVars[MTR_1].flagEnableSpeedCtrl = true;
    motorVars[MTR_1].flagEnableFlyingStart = false;

#if defined(MOTOR1_DISABLE)
    // This motor is disable
#elif defined(MOTOR1_FAST)
    motorVars[MTR_1].estimatorMode = ESTIMATOR_MODE_FAST;
#elif defined(MOTOR1_ESMO)
    motorVars[MTR_1].estimatorMode = ESTIMATOR_MODE_ESMO;
#else   // !MOTOR1_ESMO && !MOTOR1_FAST
#error No select a right estimator for motor_1 control
#endif  // MOTOR1_ESMO || MOTOR1_FAST

    motorVars[MTR_1].faultMtrMask.all = MTR1_FAULT_MASK_SET;

    motorVars[MTR_1].overModulation = USER_M1_MAX_VS_MAG_PU;

#if !defined(_SIMPLE_FAST_LIB)
    motorVars[MTR_1].RsOnLineCurrent_A = 0.1f * USER_MOTOR1_MAX_CURRENT_A;
#endif  // !_SIMPLE_FAST_LIB

    motorVars[MTR_1].Kp_spd = 0.05f;
    motorVars[MTR_1].Ki_spd = 0.005f;

    motorVars[MTR_1].currentInv_sf = USER_M1_CURRENT_INV_SF;
    motorVars[MTR_1].overCurrent_A = USER_MOTOR1_OVER_CURRENT_A;
    motorVars[MTR_1].alignCurrent_A = USER_MOTOR1_ALIGN_CURRENT_A;
    motorVars[MTR_1].startCurrent_A = USER_MOTOR1_STARTUP_CURRENT_A;
    motorVars[MTR_1].maxCurrent_A = USER_MOTOR1_MAX_CURRENT_A;

    motorVars[MTR_1].speedStart_Hz = USER_MOTOR1_SPEED_START_Hz;
    motorVars[MTR_1].speedForce_Hz = USER_MOTOR1_SPEED_FORCE_Hz;

    motorVars[MTR_1].accelerationMax_Hzps = USER_MOTOR1_ACCEL_MAX_Hzps;
    motorVars[MTR_1].accelerationStart_Hzps = USER_MOTOR1_ACCEL_START_Hzps;

    motorVars[MTR_1].angleDelayed_sf = 0.5f * MATH_TWO_PI * USER_M1_CTRL_PERIOD_sec;

    motorCtrlVars[MTR_1].VIrmsIsrScale = userParams[MTR_1].ctrlFreq_Hz;

    motorCtrlVars[MTR_1].lostPhaseSet_A = USER_MOTOR1_STARTUP_CURRENT_A * 0.05f;
    motorCtrlVars[MTR_1].unbalanceRatioSet = USER_M1_UNBALANCE_RATIO;
    motorCtrlVars[MTR_1].stallCurrentSet_A = USER_M1_STALL_CURRENT_A;

    motorCtrlVars[MTR_1].IsFailedChekSet_A = USER_M1_FAULT_CHECK_CURRENT_A;

    motorCtrlVars[MTR_1].speedFailMaxSet_Hz = USER_M1_FAIL_SPEED_MAX_Hz;
    motorCtrlVars[MTR_1].speedFailMinSet_Hz = USER_M1_FAIL_SPEED_MIN_Hz;

    motorCtrlVars[MTR_1].motorStallTimeSet = USER_M1_STALL_TIME_SET;
    motorCtrlVars[MTR_1].unbalanceTimeSet = USER_M1_UNBALANCE_TIME_SET;
    motorCtrlVars[MTR_1].lostPhaseTimeSet = USER_M1_LOST_PHASE_TIME_SET;
    motorCtrlVars[MTR_1].overSpeedTimeSet = USER_M1_OVER_SPEED_TIME_SET;
    motorCtrlVars[MTR_1].startupFailTimeSet = USER_M1_STARTUP_FAIL_TIME_SET;

    motorCtrlVars[MTR_1].overCurrentTimesSet = USER_M1_OVER_CURRENT_TIMES_SET;

    motorCtrlVars[MTR_1].stopWaitTimeSet = USER_M1_STOP_WAIT_TIME_SET;
    motorCtrlVars[MTR_1].restartWaitTimeSet = USER_M1_RESTART_WAIT_TIME_SET;
    motorCtrlVars[MTR_1].restartTimesSet = USER_M1_START_TIMES_SET;

    motorCtrlVars[MTR_1].stopWaitTimeCnt = 0;

#ifdef MOTOR1_ESMO
    // initialize the esmo
    esmoM1Handle = ESMO_init(&esmoM1, sizeof(esmoM1));

    // set parameters for ESMO controller
    ESMO_setKslideParams(esmoM1Handle,
                         USER_MOTOR1_KSLIDE_MAX, USER_MOTOR1_KSLIDE_MIN);

    ESMO_setPLLParams(esmoM1Handle, USER_MOTOR1_PLL_KP_MAX,
                      USER_MOTOR1_PLL_KP_MIN, USER_MOTOR1_PLL_KP_SF);

    ESMO_setBEMFThreshold(esmoM1Handle, USER_MOTOR1_BEMF_THRESHOLD);
    ESMO_setOffsetCoef(esmoM1Handle, USER_MOTOR1_THETA_OFFSET_SF);
    ESMO_setBEMFKslfFreq(esmoM1Handle, USER_MOTOR1_BEMF_KSLF_FC_SF);
    ESMO_setSpeedFilterFreq(esmoM1Handle, USER_MOTOR1_SPEED_LPF_FC_Hz);

    // set the ESMO controller parameters
    ESMO_setParams(esmoM1Handle, &userParams[MTR_1]);

    // initialize the spdfr
    spdfrM1Handle = SPDFR_init(&spdfrM1, sizeof(spdfrM1));

    // set the spdfr parameters
    SPDFR_setParams(spdfrM1Handle, &userParams[MTR_1]);

    frswPosM1_sf = 0.6f;
#endif  //MOTOR1_ESMO

#if defined(MOTOR1_FAST)
    // initialize the estimator
    estM1Handle = EST_initEst(MTR_1);

    // set the default estimator parameters
    EST_setParams(estM1Handle, &userParams[MTR_1]);
    EST_setFlag_enableForceAngle(estM1Handle,
                                 motorVars[MTR_1].flagEnableForceAngle);
    EST_setFlag_enableRsRecalc(estM1Handle,
                               motorVars[MTR_1].flagEnableRsRecalc);

    motorVars[MTR_1].estState = EST_STATE_IDLE;
#endif // MOTOR1_FAST

#if defined(MOTOR1_DCLINKSS)
    // Initialize dc-link single-shunt handle
    dclinkM1Handle = DCLINK_SS_init(&dclinkM1, sizeof(dclinkM1));

    DCLINK_SS_setInitialConditions(dclinkM1Handle,
                                   HAL_getTimeBasePeriod(halMtrHandle[MTR_1]), 0.5f);

    //disable full sampling
    DCLINK_SS_setFlag_enableFullSampling(dclinkM1Handle, false);

    //enable sequence control
    DCLINK_SS_setFlag_enableSequenceControl(dclinkM1Handle, false);

    // Tdt  =  2.20us (Dead-time between top and bottom switch)
    // Tpd  =  0.75us (Gate driver propagation delay)
    // Tr   =  0.35us (Rise time of amplifier including power switches turn on time)
    // Ts   =  1.0us  (Settling time of amplifier)
    // Ts&h =  180ns  (ADC sample&holder = (16 + 2) SYSCLK)
    // T_MinAVDuration = Tdt+Tpd+Tr+Ts+Ts&h
    //                 = 2200+750+350+1000+180 = 4480ns => 450 SYSCLK cycles
    // T_SampleDelay   = Tdt+Tpd+Tr+Ts
    //                 = 2200+750+350+1000 = 4300ns => 430 SYSCLK cycles
    DCLINK_SS_setMinAVDuration(dclinkM1Handle, USER_M1_DCLINKSS_MIN_DURATION);
    DCLINK_SS_setSampleDelay(dclinkM1Handle, USER_M1_DCLINKSS_SAMPLE_DELAY);
#endif  // MOTOR1_DCLINKSS
#ifdef MOTOR1_MTPA
    // initialize the Maximum torque per ampere (MTPA)
    mtpaHandle = MTPA_init(&mtpa, sizeof(mtpa));

    // compute the motor constant for MTPA
    MTPA_computeParameters(mtpaHandle,
                           userParams[MTR_1].motor_Ls_d_H,
                           userParams[MTR_1].motor_Ls_q_H,
                           userParams[MTR_1].motor_ratedFlux_Wb);
#endif  // MOTOR1_MTPA

#if defined(MOTOR1_FWC) // MPTA and FWC are only for compressor
    piHandle_fwc = PI_init(&pi_fwc, sizeof(pi_fwc));

    // set the FWC controller
    PI_setGains(piHandle_fwc,
                USER_M1_FWC_KP, USER_M1_FWC_KI);
    PI_setUi(piHandle_fwc, 0.0f);
    PI_setMinMax(piHandle_fwc,
                 USER_M1_FWC_MAX_ANGLE_RAD, USER_M1_FWC_MIN_ANGLE_RAD);

    VsRef_pu = USER_M1_FWC_VREF;
    VsRef_V = USER_M1_FWC_VREF * USER_M1_NOMINAL_DC_BUS_VOLTAGE_V;

    Kp_fwc = USER_M1_FWC_KP;
    Ki_fwc = USER_M1_FWC_KI;

    angleFWCMax_rad = USER_M1_FWC_MAX_ANGLE_RAD;
#endif  // MOTOR1_FWC

#if defined(MOTOR1_SSIPD)
    ssipdHandle = SSIPD_init(&ssipd, sizeof(ssipd));
    SSIPD_setParams(ssipdHandle, 0.75f, (MATH_TWO_PI / SSIPD_DETECT_NUM), 6);

    angleOffsetIPD_rad = MATH_PI / SSIPD_DETECT_NUM;

    flagEnableIPD = false;
#endif  // MOTOR1_SSIPD

#if defined(MOTOR1_VIBCOMPA)
    vibCompAlpha = USER_MOTOR1_VIBCOMPA_ALPHA;
    vibCompGain  = USER_MOTOR1_VIBCOMPA_GAIN;
    vibCompIndexDelta = USER_MOTOR1_VIBCOMPA_INDEX_DELTA;

    vibCompFlagEnable = false;
    vibCompFlagReset = true;

    // Only compressor needs vibration torque compensation
    // Initialize the handle for vibration compensation
    vibCompHandle = VIB_COMP_init(&vibComp, sizeof(vibComp));

    VIB_COMPA_setParams(vibCompHandle, vibCompAlpha, vibCompGain,
                       vibCompIndexDelta, USER_MOTOR1_NUM_POLE_PAIRS);

    VIB_COMP_reset(vibCompHandle);
#elif defined(MOTOR1_VIBCOMPT)
    compressorAngle = 0.0f;
    vibCompAlpha0 = 0.0f;
    vibCompAlpha120 = 0.0f;
    vibCompAlpha240 = 0.0f;

    vibCompHandle = VIB_COMP_init(&vibComp, sizeof(vibComp));

    VIB_COMPT_setParams(vibCompHandle, USER_MOTOR1_NUM_POLE_PAIRS);
#endif  // MOTOR1_VIBCOMPA || MOTOR1_VIBCOMPA


    motorVars[MTR_1].forceRunTimeDelay = (uint16_t)(userParams[MTR_1].ctrlFreq_Hz * 1.0f);
    motorVars[MTR_1].lockRotorTimeDelay = (uint16_t)(userParams[MTR_1].ctrlFreq_Hz * 0.5f);
    motorVars[MTR_1].fwcTimeDelay = 1200;               // 5ms base, 6s

    motorVars[MTR_1].dacCMPValH = 2048U + 1024U;
    motorVars[MTR_1].dacCMPValL = 2048U - 1024U;

    // initialize the Clarke modules
    clarkeHandle_I[MTR_1] =
            CLARKE_init(&clarke_I[MTR_1], sizeof(clarke_I[MTR_1]));

    clarkeHandle_V[MTR_1] =
            CLARKE_init(&clarke_V[MTR_1], sizeof(clarke_V[MTR_1]));

    // set the Clarke parameters
    setupClarke_I(clarkeHandle_I[MTR_1], userParams[MTR_1].numCurrentSensors);
    setupClarke_V(clarkeHandle_V[MTR_1], userParams[MTR_1].numVoltageSensors);

    // initialize the inverse Park module
    iparkHandle_V[MTR_1] = IPARK_init(&ipark_V[MTR_1],
                                         sizeof(ipark_V[MTR_1]));

    // initialize the Park module
    parkHandle_I[MTR_1] = PARK_init(&park_I[MTR_1],
                                       sizeof(park_I[MTR_1]));

    // initialize the Park module
    parkHandle_V[MTR_1] = PARK_init(&park_V[MTR_1],
                                       sizeof(park_V[MTR_1]));

    // initialize the PI controllers
    piHandle_Id[MTR_1]  = PI_init(&pi_Id[MTR_1], sizeof(pi_Id[MTR_1]));
    piHandle_Iq[MTR_1]  = PI_init(&pi_Iq[MTR_1], sizeof(pi_Iq[MTR_1]));
    piHandle_spd[MTR_1] = PI_init(&pi_spd[MTR_1], sizeof(pi_spd[MTR_1]));

    // initialize the space vector generator module
    svgenHandle[MTR_1] = SVGEN_init(&svgen[MTR_1],
                                     sizeof(svgen[MTR_1]));

    // initialize the speed reference trajectory
    trajHandle_spd[MTR_1] = TRAJ_init(&traj_spd[MTR_1],
                                       sizeof(traj_spd[MTR_1]));

    // configure the speed reference trajectory (Hz)
    TRAJ_setTargetValue(trajHandle_spd[MTR_1], 0.0);
    TRAJ_setIntValue(trajHandle_spd[MTR_1], 0.0);
    TRAJ_setMinValue(trajHandle_spd[MTR_1], -userParams[MTR_1].maxFrequency_Hz);
    TRAJ_setMaxValue(trajHandle_spd[MTR_1], userParams[MTR_1].maxFrequency_Hz);
    TRAJ_setMaxDelta(trajHandle_spd[MTR_1],
          (userParams[MTR_1].maxAccel_Hzps / userParams[MTR_1].ctrlFreq_Hz));

#if((DMC_BUILDLEVEL == DMC_LEVEL_2) || (DMC_BUILDLEVEL == DMC_LEVEL_3) \
        || defined(MOTOR1_ESMO))
    // initialize the angle generate module
    angleGenM1Handle = ANGLE_GEN_init(&angleGenM1, sizeof(angleGenM1));

    ANGLE_GEN_setParams(angleGenM1Handle, userParams[MTR_1].ctrlPeriod_sec);
#endif  // ((DMC_BUILDLEVEL == DMC_LEVEL_3) || defined(MOTOR1_ESMO))

#if(DMC_BUILDLEVEL == DMC_LEVEL_2)
    // initialize the Vs per Freq module
    VsFreqHandle[MTR_1] = VS_FREQ_init(&VsFreq[MTR_1],
                                        sizeof(VsFreq[MTR_1]));

    VS_FREQ_setVsMagPu(VsFreqHandle[MTR_1],
                       userParams[MTR_1].maxVsMag_pu);

    VS_FREQ_setMaxFreq(VsFreqHandle[MTR_1],
                       USER_MOTOR1_FREQ_MAX_Hz);

    VS_FREQ_setProfile(VsFreqHandle[MTR_1],
                       USER_MOTOR1_FREQ_LOW_Hz, USER_MOTOR1_FREQ_HIGH_Hz,
                       USER_MOTOR1_VOLT_MIN_V, USER_MOTOR1_VOLT_MAX_V);
#endif // (DMC_BUILDLEVEL == DMC_LEVEL_2)

#if(DMC_BUILDLEVEL == DMC_LEVEL_3)
    Idq_set_A[MTR_1].value[0] = 0.0;
    Idq_set_A[MTR_1].value[1] = motorVars[MTR_1].startCurrent_A;
#endif // (DMC_BUILDLEVEL == DMC_LEVEL_3)

    // No flyingstart for motor 1 (compressor)

    // setup the controllers, speed, d/q-axis current pid regulator
    setupControllers(MTR_1);

    // disable the PWM for stopping the motor
    HAL_disableMTRPWM(halMtrHandle[MTR_1]);

    return;
}   // end of initMotor1CtrlParameters() function

//float32_t offsetM1_Idc_A = USER_M1_IDC_OFFSET_A;  // for debug

void runMotor1OffsetsCalculation(void)
{
    HAL_MTR_Obj *obj = (HAL_MTR_Obj *)halMtrHandle[MTR_1];

    // Offsets in phase current sensing
#if defined(MOTOR1_DCLINKSS)
    EPWM_setCounterCompareValue(obj->pwmHandle[1],
                                EPWM_COUNTER_COMPARE_C, 10);
    EPWM_setCounterCompareValue(obj->pwmHandle[1],
                                EPWM_COUNTER_COMPARE_D, 100);

    adcData[MTR_1].offset_Idc_A  = USER_M1_IDC_OFFSET_A;
#else  // !MOTOR1_DCLINKSS
    adcData[MTR_1].offset_I_A[0]  = USER_M1_IA_OFFSET_A;
    adcData[MTR_1].offset_I_A[1]  = USER_M1_IB_OFFSET_A;
    adcData[MTR_1].offset_I_A[2]  = USER_M1_IC_OFFSET_A;
#endif  // !MOTOR1_DCLINKSS

#if defined(MOTOR1_FAST)
    // Offsets in phase voltage sensing
    adcData[MTR_1].offset_V_sf[0]  = USER_M1_VA_OFFSET_SF;
    adcData[MTR_1].offset_V_sf[1]  = USER_M1_VB_OFFSET_SF;
    adcData[MTR_1].offset_V_sf[2]  = USER_M1_VC_OFFSET_SF;

    adcData[MTR_1].voltage_sf = userParams[MTR_1].voltage_sf;
#endif  // MOTOR1_FAST

    // enable PWM interrupt
    EPWM_enableInterrupt(obj->pwmHandle[0]);

    if(motorVars[MTR_1].flagEnableOffsetCalc == true)
    {
        float32_t offsetK1 = 0.999167893;  // Offset filter coefficient K1: 0.2/(T+0.2);
        float32_t offsetK2 = 0.000832107;  // Offset filter coefficient K2: T/(T+0.2);

#if defined(MOTOR1_FAST)
        float32_t invVdcbus = 1.0f;
#endif  // MOTOR1_FAST
        uint16_t offsetCnt;

#if defined(MOTOR1_DCLINKSS)
        // Offsets in dc link current sensing
        float32_t offsetM1_Idc_A = USER_M1_IDC_OFFSET_A;
        adcData[MTR_1].offset_Idc_A = 0.0f;
#else  // !MOTOR1_DCLINKSS
        // Offsets in phase current sensing
        float32_t offsetM1_I_A[3] =
                {USER_M1_IA_OFFSET_A, USER_M1_IB_OFFSET_A, USER_M1_IC_OFFSET_A};
        adcData[MTR_1].offset_I_A[0] = 0.0f;
        adcData[MTR_1].offset_I_A[1] = 0.0f;
        adcData[MTR_1].offset_I_A[2] = 0.0f;
#endif  // !MOTOR1_DCLINKSS

        // Set the 3-phase output PWMs to 50% duty cycle
        pwmData[MTR_1].Vabc_pu.value[0] = 0.0f;
        pwmData[MTR_1].Vabc_pu.value[1] = 0.0f;
        pwmData[MTR_1].Vabc_pu.value[2] = 0.0f;

        // enable the PWM for motor_1
        HAL_enableMtrPWM(halMtrHandle[MTR_1]);

        // write the PWM compare values
        HAL_writePWMData(halMtrHandle[MTR_1], &pwmData[MTR_1]);

        for(offsetCnt = 0; offsetCnt < 12000; offsetCnt++)
        {
            // clear PWM interrupt
            EPWM_clearEventTriggerInterruptFlag(obj->pwmHandle[0]);

            while(EPWM_getEventTriggerInterruptStatus(obj->pwmHandle[0]) == false);

            HAL_readMtr1ADCData(&adcData[MTR_1]);

            // read the ADC data with offsets
            HAL_readPFCADCData(&adcDataPFC);
            pfcVars.VdcBus_V = adcDataPFC.VdcBus * USER_PFC_ADC_FULL_SCALE_DC_VOLTAGE_V;

            if(offsetCnt >= 2000)
            {
//                offsetCnt = 2000;  // for debug

#if defined(MOTOR1_DCLINKSS)
                // Offsets in dc link current sensing
                offsetM1_Idc_A = offsetK1 * offsetM1_Idc_A +
                        0.25f * offsetK2 * (adcData[MTR_1].Idc1_A.value[0] +
                                            adcData[MTR_1].Idc1_A.value[1] +
                                            adcData[MTR_1].Idc2_A.value[0] +
                                            adcData[MTR_1].Idc2_A.value[1] );

#else  // !MOTOR1_DCLINKSS
                // Offsets in phase current sensing
                offsetM1_I_A[0] = offsetK1 * offsetM1_I_A[0] +
                        adcData[MTR_1].I_A.value[0] * offsetK2;

                offsetM1_I_A[1] = offsetK1 * offsetM1_I_A[1] +
                        adcData[MTR_1].I_A.value[1] * offsetK2;

                offsetM1_I_A[2] = offsetK1 * offsetM1_I_A[2] +
                        adcData[MTR_1].I_A.value[2] * offsetK2;
#endif  // !MOTOR1_DCLINKSS

#if defined(MOTOR1_FAST)
                invVdcbus = 1.0f / pfcVars.VdcBus_V;

                // Offsets in phase voltage sensing
                adcData[MTR_1].offset_V_sf[0] =
                         offsetK1 * adcData[MTR_1].offset_V_sf[0] +
                         (invVdcbus * adcData[MTR_1].V_V.value[0]) * offsetK2;

                adcData[MTR_1].offset_V_sf[1] =
                         offsetK1 * adcData[MTR_1].offset_V_sf[1] +
                         (invVdcbus * adcData[MTR_1].V_V.value[1]) * offsetK2;

                adcData[MTR_1].offset_V_sf[2] =
                         offsetK1 * adcData[MTR_1].offset_V_sf[2] +
                         (invVdcbus * adcData[MTR_1].V_V.value[2]) * offsetK2;
#endif  // MOTOR1_FAST
            }   // if()
        } // for()

        // disable the PWM for stopping the motor
        HAL_disableMTRPWM(halMtrHandle[MTR_1]);

#if defined(MOTOR1_DCLINKSS)
        adcData[MTR_1].offset_Idc_A = offsetM1_Idc_A;
#else  // !MOTOR1_DCLINKSS
        adcData[MTR_1].offset_I_A[0] = offsetM1_I_A[0];
        adcData[MTR_1].offset_I_A[1] = offsetM1_I_A[1];
        adcData[MTR_1].offset_I_A[2] = offsetM1_I_A[2];
#endif  // !MOTOR1_DCLINKSS

        motorVars[MTR_1].flagEnableOffsetCalc = false;
    }

#if !defined(MOTOR1_DCLINKSS) || defined(MOTOR1_FAST)
    // disable EPWM interrupt
    EPWM_disableInterrupt(obj->pwmHandle[0]);
#endif  //  !(MOTOR1_DCLINKSS)

    return;
} // end of runOffsetsCalculation() function

//------------------------------------------------------------------------------
#if defined(MOTOR1_FWC)
void updateFWCParams(void)
{
    // Update FW control parameters
    PI_setGains(piHandle_fwc, Kp_fwc, Ki_fwc);
    PI_setOutMin(piHandle_fwc, angleFWCMax_rad);
}
#endif  // MOTOR1_FWC

#if defined(MOTOR1_MTPA)
void updateMTPAParams(void)
{
    if(flagUpdateMTPAParamsM1 == true)
    {
        //
        // update motor parameters according to current
        //
        LsOnline_d_H =
                MTPA_updateLs_d_withLUT(mtpaHandle, motorVars[MTR_1].Is_A);

        LsOnline_q_H =
                MTPA_updateLs_q_withLUT(mtpaHandle, motorVars[MTR_1].Is_A);

        fluxOnline_Wb = motorVars[MTR_1].flux_Wb;

        //
        // update the motor constant for MTPA based on
        // the update Ls_d and Ls_q which are the function of Is
        //
        MTPA_computeParameters(mtpaHandle,
                               LsOnline_d_H, LsOnline_q_H, fluxOnline_Wb);
    }

    return;
}
#endif  // MOTOR1_MTPA

//
//-- end of this file ----------------------------------------------------------
//
