//#############################################################################
// $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/motor_common.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"

// the globals
HAL_Handle  halHandle;      //!< the handle for the hardware abstraction layer
HAL_Obj     hal;            //!< the hardware abstraction layer object
#pragma DATA_SECTION(halHandle, "user_data");
#pragma DATA_SECTION(halMtr, "user_data");

HAL_MTR_Handle  halMtrHandle[2];    //!< the handle for the hardware abstraction
                                    //!< layer to motor control
HAL_MTR_Obj     halMtr[2];          //!< the hardware abstraction layer object
                                    //!< to motor control
#pragma DATA_SECTION(halMtrHandle, "user_data");
#pragma DATA_SECTION(halMtr, "user_data");

USER_Params userParams[2];
#pragma DATA_SECTION(userParams,"user_data");

// the variables for motor control
volatile MOTOR_Vars_t motorVars[2];
#pragma DATA_SECTION(motorVars, "motor_data");

// the variables for motor control
volatile MOTOR_CtrlVars_t motorCtrlVars[2];
#pragma DATA_SECTION(motorCtrlVars, "motor_data");

HAL_ADCData_t adcData[2];
#pragma DATA_SECTION(adcData, "motor_data");

HAL_PWMData_t pwmData[2];
#pragma DATA_SECTION(pwmData, "motor_data");

MATH_Vec2 Vab_out_V[2];     //!< the output control voltage on alpha&beta axis
MATH_Vec2 Vdq_out_V[2];     //!< the output control voltage on d&q axis
#pragma DATA_SECTION(Vab_out_V, "foc_data");
#pragma DATA_SECTION(Vdq_out_V, "foc_data");

MATH_Vec2 Idq_in_A[2];         //!< the d&q axis current are converter from
                                 //!< 3-phase sampling input current of motor
MATH_Vec2 Idq_ref_A[2];        //!< the reference current on d&q rotation axis

#pragma DATA_SECTION(Idq_in_A, "foc_data");
#pragma DATA_SECTION(Idq_ref_A, "foc_data");

//!< the handle for the current Clarke transform
CLARKE_Handle clarkeHandle_I[2];

//!< the current Clarke transform object
CLARKE_Obj    clarke_I[2];

#pragma DATA_SECTION(clarkeHandle_I, "foc_data");
#pragma DATA_SECTION(clarke_I, "foc_data");

//!< the handle for the voltage Clarke transform
CLARKE_Handle clarkeHandle_V[2];

//!< the voltage Clarke transform object
CLARKE_Obj    clarke_V[2];

#pragma DATA_SECTION(clarkeHandle_V, "foc_data");
#pragma DATA_SECTION(clarke_V, "foc_data");

//!< the handle for the inverse Park transform
IPARK_Handle  iparkHandle_V[2];

//!< the inverse Park transform object
IPARK_Obj     ipark_V[2];

#pragma DATA_SECTION(iparkHandle_V, "foc_data");
#pragma DATA_SECTION(ipark_V, "foc_data");

//!< the handle for the Park object
PARK_Handle   parkHandle_I[2];

//!< the Park transform object
PARK_Obj      park_I[2];

#pragma DATA_SECTION(parkHandle_I, "foc_data");
#pragma DATA_SECTION(park_I, "foc_data");

//!< the handle for the Park object
PARK_Handle   parkHandle_V[2];

//!< the Park transform object
PARK_Obj      park_V[2];

#pragma DATA_SECTION(parkHandle_V, "foc_data");
#pragma DATA_SECTION(park_V, "foc_data");

//!< the handle for the Id PI controller
PI_Handle     piHandle_Id[2];

//!< the Id PI controller object
PI_Obj        pi_Id[2];

#pragma DATA_SECTION(piHandle_Id, "foc_data");
#pragma DATA_SECTION(pi_Id, "foc_data");

//!< the handle for the Iq PI controller
PI_Handle     piHandle_Iq[2];

//!< the Iq PI controller object
PI_Obj        pi_Iq[2];

#pragma DATA_SECTION(piHandle_Iq, "foc_data");
#pragma DATA_SECTION(pi_Iq, "foc_data");

//!< the handle for the speed PI controller
PI_Handle     piHandle_spd[2];

//!< the speed PI controller object
PI_Obj        pi_spd[2];

#pragma DATA_SECTION(piHandle_spd, "foc_data");
#pragma DATA_SECTION(pi_spd, "foc_data");

//!< the handle for the space vector generator
SVGEN_Handle  svgenHandle[2];

//!< the space vector generator object
SVGEN_Obj     svgen[2];

#pragma DATA_SECTION(svgenHandle, "foc_data");
#pragma DATA_SECTION(svgen, "foc_data");

//<! the space vector generator current object
SVGENCURRENT_Handle svgencurrentHandle;

//!< the handle for the space vector generator current
SVGENCURRENT_Obj svgencurrent;

#pragma DATA_SECTION(svgencurrentHandle, "foc_data");
#pragma DATA_SECTION(svgencurrent, "foc_data");

//!< the handle for the speed reference trajectory
TRAJ_Handle  trajHandle_spd[2];

//!< the speed reference trajectory object
TRAJ_Obj     traj_spd[2];

#pragma DATA_SECTION(trajHandle_spd, "foc_data");
#pragma DATA_SECTION(traj_spd, "foc_data");

#if(DMC_BUILDLEVEL == DMC_LEVEL_2)
//!< the handles for Vs per Freq for open loop control
VS_FREQ_Handle VsFreqHandle[2];
//!< the Vs per Freq object for open loop control
VS_FREQ_Obj    VsFreq[2];

#pragma DATA_SECTION(VsFreqHandle, "foc_data");
#pragma DATA_SECTION(VsFreq, "foc_data");
#endif // (DMC_BUILDLEVEL == DMC_LEVEL_2)

#if(DMC_BUILDLEVEL <= DMC_LEVEL_3)
MATH_Vec2 Idq_set_A[2];        //!< the reference current on d&q rotation axis
#endif // (DMC_BUILDLEVEL == DMC_LEVEL_3)

uint16_t counterSpeed[2];
uint16_t counterTrajSpeed[2];
#pragma DATA_SECTION(counterSpeed, "foc_data");
#pragma DATA_SECTION(counterTrajSpeed, "foc_data");

volatile uint16_t mtr1PIEIER;
volatile uint16_t mtr2PIEIER;
#pragma DATA_SECTION(mtr1PIEIER, "foc_data");
#pragma DATA_SECTION(mtr2PIEIER, "foc_data");


#if defined(SFRA_ENABLE)
float32_t   sfraNoiseVpfc;
float32_t   sfraNoiseIpfc;
float32_t   sfraNoiseId;
float32_t   sfraNoiseIq;
float32_t   sfraNoiseSpd;
float32_t   sfraNoiseOut;
float32_t   sfraNoiseFdb;
float32_t   sfraNoiseInj_pu;
SFRA_TEST_e sfraTestLoop;
bool        sfraCollectStart;
#pragma DATA_SECTION(sfraNoiseVpfc, "foc_data");
#pragma DATA_SECTION(sfraNoiseIpfc, "foc_data");
#pragma DATA_SECTION(sfraNoiseId, "foc_data");
#pragma DATA_SECTION(sfraNoiseIq, "foc_data");
#pragma DATA_SECTION(sfraNoiseSpd, "foc_data");
#pragma DATA_SECTION(sfraNoiseOut, "foc_data");
#pragma DATA_SECTION(sfraNoiseFdb, "foc_data");
#pragma DATA_SECTION(sfraNoiseInj_pu, "foc_data");
#pragma DATA_SECTION(sfraTestLoop, "foc_data");
#pragma DATA_SECTION(sfraCollectStart, "foc_data");
#endif  // SFRA_ENABLE

//------------------------------------------------------------------------------
// Sets up control parameters for restarting motor
void restartMotorControl(MotorNum_e mtrNum)
{
    if(motorVars[mtrNum].flagEnableFlyingStart == true)
    {
        motorVars[mtrNum].motorState = MOTOR_SEEK_POS;
    }
    else
    {
        motorVars[mtrNum].motorState = MOTOR_ALIGNMENT;
    }

    motorVars[mtrNum].flagRunIdentAndOnLine = true;
    motorVars[mtrNum].stateRunTimeCnt = 0;

    TRAJ_setTargetValue(trajHandle_spd[mtrNum], 0.0f);
    TRAJ_setIntValue(trajHandle_spd[mtrNum], 0.0f);

    // clear integral outputs of the controllers
    PI_setUi(piHandle_spd[mtrNum], 0.0f);
    PI_setUi(piHandle_Id[mtrNum], 0.0f);
    PI_setUi(piHandle_Iq[mtrNum], 0.0f);

    return;
}

//------------------------------------------------------------------------------
// Resets motor control parameters for restarting motor
void resetMotorControl(MotorNum_e mtrNum)
{
    TRAJ_setTargetValue(trajHandle_spd[mtrNum], 0.0f);
    TRAJ_setIntValue(trajHandle_spd[mtrNum], 0.0f);

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

    // clear integral outputs of the controllers
    PI_setUi(piHandle_Id[mtrNum], 0.0f);
    PI_setUi(piHandle_Iq[mtrNum], 0.0f);
    PI_setUi(piHandle_spd[mtrNum], 0.0f);

    // clear current references
    Idq_ref_A[mtrNum].value[0] = 0.0f;
    Idq_ref_A[mtrNum].value[1] = 0.0f;

#if !defined(_SIMPLE_FAST_LIB)
    motorVars[mtrNum].IdRated_A = 0.0f;
#endif  // !(_SIMPLE_FAST_LIB)

    motorVars[mtrNum].IsRef_A = 0.0f;
    motorVars[mtrNum].IdqRef_A.value[0] = 0.0f;
    motorVars[mtrNum].IdqRef_A.value[1] = 0.0f;

    motorVars[mtrNum].stateRunTimeCnt = 0;
    motorVars[mtrNum].motorState = MOTOR_STOP_IDLE;

    motorCtrlVars[mtrNum].motorStallTimeCnt = 0;
    motorCtrlVars[mtrNum].lostPhaseTimeCnt = 0;
    motorCtrlVars[mtrNum].unbalanceTimeCnt = 0;
    motorCtrlVars[mtrNum].startupFailTimeCnt = 0;
    motorCtrlVars[mtrNum].overSpeedTimeCnt = 0;

    motorCtrlVars[mtrNum].restartTimesCnt = 0;

    return;
}

//------------------------------------------------------------------------------
void runMotorMonitor(MotorNum_e mtrNum)
{
    if(motorCtrlVars[mtrNum].stopWaitTimeCnt > 0)
    {
        motorCtrlVars[mtrNum].stopWaitTimeCnt--;
    }

    // check these faults when motor is running
    if(motorVars[mtrNum].motorState >= MOTOR_CL_RUNNING)
    {
        // Motor Stall
        if( (motorVars[mtrNum].Is_A > motorCtrlVars[mtrNum].stallCurrentSet_A)
          && (motorVars[mtrNum].speedAbs_Hz < motorCtrlVars[mtrNum].speedFailMinSet_Hz))
        {
            if(motorCtrlVars[mtrNum].motorStallTimeCnt >
                                    motorCtrlVars[mtrNum].motorStallTimeSet)
            {
                motorVars[mtrNum].faultMtrNow.bit.motorStall = 1;
                motorCtrlVars[mtrNum].motorStallTimeCnt = 0;
            }
            else
                motorCtrlVars[mtrNum].motorStallTimeCnt++;
        }
        else if(motorCtrlVars[mtrNum].motorStallTimeCnt > 0)
        {
            motorCtrlVars[mtrNum].motorStallTimeCnt--;
        }

        // FAULT CHECK-1ms: Motor Lost Phase Fault Check (SM-OK)
        if( (motorVars[mtrNum].speedAbs_Hz > motorCtrlVars[mtrNum].speedFailMinSet_Hz) &&
            ( (motorCtrlVars[mtrNum].Irms_A[0] < motorCtrlVars[mtrNum].lostPhaseSet_A) ||
              (motorCtrlVars[mtrNum].Irms_A[1] < motorCtrlVars[mtrNum].lostPhaseSet_A) ||
              (motorCtrlVars[mtrNum].Irms_A[2] < motorCtrlVars[mtrNum].lostPhaseSet_A) ))
        {
            if(motorCtrlVars[mtrNum].lostPhaseTimeCnt >
                                   motorCtrlVars[mtrNum].lostPhaseTimeSet)
            {
                motorVars[mtrNum].faultMtrNow.bit.motorLostPhase = 1;
                motorCtrlVars[mtrNum].lostPhaseTimeCnt = 0;
            }
            else
            {
                motorCtrlVars[mtrNum].lostPhaseTimeCnt++;
            }
        }
        else if(motorCtrlVars[mtrNum].lostPhaseTimeCnt > 0)
        {
            motorCtrlVars[mtrNum].lostPhaseTimeCnt--;
        }

        // Only when the torque is great than a setting value
        if(motorVars[mtrNum].Is_A > motorCtrlVars[mtrNum].IsFailedChekSet_A)
        {
            // Motor Phase Current Unbalance
            if(motorCtrlVars[mtrNum].unbalanceRatio >
                                     motorCtrlVars[mtrNum].unbalanceRatioSet)
            {
                if(motorCtrlVars[mtrNum].unbalanceTimeCnt >
                                      motorCtrlVars[mtrNum].unbalanceTimeSet)
                {
                    motorVars[mtrNum].faultMtrNow.bit.currentUnbalance = 1;
                    motorCtrlVars[mtrNum].unbalanceTimeCnt = 0;
                }
                else
                {
                    motorCtrlVars[mtrNum].unbalanceTimeCnt++;
                }
            }
            else if(motorCtrlVars[mtrNum].unbalanceTimeCnt > 0)
            {
                motorCtrlVars[mtrNum].unbalanceTimeCnt--;
            }

            // Motor Startup Failed
            if( (motorVars[mtrNum].Is_A < motorCtrlVars[mtrNum].stallCurrentSet_A)
               && (motorVars[mtrNum].speedAbs_Hz < motorCtrlVars[mtrNum].speedFailMinSet_Hz))
            {
                if(motorCtrlVars[mtrNum].startupFailTimeCnt >
                                       motorCtrlVars[mtrNum].startupFailTimeSet)
                {
                    motorVars[mtrNum].faultMtrNow.bit.startupFailed = 1;
                    motorCtrlVars[mtrNum].startupFailTimeCnt = 0;
                }
                else
                {
                    motorCtrlVars[mtrNum].startupFailTimeCnt++;
                }
            }
            else if(motorCtrlVars[mtrNum].startupFailTimeCnt > 0)
            {
                motorCtrlVars[mtrNum].startupFailTimeCnt--;
            }

            // Motor Over speed
            if(motorVars[mtrNum].speedAbs_Hz > motorCtrlVars[mtrNum].speedFailMaxSet_Hz)
            {
                if(motorCtrlVars[mtrNum].overSpeedTimeCnt >
                                     motorCtrlVars[mtrNum].overSpeedTimeSet)
                {
                    motorVars[mtrNum].faultMtrNow.bit.overSpeed = 1;
                    motorCtrlVars[mtrNum].overSpeedTimeCnt = 0;
                }
                else
                {
                    motorCtrlVars[mtrNum].overSpeedTimeCnt++;
                }
            }
            else if(motorCtrlVars[mtrNum].overSpeedTimeCnt > 0)
            {
                motorCtrlVars[mtrNum].overSpeedTimeCnt--;
            }

        } // motorVars[mtrNum].Is_A > motorCtrlVars[mtrNum].IsFailedChekSet_A
    } // motorVars[mtrNum].operateState == OPERATE_State_Run

    return;
}

void calculateRMSData(MotorNum_e mtrNum)
{
    float32_t IrmsMax_A, IrmsMin_A, VIrmsIsrSet;

    if(motorCtrlVars[mtrNum].flagVIrmsCal == true)
    {
        motorCtrlVars[mtrNum].flagVIrmsCal = false;

        motorCtrlVars[mtrNum].Irms_A[0] =
                sqrtf(motorCtrlVars[mtrNum].IrmsPrdSum[0] * motorCtrlVars[mtrNum].IrmsCalSF);

        motorCtrlVars[mtrNum].Irms_A[1] =
                sqrtf(motorCtrlVars[mtrNum].IrmsPrdSum[1] * motorCtrlVars[mtrNum].IrmsCalSF);

        motorCtrlVars[mtrNum].Irms_A[2] =
                sqrtf(motorCtrlVars[mtrNum].IrmsPrdSum[2] * motorCtrlVars[mtrNum].IrmsCalSF);

        if(motorCtrlVars[mtrNum].Irms_A[0] > motorCtrlVars[mtrNum].Irms_A[1])
        {
            IrmsMax_A = motorCtrlVars[mtrNum].Irms_A[0];
            IrmsMin_A = motorCtrlVars[mtrNum].Irms_A[1];
        }
        else
        {
            IrmsMax_A = motorCtrlVars[mtrNum].Irms_A[0];
            IrmsMin_A = motorCtrlVars[mtrNum].Irms_A[1];
        }

        IrmsMax_A = (motorCtrlVars[mtrNum].Irms_A[2] > IrmsMax_A) ?
                motorCtrlVars[mtrNum].Irms_A[2] : IrmsMax_A;

        IrmsMin_A = (motorCtrlVars[mtrNum].Irms_A[2] < IrmsMin_A) ?
                motorCtrlVars[mtrNum].Irms_A[2] : IrmsMin_A;

        VIrmsIsrSet = motorCtrlVars[mtrNum].VIrmsIsrScale / motorVars[mtrNum].speedAbs_Hz;

        VIrmsIsrSet = (VIrmsIsrSet > motorCtrlVars[mtrNum].VIrmsIsrScale) ?
                motorCtrlVars[mtrNum].VIrmsIsrScale : VIrmsIsrSet;

        motorCtrlVars[mtrNum].VIrmsIsrSet = (uint16_t)(VIrmsIsrSet);

        motorCtrlVars[mtrNum].IrmsCalSF = 1.0f / ((float32_t)(motorCtrlVars[mtrNum].VIrmsIsrSet));

        motorCtrlVars[mtrNum].unbalanceRatio =
                (IrmsMax_A - IrmsMin_A) / (IrmsMax_A + IrmsMin_A);
    }
}

#if !defined(_SIMPLE_FAST_LIB)
//------------------------------------------------------------------------------
void setupCurrentControllers(const MotorNum_e mtrNum)
{
    float32_t RoverL_Kp_sf = userParams[mtrNum].RoverL_Kp_sf;
    float32_t dcBus_nominal_V = userParams[mtrNum].dcBus_nominal_V;
    float32_t maxCurrent_A = userParams[mtrNum].maxCurrent_A;
    float32_t RoverL_min_rps = userParams[mtrNum].RoverL_min_rps;
    float32_t currentCtrlPeriod_sec =
                (float32_t)userParams[mtrNum].numCtrlTicksPerCurrentTick /
                    userParams[mtrNum].ctrlFreq_Hz;

    float32_t outMax_V = userParams[mtrNum].Vd_sf *
                         userParams[mtrNum].maxVsMag_V;

    float32_t Kp = RoverL_Kp_sf * dcBus_nominal_V / maxCurrent_A;
    float32_t Ki = RoverL_min_rps * currentCtrlPeriod_sec;

    // set the Id controller
    PI_setGains(piHandle_Id[mtrNum], Kp, Ki);
    PI_setUi(piHandle_Id[mtrNum], 0.0);
    PI_setRefValue(piHandle_Id[mtrNum], 0.0);
    PI_setFbackValue(piHandle_Id[mtrNum], 0.0);
    PI_setFfwdValue(piHandle_Id[mtrNum], 0.0);
    PI_setMinMax(piHandle_Id[mtrNum], -outMax_V, outMax_V);

    // set the Iq controller
    PI_setGains(piHandle_Iq[mtrNum], Kp, Ki);
    PI_setUi(piHandle_Iq[mtrNum], 0.0);
    PI_setRefValue(piHandle_Iq[mtrNum], 0.0);
    PI_setFbackValue(piHandle_Iq[mtrNum], 0.0);
    PI_setFfwdValue(piHandle_Iq[mtrNum], 0.0);
    PI_setMinMax(piHandle_Iq[mtrNum], -outMax_V, outMax_V);

    return;
} // end of setupCurrentControllers() function
#endif  // !_SIMPLE_FAST_LIB

//------------------------------------------------------------------------------
void setupControllers(const MotorNum_e mtrNum)
{
    float32_t Ls_d_H = userParams[mtrNum].motor_Ls_d_H;
    float32_t Ls_q_H = userParams[mtrNum].motor_Ls_q_H;
    float32_t Rs_Ohm = userParams[mtrNum].motor_Rs_Ohm;
    float32_t RdoverLd_rps = Rs_Ohm / Ls_d_H;
    float32_t RqoverLq_rps = Rs_Ohm / Ls_q_H;

    float32_t BWc_rps = userParams[mtrNum].BWc_rps;
    float32_t currentCtrlPeriod_sec =
                (float32_t)userParams[mtrNum].numCtrlTicksPerCurrentTick /
                userParams[mtrNum].ctrlFreq_Hz;

    float32_t outMax_V = userParams[mtrNum].Vd_sf *
            userParams[mtrNum].maxVsMag_V;

    float32_t Kp_Id = Ls_d_H * BWc_rps;
    float32_t Ki_Id = RdoverLd_rps * currentCtrlPeriod_sec;

    float32_t Kp_Iq = Ls_q_H * BWc_rps;
    float32_t Ki_Iq = RqoverLq_rps * currentCtrlPeriod_sec;

    // set the Id controller
    PI_setGains(piHandle_Id[mtrNum], Kp_Id, Ki_Id);
    PI_setUi(piHandle_Id[mtrNum], 0.0);
    PI_setRefValue(piHandle_Id[mtrNum], 0.0);
    PI_setFbackValue(piHandle_Id[mtrNum], 0.0);
    PI_setFfwdValue(piHandle_Id[mtrNum], 0.0);
    PI_setMinMax(piHandle_Id[mtrNum], -outMax_V, outMax_V);

    // set the Iq controller
    PI_setGains(piHandle_Iq[mtrNum], Kp_Iq, Ki_Iq);
    PI_setUi(piHandle_Iq[mtrNum], 0.0);
    PI_setRefValue(piHandle_Iq[mtrNum], 0.0);
    PI_setFbackValue(piHandle_Iq[mtrNum], 0.0);
    PI_setFfwdValue(piHandle_Iq[mtrNum], 0.0);
    PI_setMinMax(piHandle_Iq[mtrNum], 0.0, 0.0);

    // set the speed controller
    if(userParams[mtrNum].Kctrl_Wb_p_kgm2 <= 1.0e-7f)
    {
        PI_setGains(piHandle_spd[mtrNum], motorVars[mtrNum].Kp_spd,
                    motorVars[mtrNum].Ki_spd);
    }
    else
    {
        float32_t speedCtrlPeriod_sec =
            (float32_t)userParams[mtrNum].numCtrlTicksPerSpeedTick /
            userParams[mtrNum].ctrlFreq_Hz;

        float32_t BWdelta = userParams[mtrNum].BWdelta;

        float32_t Kctrl_Wb_p_kgm2 = userParams[mtrNum].Kctrl_Wb_p_kgm2;

        float32_t Kp_spd = MATH_TWO_PI * BWc_rps / (BWdelta * Kctrl_Wb_p_kgm2);
        float32_t Ki_spd = BWc_rps * speedCtrlPeriod_sec / (BWdelta * BWdelta);

        PI_setGains(piHandle_spd[mtrNum], Kp_spd, Ki_spd);
    }

    PI_setUi(piHandle_spd[mtrNum], 0.0);
    PI_setRefValue(piHandle_spd[mtrNum], 0.0);
    PI_setFbackValue(piHandle_spd[mtrNum], 0.0);
    PI_setFfwdValue(piHandle_spd[mtrNum], 0.0);
    PI_setMinMax(piHandle_spd[mtrNum],
                 -userParams[mtrNum].maxCurrent_A,
                 userParams[mtrNum].maxCurrent_A);

    // copy the Id, Iq and speed controller parameters to motorVars
    getControllers(mtrNum);

    return;
} // end of setupControllers() function

//------------------------------------------------------------------------------
#if (defined(MOTOR1_FAST) || defined(MOTOR2_FAST)) && !defined(_SIMPLE_FAST_LIB)
void runRsOnLine(EST_Handle handle, MotorNum_e mtrNum)
{
    // execute Rs OnLine code
    if(motorVars[mtrNum].flagRunIdentAndOnLine == true)
    {
        if((EST_getState(handle) == EST_STATE_ONLINE) &&
                (motorVars[mtrNum].flagEnableRsOnLine == true))
        {
            EST_setFlag_enableRsOnLine(handle, true);

            EST_setRsOnLineId_mag_A(handle, motorVars[mtrNum].RsOnLineCurrent_A);

            float32_t RsError_Ohm =
                    motorVars[mtrNum].RsOnLine_Ohm - motorVars[mtrNum].Rs_Ohm;

            if(fabsf(RsError_Ohm) < (motorVars[mtrNum].Rs_Ohm * 0.10f))
            {
              EST_setFlag_updateRs(handle, true);
            }
        }
        else
        {
            EST_setRsOnLineId_mag_A(handle, 0.0);
            EST_setRsOnLineId_A(handle, 0.0);
            EST_setRsOnLine_Ohm(handle, EST_getRs_Ohm(handle));

            EST_setFlag_enableRsOnLine(handle, false);
            EST_setFlag_updateRs(handle, false);
        }
    }

    return;
} // end of runRsOnLine() function
#endif // (MOTOR1_FAST || MOTOR2_FAST) && !(_SIMPLE_FAST_LIB)

#if defined(MOTOR1_VIBCOMPA)
void vibCompCtrl(VIB_COMP_Handle handle)
{
    VIB_COMP_setAlphaGain(handle, vibCompAlpha, vibCompGain);

    VIB_COMP_setAdvIndexDelta(handle, vibCompIndexDelta);

    if(vibCompFlagReset == true)
    {
        vibCompFlagReset = false;
        VIB_COMP_reset(handle);
    }
}
#endif // MOTOR1_VIBCOMPA

#if defined(MOTOR1_FAST) || defined(MOTOR2_FAST)
// update motor control variables
void updateGlobalVariables(EST_Handle handle, const MotorNum_e mtrNum)
{
    // get the states
#ifdef __TMS320C28XX_CLA__
    motorVars[mtrNum].estState = cla_EST_getState(handle);
#else
    motorVars[mtrNum].estState = EST_getState(handle);
#endif

    // get the stator resistance
#ifdef __TMS320C28XX_CLA__
    motorVars[mtrNum].Rs_Ohm = cla_EST_getRs_Ohm(handle);
#else
    motorVars[mtrNum].Rs_Ohm = EST_getRs_Ohm(handle);
#endif

    // get the stator inductance in the direct coordinate direction
#ifdef __TMS320C28XX_CLA__
    motorVars[mtrNum].Ls_d_H = cla_EST_getLs_d_H(handle);
#else
    motorVars[mtrNum].Ls_d_H = EST_getLs_d_H(handle);
#endif

    // get the stator inductance in the quadrature coordinate direction
#ifdef __TMS320C28XX_CLA__
    motorVars[mtrNum].Ls_q_H = cla_EST_getLs_q_H(handle);
#else
    motorVars[mtrNum].Ls_q_H = EST_getLs_q_H(handle);
#endif

    // get the flux, V/Hz
#ifdef __TMS320C28XX_CLA__
    motorVars[mtrNum].flux_VpHz = cla_EST_getFlux_Wb(handle) * MATH_TWO_PI;
#else
    motorVars[mtrNum].flux_VpHz = EST_getFlux_Wb(handle) * MATH_TWO_PI;
#endif

#if !defined(_SIMPLE_FAST_LIB)
    // get R/L
#ifdef __TMS320C28XX_CLA__
    motorVars[mtrNum].RoverL_rps = cla_EST_getRoverL_rps(handle);
#else
    motorVars[mtrNum].RoverL_rps = EST_getRoverL_rps(handle);
#endif

    // get the stator resistance estimate from RsOnLine
#ifdef __TMS320C28XX_CLA__
    motorVars[mtrNum].RsOnLine_Ohm = cla_EST_getRsOnLine_Ohm(handle);
#else
    motorVars[mtrNum].RsOnLine_Ohm = EST_getRsOnLine_Ohm(handle);
#endif
#endif  // !(_SIMPLE_FAST_LIB)

    // get the torque estimate
#ifdef __TMS320C28XX_CLA__
    motorVars[mtrNum].torque_Nm = cla_EST_computeTorque_Nm(handle);
#else
    motorVars[mtrNum].torque_Nm = EST_computeTorque_Nm(handle);
#endif

    motorVars[mtrNum].Is_A =
                sqrtf(Idq_in_A[mtrNum].value[0] * Idq_in_A[mtrNum].value[0] +
                      Idq_in_A[mtrNum].value[1] * Idq_in_A[mtrNum].value[1]);

    motorVars[mtrNum].Vs_V =
                sqrtf(Vdq_out_V[mtrNum].value[0] * Vdq_out_V[mtrNum].value[0] +
                      Vdq_out_V[mtrNum].value[1] * Vdq_out_V[mtrNum].value[1]);

    return;
} // end of updateGlobalVariables() function
#endif // MOTOR1_FAST || MOTOR2_FAST


#if defined(MOTOR1_ESMO) || defined(MOTOR2_ESMO)
// update motor control variables without using FAST
void updateGlobalVariablesNF(const MotorNum_e mtrNum)
{
    motorVars[mtrNum].Rs_Ohm = userParams[mtrNum].motor_Rs_Ohm;
    motorVars[mtrNum].Ls_d_H = userParams[mtrNum].motor_Ls_d_H;
    motorVars[mtrNum].Ls_q_H = userParams[mtrNum].motor_Ls_q_H;
    motorVars[mtrNum].flux_VpHz = userParams[mtrNum].motor_ratedFlux_Wb * MATH_TWO_PI;

    motorVars[mtrNum].Is_A =
                sqrtf(Idq_in_A[mtrNum].value[0] * Idq_in_A[mtrNum].value[0] +
                      Idq_in_A[mtrNum].value[1] * Idq_in_A[mtrNum].value[1]);

    motorVars[mtrNum].Vs_V =
                sqrtf(Vdq_out_V[mtrNum].value[0] * Vdq_out_V[mtrNum].value[0] +
                      Vdq_out_V[mtrNum].value[1] * Vdq_out_V[mtrNum].value[1]);

    return;
} // end of updateGlobalVariablesNF() function
#endif  // MOTOR1_ESMO || MOTOR2_ESMO

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