593 lines
15 KiB
C
593 lines
15 KiB
C
/*
|
|
* alg_simple_scalar.c
|
|
*
|
|
* Created on: 26 èşí. 2020 ã.
|
|
* Author: Yura
|
|
*/
|
|
|
|
|
|
#include <alg_simple_scalar.h>
|
|
#include <edrk_main.h>
|
|
#include <log_to_mem.h>
|
|
#include <master_slave.h>
|
|
#include <math.h>
|
|
#include <params_alg.h>
|
|
#include <params_norma.h>
|
|
#include <project.h>
|
|
#include "log_to_mem.h"
|
|
#include "IQmathLib.h"
|
|
#include "math_pi.h"
|
|
#include "mathlib.h"
|
|
#include "params_pwm24.h"
|
|
|
|
|
|
|
|
#pragma DATA_SECTION(simple_scalar1,".slow_vars");
|
|
ALG_SIMPLE_SCALAR simple_scalar1 = ALG_SIMPLE_SCALAR_DEFAULT;
|
|
|
|
|
|
|
|
void init_simple_scalar(void)
|
|
{
|
|
simple_scalar1.mzz_add_1 = _IQ(MZZ_ADD_1/NORMA_MZZ);
|
|
simple_scalar1.mzz_add_2 = _IQ(MZZ_ADD_2/NORMA_MZZ);
|
|
simple_scalar1.poluses = _IQ(POLUS);
|
|
simple_scalar1.iq_mzz_max_for_fzad = _IQ(1000.0/NORMA_MZZ);
|
|
|
|
simple_scalar1.powerzad_add = _IQ(POWERZAD_ADD_MAX);
|
|
simple_scalar1.powerzad_dec = _IQ(POWERZAD_DEC);
|
|
|
|
simple_scalar1.k_freq_for_pid = _IQ(1.0);
|
|
simple_scalar1.k_freq_for_pid = _IQ(450.0/FREQ_PWM);
|
|
|
|
simple_scalar1.iq_add_kp_df = _IQ(ADD_KP_DF);
|
|
simple_scalar1.iq_add_ki_df = _IQ(ADD_KI_DF);
|
|
|
|
simple_scalar1.min_mzz_for_df = _IQ(MIN_MZZ_FOR_DF/NORMA_MZZ);
|
|
|
|
simple_scalar1.pidF_Kp = _IQ(PID_KP_F);
|
|
simple_scalar1.pidF_Ki = _IQ(PID_KI_F);
|
|
|
|
|
|
|
|
simple_scalar1.pidIm1.Kp=_IQ(PID_KP_IM);
|
|
simple_scalar1.pidIm1.Ki=_IQ(PID_KI_IM);
|
|
|
|
simple_scalar1.pidIm_Ki = simple_scalar1.pidIm1.Ki;
|
|
|
|
simple_scalar1.pidIm1.Kc=_IQ(PID_KC_IM);
|
|
simple_scalar1.pidIm1.Kd=_IQ(PID_KD_IM);
|
|
|
|
|
|
simple_scalar1.pidIm1.OutMax=_IQ(K_STATOR_MAX);
|
|
simple_scalar1.pidIm1.OutMin=_IQ(0);
|
|
|
|
//////////////
|
|
|
|
|
|
simple_scalar1.pidF.Kp=_IQ(PID_KP_F);
|
|
simple_scalar1.pidF.Ki=_IQ(PID_KI_F);
|
|
simple_scalar1.pidF.Kc=_IQ(PID_KC_F);
|
|
simple_scalar1.pidF.Kd=_IQ(PID_KD_F);
|
|
|
|
|
|
simple_scalar1.pidF.OutMax=_IQ(500/NORMA_MZZ);
|
|
simple_scalar1.pidF.OutMin=_IQ(0);
|
|
// iq_MAX_DELTA_pidF = _IQ(MAX_DELTA_pidF/NORMA_WROTOR);
|
|
/////////////////////////
|
|
// simple_scalar1.pidPower_Kp = _IQ(PID_KP_POWER);
|
|
// simple_scalar1.pidPower_Ki = _IQ(PID_KI_POWER);
|
|
|
|
|
|
// iq_add_kp_dpower = _IQ(ADD_KP_DPOWER);
|
|
// iq_add_ki_dpower = _IQ(ADD_KI_DPOWER);
|
|
|
|
simple_scalar1.pidPower.Kp=_IQ(PID_KP_POWER);
|
|
simple_scalar1.pidPower.Ki=_IQ(PID_KI_POWER);
|
|
simple_scalar1.pidPower.Ki = _IQmpy(simple_scalar1.pidPower.Ki, simple_scalar1.k_freq_for_pid); // ââîäèì êîğğåêöèş Ki ïî ÷àñòîòå ØÈÌà
|
|
|
|
simple_scalar1.pidPower.Kc=_IQ(PID_KC_POWER);
|
|
simple_scalar1.pidPower.Kd=_IQ(PID_KD_POWER);
|
|
|
|
simple_scalar1.pidPower.OutMax=_IQ(500/NORMA_MZZ);
|
|
simple_scalar1.pidPower.OutMin=_IQ(0);
|
|
|
|
|
|
simple_scalar1.iq_spad_k = _IQ(0.993); //0.993 ~ 0.4 sek äî 5%
|
|
|
|
|
|
// ìèí. ñêîëüæåíèå
|
|
simple_scalar1.min_bpsi = _IQ(0.05/NORMA_FROTOR);
|
|
|
|
}
|
|
|
|
/***************************************************************/
|
|
/* Ï/ï óïğàâëåíèy ïñåâäî âåêòîğ 3 -
|
|
vector_moment(real Frot, - îáîğîòû ğîòîğà
|
|
real fzad, - çàäàííûå îáîğîòû ğîòîğà
|
|
real mzz_zad, - ìàêñèìàëüíûé âîçìîæíûé ìîìåíò-îãğàíè÷åíèå òîêà
|
|
real *Fz, - ğåçóëüòàò ğàñ÷åòà - ÷àñòîòà íàïğyæåíèy â ñòàòîğå
|
|
real *Uz1, - ğåçóëüòàò ğàñ÷åòà - êîıô. ìîäóëyöèè â 1 îáìîòêå ñòàòîğà
|
|
real *Uz2) - ğåçóëüòàò ğàñ÷åòà - êîıô. ìîäóëyöèè â 2 îáìîòêå ñòàòîğà
|
|
|
|
|
|
Èñïîëüçóåòñy ğåãóëyòîğ ñêîğîñòè, êîòîğûé âûïîëíyåò çàäàíèå fzad ïî ñêîğîñòè
|
|
|
|
Çàâîäèòñy ïîñòîyííîå ñêîëüæåíèå = 0.5
|
|
|
|
Èäåò ğàñ÷åò íàïğyæåíèy ÷åğåç ìîäóëü òîêà ïî îäíîé èç 3-õ ôàçíîé ñòîåê.
|
|
Çàìûêàåòñy îáğàòíày ñâyçü ïî îáîğîòàì */
|
|
/****************************************************************/
|
|
//#pragma CODE_SECTION(simple_scalar,".fast_run");
|
|
void simple_scalar(int n_alg, int n_wind_pump,
|
|
_iq Frot_pid,_iq Frot, _iq fzad,
|
|
_iq iqKoefOgran,
|
|
_iq mzz_zad, _iq bpsi_const,
|
|
_iq iqIm, _iq iqUin, _iq Iin, _iq powerzad, _iq power_pid,
|
|
_iq Izad_from_master, int master,
|
|
_iq *Fz, _iq *Uz1, _iq *Uz2, _iq *Izad_out)
|
|
{
|
|
|
|
_iq mzz, dF, dI1, Izad, Uz_t1, Kpred_Ip, pred_Ip;//, znak_moment;
|
|
_iq dI2, Uz_t2;
|
|
|
|
_iq pkk=0,ikk=0;
|
|
_iq Im_regul=0;
|
|
|
|
|
|
static _iq bpsi=0;
|
|
// static _iq IQ_POLUS=0;
|
|
|
|
|
|
static _iq mzz_zad_int=0;
|
|
static _iq mzzi=0;
|
|
|
|
static _iq I1_i=0;
|
|
static _iq I2_i=0;
|
|
|
|
static _iq Im1=0;
|
|
static _iq Im2=0;
|
|
|
|
static _iq Uze_t1=0;
|
|
static _iq Uze_t2=0;
|
|
|
|
// static _iq fzad_ogr=0;
|
|
|
|
|
|
|
|
// static _iq koef_Uz_t_filter=0;
|
|
static _iq dI1_prev=0;
|
|
static _iq Uz_t1_prev=0;
|
|
|
|
static _iq dF_prev = 0;
|
|
static _iq mzz_prev = 0;
|
|
|
|
// static _iq mzz_add_1, mzz_add_2;
|
|
|
|
static _iq fzad_int=0;//, fzad_add_max;//,iq_mzz_max_for_fzad ;
|
|
static _iq fzad_add=0; //fzad_dec
|
|
_iq halfIm1, halfIm2;
|
|
|
|
static _iq powerzad_int=0, powerzad_add_max=0 ;
|
|
//powerzad_dec powerzad_add
|
|
// static _iq koef_bpsi=0;
|
|
// static _iq min_bpsi=0;
|
|
static int flag_uz_t1=0;
|
|
|
|
// static _iq correct_err=0;
|
|
// static _iq iq_dF_min1=0;
|
|
// static _iq iq_dF_min2=0;
|
|
_iq pred_dF,Kpred_dF;
|
|
static _iq dF_PREDEL_LEVEL2 = 0,dF_PREDEL_LEVEL1=0;
|
|
_iq Uze_ogr=0;
|
|
|
|
// static _iq iq_spad_k=1;
|
|
static _iq myq_temp=0;
|
|
// static _iq mzz_int_level1_on_F=0;
|
|
|
|
|
|
// mzz = _IQsat(mzz,mzz_zad_int,0);
|
|
|
|
|
|
simple_scalar1.mzz_zad = mzz_zad;
|
|
simple_scalar1.Izad_from_master = Izad_from_master;
|
|
|
|
|
|
/* óñòàíàâëèâàåì íà÷àëüíûå óñëîâèy âñåõ ğåãóëyòîğîâ */
|
|
if ( (Frot==0) && (fzad==0) )
|
|
{
|
|
mzzi = 0;
|
|
fzad_int = 0;
|
|
powerzad_int = 0;
|
|
|
|
}
|
|
|
|
if (mzz_zad==0)
|
|
{
|
|
mzz=0;
|
|
I1_i=0;
|
|
mzzi=0;
|
|
mzz_zad_int = 0;
|
|
fzad_int = 0;
|
|
powerzad_int = 0;
|
|
|
|
simple_scalar1.pidIm1.Up1 = 0;
|
|
simple_scalar1.pidIm1.Ui = 0;
|
|
|
|
simple_scalar1.pidF.Up1 = 0;
|
|
simple_scalar1.pidF.Ui = 0;
|
|
|
|
simple_scalar1.pidPower.Up1 = 0;
|
|
simple_scalar1.pidPower.Ui = 0;
|
|
|
|
Uze_t1=0;
|
|
Uze_t2=0;
|
|
|
|
dI1_prev = 0;
|
|
Uz_t1_prev = 0;
|
|
|
|
dF_prev = 0;
|
|
mzz_prev = 0;
|
|
|
|
|
|
// çàäàò÷èê èíòåíñèâíîñòè fzad
|
|
fzad_add = _IQ(FZAD_ADD_MAX/NORMA_FROTOR);
|
|
// çàäàò÷èê èíòåíñèâíîñòè fzad ïî ìèíóñó
|
|
// fzad_dec = _IQ(FZAD_DEC/NORMA_FROTOR);
|
|
//
|
|
//
|
|
// çàäàò÷èê èíòåíñèâíîñòè mzz_max
|
|
// iq_mzz_max_for_fzad = _IQ(1000.0/NORMA_MZZ);
|
|
|
|
|
|
// êîıô. ôèëüòğà Uz_t_filter
|
|
// koef_Uz_t_filter = _IQ(0.001/0.5); //0.0333
|
|
|
|
|
|
|
|
// êîıô. ğàñ÷åòà ñêîëüæåíèå îò mzz
|
|
// koef_bpsi = _IQ((0.6/NORMA_WROTOR)/(200.0/NORMA_MZZ));
|
|
|
|
flag_uz_t1=0;
|
|
|
|
|
|
// êîıô. óñèëåíèß äëß èíòåãğàëüíîãî àäàïòèâíîãî êîıô. â ğåãóëßòîğå ñêîğîñòè
|
|
// correct_err = _IQ(2.5/NORMA_WROTOR);
|
|
|
|
// ìèí. óğîâåíü äëß ğàáîòû êîıô. óñèëåíèß äëß èíòåãğàëüíîãî àäàïòèâíîãî êîıô. â ğåãóëßòîğå ñêîğîñòè
|
|
// iq_dF_min1 = _IQ(1.0/NORMA_WROTOR);
|
|
|
|
// iq_dF_min2 = _IQ(1.5/NORMA_WROTOR);
|
|
|
|
// êîıô. çàäàíèß ñêîğîñòè ñïàäà Km
|
|
// iq_spad_k = _IQ(0.993); //0.993 ~ 0.4 sek äî 5%
|
|
|
|
// iq_spad_k = _IQ(0.9965); //0.993 ~ 0.4 sek äî 5%
|
|
|
|
|
|
// dF_PREDEL_LEVEL1 = _IQ(0.5/NORMA_WROTOR);
|
|
// dF_PREDEL_LEVEL2 = _IQ(1.5/NORMA_WROTOR);
|
|
|
|
// mzz_int_level1_on_F = _IQ(1.0/NORMA_WROTOR);
|
|
// mzz_int_level2_on_F = _IQ(1.5/NORMA_WROTOR);
|
|
|
|
|
|
}
|
|
|
|
|
|
/* çàäàò÷èê èíòåíñèâíîñòè ìîìåíòà */
|
|
if (n_alg==1)
|
|
{
|
|
|
|
mzz_zad_int = zad_intensiv_q(simple_scalar1.mzz_add_2, simple_scalar1.mzz_add_2, mzz_zad_int, mzz_zad);
|
|
|
|
// if (Frot_pid>mzz_int_level1_on_F)
|
|
// mzz_zad_int = zad_intensiv_q(mzz_add_1, mzz_add_1, mzz_zad_int, mzz_zad);
|
|
// else
|
|
// mzz_zad_int = zad_intensiv_q(mzz_add_2, mzz_add_2, mzz_zad_int, mzz_zad);
|
|
|
|
}
|
|
|
|
|
|
if (n_alg==2)
|
|
mzz_zad_int = zad_intensiv_q(simple_scalar1.mzz_add_2, simple_scalar1.mzz_add_2, mzz_zad_int, mzz_zad);
|
|
|
|
// myq_temp = _IQdiv(mzz_zad, simple_scalar1.iq_mzz_max_for_fzad);
|
|
|
|
// myq_temp = _IQmpy( myq_temp, fzad_add_max);
|
|
|
|
// fzad_add = myq_temp;
|
|
|
|
|
|
fzad_int = zad_intensiv_q(fzad_add, fzad_add, fzad_int, fzad);
|
|
|
|
powerzad_int = zad_intensiv_q(simple_scalar1.powerzad_add, simple_scalar1.powerzad_add, powerzad_int, powerzad);
|
|
|
|
if (n_alg==1)
|
|
{
|
|
/* ğåãóëyòîğ ñêîğîñòè */
|
|
if (mzz_zad_int>=0)
|
|
{
|
|
dF = fzad_int - Frot_pid;
|
|
|
|
////////// Power PI //////////////
|
|
|
|
simple_scalar1.pidPower.OutMax=mzz_zad;
|
|
|
|
// pidPower.Kp = _IQmpy( _IQdiv(iq_add_kp_dpower, _IQsat(mzz_zad,mzz_zad,MIN_MZZ_FOR_DPOWER)), pidPower_Kp);
|
|
// pidPower.Ki = _IQmpy( _IQdiv(iq_add_ki_dpower, _IQsat(mzz_zad,mzz_zad,MIN_MZZ_FOR_DPOWER)), pidPower_Ki);
|
|
|
|
// simple_scalar1.pidPower.Ki = _IQmpy(simple_scalar1.pidPower.Ki, simple_scalar1.k_freq_for_pid);
|
|
|
|
|
|
simple_scalar1.pidPower.Ref = powerzad_int;
|
|
|
|
simple_scalar1.pidPower.Fdb = power_pid;
|
|
simple_scalar1.pidPower.calc(&simple_scalar1.pidPower);
|
|
|
|
|
|
// Saturate the integral output
|
|
|
|
if (simple_scalar1.pidPower.Ui > simple_scalar1.pidPower.OutMax)
|
|
simple_scalar1.pidPower.Ui = simple_scalar1.pidPower.OutMax;
|
|
else if (simple_scalar1.pidPower.Ui < simple_scalar1.pidPower.OutMin)
|
|
simple_scalar1.pidPower.Ui = simple_scalar1.pidPower.OutMin;
|
|
|
|
|
|
//////////////////////////////
|
|
//////////////////////////////
|
|
|
|
|
|
// îãğàíè÷åíèå ìàêñ. çíà÷èíèß âûõîäà ğåãóëßòîğà
|
|
// pidF.OutMax=mzz_zad_int;
|
|
// èëè òàê
|
|
|
|
simple_scalar1.pidF.OutMax = simple_scalar1.pidPower.Out;
|
|
|
|
// pidF.OutMax = mzz_zad;
|
|
|
|
simple_scalar1.pidF.Kp = _IQmpy( _IQdiv(simple_scalar1.iq_add_kp_df, _IQsat(mzz_zad,mzz_zad,simple_scalar1.min_mzz_for_df)), simple_scalar1.pidF_Kp);
|
|
simple_scalar1.pidF.Ki = _IQmpy( _IQdiv(simple_scalar1.iq_add_ki_df, _IQsat(mzz_zad,mzz_zad,simple_scalar1.min_mzz_for_df)), simple_scalar1.pidF_Ki);
|
|
|
|
simple_scalar1.pidF.Ki = _IQmpy(simple_scalar1.pidF.Ki,simple_scalar1.k_freq_for_pid);
|
|
|
|
/////////////////////////
|
|
|
|
// if (_IQabs(dF)<iq_dF_min1)
|
|
// {
|
|
// dF=iq_dF_min1;
|
|
// m.m1.bit.w_rotor_ust = 1;
|
|
// }
|
|
//
|
|
// if (_IQabs(dF)>iq_dF_min2)
|
|
// {
|
|
// m.m1.bit.w_rotor_ust = 0;
|
|
// }
|
|
//////////////////////////////////
|
|
|
|
// áåç êîğğåêöèé dF
|
|
simple_scalar1.pidF.Ref = fzad_int;
|
|
|
|
simple_scalar1.pidF.Fdb = Frot_pid;
|
|
simple_scalar1.pidF.calc(&simple_scalar1.pidF);
|
|
|
|
|
|
// Saturate the integral output
|
|
|
|
if (simple_scalar1.pidF.Ui > simple_scalar1.pidF.OutMax)
|
|
simple_scalar1.pidF.Ui = simple_scalar1.pidF.OutMax;
|
|
else if (simple_scalar1.pidF.Ui < simple_scalar1.pidF.OutMin)
|
|
simple_scalar1.pidF.Ui = simple_scalar1.pidF.OutMin;
|
|
/////////////////////////////////////
|
|
|
|
mzz = simple_scalar1.pidF.Out;
|
|
|
|
///////////////////////////////////////
|
|
|
|
|
|
|
|
// çàäàò÷èê èíòåíñèâíîñòè íà òîê
|
|
// mzz = zad_intensiv_q(mzz_add_2, mzz_add_2, mzz, pidF.Out);
|
|
|
|
// mzzi = zad_intensiv_q(mzz_add_2, mzz_add_2, mzzi, mzz);
|
|
|
|
// îãğàíè÷èëè äèàïàçîí mzz
|
|
mzz = _IQsat(mzz,mzz_zad_int,0);
|
|
|
|
}
|
|
else
|
|
{
|
|
mzz = 0;
|
|
}
|
|
|
|
}
|
|
|
|
if (n_alg==2)
|
|
{
|
|
mzz = mzz_zad_int;
|
|
}
|
|
|
|
|
|
if (master == MODE_SLAVE)
|
|
{
|
|
mzz = Izad_from_master;
|
|
// îãğàíè÷èëè äèàïàçîí mzz
|
|
mzz = _IQsat(mzz,mzz_zad_int,0);
|
|
}
|
|
|
|
|
|
*Izad_out = mzz;
|
|
/* ïğè ïğåâûøåíèè òîêà íåêîòîğîãî ïîğîãîâîãî çíà÷åíèy I_PREDEL_LEVEL1
|
|
íà÷èíàåì ëèíåéíî ñáàâëyòü ìîìåíò */
|
|
|
|
/*
|
|
pred_Ip = (filter.I_3+filter.I_6)-I_PREDEL_LEVEL1;
|
|
|
|
|
|
if (pred_Ip<0)
|
|
Kpred_Ip=0.0; // âñå â íîğìå
|
|
else
|
|
{
|
|
// ïîğà îãğàíè÷èâàòü ìîìåíò
|
|
if (pred_Ip>=(I_PREDEL_LEVEL2-I_PREDEL_LEVEL1))
|
|
Kpred_Ip=1;
|
|
else
|
|
Kpred_Ip = pred_Ip/(I_PREDEL_LEVEL2-I_PREDEL_LEVEL1);
|
|
|
|
}
|
|
|
|
// à âîò è îãğàíè÷åíèå
|
|
Izad = mzz * (1-Kpred_Ip);
|
|
*/
|
|
|
|
|
|
iqKoefOgran = _IQsat(iqKoefOgran,CONST_IQ_1,0);
|
|
Izad = _IQmpy(mzz, iqKoefOgran);
|
|
|
|
// if ((n_alg==1) || (n_alg==2))
|
|
// {
|
|
//
|
|
// Im1 = iqIm_1;
|
|
// Im2 = iqIm_2;
|
|
//
|
|
// if (n_wind_pump==0) // ğàáîòà ïî äâóì îáìîòêàì
|
|
// {
|
|
//
|
|
// halfIm1 = Im1 >> 1;
|
|
// halfIm2 = Im2 >> 1;
|
|
//
|
|
// if (Im1>halfIm2) //if (Im1>IQdiv(Im2,_IQ(2.0)))
|
|
// {
|
|
// Im_regul=Im1;
|
|
// simple_scalar1.UpravIm1=1;
|
|
// simple_scalar1.UpravIm2=0;
|
|
// }
|
|
// else
|
|
// {
|
|
// if (Im2>halfIm1)
|
|
// {
|
|
// Im_regul=Im2;
|
|
// simple_scalar1.UpravIm2=1;
|
|
// simple_scalar1.UpravIm1=0;
|
|
// }
|
|
// else
|
|
// {
|
|
// Im_regul=Im1; //Im1
|
|
// simple_scalar1.UpravIm1=1;//1
|
|
// simple_scalar1.UpravIm2=0;//0
|
|
// }
|
|
// }
|
|
// }
|
|
//
|
|
// if (n_wind_pump==1) // íà íàñîñå 1 îáìîòêà çíà÷èò áåğåì òîê ñ 2-îé
|
|
// {
|
|
// Im_regul=Im2;
|
|
// simple_scalar1.UpravIm1=0;
|
|
// simple_scalar1.UpravIm2=1;
|
|
// }
|
|
//
|
|
// if (n_wind_pump==2) // íà íàñîñå 2 îáìîòêà çíà÷èò áåğåì òîê ñ 1-îé
|
|
// {
|
|
// Im_regul=Im1;
|
|
// simple_scalar1.UpravIm1=1;
|
|
// simple_scalar1.UpravIm2=0;
|
|
// }
|
|
|
|
Im_regul = iqIm;
|
|
|
|
simple_scalar1.Im_regul = Im_regul;
|
|
simple_scalar1.Izad = Izad;
|
|
|
|
dI1 = (Izad - Im_regul );
|
|
|
|
simple_scalar1.pidIm1.Ki = simple_scalar1.pidIm_Ki;
|
|
simple_scalar1.pidIm1.Ki = _IQmpy(simple_scalar1.pidIm1.Ki,simple_scalar1.k_freq_for_pid);
|
|
|
|
|
|
simple_scalar1.pidIm1.Ref = _IQdiv(Izad,iqUin);
|
|
simple_scalar1.pidIm1.Fdb = _IQdiv(Im_regul,iqUin);
|
|
simple_scalar1.pidIm1.calc(&simple_scalar1.pidIm1);
|
|
|
|
|
|
Uz_t1 = simple_scalar1.pidIm1.Out;
|
|
|
|
// îãğàíè÷åíèå ñïàäà Km
|
|
if (Uz_t1<Uze_t1)
|
|
{
|
|
Uze_ogr =_IQmpy(Uze_t1, simple_scalar1.iq_spad_k);
|
|
if (Uze_ogr>Uz_t1) Uze_t1 = Uze_ogr;
|
|
else
|
|
Uze_t1 = Uz_t1;
|
|
}
|
|
else
|
|
{
|
|
Uze_t1 = Uz_t1;
|
|
}
|
|
|
|
Uze_t1 = _IQsat(Uze_t1,simple_scalar1.pidIm1.OutMax,0);
|
|
|
|
// }
|
|
|
|
|
|
/* ğåçóëüòàòû îòäàåì íàğóæó */
|
|
*Uz1 = Uze_t1;
|
|
*Uz2 = Uze_t1;
|
|
|
|
|
|
|
|
|
|
|
|
bpsi = bpsi_const;
|
|
|
|
// ñêîëüæ. ~ ìîìåíòó
|
|
// bpsi = _IQmpy(koef_bpsi,mzz);
|
|
|
|
bpsi = _IQsat(bpsi,bpsi_const, simple_scalar1.min_bpsi);
|
|
|
|
#ifdef BAN_ROTOR_REVERS_DIRECT
|
|
// èñïîëüçóåì çàùèòó îò íåïğàâèëüíîãî âğàùåíèß
|
|
if (analog.filter_direct_rotor==-1)
|
|
// êğóòèìñß â äğóãóş ñòîğîíó, ïîıòîìó ïûòàåìñß ïåğåéòè íà ñêîëüæåíèè â íîğìàëüíîå âğàùåíèå
|
|
*Fz = bpsi;
|
|
else
|
|
// âñå íîğìàëüíî, íàïğàâëåíèå ïğàâèëüíîå
|
|
*Fz = _IQmpy(Frot,IQ_POLUS) + bpsi;
|
|
|
|
#else
|
|
|
|
*Fz = _IQmpy(Frot,simple_scalar1.poluses) + bpsi; /* bpsi - ñêîëüæåíèå, áåğåì ïîêà
|
|
êîíñòàíòîé õîòy òîæå äîëæåí ğåãóëèğîâàòüñy */
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
// if (n_alg==2)
|
|
// {
|
|
//
|
|
// *Fz = fzad_provorot;
|
|
// /* bpsi - ñêîëüæåíèå, áåğåì ïîêà
|
|
// êîíñòàíòîé õîòy òîæå äîëæåí ğåãóëèğîâàòüñy */
|
|
// }
|
|
|
|
|
|
// logpar.log1 = (int16)(_IQtoIQ15(Izad));
|
|
// logpar.log2 = (int16)(_IQtoIQ15(mzz_zad));//(int16)(_IQtoIQ15(Uze_t1));
|
|
// logpar.log3 = (int16)(_IQtoIQ15(fzad_int));
|
|
// logpar.log4 = (int16)(_IQtoIQ15(simple_scalar1.pidF.Ui));
|
|
// logpar.log5 = (int16)(_IQtoIQ14(simple_scalar1.pidF.Up));
|
|
// logpar.log6 = (int16)(_IQtoIQ14(simple_scalar1.pidF.SatErr));
|
|
// logpar.log7 = (int16)(_IQtoIQ15(mzz_zad_int));
|
|
// logpar.log8 = (int16)(_IQtoIQ15(simple_scalar1.pidIm1.Ref));
|
|
// logpar.log9 = (int16)(_IQtoIQ15(simple_scalar1.pidIm1.Fdb));
|
|
// logpar.log10 = (int16)(_IQtoIQ15(simple_scalar1.pidIm1.Ui));
|
|
// logpar.log11 = (int16)(_IQtoIQ15(simple_scalar1.pidIm1.Up));
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|