pid.c

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

    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;
    nevprt=1;
 
    flag = 3;
    C2F(ifthel)(&flag, &nevprt, &ntvec, &(rpar[1]), &nrd_0, &(ipar[1]),
       &nrd_0, &(outtb[1]),  &nrd_1);
     
 
	   if(flag < 0 ) return(5 - flag);
 
    if (ntvec >= 1) {
      if (funtyp[22] == -1) {
        ++(*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;
  }  
  return 0;
} /* edoit1 */      

/*---------------------------------------- ddoit2 */ 
int 
pidddoit2( 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  i1, i; 
 
  /* Local variables */ 
  integer flag, keve, nport; 
  double  tvec[3]; 
  double  rdouttb[21]; 
  double  *args[100]; 
  integer sz[100]; 
  integer ntvec; 
  integer 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; 
  --rpar; 
  --ipar; 
  --funptr; 
  --funtyp; 
  --iwa; 
  --outtb; 
 
  /* Function Body */ 
 
  /*update continuous and discrete states on event */
  if (iwa[26] == 0) {
    return 0 ;
  }
  i1 = iwa[26];
  for (i = 1; i <= i1; ++i) {
    keve = iwa[i];
    switch(keve) {
    case 1:
    flag = 2;
    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 = 2;
    nevprt=1;
   args[0]=&(outtb[13]);
   nport = 1;
   pid_actuator(&flag, &nport, &nevprt, told, (double *)args[0], &nrd_1);
	   if(flag < 0 ) return (5 - flag); 
    flag = 2;
    nevprt=1;
   args[0]=&(outtb[15]);
   nport = 2;
   pid_actuator(&flag, &nport, &nevprt, told, (double *)args[0], &nrd_1);
	   if(flag < 0 ) return (5 - flag); 
    flag = 2;
    nevprt=1;
   args[0]=&(outtb[15]);
   nport = 4;
   pid_actuator(&flag, &nport, &nevprt, told, (double *)args[0], &nrd_1);
	   if(flag < 0 ) return (5 - flag); 
    break;
    case 2:
    flag = 2;
    nevprt=1;
   args[0]=&(outtb[16]);
   nport = 3;
   pid_actuator(&flag, &nport, &nevprt, told, (double *)args[0], &nrd_1);
	   if(flag < 0 ) return (5 - flag); 
    flag = 2;
    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 = 2;
    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 = 2;
    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 = 2;
    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 = 2;
    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); 
    break;
    case 3:
    flag = 2;
    nevprt=1;
   args[0]=&(outtb[16]);
   nport = 3;
   pid_actuator(&flag, &nport, &nevprt, told, (double *)args[0], &nrd_1);
	   if(flag < 0 ) return (5 - flag); 
    flag = 2;
    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 = 2;
    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 = 2;
    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 = 2;
    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 = 2;
    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); 
    break;
    }
  }
  return 0;
} /* ddoit2 */
/*---------------------------------------- outtbini */ 
int 
pid_outtb( z, zptr, told, tevts, evtspt, nevts, pointi, outptr,
	clkptr, ordptr, ordclk, nordcl, rpar, ipar, funptr, funtyp, 
    outtb, iwa) 

     /*Constants propagation*/
     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; 
{ 
 
  /* Local variables */ 
  integer flag; 
  double  tvec[3]; 
  double  rdouttb[21]; 
  double  *args[100]; 
  integer sz[100]; 
  integer ntvec; 
  integer nevprt=0; 
 
  /* 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; 
  --rpar; 
  --ipar; 
  --funptr; 
  --funtyp; 
  --iwa; 
  --outtb; 
 
  /* Function Body */ 
  
  flag=1 ;
  args[0]=&(outtb[1]);
  args[1]=&(outtb[1]);
  C2F(bidon)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_0, &(z[1]),
     &nrd_0, tvec, &ntvec, &(rpar[1]), &nrd_0, &(ipar[3]), &nrd_1,
     (double *)args[0], &nrd_0, (double *)args[1], &nrd_0);
   
   if(flag < 0 ) return(5 - flag); 
  C2F(cstblk)(&flag, &nevprt,told, &(w[1]), &(x[1]), &nrd_0, &(z[1]),
     &nrd_0, tvec, &ntvec, &(rpar[26]), &nrd_1, &(ipar[1]), &nrd_0,
     &(outtb[1]), &nrd_1);
   
   if(flag < 0 ) return(5 - flag); 
  return 0;
} /* pid_outtb */
/*---------------------------------------- initi */ 
int 
pid_initi( z, zptr, told, tevts, evtspt, nevts, pointi, outptr,
	clkptr, ordptr, ordclk, nordcl, rpar, ipar, funptr, funtyp, 
    outtb, iwa) 

     /*Block initialization (flag=4)*/
     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; 
{ 
 
  /* Local variables */ 
  integer flag; 
  double  tvec[3]; 
  double  rdouttb[21]; 
  double  *args[100]; 
  integer sz[100]; 
  integer nport; 
  integer ntvec; 
  integer nevprt=0;