667 lines
23 KiB
C
667 lines
23 KiB
C
#include "IQmathLib.h"
|
|
#include "math.h"
|
|
#include "my_filter.h"
|
|
#include "params.h"
|
|
#include "adc_tools.h"
|
|
#include "abc_to_dq.h"
|
|
#include "regul_power.h"
|
|
#include "regul_turns.h"
|
|
#include "teta_calc.h"
|
|
#include "pwm_vector_regul.h"
|
|
#include "pid_reg3.h"
|
|
#include "vector.h"
|
|
#include "params_motor.h"
|
|
#include "v_pwm24.h"
|
|
//#include "power_distribution.h"
|
|
#include "rotation_speed.h"
|
|
//#include "calc_3_power.h"
|
|
|
|
// #pragma DATA_SECTION(pidD,".fast_vars1");
|
|
// PIDREG3 pidD = PIDREG3_UNDEPENDENT_DEFAULTS;
|
|
PIDREG3 pidD = PIDREG3_DEFAULTS;
|
|
// #pragma DATA_SECTION(pidQ,".fast_vars1");
|
|
// PIDREG3 pidQ = PIDREG3_UNDEPENDENT_DEFAULTS;
|
|
PIDREG3 pidQ = PIDREG3_DEFAULTS;
|
|
|
|
// #pragma DATA_SECTION(pidD2,".fast_vars1");
|
|
// PIDREG3 pidD2 = PIDREG3_UNDEPENDENT_DEFAULTS;
|
|
PIDREG3 pidD2 = PIDREG3_DEFAULTS;
|
|
// #pragma DATA_SECTION(pidQ2,".fast_vars1");
|
|
// PIDREG3 pidQ2 = PIDREG3_UNDEPENDENT_DEFAULTS;
|
|
PIDREG3 pidQ2 = PIDREG3_DEFAULTS;
|
|
|
|
// #pragma DATA_SECTION(cos_fi,".fast_vars1");
|
|
COS_FI_STRUCT cos_fi = {0,0};
|
|
|
|
// #pragma DATA_SECTION(vect_control,".fast_vars1");
|
|
VECTOR_CONTROL vect_control = VECTOR_CONTROL_DEFAULTS;
|
|
|
|
// #pragma DATA_SECTION(koeff_Fs_filter,".fast_vars");
|
|
long koeff_Fs_filter = _IQ(0.01);
|
|
|
|
// _iq winding_displacement = FIXED_PIna6;
|
|
|
|
WINDING a;
|
|
FLAG f;
|
|
|
|
|
|
ROTOR_VALUE rotor = ROTOR_VALUE_DEFAULTS;
|
|
|
|
#define IQ_ONE 16777216
|
|
|
|
#define CONST_IQ_2PI 105414357
|
|
#define CONST_IQ_2PI3 35138119 // 120 grad
|
|
#define CONST_IQ_PI2 26340229
|
|
|
|
#define K_MODUL_MIN 167772 //83886 //0.5% - îãðàíè÷åíèå èç-çà ïåðåíàïðóæåíèé
|
|
#define K_MODUL_MAX 15435038LL //13421772LL //80% 16106127LL ~ 96% //15435038LL ~ 0.92%
|
|
//15770583LL ~ 0.94%
|
|
#define K_MODUL_MAX_PART 15099494LL // 15099494LL ~ 0.9
|
|
|
|
#define I_ZERO_LEVEL_IQ 111848 //20A //279620 ~ 50A //55924 ~ 10A
|
|
#define K_MODUL_DEAD_TIME 503316 //3%
|
|
|
|
//#define I_ZERO_LEVEL_IQ 27962 //5A //55924 ~ 10A
|
|
|
|
#define Id_MIN 1118481 //200A //111848 ~ 20A //55924 ~ 10A
|
|
|
|
// #pragma DATA_SECTION(zadan_Id_min,".fast_vars");
|
|
_iq zadan_Id_min = Id_MIN;
|
|
|
|
#define START_TETTA_IQ 4392264 //15 grad
|
|
|
|
void analog_dq_calc(void);
|
|
void analog_Udq_calc(_iq Ud1, _iq Uq1, _iq Ud2, _iq Uq2);
|
|
void calcUdUqCompensation(_iq Frot);
|
|
|
|
float kI_D = 0.2;
|
|
float kI_Q = 0.2;
|
|
float kP_D = 0.1;//.1;
|
|
float kP_Q = 0.3;//.3;
|
|
|
|
// float kI_D_rev = 0.02;
|
|
// float kI_Q_rev = 0.03;
|
|
// float kP_D_rev = 0.1;//.3;
|
|
// float kP_Q_rev = 0.15;//.3;
|
|
|
|
void init_DQ_pid()
|
|
{
|
|
unsigned int i = 0;
|
|
int *pStr = (int*)&f;
|
|
for (i = 0; i < sizeof(f) / sizeof(int); i++) {
|
|
*(pStr + i) = 0;
|
|
}
|
|
|
|
*pStr = (int*)&a;
|
|
for (i = 0; i < sizeof(a) / sizeof(int); i++) {
|
|
*(pStr + i) = 0;
|
|
}
|
|
|
|
pidD.Ref = 0;
|
|
pidD.Kp = _IQ(kP_D);//_IQ(0.2); //_IQ(0.1);
|
|
pidD.Ki = _IQ(kI_D); //_IQ(0.2);// //
|
|
pidD.Kc = _IQ(0.3);
|
|
pidD.Kd = 0;
|
|
pidD.OutMax = K_MODUL_MAX_PART;
|
|
pidD.OutMin = -K_MODUL_MAX_PART; //0;
|
|
pidD.Up = 0;
|
|
pidD.Ui = 0;
|
|
pidD.Ud = 0;
|
|
pidD.Out = 0;
|
|
|
|
pidQ.Ref = 0;
|
|
pidQ.Kp = _IQ(kP_Q);//_IQ(0.3);//_IQ(0.3);
|
|
pidQ.Ki = _IQ(kI_Q); //_IQ(0.2); //
|
|
pidQ.Kc = _IQ(0.3);
|
|
pidQ.Kd = 0;
|
|
pidQ.OutMax = K_MODUL_MAX_PART; //_IQ(0.9);
|
|
pidQ.OutMin = -K_MODUL_MAX_PART; //-_IQ(0.9); //0;
|
|
pidQ.Up = 0;
|
|
pidQ.Ui = 0;
|
|
pidQ.Ud = 0;
|
|
pidQ.Out = 0;
|
|
|
|
pidD2.Ref = 0;
|
|
pidD2.Kp = _IQ(kP_D);//_IQ(0.1); //_IQ(0.1);
|
|
pidD2.Ki = _IQ(kI_D); //_IQ(0.2);// //
|
|
pidD2.Kc = _IQ(0.3);
|
|
pidD2.Kd = 0;
|
|
pidD2.OutMax = K_MODUL_MAX_PART;
|
|
pidD2.OutMin = -K_MODUL_MAX_PART; //0;
|
|
pidD2.Up = 0;
|
|
pidD2.Ui = 0;
|
|
pidD2.Ud = 0;
|
|
pidD2.Out = 0;
|
|
|
|
pidQ2.Ref = 0;
|
|
pidQ2.Kp = _IQ(kP_Q);// _IQ(0.3);//_IQ(0.3);
|
|
pidQ2.Ki = _IQ(kI_Q); //_IQ(0.2); //
|
|
pidQ2.Kc = _IQ(0.3);
|
|
pidQ2.Kd = 0;
|
|
pidQ2.OutMax = K_MODUL_MAX_PART; //_IQ(0.9);
|
|
pidQ2.OutMin = -K_MODUL_MAX_PART; //-_IQ(0.9); //0;
|
|
pidQ2.Up = 0;
|
|
pidQ2.Ui = 0;
|
|
pidQ2.Ud = 0;
|
|
pidQ2.Out = 0;
|
|
|
|
init_tetta_pid();
|
|
init_Fvect_pid();
|
|
init_Pvect_pid();
|
|
init_tetta_calc_struct();
|
|
|
|
init_Adc_Variables();
|
|
|
|
cos_fi.cos_fi_nom = _IQ(0.87);
|
|
cos_fi.cos_fi_nom_squared = _IQ(0.87 * 0.87);
|
|
// cos_fi.cos_fi_nom = _IQ(COS_FI);
|
|
// cos_fi.cos_fi_nom_squared = _IQ(COS_FI * COS_FI);
|
|
|
|
vect_control.koef_Ud_comp = _IQ((L_SIGMA_S + L_M * L_SIGMA_R / (L_M + L_SIGMA_R)) * 2 * 3.14 * NORMA_FROTOR); //Lsigm_s + Lm*Lsigm_r / (Lm + Lsigm_r)
|
|
vect_control.koef_Uq_comp = _IQ((L_M + L_SIGMA_S) * 2 * 3.14 * NORMA_FROTOR);
|
|
vect_control.k_modul_max = K_MODUL_MAX;
|
|
vect_control.k_modul_max_square = _IQmpy(K_MODUL_MAX, K_MODUL_MAX);
|
|
vect_control.iq_Id_out_max = _IQ(ID_OUT_SAT_POWER_LOW_SPEED / NORMA_ACP);
|
|
|
|
vect_control.koeff_correct_Id = IQ_ONE;
|
|
|
|
vect_control.equial_Iq_Proportional = _IQ(0.05);
|
|
vect_control.flag_reverse = 0;
|
|
}
|
|
|
|
void reset_DQ_pid()
|
|
{
|
|
pidD.Up = 0;
|
|
pidD.Up1 = 0;
|
|
pidD.Ui = 0;
|
|
pidD.Ud = 0;
|
|
pidD.Out = 0;
|
|
pidQ.Up = 0;
|
|
pidQ.Up1 = 0;
|
|
pidQ.Ui = 0;
|
|
pidQ.Ud = 0;
|
|
pidQ.Out = 0;
|
|
|
|
pidD2.Up = 0;
|
|
pidD2.Up1 = 0;
|
|
pidD2.Ui = 0;
|
|
pidD2.Ud = 0;
|
|
pidD2.Out = 0;
|
|
pidQ2.Up = 0;
|
|
pidQ2.Up1 = 0;
|
|
pidQ2.Ui = 0;
|
|
pidQ2.Ud = 0;
|
|
pidQ2.Out = 0;
|
|
}
|
|
|
|
// #pragma CODE_SECTION(Idq_to_Udq,".fast_run2");
|
|
void Idq_to_Udq(_iq Id_zad, _iq Iq_zad, _iq Id_measured, _iq Iq_measured, _iq* Ud_zad, _iq* Uq_zad)
|
|
{
|
|
pidD.Ref = Id_zad;
|
|
pidD.Fdb = Id_measured;
|
|
pidD.calc(&pidD);
|
|
*Ud_zad = pidD.Out;
|
|
|
|
pidQ.Ref = Iq_zad;
|
|
pidQ.Fdb = Iq_measured;
|
|
pidQ.calc(&pidQ);
|
|
*Uq_zad = pidQ.Out;
|
|
|
|
}
|
|
|
|
// inline void set_pid_coeffs() {
|
|
// if (vect_control.flag_reverse == 0) {
|
|
// pidD.Kp = _IQ(kP_D);//_IQ(0.2); //_IQ(0.1);
|
|
// pidD.Ki = _IQ(kI_D); //_IQ(0.2);// //
|
|
// pidQ.Kp = _IQ(kP_Q);//_IQ(0.3);//_IQ(0.3);
|
|
// pidQ.Ki = _IQ(kI_Q); //_IQ(0.2); //
|
|
// pidD2.Kp = _IQ(kP_D);//_IQ(0.1); //_IQ(0.1);
|
|
// pidD2.Ki = _IQ(kI_D); //_IQ(0.2);// //
|
|
// pidQ2.Kp = _IQ(kP_Q);// _IQ(0.3);//_IQ(0.3);
|
|
// pidQ2.Ki = _IQ(kI_Q); //_IQ(0.2); //
|
|
// } else {
|
|
// pidD.Kp = _IQ(kP_D_rev);//_IQ(0.2); //_IQ(0.1);
|
|
// pidD.Ki = _IQ(kI_D_rev); //_IQ(0.2);// //
|
|
// pidQ.Kp = _IQ(kP_Q_rev);//_IQ(0.3);//_IQ(0.3);
|
|
// pidQ.Ki = _IQ(kI_Q_rev); //_IQ(0.2); //
|
|
// pidD2.Kp = _IQ(kP_D_rev);//_IQ(0.1); //_IQ(0.1);
|
|
// pidD2.Ki = _IQ(kI_D_rev); //_IQ(0.2);// //
|
|
// pidQ2.Kp = _IQ(kP_Q_rev);// _IQ(0.3);//_IQ(0.3);
|
|
// pidQ2.Ki = _IQ(kI_Q_rev); //_IQ(0.2); //
|
|
// }
|
|
// }
|
|
|
|
void Idq_to_Udq_2_windings(_iq Id_zad, _iq Iq_zad, _iq Id_measured1, _iq Iq_measured1, _iq* Ud_zad1, _iq* Uq_zad1
|
|
, _iq Id_measured2, _iq Iq_measured2, _iq* Ud_zad2, _iq* Uq_zad2)
|
|
{
|
|
pidD.Ref = Id_zad;
|
|
pidD.Fdb = Id_measured1;
|
|
pidD.calc(&pidD);
|
|
*Ud_zad1 = pidD.Out;
|
|
|
|
pidQ.Ref = Iq_zad;
|
|
pidQ.Fdb = Iq_measured1;
|
|
pidQ.calc(&pidQ);
|
|
*Uq_zad1 = pidQ.Out;
|
|
|
|
pidD2.Ref = Id_zad;
|
|
pidD2.Fdb = Id_measured2;
|
|
pidD2.calc(&pidD2);
|
|
*Ud_zad2 = pidD2.Out;
|
|
|
|
pidQ2.Ref = Iq_zad;
|
|
pidQ2.Fdb = Iq_measured2;
|
|
pidQ2.calc(&pidQ2);
|
|
*Uq_zad2 = pidQ2.Out;
|
|
}
|
|
|
|
// #pragma CODE_SECTION(pwm_vector_model_titov,".fast_run");
|
|
void pwm_vector_model_titov(_iq Pzad, _iq Fzad, int direction, _iq Frot, int mode,
|
|
int reset, int calcPWMtimes)
|
|
{
|
|
int calc_channel = 1;
|
|
_iq U_zad1 = 0, U_zad2 = 0;
|
|
|
|
|
|
// if (f.flag_leading == 0 && f.read_task_from_optical_bus == 1 && f.sync_rotor_from_optical_bus == 1) {
|
|
// Frot = rotor.iqFrotFromOptica;
|
|
// }
|
|
|
|
//Êîððåêöèÿ Id ïðè ïðèâûøåíèè íîìèíàëüíûõ îáîðîòîâ
|
|
if (_IQabs(Frot) > IQ_F_ROTOR_NOM) {
|
|
vect_control.koeff_correct_Id = _IQdiv(IQ_F_ROTOR_NOM, _IQabs(Frot));
|
|
} else {
|
|
vect_control.koeff_correct_Id = IQ_ONE;
|
|
}
|
|
|
|
|
|
if(direction < 0) { Frot = -Frot; }
|
|
// prev_dir = direction;
|
|
if(reset == 1) //Ñáðîñ íàêîïëåíûõ ñîñòàâë-þùèõ ðåãóë-òîðà
|
|
{
|
|
reset_DQ_pid();
|
|
reset_tetta_pid();
|
|
analog.iqIq1_filter = 0;
|
|
analog.iqIq2_filter = 0;
|
|
analog.iqUq2_filter = 0;
|
|
U_zad1 = K_MODUL_MIN;
|
|
U_zad2 = K_MODUL_MIN;
|
|
vect_control.iq_Id_out_max = Id_out_max_low_speed;
|
|
vect_control.flag_reverse = 0;
|
|
}
|
|
|
|
set_cos_tetta_calc_params();
|
|
analog_dq_calc();
|
|
|
|
|
|
vector_turns(Fzad, Frot, analog.iqIq1, mode, &vect_control.iqIq_zad, &vect_control.iqId_zad);
|
|
vector_power(Pzad, analog.iqPvsi1 + analog.iqPvsi2, analog.iqIq1, mode, Frot, &vect_control.iqIq_zad, &vect_control.iqId_zad);
|
|
|
|
vect_control.iqPzad = pidPvect.Ref;
|
|
vect_control.iqPizm = pidPvect.Fdb;
|
|
vect_control.iqFrot = Frot;
|
|
|
|
// Çàäàíèå ìèíèìàëüíîãî çíà÷åíèÿ Id
|
|
if (vect_control.iqId_zad < Id_MIN) { vect_control.iqId_zad = zadan_Id_min; }
|
|
|
|
//Êîððåêöèÿ Id ïðè ïðèâûøåíèè íîìèíàëüíûõ îáîðîòîâ
|
|
// if (_IQabs(Frot) > IQ_F_ROTOR_NOM) {
|
|
// vect_control.iqId_zad = _IQmpy(vect_control.iqId_zad, vect_control.koeff_correct_Id);
|
|
// }
|
|
|
|
|
|
// Frot =_IQ(15.0 / 6 / NORMA_FROTOR);
|
|
// vect_control.iqIq_zad = _IQ(IQ_OUT_NOM / NORMA_ACP);
|
|
// vect_control.iqId_zad = 0;// _IQ(789.0 / NORMA_ACP);
|
|
|
|
// set_pid_coeffs();
|
|
Idq_to_Udq_2_windings(vect_control.iqId_zad, vect_control.iqIq_zad, analog.iqId1, analog.iqIq1, &vect_control.iqUdKm1, &vect_control.iqUqKm1,
|
|
analog.iqId2, analog.iqIq2, &vect_control.iqUdKm2, &vect_control.iqUqKm2);
|
|
|
|
|
|
calc_tetta_Id(Frot, vect_control.iqId_zad, vect_control.iqIq_zad, &analog.tetta, &analog.Fsl, &analog.iqFstator, reset);
|
|
filter.Fsl = exp_regul_iq(koeff_Fs_filter, filter.Fsl, analog.Fsl);
|
|
|
|
// analog.tetta = _IQ(vect_control.theta);
|
|
analog.iqIq_zadan = vect_control.iqIq_zad;
|
|
|
|
// calcUdUqCompensation(Frot); //TODO: test on power after testing rotating
|
|
vect_control.iqUdKm1Out = vect_control.iqUdKm1;
|
|
vect_control.iqUqKm1Out = vect_control.iqUqKm1;
|
|
vect_control.iqUdKm2Out = vect_control.iqUdKm2;
|
|
vect_control.iqUqKm2Out = vect_control.iqUqKm2;
|
|
|
|
if(vect_control.iqUdKm1Out > K_MODUL_MAX_PART) { vect_control.iqUdKm1Out = K_MODUL_MAX_PART;}
|
|
if(vect_control.iqUdKm1Out < -K_MODUL_MAX_PART) { vect_control.iqUdKm1Out = -K_MODUL_MAX_PART;}
|
|
if(vect_control.iqUqKm1Out > K_MODUL_MAX_PART) { vect_control.iqUqKm1Out = K_MODUL_MAX_PART;}
|
|
if(vect_control.iqUqKm1Out < -K_MODUL_MAX_PART) { vect_control.iqUqKm1Out = -K_MODUL_MAX_PART;}
|
|
|
|
if(vect_control.iqUdKm2Out > K_MODUL_MAX_PART) { vect_control.iqUdKm2Out = K_MODUL_MAX_PART;}
|
|
if(vect_control.iqUdKm2Out < -K_MODUL_MAX_PART) { vect_control.iqUdKm2Out = -K_MODUL_MAX_PART;}
|
|
if(vect_control.iqUqKm2Out > K_MODUL_MAX_PART) { vect_control.iqUqKm2Out = K_MODUL_MAX_PART;}
|
|
if(vect_control.iqUqKm2Out < -K_MODUL_MAX_PART) { vect_control.iqUqKm2Out = -K_MODUL_MAX_PART;}
|
|
|
|
U_zad1 = _IQsqrt(_IQmpy(vect_control.iqUdKm1Out, vect_control.iqUdKm1Out) + _IQmpy(vect_control.iqUqKm1Out, vect_control.iqUqKm1Out));
|
|
U_zad2 = _IQsqrt(_IQmpy(vect_control.iqUdKm2Out, vect_control.iqUdKm2Out) + _IQmpy(vect_control.iqUqKm2Out, vect_control.iqUqKm2Out));
|
|
vect_control.iqUzad1 = U_zad1;
|
|
vect_control.iqUzad2 = U_zad2;
|
|
|
|
if (U_zad1 > vect_control.k_modul_max) {
|
|
if (vect_control.iqUqKm1Out > 0) {
|
|
vect_control.iqUqKm1Out = _IQsqrt(vect_control.k_modul_max_square - _IQmpy(vect_control.iqUdKm1Out, vect_control.iqUdKm1Out));
|
|
} else {
|
|
vect_control.iqUqKm1Out = -_IQsqrt(vect_control.k_modul_max_square - _IQmpy(vect_control.iqUdKm1Out, vect_control.iqUdKm1Out));
|
|
}
|
|
}
|
|
if (U_zad2 > vect_control.k_modul_max) {
|
|
if (vect_control.iqUqKm2Out > 0) {
|
|
vect_control.iqUqKm2Out = _IQsqrt(vect_control.k_modul_max_square - _IQmpy(vect_control.iqUdKm2Out, vect_control.iqUdKm2Out));
|
|
} else {
|
|
vect_control.iqUqKm2Out = -_IQsqrt(vect_control.k_modul_max_square - _IQmpy(vect_control.iqUdKm2Out, vect_control.iqUdKm2Out));
|
|
}
|
|
}
|
|
|
|
// Êîìïåíñàöèÿ êîëåáàíèÿ Iq ìåæäó äâóìÿ îáìîòêàìè
|
|
// vect_control.equial_Iq_Delta = analog.iqIq1 - analog.iqIq2;
|
|
// vect_control.equial_Iq_Out = _IQmpy(vect_control.equial_Iq_Delta,
|
|
// vect_control.equial_Iq_Proportional);
|
|
// vect_control.iqUqKm1Out -= vect_control.equial_Iq_Out;
|
|
// vect_control.iqUqKm2Out += vect_control.equial_Iq_Out;
|
|
|
|
// if (U_zad1 < K_MODUL_DEAD_TIME) {
|
|
// vect_control.iqUdKm1 = 0;
|
|
// vect_control.iqUqKm1 = 0;
|
|
// }
|
|
// if (U_zad2 < K_MODUL_DEAD_TIME) {
|
|
// vect_control.iqUdKm2 = 0;
|
|
// vect_control.iqUqKm2 = 0;
|
|
// }
|
|
|
|
vect_control.iqUzad1 = _IQsqrt(_IQmpy(vect_control.iqUdKm1Out, vect_control.iqUdKm1Out) + _IQmpy(vect_control.iqUqKm1Out, vect_control.iqUqKm1Out));
|
|
vect_control.iqUzad2 = _IQsqrt(_IQmpy(vect_control.iqUdKm2Out, vect_control.iqUdKm2Out) + _IQmpy(vect_control.iqUqKm2Out, vect_control.iqUqKm2Out));
|
|
|
|
analog_Udq_calc(vect_control.iqUdKm1, vect_control.iqUqKm1, vect_control.iqUdKm2, vect_control.iqUqKm2);
|
|
|
|
|
|
if(mode == 0) // Îáíóë-åì ïðè ïåðâîì âõîäå ïðè âêëþ÷åíèè
|
|
{
|
|
U_zad1 = K_MODUL_MIN;
|
|
U_zad2 = K_MODUL_MIN;
|
|
vect_control.iqUdKm1Out = 0;
|
|
vect_control.iqUqKm1Out = 0;
|
|
vect_control.iqUdKm2Out = 0;
|
|
vect_control.iqUqKm2Out = 0;
|
|
// teta_st = analog.tetta;
|
|
}
|
|
|
|
a.iqk1 = vect_control.iqUzad1;
|
|
// analog.iqUdKm = Ud_zad;
|
|
// analog.iqUqKm = Uq_zad;
|
|
// analog.iqId_zad = vect_control.iqId_zad;
|
|
|
|
|
|
if(mode && f.Go && calcPWMtimes)
|
|
{
|
|
test_calc_dq_pwm24(vect_control.iqUdKm1Out, vect_control.iqUqKm1Out, vect_control.iqUdKm2Out, vect_control.iqUqKm2Out, analog.tetta,K_MODUL_MAX);
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
// #pragma DATA_SECTION(koeff_Iq_filter_slow,".fast_vars");
|
|
long koeff_Iq_filter_slow = _IQ(0.5); //_IQ(0.3);
|
|
// #pragma DATA_SECTION(koeff_Idq_filter,".fast_vars");
|
|
long koeff_Idq_filter = _IQ(0.15);
|
|
// #pragma DATA_SECTION(koeff_Uq_filter,".fast_vars");
|
|
long koeff_Uq_filter = 500;
|
|
// #pragma DATA_SECTION(koeff_Ud_filter,".fast_vars");
|
|
long koeff_Ud_filter = _IQ(0.5);
|
|
|
|
|
|
// #pragma CODE_SECTION(analog_dq_calc,".fast_run");
|
|
void analog_dq_calc()
|
|
{
|
|
ABC_TO_DQ abc_dq_converter = ABC_TO_DQ_DEFAULTS;
|
|
|
|
abc_dq_converter.Ia = analog.iqIa1_1;
|
|
abc_dq_converter.Ib = analog.iqIb1_1;
|
|
abc_dq_converter.Ic = analog.iqIc1_1;
|
|
abc_dq_converter.Tetta = analog.tetta;
|
|
abc_dq_converter.calc(&abc_dq_converter);
|
|
analog.iqId1 = abc_dq_converter.Id;
|
|
filter.iqId1 = exp_regul_iq(koeff_Idq_filter, filter.iqId1, analog.iqId1);
|
|
analog.iqIq1 = abc_dq_converter.Iq;
|
|
filter.iqIq1 = exp_regul_iq(koeff_Idq_filter, filter.iqIq1, analog.iqIq1);
|
|
// analog.iqIq1_filter = zad_intensiv_q(koeff_Iq_filter_slow, koeff_Iq_filter_slow,
|
|
// analog.iqIq1_filter, analog.iqIq1);
|
|
analog.iqIq1_filter = exp_regul_iq(koeff_Iq_filter_slow, analog.iqIq1_filter, analog.iqIq1);
|
|
|
|
abc_dq_converter.Ia = analog.iqIa2_1;
|
|
abc_dq_converter.Ib = analog.iqIb2_1;
|
|
abc_dq_converter.Ic = analog.iqIc2_1;
|
|
abc_dq_converter.Tetta = analog.tetta - winding_displacement;
|
|
abc_dq_converter.calc(&abc_dq_converter);
|
|
analog.iqId2 = abc_dq_converter.Id;
|
|
filter.iqId2 = exp_regul_iq(koeff_Idq_filter, filter.iqId2, analog.iqId2);
|
|
analog.iqIq2 = abc_dq_converter.Iq;
|
|
filter.iqIq2 = exp_regul_iq(koeff_Idq_filter, filter.iqIq2, analog.iqIq2);
|
|
// analog.iqIq2_filter = zad_intensiv_q(koeff_Iq_filter_slow, koeff_Iq_filter_slow,
|
|
// analog.iqIq2_filter, analog.iqIq2);
|
|
analog.iqIq2_filter = exp_regul_iq(koeff_Iq_filter_slow, analog.iqIq2_filter, analog.iqIq2);
|
|
|
|
if (_IQabs(analog.iqId1) < I_ZERO_LEVEL_IQ) { analog.iqId1 = 0; }
|
|
if (_IQabs(analog.iqIq1) < I_ZERO_LEVEL_IQ) { analog.iqIq1 = 0; }
|
|
if (_IQabs(analog.iqId2) < I_ZERO_LEVEL_IQ) { analog.iqId2 = 0; }
|
|
if (_IQabs(analog.iqIq2) < I_ZERO_LEVEL_IQ) { analog.iqIq2 = 0; }
|
|
|
|
// analog.iqPvsi1 = _IQmpy(_IQmpy(analog.iqIq1_filter, analog.iqUq1), 25165824L);
|
|
// analog.iqPvsi2 = _IQmpy(_IQmpy(analog.iqIq2_filter, analog.iqUq2), 25165824L);
|
|
|
|
analog.iqPvsi1 = _IQmpy(_IQmpy(analog.iqIq1_filter, _IQabs(analog.iqUq1)), 25165824L);
|
|
analog.iqPvsi2 = _IQmpy(_IQmpy(analog.iqIq2_filter, _IQabs(analog.iqUq2)), 25165824L);
|
|
|
|
// analog.iqM_1 = _IQdiv(analog.iqPvsi1, _IQmpy(rotor.iqW, CONST_IQ_2PI));
|
|
// analog.iqM_2 = _IQdiv(analog.iqPvsi2, _IQmpy(rotor.iqW, CONST_IQ_2PI));
|
|
// logpar.log11 = (int16)_IQtoIQ15(analog.iqIq1_filter);
|
|
}
|
|
|
|
// #pragma CODE_SECTION(analog_Udq_calc,".fast_run");
|
|
void analog_Udq_calc(_iq Ud1, _iq Uq1, _iq Ud2, _iq Uq2)
|
|
{
|
|
analog.iqUd1 = _IQmpy(Ud1, _IQmpy((filter.iqU_1_long + filter.iqU_2_long), 8388608L)); // 8388608 = _IQ(0.5)
|
|
analog.iqUq1 = _IQmpy(Uq1, _IQmpy((filter.iqU_1_long + filter.iqU_2_long), 8388608L));
|
|
analog.iqUd2 = _IQmpy(Ud2, _IQmpy((filter.iqU_3_long + filter.iqU_4_long), 8388608L));
|
|
analog.iqUq2 = _IQmpy(Uq2, _IQmpy((filter.iqU_3_long + filter.iqU_4_long), 8388608L));
|
|
|
|
filter.iqUd1 = exp_regul_iq(koeff_Ud_filter, filter.iqUd1, analog.iqUd1);
|
|
|
|
}
|
|
|
|
|
|
//#pragma CODE_SECTION(analog_dq_calc_const,".fast_run");
|
|
void analog_dq_calc_const()
|
|
{
|
|
ABC_TO_DQ abc_dq_converter = ABC_TO_DQ_DEFAULTS;
|
|
|
|
abc_dq_converter.Ia = analog.iqIa1_1;
|
|
abc_dq_converter.Ib = analog.iqIb1_1;
|
|
abc_dq_converter.Ic = analog.iqIc1_1;
|
|
abc_dq_converter.Tetta = analog.tetta; // svgen_pwm24_1.Alpha;
|
|
abc_dq_converter.calc(&abc_dq_converter);
|
|
analog.iqId1 = abc_dq_converter.Id;
|
|
analog.iqIq1 = abc_dq_converter.Iq;
|
|
analog.iqIq1_filter = zad_intensiv_q(koeff_Iq_filter_slow, koeff_Iq_filter_slow,
|
|
analog.iqIq1_filter, analog.iqIq1);
|
|
|
|
abc_dq_converter.Ia = analog.iqIa2_1;
|
|
abc_dq_converter.Ib = analog.iqIb2_1;
|
|
abc_dq_converter.Ic = analog.iqIc2_1;
|
|
abc_dq_converter.Tetta = analog.tetta; // svgen_pwm24_2.Alpha;
|
|
abc_dq_converter.calc(&abc_dq_converter);
|
|
analog.iqId2 = abc_dq_converter.Id;
|
|
analog.iqIq2 = abc_dq_converter.Iq;
|
|
analog.iqIq2_filter = zad_intensiv_q(koeff_Iq_filter_slow, koeff_Iq_filter_slow,
|
|
analog.iqIq2_filter, analog.iqIq2);
|
|
|
|
analog.iqPvsi1 = _IQmpy(_IQmpy(analog.iqIq1_filter, analog.iqUq1), 25165824L);
|
|
analog.iqPvsi2 = _IQmpy(_IQmpy(analog.iqIq2_filter, analog.iqUq2), 25165824L);
|
|
// analog.iqPvsi1 = _IQmpy(_IQmpy(analog.iqIq1, analog.iqUq1), 25165824L);
|
|
// analog.iqPvsi2 = _IQmpy(_IQmpy(analog.iqIq2, analog.iqUq2), 25165824L);
|
|
|
|
// analog.iqM_1 = _IQdiv(analog.iqPvsi1, _IQmpy(rotor.iqW, CONST_IQ_2PI));
|
|
// analog.iqM_2 = _IQdiv(analog.iqPvsi2, _IQmpy(rotor.iqW, CONST_IQ_2PI));
|
|
}
|
|
|
|
void calcUdUqCompensation(_iq Frot) {
|
|
_iq Uzpt = (filter.iqU_1_long + filter.iqU_2_long) >> 1;
|
|
_iq UdVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Ud_comp), analog.iqIq1);
|
|
_iq UqVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Uq_comp), analog.iqId1);
|
|
if (Uzpt != 0) {
|
|
vect_control.iqUdCompensation1 = -_IQdiv(UdVolt, Uzpt);
|
|
vect_control.iqUqCompensation1 = _IQdiv(UqVolt, Uzpt);
|
|
} else {
|
|
vect_control.iqUdCompensation1 = 0;
|
|
vect_control.iqUqCompensation1 = 0;
|
|
}
|
|
|
|
vect_control.iqUdKm1Out = vect_control.iqUdKm1 + vect_control.iqUdCompensation1;
|
|
vect_control.iqUqKm1Out = vect_control.iqUqKm1 + vect_control.iqUqCompensation1;
|
|
|
|
Uzpt = (filter.iqU_3_long + filter.iqU_4_long) >> 1;
|
|
UdVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Ud_comp), analog.iqIq2);
|
|
UqVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Uq_comp), analog.iqId2);
|
|
if (Uzpt != 0) {
|
|
vect_control.iqUdCompensation2 = -_IQdiv(UdVolt, Uzpt);
|
|
vect_control.iqUqCompensation2 = _IQdiv(UqVolt, Uzpt);
|
|
} else {
|
|
vect_control.iqUdCompensation2 = 0;
|
|
vect_control.iqUqCompensation2 = 0;
|
|
}
|
|
|
|
|
|
vect_control.iqUdKm2Out = vect_control.iqUdKm2 + vect_control.iqUdCompensation2;
|
|
vect_control.iqUqKm2Out = vect_control.iqUqKm2 + vect_control.iqUqCompensation2;
|
|
}
|
|
|
|
#define IQ_150_RPM 2097152 //150îá/ìèí
|
|
#define IQ_140_RPM 1957341 //140îá/ìèí
|
|
//#define IQ_135_RPM 1887436 //135îá/ìèí
|
|
#define IQ_125_RPM 1747626 //125îá/ìèí
|
|
#define IQ_120_RPM 1677721 //120îá/ìèí
|
|
#define IQ_115_RPM 1607816 //115îá/ìèí
|
|
#define IQ_110_RPM 1537911 //110îá/ìèí
|
|
|
|
#define K_MODUL_SWITCH_COS_FI 13925089 //83%
|
|
#define K_MODUL_SWITCH_COS_FI_2 14260633 //85%
|
|
#define K_MODUL_SWITCH_OFF 10905190 //65% //11744051 ~ 70% //12582912 ~ 75%
|
|
|
|
#define U200V 1118481
|
|
|
|
|
|
// #pragma CODE_SECTION(set_cos_tetta_calc_params,".fast_run1");
|
|
void set_cos_tetta_calc_params() {
|
|
|
|
_iq currentCos = _IQ(COS_FI);
|
|
_iq currentCosSq = _IQ(COS_FI * COS_FI);
|
|
static _iq cosFi_low_speed = _IQ(0.85);
|
|
static _iq cosFi_Sq_low_speed = _IQ(0.85 * 0.85);
|
|
static _iq cosFi_mid_speed = _IQ(COS_FI);
|
|
static _iq cosFi_Sq_mid_speed = _IQ(COS_FI * COS_FI);
|
|
static _iq cosFi_high_speed = _IQ(0.89);
|
|
static _iq cosFi_Sq_high_speed = _IQ(0.89 * 0.89);
|
|
static _iq stepChangeCos = _IQ(0.001);
|
|
static _iq kt_low_speed = _IQ(0.0045);
|
|
static _iq kt_over_140rpm = _IQ(0.0048);
|
|
static _iq kt_over_150rpm = _IQ(0.0049);
|
|
static _iq kt_single_inv = _IQ(0.0039);
|
|
|
|
_iq current_kt = _IQ(0.0045);
|
|
static _iq stepChangeKt =_IQ(0.00001);
|
|
|
|
static _iq addCompensateUd = _IQ(0.0004);
|
|
|
|
static unsigned int flag_high_Km = 0;
|
|
|
|
if (a.iqk1 < K_MODUL_SWITCH_OFF) {
|
|
flag_high_Km = 0;
|
|
}
|
|
|
|
// if (_IQabs(rotor.iqFout) > IQ_150_RPM) {
|
|
// currentCos = _IQ(0.9);
|
|
// currentCosSq = _IQ(0.9 * 0.9);
|
|
//// tetta_calc.k_t = _IQ(0.0049);
|
|
// current_kt = kt_over_150rpm; //_IQ(0.0049);
|
|
// } else
|
|
if (_IQabs(rotor.iqFout) > IQ_140_RPM || (a.iqk1 > K_MODUL_SWITCH_COS_FI_2)
|
|
|| flag_high_Km == 1) {
|
|
if (a.iqk1 > K_MODUL_SWITCH_COS_FI_2) {
|
|
flag_high_Km = 1;
|
|
}
|
|
currentCos = cosFi_high_speed;
|
|
currentCosSq = cosFi_Sq_high_speed;
|
|
// tetta_calc.k_t = _IQ(0.0048);
|
|
current_kt = kt_over_140rpm; //_IQ(0.0048);
|
|
} else if ((_IQabs(rotor.iqFout) > IQ_115_RPM && cos_fi.cos_fi_nom < _IQ(0.889))
|
|
|| (_IQabs(rotor.iqFout) < IQ_135_RPM && cos_fi.cos_fi_nom > _IQ(0.889))
|
|
|| (a.iqk1 > K_MODUL_SWITCH_COS_FI)) {
|
|
currentCos = cosFi_mid_speed;
|
|
currentCosSq = cosFi_Sq_mid_speed;
|
|
// tetta_calc.k_t = _IQ(0.0045);
|
|
current_kt = kt_low_speed; //_IQ(0.0045);
|
|
} else if (_IQabs(rotor.iqFout) < IQ_110_RPM) {
|
|
currentCos = cosFi_low_speed;
|
|
currentCosSq = cosFi_Sq_low_speed;
|
|
//(f.secondPChState != 4)
|
|
if(((analog.iqIm_1 > I_OUT_NOMINAL_IQ) || (analog.iqIm_2 > I_OUT_NOMINAL_IQ))
|
|
&& (f.secondPChState != 4)) {
|
|
current_kt = kt_single_inv; //_IQ(0.0039);
|
|
} else if ((analog.iqIm_1 < 9507089) || (analog.iqIm_2 < 9507089)) {
|
|
current_kt = kt_low_speed; //_IQ(0.0045);
|
|
}
|
|
}
|
|
|
|
if (analog.iqUd1 < -U200V || (a.iqk1 > K_MODUL_SWITCH_COS_FI_2)) {
|
|
current_kt += addCompensateUd; // _IQ(0.0004);
|
|
}
|
|
cos_fi.cos_fi_nom = zad_intensiv_q(stepChangeCos, stepChangeCos, cos_fi.cos_fi_nom, currentCos);
|
|
cos_fi.cos_fi_nom_squared = zad_intensiv_q(stepChangeCos, stepChangeCos, cos_fi.cos_fi_nom_squared, currentCosSq);
|
|
|
|
tetta_calc.k_t = zad_intensiv_q(stepChangeKt, stepChangeKt, tetta_calc.k_t, current_kt);
|
|
}
|
|
|
|
|
|
void limit_mzz_zad_power(_iq Frot)
|
|
{
|
|
Frot = labs(Frot);
|
|
|
|
if (vect_control.flag_reverse) {
|
|
// f.iq_mzz_zad = _IQ(1300.0/NORMA_MZZ);
|
|
f.iq_mzz_zad = _IQ(1100.0/NORMA_MZZ);
|
|
} else
|
|
if (Frot < 279620) //20 ob/min
|
|
{
|
|
// f.iq_mzz_zad = _IQ(1200.0/NORMA_MZZ);
|
|
f.iq_mzz_zad = _IQ(1400.0/NORMA_MZZ);
|
|
}
|
|
else if (Frot < 419430) //30 ob/min
|
|
{
|
|
f.iq_mzz_zad = _IQ(1400.0/NORMA_MZZ);
|
|
}
|
|
// else if(rotor.iqFout < 699050) //50 ob/min
|
|
else if (Frot < 838860) //60 ob/min
|
|
// else if (rotor.iqFout < 978670) //70 ob/min
|
|
{
|
|
// f.iq_mzz_zad = _IQ(1800.0/NORMA_MZZ);
|
|
f.iq_mzz_zad = _IQ(2000.0/NORMA_MZZ);
|
|
}
|
|
else
|
|
{
|
|
f.iq_mzz_zad = _IQ(2000.0/NORMA_MZZ);
|
|
// f.iq_mzz_zad = _IQ(1500.0/NORMA_MZZ);
|
|
}
|
|
}
|
|
|
|
|