• GitHub - AN14099: PMSM Sensorless FOC Using MCXA153

• 
• FRDM Development Board for MCX A14x/A15x MCUs
• MCXA153, A152, A143, A142, A133, A132 Data Sheet
• 
• PMSM Control Code

  • /*
     * Copyright 2016, Freescale Semiconductor, Inc.
     * Copyright 2016-2021 NXP
     * All rights reserved.
     *
     * SPDX-License-Identifier: BSD-3-Clause
     */
    
    #include "pmsm_control.h"
    
    /*******************************************************************************
     * Definitions
     ******************************************************************************/
    
    /*******************************************************************************
     * Prototypes
     ******************************************************************************/
    
    /*******************************************************************************
     * Variables
     ******************************************************************************/
    
    /*******************************************************************************
     * Code
     ******************************************************************************/
    
    /*!
     * @brief PMSM Openloop control.
     *
     * This function is used for Openloop control mode.
     *
     * @param psOpenloop  The pointer of the Openloop structure
     *
     * @return None
     */
    void MCS_PMSMOpenloop(mcs_openloop_t *psOpenloop){
    	psOpenloop->f16PosElExt = MLIB_Add_F16(
    		psOpenloop->f16Theta,
    		GFLIB_Integrator_F16(
                psOpenloop->f16FreqReq,
    			&psOpenloop->sFreqIntegrator
    		)
    	);
    
    }
    
    /*!
     * @brief PMSM field oriented current control.
     *
     * This function is used to compute PMSM field oriented current control.
     *
     * @param psFocPMSM     The pointer of the PMSM FOC structure
     *
     * @return None
     */
    void MCS_PMSMFocCtrl(mcs_pmsm_foc_t *psFocPMSM)
    {
        /* pass electrical position from outside function if enabled else estimated
         * electrical position is selected */
        if (psFocPMSM->bPosExtOn)
        {
            psFocPMSM->f16PosEl = psFocPMSM->f16PosElExt;
        }
        else
        {
            psFocPMSM->f16PosEl = psFocPMSM->f16PosElEst;
        }
    
        /* Position angle of the last PWM update */
        psFocPMSM->sAnglePosEl.f16Sin = GFLIB_Sin_F16(psFocPMSM->f16PosElEst);
        psFocPMSM->sAnglePosEl.f16Cos = GFLIB_Cos_F16(psFocPMSM->f16PosElEst);
    
        /* 3-phase to 2-phase transformation to stationary ref. frame */
        GMCLIB_Clark_F16(&psFocPMSM->sIABC, &psFocPMSM->sIAlBe);
    
        /* 2-phase to 2-phase transformation to rotary ref. frame */
        GMCLIB_Park_F16(&psFocPMSM->sIAlBe, &psFocPMSM->sAnglePosEl, &psFocPMSM->sIDQ);
        GMCLIB_Park_F16(&psFocPMSM->sUAlBeReq, &psFocPMSM->sAnglePosEl, &psFocPMSM->sUDQEst);
    
        /* BEMF observer in DQ system */
        psFocPMSM->sBemfObsrv.f16Error = AMCLIB_PMSMBemfObsrvDQ_F16(&psFocPMSM->sIDQ, &psFocPMSM->sUDQEst, psFocPMSM->f16SpeedElEst, &psFocPMSM->sBemfObsrv);
    
        /* Tracking observer calculation */
        psFocPMSM->f16PosElEst = AMCLIB_TrackObsrv_F16(psFocPMSM->sBemfObsrv.f16Error, &psFocPMSM->sTo);
    
        /* Speed estimation filter  */
        psFocPMSM->f16SpeedElEst =
            GDFLIB_FilterIIR1_F16(MLIB_Conv_F16l(psFocPMSM->sTo.f32Speed), &psFocPMSM->sSpeedElEstFilt);
    
        /* for open loop control enabled parallel running of observer and FOC
         * open loop electrical position passed to rest of FOC */
        if (psFocPMSM->bOpenLoop)
        {
            psFocPMSM->sAnglePosEl.f16Sin = GFLIB_Sin_F16(psFocPMSM->f16PosEl);
            psFocPMSM->sAnglePosEl.f16Cos = GFLIB_Cos_F16(psFocPMSM->f16PosEl);
            GMCLIB_Park_F16(&psFocPMSM->sIAlBe, &psFocPMSM->sAnglePosEl, &psFocPMSM->sIDQ);
        }
    
        /* perform current control loop if enabled */
        if (psFocPMSM->bCurrentLoopOn)
        {
            /* D current error calculation */
            psFocPMSM->sIDQError.f16D = MLIB_SubSat_F16(psFocPMSM->sIDQReq.f16D, psFocPMSM->sIDQ.f16D);
    
            /* Q current error calculation */
            psFocPMSM->sIDQError.f16Q = MLIB_SubSat_F16(psFocPMSM->sIDQReq.f16Q, psFocPMSM->sIDQ.f16Q);
    
            /*** D - controller limitation calculation ***/
            psFocPMSM->sIdPiParams.f16UpperLim = MLIB_Mul_F16(psFocPMSM->f16DutyCycleLimit, psFocPMSM->f16UDcBusFilt);
            psFocPMSM->sIdPiParams.f16LowerLim = MLIB_Neg_F16(psFocPMSM->sIdPiParams.f16UpperLim);
    
            /* D current PI controller */
            psFocPMSM->sUDQReq.f16D =
                GFLIB_CtrlPIpAW_F16(psFocPMSM->sIDQError.f16D, &psFocPMSM->bIdPiStopInteg, &psFocPMSM->sIdPiParams);
    
            /*** Q - controller limitation calculation ***/
            psFocPMSM->sIqPiParams.f16UpperLim = GFLIB_Sqrt_F16(
                MLIB_Sub_F16(MLIB_Mul_F16(psFocPMSM->sIdPiParams.f16UpperLim, psFocPMSM->sIdPiParams.f16UpperLim),
                             MLIB_Mul_F16(psFocPMSM->sUDQReq.f16D, psFocPMSM->sUDQReq.f16D)));
            psFocPMSM->sIqPiParams.f16LowerLim = MLIB_Neg_F16(psFocPMSM->sIqPiParams.f16UpperLim);
    
            /* Q current PI controller */
            psFocPMSM->sUDQReq.f16Q =
                GFLIB_CtrlPIpAW_F16(psFocPMSM->sIDQError.f16Q, &psFocPMSM->bIqPiStopInteg, &psFocPMSM->sIqPiParams);
        }
    
        /* 2-phase to 2-phase transformation to stationary ref. frame */
        GMCLIB_ParkInv_F16(&psFocPMSM->sUDQReq, &psFocPMSM->sAnglePosEl, &psFocPMSM->sUAlBeReq);
    
        /* DCBus ripple elimination */
        GMCLIB_ElimDcBusRipFOC_F16(psFocPMSM->f16UDcBusFilt, &psFocPMSM->sUAlBeReq, &psFocPMSM->sUAlBeComp);
    
        /* space vector modulation */
        psFocPMSM->ui16SectorSVM = GMCLIB_SvmStd_F16(&psFocPMSM->sUAlBeComp, &psFocPMSM->sDutyABC);
    }
    
    /*!
     * @brief PMSM field oriented speed control.
     *
     * This function is used to compute PMSM field oriented speed control.
     *
     * @param psFocPMSM     The pointer of the PMSM FOC structure
     * @param psSpeed       The pointer of the PMSM speed structure
     *
     * @return None
     */
    void MCS_PMSMFocCtrlSpeed(mcs_pmsm_foc_t *psFocPMSM, mcs_speed_t *psSpeed)
    {
        /* Speed saturation flag given by the Q current controller saturation flag and speed controller saturation flag */
        psSpeed->bSpeedPiStopInteg = (psSpeed->sSpeedPiParams.bLimFlag || psFocPMSM->sIqPiParams.bLimFlag) &&
                                     (MLIB_AbsSat_F16(psSpeed->f16SpeedCmd) >= MLIB_AbsSat_F16(psSpeed->f16SpeedFilt));
    
        /* Speed ramp generation */
        psSpeed->f16SpeedRamp =
            MLIB_Conv_F16l(GFLIB_Ramp_F32(MLIB_Conv_F32s(psSpeed->f16SpeedCmd), &psSpeed->sSpeedRampParams));
    
        /* Speed error calculation */
        psSpeed->f16SpeedError = MLIB_SubSat_F16(psSpeed->f16SpeedRamp, psSpeed->f16SpeedFilt);
    
        /* Desired current by the speed PI controller */
        psFocPMSM->sIDQReq.f16Q =
            GFLIB_CtrlPIpAW_F16(psSpeed->f16SpeedError, &psSpeed->bSpeedPiStopInteg, &psSpeed->sSpeedPiParams);
    }
    
    /*!
     * @brief PMSM 2-step rotor alignment - 120deg in first step and 0deg in second.
     *
     * This function is used for alignment rotor in two steps - 120deg in first step and 0deg in second
     *
     * @param psFocPMSM     The pointer of the PMSM FOC structure
     * @param psAlignment   The pointer of the motor control alignment structure
     *
     * @return None
     */
    void MCS_PMSMAlignment(mcs_pmsm_foc_t *psFocPMSM, mcs_alignment_t *psAlignment)
    {
        /* first half duration time is position set to 120 degree */
        if (psAlignment->ui16TimeHalf > 0U)
        {
            psFocPMSM->f16PosElExt = FRAC16(120.0 / 180.0);
            psAlignment->ui16TimeHalf--;
        }
        else
        {
            psFocPMSM->f16PosElExt = FRAC16(0.0);
        }
    
        /* call voltage FOC to calculate PWM duty cycles */
        MCS_PMSMFocCtrl(psFocPMSM);
    }
    
    /*!
     * @brief PMSM Open Loop Start-up
     *
     * This function is used to PMSM Open Loop Start-up
     *
     * @param psStartUp     The pointer of the PMSM open loop start up parameters structure
     *
     * @return None
     */
    void MCS_PMSMOpenLoopStartUp(mcs_pmsm_startup_t *psStartUp)
    {
        /* Open loop startup speed ramp */
        psStartUp->f16SpeedRampOpenLoop =
            MLIB_Conv_F16l(GFLIB_Ramp_F32(MLIB_Conv_F32s(psStartUp->f16SpeedReq), &psStartUp->sSpeedRampOpenLoopParams));
    
        /* generation of open loop position from the required speed */
        psStartUp->f16PosGen = GFLIB_Integrator_F16(psStartUp->f16SpeedRampOpenLoop, &psStartUp->sSpeedIntegrator);
    
        /* position merging starts above merging speed threshold*/
        if (MLIB_AbsSat_F16(psStartUp->f16SpeedRampOpenLoop) >= psStartUp->f16SpeedCatchUp)
        {
            /* increment position merging coefficient */
            psStartUp->f16RatioMerging = MLIB_AddSat_F16(psStartUp->f16RatioMerging, psStartUp->f16CoeffMerging);
    
            /* merging equation */
            psStartUp->f16PosMerged = MLIB_Add_F16(
                psStartUp->f16PosGen,
                MLIB_Mul_F16(MLIB_Sub_F16(psStartUp->f16PosEst, psStartUp->f16PosGen), psStartUp->f16RatioMerging));
        }
        else
        {
            psStartUp->f16PosMerged = psStartUp->f16PosGen;
        }
    
        /* clear open loop flag */
        if (psStartUp->f16RatioMerging == FRAC16(1.0))
        {
            psStartUp->bOpenLoop = FALSE;
        }
    }
    
    /*!
     * @brief PMSM scalar control, voltage is set based on required speed
     *
     * This function is used for alignment rotor in two steps - 120deg in first step and 0deg in second
     *
     * @param psFocPMSM      The pointer of the PMSM FOC structure
     * @param psScalarPMSM   The pointer of the PMSM scalar control structure
     *
     * @return None
     */
    void MCS_PMSMScalarCtrl(mcs_pmsm_foc_t *psFocPMSM, mcs_pmsm_scalar_ctrl_t *psScalarPMSM)
    {
        /* this part of code is executed when scalar control is turned-on */
        /* frequency ramp */
        psScalarPMSM->f16FreqRamp =
            MLIB_Conv_F16l(GFLIB_Ramp_F32(MLIB_Conv_F32s(psScalarPMSM->f16FreqCmd), &psScalarPMSM->sFreqRampParams));
    
        /* voltage calculation */
        psScalarPMSM->sUDQReq.f16Q = (frac16_t)(MLIB_ShLSat_F16(
            MLIB_Mul_F16(psScalarPMSM->f16VHzGain, psScalarPMSM->f16FreqRamp), (uint16_t)(psScalarPMSM->i16VHzGainShift)));
        psScalarPMSM->sUDQReq.f16D = 0;
        
        /* voltage limitation to f16UqMin */
        if (psScalarPMSM->sUDQReq.f16Q >= FRAC16(0.0))
        {
        	psScalarPMSM->sUDQReq.f16Q = (frac16_t)(GFLIB_LowerLimit_F16(psScalarPMSM->sUDQReq.f16Q,
        	                                                                     psScalarPMSM->f16UqMin));
        }
        else
        {
        	psScalarPMSM->sUDQReq.f16Q = (frac16_t)(GFLIB_UpperLimit_F16(psScalarPMSM->sUDQReq.f16Q,
        	                                                                     MLIB_Neg_F16(psScalarPMSM->f16UqMin)));
        }
    
                                                    
        /* stator voltage angle , used the same integrator as for the open-loop start up*/
        psScalarPMSM->f16PosElScalar = GFLIB_Integrator_F16(psScalarPMSM->f16FreqRamp, &psScalarPMSM->sFreqIntegrator);
    
        /* pass parameters to FOC structure */
        psFocPMSM->sUDQReq     = psScalarPMSM->sUDQReq;
        psFocPMSM->f16PosElExt = psScalarPMSM->f16PosElScalar;
    
        /* call voltage FOC to calculate PWM duty cycles */
        MCS_PMSMFocCtrl(psFocPMSM);
    }

Back

• 14 person(s) visited this page until now.
• Back