270 lines
9.1 KiB
C
270 lines
9.1 KiB
C
#include "vector.h"
|
||
#include "IQmathLib.h"
|
||
#include "math.h"
|
||
#include "pid_reg3.h"
|
||
#include "adc_tools.h"
|
||
#include "params.h"
|
||
#include "regul_power.h"
|
||
#include "pwm_vector_regul.h"
|
||
#include "my_filter.h"
|
||
|
||
// #pragma DATA_SECTION(pidPvect,".fast_vars1");
|
||
PIDREG3 pidPvect = PIDREG3_DEFAULTS;
|
||
|
||
|
||
#define IQ_OUT_SAT_POWER (2880.0 * COS_FI)
|
||
#define MAX_PID_CURRENT_P 2200.0 // 2200.0 ~ 1.6 Inom
|
||
|
||
#define IM_CURRENT_STOP_RMP 11184810 //2000
|
||
|
||
#define IQ_1500A 8388608
|
||
#define IQ_1800A 10066329
|
||
|
||
#define K_RMP_100A_PER_TIC 559240
|
||
#define K_RMP_400A_PER_SEC 2663
|
||
|
||
#define K_MODUL_MAX 15770583
|
||
|
||
// #define P_DELTA_ZAD_IZM 5592405 //3 MWt
|
||
|
||
#define LIMIT_Iq_ON_POWER_REVERSE
|
||
|
||
_iq Id_out_max_low_speed = _IQ(ID_OUT_SAT_POWER_LOW_SPEED / NORMA_ACP);
|
||
_iq Id_out_max_full = _IQ(ID_OUT_SAT_POWER / NORMA_ACP);
|
||
|
||
|
||
void init_Pvect_pid()
|
||
{
|
||
pidPvect.Ref = 0;
|
||
pidPvect.Kp = _IQ(1.0);//_IQ(1.0);
|
||
pidPvect.Ki = _IQ(0.1);//_IQ(0.05);//_IQ(0.06);
|
||
pidPvect.Kc = _IQ(0.5);
|
||
// pidPvect.Kp = _IQ(1); //_IQ(2.5);//_IQ(5.0);//
|
||
// pidPvect.Ki = _IQ(0.1);//_IQ(0.05);//_IQ(0.06);
|
||
// pidPvect.Kc = _IQ(0.5);
|
||
pidPvect.OutMax = _IQ(MAX_PID_CURRENT_P / NORMA_ACP);
|
||
pidPvect.OutMin = -_IQ(MAX_PID_CURRENT_P / NORMA_ACP);
|
||
}
|
||
|
||
void reset_Pvect_pid()
|
||
{
|
||
pidPvect.Up = 0;
|
||
pidPvect.Up1 = 0;
|
||
pidPvect.Ui = 0;
|
||
pidPvect.Ud = 0;
|
||
pidPvect.Out = 0;
|
||
}
|
||
|
||
_iq koeff_Iq_filter = _IQ(0.5);
|
||
|
||
#define LEVEL_LIMIT_KM 14596177 //87% //15099494 ~ 90% //15435038 ~ 92%
|
||
|
||
#define POWER_7_MWt 13048945
|
||
#define POWER_4_MWt 7456540
|
||
#define POWER_200_kWt 372827
|
||
|
||
// #pragma CODE_SECTION(vector_power,".fast_run2");
|
||
void vector_power(_iq Pzad, _iq P_measured, _iq Iq_measured, int mode, _iq Frot, _iq* Iq_zad, _iq* Id_zad)
|
||
{
|
||
static _iq Pzad_rmp = 0;
|
||
static _iq koef_slow = _IQ(0.000075); //_IQ(0.000065);15sec //_IQ(0.000085); 13sec // //_IQ(0.000065); normal rampa 13 seconds
|
||
static _iq koef_slow_low_task = _IQ(0.000040); //Ðàìïà äëÿ çàäàíèé ìîùíîñòè ìåíüøå ïîëîâèíû íîìèíàëà, ÷òîáû íåáûëî ïåðåðåãóëÿöèè
|
||
static _iq koef_slow_2 = _IQ(0.000045); //_IQ(0.000039); // _IQ(0.000015); //_IQ(0.000011) ~ 10 sec 0-10MWt //_IQ(0.00002) ~ 7 ñåê 0-10ÌÂò
|
||
static _iq koef_fast = _IQ(0.00013); //_IQ(0.00008); //_IQ(0.000025); ~3.1 sev 10-0MWt
|
||
// static _iq prev_Pzad = 0;
|
||
// static _iq cos_fi_nom_alg = _IQ(COS_FI), cos_fi_nom_squared_alg = _IQ(COS_FI * COS_FI);
|
||
// static _iq cos_fi_nom = _IQ(COS_FI), cos_fi_nom_squared = _IQ(COS_FI * COS_FI);
|
||
static _iq Iq_out_nom = _IQ(IQ_OUT_NOM / NORMA_ACP); //, Id_out_nom = _IQ(ID_OUT_NOM / NORMA_ACP);
|
||
// static _iq Iq_out_max = _IQ(IQ_OUT_SAT_POWER / NORMA_ACP);
|
||
static _iq Id_out_max = _IQ(ID_OUT_SAT_POWER_LOW_SPEED / NORMA_ACP);
|
||
static _iq pid_Out_max = _IQ(MAX_PID_CURRENT_P / NORMA_ACP);
|
||
_iq koef_rmp, koef_spad; //koef_spad - äëþ çàùèòû îò ïðåâûøåíèþ âèíòîì ðàçðåø¸ííûõ îáîðîòîâ (170îá/ìèí)
|
||
_iq Iq_out_unsat, Iq_out_sat, Id_out_sat, Id_out_unsat;
|
||
// _iq Iq_out_limited = 0;
|
||
static _iq Iq_out_filter = 0;
|
||
static _iq mzz_zad_int=0;
|
||
static int counterStopRampa = 0;
|
||
#ifdef LIMIT_Iq_ON_POWER_REVERSE
|
||
static int flag_reverse = 0;
|
||
#endif //LIMIT_Iq_ON_POWER_REVERSE
|
||
|
||
// if (f.DownTemperature) {
|
||
// mzz_zad_int = zad_intensiv_q(35480, 35480, mzz_zad_int, _IQmpy(f.iq_mzz_zad, temperature_limit_koeff));
|
||
// } else
|
||
// if(f.DownToNominal && (f.iq_mzz_zad > Iq_out_nom)) {
|
||
// mzz_zad_int = zad_intensiv_q(29480, 29480, mzz_zad_int, Iq_out_nom);
|
||
// } else if (a.iqk1 > LEVEL_LIMIT_KM || a.iqk2 > LEVEL_LIMIT_KM) {
|
||
// Pzad = 0;
|
||
// Pzad_rmp = zad_intensiv_q((koef_fast << 3), (koef_fast << 3), Pzad_rmp, Pzad);
|
||
// // f.DownToNominalVoltage = 1;
|
||
// }
|
||
// else
|
||
{
|
||
if (f.iq_mzz_zad != 0) {
|
||
mzz_zad_int = zad_intensiv_q(28480, 28480, mzz_zad_int, f.iq_mzz_zad);
|
||
} else {
|
||
mzz_zad_int = zad_intensiv_q(25480, 25480, mzz_zad_int, pid_Out_max);
|
||
}
|
||
// f.DownToNominalVoltage = 0;
|
||
}
|
||
|
||
if((mode != 2) || (!f.Go)
|
||
// || (f.flag_leading == 0 && f.read_task_from_optical_bus == 1 && f.sync_Iq_from_optical_bus == 1)
|
||
)
|
||
{
|
||
Pzad_rmp = P_measured;
|
||
// prev_Pzad = P_measured;
|
||
pidPvect.Ui = Iq_measured;
|
||
Id_out_max = Id_out_max_low_speed;
|
||
if(!f.Go)
|
||
{
|
||
*Iq_zad = *Id_zad = 0;
|
||
}
|
||
#ifdef LIMIT_Iq_ON_POWER_REVERSE
|
||
flag_reverse = 0;
|
||
#endif
|
||
return;
|
||
}
|
||
|
||
#ifdef LIMIT_Iq_ON_POWER_REVERSE
|
||
// Ïðè òîðîæåíèè îãðàíè÷èâàåì Iq íóë¸ì ñî ñòîðîíû íå òîðìîæåíèÿ
|
||
// ò.ê. ïðè òîðìîæåíèè èç-çà êîëåáàíèé çàäàíèå íà òîê ìîæåò ïîìåíÿòü çíàê
|
||
if ((Frot > 0 && Pzad_rmp < 0 && pidPvect.Out < 0) ||
|
||
(Frot < 0 && Pzad_rmp > 0 && pidPvect.Out > 0)) {
|
||
flag_reverse = 1;
|
||
vect_control.flag_reverse = 1;
|
||
} else
|
||
if ((Frot > 0 && Pzad_rmp > 0) || (Frot < 0 && Pzad_rmp < 0))
|
||
{
|
||
flag_reverse = 0;
|
||
vect_control.flag_reverse = 0;
|
||
}
|
||
if (flag_reverse == 0) {
|
||
pidPvect.OutMax = mzz_zad_int;
|
||
pidPvect.OutMin = -mzz_zad_int;
|
||
} else {
|
||
if (Pzad_rmp < 0) {
|
||
pidPvect.OutMax = 0;
|
||
pidPvect.OutMin = -mzz_zad_int;
|
||
} else {
|
||
pidPvect.OutMax = mzz_zad_int;
|
||
pidPvect.OutMin = 0;
|
||
}
|
||
}
|
||
#else
|
||
pidPvect.OutMax = mzz_zad_int;
|
||
pidPvect.OutMin = -mzz_zad_int;
|
||
#endif //LIMIT_Iq_ON_POWER_REVERSE
|
||
|
||
// if (f.setCosTerminal) {
|
||
// cos_fi.cos_fi_nom = f.cosinusTerminal;
|
||
// cos_fi.cos_fi_nom_squared = f.cosinusTerminalSquared;
|
||
// }
|
||
|
||
|
||
//if(Pzad != prev_Pzad)
|
||
//{
|
||
if((_IQabs(Pzad_rmp) <= _IQabs(Pzad)) &&
|
||
(((Pzad >= 0) && (Pzad_rmp >= 0)) || ((Pzad < 0) && (Pzad_rmp < 0))))
|
||
{
|
||
// if (_IQabs(Pzad) < POWER_4_MWt) {
|
||
// koef_rmp = koef_slow_low_task;
|
||
// } else if (_IQabs(Pzad_rmp) < POWER_7_MWt) { // 7MWt
|
||
// koef_rmp = koef_slow;
|
||
// } else {
|
||
// koef_rmp = koef_slow_2;
|
||
// }
|
||
koef_rmp = koef_slow;
|
||
//Ïðè íàáðîñå ìîùíîñòè ýëåêòðè÷åñêàÿ ìîùíîñòü ìîæåò ïðåâûñèòü çàäàííóþ
|
||
//ïîýòîìó åñëè ïðèáëèçèëèñü ê çàäàíèþ, òî çàìåäëÿåì ðàìïó âåêòîðîé ìîùíîñòè
|
||
// if (_IQabs(f.iq_p_zad_electric) - _IQabs(analog.iqW) < POWER_200_kWt) {
|
||
// koef_rmp = koef_rmp / 4;
|
||
// }
|
||
}
|
||
else
|
||
{
|
||
koef_rmp = koef_fast;
|
||
}
|
||
//}
|
||
|
||
// EfficiencyCompensation(&Pzad, &Iq_measured);
|
||
|
||
// if (analog.iqIm_1 > IM_CURRENT_STOP_RMP || analog.iqIm_2 > IM_CURRENT_STOP_RMP) {
|
||
// counterStopRampa = 400;
|
||
// } else {
|
||
// if (counterStopRampa > 0) {
|
||
// counterStopRampa -= 1;
|
||
// }
|
||
// }
|
||
// if (counterStopRampa <= 0) {
|
||
// Pzad_rmp = zad_intensiv_q(koef_rmp, koef_rmp, Pzad_rmp, Pzad);
|
||
// counterStopRampa = 0;
|
||
// } else {
|
||
// Pzad_rmp = zad_intensiv_q(koef_slow_2, koef_slow_2, Pzad_rmp, Pzad);
|
||
// }
|
||
|
||
Pzad_rmp = zad_intensiv_q(koef_rmp, koef_rmp, Pzad_rmp, Pzad);
|
||
|
||
#ifdef P_DELTA_ZAD_IZM
|
||
//Åñëè èçìåðåííàÿ ìîùíîñòü íà ìíîãî ìåíüøå çàäàííîé ðàìïû,
|
||
//òî ðàìïà îãðàíè÷èâàåòñÿ çíà÷åíèåì èçìåðåííîé ïëþñ íåêîòîðàÿ äåëüòà
|
||
if ((Pzad_rmp > 0 && P_measured > 0) &&
|
||
((Pzad_rmp - P_measured) > P_DELTA_ZAD_IZM)) {
|
||
Pzad_rmp = P_measured + P_DELTA_ZAD_IZM;
|
||
}
|
||
if ((Pzad_rmp < 0 && P_measured < 0) &&
|
||
((Pzad_rmp - P_measured) < -P_DELTA_ZAD_IZM)) {
|
||
Pzad_rmp = P_measured - P_DELTA_ZAD_IZM;
|
||
}
|
||
#endif
|
||
|
||
f.iq_p_rampa = Pzad_rmp;
|
||
|
||
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
if(_IQabs(Frot) > IQ_170_RPM)
|
||
{
|
||
koef_spad = _IQdiv((IQ_170_RPM + IQ_10_RPM - _IQabs(Frot)), IQ_10_RPM);
|
||
if(koef_spad < 0) {
|
||
koef_spad = 0;
|
||
}
|
||
pidPvect.OutMax = _IQmpy(pidPvect.OutMax, koef_spad);
|
||
pidPvect.OutMin = _IQmpy(pidPvect.OutMin, koef_spad);
|
||
Pzad_rmp = _IQmpy(Pzad_rmp, koef_spad);
|
||
}
|
||
pidPvect.Ref = Pzad_rmp;
|
||
pidPvect.Fdb = P_measured;
|
||
|
||
|
||
pidPvect.calc(&pidPvect);
|
||
|
||
Iq_out_unsat = pidPvect.Out;
|
||
|
||
Iq_out_filter = Iq_out_unsat; // exp_regul_iq(koeff_Iq_filter, Iq_out_filter, Iq_out_unsat);
|
||
|
||
// ìîäåëè çäåñü íå áûëî îãðàíè÷åíèé
|
||
Iq_out_sat = _IQsat(Iq_out_filter, mzz_zad_int, -mzz_zad_int); //Test
|
||
|
||
if(Iq_out_filter < 0)
|
||
{
|
||
Id_out_unsat = _IQdiv(_IQmpy((-Iq_out_filter), _IQsqrt(ONE_IQ24 - cos_fi.cos_fi_nom_squared)), cos_fi.cos_fi_nom);
|
||
} else {
|
||
Id_out_unsat = _IQdiv(_IQmpy(Iq_out_filter, _IQsqrt(ONE_IQ24 - cos_fi.cos_fi_nom_squared)), cos_fi.cos_fi_nom);
|
||
}
|
||
|
||
if (_IQabs(Frot) < IQ_50_RPM) {
|
||
Id_out_max = zad_intensiv_q(5592, 5592, Id_out_max, Id_out_max_low_speed); //????
|
||
} else {
|
||
Id_out_max = zad_intensiv_q(5592, 5592, Id_out_max, Id_out_max_full);
|
||
}
|
||
Id_out_sat = _IQsat(Id_out_unsat, Id_out_max, 0);
|
||
|
||
// prev_Iq_zad = Iq_out_sat;
|
||
|
||
*Iq_zad = Iq_out_sat;
|
||
*Id_zad = Id_out_sat;
|
||
|
||
}
|
||
|
||
|