pid.c

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

   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=2;
   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;
  }  
  
  switch(keve) {
  case 1:
  /*        Initialize tvec */
  ntvec=2;
	
  flag = 3;
  nevprt = 1;
 C2F(ifthel)(&flag, &nevprt, &ntvec, &(rpar[1]), &nrd_0, &(ipar[1]),
    &nrd_0, &(outtb[1]),  &nrd_1);
  
 
	 if(flag < 0 ) return(5 - flag);
 
  ++urg;
  i2 = ntvec + clkptr[22] - 1;
  C2F(putevs)(&tevts[1], &evtspt[1], nevts, pointi, told, &i2, &ierr1);
  if (ierr1 != 0) return 3;
    break;
  case 2:
    break;
  case 3:
    break;
  }
  if (urg > 0) {
 L43:
    pidedoit1(&z[1], &zptr[1], told, &tevts[1], &evtspt[1], nevts, pointi,
       &outptr[1], &clkptr[1], &ordptr[1],  &ordclk[ordclk_offset], nordcl,
       &rpar[1], &ipar[1], &funptr[1], &funtyp[1], &(z[9]), &urg, (int *)(z+
      29));
    if (urg > 0) {
      goto L43;
    }
  }
  return 0;
 
} /* ddoit1 */
  
/*---------------------------------------- edoit1.c */ 
int 
pidedoit1( z, zptr, told,tevts, evtspt, nevts, pointi,  outptr,
   clkptr, ordptr, ordclk, nordcl, rpar, ipar, funptr, funtyp, outtb,
   urg, 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 *urg,  *iwa; 
{ 
  /* System generated locals */ 
  integer ordclk_dim1, ordclk_offset, i2; 
 
  /* Local variables */ 
  integer flag, keve, nport; 
  integer nord; 
  double  tvec[3]; 
  double  rdouttb[21]; 
  double  *args[100]; 
  integer sz[100]; 
  integer ierr1, i, nx=0; 
  integer ntvec, nevprt; 
 
  /* 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 */ 
  --(*urg); 
  keve = *pointi; 
  *pointi = evtspt[keve]; 
  evtspt[keve] = -1; 
 
  nord = ordptr[keve + 1] - ordptr[keve]; 
  if (nord == 0) { 
    return 0; 
  } 
  ++(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;
   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);