estim.c

dspic30f6010a的学习事例程序

 /**********************************************************************
 *                                                                     *
 *                        Software License Agreement                   *
 *                                                                     *
 *    The software supplied herewith by Microchip Technology           *
 *    Incorporated (the "Company") for its dsPIC controller            *
 *    is intended and supplied to you, the Company's customer,         *
 *    for use solely and exclusively on Microchip dsPIC                *
 *    products. The software is owned by the Company and/or its        *
 *    supplier, and is protected under applicable copyright laws. All  *
 *    rights are reserved. Any use in violation of the foregoing       *
 *    restrictions may subject the user to criminal sanctions under    *
 *    applicable laws, as well as to civil liability for the breach of *
 *    the terms and conditions of this license.                        *
 *                                                                     *
 *    THIS SOFTWARE IS PROVIDED IN AN "AS IS" CONDITION.  NO           *
 *    WARRANTIES, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING,    *
 *    BUT NOT LIMITED TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND    *
 *    FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE     *
 *    COMPANY SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL,  *
 *    INCIDENTAL OR CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.  *
 *                                                                     *
  **********************************************************************/
 /**********************************************************************
 *                                                                     * 
 *    Author: Dr.-Ing. Hafedh Sammoud / APPCON Technologies SUARL      * 
 *    Project1: Sensorless foc control of Induction Motor			   *
 *                                                                     *
 *    Filename:       ESTIM.c                                          *
 *    Date:           19/03/07                                         *
 *                                                                     *
 *    Tools used: MPLAB GL    -> 7.50                                  *
 *                C-Compiler  -> 2.03                                  *
 *                                                                     *
 *    Linker File:    p30f6010A.gld                                    *
 ***********************************************************************
 *      Code Description											   *
 *  																   *
 * This module estimates the mechanical speed of an ACIM and calculates*
 * Frequency and angle of the magnetising current from the measured	   *
 * currents and from the reference values of the inverter output.	   *
 * This module can be used to replace an encoder	   				   *
 *                                                                     *
 *                                                                     *
 **********************************************************************/


#include "Estim.h"
#include "EstimDef.h"
#include "park.h"
#include "control.h"
#include "UserParms.h"

#define	USEIMRREF	1



extern	 tParkParm		ParkParm;
extern	 tSincosParm	SincosParm;
extern	 tCtrlParm		CtrlParm;

#ifndef USEIMRREF
long	IqInvTr2;
int		InvTr2=139;
#endif


void Estim(void)
{



if (EstimParm.qDiCounter>=8)
{
	EstimParm.qDiCounter=1;

//*******************************
// dIalpha = Ialpha-oldIalpha,  dIbeta  = Ibeta-oldIbeta

	EstimParm.qDIalpha	=	(ParkParm.qIalpha -EstimParm.qLastIalpha);
	if (EstimParm.qDIalpha>1023) EstimParm.qDIalpha=1023;
	if (EstimParm.qDIalpha<-1023) EstimParm.qDIalpha=-1023;
	EstimParm.qVIndalpha= ( (long) MotorEstimParm.qLsDt*(long) (EstimParm.qDIalpha) )>>10;

	EstimParm.qDIbeta	=	(ParkParm.qIbeta -EstimParm.qLastIbeta);
	if (EstimParm.qDIbeta>1023) EstimParm.qDIbeta=1023;
	if (EstimParm.qDIbeta<-1023) EstimParm.qDIbeta=-1023;
	EstimParm.qVIndbeta= ((long) MotorEstimParm.qLsDt*(long) (EstimParm.qDIbeta))>>10;

//*******************************
// update  LastIalpha and LastIbeta

	EstimParm.qLastIalpha=	ParkParm.qIalpha;
	EstimParm.qLastIbeta =	ParkParm.qIbeta;
} else
{
	EstimParm.qDiCounter=EstimParm.qDiCounter+1;
}
//*******************************
// Stator voltage eqations

	EstimParm.qEsa		= 	ParkParm.qValpha -
							(((long) MotorEstimParm.qRs  * (long) ParkParm.qIalpha)	>>15)
							-EstimParm.qVIndalpha;


	EstimParm.qEsb		= 	ParkParm.qVbeta -
							(((long) MotorEstimParm.qRs  * (long) ParkParm.qIbeta )	>>15)
							- EstimParm.qVIndbeta;


// Calculate Sin(Rho) and Cos(Rho)
	SincosParm.qAngle 	=	EstimParm.qRho;
	SinCos();

//*******************************
//    Esd =  Esa*cos(Angle) + Esb*sin(Rho)
	EstimParm.qEsd		=	(((long) EstimParm.qEsa * (long)SincosParm.qCos)>>15)
							+
							(((long)EstimParm.qEsb * (long)SincosParm.qSin)>>15);
//*******************************
//   Esq = -Esa*sin(Angle) + Esb*cos(Rho)
	EstimParm.qEsq		=	(((long) EstimParm.qEsb * (long)SincosParm.qCos)>>15)
							-
							(((long)EstimParm.qEsa * (long)SincosParm.qSin)>>15);

//*******************************
//*******************************
// Filter first order for Esd and Esq
// EsdFilter = 1/TFilterd * Intergal{ (Esd-EsdFilter).dt }
		EstimParm.qEsdStateVar			= EstimParm.qEsdStateVar+
										( (long)(EstimParm.qEsd - EstimParm.qEsdf) * (long)EstimParm.qKfilterd) ;
		EstimParm.qEsdf					= (int)(EstimParm.qEsdStateVar>>15);

		EstimParm.qEsqStateVar			= EstimParm.qEsqStateVar+
										( (long)(EstimParm.qEsq - EstimParm.qEsqf) * (long)EstimParm.qKfilterq) ;
		EstimParm.qEsqf					= (int)(EstimParm.qEsqStateVar>>15);
// OmegaMr= (1+SigmaR)/PsiMr * Esq -sgn(Uhqf) * Uhdf
// if Drehzahl>10% => condition Esqf<>0
if (abs(EstimParm.qVelEstim)>600)
{
	if(EstimParm.qEsqf>0)
	{
		EstimParm.qOmegaMr	=	(((long)MotorEstimParm.qInvPsi*(long)(EstimParm.qEsqf- EstimParm.qEsdf)) >>15) ;
	} else
	{
		EstimParm.qOmegaMr	=	(((long)MotorEstimParm.qInvPsi*(long)(EstimParm.qEsqf + EstimParm.qEsdf))>>15);
	}
} else // if Drehzahl<10% => condition VelRef<>0
{
	if(EstimParm.qVelEstim>0)
	{
		EstimParm.qOmegaMr	=	(((long)MotorEstimParm.qInvPsi*(long)(EstimParm.qEsqf- EstimParm.qEsdf))>>15) ;
	} else
	{
		EstimParm.qOmegaMr	=	(((long)MotorEstimParm.qInvPsi*(long)(EstimParm.qEsqf+ EstimParm.qEsdf))>>15) ;
	}
}


// Limit OmegaMr to garantee stability

if(EstimParm.qOmegaMr>EstimParm.qOmegaMrMax)
	{
		EstimParm.qOmegaMr=EstimParm.qOmegaMrMax;
	}

if(EstimParm.qOmegaMr<EstimParm.qOmegaMrMin)
	{
		EstimParm.qOmegaMr=EstimParm.qOmegaMrMin;
	}


//*******************************
// Integration of OmegaMr => Rho
	EstimParm.qRhoStateVar	= 	EstimParm.qRhoStateVar+
							(long)(EstimParm.qOmegaMr)*(long)(EstimParm.qDeltaT);
	EstimParm.qRho 		= 	(int) (EstimParm.qRhoStateVar>>15);



#ifdef USEIMRREF	
// Omeg2Estim=Iq/Imr/Tr when Reference Value of magnatising current is used

		EstimParm.qOmeg2Estim= ((long)ParkParm.qIq*(long)MotorEstimParm.qRrInvTr)>>15; //
#else

//*******************************
// Current Model
// Imr = 1/Tr * Intergal{ (Id-Imr).dt } when real value of magnetising current is used

		EstimParm.qImrStateVar			= EstimParm.qImrStateVar+
										( (long)(ParkParm.qId - EstimParm.qImr) * (long)MotorEstimParm.qInvTr) ;
		EstimParm.qImr					= (int)(EstimParm.qImrStateVar>>15);

		IqInvTr2=(long)ParkParm.qIq*(long)InvTr2;

		if (EstimParm.qImr<2000)	// limit lower value of Imr to garantee stability
		{
			EstimParm.qImr=2000;
		}

		EstimParm.qOmeg2Estim=IqInvTr2/EstimParm.qImr;

#endif
// Electrical speed: Low pass filter first order for (OmegaMr-Omeg2Estim)

		EstimParm.qVelEstimStateVar=EstimParm.qVelEstimStateVar+
								( (long)(EstimParm.qOmegaMr-EstimParm.qOmeg2Estim-EstimParm.qVelEstim)*(long)EstimParm.qVelEstimFilterK );
		EstimParm.qVelEstim=	(int)(EstimParm.qVelEstimStateVar>>15);
//qVelEstim; 			// Estimated speed in 1.15
//qVelEstimFilterK; 	// Filter Konstant for Estimated speed in 1.15
//qVelEstimStateVar; 	// State Variable for Estimated speed in 1.31

// Mechanical speed

		EstimParm.qVelEstimMech= ((long) EstimParm.qVelEstim*(long) EstimParm.qInvPol)>>15;

}	// End of Estim()


void	InitEstimParmLeeson(void)
{
// Konstants are defined in "ParameterLeeson.xls" 
		EstimParm.qInvPol=0x7FFF / diPoles;
		EstimParm.qRhoStateVar=0;
		EstimParm.qOmegaMr=0;
		EstimParm.qDiCounter=0;
		EstimParm.qEsdStateVar=0;
		EstimParm.qEsqStateVar=0;
		EstimParm.qDeltaT=1582;
		EstimParm.qKfilterd=374;
		EstimParm.qKfilterq=374;
		EstimParm.qVelEstimFilterK=109;
		EstimParm.qOmegaMrMax=5000;
		EstimParm.qOmegaMrMin=(-5000);
		MotorEstimParm.qInvTr=40;
		MotorEstimParm.qRrInvTr=2335;



#if	SUPPLY115VAC // Setting for 115VAC
		MotorEstimParm.qLsDt=3363;
		MotorEstimParm.qInvPsi=13562;
		MotorEstimParm.qRs=17588;
		StartupParm.qVelMinContrOff=300;

#else			// Settings for 230VAC
		MotorEstimParm.qLsDt=1681;
		MotorEstimParm.qInvPsi=27124;
		MotorEstimParm.qRs=8794;
		StartupParm.qVelMinContrOff=600;
#endif
}
void	InitEstimParm0(void)
{
// Konstants are defined in "Parameter0.xls" 
		EstimParm.qInvPol=0x7FFF / diPoles;
		EstimParm.qRhoStateVar=0;
		EstimParm.qOmegaMr=0;
		EstimParm.qDiCounter=0;
		EstimParm.qEsdStateVar=0;
		EstimParm.qEsqStateVar=0;
		EstimParm.qDeltaT=1582;
		EstimParm.qKfilterd=374;
		EstimParm.qKfilterq=374;
		EstimParm.qVelEstimFilterK=109;
		EstimParm.qOmegaMrMax=5000;
		EstimParm.qOmegaMrMin=(-5000);

		MotorEstimParm.qInvTr=21;
		MotorEstimParm.qRrInvTr=1215;



#if	SUPPLY115VAC	// Setting for 115VAC
		MotorEstimParm.qLsDt=9284;
		MotorEstimParm.qInvPsi=6150;
		MotorEstimParm.qRs=17588;
		StartupParm.qVelMinContrOff=300;

#else				// Setting for 230VAC
		MotorEstimParm.qLsDt=4642;
		MotorEstimParm.qInvPsi=12300;
		MotorEstimParm.qRs=8794;
		StartupParm.qVelMinContrOff=600;
#endif

}