181 lines
2.8 KiB
C
181 lines
2.8 KiB
C
|
#include "IQmathLib.h" // Include header for IQmath library
|
||
|
|
|
||
|
|
#include "smooth.h"
|
||
|
|
#include "math_pi.h"
|
||
|
|
#include "math_pi.h"
|
||
|
|
|
||
|
|
|
||
|
|
#define SIZE_SMOOTH_INPUT 180
|
||
|
|
|
||
|
|
|
||
|
|
#pragma CODE_SECTION(my_mean,".fast_run");
|
||
|
|
_iq23 my_mean(int cnt, SMOOTH *v)
|
||
|
|
{
|
||
|
|
_iq23 summ = 0;
|
||
|
|
int start;
|
||
|
|
|
||
|
|
|
||
|
|
start = v->current_pos_buf_input;
|
||
|
|
if (start==0)
|
||
|
|
start = (MAX_SIZE_SMOOTH_INPUT-1);
|
||
|
|
|
||
|
|
while(cnt>0)
|
||
|
|
{
|
||
|
|
cnt--;
|
||
|
|
start--;
|
||
|
|
summ += v->buf_input[start];
|
||
|
|
if (start==0)
|
||
|
|
start = (MAX_SIZE_SMOOTH_INPUT-1);
|
||
|
|
}
|
||
|
|
return summ;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
void smooth_init(SMOOTH *v)
|
||
|
|
{
|
||
|
|
int i=0;
|
||
|
|
|
||
|
|
v->c = 1;
|
||
|
|
v->av = 0;
|
||
|
|
v->w = _IQ23(WINDOW_START);
|
||
|
|
|
||
|
|
for (i=0;i<MAX_SIZE_SMOOTH_INPUT;i++)
|
||
|
|
{
|
||
|
|
v->buf_input[i] = 0;
|
||
|
|
}
|
||
|
|
// current_pos_buf_input = 0;
|
||
|
|
v->current_pos_buf_input = 0;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
#pragma CODE_SECTION(smooth_add,".fast_run");
|
||
|
|
void smooth_add(SMOOTH *v)
|
||
|
|
{
|
||
|
|
volatile int i;
|
||
|
|
|
||
|
|
i = v->current_pos_buf_input;
|
||
|
|
v->buf_input[i] = v->input;
|
||
|
|
|
||
|
|
v->current_pos_buf_input++;
|
||
|
|
if (v->current_pos_buf_input>=MAX_SIZE_SMOOTH_INPUT)
|
||
|
|
v->current_pos_buf_input = 0;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
#pragma CODE_SECTION(smooth_calc,".fast_run");
|
||
|
|
void smooth_calc(SMOOTH *v)
|
||
|
|
{
|
||
|
|
_iq23 e=0;
|
||
|
|
_iq23 summ=0;
|
||
|
|
_iq23 w_new;
|
||
|
|
long w_int;
|
||
|
|
|
||
|
|
w_int = _IQ23int(v->w);
|
||
|
|
|
||
|
|
if (v->c <= (WINDOW_START*2))
|
||
|
|
{
|
||
|
|
summ = my_mean(v->c,v);
|
||
|
|
v->av = _IQ23div(summ,_IQ23(v->c));
|
||
|
|
e = 0;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
e = _IQ23div(CONST_IQ_2PI,v->av) - v->w; //(2*pi*fs/av) - w
|
||
|
|
v->ee = v->ee + _IQ23mpy(v->kp,(e - v->e0)) + _IQ23mpy(v->ki, e);
|
||
|
|
w_new = v->w + v->ee;
|
||
|
|
|
||
|
|
if (w_new>_IQ23(SIZE_SMOOTH_INPUT))
|
||
|
|
w_new = _IQ23(SIZE_SMOOTH_INPUT);
|
||
|
|
|
||
|
|
w_int = _IQ23int(w_new);
|
||
|
|
summ = my_mean(w_int,v);
|
||
|
|
v->w = _IQ23(w_int);
|
||
|
|
v->av = _IQ23div(summ, v->w);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
if (v->c<SIZE_SMOOTH_INPUT)
|
||
|
|
v->c++;
|
||
|
|
|
||
|
|
v->e0 = e;
|
||
|
|
v->w_int = w_int;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
#pragma CODE_SECTION(smooth_simple_calc,".fast_run");
|
||
|
|
void smooth_simple_calc(SMOOTH *v)
|
||
|
|
{
|
||
|
|
volatile _iq23 summ=0;
|
||
|
|
|
||
|
|
if (v->c <= v->w_int_simple )
|
||
|
|
{
|
||
|
|
summ = my_mean(v->c, v);
|
||
|
|
v->summ = summ;
|
||
|
|
v->av = _IQ23div(summ,_IQ23(v->c));
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
summ = my_mean(v->w_int_simple, v);
|
||
|
|
v->summ = summ;
|
||
|
|
v->av = _IQ23div(summ, _IQ23(v->w_int_simple));
|
||
|
|
}
|
||
|
|
|
||
|
|
if (v->c<MAX_SIZE_SMOOTH_INPUT)
|
||
|
|
v->c++;
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
void iq_smooth (_iq23 *input, _iq23 *output, int n, int window)
|
||
|
|
{
|
||
|
|
int i,j,z,k1,k2,hw;
|
||
|
|
int fm1,fm2;
|
||
|
|
|
||
|
|
_iq23 tmp;
|
||
|
|
|
||
|
|
|
||
|
|
fm1 = window/2;
|
||
|
|
fm2 = fm1*2;
|
||
|
|
if ((window-fm2)==0)
|
||
|
|
window++;
|
||
|
|
|
||
|
|
hw = (window-1)/2;
|
||
|
|
output[0] = input[0];
|
||
|
|
|
||
|
|
for (i=1;i<n;i++)
|
||
|
|
{
|
||
|
|
tmp=0;
|
||
|
|
if(i<hw)
|
||
|
|
{
|
||
|
|
k1=0;
|
||
|
|
k2=2*i;
|
||
|
|
z=k2+1;
|
||
|
|
}
|
||
|
|
else if((i+hw)>(n-1))
|
||
|
|
{
|
||
|
|
k1=i-n+i+1;
|
||
|
|
k2=n-1;
|
||
|
|
z=k2-k1+1;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
k1=i-hw;
|
||
|
|
k2=i+hw;
|
||
|
|
z=window;
|
||
|
|
}
|
||
|
|
|
||
|
|
for (j=k1;j<=k2;j++)
|
||
|
|
{
|
||
|
|
tmp=tmp + input[j];
|
||
|
|
}
|
||
|
|
output[i] = _IQ23div(tmp,_IQ23(z));
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|