//#############################################################################
// $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/motor2_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(motor2CtrlISR, "ctrlfuncs");

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

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

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

#pragma DATA_SECTION(dclinkM2Handle, "motor_data");
#pragma DATA_SECTION(dclinkM2, "motor_data");
#endif  // MOTOR2_DCLINKSS

#ifdef MOTOR2_ESMO
//!< the handle for the esmo object
ESMO_Handle   esmoM2Handle;

//!< the esmo object
ESMO_Obj      esmoM2;

#pragma DATA_SECTION(esmoM2Handle, "motor_data");
#pragma DATA_SECTION(esmoM2, "motor_data");

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

//!< the speedfr object
SPDFR_Obj spdfrM2;

#pragma DATA_SECTION(spdfrM2Handle, "motor_data");
#pragma DATA_SECTION(spdfrM2, "motor_data");

float32_t angleCompM2_rad;
float32_t anglePLLM2_rad;
float32_t angleSMOM2_rad;
float32_t speedPLLM2_Hz;
float32_t frswPosM2_sf;

#pragma DATA_SECTION(angleCompM2_rad, "motor_data");
#pragma DATA_SECTION(anglePLLM2_rad, "motor_data");
#pragma DATA_SECTION(angleSMOM2_rad, "motor_data");
#pragma DATA_SECTION(speedPLLM2_Hz, "motor_data");
#pragma DATA_SECTION(frswPosM2_sf, "motor_data");
#endif  // MOTOR2_ESMO

#if defined(MOTOR2_FAST)
EST_Handle    estM2Handle;     //!< the handle for the estimator
EST_InputData_t estInputDataM2;
EST_OutputData_t estOutputDataM2;

#pragma DATA_SECTION(estM2Handle, "motor_data");
#pragma DATA_SECTION(estInputDataM2, "motor_data");
#pragma DATA_SECTION(estOutputDataM2, "motor_data");

float32_t angleDeltaM2_rad;   //!< the rotor angle compensation value
float32_t angleESTM2_rad;     //!< the rotor angle from EST modules
float32_t speedESTM2_Hz;     //!< the speed from EST modules

#pragma DATA_SECTION(angleDeltaM2_rad, "motor_data");
#pragma DATA_SECTION(angleESTM2_rad, "motor_data");
#pragma DATA_SECTION(speedESTM2_Hz, "motor_data");
#endif  // MOTOR2_FAST

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

//!< the Angle Generate onject for open loop control
ANGLE_GEN_Obj    angleGenM2;

float32_t angleGenM2_rad;     //!< the rotor angle from Generator modules
float32_t speedRefM2_Hz;

#pragma DATA_SECTION(angleGenM2Handle, "motor_data");
#pragma DATA_SECTION(angleGenM2, "motor_data");

#pragma DATA_SECTION(angleGenM2_rad, "motor_data");
#pragma DATA_SECTION(speedRefM2_Hz, "motor_data");
#endif  // ((DMC_BUILDLEVEL <= DMC_LEVEL_3) || defined(MOTOR2_ESMO))

// Fan doesn't need MTPA, FWC and SSIP functions
#if defined(MOTOR2_SSIPD)   // No available for fan
// No variables at here
#endif  // MOTOR2_SSIPD

#ifdef MOTOR2_MTPA          // No available for fan
// No variables at here
#endif  // MOTOR2_MTPA

#if defined(MOTOR2_FWC)     // No available for fan
// No variables at here
#endif  // MOTOR2_FWC

#ifdef MOTOR2_VIBCOMP       // No available for fan
// No variables at here
#endif  // MOTOR2_VIBCOMP

// Fan doesn't need MTPA, FWC and SSIP functions

// Flying start is only for fan
float32_t flyingStartSpeed_Hz;
uint16_t flyingStartTimeDelay;

#pragma DATA_SECTION(flyingStartSpeed_Hz, "motor_data");
#pragma DATA_SECTION(flyingStartTimeDelay, "motor_data");

__interrupt void motor2CtrlISR(void)
{


    motorVars[MTR_2].ISRCount++;

    // acknowledge the interrupt of motor 2
    HAL_ackMtr2ADCInt();

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

    // read the ADC data with offsets
    HAL_readMtr2ADCData(&adcData[MTR_2]);

#ifdef MOTOR2_DCLINKSS
    // run single-shunt current reconstruction
    DCLINK_SS_runCurrentReconstruction(dclinkM2Handle,
                                       &adcData[MTR_2].Idc1_A,
                                       &adcData[MTR_2].Idc2_A);     // 4 sampling

    adcData[MTR_2].I_A.value[0] = DCLINK_SS_getIa(dclinkM2Handle);
    adcData[MTR_2].I_A.value[1] = DCLINK_SS_getIb(dclinkM2Handle);
    adcData[MTR_2].I_A.value[2] = DCLINK_SS_getIc(dclinkM2Handle);
#endif  // MOTOR2_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_MOTOR2)
    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_MOTOR2_ID)
    {
        sfraNoiseId = sfraNoiseInj_pu * USER_M2_ADC_FULL_SCALE_CURRENT_A;
    }
    else if(sfraTestLoop == SFRA_TEST_MOTOR2_IQ)
    {
        sfraNoiseIq = sfraNoiseInj_pu * USER_M2_ADC_FULL_SCALE_CURRENT_A;
    }
    else if(sfraTestLoop == SFRA_TEST_MOTOR2_SPEED)
    {
        sfraNoiseSpd = sfraNoiseInj_pu * USER_MOTOR2_FREQ_MAX_Hz;
    }
#endif  // SFRA_ENABLE && SFRA_TEST_MOTOR2

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

// ---------------- FAST & eSMO ---------------------------------------
#elif defined(MOTOR2_FAST) && defined(MOTOR2_ESMO)  // run FAST and eSMO
    MATH_Vec2 phasor;
    bool flagEnableSpeedCtrl = false;
    bool flagEnableCurrentCtrl = false;

    ANGLE_GEN_run(angleGenM2Handle, estInputDataM2.speed_ref_Hz);
    angleGenM2_rad = ANGLE_GEN_getAngle(angleGenM2Handle);

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

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

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

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

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

    // run the FAST estimator
    EST_run(estM2Handle, &estInputDataM2, &estOutputDataM2);

    // compute angle with delay compensation
    angleDeltaM2_rad = userParams[MTR_2].angleDelayed_sf_sec *
                     estOutputDataM2.fm_lp_rps;

    angleESTM2_rad = MATH_incrAngle(estOutputDataM2.angle_rad, angleDeltaM2_rad);
    speedESTM2_Hz = EST_getFm_lp_Hz(estM2Handle);

    // run the eSMO
    ESMO_setSpeedRef(esmoM2Handle, estInputDataM2.speed_ref_Hz);
    ESMO_inline_run(esmoM2Handle, pfcVars.VdcBus_V,
                    &(pwmData[MTR_2].Vabc_pu), &(estInputDataM2.Iab_A));

    angleCompM2_rad = speedPLLM2_Hz * motorVars[MTR_2].angleDelayed_sf;
    anglePLLM2_rad = MATH_incrAngle(ESMO_getAnglePLL(esmoM2Handle), angleCompM2_rad);
    angleSMOM2_rad = ESMO_getAngleElec(esmoM2Handle);

    SPDFR_run(spdfrM2Handle, anglePLLM2_rad);
    speedPLLM2_Hz = SPDFR_getSpeedHz(spdfrM2Handle);

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

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

        // run the trajectories
        EST_runTraj(estM2Handle);

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

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

        motorVars[MTR_2].IdRated_A = EST_getIntValue_Id_A(estM2Handle);

        angleESTM2_rad = estOutputDataM2.angle_rad;
    }
    else if(motorVars[MTR_2].flagMotorIdentified == true)
    {
        if(motorVars[MTR_2].flagRunIdentAndOnLine == true)
        {
            counterTrajSpeed[MTR_2]++;

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

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

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

        estInputDataM2.speed_ref_Hz = TRAJ_getIntValue(trajHandle_spd[MTR_2]);

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

    motorVars[MTR_2].stateRunTimeCnt++;

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

        if(motorVars[MTR_2].motorState >= MOTOR_CL_RUNNING)
        {
            angleFOCM2_rad = angleESTM2_rad;
        }
        else if(motorVars[MTR_2].motorState == MOTOR_OL_START)
        {
            angleFOCM2_rad = angleESTM2_rad;

            if(estInputDataM2.speed_ref_Hz >= motorVars[MTR_2].speedForce_Hz)
            {
                motorVars[MTR_2].motorState = MOTOR_CL_RUNNING;
                ESMO_setAnglePu(esmoM2Handle, angleFOCM2_rad);
            }
        }
        else if(motorVars[MTR_2].motorState == MOTOR_ALIGNMENT)
        {
            angleFOCM2_rad = 0.0f;
            flagEnableSpeedCtrl = false;

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

            TRAJ_setIntValue(trajHandle_spd[MTR_2], 0.0f);

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

                EST_setAngle_rad(estM2Handle, angleFOCM2_rad);
                PI_setUi(piHandle_spd[MTR_2], 0.0);

                ESMO_setAnglePu(esmoM2Handle, angleFOCM2_rad);
                ANGLE_GEN_setAngle(angleGenM2Handle, angleFOCM2_rad);
            }
        }
    }
    else if(motorVars[MTR_2].estimatorMode == ESTIMATOR_MODE_ESMO)
    {
        motorVars[MTR_2].speed_Hz = speedPLLM2_Hz;

        if(motorVars[MTR_2].motorState >= MOTOR_CL_RUNNING)
        {
            angleFOCM2_rad = anglePLLM2_rad;

            ESMO_updateKslide(esmoM2Handle);
        }
        else if(motorVars[MTR_2].motorState == MOTOR_OL_START)
        {
            angleFOCM2_rad = angleGenM2_rad;
            flagEnableSpeedCtrl = false;

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

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

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

                    EST_setAngle_rad(estM2Handle, angleFOCM2_rad);
                    ESMO_setAnglePu(esmoM2Handle, angleFOCM2_rad);

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

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

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

                EST_setAngle_rad(estM2Handle, angleFOCM2_rad);
                ESMO_setAnglePu(esmoM2Handle, angleFOCM2_rad);
                ANGLE_GEN_setAngle(angleGenM2Handle, angleFOCM2_rad);
            }
        }
    }  // Check motor control state machine

    if(motorVars[MTR_2].motorState == MOTOR_SEEK_POS)
    {
        flagEnableSpeedCtrl = false;

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

        if(motorVars[MTR_2].stateRunTimeCnt > flyingStartTimeDelay)
        {
            motorVars[MTR_2].stateRunTimeCnt = 0;

            if(motorVars[MTR_2].speedAbs_Hz > flyingStartSpeed_Hz)
            {
                TRAJ_setIntValue(trajHandle_spd[MTR_2], motorVars[MTR_2].speed_Hz);
                PI_setUi(piHandle_spd[MTR_2], 0.0f);

                motorVars[MTR_2].motorState = MOTOR_CL_RUNNING;
            }
            else
            {
                angleFOCM2_rad = 0.0f;
                motorVars[MTR_2].motorState = MOTOR_ALIGNMENT;
            }
        }
    }   // seek rotor position for flyingstart

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

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

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

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

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

#if defined(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_MOTOR2)
            PI_run(piHandle_spd[MTR_2],
                   estInputDataM2.speed_ref_Hz + sfraNoiseSpd,
                   motorVars[MTR_2].speed_Hz,
                   (float32_t *)&motorVars[MTR_2].IsRef_A);
#else     // !(SFRA_ENABLE && SFRA_TEST_MOTOR2)
            PI_run(piHandle_spd[MTR_2],
                   estInputDataM2.speed_ref_Hz,
                   motorVars[MTR_2].speed_Hz,
                   (float32_t *)&motorVars[MTR_2].IsRef_A);
#endif  // !(SFRA_ENABLE && SFRA_TEST_MOTOR2)
        }
#if defined(MOTOR2_FWC) && defined(MOTOR2_MTPA) // No available for fan
        else if(counterSpeed[MTR_2] == 1)
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 2)
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 3)   // FWC
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 4)   // MTPA
        {
            // No any codes at here
        }
    }
    else
    {
        // No any codes at here
    }
#elif defined(MOTOR2_FWC) // No available for fan
        else if(counterSpeed[MTR_2] == 1)
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 2)
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 3)   // FWC
        {
            // No any codes at here
        }
    }
    else
    {
        // No any codes at here
    }
#elif defined(MOTOR2_MTPA) // No available for fan
        else if(counterSpeed[MTR_2] == 1)
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 4)   // MTPA
        {
            // No any codes at here
        }
    }
#else   // !MOTOR2_MTPA && !MOTOR2_FWC
    }
    Idq_ref_A[MTR_2].value[1] = motorVars[MTR_2].IsRef_A;
#endif  // !MOTOR2_MTPA && !MOTOR2_FWC

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

#if defined(MOTOR2_VIBCOMP) // No available for fan
    // No any codes at here
#else
    motorVars[MTR_2].IdqRef_A.value[1] = Idq_ref_A[MTR_2].value[1];
#endif  // MOTOR2_VIBCOMP

#ifdef MOTOR2_SSIPD  // No available for fan
    // No any codes at here
#endif  // MOTOR2_SSIPD

#endif  // (DMC_BUILDLEVEL > DMC_LEVEL_3)

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

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

    // ---------------- FAST ----------------------------------------------
#elif defined(MOTOR2_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(angleGenM2Handle, estInputDataM2.speed_ref_Hz);
    angleGenM2_rad = ANGLE_GEN_getAngle(angleGenM2Handle);
#endif  // (DMC_BUILDLEVEL <= DMC_LEVEL_3)

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

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

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

    adcData[MTR_2].V_V.value[2] -=
            adcData[MTR_2].offset_V_sf[2] * pfcVars.VdcBus_V;

    // run Clarke transform on voltage
    CLARKE_run(clarkeHandle_V[MTR_2],
               &adcData[MTR_2].V_V, &(estInputDataM2.Vab_V));

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

    // run the FAST estimator
    EST_run(estM2Handle, &estInputDataM2, &estOutputDataM2);

    // compute angle with delay compensation
    angleDeltaM2_rad = userParams[MTR_2].angleDelayed_sf_sec *
                     estOutputDataM2.fm_lp_rps;

    angleESTM2_rad = MATH_incrAngle(estOutputDataM2.angle_rad, angleDeltaM2_rad);
    speedESTM2_Hz = EST_getFm_lp_Hz(estM2Handle);

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

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

        // run the trajectories
        EST_runTraj(estM2Handle);

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

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

        motorVars[MTR_2].IdRated_A = EST_getIntValue_Id_A(estM2Handle);

        angleESTM2_rad = estOutputDataM2.angle_rad;
    }
    else if(motorVars[MTR_2].flagMotorIdentified == true)
    {
        if(motorVars[MTR_2].flagRunIdentAndOnLine == true)
        {
            counterTrajSpeed[MTR_2]++;

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

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

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

        estInputDataM2.speed_ref_Hz = TRAJ_getIntValue(trajHandle_spd[MTR_2]);

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

        angleFOCM2_rad = angleESTM2_rad;
    }

    motorVars[MTR_2].speed_Hz = speedESTM2_Hz;

    motorVars[MTR_2].stateRunTimeCnt++;

    if(motorVars[MTR_2].motorState >= MOTOR_CL_RUNNING)
    {
        angleFOCM2_rad = angleESTM2_rad;
    }
    else if(motorVars[MTR_2].motorState == MOTOR_OL_START)
    {
        angleFOCM2_rad = angleESTM2_rad;

        if(fabsf(estInputDataM2.speed_ref_Hz) >= fabsf(motorVars[MTR_2].speedForce_Hz))
        {
            motorVars[MTR_2].motorState = MOTOR_CL_RUNNING;
        }
    }
    else if(motorVars[MTR_2].motorState == MOTOR_ALIGNMENT)
    {
        angleFOCM2_rad = 0.0f;
        flagEnableSpeedCtrl = false;

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

        TRAJ_setIntValue(trajHandle_spd[MTR_2], 0.0f);

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

            EST_setAngle_rad(estM2Handle, angleFOCM2_rad);
            PI_setUi(piHandle_spd[MTR_2], 0.0);
        }
    }

    if(motorVars[MTR_2].motorState == MOTOR_SEEK_POS)
    {
        flagEnableSpeedCtrl = false;

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

        if(motorVars[MTR_2].stateRunTimeCnt > flyingStartTimeDelay)
        {
            motorVars[MTR_2].stateRunTimeCnt = 0;

            if(motorVars[MTR_2].speedAbs_Hz > flyingStartSpeed_Hz)
            {
                TRAJ_setIntValue(trajHandle_spd[MTR_2], motorVars[MTR_2].speed_Hz);
                PI_setUi(piHandle_spd[MTR_2], 0.0f);

                motorVars[MTR_2].motorState = MOTOR_CL_RUNNING;
            }
            else
            {
                angleFOCM2_rad = 0.0f;
                motorVars[MTR_2].motorState = MOTOR_ALIGNMENT;
            }
        }
    }   // seek rotor position for flyingstart

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

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

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

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

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

#if defined(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_MOTOR2)
            PI_run(piHandle_spd[MTR_2],
                   estInputDataM2.speed_ref_Hz + sfraNoiseSpd,
                   motorVars[MTR_2].speed_Hz,
                   (float32_t *)&motorVars[MTR_2].IsRef_A);
#else     // !(SFRA_ENABLE && SFRA_TEST_MOTOR2)
            PI_run(piHandle_spd[MTR_2],
                   estInputDataM2.speed_ref_Hz,
                   motorVars[MTR_2].speed_Hz,
                   (float32_t *)&motorVars[MTR_2].IsRef_A);
#endif  // !(SFRA_ENABLE && SFRA_TEST_MOTOR2)
        }
#if defined(MOTOR2_FWC) && defined(MOTOR2_MTPA) // No available for fan
        else if(counterSpeed[MTR_2] == 1)
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 2)
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 3)   // FWC
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 4)   // MTPA
        {
            // No any codes at here
        }
    }
    else
    {
        // No any codes at here
    }
#elif defined(MOTOR2_FWC) // No available for fan
        else if(counterSpeed[MTR_2] == 1)
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 2)
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 3)   // FWC
        {
            // No any codes at here
        }
    }
    else
    {
        // No any codes at here
    }
#elif defined(MOTOR2_MTPA) // No available for fan
        else if(counterSpeed[MTR_2] == 1)
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 4)   // MTPA
        {
            // No any codes at here
        }
    }
#else   // !MOTOR2_MTPA && !MOTOR2_FWC
    }
    Idq_ref_A[MTR_2].value[1] = motorVars[MTR_2].IsRef_A;
#endif  // !MOTOR2_MTPA && !MOTOR2_FWC

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

#if defined(MOTOR2_VIBCOMP) // No available for fan
    // No any codes at here
#else
    motorVars[MTR_2].IdqRef_A.value[1] = Idq_ref_A[MTR_2].value[1];
#endif  // MOTOR2_VIBCOMP

#ifdef MOTOR2_SSIPD  // No available for fan
    // No any codes at here
#endif  // MOTOR2_SSIPD

#endif  // (DMC_BUILDLEVEL > DMC_LEVEL_3)

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

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

    // ---------------- eSMO ----------------------------------------------
#elif defined(MOTOR2_ESMO)    // Only eSMO
    MATH_Vec2 Iab_A;
    MATH_Vec2 phasor;

    bool flagEnableSpeedCtrl = false;
    bool flagEnableCurrentCtrl = false;

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

    ANGLE_GEN_run(angleGenM2Handle, speedRefM2_Hz);
    angleGenM2_rad = ANGLE_GEN_getAngle(angleGenM2Handle);

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

    ESMO_setSpeedRef(esmoM2Handle, speedRefM2_Hz);
    ESMO_inline_run(esmoM2Handle, pfcVars.VdcBus_V,
                    &(pwmData[MTR_2].Vabc_pu), &Iab_A);

    angleCompM2_rad = speedPLLM2_Hz * motorVars[MTR_2].angleDelayed_sf;
    anglePLLM2_rad = MATH_incrAngle(ESMO_getAnglePLL(esmoM2Handle), angleCompM2_rad);

    SPDFR_run(spdfrM2Handle, anglePLLM2_rad);
    speedPLLM2_Hz = SPDFR_getSpeedHz(spdfrM2Handle);

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

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

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

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

    speedRefM2_Hz = TRAJ_getIntValue(trajHandle_spd[MTR_2]);
    motorVars[MTR_2].speed_Hz = speedPLLM2_Hz;

    motorVars[MTR_2].stateRunTimeCnt++;

#if(DMC_BUILDLEVEL > DMC_LEVEL_3)
    if(motorVars[MTR_2].motorState >= MOTOR_CL_RUNNING)
    {
        angleFOCM2_rad = anglePLLM2_rad;

        ESMO_updateKslide(esmoM2Handle);
    }
    else if(motorVars[MTR_2].motorState == MOTOR_OL_START)
    {
        angleFOCM2_rad = angleGenM2_rad;
        flagEnableSpeedCtrl = false;

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

        if(speedRefM2_Hz >= motorVars[MTR_2].speedForce_Hz)
        {
            TRAJ_setIntValue(trajHandle_spd[MTR_2], speedRefM2_Hz);

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

                ESMO_setAnglePu(esmoM2Handle, angleFOCM2_rad);

                PI_setUi(piHandle_spd[MTR_2], (frswPosM2_sf * Idq_ref_A[MTR_2].value[1]));

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

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

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

            ESMO_setAnglePu(esmoM2Handle, angleFOCM2_rad);
            ANGLE_GEN_setAngle(angleGenM2Handle, angleFOCM2_rad);
        }
    }
    else if(motorVars[MTR_2].motorState == MOTOR_SEEK_POS)
    {
        flagEnableSpeedCtrl = false;

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

        if(motorVars[MTR_2].stateRunTimeCnt > flyingStartTimeDelay)
        {
            motorVars[MTR_2].stateRunTimeCnt = 0;

            if(motorVars[MTR_2].speedAbs_Hz > flyingStartSpeed_Hz)
            {
                TRAJ_setIntValue(trajHandle_spd[MTR_2], motorVars[MTR_2].speed_Hz);
                PI_setUi(piHandle_spd[MTR_2], 0.0f);

                motorVars[MTR_2].motorState = MOTOR_CL_RUNNING;
            }
            else
            {
                angleFOCM2_rad = 0.0f;
                motorVars[MTR_2].motorState = MOTOR_ALIGNMENT;
            }
        }
    }

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

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

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

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

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

#if defined(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_MOTOR2)
            PI_run(piHandle_spd[MTR_2],
                          speedRefM2_Hz + sfraNoiseSpd,
                          motorVars[MTR_2].speed_Hz,
                          (float32_t *)&motorVars[MTR_2].IsRef_A);
#else   // !(SFRA_ENABLE && SFRA_TEST_MOTOR2)
            PI_run(piHandle_spd[MTR_2],
                          speedRefM2_Hz,
                          motorVars[MTR_2].speed_Hz,
                          (float32_t *)&motorVars[MTR_2].IsRef_A);
#endif  // !(SFRA_ENABLE && SFRA_TEST_MOTOR2)
        }
#if defined(MOTOR2_FWC) && defined(MOTOR2_MTPA) // No available for fan
        else if(counterSpeed[MTR_2] == 1)
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 2)
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 3)   // FWC
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 4)   // MTPA
        {
            // No any codes at here
        }
    }
    else
    {
        // No any codes at here
    }
#elif defined(MOTOR2_FWC) // No available for fan
        else if(counterSpeed[MTR_2] == 1)
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 2)
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 3)   // FWC
        {
            // No any codes at here
        }
    }
    else
    {
        // No any codes at here
    }
#elif defined(MOTOR2_MTPA) // No available for fan
        else if(counterSpeed[MTR_2] == 1)
        {
            // No any codes at here
        }
        else if(counterSpeed[MTR_2] == 4)   // MTPA
        {
            // No any codes at here
        }
    }
#else   // !MOTOR2_MTPA && !MOTOR2_FWC
    }
    Idq_ref_A[MTR_2].value[1] = motorVars[MTR_2].IsRef_A;
#endif  // !MOTOR2_MTPA && !MOTOR2_FWC

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

#if defined(MOTOR2_VIBCOMP) // No available for fan
    // No any codes at here
#else
    motorVars[MTR_2].IdqRef_A.value[1] = Idq_ref_A[MTR_2].value[1];
#endif  // MOTOR2_VIBCOMP

#ifdef MOTOR2_SSIPD  // No available for fan
    // No any codes at here
#endif  // MOTOR2_SSIPD

#else   // (DMC_BUILDLEVEL <= DMC_LEVEL_3)
    if(motorVars[MTR_2].motorState == MOTOR_ALIGNMENT)
    {
        angleFOCM2_rad = 0.0f;
        flagEnableSpeedCtrl = false;

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

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

            ESMO_setAnglePu(esmoM2Handle, angleFOCM2_rad);
            ANGLE_GEN_setAngle(angleGenM2Handle, angleFOCM2_rad);
        }
    }
#endif  // (DMC_BUILDLEVEL > DMC_LEVEL_3)

    // setup the space vector generator (SVGEN) module
    SVGEN_setup(svgenHandle[MTR_2], oneOverDcBus_invV);
#else   // !MOTOR2_ESMO && !MOTOR2_FAST
#error No select a right estimator for motor_2 control
#endif  // MOTOR2_ESMO || MOTOR2_FAST

#if !defined(MOTOR2_DISABLE)
//---------- Common Open loop for FAST or eSMO ---------------------------------
#if ((DMC_BUILDLEVEL == DMC_LEVEL_2) || (DMC_BUILDLEVEL == DMC_LEVEL_3))
    angleFOCM2_rad = angleGenM2_rad;

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

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

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

    // run the Park transform
#if defined(MOTOR2_FAST)
    PARK_run(parkHandle_I[MTR_2], &estInputDataM2.Iab_A, &Idq_in_A[MTR_2]);
#elif defined(MOTOR2_ESMO)
    PARK_run(parkHandle_I[MTR_2], &Iab_A, &Idq_in_A[MTR_2]);
#else   // !(MOTOR2_FAST || MOTOR2_ESMO)
#error No select a right estimator for motor_2 control
#endif  // !(MOTOR2_FAST || MOTOR2_ESMO)
#endif  // (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_2].maxVsMag_V =
                userParams[MTR_2].maxVsMag_pu * pfcVars.VdcBus_V;

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

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

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

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

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

//---------- v/f Open loop for FAST or eSMO ---------------------------------
#if(DMC_BUILDLEVEL == DMC_LEVEL_2)
#if defined(MOTOR2_FAST)
    VS_FREQ_run(VsFreqHandle[MTR_2], estInputDataM2.speed_ref_Hz);
#elif defined(MOTOR2_ESMO)
    VS_FREQ_run(VsFreqHandle[MTR_2], speedRefM2_Hz);
#else
#error No select a right estimator for motor_2 control
#endif  // MOTOR2_FAST || MOTOR2_ESMO

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

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

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

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

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

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

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

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

#if defined(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_MOTOR2)
    if(sfraTestLoop == SFRA_TEST_MOTOR2_SPEED)
    {
        sfraNoiseOut = motorVars[MTR_2].IsRef_A * (1.0f / USER_M2_ADC_FULL_SCALE_CURRENT_A);
        sfraNoiseFdb = motorVars[MTR_2].speed_Hz * (1.0f / USER_MOTOR2_FREQ_MAX_Hz);
    }
    else if(sfraTestLoop == SFRA_TEST_MOTOR2_ID)
    {
        sfraNoiseOut = Vdq_out_V[MTR_2].value[0] * (1.0f / USER_M2_ADC_FULL_SCALE_VOLTAGE_V);
        sfraNoiseFdb = Idq_in_A[MTR_2].value[0] * (1.0f / USER_M2_ADC_FULL_SCALE_CURRENT_A);
    }
    else if(sfraTestLoop == SFRA_TEST_MOTOR2_IQ)
    {
        sfraNoiseOut = Vdq_out_V[MTR_2].value[1] * (1.0f / USER_M2_ADC_FULL_SCALE_VOLTAGE_V);
        sfraNoiseFdb = Idq_in_A[MTR_2].value[1] * (1.0f / USER_M2_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_MOTOR2)

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

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



#if defined(EPWMDAC_MODE) && defined(PWMDAC_MOTOR)
    // connect inputs of the PWMDAC module.
    HAL_writePWMDACData(halHandle, &pwmDACData);
#endif  // EPWMDAC_MODE

#if defined(DATALOGF2_EN) && defined(DATALOG_MOTOR2)
    if(DATALOGIF_enable(datalogHandle) == true)
    {
        DATALOGIF_updateWithDMA(datalogHandle);

        // Force trig DMA channel to save the data
        HAL_trigDMAforDLOG(halHandle, 0);
        HAL_trigDMAforDLOG(halHandle, 1);
    }
#endif  // DATALOGF2_EN

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

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

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

    return;
} // end of motor2CtrlISR() function

void runMotor2Control(void)
{
    if(HAL_getPwmEnableStatus(halMtrHandle[MTR_2]) == true)
    {
        if(HAL_getMtrTripFaults(halMtrHandle[MTR_2]) != 0)
        {
            motorVars[MTR_2].faultMtrNow.bit.moduleOverCurrent = 1;
        }
    }

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

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

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

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

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

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

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

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

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

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

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

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

            // disable the PWM for stopping the motor
            HAL_disableMTRPWM(halMtrHandle[MTR_2]);
        }
        else if((motorVars[MTR_2].flagRunIdentAndOnLine == false) &&
                (motorCtrlVars[MTR_2].stopWaitTimeCnt == 0))
        {
            restartMotorControl(MTR_2);

            #if defined(MOTOR2_ESMO)
            ESMO_resetParams(esmoM2Handle);
            #endif  //MOTOR2_ESMO
        }
    }
    else if(motorVars[MTR_2].flagRunIdentAndOnLine == true)
    {
        motorVars[MTR_2].flagRunIdentAndOnLine = false;

        if(motorVars[MTR_2].flagEnableFlyingStart == false)
        {
            motorCtrlVars[MTR_2].stopWaitTimeCnt =
                    motorCtrlVars[MTR_2].stopWaitTimeSet;
        }
        else
        {
            motorCtrlVars[MTR_2].stopWaitTimeCnt = 0;
        }
    }

    #if defined(MOTOR2_FAST) && !defined(_SIMPLE_FAST_LIB)
    EST_setFlag_enableRsOnLine(estM2Handle,
                               motorVars[MTR_2].flagEnableRsOnLine);
    #endif // MOTOR2_FAST && !_SIMPLE_FAST_LIB

    if(motorVars[MTR_2].flagRunIdentAndOnLine == true)
    {
        if(HAL_getPwmEnableStatus(halMtrHandle[MTR_2]) == false)
        {
            #if defined(MOTOR2_FAST)
            // enable the estimator
            EST_enable(estM2Handle);

            // enable the trajectory generator
            EST_enableTraj(estM2Handle);
            #endif // MOTOR2_FAST

            // enable the PWM for motor_2
            HAL_enableMtrPWM(halMtrHandle[MTR_2]);
        }

        if(motorVars[MTR_2].flagMotorIdentified == true)
        {
            #if defined(MOTOR2_DISABLE)
            // This motor is disable
            #elif defined(MOTOR2_FAST) && defined(MOTOR2_ESMO)
            // enable or disable force angle
            EST_setFlag_enableForceAngle(estM2Handle,
                                         motorVars[MTR_2].flagEnableForceAngle);

            EST_setFlag_enableRsRecalc(estM2Handle,
                                       motorVars[MTR_2].flagEnableRsRecalc);

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

            EST_setFlag_enableRsRecalc(estM2Handle,
                                       motorVars[MTR_2].flagEnableRsRecalc);

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

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

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

                PI_setMinMax(piHandle_spd[MTR_2],
                             -motorVars[MTR_2].maxCurrent_A,
                             motorVars[MTR_2].maxCurrent_A);
            }
            else
            {
                TRAJ_setMaxDelta(trajHandle_spd[MTR_2],
                  (motorVars[MTR_2].accelerationStart_Hzps / userParams[MTR_2].ctrlFreq_Hz));

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

            if(motorVars[MTR_2].motorState == MOTOR_CL_RUNNING)
            {
                if(motorVars[MTR_2].stateRunTimeCnt >= motorVars[MTR_2].fwcTimeDelay)
                {
                    Idq_ref_A[MTR_2].value[0] = 0.0f;
                    motorVars[MTR_2].motorState = MOTOR_CTRL_RUN;
                }
            }
        }
    }
    else
    {
        #if defined(MOTOR2_FAST)
        if(motorVars[MTR_2].flagEnableFlyingStart == false)
        {
            // disable the estimator
            EST_disable(estM2Handle);

            // disable the trajectory generator
            EST_disableTraj(estM2Handle);
        }
        #endif // MOTOR2_FAST

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

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

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

        float32_t Id_target_A = EST_getIntValue_Id_A(estM2Handle);

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

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

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

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

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

        motorVars[MTR_2].flagMotorIdentified = EST_isMotorIdentified(estM2Handle);
    }
#else       // !MOTOR2_FAST
    motorVars[MTR_2].flagMotorIdentified = true;
#endif      // !MOTOR2_FAST

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

            setupControllers(MTR_2);
        }
#if defined(MOTOR2_VIBCOMP)
        // No vibration compensation for motor 2 (fan)
#endif // MOTOR2_VIBCOMP
    }

#if defined(MOTOR2_FAST)
#if !defined(_SIMPLE_FAST_LIB)
    // run Rs online
    runRsOnLine(estM2Handle, MTR_2);
#endif  // !(_SIMPLE_FAST_LIB)

    // update the global variables
    updateGlobalVariables(estM2Handle, MTR_2);
#else// !MOTOR2_FAST
    // update the global variables without using FAST
    updateGlobalVariablesNF(MTR_2);
#endif // !MOTOR2_FAST

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

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

    // initialize the user parameters
    USER_setMotor2Params(&userParams[MTR_2]);

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

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


    // set the current scale coefficient
    adcData[MTR_2].current_sf = userParams[MTR_2].current_sf * USER_M2_SIGN_CURRENT_SF;

    motorVars[MTR_2].flagEnableForceAngle = true;
    motorVars[MTR_2].flagEnableUserParams = true;
    motorVars[MTR_2].flagEnableSpeedCtrl = true;
    motorVars[MTR_2].flagEnableFlyingStart = true;

#if defined(MOTOR2_DISABLE)
    // This motor is disable
#elif defined(MOTOR2_FAST)
    motorVars[MTR_2].estimatorMode = ESTIMATOR_MODE_FAST;
#elif defined(MOTOR2_ESMO)
    motorVars[MTR_2].estimatorMode = ESTIMATOR_MODE_ESMO;
#else   // !MOTOR2_ESMO && !MOTOR2_FAST
#error No select a right estimator for motor_2 control
#endif  // MOTOR2_ESMO || MOTOR2_FAST

    motorVars[MTR_2].faultMtrMask.all = MTR2_FAULT_MASK_SET;

    motorVars[MTR_2].overModulation = USER_M2_MAX_VS_MAG_PU;

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

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

    motorVars[MTR_2].currentInv_sf = USER_M2_CURRENT_INV_SF;
    motorVars[MTR_2].overCurrent_A = USER_MOTOR2_OVER_CURRENT_A;
    motorVars[MTR_2].alignCurrent_A = USER_MOTOR2_ALIGN_CURRENT_A;
    motorVars[MTR_2].startCurrent_A = USER_MOTOR2_STARTUP_CURRENT_A;
    motorVars[MTR_2].maxCurrent_A = USER_MOTOR2_MAX_CURRENT_A;

    motorVars[MTR_2].speedStart_Hz = USER_MOTOR2_SPEED_START_Hz;
    motorVars[MTR_2].speedForce_Hz = USER_MOTOR2_SPEED_FORCE_Hz;

    motorVars[MTR_2].accelerationMax_Hzps = USER_MOTOR2_ACCEL_MAX_Hzps;
    motorVars[MTR_2].accelerationStart_Hzps = USER_MOTOR2_ACCEL_START_Hzps;

    motorVars[MTR_2].angleDelayed_sf = 0.5f * MATH_TWO_PI * USER_M2_CTRL_PERIOD_sec;

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

    motorCtrlVars[MTR_2].lostPhaseSet_A = USER_MOTOR2_STARTUP_CURRENT_A * 0.05f;
    motorCtrlVars[MTR_2].unbalanceRatioSet = USER_M2_UNBALANCE_RATIO;
    motorCtrlVars[MTR_2].stallCurrentSet_A = USER_M2_STALL_CURRENT_A;

    motorCtrlVars[MTR_2].IsFailedChekSet_A = USER_M2_FAULT_CHECK_CURRENT_A;

    motorCtrlVars[MTR_2].speedFailMaxSet_Hz = USER_M2_FAIL_SPEED_MAX_Hz;
    motorCtrlVars[MTR_2].speedFailMinSet_Hz = USER_M2_FAIL_SPEED_MIN_Hz;

    motorCtrlVars[MTR_2].motorStallTimeSet = USER_M2_STALL_TIME_SET;
    motorCtrlVars[MTR_2].unbalanceTimeSet = USER_M2_UNBALANCE_TIME_SET;
    motorCtrlVars[MTR_2].lostPhaseTimeSet = USER_M2_LOST_PHASE_TIME_SET;
    motorCtrlVars[MTR_2].overSpeedTimeSet = USER_M2_OVER_SPEED_TIME_SET;
    motorCtrlVars[MTR_2].startupFailTimeSet = USER_M2_STARTUP_FAIL_TIME_SET;

    motorCtrlVars[MTR_2].overCurrentTimesSet = USER_M2_OVER_CURRENT_TIMES_SET;

    motorCtrlVars[MTR_2].stopWaitTimeSet = USER_M2_STOP_WAIT_TIME_SET;
    motorCtrlVars[MTR_2].restartWaitTimeSet = USER_M2_RESTART_WAIT_TIME_SET;
    motorCtrlVars[MTR_2].restartTimesSet = USER_M2_START_TIMES_SET;

    motorCtrlVars[MTR_2].stopWaitTimeCnt = 0;

#ifdef MOTOR2_ESMO
    // initialize the esmo
    esmoM2Handle = ESMO_init(&esmoM2, sizeof(esmoM2));

    // set parameters for ESMO controller
    ESMO_setKslideParams(esmoM2Handle,
                         USER_MOTOR2_KSLIDE_MAX, USER_MOTOR2_KSLIDE_MIN);

    ESMO_setPLLParams(esmoM2Handle, USER_MOTOR2_PLL_KP_MAX,
                      USER_MOTOR2_PLL_KP_MIN, USER_MOTOR2_PLL_KP_SF);

    ESMO_setBEMFThreshold(esmoM2Handle, USER_MOTOR2_BEMF_THRESHOLD);
    ESMO_setOffsetCoef(esmoM2Handle, USER_MOTOR2_THETA_OFFSET_SF);
    ESMO_setBEMFKslfFreq(esmoM2Handle, USER_MOTOR2_BEMF_KSLF_FC_SF);
    ESMO_setSpeedFilterFreq(esmoM2Handle, USER_MOTOR2_SPEED_LPF_FC_Hz);

    // set the ESMO controller parameters
    ESMO_setParams(esmoM2Handle, &userParams[MTR_2]);

    // initialize the spdfr
    spdfrM2Handle = SPDFR_init(&spdfrM2, sizeof(spdfrM2));

    // set the spdfr parameters
    SPDFR_setParams(spdfrM2Handle, &userParams[MTR_2]);

    frswPosM2_sf = 0.6f;
#endif  //MOTOR2_ESMO

#if defined(MOTOR2_FAST)
    // initialize the estimator
    estM2Handle = EST_initEst(MTR_2);

    // set the default estimator parameters
    EST_setParams(estM2Handle, &userParams[MTR_2]);

    EST_setFlag_enableForceAngle(estM2Handle,
                                 motorVars[MTR_2].flagEnableForceAngle);
    EST_setFlag_enableRsRecalc(estM2Handle,
                               motorVars[MTR_2].flagEnableRsRecalc);

    motorVars[MTR_2].estState = EST_STATE_IDLE;
#endif // MOTOR2_FAST

#if defined(MOTOR2_DCLINKSS)
    // Initialize dc-link single-shunt handle
    dclinkM2Handle = DCLINK_SS_init(&dclinkM2, sizeof(dclinkM2));

    DCLINK_SS_setInitialConditions(dclinkM2Handle,
                                   HAL_getTimeBasePeriod(halMtrHandle[MTR_2]), 0.5f);

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

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

    // Tdt  =  1.80us (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
    //                 = 1800+750+350+1000+180 = 4080ns => 410 SYSCLK cycles
    // T_SampleDelay   = Tdt+Tpd+Tr+Ts
    //                 = 1800+750+350+1000 = 3900ns => 390 SYSCLK cycles
    DCLINK_SS_setMinAVDuration(dclinkM2Handle, USER_M2_DCLINKSS_MIN_DURATION);
    DCLINK_SS_setSampleDelay(dclinkM2Handle, USER_M2_DCLINKSS_SAMPLE_DELAY);
#endif  // MOTOR2_DCLINKSS
#ifdef MOTOR2_MTPA
    // No MTPA for motor 2 (fan)
#endif  // MOTOR2_MTPA

#if defined(MOTOR2_FWC) // MPTA and FWC are only for compressor
    // No field wekening control for motor 2 (fan)
#endif  // MOTOR2_FWC

#if defined(MOTOR2_SSIPD)
    // No six step IPD for motor 2 (fan)
#endif  // MOTOR2_SSIPD

#ifdef MOTOR2_VIBCOMP
    // No vibration compensation for motor 2 (fan)
#endif  // MOTOR2_VIBCOMP

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

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

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

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

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

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

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

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

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

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

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

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

#if((DMC_BUILDLEVEL == DMC_LEVEL_2) || (DMC_BUILDLEVEL == DMC_LEVEL_3) || \
        defined(MOTOR2_ESMO))
    // initialize the angle generate module
    angleGenM2Handle = ANGLE_GEN_init(&angleGenM2, sizeof(angleGenM2));

    ANGLE_GEN_setParams(angleGenM2Handle, userParams[MTR_2].ctrlPeriod_sec);
#endif  // ((DMC_BUILDLEVEL == DMC_LEVEL_3) || defined(MOTOR2_ESMO))

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

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

    VS_FREQ_setMaxFreq(VsFreqHandle[MTR_2],
                       USER_MOTOR2_FREQ_MAX_Hz);

    VS_FREQ_setProfile(VsFreqHandle[MTR_2],
                       USER_MOTOR2_FREQ_LOW_Hz, USER_MOTOR2_FREQ_HIGH_Hz,
                       USER_MOTOR2_VOLT_MIN_V, USER_MOTOR2_VOLT_MAX_V);
#endif // (DMC_BUILDLEVEL == DMC_LEVEL_2)

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

    flyingStartSpeed_Hz = USER_MOTOR2_SPEED_FLYST_Hz;
    flyingStartTimeDelay = USER_MOTOR2_DELAY_FLYST_s;

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

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

    return;
}   // end of initMotor2CtrlParameters() function

//float32_t offsetM2_Idc_A = USER_M2_IDC_OFFSET_A;  // for debug

void runMotor2OffsetsCalculation(void)
{
    HAL_MTR_Obj *obj = (HAL_MTR_Obj *)halMtrHandle[MTR_2];

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

    adcData[MTR_2].offset_Idc_A  = USER_M2_IDC_OFFSET_A;
#else // !MOTOR2_DCLINKSS
    adcData[MTR_2].offset_I_A[0]  = USER_M2_IA_OFFSET_A;
    adcData[MTR_2].offset_I_A[1]  = USER_M2_IB_OFFSET_A;
    adcData[MTR_2].offset_I_A[2]  = USER_M2_IC_OFFSET_A;
#endif  // !MOTOR2_DCLINKSS

#if defined(MOTOR2_FAST)
    // Offsets in phase voltage sensing
    adcData[MTR_2].offset_V_sf[0]  = USER_M2_VA_OFFSET_SF;
    adcData[MTR_2].offset_V_sf[1]  = USER_M2_VB_OFFSET_SF;
    adcData[MTR_2].offset_V_sf[2]  = USER_M2_VC_OFFSET_SF;

    adcData[MTR_2].voltage_sf = userParams[MTR_2].voltage_sf;
#endif  // MOTOR2_FAST

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

    if(motorVars[MTR_2].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(MOTOR2_FAST)
        float32_t invVdcbus = 1.0f;
#endif  // MOTOR2_FAST
        uint16_t offsetCnt;

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

        // enable the PWM for motor_2
        HAL_enableMtrPWM(halMtrHandle[MTR_2]);

#if defined(MOTOR2_DCLINKSS)
        // Offsets in dc link current sensing
        float32_t offsetM2_Idc_A = USER_M2_IDC_OFFSET_A;
        adcData[MTR_2].offset_Idc_A = 0.0f;
#else  // !MOTOR2_DCLINKSS
        // Offsets in phase current sensing
        float32_t offsetM2_I_A[3] =
                {USER_M2_IA_OFFSET_A, USER_M2_IB_OFFSET_A, USER_M2_IC_OFFSET_A};
        adcData[MTR_2].offset_I_A[0] = 0.0f;
        adcData[MTR_2].offset_I_A[1] = 0.0f;
        adcData[MTR_2].offset_I_A[2] = 0.0f;
#endif  // !MOTOR2_DCLINKSS

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

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

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

            HAL_readMtr2ADCData(&adcData[MTR_2]);

            // 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(MOTOR2_DCLINKSS)
                // Offsets in dc link current sensing
                offsetM2_Idc_A = offsetK1 * offsetM2_Idc_A +
                        0.25f * offsetK2 * (adcData[MTR_2].Idc1_A.value[0] +
                                            adcData[MTR_2].Idc1_A.value[1] +
                                            adcData[MTR_2].Idc2_A.value[0] +
                                            adcData[MTR_2].Idc2_A.value[1] );
#else  // !MOTOR2_DCLINKSS
                // Offsets in phase current sensing
                // Offsets in phase current sensing
                offsetM2_I_A[0] = offsetK1 * offsetM2_I_A[0] +
                        adcData[MTR_2].I_A.value[0] * offsetK2;

                offsetM2_I_A[1] = offsetK1 * offsetM2_I_A[1] +
                        adcData[MTR_2].I_A.value[1] * offsetK2;

                offsetM2_I_A[2] = offsetK1 * offsetM2_I_A[2] +
                        adcData[MTR_2].I_A.value[2] * offsetK2;
#endif  // !MOTOR2_DCLINKSS

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

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

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

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

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

#if defined(MOTOR2_DCLINKSS)
        adcData[MTR_2].offset_Idc_A = offsetM2_Idc_A;
#else  // !MOTOR2_DCLINKSS
        adcData[MTR_2].offset_I_A[0] = offsetM2_I_A[0];
        adcData[MTR_2].offset_I_A[1] = offsetM2_I_A[1];
        adcData[MTR_2].offset_I_A[2] = offsetM2_I_A[2];
#endif  // !MOTOR2_DCLINKSS

        motorVars[MTR_2].flagEnableOffsetCalc = false;
    }

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

    return;
} // end of runOffsetsCalculation() function

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