pid.c

rtai-3.1-test3的源代码(Real-Time Application Interface )

/* SCILAB Computational function  */
/*     Copyright INRIA */
/*     Generated by Code_Generation toolbox of Scicos with scilab-2.6 */
/*     date : 10-Dec-2002 */

#include <stdio.h>
#include <string.h>
#include "/home/mante/scilab-2.6/routines/machine.h"
#include "/home/mante/scilab-2.6/routines/sun/link.h"
#include "/home/mante/scilab-2.6/routines/scicos/scicos.h"
 
void pidmain1(double *z, double *t, double * rpar, int *nrpar,
   int *ipar, int *nipar);
 
void pidmain2(double *z, double *t, double * rpar, int *nrpar,
   int *ipar, int *nipar) ;
 
void pid_init(double *z, double *t, double * rpar, int *nrpar,
   int *ipar, int *nipar) ;
 
void pid_end(double *z, double *t, double * rpar, int *nrpar,
   int *ipar, int *nipar) ;
 
int pid()  ;
 
int pidddoit1(double *z, int * zptr, double *t, double *tevts,
   int *evtspt, int * nevts, int *pointi, int * outptr, int *clkptr,
   int *ordptr, int *ordclk, int *nordcl, double * rpar, int *ipar,
   int *funptr, int *funtyp, double *outtb, int *iwa)  ;
 
int pidddoit2(double *z, int * zptr, double *t, double *tevts,
   int *evtspt, int * nevts, int *pointi, int * outptr, int *clkptr,
   int *ordptr, int *ordclk, int *nordcl, double * rpar, int *ipar,
   int *funptr, int *funtyp, double *outtb, int *iwa)  ;
 
int pidedoit1(double *z, int * zptr, double *t, double *tevts,
   int *evtspt, int * nevts, int *pointi, int * outptr, int *clkptr,
   int *ordptr, int *ordclk, int *nordcl, double * rpar, int *ipar,
   int *funptr, int *funtyp, double *outtb, int * urg, int *iwa)  ;
 
int pid_initi(double *z, int * zptr, double *t, double *tevts,
   int *evtspt, int * nevts, int *pointi, int * outptr, int *clkptr,
   int *ordptr, int *ordclk, int *nordcl, double * rpar, int *ipar,
   int *funptr, int *funtyp, double *outtb, int *iwa)  ;
 
int pid_endi(double *z, int * zptr, double *t, double *tevts,
   int *evtspt, int * nevts, int *pointi, int * outptr, int *clkptr,
   int *ordptr, int *ordclk, int *nordcl, double * rpar, int *ipar,
   int *funptr, int *funtyp, double *outtb, int *iwa)  ;
 
int pid_outtb(double *z, int * zptr, double *t, double *tevts,
   int *evtspt, int * nevts, int *pointi, int * outptr, int *clkptr,
   int *ordptr, int *ordclk, int *nordcl, double * rpar, int *ipar,
   int *funptr, int *funtyp, double *outtb, int *iwa)  ;
 
int C2F(putevs)(double *tevts, int *evtspt, int * nevts, int *pointi,
   double *told, int *i2, int *ierr);
/* ---- block simulation functions -------*/
 

void C2F(gain)(int *flag, int *nevprt, double *told, double *xd,
   double *x, int *nx, double *z, int *nz, double *tvec, int *ntvec,
   double *rpar, int *nrpar,int *ipar,int *nipar, double *u, int *nu,
   double *y, int *ny);

void plusblk(int *flag, int *nevprt,double *told, double *xd,
   double *x, int *nx, double *z, int *nz, double *tvec,  int *ntvec,
   double *rpar, int *nrpar, int *ipar, int *nipar,  double **inptr,
   int* insz, int *nin, double **outptr,int *outsz, int *nout);

void C2F(dsslti)(int *flag, int *nevprt, double *told, double *xd,
   double *x, int *nx, double *z, int *nz, double *tvec, int *ntvec,
   double *rpar, int *nrpar,int *ipar,int *nipar, double *u, int *nu,
   double *y, int *ny);

void C2F(lusat)(int *flag, int *nevprt, double *told, double *xd,
   double *x, int *nx, double *z, int *nz, double *tvec, int *ntvec,
   double *rpar, int *nrpar,int *ipar,int *nipar, double *u, int *nu,
   double *y, int *ny);

void C2F(samphold)(int *flag, int *nevprt, double *told, double *xd,
   double *x, int *nx, double *z, int *nz, double *tvec, int *ntvec,
   double *rpar, int *nrpar,int *ipar,int *nipar, double *u, int *nu,
   double *y, int *ny);

void pid_sensor(int *flag, int *nport, int *nevprt, double *t,
   double *y, int *ny);

void pid_actuator(int *flag, int *nport, int *nevprt, double *t,
   double *u, int *nu);

void C2F(bidon)(int *flag, int *nevprt, double *told, double *xd,
   double *x, int *nx, double *z, int *nz, double *tvec, int *ntvec,
   double *rpar, int *nrpar,int *ipar,int *nipar, double *u, int *nu,
   double *y, int *ny);

void C2F(cstblk)(int *flag, int *nevprt, double *told, double *xd,
   double *x, int *nx, double *z, int *nz, double *tvec, int *ntvec,
   double *rpar, int *nrpar,int *ipar,int *nipar, double *y1, int *ny1);

void selector(int *flag, int *nevprt,double *told, double *xd,
   double *x, int *nx, double *z, int *nz, double *tvec,  int *ntvec,
   double *rpar, int *nrpar, int *ipar, int *nipar,  double **inptr,
   int* insz, int *nin, double **outptr,int *outsz, int *nout);

void C2F(ifthel)(int *flag, int *nevprt, int *ntvec, double *rpar,
   int *nrpar, int *ipar, int *nipar, double *u, int *nu);

void C2F(gensqr)(int *flag, int *nevprt, double *told, double *xd,
   double *x, int *nx, double *z, int *nz, double *tvec, int *ntvec,
   double *rpar, int *nrpar,int *ipar,int *nipar, double *u, int *nu,
   double *y, int *ny);

void C2F(rndblk)(int *flag, int *nevprt, double *told, double *xd,
   double *x, int *nx, double *z, int *nz, double *tvec, int *ntvec,
   double *rpar, int *nrpar,int *ipar,int *nipar, double *u, int *nu,
   double *y, int *ny);
/* Table of constant values */ 
 
static integer totalnevprt; 
static integer evtspt[ ]={-1,-1,-1};
static integer nevts[ ]={3};
static double x[1];
static integer zptr[ ]={1,1,1,3,3,3,3,3,4,5,5,5,5,5,5,5,5,5,5,5,5,6,6,
  7,7,7,9};
static integer clkptr[ ]={1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,2,2,2,2,2,2,
  4,4,4,4,4};
static integer ordptr[ ]={1,7,25,43};
static integer ordclk[ ]={3,14,15,17,19,22,1,11,18,23,26,10,24,25,13,
  21,7,8,9,5,6,2,4,12,1,11,18,23,26,10,24,25,13,21,7,8,9,5,6,2,4,12,1,1,
  1,1,1,1,1,1,1,1,1,1,1,1,3,1,1,1,1,1,1,3,1,3,1,1,1,1,1,1,1,1,3,2,1,1,1,
  1,1,3,1,3};
static integer outptr[ ]={1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,15,15,
  15,15,15,16,17,17,18,19,20,21};
static double tevts[ ] = {0,0,0};
static integer rdfunptr[ ]={1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,
  18,19,20,21,22,23,24,25,26};
static integer funtyp[ ]={0,2,0,0,0,0,0,0,0,0,0,2,2,0,0,0,0,0,0,1,2,
  -1,0,0,0,0};
static double w[1];
void set_nevprt(int nevprt)
{
  totalnevprt=nevprt;
}
/*---------------------------------------- Block Computational function */ 
int 
pid(flag ,nevprt ,t ,xd ,x ,nx ,z ,nz ,tvec ,ntvec ,rpar ,nrpar ,
  ipar ,nipar ,u1 ,nu1 ,y1 ,ny1 ,y2 ,ny2 ,y3 ,ny3 ,y4 ,ny4)

  integer *flag ,*nevprt ,*nx ,*nz ,*ntvec ,*nrpar ,*ipar ,*nipar ,
    *nu1 ,*ny1 ,*ny2 ,*ny3 ,*ny4;
  double  *t ,*xd ,*x ,*z ,*tvec ,*rpar ,*u1 ,*y1 ,*y2 ,*y3 ,*y4;

{
  /*  block_outtb is catenated at the end of z*/
  double* block_outtb = z+8;

  /*Copy inputs in the block outtb */
  block_outtb[14] = u1[0];
   set_nevprt(*nevprt);
 
  if (*flag == 1) { /* update outputs */
    pidmain1(z,t,rpar,nrpar,ipar,nipar);
  } 
  else if (*flag == 2) { /* update discrete states */
    pidmain2(z,t,rpar,nrpar,ipar,nipar);
  } 
  else if (*flag == 4) { /* initialisation */
    pid_init(z,t,rpar,nrpar,ipar,nipar);
  } 
  else if (*flag == 5) { /* ending */
    pid_end(z,t,rpar,nrpar,ipar,nipar);
  } 
  y1[0] =  block_outtb[15];
  y2[0] =  block_outtb[14];
  y3[0] =  block_outtb[12];
  y4[0] =  block_outtb[14];
  return 0;

} /* pid */
/*---------------------------------------- main1 */ 
void 
pidmain1(double *z, double *t, double * rpar, integer *nrpar,
  integer *ipar,integer *nipar)
{
  integer pointi[ ]={0};
integer nordcl = 42;

  pidddoit1(z, zptr, t, tevts, evtspt, nevts, pointi, outptr, clkptr,
     ordptr, ordclk, &nordcl, rpar, ipar, rdfunptr, funtyp, &(z[8]),
     (int *)(z+28));
} 
/*---------------------------------------- main2 */ 
void 
pidmain2(double *z, double *t, double * rpar, integer *nrpar,
  integer *ipar,integer *nipar)
{
  integer pointi[ ]={0};
integer nordcl = 42;

  pidddoit2(z, zptr, t, tevts, evtspt, nevts, pointi, outptr, clkptr,
     ordptr, ordclk, &nordcl, rpar, ipar, rdfunptr, funtyp, &(z[8]),
     (int *)(z+28));
} 
/*---------------------------------------- init */ 
void 
pid_init(double *z, double *t, double * rpar, integer *nrpar,
  integer *ipar,integer *nipar)
{
  integer pointi[ ]={0};
integer nordcl = 42;
  /*Block initializations*/
  pid_initi(z, zptr, t, tevts, evtspt, nevts, pointi, outptr, clkptr,
     ordptr, ordclk, &nordcl, rpar, ipar, rdfunptr,funtyp, &(z[8]),
     (int *)(z+28));
  /*Constants propagation*/
  pid_outtb(z, zptr, t, tevts, evtspt, nevts, pointi, outptr, clkptr,
     ordptr, ordclk, &nordcl, rpar, ipar, rdfunptr,funtyp, &(z[8]),
     (int *)(z+28));
} 
/*---------------------------------------- end */ 
void 
pid_end(double *z, double *t, double * rpar, integer *nrpar,
  integer *ipar,integer *nipar)
{
  integer pointi[ ]={0};
integer nordcl = 42;

  pid_endi(z, zptr, t, tevts, evtspt, nevts, pointi, outptr, clkptr,
     ordptr, ordclk, &nordcl, rpar, ipar, rdfunptr, funtyp, &(z[8]),
     (int *)(z+28));
} 
/*---------------------------------------- Scicos real constants update */ 
void 
pid_const_update(double *z, double *t, double * rpar, integer *nrpar,
  integer *ipar,integer *nipar)
{
  integer pointi[ ]={0};
integer nordcl = 42;
  /*Block initializations*/
  /*Constants propagation*/
  pid_outtb(z, zptr, t, tevts, evtspt, nevts, pointi, outptr, clkptr,
     ordptr, ordclk, &nordcl, rpar, ipar, rdfunptr,funtyp, &(z[8]),
     (int *)(z+28));
} 

/*---------------------------------------- ddoit1.c */ 
int 
pidddoit1( z, zptr, told, tevts, evtspt, nevts, pointi, outptr,
   clkptr, ordptr, ordclk, nordcl, rpar, ipar, funptr, funtyp, outtb,
   iwa)

     double  *z; 
     integer *zptr; 
     double  *told,  *tevts; 

     integer *evtspt, *nevts, *pointi, *outptr; 
     integer *clkptr, *ordptr, *ordclk, *nordcl; 
     double  *rpar, *outtb; 
     integer *ipar, *funptr, *funtyp; 
     integer *iwa; 
{
  /* System generated locals */ 
  integer ordclk_dim1, ordclk_offset, i2; 
 
  /* Local variables */ 
  integer flag, keve, nport; 
  double  tvec[3]; 
  double  rdouttb[21]; 
  double  *args[100]; 
  integer sz[100]; 
  integer ierr1; 
  integer ntvec; 
  integer nevprt; 
  integer urg; 
 
  /* Generated constants */
  integer nrd_0 = 0;
  integer nrd_1 = 1;
  integer nrd_2 = 2;
  integer nrd_3 = 3;
  integer nrd_4 = 4;
  integer nrd_5 = 5;
  integer nrd_6 = 6;
  integer nrd_7 = 7;
  integer nrd_8 = 8;
  integer nrd_9 = 9;
  /* Parameter adjustments */ 
  --z; 
  --zptr; 
  --tevts; 
  --evtspt; 
  --outptr; 
  --clkptr; 
  --ordptr; 

  ordclk_dim1 = *nordcl; 
  ordclk_offset = ordclk_dim1 + 1; 
  ordclk -= ordclk_offset; 
  --rpar; 
  --ipar; 
  --funptr; 
  --funtyp; 
  --iwa; 
  --outtb; 
 
  /* Function Body */ 
  iwa[26] = 0; 
  urg = 0; 
  *pointi=0+ totalnevprt;
  tevts[*pointi]=*told;
  keve = *pointi; 
  *pointi = evtspt[keve]; 
  evtspt[keve] = -1; 
 
  ++iwa[26]; 
  iwa[iwa[26]] = keve;
 
  switch(keve) {
  case 1:
 
    flag = 1; 
    nevprt=1;
   args[0]=&(outtb[12]);
   args[1]=&(outtb[13]);
   C2F(dsslti)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_0, &(z[1]),
      &nrd_2, tvec, &ntvec, &(rpar[2]), &nrd_9, &(ipar[1]), &nrd_0,
      (double *)args[0], &nrd_1, (double *)args[1], &nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=1;
   args[1]=&(outtb[15]);
   nport = 1;
   pid_sensor(&flag, &nport, &nevprt, told, (double *)args[1], &nrd_1);
    if(flag < 0 ) return(5 - flag);
    break;
  case 2:
 
    flag = 1; 
    nevprt=1;
   args[0]=&(outtb[15]);
   args[1]=&(outtb[2]);
   C2F(gain)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_0, &(z[1]),
      &nrd_0, tvec, &ntvec, &(rpar[1]), &nrd_1, &(ipar[1]), &nrd_0,
      (double *)args[0], &nrd_1, (double *)args[1], &nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=1;
   args[0]=&(outtb[13]);
   args[1]=&(outtb[14]);
   C2F(gain)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_0, &(z[1]),
      &nrd_0, tvec, &ntvec, &(rpar[25]), &nrd_1, &(ipar[1]), &nrd_0,
      (double *)args[0], &nrd_1, (double *)args[1], &nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=1;
   args[0]=&(outtb[1]);
   args[1]=&(outtb[17]);
   C2F(gensqr)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_0, &(z[6]),
      &nrd_1, tvec, &ntvec, &(rpar[1]), &nrd_0, &(ipar[1]), &nrd_0,
      (double *)args[0], &nrd_0, (double *)args[1], &nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=1;
   args[0]=&(outtb[1]);
   args[1]=&(outtb[20]);
   C2F(rndblk)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_1, &(z[7]),
      &nrd_2, tvec, &ntvec, &(rpar[29]), &nrd_3, &(ipar[7]), &nrd_1,
      (double *)args[0], &nrd_0, (double *)args[1], &nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=1;
   args[0]=&(outtb[2]);
   args[1]=&(outtb[3]);
   C2F(samphold)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_0, &(z[1]),
      &nrd_0, tvec, &ntvec, &(rpar[1]), &nrd_0, &(ipar[1]), &nrd_0,
      (double *)args[0], &nrd_1, (double *)args[1], &nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=1;
   args[0]=&(outtb[17]);
   args[1]=&(outtb[18]);
   C2F(gain)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_0, &(z[1]),
      &nrd_0, tvec, &ntvec, &(rpar[27]), &nrd_1, &(ipar[1]), &nrd_0,
      (double *)args[0], &nrd_1, (double *)args[1], &nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=1;
   args[0]=&(outtb[20]);
   args[1]=&(outtb[19]);
   C2F(gain)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_0, &(z[1]),
      &nrd_0, tvec, &ntvec, &(rpar[28]), &nrd_1, &(ipar[1]), &nrd_0,
      (double *)args[0], &nrd_1, (double *)args[1], &nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=3;
   args[0] = &(outtb[3]);
   sz[0] = 1;
   args[1] = &(outtb[14]);
   sz[1] = 1;
   args[2] = &(outtb[7]);
   sz[2] = 1;
   plusblk(&flag, &nevprt, told,&(w[1]), &(x[1]), &nrd_0, &(z[1]),&nrd_0,
      tvec, &ntvec, &(rpar[1]), &nrd_0,&(ipar[1]), &nrd_0, &(args[0]),
      &(sz[0]), &nrd_2,&(args[2]),&(sz[2]),&nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=1;
   args[0] = &(outtb[18]);
   sz[0] = 1;
   args[1] = &(outtb[19]);
   sz[1] = 1;
   args[2] = &(outtb[16]);
   sz[2] = 1;
   selector(&flag, &nevprt, told,&(w[1]), &(x[1]), &nrd_0, &(z[5]),
     &nrd_1, tvec, &ntvec, &(rpar[1]), &nrd_0,&(ipar[1]), &nrd_0,
      &(args[0]), &(sz[0]), &nrd_2,&(args[2]),&(sz[2]),&nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=1;
   args[0]=&(outtb[7]);
   args[1]=&(outtb[8]);
   C2F(gain)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_0, &(z[1]),
      &nrd_0, tvec, &ntvec, &(rpar[16]), &nrd_1, &(ipar[1]), &nrd_0,
      (double *)args[0], &nrd_1, (double *)args[1], &nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=1;
   args[0]=&(outtb[7]);
   args[1]=&(outtb[4]);
   C2F(dsslti)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_0, &(z[3]),
      &nrd_1, tvec, &ntvec, &(rpar[17]), &nrd_4, &(ipar[1]), &nrd_0,
      (double *)args[0], &nrd_1, (double *)args[1], &nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=1;
   args[0]=&(outtb[7]);
   args[1]=&(outtb[5]);
   C2F(dsslti)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_0, &(z[4]),
      &nrd_1, tvec, &ntvec, &(rpar[21]), &nrd_4, &(ipar[1]), &nrd_0,
      (double *)args[0], &nrd_1, (double *)args[1], &nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=1;
   args[0]=&(outtb[4]);
   args[1]=&(outtb[9]);
   C2F(gain)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_0, &(z[1]),
      &nrd_0, tvec, &ntvec, &(rpar[14]), &nrd_1, &(ipar[1]), &nrd_0,
      (double *)args[0], &nrd_1, (double *)args[1], &nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=1;
   args[0]=&(outtb[5]);
   args[1]=&(outtb[6]);
   C2F(gain)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_0, &(z[1]),
      &nrd_0, tvec, &ntvec, &(rpar[15]), &nrd_1, &(ipar[1]), &nrd_0,
      (double *)args[0], &nrd_1, (double *)args[1], &nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=3;
   args[0] = &(outtb[9]);
   sz[0] = 1;
   args[1] = &(outtb[8]);
   sz[1] = 1;
   args[2] = &(outtb[10]);
   sz[2] = 1;
   plusblk(&flag, &nevprt, told,&(w[1]), &(x[1]), &nrd_0, &(z[1]),&nrd_0,
      tvec, &ntvec, &(rpar[1]), &nrd_0,&(ipar[1]), &nrd_0, &(args[0]),
      &(sz[0]), &nrd_2,&(args[2]),&(sz[2]),&nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=1;
   args[0]=&(outtb[6]);
   args[1]=&(outtb[11]);
   C2F(lusat)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_0, &(z[1]),
      &nrd_0, tvec, &ntvec, &(rpar[11]), &nrd_3, &(ipar[1]), &nrd_0,
      (double *)args[0], &nrd_1, (double *)args[1], &nrd_1);
    
    if(flag < 0 ) return(5 - flag);
 
    flag = 1; 
    nevprt=3;
   args[0] = &(outtb[11]);
   sz[0] = 1;
   args[1] = &(outtb[10]);
   sz[1] = 1;
   args[2] = &(outtb[12]);
   sz[2] = 1;
   plusblk(&flag, &nevprt, told,&(w[1]), &(x[1]), &nrd_0, &(z[1]),&nrd_0,
      tvec, &ntvec, &(rpar[1]), &nrd_0,&(ipar[1]), &nrd_0, &(args[0]),
      &(sz[0]), &nrd_2,&(args[2]),&(sz[2]),&nrd_1);
    
    if(flag < 0 ) return(5 - flag);
    break;
  case 3:
 
    flag = 1; 
    nevprt=1;