Computational routine
eng
vco
/* Modnumlib Scicos interfacing function
* Copyright (C) 2009-2011 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.
*/
/* vco Scicos discrete VCO block with frequency divider
* Type 4 simulation function ver 1.0 - scilab-4.1
* 21 Mars 2007 - INRIA - Author : A.Layec
*/
#include <math.h>
#include <stdio.h>
#include "modnum_lib.h"
#include "scicos_block.h"
/*
* ys(t) = cos(wot + phi(t))
*
* phi(t) = int(F(u3(t) + w(t))dt)
*
*/
void vcoblk(scicos_block *block,int flag)
{
int i,k;
double *u1;
double *u2;
double *u3;
double *u__1;
double *y;
/*double *y10;*/
double *w__n; /* white noise on input */
double phi__n; /* phase noise */
double h;
double omega;
double *theta;
double *theta2;
double thetay;
double theta2y;
double kv;
double alpha;
double beta;
double gamma;
double sigma;
double f__u;
double *err;
double *err2;
double cross_val;
double cross_val2;
int with_dyncross;
int with_infunc;
int with_2evout;
int with_wnoise; /* white noise on input */
int with_pnoise; /* phase noise */
u1 = block->inptr[0]; /* input voltage */
u2 = block->inptr[1]; /* divider value or phase noise*/
u3 = block->inptr[2]; /* phase noise*/
y = block->outptr[0]; /* cos(theta) */
h = block->rpar[0];
omega = block->rpar[1];
cross_val = block->rpar[2];
cross_val2 = block->rpar[3];
kv = block->rpar[4];
alpha = block->rpar[5];
beta = block->rpar[6];
gamma = block->rpar[7];
sigma = block->rpar[8];
with_dyncross = block->ipar[0];
with_infunc = block->ipar[1];
with_2evout = block->ipar[2];
with_wnoise = block->ipar[3];
with_pnoise = block->ipar[4];
/*not needed*/
/*cnt = &block->z[0];*/
theta = &block->z[1];
theta2 = &block->z[2];
u__1 = &block->z[3]; /* last input */
err = &block->z[4]; /* last phase */
err2 = &block->z[5]; /* last phase */
w__n = &block->z[6]; /* white noise */
/* not needed */
/*phi__n = &block->z[7];*/ /* phase noise */
switch(flag)
{
/* compute output */
case 6 :
case 1 : /* get input */
f__u = u1[0];
if (with_infunc!=0) {
f__u = alpha * tanh(beta * f__u);
}
/* get phase noise */
phi__n = 0;
if (with_pnoise!=0) {
if(with_dyncross!=0) {
phi__n = u3[0];
}
else {
phi__n = u2[0];
}
/* test on phase noise */
if ((phi__n >= (M_PI/4))||(phi__n <= -(M_PI/4))) {
set_block_error(-6);
return;
}
}
/* compute theta */
vcoy_c((i=1,&i),&f__u,&h,&omega,&kv, \
w__n,&phi__n,err,u__1,&thetay);
if(with_2evout!=0) {
vcoy_c((i=1,&i),&f__u,&h,&omega,&kv, \
w__n,&phi__n,err2,u__1,&theta2y);
}
y[0] = cos(thetay);
break;
/* compute discrete state */
case 4 :
case 2 : /* get input */
f__u = u1[0];
if (with_infunc!=0) {
f__u = alpha * tanh(beta * f__u);
}
/* get phase noise */
phi__n = 0;
if (with_pnoise!=0) {
if(with_dyncross!=0) {
phi__n = u3[0];
}
else {
phi__n = u2[0];
}
/* test on phase noise */
if ((phi__n >= (M_PI/4))||(phi__n <= -(M_PI/4))) {
set_block_error(-6);
return;
}
}
if(with_dyncross!=0) {
cross_val = cross_val * u2[0];
}
/* compute theta, store input, compute err */
if(with_2evout!=0) {
vcoy_c((i=1,&i),&f__u,&h,&omega,&kv, \
w__n,&phi__n,err2,u__1,theta2);
vcoz_c((i=1,&i),&f__u,&h,&omega,&kv, \
&cross_val,w__n,&phi__n,err,u__1,theta);
(*err2) = (*theta2) - phi__n;
if( (*err2) >= cross_val2) {
(*err2) = (*err2) - cross_val2;
}
}
else {
vcoz_c((i=1,&i),&f__u,&h,&omega,&kv, \
&cross_val,w__n,&phi__n,err,u__1,theta);
}
if (with_wnoise!=0) {
noiseblk_c((i=1,&i),(k=1,&k),&sigma,&gamma,w__n);
}
break;
/* compute date of crossing */
case 3 : /* get input */
f__u = u1[0];
if (with_infunc!=0) {
f__u = alpha * tanh(beta * f__u);
}
/* get phase noise */
phi__n = 0;
if (with_pnoise!=0) {
if(with_dyncross!=0) {
phi__n = u3[0];
}
else {
phi__n = u2[0];
}
/* test on phase noise */
if ((phi__n >= (M_PI/4))||(phi__n <= -(M_PI/4))) {
set_block_error(-6);
return;
}
}
if(with_dyncross!=0) {
cross_val = cross_val * u2[0];
}
/* compute theta */
vcoy_c((i=1,&i),&f__u,&h,&omega,&kv, \
w__n,&phi__n,err,u__1,&thetay);
if((thetay) >= cross_val) {
block->evout[0] = ((cross_val - (*err)) * h) / ((thetay) - (*err));
}
if(with_2evout!=0) {
vcoy_c((i=1,&i),&f__u,&h,&omega,&kv, \
w__n,&phi__n,err2,u__1,&theta2y);
if((theta2y) >= cross_val2) {
block->evout[1] = ((cross_val2 - (*err2)) * h) / ((theta2y) - (*err2));
}
}
break;
/* finish */
case 5 :
break;
default : break;
}
}