pmsm3_4.c

移植TI的永磁同步电机（PMSM）速度闭环调速程序

  	clarke1.Bs = _IQ15toIQ((int32)ilg2_vdc1.ImeasB);
	clarke1.calc(&clarke1);

// ------------------------------------------------------------------------------
//    Connect inputs of the PARK module and call the park transformation
//    calculation function.
// ------------------------------------------------------------------------------
    park1.Alpha = clarke1.Alpha;
    park1.Beta = clarke1.Beta;
    park1.Angle = rg1.Out;
    park1.calc(&park1);
    
// ------------------------------------------------------------------------------
//    Connect inputs of the INV_PARK module and call the inverse park transformation
//    calculation function.
// ------------------------------------------------------------------------------
    ipark1.Ds = _IQ(VdTesting);
    ipark1.Qs = _IQ(VqTesting);	
    ipark1.Angle = rg1.Out;
    ipark1.calc(&ipark1);

// ------------------------------------------------------------------------------
//    Connect inputs of the SVGEN_DQ module and call the space-vector gen.
//    calculation function.
// ------------------------------------------------------------------------------
  	svgen_dq1.Ualpha = ipark1.Alpha;
 	svgen_dq1.Ubeta = ipark1.Beta;
  	svgen_dq1.calc(&svgen_dq1);	

// ------------------------------------------------------------------------------
//    Connect inputs of the PWM_DRV module and call the PWM signal generation 
//    update function.
// ------------------------------------------------------------------------------
    pwm1.MfuncC1 = (int16)_IQtoIQ15(svgen_dq1.Ta); // MfuncC1 is in Q15
    pwm1.MfuncC2 = (int16)_IQtoIQ15(svgen_dq1.Tb); // MfuncC2 is in Q15  
    pwm1.MfuncC3 = (int16)_IQtoIQ15(svgen_dq1.Tc); // MfuncC3 is in Q15
	pwm1.update(&pwm1);

// ------------------------------------------------------------------------------
//    Connect inputs of the PWMDAC module 
// ------------------------------------------------------------------------------	
    PwmDacCh1 = (int16)_IQtoIQ15(svgen_dq1.Ta);
    PwmDacCh2 = (int16)_IQtoIQ15(clarke1.As);
    PwmDacCh3 = (int16)_IQtoIQ15(rg1.Out);

// ------------------------------------------------------------------------------
//    Connect inputs of the DATALOG module 
// ------------------------------------------------------------------------------
    DlogCh1 = (int16)_IQtoIQ15(svgen_dq1.Ta);
    DlogCh2 = (int16)_IQtoIQ15(rg1.Out);
    DlogCh3 = (int16)_IQtoIQ15(clarke1.As);
    DlogCh4 = (int16)_IQtoIQ15(clarke1.Bs);

// ------------------------------------------------------------------------------
//    Connect inputs of the EN_DRV module and call the enable/disable PWM signal
//    update function. (FOR DMC1500 ONLY)
// ------------------------------------------------------------------------------ 
    #if 0  // 使用计算机 Motor Monitor 软件观测，不需要
    drv1.EnableFlag = EnableFlag;
    drv1.update(&drv1);
    #endif

#endif // (BUILDLEVEL==LEVEL2)


// ***************** LEVEL3 *****************
#if (BUILDLEVEL==LEVEL3)

// ------------------------------------------------------------------------------
//    Connect inputs of the RMP module and call the Ramp control
//    calculation function.
// ------------------------------------------------------------------------------
    rc1.TargetValue = _IQ(SpeedRef);
    rc1.calc(&rc1);

// ------------------------------------------------------------------------------
//    Connect inputs of the RAMP GEN module and call the Ramp generator
//    calculation function.
// ------------------------------------------------------------------------------
    rg1.Freq = rc1.SetpointValue;
    rg1.calc(&rg1);

// ------------------------------------------------------------------------------
//    Call the ILEG2_VDC read function.
// ------------------------------------------------------------------------------
    ilg2_vdc1.read(&ilg2_vdc1);

// ------------------------------------------------------------------------------
//    Connect inputs of the CLARKE module and call the clarke transformation
//    calculation function.
// ------------------------------------------------------------------------------
 	clarke1.As = _IQ15toIQ((int32)ilg2_vdc1.ImeasA);
  	clarke1.Bs = _IQ15toIQ((int32)ilg2_vdc1.ImeasB);
	clarke1.calc(&clarke1);

// ------------------------------------------------------------------------------
//    Connect inputs of the PARK module and call the park transformation
//    calculation function.
// ------------------------------------------------------------------------------
    park1.Alpha = clarke1.Alpha;
    park1.Beta = clarke1.Beta;
    park1.Angle = rg1.Out;
    park1.calc(&park1);
 
// ------------------------------------------------------------------------------
//    Connect inputs of the PID_REG3 module and call the PID IQ controller
//    calculation function.
// ------------------------------------------------------------------------------  
    pid1_iq.Ref = _IQ(IqRef);
	pid1_iq.Fdb = park1.Qs;
	pid1_iq.calc(&pid1_iq);

// ------------------------------------------------------------------------------
//    Connect inputs of the PID_REG3 module and call the PID ID controller
//    calculation function.
// ------------------------------------------------------------------------------  
    pid1_id.Ref = _IQ(IdRef);
	pid1_id.Fdb = park1.Ds;
	pid1_id.calc(&pid1_id);

// ------------------------------------------------------------------------------
//    Connect inputs of the INV_PARK module and call the inverse park transformation
//    calculation function.
// ------------------------------------------------------------------------------
    ipark1.Ds = pid1_id.Out;
    ipark1.Qs = pid1_iq.Out;	
    ipark1.Angle = rg1.Out;
    ipark1.calc(&ipark1);

// ------------------------------------------------------------------------------
//    Connect inputs of the SVGEN_DQ module and call the space-vector gen.
//    calculation function.
// ------------------------------------------------------------------------------
  	svgen_dq1.Ualpha = ipark1.Alpha;
 	svgen_dq1.Ubeta = ipark1.Beta;
  	svgen_dq1.calc(&svgen_dq1);	

// ------------------------------------------------------------------------------
//    Connect inputs of the PWM_DRV module and call the PWM signal generation 
//    update function.
// ------------------------------------------------------------------------------
    pwm1.MfuncC1 = (int16)_IQtoIQ15(svgen_dq1.Ta); // MfuncC1 is in Q15
    pwm1.MfuncC2 = (int16)_IQtoIQ15(svgen_dq1.Tb); // MfuncC2 is in Q15  
    pwm1.MfuncC3 = (int16)_IQtoIQ15(svgen_dq1.Tc); // MfuncC3 is in Q15
	pwm1.update(&pwm1);
	
// ------------------------------------------------------------------------------
//    Connect inputs of the PWMDAC module 
// ------------------------------------------------------------------------------	
    PwmDacCh1 = (int16)_IQtoIQ15(clarke1.As);
    PwmDacCh2 = (int16)_IQtoIQ15(rg1.Out);    
    PwmDacCh3 = (int16)_IQtoIQ15(svgen_dq1.Ta);    

// ------------------------------------------------------------------------------
//    Connect inputs of the DATALOG module 
// ------------------------------------------------------------------------------
    DlogCh1 = (int16)_IQtoIQ15(svgen_dq1.Ta);
    DlogCh2 = (int16)_IQtoIQ15(svgen_dq1.Tb);
    DlogCh3 = (int16)_IQtoIQ15(rg1.Out);
    DlogCh4 = (int16)_IQtoIQ15(clarke1.As);

// ------------------------------------------------------------------------------
//    Connect inputs of the EN_DRV module and call the enable/disable PWM signal
//    update function. (FOR DMC1500 ONLY)
// ------------------------------------------------------------------------------ 
    drv1.EnableFlag = EnableFlag;
    drv1.update(&drv1);

#endif // (BUILDLEVEL==LEVEL3)


// ***************** LEVEL4 *****************
#if (BUILDLEVEL==LEVEL4)

// ------------------------------------------------------------------------------
//    Connect inputs of the RMP module and call the Ramp control
//    calculation function.
// ------------------------------------------------------------------------------
    rc1.TargetValue = _IQ(SpeedRef);
    rc1.calc(&rc1);

// ------------------------------------------------------------------------------
//    Connect inputs of the RAMP GEN module and call the Ramp generator
//    calculation function.
// ------------------------------------------------------------------------------
    rg1.Freq = rc1.SetpointValue;
    rg1.calc(&rg1);

// ------------------------------------------------------------------------------
//    Call the ILEG2_VDC read function.
// ------------------------------------------------------------------------------
    ilg2_vdc1.read(&ilg2_vdc1);

// ------------------------------------------------------------------------------
//    Connect inputs of the CLARKE module and call the clarke transformation
//    calculation function.
// ------------------------------------------------------------------------------
 	clarke1.As = _IQ15toIQ((int32)ilg2_vdc1.ImeasA);
  	clarke1.Bs = _IQ15toIQ((int32)ilg2_vdc1.ImeasB);
	clarke1.calc(&clarke1);

//-------------------------------------------------------------------------------------
// Checking LockRotorFlag=FALSE for spinning mode, LockRotorFlag=TRUE for locked rotor mode 
//-------------------------------------------------------------------------------------
	if(LockRotorFlag==TRUE)  // locked rotor mode if LockRotorFlag = 1
    {

// ------------------------------------------------------------------------------
//    Connect inputs of the PARK module and call the park transformation
//    calculation function.
// ------------------------------------------------------------------------------
       park1.Alpha = clarke1.Alpha;
       park1.Beta = clarke1.Beta;
       park1.Angle = 0;
       park1.calc(&park1);

// ------------------------------------------------------------------------------
//    Connect inputs of the PID_REG3 module and call the PID IQ controller
//    calculation function.
// ------------------------------------------------------------------------------  
       pid1_iq.Ref = _IQ(IqRef);
	   pid1_iq.Fdb = park1.Qs;
	   pid1_iq.calc(&pid1_iq);

// ------------------------------------------------------------------------------
//    Connect inputs of the PID_REG3 module and call the PID ID controller
//    calculation function.
// ------------------------------------------------------------------------------  
       pid1_id.Ref = _IQ(IdRef);
	   pid1_id.Fdb = park1.Ds;
	   pid1_id.calc(&pid1_id);

// ------------------------------------------------------------------------------
//    Connect inputs of the INV_PARK module and call the inverse park transformation
//    calculation function.
// ------------------------------------------------------------------------------
       ipark1.Ds = pid1_id.Out;
       ipark1.Qs = pid1_iq.Out;	
       ipark1.Angle = 0;
       ipark1.calc(&ipark1);

	}   // End: LockRotorFlag==TRUE
    
    else if(LockRotorFlag==FALSE)   // spinning mode if LockRotorFlag = 0
    {   

// ------------------------------------------------------------------------------
//    Connect inputs of the PARK module and call the park transformation
//    calculation function.
// ------------------------------------------------------------------------------
       park1.Alpha = clarke1.Alpha;
       park1.Beta = clarke1.Beta;
       park1.Angle = rg1.Out;
       park1.calc(&park1);

// ------------------------------------------------------------------------------
//    Connect inputs of the PID_REG3 module and call the PID IQ controller
//    calculation function.
// ------------------------------------------------------------------------------  
       pid1_iq.Ref = _IQ(IqRef);
	   pid1_iq.Fdb = park1.Qs;
	   pid1_iq.calc(&pid1_iq);

// ------------------------------------------------------------------------------
//    Connect inputs of the PID_REG3 module and call the PID ID controller
//    calculation function.
// ------------------------------------------------------------------------------  
       pid1_id.Ref = _IQ(IdRef);
	   pid1_id.Fdb = park1.Ds;
	   pid1_id.calc(&pid1_id);

// ------------------------------------------------------------------------------
//    Connect inputs of the INV_PARK module and call the inverse park transformation
//    calculation function.
// ------------------------------------------------------------------------------
       ipark1.Ds = pid1_id.Out;
       ipark1.Qs = pid1_iq.Out;	
       ipark1.Angle = rg1.Out;
       ipark1.calc(&ipark1);

    }   // End: LockRotorFlag==FALSE

// ------------------------------------------------------------------------------
//    Connect inputs of the SVGEN_DQ module and call the space-vector gen.
//    calculation function.
// ------------------------------------------------------------------------------
    svgen_dq1.Ualpha = ipark1.Alpha;
    svgen_dq1.Ubeta = ipark1.Beta;
    svgen_dq1.calc(&svgen_dq1);	

// ------------------------------------------------------------------------------
//    Connect inputs of the PWM_DRV module and call the PWM signal generation 
//    update function.
// ------------------------------------------------------------------------------
    pwm1.MfuncC1 = (int16)_IQtoIQ15(svgen_dq1.Ta); // MfuncC1 is in Q15
    pwm1.MfuncC2 = (int16)_IQtoIQ15(svgen_dq1.Tb); // MfuncC2 is in Q15  
    pwm1.MfuncC3 = (int16)_IQtoIQ15(svgen_dq1.Tc); // MfuncC3 is in Q15
    pwm1.update(&pwm1);

// ------------------------------------------------------------------------------
//    Call the QEP_DRV calculation function.
// ------------------------------------------------------------------------------
    qep1.calc(&qep1);

// ------------------------------------------------------------------------------
//    Connect inputs of the SPEED_FR module and call the speed calculation function 
// ------------------------------------------------------------------------------
    #if (DSP_TARGET==F2808)
       speed1.ElecTheta = _IQ24toIQ((int32)qep1.ElecTheta);
       speed1.DirectionQep = (int32)(qep1.DirectionQep);
       speed1.calc(&speed1);
    #endif
    #if (DSP_TARGET==F2812)
       speed1.ElecTheta = _IQ15toIQ((int32)qep1.ElecTheta);
       speed1.DirectionQep = (int32)(qep1.DirectionQep);
       speed1.calc(&speed1);
    #endif

// ------------------------------------------------------------------------------
//    Connect inputs of the PWMDAC module 
// ------------------------------------------------------------------------------	
    PwmDacCh1 = (int16)_IQtoIQ15(svgen_dq1.Ta);
    PwmDacCh2 = (int16)_IQtoIQ15(svgen_dq1.Tb);    
    PwmDacCh3 = (int16)_IQtoIQ15(rg1.Out);    

// ------------------------------------------------------------------------------
//    Connect inputs of the DATALOG module 
// ------------------------------------------------------------------------------
    DlogCh1 = (int16)_IQtoIQ15(svgen_dq1.Ta);
    DlogCh2 = (int16)_IQtoIQ15(clarke1.As);
  #if (DSP_TARGET==F2808)
    DlogCh3 = (int16)_IQtoIQ15(_IQ24toIQ(qep1.ElecTheta));
  #endif
  #if (DSP_TARGET==F2812)
    DlogCh3 = qep1.ElecTheta;
  #endif
    DlogCh4 = (int16)_IQtoIQ15(rg1.Out);

// ------------------------------------------------------------------------------
//    Connect inputs of the EN_DRV module and call the enable/disable PWM signal
//    update function. (FOR DMC1500 ONLY)
// ------------------------------------------------------------------------------ 
    drv1.EnableFlag = EnableFlag;
    drv1.update(&drv1);

#endif // (BUILDLEVEL==LEVEL4)


// ***************** LEVEL5 *****************

#if (BUILDLEVEL==LEVEL5)   // 速度闭环

    if (PMSM_CLOSE_LOOP == bMode)
    {
    // ------------------------------------------------------------------------------
    //    Call the ILEG2_VDC read function.
    // ------------------------------------------------------------------------------
        #if 0
        ilg2_vdc1.read(&ilg2_vdc1);
        #endif

    // ------------------------------------------------------------------------------
    //    Connect inputs of the CLARKE module and call the clarke transformation
    //    calculation function.
    // ------------------------------------------------------------------------------

        //fIb    = _IQ19toF(_IQ19mpy(_IQ19(asOscSamData[ADC_CHAN_Iv_INDEX]-2048), _IQ19(FLOAT_FACTOR_Iv) ));
        //fIc    = _IQ19toF(_IQ19mpy(_IQ19(asOscSamData[ADC_CHAN_Iw_INDEX]-2048), _IQ19(FLOAT_FACTOR_Iw) ));
        //fIa    = _IQ19toF(_IQ19mpy(_IQ19(-asOscSamData[ADC_CHAN_Iv_INDEX] - asOscSamData[ADC_CHAN_Iw_INDEX] + 4096), _IQ19(FLOAT_FACTOR_Iu) ));

         
     	clarke1.As = _IQ19toIQ(iq19Iu);  //_IQ15toIQ((int32)ilg2_vdc1.ImeasA);
       	clarke1.Bs = _IQ19toIQ(iq19Iv);  //_IQ15toIQ((int32)ilg2_vdc1.ImeasB);