#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;
//������ �� ���������� ������ ����������� ��������
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;
}