[学习笔记] AC7811-FOC无感控制代码详解

/*!

* [url=home.php?mod=space&uid=247401]@brief[/url] Pre_positioning motor to an expected angle.

*预定位到指定角度上  FOC的预定位不再是六步换相那样通电流，而设置iqRef

* @param[in] focVarsCtrl: pointer to FOC_VARS_CTRL structure

* @param[in] startUp: pointer to START_UP structure

* [url=home.php?mod=space&uid=266161]@return[/url] none

*/

void Angle_Prepositioning_Process(FOC_VARS_CTRL *focVarsCtrl, START_UP *startUp)

{

    static uint32_t s_timeBase = 0;

 

    ++s_timeBase;

    if (s_timeBase <= startUp->pStCfg->alignTime)

    {

        startUp->pStCtrl->startUpElecTheta = startUp->pStCfg->alignAngle;

        focVarsCtrl->elecAngle = startUp->pStCtrl->startUpElecTheta;

        focVarsCtrl->idRef = 0;

        if (s_timeBase <= startUp->pStCfg->slopeDuration1)

        {

            focVarsCtrl->iqRef = (startUp->pStCfg->curRamp1Init + Math_Mpy(s_timeBase, startUp->pStCtrl->curAddSlope1)) * pFocVarsCfg->motorDir;

            //iqRef，q轴参考电流

        }

        else

        {

            focVarsCtrl->iqRef = startUp->pStCfg->alignCur * pFocVarsCfg->motorDir;

        }

 

    }

    else

    {

#if (defined MOTOR_POLES_OBTAIN)

        startUp->pStCtrl->startUpFlag = 2;

#else

        s_timeBase = 0;

        startUp->pStCtrl->alignState = 1;

        startUp->pStCtrl->startUpFlag = 3;

#endif

    }

/*!

* @brief Speedloop variable PI parameters calculate, it divides the speed loop PI parameters into three sections

         according to the speed, low speed PI is used at low speed, high speed PI is used at high speed, and the

         intermediate speed is linearly transitioned.

*Speedloop变量PI参数计算，它根据速度将速度环路PI参数分为三个部分，低速时使用低速PI，高速时使用高速PI，中间速度线性过渡。

* @param[in] freq: motor electrical frequency pu value in Q15

* @param[in] asrVarPI: pointer to ASR_SPEEDVARPI structure

* @param[in] asrPidCof: pointer to PID_REGULATOR_COF structure

* @return none

*/

void ASR_SpeedVarPICalc(int16_t freq, ASR_SPEEDVARPI *asrVarPI, PID_REGULATOR_COF *asrPidCof)

{

    uint16_t s_absFre = 0;

    s_absFre = Math_Qabs(freq);

    if (s_absFre > asrVarPI->frepuH)

    {

        asrPidCof->**u = asrVarPI->kpH;

        asrPidCof->kiPu = asrVarPI->kiH;

    }

    else if (s_absFre > asrVarPI->frepuL)

    {

        asrPidCof->**u = asrVarPI->kpL + Math_Mpy((s_absFre - asrVarPI->frepuL), asrVarPI->kpGain);

        asrPidCof->kiPu = asrVarPI->kiL + Math_Mpy((s_absFre - asrVarPI->frepuL), asrVarPI->kiGain);

    }

    else

    {

        asrPidCof->**u = asrVarPI->kpL;

        asrPidCof->kiPu = asrVarPI->kiL;

    }

}

/*!

* @brief Phase over current protection. When the amplitude of the phase current

*        synthesis vector exceeds the threshold value, phase over current fault is reported.

*

* @param[in] focVars: pointer to FOC_VARS_CTRL structure

* @return status: fault diagnosis status, 0 OK; 1 fault; 2 unknown

*/

int16_t Phase_Over_Current_Check(FOC_VARS_CTRL *focVars)

{

    static int16_t cnta = 0;

    int16_t status = DIAGNOSTIC_OK;

    uint16_t statusTemp = 0;

 

    if (focVars->idq > MAX_IPHASE_THRESHOLD)

    {

        statusTemp |= 0x1;

        if (cnta < IPHASE_DBC)

        {

            cnta++;

        }

        else

        {

            cnta = 0;

            Fault_Report(OVER_CURRENT_PHASE_FAILURE);

        }

    }

    else

    {

        cnta = 0;

    }

 

    if (statusTemp != 0)

    {

        status = DIAGNOSTIC_FAIL;

    }

 

    return status;

}

/*!

* @brief Motor Lose Speed protection.

*

* @param[in] speed: speed feedback pu value

* @return none

*/

void LoseSpeed_Check(int16_t speed)

{

    static int16_t oldSpeed = 0;

    static uint16_t cntLoseSpeed = 0;

 

    if (g_startUpCtrl.startUpFlag > 3)

    {

        if ((Math_Qabs(speed) > LOSE_SPEED_THRESHOLD) || ((int16_t)Math_Mpy(pFocVarsCfg->motorDir, speed) < 0) || (Math_Qabs(speed - oldSpeed) > LOSE_SPEED_ERR_THRESHOLD))

        //转速超过设定阈值或当前转速与上一次转速差值大于设定阈值，且连续达到 10 次时上报失速故障（LOSE_SPEED_FAILURE）。

        {

            if (cntLoseSpeed < LOSE_SPEED_DBC)

            {

                cntLoseSpeed++;

            }

            else

            {

                cntLoseSpeed = 0;

                oldSpeed = 0;

                g_protector.loseSpeedProFlag = 1;

                #if (defined STARTUP_CHECK_RESTART_ENABLE)

                #else

                Fault_Report(LOSE_SPEED_FAILURE);//上报失速错误

                #endif

            }

        }

        else

        {

            cntLoseSpeed = 0;

//            Fault_Clear(LOSE_SPEED_FAILURE);

        }

        oldSpeed = speed;

    }

}

/*!

* @brief Motor Zero Speed protection.

*转速低于设定阈值，且连续达到 2 次时上报零速故障（ZERO_SPEED_FAILURE）。

* @param[in] none

* @return none

*/

void ZeroSpeed_Check(int16_t speed)

{

    static uint16_t s_zeroSpeedCnt = 0;

 

    if (g_startUpCtrl.startUpFlag >= 6)

    {

        if (Math_Qabs(speed) < ZERO_SPEED_THRESHOLD)

        {

            if (s_zeroSpeedCnt < ZERO_SPEED_DBC)

            {

                s_zeroSpeedCnt++;

            }

            else

            {

                s_zeroSpeedCnt = 0;

                g_protector.zeroSpeedProFlag = 1;

                #if (defined STARTUP_CHECK_RESTART_ENABLE)

                #else

                Fault_Report(ZERO_SPEED_FAILURE);

                #endif

            }

        }

        else

        {

            s_zeroSpeedCnt = 0;

//            Fault_Clear(ZERO_SPEED_FAILURE);

        }

    }

 

}