525 lines
18 KiB
C
525 lines
18 KiB
C
/*
|
||
* vector.c
|
||
*
|
||
* Created on: 20 <20><><EFBFBD><EFBFBD>. 2023 <20>.
|
||
* Author: seklyuts
|
||
*
|
||
* Это проект epwm_test_motor_cpu2
|
||
* дополнение к EFC_Application (на CPU2)
|
||
*/
|
||
|
||
|
||
#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 "driverlib.h"
|
||
#include "device.h"
|
||
//#include "board.h"
|
||
|
||
|
||
|
||
#define IMAX_A 25.0 //A
|
||
#define IMAX (IMAX_A*BIT_MAX/FACTOR_CURRENT_MOTOR_A)
|
||
|
||
typedef struct {
|
||
int16_t Value16in; //
|
||
int16_t Value16filtered; //
|
||
int32_t Value32; //
|
||
int16_t ShiftFilter; // Filters
|
||
} Filtered;
|
||
|
||
Filtered UdcFilter = {0,0,0,10};
|
||
|
||
int16_t Filter(int16_t inValue, Filtered *Struct);
|
||
|
||
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, NoLoop = 0, StepShift = 6; ///< начальные значения
|
||
int16_t Nstep = 0;
|
||
float CurrentRegTuneAmpl = 0, FRM_cur = 0;
|
||
int16_t Angle_test=0,sin_int,cos_int, AngleErr;
|
||
float Udc;
|
||
uint16_t SectorCheckOn=1;
|
||
volatile int16_t UdcFiltered = 0;
|
||
float UdcVolt = 0;
|
||
volatile float VdcFactor = FACTOR_VDC;
|
||
volatile uint16_t VoltProcImit = 0;
|
||
|
||
float Test1, Test2;
|
||
float FRM_Ud, FRM_Uq;
|
||
|
||
|
||
void vectorFault(void)
|
||
{
|
||
Mode = OffMode;
|
||
}
|
||
|
||
/*
|
||
* Расчет шагов в автономном тесте в локали
|
||
* globals: Step
|
||
*/
|
||
void testStep()
|
||
{
|
||
// Автономная работа, прямо здесь. :(
|
||
// Разгоняемся, едем, тормозим, ждём, реверсим, едем, тормозим, ждем....
|
||
enum Pstate {Rzg, Ttim, Torm, Pauss, Pauss2, Revers};
|
||
static uint16_t razg = 0; // для контроля разгона/торможения
|
||
int16_t razgD = 88; // шаг разгона
|
||
int16_t NstepMax = 180; // считаем это Макс скоростью в тесте
|
||
int16_t NstepMax2 = -180; // считаем это Макс скоростью в тесте
|
||
static uint32_t RolTime = 1;
|
||
uint32_t RolTimeMax = 40000; // время вращения
|
||
uint32_t RolTimeMax2 = 10000; //
|
||
CurrLoop.piId.Ref = 2.55; // старое = 0;
|
||
static enum Pstate cur_state = Rzg;
|
||
static int rever = 0; // индикатор реверса
|
||
// Работа:
|
||
// Rzg - Ttim - Torm - Pauss - Revers -
|
||
|
||
//
|
||
// Turn on LED
|
||
//
|
||
GPIO_writePin(32U, 1);
|
||
|
||
switch(cur_state){
|
||
case Rzg: ///< разгон
|
||
// Gpio_rainbow(2);
|
||
GPIO_writePin(34U, 0);
|
||
if(Nstep < NstepMax){
|
||
if(razg <= razgD) razg++;
|
||
else{
|
||
razg = 0;
|
||
Nstep++;
|
||
}
|
||
}
|
||
else cur_state = Ttim;
|
||
break;
|
||
|
||
case Ttim: ///< время работы/пауза
|
||
RolTime++;
|
||
if(RolTime > RolTimeMax){
|
||
if(rever) {rever = 0; cur_state = Rzg;}
|
||
else cur_state = Torm;
|
||
RolTime = 0;}
|
||
// DEVICE_DELAY_US(5000000); // мкс 5сек
|
||
break;
|
||
|
||
case Torm: ///< торможение
|
||
if(Nstep > 0)
|
||
{
|
||
if(razg <= razgD) razg++;
|
||
else{
|
||
razg = 0;
|
||
Nstep--; }
|
||
}
|
||
else{
|
||
cur_state = Pauss;
|
||
Nstep = 0; }
|
||
break;
|
||
|
||
case Pauss: ///< пауза после торможения
|
||
if(RolTime++ > RolTimeMax2){
|
||
cur_state = Revers;
|
||
razg = 0; }
|
||
break;
|
||
|
||
case Pauss2: ///< пауза после реверса
|
||
if(RolTime++ > RolTimeMax){
|
||
cur_state = Rzg;
|
||
razg = 0; }
|
||
break;
|
||
|
||
case Revers: ///< разгоняемся назад
|
||
if(Nstep > NstepMax2){
|
||
if(razg <= razgD) razg++;
|
||
else{
|
||
razg = 0;
|
||
Nstep--;
|
||
}
|
||
}
|
||
else {cur_state = Ttim;
|
||
RolTime = 0;
|
||
rever = 1;}
|
||
break;
|
||
default: break;
|
||
}
|
||
}
|
||
|
||
|
||
/*
|
||
* Управление вектором
|
||
* AMG: добавил тетстовые штуки для автономной работы
|
||
* целевые параметры:
|
||
* CurrLoop.piId.Ref = 2.55;
|
||
* Nstep = 100;
|
||
*
|
||
*/
|
||
void vectorControl(int16_t CurrentA, int16_t CurrentB, int16_t CurrentC, int16_t sdfmUdc)
|
||
{
|
||
|
||
|
||
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;
|
||
|
||
UdcFiltered = Filter(sdfmUdc, &UdcFilter);
|
||
if(UdcFiltered < 0) UdcFiltered = 0;
|
||
if(VoltProcImit) UdcVolt = VoltProcImit;
|
||
else UdcVolt = (float)UdcFiltered*VdcFactor/32768;
|
||
|
||
testStep();
|
||
Mode = StepMode;
|
||
Step = Step + Nstep;
|
||
CurrLoop.piId.Ref = 2.55;
|
||
/** поехали!! */
|
||
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();
|
||
}
|
||
Udc = sdfmUdc * FACTOR_VDC;
|
||
vector_klark_park(SectorCheckOn,Ia,Ib,Ic);
|
||
vector_inversion();
|
||
// BissStartSet();
|
||
// FMSTREnableSet();
|
||
// AdcStartSet();
|
||
|
||
}
|
||
|
||
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: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
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: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
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: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
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: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
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, UdcVolt);
|
||
}
|
||
|
||
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)
|
||
{
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> Udc <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>(<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> Umax <20> Umin <20> <20><>-<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
|
||
Outputs->Udc = PmsmVect.UdcFilter.Output;
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> 0 <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
|
||
if (Outputs->Udc <= 0.0f)
|
||
Outputs->Udc = VOLTAGE_UDC;
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>(<28><><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
|
||
PmsmVect.UdcPwmFactor = 1.0f/Outputs->Udc;
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
|
||
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;
|
||
}
|
||
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
|
||
|
||
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;
|
||
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
|
||
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;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
|
||
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
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
|
||
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
|
||
|
||
|
||
|
||
float UmaxVolt = VOLT_MAX_FACTOR * UdcVolt;
|
||
|
||
CurrLoop.piIq.Umax = UmaxVolt; // Parameter: upper saturation limit
|
||
CurrLoop.piIq.Umin = -UmaxVolt;
|
||
CurrLoop.piId.Umax = UmaxVolt; // Parameter: upper saturation limit
|
||
CurrLoop.piId.Umin = -UmaxVolt;
|
||
|
||
PI_MACRO(CurrLoop.piIq);
|
||
PI_MACRO(CurrLoop.piId);
|
||
|
||
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> Uq <20> Ud*/
|
||
// Test1 = my_sqrtf(Test2);
|
||
|
||
|
||
float Ulim = my_sqrtf(CurrLoop.piId.Out * CurrLoop.piId.Out + CurrLoop.piIq.Out * CurrLoop.piIq.Out);
|
||
|
||
|
||
|
||
if (Ulim > UmaxVolt)// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
{
|
||
if(Ulim > ZERO_LVL)
|
||
{
|
||
float K = UmaxVolt / 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;
|
||
}
|
||
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
int16_t Filter(int16_t inValue, Filtered *Struct) {
|
||
(*Struct).Value16in = inValue;
|
||
(*Struct).Value32+=(int32_t)((*Struct).Value16in)-(int32_t)((*Struct).Value16filtered);
|
||
(*Struct).Value16filtered =(int16_t)(((*Struct).Value32)>>((*Struct).ShiftFilter));
|
||
return ((*Struct).Value16filtered);
|
||
}
|