340 lines
7.2 KiB
C
340 lines
7.2 KiB
C
/*
|
|
|
|
ÖÍÈÈ ÑÝÒ (ñ) 2002 ã.
|
|
|
|
Processor: TMS320C32
|
|
|
|
Filename: vector_troll.h
|
|
|
|
ÑÈÑÒÅÌÀ ÂÅÊÒÎÐÍÎÃÎ ÓÏÐÀÂËÅÍÈy
|
|
|
|
Edit date: 04-12-02
|
|
|
|
Function:
|
|
|
|
Revisions:
|
|
*/
|
|
#include "IQmathLib.h"
|
|
|
|
#include "DSP281x_Device.h" // DSP281x Headerfile Include File
|
|
#include "DSP281x_Examples.h" // DSP281x Examples Include File
|
|
#include <math.h>
|
|
|
|
|
|
#include "mathlib.h"
|
|
|
|
#include <params_norma.h>
|
|
|
|
|
|
|
|
_iq SQRT_32 = _IQ(0.8660254037844);
|
|
_iq CONST_23 = _IQ(2.0/3.0);
|
|
_iq CONST_15 = _IQ(1.5);
|
|
|
|
|
|
|
|
|
|
#define real float
|
|
|
|
float my_satur_float(float Input, float Positive, float Negative)
|
|
{
|
|
|
|
if (Input>=Positive) Input=Positive;
|
|
if (Input<=Negative) Input=Negative;
|
|
|
|
return Input;
|
|
}
|
|
|
|
int my_satur_int(int Input, int Positive, int Negative)
|
|
{
|
|
|
|
if (Input>=Positive) Input=Positive;
|
|
if (Input<=Negative) Input=Negative;
|
|
|
|
return Input;
|
|
}
|
|
|
|
long my_satur_long(long Input, long Positive, long Negative)
|
|
{
|
|
|
|
if (Input>=Positive) Input=Positive;
|
|
if (Input<=Negative) Input=Negative;
|
|
|
|
return Input;
|
|
}
|
|
/*
|
|
|
|
|
|
real pid_regul(real Kp_regul, real Tperiod_regul, real Minimum, real Maximum,
|
|
real yk, real *uk1, real *yk1, real *yzad, real *ek, real *ek1, real *ek2,
|
|
real d0, real d1, real d2)
|
|
{
|
|
real uk;
|
|
|
|
|
|
*ek = - yk + *yzad;
|
|
uk = *uk1 + Kp_regul * ( d0 * *ek + d1 * *ek1 + d2 * *ek2 );
|
|
|
|
if (uk>Maximum) uk=Maximum;
|
|
if (uk<Minimum) uk=Minimum;
|
|
|
|
|
|
*yk1 = yk;
|
|
*ek2 = *ek1;
|
|
*ek1 = *ek;
|
|
*uk1 = uk;
|
|
|
|
return uk;
|
|
}
|
|
|
|
|
|
real pid_regul2(real Kp_regul, real Tperiod_regul, real Minimum, real Maximum,
|
|
real yk, real *uk1, real *yk1, real *yk2, real *yzad,
|
|
real d0, real d1, real d2)
|
|
{
|
|
real uk;
|
|
|
|
|
|
uk = *uk1 + Kp_regul * ( *yk1 - yk + d1 * ( *yzad - yk ) + d2 * ( 2 * *yk1 - *yk2 - yk ) );
|
|
|
|
|
|
if (uk>Maximum) uk=Maximum;
|
|
if (uk<Minimum) uk=Minimum;
|
|
|
|
*yk2 = *yk1;
|
|
*yk1 = yk;
|
|
|
|
*uk1 = uk;
|
|
|
|
return uk;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
real pi_regul(real Kp_regul, real Tintegral_regul, real Tperiod_regul, real Minimum, real Maximum, real InpVar, real *VarIntegral)
|
|
{
|
|
real VarProp, VarOut;
|
|
*VarIntegral += InpVar*Tperiod_regul/Tintegral_regul;
|
|
VarProp = InpVar*Kp_regul;
|
|
|
|
if (*VarIntegral>Maximum) *VarIntegral=Maximum;
|
|
if (*VarIntegral<Minimum) *VarIntegral=Minimum;
|
|
VarOut = *VarIntegral+VarProp;
|
|
if (VarOut>Maximum) VarOut=Maximum;
|
|
if (VarOut<Minimum) VarOut=Minimum;
|
|
return VarOut;
|
|
|
|
}
|
|
|
|
|
|
real exp_regul(real Tperiod_regul, real Texp_regul, real InpVarCurr, real InpVarInstant)
|
|
{
|
|
real VarOut;
|
|
|
|
VarOut = InpVarCurr + Tperiod_regul*(InpVarInstant-InpVarCurr)/Texp_regul;
|
|
|
|
return VarOut;
|
|
}
|
|
|
|
|
|
|
|
|
|
real zad_intensiv(real StepP, real StepN, real InpVarCurr, real InpVarInstant)
|
|
{
|
|
real deltaVar, VarOut;
|
|
|
|
deltaVar = InpVarInstant-InpVarCurr;
|
|
|
|
if ((deltaVar>StepP) || (deltaVar<-StepN))
|
|
{
|
|
if (deltaVar>0) InpVarCurr += StepP;
|
|
else InpVarCurr -= StepN;
|
|
}
|
|
else
|
|
InpVarCurr=InpVarInstant;
|
|
|
|
|
|
VarOut = InpVarCurr;
|
|
return VarOut;
|
|
}
|
|
*/
|
|
#pragma CODE_SECTION(zad_intensiv_q,".fast_run");
|
|
_iq zad_intensiv_q(_iq StepP, _iq StepN, _iq InpVarCurr, _iq InpVarInstant)
|
|
{
|
|
_iq deltaVar, VarOut;
|
|
|
|
deltaVar = InpVarInstant-InpVarCurr;
|
|
|
|
if ((deltaVar>StepP) || (deltaVar<-StepN))
|
|
{
|
|
if (deltaVar>0) InpVarCurr += StepP;
|
|
else InpVarCurr -= StepN;
|
|
}
|
|
else
|
|
InpVarCurr=InpVarInstant;
|
|
|
|
|
|
VarOut = InpVarCurr;
|
|
return VarOut;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
real pi_regul3(real Kp_regul, real ki_regul, real Tperiod_regul, real Minimum, real Maximum,
|
|
real InpVar, real *InpVarPrev, real *OutVarPrev)
|
|
{
|
|
real VarOut;
|
|
|
|
|
|
VarOut = Kp_regul*(ki_regul/Kp_regul*Tperiod_regul/2.0+1)*InpVar + Kp_regul*(ki_regul/Kp_regul*Tperiod_regul/2.0-1) * (*InpVarPrev) + *OutVarPrev;
|
|
|
|
if (VarOut>Maximum) VarOut=Maximum;
|
|
if (VarOut<Minimum) VarOut=Minimum;
|
|
|
|
|
|
*InpVarPrev = InpVar;
|
|
*OutVarPrev = VarOut;
|
|
|
|
|
|
return VarOut;
|
|
|
|
}
|
|
|
|
|
|
|
|
real pi_regul4(real Kp_regul, real ki_regul, real Tperiod_regul, real Minimum, real Maximum,
|
|
real InpVar, real *InpVarPrev, real *OutVarPrev)
|
|
{
|
|
real VarOut;
|
|
|
|
|
|
VarOut = Kp_regul*InpVar + Kp_regul*(ki_regul/Kp_regul*Tperiod_regul-1) * (*InpVarPrev) + *OutVarPrev;
|
|
|
|
if (VarOut>Maximum) VarOut=Maximum;
|
|
if (VarOut<Minimum) VarOut=Minimum;
|
|
|
|
*InpVarPrev = InpVar;
|
|
*OutVarPrev = VarOut;
|
|
|
|
|
|
return VarOut;
|
|
|
|
}
|
|
|
|
|
|
/********************************************************************/
|
|
/* Ðàñ÷åò ìîäóëy òîêà èç ïîêàçàíèé òðåõ ôàç */
|
|
/********************************************************************/
|
|
#pragma CODE_SECTION(im_calc,".fast_run");
|
|
_iq im_calc( _iq ia, _iq ib, _iq ic)
|
|
{
|
|
_iq isa,isb, t;
|
|
|
|
|
|
isa = _IQmpy(CONST_15,ia);
|
|
isb = _IQmpy(SQRT_32,ib-ic );
|
|
|
|
// ( _IQ19mpy(SQRT_32, _IQ15toIQ19(ib)) - _IQ15mpy(SQRT_32, _IQ15toIQ19(ic)) );
|
|
|
|
t = _IQmag(isa,isb);
|
|
t = _IQmpy(CONST_23,t);
|
|
|
|
return (t);
|
|
|
|
}
|
|
|
|
float exp_regul(float Tperiod_regul, float Texp_regul, float InpVarCurr, float InpVarInstant)
|
|
{
|
|
float VarOut;
|
|
|
|
VarOut = InpVarCurr + Tperiod_regul*(InpVarInstant-InpVarCurr)/Texp_regul;
|
|
|
|
return VarOut;
|
|
}
|
|
|
|
_iq calc_rms_array_simple(RMS_CALC_ARRAY *v) {
|
|
_iq summ_squares = 0;
|
|
int i = 0;
|
|
if (v->data_array == 0) return 0;
|
|
for (i = 0; i < v->size_array; ++i) {
|
|
summ_squares += _IQmpy(v->data_array[i], v->data_array[i]);
|
|
}
|
|
return _IQsqrt(summ_squares / v->size_array);
|
|
}
|
|
|
|
//_iq calc_rms_array_var_period(RMS_CALC_ARRAY_THINNING *v) {
|
|
// _iq summ_squares = 0;
|
|
// int i = 0;
|
|
// int count_elem = 0;
|
|
// if (v->signal_period > v->size_array) {
|
|
// v->signal_period = v->size_array;
|
|
// }
|
|
// count_elem = v->signal_period;
|
|
// i = v->last_elem_position;
|
|
// while (count_elem > 0) {
|
|
// summ_squares += _IQmpy(v->data_array[i], v->data_array[i]);
|
|
// i -= 1;
|
|
// if (i < 0) { i = v->size_array - 1; }
|
|
// count_elem -= 1;
|
|
//
|
|
// }
|
|
// v->Out_rms = _IQsqrt(summ_squares / v->signal_period);
|
|
// return v->Out_rms;
|
|
//}
|
|
|
|
_iq calc_rms_array_var_period_IQ15(RMS_CALC_ARRAY_THINNING *v) {
|
|
_iq16 summ_squares = 0;
|
|
int i = 0;
|
|
int count_elem = 0;
|
|
if (v->data_array == 0) {
|
|
return 0;
|
|
}
|
|
if (v->signal_period > v->size_array) {
|
|
v->signal_period = v->size_array;
|
|
}
|
|
count_elem = v->signal_period;
|
|
i = v->last_elem_position;
|
|
while (count_elem > 0) {
|
|
summ_squares += 0;//_IQ15mpy(v->data_array[i], v->data_array[i]);
|
|
i -= 1;
|
|
if (i < 0) { i = v->size_array - 1; }
|
|
count_elem -= 1;
|
|
|
|
}
|
|
v->Out_rms =_IQ15toIQ(_IQ15sqrt(summ_squares / v->signal_period));
|
|
return v->Out_rms;
|
|
}
|
|
|
|
float fast_round(float x)
|
|
{
|
|
float d;
|
|
long i;
|
|
|
|
|
|
i = (long)x;
|
|
|
|
if (x<0)
|
|
{
|
|
d = i - x;
|
|
if (d>=0.5)
|
|
i = i - 1;
|
|
}
|
|
else
|
|
{
|
|
d = x - i;
|
|
if (d>=0.5)
|
|
i = i + 1;
|
|
}
|
|
return (float)i;
|
|
|
|
}
|
|
|