366 lines
12 KiB
C
366 lines
12 KiB
C
/*
|
|
* vector.c
|
|
*
|
|
* Created on: 20 ñåíò. 2023 ã.
|
|
* Author: seklyuts
|
|
*/
|
|
|
|
|
|
#include "f28x_project.h"
|
|
#include "pwm_interrupts.h"
|
|
#include "pwm_init.h"
|
|
#include "pi.h"
|
|
#include "vector.h"
|
|
#include "iqmath.h"
|
|
#include "sdfm.h"
|
|
#include "gpio_init.h"
|
|
#include "frm_uart.h"
|
|
#include "skvt.h"
|
|
|
|
#define IMAX_A 10.0 //A
|
|
#define IMAX (IMAX_A*BIT_MAX/FACTOR_CURRENT_MOTOR_A)
|
|
|
|
|
|
|
|
const int16_t mcgenSineTable256[257] = \
|
|
{ 0,-201,-402,-603,-804,-1005,-1206,-1407,-1607,-1808,
|
|
-2009,-2210,-2410,-2611,-2811,-3011,-3211,-3411,-3611,-3811,
|
|
-4011,-4210,-4409,-4609,-4808,-5006,-5205,-5403,-5602,-5800,
|
|
-5997,-6195,-6392,-6589,-6786,-6983,-7179,-7375,-7571,-7766,
|
|
-7961,-8156,-8351,-8545,-8739,-8933,-9126,-9319,-9512,-9704,
|
|
-9896,-10087,-10278,-10469,-10659,-10849,-11039,-11228,-11416,-11605,
|
|
-11793,-11980,-12167,-12353,-12539,-12725,-12910,-13094,-13278,-13462,
|
|
-13645,-13828,-14010,-14191,-14372,-14552,-14732,-14912,-15090,-15269,
|
|
-15446,-15623,-15800,-15976,-16151,-16325,-16499,-16673,-16846,-17018,
|
|
-17189,-17360,-17530,-17700,-17869,-18037,-18204,-18371,-18537,-18703,
|
|
-18868,-19032,-19195,-19358,-19519,-19681,-19841,-20001,-20159,-20318,
|
|
-20475,-20631,-20787,-20942,-21097,-21250,-21403,-21555,-21706,-21856,
|
|
-22005,-22154,-22301,-22448,-22594,-22740,-22884,-23027,-23170,-23312,
|
|
-23453,-23593,-23732,-23870,-24007,-24144,-24279,-24414,-24547,-24680,
|
|
-24812,-24943,-25073,-25201,-25330,-25457,-25583,-25708,-25832,-25955,
|
|
-26077,-26199,-26319,-26438,-26557,-26674,-26790,-26905,-27020,-27133,
|
|
-27245,-27356,-27466,-27576,-27684,-27791,-27897,-28002,-28106,-28208,
|
|
-28310,-28411,-28511,-28609,-28707,-28803,-28898,-28993,-29086,-29178,
|
|
-29269,-29359,-29447,-29535,-29621,-29707,-29791,-29874,-29956,-30037,
|
|
-30117,-30196,-30273,-30350,-30425,-30499,-30572,-30644,-30714,-30784,
|
|
-30852,-30919,-30985,-31050,-31114,-31176,-31237,-31298,-31357,-31414,
|
|
-31471,-31526,-31581,-31634,-31685,-31736,-31785,-31834,-31881,-31927,
|
|
-31971,-32015,-32057,-32098,-32138,-32176,-32214,-32250,-32285,-32319,
|
|
-32351,-32383,-32413,-32442,-32469,-32496,-32521,-32545,-32568,-32589,
|
|
-32610,-32629,-32647,-32663,-32679,-32693,-32706,-32718,-32728,-32737,
|
|
-32745,-32752,-32758,-32762,-32765,-32767,-32767 };
|
|
|
|
|
|
int16_t Ia,Ib,Ic;
|
|
Tvector2dq vectorUdq, vectorIdq;
|
|
Tvector2ph vectorU2ph, vectorI2ph;
|
|
Tvector3abc vectorUabc, vectorIabc;
|
|
TvectorSinCos vectorSinCos;
|
|
uint16_t VecSector, OverCur[3] = {0,0,0}, ErrCur=0, CurFault = 0;
|
|
float FactorCurrent = FACTOR_CURRENT_MOTOR;
|
|
TvectorCurrentLoop CurrLoop;
|
|
TMode Mode = OffMode;
|
|
uint16_t Step = 0, StepS, Nstep = 2, NoLoop = 0, StepShift = 6;
|
|
float CurrentRegTuneAmpl = 0, FRM_cur = 0;
|
|
int16_t Angle_test=0,sin_int,cos_int, AngleErr;
|
|
|
|
|
|
void vectorFault(void)
|
|
{
|
|
Mode = OffMode;
|
|
}
|
|
|
|
void vectorControl(int16_t CurrentA, int16_t CurrentB, int16_t CurrentC)
|
|
{
|
|
pwm_clr_PwmFlagStartADC();
|
|
Ia = CurrentA;//sdfm_get(6);
|
|
Ib = CurrentB;//sdfm_get(3);
|
|
Ic = CurrentC;//sdfm_get(4);
|
|
if((Ia > IMAX)||(Ib>IMAX)||(Ic>IMAX)||(Ia<-IMAX)||(Ib<-IMAX)||(Ic<-IMAX) ) {OverCur[0] = Ia; OverCur[1] = Ib; OverCur[2] = Ic; ErrCur++; if(ErrCur >= 3) {Mode = OffMode; CurFault++;}}
|
|
else ErrCur = 0;
|
|
|
|
Step = Step + Nstep;
|
|
CurrLoop.piId.Ref = 0;
|
|
switch(Mode)
|
|
{
|
|
case OffMode:
|
|
PWM_ABC_StopAllClose();
|
|
vectorResCurrLoop();
|
|
break;
|
|
case StepMode:
|
|
PWM_ABC_StartOut();
|
|
vectorSinCos.Angle = Step;
|
|
CurrLoop.piIq.Ref = CurrentRegTuneAmpl;
|
|
break;
|
|
case StayMode:
|
|
PWM_ABC_StartOut();
|
|
break;
|
|
case CurrentRegTune:
|
|
vectorSinCos.Angle = 0;
|
|
PWM_ABC_StartOut();
|
|
StepS = Step&((1<<StepShift)-1);
|
|
if(StepS > (1<<(StepShift-1))) CurrLoop.piId.Ref = 0;
|
|
else CurrLoop.piId.Ref = CurrentRegTuneAmpl;
|
|
break;
|
|
default:
|
|
Mode = OffMode;
|
|
PWM_ABC_StopAllClose();
|
|
}
|
|
vector_klark_park(1,Ia,Ib,Ic);
|
|
vector_inversion();
|
|
FMSTR_enable_set();
|
|
}
|
|
|
|
void vectorInitCurrLoop(void)
|
|
{
|
|
CurrLoop.CurrentLimit = CURRENT_MAX;
|
|
|
|
CurrLoop.piId.Ref = 0; // Input: reference set-point
|
|
CurrLoop.piId.Fbk = 0; // Input: feedback
|
|
CurrLoop.piId.uCorr = 0; // Input: Êîððåêöèÿ âûõîäà, äëÿ óñòðàíåíèÿ ïåðåêðåòíûõ ñâÿçåé
|
|
CurrLoop.piId.Out = 0; // Output: controller output
|
|
CurrLoop.piId.Kp = PI_REG_I_PROPOR; // Parameter: proportional loop gain
|
|
CurrLoop.piId.Ki = PI_REG_I_INTEGR; // Parameter: integral gain
|
|
CurrLoop.piId.Umax = VOLT_MAX_PROC; // Parameter: upper saturation limit
|
|
CurrLoop.piId.Umin = -VOLT_MAX_PROC; // Parameter: lower saturation limit
|
|
CurrLoop.piId.up = 0; // Data: proportional term
|
|
CurrLoop.piId.ui = 0; // Data: integral term
|
|
CurrLoop.piId.v1 = 0; // Data: pre-saturated controller output
|
|
CurrLoop.piId.i1 = 0; // Data: integrator storage: ui(k-1)
|
|
|
|
|
|
CurrLoop.piIq.Ref = 0; // Input: reference set-point
|
|
CurrLoop.piIq.Fbk = 0; // Input: feedback
|
|
CurrLoop.piIq.uCorr = 0; // Input: Êîððåêöèÿ âûõîäà, äëÿ óñòðàíåíèÿ ïåðåêðåòíûõ ñâÿçåé
|
|
CurrLoop.piIq.Out = 0; // Output: controller output
|
|
CurrLoop.piIq.Kp = PI_REG_I_PROPOR; // Parameter: proportional loop gain
|
|
CurrLoop.piIq.Ki = PI_REG_I_INTEGR; // Parameter: integral gain
|
|
CurrLoop.piIq.Umax = VOLT_MAX_PROC; // Parameter: upper saturation limit
|
|
CurrLoop.piIq.Umin = -VOLT_MAX_PROC; // Parameter: lower saturation limit
|
|
CurrLoop.piIq.up = 0; // Data: proportional term
|
|
CurrLoop.piIq.ui = 0; // Data: integral term
|
|
CurrLoop.piIq.v1 = 0; // Data: pre-saturated controller output
|
|
CurrLoop.piIq.i1 = 0; // Data: integrator storage: ui(k-1)
|
|
}
|
|
|
|
void vectorResCurrLoop(void)
|
|
{
|
|
CurrLoop.piId.Ref = 0; // Input: reference set-point
|
|
CurrLoop.piId.uCorr = 0; // Input: Êîððåêöèÿ âûõîäà, äëÿ óñòðàíåíèÿ ïåðåêðåòíûõ ñâÿçåé
|
|
CurrLoop.piId.Out = 0; // Output: controller output
|
|
CurrLoop.piId.v1 = 0; // Data: pre-saturated controller output
|
|
CurrLoop.piId.i1 = 0; // Data: integrator storage: ui(k-1)
|
|
|
|
CurrLoop.piIq.Ref = 0; // Input: reference set-point
|
|
CurrLoop.piIq.uCorr = 0; // Input: Êîððåêöèÿ âûõîäà, äëÿ óñòðàíåíèÿ ïåðåêðåòíûõ ñâÿçåé
|
|
CurrLoop.piIq.Out = 0; // Output: controller output
|
|
CurrLoop.piIq.v1 = 0; // Data: pre-saturated controller output
|
|
CurrLoop.piIq.i1 = 0; // Data: integrator storage: ui(k-1)
|
|
}
|
|
|
|
|
|
int16_t vector_mcsinPIxLUT(int16_t Value, int16_t *psinTable)
|
|
{
|
|
// set saturation on
|
|
if (Value>=0)
|
|
{
|
|
return ( Value > ANGLE_PI_DIVIDE_2_F16 ? -psinTable[(2*SIN_LENGTH_TABLE- \
|
|
(Value>> SIN_INDEX_SHIFT))] : -psinTable[(Value >> SIN_INDEX_SHIFT)] );
|
|
|
|
}
|
|
else
|
|
{
|
|
return ( Value < -ANGLE_PI_DIVIDE_2_F16 ? psinTable[(2*SIN_LENGTH_TABLE- \
|
|
-(Value >> SIN_INDEX_SHIFT))] : psinTable[(-(Value >> SIN_INDEX_SHIFT))] );
|
|
|
|
}
|
|
}
|
|
|
|
|
|
void vector_inversion(void)
|
|
{
|
|
float temp1, temp2, temp3;
|
|
|
|
vectorU2ph.Alfa = _IQmpy(vectorUdq.d,vectorSinCos.cos) - _IQmpy(vectorUdq.q,vectorSinCos.sin);
|
|
vectorU2ph.Beta = _IQmpy(vectorUdq.q,vectorSinCos.cos) + _IQmpy(vectorUdq.d,vectorSinCos.sin);
|
|
|
|
|
|
|
|
temp1= vectorU2ph.Beta;
|
|
temp2= _IQdiv2(vectorU2ph.Beta) + (_IQmpy(_IQ(0.866),vectorU2ph.Alfa));
|
|
temp3= temp2 - temp1;
|
|
|
|
VecSector=3;
|
|
VecSector=(temp2> 0)?( VecSector-1):VecSector;
|
|
VecSector=(temp3> 0)?( VecSector-1):VecSector;
|
|
VecSector=(temp1< 0)?(7-VecSector) :VecSector;
|
|
|
|
|
|
|
|
if (VecSector==1 || VecSector==4)
|
|
{
|
|
vectorUabc.a= temp2;
|
|
vectorUabc.b= temp1-temp3;
|
|
vectorUabc.c=-temp2;
|
|
}
|
|
else if(VecSector==2 || VecSector==5)
|
|
{
|
|
vectorUabc.a= temp3+temp2;
|
|
vectorUabc.b= temp1;
|
|
vectorUabc.c=-temp1;
|
|
}
|
|
else
|
|
{
|
|
vectorUabc.a= temp3;
|
|
vectorUabc.b=-temp3;
|
|
vectorUabc.c=-(temp1+temp2);
|
|
}
|
|
pwm_set_volt_3F(vectorUabc.a,vectorUabc.b,vectorUabc.c);
|
|
}
|
|
|
|
float Test1, Test2;
|
|
float FRM_Ud, FRM_Uq;
|
|
|
|
void vector_klark_park(uint16_t SectorOn, int16_t CurrentA, int16_t CurrentB, int16_t CurrentC)
|
|
{
|
|
|
|
/* if (Inputs->UpdateUdc)
|
|
{
|
|
//Çàïîìèíàåì Udc äëÿ êîíòóðà òîêà(îãðàíè÷åíèå Umax è Umin â ÏÈ-ðåãóëÿòîðå)
|
|
Outputs->Udc = PmsmVect.UdcFilter.Output;
|
|
//Çàùèòà îò äåëåíèÿ íà 0 è îòðèöàòåëüíûõ ÷èñåë
|
|
if (Outputs->Udc <= 0.0f)
|
|
Outputs->Udc = VOLTAGE_UDC;
|
|
//Ðàñ÷åò êîýôôèöèåíòà äëÿ ðàñ÷åòà ñêâàæíîñòè ØÈÌ(îäèí ðàç äåëèì, ïîòîì êàæäûé öèêë ØÈÌ óìíîæàåì, ýòî ýôôåêòèâíåå)
|
|
PmsmVect.UdcPwmFactor = 1.0f/Outputs->Udc;
|
|
//Ñáðîñ ôëàãà
|
|
Inputs->UpdateUdc = false;
|
|
}*/
|
|
|
|
|
|
if (SectorOn )
|
|
{
|
|
switch (VecSector) {
|
|
case 2:
|
|
case 3:
|
|
vectorIabc.a = (float)CurrentA * FactorCurrent;
|
|
vectorIabc.c = (float)CurrentC * FactorCurrent;
|
|
vectorIabc.b = - vectorIabc.a - vectorIabc.c;
|
|
break;
|
|
case 4:
|
|
case 5:
|
|
vectorIabc.a = (float)CurrentA * FactorCurrent;
|
|
vectorIabc.b = (float)CurrentB * FactorCurrent;
|
|
vectorIabc.c = - vectorIabc.a - vectorIabc.b;
|
|
break;
|
|
case 1:
|
|
case 6:
|
|
default:
|
|
vectorIabc.b = (float)CurrentB * FactorCurrent;
|
|
vectorIabc.c = (float)CurrentC * FactorCurrent;
|
|
vectorIabc.a = - vectorIabc.b - vectorIabc.c;
|
|
break;
|
|
}
|
|
}
|
|
else {
|
|
vectorIabc.a = (float)CurrentA * FactorCurrent;
|
|
vectorIabc.b = (float)CurrentB * FactorCurrent;
|
|
vectorIabc.c = (float)CurrentC * FactorCurrent;
|
|
}
|
|
|
|
// ïðåîáðàçîâàíèå Êëàðê
|
|
|
|
vectorI2ph.Alfa = vectorIabc.a;
|
|
vectorI2ph.Beta = _IQmpy((vectorIabc.a +_IQmpy2(vectorIabc.b)),_IQ(ONEbySQRT3)); //vectorI2ph.Beta = _IQmpy((vectorIabc.b - vectorIabc.c),_IQ(ONEbySQRT3));
|
|
|
|
sin_int = vector_mcsinPIxLUT(vectorSinCos.Angle, (int16_t *)&mcgenSineTable256);
|
|
cos_int = vector_mcsinPIxLUT((vectorSinCos.Angle + ANGLE_PI_DIVIDE_2_F16),(int16_t *)&mcgenSineTable256);
|
|
|
|
vectorSinCos.sin = ((float)sin_int)/32768.0;
|
|
vectorSinCos.cos = ((float)cos_int)/32768.0;
|
|
|
|
// ïðåîáðàçîâàíèå Ïàðêà
|
|
vectorIdq.d = _IQmpy(vectorI2ph.Alfa,vectorSinCos.cos) + _IQmpy(vectorI2ph.Beta,vectorSinCos.sin);
|
|
vectorIdq.q = _IQmpy(vectorI2ph.Beta,vectorSinCos.cos) - _IQmpy(vectorI2ph.Alfa,vectorSinCos.sin);
|
|
|
|
Angle_test = Atan(sin_int, cos_int);
|
|
|
|
AngleErr = Angle_test - vectorSinCos.Angle;
|
|
|
|
|
|
// CurrLoop.piIq.Ref = Inputs->IqRef;//Çàäàííîå
|
|
|
|
if (FABS(CurrLoop.piIq.Ref) > CurrLoop.CurrentLimit)
|
|
{
|
|
(CurrLoop.piIq.Ref >= 0.0f) ? ( CurrLoop.piIq.Ref = CurrLoop.CurrentLimit ) :
|
|
( CurrLoop.piIq.Ref = -CurrLoop.CurrentLimit);
|
|
}
|
|
|
|
CurrLoop.piIq.Fbk = vectorIdq.q;
|
|
|
|
// CurrLoop.piId.Ref = 0;
|
|
CurrLoop.piId.Fbk = vectorIdq.d;
|
|
|
|
// CurrLoop.piId.Umax = CurrLoop.piIq.Umax = Inputs->Udc;
|
|
// CurrLoop.piId.Umin = CurrLoop.piIq.Umin = -CurrLoop.piIq.Umax;
|
|
#ifdef UCORR_ENABLE
|
|
//Ïåðåêðåñòíûå ñâÿçè
|
|
float Velectr = VFbk * CurrLoop.VelElectrFactor;
|
|
CurrLoop.piId.uCorr = Velectr * CurrLoop.PhaseInduct * CurrLoop.piIq.Fbk;
|
|
CurrLoop.piIq.uCorr = -Velectr * (CurrLoop.FluxLinkage + CurrLoop.PhaseInduct * CurrLoop.piId.Fbk);
|
|
#else
|
|
CurrLoop.piId.uCorr = CurrLoop.piIq.uCorr = 0.0f;
|
|
#endif
|
|
|
|
|
|
PI_MACRO(CurrLoop.piIq);
|
|
PI_MACRO(CurrLoop.piId);
|
|
|
|
/*Ïðîïîðöèîíàëüíîå îãðàíè÷åíèå íàïðÿæåíèé Uq è Ud*/
|
|
// Test1 = my_sqrtf(Test2);
|
|
|
|
|
|
float Ulim = my_sqrtf(CurrLoop.piId.Out * CurrLoop.piId.Out + CurrLoop.piIq.Out * CurrLoop.piIq.Out);
|
|
if (Ulim > VOLT_MAX_PROC)
|
|
{
|
|
if(Ulim > ZERO_LVL)
|
|
{
|
|
float K = VOLT_MAX_PROC / Ulim;
|
|
FRM_Ud = CurrLoop.piId.Out *= K;
|
|
FRM_Uq = CurrLoop.piIq.Out *= K;
|
|
}
|
|
}
|
|
if(!NoLoop)
|
|
{
|
|
vectorUdq.q = CurrLoop.piIq.Out;
|
|
vectorUdq.d = CurrLoop.piId.Out;
|
|
}
|
|
|
|
}
|
|
|
|
float my_sqrtf(float x)
|
|
{
|
|
if (x == 0) return x;
|
|
|
|
/*
|
|
* We'll compute 1/sqrt(x) using the Newton-Raphson method from an
|
|
* initial approximation using EISQRTF32; this instruction gives
|
|
* 1/sqrtf(x) "accurate to approximately 8 bits" (SPRUEO2A). The
|
|
* number of iterations used here may not be optimal for this
|
|
* instruction.
|
|
*
|
|
* n.b. __eisqrtf32 cannot generate -0, NaN, or denormal values
|
|
*
|
|
* n.b. this loop is divergent for very small x
|
|
*/
|
|
|
|
float x0 = __eisqrtf32(x);
|
|
int i;
|
|
|
|
for (i = 0; i < 3; i++)
|
|
x0 *= (1.5f - x * 0.5f * x0 * x0);
|
|
|
|
/* now sqrt(x) = x * 1/sqrt(x) */
|
|
return x * x0;
|
|
}
|