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

/*!

* @brief Motor stall protection detection for position sensorless application.

*

* @param[in] pHandle: pointer to SPEED_RAMP_HANDLE structure

* @param[in] smc: pointer to SMC_ESTIMATOR structure

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

*/

int16_t Stall_Check_Sensorless(SPEED_RAMP_HANDLE *pHandle, SMC_ESTIMATOR *smc)

{

    int16_t status = DIAGNOSTIC_OK;

 

    static int16_t bemfReliableCnt = 0;

    int32_t avgSquareSpeed = 0;

 

    int32_t bemfReliableThreshold = 0;

    #if ((defined FLUX_OBSERVE) || (defined MRAS_OBSERVE))

    int32_t estBemfSq = 0;

    #endif

 

    if ((g_startUpCtrl.startUpFlag >= 3) && (g_startUpCtrl.startUpFlag < 6))

    {

        //ASR_AvgSpeedCalc(pHandle);

        avgSquareSpeed = (int32_t)(pHandle->avgSpeedFbk) * (int32_t)(pHandle->avgSpeedFbk);

        bemfReliableThreshold = avgSquareSpeed;

        g_bemfReliableThreshold = bemfReliableThreshold;

        #if (defined SMC_OBSERVE)

        Smc_GetBemfSq(smc);

//        g_estBemfDebug = (smc->estBemfSq * BEMF_CONSISTENT_THRESHOLD) >> 4;

        if ((((smc->estBemfSq * BEMF_CONSISTENT_THRESHOLD) >> 4) < bemfReliableThreshold))

        {

            if (++bemfReliableCnt >= BEMF_DBC)

            {

                status = DIAGNOSTIC_FAIL;

                bemfReliableCnt = 0;

                g_protector.stallProFlag = 1;

                #if (defined STARTUP_CHECK_RESTART_ENABLE)

                #else

                Fault_Report(MOTOR_STALL_FAILURE);

                #endif

            }

        }

        else

        {

            bemfReliableCnt=0;

        }

        #elif (defined FLUX_OBSERVE)//磁链观测

        estBemfSq = (g_fluxObserver.emfAlpha * g_fluxObserver.emfAlpha + g_fluxObserver.emfBeta * g_fluxObserver.emfBeta) >> 8;

        g_estBemfDebug = estBemfSq * BEMF_CONSISTENT_THRESHOLD;

        if (((estBemfSq * BEMF_CONSISTENT_THRESHOLD) < bemfReliableThreshold) && (pHandle->varianceSpeed < 500000))

            //保护方差与反电动势

        {

            if (++bemfReliableCnt >= BEMF_DBC)

            {

                status = DIAGNOSTIC_FAIL;

                bemfReliableCnt = 0;

                g_protector.stallProFlag = 1;

                #if (defined STARTUP_CHECK_RESTART_ENABLE)

                #else

                Fault_Report(MOTOR_STALL_FAILURE);

                #endif

            }

        }

        else

        {

            bemfReliableCnt = 0;

        }

        #elif (defined MRAS_OBSERVE)

        estBemfSq = (g_mrasObserver.vdFilter * g_mrasObserver.vdFilter + g_mrasObserver.vqFilter * g_mrasObserver.vqFilter) >> 8;

//        g_estBemfDebug = estBemfSq * BEMF_CONSISTENT_THRESHOLD;

        if (((estBemfSq * BEMF_CONSISTENT_THRESHOLD) < bemfReliableThreshold) && (pHandle->varianceSpeed < 500000))

        {

            if (++bemfReliableCnt >= BEMF_DBC)

            {

                status = DIAGNOSTIC_FAIL;

                bemfReliableCnt = 0;

                g_protector.stallProFlag = 1;

                #if (defined STARTUP_CHECK_RESTART_ENABLE)

                #else

                Fault_Report(MOTOR_STALL_FAILURE);

                #endif

            }

        }

        else

        {

            bemfReliableCnt = 0;

        }

        #endif

    }

    else

    {

//        g_estBemfDebug = 0;

//        g_bemfReliableThreshold = 0;

    }

 

    return status;

}

#if (CUR_SAMPLE_MODE == THREE_SHUNT_SAMPLE) //电流传感器采样零点漂移检查，电流采样ADC零漂移值超过限制，这是故障。

    /* 2048 *5% = 102 */

    if (Math_Qabs(adcSampleReal->icOffset - 2048) > 102)

    {

        Fault_Report(PHASEC_CURRENT_MIDPOINT_FAILURE);

    }

    else

    {

        Fault_Clear(PHASEC_CURRENT_MIDPOINT_FAILURE);

    }

#else

#endif