//#############################################################################
// $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/include/motor_common.h
//!
//! \brief  header file to be included in all labs
//!         support for HVAC reference board with F28002x/F28003x/F280013x
//!         This file is shared with all of device
//!

#ifndef MOTOR_COMMON_H
#define MOTOR_COMMON_H


//*****************************************************************************
//
// If building with a C++ compiler, make all of the definitions in this header
// have a C binding.
//
//*****************************************************************************
#ifdef __cplusplus
extern "C"
{
#endif

//*****************************************************************************
//
//! \defgroup MOTOR_COMMON MOTOR_COMMON
//! @{
//
//*****************************************************************************

// includes
#include "libraries/math/include/math.h"
#include <math.h>

#include "userParams.h"

#include "clarke.h"
#include "est.h"
#include "filter_fo.h"
#include "ipark.h"
#include "park.h"
#include "pi.h"
#include "svgen.h"
#include "svgen_current.h"
#include "traj.h"
#include "mtpa.h"

#include "vs_freq.h"
#include "angle_gen.h"

#include "volt_recons.h"
#include "esmo.h"
#include "speedfr.h"

#include "vib_comp.h"

#if defined(MOTOR1_SSIPD)
#include "ssipd.h"
#endif // MOTOR1_SSIPD

#if defined(MOTOR1_DCLINKSS) || defined(MOTOR2_DCLINKSS)
#include "dclink_ss.h"
#endif // MOTOR1_DCLINKSS || MOTOR2_DCLINKSS

#include "DCL.h"
#include "DCLF32.h"

// solutions
#include "user.h"
#include "hal.h"


#if defined(SFRA_ENABLE)
#include "sfra_settings.h"
#endif  // SFRA_ENABLE

//*****************************************************************************
#define M_OVER_VOLTAGE_BIT          0x0001    // DC Bus Over Voltage Fault
#define M_UNDER_VOLTAGE_BIT         0x0002    // DC Bus Under Voltage Fault
#define M_MOTOR_OVER_TEMPER_BIT     0x0004    // Motor over temperature Fault
#define M_MODULE_OVER_TEMPER_BIT    0x0008    // Module over temperature Fault

#define M_MODULE_OVER_CURRENT_BIT   0x0010    // Hardware Over Current Fault
#define M_OVER_PEAK_CURRENT_BIT     0x0020    // internal CMPSS Over Current Fault
#define M_MOTOR_OVER_LOAD_BIT       0x0040    // Over Load Error
#define M_MOTOR_LOST_PHASE_BIT      0x0080    // Motor Lost Phase

#define M_CURRENT_UNBALANCE_BIT     0x0100    // Motor Phase Current Unbalance
#define M_MOTOR_STALL_BIT           0x0200    // Motor Stall
#define M_STARTUP_FAILE_BIT         0x0400    // Startup failed
#define M_MOTOR_OVER_SPEED_BIT      0x0800    // Motor Over Speed

#define M_RESERVE_12_BIT            0x1000    // Reserved for further use
#define M_RESERVE_13_BIT            0x2000    // Reserved for further use
#define M_CURRENT_OFFSET_BIT        0x4000    // Current offsets
#define M_VOLTAGE_OFFSET_BIT        0x8000    // voltage offsets

#define M_MASK_ALL_FAULT_BITS       0x0000
#define M_ENABLE_ALL_FAULT_BITS     0xFFFF

#define M_OVERCURRENT_BITS          M_MODULE_OVER_CURRENT_BIT                  \
                                  + M_OVER_PEAK_CURRENT_BIT

#define M_RUN_FAILED_BITS           M_MOTOR_OVER_LOAD_BIT                      \
                                  + M_MOTOR_LOST_PHASE_BIT                     \
                                  + M_CURRENT_UNBALANCE_BIT                    \
                                  + M_MOTOR_STALL_BIT                          \
                                  + M_STARTUP_FAILE_BIT                        \
                                  + M_MOTOR_OVER_SPEED_BIT

#define M_HARDWARE_BITS             M_CURRENT_OFFSET_BIT                       \
                                  + M_VOLTAGE_OFFSET_BIT
//
// Block all fault protection except current, voltage and temperature faults
//
#define MTR_FAULT_ENABLE_OC             M_MODULE_OVER_CURRENT_BIT              \
                                      + M_OVER_PEAK_CURRENT_BIT

#define MTR_FAULT_ENABLE_OC_OUV         M_OVER_VOLTAGE_BIT                     \
                                      + M_UNDER_VOLTAGE_BIT                    \
                                      + M_MODULE_OVER_CURRENT_BIT              \
                                      + M_OVER_PEAK_CURRENT_BIT                \
                                      + M_CURRENT_OFFSET_BIT                   \
                                      + M_VOLTAGE_OFFSET_BIT
//
// Enable all fault protection
//
#define MTR_FAULT_ENABLE_ALL            M_OVER_VOLTAGE_BIT                     \
                                      + M_UNDER_VOLTAGE_BIT                    \
                                      + M_MOTOR_OVER_TEMPER_BIT                \
                                      + M_MODULE_OVER_TEMPER_BIT               \
                                      + M_MODULE_OVER_CURRENT_BIT              \
                                      + M_OVER_PEAK_CURRENT_BIT                \
                                      + M_MOTOR_OVER_LOAD_BIT                  \
                                      + M_MOTOR_LOST_PHASE_BIT                 \
                                      + M_CURRENT_UNBALANCE_BIT                \
                                      + M_MOTOR_STALL_BIT                      \
                                      + M_STARTUP_FAILE_BIT                    \
                                      + M_MOTOR_OVER_SPEED_BIT                 \
                                      + M_CURRENT_OFFSET_BIT                   \
                                      + M_VOLTAGE_OFFSET_BIT

// Clear all fault protection except over/under voltage and offset error
//
#define MTR_FAULT_CLEAR                 M_MODULE_OVER_TEMPER_BIT               \
                                      + M_MODULE_OVER_CURRENT_BIT              \
                                      + M_OVER_PEAK_CURRENT_BIT                \
                                      + M_MOTOR_OVER_LOAD_BIT                  \
                                      + M_MOTOR_LOST_PHASE_BIT                 \
                                      + M_CURRENT_UNBALANCE_BIT                \
                                      + M_MOTOR_STALL_BIT                      \
                                      + M_STARTUP_FAILE_BIT                    \
                                      + M_MOTOR_OVER_SPEED_BIT

#define MTR_FAULT_DISABLE_ALL       0x0000

#if (DMC_BUILDLEVEL <= DMC_LEVEL_2)
#define MTR1_FAULT_MASK_SET         MTR_FAULT_ENABLE_OC
#define MTR2_FAULT_MASK_SET         MTR_FAULT_ENABLE_OC
#else   // (DMC_BUILDLEVEL >= DMC_LEVEL_3)
//#define MTR1_FAULT_MASK_SET         MTR_FAULT_ENABLE_ALL
//#define MTR2_FAULT_MASK_SET         MTR_FAULT_ENABLE_ALL

#define MTR1_FAULT_MASK_SET         MTR_FAULT_ENABLE_OC_OUV
#define MTR2_FAULT_MASK_SET         MTR_FAULT_ENABLE_OC_OUV

#endif  // (DMC_BUILDLEVEL > DMC_LEVEL_2)

//------------------------------------------------------------------------------
//
//! \brief Enumeration for the kit boards
//
typedef enum
{
    BOARD_HVMTRPFC_REV1P1  = 0,         //!< the board is HVMTRPFC_Rev1p1
    BOARD_BSXL8323RH_REVB  = 1,         //!< the board is BOOSTXL_8323RH
    BOARD_DMCPFC_REV3P1    = 5,         //!< the board is DMCPFC_Rev3.1
    BOARD_DMCPFC_REV3P2    = 6          //!< the board is DMCPFC_Rev4.2
} Board_Kit_e;

//------------------------------------------------------------------------------
//
//! \brief Enumeration for the using estimator algorithm
//
typedef enum
{
    EST_TYPE_FAST      = 0,         //!< the estimator is only FAST
    EST_TYPE_ESMO      = 1,         //!< the estimator is only ESMO
    EST_TYPE_FAST_ESMO = 6,         //!< the estimator is FAST and ESMO
    EST_TYPE_NO_OBSERVER = 11       //!< Not select a right observer
} EST_Type_e;

//
//! \brief Enumeration for the motor control state
//
typedef enum
{
    MCD_INIT  = 0,         //!< Initialize run time parameters
    MCD_IDLE  = 1,         //!< Idle wait for
    MCD_START = 2,         //!< start the motor
    MCD_RUN   = 3,         //!< run the motor
    MCD_STOP  = 4,         //!< stop the motor
    MCD_FAULT = 5          //!< motor is in the fault mode
} MCDRIVE_State_e;

typedef enum
{
    ESTIMATOR_MODE_FAST  = 0,             //!< FAST estimator
    ESTIMATOR_MODE_ESMO  = 1              //!< ESMO estimator
} ESTIMATOR_Mode_e;

typedef enum
{
    CURSEN_TYPE_SINGLE_SHUNT  = 0,   //!< the single shunt
    CURSEN_TYPE_THREE_SHUNT   = 1,   //!< the three shunt
    CURSEN_TYPE_NO_SENSOR     = 5    //!< Not select a right current sensor
} CURRENTSEN_Type_e;

//
//! \brief typedefs for the fault
//
typedef struct _FAULT_MTR_BITS_
{             // bits  description
    uint16_t overVoltage:1;         // 0  DC Bus Over Voltage Fault
    uint16_t underVoltage:1;        // 1  DC Bus Under Voltage Fault
    uint16_t motorOverTemp:1;       // 2  Motor over temperature Fault
    uint16_t moduleOverTemp:1;      // 3  Power module over temperature Fault

    uint16_t moduleOverCurrent:1;   // 4  Hardware Over Current Fault Flag
    uint16_t overPeakCurrent:1;     // 5  internal CMPSS Over Current Fault Flag
    uint16_t overLoad:1;            // 6  Over Load Error
    uint16_t motorLostPhase:1;      // 7  Motor Lost Phase

    uint16_t currentUnbalance:1;    // 8  Motor Phase Current imbalance
    uint16_t motorStall:1;          // 9  Motor Stall
    uint16_t startupFailed:1;       // 10 Startup failed
    uint16_t overSpeed:1;           // 11 Motor Over Speed

    uint16_t reserve12:1;           // 12 Reserved
    uint16_t reserve13:1;           // 13 Reserved
    uint16_t currentOffset:1;       // 14 Current offset check
    uint16_t voltageOffset:1;       // 15 voltage offset check
} FAULT_MTR_BITS;

typedef union _FAULT_MTR_t
{
    uint16_t        all;
    FAULT_MTR_BITS  bit;
}FAULT_MTR_t;

//******************************************************************************
// typedefs
typedef struct _MOTOR_Vars_t_
{
    float32_t speedRef_Hz;
    float32_t speedTraj_Hz;
    float32_t speed_Hz;
    float32_t speedAbs_Hz;
    float32_t speedStart_Hz;
    float32_t speedForce_Hz;

    float32_t accelerationMax_Hzps;
    float32_t accelerationStart_Hzps;

    float32_t overModulation;

#if !defined(_SIMPLE_FAST_LIB)
    float32_t RsOnLineCurrent_A;
    float32_t RsOnLine_Ohm;
    float32_t RoverL_rps;
#endif  // !(_SIMPLE_FAST_LIB)

    float32_t Rs_Ohm;
    float32_t Ls_d_H;
    float32_t Ls_q_H;
    float32_t flux_VpHz;
    float32_t flux_Wb;
    float32_t torque_Nm;

    float32_t Kp_spd;
    float32_t Ki_spd;

    float32_t Kp_Id;
    float32_t Ki_Id;

    float32_t Kp_Iq;
    float32_t Ki_Iq;

    float32_t Vs_V;
    float32_t Is_A;

    float32_t IdRated_A;
    float32_t IsRef_A;
    MATH_Vec2 IdqRef_A;

    float32_t currentInv_sf;
    float32_t overCurrent_A;
    float32_t startCurrent_A;
    float32_t alignCurrent_A;
    float32_t maxCurrent_A;

    float32_t angleDelayed_sf;

    uint32_t ISRCount;

#ifdef DEBUG_MONITOR_EN
    float32_t speedMax_Hz;
    float32_t speedMin_Hz;
    float32_t speedDelta_Hz;
    uint16_t  flagClearRecord;
#endif  // DEBUG_MONITOR_EN

    uint16_t  dacCMPValH;
    uint16_t  dacCMPValL;

    uint16_t stateRunTimeCnt;
    uint16_t forceRunTimeDelay;
    uint16_t lockRotorTimeDelay;
    uint16_t fwcTimeDelay;

    FAULT_MTR_t faultMtrNow;
    FAULT_MTR_t faultMtrUse;
    FAULT_MTR_t faultMtrMask;
    FAULT_MTR_t faultMtrPrev;

    EST_State_e estState;
    ESTIMATOR_Mode_e estimatorMode;
    MOTOR_Status_e motorState;

    MotorNum_e     motorNum;

    bool flagEnableRunAndIdentify;
    bool flagRunIdentAndOnLine;
    bool flagMotorIdentified;
    bool flagEnableForceAngle;

    bool flagEnableRsRecalc;

#if !defined(_SIMPLE_FAST_LIB)
    bool flagEnableRsOnLine;
#endif  // !_SIMPLE_FAST_LIB

    bool flagEnableUserParams;
    bool flagEnableOffsetCalc;

    bool flagSetupController;
    bool flagEnableSpeedCtrl;
    bool flagEnableFlyingStart;

    bool flagClearFaults;
}MOTOR_Vars_t;


typedef struct _MOTOR_CtrlVars_t_
{
    float32_t VIrmsIsrScale;
    float32_t IrmsCalSF;

    float32_t IrmsCalSum[3];
    float32_t IrmsPrdSum[3];
    float32_t Irms_A[3];

    float32_t unbalanceRatio;

    float32_t lostPhaseSet_A;
    float32_t unbalanceRatioSet;
    float32_t stallCurrentSet_A;

    float32_t IsFailedChekSet_A;

    float32_t speedFailMaxSet_Hz;
    float32_t speedFailMinSet_Hz;

    uint16_t VIrmsIsrSet;
    uint16_t VIrmsIsrCnt;

    uint16_t motorStallTimeSet;
    uint16_t unbalanceTimeSet;
    uint16_t lostPhaseTimeSet;
    uint16_t overSpeedTimeSet;
    uint16_t startupFailTimeSet;

    uint16_t overCurrentTimesSet;

    uint16_t motorStallTimeCnt;
    uint16_t unbalanceTimeCnt;
    uint16_t lostPhaseTimeCnt;
    uint16_t overSpeedTimeCnt;
    uint16_t startupFailTimeCnt;

    uint16_t overCurrentTimesCnt;

    uint16_t stopWaitTimeSet;
    uint16_t restartWaitTimeSet;
    uint16_t restartTimesSet;

    uint16_t stopWaitTimeCnt;
    uint16_t restartTimesCnt;

    bool flagVIrmsCal;
} MOTOR_CtrlVars_t;

//*****************************************************************************
extern volatile MOTOR_Vars_t motorVars[2];
extern volatile MOTOR_CtrlVars_t motorCtrlVars[2];

extern uint16_t counterSpeed[2];
extern uint16_t counterTrajSpeed[2];

extern volatile uint16_t mtr1PIEIER;
extern volatile uint16_t mtr2PIEIER;

extern USER_Params  userParams[2];

extern HAL_ADCData_t adcData[2];
extern HAL_PWMData_t pwmData[2];

extern MATH_Vec2 Idq_in_A[2];
extern MATH_Vec2 Idq_ref_A[2];

extern MATH_Vec2 Vab_out_V[2];
extern MATH_Vec2 Vdq_out_V[2];

extern CLARKE_Handle clarkeHandle_I[2];
extern CLARKE_Obj    clarke_I[2];

extern CLARKE_Handle clarkeHandle_V[2];
extern CLARKE_Obj    clarke_V[2];

extern IPARK_Handle iparkHandle_V[2];
extern IPARK_Obj    ipark_V[2];

extern PARK_Handle  parkHandle_V[2];
extern PARK_Obj     park_V[2];

extern PI_Handle    piHandle_Id[2];
extern PI_Obj       pi_Id[2];

extern PARK_Handle   parkHandle_I[2];
extern PARK_Obj      park_I[2];

extern PI_Handle    piHandle_Iq[2];
extern PI_Obj       pi_Iq[2];

extern PI_Handle    piHandle_spd[2];
extern PI_Obj       pi_spd[2];

extern SVGEN_Handle  svgenHandle[2];
extern SVGEN_Obj     svgen[2];

extern SVGENCURRENT_Handle svgencurrentHandle;
extern SVGENCURRENT_Obj svgencurrent;

extern TRAJ_Handle   trajHandle_spd[2];
extern TRAJ_Obj      traj_spd[2];

#if(DMC_BUILDLEVEL == DMC_LEVEL_2)
extern VS_FREQ_Handle VsFreqHandle[2];
extern VS_FREQ_Obj    VsFreq[2];
#endif // (DMC_BUILDLEVEL == DMC_LEVEL_2)

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

#if defined(SFRA_ENABLE)
extern float32_t   sfraNoiseVpfc;
extern float32_t   sfraNoiseIpfc;
extern float32_t   sfraNoiseId;
extern float32_t   sfraNoiseIq;
extern float32_t   sfraNoiseSpd;
extern float32_t   sfraNoiseOut;
extern float32_t   sfraNoiseFdb;
extern float32_t   sfraNoiseInj_pu;
extern SFRA_TEST_e sfraTestLoop;
extern bool        sfraCollectStart;
#endif  // SFRA_ENABLE

//*****************************************************************************
// the function prototypes

#if !defined(MOTOR1_DCLINKSS) || !defined(MOTOR2_DCLINKSS)
//! \brief calculate motor over current threshold
static inline void calcMotorOverCurrentThreshold(const MotorNum_e mtrNum)
{
    int16_t cmpValue = (int16_t)(motorVars[mtrNum].overCurrent_A *
                                 motorVars[mtrNum].currentInv_sf);

    if(cmpValue <= 2047)
    {
        motorVars[mtrNum].dacCMPValH = 2048 + cmpValue;
        motorVars[mtrNum].dacCMPValL = 2048 - cmpValue;
    }
    else
    {
        motorVars[mtrNum].dacCMPValH = 4065;
        motorVars[mtrNum].dacCMPValL = 30;
    }

    return;
}
#endif  // !MOTOR1_DCLINKSS | !MOTOR2_DCLINKSS

#if defined(MOTOR1_DCLINKSS) || defined(MOTOR2_DCLINKSS)
//! \brief calculate motor1 over current threshold with single shunt
static inline void calcMotorDCLinkOverCurrentThreshold(const MotorNum_e mtrNum)
{
    int16_t cmpValue = (int16_t)(motorVars[mtrNum].overCurrent_A *
                                 motorVars[mtrNum].currentInv_sf);
    int16_t adcOffset = (int16_t)(fabsf(adcData[mtrNum].offset_Idc_A) *
                                  motorVars[mtrNum].currentInv_sf);

    motorVars[mtrNum].dacCMPValH = cmpValue;

    if(cmpValue < adcOffset)
    {
        motorVars[mtrNum].dacCMPValL = adcOffset - cmpValue;
    }
    else
    {
        motorVars[mtrNum].dacCMPValL = adcOffset;
    }

    return;
}
#endif  // MOTOR1_DCLINKSS | MOTOR2_DCLINKSS

//! \brief     Sets the number of current sensors
//! \param[in] handle             The Clarke (CLARKE) handle
//! \param[in] numCurrentSensors  The number of current sensors
static inline void
setupClarke_I(CLARKE_Handle handle, const uint16_t numCurrentSensors)
{
    float32_t alpha_sf, beta_sf;

    // initialize the Clarke transform module for current
    if(3 == numCurrentSensors)
    {
        alpha_sf = MATH_ONE_OVER_THREE;
        beta_sf = MATH_ONE_OVER_SQRT_THREE;
    }
    else if(2 == numCurrentSensors)
    {
        alpha_sf = 1.0;
        beta_sf = MATH_ONE_OVER_SQRT_THREE;
    }
    else
    {
        alpha_sf = 0.0;
        beta_sf = 0.0;
    }

    // set the parameters
    CLARKE_setScaleFactors(handle, alpha_sf, beta_sf);
    CLARKE_setNumSensors(handle, numCurrentSensors);

    return;
} // end of setupClarke_I() function

//! \brief     Sets the number of voltage sensors
//! \param[in] handle             The Clarke (CLARKE) handle
//! \param[in] numVoltageSensors  The number of voltage sensors
static inline void
setupClarke_V(CLARKE_Handle handle,const uint16_t numVoltageSensors)
{
    float32_t alpha_sf,beta_sf;

    // initialize the Clarke transform module for voltage
    if(3 == numVoltageSensors)
    {
        alpha_sf = MATH_ONE_OVER_THREE;
        beta_sf = MATH_ONE_OVER_SQRT_THREE;
    }
    else
    {
        alpha_sf = 0.0;
        beta_sf = 0.0;
    }

    // set the parameters
    CLARKE_setScaleFactors(handle, alpha_sf, beta_sf);
    CLARKE_setNumSensors(handle, numVoltageSensors);

    return;
} // end of setupClarke_V() function


//! \brief  Update the controllers
static inline void updateControllers(const MotorNum_e mtrNum)
{
    if((motorVars[mtrNum].estState == EST_STATE_ONLINE)
            && (motorVars[mtrNum].flagMotorIdentified == true))
    {
        // update the Id controller
        PI_setGains(piHandle_Id[mtrNum],
                    motorVars[mtrNum].Kp_Id, motorVars[mtrNum].Ki_Id);

        // update the Iq controller
        PI_setGains(piHandle_Iq[mtrNum],
                    motorVars[mtrNum].Kp_Iq, motorVars[mtrNum].Ki_Iq);

        // update the speed controller
        PI_setGains(piHandle_spd[mtrNum],
                    motorVars[mtrNum].Kp_spd, motorVars[mtrNum].Ki_spd);
    }
}


//! \brief  Get the controllers Parameters
static inline void getControllers(const MotorNum_e mtrNum)
{
    // Get the Id controller parameters
    motorVars[mtrNum].Kp_Id = PI_getKp(piHandle_Id[mtrNum]);
    motorVars[mtrNum].Ki_Id = PI_getKi(piHandle_Id[mtrNum]);

    // Get the Iq controller parameters
    motorVars[mtrNum].Kp_Iq = PI_getKp(piHandle_Iq[mtrNum]);
    motorVars[mtrNum].Ki_Iq = PI_getKi(piHandle_Iq[mtrNum]);

    // Get the speed controller parameters
    motorVars[mtrNum].Kp_spd = PI_getKp(piHandle_spd[mtrNum]);
    motorVars[mtrNum].Ki_spd = PI_getKi(piHandle_spd[mtrNum]);
}

//! \brief  Sets up control parameters for stopping motor
extern void stopMotorControl(MotorNum_e mtrNum);

//! \brief  Sets up control parameters for restarting motor
extern void restartMotorControl(MotorNum_e mtrNum);

//! \brief  Resets motor control parameters for restarting motor
extern void resetMotorControl(MotorNum_e mtrNum);

//! \brief  Calculate the RMS data
extern void calculateRMSData(MotorNum_e mtrNum);

//! \brief  Collect the current and voltage data to calculate the RMS
inline void collectRMSData(MotorNum_e mtrNum)
{
    motorCtrlVars[mtrNum].IrmsCalSum[0] +=
            adcData[mtrNum].I_A.value[0] * adcData[mtrNum].I_A.value[0];
    motorCtrlVars[mtrNum].IrmsCalSum[1] +=
            adcData[mtrNum].I_A.value[1] * adcData[mtrNum].I_A.value[1];
    motorCtrlVars[mtrNum].IrmsCalSum[2] +=
            adcData[mtrNum].I_A.value[2] * adcData[mtrNum].I_A.value[2];

    motorCtrlVars[mtrNum].VIrmsIsrCnt++;

    if(motorCtrlVars[mtrNum].VIrmsIsrCnt > motorCtrlVars[mtrNum].VIrmsIsrSet)
    {
        motorCtrlVars[mtrNum].IrmsPrdSum[0] = motorCtrlVars[mtrNum].IrmsCalSum[0];
        motorCtrlVars[mtrNum].IrmsPrdSum[1] = motorCtrlVars[mtrNum].IrmsCalSum[1];
        motorCtrlVars[mtrNum].IrmsPrdSum[2] = motorCtrlVars[mtrNum].IrmsCalSum[2];

        motorCtrlVars[mtrNum].IrmsCalSum[0] = 0;
        motorCtrlVars[mtrNum].IrmsCalSum[1] = 0;
        motorCtrlVars[mtrNum].IrmsCalSum[2] = 0;

        motorCtrlVars[mtrNum].VIrmsIsrCnt = 0;
        motorCtrlVars[mtrNum].flagVIrmsCal = true;
    }

    return;
}

//! \brief  Torque ripple control
extern void vibCompCtrl(VIB_COMP_Handle handle);

//! \brief  Sets up the current controllers
extern void setupCurrentControllers(const MotorNum_e mtrNum);

//! \brief  Sets up the controllers
extern void setupControllers(const MotorNum_e mtrNum);

//! \brief runs offset calculation using filters
extern void runMotor1OffsetsCalculation(void);

//! \brief runs offset calculation using filters
extern void runMotor2OffsetsCalculation(void);

//! \brief run motor monitor in main loop timer
extern void runMotorMonitor(MotorNum_e mtrNum);

#if (defined(MOTOR1_FAST) || defined(MOTOR2_FAST)) && !defined(_SIMPLE_FAST_LIB)
//! \brief Rs online calibration
extern void runRsOnLine(EST_Handle handle, MotorNum_e mtrNum);
#endif // (MOTOR1_FAST || MOTOR2_FAST) && !_SIMPLE_FAST_LIB

#if (defined(MOTOR1_FAST) || defined(MOTOR2_FAST))
//! \brief      Updates the global motor variables
//! \param[in]  estHandle   The estimator (EST) handle
//! \param[in]  mtrNum      The motor num
extern void updateGlobalVariables(EST_Handle handle, const MotorNum_e mtrNum);
#endif // (MOTOR1_FAST || MOTOR2_FAST)

#if defined(MOTOR1_ESMO) || defined(MOTOR2_ESMO)
//! \brief      Updates the global motor variables without using FAST
//! \param[in]  mtrNum      The motor num
extern void updateGlobalVariablesNF(const MotorNum_e mtrNum);
#endif  // MOTOR1_ESMO || MOTOR2_ESMO

//*****************************************************************************
//
// Close the Doxygen group.
//! @}
//
//*****************************************************************************

//*****************************************************************************
//
// Mark the end of the C bindings section for C++ compilers.
//
//*****************************************************************************
#ifdef __cplusplus
}
#endif // extern "C"

#endif // end of MOTOR_COMMON_H definition
