Computational routine
eng
pspec_scope
/* Modnumlib Scicos interfacing function
* Copyright (C) 2009 Alan Layec
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation; either
* version 2 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* General Public License for more details.
*
* You should have received a copy of the GNU General Public
* License along with this library; if not, write to the
* Free Software Foundation, Inc., 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/* pspec_scope Scicos Spectrum Analyzer block
*
* - Based on auto-correlation method (see
* pspect.sci from standard Signal Scilab
* library)
*
* - Based on Scilab NG routines
*
* Type 4 simulation function ver 1.0 - scilab-4.1
* 23 Mars 2007 - INRIA - Author : A.Layec
*/
/* REVISION HISTORY :
* $Log$
*/
/*Standarc C include*/
#include <stdio.h>
#include <string.h>
#include <../include/math.h>
/*for NG*/
#include "SetProperty.h"
#include "GetProperty.h"
#include "InitObjects.h"
#include "bcg.h"
#include "DrawObjects.h"
#include "BuildObjects.h"
#include "ObjectStructure.h"
#include "DestroyObjects.h"
/*Scicos block st include*/
#include "scicos_block.h"
/*Scilab include*/
#include "machine.h"
#include "modnum_lib.h"
/*some macros*/
#ifndef min
#define min(a,b) ((a) <= (b) ? (a) : (b))
#endif
#ifndef max
#define max(a,b) ((a) >= (b) ? (a) : (b))
#endif
#define PI0 (integer *) 0
#define PD0 (double *) 0
/*external functions*/
extern integer C2F(dset)();
extern integer C2F(dr1)();
extern integer C2F(xgrid)();
/*work structure of that block*/
typedef struct work_st {
double *z_t; /*time domain in buffer*/
double *win; /*window coefficient*/
double *r_io; /*real part of time/freq domain in/out buffer*/
double *i_io; /*imag part of time/freq domain in/out buffer*/
double *r_out; /*real part of freq domain display buffer*/
double *work; /*working array for fft*/
long hdl; /*hdl of the NG scope*/
int wid; /*wid of the NG scope*/
int minmax_win[2]; /*min/max sample of the x_axis*/
/*y axis autoscale variables*/
double y_up;
double y_down;
double y_delta;
} work_struct;
/*
* ipar[0] : fftsiz
* ipar[1] : sec_s
* ipar[2] : n_sec
* ipar[3] : ffttyp 0:fft842/1:dfftmx
* ipar[4] : n_work
* ipar[5] : xlog 0:no/1:yes
* ipar[6] : ylog 0:no/1:yes
* ipar[7] : wtyp 1-4
* ipar[8] : wid
* ipar[9] : yunit 0:U^2 / 1:dB
* ipar[10] : scaltyp 0:use ymin,ymax / 1:autoscale
* ipar[11] : view_in
*
* rpar[0] : Tse
* rpar[1] : cfreq -1-0:no
* rpar[2] : fdenorm -1:no
* rpar[3] : ymin
* rpar[4] : ymax
*
* z[0] : count
* z[1] : n_call
*/
/* specview_draw :
* Initialization and re-initialization
* of the pspec_scope block
*/
void specview_draw(scicos_block *block)
{
/*"fouretout" variables*/
int i;
/*variables to get rpar/ipar ptr*/
double *rpar;
int *ipar;
/*variables to store main paramaters*/
double xmin, xmax;
double ymin, ymax;
double Tse;
double stepf;
double cfreq;
double fdenorm;
int wtyp;
int l_win;
int fftsiz;
int sec_s;
int min_win;
int max_win;
int xlog;
int ylog;
int n_sec;
int yunit;
/*graphic ptr variables*/
char wnam[100];
char title[150];
char x_label[30];
char y_label[30];
sciPointObj *pTemp;
sciPointObj *pTemp2;
sciPointObj *pPolyline;
double *x_vect;
double *y_vect;
/*scope variables*/
int nsubwin = 1;
int ncurves = 1;
int color = 1;
int line_size = 1;
int dim = 2;
int win_pos[2] = {-1,-1};
int win_dim[2] = {-1,-1};
int wid; /*the windows id*/
/*ptr of the struct of that block*/
work_struct *work;
/*get he struct of that block*/
work = (work_struct*) *block->work;
/*get rpar/ipar*/
rpar = block->rpar;
ipar = block->ipar;
fftsiz = ipar[0];
sec_s = ipar[1];
n_sec = ipar[2];
xlog = ipar[5];
ylog = ipar[6];
wtyp = ipar[7];
wid = ipar[8];
yunit = ipar[9];
Tse = rpar[0];
cfreq = rpar[1];
fdenorm = rpar[2];
ymin = rpar[3];
ymax = rpar[4];
/* compute freq indices of the scope
*/
if (fdenorm<=0.) fdenorm = 1.;
/*freq step*/
stepf = 1. / ( ((double)fftsiz) * Tse) * fdenorm;
cfreq = cfreq * fdenorm;
xmin = stepf;
xmax = ((double)(fftsiz/2)-1)*stepf;
/* compute xmin/xmax bound of the scope */
if ( (cfreq<xmax) && (cfreq>xmin) ) {
/* plot from a center frequency */
if (xlog == 1) { /*logplot on x_axis*/
min_win = ((int) (cfreq/stepf)) + 1;
/*max_win = min_win + ((int)(min((xmax-cfreq)/stepf , 4e6))) - 1;*/
max_win = min_win + (int)((xmax-cfreq)/stepf) - 1;
l_win = max_win - min_win + 1; /* length of window */
xmin = stepf;
xmax = (l_win) * stepf;
}
else { /*linearplot on x_axis*/
l_win = 2 *((int) (min( (cfreq-xmin)/stepf , (xmax-cfreq)/stepf )));
/*l_win = (int) (min(l_win,4e6));*/ /* length of window */
min_win = ((int) (cfreq/stepf)) - l_win/2;
max_win = ((int) (cfreq/stepf)) + l_win/2 - 1;
xmin = ((double)min_win) * stepf;
xmax = ((double)max_win) * stepf;
}
}
else {
/* plot from DC */
l_win = fftsiz / 2;
/*l_win = (int) (min(l_win,4e6));*/ /* length of window */
min_win = 1;
max_win = l_win -1 ;
xmin = ((double)min_win) * stepf;
xmax = ((double)max_win) * stepf;
}
/*store min/max x_axis indices in my_st*/
work->minmax_win[0] = min_win;
work->minmax_win[1] = max_win;
/***** display part ****/
/* get wid of the scope
* and store it in my_st*/
if (wid<0) {
work->wid = 20000 + get_block_number();
}
else {
work->wid = wid;
}
/* set title and axes labels of the scope
*/
sprintf(wnam,"Spectrum Analyzer (%d)",work->wid);
sprintf(title,"fft size=%d - overlap size=%d",fftsiz,sec_s);
sprintf(title,"%s - avrg=%d",title,n_sec);
switch(wtyp)
{
case 1 : sprintf(title,"%s - re win",title);
break;
case 2 : sprintf(title,"%s - tr win",title);
break;
case 3 : sprintf(title,"%s - hm win",title);
break;
case 4 : sprintf(title,"%s - hn win",title);
break;
default : sprintf(title,"%s - re win",title);
break;
}
sprintf(title,"%s - step=%-3.3e Hz (%2.1f dB)",title,stepf,10*log10(stepf));
if (((cfreq<((double)(fftsiz/2)-1)*stepf)) && (cfreq>stepf)) {
sprintf(title,"%s - c.freq=%#-3.3e Hz (%2.1f dB)",title,cfreq,10*log10(stepf));
}
/* axis labels */
sprintf(x_label,"Freq [Hz]"); /*x axis*/
if (xlog==1) {
sprintf(x_label,"%s - (LOG)",x_label);
}
else {
sprintf(x_label,"%s - (LIN)",x_label);
}
if (yunit==1) { /*y axis*/
sprintf(y_label,"PSD [dB]");
}
else if (yunit==2) {
sprintf(y_label,"PSD [dB/Hz]");
}
else {
sprintf(y_label,"PSD [U^2]");
}
if ((ylog==1)||(yunit==1)) {
sprintf(y_label,"%s - (LOG)",y_label);
}
else {
sprintf(y_label,"%s - (LIN)",y_label);
}
/* Alocation/initialization of temporary x/y vectors
* to build polyline
*/
if ((x_vect=(double*)
scicos_malloc(sizeof(double)*((max_win-min_win)+1))) == NULL) {
set_block_error(-16); return;
}
if ((y_vect=(double*)
scicos_malloc(sizeof(double)*((max_win-min_win)+1))) == NULL) {
set_block_error(-16); return;
}
for(i=0;i<(max_win-min_win+1);i++) { /*init*/
x_vect[i] = xmin + i*stepf;
y_vect[i] = 0;
}
/* init NG scope
*/
DeleteObjs(work->wid);
sciSetUsedWindow(work->wid);
pTemp = sciGetCurrentFigure();
sciSetName(pTemp,wnam,strlen(wnam));
pTemp2 = sciGetPointerFromHandle(sciGetHandle(sciGetSelectedSubWin(pTemp)));
sciSetFontDeciWidth(pTemp2, 0);
sciSetIsBoxed(pTemp2,TRUE);
pSUBWIN_FEATURE(pTemp2)->tight_limits = TRUE;
pSUBWIN_FEATURE(pTemp2)->WRect[0] = 0;
pSUBWIN_FEATURE(pTemp2)->WRect[1] = 0;
pSUBWIN_FEATURE(pTemp2)->WRect[2] = 1;
pSUBWIN_FEATURE(pTemp2)->WRect[3] = 1;
pSUBWIN_FEATURE(pTemp2)->axes.axes_visible[1] = TRUE;
pSUBWIN_FEATURE(pTemp2)->SRect[2] = ymin;
pSUBWIN_FEATURE(pTemp2)->SRect[3] = ymax;
pSUBWIN_FEATURE(pTemp2)->axes.axes_visible[0] = TRUE;
pSUBWIN_FEATURE(pTemp2)->SRect[0] = xmin;
pSUBWIN_FEATURE(pTemp2)->SRect[1] = xmax;
sciSetUsedWindow(work->wid);
pPolyline=ConstructPolyline(pTemp2,x_vect,y_vect,NULL,0,l_win,1,1, \
NULL,NULL,NULL,NULL,NULL,FALSE,FALSE,TRUE,FALSE);
pPOLYLINE_FEATURE(pPolyline)->n1 = l_win - 1;
sciSetIsClipping(pPolyline, 0);
sciSetForeground(pPolyline, color);
sciSetIsLine(pPolyline, 1);
sciSetIsMark(pPolyline, 0);
sciSetText(pSUBWIN_FEATURE(pTemp2)->mon_title,title,500);
sciSetFontDeciWidth(pSUBWIN_FEATURE(pTemp2)->mon_title, 0);
sciSetText(pSUBWIN_FEATURE(pTemp2)->mon_x_label,x_label,500);
sciSetFontDeciWidth(pSUBWIN_FEATURE(pTemp2)->mon_x_label, 0);
sciSetText(pSUBWIN_FEATURE(pTemp2)->mon_y_label,y_label,500);
sciSetFontDeciWidth(pSUBWIN_FEATURE(pTemp2)->mon_y_label, 0);
sciSetLineWidth(pPolyline,line_size);
sciSetMarkSize(pPolyline,line_size);
if(xlog == 1) { /*set x logflag if needed*/
pSUBWIN_FEATURE(pTemp2)->logflags[0]='l';
}
if(ylog == 1) { /*set y logflag if needed*/
pSUBWIN_FEATURE(pTemp2)->logflags[1]='l';
}
C2F(xgrid)((i=12,&i)); /*show grid*/
sciDrawObj(pTemp); /*draw scope*/
/*store hld in work_st to know if window has been closed
* during simulation
*/
work->hdl = sciGetHandle(pTemp);
/*free vectors no more needed*/
scicos_free(x_vect);
scicos_free(y_vect);
/***** end of the display part ****/
}
/* main routine */
void pspec_scope(scicos_block *block,int flag)
{
/*"fouretout" variables*/
int i,k;
double zero=0.0;
void *bidon=NULL; /*a void parameter fo calc_win*/
double eps=1e-40;
/*variables to get block ptr*/
int *ipar;
double *rpar;
double *u1;
/*variables of the pspec method*/
int count;
int n_call;
int n_sec;
int sec_l;
int sec_s;
int ptr_u;
double sum=0.;
/*freq step*/
double stepf;
double Tse;
double fdenorm;
int fftsiz;
double steplog=0.;
/*graphic indices variables*/
int min_win;
int max_win;
int l_win;
/*ptr of the struct of that block*/
work_struct *work;
/*fft variables*/
int isn=-1; /*sign of the fft*/
int ffttyp;
int wtyp;
int n_work;
fft_pr_struct fft_pr;
/*some scope parameters*/
int yunit; /*comput log() on y axis*/
int scaltyp; /*0:ymin/ymax/1:autoscale*/
int view_in; /*view input buffer*/
/*autoscale variables*/
double min_y;
double max_y;
double y_height;
double y_delta;
double y_up;
double y_down;
double fact=0.4;
/*graphic ptr variables*/
sciPointObj *pTemp;
sciPointObj *pTemp2;
sciPointObj *pPolyline;
sciSons *MySon;
/*get block ptr*/
ipar = block->ipar;
rpar = block->rpar;
u1 = block->inptr[0];
/*get ipar variables*/
sec_l = ipar[0];
sec_s = ipar[1];
n_sec = ipar[2];
ffttyp = ipar[3];
n_work = ipar[4]; /*size of the working array is computed
*by the scilab interfacing function
*of that block*/
wtyp = ipar[7];
yunit = ipar[9];
scaltyp = ipar[10];
view_in = ipar[11];
/*ptr of the struct of that block*/
work = (work_struct*) *block->work;
/*get counters value*/
count = (int)block->z[0];
n_call = (int)block->z[1];
/*freq step*/
fftsiz = ipar[0];
Tse = rpar[0];
fdenorm = rpar[2];
/* compute freq indices of the scope
*/
if (fdenorm<=0.) fdenorm = 1.;
stepf = 1. / ( ((double)fftsiz) * Tse) * fdenorm;
if(flag==4) { /*flag 4*/
/*get campaign of allocation*/
if ((work=(work_struct*) /*my_st*/
scicos_malloc(sizeof(work_struct))) == NULL) {
set_block_error(-16); return;
}
if ((work->z_t=(double*) /*array of input time domain buffer*/
scicos_malloc(sizeof(double)*sec_l)) == NULL) {
set_block_error(-16); return;
}
if ((work->win=(double*) /*array of coef of window*/
scicos_malloc(sizeof(double)*sec_l)) == NULL) {
set_block_error(-16); return;
}
if ((work->r_io=(double*) /*array of real part for fft*/
scicos_malloc(sizeof(double)*sec_l)) == NULL) {
set_block_error(-16); return;
}
if ((work->i_io=(double*) /*array of imaginary part for fft*/
scicos_malloc(sizeof(double)*sec_l)) == NULL) {
set_block_error(-16); return;
}
if ((work->r_out=(double*) /*array of output freq buffer*/
scicos_malloc(sizeof(double)*sec_l)) == NULL) {
set_block_error(-16); return;
}
if (ffttyp==1) { /*if dfftmx is used then set a working array*/
if ((work->work=(double*)
scicos_malloc(sizeof(double)*n_work)) == NULL) {
set_block_error(-16); return;
}
}
/*init. autoscale y axis bound*/
work->y_up=1;
work->y_down=eps;
work->y_delta=0;
/*store the coefficient of the window */
calc_win_c(&sec_l,&wtyp,bidon,work->win);
/*init. vectors with zero*/
C2F(dset)(&sec_l,&zero,work->r_out,(k=1,&k));
C2F(dset)(&sec_l,&zero,work->r_io,(k=1,&k));
C2F(dset)(&sec_l,&zero,work->i_io,(k=1,&k));
*block->work=(void *)work; /*store my_st in *block->work*/
specview_draw(block); /*call specview_draw for initialization*/
}
else if(flag==2) { /*flag 2*/
if ((count + block->insz[0])<sec_l) {
for(i=0;i<block->insz[0];i++) { /*store new samples in the input buffer*/
work->z_t[count + i] = u1[i];
}
count += block->insz[0];/*update input buffer counter*/
}
else {
ptr_u = 0;
/* compute auto-correlated spectrum */
while( (count + (block->insz[0]-ptr_u)) >= sec_l) {
for(i=ptr_u ;i<(sec_l-count) ;i++) { /*store new samples in the input buffer*/
work->z_t[count+i] = u1[i];
}
ptr_u += sec_l-count;
/*if in buffer is full then compute fft */
count=sec_l-sec_s; /*update buffer counter*/
/*copy input buffer in real part of
* fft computation array
*/
C2F(dcopy)(&sec_l,work->z_t,(k=1,&k),work->r_io,(k=1,&k));
/*set imaginary vector to zero*/
C2F(dset)(&sec_l,&zero,work->i_io,(k=1,&k));
/* initialize fft_pr*/
fft_pr.lfft = sec_l;
fft_pr.isn = isn;
fft_pr.ffttyp = ffttyp;
fft_pr.nwork = n_work;
fft_pr.work = work->work;
/* call pspect_c */
pspect_c(sec_l,n_sec,&fft_pr, \
&n_call,work->win,work->r_io,work->i_io,work->r_out);
/*if average counter cross user average param
*then display spectrum
*/
if (n_call==0) {
/* get indices of the x_axis */
min_win = work->minmax_win[0];
max_win = work->minmax_win[1];
l_win = max_win-min_win; /*number of sample of the window*/
/*if window has been destroyed, then redraw it*/
if (sciGetPointerFromHandle(work->hdl) == NULL) {
specview_draw(block);
}
sciSetUsedWindow(work->wid);
pTemp = sciGetCurrentFigure();
pTemp2 = sciGetPointerFromHandle(sciGetHandle(sciGetSelectedSubWin(pTemp)));
MySon = (sciGetRelationship (pTemp2)->psons);
/*first son of axes is supposed to be the polyline*/
pPolyline = MySon->pointobj;
if (scaltyp==1) {
/***********/
/*autoscale*/
/***********/
if (yunit>=1) { /*plot 10*log10(y)*/
if (yunit==2) {/*plot 10*log10(y)-10*log10(stepf)*/
steplog = 10*log10(stepf);
}
/*def min max value*/
min_y=10*log10(work->r_out[min_win+1]/((double)sec_l)) - steplog;
max_y=min_y;
/*store value of spectrum sample in NG polyline*/
for(i=min_win+1;i<max_win;i++) {
pPOLYLINE_FEATURE(pPolyline)->pvy[i-min_win] = \
10*log10(work->r_out[i]/((double)sec_l)) - steplog;
min_y=min(min_y,pPOLYLINE_FEATURE(pPolyline)->pvy[i-min_win]);
max_y=max(max_y,pPOLYLINE_FEATURE(pPolyline)->pvy[i-min_win]);
}
pPOLYLINE_FEATURE(pPolyline)->pvy[0] = \
10*log10(work->r_out[min_win]/((double)sec_l)) - steplog;
pPOLYLINE_FEATURE(pPolyline)->pvy[max_win-min_win] = \
10*log10(work->r_out[max_win]/((double)sec_l)) - steplog;
/*set minimal y axis bound*/
if (min_y<10*log10(eps)) {
min_y=10*log10(eps);
}
y_height = max_y - min_y;
y_delta = min_y + y_height/2; /*also equal to max_y-y_height/2*/
y_down = y_delta - (1+fact) * y_height/2;
y_up = y_delta + (1+fact) * y_height/2;
if ( (((1+fact) * y_height/2) > (1+fact) * work->y_delta) || \
(((1+fact) * y_height/2) < (1-fact) * work->y_delta) ) {
work->y_delta = ( (1+fact) * y_height/2);
work->y_up = y_up;
work->y_down = y_down;
}
}
else { /*plot y*/
/*def min max value*/
min_y=work->r_out[min_win+1]/((double)sec_l);
max_y=min_y;
/*store value of spectrum sample in NG polyline*/
for(i=min_win+1;i<max_win;i++) {
pPOLYLINE_FEATURE(pPolyline)->pvy[i-min_win] = \
work->r_out[i]/((double)sec_l);
min_y=min(min_y,pPOLYLINE_FEATURE(pPolyline)->pvy[i-min_win]);
max_y=max(max_y,pPOLYLINE_FEATURE(pPolyline)->pvy[i-min_win]);
}
pPOLYLINE_FEATURE(pPolyline)->pvy[0] = \
work->r_out[min_win]/((double)sec_l);
pPOLYLINE_FEATURE(pPolyline)->pvy[max_win-min_win] = \
work->r_out[max_win]/((double)sec_l);
if (min_y<eps) min_y=eps;
max_y = log10(max_y); /*change to log scale to disble error computation*/
min_y = log10(min_y);
y_height = max_y - min_y;
y_delta = min_y + y_height/2; /*also equal to max_y-y_height/2*/
y_down = y_delta - (1+fact) * y_height/2;
y_up = y_delta + (1+fact) * y_height/2;
if ( (((1+fact) * y_height/2) > (1+fact) * work->y_delta) || \
(((1+fact) * y_height/2) < (1-fact) * work->y_delta) ) {
work->y_delta = pow(10,(((1+fact) * y_height/2)));
work->y_up = pow(10,(y_up));
work->y_down = pow(10,(y_down));
}
}
/*if window has been destroyed, then redraw it*/
if (sciGetPointerFromHandle(work->hdl) == NULL) {
specview_draw(block);
}
sciSetUsedWindow(work->wid);
pTemp = sciGetCurrentFigure();
pTemp2 = sciGetPointerFromHandle(sciGetHandle(sciGetSelectedSubWin(pTemp)));
MySon = (sciGetRelationship (pTemp2)->psons);
/*first son of axes is supposed to be the polyline*/
pPolyline = MySon->pointobj;
pSUBWIN_FEATURE(pTemp2)->SRect[2] = work->y_down;
pSUBWIN_FEATURE(pTemp2)->SRect[3] = work->y_up;
}
else { /*scale given by ymin/max*/
if (yunit>=1) { /*plot 10*log10(y)*/
if (yunit==2) {/*plot 10*log10(y)-10*log10(stepf)*/
steplog = 10*log10(stepf);
}
/*store value of spectrum sample in NG polyline*/
for(i=min_win;i<=max_win;i++) {
pPOLYLINE_FEATURE(pPolyline)->pvy[i-min_win] = \
10*log10(work->r_out[i]/((double)sec_l)) - steplog;
}
}
else { /*plot y*/
/*store value of spectrum sample in NG polyline*/
for(i=min_win;i<=max_win;i++) {
pPOLYLINE_FEATURE(pPolyline)->pvy[i-min_win] = \
work->r_out[i]/((double)sec_l);
}
}
}
sciDrawObj(pTemp2); /*draw axes*/
/*miscellaenous part to disable glitter of scope
*/
C2F(dr1)("xset","wwpc",PI0,PI0,PI0,PI0,PI0,PI0, \
PD0,PD0,PD0,PD0,0L,0L); /*clear pixmap*/
pFIGURE_FEATURE(pTemp)->pixmap=1; /*set pixmap mode*/
C2F(xgrid)((i=12,&i)); /*draw grid in the pixmap*/
C2F(dr1)("xset","wshow",PI0,PI0,PI0,PI0,PI0,PI0, \
PD0,PD0,PD0,PD0,0L,0L); /*show the pixmap*/
pFIGURE_FEATURE(pTemp)->pixmap = 0; /*unset pixmap mode*/
/***** end of the display part ****/
}
/*shift input buffer*/
C2F(dcopy)((i=sec_l-sec_s,&i),&work->z_t[sec_s],(k=1,&k),&work->z_t[0],(k=1,&k));
}
/*update input buffer*/
if(ptr_u!=block->insz[0]) {
for(i=ptr_u;i<block->insz[0];i++) {
work->z_t[count+i-ptr_u] = u1[i];
}
count += (block->insz[0]-ptr_u);
}
}
/*store counters in my_st*/
block->z[0]=count;
block->z[1]=n_call;
}
else if (flag==5) { /*flag 5*/
/*de-allocation fields of the struct of that block*/
scicos_free(work->z_t);
scicos_free(work->win);
scicos_free(work->r_io);
scicos_free(work->i_io);
scicos_free(work->r_out);
if (ffttyp==1) { /*don't forget fft working array if needed*/
scicos_free(work->work);
}
scicos_free(work);
}
}